From 556851a5115cc61624601ccaff38b5894ae364e8 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Mon, 18 Apr 2016 16:30:28 +0200 Subject: [PATCH] applying dual gps patch resolve transfer errors reformatting implementing multi-topics --- src/drivers/gps/gps.cpp | 167 +++++++++++++++++++-------- src/modules/sdlog2/sdlog2.c | 26 ++++- src/modules/sdlog2/sdlog2_messages.h | 2 + 3 files changed, 148 insertions(+), 47 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 1b12a9dd09..0e079bbaca 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -100,7 +100,7 @@ public: class GPS { public: - GPS(const char *uart_path, bool fake_gps, bool enable_sat_info); + GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num); virtual ~GPS(); virtual int init(); @@ -124,13 +124,15 @@ private: GPSHelper *_helper; ///< instance of GPS parser GPS_Sat_Info *_sat_info; ///< instance of GPS sat info data object struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position - orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position + orb_advert_t _report_gps_pos_pub[2]; ///< uORB pub for gps position + int _gps_orb_instance[2]; ///< uORB multi-topic instance struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate float _rate_rtcm_injection; ///< RTCM message injection rate unsigned _last_rate_rtcm_injection_count; ///< counter for number of RTCM messages bool _fake_gps; ///< fake gps output + int _gps_num; ///< number of GPS connected static const int _orb_inject_data_fd_count = 4; int _orb_inject_data_fd[_orb_inject_data_fd_count]; @@ -161,6 +163,11 @@ private: */ void cmd_reset(); + /** + * Publish the gps struct + */ + void publish(); + /** * This is an abstraction for the poll on serial used. * @@ -211,20 +218,23 @@ GPS *g_dev = nullptr; } -GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : + +GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num) : _task_should_exit(false), _healthy(false), _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _helper(nullptr), _sat_info(nullptr), - _report_gps_pos_pub(nullptr), + _report_gps_pos_pub{ nullptr, nullptr}, + _gps_orb_instance{ -1, -1}, _p_report_sat_info(nullptr), _report_sat_info_pub(nullptr), _rate(0.0f), _rate_rtcm_injection(0.0f), _last_rate_rtcm_injection_count(0), - _fake_gps(fake_gps) + _fake_gps(fake_gps), + _gps_num(gps_num) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -566,12 +576,7 @@ GPS::task_main() /* no time and satellite information simulated */ - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } + publish(); usleep(2e5); @@ -628,12 +633,7 @@ GPS::task_main() _report_gps_pos.epv = 10000.0f; _report_gps_pos.fix_type = 0; - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } + publish(); /* GPS is obviously detected successfully, reset statistics */ _helper->resetUpdateRates(); @@ -644,23 +644,13 @@ GPS::task_main() while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { if (helper_ret & 1) { - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } + publish(); last_rate_count++; } if (_p_report_sat_info && (helper_ret & 2)) { - if (_report_sat_info_pub != nullptr) { - orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); - - } else { - _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); - } + publish(); } /* measure update rate every 5 seconds */ @@ -810,6 +800,29 @@ GPS::print_info() usleep(100000); } +void +GPS::publish() +{ + if (_gps_num == 1) { + if (_report_gps_pos_pub[0] != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos); + + } else { + _report_gps_pos_pub[0] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[0], ORB_PRIO_DEFAULT); + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos); + } + + } else { + if (_report_gps_pos_pub[1] != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos); + + } else { + _report_gps_pos_pub[1] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[1], ORB_PRIO_DEFAULT); + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos); + } + } +} + /** * Local functions in support of the shell command. */ @@ -817,8 +830,9 @@ namespace gps { GPS *g_dev = nullptr; +GPS *g_dev2 = nullptr; -void start(const char *uart_path, bool fake_gps, bool enable_sat_info); +void start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num); void stop(); void test(); void reset(); @@ -828,34 +842,66 @@ void info(); * Start the driver. */ void -start(const char *uart_path, bool fake_gps, bool enable_sat_info) +start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num) { - if (g_dev != nullptr) { - PX4_WARN("gps already started"); + if (gps_num == 1) { + if (g_dev != nullptr) { + PX4_WARN("GPS 1 already started"); + } + + /* create the driver */ + g_dev = new GPS(uart_path, fake_gps, enable_sat_info, gps_num); + + if (g_dev == nullptr) { + goto fail1; + } + + if (OK != g_dev->init()) { + goto fail1; + } + return; - } - /* create the driver */ - g_dev = new GPS(uart_path, fake_gps, enable_sat_info); + } else { + if (gps_num == 2) { + if (g_dev2 != nullptr) { + PX4_WARN("GPS 2 already started"); + } - if (g_dev == nullptr) { - goto fail; - } + /* create the driver */ + g_dev2 = new GPS(uart_path, fake_gps, enable_sat_info, gps_num); + + if (g_dev2 == nullptr) { + goto fail2; + } + + if (OK != g_dev2->init()) { + goto fail2; + } - if (OK != g_dev->init()) { - goto fail; + return; + } } - return; -fail: +fail1: if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } - PX4_ERR("start failed"); + PX4_ERR("start of GPS 1 failed"); + return; + +fail2: + + if (g_dev2 != nullptr) { + delete g_dev2; + g_dev2 = nullptr; + } + + PX4_ERR("start of GPS 2 failed"); return; } @@ -867,6 +913,13 @@ stop() { delete g_dev; g_dev = nullptr; + + if (g_dev2 != nullptr) { + delete g_dev2; + } + g_dev2 = nullptr; + + px4_task_exit(0); } /** @@ -903,6 +956,12 @@ info() } g_dev->print_info(); + + if (g_dev2 != nullptr) { + g_dev2->print_info(); + } + + return; } } // namespace @@ -913,6 +972,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ const char *device_name = GPS_DEFAULT_UART_PORT; + const char *device_name2 = nullptr; bool fake_gps = false; bool enable_sat_info = false; @@ -949,7 +1009,24 @@ gps_main(int argc, char *argv[]) } } - gps::start(device_name, fake_gps, enable_sat_info); + /* Allow to use a second gps device */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-dualgps")) { + if (argc > i + 1) { + device_name2 = argv[i + 1]; + + } else { + PX4_ERR("Did not get second device address"); + } + } + } + + gps::start(device_name, fake_gps, enable_sat_info, 1); + + if (!(device_name2 == nullptr)) { + gps::start(device_name2, fake_gps, enable_sat_info, 2); + } + } if (!strcmp(argv[1], "stop")) { diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 351ad1da59..359dbec7b5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1188,6 +1188,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct ekf2_replay_s replay; struct vehicle_land_detected_s land_detected; struct cpuload_s cpuload; + struct vehicle_gps_position_s dual_gps_pos; } buf; memset(&buf, 0, sizeof(buf)); @@ -1368,7 +1369,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (log_on_start) { /* check GPS topic to get GPS time */ if (log_name_timestamp) { - if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { + if (!copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos)) { gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; } } @@ -1482,7 +1483,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GPS POSITION - LOG MANAGEMENT --- */ - bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos); + bool gps_pos_updated = copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; @@ -1718,6 +1719,27 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPS); } + /* --- GPS POSITION - UNIT #2 --- */ + if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub, &buf.dual_gps_pos)) { + log_msg.msg_type = LOG_GPS_MSG; + log_msg.body.log_GPS.gps_time = buf.dual_gps_pos.time_utc_usec; + log_msg.body.log_GPS.fix_type = buf.dual_gps_pos.fix_type; + log_msg.body.log_GPS.eph = buf.dual_gps_pos.eph; + log_msg.body.log_GPS.epv = buf.dual_gps_pos.epv; + log_msg.body.log_GPS.lat = buf.dual_gps_pos.lat; + log_msg.body.log_GPS.lon = buf.dual_gps_pos.lon; + log_msg.body.log_GPS.alt = buf.dual_gps_pos.alt * 0.001f; + log_msg.body.log_GPS.vel_n = buf.dual_gps_pos.vel_n_m_s; + log_msg.body.log_GPS.vel_e = buf.dual_gps_pos.vel_e_m_s; + log_msg.body.log_GPS.vel_d = buf.dual_gps_pos.vel_d_m_s; + log_msg.body.log_GPS.cog = buf.dual_gps_pos.cog_rad; + log_msg.body.log_GPS.sats = buf.dual_gps_pos.satellites_used; + log_msg.body.log_GPS.snr_mean = snr_mean; + log_msg.body.log_GPS.noise_per_ms = buf.dual_gps_pos.noise_per_ms; + log_msg.body.log_GPS.jamming_indicator = buf.dual_gps_pos.jamming_indicator; + LOGBUFFER_WRITE_AND_COUNT(GPS); + } + /* --- SATELLITE INFO - UNIT #1 --- */ if (_extended_logging) { diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 12942fde79..71913d9041 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -146,6 +146,7 @@ struct log_LPSP_s { /* --- GPS - GPS POSITION --- */ #define LOG_GPS_MSG 8 +#define LOG_DGPS_MSG 58 struct log_GPS_s { uint64_t gps_time; uint8_t fix_type; @@ -647,6 +648,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffffffffff", "X,Y,Z,Yaw,VX,VY,VZ,AX,AY,AZ"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), + LOG_FORMAT_S(DGPS, GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBB", "MainState,NavState,ArmS,Failsafe"),