Copter: reject preflight calibration when armed

This commit is contained in:
Randy Mackay 2015-03-09 14:28:29 +09:00 committed by Andrew Tridgell
parent b0e7990c90
commit 10724f5738
1 changed files with 5 additions and 0 deletions

View File

@ -1122,6 +1122,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
// exit immediately if armed
if (motors.armed()) {
result = MAV_RESULT_FAILED;
break;
}
if (packet.param1 == 1) {
// gyro offset calibration
ins.init_gyro();