forked from Archive/PX4-Autopilot
Improved GPS update rate calculation
This commit is contained in:
parent
5886e93a33
commit
3ec536a876
|
@ -288,7 +288,6 @@ GPS::task_main()
|
|||
|
||||
// GPS is obviously detected successfully, reset statistics
|
||||
_Helper->reset_update_rates();
|
||||
warnx("module configuration successful");
|
||||
|
||||
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
|
||||
// lock();
|
||||
|
@ -306,6 +305,8 @@ GPS::task_main()
|
|||
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
|
||||
last_rate_measurement = hrt_absolute_time();
|
||||
last_rate_count = 0;
|
||||
_Helper->store_update_rates();
|
||||
_Helper->reset_update_rates();
|
||||
}
|
||||
|
||||
if (!_healthy) {
|
||||
|
@ -381,7 +382,6 @@ GPS::print_info()
|
|||
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
|
||||
warnx("rate publication:\t%6.2f Hz", (double)_rate);
|
||||
|
||||
_Helper->reset_update_rates();
|
||||
}
|
||||
|
||||
usleep(100000);
|
||||
|
|
|
@ -46,13 +46,13 @@
|
|||
float
|
||||
GPS_Helper::get_position_update_rate()
|
||||
{
|
||||
return _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
|
||||
return _rate_lat_lon;
|
||||
}
|
||||
|
||||
float
|
||||
GPS_Helper::get_velocity_update_rate()
|
||||
{
|
||||
return _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
|
||||
return _rate_vel;
|
||||
}
|
||||
|
||||
float
|
||||
|
@ -63,6 +63,13 @@ GPS_Helper::reset_update_rates()
|
|||
_interval_rate_start = hrt_absolute_time();
|
||||
}
|
||||
|
||||
float
|
||||
GPS_Helper::store_update_rates()
|
||||
{
|
||||
_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
|
||||
_rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
|
||||
}
|
||||
|
||||
int
|
||||
GPS_Helper::set_baudrate(const int &fd, unsigned baud)
|
||||
{
|
||||
|
|
|
@ -52,11 +52,15 @@ public:
|
|||
float get_position_update_rate();
|
||||
float get_velocity_update_rate();
|
||||
float reset_update_rates();
|
||||
float store_update_rates();
|
||||
|
||||
protected:
|
||||
uint8_t _rate_count_lat_lon;
|
||||
uint8_t _rate_count_vel;
|
||||
|
||||
float _rate_lat_lon;
|
||||
float _rate_vel;
|
||||
|
||||
uint64_t _interval_rate_start;
|
||||
};
|
||||
|
||||
|
|
Loading…
Reference in New Issue