fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 19:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: separate out flight_tasks (into FlightModeManager)

Browse Source
This commit is contained in:
Matthias Grob
2020-12-30 10:25:08 -05:00
committed by Daniel Agar
parent 7545249215
commit f52bad87e2
6 changed files with 29 additions and 417 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/FlightModeManager.cpp
+16 -9
View File
@@ -120,6 +120,13 @@ void FlightModeManager::start_flight_task()
return;
}
// Switch to clean new task when mode switches e.g. to reset state when switching between auto modes
// exclude Orbit mode since the task is initiated in FlightTasks through the vehicle_command and we should not switch out
if (_last_vehicle_nav_state != _vehicle_status_sub.get().nav_state
&& _vehicle_status_sub.get().nav_state != vehicle_status_s::NAVIGATION_STATE_ORBIT) {
_flight_tasks.switchTask(FlightTaskIndex::None);
}
if (_vehicle_status_sub.get().in_transition_mode) {
should_disable_task = false;
@@ -233,16 +240,17 @@ void FlightModeManager::start_flight_task()
FlightTaskError error = FlightTaskError::NoError;
switch (_param_mpc_pos_mode.get()) {
case 1:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmooth);
case 0:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition);
break;
case 3:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmoothVel);
break;
case 4:
default:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition);
error = _flight_tasks.switchTask(FlightTaskIndex::ManualAcceleration);
break;
}
@@ -266,16 +274,13 @@ void FlightModeManager::start_flight_task()
FlightTaskError error = FlightTaskError::NoError;
switch (_param_mpc_pos_mode.get()) {
case 1:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmooth);
case 0:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude);
break;
case 3:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmoothVel);
break;
default:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude);
error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmoothVel);
break;
}
@@ -312,6 +317,8 @@ void FlightModeManager::start_flight_task()
} else if (should_disable_task) {
_flight_tasks.switchTask(FlightTaskIndex::None);
}
_last_vehicle_nav_state = _vehicle_status_sub.get().nav_state;
}
void FlightModeManager::check_failure(bool task_failure, uint8_t nav_state)
src/modules/flight_mode_manager/FlightModeManager.hpp
+2 -1
View File
@@ -84,6 +84,7 @@ private:
WeatherVane *_wv_controller{nullptr};
int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
int _task_failure_count{0};
uint8_t _last_vehicle_nav_state{0};
perf_counter_t _loop_perf; ///< loop duration performance counter
hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration
@@ -98,7 +99,7 @@ private:
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
uORB::PublicationQueued<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
DEFINE_PARAMETERS(