AP_GPS: UBlox: add TIM-TM2 logging
This commit is contained in:
Iampete1
Andrew Tridgell
parent
0a0178988d
commit
2589fa1b3b
@ -392,6 +392,15 @@ AP_GPS_UBLOX::_request_next_config(void)
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case STEP_TIM_TM2:
|
||||
#if UBLOX_TIM_TM2_LOGGING
|
||||
if(!_request_message_rate(CLASS_TIM, MSG_TIM_TM2)) {
|
||||
_next_message--;
|
||||
}
|
||||
#else
|
||||
_unconfigured_messages &= ~CONFIG_TIM_TM2;
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
@ -470,6 +479,15 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
|
||||
}
|
||||
break;
|
||||
#endif // UBLOX_RXM_RAW_LOGGING
|
||||
#if UBLOX_TIM_TM2_LOGGING
|
||||
case CLASS_TIM:
|
||||
if (msg_id == MSG_TIM_TM2) {
|
||||
desired_rate = RATE_TIM_TM2;
|
||||
config_msg_id = CONFIG_TIM_TM2;
|
||||
break;
|
||||
}
|
||||
return;
|
||||
#endif // UBLOX_TIM_TM2_LOGGING
|
||||
default:
|
||||
return;
|
||||
}
|
||||
@ -723,6 +741,50 @@ void AP_GPS_UBLOX::log_mon_hw2(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
#if UBLOX_TIM_TM2_LOGGING
|
||||
void AP_GPS_UBLOX::log_tim_tm2(void)
|
||||
{
|
||||
#if HAL_LOGGING_ENABLED
|
||||
if (!should_log()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// @LoggerMessage: UBXT
|
||||
// @Description: uBlox specific UBX-TIM-TM2 logging, see uBlox interface description
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: I: GPS instance number
|
||||
// @Field: ch: Channel (i.e. EXTINT) upon which the pulse was measured
|
||||
// @Field: flags: Bitmask
|
||||
// @Field: count: Rising edge counter
|
||||
// @Field: wnR: Week number of last rising edge
|
||||
// @Field: MsR: Tow of rising edge
|
||||
// @Field: SubMsR: Millisecond fraction of tow of rising edge in nanoseconds
|
||||
// @Field: wnF: Week number of last falling edge
|
||||
// @Field: MsF: Tow of falling edge
|
||||
// @Field: SubMsF: Millisecond fraction of tow of falling edge in nanoseconds
|
||||
// @Field: accEst: Accuracy estimate
|
||||
|
||||
AP::logger().WriteStreaming("UBXT",
|
||||
"TimeUS,I,ch,flags,count,wnR,MsR,SubMsR,wnF,MsF,SubMsF,accEst",
|
||||
"s#----ss-sss",
|
||||
"F-----CI-CII",
|
||||
"QBBBHHIIHIII",
|
||||
AP_HAL::micros64(),
|
||||
state.instance,
|
||||
_buffer.tim_tm2.ch,
|
||||
_buffer.tim_tm2.flags,
|
||||
_buffer.tim_tm2.count,
|
||||
_buffer.tim_tm2.wnR,
|
||||
_buffer.tim_tm2.towMsR,
|
||||
_buffer.tim_tm2.towSubMsR,
|
||||
_buffer.tim_tm2.wnF,
|
||||
_buffer.tim_tm2.towMsF,
|
||||
_buffer.tim_tm2.towSubMsF,
|
||||
_buffer.tim_tm2.accEst);
|
||||
#endif
|
||||
}
|
||||
#endif // UBLOX_TIM_TM2_LOGGING
|
||||
|
||||
#if UBLOX_RXM_RAW_LOGGING
|
||||
void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
|
||||
{
|
||||
@ -1199,6 +1261,13 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
}
|
||||
#endif // UBLOX_RXM_RAW_LOGGING
|
||||
|
||||
#if UBLOX_TIM_TM2_LOGGING
|
||||
if ((_class == CLASS_TIM) && (_msg_id == MSG_TIM_TM2) && (_payload_length == 28)) {
|
||||
log_tim_tm2();
|
||||
return false;
|
||||
}
|
||||
#endif // UBLOX_TIM_TM2_LOGGING
|
||||
|
||||
if (_class != CLASS_NAV) {
|
||||
unexpected_message();
|
||||
return false;
|
||||
@ -1828,7 +1897,8 @@ static const char *reasons[] = {"navigation rate",
|
||||
"time pulse settings",
|
||||
"TIMEGPS rate",
|
||||
"Time mode settings",
|
||||
"RTK MB"};
|
||||
"RTK MB",
|
||||
"TIM TM2"};
|
||||
|
||||
static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");
|
||||
|
||||
|
||||
@ -49,6 +49,9 @@
|
||||
#define UBLOX_MAX_RXM_RAW_SATS 22
|
||||
#define UBLOX_MAX_RXM_RAWX_SATS 32
|
||||
#define UBLOX_GNSS_SETTINGS 1
|
||||
#ifndef UBLOX_TIM_TM2_LOGGING
|
||||
#define UBLOX_TIM_TM2_LOGGING (BOARD_FLASH_SIZE>1024)
|
||||
#endif
|
||||
|
||||
#define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
|
||||
|
||||
@ -65,6 +68,7 @@
|
||||
#define RATE_DOP 1
|
||||
#define RATE_HW 5
|
||||
#define RATE_HW2 5
|
||||
#define RATE_TIM_TM2 1
|
||||
|
||||
#define CONFIG_RATE_NAV (1<<0)
|
||||
#define CONFIG_RATE_POSLLH (1<<1)
|
||||
@ -84,7 +88,8 @@
|
||||
#define CONFIG_RATE_TIMEGPS (1<<15)
|
||||
#define CONFIG_TMODE_MODE (1<<16)
|
||||
#define CONFIG_RTK_MOVBASE (1<<17)
|
||||
#define CONFIG_LAST (1<<18) // this must always be the last bit
|
||||
#define CONFIG_TIM_TM2 (1<<18)
|
||||
#define CONFIG_LAST (1<<19) // this must always be the last bit
|
||||
|
||||
#define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)
|
||||
|
||||
@ -516,6 +521,19 @@ private:
|
||||
uint32_t loadMask;
|
||||
};
|
||||
|
||||
struct PACKED ubx_tim_tm2 {
|
||||
uint8_t ch;
|
||||
uint8_t flags;
|
||||
uint16_t count;
|
||||
uint16_t wnR;
|
||||
uint16_t wnF;
|
||||
uint32_t towMsR;
|
||||
uint32_t towSubMsR;
|
||||
uint32_t towMsF;
|
||||
uint32_t towSubMsF;
|
||||
uint32_t accEst;
|
||||
};
|
||||
|
||||
// Receive buffer
|
||||
union PACKED {
|
||||
DEFINE_BYTE_ARRAY_METHODS
|
||||
@ -548,6 +566,7 @@ private:
|
||||
ubx_rxm_rawx rxm_rawx;
|
||||
#endif
|
||||
ubx_ack_ack ack;
|
||||
ubx_tim_tm2 tim_tm2;
|
||||
} _buffer;
|
||||
|
||||
enum class RELPOSNED {
|
||||
@ -573,6 +592,7 @@ private:
|
||||
CLASS_CFG = 0x06,
|
||||
CLASS_MON = 0x0A,
|
||||
CLASS_RXM = 0x02,
|
||||
CLASS_TIM = 0x0d,
|
||||
MSG_ACK_NACK = 0x00,
|
||||
MSG_ACK_ACK = 0x01,
|
||||
MSG_POSLLH = 0x2,
|
||||
@ -598,7 +618,8 @@ private:
|
||||
MSG_MON_VER = 0x04,
|
||||
MSG_NAV_SVINFO = 0x30,
|
||||
MSG_RXM_RAW = 0x10,
|
||||
MSG_RXM_RAWX = 0x15
|
||||
MSG_RXM_RAWX = 0x15,
|
||||
MSG_TIM_TM2 = 0x03
|
||||
};
|
||||
enum ubx_gnss_identifier {
|
||||
GNSS_GPS = 0x00,
|
||||
@ -655,6 +676,7 @@ private:
|
||||
STEP_RAWX,
|
||||
STEP_VERSION,
|
||||
STEP_RTK_MOVBASE, // setup moving baseline
|
||||
STEP_TIM_TM2,
|
||||
STEP_LAST
|
||||
};
|
||||
|
||||
@ -728,6 +750,7 @@ private:
|
||||
void unexpected_message(void);
|
||||
void log_mon_hw(void);
|
||||
void log_mon_hw2(void);
|
||||
void log_tim_tm2(void);
|
||||
void log_rxm_raw(const struct ubx_rxm_raw &raw);
|
||||
void log_rxm_rawx(const struct ubx_rxm_rawx &raw);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user