From 1ebea1e7590d0d66b18cd39fcc45897bca8b8256 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 11:05:44 +0200 Subject: [PATCH 01/19] ask for climbout mode when doin takeoff help --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 01d94a8881..8e516a708f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -395,7 +395,7 @@ private: /** * Do takeoff help when in altitude controlled modes */ - void do_takeoff_help(); + bool do_takeoff_help(); /** * Update desired altitude base on user pitch stick input @@ -987,16 +987,20 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -void FixedwingPositionControl::do_takeoff_help() +bool FixedwingPositionControl::do_takeoff_help() { const hrt_abstime delta_takeoff = 10000000; - const float throttle_threshold = 0.3f; - const float delta_alt_takeoff = 30.0f; + const float throttle_threshold = 0.1f; + const float delta_alt_takeoff = 50.0f; + float climbout = false; /* demand 30 m above ground if user switched into this mode during takeoff */ if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { _hold_alt = _ground_alt + delta_alt_takeoff; + climbout = true; + } + return climbout; } bool @@ -1416,7 +1420,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1509,7 +1513,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; From c91bb76b42d8d85339902c0b29123243156a0f0b Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 11:05:44 +0200 Subject: [PATCH 02/19] ask for climbout mode when doin takeoff help --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 01d94a8881..8e516a708f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -395,7 +395,7 @@ private: /** * Do takeoff help when in altitude controlled modes */ - void do_takeoff_help(); + bool do_takeoff_help(); /** * Update desired altitude base on user pitch stick input @@ -987,16 +987,20 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -void FixedwingPositionControl::do_takeoff_help() +bool FixedwingPositionControl::do_takeoff_help() { const hrt_abstime delta_takeoff = 10000000; - const float throttle_threshold = 0.3f; - const float delta_alt_takeoff = 30.0f; + const float throttle_threshold = 0.1f; + const float delta_alt_takeoff = 50.0f; + float climbout = false; /* demand 30 m above ground if user switched into this mode during takeoff */ if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { _hold_alt = _ground_alt + delta_alt_takeoff; + climbout = true; + } + return climbout; } bool @@ -1416,7 +1420,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1509,7 +1513,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; From 5c59d7a434791222a6fad49e64206432a86c22bf Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 23:05:25 +0200 Subject: [PATCH 03/19] do not run tecs if we are on ground to prevent integrator filling --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 8e516a708f..05988ef53a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1024,7 +1024,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - if (!_mTecs.getEnabled()) { + if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) { _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); } @@ -1757,6 +1757,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, unsigned mode, bool pitch_max_special) { + /* do not run tecs if we are not in air */ + if (_vehicle_status.condition_landed) { + return; + } + if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ float flightPathAngle = 0.0f; From 6ce106eea465b9bbaf59f4d44a2954bd107d1035 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 17 Jun 2015 17:36:26 +0200 Subject: [PATCH 04/19] limit minimum pitch in altitude controller modes if in a takeoff situation --- .../fw_pos_control_l1_main.cpp | 63 +++++++++++++------ 1 file changed, 45 insertions(+), 18 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 05988ef53a..c262e80c5f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -389,13 +389,16 @@ private: float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); /** - * Control position. + * Check if we are in a takeoff situation */ + bool in_takeoff_situation(); /** * Do takeoff help when in altitude controlled modes + * @param hold_altitude altitude setpoint for controller + * @param pitch_limit_min minimum pitch allowed */ - bool do_takeoff_help(); + void do_takeoff_help(float *hold_altitude, float *pitch_limit_min); /** * Update desired altitude base on user pitch stick input @@ -405,6 +408,9 @@ private: */ bool update_desired_altitude(float dt); + /** + * Control position. + */ bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); @@ -987,20 +993,26 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -bool FixedwingPositionControl::do_takeoff_help() -{ +bool FixedwingPositionControl::in_takeoff_situation() { const hrt_abstime delta_takeoff = 10000000; const float throttle_threshold = 0.1f; - const float delta_alt_takeoff = 50.0f; - float climbout = false; - - /* demand 30 m above ground if user switched into this mode during takeoff */ - if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { - _hold_alt = _ground_alt + delta_alt_takeoff; - climbout = true; + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + _parameters.climbout_diff) { + return true; + } + + return false; +} + +void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_limit_min) +{ + /* demand "climbout_diff" m above ground if user switched into this mode during takeoff */ + if (in_takeoff_situation()) { + *hold_altitude = _ground_alt + _parameters.climbout_diff; + *pitch_limit_min = math::radians(10.0f); + } else { + *pitch_limit_min = _parameters.pitch_limit_min; } - return climbout; } bool @@ -1419,8 +1431,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - climbout_requested |= do_takeoff_help(); + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + * and set limit to pitch angle to prevent stearing into ground + */ + float pitch_limit_min; + do_takeoff_help(&_hold_alt, &pitch_limit_min); + /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1437,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - math::radians(_parameters.pitch_limit_min), + pitch_limit_min, _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); @@ -1452,6 +1468,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } + /* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration + to make sure the plane does not start rolling + */ + if (in_takeoff_situation()) { + _hdg_hold_enabled = false; + _yaw_lock_engaged = true; + } + if (_yaw_lock_engaged) { /* just switched back from non heading-hold to heading hold */ @@ -1512,8 +1536,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - climbout_requested |= do_takeoff_help(); + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + * and set limit to pitch angle to prevent stearing into ground + */ + float pitch_limit_min; + do_takeoff_help(&_hold_alt, &pitch_limit_min); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1530,7 +1557,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - math::radians(_parameters.pitch_limit_min), + pitch_limit_min, _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); From 0446efa9a4b8e9399f64d4c65303bab971342b3e Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 17 Jun 2015 17:46:37 +0200 Subject: [PATCH 05/19] limit roll angle in loiter and position control mode if we are in a takeoff situation --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c262e80c5f..e4682689af 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1134,6 +1134,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + if (in_takeoff_situation()) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + } + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1505,6 +1511,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + + if (in_takeoff_situation()) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + } } } else { _hdg_hold_enabled = false; From 2485e037943222b521b6cc98eb6bcf50c82a3fbe Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:54:58 +0200 Subject: [PATCH 06/19] corrected elevon mixer for firefly6 --- ROMFS/px4fmu_common/mixers/firefly6.aux.mix | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index fda8416403..22dc2a69ce 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -11,12 +11,12 @@ Elevon mixers ------------- M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 -8000 -8000 0 -10000 10000 Landing gear mixer From 1ccded0305f439b4f8e45f23ec55bf304cc7c1ab Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:55:30 +0200 Subject: [PATCH 07/19] added generic class for vtol types --- src/modules/vtol_att_control/vtol_type.cpp | 133 +++++++++++++++++++++ src/modules/vtol_att_control/vtol_type.h | 115 ++++++++++++++++++ 2 files changed, 248 insertions(+) create mode 100644 src/modules/vtol_att_control/vtol_type.cpp create mode 100644 src/modules/vtol_att_control/vtol_type.h diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp new file mode 100644 index 0000000000..7e8d3217f1 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -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 + * + */ + +#include "vtol_type.h" +#include "drivers/drv_pwm_output.h" +#include +#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); +} \ No newline at end of file diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h new file mode 100644 index 0000000000..57448a7587 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_type.h @@ -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 + * + */ + +#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 From a212e457448ed41c8a33c39696a8963ce631f63d Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:56:11 +0200 Subject: [PATCH 08/19] added tiltrotor attitude control class --- src/modules/vtol_att_control/tiltrotor.cpp | 357 ++++++++++++++++++ src/modules/vtol_att_control/tiltrotor.h | 110 ++++++ .../vtol_att_control/tiltrotor_params.c | 118 ++++++ 3 files changed, 585 insertions(+) create mode 100644 src/modules/vtol_att_control/tiltrotor.cpp create mode 100644 src/modules/vtol_att_control/tiltrotor.h create mode 100644 src/modules/vtol_att_control/tiltrotor_params.c diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp new file mode 100644 index 0000000000..2cf074fe42 --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -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 + * +*/ + +#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); +} diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h new file mode 100644 index 0000000000..3ef1362d00 --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -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 + * + */ + +#ifndef TILTROTOR_H +#define TILTROTOR_H +#include "vtol_type.h" +#include +#include + +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 diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c new file mode 100644 index 0000000000..76f3ee6c3c --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -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 + */ + +#include + +/** + * 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); From 77077cb92ab11b78b4636072c7e1ae9c01c5e0e8 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:57:10 +0200 Subject: [PATCH 09/19] added tailsitter attitude control class --- src/modules/vtol_att_control/tailsitter.cpp | 184 ++++++++++++++++++++ src/modules/vtol_att_control/tailsitter.h | 74 ++++++++ 2 files changed, 258 insertions(+) create mode 100644 src/modules/vtol_att_control/tailsitter.cpp create mode 100644 src/modules/vtol_att_control/tailsitter.h diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp new file mode 100644 index 0000000000..4479783b92 --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -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 + * + */ + + #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 +} diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h new file mode 100644 index 0000000000..e681a9bf8b --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter.h @@ -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 + * + */ + +#ifndef TAILSITTER_H +#define TAILSITTER_H + +#include "vtol_type.h" +#include + +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 From 526698854cabd3af875259bf966a9004c65d71df Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:57:54 +0200 Subject: [PATCH 10/19] adapt vtol attitude control class to new vtol type classes --- .../vtol_att_control_main.cpp | 437 +++--------------- .../vtol_att_control/vtol_att_control_main.h | 212 +++++++++ .../vtol_att_control_params.c | 8 + 3 files changed, 293 insertions(+), 364 deletions(-) create mode 100644 src/modules/vtol_att_control/vtol_att_control_main.h diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index fdb4de4343..a565c618e3 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -43,166 +43,7 @@ * @author Thomas Gubler * */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "drivers/drv_pwm_output.h" -#include - -#include - - -extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); - -class VtolAttitudeControl -{ -public: - - VtolAttitudeControl(); - ~VtolAttitudeControl(); - - int start(); /* start the task and return OK on success */ - - -private: -//******************flags & handlers****************************************************** - bool _task_should_exit; - int _control_task; //task handle for VTOL attitude controller - - /* handlers for subscriptions */ - int _v_att_sub; //vehicle attitude subscription - int _v_att_sp_sub; //vehicle attitude setpoint subscription - int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription - int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription - int _v_control_mode_sub; //vehicle control mode subscription - int _params_sub; //parameter updates subscription - int _manual_control_sp_sub; //manual control setpoint subscription - int _armed_sub; //arming status subscription - int _local_pos_sub; // sensor subscription - int _airspeed_sub; // airspeed subscription - int _battery_status_sub; // battery status subscription - - int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs - int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs - - //handlers for publishers - orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) - orb_advert_t _actuators_1_pub; - orb_advert_t _vtol_vehicle_status_pub; - orb_advert_t _v_rates_sp_pub; -//*******************data containers*********************************************************** - struct vehicle_attitude_s _v_att; //vehicle attitude - struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint - struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint - struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint - struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint - struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint - struct vehicle_control_mode_s _v_control_mode; //vehicle control mode - struct vtol_vehicle_status_s _vtol_vehicle_status; - struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer - struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control - struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control - struct actuator_armed_s _armed; //actuator arming status - struct vehicle_local_position_s _local_pos; - struct airspeed_s _airspeed; // airspeed - struct battery_status_s _batt_status; // battery status - - struct { - param_t idle_pwm_mc; //pwm value for idle in mc mode - param_t vtol_motor_count; - param_t vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode - float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) - float mc_airspeed_trim; // trim airspeed in multicopter mode - float mc_airspeed_max; // max airpseed in multicopter mode - float fw_pitch_trim; // trim for neutral elevon position in fw mode - float power_max; // maximum power of one engine - float prop_eff; // factor to calculate prop efficiency - float arsp_lp_gain; // total airspeed estimate low pass gain - } _params; - - struct { - param_t idle_pwm_mc; - param_t vtol_motor_count; - param_t vtol_fw_permanent_stab; - param_t mc_airspeed_min; - param_t mc_airspeed_trim; - param_t mc_airspeed_max; - param_t fw_pitch_trim; - param_t power_max; - param_t prop_eff; - param_t arsp_lp_gain; - } _params_handles; - - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ - - /* for multicopters it is usual to have a non-zero idle speed of the engines - * for fixed wings we want to have an idle speed of zero since we do not want - * to waste energy when gliding. */ - bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" - unsigned _motor_count; // number of motors - float _airspeed_tot; - float _tilt_control; -//*****************Member functions*********************************************************************** - - void task_main(); //main task - static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. - - void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - void vehicle_manual_poll(); //Check for changes in manual inputs. - void arming_status_poll(); //Check for arming status updates. - void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output - void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - void vehicle_rates_sp_mc_poll(); - void vehicle_rates_sp_fw_poll(); - void vehicle_local_pos_poll(); // Check for changes in sensor values - void vehicle_airspeed_poll(); // Check for changes in airspeed - void vehicle_battery_poll(); // Check for battery updates - void parameters_update_poll(); //Check if parameters have changed - int parameters_update(); //Update local paraemter cache - void fill_mc_att_control_output(); //write mc_att_control results to actuator message - void fill_fw_att_control_output(); //write fw_att_control results to actuator message - void fill_mc_att_rates_sp(); - void fill_fw_att_rates_sp(); - void set_idle_fw(); - void set_idle_mc(); - void scale_mc_output(); - void calc_tot_airspeed(); // estimated airspeed seen by elevons -}; +#include "vtol_att_control_main.h" namespace VTOL_att_control { @@ -230,19 +71,12 @@ VtolAttitudeControl::VtolAttitudeControl() : _battery_status_sub(-1), //init publication handlers - _actuators_0_pub(-1), - _actuators_1_pub(-1), - _vtol_vehicle_status_pub(-1), - _v_rates_sp_pub(-1), + _actuators_0_pub(0), + _actuators_1_pub(0), + _vtol_vehicle_status_pub(0), + _v_rates_sp_pub(0) - _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input")) { - - flag_idle_mc = true; - _airspeed_tot = 0.0f; - _tilt_control = 0.0f; - memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); @@ -276,9 +110,20 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.power_max = param_find("VT_POWER_MAX"); _params_handles.prop_eff = param_find("VT_PROP_EFF"); _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); + _params_handles.vtol_type = param_find("VT_TYPE"); /* fetch initial parameter values */ parameters_update(); + + if (_params.vtol_type == 0) { + _tailsitter = new Tailsitter(this); + _vtol_type = _tailsitter; + } else if (_params.vtol_type == 1) { + _tiltrotor = new Tiltrotor(this); + _vtol_type = _tiltrotor; + } else { + _task_should_exit = true; + } } /** @@ -470,6 +315,7 @@ int VtolAttitudeControl::parameters_update() { float v; + int l; /* idle pwm for mc mode */ param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc); @@ -507,42 +353,12 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.arsp_lp_gain, &v); _params.arsp_lp_gain = v; + param_get(_params_handles.vtol_type, &l); + _params.vtol_type = l; + return OK; } -/** -* Prepare message to acutators with data from mc attitude controller. -*/ -void VtolAttitudeControl::fill_mc_att_control_output() -{ - _actuators_out_0.control[0] = _actuators_mc_in.control[0]; - _actuators_out_0.control[1] = _actuators_mc_in.control[1]; - _actuators_out_0.control[2] = _actuators_mc_in.control[2]; - _actuators_out_0.control[3] = _actuators_mc_in.control[3]; - //set neutral position for elevons - _actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon - _actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon - _actuators_out_1.control[4] = _tilt_control; // for tilt-rotor control -} - -/** -* Prepare message to acutators with data from fw attitude controller. -*/ -void VtolAttitudeControl::fill_fw_att_control_output() -{ - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0.control[0] = 0; - _actuators_out_0.control[1] = 0; - _actuators_out_0.control[2] = 0; - _actuators_out_0.control[3] = _actuators_fw_in.control[3]; - /*controls for the elevons */ - _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; // roll elevon - _actuators_out_1.control[1] = _actuators_fw_in.control[1] + _params.fw_pitch_trim; // pitch elevon - // unused now but still logged - _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw - _actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle -} - /** * Prepare message for mc attitude rates setpoint topic */ @@ -565,109 +381,6 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; } -/** -* Adjust idle speed for fw mode. -*/ -void VtolAttitudeControl::set_idle_fw() -{ - int ret; - char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - unsigned pwm_value = PWM_LOWEST_MIN; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < _params.vtol_motor_count; i++) { - - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -/** -* Adjust idle speed for mc mode. -*/ -void VtolAttitudeControl::set_idle_mc() -{ - int ret; - unsigned servo_count; - char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - unsigned pwm_value = _params.idle_pwm_mc; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < _params.vtol_motor_count; i++) { - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -void -VtolAttitudeControl::scale_mc_output() { - // scale around tuning airspeed - float airspeed; - calc_tot_airspeed(); // estimate air velocity seen by elevons - // if airspeed is not updating, we assume the normal average speed - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { - airspeed = _params.mc_airspeed_trim; - if (nonfinite) { - perf_count(_nonfinite_input_perf); - } - } else { - airspeed = _airspeed_tot; - airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max); - } - - _vtol_vehicle_status.airspeed_tot = airspeed; // save value for logging - /* - * For scaling our actuators using anything less than the min (close to stall) - * speed doesn't make any sense - its the strongest reasonable deflection we - * want to do in flight and its the baseline a human pilot would choose. - * - * Forcing the scaling to this value allows reasonable handheld tests. - */ - float airspeed_scaling = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed); - _actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); -} - -void VtolAttitudeControl::calc_tot_airspeed() { - float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s); // prevent numerical drama - // calculate momentary power of one engine - float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count; - P = math::constrain(P,1.0f,_params.power_max); - // calculate prop efficiency - float power_factor = 1.0f - P*_params.prop_eff/_params.power_max; - float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f; - eta = math::constrain(eta,0.001f,1.0f); // live on the safe side - // calculate induced airspeed by propeller - float v_ind = (airspeed/eta - airspeed)*2.0f; - // calculate total airspeed - float airspeed_raw = airspeed + v_ind; - // apply low-pass filter - _airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; -} - void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -701,8 +414,7 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; // make sure we start with idle in mc mode - set_idle_mc(); - flag_idle_mc = true; + _vtol_type->set_idle_mc(); /* wakeup source*/ struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ @@ -764,83 +476,80 @@ void VtolAttitudeControl::task_main() vehicle_airspeed_poll(); vehicle_battery_poll(); + // update the vtol state machine which decides which mode we are in + _vtol_type->update_vtol_state(); - if (_manual_control_sp.aux1 < 0.0f) { /* vehicle is in mc mode */ + // check in which mode we are in and call mode specific functions + if (_vtol_type->get_mode() == ROTARY_WING) { + // vehicle is in rotary wing mode _vtol_vehicle_status.vtol_in_rw_mode = true; - if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */ - set_idle_mc(); - flag_idle_mc = true; - } + _vtol_type->update_mc_state(); - /* got data from mc_att_controller */ + // got data from mc attitude controller if (fds[0].revents & POLLIN) { - vehicle_manual_poll(); /* update remote input */ orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - // scale pitch control with total airspeed - scale_mc_output(); + _vtol_type->process_mc_data(); - fill_mc_att_control_output(); fill_mc_att_rates_sp(); - - /* Only publish if the proper mode(s) are enabled */ - if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled) - { - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } - } } - } - - if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */ + } else if (_vtol_type->get_mode() == FIXED_WING) { + // vehicle is in fw mode _vtol_vehicle_status.vtol_in_rw_mode = false; - if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */ - set_idle_fw(); - flag_idle_mc = false; + _vtol_type->update_fw_state(); + + // got data from fw attitude controller + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); + vehicle_manual_poll(); + + _vtol_type->process_fw_data(); + + fill_fw_att_rates_sp(); + } + } else if (_vtol_type->get_mode() == TRANSITION) { + // vehicle is doing a transition + bool got_new_data = false; + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); + got_new_data = true; } - if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */ + if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); - vehicle_manual_poll(); //update remote input + got_new_data = true; + } - fill_fw_att_control_output(); - fill_fw_att_rates_sp(); + // update transition state if got any new data + if (got_new_data) { + _vtol_type->update_transition_state(); + } - /* Only publish if the proper mode(s) are enabled */ - if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled || - _v_control_mode.flag_control_manual_enabled) - { - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + } else if (_vtol_type->get_mode() == EXTERNAL) { + // we are using external module to generate attitude/thrust setpoint + _vtol_type->update_external_state(); + } - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + /* Only publish if the proper mode(s) are enabled */ + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled || + _v_control_mode.flag_control_manual_enabled) + { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); } } - } // publish the attitude rates setpoint if(_v_rates_sp_pub > 0) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h new file mode 100644 index 0000000000..2772f9bcb1 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -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 + * @author Lorenz Meier + * @author Thomas Gubler + * + */ +#ifndef VTOL_ATT_CONTROL_MAIN_H +#define VTOL_ATT_CONTROL_MAIN_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 6da28b1304..429d44c46c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -142,3 +142,11 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); */ PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); +/** + * VTOL Type (Tailsitter=0, Tiltrotor=1) + * + * @min 0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_TYPE, 0); From 12feef85bfd9ae92eaa50c1efcc2d27d2c7bff72 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:58:47 +0200 Subject: [PATCH 11/19] lower lowest allowed max pwm value to be able to cut rear motors for firefly6 in fw mode --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 6271ad2086..2fb9469c8a 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -93,7 +93,7 @@ __BEGIN_DECLS /** * Lowest PWM allowed as the maximum PWM */ -#define PWM_LOWEST_MAX 1400 +#define PWM_LOWEST_MAX 950 /** * Do not output a channel with this value From b3c3d6634c31322df1e17666dc97e44bcbe47302 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 00:00:23 +0200 Subject: [PATCH 12/19] added vtol types --- src/modules/vtol_att_control/module.mk | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index 0cf3072c8f..ad6efd2b27 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -38,7 +38,10 @@ MODULE_COMMAND = vtol_att_control SRCS = vtol_att_control_main.cpp \ - vtol_att_control_params.c + vtol_att_control_params.c \ + tiltrotor_params.c \ + tiltrotor.cpp \ + vtol_type.cpp \ + tailsitter.cpp EXTRACXXFLAGS = -Wno-write-strings - From d320dc8ada0ca85722a1b57d5550fd5333d53980 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:45:41 +0200 Subject: [PATCH 13/19] added VTOL type param to VTOL configuration files --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 3 ++- ROMFS/px4fmu_common/init.d/13002_firefly6 | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 5c00041492..912202f962 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -1,7 +1,7 @@ # # Generic configuration file for caipirinha VTOL version # -# Roman Bapst +# Roman Bapst # sh /etc/init.d/rc.vtol_defaults @@ -13,3 +13,4 @@ set PWM_MAX 2000 set PWM_RATE 400 param set VT_MOT_COUNT 2 param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index ed90dabf41..963341d138 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -2,7 +2,7 @@ # # Generic configuration file for BirdsEyeView Aerobotics FireFly6 # -# Roman Bapst +# Roman Bapst # sh /etc/init.d/rc.vtol_defaults @@ -41,3 +41,4 @@ set MAV_TYPE 21 param set VT_MOT_COUNT 6 param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 1 From 4e9fd5b2a4be4ef4054781f1ed5e6f6896e2fca8 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:46:20 +0200 Subject: [PATCH 14/19] rotate attitude for fw mode only if VTOL is a tailsitter --- .../fw_att_control/fw_att_control_main.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index fe27de14f5..ccf12a0791 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -193,12 +193,14 @@ private: float trim_roll; float trim_pitch; float trim_yaw; - float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ - float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ - float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ - float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float man_roll_max; /**< Max Roll in rad */ - float man_pitch_max; /**< Max Pitch in rad */ + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ + + int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ } _parameters; /**< local copies of interesting parameters */ @@ -241,6 +243,8 @@ private: param_t man_roll_max; param_t man_pitch_max; + param_t vtol_type; + } _parameter_handles; /**< handles for interesting parameters */ @@ -404,6 +408,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); + /* fetch initial parameter values */ parameters_update(); } @@ -481,6 +487,8 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -703,10 +711,8 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - if (_vehicle_status.is_vtol) { - /* vehicle type is VTOL, need to modify attitude! - * The following modification to the attitude is vehicle specific and in this case applies - * to tail-sitter models !!! + if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { + /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * * Since the VTOL airframe is initialized as a multicopter we need to * modify the estimated attitude for the fixed wing operation. From 51b1968bddc4d976b047f5edfe165dbf4259de4f Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:51:02 +0200 Subject: [PATCH 15/19] added configuration file for tailsitter with motors in quad x configuration --- .../px4fmu_common/init.d/13003_quad_tailsitter | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13003_quad_tailsitter diff --git a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter new file mode 100644 index 0000000000..daad3d9351 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter @@ -0,0 +1,17 @@ +# +# Generic configuration file for a tailsitter with motors in X configuration. +# +# Roman Bapst +# + +sh /etc/init.d/rc.vtol_defaults + +set MIXER quad_x_vtol + +set PWM_OUT 1234 +set PWM_MAX 2000 +set PWM_RATE 400 +set MAV_TYPE 20 +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 0 From fb70b0a2b59bfbf517ddb6bd0a5f989bfed16666 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:51:48 +0200 Subject: [PATCH 16/19] added mixer file for tailsitter with motors in quad x configuration and 2 elevons --- .../px4fmu_common/mixers/quad_x_vtol.main.mix | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix diff --git a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix new file mode 100644 index 0000000000..4fd323353a --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix @@ -0,0 +1,18 @@ +Mixer for Tailsitter with x motor configuration and elevons +=========================================================== + +This file defines a single mixer for tailsitter with motors in X configuration. All controls +are mixed 100%. + +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 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 1 -10000 -10000 0 -10000 10000 From aade901ef02f604c468359d8c9e937c717035f9c Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 09:02:55 +0200 Subject: [PATCH 17/19] added new tailsitter type to autostart list --- ROMFS/px4fmu_common/init.d/rc.autostart | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index de81795b44..a45ceeb276 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -268,6 +268,14 @@ then sh /etc/init.d/13002_firefly6 fi +# +# Tailsitter with 4 motors in x config +# +if param compare SYS_AUTOSTART 13003 +then + sh /etc/init.d/13003_quad_tailsitter +fi + # # TriCopter Y Yaw+ # From 60857c7940d13af7d0168e7950a9799d30c3bb68 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 09:46:40 +0200 Subject: [PATCH 18/19] add option to lock elevons for tailsitters in mc mode --- src/modules/vtol_att_control/tailsitter.cpp | 11 ++++++++--- src/modules/vtol_att_control/tiltrotor_params.c | 11 ----------- .../vtol_att_control/vtol_att_control_main.cpp | 5 +++++ src/modules/vtol_att_control/vtol_att_control_main.h | 1 + .../vtol_att_control/vtol_att_control_params.c | 11 +++++++++++ src/modules/vtol_att_control/vtol_type.h | 1 + 6 files changed, 26 insertions(+), 14 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 4479783b92..358f636561 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -178,7 +178,12 @@ void Tailsitter::fill_mc_att_control_output() _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 + + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; + } else { + _actuators_out_1->control[0] = _actuators_mc_in->control[2]; //roll elevon + _actuators_out_1->control[1] = _actuators_mc_in->control[1]; //pitch elevon + } } diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 76f3ee6c3c..7d233f6f50 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -105,14 +105,3 @@ PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f); * @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); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index a565c618e3..b70cd19dd8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -111,6 +111,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.prop_eff = param_find("VT_PROP_EFF"); _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); _params_handles.vtol_type = param_find("VT_TYPE"); + _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); /* fetch initial parameter values */ parameters_update(); @@ -356,6 +357,10 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.vtol_type, &l); _params.vtol_type = l; + /* vtol lock elevons in multicopter */ + param_get(_params_handles.elevons_mc_lock, &l); + _params.elevons_mc_lock = l; + return OK; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 2772f9bcb1..43e8969929 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -176,6 +176,7 @@ private: param_t prop_eff; param_t arsp_lp_gain; param_t vtol_type; + param_t elevons_mc_lock; } _params_handles; /* for multicopters it is usual to have a non-zero idle speed of the engines diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 429d44c46c..f302314a23 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -150,3 +150,14 @@ PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_TYPE, 0); + +/** + * 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); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 57448a7587..bbe6a8642e 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -53,6 +53,7 @@ struct Params { float prop_eff; // factor to calculate prop efficiency float arsp_lp_gain; // total airspeed estimate low pass gain int vtol_type; + int elevons_mc_lock; // lock elevons in multicopter mode }; enum mode { From fc7dc297fc168465456b868cfd5cef26174f76d6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 09:48:07 +0200 Subject: [PATCH 19/19] lock elevons in mc mode for tailsitter with motors in x config --- ROMFS/px4fmu_common/init.d/13003_quad_tailsitter | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter index daad3d9351..17560526a3 100644 --- a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter +++ b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter @@ -15,3 +15,4 @@ set MAV_TYPE 20 param set VT_MOT_COUNT 4 param set VT_IDLE_PWM_MC 1080 param set VT_TYPE 0 +param set VT_ELEV_MC_LOCK 1