fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ThrowLaunch: move into separate class

Browse Source
This commit is contained in:
Matthias Grob 2023-10-27 13:17:18 +02:00
parent 1a48f311ea
commit 9d455d5f1f
7 changed files with 238 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/commander/CMakeLists.txt
View File

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

67
src/modules/commander/Commander.cpp
View File

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

24
src/modules/commander/Commander.hpp
View File

@ -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 <lib/controllib/blocks.hpp>
#include <lib/hysteresis/hysteresis.h>
@ -80,7 +81,6 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vtol_vehicle_status.h>
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<px4::params::COM_RC_OVERRIDE>) _param_com_rc_override,
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max,
(ParamBool<px4::params::COM_THROW_EN>) _param_com_throw_en,
(ParamFloat<px4::params::COM_THROW_SPEED>) _param_com_throw_min_speed
(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max
)
};

36
src/modules/commander/MulticopterThrowLaunch/CMakeLists.txt Normal file
View File

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

91
src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp Normal file
View File

@ -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 <px4_platform_common/events.h>
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;
}
}

90
src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.hpp Normal file
View File

@ -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ś <mbarcis@mbarcis.net>
*/
#pragma once
#include <lib/matrix/matrix/math.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_local_position.h>
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<px4::params::COM_THROW_EN>) _param_com_throw_en,
(ParamFloat<px4::params::COM_THROW_SPEED>) _param_com_throw_min_speed
);
};

1
src/modules/mc_pos_control/Takeoff/Takeoff.cpp
View File

@ -39,7 +39,6 @@
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
void TakeoffHandling::generateInitialRampValue(float velocity_p_gain)
{
velocity_p_gain = math::max(velocity_p_gain, 0.01f);