PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <gps_helper.h>
Public Types | |
enum | OutputMode : uint8_t { OutputMode::GPS = 0, OutputMode::RTCM } |
enum | Interface : uint8_t { Interface::UART = 0, Interface::SPI } |
Public Member Functions | |
GPSHelper (GPSCallbackPtr callback, void *callback_user) | |
virtual | ~GPSHelper ()=default |
virtual int | configure (unsigned &baud, OutputMode output_mode)=0 |
configure the device More... | |
virtual int | receive (unsigned timeout)=0 |
receive & handle new data from the device More... | |
virtual int | reset (GPSRestartType restart_type) |
Reset GPS device. More... | |
float | getPositionUpdateRate () |
float | getVelocityUpdateRate () |
void | resetUpdateRates () |
void | storeUpdateRates () |
Protected Member Functions | |
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 | |
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 | |
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} |
Definition at line 147 of file gps_helper.h.
|
strong |
Enumerator | |
---|---|
UART | |
SPI |
Definition at line 155 of file gps_helper.h.
|
strong |
Enumerator | |
---|---|
GPS | normal GPS output |
RTCM | request RTCM output. This is used for (fixed position) base stations |
Definition at line 150 of file gps_helper.h.
GPSHelper::GPSHelper | ( | GPSCallbackPtr | callback, |
void * | callback_user | ||
) |
Definition at line 49 of file gps_helper.cpp.
|
virtualdefault |
|
pure virtual |
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. |
Implemented in GPSDriverUBX, GPSDriverEmlidReach, GPSDriverMTK, and GPSDriverAshtech.
Referenced by GPS::run().
|
staticprotected |
Convert an ECEF (Earth Centered Earth Fixed) coordinate to LLA WGS84 (Lat, Lon, Alt).
Ported from: https://stackoverflow.com/a/25428344
ecef_x | ECEF X-coordinate [m] |
ecef_y | ECEF Y-coordinate [m] |
ecef_z | ECEF Z-coordinate [m] |
latitude | [deg] |
longitude | [deg] |
altitude | [m] |
Definition at line 69 of file gps_helper.cpp.
References matrix::atan2(), matrix::cos(), M_PI, matrix::sin(), and matrix::sqrt().
Referenced by GPSDriverUBX::payloadRxDone().
|
inline |
Definition at line 190 of file gps_helper.h.
Referenced by GPS::print_status().
|
inline |
Definition at line 191 of file gps_helper.h.
Referenced by GPS::print_status().
|
inlineprotected |
got an RTCM message from the device
Definition at line 239 of file gps_helper.h.
References gotRTCMMessage.
Referenced by GPSDriverAshtech::parseChar(), GPSDriverSBF::parseChar(), and GPSDriverUBX::parseChar().
|
inlineprotected |
read from device
buf | pointer to read buffer |
buf_length | size of read buffer |
timeout | timeout in ms |
Definition at line 206 of file gps_helper.h.
References readDeviceData.
Referenced by GPSDriverAshtech::receive(), GPSDriverMTK::receive(), GPSDriverEmlidReach::receive(), GPSDriverSBF::receive(), GPSDriverUBX::receive(), and GPSDriverSBF::sendMessageAndWaitForAck().
|
pure virtual |
receive & handle new data from the device
timeout | [ms] |
Implemented in GPSDriverUBX, GPSDriverSBF, GPSDriverEmlidReach, GPSDriverMTK, and GPSDriverAshtech.
Referenced by GPS::run().
|
inlinevirtual |
Reset GPS device.
restart_type |
Reimplemented in GPSDriverUBX, and GPSDriverSBF.
Definition at line 188 of file gps_helper.h.
Referenced by GPS::reset_if_scheduled().
void GPSHelper::resetUpdateRates | ( | ) |
Definition at line 55 of file gps_helper.cpp.
References _interval_rate_start, _rate_count_lat_lon, _rate_count_vel, and gps_absolute_time.
Referenced by GPS::run().
|
inlineprotected |
set the Baudrate
baudrate |
Definition at line 228 of file gps_helper.h.
References setBaudrate.
Referenced by GPSDriverAshtech::configure(), GPSDriverMTK::configure(), GPSDriverEmlidReach::configure(), GPSDriverSBF::configure(), and GPSDriverUBX::configure().
|
inlineprotected |
Definition at line 244 of file gps_helper.h.
References setClock.
Referenced by GPSDriverAshtech::handleMessage(), GPSDriverMTK::handleMessage(), GPSDriverSBF::payloadRxDone(), and GPSDriverUBX::payloadRxDone().
void GPSHelper::storeUpdateRates | ( | ) |
Definition at line 63 of file gps_helper.cpp.
References _interval_rate_start, _rate_count_lat_lon, _rate_count_vel, _rate_lat_lon, _rate_vel, and gps_absolute_time.
Referenced by GPS::run().
|
inlineprotected |
Definition at line 233 of file gps_helper.h.
References surveyInStatus.
Referenced by GPSDriverAshtech::configure(), GPSDriverUBX::payloadRxDone(), and GPSDriverAshtech::sendSurveyInStatusUpdate().
|
inlineprotected |
write to the device
buf | |
buf_length |
Definition at line 218 of file gps_helper.h.
References writeDeviceData.
Referenced by GPSDriverAshtech::activateCorrectionOutput(), GPSDriverAshtech::configure(), GPSDriverMTK::configure(), GPSDriverSBF::sendMessage(), GPSDriverUBX::sendMessage(), GPSDriverSBF::sendMessageAndWaitForAck(), and GPSDriverAshtech::writeAckedCommand().
|
protected |
Definition at line 261 of file gps_helper.h.
|
protected |
Definition at line 262 of file gps_helper.h.
|
protected |
Definition at line 270 of file gps_helper.h.
Referenced by resetUpdateRates(), and storeUpdateRates().
|
protected |
Definition at line 264 of file gps_helper.h.
Referenced by GPSDriverEmlidReach::handleErbSentence(), GPSDriverAshtech::handleMessage(), GPSDriverMTK::handleMessage(), GPSDriverSBF::payloadRxDone(), GPSDriverUBX::payloadRxDone(), resetUpdateRates(), and storeUpdateRates().
|
protected |
Definition at line 265 of file gps_helper.h.
Referenced by GPSDriverEmlidReach::handleErbSentence(), GPSDriverAshtech::handleMessage(), GPSDriverMTK::handleMessage(), GPSDriverSBF::payloadRxDone(), GPSDriverUBX::payloadRxDone(), resetUpdateRates(), and storeUpdateRates().
|
protected |
Definition at line 267 of file gps_helper.h.
Referenced by storeUpdateRates().
|
protected |
Definition at line 268 of file gps_helper.h.
Referenced by storeUpdateRates().