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

Merge pull request #3090 from PX4/vtol 

Vtol work
This commit is contained in:
Roman Bapst 2015-11-25 17:06:49 +01:00
parent 2c07668b2f 7cb7245a84
commit ab95b4706e
20 changed files with 742 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
View File

@ -9,10 +9,10 @@ R: 4x 10000 10000 10000 0
#mixer for the elevons
M: 2
O: 10000 10000 0 -10000 10000
S: 1 0 10000 10000 0 -10000 10000
S: 1 1 10000 10000 0 -10000 10000
S: 1 0 5000 5000 0 -10000 10000
S: 1 1 5000 5000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 1 0 10000 10000 0 -10000 10000
S: 1 1 -10000 -10000 0 -10000 10000
S: 1 0 5000 5000 0 -10000 10000
S: 1 1 -5000 -5000 0 -10000 10000

2
Tools/sitl_run.sh
View File

@ -79,7 +79,7 @@ touch rootfs/eeprom/parameters
# Start Java simulator
if [ "$debugger" == "lldb" ]
then
lldb -- mainapp ../../../../${rc_script}_${program}
lldb -- mainapp ../../../../${rc_script}_${program}_${model}
elif [ "$debugger" == "gdb" ]
then
gdb --args mainapp ../../../../${rc_script}_${program}

23
msg/fw_virtual_attitude_setpoint.msg Normal file
View File

@ -0,0 +1,23 @@
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
float32 roll_body # body angle in NED frame
float32 pitch_body # body angle in NED frame
float32 yaw_body # body angle in NED frame
float32 yaw_sp_move_rate # rad/s (commanded by user)
float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame
bool R_valid # Set to true if rotation matrix is valid
# For quaternion-based attitude control
float32[4] q_d # Desired quaternion for quaternion control
bool q_d_valid # Set to true if quaternion vector is valid
float32[4] q_e # Attitude error in quaternion
bool q_e_valid # Set to true if quaternion error vector is valid
float32 thrust # Thrust in Newton the power system should generate
bool roll_reset_integral # Reset roll integral part (navigation logic change)
bool pitch_reset_integral # Reset pitch integral part (navigation logic change)
bool yaw_reset_integral # Reset yaw integral part (navigation logic change)

23
msg/mc_virtual_attitude_setpoint.msg Normal file
View File

@ -0,0 +1,23 @@
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
float32 roll_body # body angle in NED frame
float32 pitch_body # body angle in NED frame
float32 yaw_body # body angle in NED frame
float32 yaw_sp_move_rate # rad/s (commanded by user)
float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame
bool R_valid # Set to true if rotation matrix is valid
# For quaternion-based attitude control
float32[4] q_d # Desired quaternion for quaternion control
bool q_d_valid # Set to true if quaternion vector is valid
float32[4] q_e # Attitude error in quaternion
bool q_e_valid # Set to true if quaternion error vector is valid
float32 thrust # Thrust in Newton the power system should generate
bool roll_reset_integral # Reset roll integral part (navigation logic change)
bool pitch_reset_integral # Reset pitch integral part (navigation logic change)
bool yaw_reset_integral # Reset yaw integral part (navigation logic change)

1
msg/vehicle_status.msg
View File

@ -113,6 +113,7 @@ uint32 system_id # system id, inspired by MAVLink's system ID field
uint32 component_id # subsystem / component id, inspired by MAVLink's component ID field
bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
bool vtol_in_transition # True if VTOL is doing a transition
bool is_vtol # True if the system is VTOL capable
bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode
bool in_transition_mode

2
src/modules/commander/commander.cpp
View File

@ -1599,7 +1599,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
/* Make sure that this is only adjusted if vehicle really is of type vtol*/
/* Make sure that this is only adjusted if vehicle really is of type vtol */
if (is_vtol(&status)) {
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
status.in_transition_mode = vtol_status.vtol_in_trans_mode;

20
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -59,6 +59,7 @@
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
@ -142,6 +143,7 @@ private:
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
orb_id_t _attitude_setpoint_id;
struct control_state_s _ctrl_state; /**< control state */
struct accel_report _accel; /**< body frame accelerations */
@ -360,6 +362,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_rates_sp_id(0),
_actuators_id(0),
_attitude_setpoint_id(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@ -633,9 +636,11 @@ FixedwingAttitudeControl::vehicle_status_poll()
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
@ -1021,15 +1026,12 @@ FixedwingAttitudeControl::task_main()
att_sp.thrust = throttle_sp;
/* lazily publish the setpoint only once available */
if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
}
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp);
} else if (_attitude_setpoint_id) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp);
}
}

