Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
ChrisBird 2017-11-15 03:59:08 -08:00 committed by Randy Mackay
parent 52ed8d0bb0
commit 1f1bb61bfa
2 changed files with 60 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
ArduCopter/AP_Arming.cpp
View File

@ -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)
{

1
ArduCopter/AP_Arming.h
View File

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