Browse Source

GCS_MAVLink: move try_send_message handling of VIBRATION up

Also, use singleton to get ins
master
Peter Barker 7 years ago committed by WickedShell
parent
commit
e4b4a746e6
  1. 2
      libraries/GCS_MAVLink/GCS.h
  2. 9
      libraries/GCS_MAVLink/GCS_Common.cpp

2
libraries/GCS_MAVLink/GCS.h

@ -173,7 +173,7 @@ public: @@ -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);

9
libraries/GCS_MAVLink/GCS_Common.cpp

@ -1496,8 +1496,10 @@ void GCS_MAVLINK::send_local_position() const @@ -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) @@ -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

Loading…
Cancel
Save