diff --git a/msg/VehicleRatesSetpoint.msg b/msg/VehicleRatesSetpoint.msg index 35a06c35aa..813b23fa96 100644 --- a/msg/VehicleRatesSetpoint.msg +++ b/msg/VehicleRatesSetpoint.msg @@ -5,6 +5,8 @@ float32 roll # [rad/s] roll rate setpoint float32 pitch # [rad/s] pitch rate setpoint float32 yaw # [rad/s] yaw rate setpoint +float32[3] accel_feedforward # [rad/s²] angular acceleration feedforward + # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1] diff --git a/src/lib/mathlib/math/filter/second_order_reference_model.hpp b/src/lib/mathlib/math/filter/second_order_reference_model.hpp index 08fd1202f7..ac584164f6 100644 --- a/src/lib/mathlib/math/filter/second_order_reference_model.hpp +++ b/src/lib/mathlib/math/filter/second_order_reference_model.hpp @@ -41,8 +41,12 @@ #pragma once +#include +#include + #include -#include + +#include "lib/matrix/matrix/math.hpp" namespace math { @@ -150,9 +154,6 @@ public: // take a step forward from the last state (and input), update the filter states integrateStates(time_step, last_state_sample_, last_rate_sample_); - // instantaneous acceleration from current input / state - filter_accel_ = calculateInstantaneousAcceleration(state_sample, rate_sample); - // store the current samples last_state_sample_ = state_sample; last_rate_sample_ = rate_sample; @@ -174,7 +175,7 @@ public: last_rate_sample_ = rate; } -private: +protected: // A conservative multiplier (>=2) on sample frequency to bound the maximum time step static constexpr float kSampleRateMultiplier = 4.0f; @@ -247,7 +248,7 @@ private: * @param[in] state_sample [units] * @param[in] rate_sample [units/s] */ - void integrateStatesForwardEuler(const float time_step, const T &state_sample, const T &rate_sample) + virtual void integrateStatesForwardEuler(const float time_step, const T &state_sample, const T &rate_sample) { // general notation for what follows: // c: continuous @@ -278,6 +279,9 @@ private: // discrete state transition transitionStates(state_matrix, input_matrix, state_sample, rate_sample); + + // instantaneous acceleration from current input / state + filter_accel_ = calculateInstantaneousAcceleration(state_sample, rate_sample); } /** @@ -324,6 +328,9 @@ private: // discrete state transition transitionStates(state_matrix, input_matrix, state_sample, rate_sample); + + // instantaneous acceleration from current input / state + filter_accel_ = calculateInstantaneousAcceleration(state_sample, rate_sample); } /** diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index 75dd515ddb..866d461fbd 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -36,8 +36,8 @@ px4_add_module( MAIN fw_att_control SRCS FixedwingAttitudeControl.cpp - FixedwingAttitudeControl.hpp + att_ref_model.cpp fw_pitch_controller.cpp fw_roll_controller.cpp fw_wheel_controller.cpp diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index be4aaf9b23..ef7d3bfb6d 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -119,16 +119,6 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) } } -void -FixedwingAttitudeControl::vehicle_attitude_setpoint_poll() -{ - if (_att_sp_sub.update(&_att_sp)) { - _rates_sp.thrust_body[0] = _att_sp.thrust_body[0]; - _rates_sp.thrust_body[1] = _att_sp.thrust_body[1]; - _rates_sp.thrust_body[2] = _att_sp.thrust_body[2]; - } -} - void FixedwingAttitudeControl::vehicle_land_detected_poll() { @@ -260,7 +250,16 @@ void FixedwingAttitudeControl::Run() vehicle_manual_poll(euler_angles.psi()); - vehicle_attitude_setpoint_poll(); + // attitude setpoint poll + _reference_model.update(); + + _att_sp = _reference_model.getOutput(); + matrix::Vector2f vel_feedforward = _reference_model.getRateFeedforward(); + matrix::Vector2f acc_feedforward = _reference_model.getTorqueFeedforward(); + + _rates_sp.thrust_body[0] = _att_sp.thrust_body[0]; + _rates_sp.thrust_body[1] = _att_sp.thrust_body[1]; + _rates_sp.thrust_body[2] = _att_sp.thrust_body[2]; // 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); @@ -371,10 +370,14 @@ void FixedwingAttitudeControl::Run() } /* Publish the rate setpoint for analysis once available */ - _rates_sp.roll = body_rates_setpoint(0); - _rates_sp.pitch = body_rates_setpoint(1); + _rates_sp.roll = body_rates_setpoint(0) + vel_feedforward(0); + _rates_sp.pitch = body_rates_setpoint(1) + vel_feedforward(1); _rates_sp.yaw = body_rates_setpoint(2); + _rates_sp.accel_feedforward[0] = acc_feedforward(0); + _rates_sp.accel_feedforward[1] = acc_feedforward(1); + _rates_sp.accel_feedforward[2] = 0.f; + _rates_sp.timestamp = hrt_absolute_time(); _rate_sp_pub.publish(_rates_sp); @@ -404,6 +407,7 @@ void FixedwingAttitudeControl::Run() } else { // full manual _wheel_ctrl.reset_integrator(); + _reference_model.reset(); _landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(_manual_control_setpoint.yaw) ? _manual_control_setpoint.yaw : 0.f; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 6c902878e1..92bd8ca62a 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include "att_ref_model.h" #include "fw_pitch_controller.h" #include "fw_roll_controller.h" #include "fw_wheel_controller.h" @@ -98,7 +99,6 @@ private: 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 _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */ uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ @@ -165,9 +165,10 @@ private: YawController _yaw_ctrl; WheelController _wheel_ctrl; + FixedwingAttitudeReferenceModel _reference_model; + void parameters_update(); void vehicle_manual_poll(const float yaw_body); - void vehicle_attitude_setpoint_poll(); void vehicle_land_detected_poll(); float get_airspeed_constrained(); }; diff --git a/src/modules/fw_att_control/att_ref_model.cpp b/src/modules/fw_att_control/att_ref_model.cpp new file mode 100644 index 0000000000..160cc513a9 --- /dev/null +++ b/src/modules/fw_att_control/att_ref_model.cpp @@ -0,0 +1,140 @@ +#include "att_ref_model.h" + +#include + +#include +#include "lib/slew_rate/SlewRate.hpp" + +float REFERENCE_MODEL_DAMPING{1.f}; + +FixedwingAttitudeReferenceModel::FixedwingAttitudeReferenceModel() : + ModuleParams(nullptr) +{ + parameters_update(); +} + +void FixedwingAttitudeReferenceModel::update() +{ + parameters_update(); + + /* We check if the attitude setpoint topic got updated externally, put in throughput mode, + if attitude reference got updated, but into filter mode. If none, keep the current mode. + If both put throughput mode for safety. */ + _att_sp_sub.update(); + + if (_att_sp_sub.get().timestamp > _last_att_setpoint_timestamp) { + if (_mode == ModelMode::MODELMODE_FILTERING) { + PX4_INFO("FixedWingAttitudeReferenceModel: Switch to throughput mode"); + } + + _mode = ModelMode::MODELMODE_THROUGHPUT; + + } else if (_att_ref_sp_sub.updated()) { + _att_ref_sp_sub.update(); + + if (_mode == ModelMode::MODELMODE_THROUGHPUT) { + PX4_INFO("FixedWingAttitudeReferenceModel: Switch to filtering mode"); + } + + _mode = ModelMode::MODELMODE_FILTERING; + + } + + hrt_abstime now = hrt_absolute_time(); + + _attitiude_rate_feedforward_output = matrix::Vector2f{}; + _attitiude_torque_feedforward_output = matrix::Vector2f{}; + + if (_mode == ModelMode::MODELMODE_FILTERING) { + + if (!_is_initialized) { + + _is_initialized = true; + _roll_ref_model.reset(_att_ref_sp_sub.get().roll_body); + _pitch_ref_model.reset(_att_ref_sp_sub.get().pitch_body); + + } else { + if (_att_ref_sp_sub.get().timestamp > _last_update_timestamp) { + _roll_ref_model.update(static_cast(_att_ref_sp_sub.get().timestamp - _last_update_timestamp) / 1_s, + _last_attitude_reference_setpoint.roll_body); + _pitch_ref_model.update(static_cast(_att_ref_sp_sub.get().timestamp - _last_update_timestamp) / 1_s, + _last_attitude_reference_setpoint.pitch_body); + _last_update_timestamp = _att_ref_sp_sub.get().timestamp; + } + + _roll_ref_model.update(static_cast(math::max(now - _last_update_timestamp, static_cast(0U))) / 1_s, + _att_ref_sp_sub.get().roll_body); + _pitch_ref_model.update(static_cast(math::max(now - _last_update_timestamp, static_cast(0U))) / 1_s, + _att_ref_sp_sub.get().pitch_body); + + } + + _last_attitude_reference_setpoint = _att_ref_sp_sub.get(); + + _last_update_timestamp = now; + + _attitude_setpoint_output = _att_ref_sp_sub.get(); + _attitude_setpoint_output.timestamp = now; + + if (_param_ref_r_en.get()) { + _attitude_setpoint_output.roll_body = _roll_ref_model.getState(); + _attitiude_rate_feedforward_output(0) = _roll_ref_model.getRate(); + _attitiude_torque_feedforward_output(0) = _roll_ref_model.getAccel(); + } + + if (_param_ref_p_en.get()) { + _attitude_setpoint_output.pitch_body = _pitch_ref_model.getState(); + _attitiude_rate_feedforward_output(1) = _pitch_ref_model.getRate(); + _attitiude_torque_feedforward_output(1) = _pitch_ref_model.getAccel(); + + } + + // Other fields in the attitude setpoints are passed through + + // Publish attitude setpoint for logging + _att_sp_pub.publish(_attitude_setpoint_output); + _last_att_setpoint_timestamp = _attitude_setpoint_output.timestamp; + + } else { + _attitude_setpoint_output = _att_sp_sub.get(); + _roll_ref_model.reset(_att_sp_sub.get().roll_body); + _pitch_ref_model.reset(_att_sp_sub.get().pitch_body); + _is_initialized = true; + _last_update_timestamp = now; + _last_att_setpoint_timestamp = _att_sp_sub.get().timestamp; + } +} + +void FixedwingAttitudeReferenceModel::reset() +{ + _is_initialized = false; + + if (_mode == ModelMode::MODELMODE_FILTERING) { + _att_ref_sp_sub.update(); + _attitude_setpoint_output = _att_ref_sp_sub.get(); + + } else { + _att_sp_sub.update(); + _attitude_setpoint_output = _att_sp_sub.get(); + } + + _attitiude_rate_feedforward_output = matrix::Vector2f{}; + _attitiude_torque_feedforward_output = matrix::Vector2f{}; +} + +void FixedwingAttitudeReferenceModel::parameters_update() +{ + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); + + _roll_ref_model.setParameters(_param_ref_r_freq.get(), REFERENCE_MODEL_DAMPING, _param_ref_r_vel_limit.get(), + _param_ref_r_acc_limit.get()); + _pitch_ref_model.setParameters(_param_ref_p_freq.get(), REFERENCE_MODEL_DAMPING, _param_ref_p_vel_limit.get(), + _param_ref_p_acc_limit.get()); + } +} diff --git a/src/modules/fw_att_control/att_ref_model.h b/src/modules/fw_att_control/att_ref_model.h new file mode 100644 index 0000000000..6139ffc0af --- /dev/null +++ b/src/modules/fw_att_control/att_ref_model.h @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * Copyright (c) 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 + +using namespace time_literals; + +class AttitudeReferenceModel : public math::SecondOrderReferenceModel +{ +public: + AttitudeReferenceModel() + { + setDiscretizationMethod(math::SecondOrderReferenceModel::DiscretizationMethod::kForwardEuler); + }; + + /** + * Set the system parameters + * + * Calculates the damping coefficient, spring constant, and maximum allowed + * time step based on the natural frequency. + * + * @param[in] natural_freq The desired undamped natural frequency of the system [rad/s] + * @param[in] damping_ratio The desired damping ratio of the system + * @param[in] vel_limit the limit for the velocity [rad/s] + * @param[in] acc_limit the limit for the acceleration [rad/s^2] + * @param[in] jerk_limit set maximum jerk [rad/s^3]. Optional, only used in kForwardEuler mode + * @return Whether or not the param set was successful + */ + bool setParameters(const float natural_freq, const float damping_ratio, float vel_limit = INFINITY, + float acc_limit = INFINITY) + { + if (acc_limit < FLT_EPSILON) { + acc_limit_ = INFINITY; + + } else { + acc_limit_ = acc_limit; + } + + if (vel_limit < FLT_EPSILON) { + vel_limit_ = INFINITY; + + } else { + vel_limit_ = vel_limit; + } + + return math::SecondOrderReferenceModel::setParameters(natural_freq, damping_ratio); + } + +private: + float vel_limit_; /** velocity limit [rad/s]*/ + float acc_limit_; /** acceleration limit [rad/s^2]*/ + + /** + * Take one integration step using Euler-forward integration + * + * @param[in] time_step Integration time [s] + * @param[in] state_sample [rad] + * @param[in] rate_sample [rad/s] + */ + void integrateStatesForwardEuler(const float time_step, const float &state_sample, const float &rate_sample) override + { + filter_accel_ = calculateInstantaneousAcceleration(state_sample, rate_sample); + + if (filter_accel_ > 0.f) { + filter_accel_ = math::min(math::min(filter_accel_, (vel_limit_ - filter_rate_) / time_step), acc_limit_); + + } else { + filter_accel_ = math::max(math::max(filter_accel_, (-vel_limit_ - filter_rate_) / time_step), -acc_limit_); + } + + const float new_rate = filter_rate_ + filter_accel_ * time_step; + const float new_state = filter_state_ + filter_rate_ * time_step; + + filter_state_ = new_state; + filter_rate_ = new_rate; + } +}; + +class FixedwingAttitudeReferenceModel : public ModuleParams +{ +public: + FixedwingAttitudeReferenceModel(); + ~FixedwingAttitudeReferenceModel() = default; + + void update(); + + void reset(); + + const vehicle_attitude_setpoint_s &getOutput() {return _attitude_setpoint_output;}; + + const matrix::Vector2f &getRateFeedforward() {return _attitiude_rate_feedforward_output;}; + const matrix::Vector2f &getTorqueFeedforward() {return _attitiude_torque_feedforward_output;}; + +private: + + enum class ModelMode { + MODELMODE_THROUGHPUT, + MODELMODE_FILTERING + } _mode{ModelMode::MODELMODE_THROUGHPUT}; /*< Mode in which the attitude reference model works on */ + + /** + * Check for parameter changes and update them if needed. + */ + void parameters_update(); + + AttitudeReferenceModel _roll_ref_model; /*< Second order reference filter for the roll angle */ + AttitudeReferenceModel _pitch_ref_model; /*< Second order reference filter for the pitch angle */ + bool _is_initialized{false}; /*< Flag indicating if the reference model is already initialized */ + uint64_t _last_att_setpoint_timestamp{UINT64_C(0)}; /*< Timestamp of the last own published vehicle attitude setpoint topic */ + hrt_abstime _last_update_timestamp{0U}; /*< Timestamp of the last update*/ + vehicle_attitude_setpoint_s _last_attitude_reference_setpoint; /*< Last attitude reference setpoint input */ + vehicle_attitude_setpoint_s _attitude_setpoint_output; /*< Attitude setpoint to be set by output*/ + matrix::Vector2f _attitiude_rate_feedforward_output; /*< Attitude rate feedforward output */ + matrix::Vector2f _attitiude_torque_feedforward_output; /*< Attitude torque feedforward output */ + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /*< parameter update subscription */ + uORB::SubscriptionData _att_ref_sp_sub{ORB_ID(vehicle_attitude_reference_setpoint)}; /*< vehicle attitude reference setpoint */ + uORB::SubscriptionData _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /*< vehicle attitude setpoint */ + uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; /*< vehicle attitude setpoint publication when reference model is active for logging */ + + DEFINE_PARAMETERS( + (ParamFloat) _param_ref_r_freq, + (ParamFloat) _param_ref_r_vel_limit, + (ParamFloat) _param_ref_r_acc_limit, + (ParamBool) _param_ref_r_en, + (ParamFloat) _param_ref_p_freq, + (ParamFloat) _param_ref_p_vel_limit, + (ParamFloat) _param_ref_p_acc_limit, + (ParamBool) _param_ref_p_en + ) +}; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 554eac360c..51f8d4dbb4 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -254,3 +254,87 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 30.0f); + +/** + * Natural frequency of the roll reference model + * + * Frequency of the critically damped second order roll reference model + * + * @min 0.0 + * @decimal 3 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_REF_R_FREQ, 5.0f); + +/** + * Velocity limit of the roll reference model + * + * Limit of the critically damped second order roll reference model velocity. A negative value disables the limit. + * + * @min -1.0 + * @decimal 3 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_REF_R_V_LIM, -1.0f); + +/** + * Acceleration limit of the roll reference model + * + * Limit of the critically damped second order roll reference model acceleration. A negative value disables the limit. + * + * @min -1.0 + * @decimal 3 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_REF_R_A_LIM, -1.0f); + +/** + * Enable roll reference model + * + * + * @boolean + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_REF_R_EN, 0); + +/** + * Natural frequency of the roll reference model + * + * Frequency of the critically damped second order pitch reference model + * + * @min 0.0 + * @decimal 3 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_REF_P_FREQ, 5.0f); + +/** + * Velocity limit of the pitch reference model + * + * Limit of the critically damped second order pitch reference model velocity. A negative value disables the limit. + * + * @min -1.0 + * @decimal 3 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_REF_P_V_LIM, -1.0f); + +/** + * Acceleration limit of the pitch reference model + * + * Limit of the critically damped second order pitch reference model acceleration. A negative value disables the limit. + * + * @min -1.0 + * @decimal 3 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_REF_P_A_LIM, -1.0f); + +/** + * Enable pitch reference model + * + * + * @boolean + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_REF_P_EN, 0); diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 1f2cff00d6..79a304d0ae 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -358,6 +358,7 @@ void FixedwingRateControl::Run() const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, _landed); + const Vector3f reference_ff(_param_fw_ra_ff.get() * _rates_sp.accel_feedforward[0], _param_fw_pa_ff.get() * _rates_sp.accel_feedforward[1], 0.f); const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get()); const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling; diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index ec3b7e1c9e..f1af869a77 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -182,6 +182,7 @@ private: (ParamFloat) _param_fw_man_y_sc, (ParamFloat) _param_fw_pr_ff, + (ParamFloat) _param_fw_pa_ff, (ParamFloat) _param_fw_pr_i, (ParamFloat) _param_fw_pr_imax, (ParamFloat) _param_fw_pr_p, @@ -189,6 +190,7 @@ private: (ParamFloat) _param_fw_rll_to_yaw_ff, (ParamFloat) _param_fw_rr_ff, + (ParamFloat) _param_fw_ra_ff, (ParamFloat) _param_fw_rr_i, (ParamFloat) _param_fw_rr_imax, (ParamFloat) _param_fw_rr_p, diff --git a/src/modules/fw_rate_control/fw_rate_control_params.c b/src/modules/fw_rate_control/fw_rate_control_params.c index 8493f11c72..a893c2ddbd 100644 --- a/src/modules/fw_rate_control/fw_rate_control_params.c +++ b/src/modules/fw_rate_control/fw_rate_control_params.c @@ -211,6 +211,18 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); */ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); +/** + * roll acceleration feedforward gain + * + * feedforward gain for acceleration to scale rad/s^2 to percentage output + * + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_RA_FF, 0.0f); + /** * Pitch rate feed forward * @@ -225,6 +237,19 @@ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); */ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); +/** + * pitch acceleration feedforward gain + * + * feedforward gain for acceleration to scale rad/s^2 to unitless output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @group FW Rate Control + */ +PARAM_DEFINE_FLOAT(FW_PA_FF, 0.0f); + /** * Yaw rate feed forward * diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 9452cebd2d..ba73674aa2 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -124,6 +124,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_angular_velocity", 20); add_topic("vehicle_attitude", 50); add_topic("vehicle_attitude_setpoint", 50); + add_topic("vehicle_attitude_reference_setpoint", 50); add_topic("vehicle_command"); add_topic("vehicle_command_ack"); add_topic("vehicle_constraints", 1000);