PX4 Firmware
PX4 Autopilot Software http://px4.io
GPS Class Reference
Inheritance diagram for GPS:
Collaboration diagram for GPS:

Public Types

enum  Instance : uint8_t { Instance::Main = 0, Instance::Secondary, Instance::Count }
 The GPS allows to run multiple instances. More...
 

Public Member Functions

 GPS (const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps, bool enable_sat_info, Instance instance, unsigned configured_baudrate)
 
 ~GPS () override
 
void run () override
 
int print_status () override
 Diagnostics - print some basic information about the driver. More...
 
void schedule_reset (GPSRestartType restart_type)
 Schedule reset of the GPS device. More...
 
void reset_if_scheduled ()
 Reset device if reset was scheduled. More...
 

Static Public Member Functions

static int task_spawn (int argc, char *argv[])
 
static int task_spawn (int argc, char *argv[], Instance instance)
 spawn task and select the instance More...
 
static GPSinstantiate (int argc, char *argv[])
 
static GPSinstantiate (int argc, char *argv[], Instance instance)
 
static int custom_command (int argc, char *argv[])
 
static int print_usage (const char *reason=nullptr)
 
static int run_trampoline_secondary (int argc, char *argv[])
 task spawn trampoline for the secondary GPS More...
 

Private Member Functions

void publish ()
 Publish the gps struct. More...
 
void publishSatelliteInfo ()
 Publish the satellite info. More...
 
int pollOrRead (uint8_t *buf, size_t buf_length, int timeout)
 This is an abstraction for the poll on serial used. More...
 
void handleInjectDataTopic ()
 check for new messages on the inject data topic & handle them More...
 
bool injectData (uint8_t *data, size_t len)
 send data to the device, such as an RTCM stream More...
 
int setBaudrate (unsigned baud)
 set the Baudrate More...
 
void dumpGpsData (uint8_t *data, size_t len, bool msg_to_gps_device)
 Dump gps communication. More...
 
void initializeCommunicationDump ()
 

Static Private Member Functions

static int callback (GPSCallbackType type, void *data1, int data2, void *user)
 callback from the driver for the platform specific stuff More...
 

Private Attributes

int _serial_fd {-1}
 serial interface to GPS More...
 
unsigned _baudrate {0}
 current baudrate More...
 
const unsigned _configured_baudrate {0}
 configured baudrate (0=auto-detect) More...
 
char _port [20] {}
 device / serial port path More...
 
bool _healthy {false}
 flag to signal if the GPS is ok More...
 
bool _mode_auto
 if true, auto-detect which GPS is attached More...
 
gps_driver_mode_t _mode
 current mode More...
 
GPSHelper::Interface _interface
 interface More...
 
GPSHelper_helper {nullptr}
 instance of GPS parser More...
 
GPS_Sat_Info_sat_info {nullptr}
 instance of GPS sat info data object More...
 
vehicle_gps_position_s _report_gps_pos {}
 uORB topic for gps position More...
 
satellite_info_s_p_report_sat_info {nullptr}
 pointer to uORB topic for satellite info More...
 
orb_advert_t _report_gps_pos_pub {nullptr}
 uORB pub for gps position More...
 
orb_advert_t _report_sat_info_pub {nullptr}
 uORB pub for satellite info More...
 
int _gps_orb_instance {-1}
 uORB multi-topic instance More...
 
int _gps_sat_orb_instance {-1}
 uORB multi-topic instance for satellite info More...
 
float _rate {0.0f}
 position update rate More...
 
float _rate_rtcm_injection {0.0f}
 RTCM message injection rate. More...
 
unsigned _last_rate_rtcm_injection_count {0}
 counter for number of RTCM messages More...
 
const bool _fake_gps
 fake gps output More...
 
const Instance _instance
 
uORB::Subscription _orb_inject_data_sub {ORB_ID(gps_inject_data)}
 
uORB::PublicationQueued< gps_dump_s_dump_communication_pub {ORB_ID(gps_dump)}
 
gps_dump_s_dump_to_device {nullptr}
 
gps_dump_s_dump_from_device {nullptr}
 
bool _should_dump_communication {false}
 if true, dump communication More...
 
volatile GPSRestartType _scheduled_reset {GPSRestartType::None}
 

Static Private Attributes

static volatile bool _is_gps_main_advertised = false
 for the second gps we want to make sure that it gets instance 1 More...
 
static volatile GPS_secondary_instance = nullptr
 and thus we wait until the first one publishes at least one message. More...
 

Detailed Description

Definition at line 89 of file gps.cpp.

Member Enumeration Documentation

◆ Instance

enum GPS::Instance : uint8_t
strong

The GPS allows to run multiple instances.

Enumerator
Main 
Secondary 
Count 

Definition at line 94 of file gps.cpp.

Constructor & Destructor Documentation

◆ GPS()

GPS::GPS ( const char *  path,
gps_driver_mode_t  mode,
GPSHelper::Interface  interface,
bool  fake_gps,
bool  enable_sat_info,
Instance  instance,
unsigned  configured_baudrate 
)

Definition at line 257 of file gps.cpp.

References GPS_Sat_Info::_data, _mode_auto, _p_report_sat_info, _port, _report_gps_pos, _sat_info, GPS_DRIVER_MODE_NONE, vehicle_gps_position_s::heading, and vehicle_gps_position_s::heading_offset.

Referenced by instantiate().

Here is the caller graph for this function:

◆ ~GPS()

GPS::~GPS ( )
override

Definition at line 283 of file gps.cpp.

References _dump_from_device, _dump_to_device, _instance, _sat_info, _secondary_instance, and Main.

Member Function Documentation

◆ callback()

int GPS::callback ( GPSCallbackType  type,
void *  data1,
int  data2,
void *  user 
)
staticprivate

callback from the driver for the platform specific stuff

Definition at line 313 of file gps.cpp.

References _serial_fd, dumpGpsData(), gotRTCMMessage, gps, pollOrRead(), readDeviceData, setBaudrate, setBaudrate(), setClock, surveyInStatus, write(), and writeDeviceData.

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ custom_command()

int GPS::custom_command ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 971 of file gps.cpp.

References _instance, Cold, Hot, is_running(), print_usage(), schedule_reset(), and Warm.

Here is the call graph for this function:

◆ dumpGpsData()

void GPS::dumpGpsData ( uint8_t *  data,
size_t  len,
bool  msg_to_gps_device 
)
private

Dump gps communication.

Parameters
datamessage
lenlength of the message
msg_to_gps_deviceif true, this is a message sent to the gps device, otherwise it's from the device

Definition at line 572 of file gps.cpp.

References _dump_communication_pub, _dump_from_device, _dump_to_device, _should_dump_communication, gps_dump_s::data, hrt_absolute_time(), gps_dump_s::len, uORB::PublicationQueued< T >::publish(), and gps_dump_s::timestamp.

Referenced by callback(), and injectData().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ handleInjectDataTopic()

void GPS::handleInjectDataTopic ( )
private

check for new messages on the inject data topic & handle them

Definition at line 418 of file gps.cpp.

References _last_rate_rtcm_injection_count, _orb_inject_data_sub, uORB::Subscription::copy(), gps_inject_data_s::data, injectData(), gps_inject_data_s::len, msg, and uORB::Subscription::updated().

Referenced by pollOrRead().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ initializeCommunicationDump()

void GPS::initializeCommunicationDump ( )
private

Definition at line 541 of file gps.cpp.

References _dump_communication_pub, _dump_from_device, _dump_to_device, _should_dump_communication, param_find(), param_get(), PARAM_INVALID, and uORB::PublicationQueued< T >::publish().

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ injectData()

bool GPS::injectData ( uint8_t *  data,
size_t  len 
)
inlineprivate

send data to the device, such as an RTCM stream

Parameters
data
len

Definition at line 448 of file gps.cpp.

References _serial_fd, dumpGpsData(), and write().

Referenced by handleInjectDataTopic().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ instantiate() [1/2]

GPS * GPS::instantiate ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 1108 of file gps.cpp.

References Main.

Referenced by run_trampoline_secondary().

Here is the caller graph for this function:

◆ instantiate() [2/2]

