fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 08:01:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

- use index definitions to access actuator controls struct for better readability

Browse Source 
- defined vector for mc_att_ctrl_weights
- more cleanup
This commit is contained in:
tumbili
2015-08-12 19:34:44 +02:00
parent 17a92b51c7
commit b3613dea83
5 changed files with 134 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/standard.cpp
+39 -19
View File
@@ -46,12 +46,15 @@ Standard::Standard(VtolAttitudeControl *attc) :
VtolType(attc),
_flag_enable_mc_motors(true),
_pusher_throttle(0.0f),
_mc_att_ctl_weight(1.0f),
_airspeed_trans_blend_margin(0.0f)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
@@ -107,7 +110,9 @@ void Standard::update_vtol_state()
if (_vtol_schedule.flight_mode == MC_MODE) {
// in mc mode
_vtol_schedule.flight_mode = MC_MODE;
_mc_att_ctl_weight = 1.0f;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == FW_MODE) {
// transition to mc mode
@@ -118,7 +123,9 @@ void Standard::update_vtol_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// failsafe back to mc mode
_vtol_schedule.flight_mode = MC_MODE;
_mc_att_ctl_weight = 1.0f;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// keep transitioning to mc mode
@@ -138,7 +145,9 @@ void Standard::update_vtol_state()
} else if (_vtol_schedule.flight_mode == FW_MODE) {
// in fw mode
_vtol_schedule.flight_mode = FW_MODE;
_mc_att_ctl_weight = 0.0f;
_mc_roll_weight = 0.0f;
_mc_pitch_weight = 0.0f;
_mc_yaw_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
@@ -184,18 +193,28 @@ void Standard::update_transition_state()
// do blending of mc and fw controls if a blending airspeed has been provided
if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) {
_mc_att_ctl_weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
} else {
// at low speeds give full weight to mc
_mc_att_ctl_weight = 1.0f;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
}
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// continually increase mc attitude control as we transition back to mc mode
if (_params_standard.back_trans_dur > 0.0f) {
_mc_att_ctl_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
} else {
_mc_att_ctl_weight = 1.0f;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
}
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
@@ -206,7 +225,9 @@ void Standard::update_transition_state()
}
}
_mc_att_ctl_weight = math::constrain(_mc_att_ctl_weight, 0.0f, 1.0f);
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
}
void Standard::update_mc_state()
@@ -235,24 +256,23 @@ void Standard::update_external_state()
void Standard::fill_actuator_outputs()
{
/* multirotor controls */
_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _mc_att_ctl_weight; // roll
_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _mc_att_ctl_weight; // pitch
_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _mc_att_ctl_weight; // yaw
_actuators_out_0->control[3] = _actuators_mc_in->control[3]; // throttle
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; // roll
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; // yaw
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
/* fixed wing controls */
const float fw_att_ctl_weight = 1.0f - _mc_att_ctl_weight;
_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * fw_att_ctl_weight; //roll
_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim) * fw_att_ctl_weight; //pitch
_actuators_out_1->control[2] = _actuators_fw_in->control[2] * fw_att_ctl_weight; // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //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); //pitch
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw
// set the fixed wing throttle control
if (_vtol_schedule.flight_mode == FW_MODE) {
// take the throttle value commanded by the fw controller
_actuators_out_1->control[3] = _actuators_fw_in->control[3];
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
} else {
// otherwise we may be ramping up the throttle during the transition to fw mode
_actuators_out_1->control[3] = _pusher_throttle;
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
}
}