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); };