diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/sensors/sensors.cpp | 599 |
1 files changed, 261 insertions, 338 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9a90c84e0..6327d1c94 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -154,6 +154,8 @@ static const int ERROR = -1; */ extern "C" __EXPORT int sensors_main(int argc, char *argv[]); +static const int _n_multi_sensors = 3; + class Sensors { public: @@ -211,31 +213,29 @@ private: bool _hil_enabled; /**< if true, HIL is active */ bool _publishing; /**< if true, we are publishing sensor data */ - int _gyro_sub; /**< raw gyro0 data subscription */ - int _accel_sub; /**< raw accel0 data subscription */ - int _mag_sub; /**< raw mag0 data subscription */ - int _gyro1_sub; /**< raw gyro1 data subscription */ - int _accel1_sub; /**< raw accel1 data subscription */ - int _mag1_sub; /**< raw mag1 data subscription */ - int _gyro2_sub; /**< raw gyro2 data subscription */ - int _accel2_sub; /**< raw accel2 data subscription */ - int _mag2_sub; /**< raw mag2 data subscription */ + int _gyro_subs[_n_multi_sensors]; /**< raw gyro data subscriptions */ + int _accel_sub[_n_multi_sensors]; /**< raw accel data subscriptions */ + int _mag_subs[_n_multi_sensors]; /**< raw mag data subscriptions */ int _rc_sub; /**< raw rc channels data subscription */ - int _baro_sub; /**< raw baro data subscription */ - int _baro1_sub; /**< raw baro data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _diff_pres_sub; /**< raw differential pressure subscription */ + int _baro_subs[_n_multi_sensors]; /**< raw baro data subscriptions */ + int _diff_pres_subs[_n_multi_sensors]; /**< raw differential pressure subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _rc_parameter_map_sub; /**< rc parameter map subscription */ int _manual_control_sub; /**< notification of manual control updates */ + unsigned _n_gyro; + unsigned _n_accel; + unsigned _n_mag; + unsigned _n_baro; + unsigned _n_diff; + orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ - orb_advert_t _airspeed_pub; /**< airspeed */ + orb_advert_t _airspeed_pubs[i]; /**< airspeed */ orb_advert_t _diff_pres_pub; /**< differential_pressure */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -400,6 +400,11 @@ private: int baro_init(); /** + * Do airspeed-related initialisation. + */ + int diff_init(); + + /** * Do adc-related initialisation. */ int adc_init(); @@ -494,23 +499,22 @@ Sensors::Sensors() : _publishing(true), /* subscriptions */ - _gyro_sub(-1), - _accel_sub(-1), - _mag_sub(-1), - _gyro1_sub(-1), - _accel1_sub(-1), - _mag1_sub(-1), - _gyro2_sub(-1), - _accel2_sub(-1), - _mag2_sub(-1), + _gyro_subs{}, + _accel_subs{}, + _mag_subs{}, _rc_sub(-1), - _baro_sub(-1), - _baro1_sub(-1), + _baro_subs{}, _vcontrol_mode_sub(-1), _params_sub(-1), _rc_parameter_map_sub(-1), _manual_control_sub(-1), + _n_gyro(0), + _n_accel(0), + _n_mag(0), + _n_baro(0), + _n_diff(0), + /* publications */ _sensor_pub(-1), _manual_control_pub(-1), @@ -871,22 +875,32 @@ int Sensors::accel_init() { int fd; + char str[32]; - fd = open(ACCEL0_DEVICE_PATH, 0); + for (unsigned i = 0; i < _n_multi_sensors; i++) { + sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i); + fd = open(str, 0); - if (fd < 0) { - warnx("FATAL: no accelerometer found: %s", ACCEL0_DEVICE_PATH); - return ERROR; + if (fd < 0) { + if (i == 0) { + warnx("FATAL: no accelerometer found: %s", str); + return ERROR; + } else { + continue; + } - } else { + } else { - /* set the accel internal sampling rate to default rate */ - ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); + /* set the accel internal sampling rate to default rate */ + ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); - /* set the driver to poll at default rate */ - ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + /* set the driver to poll at default rate */ + if (OK == ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + _n_accel++; + } - close(fd); + close(fd); + } } return OK; @@ -896,21 +910,32 @@ int Sensors::gyro_init() { int fd; + char str[32]; - fd = open(GYRO0_DEVICE_PATH, 0); + for (unsigned i = 0; i < _n_multi_sensors; i++) { + sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, i); + fd = open(str, 0); - if (fd < 0) { - warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH); - return ERROR; + if (fd < 0) { + if (i == 0) { + warnx("FATAL: no gyro found: %s", str); + return ERROR; + } else { + continue; + } - } else { + } else { - /* set the gyro internal sampling rate to default rate */ - ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); + /* set the gyro internal sampling rate to default rate */ + ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); - /* set the driver to poll at default rate */ - ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + /* set the driver to poll at default rate */ + if (OK == ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + _n_gyro++; + } + close(fd); + } } return OK; @@ -921,38 +946,48 @@ Sensors::mag_init() { int fd; int ret; + char str[32]; - fd = open(MAG0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: no magnetometer found: %s", MAG0_DEVICE_PATH); - return ERROR; - } - - /* try different mag sampling rates */ + for (unsigned i = 0; i < _n_multi_sensors; i++) { + sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, i); + fd = open(str, 0); + if (fd < 0) { + if (i == 0) { + warnx("FATAL: no magnetometer found: %s", str); + return ERROR; + } else { + continue; + } + } - ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + /* try different mag sampling rates */ - if (ret == OK) { - /* set the pollrate accordingly */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); - } else { - ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); - /* if the slower sampling rate still fails, something is wrong */ if (ret == OK) { - /* set the driver to poll also at the slower rate */ - ioctl(fd, SENSORIOCSPOLLRATE, 100); + /* set the pollrate accordingly */ + ioctl(fd, SENSORIOCSPOLLRATE, 150); } else { - warnx("FATAL: mag sampling rate could not be set"); - return ERROR; + ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + + /* if the slower sampling rate still fails, something is wrong */ + if (ret == OK) { + /* set the driver to poll also at the slower rate */ + ioctl(fd, SENSORIOCSPOLLRATE, 100); + + } else { + warnx("FATAL: mag %u sampling rate could not be set", i); + return ERROR; + } } - } - close(fd); + _n_mag++; + + close(fd); + } return OK; } @@ -961,23 +996,39 @@ int Sensors::baro_init() { int fd; + char str[32]; - fd = open(BARO0_DEVICE_PATH, 0); + for (unsigned i = 0; i < _n_multi_sensors; i++) { + sprintf(str, "%s%u", BARO_BASE_DEVICE_PATH, i); + fd = open(str, 0); - if (fd < 0) { - warnx("FATAL: No barometer found: %s", BARO0_DEVICE_PATH); - return ERROR; - } + if (fd < 0) { + if (i == 0) { + warnx("FATAL: No barometer found: %s", str); + return ERROR; + } else { + continue; + } + } - /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); + /* set the driver to poll at 150Hz */ + if (OK == ioctl(fd, SENSORIOCSPOLLRATE, 150)) { + _n_baro++; + } - close(fd); + close(fd); + } return OK; } int +Sensors::diff_init() +{ + _n_diff = 1; +} + +int Sensors::adc_init() { @@ -994,284 +1045,151 @@ Sensors::adc_init() void Sensors::accel_poll(struct sensor_combined_s &raw) { - bool accel_updated; - orb_check(_accel_sub, &accel_updated); - - if (accel_updated) { - struct accel_report accel_report; - - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; - - raw.accelerometer_m_s2[0] = vect(0); - raw.accelerometer_m_s2[1] = vect(1); - raw.accelerometer_m_s2[2] = vect(2); - - 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_timestamp = accel_report.timestamp; - raw.accelerometer_errcount = accel_report.error_count; - } - - orb_check(_accel1_sub, &accel_updated); - - if (accel_updated) { - struct accel_report accel_report; - - orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report); - - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; - - raw.accelerometer1_m_s2[0] = vect(0); - raw.accelerometer1_m_s2[1] = vect(1); - raw.accelerometer1_m_s2[2] = vect(2); - - raw.accelerometer1_raw[0] = accel_report.x_raw; - raw.accelerometer1_raw[1] = accel_report.y_raw; - raw.accelerometer1_raw[2] = accel_report.z_raw; - - raw.accelerometer1_timestamp = accel_report.timestamp; - raw.accelerometer1_errcount = accel_report.error_count; - } - - orb_check(_accel2_sub, &accel_updated); + for (unsigned i = 0; i < _n_accel; i++) { + bool accel_updated; + orb_check(_accel_subs[i], &accel_updated); - if (accel_updated) { - struct accel_report accel_report; + if (accel_updated) { + struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel_subs[i], &accel_report); - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; - raw.accelerometer2_m_s2[0] = vect(0); - raw.accelerometer2_m_s2[1] = vect(1); - raw.accelerometer2_m_s2[2] = vect(2); + raw.accelerometer_m_s2[i][0] = vect(0); + raw.accelerometer_m_s2[i][1] = vect(1); + raw.accelerometer_m_s2[i][2] = vect(2); - raw.accelerometer2_raw[0] = accel_report.x_raw; - raw.accelerometer2_raw[1] = accel_report.y_raw; - raw.accelerometer2_raw[2] = accel_report.z_raw; + raw.accelerometer_raw[i][0] = accel_report.x_raw; + raw.accelerometer_raw[i][1] = accel_report.y_raw; + raw.accelerometer_raw[i][2] = accel_report.z_raw; - raw.accelerometer2_timestamp = accel_report.timestamp; - raw.accelerometer2_errcount = accel_report.error_count; + raw.accelerometer_timestamp[i] = accel_report.timestamp; + raw.accelerometer_errcount[i] = accel_report.error_count; + } } } void Sensors::gyro_poll(struct sensor_combined_s &raw) { - bool gyro_updated; - orb_check(_gyro_sub, &gyro_updated); - - if (gyro_updated) { - struct gyro_report gyro_report; - - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro_rad_s[0] = vect(0); - raw.gyro_rad_s[1] = vect(1); - raw.gyro_rad_s[2] = vect(2); - - 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.timestamp = gyro_report.timestamp; - raw.gyro_errcount = gyro_report.error_count; - } - - orb_check(_gyro1_sub, &gyro_updated); + for (unsigned i = 0; i < _n_gyro; i++) { + bool gyro_updated; + orb_check(_gyro_subs[i], &gyro_updated); - if (gyro_updated) { - struct gyro_report gyro_report; + if (gyro_updated) { + struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro_subs[i], &gyro_report); - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; - raw.gyro1_rad_s[0] = vect(0); - raw.gyro1_rad_s[1] = vect(1); - raw.gyro1_rad_s[2] = vect(2); + raw.gyro_rad_s[i][0] = vect(0); + raw.gyro_rad_s[i][1] = vect(1); + raw.gyro_rad_s[i][2] = vect(2); - raw.gyro1_raw[0] = gyro_report.x_raw; - raw.gyro1_raw[1] = gyro_report.y_raw; - raw.gyro1_raw[2] = gyro_report.z_raw; + raw.gyro_raw[i][0] = gyro_report.x_raw; + raw.gyro_raw[i][1] = gyro_report.y_raw; + raw.gyro_raw[i][2] = gyro_report.z_raw; - raw.gyro1_timestamp = gyro_report.timestamp; - raw.gyro1_errcount = gyro_report.error_count; - } - - orb_check(_gyro2_sub, &gyro_updated); - - if (gyro_updated) { - struct gyro_report gyro_report; - - orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report); - - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro2_rad_s[0] = vect(0); - raw.gyro2_rad_s[1] = vect(1); - raw.gyro2_rad_s[2] = vect(2); - - raw.gyro2_raw[0] = gyro_report.x_raw; - raw.gyro2_raw[1] = gyro_report.y_raw; - raw.gyro2_raw[2] = gyro_report.z_raw; - - raw.gyro2_timestamp = gyro_report.timestamp; - raw.gyro2_errcount = gyro_report.error_count; + /* pick the most recent timestamp out of the set */ + if (gyro_report.timestamp > raw.timestamp) { + raw.timestamp = gyro_report.timestamp; + } + raw.gyro_timestamp[i] = gyro_report.timestamp; + raw.gyro_errcount[i] = gyro_report.error_count; + } } } void Sensors::mag_poll(struct sensor_combined_s &raw) { - bool mag_updated; - orb_check(_mag_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; - - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - - vect = _mag_rotation[0] * vect; - - raw.magnetometer_ga[0] = vect(0); - raw.magnetometer_ga[1] = vect(1); - raw.magnetometer_ga[2] = vect(2); - - 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_timestamp = mag_report.timestamp; - raw.magnetometer_errcount = mag_report.error_count; - } - - orb_check(_mag1_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; - - orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); - - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - - vect = _mag_rotation[1] * vect; - - raw.magnetometer1_ga[0] = vect(0); - raw.magnetometer1_ga[1] = vect(1); - raw.magnetometer1_ga[2] = vect(2); - - raw.magnetometer1_raw[0] = mag_report.x_raw; - raw.magnetometer1_raw[1] = mag_report.y_raw; - raw.magnetometer1_raw[2] = mag_report.z_raw; - - raw.magnetometer1_timestamp = mag_report.timestamp; - raw.magnetometer1_errcount = mag_report.error_count; - } + for (unsigned i = 0; i < _n_mag; i++) { + bool mag_updated; + orb_check(mag_subs[i], &mag_updated); - orb_check(_mag2_sub, &mag_updated); + if (mag_updated) { + struct mag_report mag_report; - if (mag_updated) { - struct mag_report mag_report; + orb_copy(ORB_ID(sensor_mag), mag_subs[i], &mag_report); - orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + vect = _mag_rotation[0] * vect; - vect = _mag_rotation[2] * vect; + raw.magnetometer_ga[i][0] = vect(0); + raw.magnetometer_ga[i][1] = vect(1); + raw.magnetometer_ga[i][2] = vect(2); - raw.magnetometer2_ga[0] = vect(0); - raw.magnetometer2_ga[1] = vect(1); - raw.magnetometer2_ga[2] = vect(2); + raw.magnetometer_raw[i][0] = mag_report.x_raw; + raw.magnetometer_raw[i][1] = mag_report.y_raw; + raw.magnetometer_raw[i][2] = mag_report.z_raw; - raw.magnetometer2_raw[0] = mag_report.x_raw; - raw.magnetometer2_raw[1] = mag_report.y_raw; - raw.magnetometer2_raw[2] = mag_report.z_raw; - - raw.magnetometer2_timestamp = mag_report.timestamp; - raw.magnetometer2_errcount = mag_report.error_count; + raw.magnetometer_timestamp[i] = mag_report.timestamp; + raw.magnetometer_errcount[i] = mag_report.error_count; + } } } void Sensors::baro_poll(struct sensor_combined_s &raw) { - bool baro_updated; - orb_check(_baro_sub, &baro_updated); - - if (baro_updated) { - - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); - - raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar - raw.baro_alt_meter = _barometer.altitude; // Altitude in meters - raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius - - raw.baro_timestamp = _barometer.timestamp; - } - - orb_check(_baro1_sub, &baro_updated); - - if (baro_updated) { + for (unsigned i = 0; i < _n_baro; i++) { + bool baro_updated; + orb_check(baro_sub[i], &baro_updated); - struct baro_report baro_report; + if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report); + orb_copy(ORB_ID(sensor_baro), baro_sub[i], &_barometer); - raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar - raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters - raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius + raw.baro_pres_mbar[i] = _barometer.pressure; // Pressure in mbar + raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters + raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius - raw.baro1_timestamp = baro_report.timestamp; + raw.baro_timestamp[i] = _barometer.timestamp; + } } } void Sensors::diff_pres_poll(struct sensor_combined_s &raw) { - bool updated; - orb_check(_diff_pres_sub, &updated); + for (unsigned i = 0; i < _n_diff; i++) { + bool updated; + orb_check(_diff_pres_subs[i], &updated); - if (updated) { - orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); + if (updated) { - raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa; - raw.differential_pressure_timestamp = _diff_pres.timestamp; - raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; + struct differential_pressure_s diff_pres; + orb_copy(ORB_ID(differential_pressure), _diff_pres_subs[i], &diff_pres); - float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : - (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + raw.differential_pressure_pa[i] = diff_pres.differential_pressure_raw_pa; + raw.differential_pressure_timestamp[i] = diff_pres.timestamp; + raw.differential_pressure_filtered_pa[i] = diff_pres.differential_pressure_filtered_pa; - _airspeed.timestamp = _diff_pres.timestamp; + float air_temperature_celsius = (diff_pres.temperature > -300.0f) ? diff_pres.temperature : + (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); - /* don't risk to feed negative airspeed into the system */ - _airspeed.indicated_airspeed_m_s = math::max(0.0f, - calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); - _airspeed.true_airspeed_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); - _airspeed.air_temperature_celsius = air_temperature_celsius; + _airspeed[i].timestamp = diff_pres.timestamp[i]; - /* announce the airspeed if needed, just publish else */ - if (_airspeed_pub > 0) { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed); + /* don't risk to feed negative airspeed into the system */ + _airspeed[i].indicated_airspeed_m_s = math::max(0.0f, + calc_indicated_airspeed(diff_pres.differential_pressure_filtered_pa)); + _airspeed[i].true_airspeed_m_s = math::max(0.0f, + calc_true_airspeed(diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f, + raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); + _airspeed[i].air_temperature_celsius = air_temperature_celsius; - } else { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed); + /* announce the airspeed if needed, just publish else */ + if (_airspeed_pubs[i] > 0) { + orb_publish(ORB_ID(airspeed), _airspeed_pubs[i], &_airspeed); + + } else { + _airspeed_pubs[i] = orb_advertise(ORB_ID(airspeed), &_airspeed); + } } } } @@ -1720,18 +1638,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw) float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; - _diff_pres.timestamp = t; - _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; - _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + + diff_pres.timestamp = t; + diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; + diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f); - _diff_pres.temperature = -1000.0f; + diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ if (_diff_pres_pub > 0) { orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres); } else { - _diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres); + int dummy; + _diff_pres_pub = orb_advertise_multi(ORB_ID(differential_pressure), &dummy, &_diff_pres); } } } @@ -1745,7 +1664,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + int dummy; + _battery_pub = orb_advertise_multi(ORB_ID(battery_status), &dummy, &_battery_status); } } } @@ -2054,28 +1974,48 @@ Sensors::task_main() goto exit_immediate; } + /* differential pressure sensors: support only one for now */ + _n_diff = 1; + ret = adc_init(); if (ret) { goto exit_immediate; } + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + /* * do subscriptions */ - _gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); - _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); - _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); - _gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); - _accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); - _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); - _gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2); - _accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2); - _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); + for (unsigned i = 0; i < _n_gyro; i++) { + _gyro_subs[i] = orb_subscribe_multi(ORB_ID(sensor_gyro), i); + /* set high initial error counts to deselect gyros */ + raw.gyro_errcount[i] = 100000; + } + + for (unsigned i = 0; i < _n_accel; i++) { + _accel_subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); + /* set high initial error counts to deselect accels */ + raw.accelerometer_errcount[i] = 100000; + } + + for (unsigned i = 0; i < _n_mag; i++) { + _mag_subs[i] = orb_subscribe_multi(ORB_ID(sensor_mag), i); + /* set high initial error counts to deselect mags */ + raw.magnetometer_errcount[i] = 100000; + } + + for (unsigned i = 0; i < _n_baro; i++) { + _baro_subs[i] = orb_subscribe_multi(ORB_ID(sensor_baro), i); + } + + for (unsigned i = 0; i < _n_diff; i++) { + _diff_pres_subs[i] = orb_subscribe(ORB_ID(differential_pressure), i); + } + _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); - _baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1); - _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); @@ -2085,34 +2025,17 @@ Sensors::task_main() orb_set_interval(_vcontrol_mode_sub, 200); /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ - orb_set_interval(_gyro_sub, 4); + orb_set_interval(_gyro_subs[0], 4); /* * do advertisements */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); raw.adc_voltage_v[0] = 0.0f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; raw.adc_voltage_v[3] = 0.0f; - /* set high initial error counts to deselect gyros */ - raw.gyro_errcount = 100000; - raw.gyro1_errcount = 100000; - raw.gyro2_errcount = 100000; - - /* set high initial error counts to deselect accels */ - raw.accelerometer_errcount = 100000; - raw.accelerometer1_errcount = 100000; - raw.accelerometer2_errcount = 100000; - - /* set high initial error counts to deselect mags */ - raw.magnetometer_errcount = 100000; - raw.magnetometer1_errcount = 100000; - raw.magnetometer2_errcount = 100000; - memset(&_battery_status, 0, sizeof(_battery_status)); _battery_status.voltage_v = -1.0f; _battery_status.voltage_filtered_v = -1.0f; @@ -2161,19 +2084,19 @@ Sensors::task_main() /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ - gyro_poll(raw); accel_poll(raw); + gyro_poll(raw); mag_poll(raw); baro_poll(raw); /* work out if main gyro timed out and fail over to alternate gyro */ - if (hrt_elapsed_time(&raw.timestamp) > 20 * 1000) { + if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000) { /* if the secondary failed as well, go to the tertiary */ - if (hrt_elapsed_time(&raw.gyro1_timestamp) > 20 * 1000) { - fds[0].fd = _gyro2_sub; + if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) { + fds[0].fd = _gyro_subs[2]; } else { - fds[0].fd = _gyro1_sub; + fds[0].fd = _gyro_subs[1]; } } |