forked from Archive/PX4-Autopilot
FixedWingAttControl: Add a reference attitude mode.
This commit is contained in:
Konrad
parent
0c24cff5c2
commit
8212cc49af
|
|
@ -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]
|
||||
|
|
|
|||
|
|
@ -41,8 +41,12 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <matrix/SquareMatrix.hpp>
|
||||
|
||||
#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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -36,8 +36,8 @@ px4_add_module(
|
|||
MAIN fw_att_control
|
||||
SRCS
|
||||
FixedwingAttitudeControl.cpp
|
||||
FixedwingAttitudeControl.hpp
|
||||
|
||||
att_ref_model.cpp
|
||||
ecl_controller.cpp
|
||||
ecl_pitch_controller.cpp
|
||||
ecl_roll_controller.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);
|
||||
|
|
@ -383,10 +382,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);
|
||||
|
|
@ -415,6 +418,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;
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include "att_ref_model.h"
|
||||
#include "ecl_pitch_controller.h"
|
||||
#include "ecl_roll_controller.h"
|
||||
#include "ecl_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 */
|
||||
|
|
@ -164,9 +164,10 @@ private:
|
|||
ECL_YawController _yaw_ctrl;
|
||||
ECL_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();
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,140 @@
|
|||
#include "att_ref_model.h"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#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<float>(_att_ref_sp_sub.get().timestamp - _last_update_timestamp) / 1_s,
|
||||
_last_attitude_reference_setpoint.roll_body);
|
||||
_pitch_ref_model.update(static_cast<float>(_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<float>(math::max(now - _last_update_timestamp, static_cast<hrt_abstime>(0U))) / 1_s,
|
||||
_att_ref_sp_sub.get().roll_body);
|
||||
_pitch_ref_model.update(static_cast<float>(math::max(now - _last_update_timestamp, static_cast<hrt_abstime>(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());
|
||||
}
|
||||
}
|
||||
|
|
@ -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 <cstdint>
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
#include <lib/mathlib/math/filter/second_order_reference_model.hpp>
|
||||
#include <lib/slew_rate/SlewRateYaw.hpp>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class AttitudeReferenceModel : public math::SecondOrderReferenceModel<float>
|
||||
{
|
||||
public:
|
||||
AttitudeReferenceModel()
|
||||
{
|
||||
setDiscretizationMethod(math::SecondOrderReferenceModel<float>::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<float>::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<vehicle_attitude_setpoint_s> _att_ref_sp_sub{ORB_ID(vehicle_attitude_reference_setpoint)}; /*< vehicle attitude reference setpoint */
|
||||
uORB::SubscriptionData<vehicle_attitude_setpoint_s> _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /*< vehicle attitude setpoint */
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; /*< vehicle attitude setpoint publication when reference model is active for logging */
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::FW_REF_R_FREQ>) _param_ref_r_freq,
|
||||
(ParamFloat<px4::params::FW_REF_R_V_LIM>) _param_ref_r_vel_limit,
|
||||
(ParamFloat<px4::params::FW_REF_R_A_LIM>) _param_ref_r_acc_limit,
|
||||
(ParamBool<px4::params::FW_REF_R_EN>) _param_ref_r_en,
|
||||
(ParamFloat<px4::params::FW_REF_P_FREQ>) _param_ref_p_freq,
|
||||
(ParamFloat<px4::params::FW_REF_P_V_LIM>) _param_ref_p_vel_limit,
|
||||
(ParamFloat<px4::params::FW_REF_P_A_LIM>) _param_ref_p_acc_limit,
|
||||
(ParamBool<px4::params::FW_REF_P_EN>) _param_ref_p_en
|
||||
)
|
||||
};
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -360,8 +360,10 @@ void FixedwingRateControl::Run()
|
|||
const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt,
|
||||
_landed);
|
||||
|
||||
const float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0);
|
||||
const float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1);
|
||||
const float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(
|
||||
0) + _param_fw_ra_ff.get() * _rates_sp.accel_feedforward[0];
|
||||
const float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(
|
||||
1) + _param_fw_pa_ff.get() * _rates_sp.accel_feedforward[1];
|
||||
const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2);
|
||||
|
||||
const float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
|
||||
|
|
|
|||
|
|
@ -182,6 +182,7 @@ private:
|
|||
(ParamFloat<px4::params::FW_MAN_Y_SC>) _param_fw_man_y_sc,
|
||||
|
||||
(ParamFloat<px4::params::FW_PR_FF>) _param_fw_pr_ff,
|
||||
(ParamFloat<px4::params::FW_PA_FF>) _param_fw_pa_ff,
|
||||
(ParamFloat<px4::params::FW_PR_I>) _param_fw_pr_i,
|
||||
(ParamFloat<px4::params::FW_PR_IMAX>) _param_fw_pr_imax,
|
||||
(ParamFloat<px4::params::FW_PR_P>) _param_fw_pr_p,
|
||||
|
|
@ -189,6 +190,7 @@ private:
|
|||
|
||||
(ParamFloat<px4::params::FW_RLL_TO_YAW_FF>) _param_fw_rll_to_yaw_ff,
|
||||
(ParamFloat<px4::params::FW_RR_FF>) _param_fw_rr_ff,
|
||||
(ParamFloat<px4::params::FW_RA_FF>) _param_fw_ra_ff,
|
||||
(ParamFloat<px4::params::FW_RR_I>) _param_fw_rr_i,
|
||||
(ParamFloat<px4::params::FW_RR_IMAX>) _param_fw_rr_imax,
|
||||
(ParamFloat<px4::params::FW_RR_P>) _param_fw_rr_p,
|
||||
|
|
|
|||
|
|
@ -209,6 +209,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
|
||||
*
|
||||
|
|
@ -223,6 +235,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
|
||||
*
|
||||
|
|
|
|||
|
|
@ -116,6 +116,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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue