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

Merge pull request #2662 from UAVenture/vtol_beta_backport 

VTOL beta backport
This commit is contained in:
Lorenz Meier 2015-08-11 13:39:13 +02:00
parent fc4754f8e5 6b6fe5d229
commit 1b8c98386b
18 changed files with 649 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ROMFS/px4fmu_common/init.d/13002_firefly6
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

50
ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad Normal file
View File

@ -0,0 +1,50 @@
#!nsh
#
# @name Generic AAERT tailplane airframe with Quad VTOL.
#
# @type Standard VTOL
#
# @maintainer Simon Wilks <simon@uaventure.com>
#
sh /etc/init.d/rc.vtol_defaults
if [ $AUTOCNF == yes ]
then
param set VT_TYPE 2
param set VT_MOT_COUNT 4
param set VT_TRANS_THR 0.75
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.15
param set MC_ROLLRATE_I 0.002
param set MC_ROLLRATE_D 0.003
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_I 0.002
param set MC_PITCHRATE_D 0.003
param set MC_PITCHRATE_FF 0.0
param set MC_YAW_P 2.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
fi
set MIXER vtol_quad_x
set PWM_OUT 12345678
set MIXER_AUX vtol_AAERT
set PWM_AUX_RATE 50
set PWM_AUX_OUT 1234
set PWM_AUX_DISARMED 1000
set PWM_AUX_MIN 1000
set PWM_AUX_MAX 2000
set MAV_TYPE 22
param set VT_MOT_COUNT 4
param set VT_IDLE_PWM_MC 1080
param set VT_TYPE 2

4
ROMFS/px4fmu_common/init.d/rcS
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

6
ROMFS/px4fmu_common/mixers/firefly6.aux.mix
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

32
ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix Normal file
View File

@ -0,0 +1,32 @@
Mixer for an AAERT VTOL
=======================
Aileron 1 mixer
---------------
M: 1
O: 10000 10000 0 -10000 10000
S: 1 0 -7500 -7500 0 -10000 10000
Aileron 2 mixer
---------------
M: 1
O: 10000 10000 0 -10000 10000
S: 1 0 -7500 -7500 0 -10000 10000
Elevator mixer
--------------
M: 1
O: 10000 10000 0 -10000 10000
S: 1 1 10000 10000 0 -10000 10000
Rudder mixer
------------
M: 1
O: 10000 10000 0 -10000 10000
S: 1 2 -10000 -10000 0 -10000 10000
Throttle mixer
--------------
M: 1
O: 10000 10000 0 -10000 10000
S: 1 3 0 20000 -10000 -10000 10000

4
ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix Normal file
View File

@ -0,0 +1,4 @@
# VTOL quad X mixer for PX4FMU
#=============================
R: 4x 10000 10000 10000 0

9
src/modules/fw_att_control/fw_att_control_main.cpp
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 */

4
src/modules/vtol_att_control/module.mk
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

296
src/modules/vtol_att_control/standard.cpp Normal file
View File

