mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-09 02:01:36 +08:00
Add dob rate control module
This commit is contained in:
Jaeyoung Lim
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
)
|
||||
@@ -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 <px4_platform_common/defines.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
void DobControl::setGains(const Vector3f &tau)
|
||||
{
|
||||
_gain_tau = tau;
|
||||
}
|
||||
|
||||
void DobControl::setSaturationStatus(const Vector<bool, 3> &saturation_positive,
|
||||
const Vector<bool, 3> &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);
|
||||
}
|
||||
@@ -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 <matrix/matrix/math.hpp>
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <uORB/topics/rate_ctrl_status.h>
|
||||
|
||||
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<bool, 3> &saturation_positive,
|
||||
const matrix::Vector<bool, 3> &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<bool, 3> _control_allocator_saturation_negative;
|
||||
matrix::Vector<bool, 3> _control_allocator_saturation_positive;
|
||||
};
|
||||
@@ -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
|
||||
@@ -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<bool, 3> saturation_positive;
|
||||
Vector<bool, 3> 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);
|
||||
}
|
||||
@@ -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 <lib/rate_control/rate_control.hpp>
|
||||
#include "DobControl.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
#include <matrix/math.hpp>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_status.h>
|
||||
#include <uORB/topics/airspeed_validated.h>
|
||||
#include <uORB/topics/autotune_attitude_control_status.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/control_allocator_status.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/rate_ctrl_status.h>
|
||||
#include <uORB/topics/vehicle_angular_acceleration.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_thrust_setpoint.h>
|
||||
#include <uORB/topics/vehicle_torque_setpoint.h>
|
||||
|
||||
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<DobRateControl>, 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_s, 2> _control_allocator_status_subs{ORB_ID::control_allocator_status};
|
||||
|
||||
uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
||||
|
||||
uORB::Publication<actuator_controls_s> _actuator_controls_0_pub;
|
||||
uORB::Publication<actuator_controls_status_s> _actuator_controls_status_pub;
|
||||
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
|
||||
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
|
||||
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
|
||||
uORB::Publication<vehicle_torque_setpoint_s> _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<float, 3>()};
|
||||
|
||||
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<float> _spoiler_setpoint_with_slewrate;
|
||||
SlewRate<float> _flaps_setpoint_with_slewrate;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
|
||||
(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
|
||||
(ParamFloat<px4::params::FW_ACRO_Z_MAX>) _param_fw_acro_z_max,
|
||||
|
||||
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
|
||||
(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
|
||||
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
|
||||
(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
|
||||
|
||||
(ParamInt<px4::params::FW_ARSP_SCALE_EN>) _param_fw_arsp_scale_en,
|
||||
|
||||
(ParamBool<px4::params::FW_BAT_SCALE_EN>) _param_fw_bat_scale_en,
|
||||
|
||||
(ParamFloat<px4::params::FW_DTRIM_P_FLPS>) _param_fw_dtrim_p_flps,
|
||||
(ParamFloat<px4::params::FW_DTRIM_P_SPOIL>) _param_fw_dtrim_p_spoil,
|
||||
(ParamFloat<px4::params::FW_DTRIM_P_VMAX>) _param_fw_dtrim_p_vmax,
|
||||
(ParamFloat<px4::params::FW_DTRIM_P_VMIN>) _param_fw_dtrim_p_vmin,
|
||||
(ParamFloat<px4::params::FW_DTRIM_R_FLPS>) _param_fw_dtrim_r_flps,
|
||||
(ParamFloat<px4::params::FW_DTRIM_R_VMAX>) _param_fw_dtrim_r_vmax,
|
||||
(ParamFloat<px4::params::FW_DTRIM_R_VMIN>) _param_fw_dtrim_r_vmin,
|
||||
(ParamFloat<px4::params::FW_DTRIM_Y_VMAX>) _param_fw_dtrim_y_vmax,
|
||||
(ParamFloat<px4::params::FW_DTRIM_Y_VMIN>) _param_fw_dtrim_y_vmin,
|
||||
|
||||
(ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
|
||||
(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
|
||||
(ParamFloat<px4::params::FW_SPOILERS_LND>) _param_fw_spoilers_lnd,
|
||||
(ParamFloat<px4::params::FW_SPOILERS_DESC>) _param_fw_spoilers_desc,
|
||||
(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man,
|
||||
|
||||
(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
|
||||
(ParamFloat<px4::params::FW_MAN_P_SC>) _param_fw_man_p_sc,
|
||||
(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,
|
||||
(ParamFloat<px4::params::FW_MAN_R_SC>) _param_fw_man_r_sc,
|
||||
(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_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,
|
||||
(ParamFloat<px4::params::FW_PR_D>) _param_fw_pr_d,
|
||||
|
||||
(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_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,
|
||||
(ParamFloat<px4::params::FW_RR_D>) _param_fw_rr_d,
|
||||
|
||||
(ParamFloat<px4::params::FW_YR_FF>) _param_fw_yr_ff,
|
||||
(ParamFloat<px4::params::FW_YR_I>) _param_fw_yr_i,
|
||||
(ParamFloat<px4::params::FW_YR_IMAX>) _param_fw_yr_imax,
|
||||
(ParamFloat<px4::params::FW_YR_P>) _param_fw_yr_p,
|
||||
(ParamFloat<px4::params::FW_YR_D>) _param_fw_yr_d,
|
||||
|
||||
(ParamFloat<px4::params::TRIM_PITCH>) _param_trim_pitch,
|
||||
(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
|
||||
(ParamFloat<px4::params::TRIM_YAW>) _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();
|
||||
};
|
||||
@@ -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 <jalim@ethz.ch>
|
||||
*/
|
||||
|
||||
/*
|
||||
* 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);
|
||||
Reference in New Issue
Block a user