mirror of https://github.com/ArduPilot/ardupilot
AP_GPS: added logging of RXM_RAW messages for UBlox
this will allow for post-flight RTK processing of DF logs
This commit is contained in:
parent
f8e09da50e
commit
ee85d37ee2
|
@ -98,6 +98,14 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
|
||||||
AP_GROUPINFO("SBP_LOGMASK", 8, AP_GPS, _sbp_logmask, 0xFF00),
|
AP_GROUPINFO("SBP_LOGMASK", 8, AP_GPS, _sbp_logmask, 0xFF00),
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if GPS_RTK_AVAILABLE
|
||||||
|
// @Param: RXM_RAW
|
||||||
|
// @DisplayName: Raw data logging
|
||||||
|
// @Description: Enable logging of RXM raw data from uBlox which includes carrier phase and pseudo range information. This allows for post processing of dataflash logs for more precise positioning. Note that this requires a raw capable uBlox such as the 6P or 6T.
|
||||||
|
// @Values: 0:Disabled,1:log at 1MHz,5:log at 5MHz.
|
||||||
|
AP_GROUPINFO("RAW_DATA", 9, AP_GPS, _raw_data, 0),
|
||||||
|
#endif
|
||||||
|
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -333,6 +333,7 @@ public:
|
||||||
#endif
|
#endif
|
||||||
AP_Int8 _sbas_mode;
|
AP_Int8 _sbas_mode;
|
||||||
AP_Int8 _min_elevation;
|
AP_Int8 _min_elevation;
|
||||||
|
AP_Int8 _raw_data;
|
||||||
|
|
||||||
// handle sending of initialisation strings to the GPS
|
// handle sending of initialisation strings to the GPS
|
||||||
void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size);
|
void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size);
|
||||||
|
|
|
@ -47,8 +47,10 @@ extern const AP_HAL::HAL& hal;
|
||||||
*/
|
*/
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||||
#define UBLOX_HW_LOGGING 1
|
#define UBLOX_HW_LOGGING 1
|
||||||
|
#define UBLOX_RXM_RAW_LOGGING 1
|
||||||
#else
|
#else
|
||||||
#define UBLOX_HW_LOGGING 0
|
#define UBLOX_HW_LOGGING 0
|
||||||
|
#define UBLOX_RXM_RAW_LOGGING 0
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
|
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
|
||||||
|
@ -121,6 +123,11 @@ AP_GPS_UBLOX::send_next_rate_update(void)
|
||||||
case 7:
|
case 7:
|
||||||
_request_version();
|
_request_version();
|
||||||
break;
|
break;
|
||||||
|
#endif
|
||||||
|
#if UBLOX_RXM_RAW_LOGGING
|
||||||
|
case 8:
|
||||||
|
_configure_message_rate(CLASS_RXM, MSG_RXM_RAW, gps._raw_data); // 24*16+8 bytes
|
||||||
|
break;
|
||||||
#endif
|
#endif
|
||||||
default:
|
default:
|
||||||
need_rate_update = false;
|
need_rate_update = false;
|
||||||
|
@ -316,6 +323,33 @@ void AP_GPS_UBLOX::log_accuracy(void) {
|
||||||
}
|
}
|
||||||
#endif // UBLOX_HW_LOGGING
|
#endif // UBLOX_HW_LOGGING
|
||||||
|
|
||||||
|
#if UBLOX_RXM_RAW_LOGGING
|
||||||
|
void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
|
||||||
|
{
|
||||||
|
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
uint32_t now = hal.scheduler->millis();
|
||||||
|
for (uint8_t i=0; i<raw.numSV; i++) {
|
||||||
|
struct log_GPS_RAW pkt = {
|
||||||
|
LOG_PACKET_HEADER_INIT(LOG_GPS_RAW_MSG),
|
||||||
|
timestamp : now,
|
||||||
|
iTOW : raw.iTOW,
|
||||||
|
week : raw.week,
|
||||||
|
numSV : raw.numSV,
|
||||||
|
sv : raw.svinfo[i].sv,
|
||||||
|
cpMes : raw.svinfo[i].cpMes,
|
||||||
|
prMes : raw.svinfo[i].prMes,
|
||||||
|
doMes : raw.svinfo[i].doMes,
|
||||||
|
mesQI : raw.svinfo[i].mesQI,
|
||||||
|
cno : raw.svinfo[i].cno,
|
||||||
|
lli : raw.svinfo[i].lli
|
||||||
|
};
|
||||||
|
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif // UBLOX_RXM_RAW_LOGGING
|
||||||
|
|
||||||
void AP_GPS_UBLOX::unexpected_message(void)
|
void AP_GPS_UBLOX::unexpected_message(void)
|
||||||
{
|
{
|
||||||
Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
|
Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
|
||||||
|
@ -398,6 +432,13 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||||
}
|
}
|
||||||
#endif // UBLOX_HW_LOGGING
|
#endif // UBLOX_HW_LOGGING
|
||||||
|
|
||||||
|
#if UBLOX_RXM_RAW_LOGGING
|
||||||
|
if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
|
||||||
|
log_rxm_raw(_buffer.rxm_raw);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
#endif // UBLOX_RXM_RAW_LOGGING
|
||||||
|
|
||||||
if (_class != CLASS_NAV) {
|
if (_class != CLASS_NAV) {
|
||||||
unexpected_message();
|
unexpected_message();
|
||||||
return false;
|
return false;
|
||||||
|
@ -573,7 +614,7 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||||
* update checksum for a set of bytes
|
* update checksum for a set of bytes
|
||||||
*/
|
*/
|
||||||
void
|
void
|
||||||
AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b)
|
AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b)
|
||||||
{
|
{
|
||||||
while (len--) {
|
while (len--) {
|
||||||
ck_a += *data;
|
ck_a += *data;
|
||||||
|
@ -587,7 +628,7 @@ AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_
|
||||||
* send a ublox message
|
* send a ublox message
|
||||||
*/
|
*/
|
||||||
void
|
void
|
||||||
AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
|
AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size)
|
||||||
{
|
{
|
||||||
struct ubx_header header;
|
struct ubx_header header;
|
||||||
uint8_t ck_a=0, ck_b=0;
|
uint8_t ck_a=0, ck_b=0;
|
||||||
|
|
|
@ -38,6 +38,7 @@
|
||||||
* would mean we would never detect it.
|
* would mean we would never detect it.
|
||||||
*/
|
*/
|
||||||
#define UBLOX_SET_BINARY "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0003,0001,38400,0*26\r\n"
|
#define UBLOX_SET_BINARY "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0003,0001,38400,0*26\r\n"
|
||||||
|
#define UBLOX_MAX_RXM_RAW_SATS 16
|
||||||
|
|
||||||
class AP_GPS_UBLOX : public AP_GPS_Backend
|
class AP_GPS_UBLOX : public AP_GPS_Backend
|
||||||
{
|
{
|
||||||
|
@ -200,6 +201,21 @@ private:
|
||||||
uint8_t globalFlags;
|
uint8_t globalFlags;
|
||||||
uint16_t reserved;
|
uint16_t reserved;
|
||||||
};
|
};
|
||||||
|
struct PACKED ubx_rxm_raw {
|
||||||
|
int32_t iTOW;
|
||||||
|
int16_t week;
|
||||||
|
uint8_t numSV;
|
||||||
|
uint8_t reserved1;
|
||||||
|
struct ubx_rxm_raw_sv {
|
||||||
|
double cpMes;
|
||||||
|
double prMes;
|
||||||
|
float doMes;
|
||||||
|
uint8_t sv;
|
||||||
|
int8_t mesQI;
|
||||||
|
int8_t cno;
|
||||||
|
uint8_t lli;
|
||||||
|
} svinfo[UBLOX_MAX_RXM_RAW_SATS];
|
||||||
|
};
|
||||||
// Receive buffer
|
// Receive buffer
|
||||||
union PACKED {
|
union PACKED {
|
||||||
ubx_nav_posllh posllh;
|
ubx_nav_posllh posllh;
|
||||||
|
@ -212,6 +228,7 @@ private:
|
||||||
ubx_mon_hw2 mon_hw2;
|
ubx_mon_hw2 mon_hw2;
|
||||||
ubx_cfg_sbas sbas;
|
ubx_cfg_sbas sbas;
|
||||||
ubx_nav_svinfo_header svinfo_header;
|
ubx_nav_svinfo_header svinfo_header;
|
||||||
|
ubx_rxm_raw rxm_raw;
|
||||||
uint8_t bytes[];
|
uint8_t bytes[];
|
||||||
} _buffer;
|
} _buffer;
|
||||||
|
|
||||||
|
@ -222,6 +239,7 @@ private:
|
||||||
CLASS_ACK = 0x05,
|
CLASS_ACK = 0x05,
|
||||||
CLASS_CFG = 0x06,
|
CLASS_CFG = 0x06,
|
||||||
CLASS_MON = 0x0A,
|
CLASS_MON = 0x0A,
|
||||||
|
CLASS_RXM = 0x02,
|
||||||
MSG_ACK_NACK = 0x00,
|
MSG_ACK_NACK = 0x00,
|
||||||
MSG_ACK_ACK = 0x01,
|
MSG_ACK_ACK = 0x01,
|
||||||
MSG_POSLLH = 0x2,
|
MSG_POSLLH = 0x2,
|
||||||
|
@ -235,7 +253,8 @@ private:
|
||||||
MSG_CFG_SBAS = 0x16,
|
MSG_CFG_SBAS = 0x16,
|
||||||
MSG_MON_HW = 0x09,
|
MSG_MON_HW = 0x09,
|
||||||
MSG_MON_HW2 = 0x0B,
|
MSG_MON_HW2 = 0x0B,
|
||||||
MSG_NAV_SVINFO = 0x30
|
MSG_NAV_SVINFO = 0x30,
|
||||||
|
MSG_RXM_RAW = 0x10
|
||||||
};
|
};
|
||||||
enum ubs_nav_fix_type {
|
enum ubs_nav_fix_type {
|
||||||
FIX_NONE = 0,
|
FIX_NONE = 0,
|
||||||
|
@ -296,8 +315,8 @@ private:
|
||||||
void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
|
void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
|
||||||
void _configure_gps(void);
|
void _configure_gps(void);
|
||||||
void _configure_sbas(bool enable);
|
void _configure_sbas(bool enable);
|
||||||
void _update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b);
|
void _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b);
|
||||||
void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
|
void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size);
|
||||||
void send_next_rate_update(void);
|
void send_next_rate_update(void);
|
||||||
void _request_version(void);
|
void _request_version(void);
|
||||||
|
|
||||||
|
@ -306,6 +325,7 @@ private:
|
||||||
void log_mon_hw(void);
|
void log_mon_hw(void);
|
||||||
void log_mon_hw2(void);
|
void log_mon_hw2(void);
|
||||||
void log_accuracy(void);
|
void log_accuracy(void);
|
||||||
|
void log_rxm_raw(const struct ubx_rxm_raw &raw);
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // __AP_GPS_UBLOX_H__
|
#endif // __AP_GPS_UBLOX_H__
|
||||||
|
|
Loading…
Reference in New Issue