@ -0,0 +1,296 @@
/****************************************************************************
*
* 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 standard.cpp
*
* @author Simon Wilks <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "standard.h"
#include "vtol_att_control_main.h"
Standard::Standard(VtolAttitudeControl *attc) :
VtolType(attc),
_flag_enable_mc_motors(true),
_pusher_throttle(0.0f),
_mc_att_ctl_weight(1.0f),
_airspeed_trans_blend_margin(0.0f)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
_params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND");
_params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS");
}
Standard::~Standard()
{
}
int
Standard::parameters_update()
{
float v;
/* duration of a forwards transition to fw mode */
param_get(_params_handles_standard.front_trans_dur, &v);
_params_standard.front_trans_dur = math::constrain(v, 0.0f, 5.0f);
/* duration of a back transition to mc mode */
param_get(_params_handles_standard.back_trans_dur, &v);
_params_standard.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
/* target throttle value for pusher motor during the transition to fw mode */
param_get(_params_handles_standard.pusher_trans, &v);
_params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f);
/* airspeed at which it we should switch to fw mode */
param_get(_params_handles_standard.airspeed_trans, &v);
_params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f);
/* airspeed at which we start blending mc/fw controls */
param_get(_params_handles_standard.airspeed_blend, &v);
_params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f);
_airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend;
return OK;
}
void Standard::update_vtol_state()
{
parameters_update();
/* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up
* forward speed. After the vehicle has picked up enough speed the rotors shutdown.
* For the back transition the pusher motor is immediately stopped and rotors reactivated.
*/
if (_manual_control_sp->aux1 < 0.0f) {
// the transition to fw mode switch is off
if (_vtol_schedule.flight_mode == MC_MODE) {
// in mc mode
_vtol_schedule.flight_mode = MC_MODE;
_mc_att_ctl_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == FW_MODE) {
// transition to mc mode
_vtol_schedule.flight_mode = TRANSITION_TO_MC;
_flag_enable_mc_motors = true;
_vtol_schedule.transition_start = hrt_absolute_time();
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// failsafe back to mc mode
_vtol_schedule.flight_mode = MC_MODE;
_mc_att_ctl_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// keep transitioning to mc mode
_vtol_schedule.flight_mode = MC_MODE;
}
// the pusher motor should never be powered when in or transitioning to mc mode
_pusher_throttle = 0.0f;
} else {
// the transition to fw mode switch is on
if (_vtol_schedule.flight_mode == MC_MODE) {
// start transition to fw mode
_vtol_schedule.flight_mode = TRANSITION_TO_FW;
_vtol_schedule.transition_start = hrt_absolute_time();
} else if (_vtol_schedule.flight_mode == FW_MODE) {
// in fw mode
_vtol_schedule.flight_mode = FW_MODE;
_mc_att_ctl_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) {
_vtol_schedule.flight_mode = FW_MODE;
// we can turn off the multirotor motors now
_flag_enable_mc_motors = false;
// don't set pusher throttle here as it's being ramped up elsewhere
}
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// transitioning to mc mode & transition switch on - failsafe back into fw mode
_vtol_schedule.flight_mode = FW_MODE;
}
}
// map 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_TO_FW:
case TRANSITION_TO_MC:
_vtol_mode = TRANSITION;
break;
}
}
void Standard::update_transition_state()
{
if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
if (_params_standard.front_trans_dur <= 0.0f) {
// just set the final target throttle value
_pusher_throttle = _params_standard.pusher_trans;
} else if (_pusher_throttle <= _params_standard.pusher_trans) {
// ramp up throttle to the target throttle value
_pusher_throttle = _params_standard.pusher_trans *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f);
}
// do blending of mc and fw controls if a blending airspeed has been provided
if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) {
_mc_att_ctl_weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
} else {
// at low speeds give full weight to mc
_mc_att_ctl_weight = 1.0f;
}
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// continually increase mc attitude control as we transition back to mc mode
if (_params_standard.back_trans_dur > 0.0f) {
_mc_att_ctl_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
} else {
_mc_att_ctl_weight = 1.0f;
}
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
}
}
_mc_att_ctl_weight = math::constrain(_mc_att_ctl_weight, 0.0f, 1.0f);
}
void Standard::update_mc_state()
{
// do nothing
}
void Standard::process_mc_data()
{
fill_att_control_output();
}
void Standard::process_fw_data()
{
fill_att_control_output();
}
void Standard::update_fw_state()
{
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (!_flag_enable_mc_motors) {
set_max_mc(950);
set_idle_fw(); // force them to stop, not just idle
_flag_enable_mc_motors = true;
}
}
void Standard::update_external_state()
{
}
/**
* Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine
* what proportion of control should be applied to each of the control groups (mc and fw).
*/
void Standard::fill_att_control_output()
{
/* multirotor controls */
_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _mc_att_ctl_weight; // roll
_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _mc_att_ctl_weight; // pitch
_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _mc_att_ctl_weight; // yaw
_actuators_out_0->control[3] = _actuators_mc_in->control[3]; // throttle
/* fixed wing controls */
const float fw_att_ctl_weight = 1.0f - _mc_att_ctl_weight;
_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * fw_att_ctl_weight; //roll
_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim) * fw_att_ctl_weight; //pitch
_actuators_out_1->control[2] = _actuators_fw_in->control[2] * fw_att_ctl_weight; // yaw
// set the fixed wing throttle control
if (_vtol_schedule.flight_mode == FW_MODE) {
// take the throttle value commanded by the fw controller
_actuators_out_1->control[3] = _actuators_fw_in->control[3];
} else {
// otherwise we may be ramping up the throttle during the transition to fw mode
_actuators_out_1->control[3] = _pusher_throttle;
}
}
/**
* Disable all multirotor motors when in fw mode.
*/
void
Standard::set_max_mc(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++) {
pwm_values.values[i] = pwm_value;
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);
}

107
src/modules/vtol_att_control/standard.h Normal file
View File

@ -0,0 +1,107 @@
/****************************************************************************
*
* 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 standard.h
* VTOL with fixed multirotor motor configurations (such as quad) and a pusher
* (or puller aka tractor) motor for forward flight.
*
* @author Simon Wilks <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef STANDARD_H
#define STANDARD_H
#include "vtol_type.h"
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
class Standard : public VtolType
{
public:
Standard(VtolAttitudeControl * _att_controller);
~Standard();
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 pusher_trans;
float airspeed_blend;
float airspeed_trans;
} _params_standard;
struct {
param_t front_trans_dur;
param_t back_trans_dur;
param_t pusher_trans;
param_t airspeed_blend;
param_t airspeed_trans;
} _params_handles_standard;
enum vtol_mode {
MC_MODE = 0,
TRANSITION_TO_FW,
TRANSITION_TO_MC,
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_enable_mc_motors;
float _pusher_throttle;
float _mc_att_ctl_weight; // the amount of multicopter attitude control that should be applied in fixed wing mode while transitioning
float _airspeed_trans_blend_margin;
void fill_att_control_output();
void set_max_mc(unsigned pwm_value);
int parameters_update();
};
#endif

51
src/modules/vtol_att_control/standard_params.c Normal file
View File

@ -0,0 +1,51 @@
/****************************************************************************
*
* 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 standard_params.c
* Parameters for the standard VTOL controller.
*
* @author Simon Wilks <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*/
#include <systemlib/param/param.h>
/**
* Target throttle value for pusher/puller motor during the transition to fw mode
*
* @min 0.0
* @max 1.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TRANS_THR, 0.6f);

58
src/modules/vtol_att_control/tiltrotor.cpp
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
}
/**

3
src/modules/vtol_att_control/tiltrotor.h
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);

39
src/modules/vtol_att_control/tiltrotor_params.c
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);

6
src/modules/vtol_att_control/vtol_att_control_main.cpp
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) {

2
src/modules/vtol_att_control/vtol_att_control_main.h
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***********************************************************************

48
src/modules/vtol_att_control/vtol_att_control_params.c
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);

4
src/modules/vtol_att_control/vtol_type.h
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