Lorenz Meier
10 years ago
13 changed files with 1390 additions and 367 deletions
@ -0,0 +1,184 @@
@@ -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
|
||||
} |
||||
@ -0,0 +1,74 @@
@@ -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 |
||||
@ -0,0 +1,357 @@
@@ -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); |
||||
} |
||||
@ -0,0 +1,110 @@
@@ -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 |
||||
@ -0,0 +1,118 @@
@@ -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); |
||||
@ -0,0 +1,212 @@
@@ -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 |
||||
@ -0,0 +1,133 @@
@@ -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); |
||||
} |
||||
@ -0,0 +1,115 @@
@@ -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 |
||||
Loading…
Reference in new issue