Copter: compassmot and motor_test set_soft_armed

This commit is contained in:
Randy Mackay 2019-09-12 10:59:25 +09:00
parent 1a7284ca73
commit bce5154b2d
2 changed files with 5 additions and 0 deletions

View File

@ -125,6 +125,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
init_rc_out();
enable_motor_output();
motors->armed(true);
hal.util->set_soft_armed(true);
// initialise run time
last_run_time = millis();
@ -230,6 +231,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
// stop motors
motors->output_min();
motors->armed(false);
hal.util->set_soft_armed(false);
// set and save motor compensation
if (updated) {

View File

@ -41,6 +41,7 @@ void Copter::motor_test_output()
motor_test_start_ms = now;
if (!motors->armed()) {
motors->armed(true);
hal.util->set_soft_armed(true);
}
}
return;
@ -152,6 +153,7 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t
init_rc_out();
enable_motor_output();
motors->armed(true);
hal.util->set_soft_armed(true);
}
// disable throttle and gps failsafe
@ -197,6 +199,7 @@ void Copter::motor_test_stop()
// disarm motors
motors->armed(false);
hal.util->set_soft_armed(false);
// reset timeout
motor_test_start_ms = 0;