Browse Source

GCS_MAVLink: allow vehicles to override VFR_HUD.alt

mission-4.1.18
Peter Barker 6 years ago committed by Randy Mackay
parent
commit
3749cfd764
  1. 1
      libraries/GCS_MAVLink/GCS.h
  2. 7
      libraries/GCS_MAVLink/GCS_Common.cpp

1
libraries/GCS_MAVLink/GCS.h

@ -386,6 +386,7 @@ protected: @@ -386,6 +386,7 @@ protected:
virtual float vfr_hud_climbrate() const;
virtual float vfr_hud_airspeed() const;
virtual int16_t vfr_hud_throttle() const { return 0; }
virtual float vfr_hud_alt() const;
Vector3f vfr_hud_velned;
static constexpr const float magic_force_arm_value = 2989.0f;

7
libraries/GCS_MAVLink/GCS_Common.cpp

@ -1789,6 +1789,11 @@ float GCS_MAVLINK::vfr_hud_climbrate() const @@ -1789,6 +1789,11 @@ float GCS_MAVLINK::vfr_hud_climbrate() const
return -vfr_hud_velned.z;
}
float GCS_MAVLINK::vfr_hud_alt() const
{
return global_position_current_loc.alt * 0.01f; // cm -> m
}
void GCS_MAVLINK::send_vfr_hud()
{
AP_AHRS &ahrs = AP::ahrs();
@ -1803,7 +1808,7 @@ void GCS_MAVLINK::send_vfr_hud() @@ -1803,7 +1808,7 @@ void GCS_MAVLINK::send_vfr_hud()
ahrs.groundspeed(),
(ahrs.yaw_sensor / 100) % 360,
vfr_hud_throttle(),
global_position_current_loc.alt * 0.01f, // cm -> m
vfr_hud_alt(),
vfr_hud_climbrate());
}

Loading…
Cancel
Save