dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MulticoperPositionControl: 3rd pass get rid of zombie members

This commit is contained in:
Matthias Grob 2020-10-24 16:55:05 +02:00 committed by Daniel Agar
parent 62ada2e2dc
commit 7de288877a
8 changed files with 11 additions and 383 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/flight_mode_manager/FlightModeManager.hpp
View File

@ -38,9 +38,10 @@
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/flight_tasks/FlightTasks.hpp>
#include <Takeoff.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
@ -49,6 +50,8 @@
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include "Takeoff/Takeoff.hpp"
class FlightModeManager : public ModuleBase<FlightModeManager>, public ModuleParams, public px4::WorkItem
{
public:

2
src/modules/mc_pos_control/CMakeLists.txt
View File

@ -31,7 +31,6 @@
#
############################################################################
add_subdirectory(Takeoff)
add_subdirectory(PositionControl)
px4_add_module(
@ -50,5 +49,4 @@ px4_add_module(
ecl_geo
WeatherVane
CollisionPrevention
Takeoff2
)

4
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -33,6 +33,10 @@
#include "MulticopterPositionControl.hpp"
#include <float.h>
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
using namespace time_literals;
using namespace matrix;

31
src/modules/mc_pos_control/MulticopterPositionControl.hpp
View File

@ -42,11 +42,8 @@
#include <lib/controllib/blocks.hpp>
#include <lib/flight_tasks/FlightTasks.hpp>
#include <lib/hysteresis/hysteresis.h>
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <lib/weather_vane/WeatherVane.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
@ -54,25 +51,18 @@
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/hover_thrust_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/hover_thrust_estimate.h>
#include "PositionControl/PositionControl.hpp"
#include "Takeoff/Takeoff.hpp"
#include <float.h>
class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>, public control::SuperBlock,
public ModuleParams, public px4::WorkItem
@ -95,8 +85,6 @@ public:
private:
void Run() override;
Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub;
orb_advert_t _mavlink_log_pub{nullptr};
@ -114,21 +102,8 @@ private:
int _task_failure_count{0}; /**< counter for task failures */
/**< vehicle-land-detection: initialze to landed */
vehicle_land_detected_s _vehicle_land_detected = {
.timestamp = 0,
.alt_max = -1.0f,
.freefall = false,
.ground_contact = true,
.maybe_landed = true,
.landed = true,
};
vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */
vehicle_local_position_s _local_pos{}; /**< vehicle local position */
home_position_s _home_pos{}; /**< home position */
int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
DEFINE_PARAMETERS(
// Position Control

40
src/modules/mc_pos_control/Takeoff/CMakeLists.txt
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(Takeoff2
Takeoff.cpp
)
target_include_directories(Takeoff2 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(Takeoff2 PUBLIC hysteresis)
px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff2)

132
src/modules/mc_pos_control/Takeoff/Takeoff.cpp
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;
}

101
src/modules/mc_pos_control/Takeoff/Takeoff.hpp
View File

@ -1,101 +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>
using namespace time_literals;
enum class TakeoffState {
disarmed = 0,
spoolup,
ready_for_takeoff,
rampup,
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;
};

79
src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp
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);
}