fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 02:09:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tiltrotor:

Browse Source 
- added open-loop transition time for airspeed-less flying
- added ramping down the back motors during forwards transition
This commit is contained in:
Andreas Antener 2016-12-30 15:47:46 +01:00 committed by Daniel Agar
parent 35740b0b59
commit c416fc3fa0
5 changed files with 87 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -35,6 +35,7 @@
* @file tiltrotor.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author Andreas Antener <andreas@uaventure.com>
*
*/
@ -68,6 +69,7 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
_params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
_params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFFID");
_params_handles_tiltrotor.airspeed_mode = param_find("FW_ARSP_MODE");
}
Tiltrotor::~Tiltrotor()
@ -128,6 +130,10 @@ Tiltrotor::parameters_update()
if (_params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f) {
_params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f;
}
/* airspeed mode */
param_get(_params_handles_tiltrotor.airspeed_mode, &l);
_params_tiltrotor.airspeed_mode = math::constrain(l, 0, 2);
}
int Tiltrotor::get_motor_off_channels(int channels)
@ -201,17 +207,28 @@ void Tiltrotor::update_vtol_state()
case FW_MODE:
break;
case TRANSITION_FRONT_P1:
case TRANSITION_FRONT_P1: {
// allow switch if we are not armed for the sake of bench testing
bool transition_to_p2 = can_transition_on_ground();
// check if we have reached airspeed to switch to fw mode
// also allow switch if we are not armed for the sake of bench testing
if (_airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_trans || can_transition_on_ground()) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
// check if we have reached airspeed to switch to fw mode
transition_to_p2 |= _params_tiltrotor.airspeed_mode != control_state_s::AIRSPD_MODE_DISABLED &&
_airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_trans &&
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params->front_trans_time_min * 1e6f);
// check if airspeed is invalid and transition by time
transition_to_p2 |= _params_tiltrotor.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED &&
_tilt_control > _params_tiltrotor.tilt_transition &&
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params->front_trans_time_openloop * 1e6f);
if (transition_to_p2) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
}
break;
}
break;
case TRANSITION_FRONT_P2:
// if the rotors have been tilted completely we switch to fw mode
@ -290,6 +307,8 @@ void Tiltrotor::update_fw_state()
void Tiltrotor::update_transition_state()
{
VtolType::update_transition_state();
if (!_flag_was_in_trans_mode) {
// save desired heading for transition and last thrust value
_flag_was_in_trans_mode = true;
@ -308,19 +327,25 @@ void Tiltrotor::update_transition_state()
&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f);
}
// do blending of mc and fw controls
if (_airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) {
_mc_roll_weight = 0.0f;
bool use_airspeed = _params_tiltrotor.airspeed_mode != control_state_s::AIRSPD_MODE_DISABLED;
} else {
// at low speeds give full weight to mc
_mc_roll_weight = 1.0f;
}
// disable mc yaw control once the plane has picked up speed
// at low speeds give full weight to MC
_mc_roll_weight = 1.0f;
_mc_yaw_weight = 1.0f;
if (_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
// reduce MC controls once the plane has picked up speed
if (use_airspeed && _airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
}
if (use_airspeed && _airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) {
_mc_roll_weight = 0.0f;
}
// without airspeed wait until half of open-loop time has passed
if (!use_airspeed
&& (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params->front_trans_time_openloop * 1e6f) / 2) {
_mc_roll_weight = 0.0f;
_mc_yaw_weight = 0.0f;
}
@ -331,7 +356,16 @@ void Tiltrotor::update_transition_state()
_tilt_control = _params_tiltrotor.tilt_transition +
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(
&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
_mc_roll_weight = 0.0f;
_mc_yaw_weight = 0.0f;
// ramp down rear motors (setting MAX_PWM down scales the given output into the new range)
int rear_value = (1.0f - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
(_params_tiltrotor.front_trans_dur_p2 *
1000000.0f)) * (float)(PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + (float)PWM_DEFAULT_MIN;
set_rear_motor_state(VALUE, rear_value);
_thrust_transition = _mc_virtual_att_sp->thrust;
@ -395,12 +429,12 @@ void Tiltrotor::fill_actuator_outputs()
}
_actuators_out_1->timestamp = _actuators_fw_in->timestamp;
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
* (1 - _mc_roll_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
* (1 - _mc_yaw_weight); // yaw
(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim);
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] =
_actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw
_actuators_out_1->control[4] = _tilt_control;
}
@ -409,7 +443,7 @@ void Tiltrotor::fill_actuator_outputs()
* Set state of rear motors.
*/
void Tiltrotor::set_rear_motor_state(rear_motor_state state)
void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value)
{
int pwm_value = PWM_DEFAULT_MAX;
@ -429,6 +463,11 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state)
pwm_value = _params->idle_pwm_mc;
_rear_motors = IDLE;
break;
case VALUE:
pwm_value = value;
_rear_motors = VALUE;
break;
}
int ret;
@ -457,7 +496,9 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state)
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values);
if (ret != OK) {PX4_WARN("failed setting max values");}
if (ret != OK) {
PX4_WARN("failed setting max values");
}
px4_close(fd);
}

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

@ -72,6 +72,7 @@ private:
int elevons_mc_lock; /**< lock elevons in multicopter mode */
float front_trans_dur_p2;
int fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */
int airspeed_mode;
} _params_tiltrotor;
struct {
@ -85,6 +86,7 @@ private:
param_t elevons_mc_lock;
param_t front_trans_dur_p2;
param_t fw_motors_off;
param_t airspeed_mode;
} _params_handles_tiltrotor;
enum vtol_mode {
@ -103,7 +105,8 @@ private:
enum rear_motor_state {
ENABLED = 0,
DISABLED,
IDLE
IDLE,
VALUE
} _rear_motors;
struct {
@ -128,7 +131,7 @@ private:
/**
* Adjust the state of the rear motors. In fw mode they shouldn't spin.
*/
void set_rear_motor_state(rear_motor_state state);
void set_rear_motor_state(rear_motor_state state, int value = 0);
/**
* Update parameters.

12
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -132,6 +132,8 @@ VtolAttitudeControl::VtolAttitudeControl() :
_params_handles.vtol_type = param_find("VT_TYPE");
_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
_params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT");
_params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
_params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
/* fetch initial parameter values */
parameters_update();
@ -560,6 +562,16 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.fw_min_alt, &v);
_params.fw_min_alt = v;
param_get(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop);
param_get(_params_handles.front_trans_time_min, &_params.front_trans_time_min);
/*
* Minimum transition time can be maximum 90 percent of the open loop transition time,
* anything else makes no sense and can potentially lead to numerical problems.
*/
_params.front_trans_time_min = math::min(_params.front_trans_time_openloop * 0.9f,
_params.front_trans_time_min);
// update the parameters of the instances of base VtolType
if (_vtol_type != nullptr) {

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

@ -210,6 +210,8 @@ private:
param_t vtol_type;
param_t elevons_mc_lock;
param_t fw_min_alt;
param_t front_trans_time_openloop;
param_t front_trans_time_min;
} _params_handles;
/* for multicopters it is usual to have a non-zero idle speed of the engines

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

@ -60,6 +60,8 @@ struct Params {
int vtol_type;
int elevons_mc_lock; // lock elevons in multicopter mode
float fw_min_alt; // minimum relative altitude for FW mode (QuadChute)
float front_trans_time_openloop;
float front_trans_time_min;
};
// Has to match 1:1 msg/vtol_vehicle_status.msg