diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 8ff7aba0ad..a3b374a1c9 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -173,7 +173,7 @@ public: #endif void send_autopilot_version() const; void send_local_position() const; - void send_vibration(const AP_InertialSensor &ins) const; + void send_vibration() const; void send_home(const Location &home) const; void send_ekf_origin(const Location &ekf_origin) const; void send_servo_output_raw(bool hil); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 40b9ffe62e..33f621eca4 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1496,8 +1496,10 @@ void GCS_MAVLINK::send_local_position() const /* send VIBRATION message */ -void GCS_MAVLINK::send_vibration(const AP_InertialSensor &ins) const +void GCS_MAVLINK::send_vibration() const { + const AP_InertialSensor &ins = AP::ins(); + Vector3f vibration = ins.get_vibration_levels(); mavlink_msg_vibration_send( @@ -2671,6 +2673,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_ahrs(); break; + case MSG_VIBRATION: + CHECK_PAYLOAD_SIZE(VIBRATION); + send_vibration(); + break; + default: // try_send_message must always at some stage return true for // a message, or we will attempt to infinitely retry the