GPS * GPS::instantiate ( int  argc,
char *  argv[],
Instance  instance 
)
static

Definition at line 1113 of file gps.cpp.

References _secondary_instance, device_name, gps, GPS(), GPS_DRIVER_MODE_ASHTECH, GPS_DRIVER_MODE_EMLIDREACH, GPS_DRIVER_MODE_MTK, GPS_DRIVER_MODE_NONE, GPS_DRIVER_MODE_UBX, Main, Secondary, GPSHelper::SPI, task_spawn(), and GPSHelper::UART.

Here is the call graph for this function:

◆ pollOrRead()

int GPS::pollOrRead ( uint8_t *  buf,
size_t  buf_length,
int  timeout 
)
private

This is an abstraction for the poll on serial used.

Parameters
bufpointer to read buffer
buf_lengthsize of read buffer
timeouttimeout in ms
Returns
: 0 for nothing read, or poll timed out < 0 for error > 0 number of bytes read

Definition at line 352 of file gps.cpp.

References _baudrate, _serial_fd, uart_esc::baudrate, err, handleInjectDataTopic(), math::min(), and read().

Referenced by callback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ print_status()

int GPS::print_status ( )
override

Diagnostics - print some basic information about the driver.

Definition at line 845 of file gps.cpp.

References _baudrate, _fake_gps, _healthy, _helper, _instance, _mode, _p_report_sat_info, _port, _rate, _rate_rtcm_injection, _report_gps_pos, _secondary_instance, GPSHelper::getPositionUpdateRate(), GPSHelper::getVelocityUpdateRate(), GPS_DRIVER_MODE_ASHTECH, GPS_DRIVER_MODE_EMLIDREACH, GPS_DRIVER_MODE_MTK, GPS_DRIVER_MODE_UBX, Main, print_status(), Secondary, and vehicle_gps_position_s::timestamp.

Referenced by print_status().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ print_usage()

int GPS::print_usage ( const char *  reason = nullptr)
static
See also
ModuleBase

Definition at line 1007 of file gps.cpp.

Referenced by custom_command().

Here is the caller graph for this function:

◆ publish()

void GPS::publish ( )
private

Publish the gps struct.

Definition at line 946 of file gps.cpp.

References _gps_orb_instance, _instance, _is_gps_main_advertised, _report_gps_pos, _report_gps_pos_pub, vehicle_gps_position_s::heading, Main, ORB_ID, ORB_PRIO_DEFAULT, and orb_publish_auto().

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ publishSatelliteInfo()

void GPS::publishSatelliteInfo ( )
private

Publish the satellite info.

Definition at line 959 of file gps.cpp.

References _gps_sat_orb_instance, _instance, _p_report_sat_info, _report_sat_info_pub, Main, ORB_ID, ORB_PRIO_DEFAULT, and orb_publish_auto().

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ reset_if_scheduled()

void GPS::reset_if_scheduled ( )

Reset device if reset was scheduled.

Definition at line 925 of file gps.cpp.

References _helper, _scheduled_reset, None, and GPSHelper::reset().

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ run()

void GPS::run ( )
override
See also
ModuleBase::run()

Definition at line 605 of file gps.cpp.

References _baudrate, _configured_baudrate, _fake_gps, _healthy, _helper, _interface, _last_rate_rtcm_injection_count, _mode, _mode_auto, _p_report_sat_info, _port, _rate, _rate_rtcm_injection, _report_gps_pos, _report_gps_pos_pub, _serial_fd, vehicle_gps_position_s::alt, vehicle_gps_position_s::alt_ellipsoid, vehicle_gps_position_s::c_variance_rad, callback(), vehicle_gps_position_s::cog_rad, GPSHelper::configure(), dt, vehicle_gps_position_s::eph, vehicle_gps_position_s::epv, f(), vehicle_gps_position_s::fix_type, GPSHelper::GPS, GPS_DRIVER_MODE_ASHTECH, GPS_DRIVER_MODE_EMLIDREACH, GPS_DRIVER_MODE_MTK, GPS_DRIVER_MODE_NONE, GPS_DRIVER_MODE_UBX, vehicle_gps_position_s::hdop, vehicle_gps_position_s::heading, vehicle_gps_position_s::heading_offset, hrt_absolute_time(), initializeCommunicationDump(), vehicle_gps_position_s::lat, vehicle_gps_position_s::lon, orb_unadvertise(), param_find(), param_get(), PARAM_INVALID, publish(), publishSatelliteInfo(), math::radians(), RATE_MEASUREMENT_PERIOD, GPSHelper::receive(), reset_if_scheduled(), GPSHelper::resetUpdateRates(), vehicle_gps_position_s::s_variance_m_s, vehicle_gps_position_s::satellites_used, GPSHelper::SPI, GPSHelper::storeUpdateRates(), TIMEOUT_5HZ, vehicle_gps_position_s::timestamp, vehicle_gps_position_s::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, and matrix::wrap_pi().

