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

vtol_att_control vtol_type enum -> enum class

This commit is contained in:
Daniel Agar 2019-05-29 23:51:39 -04:00
parent d13dfdcd24
commit a10b1afb54
4 changed files with 57 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -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;

4
src/modules/vtol_att_control/tailsitter.h
View File

@ -43,7 +43,7 @@
#define TAILSITTER_H
#include "vtol_type.h"
#include <perf/perf_counter.h> /** is it necsacery? **/
#include <parameters/param.h>
#include <drivers/drv_hrt.h>
#include <matrix/matrix/math.hpp>
@ -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 */

58
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -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;

2
src/modules/vtol_att_control/tiltrotor.h
View File

@ -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 */