diff --git a/src/platforms/posix/drivers/barosim/CMakeLists.txt b/src/platforms/posix/drivers/barosim/CMakeLists.txt index 15963f87eb..6e3a105204 100644 --- a/src/platforms/posix/drivers/barosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/barosim/CMakeLists.txt @@ -37,7 +37,6 @@ px4_add_module( -Os SRCS baro.cpp - baro_sim.cpp DEPENDS platforms__common ) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index ac5d31b6b9..be38915595 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -53,7 +53,6 @@ #include #include -#include #include #include @@ -62,6 +61,8 @@ #include #include +#include + #include #include @@ -83,6 +84,8 @@ #define BAROSIM_CONVERSION_INTERVAL 10000 /* microseconds */ #define BAROSIM_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ +using namespace DriverFramework; + class BAROSIM : public VirtDevObj { public: @@ -100,9 +103,6 @@ public: void print_info(); protected: - BAROSIM_DEV *_interface; - - struct work_s _work; ringbuffer::RingBuffer *_reports; @@ -155,6 +155,8 @@ protected: */ void cycle(); + int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); + /** * Get the internal / external state * @@ -195,7 +197,6 @@ extern "C" __EXPORT int barosim_main(int argc, char *argv[]); BAROSIM::BAROSIM(const char *path) : VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 1e6 / 100), - _interface(nullptr), _reports(nullptr), _collect_phase(false), _measure_phase(0), @@ -212,10 +213,7 @@ BAROSIM::BAROSIM(const char *path) : _comms_errors(perf_alloc(PC_COUNT, "barosim_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "barosim_buffer_overflows")) { - // work_cancel in stop_cycle called from the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); - _interface = new BAROSIM_DEV; } BAROSIM::~BAROSIM() @@ -234,7 +232,6 @@ BAROSIM::~BAROSIM() perf_free(_comms_errors); perf_free(_buffer_overflows); - delete _interface; } int @@ -392,6 +389,8 @@ BAROSIM::devRead(void *buffer, size_t buflen) int BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) { + PX4_WARN("baro IOCTL %llu", hrt_absolute_time()); + switch (cmd) { case SENSORIOCSPOLLRATE: @@ -510,6 +509,7 @@ BAROSIM::start_cycle() void BAROSIM::stop_cycle() { + stop(); setSampleInterval(0); } @@ -523,7 +523,8 @@ void BAROSIM::cycle() { int ret; - unsigned long dummy = 0; + + //PX4_WARN("baro cycle %llu", hrt_absolute_time()); /* collection phase? */ if (_collect_phase) { @@ -532,11 +533,14 @@ BAROSIM::cycle() ret = collect(); if (ret != OK) { - /* issue a reset command to the sensor */ - _interface->devIOCTL(IOCTL_RESET, dummy); + uint8_t cmd = ADDR_RESET_CMD; + int result; + + /* bump the retry count */ + result = transfer(&cmd, 1, nullptr, 0); /* reset the collection state machine and try again */ - start_cycle(); + //start_cycle(); return; } @@ -551,7 +555,7 @@ BAROSIM::cycle() */ if (_measure_phase != 0) { - setSampleInterval(BAROSIM_CONVERSION_INTERVAL); + //setSampleInterval(BAROSIM_CONVERSION_INTERVAL); return; } @@ -563,17 +567,17 @@ BAROSIM::cycle() if (ret != OK) { //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ - _interface->devIOCTL(IOCTL_RESET, dummy); + //_interface->devIOCTL(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ - start_cycle(); + //start_cycle(); return; } /* next phase is collection */ _collect_phase = true; - setSampleInterval(BAROSIM_CONVERSION_INTERVAL); + //setSampleInterval(BAROSIM_CONVERSION_INTERVAL); } int @@ -581,6 +585,8 @@ BAROSIM::measure() { int ret; + //PX4_WARN("baro measure %llu", hrt_absolute_time()); + perf_begin(_measure_perf); /* @@ -591,7 +597,8 @@ BAROSIM::measure() /* * Send the command to begin measuring. */ - ret = _interface->devIOCTL(IOCTL_MEASURE, addr); + uint8_t cmd = addr; + ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); @@ -602,9 +609,48 @@ BAROSIM::measure() return ret; } +int +BAROSIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + if (send_len == 1 && send[0] == ADDR_RESET_CMD) { + /* reset command */ + return 0; + + } else if (send_len == 1 && (send[0] == ADDR_CMD_CONVERT_D2 || send[0] == ADDR_CMD_CONVERT_D1)) { + /* measure command */ + if (send[0] == ADDR_CMD_CONVERT_D2) { + } else { + } + + return 0; + + } else if (send[0] == 0 && send_len == 1) { + /* read requested */ + Simulator *sim = Simulator::getInstance(); + + if (sim == NULL) { + PX4_ERR("Error BAROSIM_DEV::transfer no simulator"); + return -ENODEV; + } + + PX4_DEBUG("BAROSIM_DEV::transfer getting sample"); + sim->getBaroSample(recv, recv_len); + return recv_len; + + } else { + PX4_WARN("BAROSIM_DEV::transfer invalid param %u %u %u", send_len, send[0], recv_len); + return 1; + } + + + return 0; +} + int BAROSIM::collect() { + //PX4_WARN("baro collect %llu", hrt_absolute_time()); + int ret; #pragma pack(push, 1) @@ -623,7 +669,8 @@ BAROSIM::collect() report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ - ret = _interface->devRead((void *)&baro_report, sizeof(baro_report)); + uint8_t cmd = 0; + ret = transfer(&cmd, 1, (uint8_t*)&report, sizeof(baro_report)); if (ret < 0) { perf_count(_comms_errors); @@ -660,7 +707,8 @@ BAROSIM::collect() } /* notify anyone waiting for data */ - DevMgr::updateNotify(*this); + //DevMgr::updateNotify(*this); + updateNotify(); } /* update the measurement state machine */ diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp deleted file mode 100644 index 224025bc71..0000000000 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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. - * - ****************************************************************************/ - -/** - * @file baro_sim.cpp - * - * Simulation interface for barometer - */ - -/* XXX trim includes */ -#include -#include - -#include -#include -#include - -#include -#include -#include "barosim.h" -#include "board_config.h" - -BAROSIM_DEV::BAROSIM_DEV() : - VirtDevObj("BAROSIM_DEV", "/dev/BAROSIM_DEV", BARO_BASE_DEVICE_PATH, 0) -{ -} - -BAROSIM_DEV::~BAROSIM_DEV() -{ -} - -int -BAROSIM_DEV::init() -{ - return VirtDevObj::init(); -} - -ssize_t -BAROSIM_DEV::devRead(void *data, size_t count) -{ - /* read the most recent measurement */ - uint8_t cmd = 0; - int ret = transfer(&cmd, 1, static_cast(data), count); - - return ret; -} - -int -BAROSIM_DEV::devIOCTL(unsigned long operation, unsigned long arg) -{ - int ret; - - switch (operation) { - case IOCTL_RESET: - ret = _reset(); - break; - - case IOCTL_MEASURE: - ret = _doMeasurement(arg); - break; - - default: - ret = EINVAL; - } - - return ret; -} - -int -BAROSIM_DEV::_reset() -{ - uint8_t cmd = ADDR_RESET_CMD; - int result; - - /* bump the retry count */ - result = transfer(&cmd, 1, nullptr, 0); - - return result; -} - -int -BAROSIM_DEV::_doMeasurement(unsigned addr) -{ - uint8_t cmd = addr; - return transfer(&cmd, 1, nullptr, 0); -} - -int -BAROSIM_DEV::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) -{ - if (send_len == 1 && send[0] == ADDR_RESET_CMD) { - /* reset command */ - return 0; - - } else if (send_len == 1 && (send[0] == ADDR_CMD_CONVERT_D2 || send[0] == ADDR_CMD_CONVERT_D1)) { - /* measure command */ - if (send[0] == ADDR_CMD_CONVERT_D2) { - } else { - } - - return 0; - - } else if (send[0] == 0 && send_len == 1) { - /* read requested */ - Simulator *sim = Simulator::getInstance(); - - if (sim == NULL) { - PX4_ERR("Error BAROSIM_DEV::transfer no simulator"); - return -ENODEV; - } - - PX4_DEBUG("BAROSIM_DEV::transfer getting sample"); - sim->getBaroSample(recv, recv_len); - return recv_len; - - } else { - PX4_WARN("BAROSIM_DEV::transfer invalid param %u %u %u", send_len, send[0], recv_len); - return 1; - } - - - return 0; -} - -void BAROSIM_DEV::_measure() -{ -} diff --git a/src/platforms/posix/drivers/barosim/barosim.h b/src/platforms/posix/drivers/barosim/barosim.h index 92d959753f..b029dd9d09 100644 --- a/src/platforms/posix/drivers/barosim/barosim.h +++ b/src/platforms/posix/drivers/barosim/barosim.h @@ -79,38 +79,3 @@ union prom_u { extern bool crc4(uint16_t *n_prom); } /* namespace */ - -using namespace DriverFramework; - -class BAROSIM_DEV : public VirtDevObj -{ -public: - BAROSIM_DEV(); - virtual ~BAROSIM_DEV(); - - virtual int init(); - virtual ssize_t devRead(void *data, size_t count); - virtual int devIOCTL(unsigned long operation, unsigned long arg); - - virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); - -protected: - virtual void _measure(); - -private: - /** - * Send a reset command to the barometer simulator. - * - * This is required after any bus reset. - */ - int _reset(); - - /** - * Send a measure command to the barometer simulator. - * - * @param addr Which address to use for the measure operation. - */ - int _doMeasurement(unsigned addr); - -}; diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp index 3241c6c30d..afc9d94fb4 100644 --- a/src/platforms/posix/drivers/gpssim/gpssim.cpp +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -169,7 +169,7 @@ GPSSIM *g_dev = nullptr; GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info) : - VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 0), + VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 1e6 / 10), _task_should_exit(false), //_healthy(false), //_mode_changed(false),