fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(fw_att_control): tilt-tracking controller

Browse Source
This commit is contained in:
ttechnick 2026-01-13 15:31:13 +01:00 committed by Nick
parent e370b3f4b8
commit c4330f5a47
14 changed files with 99 additions and 579 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -276,7 +276,6 @@ set(msg_files
versioned/VehicleLocalPosition.msg
versioned/VehicleOdometry.msg
versioned/VehicleRatesSetpoint.msg
versioned/VehicleEulerRatesSetpoint.msg
versioned/VehicleStatus.msg
versioned/VtolVehicleStatus.msg
versioned/Wind.msg

5
msg/versioned/VehicleEulerRatesSetpoint.msg
View File

@ -1,5 +0,0 @@
uint64 timestamp # time since system start (microseconds)
float32 roll_rate # body frame roll rate setpoint (rad/s)
float32 pitch_rate # body frame pitch rate setpoint (rad/s)
float32 yaw_rate # body frame yaw rate setpoint (rad/s)

42
src/lib/matrix/matrix/Quaternion.hpp
View File

@ -518,6 +518,48 @@ public:
return Vector3<Type>(q(1), q(2), q(3));
}
/**
* Corresponding body x-axis to an attitude quaternion /
* first orthogonal unit basis vector
*
* == first column of the equivalent rotation matrix
* but calculated more efficiently than a full conversion
*/
Vector3<Type> dcm_x() const
{
const Quaternion &q = *this;
Vector3<Type> R_x;
const Type a = q(0);
const Type b = q(1);
const Type c = q(2);
const Type d = q(3);
R_x(0) = a * a + b * b - c * c - d * d;
R_x(1) = 2 * (b * c + a * d);
R_x(2) = 2 * (b * d - a * c);
return R_x;
}
/**
* Corresponding body y-axis to an attitude quaternion /
* second orthogonal unit basis vector
*
* == second column of the equivalent rotation matrix
* but calculated more efficiently than a full conversion
*/
Vector3<Type> dcm_y() const
{
const Quaternion &q = *this;
Vector3<Type> R_y;
const Type a = q(0);
const Type b = q(1);
const Type c = q(2);
const Type d = q(3);
R_y(0) = 2 * (b * c - a * d);
R_y(1) = a * a - b * b + c * c - d * d;
R_y(2) = 2 * (c * d + a * b);
return R_y;
}
/**
* Corresponding body z-axis to an attitude quaternion /
* last orthogonal unit basis vector

3
src/modules/fw_att_control/CMakeLists.txt
View File

@ -38,10 +38,7 @@ px4_add_module(
FixedwingAttitudeControl.cpp
FixedwingAttitudeControl.hpp
fw_pitch_controller.cpp
fw_roll_controller.cpp
fw_wheel_controller.cpp
fw_yaw_controller.cpp
DEPENDS
px4_work_queue
)

149
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -72,25 +72,10 @@ FixedwingAttitudeControl::init()
void
FixedwingAttitudeControl::parameters_update()
{
_proportional_gain = matrix::Vector3f(1.0f / _param_fw_p_tc.get(),
_proportional_gain = matrix::Vector3f(1.0f / _param_fw_r_tc.get(),
1.0f / _param_fw_p_tc.get(),
0.0f);
_roll_ctrl.set_time_constant(_param_fw_r_tc.get());
_roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get()));
_pitch_ctrl.set_time_constant(_param_fw_p_tc.get());
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get()));
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get()));
_yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get()));
_max_roll_rate = radians(_param_fw_r_rmax.get());
_max_pitch_rate_pos = radians(_param_fw_p_rmax_pos.get());
_max_pitch_rate_neg = radians(_param_fw_p_rmax_neg.get());
_max_yaw_rate = radians(_param_fw_y_rmax.get());
_wheel_ctrl.set_k_p(_param_fw_wr_p.get());
_wheel_ctrl.set_k_i(_param_fw_wr_i.get());
_wheel_ctrl.set_k_ff(_param_fw_wr_ff.get());
@ -100,10 +85,8 @@ FixedwingAttitudeControl::parameters_update()
}
void
FixedwingAttitudeControl::vehicle_manual_poll(matrix::Quatf R)
FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
{
const matrix::Eulerf euler_angles(R);
float yaw_body = euler_angles.psi();
if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
// Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values
@ -111,7 +94,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(matrix::Quatf R)
if (!_vcontrol_mode.flag_control_climb_rate_enabled && _vcontrol_mode.flag_control_attitude_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs
// STABILIZED mode: setpoint generation
const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get());
@ -120,11 +103,11 @@ FixedwingAttitudeControl::vehicle_manual_poll(matrix::Quatf R)
pitch_body = constrain(pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
q.copyTo(_att_sp.q_d);
_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
_att_sp.timestamp = hrt_absolute_time();
_attitude_sp_pub.publish(_att_sp);
@ -266,9 +249,10 @@ void FixedwingAttitudeControl::Run()
/* fill in new attitude data */
_R = R_adapted;
}
const matrix::Eulerf euler_angles(_R);
vehicle_manual_poll(_R);
vehicle_manual_poll(euler_angles.psi());
vehicle_attitude_setpoint_poll();
@ -311,91 +295,61 @@ void FixedwingAttitudeControl::Run()
if (q_sp.isAllFinite()) {
// Current attitude
const Quatf q_current(att.q);
const Quatf q_sp_full(q_sp);
const Vector3f ez_world(0.f, 0.f, 1.f);
const Vector3f ex_c = q_current.dcm_x();
const Vector3f ez_c = q_current.dcm_z();
const Vector3f ez_sp = q_sp_full.dcm_z();
// Full desired attitude (setpoint)
const Quatf qd_full = q_sp;
// Tilt-only error quaternion
Quatf q_tilt_err(ez_c, ez_sp);
// --- Scheduling: 0 = normal FW, 1 = near-vertical/full-3D ---
const float pitch = Eulerf(q_current).theta(); // scheduling only
const float s = math::constrain((fabsf(pitch) - radians(60.f)) / radians(20.f), 0.f, 1.f); // 0..1
// Reduced desired attitude
Quatf q_des_red = (q_tilt_err * q_current).normalized();
Quatf q_err = (q_current.inversed() * q_des_red).canonical();
Vector3f e = 2.f * q_err.imag();
// --- Build reduced desired attitude qd_red: align "tilt" (body Z axis) but don't chase yaw ---
// Note: assumes Quatf::dcm_z() returns body Z axis expressed in world/NED (same convention as PX4 MC).
const Vector3f ez_current = q_current.dcm_z(); // current body Z in world
const Vector3f ez_desired = qd_full.dcm_z(); // desired body Z in world
Vector3f body_rates_setpoint;
body_rates_setpoint(0) = _proportional_gain(0) * e(0);
body_rates_setpoint(1) = _proportional_gain(1) * e(1) * 2.5f; // multiplied by 2.5 to maintain the gains
body_rates_setpoint(2) = 0.f;
// Quaternion rotating ez_current -> ez_desired (tilt correction only)
Quatf q_tilt_err(ez_current, ez_desired);
// Turn coordination
const float V = math::max(get_airspeed_constrained(), 0.1f);
// Convert that tilt error into an absolute reduced desired attitude.
// This matches the PX4 multicopter pattern (right multiplication).
Quatf qd_red;
Vector3f x_h = ex_c - ez_world * ex_c.dot(ez_world); // forward projected to horizontal
float r_tc_ff = 0.f;
if (fabsf(q_tilt_err(1)) > (1.f - 1e-5f) || fabsf(q_tilt_err(2)) > (1.f - 1e-5f)) {
// Corner case: vectors almost exactly opposite (180 deg). Tilt-only separation is ill-defined.
// Fall back to full desired attitude; it's safe/stable.
qd_red = qd_full;
const float xhn = x_h.norm();
} else {
// Apply tilt correction to current attitude to get a reduced desired attitude
qd_red = q_tilt_err * q_current;
if (xhn > 1e-3f) {
x_h /= xhn;
Vector3f y_h = ez_world.cross(x_h);
const float yhn = y_h.norm();
if (yhn > 1e-6f) {
y_h /= yhn;
const float cos_tilt = ez_c.dot(ez_world);
const float sin_bank = ez_c.dot(y_h);
float tan_bank = 0.f;
if (fabsf(cos_tilt) > 0.1f) {
tan_bank = sin_bank / cos_tilt;
}
r_tc_ff = (9.81f / V) * tan_bank * 0.6f;
}
}
// --- Compute rate setpoint A: tilt-only attitude tracking (normal FW) ---
// qe_tilt is rotation from current to reduced desired
const Quatf qe_tilt = (q_current.inversed() * qd_red).canonical();
// Use bounded quaternion vector part as error (stable & well-tuned pattern)
const Vector3f e_tilt = 2.f * qe_tilt.imag();
Vector3f rates_tilt = e_tilt.emult(_proportional_gain);
// In normal FW flight, yaw is NOT attitude-tracked; use existing yaw/turn coordination
const float yaw_rate_tc = _yaw_ctrl.get_body_rate_setpoint();
rates_tilt(2) = yaw_rate_tc;
// --- Compute rate setpoint B: full 3D attitude tracking (near-vertical) ---
const Quatf qe_full = (q_current.inversed() * qd_full).canonical();
const Vector3f e_full = 2.f * qe_full.imag();
Vector3f rates_full = e_full.emult(_proportional_gain);
// --- Blend A and B ---
Vector3f body_rates_setpoint = (1.f - s) * rates_tilt + s * rates_full;
// limit rates (kept identical)
body_rates_setpoint(0) = constrain(body_rates_setpoint(0), -_max_roll_rate, _max_roll_rate);
body_rates_setpoint(1) = constrain(body_rates_setpoint(1), -_max_pitch_rate_neg, _max_pitch_rate_pos);
body_rates_setpoint(2) = constrain(body_rates_setpoint(2), -_max_yaw_rate, _max_yaw_rate);
///////////////////////////////////
const Eulerf euler_sp(q_sp);
const float roll_sp = euler_sp.phi();
const float pitch_sp = euler_sp.theta();
_roll_ctrl.control_roll(roll_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
euler_angles.theta());
_pitch_ctrl.control_pitch(pitch_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
euler_angles.theta());
_yaw_ctrl.control_yaw(roll_sp, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
euler_angles.theta(), get_airspeed_constrained());
/* Update input data for rate controllers */
Vector3f _euler_body_rates_sp = Vector3f(_roll_ctrl.get_body_rate_setpoint(), _pitch_ctrl.get_body_rate_setpoint(),
_yaw_ctrl.get_body_rate_setpoint());
_euler_rates_sp.timestamp = hrt_absolute_time();
_euler_rates_sp.roll_rate = _euler_body_rates_sp(0);
_euler_rates_sp.pitch_rate = _euler_body_rates_sp(1);
_euler_rates_sp.yaw_rate = _euler_body_rates_sp(2);
///////////////////////////////////
body_rates_setpoint(2) = -r_tc_ff;
body_rates_setpoint(0) = math::constrain(body_rates_setpoint(0), -radians(_param_fw_r_rmax.get()), radians(_param_fw_r_rmax.get()));
body_rates_setpoint(1) = math::constrain(body_rates_setpoint(1), -radians(_param_fw_p_rmax_neg.get()), radians(_param_fw_p_rmax_pos.get()));
body_rates_setpoint(2) = math::constrain(body_rates_setpoint(2), -radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get()));
autotune_attitude_control_status_s pid_autotune;
matrix::Vector3f bodyrate_autotune_ff;
@ -434,7 +388,6 @@ void FixedwingAttitudeControl::Run()
_rates_sp.timestamp = hrt_absolute_time();
_rate_sp_pub.publish(_rates_sp);
_euler_rates_sp_pub.publish(_euler_rates_sp);
}
}
}

