From 7f8c183d99372206e4b0258e2328a1be39d76551 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 21 Jun 2016 22:46:10 +0200 Subject: [PATCH] added airspeed mode enum to control state topic Signed-off-by: Roman --- msg/control_state.msg | 4 +++ .../attitude_estimator_q_main.cpp | 28 +++++++++---------- src/modules/ekf2/ekf2_main.cpp | 24 ++++++++-------- src/modules/vtol_att_control/standard.cpp | 5 ++-- .../vtol_att_control/vtol_att_control_main.h | 1 + 5 files changed, 34 insertions(+), 28 deletions(-) diff --git a/msg/control_state.msg b/msg/control_state.msg index af6de428fa..96efea0f81 100644 --- a/msg/control_state.msg +++ b/msg/control_state.msg @@ -1,4 +1,8 @@ # This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */ +uint8 AIRSPD_MODE_MEAS = 0 # airspeed is measured airspeed from sensor +uint8 AIRSPD_MODE_EST = 1 # airspeed is estimated by body velocity +uint8 AIRSPD_MODE_DISABLED = 2 # airspeed is disabled + float32 x_acc # X acceleration in body frame float32 y_acc # Y acceleration in body frame float32 z_acc # Z acceleration in body frame 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 3492eaef0c..9ffd4599fe 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -631,27 +631,27 @@ void AttitudeEstimatorQ::task_main() ctrl_state.airspeed_valid = false; - // use estimated velocity for airspeed estimate - if (_airspeed_mode == 1) { - 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); - } - - // do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed - - } else if (_airspeed_mode == 2) { - - // use the measured airspeed - } else { - /* Airspeed - take airspeed measurement directly here as no wind is estimated */ + 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 */ diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 51f15cfc46..96dae5ee69 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -665,24 +665,24 @@ void Ekf2::task_main() ctrl_state.airspeed_valid = false; // use estimated velocity for airspeed estimate - if (_airspeed_mode.get() == 1) { - if (_ekf.local_position_is_valid()) { - ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2]); - } - - // do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed - - } else if (_airspeed_mode.get() == 2) { - - // use the measured airspeed - } else { - /* Airspeed - take airspeed measurement directly here as no wind is estimated */ + if (_airspeed_mode.get() == 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; } } + if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) { + if (_ekf.local_position_is_valid()) { + ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2]); + ctrl_state.airspeed_valid = true; + } + + } else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) { + // do nothing, airspeed has been declared as non-valid above, controllers + // will handle this assuming always trim airspeed + } // publish control state data if (_control_state_pub == nullptr) { diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index b84605a65c..e1343a85a6 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -187,7 +187,8 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode - if (((_params_standard.airspeed_mode == 2 || _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) && + if (((_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED || + _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) && (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)) || can_transition_on_ground()) { @@ -250,7 +251,7 @@ void Standard::update_transition_state() _mc_throttle_weight = weight; // time based blending when no airspeed sensor is set - } else if (_params_standard.airspeed_mode == 2 && + } else if (_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED && (float)hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1000000.0f) ) { float weight = 1.0f - (float)(hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_time_min * 1000000.0f)); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 90a36e652f..3412a53990 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -84,6 +84,7 @@ #include #include #include +#include #include #include #include