AC_AttitudeControl: add pre-arm checks

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -1,6 +1,7 @@
 #include "AC_AttitudeControl.h"
 #include <AP_HAL/AP_HAL.h>
-#include <AP_Math/AP_Math.h>
+
+extern const AP_HAL::HAL& hal;
 
 #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
  // default gains for Plane
@@ -1007,3 +1008,68 @@ float AC_AttitudeControl::max_rate_step_bf_yaw()
     float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
     return 2.0f*throttle_hover*AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_yaw_pid().kD())/_dt + get_rate_yaw_pid().kP());
 }
+
+bool AC_AttitudeControl::pre_arm_checks(const char *param_prefix,
+                                        char *failure_msg,
+                                        const uint8_t failure_msg_len)
+{
+    // validate AC_P members:
+    const struct {
+        const char *pid_name;
+        AC_P &p;
+    } ps[] = {
+        { "ANG_PIT", get_angle_pitch_p() },
+        { "ANG_RLL", get_angle_roll_p() },
+        { "ANG_YAW", get_angle_yaw_p() }
+    };
+    for (uint8_t i=0; i<ARRAY_SIZE(ps); i++) {
+        // all AC_P's must have a positive P value:
+        if (!is_positive(ps[i].p.kP())) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_P must be > 0", param_prefix, ps[i].pid_name);
+            return false;
+        }
+    }
+
+    // validate AC_PID members:
+    const struct {
+        const char *pid_name;
+        AC_PID &pid;
+    } pids[] = {
+        { "RAT_RLL", get_rate_roll_pid() },
+        { "RAT_PIT", get_rate_pitch_pid() },
+        { "RAT_YAW", get_rate_yaw_pid() },
+    };
+    for (uint8_t i=0; i<ARRAY_SIZE(pids); i++) {
+        // if the PID has a positive FF then we just ensure kP and
+        // kI aren't negative
+        AC_PID &pid = pids[i].pid;
+        const char *pid_name = pids[i].pid_name;
+        if (is_positive(pid.ff())) {
+            // kP and kI must be non-negative:
+            if (is_negative(pid.kP())) {
+                hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_P must be >= 0", param_prefix, pid_name);
+                return false;
+            }
+            if (is_negative(pid.kI())) {
+                hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_I must be >= 0", param_prefix, pid_name);
+                return false;
+            }
+        } else {
+            // kP and kI must be positive:
+            if (!is_positive(pid.kP())) {
+                hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_P must be > 0", param_prefix, pid_name);
+                return false;
+            }
+            if (!is_positive(pid.kI())) {
+                hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_I must be > 0", param_prefix, pid_name);
+                return false;
+            }
+        }
+        // never allow a negative D term (but zero is allowed)
+        if (is_negative(pid.kD())) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_D must be >= 0", param_prefix, pid_name);
+            return false;
+        }
+    }
+    return true;
+}

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -6,7 +6,6 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
-#include <AP_InertialSensor/AP_InertialSensor.h>
 #include <AP_AHRS/AP_AHRS_View.h>
 #include <AP_Motors/AP_Motors.h>
 #include <AC_PID/AC_PID.h>
@@ -291,6 +290,11 @@ public:
     // passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw
     virtual void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds) {};
 
+    // provide feedback on whether arming would be a good idea right now:
+    bool pre_arm_checks(const char *param_prefix,
+                        char *failure_msg,
+                        const uint8_t failure_msg_len);
+
     // enable inverted flight on backends that support it
     virtual void set_inverted_flight(bool inverted) {}
     

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -1219,3 +1219,37 @@ Vector3f AC_PosControl::sqrt_controller(const Vector3f& error, float p, float se
         return Vector3f(error.x*p, error.y*p, error.z);
     }
 }
+
+bool AC_PosControl::pre_arm_checks(const char *param_prefix,
+                                   char *failure_msg,
+                                   const uint8_t failure_msg_len)
+{
+    // validate AC_P members:
+    const struct {
+        const char *pid_name;
+        AC_P &p;
+    } ps[] = {
+        { "POSXY", get_pos_xy_p() },
+        { "POSZ", get_pos_z_p() },
+        { "VELZ", get_vel_z_p() },
+    };
+    for (uint8_t i=0; i<ARRAY_SIZE(ps); i++) {
+        // all AC_P's must have a positive P value:
+        if (!is_positive(ps[i].p.kP())) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_P must be > 0", param_prefix, ps[i].pid_name);
+            return false;
+        }
+    }
+
+    // the z-control PID doesn't use FF, so P and I must be positive
+    if (!is_positive(get_accel_z_pid().kP())) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "%s_ACCZ_P must be > 0", param_prefix);
+        return false;
+    }
+    if (!is_positive(get_accel_z_pid().kI())) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "%s_ACCZ_I must be > 0", param_prefix);
+        return false;
+    }
+
+    return true;
+}

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -294,6 +294,11 @@ public:
     // write log to dataflash
     void write_log();
 
+    // provide feedback on whether arming would be a good idea right now:
+    bool pre_arm_checks(const char *param_prefix,
+                        char *failure_msg,
+                        const uint8_t failure_msg_len);
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected: