aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: move sending of vfr_hud up

Browse Source
This commit is contained in:
Peter Barker 2018-01-29 09:59:58 +11:00 committed by Andrew Tridgell
parent 5f2221fe74
commit 8b4ffb11f5
3 changed files with 34 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
ArduPlane/GCS_Mavlink.cpp
View File

@ -237,22 +237,37 @@ void Plane::send_servo_out(mavlink_channel_t chan)
rssi.read_receiver_rssi_uint8()); rssi.read_receiver_rssi_uint8());
} }
void Plane::send_vfr_hud(mavlink_channel_t chan) float GCS_MAVLINK_Plane::vfr_hud_airspeed() const
{ {
float aspeed; // airspeed sensors are best. While the AHRS airspeed_estimate
if (airspeed.enabled()) { // will use an airspeed sensor, that value is constrained by the
aspeed = airspeed.get_airspeed(); // ground speed. When reporting we should send the true airspeed
} else if (!ahrs.airspeed_estimate(&aspeed)) { // value if possible:
aspeed = 0; if (plane.airspeed.enabled()) {
return plane.airspeed.get_airspeed();
} }
mavlink_msg_vfr_hud_send(
chan, // airspeed estimates are OK:
aspeed, float aspeed;
ahrs.groundspeed(), if (AP::ahrs().airspeed_estimate(&aspeed)) {
(ahrs.yaw_sensor / 100) % 360, return aspeed;
abs(throttle_percentage()), }
current_loc.alt / 100.0f,
(g2.soaring_controller.is_active() ? g2.soaring_controller.get_vario_reading() : barometer.get_climb_rate())); // lying is worst:
return 0;
}
int16_t GCS_MAVLINK_Plane::vfr_hud_throttle() const
{
return abs(plane.throttle_percentage());
}
float GCS_MAVLINK_Plane::vfr_hud_climbrate() const
{
if (plane.g2.soaring_controller.is_active()) {
return plane.g2.soaring_controller.get_vario_reading();
}
return AP::baro().get_climb_rate();
} }
/* /*
@ -418,11 +433,6 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
#endif #endif
break; break;
case MSG_VFR_HUD:
CHECK_PAYLOAD_SIZE(VFR_HUD);
plane.send_vfr_hud(chan);
break;
case MSG_FENCE_STATUS: case MSG_FENCE_STATUS:
#if GEOFENCE_ENABLED == ENABLED #if GEOFENCE_ENABLED == ENABLED
CHECK_PAYLOAD_SIZE(FENCE_STATUS); CHECK_PAYLOAD_SIZE(FENCE_STATUS);

5
ArduPlane/GCS_Mavlink.h
View File

@ -53,4 +53,9 @@ private:
MAV_STATE system_status() const override; MAV_STATE system_status() const override;
uint8_t radio_in_rssi() const; uint8_t radio_in_rssi() const;
float vfr_hud_airspeed() const override;
int16_t vfr_hud_throttle() const override;
float vfr_hud_climbrate() const override;
}; };

1
ArduPlane/Plane.h
View File

@ -773,7 +773,6 @@ private:
void send_extended_status1(mavlink_channel_t chan); void send_extended_status1(mavlink_channel_t chan);
void send_nav_controller_output(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan);
void send_servo_out(mavlink_channel_t chan); void send_servo_out(mavlink_channel_t chan);
void send_vfr_hud(mavlink_channel_t chan);
void send_wind(mavlink_channel_t chan); void send_wind(mavlink_channel_t chan);
void send_pid_info(const mavlink_channel_t chan, const DataFlash_Class::PID_Info *pid_info, const uint8_t axis, const float achieved); void send_pid_info(const mavlink_channel_t chan, const DataFlash_Class::PID_Info *pid_info, const uint8_t axis, const float achieved);
void send_pid_tuning(mavlink_channel_t chan); void send_pid_tuning(mavlink_channel_t chan);