fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 16:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mavlink: Use filtered update rate for HIGH_LATENCY2 stream instead of main loop delay

Browse Source
This commit is contained in:
acfloria
2018-04-11 15:15:28 +02:00
committed by Beat Küng
parent e9dde721f7
commit 2a5c7efd8d
1 changed files with 18 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+18 -9
View File
@@ -4037,6 +4037,9 @@ private:
SimpleAnalyzer _windspeed; SimpleAnalyzer _windspeed;
hrt_abstime _last_reset_time = 0; hrt_abstime _last_reset_time = 0;
hrt_abstime _last_update_time = 0;
float _update_rate_filtered = 0.0f;
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &); MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &);
@@ -4173,7 +4176,7 @@ protected:
// reset the analysers every 60 seconds if nothing should be transmitted // reset the analysers every 60 seconds if nothing should be transmitted
if ((t - _last_reset_time) > 60000000) { if ((t - _last_reset_time) > 60000000) {
reset_analysers(t); reset_analysers(t);
PX4_INFO("Resetting HIGH_LATENCY2 analysers"); PX4_DEBUG("Resetting HIGH_LATENCY2 analysers");
} }
return false; return false;
@@ -4429,21 +4432,27 @@ protected:
void update_data() void update_data()
{ {
const float update_rate = 1.0f / (_mavlink->get_main_loop_delay() * 1e-6f); const hrt_abstime t = hrt_absolute_time();
update_airspeed(update_rate); if (t > _last_update_time) {
// first order low pass filter for the update rate
_update_rate_filtered = 0.97f * _update_rate_filtered + 0.03f / ((t - _last_update_time) * 1e-6f);
_last_update_time = t;
}
update_tecs_status(update_rate); update_airspeed(_update_rate_filtered);
update_battery_status(update_rate); update_tecs_status(_update_rate_filtered);
update_global_position(update_rate); update_battery_status(_update_rate_filtered);
update_gps(update_rate); update_global_position(_update_rate_filtered);
update_vehicle_status(update_rate); update_gps(_update_rate_filtered);
update_wind_estimate(update_rate); update_vehicle_status(_update_rate_filtered);
update_wind_estimate(_update_rate_filtered);
} }
void update_airspeed(float update_rate) void update_airspeed(float update_rate)