From e2d9badcd8e29caec7a8cb15d4caada57a0ff51c Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Wed, 29 Apr 2020 23:40:48 +0200 Subject: [PATCH 01/24] Update CONTRIBUTING.md - typo --- CONTRIBUTING.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index f852462d984d..3b98196ad097 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -41,4 +41,4 @@ Since we care about safety, we will regularly ask you for test results. Best is Push changes to your repo and send a [pull request](https://github.com/PX4/Firmware/compare/). -MuncherMake sure to provide some testing feedback and if possible the link to a flight log file. Upload flight log files to [Flight Review](http://logs.px4.io) and link the resulting report. +Make sure to provide some testing feedback and if possible the link to a flight log file. Upload flight log files to [Flight Review](http://logs.px4.io) and link the resulting report. From cc3e36a3e837b729a7cb2fb8de9c24ce8da53fe9 Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Thu, 7 May 2020 12:22:00 +0200 Subject: [PATCH 02/24] added cloudship - airframe, - mixers, - manual flying --- .../px4fmu_common/init.d-posix/2507_cloudship | 36 ++ .../init.d/airframes/2507_cloudship | 38 ++ ROMFS/px4fmu_common/init.d/rc.lta_apps | 64 +++ ROMFS/px4fmu_common/init.d/rc.lta_defaults | 28 ++ ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 26 ++ ROMFS/px4fmu_common/mixers/cloudship.main.mix | 51 +++ Tools/px4airframes/srcparser.py | 2 + Tools/sitl_gazebo | 2 +- boards/px4/sitl/default.cmake | 1 + msg/vehicle_status.msg | 1 + .../px4_platform_common/i2c_spi_buses.h | 6 +- platforms/posix/cmake/sitl_target.cmake | 2 +- src/modules/lta_att_control/CMakeLists.txt | 46 ++ .../lta_att_control/lta_att_control.hpp | 196 +++++++++ .../lta_att_control/lta_att_control_main.cpp | 407 ++++++++++++++++++ 15 files changed, 901 insertions(+), 5 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/2507_cloudship create mode 100644 ROMFS/px4fmu_common/init.d/airframes/2507_cloudship create mode 100644 ROMFS/px4fmu_common/init.d/rc.lta_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.lta_defaults create mode 100644 ROMFS/px4fmu_common/mixers/cloudship.main.mix create mode 100644 src/modules/lta_att_control/CMakeLists.txt create mode 100644 src/modules/lta_att_control/lta_att_control.hpp create mode 100644 src/modules/lta_att_control/lta_att_control_main.cpp diff --git a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship new file mode 100644 index 000000000000..c5d5948ee718 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship @@ -0,0 +1,36 @@ +#!/bin/sh +# +# @name Cloudship +# @type Airship +# @class Airship +# +# @output MAIN1 stabilator +# @output MAIN2 motor 1 starboard +# @output MAIN3 motor 2 port +# @output MAIN4 rudder motor + +sh /etc/init.d/rc.lta_defaults + +param set PWM_MAX 2000 +param set PWM_MIN 1000 + +param set MPC_MANTHR_MIN 0.00 +param set MPC_THR_MIN 0.00 +param set MPC_THR_CURVE 1 +param set MPC_THR_HOVER 0.0 + +param set NAV_DLL_ACT 0 +param set NAV_RCL_ACT 0 + +param set MC_ACRO_Y_MAX 360.0 +param set MPC_MAN_Y_MAX 360.0 + +param set LNDMC_Z_VEL_MAX 0.01 +param set LNDMC_XY_VEL_MAX 0.01 +param set LNDMC_LOW_T_THR 0.01 +param set LNDMC_ROT_MAX 1.0 + +set MIXER cloudship + +# Configure this as airship. +# set MAV_TYPE 7 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship new file mode 100644 index 000000000000..3fd79288d8e0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -0,0 +1,38 @@ +#!/bin/sh +# +# @name Cloudship +# @type Airship +# @class Airship +# +# @output MAIN1 stabilator +# @output MAIN2 motor 1 starboard +# @output MAIN3 motor 2 port +# @output MAIN4 rudder motor + +sh /etc/init.d/rc.lta_defaults + +param set PWM_MAX 2000 +param set PWM_MIN 1000 + +param set MPC_MANTHR_MIN 0.00 +param set MPC_THR_MIN 0.00 +param set MPC_THR_CURVE 1 +param set MPC_THR_HOVER 0.0 + +param set NAV_DLL_ACT 0 +param set NAV_RCL_ACT 0 + +param set MC_ACRO_Y_MAX 360.0 +param set MPC_MAN_Y_MAX 360.0 + +param set LNDMC_Z_VEL_MAX 0.01 +param set LNDMC_XY_VEL_MAX 0.01 +param set LNDMC_LOW_T_THR 0.01 +param set LNDMC_ROT_MAX 1.0 + +set MIXER cloudship + +# Configure this as airship. +# set MAV_TYPE 7 + +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/rc.lta_apps b/ROMFS/px4fmu_common/init.d/rc.lta_apps new file mode 100644 index 000000000000..f314ffdb6b63 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.lta_apps @@ -0,0 +1,64 @@ +#!/bin/sh +# +# Standard apps for airships. Attitude/Position estimator, Attitude/Position control. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +############################################################################### +# Begin Estimator Group Selection # +############################################################################### + +# +# LPE +# +if param compare SYS_MC_EST_GROUP 1 +then + # + # Try to start LPE. If it fails, start EKF2 as a default. + # Unfortunately we do not build it on px4_fmu-v2 due to a limited flash. + # + if attitude_estimator_q start + then + echo "WARN [init] Estimator LPE unsupported, EKF2 recommended." + local_position_estimator start + else + echo "ERROR [init] Estimator LPE not available. Using EKF2" + param set SYS_MC_EST_GROUP 2 + param save + reboot + fi +else + # + # Q estimator (attitude estimation only) + # + if param compare SYS_MC_EST_GROUP 3 + then + attitude_estimator_q start + else + # + # EKF2 + # + param set SYS_MC_EST_GROUP 2 + ekf2 start + fi +fi + +############################################################################### +# End Estimator Group Selection # +############################################################################### + +# +# Start Airship Attitude Controller. +# +lta_att_control start + +# +# Start Position Controller. +# +mc_pos_control start + +# +# Start Land Detector. +# +land_detector start multicopter diff --git a/ROMFS/px4fmu_common/init.d/rc.lta_defaults b/ROMFS/px4fmu_common/init.d/rc.lta_defaults new file mode 100644 index 000000000000..ce9bc68b705c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.lta_defaults @@ -0,0 +1,28 @@ +#!/bin/sh +# +# Airship default parameters. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +set VEHICLE_TYPE lta + +if [ $AUTOCNF = yes ] +then + param set NAV_ACC_RAD 2 + + param set RTL_RETURN_ALT 30 + param set RTL_DESCEND_ALT 10 + param set RTL_LAND_DELAY 1 + + param set PWM_MAX 2000 + param set PWM_MIN 1000 + param set PWM_RATE 400 +fi + +# +# This is the gimbal pass mixer. +# +set MIXER_AUX pass + +set PWM_AUX_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index b6002edc8730..d5076d6288e7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -147,6 +147,32 @@ then sh /etc/init.d/rc.vtol_apps fi +# +# Airship setup. +# +if [ $VEHICLE_TYPE = lta ] +then + if [ $MIXER = none ] + then + echo "LTA mixer undefined" + fi + + if [ $MAV_TYPE = none ] + then + # Set a default MAV_TYPE = 2 if not defined. + set MAV_TYPE 2 + fi + + # Set the mav type parameter. + param set MAV_TYPE ${MAV_TYPE} + + # Load mixer and configure outputs. + sh /etc/init.d/rc.interface + + # Start standard airship apps. + sh /etc/init.d/rc.lta_apps +fi + # # UUV setup # diff --git a/ROMFS/px4fmu_common/mixers/cloudship.main.mix b/ROMFS/px4fmu_common/mixers/cloudship.main.mix new file mode 100644 index 000000000000..ec79c65e9762 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/cloudship.main.mix @@ -0,0 +1,51 @@ +Stabilator/thruster/rudder mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling an airship with +stabilator, rudder rotor, starboard and port thruster using PX4FMU. +The configuration assumes the stabilator servo is connected to PX4FMU servo +output 0, starboard thruster to output 1, port thruster to output 2, and the +rudder rotor to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps). + +Servo controlling stabilator mixer +------------ +Two scalers total (output, thrust angle). + +This mixer assumes that the stabilator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +S: 0 1 10000 10000 0 -10000 10000 + +Starboard and port thruster mixer +----------------- +Two scalers total (output, thrust). + +By default mixer output is normalized. In the case of certain input controllers +the commented lines generate a full-range output (-1 to 1) from an input in the +(0 - 1) range. Inputs below zero are treated as zero. + +M: 1 +S: 0 3 20000 20000 -10000 -10000 10000 +#S: 0 3 10000 10000 0 -10000 10000 + +M: 1 +S: 0 3 20000 20000 -10000 -10000 10000 +#S: 0 3 10000 10000 0 -10000 10000 + +Rudder rotor mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder rotor is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the motor movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +S: 0 2 10000 10000 0 -10000 10000 diff --git a/Tools/px4airframes/srcparser.py b/Tools/px4airframes/srcparser.py index 771a28de519c..1efdb0244e8e 100644 --- a/Tools/px4airframes/srcparser.py +++ b/Tools/px4airframes/srcparser.py @@ -90,6 +90,8 @@ def GetImageName(self): return "YPlus" elif (self.name == "Autogyro"): return "Autogyro" + elif (self.name == "Airship"): + return "Airship" elif (self.name == "Rover"): return "Rover" elif (self.name == "Boat"): diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 2736a9134d7f..4deba1bc9159 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 2736a9134d7fed0cae7b7836d4370f9a4dfef439 +Subproject commit 4deba1bc91596ba3861da1beaae134d5c5a01ab3 diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 8f90549c2490..22a12343f98a 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -35,6 +35,7 @@ px4_add_board( #load_mon local_position_estimator logger + lta_att_control mavlink mc_att_control mc_hover_thrust_estimator diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 1b6e3818dac8..be98be817e0d 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -52,6 +52,7 @@ uint8 VEHICLE_TYPE_UNKNOWN = 0 uint8 VEHICLE_TYPE_ROTARY_WING = 1 uint8 VEHICLE_TYPE_FIXED_WING = 2 uint8 VEHICLE_TYPE_ROVER = 3 +uint8 VEHICLE_TYPE_LTA = 4 # state machine / state of vehicle. # Encodes the complete system state and is set by the commander app. diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index 0fab276a14be..fb850ba29d8c 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -236,11 +236,11 @@ class I2CSPIDriver : public I2CSPIDriverBase virtual ~I2CSPIDriver() = default; - void Run() final { + void Run() final + { static_cast(this)->RunImpl(); - if (should_exit()) - { + if (should_exit()) { exit_and_cleanup(); } } diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index 78de84497f88..53f20020b240 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -79,7 +79,7 @@ set(models none shell if750a iris iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps px4vision solo typhoon_h480 plane plane_cam plane_catapult plane_lidar standard_vtol tailsitter tiltrotor - rover r1_rover boat + rover r1_rover boat cloudship uuv_hippocampus) set(worlds none empty baylands ksql_airport mcmillan_airfield sonoma_raceway warehouse) set(all_posix_vmd_make_targets) diff --git a/src/modules/lta_att_control/CMakeLists.txt b/src/modules/lta_att_control/CMakeLists.txt new file mode 100644 index 000000000000..eaf810693260 --- /dev/null +++ b/src/modules/lta_att_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE modules__lta_att_control + MAIN lta_att_control + STACK_MAX 3500 + COMPILE_FLAGS + SRCS + lta_att_control_main.cpp + DEPENDS + circuit_breaker + conversion + mathlib + px4_work_queue + ) diff --git a/src/modules/lta_att_control/lta_att_control.hpp b/src/modules/lta_att_control/lta_att_control.hpp new file mode 100644 index 000000000000..488f45616ced --- /dev/null +++ b/src/modules/lta_att_control/lta_att_control.hpp @@ -0,0 +1,196 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2018 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. + * + ****************************************************************************/ + +#include // Airmode +#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 + +/** + * Multicopter attitude control app start / stop handling function + */ +extern "C" __EXPORT int lta_att_control_main(int argc, char *argv[]); + +class LTAAttitudeControl : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + LTAAttitudeControl(); + + virtual ~LTAAttitudeControl(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void Run() override; + + bool init(); + +private: + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + /** + * Check for parameter update and handle it. + */ + void parameter_update_poll(); + bool vehicle_attitude_poll(); + void vehicle_motor_limits_poll(); + void vehicle_status_poll(); + + void publish_actuator_controls(); + void publish_rates_setpoint(); + void publish_rate_controller_status(); + + /** + * Generate & publish an attitude setpoint from stick inputs + */ + void generate_attitude_setpoint(float dt, bool reset_yaw_sp); + + /** + * Get the landing gear state based on the manual control switch position + * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN + */ + float get_landing_gear_state(); + + + /** + * Attitude controller. + */ + void control_attitude(); + + /** + * Attitude rates controller. + */ + void control_attitude_rates(float dt, const matrix::Vector3f &rates); + + uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude subscription */ + uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */ + uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */ + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ + uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; + uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + + orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ + orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; + + orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ + orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */ + + bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ + + 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 actuator_controls_s _actuators {}; /**< actuator controls */ + struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ + struct battery_status_s _battery_status {}; /**< battery status */ + struct vehicle_land_detected_s _vehicle_land_detected {}; + struct landing_gear_s _landing_gear {}; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */ + float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */ + + matrix::Vector3f _rates_sp; /**< angular rates setpoint */ + + matrix::Vector3f _att_control; /**< attitude control vector */ + float _thrust_sp{0.0f}; /**< thrust setpoint */ + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ + + hrt_abstime _task_start{hrt_absolute_time()}; + hrt_abstime _last_run{0}; + float _dt_accumulator{0.0f}; + int _loop_counter{0}; + + bool _reset_yaw_sp{true}; + float _attitude_dt{0.0f}; + + bool _is_tailsitter{false}; + + matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ + float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + +}; diff --git a/src/modules/lta_att_control/lta_att_control_main.cpp b/src/modules/lta_att_control/lta_att_control_main.cpp new file mode 100644 index 000000000000..e95a8311b174 --- /dev/null +++ b/src/modules/lta_att_control/lta_att_control_main.cpp @@ -0,0 +1,407 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 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 lta_att_control_main.cpp + * Multicopter attitude controller. + * + * @author Lorenz Meier + * @author Anton Babushkin + * @author Sander Smeets + * @author Matthias Grob + * @author Beat Küng + * @author Daniel Robinson + */ + +#include "lta_att_control.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +LTAAttitudeControl::LTAAttitudeControl() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, "lta_att_control")) +{ + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + /* initialize quaternions in messages to be valid */ + _v_att.q[0] = 1.f; + _v_att_sp.q_d[0] = 1.f; + + _rates_sp.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + parameters_updated(); +} + +LTAAttitudeControl::~LTAAttitudeControl() +{ + perf_free(_loop_perf); +} + +bool +LTAAttitudeControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle_angular_velocity callback registration failed!"); + return false; + } + + return true; +} + +void +LTAAttitudeControl::parameters_updated() +{ + +} + +void +LTAAttitudeControl::parameter_update_poll() +{ + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } +} + +void +LTAAttitudeControl::vehicle_status_poll() +{ + /* check if there is new status information */ + if (_vehicle_status_sub.update(&_vehicle_status)) { + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (_actuators_id == nullptr) { + if (_vehicle_status.is_vtol) { + _actuators_id = ORB_ID(actuator_controls_virtual_mc); + _attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint); + + int32_t vt_type = -1; + + if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { + _is_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); + } + + } else { + _actuators_id = ORB_ID(actuator_controls_0); + _attitude_sp_id = ORB_ID(vehicle_attitude_setpoint); + } + } + } +} + +bool +LTAAttitudeControl::vehicle_attitude_poll() +{ + /* check if there is a new message */ + const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; + + if (_v_att_sub.update(&_v_att)) { + // Check for a heading reset + if (prev_quat_reset_counter != _v_att.quat_reset_counter) { + // we only extract the heading change from the delta quaternion + _man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi(); + } + + return true; + } + + return false; +} + +float +LTAAttitudeControl::get_landing_gear_state() +{ + // Only switch the landing gear up if we are not landed and if + // the user switched from gear down to gear up. + // If the user had the switch in the gear up position and took off ignore it + // until he toggles the switch to avoid retracting the gear immediately on takeoff. + if (_vehicle_land_detected.landed) { + _gear_state_initialized = false; + } + + float landing_gear = landing_gear_s::GEAR_DOWN; // default to down + + if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) { + landing_gear = landing_gear_s::GEAR_UP; + + } else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + // Switching the gear off does put it into a safe defined state + _gear_state_initialized = true; + } + + return landing_gear; +} + +void +LTAAttitudeControl::publish_rates_setpoint() +{ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = -_rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust_body[0] = 0.0f; + _v_rates_sp.thrust_body[1] = 0.0f; + _v_rates_sp.thrust_body[2] = -_thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + _v_rates_sp_pub.publish(_v_rates_sp); +} + +void +LTAAttitudeControl::publish_rate_controller_status() +{ + rate_ctrl_status_s rate_ctrl_status = {}; + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); +} + +void +LTAAttitudeControl::publish_actuator_controls() +{ + _actuators.control[0] = 0.0f; + _actuators.control[1] = _manual_control_sp.x; + _actuators.control[2] = _manual_control_sp.r; + _actuators.control[3] = _manual_control_sp.z; + _actuators.control[7] = (float)_landing_gear.landing_gear; + // note: _actuators.timestamp_sample is set in LTAAttitudeControl::Run() + _actuators.timestamp = hrt_absolute_time(); + + if (!_actuators_0_circuit_breaker_enabled) { + orb_publish_auto(_actuators_id, &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT); + } +} + +void +LTAAttitudeControl::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + const hrt_abstime now = hrt_absolute_time(); + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); + _last_run = now; + + const Vector3f rates{angular_velocity.xyz}; + + _actuators.timestamp_sample = angular_velocity.timestamp_sample; + + /* run the rate controller immediately after a gyro update */ + if (_v_control_mode.flag_control_rates_enabled) { + publish_actuator_controls(); + publish_rate_controller_status(); + } + + /* check for updates in other topics */ + _v_control_mode_sub.update(&_v_control_mode); + _battery_status_sub.update(&_battery_status); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); + _landing_gear_sub.update(&_landing_gear); + vehicle_status_poll(); + const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); + const bool attitude_updated = vehicle_attitude_poll(); + + _attitude_dt += dt; + + bool attitude_setpoint_generated = _v_control_mode.flag_control_altitude_enabled + || _v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_position_enabled; + + if (attitude_setpoint_generated) { + if (attitude_updated) { + + if (_v_control_mode.flag_control_yawrate_override_enabled) { + /* Yaw rate override enabled, overwrite the yaw setpoint */ + _v_rates_sp_sub.update(&_v_rates_sp); + const auto yawrate_reference = _v_rates_sp.yaw; + _rates_sp(2) = yawrate_reference; + } + + publish_rates_setpoint(); + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_control_mode.flag_control_manual_enabled) { + if (manual_control_updated) { + /* manual control - feed RC commands to actuators directly */ + _v_att_sp.thrust_body[0] = _manual_control_sp.x; + _v_att_sp.thrust_body[2] = _manual_control_sp.z; + _rates_sp(2) = _manual_control_sp.r; + + // PX4_INFO("\nManual X: %.2f\nManual Z: %.2f\nManual R: %.2f\n", + // (double)_manual_control_sp.x, (double)_manual_control_sp.z, + // (double)_manual_control_sp.r); + + publish_rates_setpoint(); + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_rates_sp_sub.update(&_v_rates_sp)) { + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = -_v_rates_sp.thrust_body[2]; + } + } + } + + + if (_v_control_mode.flag_control_termination_enabled) { + if (!_vehicle_status.is_vtol) { + _rates_sp.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + publish_actuator_controls(); + } + } + + /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ + if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) { + _dt_accumulator += dt; + ++_loop_counter; + + if (_dt_accumulator > 1.f) { + const float loop_update_rate = (float)_loop_counter / _dt_accumulator; + _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; + _dt_accumulator = 0; + _loop_counter = 0; + } + } + + parameter_update_poll(); + } + + perf_end(_loop_perf); +} + +int LTAAttitudeControl::task_spawn(int argc, char *argv[]) +{ + LTAAttitudeControl *instance = new LTAAttitudeControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int LTAAttitudeControl::print_status() +{ + PX4_INFO("Running"); + + perf_print_counter(_loop_perf); + + print_message(_actuators); + + return 0; +} + +int LTAAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int LTAAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the multicopter attitude and rate controller. It takes attitude +setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode +via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. + +The controller has two loops: a P loop for angular error and a PID loop for angular rate error. + +Publication documenting the implemented Quaternion Attitude Control: +Nonlinear Quadrocopter Attitude Control (2013) +by Dario Brescianini, Markus Hehn and Raffaello D'Andrea +Institute for Dynamic Systems and Control (IDSC), ETH Zurich + +https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf + +### Implementation +To reduce control latency, the module directly polls on the gyro topic published by the IMU driver. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("lta_att_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int lta_att_control_main(int argc, char *argv[]) +{ + return LTAAttitudeControl::main(argc, argv); +} From fca67cdd972d322df6336c65d35f31889e64105b Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Fri, 8 May 2020 00:16:30 +0200 Subject: [PATCH 03/24] mixer update --- ROMFS/px4fmu_common/mixers/cloudship.main.mix | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/cloudship.main.mix b/ROMFS/px4fmu_common/mixers/cloudship.main.mix index ec79c65e9762..7c92bccdff18 100644 --- a/ROMFS/px4fmu_common/mixers/cloudship.main.mix +++ b/ROMFS/px4fmu_common/mixers/cloudship.main.mix @@ -31,12 +31,12 @@ the commented lines generate a full-range output (-1 to 1) from an input in the (0 - 1) range. Inputs below zero are treated as zero. M: 1 -S: 0 3 20000 20000 -10000 -10000 10000 -#S: 0 3 10000 10000 0 -10000 10000 +S: 0 3 10000 10000 0 -10000 10000 +#S: 0 3 20000 20000 -10000 -10000 10000 M: 1 -S: 0 3 20000 20000 -10000 -10000 10000 -#S: 0 3 10000 10000 0 -10000 10000 +S: 0 3 10000 10000 0 -10000 10000 +#S: 0 3 20000 20000 -10000 -10000 10000 Rudder rotor mixer ------------ From b00888c1c82ffeed587dbb5fae779f1a3c9180b7 Mon Sep 17 00:00:00 2001 From: Daniel Leonard Robinson Date: Sat, 9 May 2020 09:18:04 +0200 Subject: [PATCH 04/24] revert astyle check --- platforms/common/include/px4_platform_common/i2c_spi_buses.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index fb850ba29d8c..afee9d3785d4 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -236,8 +236,7 @@ class I2CSPIDriver : public I2CSPIDriverBase virtual ~I2CSPIDriver() = default; - void Run() final - { + void Run() final { static_cast(this)->RunImpl(); if (should_exit()) { From d04899ae558e724918415a65a31d964a63b80dda Mon Sep 17 00:00:00 2001 From: Daniel Leonard Robinson Date: Sat, 9 May 2020 09:20:50 +0200 Subject: [PATCH 05/24] revert i2c_spi_buses.h --- platforms/common/include/px4_platform_common/i2c_spi_buses.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index afee9d3785d4..67244f379bd2 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -239,7 +239,8 @@ class I2CSPIDriver : public I2CSPIDriverBase void Run() final { static_cast(this)->RunImpl(); - if (should_exit()) { + if (should_exit()) + { exit_and_cleanup(); } } From da5f5044103a08b09c7d4d53e67c2287366a03d2 Mon Sep 17 00:00:00 2001 From: Daniel Leonard Robinson Date: Sat, 9 May 2020 09:21:52 +0200 Subject: [PATCH 06/24] space --- platforms/common/include/px4_platform_common/i2c_spi_buses.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index 67244f379bd2..0fab276a14be 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -239,7 +239,7 @@ class I2CSPIDriver : public I2CSPIDriverBase void Run() final { static_cast(this)->RunImpl(); - if (should_exit()) + if (should_exit()) { exit_and_cleanup(); } From b6a2223452c26385ca75e5a5c26ffdba93382568 Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Sat, 9 May 2020 09:52:58 +0200 Subject: [PATCH 07/24] renamed lta -> airship --- .../px4fmu_common/init.d-posix/2507_cloudship | 2 +- .../init.d/airframes/2507_cloudship | 2 +- .../init.d/{rc.lta_apps => rc.airship_apps} | 2 +- .../{rc.lta_defaults => rc.airship_defaults} | 2 +- ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 8 ++-- boards/px4/sitl/default.cmake | 2 +- msg/vehicle_status.msg | 2 +- .../CMakeLists.txt | 6 +-- .../airship_att_control.hpp} | 8 ++-- .../airship_att_control_main.cpp} | 48 +++++++++---------- 10 files changed, 41 insertions(+), 41 deletions(-) rename ROMFS/px4fmu_common/init.d/{rc.lta_apps => rc.airship_apps} (98%) rename ROMFS/px4fmu_common/init.d/{rc.lta_defaults => rc.airship_defaults} (94%) rename src/modules/{lta_att_control => airship_att_control}/CMakeLists.txt (95%) rename src/modules/{lta_att_control/lta_att_control.hpp => airship_att_control/airship_att_control.hpp} (97%) rename src/modules/{lta_att_control/lta_att_control_main.cpp => airship_att_control/airship_att_control_main.cpp} (89%) diff --git a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship index c5d5948ee718..01b44d8e6868 100644 --- a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship @@ -9,7 +9,7 @@ # @output MAIN3 motor 2 port # @output MAIN4 rudder motor -sh /etc/init.d/rc.lta_defaults +sh /etc/init.d/rc.airship_defaults param set PWM_MAX 2000 param set PWM_MIN 1000 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship index 3fd79288d8e0..08910fe28a4b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -9,7 +9,7 @@ # @output MAIN3 motor 2 port # @output MAIN4 rudder motor -sh /etc/init.d/rc.lta_defaults +sh /etc/init.d/rc.airship_defaults param set PWM_MAX 2000 param set PWM_MIN 1000 diff --git a/ROMFS/px4fmu_common/init.d/rc.lta_apps b/ROMFS/px4fmu_common/init.d/rc.airship_apps similarity index 98% rename from ROMFS/px4fmu_common/init.d/rc.lta_apps rename to ROMFS/px4fmu_common/init.d/rc.airship_apps index f314ffdb6b63..c5fbc1bd926f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.lta_apps +++ b/ROMFS/px4fmu_common/init.d/rc.airship_apps @@ -51,7 +51,7 @@ fi # # Start Airship Attitude Controller. # -lta_att_control start +airship_att_control start # # Start Position Controller. diff --git a/ROMFS/px4fmu_common/init.d/rc.lta_defaults b/ROMFS/px4fmu_common/init.d/rc.airship_defaults similarity index 94% rename from ROMFS/px4fmu_common/init.d/rc.lta_defaults rename to ROMFS/px4fmu_common/init.d/rc.airship_defaults index ce9bc68b705c..fbff47cfc25a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.lta_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.airship_defaults @@ -5,7 +5,7 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -set VEHICLE_TYPE lta +set VEHICLE_TYPE airship if [ $AUTOCNF = yes ] then diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index d5076d6288e7..5267854f361a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -150,11 +150,11 @@ fi # # Airship setup. # -if [ $VEHICLE_TYPE = lta ] +if [ $VEHICLE_TYPE = airship ] then if [ $MIXER = none ] then - echo "LTA mixer undefined" + echo "Airship mixer undefined" fi if [ $MAV_TYPE = none ] @@ -169,8 +169,8 @@ then # Load mixer and configure outputs. sh /etc/init.d/rc.interface - # Start standard airship apps. - sh /etc/init.d/rc.lta_apps + # Start airship apps. + sh /etc/init.d/rc.airship_apps fi # diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 22a12343f98a..698cec12d7cc 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -21,6 +21,7 @@ px4_add_board( tone_alarm #uavcan MODULES + airship_att_control airspeed_selector attitude_estimator_q camera_feedback @@ -35,7 +36,6 @@ px4_add_board( #load_mon local_position_estimator logger - lta_att_control mavlink mc_att_control mc_hover_thrust_estimator diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index be98be817e0d..eb99764f8a88 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -52,7 +52,7 @@ uint8 VEHICLE_TYPE_UNKNOWN = 0 uint8 VEHICLE_TYPE_ROTARY_WING = 1 uint8 VEHICLE_TYPE_FIXED_WING = 2 uint8 VEHICLE_TYPE_ROVER = 3 -uint8 VEHICLE_TYPE_LTA = 4 +uint8 VEHICLE_TYPE_AIRSHIP = 4 # state machine / state of vehicle. # Encodes the complete system state and is set by the commander app. diff --git a/src/modules/lta_att_control/CMakeLists.txt b/src/modules/airship_att_control/CMakeLists.txt similarity index 95% rename from src/modules/lta_att_control/CMakeLists.txt rename to src/modules/airship_att_control/CMakeLists.txt index eaf810693260..ecb00da58caa 100644 --- a/src/modules/lta_att_control/CMakeLists.txt +++ b/src/modules/airship_att_control/CMakeLists.txt @@ -32,12 +32,12 @@ ############################################################################ px4_add_module( - MODULE modules__lta_att_control - MAIN lta_att_control + MODULE modules__airship_att_control + MAIN airship_att_control STACK_MAX 3500 COMPILE_FLAGS SRCS - lta_att_control_main.cpp + airship_att_control_main.cpp DEPENDS circuit_breaker conversion diff --git a/src/modules/lta_att_control/lta_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp similarity index 97% rename from src/modules/lta_att_control/lta_att_control.hpp rename to src/modules/airship_att_control/airship_att_control.hpp index 488f45616ced..466942652ebc 100644 --- a/src/modules/lta_att_control/lta_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -63,15 +63,15 @@ /** * Multicopter attitude control app start / stop handling function */ -extern "C" __EXPORT int lta_att_control_main(int argc, char *argv[]); +extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]); -class LTAAttitudeControl : public ModuleBase, public ModuleParams, +class AirshipAttitudeControl : public ModuleBase, public ModuleParams, public px4::WorkItem { public: - LTAAttitudeControl(); + AirshipAttitudeControl(); - virtual ~LTAAttitudeControl(); + virtual ~AirshipAttitudeControl(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); diff --git a/src/modules/lta_att_control/lta_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp similarity index 89% rename from src/modules/lta_att_control/lta_att_control_main.cpp rename to src/modules/airship_att_control/airship_att_control_main.cpp index e95a8311b174..55065a9bf70c 100644 --- a/src/modules/lta_att_control/lta_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file lta_att_control_main.cpp + * @file airship_att_control_main.cpp * Multicopter attitude controller. * * @author Lorenz Meier @@ -43,7 +43,7 @@ * @author Daniel Robinson */ -#include "lta_att_control.hpp" +#include "airship_att_control.hpp" #include #include @@ -54,10 +54,10 @@ using namespace matrix; -LTAAttitudeControl::LTAAttitudeControl() : +AirshipAttitudeControl::AirshipAttitudeControl() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _loop_perf(perf_alloc(PC_ELAPSED, "lta_att_control")) + _loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control")) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; @@ -72,13 +72,13 @@ LTAAttitudeControl::LTAAttitudeControl() : parameters_updated(); } -LTAAttitudeControl::~LTAAttitudeControl() +AirshipAttitudeControl::~AirshipAttitudeControl() { perf_free(_loop_perf); } bool -LTAAttitudeControl::init() +AirshipAttitudeControl::init() { if (!_vehicle_angular_velocity_sub.registerCallback()) { PX4_ERR("vehicle_angular_velocity callback registration failed!"); @@ -89,13 +89,13 @@ LTAAttitudeControl::init() } void -LTAAttitudeControl::parameters_updated() +AirshipAttitudeControl::parameters_updated() { } void -LTAAttitudeControl::parameter_update_poll() +AirshipAttitudeControl::parameter_update_poll() { // check for parameter updates if (_parameter_update_sub.updated()) { @@ -109,7 +109,7 @@ LTAAttitudeControl::parameter_update_poll() } void -LTAAttitudeControl::vehicle_status_poll() +AirshipAttitudeControl::vehicle_status_poll() { /* check if there is new status information */ if (_vehicle_status_sub.update(&_vehicle_status)) { @@ -134,7 +134,7 @@ LTAAttitudeControl::vehicle_status_poll() } bool -LTAAttitudeControl::vehicle_attitude_poll() +AirshipAttitudeControl::vehicle_attitude_poll() { /* check if there is a new message */ const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; @@ -153,7 +153,7 @@ LTAAttitudeControl::vehicle_attitude_poll() } float -LTAAttitudeControl::get_landing_gear_state() +AirshipAttitudeControl::get_landing_gear_state() { // Only switch the landing gear up if we are not landed and if // the user switched from gear down to gear up. @@ -177,7 +177,7 @@ LTAAttitudeControl::get_landing_gear_state() } void -LTAAttitudeControl::publish_rates_setpoint() +AirshipAttitudeControl::publish_rates_setpoint() { _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = -_rates_sp(1); @@ -191,7 +191,7 @@ LTAAttitudeControl::publish_rates_setpoint() } void -LTAAttitudeControl::publish_rate_controller_status() +AirshipAttitudeControl::publish_rate_controller_status() { rate_ctrl_status_s rate_ctrl_status = {}; rate_ctrl_status.timestamp = hrt_absolute_time(); @@ -199,14 +199,14 @@ LTAAttitudeControl::publish_rate_controller_status() } void -LTAAttitudeControl::publish_actuator_controls() +AirshipAttitudeControl::publish_actuator_controls() { _actuators.control[0] = 0.0f; _actuators.control[1] = _manual_control_sp.x; _actuators.control[2] = _manual_control_sp.r; _actuators.control[3] = _manual_control_sp.z; _actuators.control[7] = (float)_landing_gear.landing_gear; - // note: _actuators.timestamp_sample is set in LTAAttitudeControl::Run() + // note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run() _actuators.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { @@ -215,7 +215,7 @@ LTAAttitudeControl::publish_actuator_controls() } void -LTAAttitudeControl::Run() +AirshipAttitudeControl::Run() { if (should_exit()) { _vehicle_angular_velocity_sub.unregisterCallback(); @@ -328,9 +328,9 @@ LTAAttitudeControl::Run() perf_end(_loop_perf); } -int LTAAttitudeControl::task_spawn(int argc, char *argv[]) +int AirshipAttitudeControl::task_spawn(int argc, char *argv[]) { - LTAAttitudeControl *instance = new LTAAttitudeControl(); + AirshipAttitudeControl *instance = new AirshipAttitudeControl(); if (instance) { _object.store(instance); @@ -351,7 +351,7 @@ int LTAAttitudeControl::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -int LTAAttitudeControl::print_status() +int AirshipAttitudeControl::print_status() { PX4_INFO("Running"); @@ -362,12 +362,12 @@ int LTAAttitudeControl::print_status() return 0; } -int LTAAttitudeControl::custom_command(int argc, char *argv[]) +int AirshipAttitudeControl::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int LTAAttitudeControl::print_usage(const char *reason) +int AirshipAttitudeControl::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); @@ -394,14 +394,14 @@ To reduce control latency, the module directly polls on the gyro topic published )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("lta_att_control", "controller"); + PRINT_MODULE_USAGE_NAME("airship_att_control", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } -int lta_att_control_main(int argc, char *argv[]) +int airship_att_control_main(int argc, char *argv[]) { - return LTAAttitudeControl::main(argc, argv); + return AirshipAttitudeControl::main(argc, argv); } From b30acd367df19ef741d1cab4a1ae939cb1cc6502 Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Mon, 1 Jun 2020 13:57:46 +0200 Subject: [PATCH 08/24] edits, cleaned up --- .../px4fmu_common/init.d-posix/2507_cloudship | 28 +++------ .../init.d/airframes/2507_cloudship | 30 ++++------ .../px4fmu_common/init.d/rc.airship_defaults | 6 -- Tools/sitl_gazebo | 2 +- .../airship_att_control.hpp | 54 +---------------- .../airship_att_control_main.cpp | 60 ++----------------- 6 files changed, 25 insertions(+), 155 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship index 01b44d8e6868..7c72795fb0e9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship @@ -11,26 +11,16 @@ sh /etc/init.d/rc.airship_defaults -param set PWM_MAX 2000 -param set PWM_MIN 1000 +if [ $AUTOCNF = yes ] +then + param set MPC_MAN_Y_MAX 360.0 -param set MPC_MANTHR_MIN 0.00 -param set MPC_THR_MIN 0.00 -param set MPC_THR_CURVE 1 -param set MPC_THR_HOVER 0.0 + # do not trigger the land detecter unless in hanger + param set LNDMC_Z_VEL_MAX 0.01 + param set LNDMC_XY_VEL_MAX 0.01 + param set LNDMC_LOW_T_THR 0.01 + param set LNDMC_ROT_MAX 1.0 -param set NAV_DLL_ACT 0 -param set NAV_RCL_ACT 0 - -param set MC_ACRO_Y_MAX 360.0 -param set MPC_MAN_Y_MAX 360.0 - -param set LNDMC_Z_VEL_MAX 0.01 -param set LNDMC_XY_VEL_MAX 0.01 -param set LNDMC_LOW_T_THR 0.01 -param set LNDMC_ROT_MAX 1.0 +fi set MIXER cloudship - -# Configure this as airship. -# set MAV_TYPE 7 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship index 08910fe28a4b..00c58fae33e2 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -11,28 +11,18 @@ sh /etc/init.d/rc.airship_defaults -param set PWM_MAX 2000 -param set PWM_MIN 1000 +if [ $AUTOCNF = yes ] +then + param set MPC_MAN_Y_MAX 360.0 -param set MPC_MANTHR_MIN 0.00 -param set MPC_THR_MIN 0.00 -param set MPC_THR_CURVE 1 -param set MPC_THR_HOVER 0.0 + # do not trigger the land detecter unless in hanger + param set LNDMC_Z_VEL_MAX 0.01 + param set LNDMC_XY_VEL_MAX 0.01 + param set LNDMC_LOW_T_THR 0.01 + param set LNDMC_ROT_MAX 1.0 -param set NAV_DLL_ACT 0 -param set NAV_RCL_ACT 0 - -param set MC_ACRO_Y_MAX 360.0 -param set MPC_MAN_Y_MAX 360.0 - -param set LNDMC_Z_VEL_MAX 0.01 -param set LNDMC_XY_VEL_MAX 0.01 -param set LNDMC_LOW_T_THR 0.01 -param set LNDMC_ROT_MAX 1.0 +fi set MIXER cloudship - -# Configure this as airship. -# set MAV_TYPE 7 - set PWM_OUT 1234 + diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_defaults b/ROMFS/px4fmu_common/init.d/rc.airship_defaults index fbff47cfc25a..3e522058353c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.airship_defaults @@ -9,12 +9,6 @@ set VEHICLE_TYPE airship if [ $AUTOCNF = yes ] then - param set NAV_ACC_RAD 2 - - param set RTL_RETURN_ALT 30 - param set RTL_DESCEND_ALT 10 - param set RTL_LAND_DELAY 1 - param set PWM_MAX 2000 param set PWM_MIN 1000 param set PWM_RATE 400 diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 4deba1bc9159..f046038b14d1 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 4deba1bc91596ba3861da1beaae134d5c5a01ab3 +Subproject commit f046038b14d1e8a2bd3860bc24ea3d8fc7bbb870 diff --git a/src/modules/airship_att_control/airship_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp index 466942652ebc..8bdc22742cfd 100644 --- a/src/modules/airship_att_control/airship_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -32,22 +32,13 @@ ****************************************************************************/ #include // Airmode -#include -#include -#include -#include #include #include -#include -#include -#include #include #include #include #include -#include #include -#include #include #include #include @@ -57,11 +48,9 @@ #include #include #include -#include -#include /** - * Multicopter attitude control app start / stop handling function + * Airship attitude control app start / stop handling function */ extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]); @@ -101,35 +90,12 @@ class AirshipAttitudeControl : public ModuleBase, public */ void parameter_update_poll(); bool vehicle_attitude_poll(); - void vehicle_motor_limits_poll(); void vehicle_status_poll(); void publish_actuator_controls(); void publish_rates_setpoint(); void publish_rate_controller_status(); - /** - * Generate & publish an attitude setpoint from stick inputs - */ - void generate_attitude_setpoint(float dt, bool reset_yaw_sp); - - /** - * Get the landing gear state based on the manual control switch position - * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN - */ - float get_landing_gear_state(); - - - /** - * Attitude controller. - */ - void control_attitude(); - - /** - * Attitude rates controller. - */ - void control_attitude_rates(float dt, const matrix::Vector3f &rates); - uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude subscription */ uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */ uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ @@ -137,19 +103,14 @@ class AirshipAttitudeControl : public ModuleBase, public uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */ - uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ - uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ - uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ - orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */ @@ -163,9 +124,7 @@ class AirshipAttitudeControl : public ModuleBase, public struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */ struct actuator_controls_s _actuators {}; /**< actuator controls */ struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ - struct battery_status_s _battery_status {}; /**< battery status */ struct vehicle_land_detected_s _vehicle_land_detected {}; - struct landing_gear_s _landing_gear {}; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -174,23 +133,12 @@ class AirshipAttitudeControl : public ModuleBase, public matrix::Vector3f _rates_sp; /**< angular rates setpoint */ - matrix::Vector3f _att_control; /**< attitude control vector */ float _thrust_sp{0.0f}; /**< thrust setpoint */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ hrt_abstime _task_start{hrt_absolute_time()}; hrt_abstime _last_run{0}; float _dt_accumulator{0.0f}; int _loop_counter{0}; - bool _reset_yaw_sp{true}; - float _attitude_dt{0.0f}; - - bool _is_tailsitter{false}; - - matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ - float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ - }; diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index 55065a9bf70c..34aafdb7f233 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -67,9 +67,6 @@ AirshipAttitudeControl::AirshipAttitudeControl() : _rates_sp.zero(); _thrust_sp = 0.0f; - _att_control.zero(); - - parameters_updated(); } AirshipAttitudeControl::~AirshipAttitudeControl() @@ -88,12 +85,6 @@ AirshipAttitudeControl::init() return true; } -void -AirshipAttitudeControl::parameters_updated() -{ - -} - void AirshipAttitudeControl::parameter_update_poll() { @@ -119,12 +110,6 @@ AirshipAttitudeControl::vehicle_status_poll() _actuators_id = ORB_ID(actuator_controls_virtual_mc); _attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint); - int32_t vt_type = -1; - - if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { - _is_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); - } - } else { _actuators_id = ORB_ID(actuator_controls_0); _attitude_sp_id = ORB_ID(vehicle_attitude_setpoint); @@ -152,30 +137,6 @@ AirshipAttitudeControl::vehicle_attitude_poll() return false; } -float -AirshipAttitudeControl::get_landing_gear_state() -{ - // Only switch the landing gear up if we are not landed and if - // the user switched from gear down to gear up. - // If the user had the switch in the gear up position and took off ignore it - // until he toggles the switch to avoid retracting the gear immediately on takeoff. - if (_vehicle_land_detected.landed) { - _gear_state_initialized = false; - } - - float landing_gear = landing_gear_s::GEAR_DOWN; // default to down - - if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) { - landing_gear = landing_gear_s::GEAR_UP; - - } else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { - // Switching the gear off does put it into a safe defined state - _gear_state_initialized = true; - } - - return landing_gear; -} - void AirshipAttitudeControl::publish_rates_setpoint() { @@ -205,7 +166,7 @@ AirshipAttitudeControl::publish_actuator_controls() _actuators.control[1] = _manual_control_sp.x; _actuators.control[2] = _manual_control_sp.r; _actuators.control[3] = _manual_control_sp.z; - _actuators.control[7] = (float)_landing_gear.landing_gear; + // note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run() _actuators.timestamp = hrt_absolute_time(); @@ -247,15 +208,11 @@ AirshipAttitudeControl::Run() /* check for updates in other topics */ _v_control_mode_sub.update(&_v_control_mode); - _battery_status_sub.update(&_battery_status); _vehicle_land_detected_sub.update(&_vehicle_land_detected); - _landing_gear_sub.update(&_landing_gear); vehicle_status_poll(); const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); const bool attitude_updated = vehicle_attitude_poll(); - _attitude_dt += dt; - bool attitude_setpoint_generated = _v_control_mode.flag_control_altitude_enabled || _v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_position_enabled; @@ -299,12 +256,10 @@ AirshipAttitudeControl::Run() } } - if (_v_control_mode.flag_control_termination_enabled) { if (!_vehicle_status.is_vtol) { _rates_sp.zero(); _thrust_sp = 0.0f; - _att_control.zero(); publish_actuator_controls(); } } @@ -376,18 +331,11 @@ int AirshipAttitudeControl::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This implements the multicopter attitude and rate controller. It takes attitude -setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode +This implements the airship attitude and rate controller. Ideally it would +take attitude setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. -The controller has two loops: a P loop for angular error and a PID loop for angular rate error. - -Publication documenting the implemented Quaternion Attitude Control: -Nonlinear Quadrocopter Attitude Control (2013) -by Dario Brescianini, Markus Hehn and Raffaello D'Andrea -Institute for Dynamic Systems and Control (IDSC), ETH Zurich - -https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf +Currently it is feeding the `manual_control_setpoint` topic directly to the actuators. ### Implementation To reduce control latency, the module directly polls on the gyro topic published by the IMU driver. From 99214533fdb5909a0577be1d2a2ebf1bf6b7c062 Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Thu, 4 Jun 2020 15:44:52 +0200 Subject: [PATCH 09/24] pr: further edits - airship_att_control - params in rc.airship_defaults --- .../px4fmu_common/init.d/rc.airship_defaults | 7 ----- .../airship_att_control.hpp | 10 +------ .../airship_att_control_main.cpp | 29 ++----------------- 3 files changed, 3 insertions(+), 43 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_defaults b/ROMFS/px4fmu_common/init.d/rc.airship_defaults index 3e522058353c..90e4de12be8e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.airship_defaults @@ -7,13 +7,6 @@ set VEHICLE_TYPE airship -if [ $AUTOCNF = yes ] -then - param set PWM_MAX 2000 - param set PWM_MIN 1000 - param set PWM_RATE 400 -fi - # # This is the gimbal pass mixer. # diff --git a/src/modules/airship_att_control/airship_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp index 8bdc22742cfd..07a920295320 100644 --- a/src/modules/airship_att_control/airship_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -128,17 +128,9 @@ class AirshipAttitudeControl : public ModuleBase, public perf_counter_t _loop_perf; /**< loop performance counter */ - static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */ - float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */ - matrix::Vector3f _rates_sp; /**< angular rates setpoint */ float _thrust_sp{0.0f}; /**< thrust setpoint */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - - hrt_abstime _task_start{hrt_absolute_time()}; - hrt_abstime _last_run{0}; - float _dt_accumulator{0.0f}; - int _loop_counter{0}; + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ }; diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index 34aafdb7f233..1fae69bcb671 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -33,25 +33,18 @@ /** * @file airship_att_control_main.cpp - * Multicopter attitude controller. + * Airship attitude controller. * * @author Lorenz Meier * @author Anton Babushkin * @author Sander Smeets * @author Matthias Grob * @author Beat Küng - * @author Daniel Robinson + * @author Daniel Robinson */ #include "airship_att_control.hpp" -#include -#include -#include -#include -#include -#include - using namespace matrix; AirshipAttitudeControl::AirshipAttitudeControl() : @@ -190,11 +183,6 @@ AirshipAttitudeControl::Run() vehicle_angular_velocity_s angular_velocity; if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { - const hrt_abstime now = hrt_absolute_time(); - - // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); - _last_run = now; const Vector3f rates{angular_velocity.xyz}; @@ -264,19 +252,6 @@ AirshipAttitudeControl::Run() } } - /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ - if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) { - _dt_accumulator += dt; - ++_loop_counter; - - if (_dt_accumulator > 1.f) { - const float loop_update_rate = (float)_loop_counter / _dt_accumulator; - _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; - _dt_accumulator = 0; - _loop_counter = 0; - } - } - parameter_update_poll(); } From 7bd6474d3a8690c7983e22fb424992e8470b4562 Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Thu, 7 May 2020 12:22:00 +0200 Subject: [PATCH 10/24] added cloudship - airframe, - mixers, - manual flying --- .../px4fmu_common/init.d-posix/2507_cloudship | 36 ++ .../init.d/airframes/2507_cloudship | 38 ++ ROMFS/px4fmu_common/init.d/rc.lta_apps | 64 +++ ROMFS/px4fmu_common/init.d/rc.lta_defaults | 28 ++ ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 26 ++ ROMFS/px4fmu_common/mixers/cloudship.main.mix | 51 +++ Tools/px4airframes/srcparser.py | 2 + boards/px4/sitl/default.cmake | 1 + msg/vehicle_status.msg | 1 + platforms/posix/cmake/sitl_target.cmake | 2 +- src/modules/lta_att_control/CMakeLists.txt | 46 ++ .../lta_att_control/lta_att_control.hpp | 196 +++++++++ .../lta_att_control/lta_att_control_main.cpp | 407 ++++++++++++++++++ 13 files changed, 897 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/2507_cloudship create mode 100644 ROMFS/px4fmu_common/init.d/airframes/2507_cloudship create mode 100644 ROMFS/px4fmu_common/init.d/rc.lta_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.lta_defaults create mode 100644 ROMFS/px4fmu_common/mixers/cloudship.main.mix create mode 100644 src/modules/lta_att_control/CMakeLists.txt create mode 100644 src/modules/lta_att_control/lta_att_control.hpp create mode 100644 src/modules/lta_att_control/lta_att_control_main.cpp diff --git a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship new file mode 100644 index 000000000000..c5d5948ee718 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship @@ -0,0 +1,36 @@ +#!/bin/sh +# +# @name Cloudship +# @type Airship +# @class Airship +# +# @output MAIN1 stabilator +# @output MAIN2 motor 1 starboard +# @output MAIN3 motor 2 port +# @output MAIN4 rudder motor + +sh /etc/init.d/rc.lta_defaults + +param set PWM_MAX 2000 +param set PWM_MIN 1000 + +param set MPC_MANTHR_MIN 0.00 +param set MPC_THR_MIN 0.00 +param set MPC_THR_CURVE 1 +param set MPC_THR_HOVER 0.0 + +param set NAV_DLL_ACT 0 +param set NAV_RCL_ACT 0 + +param set MC_ACRO_Y_MAX 360.0 +param set MPC_MAN_Y_MAX 360.0 + +param set LNDMC_Z_VEL_MAX 0.01 +param set LNDMC_XY_VEL_MAX 0.01 +param set LNDMC_LOW_T_THR 0.01 +param set LNDMC_ROT_MAX 1.0 + +set MIXER cloudship + +# Configure this as airship. +# set MAV_TYPE 7 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship new file mode 100644 index 000000000000..3fd79288d8e0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -0,0 +1,38 @@ +#!/bin/sh +# +# @name Cloudship +# @type Airship +# @class Airship +# +# @output MAIN1 stabilator +# @output MAIN2 motor 1 starboard +# @output MAIN3 motor 2 port +# @output MAIN4 rudder motor + +sh /etc/init.d/rc.lta_defaults + +param set PWM_MAX 2000 +param set PWM_MIN 1000 + +param set MPC_MANTHR_MIN 0.00 +param set MPC_THR_MIN 0.00 +param set MPC_THR_CURVE 1 +param set MPC_THR_HOVER 0.0 + +param set NAV_DLL_ACT 0 +param set NAV_RCL_ACT 0 + +param set MC_ACRO_Y_MAX 360.0 +param set MPC_MAN_Y_MAX 360.0 + +param set LNDMC_Z_VEL_MAX 0.01 +param set LNDMC_XY_VEL_MAX 0.01 +param set LNDMC_LOW_T_THR 0.01 +param set LNDMC_ROT_MAX 1.0 + +set MIXER cloudship + +# Configure this as airship. +# set MAV_TYPE 7 + +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/rc.lta_apps b/ROMFS/px4fmu_common/init.d/rc.lta_apps new file mode 100644 index 000000000000..f314ffdb6b63 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.lta_apps @@ -0,0 +1,64 @@ +#!/bin/sh +# +# Standard apps for airships. Attitude/Position estimator, Attitude/Position control. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +############################################################################### +# Begin Estimator Group Selection # +############################################################################### + +# +# LPE +# +if param compare SYS_MC_EST_GROUP 1 +then + # + # Try to start LPE. If it fails, start EKF2 as a default. + # Unfortunately we do not build it on px4_fmu-v2 due to a limited flash. + # + if attitude_estimator_q start + then + echo "WARN [init] Estimator LPE unsupported, EKF2 recommended." + local_position_estimator start + else + echo "ERROR [init] Estimator LPE not available. Using EKF2" + param set SYS_MC_EST_GROUP 2 + param save + reboot + fi +else + # + # Q estimator (attitude estimation only) + # + if param compare SYS_MC_EST_GROUP 3 + then + attitude_estimator_q start + else + # + # EKF2 + # + param set SYS_MC_EST_GROUP 2 + ekf2 start + fi +fi + +############################################################################### +# End Estimator Group Selection # +############################################################################### + +# +# Start Airship Attitude Controller. +# +lta_att_control start + +# +# Start Position Controller. +# +mc_pos_control start + +# +# Start Land Detector. +# +land_detector start multicopter diff --git a/ROMFS/px4fmu_common/init.d/rc.lta_defaults b/ROMFS/px4fmu_common/init.d/rc.lta_defaults new file mode 100644 index 000000000000..ce9bc68b705c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.lta_defaults @@ -0,0 +1,28 @@ +#!/bin/sh +# +# Airship default parameters. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +set VEHICLE_TYPE lta + +if [ $AUTOCNF = yes ] +then + param set NAV_ACC_RAD 2 + + param set RTL_RETURN_ALT 30 + param set RTL_DESCEND_ALT 10 + param set RTL_LAND_DELAY 1 + + param set PWM_MAX 2000 + param set PWM_MIN 1000 + param set PWM_RATE 400 +fi + +# +# This is the gimbal pass mixer. +# +set MIXER_AUX pass + +set PWM_AUX_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index b6002edc8730..d5076d6288e7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -147,6 +147,32 @@ then sh /etc/init.d/rc.vtol_apps fi +# +# Airship setup. +# +if [ $VEHICLE_TYPE = lta ] +then + if [ $MIXER = none ] + then + echo "LTA mixer undefined" + fi + + if [ $MAV_TYPE = none ] + then + # Set a default MAV_TYPE = 2 if not defined. + set MAV_TYPE 2 + fi + + # Set the mav type parameter. + param set MAV_TYPE ${MAV_TYPE} + + # Load mixer and configure outputs. + sh /etc/init.d/rc.interface + + # Start standard airship apps. + sh /etc/init.d/rc.lta_apps +fi + # # UUV setup # diff --git a/ROMFS/px4fmu_common/mixers/cloudship.main.mix b/ROMFS/px4fmu_common/mixers/cloudship.main.mix new file mode 100644 index 000000000000..ec79c65e9762 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/cloudship.main.mix @@ -0,0 +1,51 @@ +Stabilator/thruster/rudder mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling an airship with +stabilator, rudder rotor, starboard and port thruster using PX4FMU. +The configuration assumes the stabilator servo is connected to PX4FMU servo +output 0, starboard thruster to output 1, port thruster to output 2, and the +rudder rotor to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps). + +Servo controlling stabilator mixer +------------ +Two scalers total (output, thrust angle). + +This mixer assumes that the stabilator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +S: 0 1 10000 10000 0 -10000 10000 + +Starboard and port thruster mixer +----------------- +Two scalers total (output, thrust). + +By default mixer output is normalized. In the case of certain input controllers +the commented lines generate a full-range output (-1 to 1) from an input in the +(0 - 1) range. Inputs below zero are treated as zero. + +M: 1 +S: 0 3 20000 20000 -10000 -10000 10000 +#S: 0 3 10000 10000 0 -10000 10000 + +M: 1 +S: 0 3 20000 20000 -10000 -10000 10000 +#S: 0 3 10000 10000 0 -10000 10000 + +Rudder rotor mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder rotor is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the motor movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +S: 0 2 10000 10000 0 -10000 10000 diff --git a/Tools/px4airframes/srcparser.py b/Tools/px4airframes/srcparser.py index 771a28de519c..1efdb0244e8e 100644 --- a/Tools/px4airframes/srcparser.py +++ b/Tools/px4airframes/srcparser.py @@ -90,6 +90,8 @@ def GetImageName(self): return "YPlus" elif (self.name == "Autogyro"): return "Autogyro" + elif (self.name == "Airship"): + return "Airship" elif (self.name == "Rover"): return "Rover" elif (self.name == "Boat"): diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 8f90549c2490..22a12343f98a 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -35,6 +35,7 @@ px4_add_board( #load_mon local_position_estimator logger + lta_att_control mavlink mc_att_control mc_hover_thrust_estimator diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 1b6e3818dac8..be98be817e0d 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -52,6 +52,7 @@ uint8 VEHICLE_TYPE_UNKNOWN = 0 uint8 VEHICLE_TYPE_ROTARY_WING = 1 uint8 VEHICLE_TYPE_FIXED_WING = 2 uint8 VEHICLE_TYPE_ROVER = 3 +uint8 VEHICLE_TYPE_LTA = 4 # state machine / state of vehicle. # Encodes the complete system state and is set by the commander app. diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index 78de84497f88..53f20020b240 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -79,7 +79,7 @@ set(models none shell if750a iris iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps px4vision solo typhoon_h480 plane plane_cam plane_catapult plane_lidar standard_vtol tailsitter tiltrotor - rover r1_rover boat + rover r1_rover boat cloudship uuv_hippocampus) set(worlds none empty baylands ksql_airport mcmillan_airfield sonoma_raceway warehouse) set(all_posix_vmd_make_targets) diff --git a/src/modules/lta_att_control/CMakeLists.txt b/src/modules/lta_att_control/CMakeLists.txt new file mode 100644 index 000000000000..eaf810693260 --- /dev/null +++ b/src/modules/lta_att_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE modules__lta_att_control + MAIN lta_att_control + STACK_MAX 3500 + COMPILE_FLAGS + SRCS + lta_att_control_main.cpp + DEPENDS + circuit_breaker + conversion + mathlib + px4_work_queue + ) diff --git a/src/modules/lta_att_control/lta_att_control.hpp b/src/modules/lta_att_control/lta_att_control.hpp new file mode 100644 index 000000000000..488f45616ced --- /dev/null +++ b/src/modules/lta_att_control/lta_att_control.hpp @@ -0,0 +1,196 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2018 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. + * + ****************************************************************************/ + +#include // Airmode +#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 + +/** + * Multicopter attitude control app start / stop handling function + */ +extern "C" __EXPORT int lta_att_control_main(int argc, char *argv[]); + +class LTAAttitudeControl : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + LTAAttitudeControl(); + + virtual ~LTAAttitudeControl(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void Run() override; + + bool init(); + +private: + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + /** + * Check for parameter update and handle it. + */ + void parameter_update_poll(); + bool vehicle_attitude_poll(); + void vehicle_motor_limits_poll(); + void vehicle_status_poll(); + + void publish_actuator_controls(); + void publish_rates_setpoint(); + void publish_rate_controller_status(); + + /** + * Generate & publish an attitude setpoint from stick inputs + */ + void generate_attitude_setpoint(float dt, bool reset_yaw_sp); + + /** + * Get the landing gear state based on the manual control switch position + * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN + */ + float get_landing_gear_state(); + + + /** + * Attitude controller. + */ + void control_attitude(); + + /** + * Attitude rates controller. + */ + void control_attitude_rates(float dt, const matrix::Vector3f &rates); + + uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude subscription */ + uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */ + uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */ + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ + uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; + uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + + orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ + orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; + + orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ + orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */ + + bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ + + 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 actuator_controls_s _actuators {}; /**< actuator controls */ + struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ + struct battery_status_s _battery_status {}; /**< battery status */ + struct vehicle_land_detected_s _vehicle_land_detected {}; + struct landing_gear_s _landing_gear {}; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */ + float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */ + + matrix::Vector3f _rates_sp; /**< angular rates setpoint */ + + matrix::Vector3f _att_control; /**< attitude control vector */ + float _thrust_sp{0.0f}; /**< thrust setpoint */ + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ + + hrt_abstime _task_start{hrt_absolute_time()}; + hrt_abstime _last_run{0}; + float _dt_accumulator{0.0f}; + int _loop_counter{0}; + + bool _reset_yaw_sp{true}; + float _attitude_dt{0.0f}; + + bool _is_tailsitter{false}; + + matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ + float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + +}; diff --git a/src/modules/lta_att_control/lta_att_control_main.cpp b/src/modules/lta_att_control/lta_att_control_main.cpp new file mode 100644 index 000000000000..e95a8311b174 --- /dev/null +++ b/src/modules/lta_att_control/lta_att_control_main.cpp @@ -0,0 +1,407 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 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 lta_att_control_main.cpp + * Multicopter attitude controller. + * + * @author Lorenz Meier + * @author Anton Babushkin + * @author Sander Smeets + * @author Matthias Grob + * @author Beat Küng + * @author Daniel Robinson + */ + +#include "lta_att_control.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +LTAAttitudeControl::LTAAttitudeControl() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, "lta_att_control")) +{ + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + /* initialize quaternions in messages to be valid */ + _v_att.q[0] = 1.f; + _v_att_sp.q_d[0] = 1.f; + + _rates_sp.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + parameters_updated(); +} + +LTAAttitudeControl::~LTAAttitudeControl() +{ + perf_free(_loop_perf); +} + +bool +LTAAttitudeControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle_angular_velocity callback registration failed!"); + return false; + } + + return true; +} + +void +LTAAttitudeControl::parameters_updated() +{ + +} + +void +LTAAttitudeControl::parameter_update_poll() +{ + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } +} + +void +LTAAttitudeControl::vehicle_status_poll() +{ + /* check if there is new status information */ + if (_vehicle_status_sub.update(&_vehicle_status)) { + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (_actuators_id == nullptr) { + if (_vehicle_status.is_vtol) { + _actuators_id = ORB_ID(actuator_controls_virtual_mc); + _attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint); + + int32_t vt_type = -1; + + if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { + _is_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); + } + + } else { + _actuators_id = ORB_ID(actuator_controls_0); + _attitude_sp_id = ORB_ID(vehicle_attitude_setpoint); + } + } + } +} + +bool +LTAAttitudeControl::vehicle_attitude_poll() +{ + /* check if there is a new message */ + const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; + + if (_v_att_sub.update(&_v_att)) { + // Check for a heading reset + if (prev_quat_reset_counter != _v_att.quat_reset_counter) { + // we only extract the heading change from the delta quaternion + _man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi(); + } + + return true; + } + + return false; +} + +float +LTAAttitudeControl::get_landing_gear_state() +{ + // Only switch the landing gear up if we are not landed and if + // the user switched from gear down to gear up. + // If the user had the switch in the gear up position and took off ignore it + // until he toggles the switch to avoid retracting the gear immediately on takeoff. + if (_vehicle_land_detected.landed) { + _gear_state_initialized = false; + } + + float landing_gear = landing_gear_s::GEAR_DOWN; // default to down + + if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) { + landing_gear = landing_gear_s::GEAR_UP; + + } else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + // Switching the gear off does put it into a safe defined state + _gear_state_initialized = true; + } + + return landing_gear; +} + +void +LTAAttitudeControl::publish_rates_setpoint() +{ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = -_rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust_body[0] = 0.0f; + _v_rates_sp.thrust_body[1] = 0.0f; + _v_rates_sp.thrust_body[2] = -_thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + _v_rates_sp_pub.publish(_v_rates_sp); +} + +void +LTAAttitudeControl::publish_rate_controller_status() +{ + rate_ctrl_status_s rate_ctrl_status = {}; + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); +} + +void +LTAAttitudeControl::publish_actuator_controls() +{ + _actuators.control[0] = 0.0f; + _actuators.control[1] = _manual_control_sp.x; + _actuators.control[2] = _manual_control_sp.r; + _actuators.control[3] = _manual_control_sp.z; + _actuators.control[7] = (float)_landing_gear.landing_gear; + // note: _actuators.timestamp_sample is set in LTAAttitudeControl::Run() + _actuators.timestamp = hrt_absolute_time(); + + if (!_actuators_0_circuit_breaker_enabled) { + orb_publish_auto(_actuators_id, &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT); + } +} + +void +LTAAttitudeControl::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + const hrt_abstime now = hrt_absolute_time(); + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); + _last_run = now; + + const Vector3f rates{angular_velocity.xyz}; + + _actuators.timestamp_sample = angular_velocity.timestamp_sample; + + /* run the rate controller immediately after a gyro update */ + if (_v_control_mode.flag_control_rates_enabled) { + publish_actuator_controls(); + publish_rate_controller_status(); + } + + /* check for updates in other topics */ + _v_control_mode_sub.update(&_v_control_mode); + _battery_status_sub.update(&_battery_status); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); + _landing_gear_sub.update(&_landing_gear); + vehicle_status_poll(); + const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); + const bool attitude_updated = vehicle_attitude_poll(); + + _attitude_dt += dt; + + bool attitude_setpoint_generated = _v_control_mode.flag_control_altitude_enabled + || _v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_position_enabled; + + if (attitude_setpoint_generated) { + if (attitude_updated) { + + if (_v_control_mode.flag_control_yawrate_override_enabled) { + /* Yaw rate override enabled, overwrite the yaw setpoint */ + _v_rates_sp_sub.update(&_v_rates_sp); + const auto yawrate_reference = _v_rates_sp.yaw; + _rates_sp(2) = yawrate_reference; + } + + publish_rates_setpoint(); + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_control_mode.flag_control_manual_enabled) { + if (manual_control_updated) { + /* manual control - feed RC commands to actuators directly */ + _v_att_sp.thrust_body[0] = _manual_control_sp.x; + _v_att_sp.thrust_body[2] = _manual_control_sp.z; + _rates_sp(2) = _manual_control_sp.r; + + // PX4_INFO("\nManual X: %.2f\nManual Z: %.2f\nManual R: %.2f\n", + // (double)_manual_control_sp.x, (double)_manual_control_sp.z, + // (double)_manual_control_sp.r); + + publish_rates_setpoint(); + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_rates_sp_sub.update(&_v_rates_sp)) { + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = -_v_rates_sp.thrust_body[2]; + } + } + } + + + if (_v_control_mode.flag_control_termination_enabled) { + if (!_vehicle_status.is_vtol) { + _rates_sp.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + publish_actuator_controls(); + } + } + + /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ + if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) { + _dt_accumulator += dt; + ++_loop_counter; + + if (_dt_accumulator > 1.f) { + const float loop_update_rate = (float)_loop_counter / _dt_accumulator; + _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; + _dt_accumulator = 0; + _loop_counter = 0; + } + } + + parameter_update_poll(); + } + + perf_end(_loop_perf); +} + +int LTAAttitudeControl::task_spawn(int argc, char *argv[]) +{ + LTAAttitudeControl *instance = new LTAAttitudeControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int LTAAttitudeControl::print_status() +{ + PX4_INFO("Running"); + + perf_print_counter(_loop_perf); + + print_message(_actuators); + + return 0; +} + +int LTAAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int LTAAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the multicopter attitude and rate controller. It takes attitude +setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode +via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. + +The controller has two loops: a P loop for angular error and a PID loop for angular rate error. + +Publication documenting the implemented Quaternion Attitude Control: +Nonlinear Quadrocopter Attitude Control (2013) +by Dario Brescianini, Markus Hehn and Raffaello D'Andrea +Institute for Dynamic Systems and Control (IDSC), ETH Zurich + +https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf + +### Implementation +To reduce control latency, the module directly polls on the gyro topic published by the IMU driver. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("lta_att_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int lta_att_control_main(int argc, char *argv[]) +{ + return LTAAttitudeControl::main(argc, argv); +} From 64bd18499f9a24610173b9721e787dc75f451ec8 Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Fri, 8 May 2020 00:16:30 +0200 Subject: [PATCH 11/24] mixer update --- ROMFS/px4fmu_common/mixers/cloudship.main.mix | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/cloudship.main.mix b/ROMFS/px4fmu_common/mixers/cloudship.main.mix index ec79c65e9762..7c92bccdff18 100644 --- a/ROMFS/px4fmu_common/mixers/cloudship.main.mix +++ b/ROMFS/px4fmu_common/mixers/cloudship.main.mix @@ -31,12 +31,12 @@ the commented lines generate a full-range output (-1 to 1) from an input in the (0 - 1) range. Inputs below zero are treated as zero. M: 1 -S: 0 3 20000 20000 -10000 -10000 10000 -#S: 0 3 10000 10000 0 -10000 10000 +S: 0 3 10000 10000 0 -10000 10000 +#S: 0 3 20000 20000 -10000 -10000 10000 M: 1 -S: 0 3 20000 20000 -10000 -10000 10000 -#S: 0 3 10000 10000 0 -10000 10000 +S: 0 3 10000 10000 0 -10000 10000 +#S: 0 3 20000 20000 -10000 -10000 10000 Rudder rotor mixer ------------ From 8cbebee10f6abc93ce7ae72dc0311e336e0ed3fb Mon Sep 17 00:00:00 2001 From: Daniel Leonard Robinson Date: Sat, 9 May 2020 09:20:50 +0200 Subject: [PATCH 12/24] revert i2c_spi_buses.h --- platforms/common/include/px4_platform_common/i2c_spi_buses.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index 2af0c32464e4..df4c86a3db75 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -241,7 +241,8 @@ class I2CSPIDriver : public I2CSPIDriverBase { static_cast(this)->RunImpl(); - if (should_exit()) { + if (should_exit()) + { exit_and_cleanup(); } } From e46ca854f7f79d5b6368d03eb804aa562febdc90 Mon Sep 17 00:00:00 2001 From: Daniel Leonard Robinson Date: Sat, 9 May 2020 09:21:52 +0200 Subject: [PATCH 13/24] space --- platforms/common/include/px4_platform_common/i2c_spi_buses.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index df4c86a3db75..f711da1fd5e8 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -241,7 +241,7 @@ class I2CSPIDriver : public I2CSPIDriverBase { static_cast(this)->RunImpl(); - if (should_exit()) + if (should_exit()) { exit_and_cleanup(); } From 6ad017273fc108984b1da53e312cb8809090edfe Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Sat, 9 May 2020 09:52:58 +0200 Subject: [PATCH 14/24] renamed lta -> airship --- .../px4fmu_common/init.d-posix/2507_cloudship | 2 +- .../init.d/airframes/2507_cloudship | 2 +- .../init.d/{rc.lta_apps => rc.airship_apps} | 2 +- .../{rc.lta_defaults => rc.airship_defaults} | 2 +- ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 8 ++-- boards/px4/sitl/default.cmake | 2 +- msg/vehicle_status.msg | 2 +- .../CMakeLists.txt | 6 +-- .../airship_att_control.hpp} | 8 ++-- .../airship_att_control_main.cpp} | 48 +++++++++---------- 10 files changed, 41 insertions(+), 41 deletions(-) rename ROMFS/px4fmu_common/init.d/{rc.lta_apps => rc.airship_apps} (98%) rename ROMFS/px4fmu_common/init.d/{rc.lta_defaults => rc.airship_defaults} (94%) rename src/modules/{lta_att_control => airship_att_control}/CMakeLists.txt (95%) rename src/modules/{lta_att_control/lta_att_control.hpp => airship_att_control/airship_att_control.hpp} (97%) rename src/modules/{lta_att_control/lta_att_control_main.cpp => airship_att_control/airship_att_control_main.cpp} (89%) diff --git a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship index c5d5948ee718..01b44d8e6868 100644 --- a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship @@ -9,7 +9,7 @@ # @output MAIN3 motor 2 port # @output MAIN4 rudder motor -sh /etc/init.d/rc.lta_defaults +sh /etc/init.d/rc.airship_defaults param set PWM_MAX 2000 param set PWM_MIN 1000 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship index 3fd79288d8e0..08910fe28a4b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -9,7 +9,7 @@ # @output MAIN3 motor 2 port # @output MAIN4 rudder motor -sh /etc/init.d/rc.lta_defaults +sh /etc/init.d/rc.airship_defaults param set PWM_MAX 2000 param set PWM_MIN 1000 diff --git a/ROMFS/px4fmu_common/init.d/rc.lta_apps b/ROMFS/px4fmu_common/init.d/rc.airship_apps similarity index 98% rename from ROMFS/px4fmu_common/init.d/rc.lta_apps rename to ROMFS/px4fmu_common/init.d/rc.airship_apps index f314ffdb6b63..c5fbc1bd926f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.lta_apps +++ b/ROMFS/px4fmu_common/init.d/rc.airship_apps @@ -51,7 +51,7 @@ fi # # Start Airship Attitude Controller. # -lta_att_control start +airship_att_control start # # Start Position Controller. diff --git a/ROMFS/px4fmu_common/init.d/rc.lta_defaults b/ROMFS/px4fmu_common/init.d/rc.airship_defaults similarity index 94% rename from ROMFS/px4fmu_common/init.d/rc.lta_defaults rename to ROMFS/px4fmu_common/init.d/rc.airship_defaults index ce9bc68b705c..fbff47cfc25a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.lta_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.airship_defaults @@ -5,7 +5,7 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -set VEHICLE_TYPE lta +set VEHICLE_TYPE airship if [ $AUTOCNF = yes ] then diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index d5076d6288e7..5267854f361a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -150,11 +150,11 @@ fi # # Airship setup. # -if [ $VEHICLE_TYPE = lta ] +if [ $VEHICLE_TYPE = airship ] then if [ $MIXER = none ] then - echo "LTA mixer undefined" + echo "Airship mixer undefined" fi if [ $MAV_TYPE = none ] @@ -169,8 +169,8 @@ then # Load mixer and configure outputs. sh /etc/init.d/rc.interface - # Start standard airship apps. - sh /etc/init.d/rc.lta_apps + # Start airship apps. + sh /etc/init.d/rc.airship_apps fi # diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 22a12343f98a..698cec12d7cc 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -21,6 +21,7 @@ px4_add_board( tone_alarm #uavcan MODULES + airship_att_control airspeed_selector attitude_estimator_q camera_feedback @@ -35,7 +36,6 @@ px4_add_board( #load_mon local_position_estimator logger - lta_att_control mavlink mc_att_control mc_hover_thrust_estimator diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index be98be817e0d..eb99764f8a88 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -52,7 +52,7 @@ uint8 VEHICLE_TYPE_UNKNOWN = 0 uint8 VEHICLE_TYPE_ROTARY_WING = 1 uint8 VEHICLE_TYPE_FIXED_WING = 2 uint8 VEHICLE_TYPE_ROVER = 3 -uint8 VEHICLE_TYPE_LTA = 4 +uint8 VEHICLE_TYPE_AIRSHIP = 4 # state machine / state of vehicle. # Encodes the complete system state and is set by the commander app. diff --git a/src/modules/lta_att_control/CMakeLists.txt b/src/modules/airship_att_control/CMakeLists.txt similarity index 95% rename from src/modules/lta_att_control/CMakeLists.txt rename to src/modules/airship_att_control/CMakeLists.txt index eaf810693260..ecb00da58caa 100644 --- a/src/modules/lta_att_control/CMakeLists.txt +++ b/src/modules/airship_att_control/CMakeLists.txt @@ -32,12 +32,12 @@ ############################################################################ px4_add_module( - MODULE modules__lta_att_control - MAIN lta_att_control + MODULE modules__airship_att_control + MAIN airship_att_control STACK_MAX 3500 COMPILE_FLAGS SRCS - lta_att_control_main.cpp + airship_att_control_main.cpp DEPENDS circuit_breaker conversion diff --git a/src/modules/lta_att_control/lta_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp similarity index 97% rename from src/modules/lta_att_control/lta_att_control.hpp rename to src/modules/airship_att_control/airship_att_control.hpp index 488f45616ced..466942652ebc 100644 --- a/src/modules/lta_att_control/lta_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -63,15 +63,15 @@ /** * Multicopter attitude control app start / stop handling function */ -extern "C" __EXPORT int lta_att_control_main(int argc, char *argv[]); +extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]); -class LTAAttitudeControl : public ModuleBase, public ModuleParams, +class AirshipAttitudeControl : public ModuleBase, public ModuleParams, public px4::WorkItem { public: - LTAAttitudeControl(); + AirshipAttitudeControl(); - virtual ~LTAAttitudeControl(); + virtual ~AirshipAttitudeControl(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); diff --git a/src/modules/lta_att_control/lta_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp similarity index 89% rename from src/modules/lta_att_control/lta_att_control_main.cpp rename to src/modules/airship_att_control/airship_att_control_main.cpp index e95a8311b174..55065a9bf70c 100644 --- a/src/modules/lta_att_control/lta_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file lta_att_control_main.cpp + * @file airship_att_control_main.cpp * Multicopter attitude controller. * * @author Lorenz Meier @@ -43,7 +43,7 @@ * @author Daniel Robinson */ -#include "lta_att_control.hpp" +#include "airship_att_control.hpp" #include #include @@ -54,10 +54,10 @@ using namespace matrix; -LTAAttitudeControl::LTAAttitudeControl() : +AirshipAttitudeControl::AirshipAttitudeControl() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _loop_perf(perf_alloc(PC_ELAPSED, "lta_att_control")) + _loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control")) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; @@ -72,13 +72,13 @@ LTAAttitudeControl::LTAAttitudeControl() : parameters_updated(); } -LTAAttitudeControl::~LTAAttitudeControl() +AirshipAttitudeControl::~AirshipAttitudeControl() { perf_free(_loop_perf); } bool -LTAAttitudeControl::init() +AirshipAttitudeControl::init() { if (!_vehicle_angular_velocity_sub.registerCallback()) { PX4_ERR("vehicle_angular_velocity callback registration failed!"); @@ -89,13 +89,13 @@ LTAAttitudeControl::init() } void -LTAAttitudeControl::parameters_updated() +AirshipAttitudeControl::parameters_updated() { } void -LTAAttitudeControl::parameter_update_poll() +AirshipAttitudeControl::parameter_update_poll() { // check for parameter updates if (_parameter_update_sub.updated()) { @@ -109,7 +109,7 @@ LTAAttitudeControl::parameter_update_poll() } void -LTAAttitudeControl::vehicle_status_poll() +AirshipAttitudeControl::vehicle_status_poll() { /* check if there is new status information */ if (_vehicle_status_sub.update(&_vehicle_status)) { @@ -134,7 +134,7 @@ LTAAttitudeControl::vehicle_status_poll() } bool -LTAAttitudeControl::vehicle_attitude_poll() +AirshipAttitudeControl::vehicle_attitude_poll() { /* check if there is a new message */ const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; @@ -153,7 +153,7 @@ LTAAttitudeControl::vehicle_attitude_poll() } float -LTAAttitudeControl::get_landing_gear_state() +AirshipAttitudeControl::get_landing_gear_state() { // Only switch the landing gear up if we are not landed and if // the user switched from gear down to gear up. @@ -177,7 +177,7 @@ LTAAttitudeControl::get_landing_gear_state() } void -LTAAttitudeControl::publish_rates_setpoint() +AirshipAttitudeControl::publish_rates_setpoint() { _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = -_rates_sp(1); @@ -191,7 +191,7 @@ LTAAttitudeControl::publish_rates_setpoint() } void -LTAAttitudeControl::publish_rate_controller_status() +AirshipAttitudeControl::publish_rate_controller_status() { rate_ctrl_status_s rate_ctrl_status = {}; rate_ctrl_status.timestamp = hrt_absolute_time(); @@ -199,14 +199,14 @@ LTAAttitudeControl::publish_rate_controller_status() } void -LTAAttitudeControl::publish_actuator_controls() +AirshipAttitudeControl::publish_actuator_controls() { _actuators.control[0] = 0.0f; _actuators.control[1] = _manual_control_sp.x; _actuators.control[2] = _manual_control_sp.r; _actuators.control[3] = _manual_control_sp.z; _actuators.control[7] = (float)_landing_gear.landing_gear; - // note: _actuators.timestamp_sample is set in LTAAttitudeControl::Run() + // note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run() _actuators.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { @@ -215,7 +215,7 @@ LTAAttitudeControl::publish_actuator_controls() } void -LTAAttitudeControl::Run() +AirshipAttitudeControl::Run() { if (should_exit()) { _vehicle_angular_velocity_sub.unregisterCallback(); @@ -328,9 +328,9 @@ LTAAttitudeControl::Run() perf_end(_loop_perf); } -int LTAAttitudeControl::task_spawn(int argc, char *argv[]) +int AirshipAttitudeControl::task_spawn(int argc, char *argv[]) { - LTAAttitudeControl *instance = new LTAAttitudeControl(); + AirshipAttitudeControl *instance = new AirshipAttitudeControl(); if (instance) { _object.store(instance); @@ -351,7 +351,7 @@ int LTAAttitudeControl::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -int LTAAttitudeControl::print_status() +int AirshipAttitudeControl::print_status() { PX4_INFO("Running"); @@ -362,12 +362,12 @@ int LTAAttitudeControl::print_status() return 0; } -int LTAAttitudeControl::custom_command(int argc, char *argv[]) +int AirshipAttitudeControl::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int LTAAttitudeControl::print_usage(const char *reason) +int AirshipAttitudeControl::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); @@ -394,14 +394,14 @@ To reduce control latency, the module directly polls on the gyro topic published )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("lta_att_control", "controller"); + PRINT_MODULE_USAGE_NAME("airship_att_control", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } -int lta_att_control_main(int argc, char *argv[]) +int airship_att_control_main(int argc, char *argv[]) { - return LTAAttitudeControl::main(argc, argv); + return AirshipAttitudeControl::main(argc, argv); } From b60a32d209d6a82ce6038bbc86938b1ab0c4373b Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Mon, 1 Jun 2020 13:57:46 +0200 Subject: [PATCH 15/24] edits, cleaned up --- .../px4fmu_common/init.d-posix/2507_cloudship | 28 +++------ .../init.d/airframes/2507_cloudship | 30 ++++------ .../px4fmu_common/init.d/rc.airship_defaults | 6 -- .../airship_att_control.hpp | 54 +---------------- .../airship_att_control_main.cpp | 60 ++----------------- 5 files changed, 24 insertions(+), 154 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship index 01b44d8e6868..7c72795fb0e9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship @@ -11,26 +11,16 @@ sh /etc/init.d/rc.airship_defaults -param set PWM_MAX 2000 -param set PWM_MIN 1000 +if [ $AUTOCNF = yes ] +then + param set MPC_MAN_Y_MAX 360.0 -param set MPC_MANTHR_MIN 0.00 -param set MPC_THR_MIN 0.00 -param set MPC_THR_CURVE 1 -param set MPC_THR_HOVER 0.0 + # do not trigger the land detecter unless in hanger + param set LNDMC_Z_VEL_MAX 0.01 + param set LNDMC_XY_VEL_MAX 0.01 + param set LNDMC_LOW_T_THR 0.01 + param set LNDMC_ROT_MAX 1.0 -param set NAV_DLL_ACT 0 -param set NAV_RCL_ACT 0 - -param set MC_ACRO_Y_MAX 360.0 -param set MPC_MAN_Y_MAX 360.0 - -param set LNDMC_Z_VEL_MAX 0.01 -param set LNDMC_XY_VEL_MAX 0.01 -param set LNDMC_LOW_T_THR 0.01 -param set LNDMC_ROT_MAX 1.0 +fi set MIXER cloudship - -# Configure this as airship. -# set MAV_TYPE 7 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship index 08910fe28a4b..00c58fae33e2 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -11,28 +11,18 @@ sh /etc/init.d/rc.airship_defaults -param set PWM_MAX 2000 -param set PWM_MIN 1000 +if [ $AUTOCNF = yes ] +then + param set MPC_MAN_Y_MAX 360.0 -param set MPC_MANTHR_MIN 0.00 -param set MPC_THR_MIN 0.00 -param set MPC_THR_CURVE 1 -param set MPC_THR_HOVER 0.0 + # do not trigger the land detecter unless in hanger + param set LNDMC_Z_VEL_MAX 0.01 + param set LNDMC_XY_VEL_MAX 0.01 + param set LNDMC_LOW_T_THR 0.01 + param set LNDMC_ROT_MAX 1.0 -param set NAV_DLL_ACT 0 -param set NAV_RCL_ACT 0 - -param set MC_ACRO_Y_MAX 360.0 -param set MPC_MAN_Y_MAX 360.0 - -param set LNDMC_Z_VEL_MAX 0.01 -param set LNDMC_XY_VEL_MAX 0.01 -param set LNDMC_LOW_T_THR 0.01 -param set LNDMC_ROT_MAX 1.0 +fi set MIXER cloudship - -# Configure this as airship. -# set MAV_TYPE 7 - set PWM_OUT 1234 + diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_defaults b/ROMFS/px4fmu_common/init.d/rc.airship_defaults index fbff47cfc25a..3e522058353c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.airship_defaults @@ -9,12 +9,6 @@ set VEHICLE_TYPE airship if [ $AUTOCNF = yes ] then - param set NAV_ACC_RAD 2 - - param set RTL_RETURN_ALT 30 - param set RTL_DESCEND_ALT 10 - param set RTL_LAND_DELAY 1 - param set PWM_MAX 2000 param set PWM_MIN 1000 param set PWM_RATE 400 diff --git a/src/modules/airship_att_control/airship_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp index 466942652ebc..8bdc22742cfd 100644 --- a/src/modules/airship_att_control/airship_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -32,22 +32,13 @@ ****************************************************************************/ #include // Airmode -#include -#include -#include -#include #include #include -#include -#include -#include #include #include #include #include -#include #include -#include #include #include #include @@ -57,11 +48,9 @@ #include #include #include -#include -#include /** - * Multicopter attitude control app start / stop handling function + * Airship attitude control app start / stop handling function */ extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]); @@ -101,35 +90,12 @@ class AirshipAttitudeControl : public ModuleBase, public */ void parameter_update_poll(); bool vehicle_attitude_poll(); - void vehicle_motor_limits_poll(); void vehicle_status_poll(); void publish_actuator_controls(); void publish_rates_setpoint(); void publish_rate_controller_status(); - /** - * Generate & publish an attitude setpoint from stick inputs - */ - void generate_attitude_setpoint(float dt, bool reset_yaw_sp); - - /** - * Get the landing gear state based on the manual control switch position - * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN - */ - float get_landing_gear_state(); - - - /** - * Attitude controller. - */ - void control_attitude(); - - /** - * Attitude rates controller. - */ - void control_attitude_rates(float dt, const matrix::Vector3f &rates); - uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude subscription */ uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */ uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ @@ -137,19 +103,14 @@ class AirshipAttitudeControl : public ModuleBase, public uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */ - uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ - uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ - uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ - orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */ @@ -163,9 +124,7 @@ class AirshipAttitudeControl : public ModuleBase, public struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */ struct actuator_controls_s _actuators {}; /**< actuator controls */ struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ - struct battery_status_s _battery_status {}; /**< battery status */ struct vehicle_land_detected_s _vehicle_land_detected {}; - struct landing_gear_s _landing_gear {}; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -174,23 +133,12 @@ class AirshipAttitudeControl : public ModuleBase, public matrix::Vector3f _rates_sp; /**< angular rates setpoint */ - matrix::Vector3f _att_control; /**< attitude control vector */ float _thrust_sp{0.0f}; /**< thrust setpoint */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ hrt_abstime _task_start{hrt_absolute_time()}; hrt_abstime _last_run{0}; float _dt_accumulator{0.0f}; int _loop_counter{0}; - bool _reset_yaw_sp{true}; - float _attitude_dt{0.0f}; - - bool _is_tailsitter{false}; - - matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ - float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ - }; diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index 55065a9bf70c..34aafdb7f233 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -67,9 +67,6 @@ AirshipAttitudeControl::AirshipAttitudeControl() : _rates_sp.zero(); _thrust_sp = 0.0f; - _att_control.zero(); - - parameters_updated(); } AirshipAttitudeControl::~AirshipAttitudeControl() @@ -88,12 +85,6 @@ AirshipAttitudeControl::init() return true; } -void -AirshipAttitudeControl::parameters_updated() -{ - -} - void AirshipAttitudeControl::parameter_update_poll() { @@ -119,12 +110,6 @@ AirshipAttitudeControl::vehicle_status_poll() _actuators_id = ORB_ID(actuator_controls_virtual_mc); _attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint); - int32_t vt_type = -1; - - if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { - _is_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); - } - } else { _actuators_id = ORB_ID(actuator_controls_0); _attitude_sp_id = ORB_ID(vehicle_attitude_setpoint); @@ -152,30 +137,6 @@ AirshipAttitudeControl::vehicle_attitude_poll() return false; } -float -AirshipAttitudeControl::get_landing_gear_state() -{ - // Only switch the landing gear up if we are not landed and if - // the user switched from gear down to gear up. - // If the user had the switch in the gear up position and took off ignore it - // until he toggles the switch to avoid retracting the gear immediately on takeoff. - if (_vehicle_land_detected.landed) { - _gear_state_initialized = false; - } - - float landing_gear = landing_gear_s::GEAR_DOWN; // default to down - - if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) { - landing_gear = landing_gear_s::GEAR_UP; - - } else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { - // Switching the gear off does put it into a safe defined state - _gear_state_initialized = true; - } - - return landing_gear; -} - void AirshipAttitudeControl::publish_rates_setpoint() { @@ -205,7 +166,7 @@ AirshipAttitudeControl::publish_actuator_controls() _actuators.control[1] = _manual_control_sp.x; _actuators.control[2] = _manual_control_sp.r; _actuators.control[3] = _manual_control_sp.z; - _actuators.control[7] = (float)_landing_gear.landing_gear; + // note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run() _actuators.timestamp = hrt_absolute_time(); @@ -247,15 +208,11 @@ AirshipAttitudeControl::Run() /* check for updates in other topics */ _v_control_mode_sub.update(&_v_control_mode); - _battery_status_sub.update(&_battery_status); _vehicle_land_detected_sub.update(&_vehicle_land_detected); - _landing_gear_sub.update(&_landing_gear); vehicle_status_poll(); const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); const bool attitude_updated = vehicle_attitude_poll(); - _attitude_dt += dt; - bool attitude_setpoint_generated = _v_control_mode.flag_control_altitude_enabled || _v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_position_enabled; @@ -299,12 +256,10 @@ AirshipAttitudeControl::Run() } } - if (_v_control_mode.flag_control_termination_enabled) { if (!_vehicle_status.is_vtol) { _rates_sp.zero(); _thrust_sp = 0.0f; - _att_control.zero(); publish_actuator_controls(); } } @@ -376,18 +331,11 @@ int AirshipAttitudeControl::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This implements the multicopter attitude and rate controller. It takes attitude -setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode +This implements the airship attitude and rate controller. Ideally it would +take attitude setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. -The controller has two loops: a P loop for angular error and a PID loop for angular rate error. - -Publication documenting the implemented Quaternion Attitude Control: -Nonlinear Quadrocopter Attitude Control (2013) -by Dario Brescianini, Markus Hehn and Raffaello D'Andrea -Institute for Dynamic Systems and Control (IDSC), ETH Zurich - -https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf +Currently it is feeding the `manual_control_setpoint` topic directly to the actuators. ### Implementation To reduce control latency, the module directly polls on the gyro topic published by the IMU driver. From 08129ab7e46af04c0da344b97958937ea4337293 Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Thu, 4 Jun 2020 15:44:52 +0200 Subject: [PATCH 16/24] pr: further edits - airship_att_control - params in rc.airship_defaults --- .../px4fmu_common/init.d/rc.airship_defaults | 7 ----- .../airship_att_control.hpp | 10 +------ .../airship_att_control_main.cpp | 29 ++----------------- 3 files changed, 3 insertions(+), 43 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_defaults b/ROMFS/px4fmu_common/init.d/rc.airship_defaults index 3e522058353c..90e4de12be8e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.airship_defaults @@ -7,13 +7,6 @@ set VEHICLE_TYPE airship -if [ $AUTOCNF = yes ] -then - param set PWM_MAX 2000 - param set PWM_MIN 1000 - param set PWM_RATE 400 -fi - # # This is the gimbal pass mixer. # diff --git a/src/modules/airship_att_control/airship_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp index 8bdc22742cfd..07a920295320 100644 --- a/src/modules/airship_att_control/airship_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -128,17 +128,9 @@ class AirshipAttitudeControl : public ModuleBase, public perf_counter_t _loop_perf; /**< loop performance counter */ - static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */ - float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */ - matrix::Vector3f _rates_sp; /**< angular rates setpoint */ float _thrust_sp{0.0f}; /**< thrust setpoint */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - - hrt_abstime _task_start{hrt_absolute_time()}; - hrt_abstime _last_run{0}; - float _dt_accumulator{0.0f}; - int _loop_counter{0}; + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ }; diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index 34aafdb7f233..1fae69bcb671 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -33,25 +33,18 @@ /** * @file airship_att_control_main.cpp - * Multicopter attitude controller. + * Airship attitude controller. * * @author Lorenz Meier * @author Anton Babushkin * @author Sander Smeets * @author Matthias Grob * @author Beat Küng - * @author Daniel Robinson + * @author Daniel Robinson */ #include "airship_att_control.hpp" -#include -#include -#include -#include -#include -#include - using namespace matrix; AirshipAttitudeControl::AirshipAttitudeControl() : @@ -190,11 +183,6 @@ AirshipAttitudeControl::Run() vehicle_angular_velocity_s angular_velocity; if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { - const hrt_abstime now = hrt_absolute_time(); - - // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); - _last_run = now; const Vector3f rates{angular_velocity.xyz}; @@ -264,19 +252,6 @@ AirshipAttitudeControl::Run() } } - /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ - if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) { - _dt_accumulator += dt; - ++_loop_counter; - - if (_dt_accumulator > 1.f) { - const float loop_update_rate = (float)_loop_counter / _dt_accumulator; - _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; - _dt_accumulator = 0; - _loop_counter = 0; - } - } - parameter_update_poll(); } From 06677f760450b8ed22e402bb2e77de0da5d3da92 Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Sun, 7 Jun 2020 13:34:45 +0200 Subject: [PATCH 17/24] revert submodules {sitl_gazebo, mavlink, nuttx, ecl} --- Tools/sitl_gazebo | 2 +- mavlink/include/mavlink/v2.0 | 2 +- platforms/nuttx/NuttX/nuttx | 2 +- src/lib/ecl | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index f046038b14d1..20e8dbe11502 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit f046038b14d1e8a2bd3860bc24ea3d8fc7bbb870 +Subproject commit 20e8dbe115028721b1918069dc7afd9145c7483c diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index c6e18fb3389e..5f1d5d8338ff 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit c6e18fb3389eb29ee9993829e775a903e003d92a +Subproject commit 5f1d5d8338ff15108bd11137fc6970282f369246 diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 66b4f2c4f212..f5925e194e82 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 66b4f2c4f2128994f8e8a908d4888f6d37565cfd +Subproject commit f5925e194e82b0658b0176a9657820a396674642 diff --git a/src/lib/ecl b/src/lib/ecl index 12835b999e75..b3dc06d0cb57 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 12835b999e756ef631111c4fdd0ac1c72d68694d +Subproject commit b3dc06d0cb572931000705ac04807eeae5e2401a From b4c6675340afb87ff3ce4493224b8f7f00085c61 Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Sun, 7 Jun 2020 14:17:23 +0200 Subject: [PATCH 18/24] CI successful, astyle check --- platforms/common/include/px4_platform_common/i2c_spi_buses.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index f711da1fd5e8..2af0c32464e4 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -241,8 +241,7 @@ class I2CSPIDriver : public I2CSPIDriverBase { static_cast(this)->RunImpl(); - if (should_exit()) - { + if (should_exit()) { exit_and_cleanup(); } } From 669e227f208ec8fd053b556328c98a9fd78aea68 Mon Sep 17 00:00:00 2001 From: Daniel Robinson Date: Tue, 9 Jun 2020 10:10:54 +0200 Subject: [PATCH 19/24] uorb graph fix --- .../airship_att_control/airship_att_control.hpp | 4 ---- .../airship_att_control/airship_att_control_main.cpp | 12 +----------- 2 files changed, 1 insertion(+), 15 deletions(-) diff --git a/src/modules/airship_att_control/airship_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp index 07a920295320..e87a55bf1f7f 100644 --- a/src/modules/airship_att_control/airship_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -111,10 +111,6 @@ class AirshipAttitudeControl : public ModuleBase, public uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ - - orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ - orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */ - bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ struct vehicle_attitude_s _v_att {}; /**< vehicle attitude */ diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index 1fae69bcb671..f9b29b8f1bb1 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -97,17 +97,7 @@ AirshipAttitudeControl::vehicle_status_poll() { /* check if there is new status information */ if (_vehicle_status_sub.update(&_vehicle_status)) { - /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (_actuators_id == nullptr) { - if (_vehicle_status.is_vtol) { - _actuators_id = ORB_ID(actuator_controls_virtual_mc); - _attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint); - } else { - _actuators_id = ORB_ID(actuator_controls_0); - _attitude_sp_id = ORB_ID(vehicle_attitude_setpoint); - } - } } } @@ -164,7 +154,7 @@ AirshipAttitudeControl::publish_actuator_controls() _actuators.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { - orb_publish_auto(_actuators_id, &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT); + orb_publish_auto(ORB_ID(actuator_controls_0), &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT); } } From 384662e182051f03c65a8d81fdd0ee267fbe2413 Mon Sep 17 00:00:00 2001 From: Anton Erasmus Date: Sat, 20 Jun 2020 06:12:08 +0200 Subject: [PATCH 20/24] Change actuator scales for simulator, refactor airship code --- .../px4fmu_common/init.d-posix/2507_cloudship | 21 +-- .../init.d/airframes/2507_cloudship | 20 +-- ROMFS/px4fmu_common/init.d/rc.airship_apps | 10 -- ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 4 +- ROMFS/px4fmu_common/mixers/cloudship.main.mix | 48 +++---- .../airship_att_control/CMakeLists.txt | 3 - .../airship_att_control.hpp | 38 ----- .../airship_att_control_main.cpp | 135 +----------------- src/modules/simulator/simulator_mavlink.cpp | 19 +++ 9 files changed, 56 insertions(+), 242 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship index 7c72795fb0e9..29b71a3e5952 100644 --- a/ROMFS/px4fmu_common/init.d-posix/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship @@ -4,23 +4,12 @@ # @type Airship # @class Airship # -# @output MAIN1 stabilator -# @output MAIN2 motor 1 starboard -# @output MAIN3 motor 2 port -# @output MAIN4 rudder motor +# @output MAIN1 thrust tilt +# @output MAIN2 starboard thruster +# @output MAIN3 port thruster +# @output MAIN4 tail thruster sh /etc/init.d/rc.airship_defaults -if [ $AUTOCNF = yes ] -then - param set MPC_MAN_Y_MAX 360.0 - - # do not trigger the land detecter unless in hanger - param set LNDMC_Z_VEL_MAX 0.01 - param set LNDMC_XY_VEL_MAX 0.01 - param set LNDMC_LOW_T_THR 0.01 - param set LNDMC_ROT_MAX 1.0 - -fi - set MIXER cloudship +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship index 00c58fae33e2..aa6bd7e389d1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -4,25 +4,13 @@ # @type Airship # @class Airship # -# @output MAIN1 stabilator -# @output MAIN2 motor 1 starboard -# @output MAIN3 motor 2 port -# @output MAIN4 rudder motor +# @output MAIN1 thrust tilt +# @output MAIN2 starboard thruster +# @output MAIN3 port thruster +# @output MAIN4 tail thruster sh /etc/init.d/rc.airship_defaults -if [ $AUTOCNF = yes ] -then - param set MPC_MAN_Y_MAX 360.0 - - # do not trigger the land detecter unless in hanger - param set LNDMC_Z_VEL_MAX 0.01 - param set LNDMC_XY_VEL_MAX 0.01 - param set LNDMC_LOW_T_THR 0.01 - param set LNDMC_ROT_MAX 1.0 - -fi - set MIXER cloudship set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_apps b/ROMFS/px4fmu_common/init.d/rc.airship_apps index c5fbc1bd926f..4ef1a9810270 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_apps +++ b/ROMFS/px4fmu_common/init.d/rc.airship_apps @@ -52,13 +52,3 @@ fi # Start Airship Attitude Controller. # airship_att_control start - -# -# Start Position Controller. -# -mc_pos_control start - -# -# Start Land Detector. -# -land_detector start multicopter diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 5267854f361a..c493402bab5e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -159,8 +159,8 @@ then if [ $MAV_TYPE = none ] then - # Set a default MAV_TYPE = 2 if not defined. - set MAV_TYPE 2 + # Set a default MAV_TYPE = 7 if not defined. + set MAV_TYPE 7 fi # Set the mav type parameter. diff --git a/ROMFS/px4fmu_common/mixers/cloudship.main.mix b/ROMFS/px4fmu_common/mixers/cloudship.main.mix index 7c92bccdff18..6fb91d90e40b 100644 --- a/ROMFS/px4fmu_common/mixers/cloudship.main.mix +++ b/ROMFS/px4fmu_common/mixers/cloudship.main.mix @@ -1,48 +1,44 @@ -Stabilator/thruster/rudder mixer for PX4FMU +Thrust tilt/ Starboard Thrust / Port Thrust / Tail Thrust mixer for PX4FMU ======================================================= This file defines mixers suitable for controlling an airship with -stabilator, rudder rotor, starboard and port thruster using PX4FMU. -The configuration assumes the stabilator servo is connected to PX4FMU servo -output 0, starboard thruster to output 1, port thruster to output 2, and the -rudder rotor to output 3. +a thrust tilt, starboard and port thruster and a tail thruster using PX4FMU. +The configuration assumes the starboard thruster is connected to PX4FMU +output 0, port thruster to output 1, tilt servo to output 2, and the +tail thruster to output 3. -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps). - -Servo controlling stabilator mixer ------------- -Two scalers total (output, thrust angle). - -This mixer assumes that the stabilator servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -M: 1 -S: 0 1 10000 10000 0 -10000 10000 +Inputs to the mixer come from channel group 0 (vehicle attitude), +channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust). Starboard and port thruster mixer ----------------- Two scalers total (output, thrust). -By default mixer output is normalized. In the case of certain input controllers -the commented lines generate a full-range output (-1 to 1) from an input in the -(0 - 1) range. Inputs below zero are treated as zero. +By default mixer output is normalized. The input is in the (0 - 1) range. M: 1 S: 0 3 10000 10000 0 -10000 10000 -#S: 0 3 20000 20000 -10000 -10000 10000 M: 1 S: 0 3 10000 10000 0 -10000 10000 -#S: 0 3 20000 20000 -10000 -10000 10000 -Rudder rotor mixer +Servo controlling tilt mixer +------------ +Two scalers total (output, tilt angle). + +This mixer assumes that the tilt servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +S: 0 1 10000 10000 0 -10000 10000 + +Tail thruster mixer ------------ Two scalers total (output, yaw). -This mixer assumes that the rudder rotor is set up correctly mechanically; +This mixer assumes that the tail thruster is set up correctly mechanically; depending on the actual configuration it may be necessary to reverse the scaling factors (to reverse the motor movement) and adjust the offset, scaling and endpoints to suit. diff --git a/src/modules/airship_att_control/CMakeLists.txt b/src/modules/airship_att_control/CMakeLists.txt index ecb00da58caa..9771e2ad516a 100644 --- a/src/modules/airship_att_control/CMakeLists.txt +++ b/src/modules/airship_att_control/CMakeLists.txt @@ -39,8 +39,5 @@ px4_add_module( SRCS airship_att_control_main.cpp DEPENDS - circuit_breaker - conversion - mathlib px4_work_queue ) diff --git a/src/modules/airship_att_control/airship_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp index e87a55bf1f7f..5b00bd98ca63 100644 --- a/src/modules/airship_att_control/airship_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -31,7 +31,6 @@ * ****************************************************************************/ -#include // Airmode #include #include #include @@ -40,14 +39,7 @@ #include #include #include -#include #include -#include -#include -#include -#include -#include -#include /** * Airship attitude control app start / stop handling function @@ -80,53 +72,23 @@ class AirshipAttitudeControl : public ModuleBase, public private: - /** - * initialize some vectors/matrices from parameters - */ - void parameters_updated(); - /** * Check for parameter update and handle it. */ void parameter_update_poll(); - bool vehicle_attitude_poll(); - void vehicle_status_poll(); void publish_actuator_controls(); - void publish_rates_setpoint(); - void publish_rate_controller_status(); - uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude subscription */ - uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */ - uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ - uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; - uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ - uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ - orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ - bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ - 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 actuator_controls_s _actuators {}; /**< actuator controls */ - struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ - struct vehicle_land_detected_s _vehicle_land_detected {}; perf_counter_t _loop_perf; /**< loop performance counter */ - matrix::Vector3f _rates_sp; /**< angular rates setpoint */ - - float _thrust_sp{0.0f}; /**< thrust setpoint */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - }; diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index f9b29b8f1bb1..819ba8fd5b99 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -35,12 +35,7 @@ * @file airship_att_control_main.cpp * Airship attitude controller. * - * @author Lorenz Meier - * @author Anton Babushkin - * @author Sander Smeets - * @author Matthias Grob - * @author Beat Küng - * @author Daniel Robinson + * @author Anton Erasmus */ #include "airship_att_control.hpp" @@ -52,14 +47,6 @@ AirshipAttitudeControl::AirshipAttitudeControl() : WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control")) { - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - - /* initialize quaternions in messages to be valid */ - _v_att.q[0] = 1.f; - _v_att_sp.q_d[0] = 1.f; - - _rates_sp.zero(); - _thrust_sp = 0.0f; } AirshipAttitudeControl::~AirshipAttitudeControl() @@ -70,11 +57,6 @@ AirshipAttitudeControl::~AirshipAttitudeControl() bool AirshipAttitudeControl::init() { - if (!_vehicle_angular_velocity_sub.registerCallback()) { - PX4_ERR("vehicle_angular_velocity callback registration failed!"); - return false; - } - return true; } @@ -92,56 +74,6 @@ AirshipAttitudeControl::parameter_update_poll() } } -void -AirshipAttitudeControl::vehicle_status_poll() -{ - /* check if there is new status information */ - if (_vehicle_status_sub.update(&_vehicle_status)) { - - } -} - -bool -AirshipAttitudeControl::vehicle_attitude_poll() -{ - /* check if there is a new message */ - const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; - - if (_v_att_sub.update(&_v_att)) { - // Check for a heading reset - if (prev_quat_reset_counter != _v_att.quat_reset_counter) { - // we only extract the heading change from the delta quaternion - _man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi(); - } - - return true; - } - - return false; -} - -void -AirshipAttitudeControl::publish_rates_setpoint() -{ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = -_rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust_body[0] = 0.0f; - _v_rates_sp.thrust_body[1] = 0.0f; - _v_rates_sp.thrust_body[2] = -_thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); - - _v_rates_sp_pub.publish(_v_rates_sp); -} - -void -AirshipAttitudeControl::publish_rate_controller_status() -{ - rate_ctrl_status_s rate_ctrl_status = {}; - rate_ctrl_status.timestamp = hrt_absolute_time(); - _controller_status_pub.publish(rate_ctrl_status); -} - void AirshipAttitudeControl::publish_actuator_controls() { @@ -153,16 +85,13 @@ AirshipAttitudeControl::publish_actuator_controls() // note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run() _actuators.timestamp = hrt_absolute_time(); - if (!_actuators_0_circuit_breaker_enabled) { - orb_publish_auto(ORB_ID(actuator_controls_0), &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT); - } + orb_publish_auto(ORB_ID(actuator_controls_0), &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT); } void AirshipAttitudeControl::Run() { if (should_exit()) { - _vehicle_angular_velocity_sub.unregisterCallback(); exit_and_cleanup(); return; } @@ -181,66 +110,10 @@ AirshipAttitudeControl::Run() /* run the rate controller immediately after a gyro update */ if (_v_control_mode.flag_control_rates_enabled) { publish_actuator_controls(); - publish_rate_controller_status(); } - /* check for updates in other topics */ - _v_control_mode_sub.update(&_v_control_mode); - _vehicle_land_detected_sub.update(&_vehicle_land_detected); - vehicle_status_poll(); - const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); - const bool attitude_updated = vehicle_attitude_poll(); - - bool attitude_setpoint_generated = _v_control_mode.flag_control_altitude_enabled - || _v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_position_enabled; - - if (attitude_setpoint_generated) { - if (attitude_updated) { - - if (_v_control_mode.flag_control_yawrate_override_enabled) { - /* Yaw rate override enabled, overwrite the yaw setpoint */ - _v_rates_sp_sub.update(&_v_rates_sp); - const auto yawrate_reference = _v_rates_sp.yaw; - _rates_sp(2) = yawrate_reference; - } - - publish_rates_setpoint(); - } - - } else { - /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled) { - if (manual_control_updated) { - /* manual control - feed RC commands to actuators directly */ - _v_att_sp.thrust_body[0] = _manual_control_sp.x; - _v_att_sp.thrust_body[2] = _manual_control_sp.z; - _rates_sp(2) = _manual_control_sp.r; - - // PX4_INFO("\nManual X: %.2f\nManual Z: %.2f\nManual R: %.2f\n", - // (double)_manual_control_sp.x, (double)_manual_control_sp.z, - // (double)_manual_control_sp.r); - - publish_rates_setpoint(); - } - - } else { - /* attitude controller disabled, poll rates setpoint topic */ - if (_v_rates_sp_sub.update(&_v_rates_sp)) { - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = -_v_rates_sp.thrust_body[2]; - } - } - } - - if (_v_control_mode.flag_control_termination_enabled) { - if (!_vehicle_status.is_vtol) { - _rates_sp.zero(); - _thrust_sp = 0.0f; - publish_actuator_controls(); - } - } + /* check for updates in manual control topic */ + _manual_control_sp_sub.update(&_manual_control_sp); parameter_update_poll(); } diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 6cb59f37b12c..c922d0e54c6b 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -143,6 +143,25 @@ mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs() } } + } else if (_system_type == MAV_TYPE_AIRSHIP) { + /* airship: scale starboard and port throttle to 0..1 and other channels (tilt, tail thruster) to -1..1 */ + for (unsigned i = 0; i < 16; i++) { + if (armed) { + if (i < 2) { + /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */ + msg.controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + + } else { + /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */ + msg.controls[i] = (_actuator_outputs.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); + } + + } else { + /* set 0 for disabled channels */ + msg.controls[i] = 0.0f; + } + } + } else { /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ From 1e53880da20028d1bf4cf9dad5bd21882fa564bc Mon Sep 17 00:00:00 2001 From: Anton Erasmus Date: Sun, 28 Jun 2020 07:32:31 +0200 Subject: [PATCH 21/24] Add airship land detector and handle airship arming state --- .../init.d/airframes/2507_cloudship | 13 ++-- ROMFS/px4fmu_common/init.d/rc.airship_apps | 5 ++ .../px4fmu_common/init.d/rc.airship_defaults | 1 - ROMFS/px4fmu_common/mixers/cloudship.main.mix | 8 +-- .../airship_att_control.hpp | 7 +- .../airship_att_control_main.cpp | 36 +++++++--- .../land_detector/AirshipLandDetector.cpp | 65 ++++++++++++++++++ .../land_detector/AirshipLandDetector.h | 67 +++++++++++++++++++ src/modules/land_detector/CMakeLists.txt | 1 + .../land_detector/land_detector_main.cpp | 3 + 10 files changed, 186 insertions(+), 20 deletions(-) create mode 100644 src/modules/land_detector/AirshipLandDetector.cpp create mode 100644 src/modules/land_detector/AirshipLandDetector.h diff --git a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship index aa6bd7e389d1..fd676a5bb7a3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -4,13 +4,18 @@ # @type Airship # @class Airship # -# @output MAIN1 thrust tilt -# @output MAIN2 starboard thruster -# @output MAIN3 port thruster +# @output MAIN1 starboard thruster +# @output MAIN2 port thruster +# @output MAIN3 thrust tilt # @output MAIN4 tail thruster sh /etc/init.d/rc.airship_defaults +if [ $AUTOCNF = yes ] +then + param set COM_PREARM_MODE 2 + param set CBRK_IO_SAFETY 22027 +fi + set MIXER cloudship set PWM_OUT 1234 - diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_apps b/ROMFS/px4fmu_common/init.d/rc.airship_apps index 4ef1a9810270..447d9609acc8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_apps +++ b/ROMFS/px4fmu_common/init.d/rc.airship_apps @@ -52,3 +52,8 @@ fi # Start Airship Attitude Controller. # airship_att_control start + +# +# Start Land Detector. +# +land_detector start airship diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_defaults b/ROMFS/px4fmu_common/init.d/rc.airship_defaults index 90e4de12be8e..25ac61e21213 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.airship_defaults @@ -11,5 +11,4 @@ set VEHICLE_TYPE airship # This is the gimbal pass mixer. # set MIXER_AUX pass - set PWM_AUX_OUT 1234 diff --git a/ROMFS/px4fmu_common/mixers/cloudship.main.mix b/ROMFS/px4fmu_common/mixers/cloudship.main.mix index 6fb91d90e40b..5932dd1d9874 100644 --- a/ROMFS/px4fmu_common/mixers/cloudship.main.mix +++ b/ROMFS/px4fmu_common/mixers/cloudship.main.mix @@ -4,8 +4,8 @@ Thrust tilt/ Starboard Thrust / Port Thrust / Tail Thrust mixer for PX4FMU This file defines mixers suitable for controlling an airship with a thrust tilt, starboard and port thruster and a tail thruster using PX4FMU. The configuration assumes the starboard thruster is connected to PX4FMU -output 0, port thruster to output 1, tilt servo to output 2, and the -tail thruster to output 3. +output 1, port thruster to output 2, tilt servo to output 3, and the +tail thruster to output 4. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust). @@ -17,10 +17,10 @@ Two scalers total (output, thrust). By default mixer output is normalized. The input is in the (0 - 1) range. M: 1 -S: 0 3 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 M: 1 -S: 0 3 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 Servo controlling tilt mixer ------------ diff --git a/src/modules/airship_att_control/airship_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp index 5b00bd98ca63..326785cca243 100644 --- a/src/modules/airship_att_control/airship_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -33,9 +33,10 @@ #include #include -#include +#include #include #include +#include #include #include #include @@ -80,13 +81,15 @@ class AirshipAttitudeControl : public ModuleBase, public void publish_actuator_controls(); uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; - orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ + uORB::Publication _actuators_0_pub; struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */ + struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ struct actuator_controls_s _actuators {}; /**< actuator controls */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index 819ba8fd5b99..23a37eb86b39 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -45,6 +45,7 @@ using namespace matrix; AirshipAttitudeControl::AirshipAttitudeControl() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _actuators_0_pub(ORB_ID(actuator_controls_0)), _loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control")) { } @@ -57,6 +58,10 @@ AirshipAttitudeControl::~AirshipAttitudeControl() bool AirshipAttitudeControl::init() { + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle_angular_velocity callback registration failed!"); + return false; + } return true; } @@ -77,21 +82,29 @@ AirshipAttitudeControl::parameter_update_poll() void AirshipAttitudeControl::publish_actuator_controls() { - _actuators.control[0] = 0.0f; - _actuators.control[1] = _manual_control_sp.x; - _actuators.control[2] = _manual_control_sp.r; - _actuators.control[3] = _manual_control_sp.z; + // zero actuators if not armed + if(_vehicle_status.arming_state != 2) { + for(uint8_t i = 0 ; i < 4 ; i++) { + _actuators.control[i] = 0.0f; + } + } else { + _actuators.control[0] = 0.0f; + _actuators.control[1] = _manual_control_sp.x; + _actuators.control[2] = _manual_control_sp.r; + _actuators.control[3] = _manual_control_sp.z; + } // note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run() _actuators.timestamp = hrt_absolute_time(); - orb_publish_auto(ORB_ID(actuator_controls_0), &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT); + _actuators_0_pub.publish(_actuators); } void AirshipAttitudeControl::Run() { if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); exit_and_cleanup(); return; } @@ -108,12 +121,17 @@ AirshipAttitudeControl::Run() _actuators.timestamp_sample = angular_velocity.timestamp_sample; /* run the rate controller immediately after a gyro update */ - if (_v_control_mode.flag_control_rates_enabled) { - publish_actuator_controls(); - } + publish_actuator_controls(); /* check for updates in manual control topic */ - _manual_control_sp_sub.update(&_manual_control_sp); + if (_manual_control_sp_sub.updated()) { + _manual_control_sp_sub.update(&_manual_control_sp); + } + + /* check for updates in vehicle status topic */ + if (_vehicle_status_sub.updated()) { + _vehicle_status_sub.update(&_vehicle_status); + } parameter_update_poll(); } diff --git a/src/modules/land_detector/AirshipLandDetector.cpp b/src/modules/land_detector/AirshipLandDetector.cpp new file mode 100644 index 000000000000..8aee01d0417b --- /dev/null +++ b/src/modules/land_detector/AirshipLandDetector.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2017 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 AirshipLandDetector.cpp + * Land detection algorithm for airships + * + * @author Anton Erasmus + */ + +#include "AirshipLandDetector.h" + +namespace land_detector +{ + + +bool AirshipLandDetector::_get_ground_contact_state() +{ + return false; +} + +bool AirshipLandDetector::_get_landed_state() +{ + + _vehicle_status_sub.update(&_vehicle_status); + + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { + return true; // If Landing has been requested then say we have landed. + + } else { + return !_actuator_armed.armed; // If we are armed we are not landed. + } +} + +} // namespace land_detector diff --git a/src/modules/land_detector/AirshipLandDetector.h b/src/modules/land_detector/AirshipLandDetector.h new file mode 100644 index 000000000000..23efb3649678 --- /dev/null +++ b/src/modules/land_detector/AirshipLandDetector.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2017 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 AirshipLandDetector.h + * Land detection implementation for airships. + * + * @author Anton Erasmus + */ + +#pragma once + +#include "LandDetector.h" +#include + +namespace land_detector +{ + +class AirshipLandDetector : public LandDetector +{ +public: + AirshipLandDetector() = default; + ~AirshipLandDetector() override = default; + +protected: + bool _get_ground_contact_state() override; + bool _get_landed_state() override; + + +private: + // Program crashes when Subscriptor declared here + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + vehicle_status_s _vehicle_status{}; /**< vehicle status */ + +}; + +} // namespace land_detector diff --git a/src/modules/land_detector/CMakeLists.txt b/src/modules/land_detector/CMakeLists.txt index 2ae0d3a4758c..632c90d5cf8d 100644 --- a/src/modules/land_detector/CMakeLists.txt +++ b/src/modules/land_detector/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( FixedwingLandDetector.cpp VtolLandDetector.cpp RoverLandDetector.cpp + AirshipLandDetector.cpp DEPENDS hysteresis ) diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 752af2c47927..e641cf196624 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -49,6 +49,7 @@ #include "MulticopterLandDetector.h" #include "RoverLandDetector.h" #include "VtolLandDetector.h" +#include "AirshipLandDetector.h" namespace land_detector @@ -79,6 +80,8 @@ int LandDetector::task_spawn(int argc, char *argv[]) } else if (strcmp(argv[1], "rover") == 0) { obj = new RoverLandDetector(); + } else if (strcmp(argv[1], "airship") == 0) { + obj = new AirshipLandDetector(); } else { print_usage("unknown mode"); return PX4_ERROR; From 9b9832e038dea97cbd7c6af5d8852a3caf20ef73 Mon Sep 17 00:00:00 2001 From: Anton Erasmus Date: Sun, 28 Jun 2020 11:32:25 +0200 Subject: [PATCH 22/24] Add documentation for airship land detector --- .../airship_att_control/airship_att_control_main.cpp | 6 ++++-- src/modules/land_detector/land_detector_main.cpp | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index 23a37eb86b39..36f7f536b30b 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -62,6 +62,7 @@ AirshipAttitudeControl::init() PX4_ERR("vehicle_angular_velocity callback registration failed!"); return false; } + return true; } @@ -83,10 +84,11 @@ void AirshipAttitudeControl::publish_actuator_controls() { // zero actuators if not armed - if(_vehicle_status.arming_state != 2) { - for(uint8_t i = 0 ; i < 4 ; i++) { + if (_vehicle_status.arming_state != 2) { + for (uint8_t i = 0 ; i < 4 ; i++) { _actuators.control[i] = 0.0f; } + } else { _actuators.control[0] = 0.0f; _actuators.control[1] = _manual_control_sp.x; diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index e641cf196624..c4ece04320f6 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -82,6 +82,7 @@ int LandDetector::task_spawn(int argc, char *argv[]) } else if (strcmp(argv[1], "airship") == 0) { obj = new AirshipLandDetector(); + } else { print_usage("unknown mode"); return PX4_ERROR; @@ -143,7 +144,7 @@ The module runs periodically on the HP work queue. PRINT_MODULE_USAGE_NAME("land_detector", "system"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); - PRINT_MODULE_USAGE_ARG("fixedwing|multicopter|vtol|rover", "Select vehicle type", false); + PRINT_MODULE_USAGE_ARG("fixedwing|multicopter|vtol|rover|airship", "Select vehicle type", false); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } From fcaa8b360c7b1a5fb21982965bbcc04070f8380b Mon Sep 17 00:00:00 2001 From: Anton Erasmus Date: Fri, 7 Aug 2020 22:34:03 +0200 Subject: [PATCH 23/24] Update state definition for readability MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Beat Küng --- src/modules/airship_att_control/airship_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index 36f7f536b30b..db4f81338c3b 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -84,7 +84,7 @@ void AirshipAttitudeControl::publish_actuator_controls() { // zero actuators if not armed - if (_vehicle_status.arming_state != 2) { + if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { for (uint8_t i = 0 ; i < 4 ; i++) { _actuators.control[i] = 0.0f; } From ab7cbb38c7ec7467cbf3ad76d376d21f0b61c754 Mon Sep 17 00:00:00 2001 From: Anton Erasmus Date: Sat, 8 Aug 2020 10:50:09 +0200 Subject: [PATCH 24/24] Update land detector arm variables --- src/modules/land_detector/AirshipLandDetector.cpp | 5 +---- src/modules/land_detector/AirshipLandDetector.h | 7 ------- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/src/modules/land_detector/AirshipLandDetector.cpp b/src/modules/land_detector/AirshipLandDetector.cpp index 8aee01d0417b..9a7ee8653f6e 100644 --- a/src/modules/land_detector/AirshipLandDetector.cpp +++ b/src/modules/land_detector/AirshipLandDetector.cpp @@ -43,7 +43,6 @@ namespace land_detector { - bool AirshipLandDetector::_get_ground_contact_state() { return false; @@ -52,13 +51,11 @@ bool AirshipLandDetector::_get_ground_contact_state() bool AirshipLandDetector::_get_landed_state() { - _vehicle_status_sub.update(&_vehicle_status); - if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { return true; // If Landing has been requested then say we have landed. } else { - return !_actuator_armed.armed; // If we are armed we are not landed. + return !_armed; // If we are armed we are not landed. } } diff --git a/src/modules/land_detector/AirshipLandDetector.h b/src/modules/land_detector/AirshipLandDetector.h index 23efb3649678..8b218fcca8d3 100644 --- a/src/modules/land_detector/AirshipLandDetector.h +++ b/src/modules/land_detector/AirshipLandDetector.h @@ -41,7 +41,6 @@ #pragma once #include "LandDetector.h" -#include namespace land_detector { @@ -56,12 +55,6 @@ class AirshipLandDetector : public LandDetector bool _get_ground_contact_state() override; bool _get_landed_state() override; - -private: - // Program crashes when Subscriptor declared here - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - vehicle_status_s _vehicle_status{}; /**< vehicle status */ - }; } // namespace land_detector