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

AP_GPS: moved iTow handling to GPS_Backend

Browse Source
This commit is contained in:
Andrew Tridgell 2018-06-20 08:35:41 +10:00
parent 4af857da44
commit df68d6413c
5 changed files with 21 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -284,7 +284,7 @@ AP_GPS_SBF::process_message(void)
state.time_week_ms = (uint32_t)(temp.TOW);
}
set_uart_timestamp(sbf_msg.length);
check_new_itow(temp.TOW, sbf_msg.length);
state.last_gps_time_ms = AP_HAL::millis();
// Update velocity state (don't use −2·10^10)
@ -361,6 +361,7 @@ AP_GPS_SBF::process_message(void)
case DOP:
{
const msg4001 &temp = sbf_msg.data.msg4001u;
check_new_itow(temp.TOW, sbf_msg.length);
state.hdop = temp.HDOP;
state.vdop = temp.VDOP;
@ -369,6 +370,7 @@ AP_GPS_SBF::process_message(void)
case ReceiverStatus:
{
const msg4014 &temp = sbf_msg.data.msg4014u;
check_new_itow(temp.TOW, sbf_msg.length);
RxState = temp.RxState;
if ((RxError & RX_ERROR_MASK) != (temp.RxError & RX_ERROR_MASK)) {
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF error changed (0x%08x/0x%08x)", state.instance + 1,
@ -381,6 +383,7 @@ AP_GPS_SBF::process_message(void)
{
const msg5908 &temp = sbf_msg.data.msg5908u;
check_new_itow(temp.TOW, sbf_msg.length);
// select the maximum variance, as the EKF will apply it to all the columns in it's estimate
// FIXME: Support returning the covariance matrix to the EKF
float max_variance_squared = MAX(temp.Cov_VnVn, MAX(temp.Cov_VeVe, temp.Cov_VuVu));

6
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1336,10 +1336,8 @@ void AP_GPS_UBLOX::Write_DataFlash_Log_Startup_messages() const
}
}
// uBlox specific check_new_itow(), handling message length
void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
{
if (itow != _last_itow) {
_last_itow = itow;
set_uart_timestamp(_payload_length + sizeof(ubx_header) + 2);
}
check_new_itow(itow, _payload_length + sizeof(ubx_header) + 2);
}

3
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -549,9 +549,6 @@ private:
bool havePvtMsg;
// itow from previous message
uint32_t _last_itow;
bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void _configure_rate(void);
void _configure_sbas(bool enable);

9
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -227,3 +227,12 @@ void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes)
state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U;
}
}
void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
{
if (itow != _last_itow) {
_last_itow = itow;
set_uart_timestamp(msg_length);
}
}

6
libraries/AP_GPS/GPS_Backend.h
View File

@ -93,4 +93,10 @@ protected:
assuming the message started nbytes ago
*/
void set_uart_timestamp(uint16_t nbytes);
void check_new_itow(uint32_t itow, uint32_t msg_length);
private:
// itow from previous message
uint32_t _last_itow;
};