diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-05-09 17:13:38 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-05-09 17:13:38 +0200 |
commit | 3ec536a876a66f4e56549957e81ed4547c92a0c3 (patch) | |
tree | b0f318ae468e1aa710e24287425a282b2208d55c /src/drivers | |
parent | 5886e93a33329d39dc2ea535d224fcd60ab7afd7 (diff) | |
download | px4-firmware-3ec536a876a66f4e56549957e81ed4547c92a0c3.tar.gz px4-firmware-3ec536a876a66f4e56549957e81ed4547c92a0c3.tar.bz2 px4-firmware-3ec536a876a66f4e56549957e81ed4547c92a0c3.zip |
Improved GPS update rate calculation
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/gps/gps.cpp | 4 | ||||
-rw-r--r-- | src/drivers/gps/gps_helper.cpp | 11 | ||||
-rw-r--r-- | src/drivers/gps/gps_helper.h | 4 |
3 files changed, 15 insertions, 4 deletions
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; }; |