fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 05:01:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

delete control_state and cleanup vehicle_attitude (#7882)

Browse Source
This commit is contained in:
Daniel Agar
2017-09-21 16:24:53 -04:00
committed by GitHub
parent 5bea264a5f
commit b4755297ec
42 changed files with 776 additions and 1061 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+21 -83
View File
@@ -55,7 +55,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/att_pos_mocap.h>
@@ -125,7 +124,6 @@ private:
int _airspeed_sub = -1;
int _global_pos_sub = -1;
orb_advert_t _att_pub = nullptr;
orb_advert_t _ctrl_state_pub = nullptr;
orb_advert_t _est_state_pub = nullptr;
struct {
@@ -138,7 +136,7 @@ private:
param_t acc_comp;
param_t bias_max;
param_t ext_hdg_mode;
param_t airspeed_mode;
param_t airspeed_disabled;
} _params_handles; /**< handles for interesting parameters */
float _w_accel = 0.0f;
@@ -150,7 +148,7 @@ private:
bool _acc_comp = false;
float _bias_max = 0.0f;
int _ext_hdg_mode = 0;
int _airspeed_mode = 0;
int _airspeed_disabled = 0;
Vector<3> _gyro;
Vector<3> _accel;
@@ -176,9 +174,6 @@ private:
math::LowPassFilter2p _lp_accel_x;
math::LowPassFilter2p _lp_accel_y;
math::LowPassFilter2p _lp_accel_z;
math::LowPassFilter2p _lp_gyro_x;
math::LowPassFilter2p _lp_gyro_y;
math::LowPassFilter2p _lp_gyro_z;
hrt_abstime _vel_prev_t = 0;
@@ -206,10 +201,7 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_pos_acc(0, 0, 0),
_lp_accel_x(250.0f, 30.0f),
_lp_accel_y(250.0f, 30.0f),
_lp_accel_z(250.0f, 30.0f),
_lp_gyro_x(250.0f, 30.0f),
_lp_gyro_y(250.0f, 30.0f),
_lp_gyro_z(250.0f, 30.0f)
_lp_accel_z(250.0f, 30.0f)
{
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
@@ -220,7 +212,7 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_params_handles.acc_comp = param_find("ATT_ACC_COMP");
_params_handles.bias_max = param_find("ATT_BIAS_MAX");
_params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M");
_params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
_params_handles.airspeed_disabled = param_find("FW_ARSP_MODE");
}
/**
@@ -330,14 +322,13 @@ void AttitudeEstimatorQ::task_main()
// Feed validator with recent sensor data
if (sensors.timestamp > 0) {
// Filter gyro signal since it is not fildered in the drivers.
_gyro(0) = _lp_gyro_x.apply(sensors.gyro_rad[0]);
_gyro(1) = _lp_gyro_y.apply(sensors.gyro_rad[1]);
_gyro(2) = _lp_gyro_z.apply(sensors.gyro_rad[2]);
_gyro(0) = sensors.gyro_rad[0];
_gyro(1) = sensors.gyro_rad[1];
_gyro(2) = sensors.gyro_rad[2];
}
if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
// Filter accel signal since it is not fildered in the drivers.
// Filter accel signal since it is not filtered in the drivers.
_accel(0) = _lp_accel_x.apply(sensors.accelerometer_m_s2[0]);
_accel(1) = _lp_accel_y.apply(sensors.accelerometer_m_s2[1]);
_accel(2) = _lp_accel_z.apply(sensors.accelerometer_m_s2[2]);
@@ -459,73 +450,20 @@ void AttitudeEstimatorQ::task_main()
continue;
}
{
vehicle_attitude_s att = {
.timestamp = sensors.timestamp,
.rollspeed = _rates(0),
.pitchspeed = _rates(1),
.yawspeed = _rates(2),
.q = {_q(0), _q(1), _q(2), _q(3)}
};
vehicle_attitude_s att = {
.timestamp = sensors.timestamp,
.rollspeed = _rates(0),
.pitchspeed = _rates(1),
.yawspeed = _rates(2),
/* the instance count is not used here */
int att_inst;
orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
}
.q = {_q(0), _q(1), _q(2), _q(3)},
.delta_q_reset = {},
.quat_reset_counter = 0,
};
{
struct control_state_s ctrl_state = {};
ctrl_state.timestamp = sensors.timestamp;
/* attitude quaternions for control state */
ctrl_state.q[0] = _q(0);
ctrl_state.q[1] = _q(1);
ctrl_state.q[2] = _q(2);
ctrl_state.q[3] = _q(3);
ctrl_state.x_acc = _accel(0);
ctrl_state.y_acc = _accel(1);
ctrl_state.z_acc = _accel(2);
/* attitude rates for control state */
ctrl_state.roll_rate = _rates(0);
ctrl_state.pitch_rate = _rates(1);
ctrl_state.yaw_rate = _rates(2);
/* TODO get bias estimates from estimator */
ctrl_state.roll_rate_bias = 0.0f;
ctrl_state.pitch_rate_bias = 0.0f;
ctrl_state.yaw_rate_bias = 0.0f;
ctrl_state.airspeed_valid = false;
if (_airspeed_mode == control_state_s::AIRSPD_MODE_MEAS) {
// use measured airspeed
if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
&& _airspeed.timestamp > 0) {
ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
ctrl_state.airspeed_valid = true;
}
}
else if (_airspeed_mode == control_state_s::AIRSPD_MODE_EST) {
// use estimated body velocity as airspeed estimate
if (hrt_absolute_time() - _gpos.timestamp < 1e6) {
ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e + _gpos.vel_d * _gpos.vel_d);
ctrl_state.airspeed_valid = true;
}
} else if (_airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) {
// do nothing, airspeed has been declared as non-valid above, controllers
// will handle this assuming always trim airspeed
}
/* the instance count is not used here */
int ctrl_inst;
/* publish to control state topic */
orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
}
/* the instance count is not used here */
int att_inst;
orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
{
//struct estimator_status_s est = {};
@@ -582,7 +520,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
_acc_comp = acc_comp_int != 0;
param_get(_params_handles.bias_max, &_bias_max);
param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
param_get(_params_handles.airspeed_mode, &_airspeed_mode);
param_get(_params_handles.airspeed_disabled, &_airspeed_disabled);
}
}