Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: Add support for HIGH_LATENCY2 messages

Browse Source
This commit is contained in:
Stephen Dade 2021-07-06 20:30:25 +10:00 committed by Andrew Tridgell
parent 035f65fe03
commit 6c02cd1b54
2 changed files with 80 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
ArduCopter/GCS_Mavlink.cpp
View File

@ -1374,3 +1374,74 @@ void GCS_MAVLINK_Copter::send_wind() const
wind.length(), wind.length(),
wind.z); wind.z);
} }
#if HAL_HIGH_LATENCY2_ENABLED
int16_t GCS_MAVLINK_Copter::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 (copter.ap.initialised) {
return 0.01 * (global_position_current.alt + copter.pos_control->get_pos_error_z_cm());
}
return 0;
}
uint8_t GCS_MAVLINK_Copter::high_latency_tgt_heading() const
{
if (copter.ap.initialised) {
// return units are deg/2
const Mode *flightmode = copter.flightmode;
// need to convert -18000->18000 to 0->360/2
return wrap_360_cd(flightmode->wp_bearing()) / 200;
}
return 0;
}
uint16_t GCS_MAVLINK_Copter::high_latency_tgt_dist() const
{
if (copter.ap.initialised) {
// return units are dm
const Mode *flightmode = copter.flightmode;
return MIN(flightmode->wp_distance() * 1.0e-2, UINT16_MAX) / 10;
}
return 0;
}
uint8_t GCS_MAVLINK_Copter::high_latency_tgt_airspeed() const
{
if (copter.ap.initialised) {
// return units are m/s*5
return MIN(copter.pos_control->get_vel_target_cms().length() * 5.0e-2, UINT8_MAX);
}
return 0;
}
uint8_t GCS_MAVLINK_Copter::high_latency_wind_speed() const
{
Vector3f airspeed_vec_bf;
Vector3f wind;
// return units are m/s*5
if (AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) {
wind = AP::ahrs().wind_estimate();
return wind.length() * 5;
}
return 0;
}
uint8_t GCS_MAVLINK_Copter::high_latency_wind_direction() const
{
Vector3f airspeed_vec_bf;
Vector3f wind;
// return units are deg/2
if (AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) {
wind = AP::ahrs().wind_estimate();
// need to convert -180->180 to 0->360/2
return wrap_360(degrees(atan2f(-wind.y, -wind.x))) / 2;
}
return 0;
}
#endif // HAL_HIGH_LATENCY2_ENABLED

9
ArduCopter/GCS_Mavlink.h
View File

@ -68,4 +68,13 @@ private:
void send_winch_status() const override; void send_winch_status() const override;
void send_wind() const; void send_wind() const;
#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;
#endif // HAL_HIGH_LATENCY2_ENABLED
}; };