zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

fw_att_control: move to px4::params

sbg
Daniel Agar 5 years ago committed by GitHub
parent
71fbe58e20
commit
47dd312b57
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 195 additions and 390 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 0
      Tools/parameter_update.py
  2. 2
      boards/px4/fmu-v5/stackcheck.cmake
  3. 346
      src/modules/fw_att_control/FixedwingAttitudeControl.cpp
  4. 237
      src/modules/fw_att_control/FixedwingAttitudeControl.hpp

0
Tools/parameter_update.py
Unescape View File

2
boards/px4/fmu-v5/stackcheck.cmake
Unescape View File

@ -20,7 +20,7 @@ px4_add_board(
DRIVERS DRIVERS
adc adc
barometer/ms5611 barometer/ms5611
batt_smbus #batt_smbus
camera_capture camera_capture
camera_trigger camera_trigger
differential_pressure # all available differential pressure drivers differential_pressure # all available differential pressure drivers

346
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
Unescape View File

@ -36,6 +36,9 @@
#include <vtol_att_control/vtol_type.h> #include <vtol_att_control/vtol_type.h>
using namespace time_literals; using namespace time_literals;
using math::constrain;
using math::gradual;
using math::radians;
/** /**
* Fixedwing attitude control app start / stop handling function * Fixedwing attitude control app start / stop handling function
@ -45,87 +48,21 @@ using namespace time_literals;
extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
FixedwingAttitudeControl::FixedwingAttitudeControl() : FixedwingAttitudeControl::FixedwingAttitudeControl() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
_loop_perf(perf_alloc(PC_ELAPSED, "fw_att_control: cycle")) _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{ {
// check if VTOL first // check if VTOL first
vehicle_status_poll(); vehicle_status_poll();
_parameter_handles.p_tc = param_find("FW_P_TC");
_parameter_handles.p_p = param_find("FW_PR_P");
_parameter_handles.p_i = param_find("FW_PR_I");
_parameter_handles.p_ff = param_find("FW_PR_FF");
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
_parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
_parameter_handles.r_tc = param_find("FW_R_TC");
_parameter_handles.r_p = param_find("FW_RR_P");
_parameter_handles.r_i = param_find("FW_RR_I");
_parameter_handles.r_ff = param_find("FW_RR_FF");
_parameter_handles.r_integrator_max = param_find("FW_RR_IMAX");
_parameter_handles.r_rmax = param_find("FW_R_RMAX");
_parameter_handles.y_p = param_find("FW_YR_P");
_parameter_handles.y_i = param_find("FW_YR_I");
_parameter_handles.y_ff = param_find("FW_YR_FF");
_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
_parameter_handles.roll_to_yaw_ff = param_find("FW_RLL_TO_YAW_FF");
_parameter_handles.w_en = param_find("FW_W_EN");
_parameter_handles.w_p = param_find("FW_WR_P");
_parameter_handles.w_i = param_find("FW_WR_I");
_parameter_handles.w_ff = param_find("FW_WR_FF");
_parameter_handles.w_integrator_max = param_find("FW_WR_IMAX");
_parameter_handles.w_rmax = param_find("FW_W_RMAX");
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
_parameter_handles.trim_roll = param_find("TRIM_ROLL");
_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
_parameter_handles.trim_yaw = param_find("TRIM_YAW");
_parameter_handles.dtrim_roll_vmin = param_find("FW_DTRIM_R_VMIN");
_parameter_handles.dtrim_pitch_vmin = param_find("FW_DTRIM_P_VMIN");
_parameter_handles.dtrim_yaw_vmin = param_find("FW_DTRIM_Y_VMIN");
_parameter_handles.dtrim_roll_vmax = param_find("FW_DTRIM_R_VMAX");
_parameter_handles.dtrim_pitch_vmax = param_find("FW_DTRIM_P_VMAX");
_parameter_handles.dtrim_yaw_vmax = param_find("FW_DTRIM_Y_VMAX");
_parameter_handles.dtrim_roll_flaps = param_find("FW_DTRIM_R_FLPS");
_parameter_handles.dtrim_pitch_flaps = param_find("FW_DTRIM_P_FLPS");
_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
_parameter_handles.man_roll_scale = param_find("FW_MAN_R_SC");
_parameter_handles.man_pitch_scale = param_find("FW_MAN_P_SC");
_parameter_handles.man_yaw_scale = param_find("FW_MAN_Y_SC");
_parameter_handles.acro_max_x_rate = param_find("FW_ACRO_X_MAX");
_parameter_handles.acro_max_y_rate = param_find("FW_ACRO_Y_MAX");
_parameter_handles.acro_max_z_rate = param_find("FW_ACRO_Z_MAX");
_parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL");
_parameter_handles.flaps_takeoff_scale = param_find("FW_FLAPS_TO_SCL");
_parameter_handles.flaps_land_scale = param_find("FW_FLAPS_LND_SCL");
_parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL");
_parameter_handles.rattitude_thres = param_find("FW_RATT_TH");
_parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN");
_parameter_handles.airspeed_mode = param_find("FW_ARSP_MODE");
/* fetch initial parameter values */ /* fetch initial parameter values */
parameters_update(); parameters_update();
// set initial maximum body rate setpoints // set initial maximum body rate setpoints
_roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad); _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
_pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad); _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
_pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad); _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
_yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad); _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
} }
FixedwingAttitudeControl::~FixedwingAttitudeControl() FixedwingAttitudeControl::~FixedwingAttitudeControl()
@ -147,113 +84,32 @@ FixedwingAttitudeControl::init()
int int
FixedwingAttitudeControl::parameters_update() FixedwingAttitudeControl::parameters_update()
{ {
int32_t tmp = 0;
param_get(_parameter_handles.p_tc, &(_parameters.p_tc));
param_get(_parameter_handles.p_p, &(_parameters.p_p));
param_get(_parameter_handles.p_i, &(_parameters.p_i));
param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
param_get(_parameter_handles.r_tc, &(_parameters.r_tc));
param_get(_parameter_handles.r_p, &(_parameters.r_p));
param_get(_parameter_handles.r_i, &(_parameters.r_i));
param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
param_get(_parameter_handles.y_p, &(_parameters.y_p));
param_get(_parameter_handles.y_i, &(_parameters.y_i));
param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
param_get(_parameter_handles.roll_to_yaw_ff, &(_parameters.roll_to_yaw_ff));
param_get(_parameter_handles.w_en, &tmp);
_parameters.w_en = (tmp == 1);
param_get(_parameter_handles.w_p, &(_parameters.w_p));
param_get(_parameter_handles.w_i, &(_parameters.w_i));
param_get(_parameter_handles.w_ff, &(_parameters.w_ff));
param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max));
param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax));
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
param_get(_parameter_handles.dtrim_roll_vmin, &(_parameters.dtrim_roll_vmin));
param_get(_parameter_handles.dtrim_roll_vmax, &(_parameters.dtrim_roll_vmax));
param_get(_parameter_handles.dtrim_pitch_vmin, &(_parameters.dtrim_pitch_vmin));
param_get(_parameter_handles.dtrim_pitch_vmax, &(_parameters.dtrim_pitch_vmax));
param_get(_parameter_handles.dtrim_yaw_vmin, &(_parameters.dtrim_yaw_vmin));
param_get(_parameter_handles.dtrim_yaw_vmax, &(_parameters.dtrim_yaw_vmax));
param_get(_parameter_handles.dtrim_roll_flaps, &(_parameters.dtrim_roll_flaps));
param_get(_parameter_handles.dtrim_pitch_flaps, &(_parameters.dtrim_pitch_flaps));
param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
param_get(_parameter_handles.man_roll_scale, &(_parameters.man_roll_scale));
param_get(_parameter_handles.man_pitch_scale, &(_parameters.man_pitch_scale));
param_get(_parameter_handles.man_yaw_scale, &(_parameters.man_yaw_scale));
param_get(_parameter_handles.acro_max_x_rate, &(_parameters.acro_max_x_rate_rad));
param_get(_parameter_handles.acro_max_y_rate, &(_parameters.acro_max_y_rate_rad));
param_get(_parameter_handles.acro_max_z_rate, &(_parameters.acro_max_z_rate_rad));
_parameters.acro_max_x_rate_rad = math::radians(_parameters.acro_max_x_rate_rad);
_parameters.acro_max_y_rate_rad = math::radians(_parameters.acro_max_y_rate_rad);
_parameters.acro_max_z_rate_rad = math::radians(_parameters.acro_max_z_rate_rad);
param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale);
param_get(_parameter_handles.flaps_takeoff_scale, &_parameters.flaps_takeoff_scale);
param_get(_parameter_handles.flaps_land_scale, &_parameters.flaps_land_scale);
param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale);
param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres);
param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
param_get(_parameter_handles.airspeed_mode, &tmp);
_parameters.airspeed_disabled = (tmp == 1);
/* pitch control parameters */ /* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.p_tc); _pitch_ctrl.set_time_constant(_param_fw_p_tc.get());
_pitch_ctrl.set_k_p(_parameters.p_p); _pitch_ctrl.set_k_p(_param_fw_pr_p.get());
_pitch_ctrl.set_k_i(_parameters.p_i); _pitch_ctrl.set_k_i(_param_fw_pr_i.get());
_pitch_ctrl.set_k_ff(_parameters.p_ff); _pitch_ctrl.set_k_ff(_param_fw_pr_ff.get());
_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_integrator_max(_param_fw_pr_imax.get());
/* roll control parameters */ /* roll control parameters */
_roll_ctrl.set_time_constant(_parameters.r_tc); _roll_ctrl.set_time_constant(_param_fw_r_tc.get());
_roll_ctrl.set_k_p(_parameters.r_p); _roll_ctrl.set_k_p(_param_fw_rr_p.get());
_roll_ctrl.set_k_i(_parameters.r_i); _roll_ctrl.set_k_i(_param_fw_rr_i.get());
_roll_ctrl.set_k_ff(_parameters.r_ff); _roll_ctrl.set_k_ff(_param_fw_rr_ff.get());
_roll_ctrl.set_integrator_max(_parameters.r_integrator_max); _roll_ctrl.set_integrator_max(_param_fw_rr_imax.get());
/* yaw control parameters */ /* yaw control parameters */
_yaw_ctrl.set_k_p(_parameters.y_p); _yaw_ctrl.set_k_p(_param_fw_yr_p.get());
_yaw_ctrl.set_k_i(_parameters.y_i); _yaw_ctrl.set_k_i(_param_fw_yr_i.get());
_yaw_ctrl.set_k_ff(_parameters.y_ff); _yaw_ctrl.set_k_ff(_param_fw_yr_ff.get());
_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); _yaw_ctrl.set_integrator_max(_param_fw_yr_imax.get());
/* wheel control parameters */ /* wheel control parameters */
_wheel_ctrl.set_k_p(_parameters.w_p); _wheel_ctrl.set_k_p(_param_fw_wr_p.get());
_wheel_ctrl.set_k_i(_parameters.w_i); _wheel_ctrl.set_k_i(_param_fw_wr_i.get());
_wheel_ctrl.set_k_ff(_parameters.w_ff); _wheel_ctrl.set_k_ff(_param_fw_wr_ff.get());
_wheel_ctrl.set_integrator_max(_parameters.w_integrator_max); _wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get());
_wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax)); _wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get()));
return PX4_OK; return PX4_OK;
} }
@ -288,7 +144,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
// Check if we are in rattitude mode and the pilot is above the threshold on pitch // Check if we are in rattitude mode and the pilot is above the threshold on pitch
if (_vcontrol_mode.flag_control_rattitude_enabled) { if (_vcontrol_mode.flag_control_rattitude_enabled) {
if (fabsf(_manual.y) > _parameters.rattitude_thres || fabsf(_manual.x) > _parameters.rattitude_thres) { if (fabsf(_manual.y) > _param_fw_ratt_th.get() || fabsf(_manual.x) > _param_fw_ratt_th.get()) {
_vcontrol_mode.flag_control_attitude_enabled = false; _vcontrol_mode.flag_control_attitude_enabled = false;
} }
} }
@ -298,11 +154,15 @@ FixedwingAttitudeControl::vehicle_manual_poll()
if (_vcontrol_mode.flag_control_attitude_enabled) { if (_vcontrol_mode.flag_control_attitude_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs // STABILIZED mode generate the attitude setpoint from manual user inputs
_att_sp.timestamp = hrt_absolute_time();
_att_sp.roll_body = _manual.y * _parameters.man_roll_max + _parameters.rollsp_offset_rad; _att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get()) + radians(_param_fw_rsp_off.get());
_att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max, _parameters.man_roll_max); _att_sp.roll_body = constrain(_att_sp.roll_body,
_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad; -radians(_param_fw_man_r_max.get()), radians(_param_fw_man_r_max.get()));
_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
_att_sp.pitch_body = -_manual.x * radians(_param_fw_man_p_max.get()) + radians(_param_fw_psp_off.get());
_att_sp.pitch_body = constrain(_att_sp.pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
_att_sp.yaw_body = 0.0f; _att_sp.yaw_body = 0.0f;
_att_sp.thrust_body[0] = _manual.z; _att_sp.thrust_body[0] = _manual.z;
@ -310,6 +170,8 @@ FixedwingAttitudeControl::vehicle_manual_poll()
q.copyTo(_att_sp.q_d); q.copyTo(_att_sp.q_d);
_att_sp.q_d_valid = true; _att_sp.q_d_valid = true;
_att_sp.timestamp = hrt_absolute_time();
if (_attitude_sp_pub != nullptr) { if (_attitude_sp_pub != nullptr) {
/* publish the attitude rates setpoint */ /* publish the attitude rates setpoint */
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp); orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);
@ -324,19 +186,18 @@ FixedwingAttitudeControl::vehicle_manual_poll()
// RATE mode we need to generate the rate setpoint from manual user inputs // RATE mode we need to generate the rate setpoint from manual user inputs
_rates_sp.timestamp = hrt_absolute_time(); _rates_sp.timestamp = hrt_absolute_time();
_rates_sp.roll = _manual.y * _parameters.acro_max_x_rate_rad; _rates_sp.roll = _manual.y * radians(_param_fw_acro_x_max.get());
_rates_sp.pitch = -_manual.x * _parameters.acro_max_y_rate_rad; _rates_sp.pitch = -_manual.x * radians(_param_fw_acro_y_max.get());
_rates_sp.yaw = _manual.r * _parameters.acro_max_z_rate_rad; _rates_sp.yaw = _manual.r * radians(_param_fw_acro_z_max.get());
_rates_sp.thrust_body[0] = _manual.z; _rates_sp.thrust_body[0] = _manual.z;
_rate_sp_pub.publish(_rates_sp); _rate_sp_pub.publish(_rates_sp);
} else { } else {
/* manual/direct control */ /* manual/direct control */
_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _parameters.man_roll_scale + _parameters.trim_roll; _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _param_fw_man_r_sc.get() + _param_trim_roll.get();
_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _parameters.man_pitch_scale + _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
_parameters.trim_pitch; _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _parameters.man_yaw_scale + _parameters.trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
} }
} }
@ -410,9 +271,9 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
&& !_vehicle_status.aspd_use_inhibit; && !_vehicle_status.aspd_use_inhibit;
// if no airspeed measurement is available out best guess is to use the trim airspeed // if no airspeed measurement is available out best guess is to use the trim airspeed
float airspeed = _parameters.airspeed_trim; float airspeed = _param_fw_airspd_trim.get();
if (!_parameters.airspeed_disabled && airspeed_valid) { if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */ /* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s); airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s);
@ -422,7 +283,7 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
// than the minimum airspeed // than the minimum airspeed
if (_vehicle_status.is_vtol && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING if (_vehicle_status.is_vtol && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_vehicle_status.in_transition_mode) { && !_vehicle_status.in_transition_mode) {
airspeed = _parameters.airspeed_min; airspeed = _param_fw_airspd_min.get();
} }
} }
@ -433,8 +294,8 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
* *
* Forcing the scaling to this value allows reasonable handheld tests. * Forcing the scaling to this value allows reasonable handheld tests.
*/ */
const float airspeed_constrained = math::constrain(airspeed, _parameters.airspeed_min, _parameters.airspeed_max); const float airspeed_constrained = constrain(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_max.get());
_airspeed_scaling = _parameters.airspeed_trim / airspeed_constrained; _airspeed_scaling = _param_fw_airspd_trim.get() / airspeed_constrained;
return airspeed; return airspeed;
} }
@ -461,12 +322,13 @@ void FixedwingAttitudeControl::Run()
_parameter_update_sub.copy(&pupdate); _parameter_update_sub.copy(&pupdate);
// update parameters from storage // update parameters from storage
updateParams();
parameters_update(); parameters_update();
} }
/* only run controller if attitude changed */ /* only run controller if attitude changed */
static uint64_t last_run = 0; static uint64_t last_run = 0;
float deltaT = math::constrain((hrt_elapsed_time(&last_run) / 1e6f), 0.01f, 0.1f); float deltaT = constrain((hrt_elapsed_time(&last_run) / 1e6f), 0.01f, 0.1f);
last_run = hrt_absolute_time(); last_run = hrt_absolute_time();
/* get current rotation matrix and euler angles from control state quaternions */ /* get current rotation matrix and euler angles from control state quaternions */
@ -564,7 +426,7 @@ void FixedwingAttitudeControl::Run()
*/ */
float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
_global_pos.vel_e * _global_pos.vel_e); _global_pos.vel_e * _global_pos.vel_e);
float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); float gspd_scaling_trim = (_param_fw_airspd_min.get() * 0.6f);
float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
/* reset integrals where needed */ /* reset integrals where needed */
@ -605,8 +467,8 @@ void FixedwingAttitudeControl::Run()
control_input.roll_setpoint = _att_sp.roll_body; control_input.roll_setpoint = _att_sp.roll_body;
control_input.pitch_setpoint = _att_sp.pitch_body; control_input.pitch_setpoint = _att_sp.pitch_body;
control_input.yaw_setpoint = _att_sp.yaw_body; control_input.yaw_setpoint = _att_sp.yaw_body;
control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_min = _param_fw_airspd_min.get();
control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed_max = _param_fw_airspd_max.get();
control_input.airspeed = airspeed; control_input.airspeed = airspeed;
control_input.scaler = _airspeed_scaling; control_input.scaler = _airspeed_scaling;
control_input.lock_integrator = lock_integrator; control_input.lock_integrator = lock_integrator;
@ -617,46 +479,46 @@ void FixedwingAttitudeControl::Run()
if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) { if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
if (_vcontrol_mode.flag_control_attitude_enabled if (_vcontrol_mode.flag_control_attitude_enabled
|| _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { || _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); _roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get()));
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get()));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); _pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get()));
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); _yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get()));
} else { } else {
_roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad); _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
_pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad); _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
_pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad); _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
_yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad); _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
} }
} }
_flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled; _flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled;
/* bi-linear interpolation over airspeed for actuator trim scheduling */ /* bi-linear interpolation over airspeed for actuator trim scheduling */
float trim_roll = _parameters.trim_roll; float trim_roll = _param_trim_roll.get();
float trim_pitch = _parameters.trim_pitch; float trim_pitch = _param_trim_pitch.get();
float trim_yaw = _parameters.trim_yaw; float trim_yaw = _param_trim_yaw.get();
if (airspeed < _parameters.airspeed_trim) { if (airspeed < _param_fw_airspd_trim.get()) {
trim_roll += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_roll_vmin, trim_roll += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_r_vmin.get(),
0.0f); 0.0f);
trim_pitch += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_pitch_vmin, trim_pitch += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_p_vmin.get(),
0.0f); 0.0f);
trim_yaw += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_yaw_vmin, trim_yaw += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_y_vmin.get(),
0.0f); 0.0f);
} else { } else {
trim_roll += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f, trim_roll += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_parameters.dtrim_roll_vmax); _param_fw_dtrim_r_vmax.get());
trim_pitch += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f, trim_pitch += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_parameters.dtrim_pitch_vmax); _param_fw_dtrim_p_vmax.get());
trim_yaw += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f, trim_yaw += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_parameters.dtrim_yaw_vmax); _param_fw_dtrim_y_vmax.get());
} }
/* add trim increment if flaps are deployed */ /* add trim increment if flaps are deployed */
trim_roll += _flaps_applied * _parameters.dtrim_roll_flaps; trim_roll += _flaps_applied * _param_fw_dtrim_r_flps.get();
trim_pitch += _flaps_applied * _parameters.dtrim_pitch_flaps; trim_pitch += _flaps_applied * _param_fw_dtrim_p_flps.get();
/* Run attitude controllers */ /* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) { if (_vcontrol_mode.flag_control_attitude_enabled) {
@ -688,7 +550,7 @@ void FixedwingAttitudeControl::Run()
float yaw_u = 0.0f; float yaw_u = 0.0f;
if (_parameters.w_en && _att_sp.fw_control_yaw) { if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) {
yaw_u = _wheel_ctrl.control_bodyrate(control_input); yaw_u = _wheel_ctrl.control_bodyrate(control_input);
} else { } else {
@ -712,7 +574,7 @@ void FixedwingAttitudeControl::Run()
&& !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f; && !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */ /* scale effort by battery status */
if (_parameters.bat_scale_en && if (_param_fw_bat_scale_en.get() &&
_actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) { _actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
if (_battery_status_sub.updated()) { if (_battery_status_sub.updated()) {
@ -773,8 +635,8 @@ void FixedwingAttitudeControl::Run()
// Add feed-forward from roll control output to yaw control output // Add feed-forward from roll control output to yaw control output
// This can be used to counteract the adverse yaw effect when rolling the plane // This can be used to counteract the adverse yaw effect when rolling the plane
_actuators.control[actuator_controls_s::INDEX_YAW] += _parameters.roll_to_yaw_ff * math::constrain( _actuators.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get()
_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); * constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
_actuators.control[5] = _manual.aux1; _actuators.control[5] = _manual.aux1;
@ -814,21 +676,23 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
/* map flaps by default to manual if valid */ /* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_parameters.flaps_scale) > 0.01f) { && fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
flap_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale; flap_control = 0.5f * (_manual.flaps + 1.0f) * _param_fw_flaps_scl.get();
} else if (_vcontrol_mode.flag_control_auto_enabled } else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_parameters.flaps_scale) > 0.01f) { && fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
switch (_att_sp.apply_flaps) { switch (_att_sp.apply_flaps) {
case vehicle_attitude_setpoint_s::FLAPS_OFF : flap_control = 0.0f; // no flaps case vehicle_attitude_setpoint_s::FLAPS_OFF:
flap_control = 0.0f; // no flaps
break; break;
case vehicle_attitude_setpoint_s::FLAPS_LAND : flap_control = 1.0f * _parameters.flaps_scale * case vehicle_attitude_setpoint_s::FLAPS_LAND:
_parameters.flaps_land_scale; // landing flaps flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_lnd_scl.get();
break; break;
case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF : flap_control = 1.0f * _parameters.flaps_scale * case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF:
_parameters.flaps_takeoff_scale; // take-off flaps flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_to_scl.get();
break; break;
} }
} }
@ -846,13 +710,19 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
/* map flaperons by default to manual if valid */ /* map flaperons by default to manual if valid */
if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_parameters.flaperon_scale) > 0.01f) { && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _param_fw_flaperon_scl.get();
} else if (_vcontrol_mode.flag_control_auto_enabled } else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_parameters.flaperon_scale) > 0.01f) { && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
flaperon_control = (_att_sp.apply_flaps == vehicle_attitude_setpoint_s::FLAPS_LAND) ? 1.0f *
_parameters.flaperon_scale : 0.0f; if (_att_sp.apply_flaps == vehicle_attitude_setpoint_s::FLAPS_LAND) {
flaperon_control = _param_fw_flaperon_scl.get();
} else {
flaperon_control = 0.0f;
}
} }
// move the actual control value continuous with time, full flap travel in 1sec // move the actual control value continuous with time, full flap travel in 1sec

