tailsitter auto transition test!

Conflicts:
	src/modules/vtol_att_control/tailsitter.cpp
This commit is contained in:
davidvor
2015-08-20 12:38:49 +03:00
committed by tumbili
parent 0fa8a5286b
commit 6c5638062a
3 changed files with 433 additions and 31 deletions
+286 -20
View File
@@ -31,23 +31,49 @@
*
****************************************************************************/
/**
* @file tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*
*/
#include "tailsitter.h"
#include "vtol_att_control_main.h"
Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) :
VtolType(att_controller),
_airspeed_tot(0),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
{
#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition
#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition
#define PITCH_TRANSITION_FRONT_P1 -0.78f // pitch angle to switch to TRANSITION_P2
#define PITCH_TRANSITION_FRONT_P2 -1.05f // pitch angle to switch to FW
#define PITCH_TRANSITION_BACK -0.5f // pitch angle to switch to MC
Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) :
/**VtolType(att_controller),*/
VtolType(attc),
_roll_weight_mc(1.0f),
_yaw_weight_mc(1.0f),
_min_front_trans_dur(0.5f)
_airspeed_tot(0),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_flag_was_in_trans_mode = false;
_params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
_params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR");
_params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS");
_params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND");
_params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
}
Tailsitter::~Tailsitter()
@@ -55,23 +81,153 @@ Tailsitter::~Tailsitter()
}
int
Taulsitter::parameters_update()
{
float v;
int l;
/* vtol duration of a front transition */
param_get(_params_handles_tiltrotor.front_trans_dur, &v);
_params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f);
/* vtol front transition phase 2 duration */
param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v);
_params_tiltrotor.front_trans_dur_p2 = v;
/* vtol duration of a back transition */
param_get(_params_handles_tiltrotor.back_trans_dur, &v);
_params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f);
/* vtol airspeed at which it is ok to switch to fw mode */
param_get(_params_handles_tiltrotor.airspeed_trans, &v);
_params_tiltrotor.airspeed_trans = v;
/* vtol airspeed at which we start blending mc/fw controls */
param_get(_params_handles_tiltrotor.airspeed_blend_start, &v);
_params_tiltrotor.airspeed_blend_start = v;
/* vtol lock elevons in multicopter */
param_get(_params_handles_tiltrotor.elevons_mc_lock, &l);
_params_tiltrotor.elevons_mc_lock = l;
/* avoid parameters which will lead to zero division in the transition code */
_params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur);
if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) {
_params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f;
}
return OK;
}
void Tailsitter::update_vtol_state()
{
// simply switch between the two modes
if (!_attc->is_fixed_wing_requested()) {
_vtol_mode = ROTARY_WING;
parameters_update();
} else {
_vtol_mode = FIXED_WING;
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting rotors, picking up
* forward speed. After the vehicle has picked up enough speed the rotors are tilted
* forward completely. For the backtransition the motors simply rotate back.
*/
if (_manual_control_sp->aux1 < 0.0f) { // user switchig to MC mode
// plane is in multicopter mode
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_FRONT_P2:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
// check if we have reached pitch angle to switch to MC mode
if (_v_att->pitch >= PITCH_TRANSITION_BACK) {
_vtol_schedule.flight_mode = MC_MODE;
}
break;
}
} else { // user switchig to FW mode
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1:
// check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
}
break;
case TRANSITION_FRONT_P2:
// check if we have reached pitch angle to switch to FW mode
if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) {
_vtol_schedule.flight_mode = FW_MODE;
}
break;
case TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = FW_MODE;
/* **LATER*** if pitch is closer to mc (-45>)
* go to transition P1
*/
break;
}
}
// map tailsitter specific control phases to simple control modes
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case TRANSITION_FRONT_P1:
_vtol_mode = TRANSITION;
_vtol_vehicle_status->vtol_in_trans_mode = true;
case TRANSITION_FRONT_P2:
_vtol_mode = TRANSITION;
_vtol_vehicle_status->vtol_in_trans_mode = true;
case TRANSITION_BACK:
_vtol_mode = TRANSITION;
_vtol_vehicle_status->vtol_in_trans_mode = true;
break;
}
}
void Tailsitter::update_mc_state()
{
if (!flag_idle_mc) {
// set idle speed for rotary wing mode
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
}
void Tailsitter::update_fw_state()
@@ -80,13 +236,124 @@ void Tailsitter::update_fw_state()
set_idle_fw();
flag_idle_mc = false;
}
_mc_roll_weight = 0.0f;
_mc_pitch_weight = 0.0f;
_mc_yaw_weight = 0.0f;
}
void Tailsitter::update_transition_state()
{
if (!_flag_was_in_trans_mode) {
// save desired heading for transition and last thrust value
_yaw_transition = _v_att->yaw;
_pitch_transition_start = _v_att->pitch;
_throttle_transition_start = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
_flag_was_in_trans_mode = true;
}
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/
if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P1-0.2f) ) {
_v_att_sp->pitch_body = _pitch_transition_start -
fabsf(PITCH_TRANSITION_FRONT_P1 / _params_tiltrotor.front_trans_dur) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f);
}
/** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */
if (_airspeed->true_airspeed_m_s <= _params_tiltrotor.airspeed_trans) ) {
_v_att_sp->thrust = _throttle_transition_start + (THROTTLE_TRANSITION_MAX * _throttle_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f);
/** if the limit reached stop adding thrust */
if (_v_att_sp->thrust >= (THROTTLE_TRANSITION_MAX * _throttle_transition_start) ) {
_v_att_sp->thrust = (THROTTLE_TRANSITION_MAX * _throttle_transition_start);
}
}
/** ^ else condition nescesary for publishing values ? ^ */
// disable mc yaw control once the plane has picked up speed
if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
}else {
_mc_yaw_weight = 1.0f;
}
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down
/** FW motor is switched */
/** is thrust assignment nescesary? */
/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/
if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2-0.2f) ) {
_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 -
(fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) / _params_tiltrotor.front_trans_dur_p2) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
}
/** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/
_mc_roll_weight = 1.0f - 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
_mc_pitch_weight = 1.0f - 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
/** keep yaw disabled */
_mc_yaw_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/
if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_BACK+0.2f) ) {
_v_att_sp->pitch_body = _pitch_transition_start +
fabsf(PITCH_TRANSITION_BACK / _params_tiltrotor.back_trans_dur) *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f);
}
/** smoothly move control weight to MC */
_mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f);
_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f);
/** keep yaw disabled */
_mc_yaw_weight = 0.0f;
// set zero throttle for backtransition otherwise unwanted moments will be created
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
}
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
// compute desired attitude and thrust setpoint for the transition
_v_att_sp->timestamp = hrt_absolute_time();
_v_att_sp->roll_body = 0;
_v_att_sp->yaw_body = _yaw_transition;
_v_att_sp->R_valid = true;
math::Matrix<3,3> R_sp;
R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body);
memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body));
math::Quaternion q_sp;
q_sp.from_dcm(R_sp);
memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
}
void Tailsitter::update_external_state()
{
@@ -110,8 +377,7 @@ void Tailsitter::calc_tot_airspeed()
_airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
}
void
Tailsitter::scale_mc_output()
void Tailsitter::scale_mc_output()
{
// scale around tuning airspeed
float airspeed;
+91 -11
View File
@@ -31,18 +31,21 @@
*
****************************************************************************/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*
*/
#ifndef TAILSITTER_H
#define TAILSITTER_H
#include "vtol_type.h"
#include <systemlib/perf_counter.h>
#include <systemlib/perf_counter.h> /** is it necsacery? **/
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
class Tailsitter : public VtolType
{
@@ -51,21 +54,98 @@ public:
Tailsitter(VtolAttitudeControl *_att_controller);
~Tailsitter();
/**
* Update vtol state.
*/
void update_vtol_state();
/**
* Update multicopter state.
*/
void update_mc_state();
/**
* Update fixed wing state.
*/
void update_fw_state();
/**
* Update transition state.
*/
void update_transition_state();
/**
* Update external state.
*/
void update_external_state();
private:
void fill_actuator_outputs();
void calc_tot_airspeed();
void scale_mc_output();
float _airspeed_tot;
struct {
float front_trans_dur; /**< duration of first part of front transition */
float front_trans_dur_p2;
float back_trans_dur; /**< duration of back transition */
float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */
float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */
int elevons_mc_lock; /**< lock elevons in multicopter mode */
} _params_tailsitter;
struct {
param_t front_trans_dur;
param_t front_trans_dur_p2;
param_t back_trans_dur;
param_t airspeed_trans;
param_t airspeed_blend_start;
param_t elevons_mc_lock;
} _params_handles_tailsitter;
enum vtol_mode {
MC_MODE = 0, /**< vtol is in multicopter mode */
TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */
TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */
TRANSITION_BACK, /**< vtol is in back transition mode */
FW_MODE /**< vtol is in fixed wing mode */
};
struct {
vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */
hrt_abstime transition_start; /**< absoulte time at which front transition started */
}_vtol_schedule;
float _airspeed_tot; /** speed estimation for propwash controlled surfaces */
float _roll_weight_mc; /**< multicopter desired roll moment weight */
float _yaw_weight_mc; /**< multicopter desired yaw moment weight */
/** not sure about it yet ?! **/
const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */
/** should this anouncement stay? **/
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
/**
* Speed estimation for propwash controlled surfaces.
*/
void calc_tot_airspeed();
/** is this one still needed? */
void scale_mc_output();
/**
* Write control values to actuator output topics.
*/
void fill_actuator_outputs();
/**
* Update parameters.
*/
int parameters_update();
};
#endif
@@ -0,0 +1,56 @@
/****************************************************************************
*
* Copyright (c) 2015 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 tailsitter_params.c
* Parameters for vtol attitude controller.
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*/
#include <systemlib/param/param.h>
/**
* Duration of front transition phase 2
*
* Time in seconds it should take for the rotors to rotate forward completely from the point
* when the plane has picked up enough airspeed and is ready to go into fixed wind mode.
*
* @min 0.1
* @max 2
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);