diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-12 12:58:17 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-12 12:58:17 +0100 |
commit | cd05dfcc7cb557575e62f5f93ddd3a86c073d517 (patch) | |
tree | a490b6b2909ff1b2ebb3e9b660b1c38df7cf01e1 /src | |
parent | e421260f7c3e2a3f7145c2f643f8440060a84777 (diff) | |
download | px4-firmware-cd05dfcc7cb557575e62f5f93ddd3a86c073d517.tar.gz px4-firmware-cd05dfcc7cb557575e62f5f93ddd3a86c073d517.tar.bz2 px4-firmware-cd05dfcc7cb557575e62f5f93ddd3a86c073d517.zip |
Moved to sensor_combined topic for estimator
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 148 |
1 files changed, 129 insertions, 19 deletions
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index e6747604b..466f6d51f 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -50,9 +50,17 @@ #include <poll.h> #include <time.h> #include <drivers/drv_hrt.h> + +#define SENSOR_COMBINED_SUB + + #include <drivers/drv_gyro.h> #include <drivers/drv_accel.h> #include <drivers/drv_mag.h> +#include <drivers/drv_baro.h> +#ifdef SENSOR_COMBINED_SUB +#include <uORB/topics/sensor_combined.h> +#endif #include <arch/board/board.h> #include <uORB/uORB.h> #include <uORB/topics/airspeed.h> @@ -60,14 +68,9 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/parameter_update.h> -#include <uORB/topics/mission.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <geo/geo.h> @@ -77,6 +80,8 @@ #include "../../../InertialNav/code/estimator.h" + + /** * estimator app start / stop handling function * @@ -118,12 +123,15 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _estimator_task; /**< task handle for sensor task */ - +#ifndef SENSOR_COMBINED_SUB int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ int _mag_sub; /**< mag sensor subscription */ - int _attitude_sub; /**< raw rc channels data subscription */ + #else + int _sensor_combined_sub; + #endif int _airspeed_sub; /**< airspeed subscription */ + int _baro_sub; /**< barometer subscription */ int _gps_sub; /**< GPS subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -135,12 +143,11 @@ private: orb_advert_t _local_pos_pub; /**< position in local frame */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct gyro_report _gyro; struct accel_report _accel; struct mag_report _mag; struct airspeed_s _airspeed; /**< airspeed */ + struct baro_report _baro; /**< baro readings */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_local_position_s _local_pos; /**< local vehicle position */ @@ -150,6 +157,10 @@ private: struct accel_scale _accel_offsets; struct mag_scale _mag_offsets; +#ifdef SENSOR_COMBINED_SUB + struct sensor_combined_s _sensor_combined; +#endif + 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 @@ -220,10 +231,15 @@ FixedwingEstimator::FixedwingEstimator() : _estimator_task(-1), /* subscriptions */ + #ifndef SENSOR_COMBINED_SUB _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), + #else + _sensor_combined_sub(-1), + #endif _airspeed_sub(-1), + _baro_sub(-1), _gps_sub(-1), _vstatus_sub(-1), _params_sub(-1), @@ -350,22 +366,29 @@ FixedwingEstimator::task_main() /* * do subscriptions */ - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); - //_baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); - /* rate limit gyro updates to 50 Hz */ + #ifndef SENSOR_COMBINED_SUB + + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + + /* rate limit gyro updates to 50 Hz */ /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_gyro_sub, 17); + #else + _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + /* XXX remove this!, BUT increase the data buffer size! */ + orb_set_interval(_sensor_combined_sub, 17); + #endif parameters_update(); @@ -384,8 +407,13 @@ FixedwingEstimator::task_main() /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; + #ifndef SENSOR_COMBINED_SUB fds[1].fd = _gyro_sub; fds[1].events = POLLIN; + #else + fds[1].fd = _sensor_combined_sub; + fds[1].events = POLLIN; + #endif hrt_abstime start_time = hrt_absolute_time(); @@ -422,11 +450,16 @@ FixedwingEstimator::task_main() /* check vehicle status for changes to publication state */ vehicle_status_poll(); + bool accel_updated; + bool mag_updated; + + perf_count(_perf_gyro); + /* load local copies */ + #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); - perf_count(_perf_gyro); + - bool accel_updated; orb_check(_accel_sub, &accel_updated); if (accel_updated) { @@ -434,6 +467,7 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } + IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - last_run) / 1e6f; @@ -461,6 +495,54 @@ FixedwingEstimator::task_main() dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; lastAccel = accel; + + #else + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); + + static hrt_abstime last_accel = 0; + static hrt_abstime last_mag = 0; + + if (last_accel != _sensor_combined.accelerometer_timestamp) { + accel_updated = true; + } + last_accel = _sensor_combined.accelerometer_timestamp; + + + // Copy gyro and accel + + IMUmsec = _sensor_combined.timestamp / 1e3f; + + float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; + last_run = _sensor_combined.timestamp; + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + // Always store data, independent of init status + /* fill in last data set */ + dtIMU = deltaT; + + angRate.x = _sensor_combined.gyro_rad_s[0]; + angRate.y = _sensor_combined.gyro_rad_s[1]; + angRate.z = _sensor_combined.gyro_rad_s[2]; + + accel.x = _sensor_combined.accelerometer_m_s2[0]; + accel.y = _sensor_combined.accelerometer_m_s2[1]; + accel.z = _sensor_combined.accelerometer_m_s2[2]; + + dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; + lastAngRate = angRate; + dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + lastAccel = accel; + + if (last_mag != _sensor_combined.magnetometer_timestamp) { + mag_updated = true; + } + last_mag = _sensor_combined.magnetometer_timestamp; + + #endif + if (_initialized) { /* predict states and covariances */ @@ -490,6 +572,14 @@ FixedwingEstimator::task_main() } } + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + if (baro_updated) { + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + /* XXX leverage baro */ + } + bool gps_updated; orb_check(_gps_sub, &gps_updated); if (gps_updated) { @@ -554,12 +644,16 @@ FixedwingEstimator::task_main() fuseHgtData = false; } - bool mag_updated; + #ifndef SENSOR_COMBINED_SUB orb_check(_mag_sub, &mag_updated); + #endif if (mag_updated) { - orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + perf_count(_perf_mag); + #ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + // XXX we compensate the offsets upfront - should be close to zero. // XXX the purpose of the 0.001 factor is unclear // 0.001f @@ -571,6 +665,22 @@ FixedwingEstimator::task_main() magData.z = 0.001f * _mag.z; magBias.z = 0.0f; // _mag_offsets.y_offset + + #else + + // XXX we compensate the offsets upfront - should be close to zero. + // XXX the purpose of the 0.001 factor is unclear + // 0.001f + magData.x = 0.001f * _sensor_combined.magnetometer_ga[0]; + magBias.x = 0.0f; // _mag_offsets.x_offset + + magData.y = 0.001f * _sensor_combined.magnetometer_ga[1]; + magBias.y = 0.0f; // _mag_offsets.y_offset + + magData.z = 0.001f * _sensor_combined.magnetometer_ga[2]; + magBias.z = 0.0f; // _mag_offsets.y_offset + + #endif if (_initialized) { |