diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index c685dfb35224..67fb6d0635dc 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -440,7 +440,7 @@ FixedwingAttitudeControl::vehicle_status_poll() int32_t vt_type = -1; if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { - _is_tailsitter = (vt_type == vtol_type::TAILSITTER); + _is_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); } } else { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 735be816a0ad..56a5b704f0f9 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -432,7 +432,7 @@ FixedwingPositionControl::vehicle_attitude_poll() // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset // between multirotor and fixed wing flight - if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + if (static_cast(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { Dcmf R_offset = Eulerf(0, M_PI_2_F, 0); _R_nb = _R_nb * R_offset; } @@ -2004,7 +2004,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee // tailsitters use the multicopter frame as reference, in fixed wing // we need to use the fixed wing frame - if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + if (static_cast(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { float tmp = accel_body(0); accel_body(0) = -accel_body(2); accel_body(2) = tmp; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 295ae8729643..2f708416b317 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -256,7 +256,7 @@ MulticopterAttitudeControl::vehicle_status_poll() int32_t vt_type = -1; if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { - _is_tailsitter = (vt_type == vtol_type::TAILSITTER); + _is_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); } } else { diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 0f4468d2ec35..8bcf9a0c5f61 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -51,7 +51,7 @@ using namespace matrix; Standard::Standard(VtolAttitudeControl *attc) : VtolType(attc) { - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; _vtol_schedule.transition_start = 0; _pusher_active = false; @@ -118,39 +118,39 @@ void Standard::update_vtol_state() if (!_attc->is_fixed_wing_requested()) { // the transition to fw mode switch is off - if (_vtol_schedule.flight_mode == MC_MODE) { + if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) { // in mc mode - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; mc_weight = 1.0f; _pusher_throttle = 0.0f; _reverse_output = 0.0f; - } else if (_vtol_schedule.flight_mode == FW_MODE) { + } else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { // transition to mc mode if (_vtol_vehicle_status->vtol_transition_failsafe == true) { // Failsafe event, engage mc motors immediately - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; _pusher_throttle = 0.0f; _reverse_output = 0.0f; } else { // Regular backtransition - _vtol_schedule.flight_mode = TRANSITION_TO_MC; + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_MC; _vtol_schedule.transition_start = hrt_absolute_time(); _reverse_output = _params_standard.reverse_output; } - } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) { // failsafe back to mc mode - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; mc_weight = 1.0f; _pusher_throttle = 0.0f; _reverse_output = 0.0f; - } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { // transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed const Dcmf R_to_body(Quatf(_v_att->q).inversed()); @@ -160,33 +160,33 @@ void Standard::update_vtol_state() if (time_since_trans_start > _params->back_trans_duration || (_local_pos->v_xy_valid && x_vel <= _params->mpc_xy_cruise)) { - _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; } } } else { // the transition to fw mode switch is on - if (_vtol_schedule.flight_mode == MC_MODE || _vtol_schedule.flight_mode == TRANSITION_TO_MC) { + if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE || _vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { // start transition to fw mode /* NOTE: The failsafe transition to fixed-wing was removed because it can result in an * unsafe flying state. */ - _vtol_schedule.flight_mode = TRANSITION_TO_FW; + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_FW; _vtol_schedule.transition_start = hrt_absolute_time(); - } else if (_vtol_schedule.flight_mode == FW_MODE) { + } else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { // in fw mode - _vtol_schedule.flight_mode = FW_MODE; + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; mc_weight = 0.0f; - } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode if (((_params->airspeed_disabled || _airspeed->indicated_airspeed_m_s >= _params->transition_airspeed) && time_since_trans_start > _params->front_trans_time_min) || can_transition_on_ground()) { - _vtol_schedule.flight_mode = FW_MODE; + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; // don't set pusher throttle here as it's being ramped up elsewhere _trans_finished_ts = hrt_absolute_time(); @@ -202,19 +202,19 @@ void Standard::update_vtol_state() // map 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_TO_FW: + case vtol_mode::TRANSITION_TO_FW: _vtol_mode = mode::TRANSITION_TO_FW; break; - case TRANSITION_TO_MC: + case vtol_mode::TRANSITION_TO_MC: _vtol_mode = mode::TRANSITION_TO_MC; break; } @@ -230,7 +230,7 @@ void Standard::update_transition_state() // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) { if (_params_standard.pusher_ramp_dt <= 0.0f) { // just set the final target throttle value _pusher_throttle = _params->front_trans_throttle; @@ -270,7 +270,7 @@ void Standard::update_transition_state() } } - } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { // maintain FW_PSP_OFF _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset; @@ -294,8 +294,8 @@ void Standard::update_transition_state() } // in back transition we need to start the MC motors again - if (_motor_state != ENABLED) { - _motor_state = set_motor_state(_motor_state, ENABLED); + if (_motor_state != motor_state::ENABLED) { + _motor_state = set_motor_state(_motor_state, motor_state::ENABLED); } } @@ -416,7 +416,7 @@ void Standard::fill_actuator_outputs() _actuators_out_1->timestamp = hrt_absolute_time(); _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - if (_vtol_schedule.flight_mode != MC_MODE) { + if (_vtol_schedule.flight_mode != vtol_mode::MC_MODE) { // roll _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; @@ -454,7 +454,7 @@ void Standard::fill_actuator_outputs() } // set the fixed wing throttle control - if (_vtol_schedule.flight_mode == FW_MODE) { + if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { // take the throttle value commanded by the fw controller _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 27ada9c8e6f3..da623cfc30b5 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -86,7 +86,7 @@ class Standard : public VtolType param_t reverse_delay; } _params_handles_standard; - enum vtol_mode { + enum class vtol_mode { MC_MODE = 0, TRANSITION_TO_FW, TRANSITION_TO_MC, diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index aebe026be71f..dca8cafe2719 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -151,24 +151,24 @@ void Tailsitter::update_vtol_state() // map tailsitter specific control phases to simple control modes switch (_vtol_schedule.flight_mode) { case MC_MODE: - _vtol_mode = ROTARY_WING; + _vtol_mode = mode::ROTARY_WING; _vtol_vehicle_status->vtol_in_trans_mode = false; _flag_was_in_trans_mode = false; break; case FW_MODE: - _vtol_mode = FIXED_WING; + _vtol_mode = mode::FIXED_WING; _vtol_vehicle_status->vtol_in_trans_mode = false; _flag_was_in_trans_mode = false; break; case TRANSITION_FRONT_P1: - _vtol_mode = TRANSITION_TO_FW; + _vtol_mode = mode::TRANSITION_TO_FW; _vtol_vehicle_status->vtol_in_trans_mode = true; break; case TRANSITION_BACK: - _vtol_mode = TRANSITION_TO_MC; + _vtol_mode = mode::TRANSITION_TO_MC; _vtol_vehicle_status->vtol_in_trans_mode = true; break; } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 69558f5781ee..56315b28f833 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -180,20 +180,20 @@ void Tiltrotor::update_vtol_state() // map tiltrotor specific control phases to simple control modes switch (_vtol_schedule.flight_mode) { case MC_MODE: - _vtol_mode = ROTARY_WING; + _vtol_mode = mode::ROTARY_WING; break; case FW_MODE: - _vtol_mode = FIXED_WING; + _vtol_mode = mode::FIXED_WING; break; case TRANSITION_FRONT_P1: case TRANSITION_FRONT_P2: - _vtol_mode = TRANSITION_TO_FW; + _vtol_mode = mode::TRANSITION_TO_FW; break; case TRANSITION_BACK: - _vtol_mode = TRANSITION_TO_MC; + _vtol_mode = mode::TRANSITION_TO_MC; break; } } @@ -227,8 +227,8 @@ void Tiltrotor::update_transition_state() if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // for the first part of the transition the rear rotors are enabled - if (_motor_state != ENABLED) { - _motor_state = set_motor_state(_motor_state, ENABLED); + if (_motor_state != motor_state::ENABLED) { + _motor_state = set_motor_state(_motor_state, motor_state::ENABLED); } // tilt rotors forward up to certain angle @@ -277,14 +277,14 @@ void Tiltrotor::update_transition_state() (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN; - _motor_state = set_motor_state(_motor_state, VALUE, rear_value); + _motor_state = set_motor_state(_motor_state, motor_state::VALUE, rear_value); _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { - if (_motor_state != ENABLED) { - _motor_state = set_motor_state(_motor_state, ENABLED); + if (_motor_state != motor_state::ENABLED) { + _motor_state = set_motor_state(_motor_state, motor_state::ENABLED); } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 5e7fd840cf26..8a56cdc3d9d5 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -94,13 +94,13 @@ VtolAttitudeControl::VtolAttitudeControl() /* fetch initial parameter values */ parameters_update(); - if (_params.vtol_type == vtol_type::TAILSITTER) { + if (static_cast(_params.vtol_type) == vtol_type::TAILSITTER) { _vtol_type = new Tailsitter(this); - } else if (_params.vtol_type == vtol_type::TILTROTOR) { + } else if (static_cast(_params.vtol_type) == vtol_type::TILTROTOR) { _vtol_type = new Tiltrotor(this); - } else if (_params.vtol_type == vtol_type::STANDARD) { + } else if (static_cast(_params.vtol_type) == vtol_type::STANDARD) { _vtol_type = new Standard(this); } else { @@ -499,7 +499,7 @@ VtolAttitudeControl::parameters_update() // normally the parameter fw_motors_off can be used to specify this, however, since historically standard vtol code // did not use the interface of the VtolType class to disable motors we will have users flying around with a wrong // parameter value. Therefore, explicitly set it here such that all motors will be disabled as expected. - if (_params.vtol_type == vtol_type::STANDARD) { + if (static_cast(_params.vtol_type) == vtol_type::STANDARD) { _params.fw_motors_off = 12345678; } @@ -571,13 +571,13 @@ void VtolAttitudeControl::task_main() // run vtol_att on MC actuator publications, unless in full FW mode switch (_vtol_type->get_mode()) { - case TRANSITION_TO_FW: - case TRANSITION_TO_MC: - case ROTARY_WING: + case mode::TRANSITION_TO_FW: + case mode::TRANSITION_TO_MC: + case mode::ROTARY_WING: fds[0].fd = _actuator_inputs_mc; break; - case FIXED_WING: + case mode::FIXED_WING: fds[0].fd = _actuator_inputs_fw; break; } @@ -616,13 +616,13 @@ void VtolAttitudeControl::task_main() // reset transition command if not auto control if (_v_control_mode.flag_control_manual_enabled) { - if (_vtol_type->get_mode() == ROTARY_WING) { + if (_vtol_type->get_mode() == mode::ROTARY_WING) { _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - } else if (_vtol_type->get_mode() == FIXED_WING) { + } else if (_vtol_type->get_mode() == mode::FIXED_WING) { _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; - } else if (_vtol_type->get_mode() == TRANSITION_TO_MC) { + } else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) { /* We want to make sure that a mode change (manual>auto) during the back transition * doesn't result in an unsafe state. This prevents the instant fall back to * fixed-wing on the switch from manual to auto */ @@ -631,7 +631,7 @@ void VtolAttitudeControl::task_main() } // check in which mode we are in and call mode specific functions - if (_vtol_type->get_mode() == ROTARY_WING) { + if (_vtol_type->get_mode() == mode::ROTARY_WING) { mc_virtual_att_sp_poll(); @@ -643,7 +643,7 @@ void VtolAttitudeControl::task_main() // got data from mc attitude controller _vtol_type->update_mc_state(); - } else if (_vtol_type->get_mode() == FIXED_WING) { + } else if (_vtol_type->get_mode() == mode::FIXED_WING) { fw_virtual_att_sp_poll(); @@ -654,7 +654,7 @@ void VtolAttitudeControl::task_main() _vtol_type->update_fw_state(); - } else if (_vtol_type->get_mode() == TRANSITION_TO_MC || _vtol_type->get_mode() == TRANSITION_TO_FW) { + } else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC || _vtol_type->get_mode() == mode::TRANSITION_TO_FW) { mc_virtual_att_sp_poll(); fw_virtual_att_sp_poll(); @@ -662,7 +662,7 @@ void VtolAttitudeControl::task_main() // vehicle is doing a transition _vtol_vehicle_status.vtol_in_trans_mode = true; _vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition - _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW); + _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == mode::TRANSITION_TO_FW); _vtol_type->update_transition_state(); } diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 274c8caf93c7..475dddfc4a0e 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -48,7 +48,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _attc(att_controller), - _vtol_mode(ROTARY_WING) + _vtol_mode(mode::ROTARY_WING) { _v_att = _attc->get_att(); _v_att_sp = _attc->get_att_sp(); @@ -123,8 +123,8 @@ void VtolType::update_mc_state() flag_idle_mc = set_idle_mc(); } - if (_motor_state != ENABLED) { - _motor_state = VtolType::set_motor_state(_motor_state, ENABLED); + if (_motor_state != motor_state::ENABLED) { + _motor_state = VtolType::set_motor_state(_motor_state, motor_state::ENABLED); } // copy virtual attitude setpoint to real attitude setpoint @@ -142,8 +142,8 @@ void VtolType::update_fw_state() flag_idle_mc = !set_idle_fw(); } - if (_motor_state != DISABLED) { - _motor_state = VtolType::set_motor_state(_motor_state, DISABLED); + if (_motor_state != motor_state::DISABLED) { + _motor_state = VtolType::set_motor_state(_motor_state, motor_state::DISABLED); } // copy virtual attitude setpoint to real attitude setpoint @@ -323,10 +323,10 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot } switch (next_state) { - case ENABLED: + case motor_state::ENABLED: break; - case DISABLED: + case motor_state::DISABLED: for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { pwm_values.values[i] = _disarmed_pwm_values.values[i]; @@ -335,7 +335,7 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot break; - case IDLE: + case motor_state::IDLE: for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { @@ -345,7 +345,7 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot break; - case VALUE: + case motor_state::VALUE: for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { pwm_values.values[i] = value; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index e73d9cb999c2..bcae0faf7d82 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -74,14 +74,14 @@ struct Params { }; // Has to match 1:1 msg/vtol_vehicle_status.msg -enum mode { +enum class mode { TRANSITION_TO_FW = 1, TRANSITION_TO_MC = 2, ROTARY_WING = 3, FIXED_WING = 4 }; -enum vtol_type { +enum class vtol_type { TAILSITTER = 0, TILTROTOR, STANDARD @@ -91,7 +91,7 @@ enum vtol_type { // e.g. if we need to shut off some motors after transitioning to fixed wing mode // we can individually disable them while others might still need to be enabled to produce thrust. // we can select the target motors via VT_FW_MOT_OFFID -enum motor_state { +enum class motor_state { ENABLED = 0, // motor max pwm will be set to the standard max pwm value DISABLED, // motor max pwm will be set to a value that shuts the motor off IDLE, // motor max pwm will be set to VT_IDLE_PWM_MC @@ -101,7 +101,7 @@ enum motor_state { /** * @brief Used to specify if min or max pwm values should be altered */ -enum pwm_limit_type { +enum class pwm_limit_type { TYPE_MINIMUM = 0, TYPE_MAXIMUM };