Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-29 20:18:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: sync pps rate with message rate

Browse Source
This commit is contained in:
bugobliterator 2022-01-18 17:07:15 +05:30 committed by Andrew Tridgell
parent 9469817e03
commit 46294c0fba
4 changed files with 16 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1059,7 +1059,8 @@ AP_GPS_UBLOX::_parse_gps(void)
hal.gpio->attach_interrupt(HAL_GPIO_PPS, FUNCTOR_BIND_MEMBER(&AP_GPS_UBLOX::pps_interrupt, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_FALLING); hal.gpio->attach_interrupt(HAL_GPIO_PPS, FUNCTOR_BIND_MEMBER(&AP_GPS_UBLOX::pps_interrupt, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_FALLING);
#endif #endif
const uint16_t desired_flags = 0x003f; const uint16_t desired_flags = 0x003f;
const uint16_t desired_period_hz = 1; const uint16_t desired_period_hz = _pps_freq;
if (_buffer.nav_tp5.flags != desired_flags || if (_buffer.nav_tp5.flags != desired_flags ||
_buffer.nav_tp5.freqPeriod != desired_period_hz) { _buffer.nav_tp5.freqPeriod != desired_period_hz) {
_buffer.nav_tp5.tpIdx = 0; _buffer.nav_tp5.tpIdx = 0;
@ -1543,6 +1544,13 @@ AP_GPS_UBLOX::pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us)
{ {
_last_pps_time_us = timestamp_us; _last_pps_time_us = timestamp_us;
} }
void
AP_GPS_UBLOX::set_pps_desired_freq(uint8_t freq)
{
_pps_freq = freq;
_unconfigured_messages |= CONFIG_TP5;
}
#endif #endif

2
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -759,8 +759,10 @@ private:
// return true if GPS is capable of F9 config // return true if GPS is capable of F9 config
bool supports_F9_config(void) const; bool supports_F9_config(void) const;
uint8_t _pps_freq = 1;
#ifdef HAL_GPIO_PPS #ifdef HAL_GPIO_PPS
void pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us); void pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us);
void set_pps_desired_freq(uint8_t freq) override;
#endif #endif
#if GPS_MOVING_BASELINE #if GPS_MOVING_BASELINE

3
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -277,6 +277,9 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
} else { } else {
_rate_counter = 0; _rate_counter = 0;
_last_rate_ms = dt_ms; _last_rate_ms = dt_ms;
if (_rate_ms != 0) {
set_pps_desired_freq(1000/_rate_ms);
}
} }
if (_rate_ms == 0) { if (_rate_ms == 0) {
// only allow 5Hz to 20Hz in user config // only allow 5Hz to 20Hz in user config

2
libraries/AP_GPS/GPS_Backend.h
View File

@ -150,6 +150,8 @@ protected:
return gps.get_type(state.instance); return gps.get_type(state.instance);
} }
virtual void set_pps_desired_freq(uint8_t freq) {}
#if AP_GPS_DEBUG_LOGGING_ENABLED #if AP_GPS_DEBUG_LOGGING_ENABLED
// log some data for debugging // log some data for debugging
void log_data(const uint8_t *data, uint16_t length); void log_data(const uint8_t *data, uint16_t length);