Referenced by run_trampoline_secondary().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ run_trampoline_secondary()

int GPS::run_trampoline_secondary ( int  argc,
char *  argv[] 
)
static

task spawn trampoline for the secondary GPS

Definition at line 1089 of file gps.cpp.

References _secondary_instance, gps, instantiate(), run(), and Secondary.

Referenced by task_spawn().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ schedule_reset()

void GPS::schedule_reset ( GPSRestartType  restart_type)

Schedule reset of the GPS device.

Definition at line 914 of file gps.cpp.

References _instance, _scheduled_reset, _secondary_instance, Main, and schedule_reset().

Referenced by custom_command(), and schedule_reset().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setBaudrate()

int GPS::setBaudrate ( unsigned  baud)
private

set the Baudrate

Parameters
baud
Returns
0 on success, <0 on error

Definition at line 457 of file gps.cpp.

References _serial_fd, and GPS_ERR.

Referenced by callback().

Here is the caller graph for this function:

◆ task_spawn() [1/2]

int GPS::task_spawn ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 1059 of file gps.cpp.

References Main.

Referenced by instantiate().

Here is the caller graph for this function:

◆ task_spawn() [2/2]

int GPS::task_spawn ( int  argc,
char *  argv[],
Instance  instance 
)
static

spawn task and select the instance

Definition at line 1064 of file gps.cpp.

References Main, run_trampoline_secondary(), and TASK_STACK_SIZE.

Here is the call graph for this function:

Member Data Documentation

◆ _baudrate

unsigned GPS::_baudrate {0}
private

current baudrate

Definition at line 147 of file gps.cpp.

Referenced by pollOrRead(), print_status(), and run().

◆ _configured_baudrate

const unsigned GPS::_configured_baudrate {0}
private

configured baudrate (0=auto-detect)

Definition at line 148 of file gps.cpp.

Referenced by run().

◆ _dump_communication_pub

uORB::PublicationQueued<gps_dump_s> GPS::_dump_communication_pub {ORB_ID(gps_dump)}
private

Definition at line 179 of file gps.cpp.

Referenced by dumpGpsData(), and initializeCommunicationDump().

◆ _dump_from_device

gps_dump_s* GPS::_dump_from_device {nullptr}
private

Definition at line 181 of file gps.cpp.

Referenced by dumpGpsData(), initializeCommunicationDump(), and ~GPS().

◆ _dump_to_device

gps_dump_s* GPS::_dump_to_device {nullptr}
private

Definition at line 180 of file gps.cpp.

Referenced by dumpGpsData(), initializeCommunicationDump(), and ~GPS().

◆ _fake_gps

const bool GPS::_fake_gps
private

fake gps output

Definition at line 174 of file gps.cpp.

Referenced by print_status(), and run().

◆ _gps_orb_instance

int GPS::_gps_orb_instance {-1}
private

uORB multi-topic instance

Definition at line 167 of file gps.cpp.

Referenced by publish().

◆ _gps_sat_orb_instance

int GPS::_gps_sat_orb_instance {-1}
private

uORB multi-topic instance for satellite info

Definition at line 168 of file gps.cpp.

Referenced by publishSatelliteInfo().

◆ _healthy

bool GPS::_healthy {false}
private

flag to signal if the GPS is ok

Definition at line 151 of file gps.cpp.

Referenced by print_status(), and run().

◆ _helper

