From 27e1aaeea5b4a464f74c51e9418fee8afd50d95e Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 11 Dec 2015 08:50:21 +0100 Subject: [PATCH] control state: indicate if airspeed is not valid --- msg/control_state.msg | 1 + .../attitude_estimator_q/attitude_estimator_q_main.cpp | 9 ++++++++- .../ekf_att_pos_estimator_main.cpp | 9 ++++++++- src/modules/fw_att_control/fw_att_control_main.cpp | 4 +++- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 10 +++++----- 5 files changed, 25 insertions(+), 8 deletions(-) diff --git a/msg/control_state.msg b/msg/control_state.msg index 4f130353d3..e504a765da 100644 --- a/msg/control_state.msg +++ b/msg/control_state.msg @@ -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 diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index f456e73120..3e44579c66 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -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) { diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 4a03af7890..321f742fb5 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -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; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index fc4b570687..0ea5a20f8c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -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); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 8b4e4224eb..752b1fd793 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -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; } }