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

Added lots of instrumentation to ensure all data sources are coming in clean

Browse Source
This commit is contained in:
Lorenz Meier
2014-01-03 00:35:18 +01:00
parent 963f47ff0f
commit a5e95bde0a
1 changed files with 26 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+26 -1
View File
@@ -145,6 +145,12 @@ private:
struct mag_scale _mag_offsets; struct mag_scale _mag_offsets;
perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _perf_gyro; ///<local performance counter for gyro updates
perf_counter_t _perf_accel; ///<local performance counter for accel updates
perf_counter_t _perf_mag; ///<local performance counter for mag updates
perf_counter_t _perf_gps; ///<local performance counter for gps updates
perf_counter_t _perf_baro; ///<local performance counter for baro updates
perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates
bool _initialized; bool _initialized;
@@ -223,6 +229,13 @@ FixedwingEstimator::FixedwingEstimator() :
/* performance counters */ /* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw_att_pos_estimator")), _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_pos_estimator")),
_perf_gyro(perf_alloc(PC_ELAPSED, "fw_ekf_gyro_upd")),
_perf_accel(perf_alloc(PC_ELAPSED, "fw_ekf_accel_upd")),
_perf_mag(perf_alloc(PC_ELAPSED, "fw_ekf_mag_upd")),
_perf_gps(perf_alloc(PC_ELAPSED, "fw_ekf_gps_upd")),
_perf_baro(perf_alloc(PC_ELAPSED, "fw_ekf_baro_upd")),
_perf_airspeed(perf_alloc(PC_ELAPSED, "fw_ekf_aspd_upd")),
/* states */ /* states */
_initialized(false) _initialized(false)
{ {
@@ -333,6 +346,7 @@ FixedwingEstimator::task_main()
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag));
//_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -403,7 +417,15 @@ FixedwingEstimator::task_main()
/* load local copies */ /* load local copies */
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); perf_count(_perf_gyro);
bool accel_updated;
orb_check(_accel_sub, &accel_updated);
if (accel_updated) {
perf_count(_perf_accel);
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
}
float IMUmsec = _gyro.timestamp / 1e3; float IMUmsec = _gyro.timestamp / 1e3;
@@ -465,6 +487,7 @@ FixedwingEstimator::task_main()
orb_check(_gps_sub, &gps_updated); orb_check(_gps_sub, &gps_updated);
if (gps_updated) { if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps);
if (_gps.fix_type > 2) { if (_gps.fix_type > 2) {
/* fuse GPS updates */ /* fuse GPS updates */
@@ -529,6 +552,7 @@ FixedwingEstimator::task_main()
orb_check(_mag_sub, &mag_updated); orb_check(_mag_sub, &mag_updated);
if (mag_updated) { if (mag_updated) {
orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
perf_count(_perf_mag);
// XXX we compensate the offsets upfront - should be close to zero. // XXX we compensate the offsets upfront - should be close to zero.
magData.x = _mag.x; magData.x = _mag.x;
@@ -555,6 +579,7 @@ FixedwingEstimator::task_main()
orb_check(_airspeed_sub, &airspeed_updated); orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated && _initialized) { if (airspeed_updated && _initialized) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) {