fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 06:03:35 +08:00
Code Issues 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 10:33:13 -04: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
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) {