PX4 Firmware
PX4 Autopilot Software http://px4.io
|
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 GPS * | instantiate (int argc, char *argv[]) |
static GPS * | instantiate (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... | |
|
strong |
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().
|
override |
Definition at line 283 of file gps.cpp.
References _dump_from_device, _dump_to_device, _instance, _sat_info, _secondary_instance, and Main.
|
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().
|
static |
Definition at line 971 of file gps.cpp.
References _instance, Cold, Hot, is_running(), print_usage(), schedule_reset(), and Warm.
|
private |
Dump gps communication.
data | message |
len | length of the message |
msg_to_gps_device | if 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().
|
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().
|
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().
|
inlineprivate |
send data to the device, such as an RTCM stream
data | |
len |
Definition at line 448 of file gps.cpp.
References _serial_fd, dumpGpsData(), and write().
Referenced by handleInjectDataTopic().
|
static |
Definition at line 1108 of file gps.cpp.
References Main.
Referenced by run_trampoline_secondary().
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.
|
private |
This is an abstraction for the poll on serial used.
buf | pointer to read buffer |
buf_length | size of read buffer |
timeout | timeout in ms |
Definition at line 352 of file gps.cpp.
References _baudrate, _serial_fd, uart_esc::baudrate, err, handleInjectDataTopic(), math::min(), and read().
Referenced by callback().
|
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().
|
static |
Definition at line 1007 of file gps.cpp.
Referenced by custom_command().
|
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().
|
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().
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().
|
override |
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().
|
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().
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().
|
private |
set the Baudrate
baud |
Definition at line 457 of file gps.cpp.
References _serial_fd, and GPS_ERR.
Referenced by callback().
|
static |
Definition at line 1059 of file gps.cpp.
References Main.
Referenced by instantiate().
|
static |
spawn task and select the instance
Definition at line 1064 of file gps.cpp.
References Main, run_trampoline_secondary(), and TASK_STACK_SIZE.
|
private |
current baudrate
Definition at line 147 of file gps.cpp.
Referenced by pollOrRead(), print_status(), and run().
|
private |
|
private |
Definition at line 179 of file gps.cpp.
Referenced by dumpGpsData(), and initializeCommunicationDump().
|
private |
Definition at line 181 of file gps.cpp.
Referenced by dumpGpsData(), initializeCommunicationDump(), and ~GPS().
|
private |
Definition at line 180 of file gps.cpp.
Referenced by dumpGpsData(), initializeCommunicationDump(), and ~GPS().
|
private |
|
private |
|
private |
uORB multi-topic instance for satellite info
Definition at line 168 of file gps.cpp.
Referenced by publishSatelliteInfo().
|
private |
flag to signal if the GPS is ok
Definition at line 151 of file gps.cpp.
Referenced by print_status(), and run().
|
private |
instance of GPS parser
Definition at line 157 of file gps.cpp.
Referenced by print_status(), reset_if_scheduled(), and run().
|
private |
Definition at line 176 of file gps.cpp.
Referenced by custom_command(), print_status(), publish(), publishSatelliteInfo(), schedule_reset(), and ~GPS().
|
private |
|
staticprivate |
|
private |
counter for number of RTCM messages
Definition at line 172 of file gps.cpp.
Referenced by handleInjectDataTopic(), and run().
|
private |
|
private |
|
private |
Definition at line 178 of file gps.cpp.
Referenced by handleInjectDataTopic().
|
private |
pointer to uORB topic for satellite info
Definition at line 162 of file gps.cpp.
Referenced by GPS(), print_status(), publishSatelliteInfo(), and run().
|
private |
device / serial port path
Definition at line 149 of file gps.cpp.
Referenced by GPS(), print_status(), and run().
|
private |
position update rate
Definition at line 170 of file gps.cpp.
Referenced by print_status(), and run().
|
private |
RTCM message injection rate.
Definition at line 171 of file gps.cpp.
Referenced by print_status(), and run().
|
private |
|
private |
|
private |
uORB pub for satellite info
Definition at line 165 of file gps.cpp.
Referenced by publishSatelliteInfo().
|
private |
|
private |
Definition at line 189 of file gps.cpp.
Referenced by reset_if_scheduled(), and schedule_reset().
|
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().
|
private |
serial interface to GPS
Definition at line 146 of file gps.cpp.
Referenced by callback(), injectData(), pollOrRead(), run(), and setBaudrate().
|
private |
if true, dump communication
Definition at line 182 of file gps.cpp.
Referenced by dumpGpsData(), and initializeCommunicationDump().