diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp new file mode 100644 index 0000000000..db948b73f6 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -0,0 +1,682 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gps.cpp + * Driver for the GPS on a serial port + */ + +//#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +//#include +#include +#include +//#include +#include +//#include +//#include +#include +#include +#include +#include +#include + +#include + +//#include + +#define GPS_DRIVER_MODE_UBX_SIM +#define GPS_SIM_DEVICE_PATH "/dev/gps_sim" + +//#include "ubx.h" +//#include "mtk.h" +//#include "ashtech.h" + + +#define TIMEOUT_5HZ 500 +#define RATE_MEASUREMENT_PERIOD 5000000 + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#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_SIM : public device::VDev +{ +public: + GPS_SIM(const char *uart_path, bool fake_gps, bool enable_sat_info); + virtual ~GPS_SIM(); + + virtual int init(); + + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +private: + + bool _task_should_exit; ///< flag to make the main worker task exit + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; ///< current baudrate + 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 _mode_changed; ///< flag that the GPS mode has changed + //gps_driver_mode_t _mode; ///< current mode + 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 *_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 + + /** + * Try to configure the GPS, handle outgoing communication to the GPS + */ + void config(); + + /** + * Trampoline to the worker task + */ + static void task_main_trampoline(void *arg); + + + /** + * Worker task: main GPS thread that configures the GPS and parses incoming data, always running + */ + void task_main(void); + + /** + * Set the baudrate of the UART to the GPS + */ + int set_baudrate(unsigned baud); + + /** + * Send a reset command to the GPS + */ + void cmd_reset(); + + int receive(int timeout); + +}; + + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int gps_sim_main(int argc, char *argv[]); + +namespace +{ + +GPS_SIM *g_dev = nullptr; + +} + + +GPS_SIM::GPS_SIM(const char *uart_path, bool fake_gps, bool enable_sat_info) : + VDev("gps", GPS_SIM_DEVICE_PATH), + _task_should_exit(false), + //_healthy(false), + //_mode_changed(false), + //_mode(GPS_DRIVER_MODE_UBX), + //_Helper(nullptr), + _Sat_Info(nullptr), + _report_gps_pos_pub(nullptr), + _p_report_sat_info(nullptr), + _report_sat_info_pub(nullptr), + _rate(0.0f), + _fake_gps(fake_gps) +{ + // /* store port name */ + // strncpy(_port, uart_path, sizeof(_port)); + // /* enforce null termination */ + // _port[sizeof(_port) - 1] = '\0'; + + /* we need this potentially before it could be set in task_main */ + g_dev = this; + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + + /* 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; +} + +GPS_SIM::~GPS_SIM() +{ + /* tell the task we want it to go away */ + _task_should_exit = true; + + /* spin waiting for the task to stop */ + for (unsigned i = 0; (i < 10) && (_task != -1); i++) { + /* give it another 100ms */ + usleep(100000); + } + + /* well, kill it anyway, though this will probably crash */ + if (_task != -1) + px4_task_delete(_task); + + g_dev = nullptr; + +} + +int +GPS_SIM::init() +{ + int ret = ERROR; + + /* do regular cdev init */ + if (VDev::init() != OK) + goto out; + + /* start the GPS driver worker task */ + _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPS_SIM::task_main_trampoline, nullptr); + + if (_task < 0) { + warnx("task start failed: %d", errno); + return -errno; + } + + ret = OK; +out: + return ret; +} + +int +GPS_SIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + lock(); + + int ret = OK; + + switch (cmd) { + case SENSORIOCRESET: + cmd_reset(); + break; + + default: + /* give it to parent if no one wants it */ + ret = VDev::ioctl(filp, cmd, arg); + break; + } + + unlock(); + + return ret; +} + +void +GPS_SIM::task_main_trampoline(void *arg) +{ + g_dev->task_main(); +} + +int +GPS_SIM::receive(int timeout) { + Simulator *sim = Simulator::getInstance(); + simulator::RawGPSData gps; + sim->getGPSSample((uint8_t *)&gps, sizeof(gps)); + + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = gps.lat; + _report_gps_pos.lon = gps.lon; + _report_gps_pos.alt = gps.alt; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.eph = (float)gps.eph; + _report_gps_pos.epv = (float)gps.epv; + _report_gps_pos.vel_m_s = (float)(gps.vel)/100.0f; + _report_gps_pos.vel_n_m_s = (float)(gps.vn)/100.0f; + _report_gps_pos.vel_e_m_s = (float)(gps.ve)/100.0f; + _report_gps_pos.vel_d_m_s = (float)(gps.vd)/100.0f; + _report_gps_pos.cog_rad = (float)(gps.cog)*3.1415f/(100.0f * 180.0f); + _report_gps_pos.fix_type = gps.fix_type; + _report_gps_pos.satellites_used = gps.satellites_visible; + + usleep(200000); + return 1; +} + +void +GPS_SIM::task_main() +{ + + /* loop handling received serial bytes and also configuring in between */ + while (!_task_should_exit) { + + if (_fake_gps) { + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = (int32_t)47.378301e7f; + _report_gps_pos.lon = (int32_t)8.538777e7f; + _report_gps_pos.alt = (int32_t)1200e3f; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.s_variance_m_s = 10.0f; + _report_gps_pos.c_variance_rad = 0.1f; + _report_gps_pos.fix_type = 3; + _report_gps_pos.eph = 0.9f; + _report_gps_pos.epv = 1.8f; + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.vel_ned_valid = true; + + //no time and satellite information simulated + + + if (!(_pub_blocked)) { + 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); + } + } + + usleep(2e5); + + } else { + + // if (_Helper != nullptr) { + // delete(_Helper); + // set to zero to ensure parser is not used while not instantiated + // _Helper = nullptr; + // } + + // switch (_mode) { + // case GPS_DRIVER_MODE_UBX_SIM: + // _Helper = new UBX_SIM(_serial_fd, &_report_gps_pos, _p_report_sat_info); + // break; + + // default: + // break; + // } + + //if (_Helper->configure(_baudrate) == 0) { + + //Publish initial report that we have access to a GPS + //Make sure to clear any stale data in case driver is reset + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.timestamp_time = hrt_absolute_time(); + + if (!(_pub_blocked)) { + 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); + } + } + + // GPS is obviously detected successfully, reset statistics + //_Helper->reset_update_rates(); + + while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { + // lock(); + /* opportunistic publishing - else invalid data would end up on the bus */ + + if (!(_pub_blocked)) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_p_report_sat_info) { + 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); + } + } + } + + //if (helper_ret & 1) { // consider only pos info updates for rate calculation */ + // 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(); + //} + + // if (!_healthy) { + // const char *mode_str = "unknown"; + + // switch (_mode) { + // case GPS_DRIVER_MODE_UBX_SIM: + // mode_str = "UBX"; + // break; + + // default: + // break; + // } + + // warnx("module found: %s", mode_str); + // _healthy = true; + // } + } + + // if (_healthy) { + // warnx("module lost"); + // _healthy = false; + // _rate = 0.0f; + // } + + lock(); + //} + + // /* 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_ASHTECH; + // break; + + // case GPS_DRIVER_MODE_ASHTECH: + // _mode = GPS_DRIVER_MODE_UBX; + // break; + + // default: + // break; + // } + } + + } + + warnx("exiting"); + + //::close(_serial_fd); + + /* tell the dtor that we are exiting */ + _task = -1; + return; +} + + + +void +GPS_SIM::cmd_reset() +{ + +} + +void +GPS_SIM::print_info() +{ + //GPS Mode + if(_fake_gps) { + warnx("protocol: faked"); + } + + else { + warnx("protocol: SIM"); + } + + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + warnx("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + + if (_report_gps_pos.timestamp_position != 0) { + warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); + warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); + //warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); + //warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); + warnx("rate publication:\t%6.2f Hz", (double)_rate); + + } + + usleep(100000); +} + +/** + * Local functions in support of the shell command. + */ +namespace gps_sim +{ + +GPS_SIM *g_dev = nullptr; + +void start(const char *uart_path, bool fake_gps, bool enable_sat_info); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start(const char *uart_path, bool fake_gps, bool enable_sat_info) +{ + int fd; + + if (g_dev != nullptr) + warnx("already started"); + + /* create the driver */ + g_dev = new GPS_SIM(uart_path, fake_gps, enable_sat_info); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = px4_open(GPS_SIM_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + warnx("open: %s\n", GPS0_DEVICE_PATH); + goto fail; + } + + return; + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + warnx("start failed"); +} + +/** + * Stop the driver. + */ +void +stop() +{ + delete g_dev; + g_dev = nullptr; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + + warnx("PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = px4_open(GPS_SIM_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + warnx("failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + warnx("reset failed"); +} + +/** + * Print the status of the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "not running"); + + g_dev->print_info(); +} + +} // namespace + + +int +gps_sim_main(int argc, char *argv[]) +{ + + /* set to default */ + const char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; + bool enable_sat_info = false; + + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + /* work around getopt unreliability */ + if (argc > 3) { + if (!strcmp(argv[2], "-d")) { + device_name = argv[3]; + + } else { + goto out; + } + } + + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) + fake_gps = true; + } + + /* Detect sat info option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-s")) + enable_sat_info = true; + } + + gps_sim::start(device_name, fake_gps, enable_sat_info); + } + + if (!strcmp(argv[1], "stop")) + gps_sim::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + gps_sim::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + gps_sim::reset(); + + /* + * Print driver status. + */ + if (!strcmp(argv[1], "status")) + gps_sim::info(); + + return 0; + +out: + warnx("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); + return 1; +} diff --git a/src/platforms/posix/drivers/gpssim/module.mk b/src/platforms/posix/drivers/gpssim/module.mk new file mode 100644 index 0000000000..630eaae377 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Simulated GPS driver +# + +MODULE_COMMAND = gps_sim + +SRCS = gpssim.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/drivers/gpssim/ubx_sim.cpp b/src/platforms/posix/drivers/gpssim/ubx_sim.cpp new file mode 100644 index 0000000000..a4e9043c96 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/ubx_sim.cpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ubx.cpp + * + * U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description + * including Prototol Specification. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * + * @author Hannes Delago + * (rework, add ubx7+ compatibility) + * + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "ubx_sim.h" +#include + +#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK +#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received +#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls +#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval + + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) : + _fd(fd), + _gps_position(gps_position), + _satellite_info(satellite_info), +{ + +} + +UBX::~UBX() +{ +} + + +int UBX_SIM::configure(unsigned &baudrate) +{ + return 0; +} + +int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled +UBX_SIM::receive(const unsigned timeout) +{ + /* copy data from simulator here */ + usleep(1000000); + return 1; +} diff --git a/src/platforms/posix/drivers/gpssim/ubx_sim.h b/src/platforms/posix/drivers/gpssim/ubx_sim.h new file mode 100644 index 0000000000..5722822dad --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/ubx_sim.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013, 2014 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ubx_sim.h + * + */ + +#ifndef UBX_SIM_H_ +#define UBX_SIM_H_ + + + + +class UBX_SIM +{ +public: + UBX_SIM(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); + ~UBX_SIM(); + int receive(const unsigned timeout); + int configure(unsigned &baudrate); + +private: + + + + int _fd; + struct vehicle_gps_position_s *_gps_position; + struct satellite_info_s *_satellite_info; + bool _enable_sat_info; +}; + +#endif /* UBX_SIM_H_ */