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

channel airspeed meas over control state for q estimator

Browse Source
This commit is contained in:
Youssef Demitri 2015-11-25 10:28:19 +01:00 committed by tumbili
parent 56a6809486
commit e5b2c652e2
1 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -59,6 +59,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@ -122,6 +123,7 @@ private:
int _params_sub = -1;
int _vision_sub = -1;
int _mocap_sub = -1;
int _airspeed_sub = -1;
int _global_pos_sub = -1;
orb_advert_t _att_pub = nullptr;
orb_advert_t _ctrl_state_pub = nullptr;
@ -161,6 +163,8 @@ private:
att_pos_mocap_s _mocap = {};
Vector<3> _mocap_hdg;
airspeed_s _airspeed = {};
Quaternion _q;
Vector<3> _rates;
Vector<3> _gyro_bias;
@ -292,6 +296,8 @@ void AttitudeEstimatorQ::task_main()
_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@ -473,6 +479,14 @@ void AttitudeEstimatorQ::task_main()
_mocap_hdg = Rmoc.transposed() * v;
}
// Update airspeed
bool airspeed_updated = false;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
}
// Check for timeouts on data
if (_ext_hdg_mode == 1) {
_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
@ -586,6 +600,9 @@ void AttitudeEstimatorQ::task_main()
ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
/* Airspeed - take airspeed measurement directly here as no wind is estimated */
ctrl_state.airspeed = _airspeed.true_airspeed_m_s;
/* Publish to control state topic */
if (_ctrl_state_pub == nullptr) {
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);