From c3f3ee03d08a2aa17938c23adb4d06896eb157f6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 27 Sep 2018 10:41:26 +0900 Subject: [PATCH] Rover: windvane calibration can be started with preflight-calibration msg send COMMAND_LONG with MAV_CMD_PREFLIGHT_CALIBRATION command and set param6 field to 3 --- APMrover2/GCS_Mavlink.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 3a2e8392d4..c39d9701ba 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -584,6 +584,12 @@ MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlin } else { return MAV_RESULT_FAILED; } + } else if (is_equal(packet.param6, 1.0f)) { + if (rover.g2.windvane.start_calibration()) { + return MAV_RESULT_ACCEPTED; + } else { + return MAV_RESULT_FAILED; + } } return GCS_MAVLINK::_handle_command_preflight_calibration(packet);