diff --git a/Debug/NuttX b/Debug/NuttX index 5396ab6b1c..3192326b8d 100644 --- a/Debug/NuttX +++ b/Debug/NuttX @@ -95,6 +95,7 @@ end document showfiles . showfiles . Prints the files opened by a task. +end ################################################################################ # Task display diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index f5f29fde33..c4b2aa944c 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -99,7 +99,7 @@ I2C::init() } // tell the world where we are - log("on bus %d at 0x%02x", _bus, _address); + log("on I2C bus %d at 0x%02x", _bus, _address); out: return ret; diff --git a/apps/drivers/device/spi.cpp b/apps/drivers/device/spi.cpp index f56f5433a7..a1761b7694 100644 --- a/apps/drivers/device/spi.cpp +++ b/apps/drivers/device/spi.cpp @@ -111,7 +111,7 @@ SPI::init() } // tell the workd where we are - log("on bus %d at %d", _bus, _device); + log("on SPI bus %d at %d", _bus, _device); out: return ret; diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 3ebfd5f2cf..d1025bcc6e 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -147,12 +147,9 @@ private: orb_advert_t _mag_topic; - unsigned _reads; - unsigned _measure_errors; - unsigned _read_errors; - unsigned _buf_overflows; - 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 @@ -256,11 +253,9 @@ HMC5883::HMC5883(int bus) : _oldest_report(0), _reports(nullptr), _mag_topic(-1), - _reads(0), - _measure_errors(0), - _read_errors(0), - _buf_overflows(0), - _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")) + _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), + _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")) { // enable debug() calls _debug_enabled = true; @@ -303,6 +298,12 @@ HMC5883::init() goto out; _oldest_report = _next_report = 0; + /* get a publish handle on the mag topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]); + if (_mag_topic < 0) + debug("failed to create sensor_mag object"); + ret = OK; out: return ret; @@ -313,7 +314,7 @@ HMC5883::probe() { uint8_t data[3] = {0, 0, 0}; - _retries = 3; + _retries = 10; if (read_reg(ADDR_ID_A, data[0]) || read_reg(ADDR_ID_B, data[1]) || read_reg(ADDR_ID_C, data[2])) @@ -356,8 +357,6 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) } } - _reads++; - /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } @@ -385,7 +384,6 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) /* state machine will have generated a report, copy it out */ memcpy(buffer, _reports, sizeof(*_reports)); ret = sizeof(*_reports); - _reads++; } while (0); @@ -548,31 +546,13 @@ HMC5883::cycle_trampoline(void *arg) void HMC5883::cycle() { - /* - * We have to publish the mag topic in the context of the workq - * in order to ensure that the descriptor is valid when we go to publish. - * - * @bug We can't really ever be torn down and restarted, since this - * descriptor will never be closed and on the restart we will be - * unable to re-advertise. - */ - if (_mag_topic == -1) { - struct mag_report m; - - /* if this fails (e.g. no object in the system) we will cope */ - memset(&m, 0, sizeof(m)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &m); - - if (_mag_topic < 0) - debug("failed to create sensor_mag object"); - } - /* collection phase? */ if (_collect_phase) { /* perform collection */ if (OK != collect()) { - log("FATAL collection error - restarting\n"); + log("collection error"); + /* restart the measurement state machine */ start(); return; } @@ -596,10 +576,8 @@ HMC5883::cycle() } /* measurement phase */ - if (OK != measure()) { - log("FATAL measure error - restarting\n"); - start(); - } + if (OK != measure()) + log("measure error"); /* next phase is collection */ _collect_phase = true; @@ -622,7 +600,7 @@ HMC5883::measure() ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE); if (OK != ret) - _measure_errors++; + perf_count(_comms_errors); return ret; } @@ -661,6 +639,7 @@ HMC5883::collect() ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); if (ret != OK) { + perf_count(_comms_errors); debug("data/status read error"); goto out; } @@ -692,7 +671,7 @@ HMC5883::collect() /* if we are running up against the oldest report, toss it */ if (_next_report == _oldest_report) { - _buf_overflows++; + perf_count(_buffer_overflows); INCREMENT(_oldest_report, _num_reports); } @@ -737,10 +716,9 @@ HMC5883::meas_to_float(uint8_t in[2]) void HMC5883::print_info() { - printf("reads: %u\n", _reads); - printf("measure errors: %u\n", _measure_errors); - printf("read errors: %u\n", _read_errors); - printf("read overflows: %u\n", _buf_overflows); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + 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); diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 31e2b8888a..3afb0ddda3 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -298,8 +298,8 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _reads(0), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")) { - // enable debug() calls - _debug_enabled = true; + // disable debug() calls + _debug_enabled = false; // default accel scale factors _accel_scale.x_offset = 0; @@ -463,7 +463,7 @@ MPU6000::probe() case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: - log("ID 0x%02x", _product); + debug("ID 0x%02x", _product); return OK; } diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 4921efb283..847551aa2b 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -132,12 +132,9 @@ private: orb_advert_t _baro_topic; - unsigned _reads; - unsigned _measure_errors; - unsigned _read_errors; - unsigned _buf_overflows; - 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 @@ -252,11 +249,9 @@ MS5611::MS5611(int bus) : _dT(0), _temp64(0), _baro_topic(-1), - _reads(0), - _measure_errors(0), - _read_errors(0), - _buf_overflows(0), - _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")) + _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), + _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { // enable debug() calls _debug_enabled = true; @@ -292,6 +287,12 @@ MS5611::init() _oldest_report = _next_report = 0; + /* get a publish handle on the baro topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); + if (_baro_topic < 0) + debug("failed to create sensor_baro object"); + ret = OK; out: return ret; @@ -300,7 +301,7 @@ out: int MS5611::probe() { - _retries = 3; + _retries = 10; if((OK == probe_address(MS5611_ADDRESS_1)) || (OK == probe_address(MS5611_ADDRESS_2))) { _retries = 1; @@ -358,8 +359,6 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } } - _reads++; - /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } @@ -399,7 +398,6 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) /* state machine will have generated a report, copy it out */ memcpy(buffer, _reports, sizeof(*_reports)); ret = sizeof(*_reports); - _reads++; } while (0); @@ -541,31 +539,14 @@ MS5611::cycle_trampoline(void *arg) void MS5611::cycle() { - /* - * We have to publish the baro topic in the context of the workq - * in order to ensure that the descriptor is valid when we go to publish. - * - * @bug We can't really ever be torn down and restarted, since this - * descriptor will never be closed and on the restart we will be - * unable to re-advertise. - */ - if (_baro_topic == -1) { - struct baro_report b; - - /* if this fails (e.g. no object in the system) we will cope */ - memset(&b, 0, sizeof(b)); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &b); - - if (_baro_topic < 0) - debug("failed to create sensor_baro object"); - } /* collection phase? */ if (_collect_phase) { /* perform collection */ if (OK != collect()) { - log("FATAL collection error - restarting\n"); + log("collection error"); + /* reset the collection state machine and try again */ start(); return; } @@ -592,10 +573,8 @@ MS5611::cycle() } /* measurement phase */ - if (OK != measure()) { - log("FATAL measure error - restarting\n"); - start(); - } + if (OK != measure()) + log("measure error"); /* next phase is collection */ _collect_phase = true; @@ -623,7 +602,7 @@ MS5611::measure() ret = transfer(&cmd_data, 1, nullptr, 0); if (OK != ret) - _measure_errors++; + perf_count(_comms_errors); return ret; } @@ -643,7 +622,7 @@ MS5611::collect() _reports[_next_report].timestamp = hrt_absolute_time(); if (OK != transfer(&cmd, 1, &data[0], 3)) { - _read_errors++; + perf_count(_comms_errors); return -EIO; } @@ -691,7 +670,7 @@ MS5611::collect() /* if we are running up against the oldest report, toss it */ if (_next_report == _oldest_report) { - _buf_overflows++; + perf_count(_buffer_overflows); INCREMENT(_oldest_report, _num_reports); } @@ -774,10 +753,9 @@ MS5611::crc4(uint16_t *n_prom) void MS5611::print_info() { - printf("reads: %u\n", _reads); - printf("measure errors: %u\n", _measure_errors); - printf("read errors: %u\n", _read_errors); - printf("read overflows: %u\n", _buf_overflows); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + 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); diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile index 125839bb35..526fb0164f 100644 --- a/apps/sensors/Makefile +++ b/apps/sensors/Makefile @@ -39,4 +39,7 @@ APPNAME = sensors PRIORITY = SCHED_PRIORITY_MAX-5 STACKSIZE = 4096 +CXXSRCS = sensors.cpp +CSRCS = sensor_params.c + include $(APPDIR)/mk/app.mk diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c new file mode 100644 index 0000000000..221711c7f2 --- /dev/null +++ b/apps/sensors/sensor_params.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * + * 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 sensor_params.c + * + * Parameters defined by the sensors task. + */ + +#include + +#include + +PARAM_DEFINE_FLOAT(SENSOR_GYRO_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENSOR_GYRO_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENSOR_GYRO_ZOFF, 0.0f); + +PARAM_DEFINE_FLOAT(SENSOR_MAG_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENSOR_MAG_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENSOR_MAG_ZOFF, 0.0f); + +PARAM_DEFINE_FLOAT(SENSOR_ACC_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENSOR_ACC_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENSOR_ACC_ZOFF, 0.0f); + +PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); +PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); +PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); +PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); + +PARAM_DEFINE_FLOAT(RC2_MIN, 1000); +PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC2_MAX, 2000); +PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); + +PARAM_DEFINE_FLOAT(RC3_MIN, 1000); +PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC3_MAX, 2000); +PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); + +PARAM_DEFINE_FLOAT(RC4_MIN, 1000); +PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC4_MAX, 2000); +PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); + +PARAM_DEFINE_FLOAT(RC5_MIN, 1000); +PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC5_MAX, 2000); +PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); + +PARAM_DEFINE_FLOAT(RC6_MIN, 1000); +PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC6_MAX, 2000); +PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); + +PARAM_DEFINE_FLOAT(RC7_MIN, 1000); +PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC7_MAX, 2000); +PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); + +PARAM_DEFINE_FLOAT(RC8_MIN, 1000); +PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC8_MAX, 2000); +PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); + +PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA + +/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); + +PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); +PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); +PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); +PARAM_DEFINE_INT32(RC_MAP_YAW, 4); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp new file mode 100644 index 0000000000..54b57239bd --- /dev/null +++ b/apps/sensors/sensors.cpp @@ -0,0 +1,985 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * + * 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 sensors.cpp + * + * Sensor readout process. + */ + +#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 +#include +#include +#include +#include + +#include "sensors.h" + +#define SENSOR_INTERVAL_MICROSEC 2000 + +#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ +#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ +#define MAGN_HEALTH_COUNTER_LIMIT_ERROR 100 /* 1000 ms downtime at 100 Hz update rate */ +#define BARO_HEALTH_COUNTER_LIMIT_ERROR 50 /* 500 ms downtime at 100 Hz update rate */ +#define ADC_HEALTH_COUNTER_LIMIT_ERROR 10 /* 100 ms downtime at 100 Hz update rate */ + +#define GYRO_HEALTH_COUNTER_LIMIT_OK 5 +#define ACC_HEALTH_COUNTER_LIMIT_OK 5 +#define MAGN_HEALTH_COUNTER_LIMIT_OK 5 +#define BARO_HEALTH_COUNTER_LIMIT_OK 5 +#define ADC_HEALTH_COUNTER_LIMIT_OK 5 + +#define ADC_BATTERY_VOLATGE_CHANNEL 10 + +#define BAT_VOL_INITIAL 12.f +#define BAT_VOL_LOWPASS_1 0.99f +#define BAT_VOL_LOWPASS_2 0.01f +#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f + +#ifdef CONFIG_HRT_PPM +extern "C" { + extern uint16_t ppm_buffer[]; + extern unsigned ppm_decoded_channels; + extern uint64_t ppm_last_valid_decode; +} + +/* PPM Settings */ +# define PPM_MIN 1000 +# define PPM_MAX 2000 +/* Internal resolution is 10000 */ +# define PPM_SCALE 10000/((PPM_MAX-PPM_MIN)/2) +# define PPM_MID (PPM_MIN+PPM_MAX)/2 +#endif + +/** + * Sensor app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int sensors_main(int argc, char *argv[]); + +class Sensors +{ +public: + Sensors(); + ~Sensors(); + + int start(); + void stop(); + +private: + static const unsigned _rc_max_chan_count = 8; + + /* legacy sensor descriptors */ + int _fd_bma180; + int _fd_gyro_l3gd20; + +#if CONFIG_HRT_PPM + hrt_abstime _ppm_last_valid; + void ppm_poll(); +#endif + + /* XXX should not be here - should be own driver */ + int _fd_adc; + hrt_abstime _last_adc; + + bool _task_should_exit; + int _sensors_task; + + bool _hil_enabled; + bool _publishing; + + int _gyro_sub; + int _accel_sub; + int _mag_sub; + int _baro_sub; + int _vstatus_sub; + + orb_advert_t _sensor_pub; + orb_advert_t _manual_control_pub; + orb_advert_t _rc_pub; + + perf_counter_t _loop_perf; + + struct rc_channels_s _rc; + + struct { + int min[_rc_max_chan_count]; + int trim[_rc_max_chan_count]; + int max[_rc_max_chan_count]; + int rev[_rc_max_chan_count]; + + float gyro_offset[3]; + float mag_offset[3]; + float acc_offset[3]; + + int rc_type; + + int rc_map_roll; + int rc_map_pitch; + int rc_map_yaw; + int rc_map_throttle; + int rc_map_mode_sw; + + float battery_voltage_scaling; + } _parameters; + + struct { + param_t min[_rc_max_chan_count]; + param_t trim[_rc_max_chan_count]; + param_t max[_rc_max_chan_count]; + param_t rev[_rc_max_chan_count]; + param_t rc_type; + + param_t gyro_offset[3]; + param_t mag_offset[3]; + param_t acc_offset[3]; + + param_t rc_map_roll; + param_t rc_map_pitch; + param_t rc_map_yaw; + param_t rc_map_throttle; + param_t rc_map_mode_sw; + + param_t battery_voltage_scaling; + } _parameter_handles; + + + int parameters_update(); + + void accel_init(); + void gyro_init(); + void mag_init(); + void baro_init(); + void adc_init(); + + void accel_poll(struct sensor_combined_s &raw); + void gyro_poll(struct sensor_combined_s &raw); + void mag_poll(struct sensor_combined_s &raw); + void baro_poll(struct sensor_combined_s &raw); + + void vehicle_status_poll(); + void adc_poll(struct sensor_combined_s &raw); + + static void task_main_trampoline(int argc, char *argv[]); + void task_main() __attribute__((noreturn)); + +}; + +namespace sensors +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +Sensors *g_sensors; +} + +Sensors::Sensors() : + _fd_bma180(-1), + _fd_gyro_l3gd20(-1), + _ppm_last_valid(0), + + _fd_adc(-1), + _last_adc(0), + + _task_should_exit(false), + _sensors_task(-1), + _hil_enabled(false), + _publishing(true), + + /* subscriptions */ + _gyro_sub(-1), + _accel_sub(-1), + _mag_sub(-1), + _baro_sub(-1), + _vstatus_sub(-1), + + /* publications */ + _sensor_pub(-1), + _manual_control_pub(-1), + _rc_pub(-1), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) +{ + /* min values */ + _parameter_handles.min[0] = param_find("RC1_MIN"); + _parameter_handles.min[1] = param_find("RC2_MIN"); + _parameter_handles.min[2] = param_find("RC3_MIN"); + _parameter_handles.min[3] = param_find("RC4_MIN"); + _parameter_handles.min[4] = param_find("RC5_MIN"); + _parameter_handles.min[5] = param_find("RC6_MIN"); + _parameter_handles.min[6] = param_find("RC7_MIN"); + _parameter_handles.min[7] = param_find("RC8_MIN"); + + /* trim values */ + _parameter_handles.trim[0] = param_find("RC1_TRIM"); + _parameter_handles.trim[1] = param_find("RC2_TRIM"); + _parameter_handles.trim[2] = param_find("RC3_TRIM"); + _parameter_handles.trim[3] = param_find("RC4_TRIM"); + _parameter_handles.trim[4] = param_find("RC5_TRIM"); + _parameter_handles.trim[5] = param_find("RC6_TRIM"); + _parameter_handles.trim[6] = param_find("RC7_TRIM"); + _parameter_handles.trim[7] = param_find("RC8_TRIM"); + + /* max values */ + _parameter_handles.max[0] = param_find("RC1_MAX"); + _parameter_handles.max[1] = param_find("RC2_MAX"); + _parameter_handles.max[2] = param_find("RC3_MAX"); + _parameter_handles.max[3] = param_find("RC4_MAX"); + _parameter_handles.max[4] = param_find("RC5_MAX"); + _parameter_handles.max[5] = param_find("RC6_MAX"); + _parameter_handles.max[6] = param_find("RC7_MAX"); + _parameter_handles.max[7] = param_find("RC8_MAX"); + + /* channel reverse */ + _parameter_handles.rev[0] = param_find("RC1_REV"); + _parameter_handles.rev[1] = param_find("RC2_REV"); + _parameter_handles.rev[2] = param_find("RC3_REV"); + _parameter_handles.rev[3] = param_find("RC4_REV"); + _parameter_handles.rev[4] = param_find("RC5_REV"); + _parameter_handles.rev[5] = param_find("RC6_REV"); + _parameter_handles.rev[6] = param_find("RC7_REV"); + _parameter_handles.rev[7] = param_find("RC8_REV"); + + _parameter_handles.rc_type = param_find("RC_TYPE"); + + _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); + _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); + _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); + _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + + /* gyro offsets */ + _parameter_handles.gyro_offset[0] = param_find("SENSOR_GYRO_XOFF"); + _parameter_handles.gyro_offset[1] = param_find("SENSOR_GYRO_YOFF"); + _parameter_handles.gyro_offset[2] = param_find("SENSOR_GYRO_ZOFF"); + + /* accel offsets */ + _parameter_handles.acc_offset[0] = param_find("SENSOR_ACC_XOFF"); + _parameter_handles.acc_offset[1] = param_find("SENSOR_ACC_YOFF"); + _parameter_handles.acc_offset[2] = param_find("SENSOR_ACC_ZOFF"); + + /* mag offsets */ + _parameter_handles.mag_offset[0] = param_find("SENSOR_MAG_XOFF"); + _parameter_handles.mag_offset[1] = param_find("SENSOR_MAG_YOFF"); + _parameter_handles.mag_offset[2] = param_find("SENSOR_MAG_ZOFF"); + + _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + + /* fetch initial parameter values */ + parameters_update(); +} + +Sensors::~Sensors() +{ + if (_sensors_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + unsigned i = 0; + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_sensors_task); + break; + } + } while (_sensors_task != -1); + } + + sensors::g_sensors = nullptr; +} + +int +Sensors::parameters_update() +{ + const unsigned int nchans = 8; + + /* min values */ + for (unsigned int i = 0; i < nchans; i++) { + param_get(_parameter_handles.min[i], &(_parameters.min[i])); + } + + /* trim values */ + for (unsigned int i = 0; i < nchans; i++) { + param_get(_parameter_handles.trim[i], &(_parameters.trim[i])); + } + + /* max values */ + for (unsigned int i = 0; i < nchans; i++) { + param_get(_parameter_handles.max[i], &(_parameters.max[i])); + } + + /* channel reverse */ + for (unsigned int i = 0; i < nchans; i++) { + param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); + } + + /* remote control type */ + param_get(_parameter_handles.rc_type, &(_parameters.rc_type)); + + /* channel mapping */ + param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)); + param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)); + param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)); + param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)); + param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)); + + /* gyro offsets */ + param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); + param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1])); + param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2])); + + /* accel offsets */ + param_get(_parameter_handles.acc_offset[0], &(_parameters.acc_offset[0])); + param_get(_parameter_handles.acc_offset[1], &(_parameters.acc_offset[1])); + param_get(_parameter_handles.acc_offset[2], &(_parameters.acc_offset[2])); + + /* mag offsets */ + param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_offset[0])); + param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_offset[1])); + param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_offset[2])); + + /* scaling of ADC ticks to battery voltage */ + param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)); + + return OK; +} + +void +Sensors::accel_init() +{ + int fd; + + fd = open(ACCEL_DEVICE_PATH, 0); + if (fd < 0) { + warn("%s", ACCEL_DEVICE_PATH); + + /* fall back to bma180 here (new driver would be better...) */ + _fd_bma180 = open("/dev/bma180", O_RDONLY); + if (_fd_bma180 < 0) { + warn("/dev/bma180"); + errx(1, "FATAL: no accelerometer found"); + } + + /* discard first (junk) reading */ + int16_t junk_buf[3]; + read(_fd_bma180, junk_buf, sizeof(junk_buf)); + + warnx("using BMA180"); + } else { + /* set the accel internal sampling rate up to at leat 500Hz */ + if (OK != ioctl(fd, ACCELIOCSSAMPLERATE, 500)) + warn("WARNING: failed to set minimum 500Hz sample rate for accel"); + + /* set the driver to poll at 500Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 500)) + warn("WARNING: failed to set 500Hz poll rate for accel"); + + warnx("using system accel"); + close(fd); + } +} + +void +Sensors::gyro_init() +{ + int fd; + + fd = open(GYRO_DEVICE_PATH, 0); + if (fd < 0) { + warn("%s", GYRO_DEVICE_PATH); + + /* fall back to bma180 here (new driver would be better...) */ + _fd_gyro_l3gd20 = open("/dev/l3gd20", O_RDONLY); + if (_fd_gyro_l3gd20 < 0) { + warn("/dev/l3gd20"); + errx(1, "FATAL: no gyro found"); + } + + /* discard first (junk) reading */ + int16_t junk_buf[3]; + read(_fd_gyro_l3gd20, junk_buf, sizeof(junk_buf)); + + warn("using L3GD20"); + } else { + /* set the gyro internal sampling rate up to at leat 500Hz */ + if (OK != ioctl(fd, GYROIOCSSAMPLERATE, 500)) + warn("WARNING: failed to set minimum 500Hz sample rate for gyro"); + + /* set the driver to poll at 500Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 500)) + warn("WARNING: failed to set 500Hz poll rate for gyro"); + + warnx("using system gyro"); + close(fd); + } +} + +void +Sensors::mag_init() +{ + int fd; + + fd = open(MAG_DEVICE_PATH, 0); + if (fd < 0) { + warn("%s", MAG_DEVICE_PATH); + errx(1, "FATAL: no magnetometer found"); + } + + /* set the mag internal poll rate to at least 150Hz */ + if (OK != ioctl(fd, MAGIOCSSAMPLERATE, 150)) + warn("WARNING: failed to set minimum 150Hz sample rate for mag"); + + /* set the driver to poll at 150Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 150)) + warn("WARNING: failed to set 150Hz poll rate for mag"); + + close(fd); +} + +void +Sensors::baro_init() +{ + int fd; + + fd = open(BARO_DEVICE_PATH, 0); + if (fd < 0) { + warn("%s", BARO_DEVICE_PATH); + errx(1, "FATAL: no barometer found"); + } + + /* set the driver to poll at 150Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 150)) + warn("WARNING: failed to set 150Hz poll rate for baro"); + + close(fd); +} + +void +Sensors::adc_init() +{ + + _fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK); + if (_fd_adc < 0) { + warn("/dev/adc0"); + errx(1, "FATAL: no ADC found"); + } +} + +void +Sensors::accel_poll(struct sensor_combined_s &raw) +{ + struct accel_report accel_report; + + if (_fd_bma180 >= 0) { + /* do ORB emulation for BMA180 */ + int16_t buf[3]; + + read(_fd_bma180, buf, sizeof(buf)); + + accel_report.timestamp = hrt_absolute_time(); + accel_report.x_raw = buf[0]; + accel_report.y_raw = buf[1]; + accel_report.z_raw = buf[2]; + + /* XXX scale raw values to readings */ + accel_report.x = 0; + accel_report.y = 0; + accel_report.z = 0; + + } else { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + } + + raw.accelerometer_m_s2[0] = accel_report.x; + raw.accelerometer_m_s2[1] = accel_report.y; + raw.accelerometer_m_s2[2] = accel_report.z; + + raw.accelerometer_raw[0] = accel_report.x_raw; + raw.accelerometer_raw[1] = accel_report.y_raw; + raw.accelerometer_raw[2] = accel_report.z_raw; + + raw.accelerometer_raw_counter++; +} + +void +Sensors::gyro_poll(struct sensor_combined_s &raw) +{ + struct gyro_report gyro_report; + + if (_fd_gyro_l3gd20 >= 0) { + /* do ORB emulation for L3GD20 */ + int16_t buf[3]; + + read(_fd_gyro_l3gd20, buf, sizeof(buf)); + + gyro_report.timestamp = hrt_absolute_time(); + gyro_report.x_raw = buf[0]; + gyro_report.y_raw = buf[1]; + gyro_report.z_raw = buf[2]; + + /* XXX scale raw values to readings */ + gyro_report.x = 0; + gyro_report.y = 0; + gyro_report.z = 0; + } else { + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + + raw.gyro_rad_s[0] = gyro_report.x; + raw.gyro_rad_s[1] = gyro_report.y; + raw.gyro_rad_s[2] = gyro_report.z; + + raw.gyro_raw[0] = gyro_report.x_raw; + raw.gyro_raw[1] = gyro_report.y_raw; + raw.gyro_raw[2] = gyro_report.z_raw; + + raw.gyro_raw_counter++; + } +} + +void +Sensors::mag_poll(struct sensor_combined_s &raw) +{ + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); + + raw.magnetometer_ga[0] = mag_report.x; + raw.magnetometer_ga[1] = mag_report.y; + raw.magnetometer_ga[2] = mag_report.z; + + raw.magnetometer_raw[0] = mag_report.x_raw; + raw.magnetometer_raw[1] = mag_report.y_raw; + raw.magnetometer_raw[2] = mag_report.z_raw; + + raw.magnetometer_raw_counter++; +} + +void +Sensors::baro_poll(struct sensor_combined_s &raw) +{ + struct baro_report baro_report; + + orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report); + + raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar + raw.baro_alt_meter = baro_report.altitude; // Altitude in meters + raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius + + raw.baro_raw_counter++; +} + +void +Sensors::vehicle_status_poll() +{ + struct vehicle_status_s vstatus; + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vstatus_sub, &vstatus_updated); + if (vstatus_updated) { + + orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus); + + /* switching from non-HIL to HIL mode */ + //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); + if (vstatus.flag_hil_enabled && !_hil_enabled) { + _hil_enabled = true; + _publishing = false; + + /* switching from HIL to non-HIL mode */ + + } else if (!_publishing && !_hil_enabled) { + _hil_enabled = false; + _publishing = true; + } + + /* update parameters */ + parameters_update(); + + /* Update RC scalings and function mappings */ + _rc.chan[0].scaling_factor = (1.0f / ((_parameters.max[0] - _parameters.min[0]) / 2.0f) * _parameters.rev[0]); + _rc.chan[0].mid = _parameters.trim[0]; + + _rc.chan[1].scaling_factor = (1.0f / ((_parameters.max[1] - _parameters.min[1]) / 2.0f) * _parameters.rev[1]); + _rc.chan[1].mid = _parameters.trim[1]; + + _rc.chan[2].scaling_factor = (1.0f / ((_parameters.max[2] - _parameters.min[2]) / 2.0f) * _parameters.rev[2]); + _rc.chan[2].mid = _parameters.trim[2]; + + _rc.chan[3].scaling_factor = (1.0f / ((_parameters.max[3] - _parameters.min[3]) / 2.0f) * _parameters.rev[3]); + _rc.chan[3].mid = _parameters.trim[3]; + + _rc.chan[4].scaling_factor = (1.0f / ((_parameters.max[4] - _parameters.min[4]) / 2.0f) * _parameters.rev[4]); + _rc.chan[4].mid = _parameters.trim[4]; + + _rc.chan[5].scaling_factor = (1.0f / ((_parameters.max[5] - _parameters.min[5]) / 2.0f) * _parameters.rev[5]); + _rc.chan[5].mid = _parameters.trim[5]; + + _rc.chan[6].scaling_factor = (1.0f / ((_parameters.max[6] - _parameters.min[6]) / 2.0f) * _parameters.rev[6]); + _rc.chan[6].mid = _parameters.trim[6]; + + _rc.chan[7].scaling_factor = (1.0f / ((_parameters.max[7] - _parameters.min[7]) / 2.0f) * _parameters.rev[7]); + _rc.chan[7].mid = _parameters.trim[7]; + + _rc.function[0] = _parameters.rc_map_throttle - 1; + _rc.function[1] = _parameters.rc_map_roll - 1; + _rc.function[2] = _parameters.rc_map_pitch - 1; + _rc.function[3] = _parameters.rc_map_yaw - 1; + _rc.function[4] = _parameters.rc_map_mode_sw - 1; + } +} + +void +Sensors::adc_poll(struct sensor_combined_s &raw) +{ + #pragma pack(push,1) + struct adc_msg4_s { + uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ + int32_t am_data1; /**< ADC convert result 1 (4 bytes) */ + uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */ + int32_t am_data2; /**< ADC convert result 2 (4 bytes) */ + uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */ + int32_t am_data3; /**< ADC convert result 3 (4 bytes) */ + uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */ + int32_t am_data4; /**< ADC convert result 4 (4 bytes) */ + } buf_adc; + #pragma pack(pop) + + if (hrt_absolute_time() - _last_adc >= 10000) { + read(_fd_adc, &buf_adc, sizeof(buf_adc)); + + if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) { + /* Voltage in volts */ + raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * _parameters.battery_voltage_scaling))); + + if ((buf_adc.am_data1 * _parameters.battery_voltage_scaling) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { + raw.battery_voltage_valid = false; + raw.battery_voltage_v = 0.f; + + } else { + raw.battery_voltage_valid = true; + } + + raw.battery_voltage_counter++; + } + _last_adc = hrt_absolute_time(); + } +} + +#if CONFIG_HRT_PPM +void +Sensors::ppm_poll() +{ + struct manual_control_setpoint_s manual_control; + + /* check to see whether a new frame has been decoded */ + if (_ppm_last_valid == ppm_last_valid_decode) + return; + /* require at least two chanels to consider the signal valid */ + if (ppm_decoded_channels < 2) + return; + + /* we are accepting this decode */ + _ppm_last_valid = ppm_last_valid_decode; + + /* Read out values from HRT */ + for (unsigned int i = 0; i < ppm_decoded_channels; i++) { + _rc.chan[i].raw = ppm_buffer[i]; + /* Set the range to +-, then scale up */ + _rc.chan[i].scale = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor * 10000; + _rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor; + } + + _rc.chan_count = ppm_decoded_channels; + _rc.timestamp = ppm_last_valid_decode; + + /* roll input */ + manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; + if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; + if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; + + /* pitch input */ + manual_control.pitch = _rc.chan[_rc.function[PITCH]].scaled; + if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; + if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; + + /* yaw input */ + manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled; + if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; + if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; + + /* throttle input */ + manual_control.throttle = (_rc.chan[_rc.function[THROTTLE]].scaled+1.0f)/2.0f; + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + + /* mode switch input */ + manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; + if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; + if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); +} +#endif + +void +Sensors::task_main_trampoline(int argc, char *argv[]) +{ + sensors::g_sensors->task_main(); +} + +void +Sensors::task_main() +{ + + /* inform about start */ + printf("[sensors] Initializing..\n"); + fflush(stdout); + + /* start individual sensors */ + accel_init(); + gyro_init(); + mag_init(); + baro_init(); + adc_init(); + + /* + * do subscriptions + */ + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vstatus_sub, 200); + + /* + * do advertisements + */ + struct sensor_combined_s raw; + raw.timestamp = hrt_absolute_time(); + raw.battery_voltage_v = BAT_VOL_INITIAL; + raw.adc_voltage_v[0] = 0.9f; + raw.adc_voltage_v[1] = 0.0f; + raw.adc_voltage_v[2] = 0.0f; + raw.battery_voltage_counter = 0; + raw.battery_voltage_valid = false; + + /* get a set of initial values */ + accel_poll(raw); + gyro_poll(raw); + mag_poll(raw); + baro_poll(raw); + + /* advertise the sensor_combined topic and make the initial publication */ + _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); + + /* advertise the manual_control topic */ + { + struct manual_control_setpoint_s manual_control; + manual_control.mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE; + manual_control.roll = 0.0f; + manual_control.pitch = 0.0f; + manual_control.yaw = 0.0f; + manual_control.throttle = 0.0f; + + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + } + + /* advertise the rc topic */ + { + struct rc_channels_s rc; + memset(&rc, 0, sizeof(rc)); + _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + } + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ + fds[0].fd = _gyro_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + + /* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */ + raw.timestamp = hrt_absolute_time(); + + /* copy most recent sensor data */ + accel_poll(raw); + gyro_poll(raw); + mag_poll(raw); + baro_poll(raw); + + /* check battery voltage */ + adc_poll(raw); + + /* Inform other processes that new data is available to copy */ + if (_publishing) + orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + +#ifdef CONFIG_HRT_PPM + /* Look for new r/c input data */ + ppm_poll(); +#endif + + perf_end(_loop_perf); + } + + printf("[sensors] exiting.\n"); + + _sensors_task = -1; + _exit(0); +} + +int +Sensors::start() +{ + ASSERT(_sensors_task == -1); + + /* start the task */ + _sensors_task = task_create("sensor_task", + SCHED_PRIORITY_MAX - 5, + 4096, + (main_t)&Sensors::task_main_trampoline, + nullptr); + + if (_sensors_task < 0) { + warn("task start failed"); + return -errno; + } + return OK; +} + +int sensors_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: sensors {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (sensors::g_sensors != nullptr) + errx(1, "sensors task already running"); + + sensors::g_sensors = new Sensors; + if (sensors::g_sensors == nullptr) + errx(1, "sensors task alloc failed"); + + if (OK != sensors::g_sensors->start()) + err(1, "sensors task start failed"); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (sensors::g_sensors == nullptr) + errx(1, "sensors task not running"); + delete sensors::g_sensors; + sensors::g_sensors = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (sensors::g_sensors) { + errx(0, "task is running"); + } else { + errx(1, "task is not running"); + } + } + + errx(1, "unrecognized command"); +} + diff --git a/apps/systemlib/err.c b/apps/systemlib/err.c index 9692f03dd9..f22c5ed0db 100644 --- a/apps/systemlib/err.c +++ b/apps/systemlib/err.c @@ -155,7 +155,7 @@ warn(const char *fmt, ...) void vwarn(const char *fmt, va_list args) { - warnerr_core(NOCODE, fmt, args); + warnerr_core(errno, fmt, args); } void diff --git a/apps/systemlib/param/param.h b/apps/systemlib/param/param.h index 643d0ef7b7..16ab7f5cd8 100644 --- a/apps/systemlib/param/param.h +++ b/apps/systemlib/param/param.h @@ -47,10 +47,13 @@ #include #include +#include /** Maximum size of the parameter backing file */ #define PARAM_FILE_MAXSIZE 4096 +__BEGIN_DECLS + /** * Parameter types. */ @@ -192,6 +195,10 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, * Note that these structures are not known by name; they are * collected into a section that is iterated by the parameter * code. + * + * Note that these macros cannot be used in C++ code due to + * their use of designated initializers. They should probably + * be refactored to avoid the use of a union for param_value_u. */ /** define an int32 parameter */ @@ -199,9 +206,9 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, static const \ __attribute__((used, section("__param"))) \ struct param_info_s __param__##_name = { \ - .name = #_name, \ - .type = PARAM_TYPE_INT32, \ - .val.i = _default \ + #_name, \ + PARAM_TYPE_INT32, \ + .val.i = _default \ } /** define a float parameter */ @@ -209,9 +216,9 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, static const \ __attribute__((used, section("__param"))) \ struct param_info_s __param__##_name = { \ - .name = #_name, \ - .type = PARAM_TYPE_FLOAT, \ - .val.f = _default \ + #_name, \ + PARAM_TYPE_FLOAT, \ + .val.f = _default \ } /** define a parameter that points to a structure */ @@ -219,9 +226,9 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, static const \ __attribute__((used, section("__param"))) \ struct param_info_s __param__##_name = { \ - .name = #_name, \ - .type = PARAM_TYPE_STRUCT + sizeof(_default), \ - .val.p = &_default; \ + #_name, \ + PARAM_TYPE_STRUCT + sizeof(_default), \ + .val.p = &_default; \ } /** @@ -245,4 +252,6 @@ struct param_info_s { union param_value_u val; }; +__END_DECLS + #endif diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index 28cc5d7c4b..e34376e82e 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -96,7 +96,7 @@ struct rc_channels_s { uint8_t chan_count; /**< maximum number of valid channels */ /*String array to store the names of the functions*/ - const char function_name[RC_CHANNELS_FUNCTION_MAX][20]; + char function_name[RC_CHANNELS_FUNCTION_MAX][20]; uint8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ }; /**< radio control channels. */ diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 5eb3f1f295..3f73bb76c3 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -543,7 +543,7 @@ CONFIG_DEV_CONSOLE=y CONFIG_DEV_LOWCONSOLE=n CONFIG_MUTEX_TYPES=n CONFIG_PRIORITY_INHERITANCE=y -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=8 CONFIG_SEM_NNESTPRIO=8 CONFIG_FDCLONE_DISABLE=n CONFIG_FDCLONE_STDIO=y