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);