mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-26 00:53:34 +08:00
fix code style of vtol files
This commit is contained in:
Roman
@@ -31,21 +31,21 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file tailsitter.cpp
|
||||
*
|
||||
* @author Roman Bapst <bapstroman@gmail.com>
|
||||
*
|
||||
*/
|
||||
/**
|
||||
* @file tailsitter.cpp
|
||||
*
|
||||
* @author Roman Bapst <bapstroman@gmail.com>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "tailsitter.h"
|
||||
#include "vtol_att_control_main.h"
|
||||
#include "tailsitter.h"
|
||||
#include "vtol_att_control_main.h"
|
||||
|
||||
Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) :
|
||||
VtolType(att_controller),
|
||||
_airspeed_tot(0),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
|
||||
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
|
||||
Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) :
|
||||
VtolType(att_controller),
|
||||
_airspeed_tot(0),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
|
||||
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
|
||||
{
|
||||
|
||||
}
|
||||
@@ -60,6 +60,7 @@ void Tailsitter::update_vtol_state()
|
||||
// simply switch between the two modes
|
||||
if (!_attc->is_fixed_wing_requested()) {
|
||||
_vtol_mode = ROTARY_WING;
|
||||
|
||||
} else {
|
||||
_vtol_mode = FIXED_WING;
|
||||
}
|
||||
@@ -67,7 +68,7 @@ void Tailsitter::update_vtol_state()
|
||||
|
||||
void Tailsitter::update_mc_state()
|
||||
{
|
||||
if (!flag_idle_mc) {
|
||||
if (!flag_idle_mc) {
|
||||
set_idle_mc();
|
||||
flag_idle_mc = true;
|
||||
}
|
||||
@@ -91,18 +92,18 @@ void Tailsitter::update_external_state()
|
||||
|
||||
}
|
||||
|
||||
void Tailsitter::calc_tot_airspeed()
|
||||
{
|
||||
void Tailsitter::calc_tot_airspeed()
|
||||
{
|
||||
float airspeed = math::max(1.0f, _airspeed->true_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);
|
||||
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
|
||||
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;
|
||||
float v_ind = (airspeed / eta - airspeed) * 2.0f;
|
||||
// calculate total airspeed
|
||||
float airspeed_raw = airspeed + v_ind;
|
||||
// apply low-pass filter
|
||||
@@ -115,16 +116,19 @@ 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->true_airspeed_m_s) ||
|
||||
hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
|
||||
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);
|
||||
airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max);
|
||||
}
|
||||
|
||||
_vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging
|
||||
@@ -135,8 +139,10 @@ Tailsitter::scale_mc_output()
|
||||
*
|
||||
* 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);
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -144,37 +150,50 @@ Tailsitter::scale_mc_output()
|
||||
*/
|
||||
void Tailsitter::fill_actuator_outputs()
|
||||
{
|
||||
switch(_vtol_mode) {
|
||||
case ROTARY_WING:
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
|
||||
switch (_vtol_mode) {
|
||||
case ROTARY_WING:
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
|
||||
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
|
||||
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
|
||||
|
||||
if (_params->elevons_mc_lock == 1) {
|
||||
_actuators_out_1->control[0] = 0;
|
||||
_actuators_out_1->control[1] = 0;
|
||||
} else {
|
||||
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
|
||||
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon
|
||||
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
|
||||
}
|
||||
break;
|
||||
case FIXED_WING:
|
||||
// in fixed wing mode we use engines only for providing thrust, no moments are generated
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
|
||||
if (_params->elevons_mc_lock == 1) {
|
||||
_actuators_out_1->control[0] = 0;
|
||||
_actuators_out_1->control[1] = 0;
|
||||
|
||||
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon
|
||||
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon
|
||||
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw
|
||||
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
|
||||
break;
|
||||
case TRANSITION:
|
||||
case EXTERNAL:
|
||||
// not yet implemented, we are switching brute force at the moment
|
||||
break;
|
||||
} else {
|
||||
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
|
||||
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
|
||||
_actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon
|
||||
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
|
||||
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case FIXED_WING:
|
||||
// in fixed wing mode we use engines only for providing thrust, no moments are generated
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
|
||||
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
|
||||
_actuators_fw_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]; // roll elevon
|
||||
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
|
||||
_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon
|
||||
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] =
|
||||
_actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw
|
||||
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
|
||||
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
|
||||
break;
|
||||
|
||||
case TRANSITION:
|
||||
case EXTERNAL:
|
||||
// not yet implemented, we are switching brute force at the moment
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user