Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: Add support for HIGH_LATENCY2 messages

This commit is contained in:
Stephen Dade 2021-07-06 20:30:59 +10:00 committed by Andrew Tridgell
parent 6c02cd1b54
commit d6a123b58b
2 changed files with 89 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
ArduPlane/GCS_Mavlink.cpp
View File

@ -1331,3 +1331,82 @@ uint64_t GCS_MAVLINK_Plane::capabilities() const
#endif
GCS_MAVLINK::capabilities());
}
#if HAL_HIGH_LATENCY2_ENABLED
int16_t GCS_MAVLINK_Plane::high_latency_target_altitude() const
{
AP_AHRS &ahrs = AP::ahrs();
struct Location global_position_current;
UNUSED_RESULT(ahrs.get_position(global_position_current));
const QuadPlane &quadplane = plane.quadplane;
//return units are m
if (quadplane.show_vtol_view()) {
return (plane.control_mode != &plane.mode_qstabilize) ? 0.01 * (global_position_current.alt + quadplane.pos_control->get_pos_error_z_cm()) : 0;
} else {
return 0.01 * (global_position_current.alt + plane.altitude_error_cm);
}
}
uint8_t GCS_MAVLINK_Plane::high_latency_tgt_heading() const
{
// return units are deg/2
const QuadPlane &quadplane = plane.quadplane;
if (quadplane.show_vtol_view()) {
const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
return ((uint16_t)(targets.z * 0.01)) / 2;
} else {
const AP_Navigation *nav_controller = plane.nav_controller;
// need to convert -18000->18000 to 0->360/2
return wrap_360_cd(nav_controller->target_bearing_cd() ) / 200;
}
}
uint16_t GCS_MAVLINK_Plane::high_latency_tgt_dist() const
{
// return units are dm
const QuadPlane &quadplane = plane.quadplane;
if (quadplane.show_vtol_view()) {
bool wp_nav_valid = quadplane.using_wp_nav();
return (wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination(), UINT16_MAX) : 0) / 10;
} else {
return MIN(plane.auto_state.wp_distance, UINT16_MAX) / 10;
}
}
uint8_t GCS_MAVLINK_Plane::high_latency_tgt_airspeed() const
{
// return units are m/s*5
return plane.target_airspeed_cm * 0.05;
}
uint8_t GCS_MAVLINK_Plane::high_latency_wind_speed() const
{
Vector3f wind;
wind = AP::ahrs().wind_estimate();
// return units are m/s*5
return MIN(wind.length() * 5, UINT8_MAX);
}
uint8_t GCS_MAVLINK_Plane::high_latency_wind_direction() const
{
const Vector3f wind = AP::ahrs().wind_estimate();
// return units are deg/2
// need to convert -180->180 to 0->360/2
return wrap_360(degrees(atan2f(-wind.y, -wind.x))) / 2;
}
int8_t GCS_MAVLINK_Plane::high_latency_air_temperature() const
{
// return units are degC
AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
float air_temperature = 0;
if (airspeed != nullptr &&
airspeed->enabled()) {
airspeed->get_temperature(air_temperature);
}
return air_temperature;
}
#endif // HAL_HIGH_LATENCY2_ENABLED

9
ArduPlane/GCS_Mavlink.h
View File

@ -64,4 +64,13 @@ private:
int16_t vfr_hud_throttle() const override;
float vfr_hud_climbrate() const override;
#if HAL_HIGH_LATENCY2_ENABLED
int16_t high_latency_target_altitude() const override;
uint8_t high_latency_tgt_heading() const override;
uint16_t high_latency_tgt_dist() const override;
uint8_t high_latency_tgt_airspeed() const override;
uint8_t high_latency_wind_speed() const override;
uint8_t high_latency_wind_direction() const override;
int8_t high_latency_air_temperature() const override;
#endif // HAL_HIGH_LATENCY2_ENABLED
};