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

convert precland to flight task

Browse Source
This commit is contained in:
Alessandro Simovic 2022-01-06 12:46:37 +01:00
parent 6e0dee7f11
commit facd4f4cfe
9 changed files with 170 additions and 263 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -146,6 +146,7 @@ set(msg_files
PowerButtonState.msg
PowerMonitor.msg
PpsCapture.msg
precision_landing_status.msg
PwmInput.msg
Px4ioStatus.msg
QshellReq.msg

10
msg/precision_landing_status.msg Normal file
View File

@ -0,0 +1,10 @@
# ORBIT_YAW_BEHAVIOUR
uint8 PRECLAND_STATE_START = 0
uint8 PRECLAND_STATE_HORIZONTAL_APPROACH = 1
uint8 PRECLAND_STATE_DESCEND_ABOVE_TARGET = 2
uint8 PRECLAND_STATE_FINAL_APPROACH = 3
uint8 PRECLAND_STATE_SEARCH = 4
uint8 PRECLAND_STATE_FALLBACK = 5
uint64 timestamp # time since system start (microseconds)
uint8 precland_state # Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. [m]

1
src/modules/flight_mode_manager/CMakeLists.txt
View File

@ -40,6 +40,7 @@
set(flight_tasks_all)
list(APPEND flight_tasks_all
Auto
AutoPrecisionLanding
Descend
Failsafe
ManualAcceleration

10
src/modules/flight_mode_manager/FlightModeManager.cpp
View File

@ -164,6 +164,16 @@ void FlightModeManager::start_flight_task()
task_failure = true;
}
// Take-over landing from navigator if precision landing is enabled
} else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
&& _param_rtl_pld_md.get() > 0) {
should_disable_task = false;
if (switchTask(FlightTaskIndex::AutoPrecisionLanding) != FlightTaskError::NoError) {
task_failure = true;
}
} else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
// Emergency descend

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

@ -157,6 +157,7 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max,
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md
);
};

324
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* 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
@ -31,101 +31,51 @@
*
****************************************************************************/
/**
* @file precland.cpp
*
* Helper class to do precision landing with a landing target
* @file FlightTaskAutoPrecisionLanding.cpp
*
* @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
*/
#include "precland.h"
#include "navigator.h"
#include "FlightTaskAutoPrecisionLanding.hpp"
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_command.h>
#define SEC2USEC 1000000.0f
#define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state
static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing";
PrecLand::PrecLand(Navigator *navigator) :
MissionBlock(navigator),
ModuleParams(navigator)
bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint)
{
_handle_param_acceleration_hor = param_find("MPC_ACC_HOR");
_handle_param_xy_vel_cruise = param_find("MPC_XY_CRUISE");
bool ret = FlightTask::activate(last_setpoint);
updateParams();
}
// This looks wrong at first, but is a mean little trick to avoid discontinuous setpoints.
// When this flight task is activated, _target might be outdated until the next navigator triplet
// comes in. Until then pretend that the current position setpoint is the navigator's setpoint:
_target = _position_setpoint;
void
PrecLand::on_activation()
{
_state = PrecLandState::Start;
_search_cnt = 0;
_last_slewrate_time = 0;
vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
if (!_map_ref.isInitialized()) {
_map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
}
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->next.valid = false;
pos_sp_triplet->previous.valid = false;
// Check that the current position setpoint is valid, otherwise land at current position
if (!pos_sp_triplet->current.valid) {
PX4_WARN("Reset");
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.valid = true;
pos_sp_triplet->current.timestamp = hrt_absolute_time();
}
_sp_pev = matrix::Vector2f(0, 0);
_sp_pev_prev = matrix::Vector2f(0, 0);
_last_slewrate_time = 0;
_sp_pev = matrix::Vector2f(last_setpoint.x, last_setpoint.y);
_sp_pev_prev = matrix::Vector2f(last_setpoint.x, last_setpoint.y);
switch_to_state_start();
_is_activated = true;
return ret;
}
void
PrecLand::on_active()
bool FlightTaskAutoPrecisionLanding::update()
{
// get new target measurement
_target_pose_updated = _target_pose_sub.update(&_target_pose);
bool ret = FlightTaskAuto::update();
if (_target_pose_updated) {
_target_pose_valid = true;
// Fetch uorb
if (_target_pose_sub.updated()) {
_target_pose_sub.copy(&_target_pose);
}
if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) > _param_pld_btout.get()) {
_target_pose_valid = false;
}
// stop if we are landed
if (_navigator->get_land_detected()->landed) {
switch_to_state_done();
}
// target pose can become invalid when the message timed out
_target_pose_valid = (hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get();
switch (_state) {
case PrecLandState::Start:
@ -152,88 +102,66 @@ PrecLand::on_active()
run_state_fallback();
break;
case PrecLandState::Done:
// nothing to do
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();
return ret;
}
void
PrecLand::on_inactivation()
FlightTaskAutoPrecisionLanding::run_state_start()
{
_is_activated = false;
}
_position_setpoint = _target; // Follow navigator triplet
void
PrecLand::updateParams()
{
ModuleParams::updateParams();
if (_handle_param_acceleration_hor != PARAM_INVALID) {
param_get(_handle_param_acceleration_hor, &_param_acceleration_hor);
}
if (_handle_param_xy_vel_cruise != PARAM_INVALID) {
param_get(_handle_param_xy_vel_cruise, &_param_xy_vel_cruise);
}
}
void
PrecLand::run_state_start()
{
// check if target visible and go to horizontal approach
// check if target visible and go to horizontal approach directly
if (switch_to_state_horizontal_approach()) {
return;
}
if (_mode == PrecLandMode::Opportunistic) {
} else if ((PrecLandMode)_param_rtl_pld_md.get() == PrecLandMode::Opportunistic) {
// could not see the target immediately, so just fall back to normal landing
switch_to_state_fallback();
}
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
float dist = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
} else if (_type == WaypointType::land) {
// check if we've reached the start point
if (dist < _navigator->get_acceptance_radius()) {
if (!_point_reached_time) {
_point_reached_time = hrt_absolute_time();
}
// if we don't see the target after 1 second, search for it
if (_param_pld_srch_tout.get() > 0) {
if (hrt_absolute_time() - _point_reached_time > 2000000) {
if (!switch_to_state_search()) {
switch_to_state_fallback();
}
}
} else {
switch_to_state_fallback();
}
// Navigator already entered land stage. Take over with precision landing
switch_to_state_search();
}
}
void
PrecLand::run_state_horizontal_approach()
FlightTaskAutoPrecisionLanding::run_state_horizontal_approach()
{
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
float x = _target_pose.x_abs;
float y = _target_pose.y_abs;
slewrate(x, y);
// 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) = _velocity_setpoint(2) = NAN;
// check if target visible, if not go to start
if (!check_state_conditions(PrecLandState::HorizontalApproach)) {
PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
// Stay at current position for searching for the landing target
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
// 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 (!switch_to_state_start()) {
switch_to_state_fallback();
@ -256,25 +184,18 @@ PrecLand::run_state_horizontal_approach()
}
}
float x = _target_pose.x_abs;
float y = _target_pose.y_abs;
slewrate(x, y);
// XXX need to transform to GPS coords because mc_pos_control only looks at that
_map_ref.reproject(x, y, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
pos_sp_triplet->current.alt = _approach_alt;
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_navigator->set_position_setpoint_triplet_updated();
}
void
PrecLand::run_state_descend_above_target()
FlightTaskAutoPrecisionLanding::run_state_descend_above_target()
{
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// Overwrite Auto setpoints in order to descend above target
_position_setpoint(0) = _target_pose.x_abs;
_position_setpoint(1) = _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)) {
@ -282,9 +203,7 @@ PrecLand::run_state_descend_above_target()
PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
// Stay at current position for searching for the target
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
_position_setpoint = _position;
if (!switch_to_state_start()) {
switch_to_state_fallback();
@ -293,33 +212,35 @@ PrecLand::run_state_descend_above_target()
return;
}
// XXX need to transform to GPS coords because mc_pos_control only looks at that
_map_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
_navigator->set_position_setpoint_triplet_updated();
}
void
PrecLand::run_state_final_approach()
FlightTaskAutoPrecisionLanding::run_state_final_approach()
{
// nothing to do, will land
// Overwrite Auto setpoints in order to land at target's last known location
_position_setpoint(0) = _target_pose.x_abs;
_position_setpoint(1) = _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
PrecLand::run_state_search()
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::HorizontalApproach)) {
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_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
float new_alt = _navigator->get_global_position()->alt + 1.0f;
pos_sp_triplet->current.alt = new_alt < pos_sp_triplet->current.alt ? new_alt : pos_sp_triplet->current.alt;
_navigator->set_position_setpoint_triplet_updated();
_position_setpoint = _position;
_position_setpoint(2) += 1.0f;
}
}
@ -341,18 +262,19 @@ PrecLand::run_state_search()
}
void
PrecLand::run_state_fallback()
FlightTaskAutoPrecisionLanding::run_state_fallback()
{
// nothing to do, will land
// nothing to do, just listen to navigator
_position_setpoint = _target;
_velocity_setpoint(0) = 0;
_velocity_setpoint(1) = 0;
_velocity_setpoint(2) = _param_mpc_land_speed.get();
}
bool
PrecLand::switch_to_state_start()
FlightTaskAutoPrecisionLanding::switch_to_state_start()
{
if (check_state_conditions(PrecLandState::Start)) {
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_navigator->set_position_setpoint_triplet_updated();
_search_cnt++;
_point_reached_time = 0;
@ -366,11 +288,10 @@ PrecLand::switch_to_state_start()
}
bool
PrecLand::switch_to_state_horizontal_approach()
FlightTaskAutoPrecisionLanding::switch_to_state_horizontal_approach()
{
if (check_state_conditions(PrecLandState::HorizontalApproach)) {
print_state_switch_message("horizontal approach");
_approach_alt = _navigator->get_global_position()->alt;
_point_reached_time = 0;
@ -383,7 +304,7 @@ PrecLand::switch_to_state_horizontal_approach()
}
bool
PrecLand::switch_to_state_descend_above_target()
FlightTaskAutoPrecisionLanding::switch_to_state_descend_above_target()
{
if (check_state_conditions(PrecLandState::DescendAboveTarget)) {
print_state_switch_message("descend");
@ -396,7 +317,7 @@ PrecLand::switch_to_state_descend_above_target()
}
bool
PrecLand::switch_to_state_final_approach()
FlightTaskAutoPrecisionLanding::switch_to_state_final_approach()
{
if (check_state_conditions(PrecLandState::FinalApproach)) {
print_state_switch_message("final approach");
@ -408,57 +329,33 @@ PrecLand::switch_to_state_final_approach()
return false;
}
bool
PrecLand::switch_to_state_search()
void
FlightTaskAutoPrecisionLanding::switch_to_state_search()
{
PX4_INFO("Climbing to search altitude.");
vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.alt = vehicle_local_position->ref_alt + _param_pld_srch_alt.get();
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_navigator->set_position_setpoint_triplet_updated();
PX4_INFO("Climbing to search altitude");
_target_acquired_time = 0;
_state = PrecLandState::Search;
_state_start_time = hrt_absolute_time();
return true;
}
bool
PrecLand::switch_to_state_fallback()
void
FlightTaskAutoPrecisionLanding::switch_to_state_fallback()
{
print_state_switch_message("fallback");
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
_navigator->set_position_setpoint_triplet_updated();
_state = PrecLandState::Fallback;
_state_start_time = hrt_absolute_time();
return true;
}
bool
PrecLand::switch_to_state_done()
{
_state = PrecLandState::Done;
_state_start_time = hrt_absolute_time();
return true;
}
void PrecLand::print_state_switch_message(const char *state_name)
void FlightTaskAutoPrecisionLanding::print_state_switch_message(const char *state_name)
{
PX4_INFO("Precland: switching to %s", state_name);
}
bool PrecLand::check_state_conditions(PrecLandState state)
bool FlightTaskAutoPrecisionLanding::check_state_conditions(PrecLandState state)
{
vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
switch (state) {
case PrecLandState::Start:
return _search_cnt <= _param_pld_max_srch.get();
@ -467,8 +364,7 @@ bool PrecLand::check_state_conditions(PrecLandState state)
// 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::HorizontalApproach) {
if (fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get()
&& fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get()) {
if (Vector2f(Vector2f(_target_pose.x_abs, _target_pose.y_abs) - _position.xy()).norm() <= _param_pld_hacc_rad.get()) {
// we've reached the position where we last saw the target. If we don't see it now, we need to do something
return _target_pose_valid && _target_pose.abs_pos_valid;
@ -480,15 +376,15 @@ bool PrecLand::check_state_conditions(PrecLandState state)
}
// If we're trying to switch to this state, the target needs to be visible
return _target_pose_updated && _target_pose_valid && _target_pose.abs_pos_valid;
return _target_pose_valid && _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 horizontall offset to target
// 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::FinalApproach)) {
return hrt_absolute_time() - _target_pose.timestamp < 500000; // 0.5s
return hrt_absolute_time() - _target_pose.timestamp < 500000; // 0.5s // TODO: Magic number!
} else {
return _target_pose_valid && _target_pose.abs_pos_valid;
@ -496,14 +392,14 @@ bool PrecLand::check_state_conditions(PrecLandState state)
} else {
// if not already in this state, need to be above target to enter it
return _target_pose_updated && _target_pose.abs_pos_valid
&& fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get()
&& fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get();
return _target_pose.abs_pos_valid
&& fabsf(_target_pose.x_abs - _position(0)) < _param_pld_hacc_rad.get()
&& fabsf(_target_pose.y_abs - _position(1)) < _param_pld_hacc_rad.get();
}
case PrecLandState::FinalApproach:
return _target_pose_valid && _target_pose.abs_pos_valid
&& (_target_pose.z_abs - vehicle_local_position->z) < _param_pld_fappr_alt.get();
&& (_target_pose.z_abs - _position(2)) < _param_pld_fappr_alt.get();
case PrecLandState::Search:
return true;
@ -516,7 +412,7 @@ bool PrecLand::check_state_conditions(PrecLandState state)
}
}
void PrecLand::slewrate(float &sp_x, float &sp_y)
void FlightTaskAutoPrecisionLanding::slewrate(float &sp_x, float &sp_y)
{
matrix::Vector2f sp_curr(sp_x, sp_y);
uint64_t now = hrt_absolute_time();
@ -537,10 +433,8 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
dt = 50000 / SEC2USEC;
// set a best guess for previous setpoints for smooth transition
_sp_pev = _map_ref.project(_navigator->get_position_setpoint_triplet()->current.lat,
_navigator->get_position_setpoint_triplet()->current.lon);
_sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt;
_sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt;
_sp_pev_prev(0) = _sp_pev(0) - _velocity(0) * dt;
_sp_pev_prev(1) = _sp_pev(1) - _velocity(1) * dt;
}
_last_slewrate_time = now;
@ -548,21 +442,21 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
// 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) {
sp_vel = sp_vel.normalized() * _param_xy_vel_cruise;
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) {
sp_acc = sp_acc.normalized() * _param_acceleration_hor;
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 * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x,
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

83
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp
View File

@ -1,6 +1,6 @@
/***************************************************************************
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* 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
@ -30,24 +30,32 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file precland.h
* @file FlightTaskAutoPrecisionLanding.hpp
*
* Helper class to do precision landing with a landing target
* 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 "navigator_mode.h"
#include "mission_block.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 {
Start, // Starting state
@ -55,8 +63,7 @@ enum class PrecLandState {
DescendAboveTarget, // Stay over landing target while descending
FinalApproach, // Final landing approach, even without landing target
Search, // Search for landing target
Fallback, // Fallback landing method
Done // Done landing
Fallback // Fallback landing method
};
enum class PrecLandMode {
@ -64,26 +71,17 @@ enum class PrecLandMode {
Required = 2 // try to find landing target if not visible at the beginning
};
class PrecLand : public MissionBlock, public ModuleParams
class FlightTaskAutoPrecisionLanding : public FlightTaskAuto
{
public:
PrecLand(Navigator *navigator);
~PrecLand() override = default;
FlightTaskAutoPrecisionLanding() = default;
virtual ~FlightTaskAutoPrecisionLanding() = default;
void on_activation() override;
void on_active() override;
void on_inactivation() override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
void set_mode(PrecLandMode mode) { _mode = mode; };
PrecLandMode get_mode() { return _mode; };
bool is_activated() { return _is_activated; };
bool update() override;
private:
void updateParams() override;
// run the control loop for each state
void run_state_start();
void run_state_horizontal_approach();
@ -97,9 +95,8 @@ private:
bool switch_to_state_horizontal_approach();
bool switch_to_state_descend_above_target();
bool switch_to_state_final_approach();
bool switch_to_state_search();
bool switch_to_state_fallback();
bool switch_to_state_done();
void switch_to_state_search();
void switch_to_state_fallback();
void print_state_switch_message(const char *state_name);
@ -110,10 +107,9 @@ private:
landing_target_pose_s _target_pose{}; /**< precision landing target position */
uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)};
bool _target_pose_valid{false}; /**< whether we have received a landing target position message */
bool _target_pose_updated{false}; /**< wether the landing target position message is updated */
uORB::PublicationMulti<precision_landing_status_s> _precision_landing_status_pub{ORB_ID(precision_landing_status)};
MapProjection _map_ref{}; /**< class for local/global projections */
bool _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 */
@ -121,30 +117,23 @@ private:
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 */
float _approach_alt{0.0f}; /**< altitude at which to stay during horizontal approach */
matrix::Vector2f _sp_pev;
matrix::Vector2f _sp_pev_prev;
PrecLandState _state{PrecLandState::Start};
PrecLandMode _mode{PrecLandMode::Opportunistic};
bool _is_activated {false}; /**< indicates if precland is activated */
DEFINE_PARAMETERS(
(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
)
// non-navigator parameters
param_t _handle_param_acceleration_hor{PARAM_INVALID};
param_t _handle_param_xy_vel_cruise{PARAM_INVALID};
float _param_acceleration_hor{0.0f};
float _param_xy_vel_cruise{0.0f};
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
)
};

0
src/modules/navigator/precland_params.c → src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/precland_params.c
View File

1
src/modules/logger/logged_topics.cpp
View File

@ -96,6 +96,7 @@ void LoggedTopics::add_default_topics()
add_topic("position_controller_status", 500);
add_topic("position_controller_landing_status", 100);
add_topic("position_setpoint_triplet", 200);
add_topic("precision_landing_status", 1000);
add_optional_topic("px4io_status");
add_topic("radio_status");
add_topic("rtl_time_estimate", 1000);