22
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -72,6 +72,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@ -166,6 +167,8 @@ private:
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
orb_id_t _attitude_setpoint_id;
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
@ -506,6 +509,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
_tecs_status_pub(nullptr),
_nav_capabilities_pub(nullptr),
/* publication ID */
_attitude_setpoint_id(0),
/* states */
_ctrl_state(),
_att_sp(),
@ -766,6 +772,14 @@ FixedwingPositionControl::vehicle_status_poll()
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
}
@ -1990,13 +2004,13 @@ FixedwingPositionControl::task_main()
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);
} else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
} else if (_attitude_setpoint_id) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
/* XXX check if radius makes sense here */

35
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -69,6 +69,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/mc_virtual_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
@ -141,6 +142,8 @@ private:
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
orb_id_t _attitude_setpoint_id;
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct control_state_s _ctrl_state; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
@ -336,6 +339,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_att_sp_pub(nullptr),
_local_pos_sp_pub(nullptr),
_global_vel_sp_pub(nullptr),
_attitude_setpoint_id(0),
_manual_thr_min(this, "MANTHR_MIN"),
_manual_thr_max(this, "MANTHR_MAX"),
_vel_x_deriv(this, "VELD"),
@ -533,6 +537,14 @@ MulticopterPositionControl::poll_subscriptions()
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
orb_check(_ctrl_state_sub, &updated);
@ -1183,10 +1195,9 @@ MulticopterPositionControl::task_main()
/* publish attitude setpoint */
if (_att_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
} else {
@ -1610,16 +1621,18 @@ MulticopterPositionControl::task_main()
/* publish attitude setpoint
* Do not publish if offboard is enabled but position/velocity control is disabled,
* in this case the attitude setpoint is published by the mavlink app
* in this case the attitude setpoint is published by the mavlink app. Also do not publish
* if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
* attitude setpoints for the transition).
*/
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
} else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
}

8
src/modules/uORB/objects_common.cpp
View File

@ -164,8 +164,12 @@ ORB_DEFINE(fence_vertex, struct fence_vertex_s);
#include "topics/vehicle_attitude_setpoint.h"
ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
#include "topics/mc_virtual_attitude_setpoint.h"
ORB_DEFINE(mc_virtual_attitude_setpoint, struct mc_virtual_attitude_setpoint_s);
#include "topics/fw_virtual_attitude_setpoint.h"
ORB_DEFINE(fw_virtual_attitude_setpoint, struct fw_virtual_attitude_setpoint_s);
#include "topics/manual_control_setpoint.h"
ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);

1
src/modules/uORB/uORB.h
View File

@ -366,7 +366,6 @@ __END_DECLS
/* Diverse uORB header defines */ //XXX: move to better location
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s;
typedef uint8_t arming_state_t;
typedef uint8_t main_state_t;
typedef uint8_t hil_state_t;

9
src/modules/vtol_att_control/standard.cpp
View File

@ -186,6 +186,9 @@ void Standard::update_vtol_state()
void Standard::update_transition_state()
{
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
if (_params_standard.front_trans_dur <= 0.0f) {
// just set the final target throttle value
@ -242,11 +245,15 @@ void Standard::update_transition_state()
void Standard::update_mc_state()
{
// do nothing
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
}
void Standard::update_fw_state()
{
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
// 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);

335
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -35,18 +35,42 @@
* @file tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*
*/
#include "tailsitter.h"
#include "vtol_att_control_main.h"
Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) :
VtolType(att_controller),
_airspeed_tot(0),
#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition
#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition
#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2
#define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW
#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC
Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
VtolType(attc),
_airspeed_tot(0.0f),
_min_front_trans_dur(0.5f),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_flag_was_in_trans_mode = false;
_params_handles_tailsitter.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
_params_handles_tailsitter.back_trans_dur = param_find("VT_B_TRANS_DUR");
_params_handles_tailsitter.airspeed_trans = param_find("VT_ARSP_TRANS");
_params_handles_tailsitter.airspeed_blend_start = param_find("VT_ARSP_BLEND");
_params_handles_tailsitter.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
}
@ -55,23 +79,170 @@ Tailsitter::~Tailsitter()
}
int
Tailsitter::parameters_update()
{
float v;
int l;
/* vtol duration of a front transition */
param_get(_params_handles_tailsitter.front_trans_dur, &v);
_params_tailsitter.front_trans_dur = math::constrain(v, 1.0f, 5.0f);
/* vtol front transition phase 2 duration */
param_get(_params_handles_tailsitter.front_trans_dur_p2, &v);
_params_tailsitter.front_trans_dur_p2 = v;
/* vtol duration of a back transition */
param_get(_params_handles_tailsitter.back_trans_dur, &v);
_params_tailsitter.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
/* vtol airspeed at which it is ok to switch to fw mode */
param_get(_params_handles_tailsitter.airspeed_trans, &v);
_params_tailsitter.airspeed_trans = v;
/* vtol airspeed at which we start blending mc/fw controls */
param_get(_params_handles_tailsitter.airspeed_blend_start, &v);
_params_tailsitter.airspeed_blend_start = v;
/* vtol lock elevons in multicopter */
param_get(_params_handles_tailsitter.elevons_mc_lock, &l);
_params_tailsitter.elevons_mc_lock = l;
/* avoid parameters which will lead to zero division in the transition code */
_params_tailsitter.front_trans_dur = math::max(_params_tailsitter.front_trans_dur, _min_front_trans_dur);
if (_params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f) {
_params_tailsitter.airspeed_trans = _params_tailsitter.airspeed_blend_start + 1.0f;
}
return OK;
}
void Tailsitter::update_vtol_state()
{
// simply switch between the two modes
if (!_attc->is_fixed_wing_requested()) {
_vtol_mode = ROTARY_WING;
parameters_update();
} else {
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting in MC control mode, picking up
* forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode.
* For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle.
*/
if (_manual_control_sp->aux1 < 0.0f) {
switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_FRONT_P2:
// NOT USED
// failsafe into multicopter mode
//_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
// check if we have reached pitch angle to switch to MC mode
if (_v_att->pitch >= PITCH_TRANSITION_BACK) {
_vtol_schedule.flight_mode = MC_MODE;
}
break;
}
} else { // user switchig to FW mode
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1:
// check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode
if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans
&& _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) {
_vtol_schedule.flight_mode = FW_MODE;
//_vtol_schedule.transition_start = hrt_absolute_time();
}
break;
case TRANSITION_FRONT_P2:
case TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = FW_MODE;
/* **LATER*** if pitch is closer to mc (-45>)
* go to transition P1
*/
break;
}
}
// map tailsitter specific control phases to simple control modes
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case TRANSITION_FRONT_P1:
_vtol_mode = TRANSITION;
_vtol_vehicle_status->vtol_in_trans_mode = true;
case TRANSITION_FRONT_P2:
_vtol_mode = TRANSITION;
_vtol_vehicle_status->vtol_in_trans_mode = true;
case TRANSITION_BACK:
_vtol_mode = TRANSITION;
_vtol_vehicle_status->vtol_in_trans_mode = true;
break;
}
}
void Tailsitter::update_mc_state()
{
// set idle speed for rotary wing mode
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
}
void Tailsitter::update_fw_state()
@ -80,13 +251,137 @@ void Tailsitter::update_fw_state()
set_idle_fw();
flag_idle_mc = false;
}
_mc_roll_weight = 0.0f;
_mc_pitch_weight = 0.0f;
_mc_yaw_weight = 0.0f;
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
}
void Tailsitter::update_transition_state()
{
if (!_flag_was_in_trans_mode) {
// save desired heading for transition and last thrust value
_yaw_transition = _v_att_sp->yaw_body;
_pitch_transition_start = _v_att_sp->pitch_body;
_throttle_transition = _v_att_sp->thrust;
_flag_was_in_trans_mode = true;
}
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/
_v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));
_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1 - 0.2f ,
_pitch_transition_start);
/** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */
if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) {
_v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));
_v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition ,
(1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition);
}
// disable mc yaw control once the plane has picked up speed
if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
} else {
_mc_yaw_weight = 1.0f;
}
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down
/** no motor switching */
if (flag_idle_mc) {
set_idle_fw();
flag_idle_mc = false;
}
/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/
if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {
_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 -
(fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(
&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {
_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f;
}
}
/** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/
//_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
//_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
_mc_roll_weight = 0.0f;
_mc_pitch_weight = 0.0f;
/** keep yaw disabled */
_mc_yaw_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
/** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/
_v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.back_trans_dur * 1000000.0f);
_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK + 0.2f);
// throttle value is decreesed
_v_att_sp->thrust = _throttle_transition * 0.9f;
/** keep yaw disabled */
_mc_yaw_weight = 0.0f;
/** smoothly move control weight to MC */
_mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
(_params_tailsitter.back_trans_dur * 1000000.0f);
_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
(_params_tailsitter.back_trans_dur * 1000000.0f);
}
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
// compute desired attitude and thrust setpoint for the transition
_v_att_sp->timestamp = hrt_absolute_time();
_v_att_sp->roll_body = 0.0f;
_v_att_sp->yaw_body = _yaw_transition;
_v_att_sp->R_valid = true;
math::Matrix<3, 3> R_sp;
R_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body));
math::Quaternion q_sp;
q_sp.from_dcm(R_sp);
memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
}
void Tailsitter::update_external_state()
{
@ -110,8 +405,7 @@ void Tailsitter::calc_tot_airspeed()
_airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
}
void
Tailsitter::scale_mc_output()
void Tailsitter::scale_mc_output()
{
// scale around tuning airspeed
float airspeed;
@ -167,8 +461,7 @@ void Tailsitter::fill_actuator_outputs()
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
_actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
}
break;
@ -192,6 +485,26 @@ void Tailsitter::fill_actuator_outputs()
break;
case TRANSITION:
// in transition engines are mixed by weight (BACK TRANSITION ONLY)
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
* _mc_roll_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
_mc_yaw_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
* (1 - _mc_roll_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
// **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
break;
case EXTERNAL:
// not yet implemented, we are switching brute force at the moment
break;

88
src/modules/vtol_att_control/tailsitter.h
View File

@ -35,6 +35,7 @@
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*
*/
@ -42,7 +43,9 @@
#define TAILSITTER_H
#include "vtol_type.h"
#include <systemlib/perf_counter.h>
#include <systemlib/perf_counter.h> /** is it necsacery? **/
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
class Tailsitter : public VtolType
{
@ -51,21 +54,96 @@ public:
Tailsitter(VtolAttitudeControl *_att_controller);
~Tailsitter();
/**
* Update vtol state.
*/
void update_vtol_state();
/**
* Update multicopter state.
*/
void update_mc_state();
/**
* Update fixed wing state.
*/
void update_fw_state();
/**
* Update transition state.
*/
void update_transition_state();
/**
* Update external state.
*/
void update_external_state();
private:
void fill_actuator_outputs();
void calc_tot_airspeed();
void scale_mc_output();
float _airspeed_tot;
struct {
float front_trans_dur; /**< duration of first part of front transition */
float front_trans_dur_p2;
float back_trans_dur; /**< duration of back transition */
float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */
float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */
int elevons_mc_lock; /**< lock elevons in multicopter mode */
} _params_tailsitter;
struct {
param_t front_trans_dur;
param_t front_trans_dur_p2;
param_t back_trans_dur;
param_t airspeed_trans;
param_t airspeed_blend_start;
param_t elevons_mc_lock;
} _params_handles_tailsitter;
enum vtol_mode {
MC_MODE = 0, /**< vtol is in multicopter mode */
TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */
TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */
TRANSITION_BACK, /**< vtol is in back transition mode */
FW_MODE /**< vtol is in fixed wing mode */
};
struct {
vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */
hrt_abstime transition_start; /**< absoulte time at which front transition started */
} _vtol_schedule;
float _airspeed_tot; /** speed estimation for propwash controlled surfaces */
/** not sure about it yet ?! **/
float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */
/** should this anouncement stay? **/
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
/**
* Speed estimation for propwash controlled surfaces.
*/
void calc_tot_airspeed();
/** is this one still needed? */
void scale_mc_output();
/**
* Write control values to actuator output topics.
*/
void fill_actuator_outputs();
/**
* Update parameters.
*/
int parameters_update();
};
#endif

56
src/modules/vtol_att_control/tailsitter_params.c Normal file
View File

@ -0,0 +1,56 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file tailsitter_params.c
* Parameters for vtol attitude controller.
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*/
#include <systemlib/param/param.h>
/**
* Duration of front transition phase 2
*
* Time in seconds it should take for the rotors to rotate forward completely from the point
* when the plane has picked up enough airspeed and is ready to go into fixed wind mode.
*
* @min 0.1
* @max 2
* @group VTOL Attitude Control
PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/

18
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -56,6 +56,8 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_flag_was_in_trans_mode = false;
_params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR");
_params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC");
@ -250,6 +252,9 @@ void Tiltrotor::update_vtol_state()
void Tiltrotor::update_mc_state()
{
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
// make sure motors are not tilted
_tilt_control = _params_tiltrotor.tilt_mc;
@ -271,6 +276,9 @@ void Tiltrotor::update_mc_state()
void Tiltrotor::update_fw_state()
{
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
// make sure motors are tilted forward
_tilt_control = _params_tiltrotor.tilt_fw;
@ -292,6 +300,13 @@ void Tiltrotor::update_fw_state()
void Tiltrotor::update_transition_state()
{
if (!_flag_was_in_trans_mode) {
// save desired heading for transition and last thrust value
_yaw_transition = _v_att->yaw;
_throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
_flag_was_in_trans_mode = true;
}
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
// for the first part of the transition the rear rotors are enabled
if (_rear_motors != ENABLED) {
@ -354,6 +369,9 @@ void Tiltrotor::update_transition_state()
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
}
void Tiltrotor::update_external_state()

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

@ -41,6 +41,7 @@
* @author Roman Bapst <bapstr@ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*
*/
#include "vtol_att_control_main.h"
@ -60,6 +61,8 @@ VtolAttitudeControl::VtolAttitudeControl() :
//init subscription handlers
_v_att_sub(-1),
_v_att_sp_sub(-1),
_mc_virtual_att_sp_sub(-1),
_fw_virtual_att_sp_sub(-1),
_mc_virtual_v_rates_sp_sub(-1),
_fw_virtual_v_rates_sp_sub(-1),
_v_control_mode_sub(-1),
@ -75,13 +78,16 @@ VtolAttitudeControl::VtolAttitudeControl() :
_actuators_0_pub(nullptr),
_actuators_1_pub(nullptr),
_vtol_vehicle_status_pub(nullptr),
_v_rates_sp_pub(nullptr)
_v_rates_sp_pub(nullptr),
_v_att_sp_pub(nullptr)
{
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));
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_mc_virtual_att_sp, 0, sizeof(_mc_virtual_att_sp));
memset(&_fw_virtual_att_sp, 0, sizeof(_fw_virtual_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp));
memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp));
@ -271,6 +277,36 @@ VtolAttitudeControl::vehicle_airspeed_poll()
}
}
/**
* Check for attitude set points update.
*/
void
VtolAttitudeControl::vehicle_attitude_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
}
}
/**
* Check for attitude update.
*/
void
VtolAttitudeControl::vehicle_attitude_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
}
}
/**
* Check for battery updates.
*/
@ -319,6 +355,38 @@ VtolAttitudeControl::vehicle_local_pos_poll()
}
/**
* Check for mc virtual attitude setpoint updates.
*/
void
VtolAttitudeControl::mc_virtual_att_sp_poll()
{
bool updated;
orb_check(_mc_virtual_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub , &_mc_virtual_att_sp);
}
}
/**
* Check for fw virtual attitude setpoint updates.
*/
void
VtolAttitudeControl::fw_virtual_att_sp_poll()
{
bool updated;
orb_check(_fw_virtual_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub , &_fw_virtual_att_sp);
}
}
/**
* Check for command updates.
*/
@ -439,6 +507,18 @@ void VtolAttitudeControl::fill_fw_att_rates_sp()
_v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust;
}
void VtolAttitudeControl::publish_att_sp()
{
if (_v_att_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp);
} else {
/* advertise and publish */
_v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
}
}
void
VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
@ -452,9 +532,12 @@ void VtolAttitudeControl::task_main()
/* do subscriptions */
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint));
_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@ -523,9 +606,13 @@ void VtolAttitudeControl::task_main()
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
mc_virtual_att_sp_poll();
fw_virtual_att_sp_poll();
vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
vehicle_manual_poll(); //Check for changes in manual inputs.
arming_status_poll(); //Check for arming status updates.
vehicle_attitude_setpoint_poll();//Check for changes in attitude set points
vehicle_attitude_poll(); //Check for changes in attitude
actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
vehicle_rates_sp_mc_poll();
@ -582,6 +669,7 @@ void VtolAttitudeControl::task_main()
} else if (_vtol_type->get_mode() == TRANSITION) {
// vehicle is doing a transition
_vtol_vehicle_status.vtol_in_trans_mode = true;
_vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition
bool got_new_data = false;
@ -599,6 +687,7 @@ void VtolAttitudeControl::task_main()
if (got_new_data) {
_vtol_type->update_transition_state();
fill_mc_att_rates_sp();
publish_att_sp();
}
} else if (_vtol_type->get_mode() == EXTERNAL) {
@ -606,6 +695,7 @@ void VtolAttitudeControl::task_main()
_vtol_type->update_external_state();
}
publish_att_sp();
_vtol_type->fill_actuator_outputs();
/* Only publish if the proper mode(s) are enabled */

