mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-30 02:53:34 +08:00
fix code style of vtol files
This commit is contained in:
Roman
@@ -44,10 +44,10 @@
|
||||
#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition
|
||||
|
||||
Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
|
||||
VtolType(attc),
|
||||
_rear_motors(ENABLED),
|
||||
_tilt_control(0.0f),
|
||||
_min_front_trans_dur(0.5f)
|
||||
VtolType(attc),
|
||||
_rear_motors(ENABLED),
|
||||
_tilt_control(0.0f),
|
||||
_min_front_trans_dur(0.5f)
|
||||
{
|
||||
_vtol_schedule.flight_mode = MC_MODE;
|
||||
_vtol_schedule.transition_start = 0;
|
||||
@@ -86,11 +86,11 @@ Tiltrotor::parameters_update()
|
||||
|
||||
/* vtol duration of a front transition */
|
||||
param_get(_params_handles_tiltrotor.front_trans_dur, &v);
|
||||
_params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f);
|
||||
_params_tiltrotor.front_trans_dur = math::constrain(v, 1.0f, 5.0f);
|
||||
|
||||
/* vtol duration of a back transition */
|
||||
param_get(_params_handles_tiltrotor.back_trans_dur, &v);
|
||||
_params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f);
|
||||
_params_tiltrotor.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
|
||||
|
||||
/* vtol tilt mechanism position in mc mode */
|
||||
param_get(_params_handles_tiltrotor.tilt_mc, &v);
|
||||
@@ -123,22 +123,26 @@ Tiltrotor::parameters_update()
|
||||
/* avoid parameters which will lead to zero division in the transition code */
|
||||
_params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur);
|
||||
|
||||
if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) {
|
||||
if (_params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f) {
|
||||
_params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int Tiltrotor::get_motor_off_channels(int channels) {
|
||||
int Tiltrotor::get_motor_off_channels(int channels)
|
||||
{
|
||||
int channel_bitmap = 0;
|
||||
|
||||
|
||||
int channel;
|
||||
|
||||
for (int i = 0; i < _params->vtol_motor_count; ++i) {
|
||||
channel = channels % 10;
|
||||
|
||||
if (channel == 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
channel_bitmap |= 1 << channel;
|
||||
channels = channels / 10;
|
||||
}
|
||||
@@ -148,82 +152,98 @@ int Tiltrotor::get_motor_off_channels(int channels) {
|
||||
|
||||
void Tiltrotor::update_vtol_state()
|
||||
{
|
||||
parameters_update();
|
||||
parameters_update();
|
||||
|
||||
/* simple logic using a two way switch to perform transitions.
|
||||
/* simple logic using a two way switch to perform transitions.
|
||||
* after flipping the switch the vehicle will start tilting rotors, picking up
|
||||
* forward speed. After the vehicle has picked up enough speed the rotors are tilted
|
||||
* forward completely. For the backtransition the motors simply rotate back.
|
||||
*/
|
||||
*/
|
||||
|
||||
if (!_attc->is_fixed_wing_requested()) {
|
||||
|
||||
// plane is in multicopter mode
|
||||
switch(_vtol_schedule.flight_mode) {
|
||||
case MC_MODE:
|
||||
break;
|
||||
case FW_MODE:
|
||||
_vtol_schedule.flight_mode = TRANSITION_BACK;
|
||||
_vtol_schedule.transition_start = hrt_absolute_time();
|
||||
break;
|
||||
case TRANSITION_FRONT_P1:
|
||||
// failsafe into multicopter mode
|
||||
switch (_vtol_schedule.flight_mode) {
|
||||
case MC_MODE:
|
||||
break;
|
||||
|
||||
case FW_MODE:
|
||||
_vtol_schedule.flight_mode = TRANSITION_BACK;
|
||||
_vtol_schedule.transition_start = hrt_absolute_time();
|
||||
break;
|
||||
|
||||
case TRANSITION_FRONT_P1:
|
||||
// failsafe into multicopter mode
|
||||
_vtol_schedule.flight_mode = MC_MODE;
|
||||
break;
|
||||
|
||||
case TRANSITION_FRONT_P2:
|
||||
// failsafe into multicopter mode
|
||||
_vtol_schedule.flight_mode = MC_MODE;
|
||||
break;
|
||||
|
||||
case TRANSITION_BACK:
|
||||
if (_tilt_control <= _params_tiltrotor.tilt_mc) {
|
||||
_vtol_schedule.flight_mode = MC_MODE;
|
||||
break;
|
||||
case TRANSITION_FRONT_P2:
|
||||
// failsafe into multicopter mode
|
||||
_vtol_schedule.flight_mode = MC_MODE;
|
||||
break;
|
||||
case TRANSITION_BACK:
|
||||
if (_tilt_control <= _params_tiltrotor.tilt_mc) {
|
||||
_vtol_schedule.flight_mode = MC_MODE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
switch(_vtol_schedule.flight_mode) {
|
||||
case MC_MODE:
|
||||
// initialise a front transition
|
||||
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
|
||||
switch (_vtol_schedule.flight_mode) {
|
||||
case MC_MODE:
|
||||
// initialise a front transition
|
||||
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
|
||||
_vtol_schedule.transition_start = hrt_absolute_time();
|
||||
break;
|
||||
|
||||
case FW_MODE:
|
||||
break;
|
||||
|
||||
case TRANSITION_FRONT_P1:
|
||||
|
||||
// check if we have reached airspeed to switch to fw mode
|
||||
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
|
||||
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
|
||||
_vtol_schedule.transition_start = hrt_absolute_time();
|
||||
break;
|
||||
case FW_MODE:
|
||||
break;
|
||||
case TRANSITION_FRONT_P1:
|
||||
// check if we have reached airspeed to switch to fw mode
|
||||
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
|
||||
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
|
||||
_vtol_schedule.transition_start = hrt_absolute_time();
|
||||
}
|
||||
break;
|
||||
case 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;
|
||||
_tilt_control = _params_tiltrotor.tilt_fw;
|
||||
}
|
||||
break;
|
||||
case TRANSITION_BACK:
|
||||
// failsafe into fixed wing mode
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 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;
|
||||
break;
|
||||
_tilt_control = _params_tiltrotor.tilt_fw;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case TRANSITION_BACK:
|
||||
// failsafe into fixed wing mode
|
||||
_vtol_schedule.flight_mode = FW_MODE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// map tiltrotor specific control phases to simple control modes
|
||||
switch(_vtol_schedule.flight_mode) {
|
||||
case MC_MODE:
|
||||
_vtol_mode = ROTARY_WING;
|
||||
break;
|
||||
case FW_MODE:
|
||||
_vtol_mode = FIXED_WING;
|
||||
break;
|
||||
case TRANSITION_FRONT_P1:
|
||||
case TRANSITION_FRONT_P2:
|
||||
case TRANSITION_BACK:
|
||||
_vtol_mode = TRANSITION;
|
||||
break;
|
||||
switch (_vtol_schedule.flight_mode) {
|
||||
case MC_MODE:
|
||||
_vtol_mode = ROTARY_WING;
|
||||
break;
|
||||
|
||||
case FW_MODE:
|
||||
_vtol_mode = FIXED_WING;
|
||||
break;
|
||||
|
||||
case TRANSITION_FRONT_P1:
|
||||
case TRANSITION_FRONT_P2:
|
||||
case TRANSITION_BACK:
|
||||
_vtol_mode = TRANSITION;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -248,7 +268,7 @@ void Tiltrotor::update_mc_state()
|
||||
_mc_yaw_weight = 1.0f;
|
||||
}
|
||||
|
||||
void Tiltrotor::update_fw_state()
|
||||
void Tiltrotor::update_fw_state()
|
||||
{
|
||||
// make sure motors are tilted forward
|
||||
_tilt_control = _params_tiltrotor.tilt_fw;
|
||||
@@ -276,15 +296,18 @@ void Tiltrotor::update_transition_state()
|
||||
if (_rear_motors != ENABLED) {
|
||||
set_rear_motor_state(ENABLED);
|
||||
}
|
||||
|
||||
// tilt rotors forward up to certain angle
|
||||
if (_tilt_control <= _params_tiltrotor.tilt_transition ) {
|
||||
if (_tilt_control <= _params_tiltrotor.tilt_transition) {
|
||||
_tilt_control = _params_tiltrotor.tilt_mc +
|
||||
fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f);
|
||||
fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(
|
||||
&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f);
|
||||
}
|
||||
|
||||
// do blending of mc and fw controls
|
||||
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) {
|
||||
_mc_roll_weight = 0.0f;
|
||||
|
||||
} else {
|
||||
// at low speeds give full weight to mc
|
||||
_mc_roll_weight = 1.0f;
|
||||
@@ -292,6 +315,7 @@ void Tiltrotor::update_transition_state()
|
||||
|
||||
// disable mc yaw control once the plane has picked up speed
|
||||
_mc_yaw_weight = 1.0f;
|
||||
|
||||
if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
|
||||
_mc_yaw_weight = 0.0f;
|
||||
}
|
||||
@@ -299,8 +323,10 @@ void Tiltrotor::update_transition_state()
|
||||
} else if (_vtol_schedule.flight_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) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
|
||||
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;
|
||||
|
||||
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
|
||||
if (_rear_motors != IDLE) {
|
||||
set_rear_motor_state(IDLE);
|
||||
@@ -310,10 +336,12 @@ void Tiltrotor::update_transition_state()
|
||||
set_idle_mc();
|
||||
flag_idle_mc = true;
|
||||
}
|
||||
|
||||
// tilt rotors back
|
||||
if (_tilt_control > _params_tiltrotor.tilt_mc) {
|
||||
_tilt_control = _params_tiltrotor.tilt_fw -
|
||||
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f);
|
||||
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(
|
||||
&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f);
|
||||
}
|
||||
|
||||
// set zero throttle for backtransition otherwise unwanted moments will be created
|
||||
@@ -337,19 +365,28 @@ void Tiltrotor::update_external_state()
|
||||
*/
|
||||
void Tiltrotor::fill_actuator_outputs()
|
||||
{
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
|
||||
* _mc_roll_weight;
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
|
||||
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
|
||||
_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) {
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
|
||||
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
|
||||
|
||||
} else {
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
|
||||
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
|
||||
}
|
||||
|
||||
_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_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_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_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_out_1->control[4] = _tilt_control;
|
||||
}
|
||||
|
||||
@@ -358,23 +395,26 @@ 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 pwm_value = PWM_DEFAULT_MAX;
|
||||
|
||||
// map desired rear rotor state to max allowed pwm signal
|
||||
switch (state) {
|
||||
case ENABLED:
|
||||
pwm_value = PWM_DEFAULT_MAX;
|
||||
_rear_motors = ENABLED;
|
||||
break;
|
||||
case DISABLED:
|
||||
pwm_value = PWM_LOWEST_MAX;
|
||||
_rear_motors = DISABLED;
|
||||
break;
|
||||
case IDLE:
|
||||
pwm_value = _params->idle_pwm_mc;
|
||||
_rear_motors = IDLE;
|
||||
break;
|
||||
case ENABLED:
|
||||
pwm_value = PWM_DEFAULT_MAX;
|
||||
_rear_motors = ENABLED;
|
||||
break;
|
||||
|
||||
case DISABLED:
|
||||
pwm_value = PWM_LOWEST_MAX;
|
||||
_rear_motors = DISABLED;
|
||||
break;
|
||||
|
||||
case IDLE:
|
||||
pwm_value = _params->idle_pwm_mc;
|
||||
_rear_motors = IDLE;
|
||||
break;
|
||||
}
|
||||
|
||||
int ret;
|
||||
@@ -391,9 +431,11 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) {
|
||||
for (int i = 0; i < _params->vtol_motor_count; i++) {
|
||||
if (is_motor_off_channel(i)) {
|
||||
pwm_values.values[i] = pwm_value;
|
||||
|
||||
} else {
|
||||
pwm_values.values[i] = PWM_DEFAULT_MAX;
|
||||
}
|
||||
|
||||
pwm_values.channel_count = _params->vtol_motor_count;
|
||||
}
|
||||
|
||||
@@ -404,6 +446,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) {
|
||||
px4_close(fd);
|
||||
}
|
||||
|
||||
bool Tiltrotor::is_motor_off_channel(const int channel) {
|
||||
bool Tiltrotor::is_motor_off_channel(const int channel)
|
||||
{
|
||||
return (_params_tiltrotor.fw_motors_off >> channel) & 1;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user