From 494174bf53f8d2af7ed0a01395fcdbc0c8c1d7c7 Mon Sep 17 00:00:00 2001 From: bys1123 Date: Mon, 27 Apr 2020 23:17:19 +0800 Subject: [PATCH] Add NMEA GPS Driver --- src/drivers/gps/CMakeLists.txt | 1 + src/drivers/gps/gps.cpp | 93 +++++++++++++++++++++++++++++++--- src/drivers/gps/params.c | 55 +++++++++++++++++++- 3 files changed, 139 insertions(+), 10 deletions(-) diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index f07fac969c62..2584a414b3f5 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -46,6 +46,7 @@ px4_add_module( devices/src/ubx.cpp devices/src/rtcm.cpp devices/src/emlid_reach.cpp + devices/src/nmea.cpp MODULE_CONFIG module.yaml DEPENDS diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 37712a3f1f41..078e7105d559 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -63,6 +63,7 @@ #include "devices/src/emlid_reach.h" #include "devices/src/mtk.h" #include "devices/src/ubx.h" +#include "devices/src/nmea.h" #ifdef __PX4_LINUX #include @@ -72,11 +73,12 @@ #define RATE_MEASUREMENT_PERIOD 5000000 typedef enum { - GPS_DRIVER_MODE_NONE = 0, + GPS_DRIVER_MODE_AUTO = 0, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_MTK, GPS_DRIVER_MODE_ASHTECH, - GPS_DRIVER_MODE_EMLIDREACH + GPS_DRIVER_MODE_EMLIDREACH, + GPS_DRIVER_MODE_NMEA } gps_driver_mode_t; /* struct for dynamic allocation of satellite info data */ @@ -275,7 +277,7 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); } - _mode_auto = mode == GPS_DRIVER_MODE_NONE; + _mode_auto = mode == GPS_DRIVER_MODE_AUTO; } GPS::~GPS() @@ -694,7 +696,7 @@ GPS::run() } switch (_mode) { - case GPS_DRIVER_MODE_NONE: + case GPS_DRIVER_MODE_AUTO: _mode = GPS_DRIVER_MODE_UBX; /* FALLTHROUGH */ @@ -715,6 +717,15 @@ GPS::run() _helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; + case GPS_DRIVER_MODE_NMEA: + int32_t param_nmea_baud = 38400; + param_t nmea_baud = param_find("GPS_NMEA_BAUD"); + if (nmea_baud != PARAM_INVALID) { + param_get(nmea_baud, ¶m_nmea_baud); + } + _helper = new GPSDriverNMEA(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, param_nmea_baud); + break; + default: break; } @@ -728,7 +739,8 @@ GPS::run() _report_gps_pos.heading = NAN; _report_gps_pos.heading_offset = heading_offset; - if (_mode == GPS_DRIVER_MODE_UBX) { + if ((_mode == GPS_DRIVER_MODE_UBX)|| + (_mode == GPS_DRIVER_MODE_NMEA)) { /* GPS is obviously detected successfully, reset statistics */ _helper->resetUpdateRates(); @@ -814,6 +826,10 @@ GPS::run() break; case GPS_DRIVER_MODE_EMLIDREACH: + _mode = GPS_DRIVER_MODE_NMEA; + break; + + case GPS_DRIVER_MODE_NMEA: _mode = GPS_DRIVER_MODE_UBX; px4_usleep(500000); // tried all possible drivers. Wait a bit before next round break; @@ -1043,7 +1059,7 @@ Initiate warm restart of GPS device PRINT_MODULE_USAGE_PARAM_FLAG('s', "Enable publication of satellite info", true); PRINT_MODULE_USAGE_PARAM_STRING('i', "uart", "spi|uart", "GPS interface", true); - PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml", "GPS Protocol (default=auto select)", true); + PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml|nmea", "GPS Protocol (default=auto select)", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset GPS device"); @@ -1115,7 +1131,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) bool fake_gps = false; bool enable_sat_info = false; GPSHelper::Interface interface = GPSHelper::Interface::UART; - gps_driver_mode_t mode = GPS_DRIVER_MODE_NONE; + gps_driver_mode_t mode = GPS_DRIVER_MODE_AUTO; bool error_flag = false; int myoptind = 1; @@ -1179,6 +1195,9 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) } else if (!strcmp(myoptarg, "eml")) { mode = GPS_DRIVER_MODE_EMLIDREACH; + } else if(!strcmp(myoptarg, "nmea")) { + mode = GPS_DRIVER_MODE_NMEA; + } else { PX4_ERR("unknown interface: %s", myoptarg); error_flag = true; @@ -1202,6 +1221,35 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) GPS *gps; if (instance == Instance::Main) { + int32_t gps1_protocol_type = 0; + param_t gps1_type = param_find("GPS1_PROTOCOL"); + if (gps1_type != PARAM_INVALID) { + param_get(gps1_type, &gps1_protocol_type); + } + switch (gps1_protocol_type) + { + case 0:// GPS_DRIVER_MODE_AUTO + mode = GPS_DRIVER_MODE_AUTO; + break; + case 1: //GPS_DRIVER_MODE_UBX + mode = GPS_DRIVER_MODE_UBX; + break; + case 2: //GPS_DRIVER_MODE_MTK + mode = GPS_DRIVER_MODE_MTK; + break; + case 3: //GPS_DRIVER_MODE_ASHTECH + mode = GPS_DRIVER_MODE_ASHTECH; + break; + case 4: //GPS_DRIVER_MODE_EMLIDREACH + mode = GPS_DRIVER_MODE_EMLIDREACH; + break; + case 5: //GPS_DRIVER_MODE_NMEA + mode = GPS_DRIVER_MODE_NMEA; + break; + default: + break; + } + gps = new GPS(device_name, mode, interface, fake_gps, enable_sat_info, instance, baudrate_main); if (gps && device_name_secondary) { @@ -1220,6 +1268,35 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) } } } else { // secondary instance + int32_t gps2_protocol_type = 0; + param_t gps2_type = param_find("GPS2_PROTOCOL"); + if (gps2_type != PARAM_INVALID) { + param_get(gps2_type, &gps2_protocol_type); + } + switch (gps2_protocol_type) + { + case 0:// GPS_DRIVER_MODE_AUTO + mode = GPS_DRIVER_MODE_AUTO; + break; + case 1: //GPS_DRIVER_MODE_UBX + mode = GPS_DRIVER_MODE_UBX; + break; + case 2: //GPS_DRIVER_MODE_MTK + mode = GPS_DRIVER_MODE_MTK; + break; + case 3: //GPS_DRIVER_MODE_ASHTECH + mode = GPS_DRIVER_MODE_ASHTECH; + break; + case 4: //GPS_DRIVER_MODE_EMLIDREACH + mode = GPS_DRIVER_MODE_EMLIDREACH; + break; + case 5: //GPS_DRIVER_MODE_NMEA + mode = GPS_DRIVER_MODE_NMEA; + break; + default: + break; + } + gps = new GPS(device_name_secondary, mode, interface, fake_gps, enable_sat_info, instance, baudrate_secondary); } diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c index ee105aff2b1f..4cab1599b627 100644 --- a/src/drivers/gps/params.c +++ b/src/drivers/gps/params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -64,7 +64,6 @@ PARAM_DEFINE_INT32(GPS_DUMP_COMM, 0); */ PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7); - /** * Heading/Yaw offset for dual antenna GPS * @@ -86,3 +85,55 @@ PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7); */ PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f); + +/** + * NMEA GPS baudrate + * + * This parameter is used to set the NMEA GPS driver's baudrate + * + * @min 0 + * @max 6460800 + * + * @reboot_required true + * + * @group GPS + */ +PARAM_DEFINE_INT32(GPS_NMEA_BAUD, 38400); + +/** +* GPS1 ptotocol type +* +* This parameter is used to set if use manual gps mode or auto gps mode +* +* @min 0 +* @max 5 +* @value 0 set gps mode to GPS_DRIVER_MODE_AUTO +* @value 1 set gps mode to GPS_DRIVER_MODE_UBX +* @value 2 set gps mode to GPS_DRIVER_MODE_MTK +* @value 3 set gps mode to GPS_DRIVER_MODE_ASHTECH +* @value 4 set gps mode to GPS_DRIVER_MODE_EMLIDREACH +* @value 5 set gps mode to GPS_DRIVER_MODE_NMEA +* @reboot_required true +* +* @group GPS +*/ +PARAM_DEFINE_INT32(GPS_1_PROTOCOL, 0); + +/** +* GPS2 ptotocol type +* +* This parameter is used to set if use manual gps mode or auto gps mode +* +* @min 0 +* @max 5 +* @value 0 set gps mode to GPS_DRIVER_MODE_AUTO +* @value 1 set gps mode to GPS_DRIVER_MODE_UBX +* @value 2 set gps mode to GPS_DRIVER_MODE_MTK +* @value 3 set gps mode to GPS_DRIVER_MODE_ASHTECH +* @value 4 set gps mode to GPS_DRIVER_MODE_EMLIDREACH +* @value 5 set gps mode to GPS_DRIVER_MODE_NMEA +* @reboot_required true +* +* @group GPS +*/ +PARAM_DEFINE_INT32(GPS_2_PROTOCOL, 0);