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

removed inheritance from FlightTaskAuto. Added PrecLandMode but have not implemented its use.

Browse Source
This commit is contained in:
Jacob Dahl
2023-03-15 15:42:07 -08:00
parent 2a765ee809
commit 93afe5b2e2
3 changed files with 63 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/FlightModeManager.cpp
+1
View File
@@ -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 &&
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp
+36 -30
View File
@@ -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()) {
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp
+26 -21
View File
@@ -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_s> _precision_landing_status_pub{ORB_ID(precision_landing_status)};
uORB::SubscriptionData<home_position_s> _sub_home_position {ORB_ID(home_position)};
uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status {ORB_ID(vehicle_status)};
uORB::SubscriptionData<landing_target_pose_s> _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_s> _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<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed, ///< velocity for controlled descend
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_acceleration_hor,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_xy_vel_cruise,
(ParamFloat<px4::params::PLD_BTOUT>) _param_pld_btout,
(ParamFloat<px4::params::PLD_HACC_RAD>) _param_pld_hacc_rad,
(ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_pld_fappr_alt,
(ParamFloat<px4::params::PLD_SRCH_ALT>) _param_pld_srch_alt,
(ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_pld_srch_tout,
(ParamInt<px4::params::PLD_MAX_SRCH>) _param_pld_max_srch,
(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md
)
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed, ///< velocity for controlled descend
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_acceleration_hor,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_xy_vel_cruise,
(ParamFloat<px4::params::PLD_BTOUT>) _param_pld_btout,
(ParamFloat<px4::params::PLD_HACC_RAD>) _param_pld_hacc_rad,
(ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_pld_fappr_alt,
(ParamFloat<px4::params::PLD_SRCH_ALT>) _param_pld_srch_alt,
(ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_pld_srch_tout,
(ParamInt<px4::params::PLD_MAX_SRCH>) _param_pld_max_srch,
(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md
)
};