From 3710c50c4e288219c2f0abf3687142680a31f678 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 4 Jul 2018 13:43:07 +1000 Subject: [PATCH] GCS_MAVLink: handle MAV_CMD_ACCELCAL_VEHICLE_POS in GCS base class --- libraries/GCS_MAVLink/GCS.h | 1 + libraries/GCS_MAVLink/GCS_Common.cpp | 11 +++++++++++ 2 files changed, 12 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index c53aebb9d0..ed7a77e1bc 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -355,6 +355,7 @@ protected: virtual MAV_RESULT _handle_command_preflight_calibration_baro(); void handle_command_long(mavlink_message_t* msg); + MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet); MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet); virtual MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet); MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 1c00185922..2024f41def 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2689,12 +2689,23 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_gripper(const mavlink_command_long_t & return result; } +MAV_RESULT GCS_MAVLINK::handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet) +{ + if (!AP::ins().get_acal()->gcs_vehicle_position(packet.param1)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; +} + MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t &packet) { MAV_RESULT result = MAV_RESULT_FAILED; switch (packet.command) { + case MAV_CMD_ACCELCAL_VEHICLE_POS: + result = handle_command_accelcal_vehicle_pos(packet); + case MAV_CMD_DO_SET_MODE: result = handle_command_do_set_mode(packet); break;