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

added back inheritance from FlightTaskAuto, fixed logic in multiple places, use Navigator Land submode for auto_precision_land. Last remaining issues is Pause during precision land will place vehicle back at the XY where the Land was initiated.

Browse Source
This commit is contained in:
Jacob Dahl
2023-03-16 11:56:07 -08:00
parent 93afe5b2e2
commit 7bfcbd7865
8 changed files with 54 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/FlightModeManager.cpp
+1
View File
@@ -446,6 +446,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
// activation failed
if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) {
PX4_INFO("activate failed");
_current_task.task->~FlightTask();
_current_task.task = nullptr;
_current_task.index = FlightTaskIndex::None;
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+4 -1
View File
@@ -104,6 +104,10 @@ bool FlightTaskAuto::updateInitialize()
bool FlightTaskAuto::update()
{
if (_type_previous != _type) {
PX4_INFO("_type: %d", (int)_type);
}
bool ret = FlightTask::update();
// always reset constraints because they might change depending on the type
_setDefaultConstraints();
@@ -324,7 +328,6 @@ bool FlightTaskAuto::_evaluateTriplets()
// Check if triplet is valid. There must be at least a valid altitude.
if (!_sub_triplet_setpoint.get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) {
PX4_INFO("forcing setpoint type to loiter");
// Best we can do is to just set all waypoints to current state
_prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position;
_type = WaypointType::loiter;
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp
+42 -44
View File
@@ -52,13 +52,8 @@ static constexpr int32_t RTL_PREC_LAND_REQUIRED = 2;
bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint)
{
// PX4_INFO("FlightTaskAutoPrecisionLanding::activate");
bool ret = FlightTask::activate(last_setpoint);
_position_setpoint = _position;
_velocity_setpoint = _velocity;
_yaw_setpoint = _yaw;
_yawspeed_setpoint = 0.0f;
PX4_INFO("FlightTaskAutoPrecisionLanding::activate");
bool ret = FlightTaskAuto::activate(last_setpoint);
_search_count = 0;
_last_slewrate_time = 0;
@@ -74,15 +69,19 @@ bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_
// Regular precision land mode and mission precision land mode have the same behavior
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
_mode = PrecLandMode::RegularPrecisionLand;
PX4_INFO("mode: %d", (int)_mode);
} else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND && _param_rtl_pld_md.get() >= RTL_PREC_LAND_OPPORTUNISTIC) {
_mode = PrecLandMode::RegularLandOpportunistic;
PX4_INFO("mode: %d", (int)_mode);
} else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && _param_rtl_pld_md.get() == RTL_PREC_LAND_OPPORTUNISTIC) {
_mode = PrecLandMode::ReturnToLaunchOpportunistic;
PX4_INFO("mode: %d", (int)_mode);
} else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && _param_rtl_pld_md.get() == RTL_PREC_LAND_REQUIRED) {
_mode = PrecLandMode::ReturnToLaunchRequired;
PX4_INFO("mode: %d", (int)_mode);
}
return ret;
@@ -90,7 +89,7 @@ bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_
bool FlightTaskAutoPrecisionLanding::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
bool ret = FlightTaskAuto::updateInitialize();
_sub_home_position.update();
_sub_vehicle_status.update();
@@ -112,13 +111,9 @@ bool FlightTaskAutoPrecisionLanding::updateInitialize()
bool FlightTaskAutoPrecisionLanding::update()
{
bool ret = FlightTask::update();
bool ret = FlightTaskAuto::update();
if (_landing_target_pose_sub.updated()) {
_landing_target_pose_sub.copy(&_landing_target_pose);
}
_landing_target_valid = (hrt_elapsed_time(&_landing_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get();
_landing_target_valid = (hrt_elapsed_time(&_landing_target_pose_sub.get().timestamp) / 1e6f) <= _param_pld_btout.get();
switch (_state) {
@@ -173,9 +168,9 @@ void FlightTaskAutoPrecisionLanding::state_on_enter(PrecLandState state)
_search_count++;
_search_start_time = hrt_absolute_time();
break;
case PrecLandState::HorizontalApproach:
_horizontal_approach_alt = _position(2);
break;
// case PrecLandState::HorizontalApproach:
// _horizontal_approach_alt = _position(2);
// break;
default:
break;
}
@@ -189,22 +184,22 @@ void FlightTaskAutoPrecisionLanding::state_on_exit(PrecLandState state)
void FlightTaskAutoPrecisionLanding::run_state_idle()
{
if (_fix_this_activate_update_loop) {
_fix_this_activate_update_loop = false;
return;
}
bool is_rtl = _mode == PrecLandMode::ReturnToLaunchRequired || _mode == PrecLandMode::ReturnToLaunchOpportunistic;
bool within_home_acceptance = Vector2f(Vector2f(_sub_home_position.get().x,
_sub_home_position.get().y) - _position.xy()).norm() <= _target_acceptance_radius;
// PX4_INFO("run_state_idle: waypoint type: %d", (int)_type);
// We must wait for RTL to get home before starting
if (is_rtl && !within_home_acceptance) {
return;
} else if (is_rtl && within_home_acceptance) {
PX4_INFO("RTL reached home point");
}
switch_state(PrecLandState::Start);
}
void FlightTaskAutoPrecisionLanding::run_state_start()
{
// Initialize our position setpoint to the current position
_position_setpoint = _position;
if (_landing_target_valid) {
switch_state(PrecLandState::HorizontalApproach);
@@ -212,8 +207,11 @@ void FlightTaskAutoPrecisionLanding::run_state_start()
PX4_INFO("Target not seen");
// Check if opportunistic mode is enabled
if (_param_rtl_pld_md.get() == RTL_PREC_LAND_REQUIRED) {
bool required = _mode == PrecLandMode::RegularPrecisionLand ||
_mode == PrecLandMode::MissionPrecisionLand ||
_mode == PrecLandMode::ReturnToLaunchRequired;
if (required) {
switch_state(PrecLandState::Search);
} else {
@@ -224,13 +222,15 @@ void FlightTaskAutoPrecisionLanding::run_state_start()
void FlightTaskAutoPrecisionLanding::run_state_normal_land()
{
// Do nothing
if (_landing_target_valid) {
switch_state(PrecLandState::HorizontalApproach);
return;
}
}
void FlightTaskAutoPrecisionLanding::run_state_search()
{
_position_setpoint(2) = _sub_home_position.get().z - _param_pld_srch_alt.get();
// _velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN;
_target(2) = _position_setpoint(2) = _sub_home_position.get().z - _param_pld_srch_alt.get();
// If target is seen run horizontal approach
if (_landing_target_valid) {
@@ -258,17 +258,16 @@ void FlightTaskAutoPrecisionLanding::run_state_horizontal_approach()
return;
}
float x = _landing_target_pose.x_abs;
float y = _landing_target_pose.y_abs;
float x = _landing_target_pose_sub.get().x_abs;
float y = _landing_target_pose_sub.get().y_abs;
slewrate(x, y); // TODO: Replace this with PX4's AlphaFilter
// Fly to target XY position, maintain current altitude
_position_setpoint(0) = x;
_position_setpoint(1) = y;
_position_setpoint(2) = _horizontal_approach_alt;
_target(0) = _position_setpoint(0) = x;
_target(1) = _position_setpoint(1) = y;
// Check if it's time to descend
Vector2f target_position_vector = Vector2f(_landing_target_pose.x_abs,_landing_target_pose.y_abs);
Vector2f target_position_vector = Vector2f(_landing_target_pose_sub.get().x_abs,_landing_target_pose_sub.get().y_abs);
Vector2f position_delta_vector = Vector2f(target_position_vector - _position.xy());
if (position_delta_vector.norm() <= _param_pld_hacc_rad.get() ) {
@@ -283,13 +282,12 @@ void FlightTaskAutoPrecisionLanding::run_state_descend_above_target()
return;
}
// Let's assume the FlightTask::update() function is preparing our land setpoints.
_position_setpoint(0) = _landing_target_pose.x_abs;
_position_setpoint(1) = _landing_target_pose.y_abs;
_position_setpoint(2) = 0;
// FlightTask::update() function is preparing our land setpoints.
_target(0) = _position_setpoint(0) = _landing_target_pose_sub.get().x_abs;
_target(1) = _position_setpoint(1) = _landing_target_pose_sub.get().y_abs;
// Check if we're within our final approach altitude
if ((_landing_target_pose.z_abs - _position(2)) < _param_pld_fappr_alt.get()) {
if ((_landing_target_pose_sub.get().z_abs - _position(2)) < _param_pld_fappr_alt.get()) {
switch_state(PrecLandState::Finished);
}
}
@@ -301,8 +299,8 @@ void FlightTaskAutoPrecisionLanding::run_state_finished()
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();
return Vector2f(Vector2f(_landing_target_pose_sub.get().x_abs,
_landing_target_pose_sub.get().y_abs) - _position.xy()).norm() <= _param_pld_hacc_rad.get();
}
void FlightTaskAutoPrecisionLanding::slewrate(float &sp_x, float &sp_y)
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp
+1 -3
View File
@@ -70,7 +70,7 @@ enum class PrecLandMode {
MissionPrecisionLand
};
class FlightTaskAutoPrecisionLanding : public FlightTask
class FlightTaskAutoPrecisionLanding : public FlightTaskAuto
{
public:
FlightTaskAutoPrecisionLanding() = default;
@@ -106,8 +106,6 @@ private:
uORB::PublicationMulti<precision_landing_status_s> _precision_landing_status_pub {ORB_ID(precision_landing_status)};
landing_target_pose_s _landing_target_pose {};
float _horizontal_approach_alt {};
// FIX THIS
src/modules/navigator/land.cpp
+1
View File
@@ -49,6 +49,7 @@ Land::Land(Navigator *navigator) :
void
Land::on_activation()
{
PX4_INFO("Land::on_activation");
/* set current mission item to Land */
set_land_item(&_mission_item);
_navigator->get_mission_result()->finished = false;
src/modules/navigator/mission.cpp
+2
View File
@@ -971,6 +971,7 @@ Mission::set_mission_items()
} else if (_mission_item.nav_cmd == NAV_CMD_LAND
&& _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) {
if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
PX4_INFO("new work item type is PRECISION_LAND");
new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND;
}
}
@@ -980,6 +981,7 @@ Mission::set_mission_items()
new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) {
if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
PX4_INFO("we just moved to the landing waypoint, now descend");
new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND;
}
src/modules/navigator/navigator_main.cpp
+1
View File
@@ -769,6 +769,7 @@ void Navigator::run()
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_land;
break;
src/modules/navigator/rtl.cpp
+2
View File
@@ -249,6 +249,8 @@ void RTL::find_RTL_destination()
void RTL::on_activation()
{
PX4_INFO("RTL::on_activation");
_rtl_state = RTL_STATE_NONE;
// if a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode