mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-08 21:21:37 +08:00
wip
This commit is contained in:
Alessandro Simovic
@@ -6,4 +6,4 @@ uint8 PRECLAND_STATE_TOUCHING_DOWN = 4 # Same as descend, but if sight o
|
||||
uint8 PRECLAND_STATE_FALLBACK = 5 # Fallback to conventional landing, either because of opportunistic precision landing or because maximum number of precland attempts were reached
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint8 precland_state # this flight-task's internal state
|
||||
uint8 state # this flight-task's internal state
|
||||
|
||||
@@ -180,6 +180,7 @@ void FlightModeManager::start_flight_task()
|
||||
should_disable_task = false;
|
||||
|
||||
if (switchTask(FlightTaskIndex::AutoPrecisionLanding) != FlightTaskError::NoError) {
|
||||
PX4_INFO("Switching to precland flight task");
|
||||
task_failure = true;
|
||||
}
|
||||
|
||||
|
||||
+166
-415
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2023 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -30,450 +30,201 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file FlightTaskAutoPrecisionLanding.cpp
|
||||
*
|
||||
* @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
|
||||
*/
|
||||
|
||||
#include "FlightTaskAutoPrecisionLanding.hpp"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing";
|
||||
using namespace matrix;
|
||||
|
||||
bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint)
|
||||
{
|
||||
bool ret = FlightTask::activate(last_setpoint);
|
||||
PX4_INFO("precland activated");
|
||||
|
||||
_search_cnt = 0;
|
||||
_last_slewrate_time = 0;
|
||||
_position_setpoint = _position;
|
||||
_search_count = 0;
|
||||
|
||||
try_switch_to_state_auto_rtl();
|
||||
// TODO: Use target orientation instead
|
||||
_yaw_setpoint = _yaw;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void FlightTaskAutoPrecisionLanding::do_state_transition(PRECLAND_STATE new_state)
|
||||
{
|
||||
_precland_state = new_state;
|
||||
_state_start_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
bool FlightTaskAutoPrecisionLanding::inside_acceptance_radius()
|
||||
{
|
||||
return Vector3f(_position_setpoint - _position).norm() <= _param_pld_hacc_rad.get();
|
||||
}
|
||||
|
||||
bool FlightTaskAutoPrecisionLanding::update()
|
||||
{
|
||||
bool ret = FlightTaskAuto::update();
|
||||
|
||||
// printf("state: %u\n", _precland_state);
|
||||
|
||||
// Fetch uorb
|
||||
if (_landing_target_pose_sub.updated()) {
|
||||
_landing_target_pose_sub.copy(&_landing_target_pose);
|
||||
|
||||
_precision_target_ned(0) = _landing_target_pose.x_rel;
|
||||
_precision_target_ned(1) = _landing_target_pose.y_rel;
|
||||
_precision_target_ned(2) = _landing_target_pose.z_rel;
|
||||
}
|
||||
|
||||
// target pose can become invalid when the message timed out
|
||||
_landing_target_pose_valid = (hrt_elapsed_time(&_landing_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get();
|
||||
// Initialize precision target coordinates with home position if marker has not been spotted yet
|
||||
if (_landing_target_pose.timestamp == 0) {
|
||||
if (_sub_home_position.get().valid_lpos && _sub_home_position.get().valid_alt) {
|
||||
_precision_target_ned(0) = _sub_home_position.get().x;
|
||||
_precision_target_ned(1) = _sub_home_position.get().y;
|
||||
_precision_target_ned(2) = _sub_home_position.get().z; // TODO: I think this is NAN
|
||||
} else {
|
||||
// Nothing to do without valid home location or target position
|
||||
return ret;
|
||||
}
|
||||
|
||||
switch (_state) {
|
||||
case PrecLandState::AutoRTL:
|
||||
run_state_auto_rtl();
|
||||
break;
|
||||
|
||||
case PrecLandState::MoveAboveTarget:
|
||||
run_state_move_above_target();
|
||||
break;
|
||||
|
||||
case PrecLandState::DescendAboveTarget:
|
||||
run_state_descend_above_target();
|
||||
break;
|
||||
|
||||
case PrecLandState::TouchingDown:
|
||||
run_state_touching_down();
|
||||
break;
|
||||
|
||||
case PrecLandState::Search:
|
||||
run_state_search();
|
||||
break;
|
||||
|
||||
case PrecLandState::Fallback:
|
||||
run_state_fallback();
|
||||
break;
|
||||
|
||||
default:
|
||||
// unknown state
|
||||
break;
|
||||
}
|
||||
|
||||
// Publish status message for debugging
|
||||
precision_landing_status_s precision_landing_status{};
|
||||
precision_landing_status.timestamp = hrt_absolute_time();
|
||||
precision_landing_status.precland_state = (uint8_t) _state;
|
||||
_precision_landing_status_pub.publish(precision_landing_status);
|
||||
|
||||
_constraints.want_takeoff = _checkTakeoff();
|
||||
// Fetch runtime parameters
|
||||
const float max_search_duration = _param_pld_srch_tout.get();
|
||||
const float search_rel_altitude = _param_pld_srch_alt.get();
|
||||
|
||||
switch (_precland_state) {
|
||||
case PRECLAND_STATE::AUTORTL_CLIMB:
|
||||
PX4_INFO("AUTORTL_CLIMB");
|
||||
|
||||
_position_setpoint = _position;
|
||||
_position_setpoint(2) = _precision_target_ned(2) - search_rel_altitude;
|
||||
_velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN;
|
||||
|
||||
// transition condition
|
||||
// Wait for drone to reach z position setpoint
|
||||
if (abs(_position(2) - _position_setpoint(2)) <= _param_pld_hacc_rad.get()) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Moving above target");
|
||||
do_state_transition(PRECLAND_STATE::AUTORTL_APPROACH);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PRECLAND_STATE::AUTORTL_APPROACH:
|
||||
PX4_INFO("AUTORTL_APPROACH");
|
||||
|
||||
_position_setpoint = _precision_target_ned;
|
||||
_position_setpoint(2) -= search_rel_altitude;
|
||||
_velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN;
|
||||
|
||||
// transition condition
|
||||
// Wait for drone to reach position setpoint
|
||||
if (inside_acceptance_radius()) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Moving to search center");
|
||||
do_state_transition(PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
|
||||
PX4_INFO("MOVE_TO_SEARCH_ALTITUDE");
|
||||
|
||||
// Ascend/Descend to search altitude
|
||||
_position_setpoint = _precision_target_ned;
|
||||
_position_setpoint(2) -= search_rel_altitude;
|
||||
_velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN;
|
||||
_acceleration_setpoint(0) = _acceleration_setpoint(1) = _acceleration_setpoint(2) = NAN;
|
||||
|
||||
// transition condition
|
||||
if (inside_acceptance_radius()) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Starting search");
|
||||
do_state_transition(PRECLAND_STATE::SEARCHING_TARGET);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PRECLAND_STATE::SEARCHING_TARGET:
|
||||
PX4_INFO("SEARCHING_TARGET");
|
||||
|
||||
// Currently no search pattern is implemented. Just wait for target...
|
||||
// Workaround for Orion app making unwanted changes to my parameters
|
||||
if ((hrt_absolute_time() - _state_start_time) > max_search_duration * SEC2USEC) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Search timed out");
|
||||
|
||||
if (++_search_count >= _param_pld_max_srch.get()) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Abandonning search and landing immediately");
|
||||
do_state_transition(PRECLAND_STATE::LANDING);
|
||||
|
||||
} else {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Moving to search center");
|
||||
do_state_transition(PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE);
|
||||
}
|
||||
}
|
||||
|
||||
// transition condition
|
||||
if (hrt_absolute_time() - _landing_target_pose.timestamp <= _param_pld_btout.get()*SEC2USEC) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Moving above target");
|
||||
do_state_transition(PRECLAND_STATE::MOVING_ABOVE_TARGET);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PRECLAND_STATE::MOVING_ABOVE_TARGET:
|
||||
PX4_INFO("MOVING_ABOVE_TARGET");
|
||||
|
||||
_position_setpoint(0) = _landing_target_pose.x_abs;
|
||||
_position_setpoint(1) = _landing_target_pose.y_abs;
|
||||
_position_setpoint(2) = _precision_target_ned(2) - search_rel_altitude;
|
||||
|
||||
_velocity_setpoint(0) = NAN;
|
||||
_velocity_setpoint(1) = NAN;
|
||||
_velocity_setpoint(2) = 0;
|
||||
|
||||
// Transition condition
|
||||
// - Check acceptance radius and
|
||||
// - wait long enough to observe if the landing_target_pose timed out before proceeding
|
||||
if (abs(_position(0) - _position_setpoint(0)) <= _param_pld_hacc_rad.get()
|
||||
&& abs(_position(1) - _position_setpoint(1)) <= _param_pld_hacc_rad.get()
|
||||
&& _velocity.norm() <= HOVER_SPEED_THRESHOLD
|
||||
&& (hrt_absolute_time() - _state_start_time) > _param_pld_btout.get() * SEC2USEC) {
|
||||
|
||||
if (hrt_absolute_time() - _landing_target_pose.timestamp < _param_pld_btout.get() * SEC2USEC) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Landing on target...");
|
||||
do_state_transition(PRECLAND_STATE::LANDING);
|
||||
|
||||
} else {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Lost target, back to searching");
|
||||
do_state_transition(PRECLAND_STATE::SEARCHING_TARGET);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
|
||||
case PRECLAND_STATE::LANDING:
|
||||
PX4_INFO("LANDING");
|
||||
// Control XY position and Z velocity
|
||||
_position_setpoint(0) = _landing_target_pose.x_abs;
|
||||
_position_setpoint(1) = _landing_target_pose.y_abs;
|
||||
_position_setpoint(2) = NAN;
|
||||
|
||||
_velocity_setpoint(0) = 0;
|
||||
_velocity_setpoint(1) = 0;
|
||||
_velocity_setpoint(2) = _param_mpc_land_speed.get();
|
||||
_acceleration_setpoint(2) = NAN;
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
precision_landing_status.timestamp = hrt_absolute_time();
|
||||
precision_landing_status.state = _precland_state;
|
||||
_precision_landing_status_pub.publish(precision_landing_status);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
FlightTaskAutoPrecisionLanding::run_state_auto_rtl()
|
||||
{
|
||||
// In this state simply track the navigator setpoint triplet
|
||||
// This ensures that the behaviour for precision landing during RTL / LAND
|
||||
// is the same as without precision landing.
|
||||
// This flight task will generate independent setpoints once the vehicle
|
||||
// is in the vertical landing phase.
|
||||
_position_setpoint = _target;
|
||||
|
||||
if (try_switch_to_state_move_above_target()) {
|
||||
// If target visible and go to horizontal approach directly
|
||||
return;
|
||||
|
||||
} else if ((PrecLandMode)_param_rtl_pld_md.get() == PrecLandMode::Opportunistic) {
|
||||
// could not see the target immediately, so just fall back to normal landing
|
||||
try_switch_to_state_fallback();
|
||||
|
||||
} else if (_type == WaypointType::land) {
|
||||
// Navigator already entered land stage. Take over with precision landing
|
||||
// This is where the vehicle starts to behave differently than in regular land!
|
||||
try_switch_to_state_search();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FlightTaskAutoPrecisionLanding::run_state_move_above_target()
|
||||
{
|
||||
float x = _landing_target_pose.x_abs;
|
||||
float y = _landing_target_pose.y_abs;
|
||||
slewrate(x, y); // TODO: Replace this with PX4's AlphaFilter
|
||||
|
||||
// Fly to target XY position, but keep navigator's altitude setpoint
|
||||
_position_setpoint(0) = x;
|
||||
_position_setpoint(1) = y;
|
||||
_position_setpoint(2) = _target(2);
|
||||
|
||||
_velocity_setpoint(0) = _velocity_setpoint(1) = NAN;
|
||||
|
||||
// If altitude setpoint is NAN, we stay at the current altitude
|
||||
if (!PX4_ISFINITE(_position_setpoint(2))) {
|
||||
_velocity_setpoint(2) = 0.0f;
|
||||
}
|
||||
|
||||
// check if target visible, if not go to start
|
||||
if (!check_state_conditions(PrecLandState::MoveAboveTarget)) {
|
||||
PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
|
||||
|
||||
// Stay at current position for searching for the landing target
|
||||
// TODO: This is not going to work after conversion to flight task, because
|
||||
// the position_setpoint will be overwritten at the next iteration!
|
||||
// Solution: An additional wait state
|
||||
_position_setpoint = _position;
|
||||
_velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN;
|
||||
|
||||
if (!try_switch_to_state_auto_rtl()) {
|
||||
try_switch_to_state_fallback();
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (check_state_conditions(PrecLandState::DescendAboveTarget)) {
|
||||
if (!_point_reached_time) {
|
||||
_point_reached_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
if (hrt_absolute_time() - _point_reached_time > 2000000) {
|
||||
// if close enough for descent above target go to descend above target
|
||||
if (try_switch_to_state_descend_above_target()) {
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FlightTaskAutoPrecisionLanding::run_state_descend_above_target()
|
||||
{
|
||||
// Overwrite Auto setpoints in order to descend above target
|
||||
_position_setpoint(0) = _landing_target_pose.x_abs;
|
||||
_position_setpoint(1) = _landing_target_pose.y_abs;
|
||||
_position_setpoint(2) = NAN;
|
||||
|
||||
_velocity_setpoint(0) = 0;
|
||||
_velocity_setpoint(1) = 0;
|
||||
_velocity_setpoint(2) = _param_mpc_land_speed.get();
|
||||
|
||||
// check if target visible
|
||||
if (!check_state_conditions(PrecLandState::DescendAboveTarget)) {
|
||||
if (!try_switch_to_state_touching_down()) {
|
||||
PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
|
||||
|
||||
// Stay at current position for searching for the target
|
||||
_position_setpoint = _position;
|
||||
|
||||
if (!try_switch_to_state_auto_rtl()) {
|
||||
try_switch_to_state_fallback();
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FlightTaskAutoPrecisionLanding::run_state_touching_down()
|
||||
{
|
||||
// Overwrite Auto setpoints in order to land at target's last known location
|
||||
_position_setpoint(0) = _landing_target_pose.x_abs;
|
||||
_position_setpoint(1) = _landing_target_pose.y_abs;
|
||||
_position_setpoint(2) = NAN;
|
||||
|
||||
_velocity_setpoint(0) = 0;
|
||||
_velocity_setpoint(1) = 0;
|
||||
_velocity_setpoint(2) = _param_mpc_land_speed.get();
|
||||
}
|
||||
|
||||
void
|
||||
FlightTaskAutoPrecisionLanding::run_state_search()
|
||||
{
|
||||
// Overwrite Auto setpoints in order to hover at search altitude
|
||||
_position_setpoint = _target;
|
||||
_position_setpoint(2) = _sub_home_position.get().z - _param_pld_srch_alt.get();
|
||||
|
||||
_velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN;
|
||||
|
||||
// check if we can see the target
|
||||
if (check_state_conditions(PrecLandState::MoveAboveTarget)) {
|
||||
if (!_target_acquired_time) {
|
||||
// target just became visible. Stop climbing, but give it some margin so we don't stop too apruptly
|
||||
_target_acquired_time = hrt_absolute_time();
|
||||
_position_setpoint = _position;
|
||||
_position_setpoint(2) += 1.0f;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// stay at that height for a second to allow the vehicle to settle
|
||||
if (_target_acquired_time && (hrt_absolute_time() - _target_acquired_time) > 1000000) {
|
||||
// try to switch to horizontal approach
|
||||
if (try_switch_to_state_move_above_target()) {
|
||||
return;
|
||||
}
|
||||
|
||||
} else if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get()*SEC2USEC) {
|
||||
// Search timed out and go to fallback
|
||||
PX4_WARN("Search timed out");
|
||||
|
||||
try_switch_to_state_fallback();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FlightTaskAutoPrecisionLanding::run_state_fallback()
|
||||
{
|
||||
// Try switching to horizontal approach. This works if the
|
||||
// target meanwhile became visible
|
||||
try_switch_to_state_move_above_target();
|
||||
|
||||
// Otherwise there is nothing to do, except for listening to navigator
|
||||
_position_setpoint = _target;
|
||||
}
|
||||
|
||||
bool
|
||||
FlightTaskAutoPrecisionLanding::try_switch_to_state_auto_rtl()
|
||||
{
|
||||
if (check_state_conditions(PrecLandState::AutoRTL)) {
|
||||
_search_cnt++;
|
||||
|
||||
_point_reached_time = 0;
|
||||
|
||||
_state = PrecLandState::AutoRTL;
|
||||
_state_start_time = hrt_absolute_time();
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
FlightTaskAutoPrecisionLanding::try_switch_to_state_move_above_target()
|
||||
{
|
||||
if (check_state_conditions(PrecLandState::MoveAboveTarget)) {
|
||||
print_state_switch_message("horizontal approach");
|
||||
|
||||
_point_reached_time = 0;
|
||||
|
||||
_state = PrecLandState::MoveAboveTarget;
|
||||
_state_start_time = hrt_absolute_time();
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
FlightTaskAutoPrecisionLanding::try_switch_to_state_descend_above_target()
|
||||
{
|
||||
if (check_state_conditions(PrecLandState::DescendAboveTarget)) {
|
||||
print_state_switch_message("descend");
|
||||
_state = PrecLandState::DescendAboveTarget;
|
||||
_state_start_time = hrt_absolute_time();
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
FlightTaskAutoPrecisionLanding::try_switch_to_state_touching_down()
|
||||
{
|
||||
if (check_state_conditions(PrecLandState::TouchingDown)) {
|
||||
print_state_switch_message("final approach");
|
||||
_state = PrecLandState::TouchingDown;
|
||||
_state_start_time = hrt_absolute_time();
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
FlightTaskAutoPrecisionLanding::try_switch_to_state_search()
|
||||
{
|
||||
PX4_INFO("Climbing to search altitude");
|
||||
|
||||
_target_acquired_time = 0;
|
||||
|
||||
_state = PrecLandState::Search;
|
||||
_state_start_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
void
|
||||
FlightTaskAutoPrecisionLanding::try_switch_to_state_fallback()
|
||||
{
|
||||
print_state_switch_message("fallback");
|
||||
|
||||
_state = PrecLandState::Fallback;
|
||||
_state_start_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
void FlightTaskAutoPrecisionLanding::print_state_switch_message(const char *state_name)
|
||||
{
|
||||
PX4_INFO("Precland: switching to %s", state_name);
|
||||
}
|
||||
|
||||
bool FlightTaskAutoPrecisionLanding::check_state_conditions(PrecLandState state)
|
||||
{
|
||||
switch (state) {
|
||||
case PrecLandState::AutoRTL:
|
||||
return _search_cnt <= _param_pld_max_srch.get();
|
||||
|
||||
case PrecLandState::MoveAboveTarget:
|
||||
|
||||
// if we're already in this state, only want to make it invalid if we reached the target but can't see it anymore
|
||||
if (_state == PrecLandState::MoveAboveTarget) {
|
||||
if (hor_acc_radius_check()) {
|
||||
// we've reached the position where we last saw the target. If we don't see it now, we need to do something
|
||||
return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid;
|
||||
|
||||
} else {
|
||||
// We've seen the target sometime during horizontal approach.
|
||||
// Even if we don't see it as we're moving towards it, continue approaching last known location
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// If we're trying to switch to this state, the target needs to be visible
|
||||
return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid;
|
||||
|
||||
case PrecLandState::DescendAboveTarget:
|
||||
|
||||
// if we're already in this state, only leave it if target becomes unusable, don't care about horizontal offset to target
|
||||
if (_state == PrecLandState::DescendAboveTarget) {
|
||||
// if we're close to the ground, we're more critical of target timeouts so we quickly go into descend
|
||||
if (check_state_conditions(PrecLandState::TouchingDown)) {
|
||||
return hrt_absolute_time() - _landing_target_pose.timestamp < 500000; // 0.5s // TODO: Magic number!
|
||||
|
||||
} else {
|
||||
return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid;
|
||||
}
|
||||
|
||||
} else {
|
||||
// if not already in this state, need to be above target to enter it
|
||||
return _landing_target_pose.abs_pos_valid && hor_acc_radius_check();
|
||||
}
|
||||
|
||||
case PrecLandState::TouchingDown:
|
||||
return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid
|
||||
&& (_landing_target_pose.z_abs - _position(2)) < _param_pld_fappr_alt.get();
|
||||
|
||||
case PrecLandState::Search:
|
||||
return true;
|
||||
|
||||
case PrecLandState::Fallback:
|
||||
return true;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool FlightTaskAutoPrecisionLanding::hor_acc_radius_check()
|
||||
{
|
||||
return Vector2f(Vector2f(_landing_target_pose.x_abs,
|
||||
_landing_target_pose.y_abs) - _position.xy()).norm() <= _param_pld_hacc_rad.get();
|
||||
}
|
||||
|
||||
void FlightTaskAutoPrecisionLanding::slewrate(float &sp_x, float &sp_y)
|
||||
{
|
||||
matrix::Vector2f sp_curr(sp_x, sp_y);
|
||||
uint64_t now = hrt_absolute_time();
|
||||
|
||||
float dt = (now - _last_slewrate_time);
|
||||
|
||||
if (dt < 1) {
|
||||
// bad dt, can't divide by it
|
||||
return;
|
||||
}
|
||||
|
||||
dt /= SEC2USEC;
|
||||
|
||||
if (_last_slewrate_time == 0) {
|
||||
// running the first time since switching to precland
|
||||
_sp_pev_prev(0) = sp_x;
|
||||
_sp_pev_prev(1) = sp_y;
|
||||
_sp_pev(0) = sp_x;
|
||||
_sp_pev(1) = sp_y;
|
||||
}
|
||||
|
||||
_last_slewrate_time = now;
|
||||
|
||||
// limit the setpoint speed to the maximum cruise speed
|
||||
matrix::Vector2f sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
|
||||
|
||||
if (sp_vel.length() > _param_xy_vel_cruise.get()) {
|
||||
sp_vel = sp_vel.normalized() * _param_xy_vel_cruise.get();
|
||||
sp_curr = _sp_pev + sp_vel * dt;
|
||||
}
|
||||
|
||||
// limit the setpoint acceleration to the maximum acceleration
|
||||
matrix::Vector2f sp_acc = (sp_curr - _sp_pev * 2 + _sp_pev_prev) / (dt * dt); // acceleration of the setpoints
|
||||
|
||||
if (sp_acc.length() > _param_acceleration_hor.get()) {
|
||||
sp_acc = sp_acc.normalized() * _param_acceleration_hor.get();
|
||||
sp_curr = _sp_pev * 2 - _sp_pev_prev + sp_acc * (dt * dt);
|
||||
}
|
||||
|
||||
// limit the setpoint speed such that we can stop at the setpoint given the maximum acceleration/deceleration
|
||||
float max_spd = sqrtf(_param_acceleration_hor.get() * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x,
|
||||
sp_y))).length());
|
||||
sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
|
||||
|
||||
if (sp_vel.length() > max_spd) {
|
||||
sp_vel = sp_vel.normalized() * max_spd;
|
||||
sp_curr = _sp_pev + sp_vel * dt;
|
||||
}
|
||||
|
||||
_sp_pev_prev = _sp_pev;
|
||||
_sp_pev = sp_curr;
|
||||
|
||||
sp_x = sp_curr(0);
|
||||
sp_y = sp_curr(1);
|
||||
}
|
||||
|
||||
+37
-76
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2023 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -35,41 +35,23 @@
|
||||
* @file FlightTaskAutoPrecisionLanding.hpp
|
||||
*
|
||||
* Flight task for better precision landing
|
||||
*
|
||||
* @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "FlightTaskAuto.hpp"
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/precision_landing_status.h>
|
||||
// #include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <uORB/topics/follow_target_estimator.h>
|
||||
// #include <lib/ecl/geo/geo.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#define SEC2USEC 1000000.0f // TODO: Get the correct define from some header
|
||||
#define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state
|
||||
#define ACCEPTANCE_RADIUS 0.20f // Horizontal acceptance radius for the navigation to the landing target
|
||||
// TODO: Get ACCEPTANCE_RADIUS from NAV_ACC_RAD
|
||||
|
||||
enum class PrecLandState {
|
||||
AutoRTL, // Starting state
|
||||
Search, // Search for landing target
|
||||
MoveAboveTarget, // Positioning over landing target while maintaining altitude
|
||||
DescendAboveTarget, // Stay over landing target while descending
|
||||
TouchingDown, // Final landing approach, even without landing target
|
||||
Fallback // Fallback landing method
|
||||
};
|
||||
|
||||
enum class PrecLandMode {
|
||||
Opportunistic = 1, // only do precision landing if landing target visible at the beginning
|
||||
Required = 2 // try to find landing target if not visible at the beginning
|
||||
};
|
||||
// TODO: Get the correct define from some header
|
||||
#define SEC2USEC 1000000.0f
|
||||
#define HOVER_SPEED_THRESHOLD 0.20f // If drone has speed under this value it is considered to be hovering in place
|
||||
|
||||
class FlightTaskAutoPrecisionLanding : public FlightTaskAuto
|
||||
{
|
||||
@@ -82,60 +64,39 @@ public:
|
||||
bool update() override;
|
||||
|
||||
private:
|
||||
// run the control loop for each state
|
||||
void run_state_auto_rtl();
|
||||
void run_state_search();
|
||||
void run_state_move_above_target();
|
||||
void run_state_descend_above_target();
|
||||
void run_state_touching_down();
|
||||
void run_state_fallback();
|
||||
|
||||
// attempt to switch to a different state. Returns true if state change was successful, false otherwise
|
||||
bool try_switch_to_state_auto_rtl();
|
||||
void try_switch_to_state_search();
|
||||
bool try_switch_to_state_move_above_target();
|
||||
bool try_switch_to_state_descend_above_target();
|
||||
bool try_switch_to_state_touching_down();
|
||||
void try_switch_to_state_fallback();
|
||||
|
||||
void print_state_switch_message(const char *state_name);
|
||||
|
||||
// check if a given state could be changed into. Return true if possible to transition to state, false otherwise
|
||||
bool check_state_conditions(PrecLandState state);
|
||||
void slewrate(float &sp_x, float &sp_y);
|
||||
|
||||
bool hor_acc_radius_check();
|
||||
|
||||
landing_target_pose_s _landing_target_pose{}; /**< precision landing target position */
|
||||
|
||||
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
|
||||
uORB::PublicationMulti<precision_landing_status_s> _precision_landing_status_pub{ORB_ID(precision_landing_status)};
|
||||
|
||||
bool _landing_target_pose_valid{false}; /**< whether we have received a landing target position message */
|
||||
|
||||
uint64_t _state_start_time{0}; /**< time when we entered current state */
|
||||
uint64_t _last_slewrate_time{0}; /**< time when we last limited setpoint changes */
|
||||
uint64_t _target_acquired_time{0}; /**< time when we first saw the landing target during search */
|
||||
uint64_t _point_reached_time{0}; /**< time when we reached a setpoint */
|
||||
|
||||
int _search_cnt{0}; /**< counter of how many times we had to search for the landing target */
|
||||
|
||||
matrix::Vector2f _sp_pev;
|
||||
matrix::Vector2f _sp_pev_prev;
|
||||
|
||||
PrecLandState _state{PrecLandState::AutoRTL};
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
|
||||
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed, ///< velocity for controlled descend
|
||||
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_acceleration_hor,
|
||||
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_xy_vel_cruise,
|
||||
(ParamFloat<px4::params::PLD_BTOUT>) _param_pld_btout,
|
||||
(ParamFloat<px4::params::PLD_HACC_RAD>) _param_pld_hacc_rad,
|
||||
(ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_pld_fappr_alt,
|
||||
(ParamFloat<px4::params::PLD_SRCH_ALT>) _param_pld_srch_alt,
|
||||
(ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_pld_srch_tout,
|
||||
(ParamInt<px4::params::PLD_MAX_SRCH>) _param_pld_max_srch,
|
||||
(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md
|
||||
(ParamFloat<px4::params::PLD_HACC_RAD>) _param_pld_hacc_rad,
|
||||
(ParamFloat<px4::params::PLD_BTOUT>) _param_pld_btout
|
||||
)
|
||||
|
||||
enum PRECLAND_STATE {
|
||||
AUTORTL_CLIMB,
|
||||
AUTORTL_APPROACH,
|
||||
MOVE_TO_SEARCH_ALTITUDE,
|
||||
SEARCHING_TARGET,
|
||||
MOVING_ABOVE_TARGET,
|
||||
LANDING
|
||||
};
|
||||
|
||||
void do_state_transition(PRECLAND_STATE new_state);
|
||||
|
||||
bool inside_acceptance_radius();
|
||||
|
||||
PRECLAND_STATE _precland_state = PRECLAND_STATE::AUTORTL_CLIMB;
|
||||
|
||||
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
|
||||
landing_target_pose_s _landing_target_pose{}; /**< precision landing target position */
|
||||
|
||||
uORB::PublicationMulti<precision_landing_status_s> _precision_landing_status_pub{ORB_ID(precision_landing_status)};
|
||||
|
||||
uint64_t _state_start_time{0}; /**< time when entering search state */
|
||||
int _search_count = 0;
|
||||
|
||||
orb_advert_t _mavlink_log_pub{nullptr};
|
||||
matrix::Vector3f _precision_target_ned = {NAN, NAN, NAN};
|
||||
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user