mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-09 04:41:36 +08:00
split logic into simpler functions
This commit is contained in:
Alessandro Simovic
+135
-93
@@ -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();
|
||||
|
||||
Reference in New Issue
Block a user