diff options
author | Kynos <mail01@delago.net> | 2014-06-13 14:05:47 +0200 |
---|---|---|
committer | Kynos <mail01@delago.net> | 2014-06-13 14:05:47 +0200 |
commit | b6b3ad6e1e31503d230ad6e7bc8f2d7b5b596ad7 (patch) | |
tree | 551eac9af3730169d6fcac80944400b526a1bae4 /src/drivers/gps/gps.cpp | |
parent | 07458b284d4cea48bc3b6e0cb5dcaba1c2c1d4d6 (diff) | |
download | px4-firmware-b6b3ad6e1e31503d230ad6e7bc8f2d7b5b596ad7.tar.gz px4-firmware-b6b3ad6e1e31503d230ad6e7bc8f2d7b5b596ad7.tar.bz2 px4-firmware-b6b3ad6e1e31503d230ad6e7bc8f2d7b5b596ad7.zip |
U-blox driver rework,, step 4
Config phase and parser rewrite
Diffstat (limited to 'src/drivers/gps/gps.cpp')
-rw-r--r-- | src/drivers/gps/gps.cpp | 41 |
1 files changed, 28 insertions, 13 deletions
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index f4d2f4b44..6ae325297 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -80,6 +80,14 @@ #endif static const int ERROR = -1; +/* class for dynamic allocation of satellite info data */ +class GPS_Sat_Info +{ +public: + struct satellite_info_s _data; +}; + + class GPS : public device::CDev { public: @@ -103,17 +111,17 @@ private: char _port[20]; ///< device / serial port path volatile int _task; //< worker task bool _healthy; ///< flag to signal if the GPS is ok - bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode GPS_Helper *_Helper; ///< instance of GPS parser - struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position + 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 - struct satellite_info_s _report_sat_info; ///< uORB topic for satellite info + 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 bool _fake_gps; ///< fake gps output - bool _enable_sat_info; ///< enable sat info /** @@ -165,11 +173,12 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), + _Sat_Info(nullptr), _report_gps_pos_pub(-1), + _p_report_sat_info(nullptr), _report_sat_info_pub(-1), _rate(0.0f), - _fake_gps(fake_gps), - _enable_sat_info(enable_sat_info) + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -179,7 +188,13 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : /* we need this potentially before it could be set in task_main */ g_dev = this; memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - memset(&_report_sat_info, 0, sizeof(_report_sat_info)); + + /* create satellite info data object if requested */ + if (enable_sat_info) { + _Sat_Info = new(GPS_Sat_Info); + _p_report_sat_info = &_Sat_Info->_data; + memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); + } _debug_enabled = true; } @@ -214,7 +229,7 @@ GPS::init() /* start the GPS driver worker task */ _task = task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr); + SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); @@ -320,7 +335,7 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, _enable_sat_info); + _Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: @@ -353,12 +368,12 @@ GPS::task_main() _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } - if (helper_ret & 2) { + if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub > 0) { - orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, &_report_sat_info); + 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), &_report_sat_info); + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); } } } @@ -466,7 +481,7 @@ GPS::print_info() } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - warnx("sat info: %s", (_enable_sat_info) ? "enabled" : "disabled"); + warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, |