diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index d4ed96f95c..4c897b4d6e 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -186,6 +186,7 @@ public: virtual void send_attitude() const; void send_autopilot_version() const; void send_local_position() const; + void send_vfr_hud(); void send_vibration() const; void send_named_float(const char *name, float value) const; void send_home() const; @@ -368,6 +369,13 @@ protected: virtual int32_t global_position_int_alt() const; virtual int32_t global_position_int_relative_alt() const; + // these methods are called after vfr_hud_velned is updated + virtual bool vfr_hud_make_alt_relative() const { return false; } + virtual float vfr_hud_climbrate() const; + virtual float vfr_hud_airspeed() const; + virtual int16_t vfr_hud_throttle() const { return 0; } + Vector3f vfr_hud_velned; + private: float adjust_rate_for_stream_trigger(enum streams stream_num); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 310d37372f..27c82d5ecb 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1727,6 +1727,45 @@ void GCS_MAVLINK::send_accelcal_vehicle_position(uint32_t position) } } + +float GCS_MAVLINK::vfr_hud_airspeed() const +{ + // because most vehicles don't have airspeed sensors, we return a + // different sort of speed estimate in the relevant field for + // comparison's sake. + return AP::gps().ground_speed(); +} + +float GCS_MAVLINK::vfr_hud_climbrate() const +{ + return -vfr_hud_velned.z; +} + +void GCS_MAVLINK::send_vfr_hud() +{ + AP_AHRS &ahrs = AP::ahrs(); + + // return values ignored; we send stale data + ahrs.get_position(global_position_current_loc); + ahrs.get_velocity_NED(vfr_hud_velned); + + float alt; + if (vfr_hud_make_alt_relative()) { + ahrs.get_relative_position_D_home(alt); + alt = -alt; + } else { + alt = global_position_current_loc.alt / 100.0f; + } + mavlink_msg_vfr_hud_send( + chan, + vfr_hud_airspeed(), + ahrs.groundspeed(), + (ahrs.yaw_sensor / 100) % 360, + vfr_hud_throttle(), + alt, + vfr_hud_climbrate()); +} + /* handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command @@ -2964,6 +3003,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_ahrs(); break; + case MSG_VFR_HUD: + CHECK_PAYLOAD_SIZE(VFR_HUD); + send_vfr_hud(); + break; + case MSG_VIBRATION: CHECK_PAYLOAD_SIZE(VIBRATION); send_vibration();