dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #2418 from tumbili/vtol_rework 

Vtol rework
This commit is contained in:
Lorenz Meier 2015-06-19 00:34:21 +02:00
parent e23712c47c b3c3d6634c
commit cc9d8c7a1c
13 changed files with 1392 additions and 369 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/mixers/firefly6.aux.mix
View File

@ -11,12 +11,12 @@ 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

2
src/drivers/drv_pwm_output.h
View File

@ -93,7 +93,7 @@ __BEGIN_DECLS
/**
* Lowest PWM allowed as the maximum PWM
*/
#define PWM_LOWEST_MAX 1400
#define PWM_LOWEST_MAX 950
/**
* Do not output a channel with this value

7
src/modules/vtol_att_control/module.mk
View File

@ -38,7 +38,10 @@
MODULE_COMMAND = vtol_att_control
SRCS = vtol_att_control_main.cpp \
vtol_att_control_params.c
vtol_att_control_params.c \
tiltrotor_params.c \
tiltrotor.cpp \
vtol_type.cpp \
tailsitter.cpp
EXTRACXXFLAGS = -Wno-write-strings

184
src/modules/vtol_att_control/tailsitter.cpp Normal file
View File

@ -0,0 +1,184 @@
/****************************************************************************
*
* 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.cpp
*
* @author Roman Bapst <bapstroman@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"))
{
}
Tailsitter::~Tailsitter()
{
}
void Tailsitter::update_vtol_state()
{
// simply switch between the two modes
if (_manual_control_sp->aux1 < 0.0f) {
_vtol_mode = ROTARY_WING;
} else if (_manual_control_sp->aux1 > 0.0f) {
_vtol_mode = FIXED_WING;
}
}
void Tailsitter::update_mc_state()
{
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
}
void Tailsitter::process_mc_data()
{
// scale pitch control with total airspeed
//scale_mc_output();
fill_mc_att_control_output();
}
void Tailsitter::update_fw_state()
{
if (flag_idle_mc) {
set_idle_fw();
flag_idle_mc = false;
}
}
void Tailsitter::process_fw_data()
{
fill_fw_att_control_output();
}
void Tailsitter::update_transition_state()
{
}
void Tailsitter::update_external_state()
{
}
void Tailsitter::calc_tot_airspeed()
{
float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s); // prevent numerical drama
// calculate momentary power of one engine
float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count;
P = math::constrain(P,1.0f,_params->power_max);
// calculate prop efficiency
float power_factor = 1.0f - P*_params->prop_eff/_params->power_max;
float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f;
eta = math::constrain(eta,0.001f,1.0f); // live on the safe side
// calculate induced airspeed by propeller
float v_ind = (airspeed/eta - airspeed)*2.0f;
// calculate total airspeed
float airspeed_raw = airspeed + v_ind;
// apply low-pass filter
_airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
}
void
Tailsitter::scale_mc_output()
{
// scale around tuning airspeed
float airspeed;
calc_tot_airspeed(); // estimate air velocity seen by elevons
// if airspeed is not updating, we assume the normal average speed
if (bool nonfinite = !isfinite(_airspeed->true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
airspeed = _params->mc_airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed_tot;
airspeed = math::constrain(airspeed,_params->mc_airspeed_min, _params->mc_airspeed_max);
}
_vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : airspeed);
_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
}
/**
* Prepare message to acutators with data from fw attitude controller.
*/
void Tailsitter::fill_fw_att_control_output()
{
/*For the first test in fw mode, only use engines for thrust!!!*/
_actuators_out_0->control[0] = 0;
_actuators_out_0->control[1] = 0;
_actuators_out_0->control[2] = 0;
_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
}
/**
* Prepare message to acutators with data from mc attitude controller.
*/
void Tailsitter::fill_mc_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];
//set neutral position for elevons
_actuators_out_1->control[0] = _actuators_mc_in->control[2]; //roll elevon
_actuators_out_1->control[1] = _actuators_mc_in->control[1]; //pitch elevon
}

74
src/modules/vtol_att_control/tailsitter.h Normal file
View File

@ -0,0 +1,74 @@
/****************************************************************************
*
* 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 tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef TAILSITTER_H
#define TAILSITTER_H
#include "vtol_type.h"
#include <systemlib/perf_counter.h>
class Tailsitter : public VtolType
{
public:
Tailsitter(VtolAttitudeControl * _att_controller);
~Tailsitter();
void update_vtol_state();
void update_mc_state();
void process_mc_data();
void update_fw_state();
void process_fw_data();
void update_transition_state();
void update_external_state();
private:
void fill_mc_att_control_output();
void fill_fw_att_control_output();
void calc_tot_airspeed();
void scale_mc_output();
float _airspeed_tot;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
};
#endif

357
src/modules/vtol_att_control/tiltrotor.cpp Normal file
View File

@ -0,0 +1,357 @@
/****************************************************************************
*
* 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 tiltrotor.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "tiltrotor.h"
#include "vtol_att_control_main.h"
#define ARSP_BLEND_START 8.0f // airspeed at which we start blending mc/fw controls
Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
VtolType(attc),
flag_max_mc(true),
_tilt_control(0.0f),
_roll_weight_mc(1.0f)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
_params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR");
_params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC");
_params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS");
_params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW");
_params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS");
_params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
}
Tiltrotor::~Tiltrotor()
{
}
int
Tiltrotor::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 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 tilt mechanism position in mc mode */
param_get(_params_handles_tiltrotor.tilt_mc, &v);
_params_tiltrotor.tilt_mc = v;
/* vtol tilt mechanism position in transition mode */
param_get(_params_handles_tiltrotor.tilt_transition, &v);
_params_tiltrotor.tilt_transition = v;
/* vtol tilt mechanism position in fw mode */
param_get(_params_handles_tiltrotor.tilt_fw, &v);
_params_tiltrotor.tilt_fw = v;
/* 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 lock elevons in multicopter */
param_get(_params_handles_tiltrotor.elevons_mc_lock, &l);
_params_tiltrotor.elevons_mc_lock = l;
return OK;
}
void Tiltrotor::update_vtol_state()
{
parameters_update();
/* 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 && _vtol_schedule.flight_mode == MC_MODE) {
// mc mode
_vtol_schedule.flight_mode = MC_MODE;
_tilt_control = _params_tiltrotor.tilt_mc;
_roll_weight_mc = 1.0f;
} else if (_manual_control_sp->aux1 < 0.0f && _vtol_schedule.flight_mode == FW_MODE) {
_vtol_schedule.flight_mode = TRANSITION_BACK;
flag_max_mc = true;
_vtol_schedule.transition_start = hrt_absolute_time();
} else if (_manual_control_sp->aux1 >= 0.0f && _vtol_schedule.flight_mode == MC_MODE) {
// instant of doeing a front-transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 > 0.0f) {
// check if we have reached airspeed to switch to fw mode
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
flag_max_mc = true;
_vtol_schedule.transition_start = hrt_absolute_time();
}
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 > 0.0f) {
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
_vtol_schedule.flight_mode = FW_MODE;
_tilt_control = _params_tiltrotor.tilt_fw;
}
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 < 0.0f) {
// failsave into mc mode
_vtol_schedule.flight_mode = MC_MODE;
_tilt_control = _params_tiltrotor.tilt_mc;
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 < 0.0f) {
// failsave into mc mode
_vtol_schedule.flight_mode = MC_MODE;
_tilt_control = _params_tiltrotor.tilt_mc;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 < 0.0f) {
if (_tilt_control <= _params_tiltrotor.tilt_mc) {
_vtol_schedule.flight_mode = MC_MODE;
_tilt_control = _params_tiltrotor.tilt_mc;
flag_max_mc = false;
}
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 > 0.0f) {
// failsave into fw mode
_vtol_schedule.flight_mode = FW_MODE;
_tilt_control = _params_tiltrotor.tilt_fw;
}
// tilt rotors if necessary
update_transition_state();
// map tiltrotor specific control phases to simple control modes
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
break;
case TRANSITION_FRONT_P1:
case TRANSITION_FRONT_P2:
case TRANSITION_BACK:
_vtol_mode = TRANSITION;
break;
}
}
void Tiltrotor::update_mc_state()
{
// adjust max pwm for rear motors to spin up
if (!flag_max_mc) {
set_max_mc();
flag_max_mc = true;
}
// set idle speed for rotary wing mode
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
}
void Tiltrotor::process_mc_data()
{
fill_mc_att_control_output();
}
void Tiltrotor::update_fw_state()
{
/* in fw mode we need the rear motors to stop spinning, in backtransition
* mode we let them spin in idle
*/
if (flag_max_mc) {
if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
set_max_fw(1200);
set_idle_mc();
} else {
set_max_fw(950);
set_idle_fw();
}
flag_max_mc = false;
}
// adjust idle for fixed wing flight
if (flag_idle_mc) {
set_idle_fw();
flag_idle_mc = false;
}
}
void Tiltrotor::process_fw_data()
{
fill_fw_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);
}
// do blending of mc and fw controls
if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) {
_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
_roll_weight_mc = 1.0f;
}
_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);
_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);
if (_tilt_control > _params_tiltrotor.tilt_mc) {
_tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress;
}
_roll_weight_mc = progress;
}
}
void Tiltrotor::update_external_state()
{
}
/**
* Prepare message to acutators with data from mc attitude controller.
*/
void Tiltrotor::fill_mc_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_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[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;
}
/**
* Kill rear motors for the FireFLY6 when in fw mode.
*/
void
Tiltrotor::set_max_fw(unsigned pwm_value)
{
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
if (i == 2 || i == 3) {
pwm_values.values[i] = pwm_value;
} else {
pwm_values.values[i] = 2000;
}
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting max values");}
close(fd);
}
void
Tiltrotor::set_max_mc()
{
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = 2000;
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting max values");}
close(fd);
}

