Copter: check with motors if motor test is allowed

This commit is contained in:
Iampete1 2023-06-18 19:56:49 +01:00 committed by Randy Mackay
parent d57ce2ad6f
commit 717f82f104

View File

@ -101,6 +101,13 @@ bool Copter::mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check
return false;
}
// Check Motor test is allowed
char failure_msg[50] {};
if (!motors->motor_test_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: %s", mode, failure_msg);
return false;
}
// check rc has been calibrated
if (check_rc && !arming.rc_calibration_checks(true)) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: RC not calibrated", mode);