fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 14:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors : add sensor priority order

Browse Source
This commit is contained in:
Mohammed Kabir
2015-08-17 09:25:33 +02:00
committed by Lorenz Meier
parent 41b3452e22
commit 10360a93b7
2 changed files with 75 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+54 -2
View File
@@ -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);