From 93afe5b2e28926cc78b34945ee357bb5dc4f5483 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 15 Mar 2023 15:42:07 -0800 Subject: [PATCH] removed inheritance from FlightTaskAuto. Added PrecLandMode but have not implemented its use. --- .../flight_mode_manager/FlightModeManager.cpp | 1 + .../FlightTaskAutoPrecisionLanding.cpp | 66 ++++++++++--------- .../FlightTaskAutoPrecisionLanding.hpp | 47 +++++++------ 3 files changed, 63 insertions(+), 51 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index ba0e8327a5..3f31673a71 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -146,6 +146,7 @@ void FlightModeManager::start_flight_task() _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) && _param_rtl_pld_md.get() > 0; + // Mission item precland const bool precland_mission_item_active = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION && diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp index 14fcc98a62..7204f09cf1 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp @@ -46,48 +46,62 @@ const char* STATE_STRINGS[] = {"Idle", "Start", "HorizontalApproach", "DescendAboveTarget", "Search", "NormalLand", "Finished"}; static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing"; +static constexpr int32_t RTL_PREC_LAND_OPPORTUNISTIC = 1; +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; + _search_count = 0; _last_slewrate_time = 0; _landing_target_valid = false; _state = PrecLandState::Idle; _fix_this_activate_update_loop = true; - _sub_vehicle_status.update(); + uint8_t nav_state = _sub_vehicle_status.get().nav_state; + // TODO: use mode information to determine behaviors? + // 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; + } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND && _param_rtl_pld_md.get() >= RTL_PREC_LAND_OPPORTUNISTIC) { + _mode = PrecLandMode::RegularLandOpportunistic; + } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && _param_rtl_pld_md.get() == RTL_PREC_LAND_OPPORTUNISTIC) { + _mode = PrecLandMode::ReturnToLaunchOpportunistic; + + } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && _param_rtl_pld_md.get() == RTL_PREC_LAND_REQUIRED) { + _mode = PrecLandMode::ReturnToLaunchRequired; + } return ret; } bool FlightTaskAutoPrecisionLanding::updateInitialize() { - // PX4_INFO("updateInitialize"); bool ret = FlightTask::updateInitialize(); _sub_home_position.update(); _sub_vehicle_status.update(); + _landing_target_pose_sub.update(); + uint8_t nav_state = _sub_vehicle_status.get().nav_state; - // Mission precision land - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { - _type = WaypointType::land; - - // Auto RTL - } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _type = WaypointType::loiter; - - // Auto Land - } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { - _type = WaypointType::land; + if (_nav_state != nav_state) { + PX4_INFO("nav_state: %u", nav_state); + _nav_state = nav_state; } // require valid position @@ -98,13 +112,12 @@ bool FlightTaskAutoPrecisionLanding::updateInitialize() bool FlightTaskAutoPrecisionLanding::update() { - bool ret = FlightTaskAuto::update(); + bool ret = FlightTask::update(); if (_landing_target_pose_sub.updated()) { _landing_target_pose_sub.copy(&_landing_target_pose); } - // target pose can become invalid when the message timed out _landing_target_valid = (hrt_elapsed_time(&_landing_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get(); switch (_state) { @@ -134,18 +147,13 @@ bool FlightTaskAutoPrecisionLanding::update() 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); - - // What is this? Wtf? - // _constraints.want_takeoff = _checkTakeoff(); + precision_landing_status_s status = {}; + status.timestamp = hrt_absolute_time(); + status.precland_state = (uint8_t) _state; + _precision_landing_status_pub.publish(status); return ret; } @@ -188,15 +196,13 @@ void FlightTaskAutoPrecisionLanding::run_state_idle() // PX4_INFO("run_state_idle: waypoint type: %d", (int)_type); - if (_type == WaypointType::land) { - switch_state(PrecLandState::Start); - } + switch_state(PrecLandState::Start); } void FlightTaskAutoPrecisionLanding::run_state_start() { // Initialize our position setpoint to the current position - _position_setpoint = _target = _position; + _position_setpoint = _position; if (_landing_target_valid) { @@ -207,8 +213,7 @@ void FlightTaskAutoPrecisionLanding::run_state_start() PX4_INFO("Target not seen"); // Check if opportunistic mode is enabled - static constexpr int32_t REQUIRED = 2; - if (_param_rtl_pld_md.get() == REQUIRED) { + if (_param_rtl_pld_md.get() == RTL_PREC_LAND_REQUIRED) { switch_state(PrecLandState::Search); } else { @@ -281,6 +286,7 @@ void FlightTaskAutoPrecisionLanding::run_state_descend_above_target() // 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; // Check if we're within our final approach altitude if ((_landing_target_pose.z_abs - _position(2)) < _param_pld_fappr_alt.get()) { diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp index 69e36a2f91..2e735271c8 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp @@ -63,11 +63,14 @@ enum class PrecLandState { }; enum class PrecLandMode { - Opportunistic = 1, // only do precision landing if landing target visible at the beginning - Required = 2 // try to find landing target if not visible at the beginning + RegularPrecisionLand, + RegularLandOpportunistic, + ReturnToLaunchOpportunistic, + ReturnToLaunchRequired, + MissionPrecisionLand }; -class FlightTaskAutoPrecisionLanding : public FlightTaskAuto +class FlightTaskAutoPrecisionLanding : public FlightTask { public: FlightTaskAutoPrecisionLanding() = default; @@ -97,26 +100,27 @@ private: void slewrate(float &sp_x, float &sp_y); bool hor_acc_radius_check(); - uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; - uORB::PublicationMulti _precision_landing_status_pub{ORB_ID(precision_landing_status)}; + uORB::SubscriptionData _sub_home_position {ORB_ID(home_position)}; + uORB::SubscriptionData _sub_vehicle_status {ORB_ID(vehicle_status)}; + uORB::SubscriptionData _landing_target_pose_sub {ORB_ID(landing_target_pose)}; - landing_target_pose_s _landing_target_pose {}; /**< precision landing target position */ + uORB::PublicationMulti _precision_landing_status_pub {ORB_ID(precision_landing_status)}; + + landing_target_pose_s _landing_target_pose {}; float _horizontal_approach_alt {}; // FIX THIS // INFO [FlightTaskAutoPrecisionLanding] Switching to Start // INFO [FlightTaskAutoPrecisionLanding] FlightTaskAutoPrecisionLanding::activate - // INFO [FlightTaskAutoPrecisionLanding] run_state_idle: waypoint type: 4 // INFO [FlightTaskAutoPrecisionLanding] Switching to Start // INFO [FlightTaskAutoPrecisionLanding] FlightTaskAutoPrecisionLanding::activate - // INFO [FlightTaskAutoPrecisionLanding] run_state_idle: waypoint type: 4 // INFO [FlightTaskAutoPrecisionLanding] Switching to Start // INFO [FlightTaskAutoPrecisionLanding] FlightTaskAutoPrecisionLanding::activate - // INFO [FlightTaskAutoPrecisionLanding] run_state_idle: waypoint type: 4 // INFO [FlightTaskAutoPrecisionLanding] Switching to Start bool _fix_this_activate_update_loop {true}; + uint8_t _nav_state {0}; bool _landing_target_valid {false}; /**< whether we have received a landing target position message */ @@ -128,19 +132,20 @@ private: matrix::Vector2f _sp_pev; matrix::Vector2f _sp_pev_prev; - PrecLandState _state{PrecLandState::Idle}; + PrecLandMode _mode {PrecLandMode::RegularPrecisionLand}; + PrecLandState _state {PrecLandState::Idle}; DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, - (ParamFloat) _param_mpc_land_speed, ///< velocity for controlled descend - (ParamFloat) _param_acceleration_hor, - (ParamFloat) _param_xy_vel_cruise, - (ParamFloat) _param_pld_btout, - (ParamFloat) _param_pld_hacc_rad, - (ParamFloat) _param_pld_fappr_alt, - (ParamFloat) _param_pld_srch_alt, - (ParamFloat) _param_pld_srch_tout, - (ParamInt) _param_pld_max_srch, - (ParamInt) _param_rtl_pld_md - ) + (ParamFloat) _param_mpc_land_speed, ///< velocity for controlled descend + (ParamFloat) _param_acceleration_hor, + (ParamFloat) _param_xy_vel_cruise, + (ParamFloat) _param_pld_btout, + (ParamFloat) _param_pld_hacc_rad, + (ParamFloat) _param_pld_fappr_alt, + (ParamFloat) _param_pld_srch_alt, + (ParamFloat) _param_pld_srch_tout, + (ParamInt) _param_pld_max_srch, + (ParamInt) _param_rtl_pld_md + ) };