21
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -34,10 +34,8 @@
#pragma once
#include <drivers/drv_hrt.h>
#include "fw_pitch_controller.h"
#include "fw_roll_controller.h"
#include "fw_wheel_controller.h"
#include "fw_yaw_controller.h"
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
@ -65,7 +63,6 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_euler_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
using matrix::Eulerf;
@ -116,14 +113,12 @@ private:
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_euler_rates_setpoint_s> _euler_rates_sp_pub{ORB_ID(vehicle_euler_rates_setpoint)};
uORB::Publication<landing_gear_wheel_s> _landing_gear_wheel_pub{ORB_ID(landing_gear_wheel)};
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_euler_rates_setpoint_s _euler_rates_sp{};
vehicle_status_s _vehicle_status{};
landing_gear_wheel_s _landing_gear_wheel{};
@ -164,24 +159,14 @@ private:
(ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax,
(ParamFloat<px4::params::FW_MAN_YR_MAX>) _param_man_yr_max
)
matrix::Vector3f _proportional_gain;
RollController _roll_ctrl;
PitchController _pitch_ctrl;
YawController _yaw_ctrl;
float _max_roll_rate;
float _max_pitch_rate_pos;
float _max_pitch_rate_neg;
float _max_yaw_rate;
WheelController _wheel_ctrl;
void parameters_update();
void vehicle_manual_poll(matrix::Quatf R);
void vehicle_manual_poll(const float yaw_body);
void vehicle_attitude_setpoint_poll();
void vehicle_land_detected_poll();
float get_airspeed_constrained();
};
};