110
src/modules/vtol_att_control/tiltrotor.h Normal file
View File

@ -0,0 +1,110 @@
/****************************************************************************
*
* 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 tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef TILTROTOR_H
#define TILTROTOR_H
#include "vtol_type.h"
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
class Tiltrotor : public VtolType
{
public:
Tiltrotor(VtolAttitudeControl * _att_controller);
~Tiltrotor();
void update_vtol_state();
void update_mc_state();
void process_mc_data();
void update_fw_state();
void process_fw_data();
void update_transition_state();
void update_external_state();
private:
struct {
float front_trans_dur;
float back_trans_dur;
float tilt_mc;
float tilt_transition;
float tilt_fw;
float airspeed_trans;
int elevons_mc_lock; // lock elevons in multicopter mode
} _params_tiltrotor;
struct {
param_t front_trans_dur;
param_t back_trans_dur;
param_t tilt_mc;
param_t tilt_transition;
param_t tilt_fw;
param_t airspeed_trans;
param_t elevons_mc_lock;
} _params_handles_tiltrotor;
enum vtol_mode {
MC_MODE = 0,
TRANSITION_FRONT_P1,
TRANSITION_FRONT_P2,
TRANSITION_BACK,
FW_MODE
};
struct {
vtol_mode flight_mode; // indicates in which mode the vehicle is in
hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition)
}_vtol_schedule;
bool flag_max_mc;
float _tilt_control;
float _roll_weight_mc;
void fill_mc_att_control_output();
void fill_fw_att_control_output();
void set_max_mc();
void set_max_fw(unsigned pwm_value);
int parameters_update();
};
#endif

118
src/modules/vtol_att_control/tiltrotor_params.c Normal file
View File

@ -0,0 +1,118 @@
/****************************************************************************
*
* 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 tiltrotor_params.c
* Parameters for vtol attitude controller.
*
* @author Roman Bapst <bapstroman@gmail.com>
*/
#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
*
* Position of tilt servo in mc mode
*
* @min 0.0
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f);
/**
* Position of tilt servo in transition mode
*
* Position of tilt servo in transition mode
*
* @min 0.0
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f);
/**
* Position of tilt servo in fw mode
*
* Position of tilt servo in fw mode
*
* @min 0.0
* @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);
/**
* Lock elevons in multicopter mode
*
* If set to 1 the elevons are locked in multicopter mode
*
* @min 0
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK,0);

437
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -43,166 +43,7 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
*
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_mc.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/battery_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include "drivers/drv_pwm_output.h"
#include <nuttx/fs/ioctl.h>
#include <fcntl.h>
extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
class VtolAttitudeControl
{
public:
VtolAttitudeControl();
~VtolAttitudeControl();
int start(); /* start the task and return OK on success */
private:
//******************flags & handlers******************************************************
bool _task_should_exit;
int _control_task; //task handle for VTOL attitude controller
/* handlers for subscriptions */
int _v_att_sub; //vehicle attitude subscription
int _v_att_sp_sub; //vehicle attitude setpoint subscription
int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
int _v_control_mode_sub; //vehicle control mode subscription
int _params_sub; //parameter updates subscription
int _manual_control_sp_sub; //manual control setpoint subscription
int _armed_sub; //arming status subscription
int _local_pos_sub; // sensor subscription
int _airspeed_sub; // airspeed subscription
int _battery_status_sub; // battery status subscription
int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs
int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs
//handlers for publishers
orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust)
orb_advert_t _actuators_1_pub;
orb_advert_t _vtol_vehicle_status_pub;
orb_advert_t _v_rates_sp_pub;
//*******************data containers***********************************************************
struct vehicle_attitude_s _v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint
struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint
struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint
struct vehicle_control_mode_s _v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s _vtol_vehicle_status;
struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer
struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons)
struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control
struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control
struct actuator_armed_s _armed; //actuator arming status
struct vehicle_local_position_s _local_pos;
struct airspeed_s _airspeed; // airspeed
struct battery_status_s _batt_status; // battery status
struct {
param_t idle_pwm_mc; //pwm value for idle in mc mode
param_t vtol_motor_count;
param_t vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode
float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash)
float mc_airspeed_trim; // trim airspeed in multicopter mode
float mc_airspeed_max; // max airpseed in multicopter mode
float fw_pitch_trim; // trim for neutral elevon position in fw mode
float power_max; // maximum power of one engine
float prop_eff; // factor to calculate prop efficiency
float arsp_lp_gain; // total airspeed estimate low pass gain
} _params;
struct {
param_t idle_pwm_mc;
param_t vtol_motor_count;
param_t vtol_fw_permanent_stab;
param_t mc_airspeed_min;
param_t mc_airspeed_trim;
param_t mc_airspeed_max;
param_t fw_pitch_trim;
param_t power_max;
param_t prop_eff;
param_t arsp_lp_gain;
} _params_handles;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
/* for multicopters it is usual to have a non-zero idle speed of the engines
* for fixed wings we want to have an idle speed of zero since we do not want
* to waste energy when gliding. */
bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
unsigned _motor_count; // number of motors
float _airspeed_tot;
float _tilt_control;
//*****************Member functions***********************************************************************
void task_main(); //main task
static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create.
void vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
void vehicle_manual_poll(); //Check for changes in manual inputs.
void arming_status_poll(); //Check for arming status updates.
void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
void vehicle_rates_sp_mc_poll();
void vehicle_rates_sp_fw_poll();
void vehicle_local_pos_poll(); // Check for changes in sensor values
void vehicle_airspeed_poll(); // Check for changes in airspeed
void vehicle_battery_poll(); // Check for battery updates
void parameters_update_poll(); //Check if parameters have changed
int parameters_update(); //Update local paraemter cache
void fill_mc_att_control_output(); //write mc_att_control results to actuator message
void fill_fw_att_control_output(); //write fw_att_control results to actuator message
void fill_mc_att_rates_sp();
void fill_fw_att_rates_sp();
void set_idle_fw();
void set_idle_mc();
void scale_mc_output();
void calc_tot_airspeed(); // estimated airspeed seen by elevons
};
#include "vtol_att_control_main.h"
namespace VTOL_att_control
{
@ -230,19 +71,12 @@ VtolAttitudeControl::VtolAttitudeControl() :
_battery_status_sub(-1),
//init publication handlers
_actuators_0_pub(-1),
_actuators_1_pub(-1),
_vtol_vehicle_status_pub(-1),
_v_rates_sp_pub(-1),
_actuators_0_pub(0),
_actuators_1_pub(0),
_vtol_vehicle_status_pub(0),
_v_rates_sp_pub(0)
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input"))
{
flag_idle_mc = true;
_airspeed_tot = 0.0f;
_tilt_control = 0.0f;
memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
_vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/
memset(&_v_att, 0, sizeof(_v_att));
@ -276,9 +110,20 @@ VtolAttitudeControl::VtolAttitudeControl() :
_params_handles.power_max = param_find("VT_POWER_MAX");
_params_handles.prop_eff = param_find("VT_PROP_EFF");
_params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN");
_params_handles.vtol_type = param_find("VT_TYPE");
/* fetch initial parameter values */
parameters_update();
if (_params.vtol_type == 0) {
_tailsitter = new Tailsitter(this);
_vtol_type = _tailsitter;
} else if (_params.vtol_type == 1) {
_tiltrotor = new Tiltrotor(this);
_vtol_type = _tiltrotor;
} else {
_task_should_exit = true;
}
}
/**
@ -470,6 +315,7 @@ int
VtolAttitudeControl::parameters_update()
{
float v;
int l;
/* idle pwm for mc mode */
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
@ -507,42 +353,12 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.arsp_lp_gain, &v);
_params.arsp_lp_gain = v;
param_get(_params_handles.vtol_type, &l);
_params.vtol_type = l;
return OK;
}
/**
* Prepare message to acutators with data from mc attitude controller.
*/
void VtolAttitudeControl::fill_mc_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];
//set neutral position for elevons
_actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon
_actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon
_actuators_out_1.control[4] = _tilt_control; // for tilt-rotor control
}
/**
* Prepare message to acutators with data from fw attitude controller.
*/
void VtolAttitudeControl::fill_fw_att_control_output()
{
/*For the first test in fw mode, only use engines for thrust!!!*/
_actuators_out_0.control[0] = 0;
_actuators_out_0.control[1] = 0;
_actuators_out_0.control[2] = 0;
_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
}
/**
* Prepare message for mc attitude rates setpoint topic
*/
@ -565,109 +381,6 @@ void VtolAttitudeControl::fill_fw_att_rates_sp()
_v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust;
}
/**
* Adjust idle speed for fw mode.
*/
void VtolAttitudeControl::set_idle_fw()
{
int ret;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
unsigned pwm_value = PWM_LOWEST_MIN;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (unsigned i = 0; i < _params.vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
close(fd);
}
/**
* Adjust idle speed for mc mode.
*/
void VtolAttitudeControl::set_idle_mc()
{
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
unsigned pwm_value = _params.idle_pwm_mc;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (unsigned i = 0; i < _params.vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
close(fd);
}
void
VtolAttitudeControl::scale_mc_output() {
// scale around tuning airspeed
float airspeed;
calc_tot_airspeed(); // estimate air velocity seen by elevons
// if airspeed is not updating, we assume the normal average speed
if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _params.mc_airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed_tot;
airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max);
}
_vtol_vehicle_status.airspeed_tot = airspeed; // save value for logging
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed);
_actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
}
void VtolAttitudeControl::calc_tot_airspeed() {
float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s); // prevent numerical drama
// calculate momentary power of one engine
float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count;
P = math::constrain(P,1.0f,_params.power_max);
// calculate prop efficiency
float power_factor = 1.0f - P*_params.prop_eff/_params.power_max;
float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f;
eta = math::constrain(eta,0.001f,1.0f); // live on the safe side
// calculate induced airspeed by propeller
float v_ind = (airspeed/eta - airspeed)*2.0f;
// calculate total airspeed
float airspeed_raw = airspeed + v_ind;
// apply low-pass filter
_airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
}
void
VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
@ -701,8 +414,7 @@ void VtolAttitudeControl::task_main()
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
// make sure we start with idle in mc mode
set_idle_mc();
flag_idle_mc = true;
_vtol_type->set_idle_mc();
/* wakeup source*/
struct pollfd fds[3]; /*input_mc, input_fw, parameters*/
@ -764,83 +476,80 @@ void VtolAttitudeControl::task_main()
vehicle_airspeed_poll();
vehicle_battery_poll();
// update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state();
if (_manual_control_sp.aux1 < 0.0f) { /* vehicle is in mc mode */
// check in which mode we are in and call mode specific functions
if (_vtol_type->get_mode() == ROTARY_WING) {
// vehicle is in rotary wing mode
_vtol_vehicle_status.vtol_in_rw_mode = true;
if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */
set_idle_mc();
flag_idle_mc = true;
}
_vtol_type->update_mc_state();
/* got data from mc_att_controller */
// got data from mc attitude controller
if (fds[0].revents & POLLIN) {
vehicle_manual_poll(); /* update remote input */
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
// scale pitch control with total airspeed
scale_mc_output();
_vtol_type->process_mc_data();
fill_mc_att_control_output();
fill_mc_att_rates_sp();
/* Only publish if the proper mode(s) are enabled */
if(_v_control_mode.flag_control_attitude_enabled ||
_v_control_mode.flag_control_rates_enabled)
{
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
}
}
if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */
} else if (_vtol_type->get_mode() == FIXED_WING) {
// vehicle is in fw mode
_vtol_vehicle_status.vtol_in_rw_mode = false;
if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */
set_idle_fw();
flag_idle_mc = false;
_vtol_type->update_fw_state();
// got data from fw attitude controller
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
vehicle_manual_poll();
_vtol_type->process_fw_data();
fill_fw_att_rates_sp();
}
} else if (_vtol_type->get_mode() == TRANSITION) {
// vehicle is doing a transition
bool got_new_data = false;
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
got_new_data = true;
}
if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
vehicle_manual_poll(); //update remote input
got_new_data = true;
}
fill_fw_att_control_output();
fill_fw_att_rates_sp();
// update transition state if got any new data
if (got_new_data) {
_vtol_type->update_transition_state();
}
/* Only publish if the proper mode(s) are enabled */
if(_v_control_mode.flag_control_attitude_enabled ||
_v_control_mode.flag_control_rates_enabled ||
_v_control_mode.flag_control_manual_enabled)
{
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else if (_vtol_type->get_mode() == EXTERNAL) {
// we are using external module to generate attitude/thrust setpoint
_vtol_type->update_external_state();
}
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
/* Only publish if the proper mode(s) are enabled */
if(_v_control_mode.flag_control_attitude_enabled ||
_v_control_mode.flag_control_rates_enabled ||
_v_control_mode.flag_control_manual_enabled)
{
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
}
// publish the attitude rates setpoint
if(_v_rates_sp_pub > 0) {

212
src/modules/vtol_att_control/vtol_att_control_main.h Normal file
View File

@ -0,0 +1,212 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 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 VTOL_att_control_main.cpp
* Implementation of an attitude controller for VTOL airframes. This module receives data
* from both the fixed wing- and the multicopter attitude controllers and processes it.
* It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward-
* flight or transition). It also publishes the resulting controls on the actuator controls topics.
*
* @author Roman Bapst <bapstr@ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
*/
#ifndef VTOL_ATT_CONTROL_MAIN_H
#define VTOL_ATT_CONTROL_MAIN_H
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_mc.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/battery_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <nuttx/fs/ioctl.h>
#include <fcntl.h>
#include "tiltrotor.h"
#include "tailsitter.h"
extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
class VtolAttitudeControl
{
public:
VtolAttitudeControl();
~VtolAttitudeControl();
int start(); /* start the task and return OK on success */
struct vehicle_attitude_s* get_att () {return &_v_att;}
struct vehicle_attitude_setpoint_s* get_att_sp () {return &_v_att_sp;}
struct vehicle_rates_setpoint_s* get_rates_sp () {return &_v_rates_sp;}
struct vehicle_rates_setpoint_s* get_mc_virtual_rates_sp () {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s* get_fw_virtual_rates_sp () {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s* get_manual_control_sp () {return &_manual_control_sp;}
struct vehicle_control_mode_s* get_control_mode () {return &_v_control_mode;}
struct vtol_vehicle_status_s* get_vehicle_status () {return &_vtol_vehicle_status;}
struct actuator_controls_s* get_actuators_out0 () {return &_actuators_out_0;}
struct actuator_controls_s* get_actuators_out1 () {return &_actuators_out_1;}
struct actuator_controls_s* get_actuators_mc_in () {return &_actuators_mc_in;}
struct actuator_controls_s* get_actuators_fw_in () {return &_actuators_fw_in;}
struct actuator_armed_s* get_armed () {return &_armed;}
struct vehicle_local_position_s* get_local_pos () {return &_local_pos;}
struct airspeed_s* get_airspeed () {return &_airspeed;}
struct battery_status_s* get_batt_status () {return &_batt_status;}
struct Params* get_params () {return &_params;}
private:
//******************flags & handlers******************************************************
bool _task_should_exit;
int _control_task; //task handle for VTOL attitude controller
/* handlers for subscriptions */
int _v_att_sub; //vehicle attitude subscription
int _v_att_sp_sub; //vehicle attitude setpoint subscription
int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
int _v_control_mode_sub; //vehicle control mode subscription
int _params_sub; //parameter updates subscription
int _manual_control_sp_sub; //manual control setpoint subscription
int _armed_sub; //arming status subscription
int _local_pos_sub; // sensor subscription
int _airspeed_sub; // airspeed subscription
int _battery_status_sub; // battery status subscription
int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs
int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs
//handlers for publishers
orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust)
orb_advert_t _actuators_1_pub;
orb_advert_t _vtol_vehicle_status_pub;
orb_advert_t _v_rates_sp_pub;
//*******************data containers***********************************************************
struct vehicle_attitude_s _v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint
struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint
struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint
struct vehicle_control_mode_s _v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s _vtol_vehicle_status;
struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer
struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons)
struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control
struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control
struct actuator_armed_s _armed; //actuator arming status
struct vehicle_local_position_s _local_pos;
struct airspeed_s _airspeed; // airspeed
struct battery_status_s _batt_status; // battery status
Params _params; // struct holding the parameters
struct {
param_t idle_pwm_mc;
param_t vtol_motor_count;
param_t vtol_fw_permanent_stab;
param_t mc_airspeed_min;
param_t mc_airspeed_trim;
param_t mc_airspeed_max;
param_t fw_pitch_trim;
param_t power_max;
param_t prop_eff;
param_t arsp_lp_gain;
param_t vtol_type;
} _params_handles;
/* for multicopters it is usual to have a non-zero idle speed of the engines
* for fixed wings we want to have an idle speed of zero since we do not want
* to waste energy when gliding. */
unsigned _motor_count; // number of motors
float _airspeed_tot;
VtolType * _vtol_type; // base class for different vtol types
Tiltrotor * _tiltrotor; // tailsitter vtol type
Tailsitter * _tailsitter; // tiltrotor vtol type
//*****************Member functions***********************************************************************
void task_main(); //main task
static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create.
void vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
void vehicle_manual_poll(); //Check for changes in manual inputs.
void arming_status_poll(); //Check for arming status updates.
void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
void vehicle_rates_sp_mc_poll();
void vehicle_rates_sp_fw_poll();
void vehicle_local_pos_poll(); // Check for changes in sensor values
void vehicle_airspeed_poll(); // Check for changes in airspeed
void vehicle_battery_poll(); // Check for battery updates
void parameters_update_poll(); //Check if parameters have changed
int parameters_update(); //Update local paraemter cache
void fill_mc_att_rates_sp();
void fill_fw_att_rates_sp();
};
#endif

8
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -142,3 +142,11 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f);
*/
PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f);
/**
* VTOL Type (Tailsitter=0, Tiltrotor=1)
*
* @min 0
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_TYPE, 0);

133
src/modules/vtol_att_control/vtol_type.cpp Normal file
View File

@ -0,0 +1,133 @@
/****************************************************************************
*
* 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 airframe.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "vtol_type.h"
#include "drivers/drv_pwm_output.h"
#include <nuttx/fs/ioctl.h>
#include "vtol_att_control_main.h"
VtolType::VtolType(VtolAttitudeControl *att_controller) :
_attc(att_controller),
_vtol_mode(ROTARY_WING)
{
_v_att = _attc->get_att();
_v_att_sp = _attc->get_att_sp();
_v_rates_sp = _attc->get_rates_sp();
_mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp();
_fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp();
_manual_control_sp = _attc->get_manual_control_sp();
_v_control_mode = _attc->get_control_mode();
_vtol_vehicle_status = _attc->get_vehicle_status();
_actuators_out_0 = _attc->get_actuators_out0();
_actuators_out_1 = _attc->get_actuators_out1();
_actuators_mc_in = _attc->get_actuators_mc_in();
_actuators_fw_in = _attc->get_actuators_fw_in();
_armed = _attc->get_armed();
_local_pos = _attc->get_local_pos();
_airspeed = _attc->get_airspeed();
_batt_status = _attc->get_batt_status();
_params = _attc->get_params();
flag_idle_mc = true;
}
VtolType::~VtolType()
{
}
/**
* Adjust idle speed for mc mode.
*/
void VtolType::set_idle_mc()
{
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
unsigned pwm_value = _params->idle_pwm_mc;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
close(fd);
flag_idle_mc = true;
}
/**
* Adjust idle speed for fw mode.
*/
void VtolType::set_idle_fw()
{
int ret;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
unsigned pwm_value = PWM_LOWEST_MIN;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
close(fd);
}

