Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLINK: move try_send_message handling of vfr_hud up

Browse Source
This commit is contained in:
Peter Barker 2018-05-09 20:55:37 +10:00 committed by Andrew Tridgell
parent 1331e6ca14
commit d1174bcf70
2 changed files with 52 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/GCS_MAVLink/GCS.h
View File

@ -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);

44
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -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();