From 1f1bb61bfa8527edc971a8ecf191c87084680623 Mon Sep 17 00:00:00 2001
From: ChrisBird <ChrisBird77@gmail.com>
Date: Wed, 15 Nov 2017 03:59:08 -0800
Subject: [PATCH] Copter: Add PreArm - PID Parameter check

This addresses the enhancement requested in: https://github.com/ArduPilot/ardupilot/issues/2424

It checks various PID values to check if they are zero, if they are and the corresponding rate forward feed is zero then it flags it as PreArm failure.
It has been added to the parameter check, so can be overridden if required.
---
 ArduCopter/AP_Arming.cpp | 59 ++++++++++++++++++++++++++++++++++++++++
 ArduCopter/AP_Arming.h   |  1 +
 2 files changed, 60 insertions(+)

diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp
index 953b394a49..c6026a4554 100644
--- a/ArduCopter/AP_Arming.cpp
+++ b/ArduCopter/AP_Arming.cpp
@@ -254,11 +254,70 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
         if (!pre_arm_proximity_check(display_failure)) {
             return false;
         }
+
+        // Check for 0 value PID's - some items can / should be 0 and as such are not checked.
+        // If the ATC_RAT_*_FF is non zero then the corresponding ATC_RAT_* PIDS can be 0.
+        if (is_zero(copter.g.p_pos_xy.kP())) {
+            parameter_checks_pid_warning_message(display_failure, "POS_XY_P");
+            return false;
+        } else if (is_zero(copter.g.p_alt_hold.kP())) {
+            parameter_checks_pid_warning_message(display_failure, "POS_Z_P");
+            return false;
+        } else if (is_zero(copter.g.p_vel_z.kP())) {
+            parameter_checks_pid_warning_message(display_failure, "VEL_Z_P");
+            return false;
+        } else if (is_zero(copter.g.pid_accel_z.kP())) {
+            parameter_checks_pid_warning_message(display_failure, "ACCEL_Z_P");
+            return false;
+        } else if (is_zero(copter.g.pid_accel_z.kI())) {
+            parameter_checks_pid_warning_message(display_failure, "ACCEL_Z_I");
+            return false;
+        } else if (is_zero(copter.attitude_control->get_rate_roll_pid().kP()) && is_zero(copter.attitude_control->get_rate_roll_pid().ff())) {
+            parameter_checks_pid_warning_message(display_failure, "ATC_RAT_RLL_P");
+            return false;
+        } else if (is_zero(copter.attitude_control->get_rate_roll_pid().kI()) && is_zero(copter.attitude_control->get_rate_roll_pid().ff())) {
+            parameter_checks_pid_warning_message(display_failure, "ATC_RAT_RLL_I");
+            return false;
+        } else if (is_zero(copter.attitude_control->get_rate_roll_pid().kD()) && is_zero(copter.attitude_control->get_rate_roll_pid().ff())) {
+            parameter_checks_pid_warning_message(display_failure, "ATC_RAT_RLL_D");
+            return false;
+        } else if (is_zero(copter.attitude_control->get_rate_pitch_pid().kP()) && is_zero(copter.attitude_control->get_rate_pitch_pid().ff())) {
+            parameter_checks_pid_warning_message(display_failure, "ATC_RAT_PIT_P");
+            return false;
+        } else if (is_zero(copter.attitude_control->get_rate_pitch_pid().kI()) && is_zero(copter.attitude_control->get_rate_pitch_pid().ff())) {
+            parameter_checks_pid_warning_message(display_failure, "ATC_RAT_PIT_I");
+            return false;
+        } else if (is_zero(copter.attitude_control->get_rate_pitch_pid().kD()) && is_zero(copter.attitude_control->get_rate_pitch_pid().ff())) {
+            parameter_checks_pid_warning_message(display_failure, "ATC_RAT_PIT_D");
+            return false;
+        } else if (is_zero(copter.attitude_control->get_rate_yaw_pid().kP()) && is_zero(copter.attitude_control->get_rate_yaw_pid().ff())) {
+            parameter_checks_pid_warning_message(display_failure, "ATC_RAT_YAW_P");
+            return false;
+        } else if (is_zero(copter.attitude_control->get_rate_yaw_pid().kI()) && is_zero(copter.attitude_control->get_rate_yaw_pid().ff())) {
+            parameter_checks_pid_warning_message(display_failure, "ATC_RAT_YAW_I");
+            return false;
+        } else if (is_zero(copter.attitude_control->get_angle_pitch_p().kP())) {
+            parameter_checks_pid_warning_message(display_failure, "ATC_ANG_PIT_P");
+            return false;
+        } else if (is_zero(copter.attitude_control->get_angle_roll_p().kP())) {
+            parameter_checks_pid_warning_message(display_failure, "ATC_ANG_RLL_P");
+            return false;
+        } else if (is_zero(copter.attitude_control->get_angle_yaw_p().kP())) {
+            parameter_checks_pid_warning_message(display_failure, "ATC_ANG_YAW_P");
+            return false;
+        }
     }
 
     return true;
 }
 
+void AP_Arming_Copter::parameter_checks_pid_warning_message(bool display_failure, const char *error_msg)
+{
+    if (display_failure) {
+        gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check PIDs - %s", error_msg);
+    }
+}
+
 // check motor setup was successful
 bool AP_Arming_Copter::motor_checks(bool display_failure)
 {
diff --git a/ArduCopter/AP_Arming.h b/ArduCopter/AP_Arming.h
index 6fdefd8e87..ac73d74a53 100644
--- a/ArduCopter/AP_Arming.h
+++ b/ArduCopter/AP_Arming.h
@@ -62,4 +62,5 @@ private:
     const AP_InertialSensor &_ins;
     const AP_AHRS_NavEKF &_ahrs_navekf;
 
+    void parameter_checks_pid_warning_message(bool display_failure, const char *error_msg);
 };