65
src/modules/fw_att_control/fw_pitch_controller.cpp
View File

@ -1,65 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020-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 ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fw_pitch_controller.cpp
* Implementation of a simple pitch P controller.
*/
#include "fw_pitch_controller.h"
#include <float.h>
#include <lib/geo/geo.h>
#include <mathlib/mathlib.h>
float PitchController::control_pitch(float pitch_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(pitch_setpoint) &&
PX4_ISFINITE(euler_yaw_rate_setpoint) &&
PX4_ISFINITE(pitch) &&
PX4_ISFINITE(roll))) {
return _body_rate_setpoint;
}
const float pitch_error = pitch_setpoint - pitch;
_euler_rate_setpoint = pitch_error / _tc;
/* Transform setpoint to body angular rates (jacobian) */
const float pitch_body_rate_setpoint_raw = cosf(roll) * _euler_rate_setpoint +
cosf(pitch) * sinf(roll) * euler_yaw_rate_setpoint;
_body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate_pos);
return _body_rate_setpoint;
}

74
src/modules/fw_att_control/fw_pitch_controller.h
View File

@ -1,74 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fw_pitch_controller.h
* Definition of a simple pitch P controller.
*/
#ifndef FW_PITCH_CONTROLLER_H
#define FW_PITCH_CONTROLLER_H
class PitchController
{
public:
PitchController() = default;
~PitchController() = default;
/**
* @brief Calculates both euler and body pitch rate setpoints.
*
* @param pitch_setpoint pitch setpoint [rad]
* @param euler_yaw_rate_setpoint euler yaw rate setpoint [rad/s]
* @param roll estimated roll [rad]
* @param pitch estimated pitch [rad]
* @return Pitch body rate setpoint [rad/s]
*/
float control_pitch(float pitch_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch);
void set_time_constant(float time_constant) { _tc = time_constant; }
void set_max_rate_pos(float max_rate_pos) { _max_rate_pos = max_rate_pos; }
void set_max_rate_neg(float max_rate_neg) { _max_rate_neg = max_rate_neg; }
float get_euler_rate_setpoint() { return _euler_rate_setpoint; }
float get_body_rate_setpoint() { return _body_rate_setpoint; }
private:
float _tc{};
float _max_rate_pos{};
float _max_rate_neg{};
float _euler_rate_setpoint{};
float _body_rate_setpoint{};
};
#endif // FW_PITCH_CONTROLLER_H

