diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index dca8cafe271..523abacadc6 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -52,7 +52,7 @@ using namespace matrix; Tailsitter::Tailsitter(VtolAttitudeControl *attc) : VtolType(attc) { - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; _vtol_schedule.transition_start = 0; _mc_roll_weight = 1.0f; @@ -92,25 +92,25 @@ void Tailsitter::update_vtol_state() if (!_attc->is_fixed_wing_requested()) { switch (_vtol_schedule.flight_mode) { // user switchig to MC mode - case MC_MODE: + case vtol_mode::MC_MODE: break; - case FW_MODE: - _vtol_schedule.flight_mode = TRANSITION_BACK; + case vtol_mode::FW_MODE: + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK; _vtol_schedule.transition_start = hrt_absolute_time(); break; - case TRANSITION_FRONT_P1: + case vtol_mode::TRANSITION_FRONT_P1: // failsafe into multicopter mode - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; break; - case TRANSITION_BACK: + case vtol_mode::TRANSITION_BACK: float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; // check if we have reached pitch angle to switch to MC mode if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _params->back_trans_duration) { - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; } break; @@ -119,55 +119,55 @@ void Tailsitter::update_vtol_state() } else { // user switchig to FW mode switch (_vtol_schedule.flight_mode) { - case MC_MODE: + case vtol_mode::MC_MODE: // initialise a front transition - _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1; _vtol_schedule.transition_start = hrt_absolute_time(); break; - case FW_MODE: + case vtol_mode::FW_MODE: break; - case TRANSITION_FRONT_P1: { + case vtol_mode::TRANSITION_FRONT_P1: { bool airspeed_condition_satisfied = _airspeed->indicated_airspeed_m_s >= _params->transition_airspeed; airspeed_condition_satisfied |= _params->airspeed_disabled; // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode if ((airspeed_condition_satisfied && pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) { - _vtol_schedule.flight_mode = FW_MODE; + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; } break; } - case TRANSITION_BACK: + case vtol_mode::TRANSITION_BACK: // failsafe into fixed wing mode - _vtol_schedule.flight_mode = FW_MODE; + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; break; } } // map tailsitter specific control phases to simple control modes switch (_vtol_schedule.flight_mode) { - case MC_MODE: + case vtol_mode::MC_MODE: _vtol_mode = mode::ROTARY_WING; _vtol_vehicle_status->vtol_in_trans_mode = false; _flag_was_in_trans_mode = false; break; - case FW_MODE: + case vtol_mode::FW_MODE: _vtol_mode = mode::FIXED_WING; _vtol_vehicle_status->vtol_in_trans_mode = false; _flag_was_in_trans_mode = false; break; - case TRANSITION_FRONT_P1: + case vtol_mode::TRANSITION_FRONT_P1: _vtol_mode = mode::TRANSITION_TO_FW; _vtol_vehicle_status->vtol_in_trans_mode = true; break; - case TRANSITION_BACK: + case vtol_mode::TRANSITION_BACK: _vtol_mode = mode::TRANSITION_TO_MC; _vtol_vehicle_status->vtol_in_trans_mode = true; break; @@ -181,7 +181,7 @@ void Tailsitter::update_transition_state() if (!_flag_was_in_trans_mode) { _flag_was_in_trans_mode = true; - if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) { // calculate rotation axis for transition. _q_trans_start = Quatf(_v_att->q); Vector3f z = -_q_trans_start.dcm_z(); @@ -198,7 +198,7 @@ void Tailsitter::update_transition_state() // multirotor frame _q_trans_start = _q_trans_start * Quatf(Eulerf(0, -M_PI_2_F, 0)); - } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) { // initial attitude setpoint for the transition should be with wings level _q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body); Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1, 0, 0); @@ -212,7 +212,7 @@ void Tailsitter::update_transition_state() float tilt = acosf(_q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - _q_trans_sp(2) * _q_trans_sp( 2) + _q_trans_sp(3) * _q_trans_sp(3)); - if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) { const float trans_pitch_rate = M_PI_2_F / _params->front_trans_duration; @@ -221,7 +221,7 @@ void Tailsitter::update_transition_state() time_since_trans_start * trans_pitch_rate)) * _q_trans_start; } - } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) { const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration; @@ -287,7 +287,7 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; - if (_vtol_schedule.flight_mode == FW_MODE) { + if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; @@ -296,7 +296,7 @@ void Tailsitter::fill_actuator_outputs() _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; } - if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == MC_MODE) { + if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) { _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0; _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0; diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index da5715b2e0c..e718ee79b04 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -43,7 +43,7 @@ #define TAILSITTER_H #include "vtol_type.h" -#include /** is it necsacery? **/ + #include #include #include @@ -73,7 +73,7 @@ private: param_t fw_pitch_sp_offset; } _params_handles_tailsitter{}; - enum vtol_mode { + enum class vtol_mode { MC_MODE = 0, /**< vtol is in multicopter mode */ TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ TRANSITION_BACK, /**< vtol is in back transition mode */ diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 56315b28f83..45312bb32c6 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -47,7 +47,7 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : VtolType(attc) { - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; _vtol_schedule.transition_start = 0; _mc_roll_weight = 1.0f; @@ -96,29 +96,29 @@ void Tiltrotor::update_vtol_state() // plane is in multicopter mode switch (_vtol_schedule.flight_mode) { - case MC_MODE: + case vtol_mode::MC_MODE: break; - case FW_MODE: - _vtol_schedule.flight_mode = TRANSITION_BACK; + case vtol_mode::FW_MODE: + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK; _vtol_schedule.transition_start = hrt_absolute_time(); break; - case TRANSITION_FRONT_P1: + case vtol_mode::TRANSITION_FRONT_P1: // failsafe into multicopter mode - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; break; - case TRANSITION_FRONT_P2: + case vtol_mode::TRANSITION_FRONT_P2: // failsafe into multicopter mode - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; break; - case TRANSITION_BACK: + case vtol_mode::TRANSITION_BACK: float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; if (_tilt_control <= _params_tiltrotor.tilt_mc && time_since_trans_start > _params->back_trans_duration) { - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; } break; @@ -127,16 +127,16 @@ void Tiltrotor::update_vtol_state() } else { switch (_vtol_schedule.flight_mode) { - case MC_MODE: + case vtol_mode::MC_MODE: // initialise a front transition - _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1; _vtol_schedule.transition_start = hrt_absolute_time(); break; - case FW_MODE: + case vtol_mode::FW_MODE: break; - case TRANSITION_FRONT_P1: { + case vtol_mode::TRANSITION_FRONT_P1: { // allow switch if we are not armed for the sake of bench testing bool transition_to_p2 = can_transition_on_ground(); @@ -153,46 +153,46 @@ void Tiltrotor::update_vtol_state() time_since_trans_start > _params->front_trans_time_openloop; if (transition_to_p2) { - _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P2; _vtol_schedule.transition_start = hrt_absolute_time(); } break; } - case TRANSITION_FRONT_P2: + case vtol_mode::TRANSITION_FRONT_P2: // if the rotors have been tilted completely we switch to fw mode if (_tilt_control >= _params_tiltrotor.tilt_fw) { - _vtol_schedule.flight_mode = FW_MODE; + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; _tilt_control = _params_tiltrotor.tilt_fw; } break; - case TRANSITION_BACK: + case vtol_mode::TRANSITION_BACK: // failsafe into fixed wing mode - _vtol_schedule.flight_mode = FW_MODE; + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; break; } } // map tiltrotor specific control phases to simple control modes switch (_vtol_schedule.flight_mode) { - case MC_MODE: + case vtol_mode::MC_MODE: _vtol_mode = mode::ROTARY_WING; break; - case FW_MODE: + case vtol_mode::FW_MODE: _vtol_mode = mode::FIXED_WING; break; - case TRANSITION_FRONT_P1: - case TRANSITION_FRONT_P2: + case vtol_mode::TRANSITION_FRONT_P1: + case vtol_mode::TRANSITION_FRONT_P2: _vtol_mode = mode::TRANSITION_TO_FW; break; - case TRANSITION_BACK: + case vtol_mode::TRANSITION_BACK: _vtol_mode = mode::TRANSITION_TO_MC; break; } @@ -225,7 +225,7 @@ void Tiltrotor::update_transition_state() _flag_was_in_trans_mode = true; } - if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) { // for the first part of the transition the rear rotors are enabled if (_motor_state != motor_state::ENABLED) { _motor_state = set_motor_state(_motor_state, motor_state::ENABLED); @@ -263,7 +263,7 @@ void Tiltrotor::update_transition_state() _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; - } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, tilt the rotors forward completely _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * time_since_trans_start / @@ -282,7 +282,7 @@ void Tiltrotor::update_transition_state() _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; - } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) { if (_motor_state != motor_state::ENABLED) { _motor_state = set_motor_state(_motor_state, motor_state::ENABLED); } @@ -345,7 +345,7 @@ void Tiltrotor::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; - if (_vtol_schedule.flight_mode == FW_MODE) { + if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; @@ -366,7 +366,7 @@ void Tiltrotor::fill_actuator_outputs() _actuators_out_1->control[4] = _tilt_control; - if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == MC_MODE) { + if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) { _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0.0f; _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0.0f; _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = 0.0f; diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index edfe89dd4f7..7ee36d76156 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -75,7 +75,7 @@ private: param_t front_trans_dur_p2; } _params_handles_tiltrotor; - enum vtol_mode { + enum class vtol_mode { MC_MODE = 0, /**< vtol is in multicopter mode */ TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */