mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-14 02:00:35 +08:00
Sensors app: remove redundant code
This commit is contained in:
Lorenz Meier
+139
-339
@@ -201,19 +201,16 @@ 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_sub[ORB_MULTI_MAX_INSTANCES - 1]; /**< raw gyro0 data subscription */
|
||||
int _accel_sub[ORB_MULTI_MAX_INSTANCES - 1]; /**< raw accel0 data subscription */
|
||||
int _mag_sub[ORB_MULTI_MAX_INSTANCES - 1]; /**< raw mag0 data subscription */
|
||||
int _baro_sub[ORB_MULTI_MAX_INSTANCES - 1]; /**< raw baro0 data subscription */
|
||||
unsigned _gyro_count; /**< raw gyro0 data subscription */
|
||||
unsigned _accel_count; /**< raw accel0 data subscription */
|
||||
unsigned _mag_count; /**< raw mag0 data subscription */
|
||||
unsigned _baro_count; /**< raw baro0 data subscription */
|
||||
|
||||
int _rc_sub; /**< raw rc channels data subscription */
|
||||
int _baro_sub; /**< raw baro0 data subscription */
|
||||
int _baro1_sub; /**< raw baro1 data subscription */
|
||||
//int _airspeed_sub; /**< airspeed subscription */
|
||||
int _diff_pres_sub; /**< raw differential pressure subscription */
|
||||
int _vcontrol_mode_sub; /**< vehicle control mode subscription */
|
||||
int _params_sub; /**< notification of parameter updates */
|
||||
@@ -228,18 +225,6 @@ private:
|
||||
orb_advert_t _airspeed_pub; /**< airspeed */
|
||||
orb_advert_t _diff_pres_pub; /**< differential_pressure */
|
||||
|
||||
int32_t _gyro_prio; /**< gyro0 sensor priority */
|
||||
int32_t _accel_prio; /**< accel0 sensor priority */
|
||||
int32_t _mag_prio; /**< mag0 sensor priority */
|
||||
int32_t _gyro1_prio; /**< gyro1 sensor priority */
|
||||
int32_t _accel1_prio; /**<accel1 sensor priority */
|
||||
int32_t _mag1_prio; /**< mag1 sensor priority */
|
||||
int32_t _gyro2_prio; /**< gyro2 sensor priority */
|
||||
int32_t _accel2_prio; /**< accel2 sensor priority */
|
||||
int32_t _mag2_prio; /**< mag2 sensor priority */
|
||||
int32_t _baro_prio; /**< baro0 sensor priority */
|
||||
int32_t _baro1_prio; /**< baro1 sensor priority */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
struct rc_channels_s _rc; /**< r/c channel data */
|
||||
@@ -377,6 +362,9 @@ private:
|
||||
} _parameter_handles; /**< handles for interesting parameters */
|
||||
|
||||
|
||||
void init_sensor_class(const struct orb_metadata *meta, int *subs,
|
||||
unsigned *priorities, unsigned *errcount);
|
||||
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
*/
|
||||
@@ -497,18 +485,15 @@ 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_sub{-1, -1, -1},
|
||||
_accel_sub{-1, -1, -1},
|
||||
_mag_sub{-1, -1, -1},
|
||||
_baro_sub{-1, -1, -1},
|
||||
_gyro_count(0),
|
||||
_accel_count(0),
|
||||
_mag_count(0),
|
||||
_baro_count(0),
|
||||
_rc_sub(-1),
|
||||
_baro_sub(-1),
|
||||
_baro1_sub(-1),
|
||||
_vcontrol_mode_sub(-1),
|
||||
_params_sub(-1),
|
||||
_rc_parameter_map_sub(-1),
|
||||
@@ -523,19 +508,6 @@ Sensors::Sensors() :
|
||||
_airspeed_pub(nullptr),
|
||||
_diff_pres_pub(nullptr),
|
||||
|
||||
/* sensor priorities */
|
||||
_gyro_prio(-1),
|
||||
_accel_prio(-1),
|
||||
_mag_prio(-1),
|
||||
_gyro1_prio(-1),
|
||||
_accel1_prio(-1),
|
||||
_mag1_prio(-1),
|
||||
_gyro2_prio(-1),
|
||||
_accel2_prio(-1),
|
||||
_mag2_prio(-1),
|
||||
_baro_prio(-1),
|
||||
_baro1_prio(-1),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
|
||||
|
||||
@@ -1032,287 +1004,132 @@ Sensors::adc_init()
|
||||
void
|
||||
Sensors::accel_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
bool accel_updated;
|
||||
orb_check(_accel_sub, &accel_updated);
|
||||
for (unsigned i = 0; i < _accel_count; i++) {
|
||||
bool accel_updated;
|
||||
orb_check(_accel_sub[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), _accel_sub, &accel_report);
|
||||
orb_copy(ORB_ID(sensor_accel), _accel_sub[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.accelerometer_m_s2[0] = vect(0);
|
||||
raw.accelerometer_m_s2[1] = vect(1);
|
||||
raw.accelerometer_m_s2[2] = vect(2);
|
||||
raw.accelerometer_m_s2[i * 3 + 0] = vect(0);
|
||||
raw.accelerometer_m_s2[i * 3 + 1] = vect(1);
|
||||
raw.accelerometer_m_s2[i * 3 + 2] = vect(2);
|
||||
|
||||
math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
|
||||
vect_int = _board_rotation * vect_int;
|
||||
math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
|
||||
vect_int = _board_rotation * vect_int;
|
||||
|
||||
raw.accelerometer_integral_m_s[0] = vect_int(0);
|
||||
raw.accelerometer_integral_m_s[1] = vect_int(1);
|
||||
raw.accelerometer_integral_m_s[2] = vect_int(2);
|
||||
raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0);
|
||||
raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1);
|
||||
raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2);
|
||||
|
||||
raw.accelerometer_integral_dt = accel_report.integral_dt;
|
||||
raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
|
||||
|
||||
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[i * 3 + 0] = accel_report.x_raw;
|
||||
raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw;
|
||||
raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw;
|
||||
|
||||
raw.accelerometer_timestamp = accel_report.timestamp;
|
||||
raw.accelerometer_priority = _accel_prio;
|
||||
raw.accelerometer_errcount = accel_report.error_count;
|
||||
raw.accelerometer_temp = accel_report.temperature;
|
||||
}
|
||||
|
||||
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_priority = _accel1_prio;
|
||||
raw.accelerometer1_errcount = accel_report.error_count;
|
||||
raw.accelerometer1_temp = accel_report.temperature;
|
||||
}
|
||||
|
||||
orb_check(_accel2_sub, &accel_updated);
|
||||
|
||||
if (accel_updated) {
|
||||
struct accel_report accel_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report);
|
||||
|
||||
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.accelerometer2_raw[0] = accel_report.x_raw;
|
||||
raw.accelerometer2_raw[1] = accel_report.y_raw;
|
||||
raw.accelerometer2_raw[2] = accel_report.z_raw;
|
||||
|
||||
raw.accelerometer2_timestamp = accel_report.timestamp;
|
||||
raw.accelerometer2_priority = _accel2_prio;
|
||||
raw.accelerometer2_errcount = accel_report.error_count;
|
||||
raw.accelerometer2_temp = accel_report.temperature;
|
||||
raw.accelerometer_timestamp[i] = accel_report.timestamp;
|
||||
raw.accelerometer_errcount[i] = accel_report.error_count;
|
||||
raw.accelerometer_temp[i] = accel_report.temperature;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::gyro_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
bool gyro_updated;
|
||||
orb_check(_gyro_sub, &gyro_updated);
|
||||
for (unsigned i = 0; i < _gyro_count; i++) {
|
||||
bool gyro_updated;
|
||||
orb_check(_gyro_sub[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), _gyro_sub, &gyro_report);
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro_sub[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.gyro_rad_s[0] = vect(0);
|
||||
raw.gyro_rad_s[1] = vect(1);
|
||||
raw.gyro_rad_s[2] = vect(2);
|
||||
raw.gyro_rad_s[i * 3 + 0] = vect(0);
|
||||
raw.gyro_rad_s[i * 3 + 1] = vect(1);
|
||||
raw.gyro_rad_s[i * 3 + 2] = vect(2);
|
||||
|
||||
math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
|
||||
vect_int = _board_rotation * vect_int;
|
||||
math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
|
||||
vect_int = _board_rotation * vect_int;
|
||||
|
||||
raw.gyro_integral_rad[0] = vect_int(0);
|
||||
raw.gyro_integral_rad[1] = vect_int(1);
|
||||
raw.gyro_integral_rad[2] = vect_int(2);
|
||||
raw.gyro_integral_rad[i * 3 + 0] = vect_int(0);
|
||||
raw.gyro_integral_rad[i * 3 + 1] = vect_int(1);
|
||||
raw.gyro_integral_rad[i * 3 + 2] = vect_int(2);
|
||||
|
||||
raw.gyro_integral_dt = gyro_report.integral_dt;
|
||||
raw.gyro_integral_dt[i] = gyro_report.integral_dt;
|
||||
|
||||
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[i * 3 + 0] = gyro_report.x_raw;
|
||||
raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw;
|
||||
raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw;
|
||||
|
||||
raw.timestamp = gyro_report.timestamp;
|
||||
raw.gyro_priority = _gyro_prio;
|
||||
raw.gyro_errcount = gyro_report.error_count;
|
||||
raw.gyro_temp = gyro_report.temperature;
|
||||
}
|
||||
|
||||
orb_check(_gyro1_sub, &gyro_updated);
|
||||
|
||||
if (gyro_updated) {
|
||||
struct gyro_report gyro_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report);
|
||||
|
||||
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.gyro1_raw[0] = gyro_report.x_raw;
|
||||
raw.gyro1_raw[1] = gyro_report.y_raw;
|
||||
raw.gyro1_raw[2] = gyro_report.z_raw;
|
||||
|
||||
raw.gyro1_timestamp = gyro_report.timestamp;
|
||||
raw.gyro1_priority = _gyro1_prio;
|
||||
raw.gyro1_errcount = gyro_report.error_count;
|
||||
raw.gyro1_temp = gyro_report.temperature;
|
||||
}
|
||||
|
||||
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_priority = _gyro2_prio;
|
||||
raw.gyro2_errcount = gyro_report.error_count;
|
||||
raw.gyro2_temp = gyro_report.temperature;
|
||||
raw.gyro_timestamp[i] = gyro_report.timestamp;
|
||||
if (i == 0) {
|
||||
raw.timestamp = gyro_report.timestamp;
|
||||
}
|
||||
raw.gyro_errcount[i] = gyro_report.error_count;
|
||||
raw.gyro_temp[i] = gyro_report.temperature;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::mag_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
bool mag_updated;
|
||||
orb_check(_mag_sub, &mag_updated);
|
||||
for (unsigned i = 0; i < _mag_count; i++) {
|
||||
bool mag_updated;
|
||||
orb_check(_mag_sub[i], &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_sub, &mag_report);
|
||||
orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &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[i] * vect;
|
||||
|
||||
raw.magnetometer_ga[0] = vect(0);
|
||||
raw.magnetometer_ga[1] = vect(1);
|
||||
raw.magnetometer_ga[2] = vect(2);
|
||||
raw.magnetometer_ga[i * 3 + 0] = vect(0);
|
||||
raw.magnetometer_ga[i * 3 + 1] = vect(1);
|
||||
raw.magnetometer_ga[i * 3 + 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_raw[i * 3 + 0] = mag_report.x_raw;
|
||||
raw.magnetometer_raw[i * 3 + 1] = mag_report.y_raw;
|
||||
raw.magnetometer_raw[i * 3 + 2] = mag_report.z_raw;
|
||||
|
||||
raw.magnetometer_timestamp = mag_report.timestamp;
|
||||
raw.magnetometer_priority = _mag_prio;
|
||||
raw.magnetometer_errcount = mag_report.error_count;
|
||||
raw.magnetometer_temp = mag_report.temperature;
|
||||
}
|
||||
|
||||
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_priority = _mag1_prio;
|
||||
raw.magnetometer1_errcount = mag_report.error_count;
|
||||
raw.magnetometer1_temp = mag_report.temperature;
|
||||
}
|
||||
|
||||
orb_check(_mag2_sub, &mag_updated);
|
||||
|
||||
if (mag_updated) {
|
||||
struct mag_report 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);
|
||||
|
||||
vect = _mag_rotation[2] * vect;
|
||||
|
||||
raw.magnetometer2_ga[0] = vect(0);
|
||||
raw.magnetometer2_ga[1] = vect(1);
|
||||
raw.magnetometer2_ga[2] = vect(2);
|
||||
|
||||
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_priority = _mag2_prio;
|
||||
raw.magnetometer2_errcount = mag_report.error_count;
|
||||
raw.magnetometer2_temp = mag_report.temperature;
|
||||
raw.magnetometer_timestamp[i] = mag_report.timestamp;
|
||||
raw.magnetometer_errcount[i] = mag_report.error_count;
|
||||
raw.magnetometer_temp[i] = mag_report.temperature;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::baro_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
bool baro_updated;
|
||||
orb_check(_baro_sub, &baro_updated);
|
||||
for (unsigned i = 0; i < _baro_count; i++) {
|
||||
bool baro_updated;
|
||||
orb_check(_baro_sub[i], &baro_updated);
|
||||
|
||||
if (baro_updated) {
|
||||
if (baro_updated) {
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
|
||||
orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_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_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.baro_timestamp = _barometer.timestamp;
|
||||
raw.baro_priority = _baro_prio;
|
||||
}
|
||||
|
||||
orb_check(_baro1_sub, &baro_updated);
|
||||
|
||||
if (baro_updated) {
|
||||
|
||||
struct baro_report baro_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report);
|
||||
|
||||
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.baro1_timestamp = baro_report.timestamp;
|
||||
raw.baro1_priority = _baro1_prio;
|
||||
raw.baro_timestamp[i] = _barometer.timestamp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1325,12 +1142,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
|
||||
|
||||
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;
|
||||
raw.differential_pressure_pa[0] = _diff_pres.differential_pressure_raw_pa;
|
||||
raw.differential_pressure_timestamp[0] = _diff_pres.timestamp;
|
||||
raw.differential_pressure_filtered_pa[0] = _diff_pres.differential_pressure_filtered_pa;
|
||||
|
||||
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
|
||||
(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
|
||||
(raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG);
|
||||
|
||||
_airspeed.timestamp = _diff_pres.timestamp;
|
||||
|
||||
@@ -1338,11 +1155,11 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
|
||||
_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));
|
||||
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.true_airspeed_unfiltered_m_s = math::max(0.0f,
|
||||
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar * 1e2f,
|
||||
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
|
||||
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f,
|
||||
raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
|
||||
|
||||
_airspeed.air_temperature_celsius = air_temperature_celsius;
|
||||
|
||||
@@ -2117,6 +1934,17 @@ Sensors::task_main_trampoline(int argc, char *argv[])
|
||||
sensors::g_sensors->task_main();
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
|
||||
unsigned *priorities, unsigned *errcount)
|
||||
{
|
||||
for (unsigned i = 0; i < (unsigned)orb_group_count(meta); i++) {
|
||||
subs[i] = orb_subscribe_multi(meta, i);
|
||||
orb_priority(subs[i], (int32_t*)&priorities[i]);
|
||||
errcount[i] = 100000;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::task_main()
|
||||
{
|
||||
@@ -2147,71 +1975,43 @@ Sensors::task_main()
|
||||
return;
|
||||
}
|
||||
|
||||
struct sensor_combined_s 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);
|
||||
|
||||
init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
|
||||
&raw.gyro_priority[0], &raw.gyro_errcount[0]);
|
||||
|
||||
init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
|
||||
&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
|
||||
|
||||
init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
|
||||
&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
|
||||
|
||||
init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
|
||||
&raw.baro_priority[0], &raw.baro_errcount[0]);
|
||||
|
||||
_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));
|
||||
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
|
||||
/*
|
||||
* get sensor priorities
|
||||
*/
|
||||
orb_priority(_gyro_sub, &_gyro_prio);
|
||||
orb_priority(_accel_sub, &_accel_prio);
|
||||
orb_priority(_mag_sub, &_mag_prio);
|
||||
orb_priority(_gyro1_sub, &_gyro1_prio);
|
||||
orb_priority(_accel1_sub, &_accel1_prio);
|
||||
orb_priority(_mag1_sub, &_mag1_prio);
|
||||
orb_priority(_gyro2_sub, &_gyro2_prio);
|
||||
orb_priority(_accel2_sub, &_accel2_prio);
|
||||
orb_priority(_mag2_sub, &_mag2_prio);
|
||||
orb_priority(_baro_sub, &_baro_prio);
|
||||
orb_priority(_baro1_sub, &_baro1_prio);
|
||||
|
||||
/* rate limit vehicle status updates to 5Hz */
|
||||
orb_set_interval(_vcontrol_mode_sub, 200);
|
||||
|
||||
/*
|
||||
* 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;
|
||||
@@ -2234,8 +2034,8 @@ Sensors::task_main()
|
||||
/* wakeup source(s) */
|
||||
px4_pollfd_struct_t fds[1];
|
||||
|
||||
/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
|
||||
fds[0].fd = _gyro_sub;
|
||||
/* use the gyro to pace output */
|
||||
fds[0].fd = _gyro_sub[0];
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
_task_should_exit = false;
|
||||
@@ -2268,13 +2068,13 @@ Sensors::task_main()
|
||||
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_sub[2];
|
||||
} else {
|
||||
fds[0].fd = _gyro1_sub;
|
||||
fds[0].fd = _gyro_sub[1];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user