mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-09 14:11:38 +08:00
convert precland to flight task
This commit is contained in:
Alessandro Simovic
+109
-215
@@ -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
|
||||
|
||||
|
||||
Reference in New Issue
Block a user