Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

VTOL transitions: use FW attitude loop (#11911)

Browse Source 
* 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
This commit is contained in:
Roman Bapst
2019-05-08 16:33:14 +02:00
committed by Daniel Agar
parent d4b7441d12
commit 816aa0ffb6
8 changed files with 60 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/uorb_graph/create.py
+1
View File
@@ -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$'),
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+23 -24
View File
@@ -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) {
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+2 -5
View File
@@ -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;
src/modules/mc_att_control/mc_att_control.hpp
+4
View File
@@ -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 */
src/modules/mc_att_control/mc_att_control_main.cpp
+25 -8
View File
@@ -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;
}
src/modules/vtol_att_control/vtol_att_control_main.cpp
+1 -1
View File
@@ -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);
src/modules/vtol_att_control/vtol_att_control_params.c
+1
View File
@@ -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);
src/modules/vtol_att_control/vtol_type.cpp
+3 -1
View File
@@ -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) {