mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 14:24:21 +08:00
sensors : add sensor priority order
This commit is contained in:
Mohammed Kabir
committed by
Lorenz Meier
Lorenz Meier
parent
41b3452e22
commit
10360a93b7
@@ -4,6 +4,14 @@ int32 MAGNETOMETER_MODE_NORMAL = 0
|
||||
int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1
|
||||
int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2
|
||||
|
||||
uint32 SENSOR_PRIO_MIN = 0
|
||||
uint32 SENSOR_PRIO_VERY_LOW = 25
|
||||
uint32 SENSOR_PRIO_LOW = 50
|
||||
uint32 SENSOR_PRIO_DEFAULT = 75
|
||||
uint32 SENSOR_PRIO_HIGH = 100
|
||||
uint32 SENSOR_PRIO_VERY_HIGH = 125
|
||||
uint32 SENSOR_PRIO_MAX = 255
|
||||
|
||||
# Sensor readings in raw and SI-unit form.
|
||||
#
|
||||
# These values are read from the sensors. Raw values are in sensor-specific units,
|
||||
@@ -21,6 +29,7 @@ uint64 timestamp # Timestamp in microseconds since boot, from gyro
|
||||
#
|
||||
int16[3] gyro_raw # Raw sensor values of angular velocity
|
||||
float32[3] gyro_rad_s # Angular velocity in radian per seconds
|
||||
uint32 gyro_priority # Sensor priority
|
||||
uint32 gyro_errcount # Error counter for gyro 0
|
||||
float32 gyro_temp # Temperature of gyro 0
|
||||
|
||||
@@ -29,6 +38,7 @@ float32[3] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2
|
||||
int16 accelerometer_mode # Accelerometer measurement mode
|
||||
float32 accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2
|
||||
uint64 accelerometer_timestamp # Accelerometer timestamp
|
||||
uint32 accelerometer_priority # Sensor priority
|
||||
uint32 accelerometer_errcount # Error counter for accel 0
|
||||
float32 accelerometer_temp # Temperature of accel 0
|
||||
|
||||
@@ -38,42 +48,49 @@ int16 magnetometer_mode # Magnetometer measurement mode
|
||||
float32 magnetometer_range_ga # measurement range in Gauss
|
||||
float32 magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor
|
||||
uint64 magnetometer_timestamp # Magnetometer timestamp
|
||||
uint32 magnetometer_priority # Sensor priority
|
||||
uint32 magnetometer_errcount # Error counter for mag 0
|
||||
float32 magnetometer_temp # Temperature of mag 0
|
||||
|
||||
int16[3] gyro1_raw # Raw sensor values of angular velocity
|
||||
float32[3] gyro1_rad_s # Angular velocity in radian per seconds
|
||||
uint64 gyro1_timestamp # Gyro timestamp
|
||||
uint32 gyro1_priority # Sensor priority
|
||||
uint32 gyro1_errcount # Error counter for gyro 1
|
||||
float32 gyro1_temp # Temperature of gyro 1
|
||||
|
||||
int16[3] accelerometer1_raw # Raw acceleration in NED body frame
|
||||
float32[3] accelerometer1_m_s2 # Acceleration in NED body frame, in m/s^2
|
||||
uint64 accelerometer1_timestamp # Accelerometer timestamp
|
||||
uint32 accelerometer1_priority # Sensor priority
|
||||
uint32 accelerometer1_errcount # Error counter for accel 1
|
||||
float32 accelerometer1_temp # Temperature of accel 1
|
||||
|
||||
int16[3] magnetometer1_raw # Raw magnetic field in NED body frame
|
||||
float32[3] magnetometer1_ga # Magnetic field in NED body frame, in Gauss
|
||||
uint64 magnetometer1_timestamp # Magnetometer timestamp
|
||||
uint32 magnetometer1_priority # Sensor priority
|
||||
uint32 magnetometer1_errcount # Error counter for mag 1
|
||||
float32 magnetometer1_temp # Temperature of mag 1
|
||||
|
||||
int16[3] gyro2_raw # Raw sensor values of angular velocity
|
||||
float32[3] gyro2_rad_s # Angular velocity in radian per seconds
|
||||
uint64 gyro2_timestamp # Gyro timestamp
|
||||
uint32 gyro2_priority # Sensor priority
|
||||
uint32 gyro2_errcount # Error counter for gyro 1
|
||||
float32 gyro2_temp # Temperature of gyro 1
|
||||
|
||||
int16[3] accelerometer2_raw # Raw acceleration in NED body frame
|
||||
float32[3] accelerometer2_m_s2 # Acceleration in NED body frame, in m/s^2
|
||||
uint64 accelerometer2_timestamp # Accelerometer timestamp
|
||||
uint32 accelerometer2_priority # Sensor priority
|
||||
uint32 accelerometer2_errcount # Error counter for accel 2
|
||||
float32 accelerometer2_temp # Temperature of accel 2
|
||||
|
||||
int16[3] magnetometer2_raw # Raw magnetic field in NED body frame
|
||||
float32[3] magnetometer2_ga # Magnetic field in NED body frame, in Gauss
|
||||
uint64 magnetometer2_timestamp # Magnetometer timestamp
|
||||
uint32 magnetometer2_priority # Sensor priority
|
||||
uint32 magnetometer2_errcount # Error counter for mag 2
|
||||
float32 magnetometer2_temp # Temperature of mag 2
|
||||
|
||||
@@ -81,11 +98,13 @@ float32 baro_pres_mbar # Barometric pressure, already temp. comp.
|
||||
float32 baro_alt_meter # Altitude, already temp. comp.
|
||||
float32 baro_temp_celcius # Temperature in degrees celsius
|
||||
uint64 baro_timestamp # Barometer timestamp
|
||||
uint32 baro_priority # Sensor priority
|
||||
|
||||
float32 baro1_pres_mbar # Barometric pressure, already temp. comp.
|
||||
float32 baro1_alt_meter # Altitude, already temp. comp.
|
||||
float32 baro1_temp_celcius # Temperature in degrees celsius
|
||||
uint64 baro1_timestamp # Barometer timestamp
|
||||
uint32 baro1_priority # Sensor priority
|
||||
|
||||
float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1
|
||||
uint16[10] adc_mapping # Channel indices of each of these values
|
||||
@@ -94,7 +113,9 @@ float32 mcu_temp_celcius # Internal temperature measurement of MCU
|
||||
float32 differential_pressure_pa # Airspeed sensor differential pressure
|
||||
uint64 differential_pressure_timestamp # Last measurement timestamp
|
||||
float32 differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading
|
||||
uint32 differential_pressure_priority # Sensor priority
|
||||
|
||||
float32 differential_pressure1_pa # Airspeed sensor differential pressure
|
||||
uint64 differential_pressure1_timestamp # Last measurement timestamp
|
||||
float32 differential_pressure1_filtered_pa # Low pass filtered airspeed sensor differential pressure reading
|
||||
uint32 differential_pressure1_priority # Sensor priority
|
||||
|
||||
@@ -211,8 +211,8 @@ private:
|
||||
int _accel2_sub; /**< raw accel2 data subscription */
|
||||
int _mag2_sub; /**< raw mag2 data subscription */
|
||||
int _rc_sub; /**< raw rc channels data subscription */
|
||||
int _baro_sub; /**< raw baro data subscription */
|
||||
int _baro1_sub; /**< raw baro 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 */
|
||||
@@ -227,6 +227,19 @@ private:
|
||||
orb_advert_t _battery_pub; /**< battery status */
|
||||
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 */
|
||||
int32_t _diff_pres_prio; /**< baro1 sensor priority */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
@@ -511,6 +524,19 @@ 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")),
|
||||
|
||||
@@ -1027,6 +1053,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
|
||||
raw.accelerometer_raw[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;
|
||||
}
|
||||
@@ -1050,6 +1077,7 @@ Sensors::accel_poll(struct sensor_combined_s &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;
|
||||
}
|
||||
@@ -1073,6 +1101,7 @@ Sensors::accel_poll(struct sensor_combined_s &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;
|
||||
}
|
||||
@@ -1101,6 +1130,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
|
||||
raw.gyro_raw[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;
|
||||
}
|
||||
@@ -1124,6 +1154,7 @@ Sensors::gyro_poll(struct sensor_combined_s &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;
|
||||
}
|
||||
@@ -1147,6 +1178,7 @@ Sensors::gyro_poll(struct sensor_combined_s &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;
|
||||
}
|
||||
@@ -1176,6 +1208,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
|
||||
raw.magnetometer_raw[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;
|
||||
}
|
||||
@@ -1200,6 +1233,7 @@ Sensors::mag_poll(struct sensor_combined_s &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;
|
||||
}
|
||||
@@ -1224,6 +1258,7 @@ Sensors::mag_poll(struct sensor_combined_s &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;
|
||||
}
|
||||
@@ -1244,6 +1279,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
|
||||
raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius
|
||||
|
||||
raw.baro_timestamp = _barometer.timestamp;
|
||||
raw.baro_priority = _baro_prio;
|
||||
}
|
||||
|
||||
orb_check(_baro1_sub, &baro_updated);
|
||||
@@ -1259,6 +1295,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
|
||||
raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
|
||||
|
||||
raw.baro1_timestamp = baro_report.timestamp;
|
||||
raw.baro1_priority = _baro1_prio;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2114,6 +2151,21 @@ Sensors::task_main()
|
||||
_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);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user