diff options
-rw-r--r-- | src/drivers/ets_airspeed/ets_airspeed.cpp | 139 |
1 files changed, 71 insertions, 68 deletions
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index b1d9a1cb9..adabc1525 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -37,7 +37,7 @@ * * Driver for the Eagle Tree Airspeed V3 connected via I2C. */ - + #include <nuttx/config.h> #include <drivers/device/i2c.h> @@ -81,9 +81,9 @@ /* Register address */ #define READ_CMD 0x07 /* Read the data */ - + /** - * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. + * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. */ #define MIN_ACCURATE_DIFF_PRES_PA 12 @@ -105,38 +105,38 @@ class ETSAirspeed : public device::I2C public: ETSAirspeed(int bus, int address = I2C_ADDRESS); virtual ~ETSAirspeed(); - + virtual int init(); - + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - + /** * Diagnostics - print some basic information about the driver. */ void print_info(); - + protected: virtual int probe(); private: - work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; + work_s _work; + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; differential_pressure_s *_reports; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; - int _diff_pres_offset; - - orb_advert_t _airspeed_pub; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - - + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _diff_pres_offset; + + orb_advert_t _airspeed_pub; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** * Test whether the device supported by the driver is present at a * specific address. @@ -144,28 +144,28 @@ private: * @param address The I2C bus address to probe. * @return True if the device is present. */ - int probe_address(uint8_t address); - + int probe_address(uint8_t address); + /** * Initialise the automatic measurement state machine and start it. * * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start(); - + void start(); + /** * Stop the automatic measurement state machine. */ - void stop(); - + void stop(); + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); - int measure(); - int collect(); + void cycle(); + int measure(); + int collect(); /** * Static trampoline from the workq context; because we don't have a @@ -173,9 +173,9 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); - - + static void cycle_trampoline(void *arg); + + }; /* helper macro for handling report buffer indices */ @@ -203,7 +203,7 @@ ETSAirspeed::ETSAirspeed(int bus, int address) : { // enable debug() calls _debug_enabled = true; - + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -230,6 +230,7 @@ ETSAirspeed::init() /* allocate basic report buffers */ _num_reports = 2; _reports = new struct differential_pressure_s[_num_reports]; + for (unsigned i = 0; i < _num_reports; i++) _reports[i].max_differential_pressure_pa = 0; @@ -351,11 +352,11 @@ ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGQUEUEDEPTH: return _num_reports - 1; - + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; - + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -432,14 +433,14 @@ ETSAirspeed::measure() uint8_t cmd = READ_CMD; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); return ret; } + ret = OK; - + return ret; } @@ -447,27 +448,28 @@ int ETSAirspeed::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[2] = {0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 2); - + if (ret < 0) { log("error reading from sensor: %d", ret); return ret; } - + uint16_t diff_pres_pa = val[1] << 8 | val[0]; param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); - - if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { + + if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { diff_pres_pa = 0; + } else { - diff_pres_pa -= _diff_pres_offset; + diff_pres_pa -= _diff_pres_offset; } // XXX we may want to smooth out the readings to remove noise. @@ -498,7 +500,7 @@ ETSAirspeed::collect() ret = OK; perf_end(_sample_perf); - + return ret; } @@ -511,17 +513,19 @@ ETSAirspeed::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, true, true, - SUBSYSTEM_TYPE_DIFFPRESSURE}; + SUBSYSTEM_TYPE_DIFFPRESSURE + }; static orb_advert_t pub = -1; if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -597,7 +601,7 @@ ETSAirspeed::print_info() perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _num_reports, _oldest_report, _next_report, _reports); } /** @@ -653,8 +657,7 @@ start(int i2c_bus) fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -668,15 +671,14 @@ fail: void stop() { - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } - else - { + + } else { errx(1, "driver not running"); } + exit(0); } @@ -773,8 +775,8 @@ info() } // namespace -static void -ets_airspeed_usage() +static void +ets_airspeed_usage() { fprintf(stderr, "usage: ets_airspeed command [options]\n"); fprintf(stderr, "options:\n"); @@ -789,6 +791,7 @@ ets_airspeed_main(int argc, char *argv[]) int i2c_bus = PX4_I2C_BUS_DEFAULT; int i; + for (i = 1; i < argc; i++) { if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { if (argc > i + 1) { @@ -802,12 +805,12 @@ ets_airspeed_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) ets_airspeed::start(i2c_bus); - - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - ets_airspeed::stop(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + ets_airspeed::stop(); /* * Test the driver/device. |