From 1980df66ca1e395a198b75bea36f4a64b4e97e3a Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Mon, 11 Dec 2023 10:22:58 +0100 Subject: [PATCH] Add offboard swtich --- src/modules/offboard_switch/CMakeLists.txt | 42 ++ src/modules/offboard_switch/Kconfig | 12 + .../offboard_switch/offboard_switch.cpp | 584 ++++++++++++++++++ .../offboard_switch/offboard_switch.hpp | 223 +++++++ .../offboard_switch/offboard_switch_params.c | 454 ++++++++++++++ 5 files changed, 1315 insertions(+) create mode 100644 src/modules/offboard_switch/CMakeLists.txt create mode 100644 src/modules/offboard_switch/Kconfig create mode 100644 src/modules/offboard_switch/offboard_switch.cpp create mode 100644 src/modules/offboard_switch/offboard_switch.hpp create mode 100644 src/modules/offboard_switch/offboard_switch_params.c diff --git a/src/modules/offboard_switch/CMakeLists.txt b/src/modules/offboard_switch/CMakeLists.txt new file mode 100644 index 000000000000..9b05a9db413c --- /dev/null +++ b/src/modules/offboard_switch/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__offboard_switch + MAIN offboard_switch + SRCS + offboard_switch.cpp + offboard_switch.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/modules/offboard_switch/Kconfig b/src/modules/offboard_switch/Kconfig new file mode 100644 index 000000000000..1d9dc7808549 --- /dev/null +++ b/src/modules/offboard_switch/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_FW_RATE_CONTROL + bool "offboard_switch" + default n + ---help--- + Enable support for fw_att_control + +menuconfig USER_FW_RATE_CONTROL + bool "fw_rate_control running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_FW_RATE_CONTROL + ---help--- + Put fw_rate_control in userspace memory diff --git a/src/modules/offboard_switch/offboard_switch.cpp b/src/modules/offboard_switch/offboard_switch.cpp new file mode 100644 index 000000000000..4d1bab51bd6e --- /dev/null +++ b/src/modules/offboard_switch/offboard_switch.cpp @@ -0,0 +1,584 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "FixedwingRateControl.hpp" + +using namespace time_literals; +using namespace matrix; + +using math::constrain; +using math::interpolate; +using math::radians; + +FixedwingRateControl::FixedwingRateControl(bool vtol) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)), + _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_fw) : ORB_ID(vehicle_torque_setpoint)), + _vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_fw) : ORB_ID(vehicle_thrust_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + _handle_param_vt_fw_difthr_en = param_find("VT_FW_DIFTHR_EN"); + + /* fetch initial parameter values */ + parameters_update(); + + _rate_ctrl_status_pub.advertise(); +} + +FixedwingRateControl::~FixedwingRateControl() +{ + perf_free(_loop_perf); +} + +bool +FixedwingRateControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +int +FixedwingRateControl::parameters_update() +{ + const Vector3f rate_p = Vector3f(_param_fw_rr_p.get(), _param_fw_pr_p.get(), _param_fw_yr_p.get()); + const Vector3f rate_i = Vector3f(_param_fw_rr_i.get(), _param_fw_pr_i.get(), _param_fw_yr_i.get()); + const Vector3f rate_d = Vector3f(_param_fw_rr_d.get(), _param_fw_pr_d.get(), _param_fw_yr_d.get()); + + _rate_control.setPidGains(rate_p, rate_i, rate_d); + + _rate_control.setIntegratorLimit( + Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get())); + + _rate_control.setFeedForwardGain( + // set FF gains to 0 as we add the FF control outside of the rate controller + Vector3f(0.f, 0.f, 0.f)); + + if (_handle_param_vt_fw_difthr_en != PARAM_INVALID) { + param_get(_handle_param_vt_fw_difthr_en, &_param_vt_fw_difthr_en); + } + + + return PX4_OK; +} + +void +FixedwingRateControl::vehicle_manual_poll() +{ + if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) { + + // Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { + + if (_vcontrol_mode.flag_control_rates_enabled && + !_vcontrol_mode.flag_control_attitude_enabled) { + + // RATE mode we need to generate the rate setpoint from manual user inputs + + if (_vehicle_status.is_vtol_tailsitter && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + // the rate_sp must always be published in body (hover) frame + _rates_sp.roll = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get()); + _rates_sp.yaw = -_manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get()); + + } else { + _rates_sp.roll = _manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get()); + _rates_sp.yaw = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get()); + } + + _rates_sp.timestamp = hrt_absolute_time(); + _rates_sp.pitch = -_manual_control_setpoint.pitch * radians(_param_fw_acro_y_max.get()); + _rates_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; + + _rate_sp_pub.publish(_rates_sp); + + } else { + // Manual/direct control, filled in FW-frame. Note that setpoints will get transformed to body frame prior publishing. + + _vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + + _param_trim_roll.get(), -1.f, 1.f); + _vehicle_torque_setpoint.xyz[1] = math::constrain(-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + + _param_trim_pitch.get(), -1.f, 1.f); + _vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + + _param_trim_yaw.get(), -1.f, 1.f); + + _vehicle_thrust_setpoint.xyz[0] = math::constrain((_manual_control_setpoint.throttle + 1.f) * .5f, 0.f, 1.f); + } + } + } +} + +void +FixedwingRateControl::vehicle_land_detected_poll() +{ + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected {}; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + } + } +} + +float FixedwingRateControl::get_airspeed_and_update_scaling() +{ + _airspeed_validated_sub.update(); + const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s) + && (hrt_elapsed_time(&_airspeed_validated_sub.get().timestamp) < 1_s); + + // if no airspeed measurement is available out best guess is to use the trim airspeed + float airspeed = _param_fw_airspd_trim.get(); + + if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) { + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s); + + } else { + // VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible + // this assumption is good as long as the vehicle is not hovering in a headwind which is much larger + // than the stall airspeed + if (_vehicle_status.is_vtol && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode) { + airspeed = _param_fw_airspd_stall.get(); + } + } + + /* + * For scaling our actuators using anything less than the stall + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and it's the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(), + _param_fw_airspd_max.get()), 0.1f, 1000.0f); + + _airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f; + + return airspeed; +} + +void FixedwingRateControl::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // only run controller if angular velocity changed + if (_vehicle_angular_velocity_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) { //TODO rate! + + // only update parameters if they changed + bool params_updated = _parameter_update_sub.updated(); + + // check for parameter updates + if (params_updated) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + parameters_update(); + } + + float dt = 0.f; + + static constexpr float DT_MIN = 0.002f; + static constexpr float DT_MAX = 0.04f; + + vehicle_angular_velocity_s vehicle_angular_velocity{}; + + if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) { + dt = math::constrain((vehicle_angular_velocity.timestamp_sample - _last_run) * 1e-6f, DT_MIN, DT_MAX); + _last_run = vehicle_angular_velocity.timestamp_sample; + } + + if (dt < DT_MIN || dt > DT_MAX) { + const hrt_abstime time_now_us = hrt_absolute_time(); + dt = math::constrain((time_now_us - _last_run) * 1e-6f, DT_MIN, DT_MAX); + _last_run = time_now_us; + } + + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&angular_velocity); + + Vector3f rates(angular_velocity.xyz); + Vector3f angular_accel{angular_velocity.xyz_derivative}; + + // Tailsitter: rotate setpoint from hover to fixed-wing frame (controller is in fixed-wing frame, interface in hover) + if (_vehicle_status.is_vtol_tailsitter) { + rates = Vector3f(-angular_velocity.xyz[2], angular_velocity.xyz[1], angular_velocity.xyz[0]); + angular_accel = Vector3f(-angular_velocity.xyz_derivative[2], angular_velocity.xyz_derivative[1], + angular_velocity.xyz_derivative[0]); + } + + // vehicle status update must be before the vehicle_control_mode poll, otherwise rate sp are not published during whole transition + _vehicle_status_sub.update(&_vehicle_status); + const bool is_in_transition_except_tailsitter = _vehicle_status.in_transition_mode + && !_vehicle_status.is_vtol_tailsitter; + const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + _in_fw_or_transition_wo_tailsitter_transition = is_fixed_wing || is_in_transition_except_tailsitter; + + _vehicle_control_mode_sub.update(&_vcontrol_mode); + + vehicle_land_detected_poll(); + + vehicle_manual_poll(); + vehicle_land_detected_poll(); + + /* if we are in rotary wing mode, do nothing */ + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) { + perf_end(_loop_perf); + return; + } + + if (_vcontrol_mode.flag_control_rates_enabled) { + + const float airspeed = get_airspeed_and_update_scaling(); + + /* reset integrals where needed */ + if (_rates_sp.reset_integral) { + _rate_control.resetIntegral(); + } + + // Reset integrators if the aircraft is on ground or not in a state where the fw attitude controller is run + if (_landed || !_in_fw_or_transition_wo_tailsitter_transition) { + + _rate_control.resetIntegral(); + } + + // Update saturation status from control allocation feedback + // TODO: send the unallocated value directly for better anti-windup + Vector3 diffthr_enabled( + _param_vt_fw_difthr_en & static_cast(VTOLFixedWingDifferentialThrustEnabledBit::ROLL_BIT), + _param_vt_fw_difthr_en & static_cast(VTOLFixedWingDifferentialThrustEnabledBit::PITCH_BIT), + _param_vt_fw_difthr_en & static_cast(VTOLFixedWingDifferentialThrustEnabledBit::YAW_BIT) + ); + + if (_vehicle_status.is_vtol_tailsitter) { + // Swap roll and yaw + diffthr_enabled.swapRows(0, 2); + } + + // saturation handling for axis controlled by differential thrust (VTOL only) + control_allocator_status_s control_allocator_status; + + // Set saturation flags for VTOL differential thrust feature + // If differential thrust is enabled in an axis, assume it's the only torque authority and only update saturation using matrix 0 allocating the motors. + if (_control_allocator_status_subs[0].update(&control_allocator_status)) { + for (size_t i = 0; i < 3; i++) { + if (diffthr_enabled(i)) { + _rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON); + _rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON); + } + } + } + + // Set saturation flags for control surface controlled axes + if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) { + for (size_t i = 0; i < 3; i++) { + if (!diffthr_enabled(i)) { + _rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON); + _rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON); + } + } + } + + /* bi-linear interpolation over airspeed for actuator trim scheduling */ + float trim_roll = _param_trim_roll.get(); + float trim_pitch = _param_trim_pitch.get(); + float trim_yaw = _param_trim_yaw.get(); + + if (airspeed < _param_fw_airspd_trim.get()) { + trim_roll += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_r_vmin.get(), + 0.0f); + trim_pitch += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_p_vmin.get(), + 0.0f); + trim_yaw += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_y_vmin.get(), + 0.0f); + + } else { + trim_roll += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_r_vmax.get()); + trim_pitch += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_p_vmax.get()); + trim_yaw += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_y_vmax.get()); + } + + if (_vcontrol_mode.flag_control_rates_enabled) { + _rates_sp_sub.update(&_rates_sp); + + Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw); + + // Tailsitter: rotate setpoint from hover to fixed-wing frame (controller is in fixed-wing frame, interface in hover) + if (_vehicle_status.is_vtol_tailsitter) { + body_rates_setpoint = Vector3f(-_rates_sp.yaw, _rates_sp.pitch, _rates_sp.roll); + } + + // Run attitude RATE controllers which need the desired attitudes from above, add trim. + const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, + _landed); + + const float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0); + const float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1); + const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2); + + const float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward; + const float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward; + + // Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw + float yaw_u = 0.f; + + if (_vcontrol_mode.flag_control_attitude_enabled || _param_fw_acro_yaw_en.get()) { + yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward; + + } else { + yaw_u = _manual_control_setpoint.yaw * _param_fw_man_y_sc.get(); + _rate_control.resetIntegral(2); + } + + if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) { + _rate_control.resetIntegral(); + } + + _vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll; + _vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch; + _vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw; + + /* throttle passed through if it is finite */ + _vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f; + + /* scale effort by battery status */ + if (_param_fw_bat_scale_en.get() && _vehicle_thrust_setpoint.xyz[0] > 0.1f) { + + if (_battery_status_sub.updated()) { + battery_status_s battery_status{}; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_scale = battery_status.scale; + } + } + + _vehicle_thrust_setpoint.xyz[0] *= _battery_scale; + } + } + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + + _rate_ctrl_status_pub.publish(rate_ctrl_status); + + } else { + // full manual + _rate_control.resetIntegral(); + } + + // 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 + _vehicle_torque_setpoint.xyz[2] = math::constrain(_vehicle_torque_setpoint.xyz[2] + _param_fw_rll_to_yaw_ff.get() * + _vehicle_torque_setpoint.xyz[0], -1.f, 1.f); + + // Tailsitter: rotate back to body frame from airspeed frame + if (_vehicle_status.is_vtol_tailsitter) { + const float helper = _vehicle_torque_setpoint.xyz[0]; + _vehicle_torque_setpoint.xyz[0] = _vehicle_torque_setpoint.xyz[2]; + _vehicle_torque_setpoint.xyz[2] = -helper; + } + + /* Only publish if any of the proper modes are enabled */ + if (_vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { + { + _vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + _vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint); + + _vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + _vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint); + } + } + + updateActuatorControlsStatus(dt); + + // Manual flaps/spoilers control, also active in VTOL Hover. Is handled and published in FW Position controller/VTOL module if Auto. + if (_vcontrol_mode.flag_control_manual_enabled) { + + // Flaps control + float flaps_control = 0.f; // default to no flaps + + /* map flaps by default to manual if valid */ + if (PX4_ISFINITE(_manual_control_setpoint.flaps)) { + flaps_control = math::max(_manual_control_setpoint.flaps, 0.f); // do not consider negative switch settings + } + + normalized_unsigned_setpoint_s flaps_setpoint; + flaps_setpoint.timestamp = hrt_absolute_time(); + flaps_setpoint.normalized_setpoint = flaps_control; + _flaps_setpoint_pub.publish(flaps_setpoint); + + // Spoilers control + float spoilers_control = 0.f; // default to no spoilers + + switch (_param_fw_spoilers_man.get()) { + case 0: + break; + + case 1: + // do not consider negative switch settings + spoilers_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? math::max(_manual_control_setpoint.flaps, 0.f) : 0.f; + break; + + case 2: + // do not consider negative switch settings + spoilers_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? math::max(_manual_control_setpoint.aux1, 0.f) : 0.f; + break; + } + + normalized_unsigned_setpoint_s spoilers_setpoint; + spoilers_setpoint.timestamp = hrt_absolute_time(); + spoilers_setpoint.normalized_setpoint = spoilers_control; + _spoilers_setpoint_pub.publish(spoilers_setpoint); + } + } + + // backup schedule + ScheduleDelayed(20_ms); + + perf_end(_loop_perf); +} + +void FixedwingRateControl::updateActuatorControlsStatus(float dt) +{ + for (int i = 0; i < 3; i++) { + + // We assume that the attitude is actuated by control surfaces + // consuming power only when they move + const float control_signal = _vehicle_torque_setpoint.xyz[i] - _control_prev[i]; + _control_prev[i] = _vehicle_torque_setpoint.xyz[i]; + + _control_energy[i] += control_signal * control_signal * dt; + } + + _energy_integration_time += dt; + + if (_energy_integration_time > 500e-3f) { + + actuator_controls_status_s status; + status.timestamp = _vehicle_torque_setpoint.timestamp; + + for (int i = 0; i < 3; i++) { + status.control_power[i] = _control_energy[i] / _energy_integration_time; + _control_energy[i] = 0.f; + } + + _actuator_controls_status_pub.publish(status); + _energy_integration_time = 0.f; + } +} + +int FixedwingRateControl::task_spawn(int argc, char *argv[]) +{ + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + FixedwingRateControl *instance = new FixedwingRateControl(vtol); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int FixedwingRateControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FixedwingRateControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +fw_rate_control is the fixed-wing rate controller. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("fw_rate_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int fw_rate_control_main(int argc, char *argv[]) +{ + return FixedwingRateControl::main(argc, argv); +} diff --git a/src/modules/offboard_switch/offboard_switch.hpp b/src/modules/offboard_switch/offboard_switch.hpp new file mode 100644 index 000000000000..14f4a6427f29 --- /dev/null +++ b/src/modules/offboard_switch/offboard_switch.hpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::Eulerf; +using matrix::Quatf; + +using uORB::SubscriptionData; + +using namespace time_literals; + +class FixedwingRateControl final : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + FixedwingRateControl(bool vtol = false); + ~FixedwingRateControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; + + uORB::SubscriptionMultiArray _control_allocator_status_subs{ORB_ID::control_allocator_status}; + + uORB::SubscriptionData _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + + uORB::Publication _actuator_controls_status_pub; + uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _vehicle_torque_setpoint_pub; + uORB::Publication _vehicle_thrust_setpoint_pub; + uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; + uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; + + manual_control_setpoint_s _manual_control_setpoint{}; + vehicle_control_mode_s _vcontrol_mode{}; + vehicle_thrust_setpoint_s _vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s _vehicle_torque_setpoint{}; + vehicle_rates_setpoint_s _rates_sp{}; + vehicle_status_s _vehicle_status{}; + + perf_counter_t _loop_perf; + + hrt_abstime _last_run{0}; + + float _airspeed_scaling{1.0f}; + + bool _landed{true}; + + float _battery_scale{1.0f}; + + float _energy_integration_time{0.0f}; + float _control_energy[4] {}; + float _control_prev[3] {}; + + bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states + + // enum for bitmask of VT_FW_DIFTHR_EN parameter options + enum class VTOLFixedWingDifferentialThrustEnabledBit : int32_t { + YAW_BIT = (1 << 0), + ROLL_BIT = (1 << 1), + PITCH_BIT = (1 << 2), + }; + + param_t _handle_param_vt_fw_difthr_en{PARAM_INVALID}; + int32_t _param_vt_fw_difthr_en{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_fw_acro_x_max, + (ParamFloat) _param_fw_acro_y_max, + (ParamFloat) _param_fw_acro_z_max, + (ParamInt) _param_fw_acro_yaw_en, + + (ParamFloat) _param_fw_airspd_max, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_stall, + (ParamFloat) _param_fw_airspd_trim, + (ParamInt) _param_fw_arsp_mode, + + (ParamInt) _param_fw_arsp_scale_en, + + (ParamBool) _param_fw_bat_scale_en, + + (ParamFloat) _param_fw_dtrim_p_vmax, + (ParamFloat) _param_fw_dtrim_p_vmin, + (ParamFloat) _param_fw_dtrim_r_vmax, + (ParamFloat) _param_fw_dtrim_r_vmin, + (ParamFloat) _param_fw_dtrim_y_vmax, + (ParamFloat) _param_fw_dtrim_y_vmin, + + (ParamFloat) _param_fw_man_p_sc, + (ParamFloat) _param_fw_man_r_sc, + (ParamFloat) _param_fw_man_y_sc, + + (ParamFloat) _param_fw_pr_ff, + (ParamFloat) _param_fw_pr_i, + (ParamFloat) _param_fw_pr_imax, + (ParamFloat) _param_fw_pr_p, + (ParamFloat) _param_fw_pr_d, + + (ParamFloat) _param_fw_rll_to_yaw_ff, + (ParamFloat) _param_fw_rr_ff, + (ParamFloat) _param_fw_rr_i, + (ParamFloat) _param_fw_rr_imax, + (ParamFloat) _param_fw_rr_p, + (ParamFloat) _param_fw_rr_d, + + (ParamFloat) _param_fw_yr_ff, + (ParamFloat) _param_fw_yr_i, + (ParamFloat) _param_fw_yr_imax, + (ParamFloat) _param_fw_yr_p, + (ParamFloat) _param_fw_yr_d, + + (ParamFloat) _param_trim_pitch, + (ParamFloat) _param_trim_roll, + (ParamFloat) _param_trim_yaw, + + (ParamInt) _param_fw_spoilers_man + ) + + RateControl _rate_control; ///< class for rate control calculations + + void updateActuatorControlsStatus(float dt); + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + void vehicle_manual_poll(); + void vehicle_land_detected_poll(); + + float get_airspeed_and_update_scaling(); +}; diff --git a/src/modules/offboard_switch/offboard_switch_params.c b/src/modules/offboard_switch/offboard_switch_params.c new file mode 100644 index 000000000000..ee6e194ce3a9 --- /dev/null +++ b/src/modules/offboard_switch/offboard_switch_params.c @@ -0,0 +1,454 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fw_rate_control_params.c + * + * Parameters defined by the fixed-wing rate control task + * + * @author Lorenz Meier + * @author Thomas Gubler + */ + +/** + * Airspeed mode + * + * On vehicles without airspeed sensor this parameter can be used to + * enable flying without an airspeed reading + * + * @value 0 Use airspeed in controller + * @value 1 Do not use airspeed in controller + * @group FW Rate Control + */ +PARAM_DEFINE_INT32(FW_ARSP_MODE, 0); + +/** + * Pitch rate proportional gain. + * + * @unit %/rad/s + * @min 0.0 + * @max 10 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); + +/** + * Pitch rate derivative gain. + * + * Pitch rate differential gain. + * + * @unit %/rad/s + * @min 0.0 + * @max 10 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_PR_D, 0.f); + +/** + * Pitch rate integrator gain. + * + * @unit %/rad + * @min 0.0 + * @max 10 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_PR_I, 0.1f); + +/** + * Pitch rate integrator limit + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); + +/** + * Roll rate proportional gain + * + * @unit %/rad/s + * @min 0.0 + * @max 10 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); + +/** + * Roll rate derivative gain + * + * @unit %/rad/s + * @min 0.0 + * @max 10 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); + +/** + * Roll rate integrator gain + * + * @unit %/rad + * @min 0.0 + * @max 10 + * @decimal 2 + * @increment 0.01 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RR_I, 0.1f); + +/** + * Roll integrator limit + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); + +/** + * Yaw rate proportional gain + * + * @unit %/rad/s + * @min 0.0 + * @max 10 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); + +/** + * Yaw rate derivative gain + * + * @unit %/rad/s + * @min 0.0 + * @max 10 + * @decimal 3 + * @increment 0.005 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_YR_D, 0.0f); + +/** + * Yaw rate integrator gain + * + * @unit %/rad + * @min 0.0 + * @max 10 + * @decimal 1 + * @increment 0.5 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_YR_I, 0.1f); + +/** + * Yaw rate integrator limit + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); + +/** + * Roll rate feed forward + * + * Direct feed forward from rate setpoint to control surface output. + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); + +/** + * Pitch rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); + +/** + * Yaw rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); + +/** + * Acro body roll max rate setpoint + * + * @min 10 + * @max 720 + * @unit deg + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90); + +/** + * Acro body pitch max rate setpoint + * + * @min 10 + * @max 720 + * @unit deg + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90); + +/** + * Acro body yaw max rate setpoint + * + * @min 10 + * @max 720 + * @unit deg + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45); + +/** + * Enable throttle scale by battery level + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. + * + * @boolean + * @group FW Rate Control + */ +PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0); + +/** + * Enable airspeed scaling + * + * This enables a logic that automatically adjusts the output of the rate controller to take + * into account the real torque produced by an aerodynamic control surface given + * the current deviation from the trim airspeed (FW_AIRSPD_TRIM). + * + * Enable when using aerodynamic control surfaces (e.g.: plane) + * Disable when using rotor wings (e.g.: autogyro) + * + * @boolean + * @group FW Rate Control + */ +PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1); + +/** +* Roll trim increment at minimum airspeed +* +* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MIN. + * + * @group FW Rate Control + * @min -0.5 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMIN, 0.0f); + +/** +* Pitch trim increment at minimum airspeed +* +* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MIN. + * + * @group FW Rate Control + * @min -0.5 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMIN, 0.0f); + +/** +* Yaw trim increment at minimum airspeed +* +* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MIN. + * + * @group FW Rate Control + * @min -0.5 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMIN, 0.0f); + +/** +* Roll trim increment at maximum airspeed +* +* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MAX. + * + * @group FW Rate Control + * @min -0.5 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMAX, 0.0f); + +/** +* Pitch trim increment at maximum airspeed +* +* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MAX. + * + * @group FW Rate Control + * @min -0.5 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMAX, 0.0f); + +/** +* Yaw trim increment at maximum airspeed +* +* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MAX. + * + * @group FW Rate Control + * @min -0.5 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f); + +/** + * Manual roll scale + * + * Scale factor applied to the desired roll actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f); + +/** + * Manual pitch scale + * + * Scale factor applied to the desired pitch actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f); + +/** + * Manual yaw scale + * + * Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f); + +/** + * Roll control to yaw control feedforward gain. + * + * This gain can be used to counteract the "adverse yaw" effect for fixed wings. + * When the plane enters a roll it will tend to yaw the nose out of the turn. + * This gain enables the use of a yaw actuator to counteract this effect. + * + * @min 0.0 + * @decimal 1 + * @increment 0.01 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f); + +/** + * Spoiler input in manual flight + * + * Chose source for manual setting of spoilers in manual flight modes. + * + * @value 0 Disabled + * @value 1 Flaps channel + * @value 2 Aux1 + * @group FW Rate Control + */ +PARAM_DEFINE_INT32(FW_SPOILERS_MAN, 0); + +/** + * Enable yaw rate controller in Acro + * + * If this parameter is set to 1, the yaw rate controller is enabled in Fixed-wing Acro mode. + * Otherwise the pilot commands directly the yaw actuator. + * It is disabled by default because an active yaw rate controller will fight against the + * natural turn coordination of the plane. + * + * @boolean + * @group FW Rate Control + */ +PARAM_DEFINE_INT32(FW_ACRO_YAW_EN, 0);