Copter: support pre-flight calibration of gyro

This commit is contained in:
Randy Mackay 2014-10-08 20:19:18 +09:00
parent 9a0a83f404
commit a8733ae8a8

View File

@ -1100,10 +1100,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
if (packet.param1 == 1 ||
packet.param2 == 1) {
ins.init_accel();
ahrs.set_trim(Vector3f(0,0,0)); // clear out saved trim
if (packet.param1 == 1) {
// gyro offset calibration
ins.init_gyro();
result = MAV_RESULT_ACCEPTED;
}
if (packet.param3 == 1) {