From 7954540f45b662ade0ad4038cf936934d3d1eef7 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 17:52:43 +0100 Subject: removed files which should not be in here --- ROMFS/px4fmu_common/init.d/13000_quadshot | 14 - .../px4fmu_common/init.d/Roman_mavlink_stream_conf | 12 - ROMFS/px4fmu_common/init.d/rc.vtol_apps | 15 - ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 63 -- ROMFS/px4fmu_common/mixers/FMU_quadshot.mix | 15 - src/modules/vtol_att_control/module.mk | 40 -- .../vtol_att_control/vtol_att_control_main.cpp | 642 --------------------- 7 files changed, 801 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/13000_quadshot delete mode 100644 ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf delete mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_apps delete mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_defaults delete mode 100644 ROMFS/px4fmu_common/mixers/FMU_quadshot.mix delete mode 100644 src/modules/vtol_att_control/module.mk delete mode 100644 src/modules/vtol_att_control/vtol_att_control_main.cpp diff --git a/ROMFS/px4fmu_common/init.d/13000_quadshot b/ROMFS/px4fmu_common/init.d/13000_quadshot deleted file mode 100644 index 8ee306a38..000000000 --- a/ROMFS/px4fmu_common/init.d/13000_quadshot +++ /dev/null @@ -1,14 +0,0 @@ -#!nsh -# -# Generic quadshot configuration file -# -# Roman Bapst -# - -sh /etc/init.d/rc.mc_defaults - -set MIXER FMU_quadshot - -set PWM_OUTPUTS 1234 -set PWM_MIN 1070 -set PWM_MAX 2000 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf b/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf deleted file mode 100644 index d26e4a372..000000000 --- a/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf +++ /dev/null @@ -1,12 +0,0 @@ -#!nsh -# Configure stream for Mavlink instance on TELEM2 because it is annoying always removing the SDcard -# -#usleep 100000 -#mavlink stream -d /dev/ttyS2 -s ATTITUDE_CONTROLS -r 50 -#usleep 100000 -#mavlink stream -d /dev/ttyS2 -s RC_CHANNELS_RAW -r 50 -#usleep 100000 -#mavlink stream -d /dev/ttyS2 -s VFR_HUD -r 50 -usleep 100000 -mavlink stream -d /dev/ttyS2 -s MANUAL_CONTROL -r 50 -echo "Added additional streams on TELEM2" \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps deleted file mode 100644 index 23ade6d78..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ /dev/null @@ -1,15 +0,0 @@ -#!nsh -# -# Standard apps for vtol: -# att & pos estimator, att & pos control. -# - -attitude_estimator_ekf start -#ekf_att_pos_estimator start -position_estimator_inav start - -vtol_att_control start -mc_att_control start -mc_pos_control start -fw_att_control start -fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults deleted file mode 100644 index f0ea9add0..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ /dev/null @@ -1,63 +0,0 @@ -#!nsh - -set VEHICLE_TYPE vtol - -if [ $DO_AUTOCONFIG == yes ] -then - #Default parameters for MC - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.003 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.003 - param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAW_FF 0.5 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILTMAX_AIR 45.0 - param set MPC_TILTMAX_LND 15.0 - param set MPC_LAND_SPEED 1.0 - - param set PE_VELNE_NOISE 0.5 - param set PE_VELD_NOISE 0.7 - param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 1.0 - - param set NAV_ACCEPT_RAD 2.0 - - # - # Default parameters for FW - # - param set NAV_LAND_ALT 90 - param set NAV_RTL_ALT 100 - param set NAV_RTL_LAND_T -1 - param set NAV_ACCEPT_RAD 50 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 -fi - -set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1075 -set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix b/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix deleted file mode 100644 index b077ff30a..000000000 --- a/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix +++ /dev/null @@ -1,15 +0,0 @@ -#!nsh -#Quadshot mixer for PX4FMU -#=========================== -R: 4v 10000 10000 10000 0 - -#mixer for the elevons -M: 2 -O: 10000 10000 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 1 -8000 -8000 0 -10000 10000 diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk deleted file mode 100644 index c349c2340..000000000 --- a/src/modules/vtol_att_control/module.mk +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# VTOL attitude controller -# - -MODULE_COMMAND = vtol_att_control - -SRCS = vtol_att_control_main.cpp diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp deleted file mode 100644 index 38fa4eec1..000000000 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ /dev/null @@ -1,642 +0,0 @@ -/**************************************************************************** - * - * 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 - * - */ - -#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 - -#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 _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 _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; -//*******************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 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 { - float min_pwm_mc; //pwm value for idle in mc mode - } _params; - - struct { - param_t min_pwm_mc; - } _params_handles; - - perf_counter_t _loop_perf; /**< loop performance counter */ - - /* 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" - -//*****************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 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 set_idle_fw(); - void set_idle_mc(); -}; - -namespace VTOL_att_control -{ -VtolAttitudeControl *g_control; -} - -/** -* Constructor -*/ -VtolAttitudeControl::VtolAttitudeControl() : - _task_should_exit(false), - _control_task(-1), - - //init subscription handlers - _v_att_sub(-1), - _v_att_sp_sub(-1), - _v_control_mode_sub(-1), - _params_sub(-1), - _manual_control_sp_sub(-1), - _armed_sub(-1), - - //init publication handlers - _actuators_0_pub(-1), - _actuators_1_pub(-1), - _vtol_vehicle_status_pub(-1), - - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) -{ - - flag_idle_mc = true; /*assume we always start in mc mode for a VTOL airframe */ - - memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); - _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ - memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); - memset(&_actuators_out_0, 0, sizeof(_actuators_out_0)); - memset(&_actuators_out_1, 0, sizeof(_actuators_out_1)); - memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); - memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); - memset(&_armed, 0, sizeof(_armed)); - - _params_handles.min_pwm_mc = param_find("PWM_MIN"); - - /* fetch initial parameter values */ - parameters_update(); -} - -/** -* Destructor -*/ -VtolAttitudeControl::~VtolAttitudeControl() -{ - if (_control_task != -1) { - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_control_task); - break; - } - } while (_control_task != -1); - } - - VTOL_att_control::g_control = nullptr; -} - -/** -* Check for changes in vehicle control mode. -*/ -void VtolAttitudeControl::vehicle_control_mode_poll() -{ - bool updated; - - /* Check if vehicle control mode has changed */ - orb_check(_v_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); - } -} - -/** -* Check for changes in manual inputs. -*/ -void VtolAttitudeControl::vehicle_manual_poll() -{ - bool updated; - - /* get pilots inputs */ - orb_check(_manual_control_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); - } -} -/** -* Check for arming status updates. -*/ -void VtolAttitudeControl::arming_status_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - } -} - -/** -* Check for inputs from mc attitude controller. -*/ -void VtolAttitudeControl::actuator_controls_mc_poll() -{ - bool updated; - orb_check(_actuator_inputs_mc, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in); - } -} - -/** -* Check for inputs from fw attitude controller. -*/ -void VtolAttitudeControl::actuator_controls_fw_poll() -{ - bool updated; - orb_check(_actuator_inputs_fw, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in); - } -} - -/** -* Check for parameter updates. -*/ -void -VtolAttitudeControl::parameters_update_poll() -{ - bool updated; - - /* Check if parameters have changed */ - orb_check(_params_sub, &updated); - - if (updated) { - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); - parameters_update(); - } -} - -/** -* Update parameters. -*/ -int -VtolAttitudeControl::parameters_update() -{ - /* idle pwm */ - float v; - param_get(_params_handles.min_pwm_mc, &v); - _params.min_pwm_mc = v; - - 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] = 0; //roll elevon - _actuators_out_1.control[1] = 0; //pitch elevon -} - -/** -* 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]; /*pitch elevon */ -} - -/** -* Adjust idle speed for fw mode. -*/ -void VtolAttitudeControl::set_idle_fw() -{ - int ret; - char *dev = PWM_OUTPUT_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 < 4; 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_OUTPUT_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 = 1100; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < 4; 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::task_main_trampoline(int argc, char *argv[]) -{ - VTOL_att_control::g_control->task_main(); -} - -void VtolAttitudeControl::task_main() -{ - warnx("started"); - fflush(stdout); - - /* do subscriptions */ - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - - _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); - _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); - - parameters_update(); /*initialize parameter cache/* - - /* wakeup source*/ - struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ - - fds[0].fd = _actuator_inputs_mc; - fds[0].events = POLLIN; - fds[1].fd = _actuator_inputs_fw; - fds[1].events = POLLIN; - fds[2].fd = _params_sub; - fds[2].events = POLLIN; - - while (!_task_should_exit) { - /*Advertise/Publish vtol vehicle status*/ - if (_vtol_vehicle_status_pub > 0) { - orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); - - } else { - _vtol_vehicle_status.timestamp = hrt_absolute_time(); - _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); - } - - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - - /* timed out - periodic check for _task_should_exit */ - if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - usleep(100000); - continue; - } - - if (fds[2].revents & POLLIN) { //parameters were updated, read them now - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - /* update parameters from storage */ - parameters_update(); - } - - vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - vehicle_manual_poll(); //Check for changes in manual inputs. - arming_status_poll(); //Check for arming status updates. - actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output - actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - parameters_update_poll(); - - if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc 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; - } - - /* got data from mc_att_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); - - fill_mc_att_control_output(); - - 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 */ - _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; - } - - if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */ - orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); - vehicle_manual_poll(); //update remote input - - fill_fw_att_control_output(); - - 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); - } - } - } - } - - warnx("exit"); - _control_task = -1; - _exit(0); -} - -int -VtolAttitudeControl::start() -{ - ASSERT(_control_task == -1); - - /* start the task */ - _control_task = task_spawn_cmd("vtol_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 2048, - (main_t)&VtolAttitudeControl::task_main_trampoline, - nullptr); - - if (_control_task < 0) { - warn("task start failed"); - return -errno; - } - - return OK; -} - - -int vtol_att_control_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "usage: vtol_att_control {start|stop|status}"); - } - - if (!strcmp(argv[1], "start")) { - - if (VTOL_att_control::g_control != nullptr) { - errx(1, "already running"); - } - - VTOL_att_control::g_control = new VtolAttitudeControl; - - if (VTOL_att_control::g_control == nullptr) { - errx(1, "alloc failed"); - } - - if (OK != VTOL_att_control::g_control->start()) { - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - err(1, "start failed"); - } - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - if (VTOL_att_control::g_control == nullptr) { - errx(1, "not running"); - } - - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (VTOL_att_control::g_control) { - errx(0, "running"); - - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - return 1; -} -- cgit v1.2.3