fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 03:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Backported VTOL to beta branch.

Browse Source
This commit is contained in:
Simon Wilks
2015-08-09 11:36:38 +02:00
parent e07e4743c5
commit 4c79f8d4a2
12 changed files with 109 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/13002_firefly6
+9 -5
View File
@@ -12,21 +12,25 @@ sh /etc/init.d/rc.vtol_defaults
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.15
param set MC_ROLLRATE_P 0.17
param set MC_ROLLRATE_I 0.002
param set MC_ROLLRATE_D 0.003
param set MC_ROLLRATE_D 0.004
param set MC_ROLLRATE_FF 0.0
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.12
param set MC_PITCHRATE_P 0.14
param set MC_PITCHRATE_I 0.002
param set MC_PITCHRATE_D 0.003
param set MC_PITCHRATE_D 0.004
param set MC_PITCHRATE_FF 0.0
param set MC_YAW_P 2.8
param set MC_YAW_P 3.8
param set MC_YAW_FF 0.5
param set MC_YAWRATE_P 0.22
param set MC_YAWRATE_I 0.02
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_FF 0.0
param set VT_TILT_MC 0.08
param set VT_TILT_TRANS 0.5
param set VT_TILT_FW 0.9
fi
set MIXER firefly6
ROMFS/px4fmu_common/init.d/rcS
+4
View File
@@ -626,6 +626,10 @@ then
then
set MAV_TYPE 21
fi
if [ $MIXER == quad_x_pusher_vtol ]
then
set MAV_TYPE 22
fi
fi
# Still no MAV_TYPE found
ROMFS/px4fmu_common/mixers/firefly6.aux.mix
+3 -3
View File
@@ -5,18 +5,18 @@ Tilt mechanism servo mixer
---------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 1 4 10000 10000 0 -10000 10000
S: 1 4 0 20000 -10000 -10000 10000
Elevon mixers
-------------
M: 2
O: 10000 10000 0 -10000 10000
S: 1 0 -7500 -7500 0 -10000 10000
S: 1 0 -7500 -7500 0 -10000 10000
S: 1 1 8000 8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 1 0 -7500 -7500 0 -10000 10000
S: 1 0 -7500 -7500 0 -10000 10000
S: 1 1 -8000 -8000 0 -10000 10000
Landing gear mixer
src/modules/fw_att_control/fw_att_control_main.cpp
+5 -4
View File
@@ -695,7 +695,6 @@ FixedwingAttitudeControl::task_main()
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
@@ -799,6 +798,11 @@ FixedwingAttitudeControl::task_main()
//warnx("_actuators_airframe.control[1] = -1.0f;");
}
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) {
continue;
}
/* default flaps to center */
float flaps_control = 0.0f;
@@ -808,11 +812,8 @@ FixedwingAttitudeControl::task_main()
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) {
/* scale around tuning airspeed */
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
src/modules/vtol_att_control/module.mk
+3 -1
View File
@@ -42,6 +42,8 @@ SRCS = vtol_att_control_main.cpp \
tiltrotor_params.c \
tiltrotor.cpp \
vtol_type.cpp \
tailsitter.cpp
tailsitter.cpp \
standard_params.c \
standard.cpp
EXTRACXXFLAGS = -Wno-write-strings
src/modules/vtol_att_control/tiltrotor.cpp
+25 -33
View File
@@ -194,7 +194,7 @@ void Tiltrotor::update_mc_state()
void Tiltrotor::process_mc_data()
{
fill_mc_att_control_output();
fill_att_control_output();
}
void Tiltrotor::update_fw_state()
@@ -222,19 +222,22 @@ void Tiltrotor::process_mc_data()
void Tiltrotor::process_fw_data()
{
fill_fw_att_control_output();
fill_att_control_output();
}
void Tiltrotor::update_transition_state()
{
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
// tilt rotors forward up to certain angle
if (_tilt_control <= _params_tiltrotor.tilt_transition) {
_tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur*1000000.0f);
if (_params_tiltrotor.front_trans_dur <= 0.0f) {
_tilt_control = _params_tiltrotor.tilt_transition;
} else if (_tilt_control <= _params_tiltrotor.tilt_transition) {
_tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) *
(float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f);
}
// do blending of mc and fw controls
if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) {
if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START && _params_tiltrotor.airspeed_trans - ARSP_BLEND_START > 0.0f) {
_roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START);
} else {
// at low speeds give full weight to mc
@@ -244,13 +247,14 @@ void Tiltrotor::update_transition_state()
_roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f);
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
_tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(0.5f*1000000.0f);
_tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) *
(float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (0.5f * 1000000.0f);
_roll_weight_mc = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
// tilt rotors forward up to certain angle
float progress = (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f);
float progress = (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f);
if (_tilt_control > _params_tiltrotor.tilt_mc) {
_tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress;
_tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * progress;
}
_roll_weight_mc = progress;
@@ -263,38 +267,26 @@ void Tiltrotor::update_external_state()
}
/**
* Prepare message to acutators with data from mc attitude controller.
* Prepare message to acutators with data from the attitude controllers.
*/
void Tiltrotor::fill_mc_att_control_output()
void Tiltrotor::fill_att_control_output()
{
_actuators_out_0->control[0] = _actuators_mc_in->control[0];
_actuators_out_0->control[1] = _actuators_mc_in->control[1];
_actuators_out_0->control[2] = _actuators_mc_in->control[2];
_actuators_out_0->control[3] = _actuators_mc_in->control[3];
_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; // roll
_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; // pitch
_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; // yaw
if (_vtol_schedule.flight_mode == FW_MODE) {
_actuators_out_1->control[3] = _actuators_fw_in->control[3]; // fw throttle
} else {
_actuators_out_0->control[3] = _actuators_mc_in->control[3]; // mc throttle
}
_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc); //roll elevon
_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon
_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon
_actuators_out_1->control[4] = _tilt_control; // for tilt-rotor control
}
/**
* Prepare message to acutators with data from fw attitude controller.
*/
void Tiltrotor::fill_fw_att_control_output()
{
/*For the first test in fw mode, only use engines for thrust!!!*/
_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc;
_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc;
_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc;
_actuators_out_0->control[3] = _actuators_fw_in->control[3];
/*controls for the elevons */
_actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon
_actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon
// unused now but still logged
_actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw
_actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle
_actuators_out_1->control[4] = _tilt_control;
_actuators_out_1->control[2] = _actuators_fw_in->control[2]; // fw yaw
}
/**
src/modules/vtol_att_control/tiltrotor.h
+1 -2
View File
@@ -99,8 +99,7 @@ private:
float _tilt_control;
float _roll_weight_mc;
void fill_mc_att_control_output();
void fill_fw_att_control_output();
void fill_att_control_output();
void set_max_mc();
void set_max_fw(unsigned pwm_value);
src/modules/vtol_att_control/tiltrotor_params.c
+3 -36
View File
@@ -39,28 +39,6 @@
*/
#include <systemlib/param/param.h>
/**
* Duration of a front transition
*
* Time in seconds used for a transition
*
* @min 0.0
* @max 5
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR,3.0f);
/**
* Duration of a back transition
*
* Time in seconds used for a back transition
*
* @min 0.0
* @max 5
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f);
/**
* Position of tilt servo in mc mode
@@ -71,7 +49,7 @@ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f);
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f);
PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f);
/**
* Position of tilt servo in transition mode
@@ -82,7 +60,7 @@ PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f);
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f);
PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f);
/**
* Position of tilt servo in fw mode
@@ -93,15 +71,4 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f);
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f);
/**
* Transition airspeed
*
* Airspeed at which we can switch to fw mode
*
* @min 0.0
* @max 20
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f);
PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f);
src/modules/vtol_att_control/vtol_att_control_main.cpp
+6
View File
@@ -122,6 +122,9 @@ VtolAttitudeControl::VtolAttitudeControl() :
} else if (_params.vtol_type == 1) {
_tiltrotor = new Tiltrotor(this);
_vtol_type = _tiltrotor;
} else if (_params.vtol_type == 2) {
_standard = new Standard(this);
_vtol_type = _standard;
} else {
_task_should_exit = true;
}
@@ -530,6 +533,9 @@ void VtolAttitudeControl::task_main()
// update transition state if got any new data
if (got_new_data) {
_vtol_type->update_transition_state();
_vtol_type->process_mc_data();
fill_mc_att_rates_sp();
}
} else if (_vtol_type->get_mode() == EXTERNAL) {
src/modules/vtol_att_control/vtol_att_control_main.h
+2
View File
@@ -84,6 +84,7 @@
#include "tiltrotor.h"
#include "tailsitter.h"
#include "standard.h"
extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
@@ -188,6 +189,7 @@ private:
VtolType * _vtol_type; // base class for different vtol types
Tiltrotor * _tiltrotor; // tailsitter vtol type
Tailsitter * _tailsitter; // tiltrotor vtol type
Standard * _standard; // standard vtol type
//*****************Member functions***********************************************************************
src/modules/vtol_att_control/vtol_att_control_params.c
+46 -2
View File
@@ -143,10 +143,10 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f);
PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f);
/**
* VTOL Type (Tailsitter=0, Tiltrotor=1)
* VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2)
*
* @min 0
* @max 1
* @max 2
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_TYPE, 0);
@@ -161,3 +161,47 @@ PARAM_DEFINE_INT32(VT_TYPE, 0);
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0);
/**
* Duration of a front transition
*
* Time in seconds used for a transition
*
* @min 0.0
* @max 5
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 3.0f);
/**
* Duration of a back transition
*
* Time in seconds used for a back transition
*
* @min 0.0
* @max 5
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 2.0f);
/**
* Transition blending airspeed
*
* Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable.
*
* @min 0.0
* @max 20.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f);
/**
* Transition airspeed
*
* Airspeed at which we can switch to fw mode
*
* @min 1.0
* @max 20
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f);
src/modules/vtol_att_control/vtol_type.h
+2 -2
View File
@@ -38,8 +38,8 @@
*
*/
#ifndef VTOL_YYPE_H
#define VTOL_YYPE_H
#ifndef VTOL_TYPE_H
#define VTOL_TYPE_H
struct Params {
int idle_pwm_mc; // pwm value for idle in mc mode