dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

gps: keep track of RX data rate

This commit is contained in:
Beat Küng 2020-06-17 15:15:23 +02:00 committed by Daniel Agar
parent c5c521f5e0
commit ab43a94224
1 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/drivers/gps/gps.cpp
View File

@ -170,6 +170,8 @@ private:
float _rate{0.0f}; ///< position update rate float _rate{0.0f}; ///< position update rate
float _rate_rtcm_injection{0.0f}; ///< RTCM message injection rate float _rate_rtcm_injection{0.0f}; ///< RTCM message injection rate
unsigned _last_rate_rtcm_injection_count{0}; ///< counter for number of RTCM messages unsigned _last_rate_rtcm_injection_count{0}; ///< counter for number of RTCM messages
unsigned _num_bytes_read{0}; ///< counter for number of read bytes from the UART (within update interval)
unsigned _rate_reading{0}; ///< reading rate in B/s
const bool _fake_gps; ///< fake gps output const bool _fake_gps; ///< fake gps output
@ -392,6 +394,10 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
ret = ::read(_serial_fd, buf, buf_length); ret = ::read(_serial_fd, buf, buf_length);
if (ret > 0) {
_num_bytes_read += ret;
}
} else { } else {
ret = -1; ret = -1;
} }
@ -470,6 +476,12 @@ int GPS::setBaudrate(unsigned baud)
case 230400: speed = B230400; break; case 230400: speed = B230400; break;
#ifndef B460800
#define B460800 460800
#endif
case 460800: speed = B460800; break;
default: default:
PX4_ERR("ERR: unknown baudrate: %d", baud); PX4_ERR("ERR: unknown baudrate: %d", baud);
return -EINVAL; return -EINVAL;
@ -549,6 +561,10 @@ void GPS::initializeCommunicationDump()
return; //dumping disabled return; //dumping disabled
} }
if (_instance != Instance::Main) {
return;
}
_dump_from_device = new gps_dump_s(); _dump_from_device = new gps_dump_s();
_dump_to_device = new gps_dump_s(); _dump_to_device = new gps_dump_s();
@ -773,9 +789,11 @@ GPS::run()
float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
_rate = last_rate_count / dt; _rate = last_rate_count / dt;
_rate_rtcm_injection = _last_rate_rtcm_injection_count / dt; _rate_rtcm_injection = _last_rate_rtcm_injection_count / dt;
_rate_reading = _num_bytes_read / dt;
last_rate_measurement = hrt_absolute_time(); last_rate_measurement = hrt_absolute_time();
last_rate_count = 0; last_rate_count = 0;
_last_rate_rtcm_injection_count = 0; _last_rate_rtcm_injection_count = 0;
_num_bytes_read = 0;
_helper->storeUpdateRates(); _helper->storeUpdateRates();
_helper->resetUpdateRates(); _helper->resetUpdateRates();
} }
@ -901,6 +919,7 @@ GPS::print_status()
PX4_INFO("status: %s, port: %s, baudrate: %d", _healthy ? "OK" : "NOT OK", _port, _baudrate); PX4_INFO("status: %s, port: %s, baudrate: %d", _healthy ? "OK" : "NOT OK", _port, _baudrate);
PX4_INFO("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled"); PX4_INFO("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
PX4_INFO("rate reading: \t\t%6i B/s", _rate_reading);
if (_report_gps_pos.timestamp != 0) { if (_report_gps_pos.timestamp != 0) {
if (_helper) { if (_helper) {