64
src/modules/fw_att_control/fw_roll_controller.cpp
View File

@ -1,64 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020-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 ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fw_roll_controller.cpp
* Implementation of a simple roll P controller.
*/
#include "fw_roll_controller.h"
#include <float.h>
#include <lib/geo/geo.h>
#include <mathlib/mathlib.h>
float RollController::control_roll(float roll_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(roll_setpoint) &&
PX4_ISFINITE(euler_yaw_rate_setpoint) &&
PX4_ISFINITE(pitch) &&
PX4_ISFINITE(roll))) {
return _body_rate_setpoint;
}
const float roll_error = roll_setpoint - roll;
_euler_rate_setpoint = roll_error / _tc;
/* Transform setpoint to body angular rates (jacobian) */
const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(pitch) *
euler_yaw_rate_setpoint;
_body_rate_setpoint = math::constrain(roll_body_rate_setpoint_raw, -_max_rate, _max_rate);
return _body_rate_setpoint;
}

72
src/modules/fw_att_control/fw_roll_controller.h
View File

@ -1,72 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fw_roll_controller.h
* Definition of a simple roll P controller.
*/
#ifndef FW_ROLL_CONTROLLER_H
#define FW_ROLL_CONTROLLER_H
class RollController
{
public:
RollController() = default;
~RollController() = default;
/**
* @brief Calculates both euler and body roll rate setpoints.
*
* @param roll_setpoint roll setpoint [rad]
* @param euler_yaw_rate_setpoint euler yaw rate setpoint [rad/s]
* @param roll estimated roll [rad]
* @param pitch estimated pitch [rad]
* @return Roll body rate setpoint [rad/s]
*/
float control_roll(float roll_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch);
void set_time_constant(float time_constant) { _tc = time_constant; }
void set_max_rate(float max_rate) { _max_rate = max_rate; }
float get_euler_rate_setpoint() { return _euler_rate_setpoint; }
float get_body_rate_setpoint() { return _body_rate_setpoint; }
private:
float _tc{};
float _max_rate{};
float _euler_rate_setpoint{};
float _body_rate_setpoint{};
};
#endif // FW_ROLL_CONTROLLER_H

98
src/modules/fw_att_control/fw_yaw_controller.cpp
View File

