Improved GPS update rate calculation

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;
 };