PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <ubx.h>
Public Member Functions | |
GPSDriverUBX (Interface gpsInterface, GPSCallbackPtr callback, void *callback_user, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, uint8_t dynamic_model=7) | |
virtual | ~GPSDriverUBX () |
int | receive (unsigned timeout) override |
receive & handle new data from the device More... | |
int | configure (unsigned &baudrate, OutputMode output_mode) override |
configure the device More... | |
int | reset (GPSRestartType restart_type) override |
Reset GPS device. More... | |
Public Member Functions inherited from GPSBaseStationSupport | |
GPSBaseStationSupport (GPSCallbackPtr callback, void *callback_user) | |
virtual | ~GPSBaseStationSupport ()=default |
void | setSurveyInSpecs (uint32_t survey_in_acc_limit, uint32_t survey_in_min_dur) |
set survey-in specs for RTK base station setup (for finding an accurate base station position by averaging the position measurements over time). More... | |
void | setBasePosition (double latitude, double longitude, float altitude, float position_accuracy) |
Set a fixed base station position. More... | |
Public Member Functions inherited from GPSHelper | |
GPSHelper (GPSCallbackPtr callback, void *callback_user) | |
virtual | ~GPSHelper ()=default |
float | getPositionUpdateRate () |
float | getVelocityUpdateRate () |
void | resetUpdateRates () |
void | storeUpdateRates () |
Private Types | |
enum | Board : uint8_t { Board::unknown = 0, Board::u_blox5 = 5, Board::u_blox6 = 6, Board::u_blox7 = 7, Board::u_blox8 = 8, Board::u_blox9 = 9 } |
Private Member Functions | |
int | restartSurveyIn () |
Start or restart the survey-in procees. More... | |
int | restartSurveyInPreV27 () |
restartSurveyIn for protocol version < 27 (_proto_ver_27_or_higher == false) More... | |
int | parseChar (const uint8_t b) |
Parse the binary UBX packet. More... | |
int | payloadRxInit (void) |
Start payload rx. More... | |
int | payloadRxAdd (const uint8_t b) |
Add payload rx byte. More... | |
int | payloadRxAddNavSvinfo (const uint8_t b) |
Add NAV-SVINFO payload rx byte. More... | |
int | payloadRxAddNavSat (const uint8_t b) |
int | payloadRxAddMonVer (const uint8_t b) |
Add MON-VER payload rx byte. More... | |
int | payloadRxDone (void) |
Finish payload rx. More... | |
void | decodeInit (void) |
Reset the parse state machine for a fresh start. More... | |
void | addByteToChecksum (const uint8_t) |
While parsing add every byte (except the sync bytes) to the checksum. More... | |
bool | sendMessage (const uint16_t msg, const uint8_t *payload, const uint16_t length) |
Send a message. More... | |
bool | configureMessageRate (const uint16_t msg, const uint8_t rate) |
Configure message rate. More... | |
void | calcChecksum (const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum) |
Calculate & add checksum for given buffer. More... | |
int | waitForAck (const uint16_t msg, const unsigned timeout, const bool report) |
Wait for message acknowledge. More... | |
bool | configureMessageRateAndAck (uint16_t msg, uint8_t rate, bool report_ack_error=false) |
Combines the configure_message_rate & wait_for_ack calls. More... | |
int | configureDevice () |
Send configuration values and desired message rates. More... | |
int | configureDevicePreV27 () |
Send configuration values and desired message rates (for protocol version < 27) More... | |
int | initCfgValset () |
Init _buf as CFG-VALSET. More... | |
template<typename T > | |
bool | cfgValset (uint32_t key_id, T value, int &msg_size) |
Add a configuration value to _buf and increase the message size msg_size as needed. More... | |
bool | cfgValsetPort (uint32_t key_id, uint8_t value, int &msg_size) |
Add a configuration value that is port-specific (MSGOUT messages). More... | |
int | activateRTCMOutput () |
uint32_t | fnv1_32_str (uint8_t *str, uint32_t hval) |
Calculate FNV1 hash. More... | |
Private Attributes | |
struct vehicle_gps_position_s * | _gps_position {nullptr} |
struct satellite_info_s * | _satellite_info {nullptr} |
uint64_t | _last_timestamp_time {0} |
bool | _configured {false} |
ubx_ack_state_t | _ack_state {UBX_ACK_IDLE} |
bool | _got_posllh {false} |
bool | _got_velned {false} |
ubx_decode_state_t | _decode_state {} |
uint16_t | _rx_msg {} |
ubx_rxmsg_state_t | _rx_state {UBX_RXMSG_IGNORE} |
uint16_t | _rx_payload_length {} |
uint16_t | _rx_payload_index {} |
uint8_t | _rx_ck_a {} |
uint8_t | _rx_ck_b {} |
gps_abstime | _disable_cmd_last {0} |
uint16_t | _ack_waiting_msg {0} |
ubx_buf_t | _buf {} |
uint32_t | _ubx_version {0} |
bool | _use_nav_pvt {false} |
bool | _proto_ver_27_or_higher {false} |
true if protocol version 27 or higher detected More... | |
OutputMode | _output_mode {OutputMode::GPS} |
RTCMParsing * | _rtcm_parsing {nullptr} |
const Interface | _interface |
Board | _board {Board::unknown} |
uint8_t | _dyn_model {7} |
Additional Inherited Members | |
Public Types inherited from GPSHelper | |
enum | OutputMode : uint8_t { OutputMode::GPS = 0, OutputMode::RTCM } |
enum | Interface : uint8_t { Interface::UART = 0, Interface::SPI } |
Protected Types inherited from GPSBaseStationSupport | |
enum | BaseSettingsType : uint8_t { BaseSettingsType::survey_in, BaseSettingsType::fixed_position } |
Protected Member Functions inherited from GPSHelper | |
int | read (uint8_t *buf, int buf_length, int timeout) |
read from device More... | |
int | write (const void *buf, int buf_length) |
write to the device More... | |
int | setBaudrate (int baudrate) |
set the Baudrate More... | |
void | surveyInStatus (SurveyInStatus &status) |
void | gotRTCMMessage (uint8_t *buf, int buf_length) |
got an RTCM message from the device More... | |
void | setClock (timespec &t) |
Static Protected Member Functions inherited from GPSHelper | |
static void | ECEF2lla (double ecef_x, double ecef_y, double ecef_z, double &latitude, double &longitude, float &altitude) |
Convert an ECEF (Earth Centered Earth Fixed) coordinate to LLA WGS84 (Lat, Lon, Alt). More... | |
Protected Attributes inherited from GPSBaseStationSupport | |
BaseSettings | _base_settings |
Protected Attributes inherited from GPSHelper | |
GPSCallbackPtr | _callback {nullptr} |
void * | _callback_user {} |
uint8_t | _rate_count_lat_lon {} |
uint8_t | _rate_count_vel {} |
float | _rate_lat_lon {0.0f} |
float | _rate_vel {0.0f} |
uint64_t | _interval_rate_start {0} |
|
strongprivate |
GPSDriverUBX::GPSDriverUBX | ( | Interface | gpsInterface, |
GPSCallbackPtr | callback, | ||
void * | callback_user, | ||
struct vehicle_gps_position_s * | gps_position, | ||
struct satellite_info_s * | satellite_info, | ||
uint8_t | dynamic_model = 7 |
||
) |
Definition at line 82 of file ubx.cpp.
References decodeInit().
|
virtual |
Definition at line 95 of file ubx.cpp.
References _rtcm_parsing.
|
private |
Definition at line 1674 of file ubx.cpp.
References _buf, _proto_ver_27_or_higher, cfgValsetPort(), configureMessageRate(), initCfgValset(), ubx_payload_tx_cfg_rate_t::measRate, ubx_payload_tx_cfg_rate_t::navRate, ubx_buf_t::payload_tx_cfg_rate, sendMessage(), ubx_payload_tx_cfg_rate_t::timeRef, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1005_I2C, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_I2C, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_I2C, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_I2C, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_I2C, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_I2C, UBX_CFG_KEY_MSGOUT_UBX_NAV_SVIN_I2C, UBX_CFG_KEY_RATE_MEAS, UBX_CONFIG_TIMEOUT, UBX_MSG_CFG_RATE, UBX_MSG_CFG_VALSET, UBX_MSG_NAV_SVIN, UBX_MSG_RTCM3_1005, UBX_MSG_RTCM3_1077, UBX_MSG_RTCM3_1087, UBX_MSG_RTCM3_1097, UBX_MSG_RTCM3_1127, UBX_MSG_RTCM3_1230, UBX_TX_CFG_RATE_NAVRATE, UBX_TX_CFG_RATE_TIMEREF, and waitForAck().
Referenced by payloadRxDone(), restartSurveyIn(), and restartSurveyInPreV27().
|
private |
While parsing add every byte (except the sync bytes) to the checksum.
Definition at line 1757 of file ubx.cpp.
References _rx_ck_a, and _rx_ck_b.
Referenced by parseChar().
|
private |
Calculate & add checksum for given buffer.
Definition at line 1764 of file ubx.cpp.
References ubx_checksum_t::ck_a, and ubx_checksum_t::ck_b.
Referenced by sendMessage().
|
private |
Add a configuration value to _buf and increase the message size msg_size as needed.
key_id | one of the UBX_CFG_KEY_* constants |
value | configuration value |
msg_size | CFG-VALSET message size: this is an input & output param |
Definition at line 493 of file ubx.cpp.
References _buf, ubx_buf_t::payload_tx_cfg_valset, and UBX_WARN.
|
private |
Add a configuration value that is port-specific (MSGOUT messages).
Note: Key ID must be the one for I2C, and the implementation assumes the Key ID's are in increasing order for the other ports: I2C, UART1, UART2, USB, SPI (this is a safe assumption for all MSGOUT messages according to u-blox).
key_id | I2C key ID |
value | configuration value |
msg_size | CFG-VALSET message size: this is an input & output param |
Definition at line 509 of file ubx.cpp.
References _interface, and GPSHelper::SPI.
Referenced by activateRTCMOutput(), configureDevice(), and restartSurveyIn().
|
overridevirtual |
configure the device
baud | Input and output parameter: if set to 0, the baudrate will be automatically detected and set to the detected baudrate. If not 0, a fixed baudrate is used. |
Implements GPSHelper.
Definition at line 103 of file ubx.cpp.
References _board, _buf, _configured, _dyn_model, _interface, _output_mode, _proto_ver_27_or_higher, uart_esc::baudrate, ubx_payload_tx_cfg_prt_t::baudRate, configureDevice(), configureDevicePreV27(), decodeInit(), GPSHelper::GPS, initCfgValset(), ubx_payload_tx_cfg_prt_t::inProtoMask, ubx_payload_tx_cfg_prt_t::mode, ubx_payload_tx_cfg_prt_t::outProtoMask, ubx_payload_tx_cfg_prt_t::portID, receive(), restartSurveyIn(), GPSHelper::RTCM, sendMessage(), GPSHelper::setBaudrate(), GPSHelper::SPI, u_blox8, GPSHelper::UART, UBX_BAUDRATE_M8_AND_NEWER, UBX_CFG_KEY_CFG_SPIINPROT_NMEA, UBX_CFG_KEY_CFG_SPIINPROT_RTCM3X, UBX_CFG_KEY_CFG_SPIINPROT_UBX, UBX_CFG_KEY_CFG_SPIOUTPROT_NMEA, UBX_CFG_KEY_CFG_SPIOUTPROT_RTCM3X, UBX_CFG_KEY_CFG_SPIOUTPROT_UBX, UBX_CFG_KEY_CFG_UART1_BAUDRATE, UBX_CFG_KEY_CFG_UART1_DATABITS, UBX_CFG_KEY_CFG_UART1_PARITY, UBX_CFG_KEY_CFG_UART1_STOPBITS, UBX_CFG_KEY_CFG_UART1INPROT_NMEA, UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X, UBX_CFG_KEY_CFG_UART1INPROT_UBX, UBX_CFG_KEY_CFG_UART1OUTPROT_NMEA, UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, UBX_CFG_KEY_CFG_USBINPROT_NMEA, UBX_CFG_KEY_CFG_USBINPROT_RTCM3X, UBX_CFG_KEY_CFG_USBINPROT_UBX, UBX_CFG_KEY_CFG_USBOUTPROT_NMEA, UBX_CFG_KEY_CFG_USBOUTPROT_RTCM3X, UBX_CFG_KEY_CFG_USBOUTPROT_UBX, UBX_CFG_KEY_SPI_ENABLED, UBX_CFG_KEY_SPI_MAXFF, UBX_CONFIG_TIMEOUT, UBX_DEBUG, UBX_MSG_CFG_PRT, UBX_MSG_CFG_VALSET, UBX_MSG_MON_VER, UBX_TX_CFG_PRT_BAUDRATE, UBX_TX_CFG_PRT_INPROTOMASK_GPS, UBX_TX_CFG_PRT_INPROTOMASK_RTCM, UBX_TX_CFG_PRT_MODE, UBX_TX_CFG_PRT_MODE_SPI, UBX_TX_CFG_PRT_OUTPROTOMASK_GPS, UBX_TX_CFG_PRT_OUTPROTOMASK_RTCM, UBX_TX_CFG_PRT_PORTID, UBX_TX_CFG_PRT_PORTID_SPI, UBX_TX_CFG_PRT_PORTID_USB, and waitForAck().
|
private |
Send configuration values and desired message rates.
Definition at line 436 of file ubx.cpp.
References _buf, _dyn_model, _satellite_info, _use_nav_pvt, cfgValsetPort(), initCfgValset(), sendMessage(), UBX_CFG_KEY_MSGOUT_UBX_MON_RF_I2C, UBX_CFG_KEY_MSGOUT_UBX_NAV_DOP_I2C, UBX_CFG_KEY_MSGOUT_UBX_NAV_PVT_I2C, UBX_CFG_KEY_MSGOUT_UBX_NAV_SAT_I2C, UBX_CFG_KEY_NAVHPG_DGNSSMODE, UBX_CFG_KEY_NAVSPG_DYNMODEL, UBX_CFG_KEY_NAVSPG_FIXMODE, UBX_CFG_KEY_NAVSPG_UTCSTANDARD, UBX_CFG_KEY_ODO_OUTLPCOG, UBX_CFG_KEY_ODO_OUTLPVEL, UBX_CFG_KEY_ODO_USE_COG, UBX_CFG_KEY_ODO_USE_ODO, UBX_CFG_KEY_RATE_MEAS, UBX_CFG_KEY_RATE_NAV, UBX_CFG_KEY_RATE_TIMEREF, UBX_CONFIG_TIMEOUT, UBX_MSG_CFG_VALSET, and waitForAck().
Referenced by configure().
|
private |
Send configuration values and desired message rates (for protocol version < 27)
Definition at line 355 of file ubx.cpp.
References _buf, _dyn_model, _satellite_info, _use_nav_pvt, configureMessageRate(), configureMessageRateAndAck(), ubx_payload_tx_cfg_rate_t::measRate, ubx_payload_tx_cfg_rate_t::navRate, ubx_buf_t::payload_tx_cfg_rate, sendMessage(), ubx_payload_tx_cfg_rate_t::timeRef, UBX_CONFIG_TIMEOUT, UBX_DEBUG, UBX_MSG_CFG_MSG, UBX_MSG_CFG_NAV5, UBX_MSG_CFG_RATE, UBX_MSG_MON_HW, UBX_MSG_NAV_DOP, UBX_MSG_NAV_POSLLH, UBX_MSG_NAV_PVT, UBX_MSG_NAV_SOL, UBX_MSG_NAV_SVINFO, UBX_MSG_NAV_TIMEUTC, UBX_MSG_NAV_VELNED, UBX_TX_CFG_NAV5_FIXMODE, UBX_TX_CFG_NAV5_MASK, UBX_TX_CFG_RATE_MEASINTERVAL, UBX_TX_CFG_RATE_NAVRATE, UBX_TX_CFG_RATE_TIMEREF, and waitForAck().
Referenced by configure().
|
private |
Configure message rate.
Note: this is deprecated with protocol version >= 27
Definition at line 1773 of file ubx.cpp.
References _proto_ver_27_or_higher, msg, ubx_payload_tx_cfg_msg_t::msg, ubx_payload_tx_cfg_msg_t::rate, sendMessage(), UBX_MSG_CFG_MSG, and UBX_WARN.
Referenced by activateRTCMOutput(), configureDevicePreV27(), configureMessageRateAndAck(), payloadRxInit(), and restartSurveyInPreV27().
|
inlineprivate |
Combines the configure_message_rate & wait_for_ack calls.
Note: this is deprecated with protocol version >= 27
Definition at line 1791 of file ubx.cpp.
References configureMessageRate(), UBX_CONFIG_TIMEOUT, UBX_MSG_CFG_MSG, and waitForAck().
Referenced by configureDevicePreV27(), and restartSurveyInPreV27().
|
private |
Reset the parse state machine for a fresh start.
Definition at line 1739 of file ubx.cpp.
References _decode_state, _output_mode, _rtcm_parsing, _rx_ck_a, _rx_ck_b, _rx_payload_index, _rx_payload_length, RTCMParsing::reset(), GPSHelper::RTCM, and UBX_DECODE_SYNC1.
Referenced by configure(), GPSDriverUBX(), and parseChar().
|
private |
Calculate FNV1 hash.
Definition at line 1834 of file ubx.cpp.
References FNV1_32_PRIME.
Referenced by payloadRxAddMonVer().
|
private |
Init _buf as CFG-VALSET.
Definition at line 485 of file ubx.cpp.
References _buf, ubx_payload_tx_cfg_valset_t::cfgData, ubx_payload_tx_cfg_valset_t::layers, ubx_buf_t::payload_tx_cfg_valset, and UBX_CFG_LAYER_RAM.
Referenced by activateRTCMOutput(), configure(), configureDevice(), and restartSurveyIn().
|
private |
Parse the binary UBX packet.
Definition at line 770 of file ubx.cpp.
References _decode_state, _rtcm_parsing, _rx_ck_a, _rx_ck_b, _rx_msg, _rx_payload_length, RTCMParsing::addByte(), addByteToChecksum(), decodeInit(), GPSHelper::gotRTCMMessage(), RTCMParsing::message(), RTCMParsing::messageLength(), payloadRxAdd(), payloadRxAddMonVer(), payloadRxAddNavSat(), payloadRxAddNavSvinfo(), payloadRxDone(), payloadRxInit(), RTCM3_PREAMBLE, UBX_DEBUG, UBX_DECODE_CHKSUM1, UBX_DECODE_CHKSUM2, UBX_DECODE_CLASS, UBX_DECODE_ID, UBX_DECODE_LENGTH1, UBX_DECODE_LENGTH2, UBX_DECODE_PAYLOAD, UBX_DECODE_RTCM3, UBX_DECODE_SYNC1, UBX_DECODE_SYNC2, UBX_MSG_MON_VER, UBX_MSG_NAV_SAT, UBX_MSG_NAV_SVINFO, UBX_SYNC1, UBX_SYNC2, and UBX_TRACE_PARSER.
Referenced by receive().
|
private |
Add payload rx byte.
Definition at line 1138 of file ubx.cpp.
References _buf, _rx_payload_index, and _rx_payload_length.
Referenced by parseChar().
|
private |
Add MON-VER payload rx byte.
Definition at line 1264 of file ubx.cpp.
References _board, _buf, _rx_payload_index, _rx_payload_length, _ubx_version, ubx_payload_rx_mon_ver_part2_t::extension, FNV1_32_INIT, fnv1_32_str(), ubx_payload_rx_mon_ver_part1_t::hwVersion, ubx_buf_t::payload_rx_mon_ver_part1, ubx_buf_t::payload_rx_mon_ver_part2, ubx_payload_rx_mon_ver_part1_t::swVersion, u_blox5, u_blox6, u_blox7, u_blox8, u_blox9, UBX_DEBUG, and UBX_WARN.
Referenced by parseChar().
|
private |
Definition at line 1153 of file ubx.cpp.
References _buf, _rx_payload_index, _rx_payload_length, _satellite_info, ubx_payload_rx_nav_sat_part2_t::azim, satellite_info_s::azimuth, ubx_payload_rx_nav_sat_part2_t::cno, satellite_info_s::count, ubx_payload_rx_nav_sat_part2_t::elev, satellite_info_s::elevation, ubx_payload_rx_nav_sat_part2_t::flags, MIN, ubx_payload_rx_nav_sat_part1_t::numSvs, ubx_buf_t::payload_rx_nav_sat_part1, ubx_buf_t::payload_rx_nav_sat_part2, satellite_info_s::snr, satellite_info_s::svid, ubx_payload_rx_nav_sat_part2_t::svId, UBX_TRACE_SVINFO, and satellite_info_s::used.
Referenced by parseChar().
|
private |
Add NAV-SVINFO payload rx byte.
Definition at line 1208 of file ubx.cpp.
References _buf, _rx_payload_index, _rx_payload_length, _satellite_info, ubx_payload_rx_nav_svinfo_part2_t::azim, satellite_info_s::azimuth, ubx_payload_rx_nav_svinfo_part2_t::cno, satellite_info_s::count, ubx_payload_rx_nav_svinfo_part2_t::elev, satellite_info_s::elevation, ubx_payload_rx_nav_svinfo_part2_t::flags, MIN, ubx_payload_rx_nav_svinfo_part1_t::numCh, ubx_buf_t::payload_rx_nav_svinfo_part1, ubx_buf_t::payload_rx_nav_svinfo_part2, satellite_info_s::snr, satellite_info_s::svid, ubx_payload_rx_nav_svinfo_part2_t::svid, UBX_TRACE_SVINFO, and satellite_info_s::used.
Referenced by parseChar().
|
private |
Finish payload rx.
Definition at line 1332 of file ubx.cpp.
References _ack_state, _ack_waiting_msg, _buf, _got_posllh, _got_velned, _gps_position, _last_timestamp_time, GPSHelper::_rate_count_lat_lon, GPSHelper::_rate_count_vel, _rx_msg, _rx_payload_length, _rx_state, _satellite_info, activateRTCMOutput(), ubx_payload_rx_nav_svin_t::active, vehicle_gps_position_s::alt, vehicle_gps_position_s::alt_ellipsoid, ubx_payload_rx_mon_rf_t::block, vehicle_gps_position_s::c_variance_rad, ubx_payload_rx_nav_velned_t::cAcc, vehicle_gps_position_s::cog_rad, ubx_payload_rx_nav_pvt_t::day, ubx_payload_rx_nav_timeutc_t::day, ubx_payload_rx_nav_svin_t::dur, GPSHelper::ECEF2lla(), vehicle_gps_position_s::eph, vehicle_gps_position_s::epv, f(), vehicle_gps_position_s::fix_type, ubx_payload_rx_nav_pvt_t::fixType, ubx_payload_rx_nav_pvt_t::flags, gps_absolute_time, GPS_EPOCH_SECS, ubx_payload_rx_nav_sol_t::gpsFix, ubx_payload_rx_nav_pvt_t::gSpeed, ubx_payload_rx_nav_posllh_t::hAcc, ubx_payload_rx_nav_pvt_t::hAcc, vehicle_gps_position_s::hdop, ubx_payload_rx_nav_dop_t::hDOP, ubx_payload_rx_nav_pvt_t::headAcc, ubx_payload_rx_nav_velned_t::heading, ubx_payload_rx_nav_pvt_t::headMot, ubx_payload_rx_nav_posllh_t::height, ubx_payload_rx_nav_pvt_t::height, ubx_payload_rx_nav_posllh_t::hMSL, ubx_payload_rx_nav_pvt_t::hMSL, ubx_payload_rx_nav_pvt_t::hour, ubx_payload_rx_nav_timeutc_t::hour, ubx_payload_rx_mon_hw_ubx6_t::jamInd, ubx_payload_rx_mon_hw_ubx7_t::jamInd, ubx_payload_rx_mon_rf_t::ubx_payload_rx_mon_rf_block_t::jamInd, vehicle_gps_position_s::jamming_indicator, vehicle_gps_position_s::lat, ubx_payload_rx_nav_posllh_t::lat, ubx_payload_rx_nav_pvt_t::lat, vehicle_gps_position_s::lon, ubx_payload_rx_nav_posllh_t::lon, ubx_payload_rx_nav_pvt_t::lon, ubx_payload_rx_nav_svin_t::meanAcc, ubx_payload_rx_nav_svin_t::meanX, ubx_payload_rx_nav_svin_t::meanXHP, ubx_payload_rx_nav_svin_t::meanY, ubx_payload_rx_nav_svin_t::meanYHP, ubx_payload_rx_nav_svin_t::meanZ, ubx_payload_rx_nav_svin_t::meanZHP, ubx_payload_rx_nav_pvt_t::min, ubx_payload_rx_nav_timeutc_t::min, ubx_payload_rx_nav_pvt_t::month, ubx_payload_rx_nav_timeutc_t::month, ubx_payload_rx_ack_ack_t::msg, ubx_payload_rx_nav_pvt_t::nano, ubx_payload_rx_nav_timeutc_t::nano, vehicle_gps_position_s::noise_per_ms, ubx_payload_rx_mon_hw_ubx6_t::noisePerMS, ubx_payload_rx_mon_hw_ubx7_t::noisePerMS, ubx_payload_rx_mon_rf_t::ubx_payload_rx_mon_rf_block_t::noisePerMS, ubx_payload_rx_nav_sol_t::numSV, ubx_payload_rx_nav_pvt_t::numSV, ubx_payload_rx_nav_svin_t::obs, ubx_buf_t::payload_rx_ack_ack, ubx_buf_t::payload_rx_mon_hw_ubx6, ubx_buf_t::payload_rx_mon_hw_ubx7, ubx_buf_t::payload_rx_mon_rf, ubx_buf_t::payload_rx_nav_dop, ubx_buf_t::payload_rx_nav_posllh, ubx_buf_t::payload_rx_nav_pvt, ubx_buf_t::payload_rx_nav_sol, ubx_buf_t::payload_rx_nav_svin, ubx_buf_t::payload_rx_nav_timeutc, ubx_buf_t::payload_rx_nav_velned, vehicle_gps_position_s::s_variance_m_s, ubx_payload_rx_nav_sol_t::sAcc, ubx_payload_rx_nav_pvt_t::sAcc, vehicle_gps_position_s::satellites_used, ubx_payload_rx_nav_pvt_t::sec, ubx_payload_rx_nav_timeutc_t::sec, GPSHelper::setClock(), ubx_payload_rx_nav_velned_t::speed, status, GPSHelper::surveyInStatus(), vehicle_gps_position_s::time_utc_usec, vehicle_gps_position_s::timestamp, satellite_info_s::timestamp, vehicle_gps_position_s::timestamp_time_relative, UBX_ACK_GOT_ACK, UBX_ACK_GOT_NAK, UBX_ACK_WAITING, UBX_DEBUG, UBX_MSG_ACK_ACK, UBX_MSG_ACK_NAK, UBX_MSG_INF_DEBUG, UBX_MSG_INF_ERROR, UBX_MSG_INF_NOTICE, UBX_MSG_INF_WARNING, UBX_MSG_MON_HW, UBX_MSG_MON_RF, UBX_MSG_MON_VER, UBX_MSG_NAV_DOP, UBX_MSG_NAV_POSLLH, UBX_MSG_NAV_PVT, UBX_MSG_NAV_SAT, UBX_MSG_NAV_SOL, UBX_MSG_NAV_SVIN, UBX_MSG_NAV_SVINFO, UBX_MSG_NAV_TIMEUTC, UBX_MSG_NAV_VELNED, UBX_RX_NAV_PVT_FLAGS_DIFFSOLN, UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK, UBX_RX_NAV_PVT_VALID_FULLYRESOLVED, UBX_RX_NAV_PVT_VALID_VALIDDATE, UBX_RX_NAV_PVT_VALID_VALIDTIME, UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC, UBX_RXMSG_HANDLE, UBX_TRACE_RXMSG, UBX_WARN, ubx_payload_rx_nav_posllh_t::vAcc, ubx_payload_rx_nav_pvt_t::vAcc, ubx_payload_rx_nav_pvt_t::valid, ubx_payload_rx_nav_timeutc_t::valid, ubx_payload_rx_nav_svin_t::valid, vehicle_gps_position_s::vdop, ubx_payload_rx_nav_dop_t::vDOP, vehicle_gps_position_s::vel_d_m_s, vehicle_gps_position_s::vel_e_m_s, vehicle_gps_position_s::vel_m_s, vehicle_gps_position_s::vel_n_m_s, vehicle_gps_position_s::vel_ned_valid, ubx_payload_rx_nav_pvt_t::velD, ubx_payload_rx_nav_velned_t::velD, ubx_payload_rx_nav_pvt_t::velE, ubx_payload_rx_nav_velned_t::velE, ubx_payload_rx_nav_pvt_t::velN, ubx_payload_rx_nav_velned_t::velN, ubx_payload_rx_nav_pvt_t::year, and ubx_payload_rx_nav_timeutc_t::year.
Referenced by parseChar().
|
private |
Start payload rx.
Definition at line 924 of file ubx.cpp.
References _configured, _disable_cmd_last, _proto_ver_27_or_higher, _rx_msg, _rx_payload_length, _rx_state, _satellite_info, _use_nav_pvt, configureMessageRate(), DISABLE_MSG_INTERVAL, gps_absolute_time, SWAP16, UBX_DEBUG, UBX_MSG_ACK_ACK, UBX_MSG_ACK_NAK, UBX_MSG_INF_DEBUG, UBX_MSG_INF_ERROR, UBX_MSG_INF_NOTICE, UBX_MSG_INF_WARNING, UBX_MSG_MON_HW, UBX_MSG_MON_RF, UBX_MSG_MON_VER, UBX_MSG_NAV_DOP, UBX_MSG_NAV_POSLLH, UBX_MSG_NAV_PVT, UBX_MSG_NAV_SAT, UBX_MSG_NAV_SOL, UBX_MSG_NAV_SVIN, UBX_MSG_NAV_SVINFO, UBX_MSG_NAV_TIMEUTC, UBX_MSG_NAV_VELNED, UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7, UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8, UBX_RXMSG_DISABLE, UBX_RXMSG_ERROR_LENGTH, UBX_RXMSG_HANDLE, UBX_RXMSG_IGNORE, and UBX_WARN.
Referenced by parseChar().
|
overridevirtual |
receive & handle new data from the device
timeout | [ms] |
Implements GPSHelper.
Definition at line 713 of file ubx.cpp.
References _configured, _got_posllh, _got_velned, _interface, gps_absolute_time, GPS_READ_BUFFER_SIZE, parseChar(), GPSHelper::read(), GPSHelper::SPI, UBX_DEBUG, UBX_PACKET_TIMEOUT, and UBX_WARN.
Referenced by configure(), and waitForAck().
|
overridevirtual |
Reset GPS device.
restart_type |
Reimplemented from GPSHelper.
Definition at line 1859 of file ubx.cpp.
References _buf, Cold, Hot, ubx_payload_tx_cfg_rst_t::navBbrMask, ubx_buf_t::payload_tx_cfg_rst, ubx_payload_tx_cfg_rst_t::resetMode, sendMessage(), UBX_MSG_CFG_RST, UBX_TX_CFG_RST_BBR_MODE_COLD_START, UBX_TX_CFG_RST_BBR_MODE_HOT_START, UBX_TX_CFG_RST_BBR_MODE_WARM_START, UBX_TX_CFG_RST_MODE_SOFTWARE, and Warm.
|
private |
Start or restart the survey-in procees.
This is only used in RTCM ouput mode. It will be called automatically after configuring.
Definition at line 609 of file ubx.cpp.
References GPSBaseStationSupport::_base_settings, _buf, _output_mode, _proto_ver_27_or_higher, GPSBaseStationSupport::SurveyInSettings::acc_limit, activateRTCMOutput(), GPSBaseStationSupport::FixedPositionSettings::altitude, cfgValsetPort(), GPSBaseStationSupport::BaseSettings::fixed_position, initCfgValset(), GPSBaseStationSupport::FixedPositionSettings::latitude, GPSBaseStationSupport::FixedPositionSettings::longitude, GPSBaseStationSupport::SurveyInSettings::min_dur, GPSBaseStationSupport::FixedPositionSettings::position_accuracy, restartSurveyInPreV27(), GPSHelper::RTCM, sendMessage(), GPSBaseStationSupport::BaseSettings::settings, GPSBaseStationSupport::survey_in, GPSBaseStationSupport::BaseSettings::survey_in, GPSBaseStationSupport::BaseSettings::type, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1005_I2C, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_I2C, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_I2C, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_I2C, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_I2C, UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_I2C, UBX_CFG_KEY_MSGOUT_UBX_NAV_SVIN_I2C, UBX_CFG_KEY_TMODE_FIXED_POS_ACC, UBX_CFG_KEY_TMODE_HEIGHT, UBX_CFG_KEY_TMODE_HEIGHT_HP, UBX_CFG_KEY_TMODE_LAT, UBX_CFG_KEY_TMODE_LAT_HP, UBX_CFG_KEY_TMODE_LON, UBX_CFG_KEY_TMODE_LON_HP, UBX_CFG_KEY_TMODE_MODE, UBX_CFG_KEY_TMODE_POS_TYPE, UBX_CFG_KEY_TMODE_SVIN_ACC_LIMIT, UBX_CFG_KEY_TMODE_SVIN_MIN_DUR, UBX_CONFIG_TIMEOUT, UBX_DEBUG, UBX_MSG_CFG_VALSET, and waitForAck().
Referenced by configure().
|
private |
restartSurveyIn for protocol version < 27 (_proto_ver_27_or_higher == false)
Definition at line 530 of file ubx.cpp.
References GPSBaseStationSupport::_base_settings, _buf, GPSBaseStationSupport::SurveyInSettings::acc_limit, activateRTCMOutput(), GPSBaseStationSupport::FixedPositionSettings::altitude, configureMessageRate(), configureMessageRateAndAck(), GPSBaseStationSupport::BaseSettings::fixed_position, ubx_payload_tx_cfg_tmode3_t::flags, GPSBaseStationSupport::FixedPositionSettings::latitude, GPSBaseStationSupport::FixedPositionSettings::longitude, GPSBaseStationSupport::SurveyInSettings::min_dur, ubx_buf_t::payload_tx_cfg_tmode3, GPSBaseStationSupport::FixedPositionSettings::position_accuracy, sendMessage(), GPSBaseStationSupport::BaseSettings::settings, GPSBaseStationSupport::survey_in, GPSBaseStationSupport::BaseSettings::survey_in, GPSBaseStationSupport::BaseSettings::type, UBX_CONFIG_TIMEOUT, UBX_DEBUG, UBX_MSG_CFG_TMODE3, UBX_MSG_NAV_SVIN, UBX_MSG_RTCM3_1005, UBX_MSG_RTCM3_1077, UBX_MSG_RTCM3_1087, UBX_MSG_RTCM3_1097, UBX_MSG_RTCM3_1127, UBX_MSG_RTCM3_1230, UBX_WARN, and waitForAck().
Referenced by restartSurveyIn().
|
private |
Send a message.
Definition at line 1801 of file ubx.cpp.
References calcChecksum(), ubx_header_t::length, msg, ubx_header_t::msg, UBX_SYNC1, UBX_SYNC2, and GPSHelper::write().
Referenced by activateRTCMOutput(), configure(), configureDevice(), configureDevicePreV27(), configureMessageRate(), reset(), restartSurveyIn(), and restartSurveyInPreV27().
|
private |
Wait for message acknowledge.
Definition at line 683 of file ubx.cpp.
References _ack_state, _ack_waiting_msg, gps_absolute_time, msg, receive(), SWAP16, UBX_ACK_GOT_ACK, UBX_ACK_GOT_NAK, UBX_ACK_IDLE, UBX_ACK_WAITING, and UBX_DEBUG.
Referenced by activateRTCMOutput(), configure(), configureDevice(), configureDevicePreV27(), configureMessageRateAndAck(), restartSurveyIn(), and restartSurveyInPreV27().
|
private |
Definition at line 912 of file ubx.h.
Referenced by payloadRxDone(), and waitForAck().
|
private |
Definition at line 923 of file ubx.h.
Referenced by payloadRxDone(), and waitForAck().
|
private |
Definition at line 933 of file ubx.h.
Referenced by configure(), and payloadRxAddMonVer().
|
private |
Definition at line 924 of file ubx.h.
Referenced by activateRTCMOutput(), cfgValset(), configure(), configureDevice(), configureDevicePreV27(), initCfgValset(), payloadRxAdd(), payloadRxAddMonVer(), payloadRxAddNavSat(), payloadRxAddNavSvinfo(), payloadRxDone(), reset(), restartSurveyIn(), and restartSurveyInPreV27().
|
private |
Definition at line 911 of file ubx.h.
Referenced by configure(), payloadRxInit(), and receive().
|
private |
Definition at line 915 of file ubx.h.
Referenced by decodeInit(), and parseChar().
|
private |
Definition at line 922 of file ubx.h.
Referenced by payloadRxInit().
|
private |
Definition at line 936 of file ubx.h.
Referenced by configure(), configureDevice(), and configureDevicePreV27().
|
private |
Definition at line 913 of file ubx.h.
Referenced by payloadRxDone(), and receive().
|
private |
Definition at line 914 of file ubx.h.
Referenced by payloadRxDone(), and receive().
|
private |
Definition at line 908 of file ubx.h.
Referenced by payloadRxDone().
|
private |
Definition at line 932 of file ubx.h.
Referenced by cfgValsetPort(), configure(), and receive().
|
private |
Definition at line 910 of file ubx.h.
Referenced by payloadRxDone().
|
private |
Definition at line 928 of file ubx.h.
Referenced by configure(), decodeInit(), and restartSurveyIn().
|
private |
true if protocol version 27 or higher detected
Definition at line 927 of file ubx.h.
Referenced by activateRTCMOutput(), configure(), configureMessageRate(), payloadRxInit(), and restartSurveyIn().
|
private |
Definition at line 930 of file ubx.h.
Referenced by decodeInit(), parseChar(), and ~GPSDriverUBX().
|
private |
Definition at line 920 of file ubx.h.
Referenced by addByteToChecksum(), decodeInit(), and parseChar().
|
private |
Definition at line 921 of file ubx.h.
Referenced by addByteToChecksum(), decodeInit(), and parseChar().
|
private |
Definition at line 916 of file ubx.h.
Referenced by parseChar(), payloadRxDone(), and payloadRxInit().
|
private |
Definition at line 919 of file ubx.h.
Referenced by decodeInit(), payloadRxAdd(), payloadRxAddMonVer(), payloadRxAddNavSat(), and payloadRxAddNavSvinfo().
|
private |
Definition at line 918 of file ubx.h.
Referenced by decodeInit(), parseChar(), payloadRxAdd(), payloadRxAddMonVer(), payloadRxAddNavSat(), payloadRxAddNavSvinfo(), payloadRxDone(), and payloadRxInit().
|
private |
Definition at line 917 of file ubx.h.
Referenced by payloadRxDone(), and payloadRxInit().
|
private |
Definition at line 909 of file ubx.h.
Referenced by configureDevice(), configureDevicePreV27(), payloadRxAddNavSat(), payloadRxAddNavSvinfo(), payloadRxDone(), and payloadRxInit().
|
private |
Definition at line 925 of file ubx.h.
Referenced by payloadRxAddMonVer().
|
private |
Definition at line 926 of file ubx.h.
Referenced by configureDevice(), configureDevicePreV27(), and payloadRxInit().