diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 4cffd59ee3..52b4470cd7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -1,6 +1,6 @@ #!nsh # -# Standard startup script for PX4FMU v2, v3, v4 onboard sensor drivers. +# Standard startup script for sensor drivers. # if ver hwcmp AEROFC_V1 @@ -68,8 +68,6 @@ if ver hwcmp PX4FMU_V2 then # External I2C bus hmc5883 -C -T -X start - - # External I2C bus lis3mdl -X start # Internal I2C bus @@ -117,6 +115,7 @@ then hmc5883 -C -T -S -R 8 start fi + if [ $BOARD_FMUV3 == 21 ] then # v2.1 internal MPU9250 is rotated 180 deg roll, 270 deg yaw @@ -133,7 +132,7 @@ then # V3 build hwtypecmp supports V2|V2M|V30 if ver hwtypecmp V2M then - # On the PixhawkMini the mpu9250 has been disabled due to HW errata + # On the PixhawkMini the mpu9250 has been disabled due to HW errata else mpu9250 start fi @@ -156,51 +155,63 @@ if ver hwcmp PX4FMU_V4 then # External I2C bus hmc5883 -C -T -X start + lis3mdl -X start + bmp280 -I start - lis3mdl -R 2 start - - # Internal SPI bus is rotated 90 deg yaw - hmc5883 -C -T -S -R 2 start - - # Internal SPI bus ICM-20608-G is rotated 90 deg yaw - mpu6000 -R 2 -T 20608 start + # expansion i2c used for BMM150 rotated by 90deg + bmm150 -R 2 start - # Internal SPI bus ICM-20602-G is rotated 90 deg yaw - mpu6000 -R 2 -T 20602 start + if hmc5883 -C -T -S -R 2 start + then + # hmc5883 internal SPI bus is rotated 90 deg yaw + else + if lis3mdl -R 2 start + then + # lis3mdl internal SPI bus is rotated 90 deg yaw + else + # BMI055 gyro internal SPI bus + bmi055 -G start + fi + fi - # Start either MPU9250 or BMI160. They are both connected to the same SPI bus and use the same + # Start either ICM2060X or BMI055. They are both connected to the same SPI bus and use the same # chip select pin. There are different boards with either one of them and the WHO_AM_I register # will prevent the incorrect driver from a successful initialization. - # Internal SPI bus mpu9250 is rotated 90 deg yaw - mpu9250 -R 2 start - - # Internal SPI bus BMI160 - bmi160 start + if mpu6000 -R 2 -T 20602 start + then + # ICM20602 internal SPI bus ICM-20608-G is rotated 90 deg yaw + else + if mpu6000 -R 2 -T 20608 start + then + # ICM20608 internal SPI bus ICM-20602-G is rotated 90 deg yaw + else + # BMI055 accel internal SPI bus + bmi055 -A start + fi + fi - # Start either ICM2060X or BMI055. They are both connected to the same SPI bus and use the same + # Start either MPU9250 or BMI160. They are both connected to the same SPI bus and use the same # chip select pin. There are different boards with either one of them and the WHO_AM_I register # will prevent the incorrect driver from a successful initialization. - # Internal SPI bus BMI055_ACC - bmi055 -A start - - # Internal SPI bus BMI055_GYR - bmi055 -G start - - # expansion i2c used for BMM150 rotated by 90deg - bmm150 -R 2 start - - # expansion i2c used for BMP280 - bmp280 -I start + if mpu9250 -R 2 start + then + # mpu9250 internal SPI bus mpu9250 is rotated 90 deg yaw + else + # BMI160 internal SPI bus + bmi160 start + fi fi if ver hwcmp MINDPX_V2 then # External I2C bus hmc5883 -C -T -X start + # Internal I2C bus hmc5883 -C -T -I -R 12 start + mpu6000 -s -R 8 start mpu9250 -s -R 8 start lsm303d -R 10 start @@ -219,16 +230,13 @@ fi if ver hwcmp AEROFC_V1 then ms5611 -T 0 start - mpu9250 -s -R 14 start # Possible external compasses hmc5883 -X start ist8310 -C -b 1 -R 4 start - aerofc_adc start - ll40ls start i2c fi @@ -287,25 +295,40 @@ fi # Optional drivers # -sdp3x_airspeed start -sdp3x_airspeed start -b 2 - -# Pixhawk 2.1 has a MS5611 on I2C which gets wrongly -# detected as MS5525 because the chip manufacturer was so -# clever to assign the same I2C address and skip a WHO_AM_I -# register. -if [ $BOARD_FMUV3 == 21 ] +if [ ${VEHICLE_TYPE} == fw -o ${VEHICLE_TYPE} == vtol ] then - ms5525_airspeed start -b 2 -else - ms5525_airspeed start -fi + if param compare CBRK_AIRSPD_CHK 0 + then + if sdp3x_airspeed start + then + else + sdp3x_airspeed start -b 2 + fi -ms4525_airspeed start -ms4525_airspeed start -b 2 + # Pixhawk 2.1 has a MS5611 on I2C which gets wrongly + # detected as MS5525 because the chip manufacturer was so + # clever to assign the same I2C address and skip a WHO_AM_I + # register. + if [ $BOARD_FMUV3 == 21 ] + then + ms5525_airspeed start -b 2 + else + ms5525_airspeed start + fi -ets_airspeed start -ets_airspeed start -b 1 + if ms4525_airspeed start + then + else + ms4525_airspeed start -b 2 + fi + + if ets_airspeed start + then + else + ets_airspeed start -b 1 + fi + fi +fi # Sensors on the PWM interface bank if param compare SENS_EN_LL40LS 1 @@ -323,34 +346,29 @@ then fi # lightware serial lidar sensor -if param compare SENS_EN_SF0X 0 +if param greater SENS_EN_SF0X 0 then -else sf0x start fi # lightware i2c lidar sensor -if param compare SENS_EN_SF1XX 0 +if param greater SENS_EN_SF1XX 0 then -else sf1xx start fi # mb12xx sonar sensor -if param compare SENS_EN_MB12XX 1 +if param greater SENS_EN_MB12XX 0 then mb12xx start fi # teraranger one tof sensor -if param compare SENS_EN_TRANGER 0 +if param greater SENS_EN_TRANGER 0 then -else teraranger start fi # Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire) usleep 20000 -if sensors start -then -fi +sensors start diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 4b2bfaba7a..1a064cbf36 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -60,7 +60,6 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) : I2C("Airspeed", path, bus, address, 100000), - _reports(nullptr), _sensor_ok(false), _last_published_sensor_ok(true), /* initialize differently to force publication */ _measure_ticks(0), @@ -90,11 +89,6 @@ Airspeed::~Airspeed() unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); } - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -103,18 +97,9 @@ Airspeed::~Airspeed() int Airspeed::init() { - int ret = PX4_ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) { - goto out; - } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); - - if (_reports == nullptr) { - goto out; + if (I2C::init() != PX4_OK) { + return PX4_ERROR; } /* register alternate interfaces if we have to */ @@ -122,8 +107,7 @@ Airspeed::init() /* advertise sensor topic, measure manually to initialize valid report */ measure(); - differential_pressure_s arp; - _reports->get(&arp); + differential_pressure_s arp = {}; /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise_multi(ORB_ID(differential_pressure), &arp, &_airspeed_orb_class_instance, @@ -133,10 +117,7 @@ Airspeed::init() PX4_WARN("uORB started?"); } - ret = OK; - -out: - return ret; + return PX4_OK; } int @@ -217,6 +198,7 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) } } } + break; case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) { @@ -225,27 +207,6 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) return (1000 / _measure_ticks); - case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { - return -EINVAL; - } - - ATOMIC_ENTER; - - if (!_reports->resize(arg)) { - ATOMIC_LEAVE; - return -ENOMEM; - } - - ATOMIC_LEAVE; - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _reports->size(); - case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; @@ -269,72 +230,11 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) } } -ssize_t -Airspeed::read(device::file_t *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(differential_pressure_s); - differential_pressure_s *abuf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(abuf)) { - ret += sizeof(*abuf); - abuf++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - do { - _reports->flush(); - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - usleep(_conversion_interval); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - if (_reports->get(abuf)) { - ret = sizeof(*abuf); - } - - } while (0); - - return ret; -} - void Airspeed::start() { /* reset the report ring and state machine */ _collect_phase = false; - _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); @@ -372,25 +272,7 @@ void Airspeed::cycle_trampoline(void *arg) { Airspeed *dev = (Airspeed *)arg; - dev->cycle(); - // XXX we do not know if this is - // really helping - do not update the - // subsys state right now - //dev->update_status(); -} -void -Airspeed::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - warnx("poll interval: %u ticks", _measure_ticks); - _reports->print_info("report queue"); -} - -void -Airspeed::new_report(const differential_pressure_s &report) -{ - _reports->force(&report); + dev->update_status(); } diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 6b6af4b358..dc9dccc3fa 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -31,23 +31,13 @@ * ****************************************************************************/ -/** - * @file airspeed.h - * @author Simon Wilks - * - * Generic driver for airspeed sensors connected via I2C. - */ - #include -#include #include #include #include #include #include #include -#include -#include #include #include #include @@ -56,10 +46,6 @@ /* Default I2C bus */ static constexpr uint8_t PX4_I2C_BUS_DEFAULT = PX4_I2C_BUS_EXPANSION; -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - class __EXPORT Airspeed : public device::I2C { public: @@ -68,17 +54,9 @@ public: virtual int init(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - /** - * Diagnostics - print some basic information about the driver. - */ - virtual void print_info(); - private: - ringbuffer::RingBuffer *_reports; - /* this class has pointer data members and should not be copied */ Airspeed(const Airspeed &); Airspeed &operator=(const Airspeed &); @@ -118,16 +96,6 @@ protected: perf_counter_t _sample_perf; perf_counter_t _comms_errors; - - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - /** * Initialise the automatic measurement state machine and start it. * diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 9536843c43..439b153e6e 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -165,11 +165,6 @@ ETSAirspeed::collect() orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); } - new_report(report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - ret = OK; perf_end(_sample_perf); @@ -244,10 +239,10 @@ namespace ets_airspeed ETSAirspeed *g_dev; int start(int i2c_bus); -int stop(); -int test(); -int reset(); -int info(); +int stop(); +int test(); +int reset(); +int info(); /** * Start the driver. @@ -272,7 +267,7 @@ start(int i2c_bus) goto fail; } - if (OK != g_dev->Airspeed::init()) { + if (OK != g_dev->init()) { goto fail; } @@ -296,7 +291,8 @@ fail: g_dev = nullptr; } - PX4_WARN("no ETS airspeed sensor connected on bus %d", i2c_bus); + PX4_WARN("not started on bus %d", i2c_bus); + return PX4_ERROR; } @@ -346,7 +342,7 @@ test() } PX4_INFO("single read"); - PX4_INFO("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); + PX4_INFO("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -356,22 +352,23 @@ test() /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; + px4_pollfd_struct_t fds; /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; - ret = poll(&fds, 1, 2000); + ret = px4_poll(&fds, 1, 2000); if (ret != 1) { PX4_ERR("timed out waiting for sensor data"); + return PX4_ERROR; } /* now go get it */ sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - err(1, "periodic read failed"); + PX4_ERR("periodic read failed"); } PX4_INFO("periodic read %u", i); @@ -396,7 +393,7 @@ reset() int fd = px4_open(ETS_PATH, O_RDONLY); if (fd < 0) { - PX4_ERR("failed "); + PX4_ERR("failed"); return PX4_ERROR; } @@ -413,22 +410,6 @@ reset() return PX4_OK; } -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - PX4_INFO("state @ %p", g_dev); - g_dev->print_info(); - return PX4_OK; -} - } // namespace @@ -485,13 +466,6 @@ ets_airspeed_main(int argc, char *argv[]) return ets_airspeed::reset(); } - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { - return ets_airspeed::info(); - } - ets_airspeed_usage(); return PX4_OK; diff --git a/src/drivers/ms4525_airspeed/ms4525_airspeed.cpp b/src/drivers/ms4525_airspeed/ms4525_airspeed.cpp index 0ca1ee7e90..65c778ccc0 100644 --- a/src/drivers/ms4525_airspeed/ms4525_airspeed.cpp +++ b/src/drivers/ms4525_airspeed/ms4525_airspeed.cpp @@ -49,13 +49,10 @@ * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) */ - #include #include -#include - #include #include #include @@ -238,11 +235,6 @@ MEASAirspeed::collect() orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); } - new_report(report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - ret = OK; perf_end(_sample_perf); @@ -382,11 +374,10 @@ namespace meas_airspeed MEASAirspeed *g_dev = nullptr; -int start(int i2c_bus); -int stop(); -int test(); -int reset(); -int info(); +int start(int i2c_bus); +int stop(); +int test(); +int reset(); /** * Start the driver. @@ -412,8 +403,7 @@ start(int i2c_bus) goto fail; } - /* both versions failed if the init for the MS5525DSO fails, give up */ - if (OK != g_dev->Airspeed::init()) { + if (OK != g_dev->init()) { goto fail; } @@ -437,7 +427,8 @@ fail: g_dev = nullptr; } - PX4_WARN("no MS4525 airspeed sensor connected on bus %d", i2c_bus); + PX4_WARN("not started on bus %d", i2c_bus); + return PX4_ERROR; } @@ -505,7 +496,8 @@ test() ret = px4_poll(&fds, 1, 2000); if (ret != 1) { - PX4_ERR("timed out"); + PX4_ERR("timed out waiting for sensor data"); + return PX4_ERROR; } /* now go get it */ @@ -539,7 +531,7 @@ reset() int fd = px4_open(PATH_MS4525, O_RDONLY); if (fd < 0) { - PX4_ERR("failed "); + PX4_ERR("failed"); return PX4_ERROR; } @@ -556,23 +548,6 @@ reset() return PX4_OK; } -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - PX4_INFO("state @ %p", g_dev); - g_dev->print_info(); - - return PX4_OK; -} - } // namespace @@ -583,7 +558,7 @@ meas_airspeed_usage() PX4_INFO("options:"); PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); PX4_INFO("command:"); - PX4_INFO("\tstart|stop|reset|test|info"); + PX4_INFO("\tstart|stop|reset|test"); } int @@ -629,13 +604,6 @@ ms4525_airspeed_main(int argc, char *argv[]) return meas_airspeed::reset(); } - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { - return meas_airspeed::info(); - } - meas_airspeed_usage(); return PX4_OK; diff --git a/src/drivers/ms5525_airspeed/MS5525.cpp b/src/drivers/ms5525_airspeed/MS5525.cpp index 9ce607f928..2c830b7e12 100644 --- a/src/drivers/ms5525_airspeed/MS5525.cpp +++ b/src/drivers/ms5525_airspeed/MS5525.cpp @@ -264,11 +264,6 @@ MS5525::collect() orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &diff_pressure); } - new_report(diff_pressure); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - ret = OK; perf_end(_sample_perf); diff --git a/src/drivers/ms5525_airspeed/MS5525.hpp b/src/drivers/ms5525_airspeed/MS5525.hpp index ecaba5fa15..325b9a5ae6 100644 --- a/src/drivers/ms5525_airspeed/MS5525.hpp +++ b/src/drivers/ms5525_airspeed/MS5525.hpp @@ -48,7 +48,6 @@ /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ static constexpr uint8_t I2C_ADDRESS_1_MS5525DSO = 0x76; -static constexpr uint8_t I2C_ADDRESS_2_MS5525DSO = 0x77; static constexpr const char PATH_MS5525[] = "/dev/ms5525"; diff --git a/src/drivers/ms5525_airspeed/MS5525_main.cpp b/src/drivers/ms5525_airspeed/MS5525_main.cpp index e186b55a1f..b9cf2cf543 100644 --- a/src/drivers/ms5525_airspeed/MS5525_main.cpp +++ b/src/drivers/ms5525_airspeed/MS5525_main.cpp @@ -66,22 +66,9 @@ start(uint8_t i2c_bus) goto fail; } - /* try the next MS5525DSO if init fails */ - if (OK != g_dev->Airspeed::init()) { - delete g_dev; - - g_dev = new MS5525(i2c_bus, I2C_ADDRESS_2_MS5525DSO, PATH_MS5525); - - /* check if the MS5525DSO was instantiated */ - if (g_dev == nullptr) { - PX4_WARN("MS5525 was not instantiated"); - goto fail; - } - - /* both versions failed if the init for the MS5525DSO fails, give up */ - if (OK != g_dev->Airspeed::init()) { - goto fail; - } + /* try to initialize */ + if (g_dev->init() != PX4_OK) { + goto fail; } /* set the poll rate to default, starts automatic data collection */ @@ -104,7 +91,8 @@ fail: g_dev = nullptr; } - PX4_WARN("no MS5525 airspeed sensor connected on bus %d", i2c_bus); + PX4_WARN("not started on bus %d", i2c_bus); + return PX4_ERROR; } diff --git a/src/drivers/sdp3x_airspeed/SDP3X.cpp b/src/drivers/sdp3x_airspeed/SDP3X.cpp index 622a71d2d9..0fa7c9bd0a 100644 --- a/src/drivers/sdp3x_airspeed/SDP3X.cpp +++ b/src/drivers/sdp3x_airspeed/SDP3X.cpp @@ -160,11 +160,6 @@ SDP3X::collect() orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); } - new_report(report); - - // notify anyone waiting for data - poll_notify(POLLIN); - ret = OK; perf_end(_sample_perf); diff --git a/src/drivers/sdp3x_airspeed/SDP3X_main.cpp b/src/drivers/sdp3x_airspeed/SDP3X_main.cpp index 3ae776c732..76dfceacb4 100644 --- a/src/drivers/sdp3x_airspeed/SDP3X_main.cpp +++ b/src/drivers/sdp3x_airspeed/SDP3X_main.cpp @@ -41,22 +41,21 @@ namespace sdp3x_airspeed { SDP3X *g_dev = nullptr; -int start(int i2c_bus); +int start(uint8_t i2c_bus); int stop(); int test(); int reset(); -int info(); // Start the driver. // This function call only returns once the driver is up and running // or failed to detect the sensor. int -start(int i2c_bus) +start(uint8_t i2c_bus) { int fd = -1; if (g_dev != nullptr) { - PX4_WARN("driver already started"); + PX4_WARN("already started"); return PX4_ERROR; } @@ -68,7 +67,7 @@ start(int i2c_bus) } /* try the next SDP3XDSO if init fails */ - if (OK != g_dev->Airspeed::init()) { + if (g_dev->init() != PX4_OK) { delete g_dev; g_dev = new SDP3X(i2c_bus, I2C_ADDRESS_2_SDP3X, PATH_SDP3X); @@ -80,7 +79,7 @@ start(int i2c_bus) } /* both versions failed if the init for the SDP3XDSO fails, give up */ - if (OK != g_dev->Airspeed::init()) { + if (g_dev->init() != PX4_OK) { goto fail; } } @@ -105,7 +104,8 @@ fail: g_dev = nullptr; } - PX4_WARN("no SDP3X airspeed sensor connected on bus %d", i2c_bus); + PX4_WARN("not started on bus %d", i2c_bus); + return PX4_ERROR; } @@ -213,20 +213,6 @@ int reset() return PX4_OK; } -// print a little info about the driver -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - PX4_INFO("state @ %p", g_dev); - g_dev->print_info(); - return PX4_OK; -} - } // namespace sdp3x_airspeed @@ -237,13 +223,13 @@ sdp3x_airspeed_usage() PX4_WARN("options:"); PX4_WARN("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); PX4_WARN("command:"); - PX4_WARN("\tstart|stop|reset|test|info"); + PX4_WARN("\tstart|stop|reset|test"); } int sdp3x_airspeed_main(int argc, char *argv[]) { - int i2c_bus = PX4_I2C_BUS_DEFAULT; + uint8_t i2c_bus = PX4_I2C_BUS_DEFAULT; for (int i = 1; i < argc; i++) { if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { @@ -281,13 +267,6 @@ sdp3x_airspeed_main(int argc, char *argv[]) return sdp3x_airspeed::reset(); } - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { - return sdp3x_airspeed::info(); - } - sdp3x_airspeed_usage(); return PX4_OK;