From 816aa0ffb6ba59db83552c0ccaadedf9c152f666 Mon Sep 17 00:00:00 2001
From: Roman Bapst <bapstroman@gmail.com>
Date: Wed, 8 May 2019 16:33:14 +0200
Subject: [PATCH] VTOL transitions: use FW attitude loop (#11911)
* VTOL trans: changed that now in transition for both MC and FW the corresponding (correct) attitude controller is running
* attitude setpoint for stabilized mode is generated by tailsitter.cpp
* reset yaw setpoint during transition to avoid big yaw errors after
transition back to hover
* VT_TYPE parameter: added "reboot required" metadata
---
Tools/uorb_graph/create.py | 1 +
.../FixedwingAttitudeControl.cpp | 47 +++++++++----------
.../FixedwingAttitudeControl.hpp | 7 +--
src/modules/mc_att_control/mc_att_control.hpp | 4 ++
.../mc_att_control/mc_att_control_main.cpp | 33 +++++++++----
.../vtol_att_control_main.cpp | 2 +-
.../vtol_att_control_params.c | 1 +
src/modules/vtol_att_control/vtol_type.cpp | 4 +-
8 files changed, 60 insertions(+), 39 deletions(-)
diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py
index 64e0da5ffa..7a002bdd30 100755
--- a/Tools/uorb_graph/create.py
+++ b/Tools/uorb_graph/create.py
@@ -247,6 +247,7 @@ class Graph(object):
('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
+ ('mc_att_control', r'mc_att_control_main\.cpp$', r'\_attitude_sp_id=([^,)]+)', r'^_attitude_sp_id$'),
('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index e5062095e6..c685dfb352 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -33,6 +33,8 @@
#include "FixedwingAttitudeControl.hpp"
+#include <vtol_att_control/vtol_type.h>
+
using namespace time_literals;
/**
@@ -120,9 +122,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN");
_parameter_handles.airspeed_mode = param_find("FW_ARSP_MODE");
- // initialize to invalid VTOL type
- _parameters.vtol_type = -1;
-
/* fetch initial parameter values */
parameters_update();
@@ -242,10 +241,6 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres);
- if (_vehicle_status.is_vtol) {
- param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
- }
-
param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
param_get(_parameter_handles.airspeed_mode, &tmp);
@@ -292,13 +287,25 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
}
+
+ if (_vehicle_status.is_vtol) {
+ const bool is_hovering = _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode;
+ const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter;
+
+ if (is_hovering || is_tailsitter_transition) {
+ _vcontrol_mode.flag_control_attitude_enabled = false;
+ _vcontrol_mode.flag_control_manual_enabled = false;
+ }
+ }
}
void
FixedwingAttitudeControl::vehicle_manual_poll()
{
+ const bool is_tailsitter_transition = _is_tailsitter && _vehicle_status.in_transition_mode;
+ const bool is_fixed_wing = !_vehicle_status.is_rotary_wing;
- if (_vcontrol_mode.flag_control_manual_enabled && !_vehicle_status.is_rotary_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 (orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual) == PX4_OK) {
@@ -394,7 +401,7 @@ FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
if (updated) {
orb_copy(ORB_ID(vehicle_rates_setpoint), _rates_sp_sub, &_rates_sp);
- if (_parameters.vtol_type == vtol_type::TAILSITTER) {
+ if (_is_tailsitter) {
float tmp = _rates_sp.roll;
_rates_sp.roll = -_rates_sp.yaw;
_rates_sp.yaw = tmp;
@@ -424,25 +431,17 @@ FixedwingAttitudeControl::vehicle_status_poll()
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
- // if VTOL and not in fixed wing mode we should only control body-rates which are published
- // by the multicoper attitude controller. Therefore, modify the control mode to achieve rate
- // control only
- if (_vehicle_status.is_vtol) {
- if (_vehicle_status.is_rotary_wing) {
- _vcontrol_mode.flag_control_attitude_enabled = false;
- _vcontrol_mode.flag_control_manual_enabled = false;
- }
- }
-
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_actuators_id) {
if (_vehicle_status.is_vtol) {
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
- _parameter_handles.vtol_type = param_find("VT_TYPE");
+ int32_t vt_type = -1;
- parameters_update();
+ if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
+ _is_tailsitter = (vt_type == vtol_type::TAILSITTER);
+ }
} else {
_actuators_id = ORB_ID(actuator_controls_0);
@@ -566,7 +565,7 @@ void FixedwingAttitudeControl::run()
/* get current rotation matrix and euler angles from control state quaternions */
matrix::Dcmf R = matrix::Quatf(_att.q);
- if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) {
+ if (_is_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
@@ -609,7 +608,7 @@ void FixedwingAttitudeControl::run()
_att.yawspeed = helper;
}
- matrix::Eulerf euler_angles(R);
+ const matrix::Eulerf euler_angles(R);
vehicle_attitude_setpoint_poll();
vehicle_control_mode_poll();
@@ -623,7 +622,7 @@ void FixedwingAttitudeControl::run()
_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
/* lock integrator until control is started */
- bool lock_integrator = !(_vcontrol_mode.flag_control_rates_enabled && !_vehicle_status.is_rotary_wing);
+ bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled || _vehicle_status.is_rotary_wing;
/* Simple handling of failsafe: deploy parachute if failsafe is on */
if (_vcontrol_mode.flag_control_termination_enabled) {
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
index abf960073b..53fe29f03d 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -60,7 +60,6 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
-#include <vtol_att_control/vtol_type.h>
using matrix::Eulerf;
using matrix::Quatf;
@@ -140,6 +139,8 @@ private:
bool _flag_control_attitude_enabled_last{false};
+ bool _is_tailsitter{false};
+
struct {
float p_tc;
float p_p;
@@ -204,8 +205,6 @@ private:
float rattitude_thres;
- int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */
-
int32_t bat_scale_en; /**< Battery scaling enabled */
bool airspeed_disabled;
@@ -274,8 +273,6 @@ private:
param_t rattitude_thres;
- param_t vtol_type;
-
param_t bat_scale_en;
param_t airspeed_mode;
diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp
index c6fd04743e..3185932db4 100644
--- a/src/modules/mc_att_control/mc_att_control.hpp
+++ b/src/modules/mc_att_control/mc_att_control.hpp
@@ -57,6 +57,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/landing_gear.h>
+#include <vtol_att_control/vtol_type.h>
#include <AttitudeControl.hpp>
@@ -174,6 +175,7 @@ private:
orb_advert_t _landing_gear_pub{nullptr};
orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */
+ orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */
bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */
@@ -277,6 +279,8 @@ private:
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode
)
+ bool _is_tailsitter{false};
+
matrix::Vector3f _rate_p; /**< P gain for angular rate error */
matrix::Vector3f _rate_i; /**< I gain for angular rate error */
matrix::Vector3f _rate_int_lim; /**< integrator state limit for rate loop */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index d55b468aff..b12bdaed6e 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -252,9 +252,16 @@ MulticopterAttitudeControl::vehicle_status_poll()
if (_actuators_id == nullptr) {
if (_vehicle_status.is_vtol) {
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
+ _attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint);
+
+ int32_t vt_type = -1;
+ if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
+ _is_tailsitter = (vt_type == vtol_type::TAILSITTER);
+ }
} else {
_actuators_id = ORB_ID(actuator_controls_0);
+ _attitude_sp_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
@@ -407,7 +414,6 @@ void
MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp)
{
vehicle_attitude_setpoint_s attitude_setpoint{};
- landing_gear_s landing_gear{};
const float yaw = Eulerf(Quatf(_v_att.q)).psi();
/* reset yaw setpoint to current position if needed */
@@ -495,12 +501,12 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
attitude_setpoint.q_d_valid = true;
attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
+ attitude_setpoint.timestamp = hrt_absolute_time();
+ if (_attitude_sp_id != nullptr) {
+ orb_publish_auto(_attitude_sp_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
+ }
_landing_gear.landing_gear = get_landing_gear_state();
-
- attitude_setpoint.timestamp = landing_gear.timestamp = hrt_absolute_time();
- orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
-
_landing_gear.timestamp = hrt_absolute_time();
orb_publish_auto(ORB_ID(landing_gear), &_landing_gear_pub, &_landing_gear, nullptr, ORB_PRIO_DEFAULT);
}
@@ -805,7 +811,15 @@ MulticopterAttitudeControl::run()
bool attitude_setpoint_generated = false;
- if (_v_control_mode.flag_control_attitude_enabled && _vehicle_status.is_rotary_wing) {
+ const bool is_hovering = _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode;
+
+ // vehicle is a tailsitter in transition mode
+ const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter;
+
+ bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
+
+
+ if (run_att_ctrl) {
if (attitude_updated) {
// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
if (_v_control_mode.flag_control_manual_enabled &&
@@ -822,7 +836,7 @@ MulticopterAttitudeControl::run()
} else {
/* attitude controller disabled, poll rates setpoint topic */
- if (_v_control_mode.flag_control_manual_enabled && _vehicle_status.is_rotary_wing) {
+ if (_v_control_mode.flag_control_manual_enabled && is_hovering) {
if (manual_control_updated) {
/* manual rates control - ACRO mode */
Vector3f man_rate_sp(
@@ -856,9 +870,12 @@ MulticopterAttitudeControl::run()
}
if (attitude_updated) {
+ // reset yaw setpoint during transitions, tailsitter.cpp generates
+ // attitude setpoint for the transition
reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) ||
_vehicle_land_detected.landed ||
- (_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing); // VTOL in FW mode
+ (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode);
+
attitude_dt = 0.f;
}
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index ada9969d25..5e7fd840cf 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -752,7 +752,7 @@ VtolAttitudeControl::start()
_control_task = px4_task_spawn_cmd("vtol_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_ATTITUDE_CONTROL + 1,
- 1230,
+ 1320,
(px4_main_t)&VtolAttitudeControl::task_main_trampoline,
nullptr);
diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
index 988b73a0e9..2b870e2121 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -82,6 +82,7 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0);
* @min 0
* @max 2
* @decimal 0
+ * @reboot_required true
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_TYPE, 0);
diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
index 53aabcdb85..274c8caf93 100644
--- a/src/modules/vtol_att_control/vtol_type.cpp
+++ b/src/modules/vtol_att_control/vtol_type.cpp
@@ -370,7 +370,9 @@ bool VtolType::is_motor_off_channel(const int channel)
int tmp;
int channels = _params->fw_motors_off;
- for (int i = 0; i < _params->vtol_motor_count; ++i) {
+ static constexpr int num_outputs_max = 8;
+
+ for (int i = 0; i < num_outputs_max; ++i) {
tmp = channels % 10;
if (tmp == 0) {
--
GitLab