From 3ec536a876a66f4e56549957e81ed4547c92a0c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 May 2013 17:13:38 +0200 Subject: Improved GPS update rate calculation --- src/drivers/gps/gps.cpp | 4 ++-- src/drivers/gps/gps_helper.cpp | 11 +++++++++-- src/drivers/gps/gps_helper.h | 4 ++++ 3 files changed, 15 insertions(+), 4 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 7db257816..38835418b 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -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); diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 54ac90dab..ba86d370a 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -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) { diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index 7e456a6aa..defc1a074 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -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; }; -- cgit v1.2.3