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

vtol_att avoid unnecessary work and delete unused

Browse Source
This commit is contained in:
Daniel Agar
2018-01-02 01:35:57 +01:00
committed by Lorenz Meier
parent d3a220f807
commit 66f614435f
12 changed files with 178 additions and 565 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/tailsitter.cpp
+1 -64
View File
@@ -50,13 +50,10 @@
Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
VtolType(attc),
_airspeed_tot(0.0f),
_min_front_trans_dur(0.5f),
_thrust_transition_start(0.0f),
_yaw_transition(0.0f),
_pitch_transition_start(0.0f),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
_pitch_transition_start(0.0f)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
@@ -133,7 +130,6 @@ void Tailsitter::update_vtol_state()
if (!_attc->is_fixed_wing_requested()) {
switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
case MC_MODE:
break;
@@ -334,9 +330,6 @@ void Tailsitter::update_transition_state()
}
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
@@ -358,58 +351,6 @@ void Tailsitter::waiting_on_tecs()
_v_att_sp->thrust = _thrust_transition;
}
void Tailsitter::calc_tot_airspeed()
{
float airspeed = math::max(1.0f, _airspeed->indicated_airspeed_m_s); // prevent numerical drama
// calculate momentary power of one engine
float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count;
P = math::constrain(P, 1.0f, _params->power_max);
// calculate prop efficiency
float power_factor = 1.0f - P * _params->prop_eff / _params->power_max;
float eta = (1.0f / (1 + expf(-0.4f * power_factor * airspeed)) - 0.5f) * 2.0f;
eta = math::constrain(eta, 0.001f, 1.0f); // live on the safe side
// calculate induced airspeed by propeller
float v_ind = (airspeed / eta - airspeed) * 2.0f;
// calculate total airspeed
float airspeed_raw = airspeed + v_ind;
// apply low-pass filter
_airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
}
void Tailsitter::scale_mc_output()
{
// scale around tuning airspeed
float airspeed;
calc_tot_airspeed(); // estimate air velocity seen by elevons
// if airspeed is not updating, we assume the normal average speed
if (bool nonfinite = !PX4_ISFINITE(_airspeed->indicated_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
airspeed = _params->mc_airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed_tot;
airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max);
}
_vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min :
airspeed);
_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling,
-1.0f, 1.0f);
}
void Tailsitter::update_mc_state()
{
VtolType::update_mc_state();
@@ -504,9 +445,5 @@ void Tailsitter::fill_actuator_outputs()
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
break;
case EXTERNAL:
// not yet implemented, we are switching brute force at the moment
break;
}
}