diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 183bf23a19..b718fd2cc3 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -414,6 +414,8 @@ protected: void handle_optical_flow(const mavlink_message_t &msg); + MAV_RESULT handle_fixed_mag_cal_yaw(const mavlink_command_long_t &packet); + // vehicle-overridable message send function virtual bool try_send_message(enum ap_message id); virtual void send_global_position_int(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 02460708b8..57620fb6b7 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2951,6 +2951,19 @@ void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg) optflow->handle_msg(msg); } + +/* + handle MAV_CMD_FIXED_MAG_CAL_YAW + */ +MAV_RESULT GCS_MAVLINK::handle_fixed_mag_cal_yaw(const mavlink_command_long_t &packet) +{ + Compass &compass = AP::compass(); + return compass.mag_cal_fixed_yaw(packet.param1, + uint8_t(packet.param2), + packet.param3, + packet.param4); +} + /* handle messages which don't require vehicle specific data */ @@ -3665,6 +3678,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t return MAV_RESULT_UNSUPPORTED; + case MAV_CMD_FIXED_MAG_CAL_YAW: + result = handle_fixed_mag_cal_yaw(packet); + break; + default: result = MAV_RESULT_UNSUPPORTED; break;