dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

control state: indicate if airspeed is not valid

This commit is contained in:
Roman 2015-12-11 08:50:21 +01:00 committed by Lorenz Meier
parent 48bbe8f6ba
commit 27e1aaeea5
5 changed files with 25 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/control_state.msg
View File

@ -10,6 +10,7 @@ float32 x_pos # X position in local frame
float32 y_pos # Y position in local frame
float32 z_pos # z position in local frame
float32 airspeed # Airspeed, estimated
bool airspeed_valid # False: Non-finite values or non-updating sensor
float32[3] vel_variance # Variance in body velocity estimate
float32[3] pos_variance # Variance in local position estimate
float32[4] q # Attitude Quaternion

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

@ -623,7 +623,14 @@ 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.indicated_airspeed_m_s;
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 {
ctrl_state.airspeed_valid = false;
}
/* Publish to control state topic */
if (_ctrl_state_pub == nullptr) {

9
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -896,7 +896,14 @@ void AttitudePositionEstimatorEKF::publishControlState()
//_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
// the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL
// and in outdoor tests
_ctrl_state.airspeed = _airspeed.true_airspeed_m_s;
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 {
_ctrl_state.airspeed_valid = false;
}
/* Attitude Rates */
_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;

4
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -889,11 +889,13 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) {
if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed) || !_ctrl_state.airspeed_valid) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _ctrl_state.airspeed);

10
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -229,7 +229,7 @@ private:
/* throttle and airspeed states */
float _airspeed_error; ///< airspeed error to setpoint in m/s
bool _airspeed_valid; ///< flag if a valid airspeed estimate exists
uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures
uint64_t _airspeed_last_received; ///< last time airspeed was received. Used to detect timeouts.
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
bool _global_pos_valid; ///< global position is valid
math::Matrix<3, 3> _R_nb; ///< current attitude
@ -558,7 +558,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
launchDetector(),
_airspeed_error(0.0f),
_airspeed_valid(false),
_airspeed_last_valid(0),
_airspeed_last_received(0),
_groundspeed_undershoot(0.0f),
_global_pos_valid(false),
_l1_control(),
@ -807,13 +807,13 @@ FixedwingPositionControl::control_state_poll()
if (ctrl_state_updated) {
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
_airspeed_valid = true;
_airspeed_last_valid = hrt_absolute_time();
_airspeed_valid = _ctrl_state.airspeed_valid;
_airspeed_last_received = hrt_absolute_time();
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_received) > 1e6) {
_airspeed_valid = false;
}
}