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

split logic into simpler functions

Browse Source
This commit is contained in:
Alessandro Simovic
2023-03-31 19:06:00 +02:00
parent d20429a72a
commit 7801806b1c
2 changed files with 144 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.cpp
+135 -93
View File
@@ -44,14 +44,11 @@ using namespace matrix;
bool FlightTaskPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
PX4_INFO("precland activated");
_precland_state = PRECLAND_STATE::AUTORTL_CLIMB;
_position_setpoint = _position;
_search_count = 0;
// TODO: Use target orientation instead
_initial_yaw = _yaw;
_initial_position = _position;
@@ -72,45 +69,118 @@ bool FlightTaskPrecisionLanding::inside_acceptance_radius()
bool FlightTaskPrecisionLanding::precision_target_available()
{
// TODO: Add timeout
const bool ever_received = _landing_target_pose.timestamp != 0;
const bool timed_out = hrt_absolute_time() - _landing_target_pose.timestamp > _param_pld_btout.get() * SEC2USEC;
return ever_received && !timed_out;
}
bool FlightTaskPrecisionLanding::update()
void FlightTaskPrecisionLanding::generate_pos_xy_setpoints()
{
// Get setpoints from FlightTask and later override if necessary
bool ret = FlightTask::update();
switch (_precland_state) {
case PRECLAND_STATE::AUTORTL_CLIMB:
_position_setpoint(0) = _initial_position(0);
_position_setpoint(1) = _initial_position(1);
break;
// Fetch uorb
if (_landing_target_pose_sub.updated()) {
_landing_target_pose_sub.copy(&_landing_target_pose);
case PRECLAND_STATE::AUTORTL_APPROACH:
case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
_precision_target_ned(0) = _landing_target_pose.x_abs;
_precision_target_ned(1) = _landing_target_pose.y_abs;
// Horizontal approach to home or precision target
if (precision_target_available()) {
_position_setpoint(0) = _landing_target_pose.x_abs;
_position_setpoint(1) = _landing_target_pose.y_abs;
// Supplement precision target altitude with home postion's altitude if necessary
_precision_target_ned(2) = PX4_ISFINITE(_landing_target_pose.z_rel) ? _landing_target_pose.z_rel :
_sub_home_position.get().z;
} else {
_position_setpoint(0) = _sub_home_position.get().x;
_position_setpoint(1) = _sub_home_position.get().y;
}
break;
case PRECLAND_STATE::SEARCHING_TARGET:
case PRECLAND_STATE::MOVING_ABOVE_TARGET:
case PRECLAND_STATE::LANDING:
_position_setpoint(0) = _landing_target_pose.x_abs;
_position_setpoint(1) = _landing_target_pose.y_abs;
break;
}
}
// Fetch runtime parameters
const float max_search_duration = _param_pld_srch_tout.get();
void FlightTaskPrecisionLanding::generate_pos_z_setpoints()
{
const float search_rel_altitude = _param_pld_srch_alt.get();
switch (_precland_state) {
case PRECLAND_STATE::AUTORTL_CLIMB:
case PRECLAND_STATE::AUTORTL_APPROACH:
// Horizontal approach to home or precision target
_position_setpoint(2) = _sub_home_position.get().z - _param_rtl_return_alt.get();
break;
case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
case PRECLAND_STATE::SEARCHING_TARGET:
case PRECLAND_STATE::MOVING_ABOVE_TARGET:
// TODO: Should use current altitude when state was entered as the altitude setpoint
if (PX4_ISFINITE(_landing_target_pose.z_abs)){
// For safety reasons take whichever reference altitude is higher up (lower in NED)
_position_setpoint(2) = math::min(_sub_home_position.get().z, _landing_target_pose.z_abs) - search_rel_altitude;
} else {
_position_setpoint(2) = _sub_home_position.get().z - search_rel_altitude;
}
break;
case PRECLAND_STATE::LANDING:
_position_setpoint(2) = NAN;
break;
}
}
void FlightTaskPrecisionLanding::generate_vel_setpoints()
{
switch (_precland_state) {
case PRECLAND_STATE::AUTORTL_CLIMB:
case PRECLAND_STATE::AUTORTL_APPROACH:
case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
case PRECLAND_STATE::SEARCHING_TARGET:
case PRECLAND_STATE::MOVING_ABOVE_TARGET:
_velocity_setpoint.setNaN();
break;
case PRECLAND_STATE::LANDING:
_velocity_setpoint(0) = 0;
_velocity_setpoint(1) = 0;
_velocity_setpoint(2) = _param_mpc_land_speed.get();
break;
}
}
void FlightTaskPrecisionLanding::generate_acc_setpoints()
{
_acceleration_setpoint.setNaN();
}
void FlightTaskPrecisionLanding::generate_yaw_setpoint()
{
switch (_precland_state) {
case PRECLAND_STATE::AUTORTL_CLIMB:
_yaw_setpoint = _initial_yaw;
break;
case PRECLAND_STATE::AUTORTL_APPROACH:
// TODO: Point in direction of flight
case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
case PRECLAND_STATE::SEARCHING_TARGET:
case PRECLAND_STATE::MOVING_ABOVE_TARGET:
case PRECLAND_STATE::LANDING:
_yaw_setpoint = _target_yaw;
break;
}
}
void FlightTaskPrecisionLanding::check_state_transitions()
{
switch (_precland_state) {
case PRECLAND_STATE::AUTORTL_CLIMB:
PX4_INFO("AUTORTL_CLIMB");
_position_setpoint(0) = _initial_position(0);
_position_setpoint(1) = _initial_position(1);
_position_setpoint(2) = _sub_home_position.get().z - _param_rtl_return_alt.get();
_velocity_setpoint.setNaN(); // TODO: Is there a RTL climb speed?
_acceleration_setpoint.setNaN();
_yaw_setpoint = _initial_yaw;
// transition condition
// Wait for drone to reach z position setpoint or be higher
if (_position(2) <= _position_setpoint(2) + _param_nav_mc_alt_rad.get()) {
@@ -123,21 +193,6 @@ bool FlightTaskPrecisionLanding::update()
case PRECLAND_STATE::AUTORTL_APPROACH: {
PX4_INFO("AUTORTL_APPROACH");
// Horizontal approach to home or precision target
if (precision_target_available()) {
_position_setpoint(0) = _precision_target_ned(0);
_position_setpoint(1) = _precision_target_ned(1);
_position_setpoint(2) = _precision_target_ned(2);
} else {
PX4_INFO("reverting to HOME...");
_position_setpoint(0) = _sub_home_position.get().x;
_position_setpoint(1) = _sub_home_position.get().y;
_position_setpoint(2) = _sub_home_position.get().z - _param_rtl_return_alt.get();
}
_velocity_setpoint.setNaN();
// transition condition
// Wait for drone to reach position setpoint
if (inside_acceptance_radius()) {
@@ -159,16 +214,6 @@ bool FlightTaskPrecisionLanding::update()
case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
PX4_INFO("MOVE_TO_SEARCH_ALTITUDE");
// Ascend/Descend to search altitude
_position_setpoint(0) = _sub_home_position.get().x;
_position_setpoint(1) = _sub_home_position.get().y;
_position_setpoint(2) = _sub_home_position.get().z - search_rel_altitude;
_velocity_setpoint.setNaN();
_acceleration_setpoint.setNaN();
_yaw_setpoint = _target_yaw;
// transition condition
if (inside_acceptance_radius()) {
mavlink_log_info(&_mavlink_log_pub, "Starting search");
@@ -178,44 +223,36 @@ bool FlightTaskPrecisionLanding::update()
break;
case PRECLAND_STATE::SEARCHING_TARGET:
PX4_INFO("SEARCHING_TARGET");
{
PX4_INFO("SEARCHING_TARGET");
const float max_search_duration = _param_pld_srch_tout.get();
_yaw_setpoint = _target_yaw;
// 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");
// 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);
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::SEARCHING_TARGET);
} else {
mavlink_log_info(&_mavlink_log_pub, "Moving to search center");
do_state_transition(PRECLAND_STATE::SEARCHING_TARGET);
}
}
}
// transition condition
if (precision_target_available()) {
mavlink_log_info(&_mavlink_log_pub, "Moving above target");
do_state_transition(PRECLAND_STATE::MOVING_ABOVE_TARGET);
}
// transition condition
if (precision_target_available()) {
mavlink_log_info(&_mavlink_log_pub, "Moving above target");
do_state_transition(PRECLAND_STATE::MOVING_ABOVE_TARGET);
}
break;
break;
}
case PRECLAND_STATE::MOVING_ABOVE_TARGET:
PX4_INFO("MOVING_ABOVE_TARGET");
// TODO: Should use current altitude when state was entered as the altitude setpoint
_position_setpoint(0) = _precision_target_ned(0);
_position_setpoint(1) = _precision_target_ned(1);
_position_setpoint(2) = _sub_home_position.get().z - search_rel_altitude;
_velocity_setpoint.setNaN();
_acceleration_setpoint.setNaN();
_yaw_setpoint = _target_yaw;
// Transition condition
// - Check acceptance radius and
@@ -237,23 +274,28 @@ bool FlightTaskPrecisionLanding::update()
break;
case PRECLAND_STATE::LANDING:
PX4_INFO("LANDING");
// Control XY position and Z velocity
_position_setpoint(0) = _precision_target_ned(0);
_position_setpoint(1) = _precision_target_ned(1);
_position_setpoint(2) = NAN;
_velocity_setpoint(0) = 0;
_velocity_setpoint(1) = 0;
_velocity_setpoint(2) = _param_mpc_land_speed.get();
_acceleration_setpoint(2) = NAN;
_yaw_setpoint = _target_yaw;
break;
}
}
bool FlightTaskPrecisionLanding::update()
{
// Get setpoints from FlightTask and later override if necessary
bool ret = FlightTask::update();
// Fetch uorb
if (_landing_target_pose_sub.updated()) {
_landing_target_pose_sub.copy(&_landing_target_pose);
}
generate_pos_xy_setpoints();
generate_pos_z_setpoints();
generate_vel_setpoints();
generate_acc_setpoints();
generate_yaw_setpoint();
check_state_transitions();
precision_landing_status_s precision_landing_status{};
precision_landing_status.timestamp = hrt_absolute_time();
src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.hpp
+9 -2
View File
@@ -92,6 +92,15 @@ private:
bool precision_target_available();
void generate_setpoints();
void generate_pos_xy_setpoints();
void generate_pos_z_setpoints();
void generate_vel_setpoints();
void generate_acc_setpoints();
void generate_yaw_setpoint();
void check_state_transitions();
PRECLAND_STATE _precland_state;
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
@@ -106,6 +115,4 @@ private:
matrix::Vector3f _initial_position;
orb_advert_t _mavlink_log_pub{nullptr};
matrix::Vector3f _precision_target_ned = {NAN, NAN, NAN};
};