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

Blimp: Add support for HIGH_LATENCY2 messages

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

28
Blimp/GCS_Mavlink.cpp
View File

@ -647,3 +647,31 @@ void GCS_MAVLINK_Blimp::send_wind() const
wind.length(),
wind.z);
}
#if HAL_HIGH_LATENCY2_ENABLED
uint8_t GCS_MAVLINK_Blimp::high_latency_wind_speed() const
{
Vector3f airspeed_vec_bf;
if (!AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) {
// if we don't have an airspeed estimate then we don't have a
// valid wind estimate on blimps
return 0;
}
// return units are m/s*5
const Vector3f wind = AP::ahrs().wind_estimate();
return wind.length() * 5;
}
uint8_t GCS_MAVLINK_Blimp::high_latency_wind_direction() const
{
Vector3f airspeed_vec_bf;
if (!AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) {
// if we don't have an airspeed estimate then we don't have a
// valid wind estimate on blimps
return 0;
}
const Vector3f wind = AP::ahrs().wind_estimate();
// need to convert -180->180 to 0->360/2
return wrap_360(degrees(atan2f(-wind.y, -wind.x))) / 2;
}
#endif // HAL_HIGH_LATENCY2_ENABLED

5
Blimp/GCS_Mavlink.h
View File

@ -76,4 +76,9 @@ private:
POSZ = 7,
POSYAW = 8,
};
#if HAL_HIGH_LATENCY2_ENABLED
uint8_t high_latency_wind_speed() const override;
uint8_t high_latency_wind_direction() const override;
#endif // HAL_HIGH_LATENCY2_ENABLED
};