zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: use attitude and position controller prearm checks

master
Peter Barker 6 years ago committed by Randy Mackay
parent
b40f03dfbe
commit
cac87b8e0b
2 changed files with 7 additions and 56 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 62
      ArduCopter/AP_Arming.cpp
  2. 1
      ArduCopter/AP_Arming.h

62
ArduCopter/AP_Arming.cpp
Unescape View File

@ -239,55 +239,14 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) @@ -239,55 +239,14 @@ bool AP_Arming_Copter::parameter_checks(bool 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.pos_control->get_pos_xy_p().kP())) {
parameter_checks_pid_warning_message(display_failure, "PSC_POSXY_P");
// ensure controllers are OK with us arming:
char failure_msg[50];
if (!copter.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
return false;
} else if (is_zero(copter.pos_control->get_pos_z_p().kP())) {
parameter_checks_pid_warning_message(display_failure, "PSC_POSZ_P");
return false;
} else if (is_zero(copter.pos_control->get_vel_z_p().kP())) {
parameter_checks_pid_warning_message(display_failure, "PSC_VELZ_P");
return false;
} else if (is_zero(copter.pos_control->get_accel_z_pid().kP())) {
parameter_checks_pid_warning_message(display_failure, "PSC_ACCZ_P");
return false;
} else if (is_zero(copter.pos_control->get_accel_z_pid().kI())) {
parameter_checks_pid_warning_message(display_failure, "PSC_ACCZ_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");
}
if (!copter.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
return false;
}
}
@ -295,13 +254,6 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) @@ -295,13 +254,6 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
return true;
}
void AP_Arming_Copter::parameter_checks_pid_warning_message(bool display_failure, const char *error_msg)
{
check_failed(ARMING_CHECK_PARAMETERS,
display_failure,
"Check PIDs - %s", error_msg);
}
// check motor setup was successful
bool AP_Arming_Copter::motor_checks(bool display_failure)
{

1
ArduCopter/AP_Arming.h
Unescape View File

@ -49,5 +49,4 @@ protected: @@ -49,5 +49,4 @@ protected:
private:
void parameter_checks_pid_warning_message(bool display_failure, const char *error_msg);
};

Write Preview
Loading…
Cancel
Save