From fa4818e4679e037eb876036959c451d81af93127 Mon Sep 17 00:00:00 2001 From: Daniel Leonard Robinson Date: Mon, 10 Aug 2020 08:52:51 +0200 Subject: [PATCH] vehicles: add new vehicle type: Airship (#14862) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Anton Erasmus Co-authored-by: Beat Küng --- .../px4fmu_common/init.d-posix/2507_cloudship | 15 ++ .../init.d/airframes/2507_cloudship | 21 ++ ROMFS/px4fmu_common/init.d/rc.airship_apps | 59 +++++ .../px4fmu_common/init.d/rc.airship_defaults | 14 ++ ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 26 +++ ROMFS/px4fmu_common/mixers/cloudship.main.mix | 47 ++++ Tools/px4airframes/srcparser.py | 2 + boards/px4/sitl/default.cmake | 1 + msg/vehicle_status.msg | 1 + platforms/posix/cmake/sitl_target.cmake | 2 +- .../airship_att_control/CMakeLists.txt | 43 ++++ .../airship_att_control.hpp | 97 ++++++++ .../airship_att_control_main.cpp | 213 ++++++++++++++++++ .../land_detector/AirshipLandDetector.cpp | 62 +++++ .../land_detector/AirshipLandDetector.h | 60 +++++ src/modules/land_detector/CMakeLists.txt | 1 + .../land_detector/land_detector_main.cpp | 6 +- src/modules/simulator/simulator_mavlink.cpp | 19 ++ 18 files changed, 687 insertions(+), 2 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.airship_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.airship_defaults create mode 100644 ROMFS/px4fmu_common/mixers/cloudship.main.mix create mode 100644 src/modules/airship_att_control/CMakeLists.txt create mode 100644 src/modules/airship_att_control/airship_att_control.hpp create mode 100644 src/modules/airship_att_control/airship_att_control_main.cpp 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-posix/2507_cloudship b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship new file mode 100644 index 0000000000..29b71a3e59 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/2507_cloudship @@ -0,0 +1,15 @@ +#!/bin/sh +# +# @name Cloudship +# @type Airship +# @class Airship +# +# @output MAIN1 thrust tilt +# @output MAIN2 starboard thruster +# @output MAIN3 port thruster +# @output MAIN4 tail thruster + +sh /etc/init.d/rc.airship_defaults + +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 new file mode 100644 index 0000000000..fd676a5bb7 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -0,0 +1,21 @@ +#!/bin/sh +# +# @name Cloudship +# @type Airship +# @class Airship +# +# @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 new file mode 100644 index 0000000000..447d9609ac --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.airship_apps @@ -0,0 +1,59 @@ +#!/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. +# +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 new file mode 100644 index 0000000000..25ac61e212 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.airship_defaults @@ -0,0 +1,14 @@ +#!/bin/sh +# +# Airship default parameters. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +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/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index b6002edc87..c493402bab 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 = airship ] +then + if [ $MIXER = none ] + then + echo "Airship mixer undefined" + fi + + if [ $MAV_TYPE = none ] + then + # Set a default MAV_TYPE = 7 if not defined. + set MAV_TYPE 7 + fi + + # Set the mav type parameter. + param set MAV_TYPE ${MAV_TYPE} + + # Load mixer and configure outputs. + sh /etc/init.d/rc.interface + + # Start airship apps. + sh /etc/init.d/rc.airship_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 0000000000..5932dd1d98 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/cloudship.main.mix @@ -0,0 +1,47 @@ +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 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). + +Starboard and port thruster mixer +----------------- +Two scalers total (output, thrust). + +By default mixer output is normalized. The input is in the (0 - 1) range. + +M: 1 +S: 0 3 0 20000 -10000 -10000 10000 + +M: 1 +S: 0 3 0 20000 -10000 -10000 10000 + +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 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. + +M: 1 +S: 0 2 10000 10000 0 -10000 10000 diff --git a/Tools/px4airframes/srcparser.py b/Tools/px4airframes/srcparser.py index 771a28de51..1efdb0244e 100644 --- a/Tools/px4airframes/srcparser.py +++ b/Tools/px4airframes/srcparser.py @@ -90,6 +90,8 @@ class ParameterGroup(object): 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 142b04c1b9..7e9dbfbc1c 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 diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 5077227650..85b45cf43e 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -53,6 +53,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_AIRSHIP = 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 105a242d2a..c4b27ddfc0 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -80,7 +80,7 @@ set(models none shell if750a iris iris_dual_gps 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 windy) set(all_posix_vmd_make_targets) diff --git a/src/modules/airship_att_control/CMakeLists.txt b/src/modules/airship_att_control/CMakeLists.txt new file mode 100644 index 0000000000..9771e2ad51 --- /dev/null +++ b/src/modules/airship_att_control/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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__airship_att_control + MAIN airship_att_control + STACK_MAX 3500 + COMPILE_FLAGS + SRCS + airship_att_control_main.cpp + DEPENDS + 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 new file mode 100644 index 0000000000..326785cca2 --- /dev/null +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Airship attitude control app start / stop handling function + */ +extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]); + +class AirshipAttitudeControl : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + AirshipAttitudeControl(); + + virtual ~AirshipAttitudeControl(); + + /** @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: + + /** + * Check for parameter update and handle it. + */ + void parameter_update_poll(); + + 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)}; + + 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 new file mode 100644 index 0000000000..db4f81338c --- /dev/null +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -0,0 +1,213 @@ +/**************************************************************************** + * + * 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 airship_att_control_main.cpp + * Airship attitude controller. + * + * @author Anton Erasmus + */ + +#include "airship_att_control.hpp" + +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")) +{ +} + +AirshipAttitudeControl::~AirshipAttitudeControl() +{ + perf_free(_loop_perf); +} + +bool +AirshipAttitudeControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle_angular_velocity callback registration failed!"); + return false; + } + + return true; +} + +void +AirshipAttitudeControl::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 +AirshipAttitudeControl::publish_actuator_controls() +{ + // zero actuators if not armed + if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + 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(); + + _actuators_0_pub.publish(_actuators); +} + +void +AirshipAttitudeControl::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 Vector3f rates{angular_velocity.xyz}; + + _actuators.timestamp_sample = angular_velocity.timestamp_sample; + + /* run the rate controller immediately after a gyro update */ + publish_actuator_controls(); + + /* check for updates in manual control topic */ + 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(); + } + + perf_end(_loop_perf); +} + +int AirshipAttitudeControl::task_spawn(int argc, char *argv[]) +{ + AirshipAttitudeControl *instance = new AirshipAttitudeControl(); + + 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 AirshipAttitudeControl::print_status() +{ + PX4_INFO("Running"); + + perf_print_counter(_loop_perf); + + print_message(_actuators); + + return 0; +} + +int AirshipAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int AirshipAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +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. + +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. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("airship_att_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int airship_att_control_main(int argc, char *argv[]) +{ + return AirshipAttitudeControl::main(argc, argv); +} diff --git a/src/modules/land_detector/AirshipLandDetector.cpp b/src/modules/land_detector/AirshipLandDetector.cpp new file mode 100644 index 0000000000..9a7ee8653f --- /dev/null +++ b/src/modules/land_detector/AirshipLandDetector.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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() +{ + + 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 !_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 0000000000..8b218fcca8 --- /dev/null +++ b/src/modules/land_detector/AirshipLandDetector.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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" + +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; + +}; + +} // namespace land_detector diff --git a/src/modules/land_detector/CMakeLists.txt b/src/modules/land_detector/CMakeLists.txt index 2ae0d3a475..632c90d5cf 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 0924519bdf..a64bdc9e00 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 @@ -77,6 +78,9 @@ 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; @@ -138,7 +142,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; } diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index d294506a8e..1ee497f459 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 */