40 #include <nuttx/clock.h> 41 #include <nuttx/arch.h> 51 #include <mathlib/mathlib.h> 53 #include <px4_platform_common/cli.h> 54 #include <px4_platform_common/getopt.h> 55 #include <px4_platform_common/module.h> 67 #include <linux/spi/spidev.h> 70 #define TIMEOUT_5HZ 500 71 #define RATE_MEASUREMENT_PERIOD 5000000 89 class GPS :
public ModuleBase<GPS>
106 static int task_spawn(
int argc,
char *argv[]);
109 static int task_spawn(
int argc,
char *argv[],
Instance instance);
112 static GPS *instantiate(
int argc,
char *argv[]);
114 static GPS *instantiate(
int argc,
char *argv[],
Instance instance);
117 static int custom_command(
int argc,
char *argv[]);
120 static int print_usage(
const char *reason =
nullptr);
125 static int run_trampoline_secondary(
int argc,
char *argv[]);
143 void reset_if_scheduled();
147 unsigned _baudrate{0};
148 const unsigned _configured_baudrate{0};
151 bool _healthy{
false};
167 int _gps_orb_instance{-1};
168 int _gps_sat_orb_instance{-1};
171 float _rate_rtcm_injection{0.0f};
172 unsigned _last_rate_rtcm_injection_count{0};
182 bool _should_dump_communication{
false};
199 void publishSatelliteInfo();
211 int pollOrRead(uint8_t *buf,
size_t buf_length,
int timeout);
216 void handleInjectDataTopic();
223 inline bool injectData(uint8_t *
data,
size_t len);
235 static int callback(
GPSCallbackType type,
void *data1,
int data2,
void *user);
243 void dumpGpsData(uint8_t *data,
size_t len,
bool msg_to_gps_device);
245 void initializeCommunicationDump();
259 _configured_baudrate(configured_baudrate),
261 _interface(interface),
274 if (enable_sat_info) {
288 secondary_instance->request_stop();
319 int num_read = gps->
pollOrRead((uint8_t *)data1, data2, *((
int *)data1));
322 gps->
dumpGpsData((uint8_t *)data1, (
size_t)num_read,
false);
329 gps->
dumpGpsData((uint8_t *)data1, (
size_t)data2,
true);
345 px4_clock_settime(CLOCK_REALTIME, (timespec *)data1);
356 #if !defined(__PX4_QURT) 366 const int max_timeout = 50;
370 fds[0].events = POLLIN;
372 int ret = poll(fds,
sizeof(fds) /
sizeof(fds[0]),
math::min(max_timeout, timeout));
376 if (fds[0].revents & POLLIN) {
384 const unsigned character_count = 32;
386 const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
390 int bytes_available = 0;
391 err = ioctl(
_serial_fd, FIONREAD, (
unsigned long)&bytes_available);
393 if (err != 0 || bytes_available < (
int)character_count) {
394 px4_usleep(sleeptime);
398 px4_usleep(sleeptime);
420 bool updated =
false;
426 const size_t max_num_injections = 6;
427 size_t num_injections = 0;
445 }
while (updated && num_injections < max_num_injections);
454 return written == len;
463 case 9600: speed = B9600;
break;
465 case 19200: speed = B19200;
break;
467 case 38400: speed = B38400;
break;
469 case 57600: speed = B57600;
break;
471 case 115200: speed = B115200;
break;
473 case 230400: speed = B230400;
break;
476 PX4_ERR(
"ERR: unknown baudrate: %d", baud);
480 struct termios uart_config;
497 uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
498 INLCR | PARMRK | INPCK | ISTRIP | IXON);
509 uart_config.c_oflag = 0;
517 uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
520 uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
523 if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
524 GPS_ERR(
"ERR: %d (cfsetispeed)", termios_state);
528 if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
529 GPS_ERR(
"ERR: %d (cfsetospeed)", termios_state);
533 if ((termios_state = tcsetattr(
_serial_fd, TCSANOW, &uart_config)) < 0) {
534 GPS_ERR(
"ERR: %d (tcsetattr)", termios_state);
544 int32_t param_dump_comm;
550 if (param_dump_comm != 1) {
558 PX4_ERR(
"failed to allocated dump data");
581 size_t write_len = len;
583 if (write_len >
sizeof(dump_data->
data) - dump_data->
len) {
584 write_len =
sizeof(dump_data->
data) - dump_data->
len;
587 memcpy(dump_data->
data + dump_data->
len, data, write_len);
589 dump_data->
len += write_len;
592 if (dump_data->
len >=
sizeof(dump_data->
data)) {
593 if (msg_to_gps_device) {
594 dump_data->
len |= 1 << 7;
612 PX4_ERR(
"GPS: failed to open serial port: %s err: %d",
_port, errno);
619 int spi_speed = 1000000;
620 int status_value = ioctl(
_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
622 if (status_value < 0) {
623 PX4_ERR(
"SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)",
_port, errno);
627 status_value = ioctl(
_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
629 if (status_value < 0) {
630 PX4_ERR(
"SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)",
_port, errno);
639 float heading_offset = 0.f;
646 int32_t gps_ubx_dynmodel = 7;
656 unsigned last_rate_count = 0;
659 while (!should_exit()) {
743 if (helper_ret & 1) {
849 PX4_INFO(
"Main GPS");
854 PX4_INFO(
"Secondary GPS");
863 PX4_INFO(
"protocol: SIMULATED");
868 PX4_INFO(
"protocol: UBX");
872 PX4_INFO(
"protocol: MTK");
876 PX4_INFO(
"protocol: ASHTECH");
880 PX4_INFO(
"protocol: EMLIDREACH");
898 PX4_INFO(
"rate publication:\t\t%6.2f Hz", (
double)
_rate);
934 PX4_INFO(
"Reset is not supported on this device.");
936 }
else if (res < 0) {
937 PX4_INFO(
"Reset failed.");
940 PX4_INFO(
"Reset succeeded.");
975 PX4_INFO(
"not running");
983 if (argc == 2 && !strcmp(argv[0],
"reset")) {
985 if (!strcmp(argv[1],
"hot")) {
989 }
else if (!strcmp(argv[1],
"cold")) {
993 }
else if (!strcmp(argv[1],
"warm")) {
1000 PX4_INFO(
"Resetting GPS - %s", argv[1]);
1004 return (res) ? 0 :
print_usage(
"unknown command");
1010 PX4_WARN(
"%s\n", reason);
1013 PRINT_MODULE_DESCRIPTION(
1016 GPS driver module that handles the communication with the device and publishes the position via uORB. 1017 It supports multiple protocols (device vendors) and by default automatically selects the correct one. 1019 The module supports a secondary GPS device, specified via `-e` parameter. The position will be published 1020 on the second uORB topic instance, but it's currently not used by the rest of the system (however the 1021 data will be logged, so that it can be used for comparisons). 1024 There is a thread for each device polling for data. The GPS protocol classes are implemented with callbacks 1025 so that they can be used in other projects as well (eg. QGroundControl uses them too). 1028 For testing it can be useful to fake a GPS signal (it will signal the system that it has a valid position): 1032 Starting 2 GPS devices (the main GPS on /dev/ttyS3 and the secondary on /dev/ttyS4): 1033 $ gps start -d /dev/ttyS3 -e /dev/ttyS4 1035 Initiate warm restart of GPS device 1039 PRINT_MODULE_USAGE_NAME("gps",
"driver");
1040 PRINT_MODULE_USAGE_COMMAND(
"start");
1041 PRINT_MODULE_USAGE_PARAM_STRING(
'd',
"/dev/ttyS3",
"<file:dev>",
"GPS device",
true);
1042 PRINT_MODULE_USAGE_PARAM_INT(
'b', 0, 0, 3000000,
"Baudrate (can also be p:<param_name>)",
true);
1043 PRINT_MODULE_USAGE_PARAM_STRING(
'e',
nullptr,
"<file:dev>",
"Optional secondary GPS device",
true);
1044 PRINT_MODULE_USAGE_PARAM_INT(
'g', 0, 0, 3000000,
"Baudrate (secondary GPS, can also be p:<param_name>)",
true);
1046 PRINT_MODULE_USAGE_PARAM_FLAG(
'f',
"Fake a GPS signal (useful for testing)",
true);
1047 PRINT_MODULE_USAGE_PARAM_FLAG(
's',
"Enable publication of satellite info",
true);
1049 PRINT_MODULE_USAGE_PARAM_STRING(
'i',
"uart",
"spi|uart",
"GPS interface",
true);
1050 PRINT_MODULE_USAGE_PARAM_STRING(
'p',
nullptr,
"ubx|mtk|ash|eml",
"GPS Protocol (default=auto select)",
true);
1052 PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
1053 PRINT_MODULE_USAGE_COMMAND_DESCR(
"reset",
"Reset GPS device");
1054 PRINT_MODULE_USAGE_ARG(
"cold|warm|hot",
"Specify reset type",
false);
1066 px4_main_t entry_point;
1068 entry_point = (px4_main_t)&run_trampoline;
1073 int task_id = px4_task_spawn_cmd(
"gps", SCHED_DEFAULT,
1075 entry_point, (
char *
const *)argv);
1116 const char *device_name_secondary =
nullptr;
1117 int baudrate_main = 0;
1118 int baudrate_secondary = 0;
1119 bool fake_gps =
false;
1120 bool enable_sat_info =
false;
1124 bool error_flag =
false;
1127 const char *myoptarg =
nullptr;
1129 while ((ch = px4_getopt(argc, argv,
"b:d:e:fg:si:p:", &myoptind, &myoptarg)) != EOF) {
1132 if (px4_get_parameter_value(myoptarg, baudrate_main) != 0) {
1133 PX4_ERR(
"baudrate parsing failed");
1138 if (px4_get_parameter_value(myoptarg, baudrate_secondary) != 0) {
1139 PX4_ERR(
"baudrate parsing failed");
1145 device_name = myoptarg;
1149 device_name_secondary = myoptarg;
1157 enable_sat_info =
true;
1161 if (!strcmp(myoptarg,
"spi")) {
1164 }
else if (!strcmp(myoptarg,
"uart")) {
1168 PX4_ERR(
"unknown interface: %s", myoptarg);
1174 if (!strcmp(myoptarg,
"ubx")) {
1177 }
else if (!strcmp(myoptarg,
"mtk")) {
1180 }
else if (!strcmp(myoptarg,
"ash")) {
1183 }
else if (!strcmp(myoptarg,
"eml")) {
1187 PX4_ERR(
"unknown interface: %s", myoptarg);
1197 PX4_WARN(
"unrecognized flag");
1209 gps =
new GPS(device_name, mode, interface, fake_gps, enable_sat_info, instance, baudrate_main);
1211 if (gps && device_name_secondary) {
1223 PX4_ERR(
"Timed out while waiting for thread to start");
1227 gps =
new GPS(device_name_secondary, mode, interface, fake_gps, enable_sat_info, instance, baudrate_secondary);
float getPositionUpdateRate()
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
bool _should_dump_communication
if true, dump communication
gps_dump_s * _dump_to_device
static volatile GPS * _secondary_instance
and thus we wait until the first one publishes at least one message.
GPSHelper::Interface _interface
interface
void initializeCommunicationDump()
virtual int configure(unsigned &baud, OutputMode output_mode)=0
configure the device
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
void handleInjectDataTopic()
check for new messages on the inject data topic & handle them
bool injectData(uint8_t *data, size_t len)
send data to the device, such as an RTCM stream
Instance
The GPS allows to run multiple instances.
const bool _fake_gps
fake gps output
int _serial_fd
serial interface to GPS
#define RATE_MEASUREMENT_PERIOD
int main(int argc, char **argv)
orb_advert_t _report_sat_info_pub
uORB pub for satellite info
satellite_info_s * _p_report_sat_info
pointer to uORB topic for satellite info
bool publish(const T &data)
Publish the struct.
static int callback(GPSCallbackType type, void *data1, int data2, void *user)
callback from the driver for the platform specific stuff
int pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
This is an abstraction for the poll on serial used.
static GPS * instantiate(int argc, char *argv[])
static constexpr int TASK_STACK_SIZE
static void print_usage()
virtual int reset(GPSRestartType restart_type)
Reset GPS device.
struct satellite_info_s _data
bool _healthy
flag to signal if the GPS is ok
GPS_Sat_Info * _sat_info
instance of GPS sat info data object
virtual int receive(unsigned timeout)=0
receive & handle new data from the device
float _rate
position update rate
void reset_if_scheduled()
Reset device if reset was scheduled.
Global flash based parameter store.
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
char _port[20]
device / serial port path
bool _mode_auto
if true, auto-detect which GPS is attached
static void read(bootloader_app_shared_t *pshared)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
static int run_trampoline_secondary(int argc, char *argv[])
task spawn trampoline for the secondary GPS
float _rate_rtcm_injection
RTCM message injection rate.
set Baudrate data1: ignored data2: baudrate return: 0 on success
volatile GPSRestartType _scheduled_reset
static int task_spawn(int argc, char *argv[])
orb_advert_t _report_gps_pos_pub
uORB pub for gps position
int _gps_orb_instance
uORB multi-topic instance
constexpr T radians(T degrees)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
U-Blox protocol definition.
Got an RTCM message from the device.
void publishSatelliteInfo()
Publish the satellite info.
In cold start mode, the receiver has no information from the last position at startup.
bool updated()
Check if there is a new update.
GPSHelper * _helper
instance of GPS parser
void publish()
Publish the gps struct.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
In warm start mode, the receiver has approximate information for time, position, and coarse satellite...
uORB::PublicationQueued< gps_dump_s > _dump_communication_pub
can be used to set the current clock accurately data1: pointer to a timespec struct data2: ignored re...
unsigned _baudrate
current baudrate
constexpr _Tp min(_Tp a, _Tp b)
Type wrap_pi(Type x)
Wrap value in range [-π, π)
unsigned _last_rate_rtcm_injection_count
counter for number of RTCM messages
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
static void write(bootloader_app_shared_t *pshared)
__EXPORT int gps_main(int argc, char *argv[])
void schedule_reset(GPSRestartType restart_type)
Schedule reset of the GPS device.
static int print_usage(const char *reason=nullptr)
static int custom_command(int argc, char *argv[])
static volatile bool _is_gps_main_advertised
for the second gps we want to make sure that it gets instance 1
const unsigned _configured_baudrate
configured baudrate (0=auto-detect)
int orb_unadvertise(orb_advert_t handle)
GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps, bool enable_sat_info, Instance instance, unsigned configured_baudrate)
In hot start mode, the receiver was powered down only for a short time (4 hours or less)...
int setBaudrate(unsigned baud)
set the Baudrate
Driver class for Emlid Reach Populates caller provided vehicle_gps_position_s Some ERB messages are c...
gps_driver_mode_t _mode
current mode
int print_status() override
Diagnostics - print some basic information about the driver.
int _gps_sat_orb_instance
uORB multi-topic instance for satellite info
bool copy(void *dst)
Copy the struct.
uORB::Subscription _orb_inject_data_sub
void dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
Dump gps communication.
vehicle_gps_position_s _report_gps_pos
uORB topic for gps position
message about current survey-in status data1: points to a SurveyInStatus struct data2: ignored return...
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
static int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, int priority)
Advertise as the publisher of a topic.
float getVelocityUpdateRate()
Write data to device data1: data to be written data2: number of bytes to write return: num written by...
gps_dump_s * _dump_from_device