@ -1,98 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020-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 ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fw_yaw_controller.cpp
* Implementation of a simple coordinated turn yaw controller.
*/
#include "fw_yaw_controller.h"
#include <float.h>
#include <lib/geo/geo.h>
#include <mathlib/mathlib.h>
float YawController::control_yaw(float roll_setpoint, float euler_pitch_rate_setpoint, float roll, float pitch,
float airspeed)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(roll_setpoint) &&
PX4_ISFINITE(roll) &&
PX4_ISFINITE(pitch) &&
PX4_ISFINITE(euler_pitch_rate_setpoint) &&
PX4_ISFINITE(airspeed))) {
return _body_rate_setpoint;
}
float constrained_roll;
bool inverted = false;
/* roll is used as feedforward term and inverted flight needs to be considered */
if (fabsf(roll) < math::radians(90.f)) {
/* not inverted, but numerically still potentially close to infinity */
constrained_roll = math::constrain(roll, math::radians(-80.f), math::radians(80.f));
} else {
inverted = true;
// inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity
//note: the ranges are extended by 10 deg here to avoid numeric resolution effects
if (roll > 0.f) {
/* right hemisphere */
constrained_roll = math::constrain(roll, math::radians(100.f), math::radians(180.f));
} else {
/* left hemisphere */
constrained_roll = math::constrain(roll, math::radians(-180.f), math::radians(-100.f));
}
}
constrained_roll = math::constrain(constrained_roll, -fabsf(roll_setpoint), fabsf(roll_setpoint));
if (!inverted) {
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_euler_rate_setpoint = tanf(constrained_roll) * cosf(pitch) * CONSTANTS_ONE_G / airspeed;
/* Transform setpoint to body angular rates (jacobian) */
const float yaw_body_rate_setpoint_raw = -sinf(roll) * euler_pitch_rate_setpoint +
cosf(roll) * cosf(pitch) * _euler_rate_setpoint;
_body_rate_setpoint = math::constrain(yaw_body_rate_setpoint_raw, -_max_rate, _max_rate);
}
if (!PX4_ISFINITE(_body_rate_setpoint)) {
_body_rate_setpoint = 0.f;
}
return _body_rate_setpoint;
}

79
src/modules/fw_att_control/fw_yaw_controller.h
View File

@ -1,79 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020-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.
*
****************************************************************************/
/**
* @file fw_yaw_controller.h
* Definition of a simple coordinated turn controller.
*
* Acknowledgements:
*
* The control design is based on a design
* by Paul Riseborough and Andrew Tridgell, 2013,
* which in turn is based on initial work of
* Jonathan Challinger, 2012.
*/
#ifndef FW_YAW_CONTROLLER_H
#define FW_YAW_CONTROLLER_H
class YawController
{
public:
YawController() = default;
~YawController() = default;
/**
* @brief Calculates both euler and body yaw rate setpoints for coordinated turn based on current attitude and airspeed
*
* @param roll_setpoint roll setpoint [rad]
* @param euler_pitch_rate_setpoint euler pitch rate setpoint [rad/s]
* @param roll estimated roll [rad]
* @param pitch estimated pitch [rad]
* @param airspeed airspeed [m/s]
* @return Roll body rate setpoint [rad/s]
*/
float control_yaw(float roll_setpoint, float euler_pitch_rate_setpoint, float roll, float pitch,
float airspeed);
void set_max_rate(float max_rate) { _max_rate = max_rate; }
float get_euler_rate_setpoint() { return _euler_rate_setpoint; }
float get_body_rate_setpoint() { return _body_rate_setpoint; }
private:
float _max_rate{};
float _euler_rate_setpoint{};
float _body_rate_setpoint{};
};
#endif // FW_YAW_CONTROLLER_H

4
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
View File

@ -298,7 +298,9 @@ void FwLateralLongitudinalControl::Run()
// additional is_finite checks that should not be necessary, but are kept for safety
float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f;
float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f;
const float yaw_body = _yaw; // yaw is not controlled in fixed wing, need to set it though for quaternion generation
float yaw_body = _yaw;
const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f;
if (_control_mode_sub.get().flag_control_manual_enabled) {

1
src/modules/logger/logged_topics.cpp
View File

@ -148,7 +148,6 @@ void LoggedTopics::add_default_topics()
add_topic("vehicle_local_position_setpoint", 100);
add_topic("vehicle_magnetometer", 200);
add_topic("vehicle_rates_setpoint", 20);
add_topic("vehicle_euler_rates_setpoint", 20);
add_topic("vehicle_roi", 1000);
add_topic("vehicle_status");
add_topic("vtx");