115
src/modules/vtol_att_control/vtol_type.h Normal file
View File

@ -0,0 +1,115 @@
/****************************************************************************
*
* 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 airframe.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef VTOL_YYPE_H
#define VTOL_YYPE_H
struct Params {
int idle_pwm_mc; // pwm value for idle in mc mode
int vtol_motor_count; // number of motors
int vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode
float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash)
float mc_airspeed_trim; // trim airspeed in multicopter mode
float mc_airspeed_max; // max airpseed in multicopter mode
float fw_pitch_trim; // trim for neutral elevon position in fw mode
float power_max; // maximum power of one engine
float prop_eff; // factor to calculate prop efficiency
float arsp_lp_gain; // total airspeed estimate low pass gain
int vtol_type;
};
enum mode {
ROTARY_WING = 0,
FIXED_WING,
TRANSITION,
EXTERNAL
};
class VtolAttitudeControl;
class VtolType
{
public:
VtolType(VtolAttitudeControl *att_controller);
virtual ~VtolType();
virtual void update_vtol_state() = 0;
virtual void update_mc_state() = 0;
virtual void process_mc_data() = 0;
virtual void update_fw_state() = 0;
virtual void process_fw_data() = 0;
virtual void update_transition_state() = 0;
virtual void update_external_state() = 0;
void set_idle_mc();
void set_idle_fw();
mode get_mode () {return _vtol_mode;};
protected:
VtolAttitudeControl *_attc;
mode _vtol_mode;
struct vehicle_attitude_s *_v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint
struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint
struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint
struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s *_vtol_vehicle_status;
struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer
struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons)
struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control
struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control
struct actuator_armed_s *_armed; //actuator arming status
struct vehicle_local_position_s *_local_pos;
struct airspeed_s *_airspeed; // airspeed
struct battery_status_s *_batt_status; // battery status
struct Params *_params;
bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
};
#endif