mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 01:23:34 +08:00
rename FWPositionController to FWModeManager
Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan
@@ -0,0 +1,67 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015-2025 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(launchdetection)
|
||||
add_subdirectory(runway_takeoff)
|
||||
|
||||
set(POSCONTROL_DEPENDENCIES
|
||||
launchdetection
|
||||
npfg
|
||||
runway_takeoff
|
||||
SlewRate
|
||||
tecs
|
||||
motion_planning
|
||||
performance_model
|
||||
)
|
||||
|
||||
if(CONFIG_FIGURE_OF_EIGHT)
|
||||
add_subdirectory(figure_eight)
|
||||
set(POSCONTROL_DEPENDENCIES
|
||||
${POSCONTROL_DEPENDENCIES}
|
||||
figure_eight
|
||||
)
|
||||
endif()
|
||||
|
||||
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__fw_mode_manager
|
||||
MAIN fw_mode_manager
|
||||
SRCS
|
||||
FixedWingModeManager.cpp
|
||||
FixedWingModeManager.hpp
|
||||
ControllerConfigurationHandler.cpp
|
||||
ControllerConfigurationHandler.hpp
|
||||
DEPENDS
|
||||
${POSCONTROL_DEPENDENCIES}
|
||||
)
|
||||
@@ -0,0 +1,139 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 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 CombinedControllerConfigurationHandler.cpp
|
||||
*/
|
||||
|
||||
#include "ControllerConfigurationHandler.hpp"
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
|
||||
void CombinedControllerConfigurationHandler::update(const hrt_abstime now)
|
||||
{
|
||||
_longitudinal_updated = floatValueChanged(_longitudinal_configuration_current_cycle.pitch_min,
|
||||
_longitudinal_publisher.get().pitch_min);
|
||||
_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.pitch_max,
|
||||
_longitudinal_publisher.get().pitch_max);
|
||||
_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.throttle_min,
|
||||
_longitudinal_publisher.get().throttle_min);
|
||||
_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.throttle_max,
|
||||
_longitudinal_publisher.get().throttle_max);
|
||||
_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.speed_weight,
|
||||
_longitudinal_publisher.get().speed_weight);
|
||||
_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.climb_rate_target,
|
||||
_longitudinal_publisher.get().climb_rate_target);
|
||||
_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.sink_rate_target,
|
||||
_longitudinal_publisher.get().sink_rate_target);
|
||||
_longitudinal_updated |= booleanValueChanged(_longitudinal_configuration_current_cycle.enforce_low_height_condition,
|
||||
_longitudinal_publisher.get().enforce_low_height_condition);
|
||||
_longitudinal_updated |= booleanValueChanged(_longitudinal_configuration_current_cycle.disable_underspeed_protection,
|
||||
_longitudinal_publisher.get().disable_underspeed_protection);
|
||||
|
||||
_lateral_updated |= floatValueChanged(_lateral_configuration_current_cycle.lateral_accel_max,
|
||||
_lateral_publisher.get().lateral_accel_max);
|
||||
|
||||
if (_longitudinal_updated || now - _time_last_longitudinal_publish > 1_s) {
|
||||
_longitudinal_configuration_current_cycle.timestamp = now;
|
||||
_longitudinal_publisher.update(_longitudinal_configuration_current_cycle);
|
||||
_time_last_longitudinal_publish = _longitudinal_configuration_current_cycle.timestamp;
|
||||
}
|
||||
|
||||
if (_lateral_updated || now - _time_last_lateral_publish > 1_s) {
|
||||
_lateral_configuration_current_cycle.timestamp = now;
|
||||
_lateral_publisher.update(_lateral_configuration_current_cycle);
|
||||
_time_last_lateral_publish = _lateral_configuration_current_cycle.timestamp;
|
||||
}
|
||||
|
||||
_longitudinal_updated = _lateral_updated = false;
|
||||
_longitudinal_configuration_current_cycle = empty_longitudinal_control_configuration;
|
||||
_lateral_configuration_current_cycle = empty_lateral_control_configuration;
|
||||
}
|
||||
|
||||
void CombinedControllerConfigurationHandler::setThrottleMax(float throttle_max)
|
||||
{
|
||||
_longitudinal_configuration_current_cycle.throttle_max = throttle_max;
|
||||
}
|
||||
|
||||
void CombinedControllerConfigurationHandler::setThrottleMin(float throttle_min)
|
||||
{
|
||||
_longitudinal_configuration_current_cycle.throttle_min = throttle_min;
|
||||
}
|
||||
|
||||
void CombinedControllerConfigurationHandler::setSpeedWeight(float speed_weight)
|
||||
{
|
||||
_longitudinal_configuration_current_cycle.speed_weight = speed_weight;
|
||||
}
|
||||
|
||||
void CombinedControllerConfigurationHandler::setPitchMin(const float pitch_min)
|
||||
{
|
||||
_longitudinal_configuration_current_cycle.pitch_min = pitch_min;
|
||||
}
|
||||
|
||||
void CombinedControllerConfigurationHandler::setPitchMax(const float pitch_max)
|
||||
{
|
||||
_longitudinal_configuration_current_cycle.pitch_max = pitch_max;
|
||||
}
|
||||
|
||||
void CombinedControllerConfigurationHandler::setClimbRateTarget(float climbrate_target)
|
||||
{
|
||||
_longitudinal_configuration_current_cycle.climb_rate_target = climbrate_target;
|
||||
}
|
||||
|
||||
void CombinedControllerConfigurationHandler::setDisableUnderspeedProtection(bool disable)
|
||||
{
|
||||
_longitudinal_configuration_current_cycle.disable_underspeed_protection = disable;
|
||||
}
|
||||
|
||||
void CombinedControllerConfigurationHandler::setSinkRateTarget(const float sinkrate_target)
|
||||
{
|
||||
_longitudinal_configuration_current_cycle.sink_rate_target = sinkrate_target;
|
||||
}
|
||||
|
||||
void CombinedControllerConfigurationHandler::setLateralAccelMax(float lateral_accel_max)
|
||||
{
|
||||
_lateral_configuration_current_cycle.lateral_accel_max = lateral_accel_max;
|
||||
}
|
||||
|
||||
void CombinedControllerConfigurationHandler::setEnforceLowHeightCondition(bool low_height_condition)
|
||||
{
|
||||
_longitudinal_configuration_current_cycle.enforce_low_height_condition = low_height_condition;
|
||||
}
|
||||
|
||||
void CombinedControllerConfigurationHandler::resetLastPublishTime()
|
||||
{
|
||||
_time_last_longitudinal_publish = _time_last_lateral_publish = 0;
|
||||
}
|
||||
@@ -0,0 +1,113 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 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 CombinedControllerConfigurationHandler.hpp
|
||||
*/
|
||||
#ifndef PX4_CONTROLLERCONFIGURATIONHANDLER_HPP
|
||||
#define PX4_CONTROLLERCONFIGURATIONHANDLER_HPP
|
||||
|
||||
#include <uORB/topics/longitudinal_control_configuration.h>
|
||||
#include <uORB/topics/lateral_control_configuration.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
static constexpr longitudinal_control_configuration_s empty_longitudinal_control_configuration = {.timestamp = 0, .pitch_min = NAN, .pitch_max = NAN, .throttle_min = NAN, .throttle_max = NAN, .climb_rate_target = NAN, .sink_rate_target = NAN, .speed_weight = NAN, .enforce_low_height_condition = false, .disable_underspeed_protection = false };
|
||||
static constexpr lateral_control_configuration_s empty_lateral_control_configuration = {.timestamp = 0, .lateral_accel_max = NAN};
|
||||
|
||||
|
||||
class CombinedControllerConfigurationHandler
|
||||
{
|
||||
public:
|
||||
CombinedControllerConfigurationHandler() = default;
|
||||
~CombinedControllerConfigurationHandler() = default;
|
||||
|
||||
void update(const hrt_abstime now);
|
||||
|
||||
void setEnforceLowHeightCondition(bool low_height_condition);
|
||||
|
||||
void setThrottleMax(float throttle_max);
|
||||
|
||||
void setThrottleMin(float throttle_min);
|
||||
|
||||
void setSpeedWeight(float speed_weight);
|
||||
|
||||
void setPitchMin(const float pitch_min);
|
||||
|
||||
void setPitchMax(const float pitch_max);
|
||||
|
||||
void setClimbRateTarget(float climbrate_target);
|
||||
|
||||
void setDisableUnderspeedProtection(bool disable);
|
||||
|
||||
void setSinkRateTarget(const float sinkrate_target);
|
||||
|
||||
void setLateralAccelMax(float lateral_accel_max);
|
||||
|
||||
void resetLastPublishTime();
|
||||
|
||||
private:
|
||||
bool booleanValueChanged(bool new_value, bool current_value)
|
||||
{
|
||||
return current_value != new_value;
|
||||
}
|
||||
|
||||
bool floatValueChanged(float new_value, float current_value)
|
||||
{
|
||||
bool diff;
|
||||
|
||||
if (PX4_ISFINITE(new_value)) {
|
||||
diff = PX4_ISFINITE(current_value) ? (fabsf(new_value - current_value) > FLT_EPSILON) : true;
|
||||
|
||||
} else {
|
||||
diff = PX4_ISFINITE(current_value);
|
||||
}
|
||||
|
||||
return diff;
|
||||
}
|
||||
|
||||
bool _lateral_updated{false};
|
||||
bool _longitudinal_updated{false};
|
||||
|
||||
hrt_abstime _time_last_longitudinal_publish{0};
|
||||
hrt_abstime _time_last_lateral_publish{0};
|
||||
|
||||
uORB::PublicationData<lateral_control_configuration_s> _lateral_publisher{ORB_ID(lateral_control_configuration)};
|
||||
uORB::PublicationData<longitudinal_control_configuration_s> _longitudinal_publisher{ORB_ID(longitudinal_control_configuration)};
|
||||
|
||||
lateral_control_configuration_s _lateral_configuration_current_cycle{empty_lateral_control_configuration};
|
||||
longitudinal_control_configuration_s _longitudinal_configuration_current_cycle {empty_longitudinal_control_configuration};
|
||||
};
|
||||
|
||||
#endif //PX4_CONTROLLERCONFIGURATIONHANDLER_HPP
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,875 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2025 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 FixedWingModeManager.hpp
|
||||
* Implementation of various fixed-wing control modes.
|
||||
*/
|
||||
|
||||
#ifndef FIXEDWINGMODEMANAGER_HPP_
|
||||
#define FIXEDWINGMODEMANAGER_HPP_
|
||||
|
||||
#include "launchdetection/LaunchDetector.h"
|
||||
#include "runway_takeoff/RunwayTakeoff.h"
|
||||
#include "ControllerConfigurationHandler.hpp"
|
||||
|
||||
#include <float.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/atmosphere/atmosphere.h>
|
||||
#include <lib/npfg/DirectionalGuidance.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/airspeed_validated.h>
|
||||
#include <uORB/topics/fixed_wing_lateral_setpoint.h>
|
||||
#include <uORB/topics/fixed_wing_lateral_guidance_status.h>
|
||||
#include <uORB/topics/fixed_wing_longitudinal_setpoint.h>
|
||||
#include <uORB/topics/fixed_wing_runway_control.h>
|
||||
#include <uORB/topics/landing_gear.h>
|
||||
#include <uORB/topics/launch_detection_status.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/normalized_unsigned_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/position_controller_landing_status.h>
|
||||
#include <uORB/topics/position_controller_status.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_control_mode.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/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/wind.h>
|
||||
#include <uORB/topics/orbit_status.h>
|
||||
|
||||
#ifdef CONFIG_FIGURE_OF_EIGHT
|
||||
#include "figure_eight/FigureEight.hpp"
|
||||
#include <uORB/topics/figure_eight_status.h>
|
||||
#endif // CONFIG_FIGURE_OF_EIGHT
|
||||
|
||||
using namespace launchdetection;
|
||||
using namespace runwaytakeoff;
|
||||
using namespace time_literals;
|
||||
|
||||
using matrix::Vector2d;
|
||||
using matrix::Vector2f;
|
||||
|
||||
// [m] initial distance of waypoint in front of plane in heading hold mode
|
||||
static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f;
|
||||
|
||||
// [rad/s] max yawrate at which plane locks yaw for heading hold mode
|
||||
static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;
|
||||
|
||||
// [.] max manual roll/yaw normalized input from user which does not change the locked heading
|
||||
static constexpr float HDG_HOLD_MAN_INPUT_THRESH = 0.01f;
|
||||
|
||||
// [us] time after which we abort landing if terrain estimate is not valid. this timer start whenever the terrain altitude
|
||||
// was previously valid, and has changed to invalid.
|
||||
static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s;
|
||||
|
||||
// [us] within this timeout, if a distance sensor measurement not yet made, the land waypoint altitude is used for terrain
|
||||
// altitude. this timer starts at the beginning of the landing glide slope.
|
||||
static constexpr hrt_abstime TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT = 10_s;
|
||||
|
||||
// [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
|
||||
static constexpr float THROTTLE_THRESH = -.9f;
|
||||
|
||||
// [us] time after which the wind estimate is disabled if no longer updating
|
||||
static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s;
|
||||
|
||||
// [s] minimum time step between auto control updates
|
||||
static constexpr float MIN_AUTO_TIMESTEP = 0.01f;
|
||||
|
||||
// [s] maximum time step between auto control updates
|
||||
static constexpr float MAX_AUTO_TIMESTEP = 0.05f;
|
||||
|
||||
// [rad] minimum pitch while airspeed has not yet reached a controllable value in manual position controlled takeoff modes
|
||||
static constexpr float MIN_PITCH_DURING_MANUAL_TAKEOFF = 0.0f;
|
||||
|
||||
// [m] arbitrary buffer altitude added to clearance altitude setpoint during takeoff to ensure aircraft passes the clearance
|
||||
// altitude while waiting for navigator to flag it exceeded
|
||||
static constexpr float kClearanceAltitudeBuffer = 10.0f;
|
||||
|
||||
// [m/s] maximum rate at which the touchdown position can be nudged
|
||||
static constexpr float MAX_TOUCHDOWN_POSITION_NUDGE_RATE = 4.0f;
|
||||
|
||||
// [.] normalized deadzone threshold for manual nudging input
|
||||
static constexpr float MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE = 0.15f;
|
||||
|
||||
// [s] time interval after touchdown for ramping in runway clamping constraints (touchdown is assumed at FW_LND_TD_TIME after start of flare)
|
||||
static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f;
|
||||
|
||||
// [] Stick deadzon
|
||||
static constexpr float kStickDeadBand = 0.06f;
|
||||
|
||||
class FixedWingModeManager final : public ModuleBase<FixedWingModeManager>, public ModuleParams,
|
||||
public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
FixedWingModeManager();
|
||||
~FixedWingModeManager() override;
|
||||
|
||||
/** @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);
|
||||
|
||||
bool init();
|
||||
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
||||
uORB::Subscription _wind_sub{ORB_ID(wind)};
|
||||
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
|
||||
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
|
||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
|
||||
uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};
|
||||
uORB::Publication<launch_detection_status_s> _launch_detection_status_pub{ORB_ID(launch_detection_status)};
|
||||
uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
|
||||
uORB::Publication<landing_gear_s> _landing_gear_pub {ORB_ID(landing_gear)};
|
||||
uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
|
||||
uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
|
||||
uORB::PublicationData<fixed_wing_lateral_setpoint_s> _lateral_ctrl_sp_pub{ORB_ID(fixed_wing_lateral_setpoint)};
|
||||
uORB::PublicationData<fixed_wing_longitudinal_setpoint_s> _longitudinal_ctrl_sp_pub{ORB_ID(fixed_wing_longitudinal_setpoint)};
|
||||
uORB::Publication<fixed_wing_lateral_guidance_status_s> _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)};
|
||||
uORB::Publication<fixed_wing_runway_control_s> _fixed_wing_runway_control_pub{ORB_ID(fixed_wing_runway_control)};
|
||||
|
||||
manual_control_setpoint_s _manual_control_setpoint{};
|
||||
position_setpoint_triplet_s _pos_sp_triplet{};
|
||||
vehicle_control_mode_s _control_mode{};
|
||||
vehicle_local_position_s _local_pos{};
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
||||
CombinedControllerConfigurationHandler _ctrl_configuration_handler;
|
||||
|
||||
Vector2f _lpos_where_backtrans_started;
|
||||
|
||||
bool _position_setpoint_previous_valid{false};
|
||||
bool _position_setpoint_current_valid{false};
|
||||
bool _position_setpoint_next_valid{false};
|
||||
|
||||
perf_counter_t _loop_perf; // loop performance counter
|
||||
|
||||
// [us] Last absolute time position control has been called
|
||||
hrt_abstime _last_time_position_control_called{0};
|
||||
|
||||
uint8_t _position_sp_type{0};
|
||||
|
||||
enum FW_POSCTRL_MODE {
|
||||
FW_POSCTRL_MODE_AUTO,
|
||||
FW_POSCTRL_MODE_AUTO_ALTITUDE,
|
||||
FW_POSCTRL_MODE_AUTO_CLIMBRATE,
|
||||
FW_POSCTRL_MODE_AUTO_TAKEOFF,
|
||||
FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
|
||||
FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
|
||||
FW_POSCTRL_MODE_AUTO_PATH,
|
||||
FW_POSCTRL_MODE_MANUAL_POSITION,
|
||||
FW_POSCTRL_MODE_MANUAL_ALTITUDE,
|
||||
FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW,
|
||||
FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD,
|
||||
FW_POSCTRL_MODE_OTHER
|
||||
} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
|
||||
|
||||
enum StickConfig {
|
||||
STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0),
|
||||
STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT = (1 << 1)
|
||||
};
|
||||
|
||||
// VEHICLE STATES
|
||||
|
||||
uint8_t _nav_state;
|
||||
|
||||
double _current_latitude{0};
|
||||
double _current_longitude{0};
|
||||
float _current_altitude{0.f};
|
||||
|
||||
float _yaw{0.0f};
|
||||
float _yawrate{0.0f};
|
||||
|
||||
float _body_acceleration_x{0.f};
|
||||
float _body_velocity_x{0.f};
|
||||
|
||||
MapProjection _global_local_proj_ref{};
|
||||
|
||||
float _reference_altitude{NAN}; // [m AMSL] altitude of the local projection reference point
|
||||
|
||||
bool _landed{true};
|
||||
|
||||
// MANUAL MODES
|
||||
|
||||
// indicates whether we have completed a manual takeoff in a position control mode
|
||||
bool _completed_manual_takeoff{false};
|
||||
|
||||
// [rad] yaw setpoint for manual position mode heading hold
|
||||
float _hdg_hold_yaw{0.0f};
|
||||
|
||||
bool _hdg_hold_enabled{false}; // heading hold enabled
|
||||
bool _yaw_lock_engaged{false}; // yaw is locked for heading hold
|
||||
|
||||
Vector2f _hdg_hold_position{}; // position where heading hold started
|
||||
|
||||
// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
|
||||
float _manual_control_setpoint_for_height_rate{0.0f};
|
||||
|
||||
// [.] normalized setpoint for manual airspeed control [-1,1]; -1,0,1 maps to min,cruise,max airspeed commands
|
||||
float _manual_control_setpoint_for_airspeed{0.0f};
|
||||
|
||||
// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
|
||||
float _commanded_manual_airspeed_setpoint{NAN};
|
||||
|
||||
// AUTO TAKEOFF
|
||||
|
||||
// [m] ground altitude AMSL where the plane was launched
|
||||
float _takeoff_ground_alt{0.0f};
|
||||
|
||||
// class handling launch detection methods for fixed-wing takeoff
|
||||
LaunchDetector _launchDetector;
|
||||
|
||||
// true if a launch, specifically using the launch detector, has been detected
|
||||
bool _launch_detected{false};
|
||||
|
||||
// [deg] global position of the vehicle at the time launch is detected (using launch detector) or takeoff is started (runway)
|
||||
Vector2d _takeoff_init_position{0, 0};
|
||||
|
||||
// [rad] current vehicle yaw at the time the launch is detected
|
||||
float _launch_current_yaw{0.f};
|
||||
|
||||
// class handling runway takeoff for fixed-wing UAVs with steerable wheels
|
||||
RunwayTakeoff _runway_takeoff;
|
||||
|
||||
bool _skipping_takeoff_detection{false};
|
||||
|
||||
// AUTO LANDING
|
||||
|
||||
// corresponds to param FW_LND_NUDGE
|
||||
enum LandingNudgingOption {
|
||||
kNudgingDisabled = 0,
|
||||
kNudgeApproachAngle,
|
||||
kNudgeApproachPath
|
||||
};
|
||||
|
||||
// [us] Start time of the landing approach. If a fixed-wing landing pattern is used, this timer starts *after any
|
||||
// orbit to altitude only when the aircraft has entered the final *straight approach.
|
||||
hrt_abstime _time_started_landing{0};
|
||||
|
||||
// [m] lateral touchdown position offset manually commanded during landing
|
||||
float _lateral_touchdown_position_offset{0.0f};
|
||||
|
||||
// [m] relative vector from land point to approach entrance (NE)
|
||||
Vector2f _landing_approach_entrance_offset_vector{};
|
||||
|
||||
// [m] relative height above land point
|
||||
float _landing_approach_entrance_rel_alt{0.0f};
|
||||
|
||||
uint8_t _landing_abort_status{position_controller_landing_status_s::NOT_ABORTED};
|
||||
|
||||
// organize flare states XXX: need to split into a separate class at some point!
|
||||
struct FlareStates {
|
||||
bool flaring{false};
|
||||
hrt_abstime start_time{0}; // [us]
|
||||
float initial_height_rate_setpoint{0.0f}; // [m/s]
|
||||
} _flare_states;
|
||||
|
||||
// [m] last terrain estimate which was valid
|
||||
float _last_valid_terrain_alt_estimate{0.0f};
|
||||
|
||||
// [us] time at which we had last valid terrain alt
|
||||
hrt_abstime _last_time_terrain_alt_was_valid{0};
|
||||
|
||||
enum TerrainEstimateUseOnLanding {
|
||||
kDisableTerrainEstimation = 0,
|
||||
kTriggerFlareWithTerrainEstimate,
|
||||
kFollowTerrainRelativeLandingGlideSlope
|
||||
};
|
||||
|
||||
// AIRSPEED
|
||||
|
||||
float _airspeed_eas{0.f};
|
||||
bool _airspeed_valid{false};
|
||||
|
||||
// [us] last time airspeed was received. used to detect timeouts.
|
||||
hrt_abstime _time_airspeed_last_valid{0};
|
||||
|
||||
// WIND
|
||||
|
||||
// [m/s] wind velocity vector
|
||||
Vector2f _wind_vel{0.0f, 0.0f};
|
||||
|
||||
bool _wind_valid{false};
|
||||
|
||||
hrt_abstime _time_wind_last_received{0}; // [us]
|
||||
|
||||
// VTOL / TRANSITION
|
||||
matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
|
||||
float _backtrans_heading{NAN}; // used to lock the initial heading for backtransition with no position control
|
||||
|
||||
// ESTIMATOR RESET COUNTERS
|
||||
uint8_t _xy_reset_counter{0};
|
||||
uint64_t _time_last_xy_reset{0};
|
||||
|
||||
// LATERAL-DIRECTIONAL GUIDANCE
|
||||
|
||||
// CLosest point on path to track
|
||||
matrix::Vector2f _closest_point_on_path;
|
||||
|
||||
// nonlinear path following guidance - lateral-directional position control
|
||||
DirectionalGuidance _directional_guidance;
|
||||
|
||||
// LANDING GEAR
|
||||
int8_t _new_landing_gear_position{landing_gear_s::GEAR_KEEP};
|
||||
|
||||
// FLAPS/SPOILERS
|
||||
float _flaps_setpoint{0.f};
|
||||
float _spoilers_setpoint{0.f};
|
||||
|
||||
hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
|
||||
float _min_current_sp_distance_xy{FLT_MAX};
|
||||
|
||||
#ifdef CONFIG_FIGURE_OF_EIGHT
|
||||
/* Loitering */
|
||||
FigureEight _figure_eight;
|
||||
uORB::Publication<figure_eight_status_s> _figure_eight_status_pub {ORB_ID(figure_eight_status)};
|
||||
/**
|
||||
* Vehicle control for the autonomous figure 8 mode.
|
||||
*
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos the current 2D absolute position of the vehicle in [deg].
|
||||
* @param ground_speed the 2D ground speed of the vehicle in [m/s].
|
||||
* @param pos_sp_curr the current position setpoint.
|
||||
*/
|
||||
void controlAutoFigureEight(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
void publishFigureEightStatus(const position_setpoint_s pos_sp);
|
||||
#endif // CONFIG_FIGURE_OF_EIGHT
|
||||
|
||||
// Update our local parameter cache.
|
||||
void parameters_update();
|
||||
|
||||
// Update subscriptions
|
||||
void airspeed_poll();
|
||||
|
||||
void manual_control_setpoint_poll();
|
||||
void vehicle_attitude_poll();
|
||||
void vehicle_command_poll();
|
||||
void vehicle_control_mode_poll();
|
||||
|
||||
void wind_poll(const hrt_abstime now);
|
||||
|
||||
void landing_status_publish();
|
||||
|
||||
void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint);
|
||||
|
||||
/**
|
||||
* @brief Sets the landing abort status and publishes landing status.
|
||||
*
|
||||
* @param new_abort_status Either 0 (not aborted) or the singular bit >0 which triggered the abort
|
||||
*/
|
||||
void updateLandingAbortStatus(const uint8_t new_abort_status = position_controller_landing_status_s::NOT_ABORTED);
|
||||
|
||||
/**
|
||||
* @brief Checks if the automatic abort bitmask (from FW_LND_ABORT) contains the given abort criterion.
|
||||
*
|
||||
* @param automatic_abort_criteria_bitmask Bitmask containing all active abort criteria
|
||||
* @param landing_abort_criterion The specifc criterion we are checking for
|
||||
* @return true if the bitmask contains the criterion
|
||||
*/
|
||||
bool checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, uint8_t landing_abort_criterion);
|
||||
|
||||
/**
|
||||
* @brief Maps the manual control setpoint (pilot sticks) to height rate commands
|
||||
*
|
||||
* @return Manual height rate setpoint [m/s]
|
||||
*/
|
||||
float getManualHeightRateSetpoint();
|
||||
|
||||
/**
|
||||
* @brief Updates a state indicating whether a manual takeoff has been completed.
|
||||
*
|
||||
* Criteria include passing an airspeed threshold and not being in a landed state. VTOL airframes always pass.
|
||||
*/
|
||||
void updateManualTakeoffStatus();
|
||||
|
||||
/**
|
||||
* @brief Updates timing information for landed and in-air states.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
*/
|
||||
void update_in_air_states(const hrt_abstime now);
|
||||
|
||||
/**
|
||||
* @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in a VTOL
|
||||
* transition.
|
||||
*
|
||||
* @param[in,out] current_sp Current position setpoint
|
||||
*/
|
||||
void move_position_setpoint_for_vtol_transition(position_setpoint_s ¤t_sp);
|
||||
|
||||
/**
|
||||
* @brief Changes the position setpoint type to achieve the desired behavior in some instances.
|
||||
*
|
||||
* @param pos_sp_curr Current position setpoint
|
||||
* @return Adjusted position setpoint type
|
||||
*/
|
||||
uint8_t handle_setpoint_type(const position_setpoint_s &pos_sp_curr,
|
||||
const position_setpoint_s &pos_sp_next);
|
||||
|
||||
/* automatic control methods */
|
||||
|
||||
/**
|
||||
* @brief Automatic position control for waypoints, orbits, and velocity control
|
||||
*
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_prev previous position setpoint
|
||||
* @param pos_sp_curr current position setpoint
|
||||
* @param pos_sp_next next position setpoint
|
||||
*/
|
||||
void control_auto(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
|
||||
|
||||
/**
|
||||
* @brief Controls altitude and airspeed for a fixed-bank loiter.
|
||||
*
|
||||
* Used as a failsafe mode after a lateral position estimate failure.
|
||||
*/
|
||||
void control_auto_fixed_bank_alt_hold();
|
||||
|
||||
/**
|
||||
* @brief Control airspeed with a fixed descent rate and roll angle.
|
||||
*
|
||||
* Used as a failsafe mode after a lateral position estimate failure.
|
||||
*/
|
||||
void control_auto_descend();
|
||||
|
||||
/**
|
||||
* @brief Vehicle control for position waypoints.
|
||||
*
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_prev previous position setpoint
|
||||
* @param pos_sp_curr current position setpoint
|
||||
*/
|
||||
void control_auto_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
/**
|
||||
* @brief Vehicle control for loiter waypoints.
|
||||
*
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_curr current position setpoint
|
||||
* @param pos_sp_next next position setpoint
|
||||
*/
|
||||
void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Vehicle control for following a path.
|
||||
*
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_prev previous position setpoint
|
||||
* @param pos_sp_curr current position setpoint
|
||||
*/
|
||||
void control_auto_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
/**
|
||||
* @brief Controls a desired airspeed, bearing, and height rate.
|
||||
*
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_curr current position setpoint
|
||||
*/
|
||||
void control_auto_velocity(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
/**
|
||||
* @brief Controls automatic takeoff.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param global_position Vechile global position [deg]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_curr current position setpoint
|
||||
*/
|
||||
void control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &global_position,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
/**
|
||||
* @brief Controls automatic landing with straight approach.
|
||||
*
|
||||
* To be used in Missions that contain a loiter down followed by a land waypoint.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param control_interval Time since the last position control update [s]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_prev previous position setpoint
|
||||
* @param pos_sp_curr current position setpoint
|
||||
*/
|
||||
void control_auto_landing_straight(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
/**
|
||||
* @brief Controls automatic landing with circular final appraoch.
|
||||
*
|
||||
* To be used outside of Mission landings. Vehicle will orbit down around the landing position setpoint until flaring.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param control_interval Time since the last position control update [s]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_curr current position setpoint
|
||||
*/
|
||||
void control_auto_landing_circular(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
/* manual control methods */
|
||||
|
||||
/**
|
||||
* @brief Controls altitude and airspeed, user commands roll setpoint.
|
||||
*
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
*/
|
||||
void control_manual_altitude(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed);
|
||||
|
||||
/**
|
||||
* @brief Controls user commanded altitude, airspeed, and bearing.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
*/
|
||||
void control_manual_position(const hrt_abstime now, const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed);
|
||||
|
||||
/**
|
||||
* @brief Holds the initial heading during the course of a transition to hover. Used when there is no local
|
||||
* position to do line following.
|
||||
*/
|
||||
void control_backtransition_heading_hold();
|
||||
|
||||
/**
|
||||
* @brief Controls flying towards a transition waypoint and then transitioning to MC mode.
|
||||
*
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_curr current position setpoint
|
||||
*/
|
||||
void control_backtransition_line_follow(const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
float get_manual_airspeed_setpoint();
|
||||
|
||||
void reset_takeoff_state();
|
||||
void reset_landing_state();
|
||||
|
||||
/**
|
||||
* @brief Decides which control mode to execute.
|
||||
*
|
||||
* May also change the position setpoint type depending on the desired behavior.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
*/
|
||||
void set_control_mode_current(const hrt_abstime &now);
|
||||
|
||||
void publishOrbitStatus(const position_setpoint_s pos_sp);
|
||||
|
||||
float getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const;
|
||||
|
||||
/**
|
||||
* @brief Calculates the touchdown position for landing with optional manual lateral adjustments.
|
||||
*
|
||||
* Manual inputs (from the remote) are used to command a rate at which the position moves and the integrated
|
||||
* position is bounded. This is useful for manually adjusting the landing point in real time when map or GNSS
|
||||
* errors cause an offset from the desired landing vector.
|
||||
*
|
||||
* @param control_interval Time since the last position control update [s]
|
||||
* @param local_land_position Originally commanded local land position (NE) [m]
|
||||
* @return (Nudged) Local touchdown position (NE) [m]
|
||||
*/
|
||||
Vector2f calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position);
|
||||
|
||||
/**
|
||||
* @brief Calculates the vector from landing approach entrance to touchdown point
|
||||
*
|
||||
* NOTE: calculateTouchdownPosition() MUST be called before this method
|
||||
*
|
||||
* @return Landing approach vector [m]
|
||||
*/
|
||||
Vector2f calculateLandingApproachVector() const;
|
||||
|
||||
/**
|
||||
* @brief Returns a terrain altitude estimate with consideration of altimeter measurements.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param land_point_altitude Altitude (AMSL) of the land point [m]
|
||||
* @param abort_on_terrain_measurement_timeout Abort if distance to ground estimation doesn't get valid when we expect it to
|
||||
* @param abort_on_terrain_timeout Abort if distance to ground estimation is invalid after being valid before
|
||||
* @return Terrain altitude (AMSL) [m]
|
||||
*/
|
||||
float getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude,
|
||||
const bool abort_on_terrain_measurement_timeout, const bool abort_on_terrain_timeout);
|
||||
|
||||
/**
|
||||
* @brief Initializes landing states
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param pos_sp_prev Previous position setpoint
|
||||
* @param land_point_alt Landing point altitude setpoint AMSL [m]
|
||||
* @param local_position Local aircraft position (NE) [m]
|
||||
* @param local_land_point Local land point (NE) [m]
|
||||
*/
|
||||
void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
|
||||
const float land_point_alt, const Vector2f &local_position, const Vector2f &local_land_point);
|
||||
|
||||
/*
|
||||
* Waypoint handling logic following closely to the ECL_L1_Pos_Controller
|
||||
* method of the same name. Takes two waypoints, steering the vehicle to track
|
||||
* the line segment between them.
|
||||
*
|
||||
* @param[in] start_waypoint Segment starting position in local coordinates. (N,E) [m]
|
||||
* @param[in] end_waypoint Segment end position in local coordinates. (N,E) [m]
|
||||
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
|
||||
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
|
||||
* @param[in] wind_vel Wind velocity vector [m/s]
|
||||
*/
|
||||
DirectionalGuidanceOutput navigateWaypoints(const matrix::Vector2f &start_waypoint,
|
||||
const matrix::Vector2f &end_waypoint,
|
||||
const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
|
||||
const matrix::Vector2f &wind_vel);
|
||||
|
||||
/*
|
||||
* Takes one waypoint and steers the vehicle towards this.
|
||||
*
|
||||
* NOTE: this *will lead to "flowering" behavior if no higher level state machine or
|
||||
* switching condition changes the waypoint.
|
||||
*
|
||||
* @param[in] waypoint_pos Waypoint position in local coordinates. (N,E) [m]
|
||||
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
|
||||
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
|
||||
* @param[in] wind_vel Wind velocity vector [m/s]
|
||||
*/
|
||||
DirectionalGuidanceOutput navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos,
|
||||
const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);
|
||||
|
||||
/*
|
||||
* Line (infinite) following logic. Two points on the line are used to define the
|
||||
* line in 2D space (first to second point determines the direction). Determines the
|
||||
* relevant parameters for evaluating the NPFG guidance law, then updates control setpoints.
|
||||
*
|
||||
* @param[in] point_on_line_1 Arbitrary first position on line in local coordinates. (N,E) [m]
|
||||
* @param[in] point_on_line_2 Arbitrary second position on line in local coordinates. (N,E) [m]
|
||||
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
|
||||
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
|
||||
* @param[in] wind_vel Wind velocity vector [m/s]
|
||||
*/
|
||||
DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2,
|
||||
const Vector2f &vehicle_pos,
|
||||
const Vector2f &ground_vel, const Vector2f &wind_vel);
|
||||
|
||||
/*
|
||||
* Line (infinite) following logic. One point on the line and a line bearing are used to define
|
||||
* the line in 2D space. Determines the relevant parameters for evaluating the NPFG guidance law,
|
||||
* then updates control setpoints.
|
||||
*
|
||||
* @param[in] point_on_line Arbitrary position on line in local coordinates. (N,E) [m]
|
||||
* @param[in] line_bearing Line bearing [rad] (from north)
|
||||
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
|
||||
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
|
||||
* @param[in] wind_vel Wind velocity vector [m/s]
|
||||
*/
|
||||
DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line, const float line_bearing,
|
||||
const Vector2f &vehicle_pos,
|
||||
const Vector2f &ground_vel, const Vector2f &wind_vel);
|
||||
|
||||
/*
|
||||
* Loitering (unlimited) logic. Takes loiter center, radius, and direction and
|
||||
* determines the relevant parameters for evaluating the NPFG guidance law,
|
||||
* then updates control setpoints.
|
||||
*
|
||||
* @param[in] loiter_center The position of the center of the loiter circle [m]
|
||||
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
|
||||
* @param[in] radius Loiter radius [m]
|
||||
* @param[in] loiter_direction_counter_clockwise Specifies loiter direction
|
||||
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
|
||||
* @param[in] wind_vel Wind velocity vector [m/s]
|
||||
*/
|
||||
DirectionalGuidanceOutput navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
|
||||
float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
|
||||
const matrix::Vector2f &wind_vel);
|
||||
|
||||
/*
|
||||
* Path following logic. Takes poisiton, path tangent, curvature and
|
||||
* then updates control setpoints to follow a path setpoint.
|
||||
*
|
||||
* TODO: deprecate this function with a proper API to NPFG.
|
||||
*
|
||||
* @param[in] vehicle_pos vehicle_pos Vehicle position in local coordinates. (N,E) [m]
|
||||
* @param[in] position_setpoint closest point on a path in local coordinates. (N,E) [m]
|
||||
* @param[in] tangent_setpoint unit tangent vector of the path [m]
|
||||
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
|
||||
* @param[in] wind_vel Wind velocity vector [m/s]
|
||||
* @param[in] curvature of the path setpoint [1/m]
|
||||
*/
|
||||
DirectionalGuidanceOutput navigatePathTangent(const matrix::Vector2f &vehicle_pos,
|
||||
const matrix::Vector2f &position_setpoint,
|
||||
const matrix::Vector2f &tangent_setpoint,
|
||||
const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
|
||||
|
||||
/*
|
||||
* Navigate on a fixed bearing.
|
||||
*
|
||||
* This only holds a certain (ground relative) direction and does not perform
|
||||
* cross track correction. Helpful for semi-autonomous modes.
|
||||
*
|
||||
* @param[in] vehicle_pos vehicle_pos Vehicle position in local coordinates. (N,E) [m]
|
||||
* @param[in] bearing Bearing angle [rad]
|
||||
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
|
||||
* @param[in] wind_vel Wind velocity vector [m/s]
|
||||
*/
|
||||
DirectionalGuidanceOutput navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing,
|
||||
const matrix::Vector2f &ground_vel,
|
||||
const matrix::Vector2f &wind_vel);
|
||||
|
||||
void control_idle();
|
||||
void publish_lateral_guidance_status(const hrt_abstime now);
|
||||
|
||||
float rollAngleToLateralAccel(float roll_body) const;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,
|
||||
|
||||
(ParamFloat<px4::params::NPFG_PERIOD>) _param_npfg_period,
|
||||
(ParamFloat<px4::params::NPFG_DAMPING>) _param_npfg_damping,
|
||||
(ParamBool<px4::params::NPFG_LB_PERIOD>) _param_npfg_en_period_lb,
|
||||
(ParamBool<px4::params::NPFG_UB_PERIOD>) _param_npfg_en_period_ub,
|
||||
(ParamFloat<px4::params::NPFG_ROLL_TC>) _param_npfg_roll_time_const,
|
||||
(ParamFloat<px4::params::NPFG_SW_DST_MLT>) _param_npfg_switch_distance_multiplier,
|
||||
(ParamFloat<px4::params::NPFG_PERIOD_SF>) _param_npfg_period_safety_factor,
|
||||
|
||||
(ParamFloat<px4::params::FW_LND_AIRSPD>) _param_fw_lnd_airspd,
|
||||
(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
|
||||
(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,
|
||||
(ParamFloat<px4::params::FW_LND_FL_PMIN>) _param_fw_lnd_fl_pmin,
|
||||
(ParamFloat<px4::params::FW_LND_FLALT>) _param_fw_lnd_flalt,
|
||||
(ParamBool<px4::params::FW_LND_EARLYCFG>) _param_fw_lnd_earlycfg,
|
||||
(ParamInt<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
|
||||
|
||||
(ParamFloat<px4::params::FW_P_LIM_MAX>) _param_fw_p_lim_max,
|
||||
(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,
|
||||
(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
|
||||
(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
|
||||
(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
|
||||
(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
|
||||
(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
|
||||
(ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
|
||||
(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
|
||||
(ParamFloat<px4::params::FW_SPOILERS_LND>) _param_fw_spoilers_lnd,
|
||||
(ParamInt<px4::params::FW_POS_STK_CONF>) _param_fw_pos_stk_conf,
|
||||
(ParamInt<px4::params::FW_GPSF_LT>) _param_nav_gpsf_lt,
|
||||
(ParamFloat<px4::params::FW_GPSF_R>) _param_nav_gpsf_r,
|
||||
(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_t_spdweight,
|
||||
|
||||
// external parameters
|
||||
(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
|
||||
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
|
||||
(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
|
||||
(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
|
||||
(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
|
||||
(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,
|
||||
(ParamBool<px4::params::RWTO_NUDGE>) _param_rwto_nudge,
|
||||
(ParamFloat<px4::params::FW_LND_FL_TIME>) _param_fw_lnd_fl_time,
|
||||
(ParamFloat<px4::params::FW_LND_FL_SINK>) _param_fw_lnd_fl_sink,
|
||||
(ParamFloat<px4::params::FW_LND_TD_TIME>) _param_fw_lnd_td_time,
|
||||
(ParamFloat<px4::params::FW_LND_TD_OFF>) _param_fw_lnd_td_off,
|
||||
(ParamInt<px4::params::FW_LND_NUDGE>) _param_fw_lnd_nudge,
|
||||
(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort,
|
||||
(ParamFloat<px4::params::FW_TKO_AIRSPD>) _param_fw_tko_airspd,
|
||||
(ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp,
|
||||
(ParamBool<px4::params::FW_LAUN_DETCN_ON>) _param_fw_laun_detcn_on,
|
||||
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
|
||||
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
|
||||
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
|
||||
(ParamFloat<px4::params::FW_T_CLMB_MAX>) _param_fw_t_clmb_max
|
||||
)
|
||||
};
|
||||
|
||||
#endif // FIXEDWINGMODEMANAGER_HPP_
|
||||
@@ -0,0 +1,20 @@
|
||||
menuconfig MODULES_FW_MODE_MANAGER
|
||||
bool "fw_mode_manager"
|
||||
default n
|
||||
---help---
|
||||
Enable support for fw_mode_manager
|
||||
|
||||
menuconfig USER_FW_MODE_MANAGER
|
||||
bool "fw_mode_manager running as userspace module"
|
||||
default n
|
||||
depends on BOARD_PROTECTED && MODULES_FW_MODE_MANAGER
|
||||
---help---
|
||||
Put fw_mode_manager in userspace memory
|
||||
|
||||
menuconfig FIGURE_OF_EIGHT
|
||||
bool "fw_mode_manager figure of eight loiter support"
|
||||
default n
|
||||
depends on MODULES_FW_MODE_MANAGER
|
||||
---help---
|
||||
Enable support for the figure of eight loitering pattern in fixed wing.
|
||||
NOTE: Enable Mavlink development support to get feedback message.
|
||||
@@ -0,0 +1,36 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2022 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(figure_eight
|
||||
FigureEight.cpp
|
||||
)
|
||||
@@ -0,0 +1,428 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022-2025 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 FigureEight.cpp
|
||||
* Helper class for fixed wing position controller when flying a figure 8 loiter pattern.
|
||||
*/
|
||||
|
||||
#include "FigureEight.hpp"
|
||||
|
||||
#include "lib/geo/geo.h"
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
static constexpr float NORMALIZED_MAJOR_RADIUS{1.0f};
|
||||
static constexpr bool NORTH_CIRCLE_IS_COUNTER_CLOCKWISE{false};
|
||||
static constexpr bool SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE{true};
|
||||
static constexpr float DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO{2.5f};
|
||||
static constexpr float MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO{2.0f};
|
||||
|
||||
FigureEight::FigureEight(DirectionalGuidance &npfg, matrix::Vector2f &wind_vel) :
|
||||
ModuleParams(nullptr),
|
||||
_directional_guidance(npfg),
|
||||
_wind_vel(wind_vel)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void FigureEight::resetPattern()
|
||||
{
|
||||
// Set the current segment invalid
|
||||
_current_segment = FigureEightSegment::SEGMENT_UNDEFINED;
|
||||
_pos_passed_circle_center_along_major_axis = false;
|
||||
}
|
||||
|
||||
DirectionalGuidanceOutput FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local,
|
||||
const matrix::Vector2f &ground_speed,
|
||||
const FigureEightPatternParameters ¶meters)
|
||||
{
|
||||
// Sanitize inputs
|
||||
FigureEightPatternParameters valid_parameters{sanitizeParameters(parameters)};
|
||||
|
||||
// Calculate the figure eight pattern points.
|
||||
FigureEightPatternPoints pattern_points;
|
||||
calculateFigureEightPoints(pattern_points, valid_parameters);
|
||||
|
||||
// Do the figure of eight initialization if needed.
|
||||
initializePattern(curr_pos_local, ground_speed, valid_parameters, pattern_points);
|
||||
|
||||
// Check if we need to switch to next segment
|
||||
updateSegment(curr_pos_local, valid_parameters, pattern_points);
|
||||
|
||||
// Apply control logic based on segment
|
||||
return applyControl(curr_pos_local, ground_speed, valid_parameters, pattern_points);
|
||||
}
|
||||
|
||||
FigureEight::FigureEightPatternParameters FigureEight::sanitizeParameters(const FigureEightPatternParameters
|
||||
¶meters)
|
||||
{
|
||||
FigureEightPatternParameters valid_parameters{parameters};
|
||||
|
||||
if (!PX4_ISFINITE(parameters.loiter_minor_radius)) {
|
||||
valid_parameters.loiter_minor_radius = fabsf(_param_nav_loiter_rad.get());
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(parameters.loiter_radius)) {
|
||||
valid_parameters.loiter_radius = DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO * valid_parameters.loiter_minor_radius;
|
||||
valid_parameters.loiter_direction_counter_clockwise = _param_nav_loiter_rad.get() < 0;
|
||||
}
|
||||
|
||||
valid_parameters.loiter_radius = math::max(valid_parameters.loiter_radius,
|
||||
MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO * valid_parameters.loiter_minor_radius);
|
||||
|
||||
return valid_parameters;
|
||||
}
|
||||
|
||||
void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
|
||||
const FigureEightPatternParameters ¶meters, FigureEightPatternPoints pattern_points)
|
||||
{
|
||||
// Initialize the currently active segment, if it hasn't been active yet, or the pattern has been changed.
|
||||
if ((_current_segment == FigureEightSegment::SEGMENT_UNDEFINED) || (_active_parameters != parameters)) {
|
||||
Vector2f center_to_pos_local;
|
||||
calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters);
|
||||
Vector2f ground_speed_rotated = Dcm2f(-calculateRotationAngle(parameters)) * ground_speed;
|
||||
|
||||
Vector2f north_center_to_pos_local{center_to_pos_local - pattern_points.normalized_north_circle_offset};
|
||||
Vector2f south_center_to_pos_local{center_to_pos_local - pattern_points.normalized_south_circle_offset};
|
||||
const bool north_is_closer = north_center_to_pos_local.norm() < south_center_to_pos_local.norm();
|
||||
|
||||
// Get the normalized switch distance.
|
||||
float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS /
|
||||
parameters.loiter_radius;
|
||||
|
||||
//Far away from current figure of eight. Fly towards closer circle
|
||||
|
||||
if (center_to_pos_local.norm() > NORMALIZED_MAJOR_RADIUS + switch_distance_normalized) {
|
||||
if (north_is_closer) {
|
||||
_current_segment = FigureEightSegment::SEGMENT_CIRCLE_NORTH;
|
||||
|
||||
} else {
|
||||
_current_segment = FigureEightSegment::SEGMENT_CIRCLE_SOUTH;
|
||||
}
|
||||
|
||||
_pos_passed_circle_center_along_major_axis = true;
|
||||
|
||||
} else {
|
||||
if (north_is_closer) {
|
||||
const bool is_circling_counter_clockwise{north_center_to_pos_local.cross(ground_speed_rotated) < 0.f};
|
||||
|
||||
if ((ground_speed_rotated(0) > 0.f) && (is_circling_counter_clockwise == NORTH_CIRCLE_IS_COUNTER_CLOCKWISE)) {
|
||||
// Flying north and right rotation
|
||||
_current_segment = FigureEightSegment::SEGMENT_CIRCLE_NORTH;
|
||||
_pos_passed_circle_center_along_major_axis = true;
|
||||
|
||||
} else {
|
||||
// Flying to the entry of the south circle
|
||||
_current_segment = FigureEightSegment::SEGMENT_POINT_SOUTHWEST;
|
||||
_pos_passed_circle_center_along_major_axis = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
const bool is_circling_counter_clockwise{south_center_to_pos_local.cross(ground_speed_rotated) < 0.f};
|
||||
|
||||
if ((ground_speed_rotated(0) < 0.f) && (is_circling_counter_clockwise == SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE)) {
|
||||
// Flying south and right rotation
|
||||
_current_segment = FigureEightSegment::SEGMENT_CIRCLE_SOUTH;
|
||||
_pos_passed_circle_center_along_major_axis = true;
|
||||
|
||||
} else {
|
||||
// Flying to the entry of the north circle
|
||||
_current_segment = FigureEightSegment::SEGMENT_POINT_NORTHWEST;
|
||||
_pos_passed_circle_center_along_major_axis = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_active_parameters = parameters;
|
||||
}
|
||||
}
|
||||
|
||||
void FigureEight::calculateFigureEightPoints(FigureEightPatternPoints &pattern_points,
|
||||
const FigureEightPatternParameters ¶meters)
|
||||
{
|
||||
const float normalized_minor_radius = (parameters.loiter_minor_radius / parameters.loiter_radius) *
|
||||
NORMALIZED_MAJOR_RADIUS;
|
||||
const float cos_transition_angle = parameters.loiter_minor_radius / (parameters.loiter_radius -
|
||||
parameters.loiter_minor_radius);
|
||||
const float sin_transition_angle = sqrtf(1.0f - cos_transition_angle * cos_transition_angle);
|
||||
pattern_points.normalized_north_circle_offset = Vector2f{NORMALIZED_MAJOR_RADIUS - normalized_minor_radius, 0.0f};
|
||||
pattern_points.normalized_north_entry_offset = Vector2f{NORMALIZED_MAJOR_RADIUS - ((normalized_minor_radius) * (1.0f + cos_transition_angle)),
|
||||
-normalized_minor_radius * sin_transition_angle};
|
||||
pattern_points.normalized_north_exit_offset = Vector2f{NORMALIZED_MAJOR_RADIUS - ((normalized_minor_radius) * (1.0f + cos_transition_angle)),
|
||||
normalized_minor_radius * sin_transition_angle};
|
||||
pattern_points.normalized_south_circle_offset = Vector2f{-NORMALIZED_MAJOR_RADIUS + normalized_minor_radius, 0.0f};
|
||||
pattern_points.normalized_south_entry_offset = Vector2f{-NORMALIZED_MAJOR_RADIUS + ((normalized_minor_radius) * (1.0f + cos_transition_angle)),
|
||||
-normalized_minor_radius * sin_transition_angle};
|
||||
pattern_points.normalized_south_exit_offset = Vector2f{-NORMALIZED_MAJOR_RADIUS + ((normalized_minor_radius) * (1.0f + cos_transition_angle)),
|
||||
normalized_minor_radius * sin_transition_angle};
|
||||
}
|
||||
|
||||
void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters ¶meters,
|
||||
const FigureEightPatternPoints &pattern_points)
|
||||
{
|
||||
Vector2f center_to_pos_local;
|
||||
calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters);
|
||||
|
||||
// Get the normalized switch distance.
|
||||
float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS /
|
||||
parameters.loiter_radius;
|
||||
|
||||
// Update segment if segment exit condition has been reached
|
||||
switch (_current_segment) {
|
||||
case FigureEightSegment::SEGMENT_CIRCLE_NORTH: {
|
||||
if (center_to_pos_local(0) > pattern_points.normalized_north_circle_offset(0)) {
|
||||
_pos_passed_circle_center_along_major_axis = true;
|
||||
}
|
||||
|
||||
Vector2f vector_to_exit_normalized = pattern_points.normalized_north_exit_offset - center_to_pos_local;
|
||||
|
||||
/* Exit condition: Switch distance away from north-east point of north circle and at least once was above the circle center. Failsafe action, if poor tracking,
|
||||
- switch to next if the vehicle is on the east side and below the north exit point. */
|
||||
if (_pos_passed_circle_center_along_major_axis &&
|
||||
((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
|
||||
((center_to_pos_local(0) < pattern_points.normalized_north_exit_offset(0)) &&
|
||||
(center_to_pos_local(1) > FLT_EPSILON) &&
|
||||
(center_to_pos_local.norm() < NORMALIZED_MAJOR_RADIUS)))) {
|
||||
_current_segment = FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case FigureEightSegment::SEGMENT_POINT_SOUTHWEST: // fall through
|
||||
case FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST: {
|
||||
_pos_passed_circle_center_along_major_axis = false;
|
||||
Vector2f vector_to_exit_normalized = pattern_points.normalized_south_entry_offset - center_to_pos_local;
|
||||
|
||||
/* Exit condition: Switch distance away from south-west point of south circle. Failsafe action, if poor tracking,
|
||||
switch to next if the vehicle is on the west side and below entry point of the south circle or has left the radius. */
|
||||
if ((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
|
||||
((center_to_pos_local(0) < pattern_points.normalized_south_entry_offset(0)) && (center_to_pos_local(1) < FLT_EPSILON))
|
||||
||
|
||||
(center_to_pos_local(0) < -NORMALIZED_MAJOR_RADIUS)) {
|
||||
_current_segment = FigureEightSegment::SEGMENT_CIRCLE_SOUTH;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case FigureEightSegment::SEGMENT_CIRCLE_SOUTH: {
|
||||
if (center_to_pos_local(0) < pattern_points.normalized_south_circle_offset(0)) {
|
||||
_pos_passed_circle_center_along_major_axis = true;
|
||||
}
|
||||
|
||||
Vector2f vector_to_exit_normalized = pattern_points.normalized_south_exit_offset - center_to_pos_local;
|
||||
|
||||
/* Exit condition: Switch distance away from south-east point of south circle and at least once was below the circle center. Failsafe action, if poor tracking,
|
||||
- switch to next if the vehicle is on the east side and above the south exit point. */
|
||||
if (_pos_passed_circle_center_along_major_axis &&
|
||||
((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
|
||||
((center_to_pos_local(0) > pattern_points.normalized_south_exit_offset(0)) &&
|
||||
(center_to_pos_local(1) > FLT_EPSILON) &&
|
||||
(center_to_pos_local.norm() < NORMALIZED_MAJOR_RADIUS)))) {
|
||||
_current_segment = FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST;
|
||||
}
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
case FigureEightSegment::SEGMENT_POINT_NORTHWEST: // Fall through
|
||||
case FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST: {
|
||||
_pos_passed_circle_center_along_major_axis = false;
|
||||
Vector2f vector_to_exit_normalized = pattern_points.normalized_north_entry_offset - center_to_pos_local;
|
||||
|
||||
/* Exit condition: Switch distance away from north-west point of north circle. Failsafe action, if poor tracking,
|
||||
switch to next if the vehicle is on the west side and above entry point of the north circle or has left the radius. */
|
||||
if ((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
|
||||
((center_to_pos_local(0) > pattern_points.normalized_north_entry_offset(0)) && (center_to_pos_local(1) < FLT_EPSILON))
|
||||
||
|
||||
(center_to_pos_local(0) > NORMALIZED_MAJOR_RADIUS)) {
|
||||
_current_segment = FigureEightSegment::SEGMENT_CIRCLE_NORTH;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case FigureEightSegment::SEGMENT_UNDEFINED:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
DirectionalGuidanceOutput FigureEight::applyControl(const matrix::Vector2f &curr_pos_local,
|
||||
const matrix::Vector2f &ground_speed,
|
||||
const FigureEightPatternParameters ¶meters,
|
||||
const FigureEightPatternPoints &pattern_points)
|
||||
{
|
||||
Vector2f center_to_pos_local;
|
||||
calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters);
|
||||
|
||||
switch (_current_segment) {
|
||||
case FigureEightSegment::SEGMENT_CIRCLE_NORTH: {
|
||||
return applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local,
|
||||
ground_speed, parameters);
|
||||
}
|
||||
break;
|
||||
|
||||
case FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST: {
|
||||
// Follow path from north-east to south-west
|
||||
return applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset,
|
||||
curr_pos_local,
|
||||
ground_speed, parameters);
|
||||
}
|
||||
break;
|
||||
|
||||
case FigureEightSegment::SEGMENT_CIRCLE_SOUTH: {
|
||||
return applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local,
|
||||
ground_speed, parameters);
|
||||
}
|
||||
break;
|
||||
|
||||
case FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST: {
|
||||
// follow path from south-east to north-west
|
||||
return applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset,
|
||||
curr_pos_local,
|
||||
ground_speed, parameters);
|
||||
}
|
||||
break;
|
||||
|
||||
case FigureEightSegment::SEGMENT_POINT_SOUTHWEST: {
|
||||
// Follow path from current position to south-west
|
||||
return applyLine(center_to_pos_local, pattern_points.normalized_south_entry_offset, curr_pos_local,
|
||||
ground_speed, parameters);
|
||||
}
|
||||
break;
|
||||
|
||||
case FigureEightSegment::SEGMENT_POINT_NORTHWEST: {
|
||||
// Follow path from current position to north-west
|
||||
return applyLine(center_to_pos_local, pattern_points.normalized_north_entry_offset, curr_pos_local,
|
||||
ground_speed, parameters);
|
||||
}
|
||||
break;
|
||||
|
||||
case FigureEightSegment::SEGMENT_UNDEFINED:
|
||||
default:
|
||||
return DirectionalGuidanceOutput{};
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void FigureEight::calculatePositionToCenterNormalizedRotated(matrix::Vector2f ¢er_to_pos_local_normalized_rotated,
|
||||
const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters ¶meters) const
|
||||
{
|
||||
Vector2f center_to_pos_local = curr_pos_local - parameters.center_pos_local;
|
||||
|
||||
// normalize position with respect to radius
|
||||
Vector2f center_to_pos_local_normalized;
|
||||
center_to_pos_local_normalized(0) = center_to_pos_local(0) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
|
||||
center_to_pos_local_normalized(1) = center_to_pos_local(1) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
|
||||
|
||||
// rotate position with respect to figure eight orientation and direction.
|
||||
center_to_pos_local_normalized_rotated = Dcm2f(-calculateRotationAngle(parameters)) * center_to_pos_local_normalized;
|
||||
}
|
||||
|
||||
float FigureEight::calculateRotationAngle(const FigureEightPatternParameters ¶meters) const
|
||||
{
|
||||
// rotate position with respect to figure eight orientation and direction.
|
||||
float yaw_rotation = parameters.loiter_orientation;
|
||||
|
||||
// figure eight pattern is symmetric, changing the direction is the same as a rotation by 180° around center
|
||||
if (parameters.loiter_direction_counter_clockwise) {
|
||||
yaw_rotation += M_PI_F;
|
||||
}
|
||||
|
||||
return yaw_rotation;
|
||||
}
|
||||
|
||||
DirectionalGuidanceOutput FigureEight::applyCircle(bool loiter_direction_counter_clockwise,
|
||||
const matrix::Vector2f &normalized_circle_offset,
|
||||
const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
|
||||
const FigureEightPatternParameters ¶meters)
|
||||
{
|
||||
const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
|
||||
|
||||
Vector2f circle_offset = normalized_circle_offset * (parameters.loiter_radius / NORMALIZED_MAJOR_RADIUS);
|
||||
Vector2f circle_offset_rotated = Dcm2f(calculateRotationAngle(parameters)) * circle_offset;
|
||||
Vector2f circle_center = parameters.center_pos_local + circle_offset_rotated;
|
||||
|
||||
|
||||
const Vector2f vector_center_to_vehicle = curr_pos_local - circle_center;
|
||||
const float dist_to_center = vector_center_to_vehicle.norm();
|
||||
|
||||
Vector2f unit_vec_center_to_closest_pt = vector_center_to_vehicle.normalized();
|
||||
|
||||
if (dist_to_center < 0.1f) {
|
||||
// the logic breaks down at the circle center, employ some mitigation strategies
|
||||
// until we exit this region
|
||||
if (ground_speed.norm() < 0.1f) {
|
||||
// arbitrarily set the point in the northern top of the circle
|
||||
unit_vec_center_to_closest_pt = Vector2f{1.0f, 0.0f};
|
||||
|
||||
} else {
|
||||
// set the point in the direction we are moving
|
||||
unit_vec_center_to_closest_pt = ground_speed.normalized();
|
||||
}
|
||||
}
|
||||
|
||||
const Vector2f unit_path_tangent = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
|
||||
|
||||
float path_curvature = loiter_direction_multiplier / parameters.loiter_minor_radius;
|
||||
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
|
||||
_closest_point_on_path = unit_vec_center_to_closest_pt * parameters.loiter_minor_radius + circle_center;
|
||||
return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent,
|
||||
_closest_point_on_path, path_curvature);
|
||||
|
||||
}
|
||||
|
||||
DirectionalGuidanceOutput FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset,
|
||||
const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local,
|
||||
const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters)
|
||||
{
|
||||
const Dcm2f rotation_matrix(calculateRotationAngle(parameters));
|
||||
|
||||
// Calculate start offset depending on radius
|
||||
const Vector2f start_offset = normalized_line_start_offset * (parameters.loiter_radius / NORMALIZED_MAJOR_RADIUS);
|
||||
const Vector2f start_offset_rotated = rotation_matrix * start_offset;
|
||||
const Vector2f line_segment_start_position = parameters.center_pos_local + start_offset_rotated;
|
||||
|
||||
const Vector2f end_offset = normalized_line_end_offset * (parameters.loiter_radius / NORMALIZED_MAJOR_RADIUS);
|
||||
const Vector2f end_offset_rotated = rotation_matrix * end_offset;
|
||||
const Vector2f line_segment_end_position = parameters.center_pos_local + end_offset_rotated;
|
||||
|
||||
const Vector2f path_tangent = line_segment_end_position - line_segment_start_position;
|
||||
const Vector2f unit_path_tangent = path_tangent.normalized();
|
||||
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
|
||||
const Vector2f vector_A_to_vehicle = curr_pos_local - line_segment_start_position;
|
||||
_closest_point_on_path = line_segment_start_position + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
|
||||
return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(),
|
||||
line_segment_start_position,
|
||||
0.0f);
|
||||
}
|
||||
@@ -0,0 +1,274 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 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 FigureEight.hpp
|
||||
* Helper class for lateral fixed wing position controller when flying a figure 8 loiter pattern.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef FIGUREEIGHT_HPP_
|
||||
#define FIGUREEIGHT_HPP_
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/param.h>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
|
||||
#include "lib/npfg/DirectionalGuidance.hpp"
|
||||
|
||||
class FigureEight : public ModuleParams
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Figure eight pattern points strust
|
||||
*
|
||||
* Struct defining all relevant points for the figure eight pattern.
|
||||
*
|
||||
*/
|
||||
struct FigureEightPatternPoints {
|
||||
matrix::Vector2f normalized_north_circle_offset;
|
||||
matrix::Vector2f normalized_north_entry_offset;
|
||||
matrix::Vector2f normalized_north_exit_offset;
|
||||
matrix::Vector2f normalized_south_circle_offset;
|
||||
matrix::Vector2f normalized_south_entry_offset;
|
||||
matrix::Vector2f normalized_south_exit_offset;
|
||||
};
|
||||
struct FigureEightPatternParameters {
|
||||
matrix::Vector2f center_pos_local;
|
||||
float loiter_radius;
|
||||
float loiter_minor_radius;
|
||||
float loiter_orientation;
|
||||
bool loiter_direction_counter_clockwise;
|
||||
|
||||
bool operator!=(const FigureEightPatternParameters &other) const
|
||||
{
|
||||
return ((fabsf(center_pos_local(0) - other.center_pos_local(0)) > FLT_EPSILON) ||
|
||||
(fabsf(center_pos_local(1) - other.center_pos_local(1)) > FLT_EPSILON) ||
|
||||
(fabsf(loiter_radius - other.loiter_radius) > FLT_EPSILON) ||
|
||||
(fabsf(loiter_minor_radius - other.loiter_minor_radius) > FLT_EPSILON) ||
|
||||
(fabsf(loiter_orientation - other.loiter_orientation) > FLT_EPSILON) ||
|
||||
(loiter_direction_counter_clockwise != other.loiter_direction_counter_clockwise));
|
||||
};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Construct a new Figure Eight object
|
||||
*
|
||||
* @param[in] npfg is the reference to the parent npfg object.
|
||||
* @param[in] wind_vel is the reference to the parent wind velocity [m/s].
|
||||
*/
|
||||
FigureEight(DirectionalGuidance &directional_guidance, matrix::Vector2f &wind_vel);
|
||||
|
||||
/**
|
||||
* @brief reset the figure eight pattern.
|
||||
*
|
||||
* Reset the figure eight pattern such that it can be properly initialized on a new figure eight pattern.
|
||||
*
|
||||
*/
|
||||
void resetPattern();
|
||||
|
||||
/**
|
||||
* @brief Update roll setpoint
|
||||
*
|
||||
* @param[in] curr_pos_local is the current local position of the vehicle in [m].
|
||||
* @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
|
||||
* @param[in] parameters is the parameter set defining the figure eight shape.
|
||||
*/
|
||||
DirectionalGuidanceOutput updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
|
||||
const FigureEightPatternParameters ¶meters);
|
||||
/**
|
||||
* @brief Get the target bearing of current point on figure of eight
|
||||
*
|
||||
* @return target bearing in [rad]
|
||||
*/
|
||||
float getTargetBearing() const {return _target_bearing;};
|
||||
/**
|
||||
* @brief Get the closest point on the figure of eight
|
||||
*
|
||||
* @return Local coordinates of closes point on the figure of eight
|
||||
*/
|
||||
matrix::Vector2f getClosestPoint() const {return _closest_point_on_path;};
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief
|
||||
*
|
||||
* @param[in] parameters are gotten the figure of eight parameters
|
||||
* @return are the sanitized figure of eight parameters
|
||||
*/
|
||||
FigureEightPatternParameters sanitizeParameters(const FigureEightPatternParameters ¶meters);
|
||||
|
||||
/**
|
||||
* @brief Initialize the figure eight pattern.
|
||||
*
|
||||
* Initialize the figure eight pattern by determining the current active segment.
|
||||
*
|
||||
* @param[in] curr_pos_local is the current local position of the vehicle in [m].
|
||||
* @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
|
||||
* @param[in] parameters is the parameter set defining the figure eight shape.
|
||||
* @param[in] pattern_points are the figure of eight pattern points.
|
||||
*/
|
||||
void initializePattern(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
|
||||
const FigureEightPatternParameters ¶meters, FigureEightPatternPoints pattern_points);
|
||||
|
||||
/**
|
||||
* @brief Calculate figure eight pattern points
|
||||
*
|
||||
* @param[out] pattern_points is the output with the calculated points for the figure eight.
|
||||
* @param[in] parameters is the parameter set defining the figure eight shape.
|
||||
*/
|
||||
void calculateFigureEightPoints(FigureEightPatternPoints &pattern_points,
|
||||
const FigureEightPatternParameters ¶meters);
|
||||
/**
|
||||
* @brief Apply lateral control logic
|
||||
*
|
||||
*
|
||||
* @param[in] curr_pos_local is the current local position of the vehicle in [m].
|
||||
* @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
|
||||
* @param[in] parameters is the parameter set defining the figure eight shape.
|
||||
* @param[in] pattern_points are the relevant points defining the figure eight pattern.
|
||||
*/
|
||||
DirectionalGuidanceOutput applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
|
||||
const FigureEightPatternParameters ¶meters,
|
||||
const FigureEightPatternPoints &pattern_points);
|
||||
/**
|
||||
* @brief Update active segment.
|
||||
*
|
||||
* @param[in] curr_pos_local is the current local position of the vehicle in [m].
|
||||
* @param[in] parameters is the parameter set defining the figure eight shape.
|
||||
* @param[in] pattern_points are the relevant points defining the figure eight pattern.
|
||||
*/
|
||||
void updateSegment(const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters ¶meters,
|
||||
const FigureEightPatternPoints &pattern_points);
|
||||
/**
|
||||
* @brief calculate normalized and rotated relative vehicle position to pattern center.
|
||||
*
|
||||
* @param[out] center_to_pos_local_normalized_rotated is the calculated normalized and rotated relative vehicle position with respect to the pattern center.
|
||||
* @param[in] curr_pos_local is the current local position of the vehicle in [m].
|
||||
* @param[in] parameters is the parameter set defining the figure eight shape.
|
||||
*/
|
||||
void calculatePositionToCenterNormalizedRotated(matrix::Vector2f ¢er_to_pos_local_normalized_rotated,
|
||||
const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters ¶meters) const;
|
||||
/**
|
||||
* @brief Calculate rotation angle.
|
||||
*
|
||||
* @param[in] parameters is the parameter set defining the figure eight shape.
|
||||
*
|
||||
* @return is the rotation angle of the major axis compared to north in [rad].
|
||||
*/
|
||||
float calculateRotationAngle(const FigureEightPatternParameters ¶meters) const;
|
||||
/**
|
||||
* @brief Apply circular lateral control
|
||||
*
|
||||
* @param[in] loiter_direction_counter_clockwise flag if the circle direction should be counter-clockwise.
|
||||
* @param[in] normalized_circle_offset is the normalized position offset of the circle compared to the pattern center.
|
||||
* @param[in] curr_pos_local is the current local position of the vehicle in [m].
|
||||
* @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
|
||||
* @param[in] parameters is the parameter set defining the figure eight shape.
|
||||
*/
|
||||
DirectionalGuidanceOutput applyCircle(bool loiter_direction_counter_clockwise,
|
||||
const matrix::Vector2f &normalized_circle_offset,
|
||||
const matrix::Vector2f &curr_pos_local,
|
||||
const matrix::Vector2f &ground_speed, const FigureEightPatternParameters ¶meters);
|
||||
/**
|
||||
* @brief Apply path lateral control
|
||||
*
|
||||
* @param[in] normalized_line_start_offset is the normalized position offset of the start point of the path compared to the pattern center.
|
||||
* @param[in] normalized_line_end_offset is the normalized position offset of the end point of the path compared to the pattern center.
|
||||
* @param[in] curr_pos_local is the current local position of the vehicle in [m].
|
||||
* @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
|
||||
* @param[in] parameters is the parameter set defining the figure eight shape.
|
||||
*/
|
||||
DirectionalGuidanceOutput applyLine(const matrix::Vector2f &normalized_line_start_offset,
|
||||
const matrix::Vector2f &normalized_line_end_offset,
|
||||
const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
|
||||
const FigureEightPatternParameters ¶meters);
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief npfg lateral control object.
|
||||
*
|
||||
*/
|
||||
DirectionalGuidance &_directional_guidance;
|
||||
|
||||
/**
|
||||
* @brief Wind velocity in [m/s].
|
||||
*
|
||||
*/
|
||||
const matrix::Vector2f &_wind_vel;
|
||||
/**
|
||||
* @brief active figure eight position setpoint.
|
||||
*
|
||||
*/
|
||||
FigureEightPatternParameters _active_parameters;
|
||||
|
||||
/**
|
||||
* @brief Target bearing in [rad].
|
||||
*
|
||||
*/
|
||||
float _target_bearing{0.0f};
|
||||
|
||||
/**
|
||||
* @brief Closest point on figure of eight to track
|
||||
*
|
||||
*/
|
||||
matrix::Vector2f _closest_point_on_path;
|
||||
|
||||
enum class FigureEightSegment {
|
||||
SEGMENT_UNDEFINED,
|
||||
SEGMENT_CIRCLE_NORTH,
|
||||
SEGMENT_NORTHEAST_SOUTHWEST,
|
||||
SEGMENT_CIRCLE_SOUTH,
|
||||
SEGMENT_SOUTHEAST_NORTHWEST,
|
||||
SEGMENT_POINT_SOUTHWEST,
|
||||
SEGMENT_POINT_NORTHWEST
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Current active segment of the figure eight pattern.
|
||||
*
|
||||
*/
|
||||
FigureEightSegment _current_segment{FigureEightSegment::SEGMENT_UNDEFINED};
|
||||
/**
|
||||
* @brief flag if vehicle position passed circle center along major axis when on circle segment.
|
||||
*
|
||||
*/
|
||||
bool _pos_passed_circle_center_along_major_axis;
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
|
||||
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad
|
||||
)
|
||||
};
|
||||
|
||||
#endif // FIGUREEIGHT_HPP_
|
||||
@@ -0,0 +1,626 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2025 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* NPFG period
|
||||
*
|
||||
* Period of NPFG control law.
|
||||
*
|
||||
* @unit s
|
||||
* @min 1.0
|
||||
* @max 100.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW NPFG Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NPFG_PERIOD, 10.0f);
|
||||
|
||||
/**
|
||||
* NPFG damping ratio
|
||||
*
|
||||
* Damping ratio of NPFG control law.
|
||||
*
|
||||
* @min 0.10
|
||||
* @max 1.00
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW NPFG Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NPFG_DAMPING, 0.7f);
|
||||
|
||||
/**
|
||||
* Enable automatic lower bound on the NPFG period
|
||||
*
|
||||
* Avoids limit cycling from a too aggressively tuned period/damping combination.
|
||||
* If false, also disables upper bound NPFG_PERIOD_UB.
|
||||
*
|
||||
* @boolean
|
||||
* @group FW NPFG Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NPFG_LB_PERIOD, 1);
|
||||
|
||||
/**
|
||||
* Enable automatic upper bound on the NPFG period
|
||||
*
|
||||
* Adapts period to maintain track keeping in variable winds and path curvature.
|
||||
*
|
||||
* @boolean
|
||||
* @group FW NPFG Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NPFG_UB_PERIOD, 1);
|
||||
|
||||
/**
|
||||
* Roll time constant
|
||||
*
|
||||
* Time constant of roll controller command / response, modeled as first order delay.
|
||||
* Used to determine lower period bound. Setting zero disables automatic period bounding.
|
||||
*
|
||||
* @unit s
|
||||
* @min 0.00
|
||||
* @max 2.00
|
||||
* @decimal 2
|
||||
* @increment 0.05
|
||||
* @group FW NPFG Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NPFG_ROLL_TC, 0.5f);
|
||||
|
||||
/**
|
||||
* NPFG switch distance multiplier
|
||||
*
|
||||
* Multiplied by the track error boundary to determine when the aircraft switches
|
||||
* to the next waypoint and/or path segment. Should be less than 1.
|
||||
*
|
||||
* @min 0.1
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW NPFG Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NPFG_SW_DST_MLT, 0.32f);
|
||||
|
||||
/**
|
||||
* Period safety factor
|
||||
*
|
||||
* Multiplied by period for conservative minimum period bounding (when period lower
|
||||
* bounding is enabled). 1.0 bounds at marginal stability.
|
||||
*
|
||||
* @min 1.0
|
||||
* @max 10.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW NPFG Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NPFG_PERIOD_SF, 1.5f);
|
||||
|
||||
|
||||
/**
|
||||
* Minimum pitch angle setpoint
|
||||
*
|
||||
* Applies in any altitude controlled flight mode.
|
||||
*
|
||||
* @unit deg
|
||||
* @min -60.0
|
||||
* @max 0.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -30.0f);
|
||||
|
||||
/**
|
||||
* Maximum pitch angle setpoint
|
||||
*
|
||||
* Applies in any altitude controlled flight mode.
|
||||
*
|
||||
* @unit deg
|
||||
* @min 0.0
|
||||
* @max 60.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 30.0f);
|
||||
|
||||
/**
|
||||
* Maximum roll angle setpoint
|
||||
*
|
||||
* Applies in any altitude controlled flight mode.
|
||||
*
|
||||
* @unit deg
|
||||
* @min 35.0
|
||||
* @max 65.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f);
|
||||
|
||||
/**
|
||||
* Throttle limit max
|
||||
*
|
||||
* Applies in any altitude controlled flight mode.
|
||||
* Should be set accordingly to achieve FW_T_CLMB_MAX.
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Throttle limit min
|
||||
*
|
||||
* Applies in any altitude controlled flight mode.
|
||||
* Usually set to 0 but can be increased to prevent the motor from stopping when
|
||||
* descending, which can increase achievable descent rates.
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
|
||||
|
||||
/**
|
||||
* Idle throttle
|
||||
*
|
||||
* This is the minimum throttle while on the ground ("landed") in auto modes.
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 0.4
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.0f);
|
||||
|
||||
/**
|
||||
* Maximum landing slope angle
|
||||
*
|
||||
* Typically the desired landing slope angle when landing configuration (flaps, airspeed) is enabled.
|
||||
* Set this value within the vehicle's performance limits.
|
||||
*
|
||||
* @unit deg
|
||||
* @min 1.0
|
||||
* @max 15.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
|
||||
|
||||
/**
|
||||
* Minimum pitch during takeoff.
|
||||
*
|
||||
* @unit deg
|
||||
* @min -5.0
|
||||
* @max 30.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group FW Auto Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f);
|
||||
|
||||
/**
|
||||
* Takeoff Airspeed
|
||||
*
|
||||
* The calibrated airspeed setpoint during the takeoff climbout.
|
||||
*
|
||||
* If set <= 0, FW_AIRSPD_MIN will be set by default.
|
||||
*
|
||||
* @unit m/s
|
||||
* @min -1.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW Auto Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_TKO_AIRSPD, -1.0f);
|
||||
|
||||
/**
|
||||
* Landing flare altitude (relative to landing altitude)
|
||||
*
|
||||
* NOTE: max(FW_LND_FLALT, FW_LND_FL_TIME * |z-velocity|) is taken as the flare altitude
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 0.5f);
|
||||
|
||||
/**
|
||||
* Use terrain estimation during landing.
|
||||
*
|
||||
* This is critical for detecting when to flare, and should be enabled if possible.
|
||||
*
|
||||
* If enabled and no measurement is found within a given timeout, the landing waypoint altitude will be used OR the landing
|
||||
* will be aborted, depending on the criteria set in FW_LND_ABORT.
|
||||
*
|
||||
* If disabled, FW_LND_ABORT terrain based criteria are ignored.
|
||||
*
|
||||
* @min 0
|
||||
* @max 2
|
||||
* @value 0 Disable the terrain estimate
|
||||
* @value 1 Use the terrain estimate to trigger the flare (only)
|
||||
* @value 2 Calculate landing glide slope relative to the terrain estimate
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_LND_USETER, 1);
|
||||
|
||||
/**
|
||||
* Early landing configuration deployment
|
||||
*
|
||||
* Allows to deploy the landing configuration (flaps, landing airspeed, etc.) already in
|
||||
* the loiter-down waypoint before the final approach.
|
||||
* Otherwise is enabled only in the final approach.
|
||||
*
|
||||
* @boolean
|
||||
*
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_LND_EARLYCFG, 0);
|
||||
|
||||
/**
|
||||
* Flare, minimum pitch
|
||||
*
|
||||
* Minimum pitch during landing flare.
|
||||
*
|
||||
* @unit deg
|
||||
* @min -5
|
||||
* @max 15.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_FL_PMIN, 2.5f);
|
||||
|
||||
/**
|
||||
* Flare, maximum pitch
|
||||
*
|
||||
* Maximum pitch during landing flare.
|
||||
*
|
||||
* @unit deg
|
||||
* @min 0
|
||||
* @max 45.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f);
|
||||
|
||||
/**
|
||||
* Landing airspeed
|
||||
*
|
||||
* The calibrated airspeed setpoint during landing.
|
||||
*
|
||||
* If set <= 0, landing airspeed = FW_AIRSPD_MIN by default.
|
||||
*
|
||||
* @unit m/s
|
||||
* @min -1.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_AIRSPD, -1.f);
|
||||
|
||||
/**
|
||||
* Altitude time constant factor for landing and low-height flight
|
||||
*
|
||||
* The TECS altitude time constant (FW_T_ALT_TC) is multiplied by this value.
|
||||
*
|
||||
* @unit
|
||||
* @min 0.2
|
||||
* @max 1.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
|
||||
|
||||
/**
|
||||
* Speed <--> Altitude weight
|
||||
*
|
||||
* Adjusts the amount of weighting that the pitch control
|
||||
* applies to speed vs height errors.
|
||||
* 0 -> control height only
|
||||
* 2 -> control speed only (gliders)
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 2.0
|
||||
* @decimal 1
|
||||
* @increment 1.0
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
|
||||
|
||||
/**
|
||||
* Custom stick configuration
|
||||
*
|
||||
* Applies in manual Position and Altitude flight modes.
|
||||
*
|
||||
* @min 0
|
||||
* @max 3
|
||||
* @bit 0 Alternative stick configuration (height rate on throttle stick, airspeed on pitch stick)
|
||||
* @bit 1 Enable airspeed setpoint via sticks in altitude and position flight mode
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2);
|
||||
|
||||
/**
|
||||
* Default target climbrate.
|
||||
*
|
||||
* In auto modes: default climb rate output by controller to achieve altitude setpoints.
|
||||
* In manual modes: maximum climb rate setpoint.
|
||||
*
|
||||
* @unit m/s
|
||||
* @min 0.5
|
||||
* @max 15
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f);
|
||||
|
||||
/**
|
||||
* Default target sinkrate.
|
||||
*
|
||||
* In auto modes: default sink rate output by controller to achieve altitude setpoints.
|
||||
* In manual modes: maximum sink rate setpoint.
|
||||
*
|
||||
* @unit m/s
|
||||
* @min 0.5
|
||||
* @max 15
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f);
|
||||
|
||||
/**
|
||||
* GPS failure loiter time
|
||||
*
|
||||
* The time the system should do open loop loiter and wait for GPS recovery
|
||||
* before it starts descending. Set to 0 to disable. Roll angle is set to FW_GPSF_R.
|
||||
* Does only apply for fixed-wing vehicles or VTOLs with NAV_FORCE_VT set to 0.
|
||||
*
|
||||
* @unit s
|
||||
* @min 0
|
||||
* @max 3600
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_GPSF_LT, 30);
|
||||
|
||||
/**
|
||||
* GPS failure fixed roll angle
|
||||
*
|
||||
* Roll angle in GPS failure loiter mode.
|
||||
*
|
||||
* @unit deg
|
||||
* @min 0.0
|
||||
* @max 30.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f);
|
||||
|
||||
|
||||
/**
|
||||
* The aircraft's wing span (length from tip to tip).
|
||||
*
|
||||
* This is used for limiting the roll setpoint near the ground. (if multiple wings, take the longest span)
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.1
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_WING_SPAN, 3.0);
|
||||
|
||||
/**
|
||||
* Height (AGL) of the wings when the aircraft is on the ground.
|
||||
*
|
||||
* This is used to constrain a minimum altitude below which we keep wings level to avoid wing tip strike. It's safer
|
||||
* to give a slight margin here (> 0m)
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @decimal 1
|
||||
* @increment 1
|
||||
* @group FW General
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_WING_HEIGHT, 0.5);
|
||||
|
||||
/**
|
||||
* Landing flare time
|
||||
*
|
||||
* Multiplied by the descent rate to calculate a dynamic altitude at which
|
||||
* to trigger the flare.
|
||||
*
|
||||
* NOTE: max(FW_LND_FLALT, FW_LND_FL_TIME * descent rate) is taken as the flare altitude
|
||||
*
|
||||
* @unit s
|
||||
* @min 0.1
|
||||
* @max 5.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_FL_TIME, 1.0f);
|
||||
|
||||
/**
|
||||
* Landing touchdown time (since flare start)
|
||||
*
|
||||
* This is the time after the start of flaring that we expect the vehicle to touch the runway.
|
||||
* At this time, a 0.5s clamp down ramp will engage, constraining the pitch setpoint to RWTO_PSP.
|
||||
* If enabled, ensure that RWTO_PSP is configured appropriately for full gear contact on ground roll.
|
||||
*
|
||||
* Set to -1.0 to disable touchdown clamping. E.g. it may not be desirable to clamp on belly landings.
|
||||
*
|
||||
* The touchdown time will be constrained to be greater than or equal to the flare time (FW_LND_FL_TIME).
|
||||
*
|
||||
* @unit s
|
||||
* @min -1.0
|
||||
* @max 5.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_TD_TIME, -1.0f);
|
||||
|
||||
/**
|
||||
* Landing flare sink rate
|
||||
*
|
||||
* TECS will attempt to control the aircraft to this sink rate via pitch angle (throttle killed during flare)
|
||||
*
|
||||
* @unit m/s
|
||||
* @min 0.0
|
||||
* @max 2
|
||||
* @decimal 2
|
||||
* @increment 0.1
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_FL_SINK, 0.25f);
|
||||
|
||||
/**
|
||||
* Maximum lateral position offset for the touchdown point
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @decimal 1
|
||||
* @increment 1
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_TD_OFF, 3.0);
|
||||
|
||||
/**
|
||||
* Landing touchdown nudging option.
|
||||
*
|
||||
* Approach angle nudging: shifts the touchdown point laterally while keeping the approach entrance point constant
|
||||
* Approach path nudging: shifts the touchdown point laterally along with the entire approach path
|
||||
*
|
||||
* This is useful for manually adjusting the landing point in real time when map or GNSS errors cause an offset from the
|
||||
* desired landing vector. Nudging is done with yaw stick, constrained to FW_LND_TD_OFF (in meters) and the direction is
|
||||
* relative to the vehicle heading (stick deflection to the right = land point moves to the right as seen by the vehicle).
|
||||
*
|
||||
* @min 0
|
||||
* @max 2
|
||||
* @value 0 Disable nudging
|
||||
* @value 1 Nudge approach angle
|
||||
* @value 2 Nudge approach path
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_LND_NUDGE, 2);
|
||||
|
||||
/**
|
||||
* Bit mask to set the automatic landing abort conditions.
|
||||
*
|
||||
* Terrain estimation:
|
||||
* bit 0: Abort if terrain is not found
|
||||
* bit 1: Abort if terrain times out (after a first successful measurement)
|
||||
*
|
||||
* The last estimate is always used as ground, whether the last valid measurement or the land waypoint, depending on the
|
||||
* selected abort criteria, until an abort condition is entered. If FW_LND_USETER == 0, these bits are ignored.
|
||||
*
|
||||
* TODO: Extend automatic abort conditions
|
||||
* e.g. glide slope tracking error (horizontal and vertical)
|
||||
*
|
||||
* @min 0
|
||||
* @max 3
|
||||
* @bit 0 Abort if terrain is not found (only applies to mission landings)
|
||||
* @bit 1 Abort if terrain times out (after a first successful measurement)
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_LND_ABORT, 3);
|
||||
|
||||
/**
|
||||
* Fixed-wing launch detection
|
||||
*
|
||||
* Enables automatic launch detection based on measured acceleration. Use for hand- or catapult-launched vehicles.
|
||||
* Not compatible with runway takeoff.
|
||||
*
|
||||
* @boolean
|
||||
* @group FW Auto Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_LAUN_DETCN_ON, 0);
|
||||
|
||||
/**
|
||||
* Flaps setting during take-off
|
||||
*
|
||||
* Sets a fraction of full flaps during take-off.
|
||||
* Also applies to flaperons if enabled in the mixer/allocation.
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW Auto Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f);
|
||||
|
||||
/**
|
||||
* Flaps setting during landing
|
||||
*
|
||||
* Sets a fraction of full flaps during landing.
|
||||
* Also applies to flaperons if enabled in the mixer/allocation.
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f);
|
||||
|
||||
/**
|
||||
* Spoiler landing setting
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW Auto Landing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f);
|
||||
@@ -0,0 +1,36 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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_library(launchdetection
|
||||
LaunchDetector.cpp
|
||||
)
|
||||
@@ -0,0 +1,121 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2022 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 LaunchDetector.cpp
|
||||
* Auto launch detection for catapult/hand-launch vehicles
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include "LaunchDetector.h"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
namespace launchdetection
|
||||
{
|
||||
|
||||
void LaunchDetector::update(const float dt, const float accel_x)
|
||||
{
|
||||
switch (state_) {
|
||||
case launch_detection_status_s::STATE_WAITING_FOR_LAUNCH:
|
||||
|
||||
info_delay_counter_s_ += dt;
|
||||
|
||||
/* Inform user that launchdetection is running every kInfoDelay seconds */
|
||||
if (info_delay_counter_s_ >= kInfoDelay) {
|
||||
events::send(events::ID("launch_detection_running_info"), events::Log::Info, "Launch detection running");
|
||||
info_delay_counter_s_ = 0.f; // reset counter
|
||||
}
|
||||
|
||||
/* Detect a acceleration that is longer and stronger as the minimum given by the params */
|
||||
if (accel_x > param_fw_laun_ac_thld_.get()) {
|
||||
acceleration_detected_counter_ += dt;
|
||||
|
||||
if (acceleration_detected_counter_ > param_fw_laun_ac_t_.get()) {
|
||||
if (param_fw_laun_mot_del_.get() > 0.f) {
|
||||
state_ = launch_detection_status_s::STATE_LAUNCH_DETECTED_DISABLED_MOTOR;
|
||||
events::send<float>(events::ID("launch_detection_wait_for_throttle"), {events::Log::Info, events::LogInternal::Info},
|
||||
"Launch detected: enablecontrol, waiting {1:.1}s until full throttle", (double)param_fw_laun_mot_del_.get());
|
||||
|
||||
} else {
|
||||
/* No motor delay set: go directly to enablemotors state */
|
||||
state_ = launch_detection_status_s::STATE_FLYING;
|
||||
events::send(events::ID("launch_detection_no_motor_delay"), {events::Log::Info, events::LogInternal::Info},
|
||||
"Launch detected: enable motors (no motor delay)");
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
acceleration_detected_counter_ = 0.f;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case launch_detection_status_s::STATE_LAUNCH_DETECTED_DISABLED_MOTOR:
|
||||
/* Vehicle is currently controlling attitude but at idle throttle. Waiting until delay is
|
||||
* over to allow full throttle */
|
||||
motor_delay_counter_ += dt;
|
||||
|
||||
if (motor_delay_counter_ > param_fw_laun_mot_del_.get()) {
|
||||
events::send(events::ID("launch_detection_enable_motors"), {events::Log::Info, events::LogInternal::Info},
|
||||
"Launch detected: enable motors");
|
||||
state_ = launch_detection_status_s::STATE_FLYING;
|
||||
}
|
||||
|
||||
info_delay_counter_s_ = kInfoDelay; // reset counter
|
||||
|
||||
break;
|
||||
|
||||
default: // state flying
|
||||
info_delay_counter_s_ = kInfoDelay; // reset counter
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
uint LaunchDetector::getLaunchDetected() const
|
||||
{
|
||||
return state_;
|
||||
}
|
||||
|
||||
void LaunchDetector::reset()
|
||||
{
|
||||
info_delay_counter_s_ = 0.f;
|
||||
acceleration_detected_counter_ = 0.f;
|
||||
motor_delay_counter_ = 0.f;
|
||||
state_ = launch_detection_status_s::STATE_WAITING_FOR_LAUNCH;
|
||||
}
|
||||
|
||||
|
||||
} // namespace launchdetection
|
||||
@@ -0,0 +1,117 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2022 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 LaunchDetector.h
|
||||
* Auto launch detection for catapult/hand-launch vehicles
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef LAUNCHDETECTOR_H
|
||||
#define LAUNCHDETECTOR_H
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/topics/launch_detection_status.h>
|
||||
|
||||
namespace launchdetection
|
||||
{
|
||||
|
||||
// Info delay threshold (to publish info every kInfoDelay seconds)
|
||||
static constexpr float kInfoDelay = 4.f;
|
||||
|
||||
class __EXPORT LaunchDetector : public ModuleParams
|
||||
{
|
||||
public:
|
||||
LaunchDetector(ModuleParams *parent) : ModuleParams(parent) {}
|
||||
~LaunchDetector() = default;
|
||||
|
||||
LaunchDetector(const LaunchDetector &) = delete;
|
||||
LaunchDetector operator=(const LaunchDetector &) = delete;
|
||||
|
||||
/**
|
||||
* @brief Reset launch detection state machine.
|
||||
*/
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* @brief Updates the state machine based on the current vehicle condition.
|
||||
*
|
||||
* @param dt Time step [us]
|
||||
* @param accel_x Measured acceleration in body x [m/s/s]
|
||||
*/
|
||||
void update(const float dt, const float accel_x);
|
||||
|
||||
/**
|
||||
* @brief Get the Launch Detected state
|
||||
*
|
||||
* @return uint (aligned with launch_detection_status_s::launch_detection_state)
|
||||
*/
|
||||
uint getLaunchDetected() const;
|
||||
|
||||
/**
|
||||
* @brief Forces state of launch detection state machine to STATE_FLYING.
|
||||
*/
|
||||
void forceSetFlyState() { state_ = launch_detection_status_s::STATE_FLYING; }
|
||||
|
||||
private:
|
||||
/**
|
||||
* Motor delay counter [s]
|
||||
*/
|
||||
float motor_delay_counter_{0.f};
|
||||
|
||||
/**
|
||||
* Info delay counter (to publish info every kInfoDelay seconds) [s]
|
||||
*/
|
||||
float info_delay_counter_s_{kInfoDelay};
|
||||
|
||||
/**
|
||||
* Counter for how long the measured acceleration is above the defined threshold [s]
|
||||
*/
|
||||
float acceleration_detected_counter_{0.f};
|
||||
|
||||
/**
|
||||
* Current state of the launch detection state machine [launch_detection_status_s::launch_detection_state]
|
||||
*/
|
||||
uint state_{launch_detection_status_s::STATE_WAITING_FOR_LAUNCH};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::FW_LAUN_AC_THLD>) param_fw_laun_ac_thld_,
|
||||
(ParamFloat<px4::params::FW_LAUN_AC_T>) param_fw_laun_ac_t_,
|
||||
(ParamFloat<px4::params::FW_LAUN_MOT_DEL>) param_fw_laun_mot_del_
|
||||
)
|
||||
};
|
||||
|
||||
} // namespace launchdetection
|
||||
|
||||
#endif // LAUNCHDETECTOR_H
|
||||
@@ -0,0 +1,81 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013, 2014 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 launchdetection_params.c
|
||||
*
|
||||
* Parameters for launchdetection
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
/**
|
||||
* Trigger acceleration threshold
|
||||
*
|
||||
* Launch is detected when acceleration in body forward direction is above FW_LAUN_AC_THLD for FW_LAUN_AC_T seconds.
|
||||
*
|
||||
* @unit m/s^2
|
||||
* @min 0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group FW Auto Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LAUN_AC_THLD, 30.0f);
|
||||
|
||||
/**
|
||||
* Trigger time
|
||||
*
|
||||
* Launch is detected when acceleration in body forward direction is above FW_LAUN_AC_THLD for FW_LAUN_AC_T seconds.
|
||||
*
|
||||
* @unit s
|
||||
* @min 0.0
|
||||
* @max 5.0
|
||||
* @decimal 2
|
||||
* @increment 0.05
|
||||
* @group FW Auto Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LAUN_AC_T, 0.05f);
|
||||
|
||||
/**
|
||||
* Motor delay
|
||||
*
|
||||
* Start the motor(s) this amount of seconds after launch is detected.
|
||||
*
|
||||
* @unit s
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group FW Auto Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LAUN_MOT_DEL, 0.0f);
|
||||
@@ -0,0 +1,36 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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_library(runway_takeoff
|
||||
RunwayTakeoff.cpp
|
||||
)
|
||||
@@ -0,0 +1,193 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
/**
|
||||
* @file RunwayTakeoff.cpp
|
||||
* Runway takeoff handling for fixed-wing UAVs with steerable wheels.
|
||||
*
|
||||
* @author Roman Bapst <roman@px4.io>
|
||||
* @author Andreas Antener <andreas@uaventure.com>
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "RunwayTakeoff.h"
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
namespace runwaytakeoff
|
||||
{
|
||||
|
||||
void RunwayTakeoff::init(const hrt_abstime &time_now)
|
||||
{
|
||||
takeoff_state_ = RunwayTakeoffState::THROTTLE_RAMP;
|
||||
initialized_ = true;
|
||||
time_initialized_ = time_now;
|
||||
takeoff_time_ = 0;
|
||||
}
|
||||
|
||||
void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed,
|
||||
const float vehicle_altitude, const float clearance_altitude)
|
||||
{
|
||||
switch (takeoff_state_) {
|
||||
case RunwayTakeoffState::THROTTLE_RAMP:
|
||||
if ((time_now - time_initialized_) > (param_rwto_ramp_time_.get() * 1_s)) {
|
||||
takeoff_state_ = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case RunwayTakeoffState::CLAMPED_TO_RUNWAY: {
|
||||
const float rotation_airspeed = (param_rwto_rot_airspd_.get() > FLT_EPSILON) ? math::min(param_rwto_rot_airspd_.get(),
|
||||
takeoff_airspeed) : 0.9f * takeoff_airspeed;
|
||||
|
||||
if (calibrated_airspeed > rotation_airspeed) {
|
||||
takeoff_time_ = time_now;
|
||||
takeoff_state_ = RunwayTakeoffState::CLIMBOUT;
|
||||
events::send(events::ID("runway_takeoff_reached_airspeed"), events::Log::Info,
|
||||
"Takeoff airspeed reached, climbout");
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case RunwayTakeoffState::CLIMBOUT:
|
||||
if (vehicle_altitude > clearance_altitude) {
|
||||
takeoff_state_ = RunwayTakeoffState::FLY;
|
||||
events::send(events::ID("runway_takeoff_reached_clearance_altitude"), events::Log::Info, "Reached clearance altitude");
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
float RunwayTakeoff::getPitch()
|
||||
{
|
||||
if (takeoff_state_ <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
|
||||
return math::radians(param_rwto_psp_.get());
|
||||
}
|
||||
|
||||
return NAN;
|
||||
}
|
||||
|
||||
float RunwayTakeoff::getRoll()
|
||||
{
|
||||
// until we have enough ground clearance, set roll to 0
|
||||
if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
return NAN;
|
||||
}
|
||||
|
||||
float RunwayTakeoff::getThrottle(const float idle_throttle) const
|
||||
{
|
||||
float throttle = idle_throttle;
|
||||
|
||||
switch (takeoff_state_) {
|
||||
case RunwayTakeoffState::THROTTLE_RAMP:
|
||||
|
||||
throttle = interpolateValuesOverAbsoluteTime(idle_throttle, param_rwto_max_thr_.get(), time_initialized_,
|
||||
param_rwto_ramp_time_.get());
|
||||
|
||||
break;
|
||||
|
||||
case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
|
||||
case RunwayTakeoffState::CLIMBOUT:
|
||||
throttle = param_rwto_max_thr_.get();
|
||||
|
||||
break;
|
||||
|
||||
case RunwayTakeoffState::FLY:
|
||||
throttle = NAN;
|
||||
}
|
||||
|
||||
return throttle;
|
||||
}
|
||||
|
||||
float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) const
|
||||
{
|
||||
if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
|
||||
// constrain to the taxi pitch setpoint
|
||||
return math::radians(param_rwto_psp_.get() - 0.01f);
|
||||
|
||||
} else if (takeoff_state_ < RunwayTakeoffState::FLY) {
|
||||
// ramp in the climbout pitch constraint over the rotation transition time
|
||||
const float taxi_pitch_min = math::radians(param_rwto_psp_.get() - 0.01f);
|
||||
return interpolateValuesOverAbsoluteTime(taxi_pitch_min, min_pitch_in_climbout, takeoff_time_,
|
||||
param_rwto_rot_time_.get());
|
||||
|
||||
} else {
|
||||
return min_pitch;
|
||||
}
|
||||
}
|
||||
|
||||
float RunwayTakeoff::getMaxPitch(const float max_pitch) const
|
||||
{
|
||||
if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
|
||||
// constrain to the taxi pitch setpoint
|
||||
return math::radians(param_rwto_psp_.get() + 0.01f);
|
||||
|
||||
} else if (takeoff_state_ < RunwayTakeoffState::FLY) {
|
||||
// ramp in the climbout pitch constraint over the rotation transition time
|
||||
const float taxi_pitch_max = math::radians(param_rwto_psp_.get() + 0.01f);
|
||||
return interpolateValuesOverAbsoluteTime(taxi_pitch_max, max_pitch, takeoff_time_, param_rwto_rot_time_.get());
|
||||
|
||||
} else {
|
||||
return max_pitch;
|
||||
}
|
||||
}
|
||||
|
||||
void RunwayTakeoff::reset()
|
||||
{
|
||||
initialized_ = false;
|
||||
takeoff_state_ = RunwayTakeoffState::THROTTLE_RAMP;
|
||||
takeoff_time_ = 0;
|
||||
}
|
||||
|
||||
float RunwayTakeoff::interpolateValuesOverAbsoluteTime(const float start_value, const float end_value,
|
||||
const hrt_abstime &start_time, const float interpolation_time) const
|
||||
{
|
||||
const float seconds_since_start = hrt_elapsed_time(&start_time) * 1.e-6f;
|
||||
const float interpolator = math::constrain(seconds_since_start / interpolation_time, 0.0f, 1.0f);
|
||||
|
||||
return interpolator * end_value + (1.0f - interpolator) * start_value;
|
||||
}
|
||||
|
||||
} // namespace runwaytakeoff
|
||||
@@ -0,0 +1,190 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
/**
|
||||
* @file RunwayTakeoff.h
|
||||
* Runway takeoff handling for fixed-wing UAVs with steerable wheels.
|
||||
*
|
||||
* @author Roman Bapst <roman@px4.io>
|
||||
* @author Andreas Antener <andreas@uaventure.com>
|
||||
*/
|
||||
|
||||
#ifndef RUNWAYTAKEOFF_H
|
||||
#define RUNWAYTAKEOFF_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace runwaytakeoff
|
||||
{
|
||||
|
||||
enum RunwayTakeoffState {
|
||||
THROTTLE_RAMP = 0, // ramping up throttle
|
||||
CLAMPED_TO_RUNWAY, // clamped to runway, controlling yaw directly (wheel or rudder)
|
||||
CLIMBOUT, // climbout to safe height before navigation
|
||||
FLY // navigate freely
|
||||
};
|
||||
|
||||
class __EXPORT RunwayTakeoff : public ModuleParams
|
||||
{
|
||||
public:
|
||||
RunwayTakeoff(ModuleParams *parent) : ModuleParams(parent) {}
|
||||
~RunwayTakeoff() = default;
|
||||
|
||||
/**
|
||||
* @brief Initializes the state machine.
|
||||
*
|
||||
* @param time_now Absolute time since system boot [us]
|
||||
*/
|
||||
void init(const hrt_abstime &time_now);
|
||||
|
||||
/**
|
||||
* @brief Updates the state machine based on the current vehicle condition.
|
||||
*
|
||||
* @param time_now Absolute time since system boot [us]
|
||||
* @param takeoff_airspeed Calibrated airspeed setpoint for the takeoff climbout [m/s]
|
||||
* @param calibrated_airspeed Vehicle calibrated airspeed [m/s]
|
||||
* @param vehicle_altitude Vehicle altitude (AGL) [m]
|
||||
* @param clearance_altitude Altitude (AGL) above which we have cleared all occlusions in the runway path [m]
|
||||
*/
|
||||
void update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed,
|
||||
const float vehicle_altitude, const float clearance_altitude);
|
||||
|
||||
/**
|
||||
* @return Current takeoff state
|
||||
*/
|
||||
RunwayTakeoffState getState() { return takeoff_state_; }
|
||||
|
||||
/**
|
||||
* @return The state machine is initialized
|
||||
*/
|
||||
bool isInitialized() { return initialized_; }
|
||||
|
||||
/**
|
||||
* @return Runway takeoff is enabled
|
||||
*/
|
||||
bool runwayTakeoffEnabled() { return param_rwto_tkoff_.get(); }
|
||||
|
||||
/**
|
||||
* @return Pitch angle setpoint (limited while plane is on runway) [rad]
|
||||
*/
|
||||
float getPitch();
|
||||
|
||||
/**
|
||||
* @return Roll angle setpoint [rad]
|
||||
*/
|
||||
float getRoll();
|
||||
|
||||
/**
|
||||
* @brief Returns the throttle setpoint.
|
||||
*
|
||||
* Ramps up over RWTO_RAMP_TIME to RWTO_MAX_THR until the aircraft lifts off the runway and then
|
||||
* ramps from RWTO_MAX_THR to the externally defined throttle setting over the takeoff rotation time
|
||||
*
|
||||
* @param idle_throttle normalized [0,1]
|
||||
* @return Throttle setpoint, normalized [0,1]
|
||||
*/
|
||||
float getThrottle(const float idle_throttle) const;
|
||||
|
||||
/**
|
||||
* @param min_pitch_in_climbout Minimum pitch angle during climbout [rad]
|
||||
* @param min_pitch Externally commanded minimum pitch angle [rad]
|
||||
* @return Minimum pitch angle [rad]
|
||||
*/
|
||||
float getMinPitch(const float min_pitch_in_climbout, const float min_pitch) const;
|
||||
|
||||
/**
|
||||
* @param max_pitch Externally commanded maximum pitch angle [rad]
|
||||
* @return Maximum pitch angle [rad]
|
||||
*/
|
||||
float getMaxPitch(const float max_pitch) const;
|
||||
|
||||
// NOTE: this is only to be used for mistaken mode transitions to takeoff while already in air
|
||||
void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLY; }
|
||||
|
||||
/**
|
||||
* @brief Reset the state machine.
|
||||
*/
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* @brief Linearly interpolates between a start and end value over an absolute time span.
|
||||
*
|
||||
* @param start_value
|
||||
* @param end_value
|
||||
* @param start_time Absolute start time [us]
|
||||
* @param interpolation_time The time span to interpolate over [s]
|
||||
* @return interpolated value
|
||||
*/
|
||||
float interpolateValuesOverAbsoluteTime(const float start_value, const float end_value, const hrt_abstime &start_time,
|
||||
const float interpolation_time) const;
|
||||
|
||||
private:
|
||||
/**
|
||||
* Current state of runway takeoff procedure
|
||||
*/
|
||||
RunwayTakeoffState takeoff_state_{THROTTLE_RAMP};
|
||||
|
||||
/**
|
||||
* True if the runway state machine is initialized
|
||||
*/
|
||||
bool initialized_{false};
|
||||
|
||||
/**
|
||||
* The absolute time since system boot at which the state machine was intialized [us]
|
||||
*/
|
||||
hrt_abstime time_initialized_{0};
|
||||
|
||||
/**
|
||||
* The absolute time the takeoff state transitions from CLAMPED_TO_RUNWAY -> CLIMBOUT [us]
|
||||
*/
|
||||
hrt_abstime takeoff_time_{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamBool<px4::params::RWTO_TKOFF>) param_rwto_tkoff_,
|
||||
(ParamFloat<px4::params::RWTO_MAX_THR>) param_rwto_max_thr_,
|
||||
(ParamFloat<px4::params::RWTO_PSP>) param_rwto_psp_,
|
||||
(ParamFloat<px4::params::RWTO_RAMP_TIME>) param_rwto_ramp_time_,
|
||||
(ParamFloat<px4::params::RWTO_ROT_AIRSPD>) param_rwto_rot_airspd_,
|
||||
(ParamFloat<px4::params::RWTO_ROT_TIME>) param_rwto_rot_time_
|
||||
)
|
||||
};
|
||||
|
||||
} // namespace runwaytakeoff
|
||||
|
||||
#endif // RUNWAYTAKEOFF_H
|
||||
@@ -0,0 +1,128 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file runway_takeoff_params.c
|
||||
*
|
||||
* Parameters for runway takeoff
|
||||
*
|
||||
* @author Andreas Antener <andreas@uaventure.com>
|
||||
*/
|
||||
|
||||
/**
|
||||
* Runway takeoff with landing gear
|
||||
*
|
||||
* @boolean
|
||||
* @group Runway Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RWTO_TKOFF, 0);
|
||||
|
||||
/**
|
||||
* Throttle during runway takeoff.
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Runway Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0);
|
||||
|
||||
/**
|
||||
* Pitch setpoint during taxi / before takeoff rotation airspeed is reached.
|
||||
*
|
||||
* A taildragger with steerable wheel might need to pitch up
|
||||
* a little to keep its wheel on the ground before airspeed
|
||||
* to takeoff is reached.
|
||||
*
|
||||
* @unit deg
|
||||
* @min -10.0
|
||||
* @max 20.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group Runway Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0);
|
||||
|
||||
/**
|
||||
* Throttle ramp up time for runway takeoff
|
||||
*
|
||||
* @unit s
|
||||
* @min 1.0
|
||||
* @max 15.0
|
||||
* @decimal 2
|
||||
* @increment 0.1
|
||||
* @group Runway Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f);
|
||||
|
||||
/**
|
||||
* Enable use of yaw stick for nudging the wheel during runway ground roll
|
||||
*
|
||||
* This is useful when map, GNSS, or yaw errors on ground are misaligned with what the operator intends for takeoff course.
|
||||
* Particularly useful for skinny runways or if the wheel servo is a bit off trim.
|
||||
*
|
||||
* @boolean
|
||||
* @group Runway Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RWTO_NUDGE, 1);
|
||||
|
||||
/**
|
||||
* Takeoff rotation airspeed
|
||||
*
|
||||
* The calibrated airspeed threshold during the takeoff ground roll when the plane should start rotating (pitching up).
|
||||
* Must be less than the takeoff airspeed, will otherwise be capped at the takeoff airpeed (see FW_TKO_AIRSPD).
|
||||
*
|
||||
* If set <= 0.0, defaults to 0.9 * takeoff airspeed (see FW_TKO_AIRSPD)
|
||||
*
|
||||
* @unit m/s
|
||||
* @min -1.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group Runway Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RWTO_ROT_AIRSPD, -1.0f);
|
||||
|
||||
/**
|
||||
* Takeoff rotation time
|
||||
*
|
||||
* This is the time desired to linearly ramp in takeoff pitch constraints during the takeoff rotation
|
||||
*
|
||||
* @unit s
|
||||
* @min 0.1
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group Runway Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RWTO_ROT_TIME, 1.0f);
|
||||
Reference in New Issue
Block a user