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

Rover: Add support for HIGH_LATENCY2 messages

Browse Source
This commit is contained in:
Stephen Dade 2021-07-06 20:53:38 +10:00 committed by Andrew Tridgell
parent 2a1f6e1b31
commit 7f0bd06e8b
2 changed files with 57 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
Rover/GCS_Mavlink.cpp
View File

@ -1054,3 +1054,53 @@ uint64_t GCS_MAVLINK_Rover::capabilities() const
MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
GCS_MAVLINK::capabilities());
}
#if HAL_HIGH_LATENCY2_ENABLED
uint8_t GCS_MAVLINK_Rover::high_latency_tgt_heading() const
{
const Mode *control_mode = rover.control_mode;
if (rover.control_mode->is_autopilot_mode()) {
// need to convert -180->180 to 0->360/2
return wrap_360(control_mode->wp_bearing()) / 2;
}
return 0;
}
uint16_t GCS_MAVLINK_Rover::high_latency_tgt_dist() const
{
const Mode *control_mode = rover.control_mode;
if (rover.control_mode->is_autopilot_mode()) {
// return units are dm
return MIN((control_mode->get_distance_to_destination()) / 10, UINT16_MAX);
}
return 0;
}
uint8_t GCS_MAVLINK_Rover::high_latency_tgt_airspeed() const
{
const Mode *control_mode = rover.control_mode;
if (rover.control_mode->is_autopilot_mode()) {
// return units are m/s*5
return MIN((vfr_hud_airspeed() - control_mode->speed_error()) * 5, UINT8_MAX);
}
return 0;
}
uint8_t GCS_MAVLINK_Rover::high_latency_wind_speed() const
{
if (rover.g2.windvane.enabled()) {
// return units are m/s*5
return MIN(rover.g2.windvane.get_true_wind_speed() * 5, UINT8_MAX);
}
return 0;
}
uint8_t GCS_MAVLINK_Rover::high_latency_wind_direction() const
{
if (rover.g2.windvane.enabled()) {
// return units are deg/2
return wrap_360(degrees(rover.g2.windvane.get_true_wind_direction_rad())) / 2;
}
return 0;
}
#endif // HAL_HIGH_LATENCY2_ENABLED

7
Rover/GCS_Mavlink.h
View File

@ -53,4 +53,11 @@ private:
void send_rangefinder() const override;
#if HAL_HIGH_LATENCY2_ENABLED
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;
#endif // HAL_HIGH_LATENCY2_ENABLED
};