GPSHelper* GPS::_helper {nullptr}
private

instance of GPS parser

Definition at line 157 of file gps.cpp.

Referenced by print_status(), reset_if_scheduled(), and run().

◆ _instance

const Instance GPS::_instance
private

◆ _interface

GPSHelper::Interface GPS::_interface
private

interface

Definition at line 156 of file gps.cpp.

Referenced by run().

◆ _is_gps_main_advertised

volatile bool GPS::_is_gps_main_advertised = false
staticprivate

for the second gps we want to make sure that it gets instance 1

Definition at line 184 of file gps.cpp.

Referenced by publish().

◆ _last_rate_rtcm_injection_count

unsigned GPS::_last_rate_rtcm_injection_count {0}
private

counter for number of RTCM messages

Definition at line 172 of file gps.cpp.

Referenced by handleInjectDataTopic(), and run().

◆ _mode

gps_driver_mode_t GPS::_mode
private

current mode

Definition at line 154 of file gps.cpp.

Referenced by print_status(), and run().

◆ _mode_auto

bool GPS::_mode_auto
private

if true, auto-detect which GPS is attached

Definition at line 152 of file gps.cpp.

Referenced by GPS(), and run().

◆ _orb_inject_data_sub

uORB::Subscription GPS::_orb_inject_data_sub {ORB_ID(gps_inject_data)}
private

Definition at line 178 of file gps.cpp.

Referenced by handleInjectDataTopic().

◆ _p_report_sat_info

satellite_info_s* GPS::_p_report_sat_info {nullptr}
private

pointer to uORB topic for satellite info

Definition at line 162 of file gps.cpp.

Referenced by GPS(), print_status(), publishSatelliteInfo(), and run().

◆ _port

char GPS::_port[20] {}
private

device / serial port path

Definition at line 149 of file gps.cpp.

Referenced by GPS(), print_status(), and run().

◆ _rate

float GPS::_rate {0.0f}
private

position update rate

Definition at line 170 of file gps.cpp.

Referenced by print_status(), and run().

◆ _rate_rtcm_injection

float GPS::_rate_rtcm_injection {0.0f}
private

RTCM message injection rate.

Definition at line 171 of file gps.cpp.

Referenced by print_status(), and run().

◆ _report_gps_pos

vehicle_gps_position_s GPS::_report_gps_pos {}
private

uORB topic for gps position

Definition at line 161 of file gps.cpp.

Referenced by GPS(), print_status(), publish(), and run().

◆ _report_gps_pos_pub

orb_advert_t GPS::_report_gps_pos_pub {nullptr}
private

uORB pub for gps position

Definition at line 164 of file gps.cpp.

Referenced by publish(), and run().

◆ _report_sat_info_pub

orb_advert_t GPS::_report_sat_info_pub {nullptr}
private

uORB pub for satellite info

Definition at line 165 of file gps.cpp.

Referenced by publishSatelliteInfo().

◆ _sat_info

GPS_Sat_Info* GPS::_sat_info {nullptr}
private

instance of GPS sat info data object

Definition at line 159 of file gps.cpp.

Referenced by GPS(), and ~GPS().

◆ _scheduled_reset

volatile GPSRestartType GPS::_scheduled_reset {GPSRestartType::None}
private

Definition at line 189 of file gps.cpp.

Referenced by reset_if_scheduled(), and schedule_reset().

◆ _secondary_instance

volatile GPS * GPS::_secondary_instance = nullptr
staticprivate

and thus we wait until the first one publishes at least one message.

Definition at line 187 of file gps.cpp.

Referenced by instantiate(), print_status(), run_trampoline_secondary(), schedule_reset(), and ~GPS().

◆ _serial_fd

int GPS::_serial_fd {-1}
private

serial interface to GPS

Definition at line 146 of file gps.cpp.

Referenced by callback(), injectData(), pollOrRead(), run(), and setBaudrate().

◆ _should_dump_communication

bool GPS::_should_dump_communication {false}
private

if true, dump communication

Definition at line 182 of file gps.cpp.

Referenced by dumpGpsData(), and initializeCommunicationDump().


The documentation for this class was generated from the following file: