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

ArduSub: Add support for HIGH_LATENCY2 messages

This commit is contained in:
Stephen Dade 2021-07-06 20:31:04 +10:00 committed by Andrew Tridgell
parent d6a123b58b
commit 2a1f6e1b31
2 changed files with 50 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduSub/GCS_Mavlink.cpp
View File

@ -799,3 +799,47 @@ int32_t GCS_MAVLINK_Sub::global_position_int_relative_alt() const {
}
return GCS_MAVLINK::global_position_int_relative_alt();
}
#if HAL_HIGH_LATENCY2_ENABLED
int16_t GCS_MAVLINK_Sub::high_latency_target_altitude() const
{
AP_AHRS &ahrs = AP::ahrs();
struct Location global_position_current;
UNUSED_RESULT(ahrs.get_position(global_position_current));
//return units are m
if (sub.control_mode == AUTO || sub.control_mode == GUIDED) {
return 0.01 * (global_position_current.alt + sub.pos_control.get_pos_error_z_cm());
}
return 0;
}
uint8_t GCS_MAVLINK_Sub::high_latency_tgt_heading() const
{
// return units are deg/2
if (sub.control_mode == AUTO || sub.control_mode == GUIDED) {
// need to convert -18000->18000 to 0->360/2
return wrap_360_cd(sub.wp_nav.get_wp_bearing_to_destination()) / 200;
}
return 0;
}
uint16_t GCS_MAVLINK_Sub::high_latency_tgt_dist() const
{
// return units are dm
if (sub.control_mode == AUTO || sub.control_mode == GUIDED) {
return MIN(sub.wp_nav.get_wp_distance_to_destination() * 0.001, UINT16_MAX);
}
return 0;
}
uint8_t GCS_MAVLINK_Sub::high_latency_tgt_airspeed() const
{
// return units are m/s*5
if (sub.control_mode == AUTO || sub.control_mode == GUIDED) {
return MIN((sub.pos_control.get_vel_desired_cms().length()/100) * 5, UINT8_MAX);
}
return 0;
}
#endif // HAL_HIGH_LATENCY2_ENABLED

6
ArduSub/GCS_Mavlink.h
View File

@ -53,4 +53,10 @@ private:
int16_t vfr_hud_throttle() 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;
#endif // HAL_HIGH_LATENCY2_ENABLED
};