fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 16:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

move takeoff state machine flight_mode_manager -> mc_pos_control

Browse Source
This commit is contained in:
Daniel Agar
2021-03-03 18:53:56 +01:00
committed by Matthias Grob
parent 823c6078d9
commit 266ea377da
20 changed files with 288 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/CMakeLists.txt
-3
View File
@@ -108,8 +108,6 @@ add_compile_options(
# add subdirectory containing all tasks
add_subdirectory(tasks)
add_subdirectory(Takeoff)
px4_add_module(
MODULE modules__flight_mode_manager
MAIN flight_mode_manager
@@ -124,7 +122,6 @@ px4_add_module(
${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.hpp
${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.cpp
DEPENDS
Takeoff
px4_work_queue
WeatherVane
)
src/modules/flight_mode_manager/FlightModeManager.cpp
+6 -68
View File
@@ -115,10 +115,6 @@ void FlightModeManager::Run()
}
}
// an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes
_takeoff.updateTakeoffState(_vehicle_control_mode_sub.get().flag_armed, _vehicle_land_detected_sub.get().landed, false,
10.f, !_vehicle_control_mode_sub.get().flag_control_climb_rate_enabled, time_stamp_now);
// activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy
// that turns the nose of the vehicle into the wind
if (_wv_controller != nullptr) {
@@ -162,10 +158,6 @@ void FlightModeManager::updateParams()
_current_task.task->handleParameterUpdate();
}
_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
_takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get());
if (_wv_controller != nullptr) {
_wv_controller->update_parameters();
}
@@ -517,21 +509,15 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
// limit altitude according to land detector
limitAltitude(setpoint, vehicle_local_position);
const bool not_taken_off = _takeoff.getTakeoffState() < TakeoffState::rampup;
const bool flying = _takeoff.getTakeoffState() >= TakeoffState::flight;
const bool flying_but_ground_contact = flying && _vehicle_land_detected_sub.get().ground_contact;
if (_takeoff_status_sub.updated()) {
takeoff_status_s takeoff_status;
if (not_taken_off || flying_but_ground_contact) {
// we are not flying yet and need to avoid any corrections
reset_setpoint_to_nan(setpoint);
Vector3f(0.f, 0.f, 100.f).copyTo(setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
// set yaw-sp to current yaw
setpoint.yawspeed = 0.f;
// prevent any integrator windup
constraints.reset_integral = true;
if (_takeoff_status_sub.copy(&takeoff_status)) {
_takeoff_state = takeoff_status.takeoff_state;
}
}
if (not_taken_off) {
if (_takeoff_state < takeoff_status_s::TAKEOFF_STATE_RAMPUP) {
// reactivate the task which will reset the setpoint to current state
_current_task.task->reActivate();
}
@@ -540,38 +526,9 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
setpoint.timestamp = hrt_absolute_time();
_trajectory_setpoint_pub.publish(setpoint);
// Allow ramping from zero thrust on takeoff
if (flying) {
constraints.minimum_thrust = _param_mpc_thr_min.get();
} else {
// allow zero thrust when taking off and landing
constraints.minimum_thrust = 0.f;
}
// fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN
// TODO: this should get obsolete once the takeoff limiting moves into the flight tasks
if (!PX4_ISFINITE(constraints.speed_up) || (constraints.speed_up > _param_mpc_z_vel_max_up.get())) {
constraints.speed_up = _param_mpc_z_vel_max_up.get();
}
// limit tilt during takeoff ramupup
if (_takeoff.getTakeoffState() < TakeoffState::flight) {
constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
}
// handle smooth takeoff
_takeoff.updateTakeoffState(_vehicle_control_mode_sub.get().flag_armed, _vehicle_land_detected_sub.get().landed,
constraints.want_takeoff, constraints.speed_up, !_vehicle_control_mode_sub.get().flag_control_climb_rate_enabled,
_time_stamp_last_loop);
constraints.speed_up = _takeoff.updateRamp(dt, constraints.speed_up);
constraints.flight_task = static_cast<uint32_t>(_current_task.index);
constraints.timestamp = hrt_absolute_time();
_vehicle_constraints_pub.publish(constraints);
// if there's any change in landing gear setpoint publish it
landing_gear_s landing_gear = _current_task.task->getGear();
@@ -583,16 +540,6 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
}
_old_landing_gear_position = landing_gear.landing_gear;
// Publish takeoff status
takeoff_status_s takeoff_status;
takeoff_status.takeoff_state = static_cast<uint8_t>(_takeoff.getTakeoffState());
if (takeoff_status.takeoff_state != _old_takeoff_state) {
takeoff_status.timestamp = hrt_absolute_time();
_takeoff_status_pub.publish(takeoff_status);
_old_takeoff_state = takeoff_status.takeoff_state;
}
}
void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint,
@@ -614,15 +561,6 @@ void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoin
}
}
void FlightModeManager::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
{
setpoint.x = setpoint.y = setpoint.z = NAN;
setpoint.vx = setpoint.vy = setpoint.vz = NAN;
setpoint.yaw = setpoint.yawspeed = NAN;
setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN;
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
}
FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
{
// switch to the running task, nothing to do
src/modules/flight_mode_manager/FlightModeManager.hpp
+3 -13
View File
@@ -56,8 +56,6 @@
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include "Takeoff/Takeoff.hpp"
#include <new>
enum class FlightTaskError : int {
@@ -95,7 +93,6 @@ private:
void handleCommand();
void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position);
void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint);
/**
* Switch to a specific task (for normal usage)
@@ -133,10 +130,9 @@ private:
FlightTaskIndex index{FlightTaskIndex::None};
} _current_task{};
Takeoff _takeoff; ///< state machine and ramp to bring the vehicle off the ground without a jump
WeatherVane *_wv_controller{nullptr};
int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
uint8_t _old_takeoff_state{0};
uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED};
int _task_failure_count{0};
uint8_t _last_vehicle_nav_state{0};
@@ -145,6 +141,7 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
@@ -156,19 +153,12 @@ private:
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<takeoff_status_s> _takeoff_status_pub{ORB_ID(takeoff_status)};
uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
DEFINE_PARAMETERS(
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time,
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t,
(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>) _param_mpc_z_vel_p_acc,
(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode
);
};
src/modules/flight_mode_manager/Takeoff/CMakeLists.txt
-40
View File
@@ -1,40 +0,0 @@
############################################################################
#
# Copyright (c) 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.
#
############################################################################
px4_add_library(Takeoff
Takeoff.cpp
)
target_include_directories(Takeoff PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(Takeoff PUBLIC hysteresis)
px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff)
src/modules/flight_mode_manager/Takeoff/Takeoff.cpp
-132
View File
@@ -1,132 +0,0 @@
/****************************************************************************
*
* Copyright (c) 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 Takeoff.cpp
*/
#include "Takeoff.hpp"
#include <mathlib/mathlib.h>
#include <lib/ecl/geo/geo.h>
void Takeoff::generateInitialRampValue(float velocity_p_gain)
{
velocity_p_gain = math::max(velocity_p_gain, 0.01f);
_takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain;
}
void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us)
{
_spoolup_time_hysteresis.set_state_and_update(armed, now_us);
switch (_takeoff_state) {
case TakeoffState::disarmed:
if (armed) {
_takeoff_state = TakeoffState::spoolup;
} else {
break;
}
// FALLTHROUGH
case TakeoffState::spoolup:
if (_spoolup_time_hysteresis.get_state()) {
_takeoff_state = TakeoffState::ready_for_takeoff;
} else {
break;
}
// FALLTHROUGH
case TakeoffState::ready_for_takeoff:
if (want_takeoff) {
_takeoff_state = TakeoffState::rampup;
_takeoff_ramp_vz = _takeoff_ramp_vz_init;
} else {
break;
}
// FALLTHROUGH
case TakeoffState::rampup:
if (_takeoff_ramp_vz >= takeoff_desired_vz) {
_takeoff_state = TakeoffState::flight;
} else {
break;
}
// FALLTHROUGH
case TakeoffState::flight:
if (landed) {
_takeoff_state = TakeoffState::ready_for_takeoff;
}
break;
default:
break;
}
if (armed && skip_takeoff) {
_takeoff_state = TakeoffState::flight;
}
// TODO: need to consider free fall here
if (!armed) {
_takeoff_state = TakeoffState::disarmed;
}
}
float Takeoff::updateRamp(const float dt, const float takeoff_desired_vz)
{
if (_takeoff_state < TakeoffState::rampup) {
return _takeoff_ramp_vz_init;
}
if (_takeoff_state == TakeoffState::rampup) {
if (_takeoff_ramp_time > dt) {
_takeoff_ramp_vz += (takeoff_desired_vz - _takeoff_ramp_vz_init) * dt / _takeoff_ramp_time;
} else {
_takeoff_ramp_vz = takeoff_desired_vz;
}
if (_takeoff_ramp_vz < takeoff_desired_vz) {
return _takeoff_ramp_vz;
}
}
return takeoff_desired_vz;
}
src/modules/flight_mode_manager/Takeoff/Takeoff.hpp
-102
View File
@@ -1,102 +0,0 @@
/****************************************************************************
*
* Copyright (c) 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 Takeoff.hpp
*
* A class handling all takeoff states and a smooth ramp up of the motors.
*/
#pragma once
#include <lib/hysteresis/hysteresis.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/takeoff_status.h>
using namespace time_literals;
enum class TakeoffState {
disarmed = takeoff_status_s::TAKEOFF_STATE_DISARMED,
spoolup = takeoff_status_s::TAKEOFF_STATE_SPOOLUP,
ready_for_takeoff = takeoff_status_s::TAKEOFF_STATE_READY_FOR_TAKEOFF,
rampup = takeoff_status_s::TAKEOFF_STATE_RAMPUP,
flight = takeoff_status_s::TAKEOFF_STATE_FLIGHT
};
class Takeoff
{
public:
Takeoff() = default;
~Takeoff() = default;
// initialize parameters
void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); }
void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
/**
* Calculate a vertical velocity to initialize the takeoff ramp
* that when passed to the velocity controller results in a zero throttle setpoint.
* @param hover_thrust normalized thrsut value with which the vehicle hovers
* @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust
*/
void generateInitialRampValue(const float velocity_p_gain);
/**
* Update the state for the takeoff.
* @param setpoint a vehicle_local_position_setpoint_s structure
* @return true if setpoint has updated correctly
*/
void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us);
/**
* Update and return the velocity constraint ramp value during takeoff.
* By ramping up _takeoff_ramp_vz during the takeoff and using it to constain the maximum climb rate a smooth takeoff behavior is achieved.
* Returns zero on the ground and takeoff_desired_vz in flight.
* @param dt time in seconds since the last call/loop iteration
* @param takeoff_desired_vz end value for the velocity ramp
* @return true if setpoint has updated correctly
*/
float updateRamp(const float dt, const float takeoff_desired_vz);
TakeoffState getTakeoffState() { return _takeoff_state; }
private:
TakeoffState _takeoff_state = TakeoffState::disarmed;
systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */
float _takeoff_ramp_time = 0.f;
float _takeoff_ramp_vz_init = 0.f;
float _takeoff_ramp_vz = 0.f;
};
src/modules/flight_mode_manager/Takeoff/TakeoffTest.cpp
-79
View File
@@ -1,79 +0,0 @@
/****************************************************************************
*
* Copyright (C) 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.
*
****************************************************************************/
#include <gtest/gtest.h>
#include <Takeoff.hpp>
#include <drivers/drv_hrt.h>
#include <lib/ecl/geo/geo.h>
TEST(TakeoffTest, Initialization)
{
Takeoff takeoff;
EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed);
}
TEST(TakeoffTest, RegularTakeoffRamp)
{
Takeoff takeoff;
takeoff.setSpoolupTime(1.f);
takeoff.setTakeoffRampTime(2.0);
takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f);
// disarmed, landed, don't want takeoff
takeoff.updateTakeoffState(false, true, false, 1.f, false, 0);
EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed);
// armed, not landed anymore, don't want takeoff
takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms);
EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup);
// armed, not landed, don't want takeoff yet, spoolup time passed
takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s);
EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff);
// armed, not landed, want takeoff
takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s);
EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup);
// armed, not landed, want takeoff, ramping up
takeoff.updateTakeoffState(true, false, true, 1.f, false, 4_s);
EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 0.f);
EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), .5f);
EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.f);
EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f);
EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f);
// armed, not landed, want takeoff, rampup time passed
takeoff.updateTakeoffState(true, false, true, 1.f, false, 6500_ms);
EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::flight);
}
src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp
-1
View File
@@ -65,7 +65,6 @@ protected:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
(ParamFloat<px4::params::MPC_LAND_ALT1>)
_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
+1 -4
View File
@@ -6,7 +6,7 @@ constexpr uint64_t FlightTask::_timeout;
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, {}};
const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint)
@@ -172,9 +172,6 @@ void FlightTask::_setDefaultConstraints()
_constraints.speed_xy = _param_mpc_xy_vel_max.get();
_constraints.speed_up = _param_mpc_z_vel_max_up.get();
_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
_constraints.tilt = NAN;
_constraints.min_distance_to_ground = NAN;
_constraints.max_distance_to_ground = NAN;
_constraints.want_takeoff = false;
}
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
+3
View File
@@ -227,6 +227,9 @@ protected:
float _dist_to_bottom{}; /**< current height above ground level */
float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */
float _min_distance_to_ground{-INFINITY}; /**< min distance to ground constraint */
float _max_distance_to_ground{INFINITY}; /**< max distance to ground constraint */
/**
* Setpoints which the position controller has to execute.
* Setpoints that are set to NAN are not controlled. Not all setpoints can be set at the same time.
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+12 -17
View File
@@ -81,17 +81,17 @@ bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s
void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
{
if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) {
_constraints.min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
_min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
} else {
_constraints.min_distance_to_ground = -INFINITY;
_min_distance_to_ground = -INFINITY;
}
if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) {
_constraints.max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max;
_max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max;
} else {
_constraints.max_distance_to_ground = INFINITY;
_max_distance_to_ground = INFINITY;
}
}
@@ -184,7 +184,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
// Ensure that minimum altitude is respected if
// there is a distance sensor and distance to bottom is below minimum.
if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _constraints.min_distance_to_ground) {
if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _min_distance_to_ground) {
_terrainFollowing(apply_brake, stopped);
} else {
@@ -210,14 +210,10 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
void FlightTaskManualAltitude::_respectMinAltitude()
{
const bool respectAlt = PX4_ISFINITE(_dist_to_bottom)
&& _dist_to_bottom < _constraints.min_distance_to_ground;
// Height above ground needs to be limited (flow / range-finder)
if (respectAlt) {
if (PX4_ISFINITE(_dist_to_bottom) && (_dist_to_bottom < _min_distance_to_ground)) {
// increase altitude to minimum flow distance
_position_setpoint(2) = _position(2)
- (_constraints.min_distance_to_ground - _dist_to_bottom);
_position_setpoint(2) = _position(2) - (_min_distance_to_ground - _dist_to_bottom);
}
}
@@ -253,8 +249,8 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
// if there is a valid maximum distance to ground, linearly increase speed limit with distance
// below the maximum, preserving control loop vertical position error gain.
if (PX4_ISFINITE(_constraints.max_distance_to_ground)) {
_constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_constraints.max_distance_to_ground - _dist_to_bottom),
if (PX4_ISFINITE(_max_distance_to_ground)) {
_constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom),
-_max_speed_down, _max_speed_up);
} else {
@@ -262,17 +258,16 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
}
// if distance to bottom exceeded maximum distance, slowly approach maximum distance
if (_dist_to_bottom > _constraints.max_distance_to_ground) {
if (_dist_to_bottom > _max_distance_to_ground) {
// difference between current distance to ground and maximum distance to ground
const float delta_distance_to_max = _dist_to_bottom - _constraints.max_distance_to_ground;
const float delta_distance_to_max = _dist_to_bottom - _max_distance_to_ground;
// set position setpoint to maximum distance to ground
_position_setpoint(2) = _position(2) + delta_distance_to_max;
_position_setpoint(2) = _position(2) + delta_distance_to_max;
// limit speed downwards to 0.7m/s
_constraints.speed_down = math::min(_max_speed_down, 0.7f);
} else {
_constraints.speed_down = _max_speed_down;
}
}
}