diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-11 09:26:36 +0100 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-11 09:26:36 +0100 |
commit | 94909e9d8c2f0d25372fd3aeb713cf4893434b54 (patch) | |
tree | 077e29ff3a9a625553d44b575b018038761ad651 | |
parent | 3f6f26e5f6bb89056a0b07b4761fdf81f0f8491a (diff) | |
parent | 42268347d407cdb6983e710d56af895b740571dc (diff) | |
download | px4-firmware-94909e9d8c2f0d25372fd3aeb713cf4893434b54.tar.gz px4-firmware-94909e9d8c2f0d25372fd3aeb713cf4893434b54.tar.bz2 px4-firmware-94909e9d8c2f0d25372fd3aeb713cf4893434b54.zip |
Merge branch 'master' into autostart_cleanup
-rw-r--r-- | src/drivers/gps/gps.cpp | 194 |
1 files changed, 119 insertions, 75 deletions
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index fc500a9ec..6b72d560f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char *uart_path); + GPS(const char *uart_path, bool fake_gps); virtual ~GPS(); virtual int init(); @@ -112,6 +112,7 @@ private: struct vehicle_gps_position_s _report; ///< uORB topic for gps position orb_advert_t _report_pub; ///< uORB pub for gps position float _rate; ///< position update rate + bool _fake_gps; ///< fake gps output /** @@ -156,7 +157,7 @@ GPS *g_dev; } -GPS::GPS(const char *uart_path) : +GPS::GPS(const char *uart_path, bool fake_gps) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) : _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), - _rate(0.0f) + _rate(0.0f), + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -264,98 +266,133 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { - if (_Helper != nullptr) { - delete(_Helper); - /* set to zero to ensure parser is not used while not instantiated */ - _Helper = nullptr; - } + if (_fake_gps) { + + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 0.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; + + //no time and satellite information simulated + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; + usleep(2e5); - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; + } else { - default: - break; - } + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; + } - unlock(); + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + + default: + break; + } - if (_Helper->configure(_baudrate) == 0) { unlock(); - // GPS is obviously detected successfully, reset statistics - _Helper->reset_update_rates(); + if (_Helper->configure(_baudrate) == 0) { + unlock(); - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { -// lock(); - /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + // GPS is obviously detected successfully, reset statistics + _Helper->reset_update_rates(); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { + // lock(); + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } - last_rate_count++; + last_rate_count++; - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - _Helper->store_update_rates(); - _Helper->reset_update_rates(); - } + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + _Helper->store_update_rates(); + _Helper->reset_update_rates(); + } - if (!_healthy) { - char *mode_str = "unknown"; + if (!_healthy) { + char *mode_str = "unknown"; - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - mode_str = "UBX"; - break; + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "UBX"; + break; - case GPS_DRIVER_MODE_MTK: - mode_str = "MTK"; - break; + case GPS_DRIVER_MODE_MTK: + mode_str = "MTK"; + break; - default: - break; + default: + break; + } + + warnx("module found: %s", mode_str); + _healthy = true; } + } - warnx("module found: %s", mode_str); - _healthy = true; + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; } - } - if (_healthy) { - warnx("module lost"); - _healthy = false; - _rate = 0.0f; + lock(); } lock(); - } - - lock(); - /* select next mode */ - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; - default: - break; + default: + break; + } } } @@ -417,7 +454,7 @@ namespace gps GPS *g_dev; -void start(const char *uart_path); +void start(const char *uart_path, bool fake_gps); void stop(); void test(); void reset(); @@ -427,7 +464,7 @@ void info(); * Start the driver. */ void -start(const char *uart_path) +start(const char *uart_path, bool fake_gps) { int fd; @@ -435,7 +472,7 @@ start(const char *uart_path) errx(1, "already started"); /* create the driver */ - g_dev = new GPS(uart_path); + g_dev = new GPS(uart_path, fake_gps); if (g_dev == nullptr) goto fail; @@ -527,6 +564,7 @@ gps_main(int argc, char *argv[]) /* set to default */ char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; /* * Start/load the driver. @@ -542,7 +580,13 @@ gps_main(int argc, char *argv[]) } } - gps::start(device_name); + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) + fake_gps = true; + } + + gps::start(device_name, fake_gps); } if (!strcmp(argv[1], "stop")) @@ -567,5 +611,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]"); } |