diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 426a9b83d1..8a70c0fd9a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -18,7 +18,8 @@ control_allocator start # # Start attitude controller. # -fw_rate_control start +# fw_rate_control start +dob_rate_control start fw_att_control start fw_pos_control_l1 start airspeed_selector start diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index e4bd96991a..afc0123091 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -12,6 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_DOB_RATE_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/src/modules/dob_rate_control/CMakeLists.txt b/src/modules/dob_rate_control/CMakeLists.txt new file mode 100644 index 0000000000..5b4fbf0ac6 --- /dev/null +++ b/src/modules/dob_rate_control/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__dob_rate_control + MAIN dob_rate_control + SRCS + dob_rate_control.hpp + dob_rate_control.cpp + DobControl.hpp + DobControl.cpp + DEPENDS + ) diff --git a/src/modules/dob_rate_control/DobControl.cpp b/src/modules/dob_rate_control/DobControl.cpp new file mode 100644 index 0000000000..3971197289 --- /dev/null +++ b/src/modules/dob_rate_control/DobControl.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 DobControl.cpp + */ + +#include "DobControl.hpp" +#include + +using namespace matrix; + +void DobControl::setGains(const Vector3f &tau) +{ + _gain_tau = tau; +} + +void DobControl::setSaturationStatus(const Vector &saturation_positive, + const Vector &saturation_negative) +{ + _control_allocator_saturation_positive = saturation_positive; + _control_allocator_saturation_negative = saturation_negative; +} + +Vector3f DobControl::update(const Vector3f &rate, + const Vector3f actuator_sp, const float dt, const bool landed) +{ + // angular rates error + Vector3f diff = differentiator(rate, dt); + + matrix::Vector3f integ_input = actuator_sp - diff; + + // update integral only if we are not landed + if (!landed) { + updateIntegral(integ_input, dt); + } + + const matrix::Vector3f torque = integ_input + _input_rate; + return torque; +} + +Vector3f DobControl::differentiator(const matrix::Vector3f &input, const float dt) +{ + Vector3f output = (1 - _c0) * _prev_output + (_c0 / dt) * (input - _prev_input); + _prev_input = input; + _prev_output = output; + return output; +} + +void DobControl::updateIntegral(Vector3f &integ_input, const float dt) +{ + _input_rate = _input_rate + _gain_tau.emult(integ_input); + +} + +void DobControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status) +{ + rate_ctrl_status.rollspeed_integ = _input_rate(0); + rate_ctrl_status.pitchspeed_integ = _input_rate(1); + rate_ctrl_status.yawspeed_integ = _input_rate(2); +} diff --git a/src/modules/dob_rate_control/DobControl.hpp b/src/modules/dob_rate_control/DobControl.hpp new file mode 100644 index 0000000000..9148c7180e --- /dev/null +++ b/src/modules/dob_rate_control/DobControl.hpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 rate_control.hpp + * + * PID 3 axis angular rate / angular velocity control. + */ + +#pragma once + +#include + +#include +#include + +class DobControl +{ +public: + DobControl() = default; + ~DobControl() = default; + + /** + * Set the rate control gains + * @param P 3D vector of proportional gains for body x,y,z axis + * @param I 3D vector of integral gains + * @param D 3D vector of derivative gains + */ + void setGains(const matrix::Vector3f &tau); + + /** + * Set the mximum absolute value of the integrator for all axes + * @param integrator_limit limit value for all axes x, y, z + */ + void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; }; + + /** + * Set saturation status + * @param control saturation vector from control allocator + */ + void setSaturationStatus(const matrix::Vector &saturation_positive, + const matrix::Vector &saturation_negative); + + /** + * Run one control loop cycle calculation + * @param rate estimation of the current vehicle angular rate + * @param rate_sp desired vehicle angular rate setpoint + * @param dt desired vehicle angular rate setpoint + * @return [-1,1] normalized torque vector to apply to the vehicle + */ + matrix::Vector3f update(const matrix::Vector3f &rate_sp, + const matrix::Vector3f actuator_sp, const float dt, const bool landed); + + /** + * Get status message of controller for logging/debugging + * @param rate_ctrl_status status message to fill with internal states + */ + void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status); + +private: + void updateIntegral(matrix::Vector3f &rate_error, const float dt); + + matrix::Vector3f differentiator(const matrix::Vector3f &input, const float dt); + + // Gains + matrix::Vector3f _gain_tau{matrix::Vector3f(0.000000000001, 0.0000000000001, 0.0000000000001)}; /// + matrix::Vector3f _lim_int; + matrix::Vector3f _input_rate{matrix::Vector3f(0.0, 0.0, 0.0)}; + matrix::Vector3f _prev_input{matrix::Vector3f(0.0, 0.0, 0.0)}; + matrix::Vector3f _prev_output{matrix::Vector3f(0.0, 0.0, 0.0)}; + float _c0{0.0025}; + + // Feedback from control allocation + matrix::Vector _control_allocator_saturation_negative; + matrix::Vector _control_allocator_saturation_positive; +}; diff --git a/src/modules/dob_rate_control/Kconfig b/src/modules/dob_rate_control/Kconfig new file mode 100644 index 0000000000..5fa8aa7495 --- /dev/null +++ b/src/modules/dob_rate_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_DOB_RATE_CONTROL + bool "dob_rate_control" + default n + ---help--- + Enable support for dob_rate_control + +menuconfig USER_FW_RATE_CONTROL + bool "dob_rate_control running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_DOB_RATE_CONTROL + ---help--- + Put fw_rate_control in userspace memory diff --git a/src/modules/dob_rate_control/dob_rate_control.cpp b/src/modules/dob_rate_control/dob_rate_control.cpp new file mode 100644 index 0000000000..24e1879d96 --- /dev/null +++ b/src/modules/dob_rate_control/dob_rate_control.cpp @@ -0,0 +1,638 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 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 "dob_rate_control.hpp" + +using namespace time_literals; +using namespace matrix; + +using math::constrain; +using math::interpolate; +using math::radians; + +DobRateControl::DobRateControl(bool vtol) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)), + _actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + /* fetch initial parameter values */ + parameters_update(); + + _rate_ctrl_status_pub.advertise(); + _spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate); + _flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate); +} + +DobRateControl::~DobRateControl() +{ + perf_free(_loop_perf); +} + +bool +DobRateControl::init() +{ + if (!_att_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +int +DobRateControl::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.setGains(rate_p, rate_i, rate_d); + + ///TODO: parameterize tau gains + const Vector3f dob_rate_tau = Vector3f(0.000001, 0.000001, 0.000001); + + _dob_control.setGains(dob_rate_tau); + + _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)); + + + return PX4_OK; +} + +void +DobRateControl::vehicle_control_mode_poll() +{ + _vcontrol_mode_sub.update(&_vcontrol_mode); + + if (_vehicle_status.is_vtol) { + const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode; + const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _vehicle_status.is_vtol_tailsitter; + + if (is_hovering || is_tailsitter_transition) { + _vcontrol_mode.flag_control_attitude_enabled = false; + _vcontrol_mode.flag_control_manual_enabled = false; + } + } +} + +void +DobRateControl::vehicle_manual_poll(const float yaw_body) +{ + const bool is_tailsitter_transition = _vehicle_status.is_vtol_tailsitter && _vehicle_status.in_transition_mode; + const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) { + + // 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_climb_rate_enabled) { + + 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 + _rates_sp.timestamp = hrt_absolute_time(); + _rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get()); + _rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get()); + _rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get()); + _rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + + _rate_sp_pub.publish(_rates_sp); + + } else { + /* manual/direct control */ + _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = + _manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get(); + _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = + -_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); + _actuator_controls.control[actuator_controls_s::INDEX_YAW] = + _manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f, + 1.0f); + } + } + } + } +} + +void +DobRateControl::vehicle_attitude_setpoint_poll() +{ + _att_sp_sub.update(&_att_sp); +} + +void +DobRateControl::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 DobRateControl::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 DobRateControl::Run() +{ + if (should_exit()) { + _att_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + ///TODO: Publish offboard mode topic + + ///TODO: Only publish actuator commands when offboard is engaged + + // only run controller if attitude changed + if (_att_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) { + + // 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_attitude_s att{}; + + if (_att_sub.copy(&att)) { + dt = math::constrain((att.timestamp_sample - _last_run) * 1e-6f, DT_MIN, DT_MAX); + _last_run = att.timestamp_sample; + + // get current rotation matrix and euler angles from control state quaternions + _R = matrix::Quatf(att.q); + } + + 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_rates_sub.copy(&angular_velocity); + float rollspeed = angular_velocity.xyz[0]; + float pitchspeed = angular_velocity.xyz[1]; + float yawspeed = angular_velocity.xyz[2]; + const Vector3f rates(rollspeed, pitchspeed, yawspeed); + + vehicle_angular_acceleration_s angular_acceleration{}; + _vehicle_angular_acceleration_sub.copy(&angular_acceleration); + const Vector3f angular_accel{angular_acceleration.xyz}; + + if (_vehicle_status.is_vtol_tailsitter) { + /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode + * + * Since the VTOL airframe is initialized as a multicopter we need to + * modify the estimated attitude for the fixed wing operation. + * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around + * the pitch axis compared to the neutral position of the vehicle in multicopter mode + * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. + * Additionally, in order to get the correct sign of the pitch, we need to multiply + * the new x axis of the rotation matrix with -1 + * + * original: modified: + * + * Rxx Ryx Rzx -Rzx Ryx Rxx + * Rxy Ryy Rzy -Rzy Ryy Rxy + * Rxz Ryz Rzz -Rzz Ryz Rxz + * */ + matrix::Dcmf R_adapted = _R; //modified rotation matrix + + /* move z to x */ + R_adapted(0, 0) = _R(0, 2); + R_adapted(1, 0) = _R(1, 2); + R_adapted(2, 0) = _R(2, 2); + + /* move x to z */ + R_adapted(0, 2) = _R(0, 0); + R_adapted(1, 2) = _R(1, 0); + R_adapted(2, 2) = _R(2, 0); + + /* change direction of pitch (convert to right handed system) */ + R_adapted(0, 0) = -R_adapted(0, 0); + R_adapted(1, 0) = -R_adapted(1, 0); + R_adapted(2, 0) = -R_adapted(2, 0); + + /* fill in new attitude data */ + _R = R_adapted; + + /* lastly, roll- and yawspeed have to be swaped */ + float helper = rollspeed; + rollspeed = -yawspeed; + yawspeed = helper; + } + + const matrix::Eulerf euler_angles(_R); + + vehicle_attitude_setpoint_poll(); + + // 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); + + vehicle_control_mode_poll(); + vehicle_manual_poll(euler_angles.psi()); + 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) { + _spoiler_setpoint_with_slewrate.setForcedValue(0.f); + _flaps_setpoint_with_slewrate.setForcedValue(0.f); + 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 (_att_sp.reset_rate_integrals) { + _rate_control.resetIntegral(); + } + + /* Reset integrators if the aircraft is on ground + * or a multicopter (but not transitioning VTOL or tailsitter) + */ + if (_landed + || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) { + + _rate_control.resetIntegral(); + } + + // update saturation status from control allocation feedback + control_allocator_status_s control_allocator_status; + + if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; + + if (!control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } + } + } + + // TODO: send the unallocated value directly for better anti-windup + _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + } + + _flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled; + + /* 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_stall.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_r_vmin.get(), + 0.0f); + trim_pitch += interpolate(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_p_vmin.get(), + 0.0f); + trim_yaw += interpolate(airspeed, _param_fw_airspd_stall.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()); + } + + /* add trim increment if flaps are deployed */ + trim_roll += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_r_flps.get(); + trim_pitch += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_flps.get(); + + // add trim increment from spoilers (only pitch) + trim_pitch += _spoiler_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_spoil.get(); + + if (_vcontrol_mode.flag_control_rates_enabled) { + _rates_sp_sub.update(&_rates_sp); + + const Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw); + + /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ + Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, + _landed); + bool enable_dob = true; + + // PX4_INFO("==================="); + // PX4_INFO(" - PID output: %f, %f, %f", double(angular_acceleration_setpoint(0)), double(angular_acceleration_setpoint(1)), double(angular_acceleration_setpoint(2))); + if (enable_dob && !_landed) { + angular_acceleration_setpoint = _dob_control.update(rates, angular_acceleration_setpoint, dt, _landed); + } + + // PX4_INFO(" - DOB output: %f, %f, %f", double(angular_acceleration_setpoint(0)), double(angular_acceleration_setpoint(1)), double(angular_acceleration_setpoint(2))); + + float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0); + float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward; + _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = + (PX4_ISFINITE(roll_u)) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll; + + float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1); + float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward; + _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = + (PX4_ISFINITE(pitch_u)) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch; + + const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2); + float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward; + + _actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw, + -1.f, 1.f) : trim_yaw; + + if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) { + _rate_control.resetIntegral(); + } + + /* throttle passed through if it is finite */ + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = + (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() && + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] > 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; + } + } + + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] *= _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 + _actuator_controls.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get() + * constrain(_actuator_controls.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); + + _actuator_controls.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState(); + _actuator_controls.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState(); + _actuator_controls.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f; + // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future + _actuator_controls.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3; + + /* lazily publish the setpoint only once available */ + _actuator_controls.timestamp = hrt_absolute_time(); + _actuator_controls.timestamp_sample = att.timestamp; + + /* 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) { + _actuator_controls_0_pub.publish(_actuator_controls); + + if (!_vehicle_status.is_vtol) { + publishTorqueSetpoint(angular_velocity.timestamp_sample); + publishThrustSetpoint(angular_velocity.timestamp_sample); + } + } + + updateActuatorControlsStatus(dt); + } + + // backup schedule + ScheduleDelayed(20_ms); + + perf_end(_loop_perf); +} + +void DobRateControl::publishTorqueSetpoint(const hrt_abstime ×tamp_sample) +{ + vehicle_torque_setpoint_s v_torque_sp = {}; + v_torque_sp.timestamp = hrt_absolute_time(); + v_torque_sp.timestamp_sample = timestamp_sample; + v_torque_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_ROLL]; + v_torque_sp.xyz[1] = _actuator_controls.control[actuator_controls_s::INDEX_PITCH]; + v_torque_sp.xyz[2] = _actuator_controls.control[actuator_controls_s::INDEX_YAW]; + + _vehicle_torque_setpoint_pub.publish(v_torque_sp); +} + +void DobRateControl::publishThrustSetpoint(const hrt_abstime ×tamp_sample) +{ + vehicle_thrust_setpoint_s v_thrust_sp = {}; + v_thrust_sp.timestamp = hrt_absolute_time(); + v_thrust_sp.timestamp_sample = timestamp_sample; + v_thrust_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; + v_thrust_sp.xyz[1] = 0.0f; + v_thrust_sp.xyz[2] = 0.0f; + + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); +} + +void DobRateControl::updateActuatorControlsStatus(float dt) +{ + for (int i = 0; i < 4; i++) { + float control_signal; + + if (i <= actuator_controls_status_s::INDEX_YAW) { + // We assume that the attitude is actuated by control surfaces + // consuming power only when they move + control_signal = _actuator_controls.control[i] - _control_prev[i]; + _control_prev[i] = _actuator_controls.control[i]; + + } else { + control_signal = _actuator_controls.control[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 = _actuator_controls.timestamp; + + for (int i = 0; i < 4; 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 DobRateControl::task_spawn(int argc, char *argv[]) +{ + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + DobRateControl *instance = new DobRateControl(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 DobRateControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int DobRateControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +dob_rate_control is the fixed wing attitude controller. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("dob_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 dob_rate_control_main(int argc, char *argv[]) +{ + return DobRateControl::main(argc, argv); +} diff --git a/src/modules/dob_rate_control/dob_rate_control.hpp b/src/modules/dob_rate_control/dob_rate_control.hpp new file mode 100644 index 0000000000..38fcefca13 --- /dev/null +++ b/src/modules/dob_rate_control/dob_rate_control.hpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2022 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 "DobControl.hpp" + +#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 +#include +#include +#include + +using matrix::Eulerf; +using matrix::Quatf; + +using uORB::SubscriptionData; + +using namespace time_literals; + +static constexpr float kFlapSlewRate = 1.f; //minimum time from none to full flap deflection [s] +static constexpr float kSpoilerSlewRate = 1.f; //minimum time from none to full spoiler deflection [s] + +class DobRateControl final : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + DobRateControl(bool vtol = false); + ~DobRateControl() 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; + + void publishTorqueSetpoint(const hrt_abstime ×tamp_sample); + void publishThrustSetpoint(const hrt_abstime ×tamp_sample); + + uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)}; /**< vehicle attitude */ + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */ + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */ + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; + + uORB::SubscriptionMultiArray _control_allocator_status_subs{ORB_ID::control_allocator_status}; + + uORB::SubscriptionData _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + + uORB::Publication _actuator_controls_0_pub; + 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_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + + actuator_controls_s _actuator_controls{}; + manual_control_setpoint_s _manual_control_setpoint{}; + vehicle_attitude_setpoint_s _att_sp{}; + vehicle_control_mode_s _vcontrol_mode{}; + vehicle_rates_setpoint_s _rates_sp{}; + vehicle_status_s _vehicle_status{}; + + matrix::Dcmf _R{matrix::eye()}; + + perf_counter_t _loop_perf; + + hrt_abstime _last_run{0}; + + float _airspeed_scaling{1.0f}; + + bool _landed{true}; + + float _battery_scale{1.0f}; + + bool _flag_control_attitude_enabled_last{false}; + + float _energy_integration_time{0.0f}; + float _control_energy[4] {}; + float _control_prev[3] {}; + + SlewRate _spoiler_setpoint_with_slewrate; + SlewRate _flaps_setpoint_with_slewrate; + + DEFINE_PARAMETERS( + (ParamFloat) _param_fw_acro_x_max, + (ParamFloat) _param_fw_acro_y_max, + (ParamFloat) _param_fw_acro_z_max, + + (ParamFloat) _param_fw_airspd_max, + (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_flps, + (ParamFloat) _param_fw_dtrim_p_spoil, + (ParamFloat) _param_fw_dtrim_p_vmax, + (ParamFloat) _param_fw_dtrim_p_vmin, + (ParamFloat) _param_fw_dtrim_r_flps, + (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_flaps_lnd_scl, + (ParamFloat) _param_fw_flaps_to_scl, + (ParamFloat) _param_fw_spoilers_lnd, + (ParamFloat) _param_fw_spoilers_desc, + (ParamInt) _param_fw_spoilers_man, + + (ParamFloat) _param_fw_man_p_max, + (ParamFloat) _param_fw_man_p_sc, + (ParamFloat) _param_fw_man_r_max, + (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 + ) + + RateControl _rate_control; ///< class for rate control calculations + DobControl _dob_control; + + void updateActuatorControlsStatus(float dt); + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + void vehicle_control_mode_poll(); + void vehicle_manual_poll(const float yaw_body); + void vehicle_attitude_setpoint_poll(); + void vehicle_land_detected_poll(); + + float get_airspeed_and_update_scaling(); +}; diff --git a/src/modules/dob_rate_control/dob_rate_control_params.c b/src/modules/dob_rate_control/dob_rate_control_params.c new file mode 100644 index 0000000000..75c02edd77 --- /dev/null +++ b/src/modules/dob_rate_control/dob_rate_control_params.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +/** + * @file dob_rate_control_params.c + * + * Parameters defined by the fixed-wing attitude control task + * + * @author Jaeyoung Lim + */ + +/* + * Controller parameters, accessible via MAVLink + * + */ + +/** + * Enable DOB Controller. + * + * Enable dob controller to compensate for disturbances. + * + * @boolean + * @group DOB Rate Control + */ +PARAM_DEFINE_FLOAT(DOB_ENABLE, 0);