diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 69b3ae4bd5..8e37de579b 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2023 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 @@ -31,11 +31,12 @@ # ############################################################################ +add_subdirectory(Arming) +add_subdirectory(failsafe) add_subdirectory(failure_detector) add_subdirectory(HealthAndArmingChecks) -add_subdirectory(failsafe) -add_subdirectory(Arming) add_subdirectory(ModeUtil) +add_subdirectory(MulticopterThrowLaunch) px4_add_module( MODULE modules__commander @@ -52,24 +53,25 @@ px4_add_module( factory_calibration_storage.cpp gyro_calibration.cpp HomePosition.cpp - UserModeIntention.cpp level_calibration.cpp lm_fit.cpp mag_calibration.cpp rc_calibration.cpp Safety.cpp + UserModeIntention.cpp worker_thread.cpp DEPENDS + ArmAuthorization circuit_breaker + failsafe failure_detector geo health_and_arming_checks hysteresis - ArmAuthorization + mode_util + MulticopterThrowLaunch sensor_calibration world_magnetic_model - mode_util - failsafe ) px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 890d385753..f5bdf73378 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1694,7 +1694,7 @@ void Commander::run() safetyButtonUpdate(); - throwLaunchUpdate(); + _multicopter_throw_launch.update(isArmed()); vtolStatusUpdate(); @@ -1776,7 +1776,7 @@ void Commander::run() _actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed(); _actuator_armed.lockdown = ((_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION) || (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) - || isThrowLaunchInProgress()); + || _multicopter_throw_launch.isThrowLaunchInProgress()); // _actuator_armed.manual_lockdown // action_request_s::ACTION_KILL _actuator_armed.force_failsafe = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION); // _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION @@ -2030,61 +2030,6 @@ void Commander::safetyButtonUpdate() } } -bool Commander::isThrowLaunchInProgress() const -{ - return !(_throw_launch_state == ThrowLaunchState::DISABLED || _throw_launch_state == ThrowLaunchState::FLYING); -} - -void Commander::throwLaunchUpdate() -{ - if (_param_com_throw_en.get()) { - if (_vehicle_local_position_sub.updated()) { - _vehicle_local_position_sub.copy(&_vehicle_local_position); - } - - if (!isArmed() && _throw_launch_state != ThrowLaunchState::IDLE) { - mavlink_log_info(&_mavlink_log_pub, "The vehicle is DISARMED with throw launch enabled. Do NOT throw it.\t"); - _throw_launch_state = ThrowLaunchState::IDLE; - } - - switch (_throw_launch_state) { - case ThrowLaunchState::IDLE: - if (isArmed()) { - mavlink_log_info(&_mavlink_log_pub, "The vehicle is ARMED with throw launch enabled. Throw the vehicle now.\t"); - _throw_launch_state = ThrowLaunchState::ARMED; - } - - break; - - case ThrowLaunchState::ARMED: - if (matrix::Vector3f( - _vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz - ).longerThan(_param_com_throw_min_speed.get())) { - PX4_INFO("Minimum throw speed exceeded; the motors will start when the vehicle starts falling down."); - _throw_launch_state = ThrowLaunchState::UNSAFE; - } - - break; - - case ThrowLaunchState::UNSAFE: - if (_vehicle_local_position.vz > 0) { - PX4_INFO("Throw successful, starting motors."); - _throw_launch_state = ThrowLaunchState::FLYING; - } - - break; - - default: - - break; - } - - } else if (_throw_launch_state != ThrowLaunchState::DISABLED) { - // make sure everything is reset when the throw launch is disabled - _throw_launch_state = ThrowLaunchState::DISABLED; - } -} - void Commander::vtolStatusUpdate() { // Make sure that this is only adjusted if vehicle really is of type vtol @@ -2143,7 +2088,7 @@ void Commander::updateTunes() } else if (_vehicle_status.failsafe && isArmed()) { tune_failsafe(true); - } else if (_throw_launch_state == ThrowLaunchState::ARMED) { + } else if (_multicopter_throw_launch.isReadyToThrow()) { set_tune(tune_control_s::TUNE_ID_ARMING_WARNING); } else { @@ -2202,7 +2147,7 @@ void Commander::handleAutoDisarm() _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); } - if (_auto_disarm_landed.get_state() && !isThrowLaunchInProgress()) { + if (_auto_disarm_landed.get_state() && !_multicopter_throw_launch.isThrowLaunchInProgress()) { if (_have_taken_off_since_arming) { disarm(arm_disarm_reason_t::auto_disarm_land); @@ -2222,7 +2167,7 @@ void Commander::handleAutoDisarm() } //don't disarm if throw launch is in progress - auto_disarm &= !isThrowLaunchInProgress(); + auto_disarm &= !_multicopter_throw_launch.isThrowLaunchInProgress(); _auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time()); @@ -2362,7 +2307,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_PURPLE; - } else if (_throw_launch_state == ThrowLaunchState::ARMED) { + } else if (_multicopter_throw_launch.isReadyToThrow()) { led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_YELLOW; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 2920889488..5ead8e8b27 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -34,13 +34,14 @@ #pragma once /* Helper classes */ -#include "failure_detector/FailureDetector.hpp" #include "failsafe/failsafe.h" -#include "Safety.hpp" -#include "worker_thread.hpp" +#include "failure_detector/FailureDetector.hpp" #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp" #include "HomePosition.hpp" +#include "MulticopterThrowLaunch/MulticopterThrowLaunch.hpp" +#include "Safety.hpp" #include "UserModeIntention.hpp" +#include "worker_thread.hpp" #include #include @@ -80,7 +81,6 @@ #include #include #include -#include #include using math::constrain; @@ -205,14 +205,6 @@ private: OFFBOARD_MODE_BIT = (1 << 1), }; - enum class ThrowLaunchState { - DISABLED = 0, - IDLE = 1, - ARMED = 2, - UNSAFE = 3, - FLYING = 4 - }; - /* Decouple update interval and hysteresis counters, all depends on intervals */ static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; @@ -222,6 +214,7 @@ private: FailsafeBase &_failsafe{_failsafe_instance}; FailureDetector _failure_detector{this}; HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status}; + MulticopterThrowLaunch _multicopter_throw_launch{this}; Safety _safety{}; UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks}; WorkerThread _worker_thread{}; @@ -273,9 +266,7 @@ private: bool _arm_tune_played{false}; bool _have_taken_off_since_arming{false}; bool _status_changed{true}; - ThrowLaunchState _throw_launch_state{ThrowLaunchState::DISABLED}; - vehicle_local_position_s _vehicle_local_position{}; vehicle_land_detected_s _vehicle_land_detected{}; // commander publications @@ -291,7 +282,6 @@ private: uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -343,8 +333,6 @@ private: (ParamInt) _param_com_rc_override, (ParamInt) _param_flight_uuid, (ParamInt) _param_takeoff_finished_action, - (ParamFloat) _param_com_cpu_max, - (ParamBool) _param_com_throw_en, - (ParamFloat) _param_com_throw_min_speed + (ParamFloat) _param_com_cpu_max ) }; diff --git a/src/modules/commander/MulticopterThrowLaunch/CMakeLists.txt b/src/modules/commander/MulticopterThrowLaunch/CMakeLists.txt new file mode 100644 index 0000000000..c2316b66c2 --- /dev/null +++ b/src/modules/commander/MulticopterThrowLaunch/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2023 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_library(MulticopterThrowLaunch + MulticopterThrowLaunch.cpp +) diff --git a/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp b/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp new file mode 100644 index 0000000000..89c59f8d82 --- /dev/null +++ b/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "MulticopterThrowLaunch.hpp" +#include + +MulticopterThrowLaunch::MulticopterThrowLaunch(ModuleParams *parent) : + ModuleParams(parent) +{} + +void MulticopterThrowLaunch::update(const bool armed) +{ + if (_param_com_throw_en.get()) { + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { + _last_velocity = matrix::Vector3f(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + } + } + + if (!armed && _throw_launch_state != ThrowLaunchState::IDLE) { + events::send(events::ID("mc_throw_launch_not_ready"), events::Log::Critical, "Disarmed, don't throw"); + _throw_launch_state = ThrowLaunchState::IDLE; + } + + switch (_throw_launch_state) { + case ThrowLaunchState::IDLE: + if (armed) { + events::send(events::ID("mc_throw_launch_ready"), events::Log::Critical, "Ready for throw launch"); + _throw_launch_state = ThrowLaunchState::ARMED; + } + + break; + + case ThrowLaunchState::ARMED: + if (_last_velocity.longerThan(_param_com_throw_min_speed.get())) { + PX4_INFO("Throw detected, motors will start once falling"); + _throw_launch_state = ThrowLaunchState::UNSAFE; + } + + break; + + case ThrowLaunchState::UNSAFE: + if (_last_velocity(2) > 0.f) { + PX4_INFO("Throw and fall detected, starting motors"); + _throw_launch_state = ThrowLaunchState::FLYING; + } + + break; + + case ThrowLaunchState::DISABLED: + case ThrowLaunchState::FLYING: + // Nothing to do + break; + } + + } else if (_throw_launch_state != ThrowLaunchState::DISABLED) { + // make sure everything is reset when the throw launch is disabled + _throw_launch_state = ThrowLaunchState::DISABLED; + } +} diff --git a/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.hpp b/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.hpp new file mode 100644 index 0000000000..973ce3c023 --- /dev/null +++ b/src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 MulticopterThrowLaunch.hpp + * + * Changes to manage a takeoff of a multicopter by manually throwing it into the air. + * + * @author Michał Barciś + */ + +#pragma once + +#include +#include + +#include +#include + +class MulticopterThrowLaunch : public ModuleParams +{ +public: + explicit MulticopterThrowLaunch(ModuleParams *parent); + ~MulticopterThrowLaunch() override = default; + + /** + * @return false if feature disabled or already flying + */ + bool isThrowLaunchInProgress() const { + return _throw_launch_state != ThrowLaunchState::DISABLED + && _throw_launch_state != ThrowLaunchState::FLYING; + } + + bool isReadyToThrow() const { return _throw_launch_state == ThrowLaunchState::ARMED; } + + /** + * Main update of the state + * @param armed true if vehicle is armed + */ + void update(const bool armed); + + enum class ThrowLaunchState { + DISABLED = 0, + IDLE = 1, + ARMED = 2, + UNSAFE = 3, + FLYING = 4 + }; + +private: + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + + ThrowLaunchState _throw_launch_state{ThrowLaunchState::DISABLED}; + matrix::Vector3f _last_velocity{}; + + DEFINE_PARAMETERS( + (ParamBool) _param_com_throw_en, + (ParamFloat) _param_com_throw_min_speed + ); +}; diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index bc6afcf50a..01278bc002 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -39,7 +39,6 @@ #include #include - void TakeoffHandling::generateInitialRampValue(float velocity_p_gain) { velocity_p_gain = math::max(velocity_p_gain, 0.01f);