From d32a80df3ade13980e63aa3034da2c0ad398caf8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 9 Jan 2020 10:30:20 -0500 Subject: [PATCH] simulator: replace gpssim with simple orb publication - this is one of the last pieces of the system that still depend on DriverFramework - add new SIM_GPS_NOISE_X parameter for optionally increasing the GPS noise multiplier (was previously a gpssim command line option) - add SIM_x_BLOCK parameters to block sensor publication - SIM_GPS_BLOCK - SIM_ACCEL_BLOCK - SIM_GYRO_BLOCK - SIM_MAG_BLOCK - SIM_BARO_BLOCK - SIM_DPRES_BLOCK --- ROMFS/px4fmu_common/init.d-posix/rcS | 1 - posix-configs/SITL/init/test/cmd_template.in | 1 - posix-configs/SITL/init/test/test_mavlink | 1 - posix-configs/SITL/init/test/test_shutdown | 2 - posix-configs/SITL/init/test/test_template.in | 1 - src/modules/simulator/CMakeLists.txt | 2 - src/modules/simulator/gpssim/CMakeLists.txt | 45 -- src/modules/simulator/gpssim/gpssim.cpp | 559 ------------------ src/modules/simulator/gpssim/ubx_sim.cpp | 96 --- src/modules/simulator/gpssim/ubx_sim.h | 57 -- src/modules/simulator/simulator.cpp | 27 - src/modules/simulator/simulator.h | 129 +--- src/modules/simulator/simulator_mavlink.cpp | 64 +- src/modules/simulator/simulator_params.c | 73 ++- 14 files changed, 126 insertions(+), 932 deletions(-) delete mode 100644 src/modules/simulator/gpssim/CMakeLists.txt delete mode 100644 src/modules/simulator/gpssim/gpssim.cpp delete mode 100644 src/modules/simulator/gpssim/ubx_sim.cpp delete mode 100644 src/modules/simulator/gpssim/ubx_sim.h diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 359119da48..dfae60cd36 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -237,7 +237,6 @@ dataman start replay tryapplyparams simulator start -c $simulator_tcp_port tone_alarm start -gpssim start rc_update start sensors start commander start diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index fdc256c910..2b447498bb 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -12,7 +12,6 @@ dataman start simulator start tone_alarm start -gpssim start pwm_out_sim start ver all diff --git a/posix-configs/SITL/init/test/test_mavlink b/posix-configs/SITL/init/test/test_mavlink index 7fe67fc8f5..2735228766 100644 --- a/posix-configs/SITL/init/test/test_mavlink +++ b/posix-configs/SITL/init/test/test_mavlink @@ -12,7 +12,6 @@ dataman start simulator start tone_alarm start -gpssim start pwm_out_sim start ver all diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 908f0cfdd2..ccaead269f 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -16,7 +16,6 @@ dataman start simulator start tone_alarm start -gpssim start pwm_out_sim start rc_update start @@ -84,7 +83,6 @@ sleep 2 simulator stop tone_alarm stop -gpssim stop dataman stop #uorb stop diff --git a/posix-configs/SITL/init/test/test_template.in b/posix-configs/SITL/init/test/test_template.in index 762ee48b2c..4612ac4115 100644 --- a/posix-configs/SITL/init/test/test_template.in +++ b/posix-configs/SITL/init/test/test_template.in @@ -12,7 +12,6 @@ dataman start simulator start tone_alarm start -gpssim start pwm_out_sim start ver all diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt index 08e77afd6f..639ce2096e 100644 --- a/src/modules/simulator/CMakeLists.txt +++ b/src/modules/simulator/CMakeLists.txt @@ -76,5 +76,3 @@ px4_add_module( drivers_magnetometer ) target_include_directories(modules__simulator INTERFACE ${PX4_SOURCE_DIR}/mavlink/include/mavlink) - -add_subdirectory(gpssim) diff --git a/src/modules/simulator/gpssim/CMakeLists.txt b/src/modules/simulator/gpssim/CMakeLists.txt deleted file mode 100644 index 33c42b287d..0000000000 --- a/src/modules/simulator/gpssim/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 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. -# -############################################################################ - -px4_add_module( - MODULE drivers__gpssim - MAIN gpssim - COMPILE_FLAGS - -Wno-double-promotion - -Wno-cast-align # TODO: fix and enable - -Wno-address-of-packed-member # TODO: fix in c_library_v2 - SRCS - gpssim.cpp - DEPENDS - modules__simulator - ) diff --git a/src/modules/simulator/gpssim/gpssim.cpp b/src/modules/simulator/gpssim/gpssim.cpp deleted file mode 100644 index 608bc80ee1..0000000000 --- a/src/modules/simulator/gpssim/gpssim.cpp +++ /dev/null @@ -1,559 +0,0 @@ -/**************************************************************************** - * - * 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 gpssim.cpp - * Simulated GPS driver - */ - -#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 "DevMgr.hpp" -#include "VirtDevObj.hpp" - -using namespace DriverFramework; - -#define GPS_DRIVER_MODE_UBX_SIM -#define GPSSIM_DEVICE_PATH "/dev/gpssim" - -#define TIMEOUT_100MS 100000 -#define RATE_MEASUREMENT_PERIOD 5000000 - -class GPSSIM : public VirtDevObj -{ -public: - GPSSIM(bool fake_gps, bool enable_sat_info, int fix_type, int num_sat, int noise_multiplier); - ~GPSSIM() override; - - int init() override; - - int devIOCTL(unsigned long cmd, unsigned long arg) override; - - void set(int fix_type, int num_sat, int noise_multiplier); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - void _measure() override {} - -private: - - bool _task_should_exit{false}; ///< flag to make the main worker task exit - volatile int _task{-1}; ///< worker task - vehicle_gps_position_s _report_gps_pos{}; ///< uORB topic for gps position - uORB::PublicationMulti _report_gps_pos_pub{ORB_ID(vehicle_gps_position)}; ///< uORB pub for gps position - SyncObj _sync; - int _fix_type{0}; - int _num_sat{0}; - int _noise_multiplier{0}; - - std::default_random_engine _gen; - - /** - * Try to configure the GPS, handle outgoing communication to the GPS - */ - void config(); - - /** - * Trampoline to the worker task - */ - static int task_main_trampoline(int argc, char *argv[]); - - - /** - * Worker task: main GPS thread that configures the GPS and parses incoming data, always running - */ - void task_main(); - - /** - * 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 gpssim_main(int argc, char *argv[]); - -namespace -{ - -GPSSIM *g_dev = nullptr; - -} - - -GPSSIM::GPSSIM(bool fake_gps, bool enable_sat_info, int fix_type, int num_sat, int noise_multiplier) : - VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 1e6 / 10), - _fix_type(fix_type), - _num_sat(num_sat), - _noise_multiplier(noise_multiplier) -{ - /* we need this potentially before it could be set in task_main */ - g_dev = this; - _report_gps_pos.heading = NAN; - _report_gps_pos.heading_offset = NAN; -} - -GPSSIM::~GPSSIM() -{ - /* 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 */ - px4_usleep(100000); - } - - /* well, kill it anyway, though this will probably crash */ - if (_task != -1) { - px4_task_delete(_task); - } - - g_dev = nullptr; -} - -int -GPSSIM::init() -{ - int ret = PX4_ERROR; - - /* do regular cdev init */ - if (VirtDevObj::init() != OK) { - goto out; - } - - /* start the GPS driver worker task */ - _task = px4_task_spawn_cmd("gpssim", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPSSIM::task_main_trampoline, nullptr); - - if (_task < 0) { - PX4_ERR("task start failed: %d", errno); - return -errno; - } - - ret = OK; -out: - return ret; -} - -int -GPSSIM::devIOCTL(unsigned long cmd, unsigned long arg) -{ - _sync.lock(); - - int ret = OK; - - switch (cmd) { - case SENSORIOCRESET: - cmd_reset(); - break; - - default: - /* give it to parent if no one wants it */ - ret = VirtDevObj::devIOCTL(cmd, arg); - break; - } - - _sync.unlock(); - - return ret; -} - -int -GPSSIM::task_main_trampoline(int argc, char *argv[]) -{ - g_dev->task_main(); - return 0; -} - -int -GPSSIM::receive(int timeout) -{ - Simulator *sim = Simulator::getInstance(); - simulator::RawGPSData gps; - - static uint64_t timestamp_last = 0; - - if (sim->getGPSSample((uint8_t *)&gps, sizeof(gps)) && - (gps.timestamp != timestamp_last || timestamp_last == 0)) { - _report_gps_pos.timestamp = hrt_absolute_time(); - _report_gps_pos.lat = gps.lat; - _report_gps_pos.lon = gps.lon; - _report_gps_pos.alt = gps.alt; - _report_gps_pos.eph = (float)gps.eph * 1e-2f; - _report_gps_pos.epv = (float)gps.epv * 1e-2f; - _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; - _report_gps_pos.s_variance_m_s = 0.25f; - - timestamp_last = gps.timestamp; - - // check for data set by the user - _report_gps_pos.fix_type = (_fix_type < 0) ? _report_gps_pos.fix_type : _fix_type; - _report_gps_pos.satellites_used = (_num_sat < 0) ? _report_gps_pos.satellites_used : _num_sat; - // use normal distribution for noise - std::normal_distribution normal_distribution(0.0f, 1.0f); - _report_gps_pos.lat += (int32_t)(_noise_multiplier * normal_distribution(_gen)); - _report_gps_pos.lon += (int32_t)(_noise_multiplier * normal_distribution(_gen)); - - return 1; - - } else { - - px4_usleep(timeout); - return 0; - } -} - -void -GPSSIM::task_main() -{ - /* loop handling received serial bytes and also configuring in between */ - while (!_task_should_exit) { - - int recv_ret = receive(TIMEOUT_100MS); - - if (recv_ret > 0) { - /* opportunistic publishing - else invalid data would end up on the bus */ - _report_gps_pos_pub.publish(_report_gps_pos); - } - } - - PX4_INFO("exiting"); - - /* tell the dtor that we are exiting */ - _task = -1; -} - - - -void -GPSSIM::cmd_reset() -{ -} - -void -GPSSIM::print_info() -{ - // GPS Mode - PX4_INFO("protocol: SIM"); - - if (_report_gps_pos.timestamp != 0) { - print_message(_report_gps_pos); - } - - px4_usleep(100000); -} - -void -GPSSIM::set(int fix_type, int num_sat, int noise_multiplier) -{ - _fix_type = fix_type; - _num_sat = num_sat; - _noise_multiplier = noise_multiplier; - PX4_INFO("Parameters set"); -} - -/** - * Local functions in support of the shell command. - */ -namespace gpssim -{ - -GPSSIM *g_dev = nullptr; - -void start(bool fake_gps, bool enable_sat_info, - int fix_type, int num_sat, int noise_multiplier); -void stop(); -void test(); -void reset(); -void info(); -void usage(const char *reason); - -/** - * Start the driver. - */ -void -start(bool fake_gps, bool enable_sat_info, int fix_type, int num_sat, int noise_multiplier) -{ - DevHandle h; - - /* create the driver */ - g_dev = new GPSSIM(fake_gps, enable_sat_info, fix_type, num_sat, noise_multiplier); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - DevMgr::getHandle(GPSSIM_DEVICE_PATH, h); - - if (!h.isValid()) { - PX4_ERR("getHandle failed: %s", GPSSIM_DEVICE_PATH); - goto fail; - } - - return; - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - PX4_ERR("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() -{ - PX4_INFO("PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - DevHandle h; - DevMgr::getHandle(GPSSIM_DEVICE_PATH, h); - - if (!h.isValid()) { - PX4_ERR("failed "); - } - - if (h.ioctl(SENSORIOCRESET, 0) < 0) { - PX4_ERR("reset failed"); - } -} - -/** - * Print the status of the driver. - */ -void -info() -{ - if (g_dev == nullptr) { - PX4_ERR("gpssim not running"); - return; - } - - g_dev->print_info(); -} - -void -usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s", reason); - } - - PX4_INFO("usage:"); - PX4_INFO("gpssim {start|stop|test|reset|status|set}"); - PX4_INFO(" [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); - PX4_INFO(" [-t # (for setting fix_type)][-n # (for setting # satellites)][-m # (for setting noise multiplier [scalar] for normal distribution)]"); -} - -} // namespace - - - -int -gpssim_main(int argc, char *argv[]) -{ - // set to default - bool fake_gps = false; - bool enable_sat_info = false; - int fix_type = -1; - int num_sat = -1; - int noise_multiplier = 0; - - // check for optional arguments - int ch; - int myoptind = 1; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "fst:n:m:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'f': - fake_gps = true; - PX4_INFO("Using fake GPS"); - break; - - case 's': - enable_sat_info = true; - PX4_INFO("Satellite info enabled"); - break; - - case 't': - fix_type = atoi(myoptarg); - PX4_INFO("Setting fix_type to %d", fix_type); - break; - - case 'n': - num_sat = atoi(myoptarg); - PX4_INFO("Setting number of satellites to %d", num_sat); - break; - - case 'm': - noise_multiplier = atoi(myoptarg); - PX4_INFO("Setting noise multiplier to %d", noise_multiplier); - break; - - default: - PX4_WARN("Unknown option!"); - } - } - - if (myoptind >= argc) { - gpssim::usage("not enough arguments supplied"); - return 1; - } - - /* - * Start/load the driver. - */ - if (!strcmp(argv[myoptind], "start")) { - if (g_dev != nullptr) { - PX4_WARN("already started"); - return 0; - } - - gpssim::start(fake_gps, enable_sat_info, fix_type, num_sat, noise_multiplier); - return 0; - } - - /* The following need gpssim running. */ - if (g_dev == nullptr) { - PX4_WARN("not running"); - return 1; - } - - if (!strcmp(argv[myoptind], "stop")) { - gpssim::stop(); - } - - /* - * Test the driver/device. - */ - if (!strcmp(argv[myoptind], "test")) { - gpssim::test(); - } - - /* - * Reset the driver. - */ - if (!strcmp(argv[myoptind], "reset")) { - gpssim::reset(); - } - - /* - * Print driver status. - */ - if (!strcmp(argv[myoptind], "status")) { - gpssim::info(); - } - - /* - * Set parameters - */ - if (!strcmp(argv[myoptind], "set")) { - g_dev->set(fix_type, num_sat, noise_multiplier); - } - - return 0; -} diff --git a/src/modules/simulator/gpssim/ubx_sim.cpp b/src/modules/simulator/gpssim/ubx_sim.cpp deleted file mode 100644 index 0e4270402e..0000000000 --- a/src/modules/simulator/gpssim/ubx_sim.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * 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/modules/simulator/gpssim/ubx_sim.h b/src/modules/simulator/gpssim/ubx_sim.h deleted file mode 100644 index 98f6fdf3bc..0000000000 --- a/src/modules/simulator/gpssim/ubx_sim.h +++ /dev/null @@ -1,57 +0,0 @@ -/**************************************************************************** - * - * 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_ */ diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 8289092343..a80904df7b 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -47,27 +47,10 @@ #include "simulator.h" -using namespace simulator; - static px4_task_t g_sim_task = -1; Simulator *Simulator::_instance = nullptr; -Simulator *Simulator::getInstance() -{ - return _instance; -} - -bool Simulator::getGPSSample(uint8_t *buf, int len) -{ - return _gps.copyData(buf, len); -} - -void Simulator::write_gps_data(void *buf) -{ - _gps.writeData(buf); -} - void Simulator::parameters_update(bool force) { // check for parameter updates @@ -107,16 +90,6 @@ int Simulator::start(int argc, char *argv[]) } } -void Simulator::set_ip(InternetProtocol ip) -{ - _ip = ip; -} - -void Simulator::set_port(unsigned port) -{ - _port = port; -} - static void usage() { PX4_INFO("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}"); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index e69a3041f4..d65e918656 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include #include @@ -68,99 +68,21 @@ #include #include #include +#include #include #include #include -#include + +#include #include #include #include -namespace simulator -{ - -#pragma pack(push, 1) -struct RawGPSData { - uint64_t timestamp; - int32_t lat; - int32_t lon; - int32_t alt; - uint16_t eph; - uint16_t epv; - uint16_t vel; - int16_t vn; - int16_t ve; - int16_t vd; - uint16_t cog; - uint8_t fix_type; - uint8_t satellites_visible; -}; -#pragma pack(pop) - -template class Report -{ -public: - Report(int readers) : - _readidx(0), - _max_readers(readers), - _report_len(sizeof(RType)) - { - memset(_buf, 0, sizeof(_buf)); - px4_sem_init(&_lock, 0, _max_readers); - } - - ~Report() {} - - bool copyData(void *outbuf, int len) - { - if (len != _report_len) { - return false; - } - - read_lock(); - memcpy(outbuf, &_buf[_readidx], _report_len); - read_unlock(); - return true; - } - void writeData(void *inbuf) - { - write_lock(); - memcpy(&_buf[!_readidx], inbuf, _report_len); - _readidx = !_readidx; - write_unlock(); - } - -protected: - void read_lock() { px4_sem_wait(&_lock); } - void read_unlock() { px4_sem_post(&_lock); } - void write_lock() - { - for (int i = 0; i < _max_readers; i++) { - px4_sem_wait(&_lock); - } - } - void write_unlock() - { - for (int i = 0; i < _max_readers; i++) { - px4_sem_post(&_lock); - } - } - - int _readidx; - px4_sem_t _lock; - const int _max_readers; - const int _report_len; - RType _buf[2]; -}; - -} // namespace simulator - - class Simulator : public ModuleParams { public: - static Simulator *getInstance(); + static Simulator *getInstance() { return _instance; } enum class InternetProtocol { TCP, @@ -169,27 +91,16 @@ public: static int start(int argc, char *argv[]); - bool getGPSSample(uint8_t *buf, int len); - - void write_gps_data(void *buf); - - void set_ip(InternetProtocol ip); - void set_port(unsigned port); + void set_ip(InternetProtocol ip) { _ip = ip; } + void set_port(unsigned port) { _port = port; } #if defined(ENABLE_LOCKSTEP_SCHEDULER) bool has_initialized() {return _has_initialized.load(); } #endif private: - Simulator() : - ModuleParams(nullptr) + Simulator() : ModuleParams(nullptr) { - simulator::RawGPSData gps_data{}; - gps_data.eph = UINT16_MAX; - gps_data.epv = UINT16_MAX; - - _gps.writeData(&gps_data); - _px4_accel.set_sample_rate(250); _px4_gyro.set_sample_rate(250); } @@ -197,7 +108,6 @@ private: ~Simulator() { // free perf counters - perf_free(_perf_gps); perf_free(_perf_sim_delay); perf_free(_perf_sim_interval); @@ -218,9 +128,6 @@ private: PX4Magnetometer _px4_mag{197388, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION PX4Barometer _px4_baro{6620172, ORB_PRIO_DEFAULT}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION - simulator::Report _gps{1}; - - perf_counter_t _perf_gps{perf_alloc(PC_ELAPSED, MODULE_NAME": gps delay")}; perf_counter_t _perf_sim_delay{perf_alloc(PC_ELAPSED, MODULE_NAME": network delay")}; perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")}; @@ -287,7 +194,6 @@ private: void send_heartbeat(); void send_mavlink_message(const mavlink_message_t &aMsg); void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &imu); - void update_gps(const mavlink_hil_gps_t *gps_sim); static void *sending_trampoline(void *); @@ -301,6 +207,10 @@ private: uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; uORB::Publication _input_rc_pub{ORB_ID(input_rc)}; + // HIL GPS + uORB::Publication _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)}; + std::default_random_engine _gen{}; + // uORB subscription handlers int _actuator_outputs_sub{-1}; actuator_outputs_s _actuator_outputs{}; @@ -322,19 +232,18 @@ private: #if defined(ENABLE_LOCKSTEP_SCHEDULER) px4::atomic _has_initialized {false}; - - int _ekf2_timestamps_sub{-1}; - - enum class State { - WaitingForFirstEkf2Timestamp = 0, - WaitingForActuatorControls = 1, - WaitingForEkf2Timestamp = 2, - }; #endif DEFINE_PARAMETERS( (ParamFloat) _param_sim_bat_drain, ///< battery drain interval (ParamFloat) _battery_min_percentage, //< minimum battery percentage + (ParamFloat) _param_sim_gps_noise_x, + (ParamBool) _param_sim_gps_block, + (ParamBool) _param_sim_accel_block, + (ParamBool) _param_sim_gyro_block, + (ParamBool) _param_sim_baro_block, + (ParamBool) _param_sim_mag_block, + (ParamBool) _param_sim_dpres_block, (ParamInt) _param_mav_type, (ParamInt) _param_mav_sys_id, (ParamInt) _param_mav_comp_id diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 6e0d436528..8b383c29a5 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -71,7 +71,6 @@ static unsigned _addrlen = sizeof(_srcaddr); const unsigned mode_flag_armed = 128; const unsigned mode_flag_custom = 1; -using namespace simulator; using namespace time_literals; mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs() @@ -192,7 +191,7 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor } // gyro - { + if (!_param_sim_gyro_block.get()) { static constexpr float scaling = 1000.0f; _px4_gyro.set_scale(1 / scaling); _px4_gyro.set_temperature(imu.temperature); @@ -200,7 +199,7 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor } // accel - { + if (!_param_sim_accel_block.get()) { static constexpr float scaling = 1000.0f; _px4_accel.set_scale(1 / scaling); _px4_accel.set_temperature(imu.temperature); @@ -208,7 +207,7 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor } // magnetometer - { + if (!_param_sim_mag_block.get()) { static constexpr float scaling = 1000.0f; _px4_mag.set_scale(1 / scaling); _px4_mag.set_temperature(imu.temperature); @@ -216,13 +215,13 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor } // baro - { + if (!_param_sim_baro_block.get()) { _px4_baro.set_temperature(imu.temperature); _px4_baro.update(time, imu.abs_pressure); } // differential pressure - { + if (!_param_sim_dpres_block.get()) { differential_pressure_s report{}; report.timestamp = time; report.temperature = imu.temperature; @@ -233,26 +232,6 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor } } -void Simulator::update_gps(const mavlink_hil_gps_t *gps_sim) -{ - RawGPSData gps = {}; - gps.timestamp = hrt_absolute_time(); - gps.lat = gps_sim->lat; - gps.lon = gps_sim->lon; - gps.alt = gps_sim->alt; - gps.eph = gps_sim->eph; - gps.epv = gps_sim->epv; - gps.vel = gps_sim->vel; - gps.vn = gps_sim->vn; - gps.ve = gps_sim->ve; - gps.vd = gps_sim->vd; - gps.cog = gps_sim->cog; - gps.fix_type = gps_sim->fix_type; - gps.satellites_visible = gps_sim->satellites_visible; - - write_gps_data((void *)&gps); -} - void Simulator::handle_message(const mavlink_message_t *msg) { switch (msg->msgid) { @@ -303,10 +282,37 @@ void Simulator::handle_message_distance_sensor(const mavlink_message_t *msg) void Simulator::handle_message_hil_gps(const mavlink_message_t *msg) { - mavlink_hil_gps_t gps_sim; - mavlink_msg_hil_gps_decode(msg, &gps_sim); + mavlink_hil_gps_t hil_gps; + mavlink_msg_hil_gps_decode(msg, &hil_gps); - update_gps(&gps_sim); + if (!_param_sim_gps_block.get()) { + vehicle_gps_position_s gps{}; + + gps.timestamp = hrt_absolute_time(); + gps.time_utc_usec = hil_gps.time_usec; + gps.fix_type = hil_gps.fix_type; + gps.lat = hil_gps.lat; + gps.lon = hil_gps.lon; + gps.alt = hil_gps.alt; + gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m + gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m + gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s + gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s + gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s + gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s + gps.cog_rad = math::radians((float)(hil_gps.cog) / 100.0f); // cdeg -> rad + gps.satellites_used = hil_gps.satellites_visible; + gps.s_variance_m_s = 0.25f; + + // use normal distribution for noise + if (_param_sim_gps_noise_x.get() > 0.0f) { + std::normal_distribution normal_distribution(0.0f, 1.0f); + gps.lat += (int32_t)(_param_sim_gps_noise_x.get() * normal_distribution(_gen)); + gps.lon += (int32_t)(_param_sim_gps_noise_x.get() * normal_distribution(_gen)); + } + + _vehicle_gps_position_pub.publish(gps); + } } void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) diff --git a/src/modules/simulator/simulator_params.c b/src/modules/simulator/simulator_params.c index 8a0b582855..4b9033237a 100644 --- a/src/modules/simulator/simulator_params.c +++ b/src/modules/simulator/simulator_params.c @@ -38,7 +38,6 @@ * * @author Mohamed Abdelkader */ -#include /** * Simulator Battery drain interval @@ -65,3 +64,75 @@ PARAM_DEFINE_FLOAT(SIM_BAT_DRAIN, 60); * @group SITL */ PARAM_DEFINE_FLOAT(SIM_BAT_MIN_PCT, 50.0f); + +/** + * Simulator GPS noise multiplier. + * + * @min 0 + * @max 10 + * @increment 0.1 + * @unit % + * + * @group SITL + */ +PARAM_DEFINE_FLOAT(SIM_GPS_NOISE_X, 0.0f); + +/** + * Simulator block GPS data. + * + * Enable to block the publication of any incoming simulation GPS data. + * + * @boolean + * @group SITL + */ +PARAM_DEFINE_INT32(SIM_GPS_BLOCK, 0); + +/** + * Simulator block accelerometer data. + * + * Enable to block the publication of any incoming simulation accelerometer data. + * + * @boolean + * @group SITL + */ +PARAM_DEFINE_INT32(SIM_ACCEL_BLOCK, 0); + +/** + * Simulator block gyroscope data. + * + * Enable to block the publication of any incoming simulation gyroscope data. + * + * @boolean + * @group SITL + */ +PARAM_DEFINE_INT32(SIM_GYRO_BLOCK, 0); + +/** + * Simulator block magnetometer data. + * + * Enable to block the publication of any incoming simulation magnetometer data. + * + * @boolean + * @group SITL + */ +PARAM_DEFINE_INT32(SIM_MAG_BLOCK, 0); + +/** + * Simulator block barometer data. + * + * Enable to block the publication of any incoming simulation barometer data. + * + * @boolean + * @group SITL + */ +PARAM_DEFINE_INT32(SIM_BARO_BLOCK, 0); + +/** + * Simulator block differential pressure data. + * + * Enable to block the publication of any incoming simulation differential pressure data. + * + * @boolean + * @group SITL + */ +PARAM_DEFINE_INT32(SIM_DPRES_BLOCK, 0);