237
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
Unescape View File

@ -31,22 +31,23 @@
* *
****************************************************************************/ ****************************************************************************/
#include <px4_module.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <ecl/attitude_fw/ecl_pitch_controller.h> #include <lib/ecl/attitude_fw/ecl_pitch_controller.h>
#include <ecl/attitude_fw/ecl_roll_controller.h> #include <lib/ecl/attitude_fw/ecl_roll_controller.h>
#include <ecl/attitude_fw/ecl_wheel_controller.h> #include <lib/ecl/attitude_fw/ecl_wheel_controller.h>
#include <ecl/attitude_fw/ecl_yaw_controller.h> #include <lib/ecl/attitude_fw/ecl_yaw_controller.h>
#include <lib/ecl/geo/geo.h> #include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <matrix/math.hpp> #include <matrix/math.hpp>
#include <px4_config.h> #include <px4_config.h>
#include <px4_defines.h> #include <px4_defines.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <px4_posix.h> #include <px4_posix.h>
#include <px4_tasks.h> #include <px4_tasks.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp> #include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
@ -71,7 +72,8 @@ using matrix::Quatf;
using uORB::SubscriptionData; using uORB::SubscriptionData;
class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>, public px4::WorkItem class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>, public ModuleParams,
public px4::WorkItem
{ {
public: public:
FixedwingAttitudeControl(); FixedwingAttitudeControl();
@ -145,147 +147,80 @@ private:
bool _is_tailsitter{false}; bool _is_tailsitter{false};
struct { DEFINE_PARAMETERS(
float p_tc; (ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
float p_p; (ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
float p_i; (ParamFloat<px4::params::FW_ACRO_Z_MAX>) _param_fw_acro_z_max,
float p_ff;
float p_rmax_pos; (ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
float p_rmax_neg; (ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
float p_integrator_max; (ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
float r_tc; (ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
float r_p;
float r_i; (ParamBool<px4::params::FW_BAT_SCALE_EN>) _param_fw_bat_scale_en,
float r_ff;
float r_integrator_max; (ParamFloat<px4::params::FW_DTRIM_P_FLPS>) _param_fw_dtrim_p_flps,
float r_rmax; (ParamFloat<px4::params::FW_DTRIM_P_VMAX>) _param_fw_dtrim_p_vmax,
float y_p; (ParamFloat<px4::params::FW_DTRIM_P_VMIN>) _param_fw_dtrim_p_vmin,
float y_i; (ParamFloat<px4::params::FW_DTRIM_R_FLPS>) _param_fw_dtrim_r_flps,
float y_ff; (ParamFloat<px4::params::FW_DTRIM_R_VMAX>) _param_fw_dtrim_r_vmax,
float y_integrator_max; (ParamFloat<px4::params::FW_DTRIM_R_VMIN>) _param_fw_dtrim_r_vmin,
float roll_to_yaw_ff; (ParamFloat<px4::params::FW_DTRIM_Y_VMAX>) _param_fw_dtrim_y_vmax,
float y_rmax; (ParamFloat<px4::params::FW_DTRIM_Y_VMIN>) _param_fw_dtrim_y_vmin,
bool w_en; (ParamFloat<px4::params::FW_FLAPERON_SCL>) _param_fw_flaperon_scl,
float w_p; (ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
float w_i; (ParamFloat<px4::params::FW_FLAPS_SCL>) _param_fw_flaps_scl,
float w_ff; (ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
float w_integrator_max;
float w_rmax; (ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
(ParamFloat<px4::params::FW_MAN_P_SC>) _param_fw_man_p_sc,
float airspeed_min; (ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,
float airspeed_trim; (ParamFloat<px4::params::FW_MAN_R_SC>) _param_fw_man_r_sc,
float airspeed_max; (ParamFloat<px4::params::FW_MAN_Y_SC>) _param_fw_man_y_sc,
float trim_roll; (ParamFloat<px4::params::FW_P_RMAX_NEG>) _param_fw_p_rmax_neg,
float trim_pitch; (ParamFloat<px4::params::FW_P_RMAX_POS>) _param_fw_p_rmax_pos,
float trim_yaw; (ParamFloat<px4::params::FW_P_TC>) _param_fw_p_tc,
float dtrim_roll_vmin; (ParamFloat<px4::params::FW_PR_FF>) _param_fw_pr_ff,
float dtrim_pitch_vmin; (ParamFloat<px4::params::FW_PR_I>) _param_fw_pr_i,
float dtrim_yaw_vmin; (ParamFloat<px4::params::FW_PR_IMAX>) _param_fw_pr_imax,
float dtrim_roll_vmax; (ParamFloat<px4::params::FW_PR_P>) _param_fw_pr_p,
float dtrim_pitch_vmax; (ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
float dtrim_yaw_vmax;
float dtrim_roll_flaps; (ParamFloat<px4::params::FW_RATT_TH>) _param_fw_ratt_th,
float dtrim_pitch_flaps;
float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ (ParamFloat<px4::params::FW_R_RMAX>) _param_fw_r_rmax,
float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ (ParamFloat<px4::params::FW_R_TC>) _param_fw_r_tc,
float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ (ParamFloat<px4::params::FW_RLL_TO_YAW_FF>) _param_fw_rll_to_yaw_ff,
float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ (ParamFloat<px4::params::FW_RR_FF>) _param_fw_rr_ff,
float man_roll_max; /**< Max Roll in rad */ (ParamFloat<px4::params::FW_RR_I>) _param_fw_rr_i,
float man_pitch_max; /**< Max Pitch in rad */ (ParamFloat<px4::params::FW_RR_IMAX>) _param_fw_rr_imax,
float man_roll_scale; /**< scale factor applied to roll actuator control in pure manual mode */ (ParamFloat<px4::params::FW_RR_P>) _param_fw_rr_p,
float man_pitch_scale; /**< scale factor applied to pitch actuator control in pure manual mode */ (ParamFloat<px4::params::FW_RSP_OFF>) _param_fw_rsp_off,
float man_yaw_scale; /**< scale factor applied to yaw actuator control in pure manual mode */
(ParamBool<px4::params::FW_W_EN>) _param_fw_w_en,
float acro_max_x_rate_rad; (ParamFloat<px4::params::FW_W_RMAX>) _param_fw_w_rmax,
float acro_max_y_rate_rad; (ParamFloat<px4::params::FW_WR_FF>) _param_fw_wr_ff,
float acro_max_z_rate_rad; (ParamFloat<px4::params::FW_WR_I>) _param_fw_wr_i,
(ParamFloat<px4::params::FW_WR_IMAX>) _param_fw_wr_imax,
float flaps_scale; /**< Scale factor for flaps */ (ParamFloat<px4::params::FW_WR_P>) _param_fw_wr_p,
float flaps_takeoff_scale; /**< Scale factor for flaps on take-off */
float flaps_land_scale; /**< Scale factor for flaps on landing */ (ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax,
float flaperon_scale; /**< Scale factor for flaperons */ (ParamFloat<px4::params::FW_YR_FF>) _param_fw_yr_ff,
(ParamFloat<px4::params::FW_YR_I>) _param_fw_yr_i,
float rattitude_thres; (ParamFloat<px4::params::FW_YR_IMAX>) _param_fw_yr_imax,
(ParamFloat<px4::params::FW_YR_P>) _param_fw_yr_p,
int32_t bat_scale_en; /**< Battery scaling enabled */
bool airspeed_disabled; (ParamFloat<px4::params::TRIM_PITCH>) _param_trim_pitch,
(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
} _parameters{}; /**< local copies of interesting parameters */ (ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw
)
struct {
ECL_RollController _roll_ctrl;
param_t p_tc; ECL_PitchController _pitch_ctrl;
param_t p_p; ECL_YawController _yaw_ctrl;
param_t p_i; ECL_WheelController _wheel_ctrl;
param_t p_ff;
param_t p_rmax_pos;
param_t p_rmax_neg;
param_t p_integrator_max;
param_t r_tc;
param_t r_p;
param_t r_i;
param_t r_ff;
param_t r_integrator_max;
param_t r_rmax;
param_t y_p;
param_t y_i;
param_t y_ff;
param_t y_integrator_max;
param_t roll_to_yaw_ff;
param_t y_rmax;
param_t w_en;
param_t w_p;
param_t w_i;
param_t w_ff;
param_t w_integrator_max;
param_t w_rmax;
param_t airspeed_min;
param_t airspeed_trim;
param_t airspeed_max;
param_t trim_roll;
param_t trim_pitch;
param_t trim_yaw;
param_t dtrim_roll_vmin;
param_t dtrim_pitch_vmin;
param_t dtrim_yaw_vmin;
param_t dtrim_roll_vmax;
param_t dtrim_pitch_vmax;
param_t dtrim_yaw_vmax;
param_t dtrim_roll_flaps;
param_t dtrim_pitch_flaps;
param_t rollsp_offset_deg;
param_t pitchsp_offset_deg;
param_t man_roll_max;
param_t man_pitch_max;
param_t man_roll_scale;
param_t man_pitch_scale;
param_t man_yaw_scale;
param_t acro_max_x_rate;
param_t acro_max_y_rate;
param_t acro_max_z_rate;
param_t flaps_scale;
param_t flaps_takeoff_scale;
param_t flaps_land_scale;
param_t flaperon_scale;
param_t rattitude_thres;
param_t bat_scale_en;
param_t airspeed_mode;
} _parameter_handles{}; /**< handles for interesting parameters */
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
ECL_YawController _yaw_ctrl;
ECL_WheelController _wheel_ctrl;
void control_flaps(const float dt); void control_flaps(const float dt);

Write Preview
Loading…
Cancel
Save