aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: optionally configure a hardware PPS pin for uBlox

Browse Source 
useful for testing time lag estimation
This commit is contained in:
Andrew Tridgell 2018-06-22 15:25:27 +10:00
parent 2eae4901c1
commit d761b24b92
2 changed files with 67 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -34,6 +34,7 @@
#define UBLOX_DEBUGGING 0
#define UBLOX_FAKE_3DLOCK 0
#define CONFIGURE_PPS_PIN 0
extern const AP_HAL::HAL& hal;
@ -57,6 +58,10 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
// start the process of updating the GPS rates
_request_next_config();
#if CONFIGURE_PPS_PIN
_unconfigured_messages |= CONFIG_TP5;
#endif
}
void
@ -73,7 +78,7 @@ AP_GPS_UBLOX::_request_next_config(void)
return;
}
Debug("Unconfigured messages: %d Current message: %d\n", _unconfigured_messages, _next_message);
Debug("Unconfigured messages: %u Current message: %u\n", (unsigned)_unconfigured_messages, (unsigned)_next_message);
// check AP_GPS_UBLOX.h for the enum that controls the order.
// This switch statement isn't maintained against the enum in order to reduce code churn
@ -111,6 +116,13 @@ AP_GPS_UBLOX::_request_next_config(void)
_next_message--;
}
break;
case STEP_POLL_TP5:
#if CONFIGURE_PPS_PIN
if (!_send_message(CLASS_CFG, MSG_CFG_TP5, nullptr, 0)) {
_next_message--;
}
#endif
break;
case STEP_NAV_RATE:
if (!_send_message(CLASS_CFG, MSG_CFG_RATE, nullptr, 0)) {
_next_message--;
@ -622,6 +634,9 @@ AP_GPS_UBLOX::_parse_gps(void)
case MSG_CFG_SBAS:
_unconfigured_messages &= ~CONFIG_SBAS;
break;
case MSG_CFG_TP5:
_unconfigured_messages &= ~CONFIG_TP5;
break;
}
break;
case CLASS_MON:
@ -779,6 +794,40 @@ AP_GPS_UBLOX::_parse_gps(void)
_unconfigured_messages &= ~CONFIG_RATE_NAV;
}
return false;
#if CONFIGURE_PPS_PIN
case MSG_CFG_TP5: {
// configure the PPS pin for 1Hz, zero delay
Debug("Got TP5 ver=%u 0x%04x %u\n",
(unsigned)_buffer.nav_tp5.version,
(unsigned)_buffer.nav_tp5.flags,
(unsigned)_buffer.nav_tp5.freqPeriod);
const uint16_t desired_flags = 0x003f;
const uint16_t desired_period_hz = 1;
if (_buffer.nav_tp5.flags != desired_flags ||
_buffer.nav_tp5.freqPeriod != desired_period_hz) {
_buffer.nav_tp5.tpIdx = 0;
_buffer.nav_tp5.reserved1[0] = 0;
_buffer.nav_tp5.reserved1[1] = 0;
_buffer.nav_tp5.antCableDelay = 0;
_buffer.nav_tp5.rfGroupDelay = 0;
_buffer.nav_tp5.freqPeriod = desired_period_hz;
_buffer.nav_tp5.freqPeriodLock = desired_period_hz;
_buffer.nav_tp5.pulseLenRatio = 1;
_buffer.nav_tp5.pulseLenRatioLock = 2;
_buffer.nav_tp5.userConfigDelay = 0;
_buffer.nav_tp5.flags = desired_flags;
_send_message(CLASS_CFG, MSG_CFG_TP5,
&_buffer.nav_tp5,
sizeof(_buffer.nav_tp5));
_unconfigured_messages |= CONFIG_TP5;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_TP5;
}
return false;
}
#endif // CONFIGURE_PPS_PIN
}
}

17
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -72,6 +72,7 @@
#define CONFIG_GNSS (1<<11)
#define CONFIG_SBAS (1<<12)
#define CONFIG_RATE_PVT (1<<13)
#define CONFIG_TP5 (1<<14)
#define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)
@ -182,6 +183,19 @@ private:
uint32_t res3;
uint32_t res4;
};
struct PACKED ubx_cfg_tp5 {
uint8_t tpIdx;
uint8_t version;
uint8_t reserved1[2];
int16_t antCableDelay;
int16_t rfGroupDelay;
uint32_t freqPeriod;
uint32_t freqPeriodLock;
uint32_t pulseLenRatio;
uint32_t pulseLenRatioLock;
int32_t userConfigDelay;
uint32_t flags;
};
struct PACKED ubx_cfg_prt {
uint8_t portID;
};
@ -409,6 +423,7 @@ private:
ubx_mon_hw_68 mon_hw_68;
ubx_mon_hw2 mon_hw2;
ubx_mon_ver mon_ver;
ubx_cfg_tp5 nav_tp5;
#if UBLOX_GNSS_SETTINGS
ubx_cfg_gnss gnss;
#endif
@ -444,6 +459,7 @@ private:
MSG_CFG_PRT = 0x00,
MSG_CFG_SBAS = 0x16,
MSG_CFG_GNSS = 0x3E,
MSG_CFG_TP5 = 0x31,
MSG_MON_HW = 0x09,
MSG_MON_HW2 = 0x0B,
MSG_MON_VER = 0x04,
@ -494,6 +510,7 @@ private:
STEP_POLL_SBAS, // poll SBAS
STEP_POLL_NAV, // poll NAV settings
STEP_POLL_GNSS, // poll GNSS
STEP_POLL_TP5, // poll TP5
STEP_DOP,
STEP_MON_HW,
STEP_MON_HW2,