Browse Source

Copter: check all gyros and accels in pre-arm check

mission-4.1.18
Randy Mackay 11 years ago
parent
commit
d7343d5dc7
  1. 14
      ArduCopter/motors.pde

14
ArduCopter/motors.pde

@ -338,10 +338,18 @@ static void pre_arm_checks(bool display_failure)
return; return;
} }
// check accels and gyros are healthy // check accels are healthy
if(!ins.healthy()) { if(!ins.get_accel_health_all()) {
if (display_failure) { if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not healthy")); gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Accels not healthy"));
}
return;
}
// check gyros are healthy
if(!ins.get_gyro_health_all()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Gyros not healthy"));
} }
return; return;
} }

Loading…
Cancel
Save