diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index c06bbb16cf..2ae8d8118d 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit c06bbb16cf529de0c09d332bb85d49004ce3c1c9 +Subproject commit 2ae8d8118db0e95867cd1946d5b8b85d7e4ef9d3 diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index b15a9c08c9..030791415c 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -530,9 +530,14 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) /* adjust filters */ accel_set_driver_lowpass_filter((float)ul_arg, _accel_filter_x.get_cutoff_freq()); + bool want_start = (m_sample_interval_usecs == 0); + /* update interval for next measurement */ setSampleInterval(interval); + if (want_start) { + start(); + } return OK; } } @@ -651,9 +656,14 @@ ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg) return -EINVAL; } + bool want_start = (_mag->m_sample_interval_usecs == 0); + /* update interval for next measurement */ _mag->setSampleInterval(interval); + if (want_start) { + _mag->start(); + } return OK; } } @@ -765,11 +775,7 @@ ACCELSIM::start() { //PX4_INFO("ACCELSIM::start"); /* make sure we are stopped first */ - int ret = stop(); - - if (ret != 0) { - PX4_ERR("ACCELSIM::start stop failed"); - } + stop(); /* reset the report ring */ _accel_reports->flush(); @@ -781,7 +787,7 @@ ACCELSIM::start() PX4_ERR("ACCELSIM::start base class start failed"); } - return (ret != 0 || ret2 != 0) ? -1 : 0; + return (ret2 != 0) ? -1 : 0; } int diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 15c0882aea..37cc5656c9 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -68,16 +68,13 @@ #include "barosim.h" #include "VirtDevObj.hpp" -enum BAROSIM_BUS { - BAROSIM_BUS_SIM_EXTERNAL -}; - /* helper macro for handling report buffer indices */ #define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) /* helper macro for arithmetic - returns the square of the argument */ #define POW2(_x) ((_x) * (_x)) +#define BAROSIM_DEV_PATH "/dev/barosim" /* * BAROSIM internal constants and data structures. */ @@ -89,7 +86,7 @@ enum BAROSIM_BUS { class BAROSIM : public VirtDevObj { public: - BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path); + BAROSIM(const char *path); ~BAROSIM(); virtual int init(); @@ -103,12 +100,9 @@ public: void print_info(); protected: - DevObj *_interface; - - barosim::prom_s _prom; + BAROSIM_DEV *_interface; struct work_s _work; - unsigned _measure_ticks; ringbuffer::RingBuffer *_reports; @@ -127,7 +121,6 @@ protected: orb_advert_t _baro_topic; int _orb_class_instance; - int _class_instance; perf_counter_t _sample_perf; perf_counter_t _measure_perf; @@ -185,7 +178,7 @@ protected: int measure(); // Unused - virtual void _measure() {} + virtual void _measure(); /** * Collect the result of the most recent measurement. @@ -193,16 +186,16 @@ protected: virtual int collect(); }; +static BAROSIM *g_barosim = nullptr; + /* * Driver 'main' command. */ extern "C" __EXPORT int barosim_main(int argc, char *argv[]); -BAROSIM::BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path) : +BAROSIM::BAROSIM(const char *path) : VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 1e6 / 100), - _interface(interface), - _prom(prom_buf.s), - _measure_ticks(0), + _interface(nullptr), _reports(nullptr), _collect_phase(false), _measure_phase(0), @@ -212,7 +205,6 @@ BAROSIM::BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path) _msl_pressure(101325), _baro_topic(nullptr), _orb_class_instance(-1), - _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "barosim_read")), _measure_perf(perf_alloc(PC_ELAPSED, "barosim_measure")), _comms_errors(perf_alloc(PC_COUNT, "barosim_comms_errors")), @@ -220,6 +212,8 @@ BAROSIM::BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path) { // 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() @@ -334,7 +328,7 @@ BAROSIM::devRead(void *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { + if (m_sample_interval_usecs > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -396,7 +390,6 @@ BAROSIM::devRead(void *buffer, size_t buflen) int BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) { - switch (cmd) { case SENSORIOCSPOLLRATE: @@ -405,7 +398,8 @@ BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop_cycle(); - _measure_ticks = 0; + setSampleInterval(0); + m_sample_interval_usecs = 0; return OK; /* external signalling not supported */ @@ -418,39 +412,27 @@ BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(BAROSIM_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start_cycle(); - } - + setSampleInterval(BAROSIM_CONVERSION_INTERVAL); return OK; } /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - /* convert hz to tick interval via microseconds */ - unsigned long ticks = USEC2TICK(1000000 / arg); + unsigned long interval = 1000000 / arg; /* check against maximum rate */ - if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) { + if (interval < BAROSIM_CONVERSION_INTERVAL) { return -EINVAL; } - /* update interval for next measurement */ - _measure_ticks = ticks; + bool want_start = (m_sample_interval_usecs == 0); + + /* update interval for next measurement */ + setSampleInterval(interval); - /* if we need to start the poll state machine, do it */ if (want_start) { - start_cycle(); + start(); } return OK; @@ -458,11 +440,11 @@ BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) { + if (m_sample_interval_usecs == 0) { return SENSOR_POLLRATE_MANUAL; } - return (1000 / _measure_ticks); + return (1000000 / m_sample_interval_usecs); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ @@ -519,21 +501,20 @@ BAROSIM::start_cycle() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&BAROSIM::cycle_trampoline, this, 1); + setSampleInterval(1000); + start(); } void BAROSIM::stop_cycle() { - work_cancel(HPWORK, &_work); + setSampleInterval(0); } void -BAROSIM::cycle_trampoline(void *arg) +BAROSIM::_measure() { - BAROSIM *dev = reinterpret_cast(arg); - - dev->cycle(); + cycle(); } void @@ -551,8 +532,10 @@ BAROSIM::cycle() if (ret != OK) { /* issue a reset command to the sensor */ _interface->devIOCTL(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ start_cycle(); + return; } @@ -564,15 +547,9 @@ BAROSIM::cycle() * Don't inject one after temperature measurements, so we can keep * doing pressure measurements at something close to the desired rate. */ - if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(BAROSIM_CONVERSION_INTERVAL))) { + if (_measure_phase != 0) { - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&BAROSIM::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(BAROSIM_CONVERSION_INTERVAL)); + setSampleInterval(BAROSIM_CONVERSION_INTERVAL); return; } @@ -585,6 +562,7 @@ BAROSIM::cycle() //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ _interface->devIOCTL(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ start_cycle(); return; @@ -593,12 +571,7 @@ BAROSIM::cycle() /* next phase is collection */ _collect_phase = true; - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&BAROSIM::cycle_trampoline, - this, - USEC2TICK(BAROSIM_CONVERSION_INTERVAL)); + setSampleInterval(BAROSIM_CONVERSION_INTERVAL); } int @@ -702,7 +675,7 @@ BAROSIM::print_info() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - PX4_INFO("poll interval: %u ticks", _measure_ticks); + PX4_INFO("poll interval: %u usec", m_sample_interval_usecs); _reports->print_info("report queue"); PX4_INFO("TEMP: %ld", (long)_TEMP); PX4_INFO("SENS: %lld", (long long)_SENS); @@ -710,44 +683,9 @@ BAROSIM::print_info() PX4_INFO("P: %.3f", (double)_P); PX4_INFO("T: %.3f", (double)_T); PX4_INFO("MSL pressure: %10.4f", (double)(_msl_pressure / 100.f)); - - PX4_INFO("factory_setup %u", _prom.factory_setup); - PX4_INFO("c1_pressure_sens %u", _prom.c1_pressure_sens); - PX4_INFO("c2_pressure_offset %u", _prom.c2_pressure_offset); - PX4_INFO("c3_temp_coeff_pres_sens %u", _prom.c3_temp_coeff_pres_sens); - PX4_INFO("c4_temp_coeff_pres_offset %u", _prom.c4_temp_coeff_pres_offset); - PX4_INFO("c5_reference_temp %u", _prom.c5_reference_temp); - PX4_INFO("c6_temp_coeff_temp %u", _prom.c6_temp_coeff_temp); - PX4_INFO("serial_and_crc %u", _prom.serial_and_crc); } -/** - * Local functions in support of the shell command. - */ -namespace barosim -{ - -/* - list of supported bus configurations - */ -struct barosim_bus_option { - enum BAROSIM_BUS busid; - const char *devpath; - BAROSIM_constructor interface_constructor; - uint8_t busnum; - BAROSIM *dev; -} bus_options[] = { - { BAROSIM_BUS_SIM_EXTERNAL, "/dev/baro_sim", &BAROSIM_sim_interface, PX4_SIM_BUS_TEST, NULL }, -}; -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) - -bool start_bus(struct barosim_bus_option &bus); -int start(); -int test(); -int reset(); -int info(); -int calibrate(unsigned altitude); -void usage(); +namespace barosim { /** * BAROSIM crc4 cribbed from the datasheet @@ -795,38 +733,26 @@ crc4(uint16_t *n_prom) return (0x000F & crc_read) == (n_rem ^ 0x00); } - /** * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. */ -bool -start_bus(struct barosim_bus_option &bus) +static int +start() { - if (bus.dev != nullptr) { - PX4_ERR("bus option already started"); - return false; - } + g_barosim = new BAROSIM(BAROSIM_DEV_PATH); - prom_u prom_buf; - DevObj *interface = bus.interface_constructor(prom_buf, bus.busnum); - - if (interface->init() != OK) { - delete interface; - PX4_ERR("no device on bus %u", (unsigned)bus.busid); - return false; - } - - bus.dev = new BAROSIM(interface, prom_buf, bus.devpath); - - if (bus.dev != nullptr && OK != bus.dev->init()) { - delete bus.dev; - bus.dev = NULL; - PX4_ERR("bus init failed %p", bus.dev); + if (g_barosim != nullptr && OK != g_barosim->init()) { + delete g_barosim; + g_barosim = NULL; + PX4_ERR("bus init failed"); return false; } DevHandle h; - DevMgr::getHandle(bus.devpath, h); + DevMgr::getHandle(BAROSIM_DEV_PATH, h); /* set the poll rate to default, starts automatic data collection */ if (!h.isValid()) { @@ -845,44 +771,20 @@ start_bus(struct barosim_bus_option &bus) } -/** - * Start the driver. - * - * This function call only returns once the driver - * is either successfully up and running or failed to start. - */ -int -start() -{ - bool started = false; - - started |= start_bus(bus_options[0]); - - if (!started) { - PX4_ERR("driver start failed"); - return 1; - } - - // driver started OK - return 0; -} - - /** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ -int +static int test() { - struct barosim_bus_option &bus = bus_options[0]; struct baro_report report; ssize_t sz; int ret; DevHandle h; - DevMgr::getHandle(bus.devpath, h); + DevMgr::getHandle(BAROSIM_DEV_PATH, h); if (!h.isValid()) { PX4_ERR("getHandle failed (try 'barosim start' if the driver is not running)"); @@ -953,13 +855,11 @@ test() /** * Reset the driver. */ -int +static int reset() { - struct barosim_bus_option &bus = bus_options[0]; - DevHandle h; - DevMgr::getHandle(bus.devpath, h); + DevMgr::getHandle(BAROSIM_DEV_PATH, h); if (!h.isValid()) { PX4_ERR("failed "); @@ -982,16 +882,12 @@ reset() /** * Print a little info about the driver. */ -int +static int info() { - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - struct barosim_bus_option &bus = bus_options[i]; - - if (bus.dev != nullptr) { - PX4_INFO("%s", bus.devpath); - bus.dev->print_info(); - } + if (g_barosim != nullptr) { + PX4_INFO("%s", BAROSIM_DEV_PATH); + g_barosim->print_info(); } return 0; @@ -1000,16 +896,15 @@ info() /** * Calculate actual MSL pressure given current altitude */ -int +static int calibrate(unsigned altitude) { - struct barosim_bus_option &bus = bus_options[0]; struct baro_report report; float pressure; float p1; DevHandle h; - DevMgr::getHandle(bus.devpath, h); + DevMgr::getHandle(BAROSIM_DEV_PATH, h); if (!h.isValid()) { PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); @@ -1077,13 +972,13 @@ calibrate(unsigned altitude) return 0; } -void +static void usage() { PX4_WARN("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate '"); } -} // namespace +}; // namespace barosim int barosim_main(int argc, char *argv[]) diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 22574c27dc..5c5b2105fa 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -42,11 +42,7 @@ #include #include -#include -#include #include -//#include -#include #include #include @@ -54,96 +50,33 @@ #include "barosim.h" #include "board_config.h" -DevObj *BAROSIM_sim_interface(barosim::prom_u &prom_buf); - -class BARO_SIM : public device::SIM -{ -public: - BARO_SIM(uint8_t bus, barosim::prom_u &prom_buf); - virtual ~BARO_SIM(); - - virtual int init(); - virtual int dev_read(unsigned offset, void *data, unsigned count); - virtual int dev_ioctl(unsigned operation, unsigned arg); - - virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); -private: - //barosim::prom_u &_prom; - - /** - * 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 _measure(unsigned addr); - - /** - * Read the MS5611 PROM - * - * @return PX4_OK if the PROM reads successfully. - */ - int _read_prom(); - -}; - -DevObj * -BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum) -{ - return reinterpret_cast(new BARO_SIM(busnum, prom_buf)); -} - -BARO_SIM::BARO_SIM(uint8_t bus, barosim::prom_u &prom) : - SIM("BARO_SIM", "/dev/BARO_SIM", bus, 0) +BAROSIM_DEV::BAROSIM_DEV() : + VirtDevObj("BAROSIM_DEV", "/dev/BAROSIM_DEV", BARO_BASE_DEVICE_PATH, 0) { } -BARO_SIM::~BARO_SIM() +BAROSIM_DEV::~BAROSIM_DEV() { } int -BARO_SIM::init() +BAROSIM_DEV::init() { - return SIM::init(); + return VirtDevObj::init(); } -int -BARO_SIM::dev_read(unsigned offset, void *data, unsigned count) +ssize_t +BAROSIM_DEV::devRead(void *data, size_t count) { - /* - union _cvt { - uint8_t b[4]; - uint32_t w; - } *cvt = (_cvt *)data; - */ - /* read the most recent measurement */ uint8_t cmd = 0; int ret = transfer(&cmd, 1, static_cast(data), count); - /* - if (ret == PX4_OK) { - // fetch the raw value - cvt->b[0] = buf[2]; - cvt->b[1] = buf[1]; - cvt->b[2] = buf[0]; - cvt->b[3] = 0; - } - */ - return ret; } int -BARO_SIM::dev_ioctl(unsigned operation, unsigned arg) +BAROSIM_DEV::devIOCTL(unsigned long operation, unsigned long arg) { int ret; @@ -153,7 +86,7 @@ BARO_SIM::dev_ioctl(unsigned operation, unsigned arg) break; case IOCTL_MEASURE: - ret = _measure(arg); + ret = _doMeasurement(arg); break; default: @@ -164,69 +97,60 @@ BARO_SIM::dev_ioctl(unsigned operation, unsigned arg) } int -BARO_SIM::_reset() +BAROSIM_DEV::_reset() { - unsigned old_retrycount = _retries; uint8_t cmd = ADDR_RESET_CMD; int result; /* bump the retry count */ - _retries = 10; result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; return result; } int -BARO_SIM::_measure(unsigned addr) +BAROSIM_DEV::_doMeasurement(unsigned addr) { - /* - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the command. - */ - _retries = 0; - uint8_t cmd = addr; return transfer(&cmd, 1, nullptr, 0); } int -BARO_SIM::_read_prom() +BAROSIM_DEV::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { - int ret = 1; - // TODO input simlation data - return ret; -} - -int -BARO_SIM::transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len) -{ - // TODO add Simulation data connection so calls retrieve - // data from the simulator - if (recv_len == 0) { - PX4_DEBUG("BARO_SIM measurement requested"); - - } else if (send_len == 1 || send[0] != 12) { - /* measure command */ + if (send_len == 1 && send[0] == ADDR_RESET_CMD) { + /* reset command */ return 0; - } else if (send_len != 1 || send[0] != 0) { - PX4_WARN("BARO_SIM::transfer invalid param %u %u %u", send_len, send[0], recv_len); - return 1; + } 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 { + } else if (send[0] == 0 && send_len == 1) { + /* read requested */ Simulator *sim = Simulator::getInstance(); if (sim == NULL) { - PX4_ERR("Error BARO_SIM::transfer no simulator"); + PX4_ERR("Error BAROSIM_DEV::transfer no simulator"); return -ENODEV; } - PX4_DEBUG("BARO_SIM::transfer getting sample"); + 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 db7f009a51..92d959753f 100644 --- a/src/platforms/posix/drivers/barosim/barosim.h +++ b/src/platforms/posix/drivers/barosim/barosim.h @@ -36,7 +36,7 @@ * * Shared defines for the ms5611 driver. */ -#include "DevObj.hpp" +#include "VirtDevObj.hpp" #define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ #define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */ @@ -82,6 +82,35 @@ extern bool crc4(uint16_t *n_prom); using namespace DriverFramework; -/* interface factories */ -extern DevObj *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum); -typedef DevObj *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum); +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/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 43bfa61288..413abb0932 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -384,6 +384,7 @@ GYROSIM::~GYROSIM() int GYROSIM::init() { + PX4_INFO("GYROSIM::init"); int ret = 1; struct accel_report arp = {}; @@ -443,7 +444,13 @@ GYROSIM::init() return ret; } - start(); + ret = start(); + if (ret != OK) { + PX4_ERR("gyro accel start failed (%d)", ret); + return ret; + } + + // Do not call _gyro->start() because polling is done in accel _measure(); @@ -1179,8 +1186,8 @@ int GYROSIM_gyro::init() { int ret = VirtDevObj::init(); - printf("init ret: %d\n", ret); - return ret ? ret : start(); + PX4_INFO("GYROSIM_gyro::init base class ret: %d", ret); + return ret; } void diff --git a/src/systemcmds/mixer/CMakeLists.txt b/src/systemcmds/mixer/CMakeLists.txt index 703d8210fb..117f96ff2a 100644 --- a/src/systemcmds/mixer/CMakeLists.txt +++ b/src/systemcmds/mixer/CMakeLists.txt @@ -31,9 +31,7 @@ # ############################################################################ set(MIXER_CFLAGS -Os) -if(${OS} STREQUAL "qurt") - list(APPEND MIXER_CFLAGS -Wframe-larger-than=2176) -else() +if(${OS} STREQUAL "nuttx") list(APPEND MIXER_CFLAGS -Wframe-larger-than=2100) endif()