From 247bfc35c5b48e639b9bb7aef2d8ba1c1d94ed93 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Wed, 14 May 2025 08:31:03 +0200 Subject: [PATCH] Add iNDI control module This commit adds an implementation of a indi position controller for fixed-wings. --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 1 + boards/px4/sitl/default.px4board | 1 + .../fw_indi_pos_control/CMakeLists.txt | 44 ++ .../FixedwingIndiPosControl.cpp | 523 ++++++++++++++++++ .../FixedwingIndiPosControl.hpp | 211 +++++++ src/modules/fw_indi_pos_control/Kconfig | 12 + .../fw_indi_pos_control_params.c | 118 ++++ 7 files changed, 910 insertions(+) create mode 100644 src/modules/fw_indi_pos_control/CMakeLists.txt create mode 100644 src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp create mode 100644 src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp create mode 100644 src/modules/fw_indi_pos_control/Kconfig create mode 100644 src/modules/fw_indi_pos_control/fw_indi_pos_control_params.c diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 7597c989b3..adbd69a8f7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -14,6 +14,7 @@ control_allocator start # Start attitude controller. # fw_rate_control start +fw_indi_pos_control start fw_att_control start fw_mode_manager start fw_lat_lon_control start diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 28c16f931f..8f494c169a 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -23,6 +23,7 @@ CONFIG_MODULES_FW_MODE_MANAGER=y CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y CONFIG_FIGURE_OF_EIGHT=y CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_FW_INDI_POS_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/src/modules/fw_indi_pos_control/CMakeLists.txt b/src/modules/fw_indi_pos_control/CMakeLists.txt new file mode 100644 index 0000000000..f897988a96 --- /dev/null +++ b/src/modules/fw_indi_pos_control/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# 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__fw_indi_pos_control + MAIN fw_indi_pos_control + SRCS + FixedwingIndiPosControl.cpp + FixedwingIndiPosControl.hpp + DEPENDS + px4_work_queue + RateControl + SlewRate + ) diff --git a/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp new file mode 100644 index 0000000000..def37d9b38 --- /dev/null +++ b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp @@ -0,0 +1,523 @@ +/**************************************************************************** + * + * 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 "FixedwingIndiPosControl.hpp" + +using namespace time_literals; +using namespace matrix; + +using math::constrain; +using math::interpolate; +using math::radians; + +FixedwingIndiPosControl::FixedwingIndiPosControl(bool vtol) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingIndiPosControl::~FixedwingIndiPosControl() +{ + perf_free(_loop_perf); +} + +bool +FixedwingIndiPosControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +int +FixedwingIndiPosControl::parameters_update() +{ + _mass = _param_fw_mass.get(); + _K_x *= 0.f; + _K_x(0,0) = _param_kp_x.get(); + _K_x(1,1) = _param_kp_y.get(); + _K_x(2,2) = _param_kp_z.get(); + _K_q(0,0) = _param_rot_k_roll.get(); + _K_q(1,1) = _param_rot_k_pitch.get(); + _K_q(2,2) = 0.5; + return PX4_OK; +} + +void +FixedwingIndiPosControl::vehicle_acceleration_poll() +{ + vehicle_acceleration_s acceleration; + + if (_vehicle_acceleration_sub.update(&acceleration)) { + Dcmf R_ib(vehicle_attitude_); + _acc = R_ib * Vector3f(acceleration.xyz) - Vector3f{0.f, 0.f, 9.81f}; + } + + if (hrt_absolute_time() - acceleration.timestamp > 20_ms + && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + PX4_ERR("linear acceleration sample is too old"); + } +} + +void +FixedwingIndiPosControl::vehicle_attitude_poll() +{ + vehicle_attitude_s vehicle_attitude; + + if (_vehicle_attitude_sub.update(&vehicle_attitude)) { + vehicle_attitude_ = Quatf(vehicle_attitude.q); + } + +} + + +void +FixedwingIndiPosControl::vehicle_local_position_poll() +{ + vehicle_local_position_s pos; + + if (_vehicle_local_position_sub.update(&pos)) { + vehicle_position_ = Vector3f{pos.x, pos.y, pos.z}; + vehicle_velocity_ = Vector3f{pos.vx, pos.vy, pos.vz}; + // take accel from faster message, since 50Hz is too slow... + } +} + +void +FixedwingIndiPosControl::airspeed_poll() +{ + bool airspeed_valid = _airspeed_valid; + airspeed_validated_s airspeed_validated; + + if (_airspeed_validated_sub.update(&airspeed_validated)) { + + if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) + && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) + && (airspeed_validated.calibrated_airspeed_m_s > 0.0f)) { + + airspeed_valid = true; + + _airspeed_last_valid = airspeed_validated.timestamp; + _true_airspeed = airspeed_validated.true_airspeed_m_s; + _cal_airspeed = airspeed_validated.calibrated_airspeed_m_s; + } + + } else { + // no airspeed updates for one second + if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) { + airspeed_valid = false; + } + } + + _airspeed_valid = airspeed_valid; +} + +Quatf +FixedwingIndiPosControl::get_flat_attitude(Vector3f vel, Vector3f f) +{ + + Vector3f vel_air = vel - wind_estimate_; + // compute force component projected onto lift axis + Vector3f vel_normalized = vel_air.normalized(); + Vector3f f_lift = f - (f * vel_normalized) * vel_normalized; + Vector3f lift_normalized = f_lift.normalized(); + Vector3f wing_normalized = -vel_normalized.cross(lift_normalized); + // compute rotation matrix between ENU and FRD frame + Dcmf R_bi; + R_bi(0, 0) = vel_normalized(0); + R_bi(0, 1) = vel_normalized(1); + R_bi(0, 2) = vel_normalized(2); + R_bi(1, 0) = wing_normalized(0); + R_bi(1, 1) = wing_normalized(1); + R_bi(1, 2) = wing_normalized(2); + R_bi(2, 0) = lift_normalized(0); + R_bi(2, 1) = lift_normalized(1); + R_bi(2, 2) = lift_normalized(2); + R_bi.renormalize(); + + float rho_corrected; + + if (_cal_airspeed >= _stall_speed) { + rho_corrected = _rho * powf(_cal_airspeed / _true_airspeed, 2); + + } else { + rho_corrected = _rho; + } + + // compute required AoA + Vector3f f_phi = R_bi * f_lift; + float AoA = ((2.f * f_phi(2)) / (rho_corrected * _area * (vel_air * vel_air) + 0.001f) - _C_L0) / _C_L1 - _aoa_offset; + // compute final rotation matrix + Eulerf e(0.f, AoA, 0.f); + Dcmf R_pitch(e); + Dcmf Rotation(R_pitch * R_bi); + // switch from FRD to ENU frame + Rotation(1, 0) *= -1.f; + Rotation(1, 1) *= -1.f; + Rotation(1, 2) *= -1.f; + Rotation(2, 0) *= -1.f; + Rotation(2, 1) *= -1.f; + Rotation(2, 2) *= -1.f; + + Quatf q(Rotation.transpose()); + return q; +} + +Vector3f +FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref) +{ + Dcmf R_ib(vehicle_attitude_); + Dcmf R_bi(R_ib.transpose()); + + // apply LP filter to acceleration & velocity + Vector3f acc_filtered; + // Poll acceleration + PX4_INFO("[PX4PosiNDI] - _acc: %f, %f, %f", double(_acc(0)), double(_acc(1)), double(_acc(2))); + acc_filtered(0) = _lp_filter_accel[0].apply(_acc(0)); + acc_filtered(1) = _lp_filter_accel[1].apply(_acc(1)); + acc_filtered(2) = _lp_filter_accel[2].apply(_acc(2)); + + // ========================================= + // apply PD control law on the body position + // ========================================= + Vector3f acc_command_local = (_K_x * R_bi * (pos_ref - vehicle_position_) + _K_v * R_bi * + (vel_ref - vehicle_velocity_) + _K_a * R_bi * + (acc_ref - _acc)); + Vector3f acc_command = R_ib * acc_command_local + acc_ref; ///TODO: Why add acc_ref again? + + // ================================== + // compute expected aerodynamic force + // ================================== + Vector3f _wind_estimate{0.0, 0.0, 0.0}; ///TODO: Plug in wind estimate + Vector3f airvel_body = R_bi * (vehicle_velocity_ - _wind_estimate); + + float angle_of_attack = atan2f(airvel_body(2), airvel_body(0)) + _aoa_offset; + + float C_l = _C_L0 + _C_L1 * angle_of_attack; + float C_d = _C_D0 + _C_D1 * angle_of_attack + _C_D2 * powf(angle_of_attack, 2); + // compute actual air density + float rho_corrected; + + if (_cal_airspeed >= _stall_speed) { + rho_corrected = _rho * powf(_cal_airspeed / _true_airspeed, 2); + + } else { + rho_corrected = _rho; + } + + float factor = -0.5f * rho_corrected * _area * sqrtf(airvel_body * airvel_body); + + // Wind axis + Vector3f w_x = airvel_body; + Vector3f w_z = w_x.cross(Vector3f{0.f, 1.f, 0.f}); + Vector3f f_current = R_ib * (factor * (C_l * w_z + C_d * w_x)); + // apply LP filter to force + Vector3f f_current_filtered; + f_current_filtered(0) = _lp_filter_force[0].apply(f_current(0)); + f_current_filtered(1) = _lp_filter_force[1].apply(f_current(1)); + f_current_filtered(2) = _lp_filter_force[2].apply(f_current(2)); + + // ================================ + // get force command in world frame + // ================================ + Vector3f f_command = _mass * (acc_command - acc_filtered) + f_current_filtered; + + // ============================================================================================================ + // apply some filtering to the force command. This introduces some time delay, + // which is not desired for stability reasons, but it rejects some of the noise fed to the low-level controller + // ============================================================================================================ + f_command(0) = _lp_filter_ctrl0[0].apply(f_command(0)); + f_command(1) = _lp_filter_ctrl0[1].apply(f_command(1)); + f_command(2) = _lp_filter_ctrl0[2].apply(f_command(2)); + _f_command = f_command; + // limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 15° aoa) + + // ==================================================================== + // saturate force command to avoid overly agressive maneuvers and stall + // ==================================================================== + if (_switch_saturation) { + float speed = airvel_body * airvel_body; + // compute maximum achievable force + float f_max; + + if (speed > _stall_speed) { + f_max = -factor * sqrtf(airvel_body * airvel_body) * (_C_L0 + _C_L1 * 0.25f); // assume stall at 15° AoA + + } else { + f_max = -factor * _stall_speed * (_C_L0 + _C_L1 * 0.25f); // assume stall at 15° AoA + } + + // compute current command + float f_now = sqrtf(f_command * f_command); + + // saturate current command + if (f_now > f_max) { + f_command = f_max / f_now * f_command; + } + } + + PX4_INFO("[PX4PosiNDI] - f_command: %f, %f, %f", double(f_command(0)), double(f_command(1)), double(f_command(2))); + PX4_INFO("[PX4PosiNDI] - vel_ref: %f, %f, %f", double(vel_ref(0)), double(vel_ref(1)), double(vel_ref(2))); + + // ========================================================================== + // get required attitude (assuming we can fly the target velocity), and error + // ========================================================================== + Dcmf R_ref(get_flat_attitude(vel_ref, f_command)); + Quatf ref_attitude(R_ref); + PX4_INFO("[PX4PosiNDI] - R_ref: %f, %f, %f, %f", double(ref_attitude(0)), double(ref_attitude(1)), + double(ref_attitude(2)), double(ref_attitude(3))); + // get attitude error + Dcmf R_ref_true(R_ref.transpose()*R_ib); + // get required rotation vector (in body frame) + AxisAnglef q_err(R_ref_true); + Vector3f w_err; + + // project rotation angle to [-pi,pi] + if (q_err.angle()*q_err.angle() < M_PI_F * M_PI_F) { + w_err = -q_err.angle() * q_err.axis(); + + } else { + if (q_err.angle() > 0.f) { + w_err = (2.f * M_PI_F - (float)fmod(q_err.angle(), 2.f * M_PI_F)) * q_err.axis(); + + } else { + w_err = (-2.f * M_PI_F - (float)fmod(q_err.angle(), 2.f * M_PI_F)) * q_err.axis(); + } + } + + // TODO: Reference rate to rate controller + // ========================================= + // apply PD control law on the body attitude + // ========================================= + // Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref; + Vector3f rot_vel_command = _K_q * w_err; + PX4_INFO("[PX4PosiNDI] - w_err: %f, %f, %f", double(w_err(0)), double(w_err(1)), double(w_err(2))); + + if (sqrtf(w_err * w_err) > M_PI_F) { + PX4_ERR("rotation angle larger than pi: \t%.2f, \t%.2f, \t%.2f", (double)sqrtf(w_err * w_err), (double)q_err.angle(), + (double)(q_err.axis()*q_err.axis())); + } + + + // ============================================================== + // overwrite rudder rot_acc_command with turn coordination values + // ============================================================== + Vector3f vel_air = vehicle_velocity_ - _wind_estimate; + Vector3f vel_normalized = vel_air.normalized(); + Vector3f acc_normalized = acc_filtered.normalized(); + PX4_INFO("[PX4PosiNDI] - acc_normalized: %f, %f, %f", double(acc_normalized(0)), double(acc_normalized(1)), + double(acc_normalized(2))); + + // compute ideal angular velocity + Vector3f omega_turn_ref_normalized = vel_normalized.cross(acc_normalized); + Vector3f omega_turn_ref; + // constuct acc perpendicular to flight path + Vector3f acc_perp = acc_filtered - (acc_filtered * vel_normalized) * vel_normalized; + + if (_airspeed_valid && _cal_airspeed > _stall_speed) { + omega_turn_ref = sqrtf(acc_perp * acc_perp) / (_true_airspeed) * R_bi * omega_turn_ref_normalized.normalized(); + //PX4_INFO("yaw rate ref, yaw rate: \t%.2f\t%.2f", (double)(omega_turn_ref(2)), (double)(omega_filtered(2))); + + } else { + omega_turn_ref = sqrtf(acc_perp * acc_perp) / (_stall_speed) * R_bi * omega_turn_ref_normalized.normalized(); + //PX4_ERR("No valid airspeed message detected or airspeed too low"); + } + + // apply some smoothing since we don't want HF components in our rudder output + omega_turn_ref(2) = _lp_filter_rud.apply(omega_turn_ref(2)); + PX4_INFO("[PX4PosiNDI] - omega_turn_ref: %f, %f, %f", double(omega_turn_ref(0)), double(omega_turn_ref(1)), + double(omega_turn_ref(2))); + rot_vel_command(2) = omega_turn_ref(2); + + return rot_vel_command; +} + +void FixedwingIndiPosControl::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(); + } + + // Poll local position + vehicle_attitude_poll(); + + vehicle_local_position_poll(); + vehicle_acceleration_poll(); + + // Poll airspeed + airspeed_poll(); + + // Poll trajectory setpoints + trajectory_setpoint_s trajectory_setpoint; + + if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { + ///TODO: Check if references are invalid + pos_ref_ = Vector3f(trajectory_setpoint.position); + vel_ref_ = Vector3f(trajectory_setpoint.velocity); + acc_ref_ = Vector3f(trajectory_setpoint.acceleration); + + ///TODO: Publish offboard mode + + ///TODO: Publish bodyrate commands in offboard mode + // if (_control_mode.flag_control_offboard_enabled) { + // } + + } + + ///TODO: Need to handle invalid velocity (zero) + pos_ref_(2) = -30.0; + + vel_ref_(0) = 15.0; + vel_ref_(1) = 0.0; + vel_ref_(2) = 0.0; + + // Compute bodyrate commands + Vector3f rate_command = computeIndi(pos_ref_, vel_ref_, acc_ref_); + PX4_INFO("[PX4PosiNDI] angular_rate_command: %f, %f, %f", double(rate_command(0)), double(rate_command(1)), + double(rate_command(2))); + + vehicle_rates_setpoint_s rates_sp; + rates_sp.thrust_body[0] = 0.5; ///TODO: Add some throttle + rates_sp.thrust_body[1] = 0.0; + rates_sp.thrust_body[2] = 0.0; + rates_sp.roll = rate_command(0); + rates_sp.pitch = rate_command(1); + rates_sp.yaw = rate_command(2); + _rate_sp_pub.publish(rates_sp); + + // ================================= + // publish offboard control commands + // ================================= + offboard_control_mode_s ocm{}; + ocm.body_rate = true; + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); + + + + } + + // backup schedule + ScheduleDelayed(20_ms); + + perf_end(_loop_perf); +} + +int FixedwingIndiPosControl::task_spawn(int argc, char *argv[]) +{ + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + FixedwingIndiPosControl *instance = new FixedwingIndiPosControl(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 FixedwingIndiPosControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FixedwingIndiPosControl::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_indi_pos_control_main(int argc, char *argv[]) +{ + return FixedwingIndiPosControl::main(argc, argv); +} diff --git a/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp new file mode 100644 index 0000000000..ae6749cd2c --- /dev/null +++ b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp @@ -0,0 +1,211 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +using matrix::Eulerf; +using matrix::Quatf; + +using uORB::SubscriptionData; + +using namespace time_literals; + +class FixedwingIndiPosControl final : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + FixedwingIndiPosControl(bool vtol = false); + ~FixedwingIndiPosControl() 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; + + + matrix::Quatf get_flat_attitude(matrix::Vector3f vel, matrix::Vector3f f); + + matrix::Vector3f computeIndi(matrix::Vector3f pos_ref, matrix::Vector3f vel_ref, matrix::Vector3f acc_ref); + + + 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 _manual_control_setpoint_sub{ORB_ID(manual_control_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::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; // linear acceleration + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + + uORB::SubscriptionMultiArray _control_allocator_status_subs{ORB_ID::control_allocator_status}; + + + uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; + + manual_control_setpoint_s _manual_control_setpoint{}; + vehicle_control_mode_s _vcontrol_mode{}; + vehicle_rates_setpoint_s _rates_sp{}; + vehicle_status_s _vehicle_status{}; + + perf_counter_t _loop_perf; + + hrt_abstime _last_run{0}; + hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts. + + // controller frequency + const float _sample_frequency = 200.f; + // Low-Pass filters stage 1 + const float _cutoff_frequency_1 = 20.f; + // smoothing filter to reject HF noise in control output + const float _cutoff_frequency_smoothing = + 20.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz) + math::LowPassFilter2p _lp_filter_accel[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // linear acceleration + math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // force command + math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // body rates + math::LowPassFilter2p _lp_filter_ctrl0[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}}; // force command stage 1 + math::LowPassFilter2p _lp_filter_rud {_sample_frequency, 10}; // rudder command + + matrix::Vector3f vehicle_position_; + matrix::Vector3f vehicle_velocity_; + matrix::Vector3f _acc; + matrix::Vector3f wind_estimate_; + matrix::Quatf vehicle_attitude_; + float _cal_airspeed{0.0f}; + float _rho{1.0}; + float _true_airspeed{1.0}; + + + matrix::Vector3f pos_ref_; + matrix::Vector3f vel_ref_; + matrix::Vector3f acc_ref_; + + matrix::Vector3f _f_command {}; + matrix::Vector3f _m_command {}; + + matrix::Matrix3f _K_x; + matrix::Matrix3f _K_v; + matrix::Matrix3f _K_a; + matrix::Matrix3f _K_q; + + // Vehicle parameters (From Easyglider) + float _aoa_offset{0.07}; + float _C_L0{0.3}; + float _C_D0{0.0288}; + float _C_L1{2.354}; + float _C_D1{0.3783}; + float _C_D2{1.984}; + float _area{0.4}; + float _stall_speed{1.0}; + float _mass{1.0}; + + bool _landed{true}; + bool _switch_saturation{true}; + bool _airspeed_valid{false}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_fw_mass, + (ParamFloat) _param_kp_x, + (ParamFloat) _param_kp_y, + (ParamFloat) _param_kp_z, + (ParamFloat) _param_rot_k_roll, + (ParamFloat) _param_rot_k_pitch + ) + + RateControl _rate_control; ///< class for rate control calculations + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + void airspeed_poll(); + void vehicle_acceleration_poll(); + void vehicle_local_position_poll(); + void vehicle_attitude_poll(); +}; diff --git a/src/modules/fw_indi_pos_control/Kconfig b/src/modules/fw_indi_pos_control/Kconfig new file mode 100644 index 0000000000..9ad1d45d70 --- /dev/null +++ b/src/modules/fw_indi_pos_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_FW_INDI_POS_CONTROL + bool "fw_indi_pos_control" + default n + ---help--- + Enable support for fw_att_control + +menuconfig USER_FW_INDI_POS_CONTROL + bool "fw_indi_pos_control running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_FW_INDI_POS_CONTROL + ---help--- + Put fw_indi_pos_control in userspace memory diff --git a/src/modules/fw_indi_pos_control/fw_indi_pos_control_params.c b/src/modules/fw_indi_pos_control/fw_indi_pos_control_params.c new file mode 100644 index 0000000000..4ee1e90172 --- /dev/null +++ b/src/modules/fw_indi_pos_control/fw_indi_pos_control_params.c @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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_indi_pos_control_params.c + * + * Parameters defined by the fixed-wing rate control task + * + * @author Lorenz Meier + * @author Thomas Gubler + */ + +/** + * total takeoff mass + * + * This is the mass of the aircraft, used for the INDI + * + * @unit kg + * @min 1.0 + * @max 2.0 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(FW_INDI_MASS, 1.4f); + +// ======================================================== +// =================== CONTROL GAINS ====================== +// ======================================================== +/** + * control gain of position PD-controller (body x-direction) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ + PARAM_DEFINE_FLOAT(FW_INDI_KP_X, 1.0f); + + /** + * control gain of position PD-controller (body x-direction) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ + PARAM_DEFINE_FLOAT(FW_INDI_KP_Y, 1.0f); + + /** + * control gain of position PD-controller (body x-direction) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ + PARAM_DEFINE_FLOAT(FW_INDI_KP_Z, 1.0f); + + /** + * control gain of attitude PD-controller (body roll-direction) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(FW_INDI_K_ROLL, 30.0f); + + /** + * control gain of attitude PD-controller (body roll-direction) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ + PARAM_DEFINE_FLOAT(FW_INDI_K_PITCH, 30.0f);