diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 805b7589c8..3109660627 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -1,6 +1,7 @@ #include "AC_AttitudeControl.h" #include -#include + +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; isnprintf(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; isnprintf(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; +} diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 41eb6fedbc..5102d36501 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -6,7 +6,6 @@ #include #include #include -#include #include #include #include @@ -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) {} diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 06eb4ebae9..a6394bc13b 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/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; isnprintf(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; +} diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index fae0e64d0a..de3fada0ee 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/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: