PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <mtk.h>
Public Member Functions | |
GPSDriverMTK (GPSCallbackPtr callback, void *callback_user, struct vehicle_gps_position_s *gps_position) | |
virtual | ~GPSDriverMTK ()=default |
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... | |
Public Member Functions inherited from GPSHelper | |
GPSHelper (GPSCallbackPtr callback, void *callback_user) | |
virtual | ~GPSHelper ()=default |
virtual int | reset (GPSRestartType restart_type) |
Reset GPS device. More... | |
float | getPositionUpdateRate () |
float | getVelocityUpdateRate () |
void | resetUpdateRates () |
void | storeUpdateRates () |
Private Member Functions | |
int | parseChar (uint8_t b, gps_mtk_packet_t &packet) |
Parse the binary MTK packet. More... | |
void | handleMessage (gps_mtk_packet_t &packet) |
Handle the package once it has arrived. More... | |
void | decodeInit () |
Reset the parse state machine for a fresh start. More... | |
void | addByteToChecksum (uint8_t) |
While parsing add every byte (except the sync bytes) to the checksum. More... | |
Private Attributes | |
struct vehicle_gps_position_s * | _gps_position {nullptr} |
mtk_decode_state_t | _decode_state {MTK_DECODE_UNINIT} |
uint8_t | _mtk_revision {0} |
unsigned | _rx_count {} |
uint8_t | _rx_ck_a {} |
uint8_t | _rx_ck_b {} |
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 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 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} |
GPSDriverMTK::GPSDriverMTK | ( | GPSCallbackPtr | callback, |
void * | callback_user, | ||
struct vehicle_gps_position_s * | gps_position | ||
) |
Definition at line 50 of file mtk.cpp.
References decodeInit().
|
virtualdefault |
|
private |
While parsing add every byte (except the sync bytes) to the checksum.
Definition at line 302 of file mtk.cpp.
References _rx_ck_a, and _rx_ck_b.
Referenced by parseChar().
|
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 58 of file mtk.cpp.
References GPSHelper::GPS, gps_usleep, GPS_WARN, MTK_BAUDRATE, MTK_NAVTHRES_OFF, MTK_OUTPUT_5HZ, MTK_SBAS_ON, MTK_SET_BINARY, MTK_WAAS_ON, GPSHelper::setBaudrate(), and GPSHelper::write().
|
private |
Reset the parse state machine for a fresh start.
Definition at line 156 of file mtk.cpp.
References _decode_state, _rx_ck_a, _rx_ck_b, _rx_count, and MTK_DECODE_UNINIT.
Referenced by GPSDriverMTK(), and parseChar().
|
private |
Handle the package once it has arrived.
Definition at line 217 of file mtk.cpp.
References _gps_position, _mtk_revision, GPSHelper::_rate_count_lat_lon, GPSHelper::_rate_count_vel, vehicle_gps_position_s::alt, vehicle_gps_position_s::cog_rad, gps_mtk_packet_t::date, vehicle_gps_position_s::eph, vehicle_gps_position_s::epv, f(), vehicle_gps_position_s::fix_type, gps_mtk_packet_t::fix_type, gps_absolute_time, GPS_EPOCH_SECS, GPS_WARN, gps_mtk_packet_t::ground_speed, vehicle_gps_position_s::hdop, gps_mtk_packet_t::hdop, gps_mtk_packet_t::heading, vehicle_gps_position_s::lat, gps_mtk_packet_t::latitude, vehicle_gps_position_s::lon, gps_mtk_packet_t::longitude, gps_mtk_packet_t::msl_altitude, gps_mtk_packet_t::satellites, vehicle_gps_position_s::satellites_used, GPSHelper::setClock(), vehicle_gps_position_s::time_utc_usec, vehicle_gps_position_s::timestamp, vehicle_gps_position_s::timestamp_time_relative, gps_mtk_packet_t::utc_time, vehicle_gps_position_s::vdop, and vehicle_gps_position_s::vel_m_s.
Referenced by receive().
|
private |
Parse the binary MTK packet.
Definition at line 164 of file mtk.cpp.
References _decode_state, _mtk_revision, _rx_ck_a, _rx_ck_b, _rx_count, addByteToChecksum(), gps_mtk_packet_t::ck_a, gps_mtk_packet_t::ck_b, decodeInit(), MTK_DECODE_GOT_CK_A, MTK_DECODE_GOT_CK_B, MTK_DECODE_UNINIT, MTK_SYNC1_V16, MTK_SYNC1_V19, and MTK_SYNC2.
Referenced by receive().
|
overridevirtual |
receive & handle new data from the device
timeout | [ms] |
Implements GPSHelper.
Definition at line 113 of file mtk.cpp.
References gps_absolute_time, GPS_READ_BUFFER_SIZE, gps_usleep, handleMessage(), parseChar(), and GPSHelper::read().
|
private |
Definition at line 119 of file mtk.h.
Referenced by decodeInit(), and parseChar().
|
private |
Definition at line 118 of file mtk.h.
Referenced by handleMessage().
|
private |
Definition at line 120 of file mtk.h.
Referenced by handleMessage(), and parseChar().
|
private |
Definition at line 122 of file mtk.h.
Referenced by addByteToChecksum(), decodeInit(), and parseChar().
|
private |
Definition at line 123 of file mtk.h.
Referenced by addByteToChecksum(), decodeInit(), and parseChar().
|
private |
Definition at line 121 of file mtk.h.
Referenced by decodeInit(), and parseChar().