50
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -41,6 +41,7 @@
* @author Roman Bapst <bapstr@ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*
*/
#ifndef VTOL_ATT_CONTROL_MAIN_H
@ -62,6 +63,8 @@
#include <drivers/drv_pwm_output.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/mc_virtual_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_mc.h>
@ -104,22 +107,24 @@ public:
int start(); /* start the task and return OK on success */
bool is_fixed_wing_requested();
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 vehicle_attitude_s *get_att() {return &_v_att;}
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;}
struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_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;}
@ -132,6 +137,8 @@ private:
/* handlers for subscriptions */
int _v_att_sub; //vehicle attitude subscription
int _v_att_sp_sub; //vehicle attitude setpoint subscription
int _mc_virtual_att_sp_sub;
int _fw_virtual_att_sp_sub;
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
@ -151,9 +158,12 @@ private:
orb_advert_t _actuators_1_pub;
orb_advert_t _vtol_vehicle_status_pub;
orb_advert_t _v_rates_sp_pub;
orb_advert_t _v_att_sp_pub;
//*******************data containers***********************************************************
struct vehicle_attitude_s _v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint
struct mc_virtual_attitude_setpoint_s _mc_virtual_att_sp; // virtual mc attitude setpoint
struct fw_virtual_attitude_setpoint_s _fw_virtual_att_sp; // virtual fw 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
@ -187,6 +197,9 @@ private:
param_t elevons_mc_lock;
} _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. */
int _transition_command;
VtolType *_vtol_type; // base class for different vtol types
@ -202,12 +215,16 @@ private:
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 mc_virtual_att_sp_poll();
void fw_virtual_att_sp_poll();
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_attitude_setpoint_poll(); //Check for attitude setpoint updates.
void vehicle_attitude_poll(); //Check for attitude updates.
void vehicle_battery_poll(); // Check for battery updates
void vehicle_cmd_poll();
void parameters_update_poll(); //Check if parameters have changed
@ -215,6 +232,7 @@ private:
void fill_mc_att_rates_sp();
void fill_fw_att_rates_sp();
void handle_command();
void publish_att_sp();
};
#endif

2
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -49,6 +49,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
{
_v_att = _attc->get_att();
_v_att_sp = _attc->get_att_sp();
_mc_virtual_att_sp = _attc->get_mc_virtual_att_sp();
_fw_virtual_att_sp = _attc->get_fw_virtual_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();

21
src/modules/vtol_att_control/vtol_type.h
View File

@ -93,20 +93,22 @@ protected:
struct vehicle_attitude_s *_v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint
struct mc_virtual_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint
struct fw_virtual_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw 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 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 airspeed_s *_airspeed; // airspeed
struct battery_status_s *_batt_status; // battery status
struct Params *_params;
@ -116,6 +118,11 @@ protected:
float _mc_pitch_weight; // weight for multicopter attitude controller pitch output
float _mc_yaw_weight; // weight for multicopter attitude controller yaw output
float _yaw_transition; // yaw angle in which transition will take place
float _pitch_transition_start; // pitch angle at the start of transition (tailsitter)
float _throttle_transition; // throttle value used for the transition phase
bool _flag_was_in_trans_mode; // true if mode has just switched to transition
};
#endif