From f52bad87e2a2e1ed12bf4715ddef93c2efa6d996 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 24 Oct 2020 10:30:12 +0200 Subject: [PATCH] mc_pos_control: separate out flight_tasks (into FlightModeManager) --- .../flight_mode_manager/FlightModeManager.cpp | 25 +- .../flight_mode_manager/FlightModeManager.hpp | 3 +- src/modules/mc_pos_control/CMakeLists.txt | 2 +- .../MulticopterPositionControl.cpp | 377 +----------------- .../MulticopterPositionControl.hpp | 31 +- .../mc_pos_control/Takeoff/CMakeLists.txt | 8 +- 6 files changed, 29 insertions(+), 417 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index e070067cba..6d343638ef 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -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) diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index c4370f6de5..26584ba93f 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -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_pub{ORB_ID(landing_gear)}; uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; - uORB::PublicationQueued _vehicle_command_pub{ORB_ID(vehicle_command)}; + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; uORB::Publication _vehicle_constraints_pub{ORB_ID(vehicle_constraints)}; DEFINE_PARAMETERS( diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index 552b65c23a..832178432f 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -50,5 +50,5 @@ px4_add_module( ecl_geo WeatherVane CollisionPrevention - Takeoff + Takeoff2 ) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 472f97fa73..468352d9c9 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -139,8 +139,6 @@ int MulticopterPositionControl::parameters_update(bool force) _hover_thrust_initialized = true; } - _flight_tasks.handleParameterUpdate(); - // initialize vectors from params and enforce constraints _param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get())); _param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get())); @@ -160,7 +158,6 @@ int MulticopterPositionControl::parameters_update(bool force) void MulticopterPositionControl::poll_subscriptions() { - _vehicle_status_sub.update(&_vehicle_status); _vehicle_land_detected_sub.update(&_vehicle_land_detected); _control_mode_sub.update(&_control_mode); _home_pos_sub.update(&_home_pos); @@ -248,20 +245,6 @@ void MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) } } -int MulticopterPositionControl::print_status() -{ - if (_flight_tasks.isAnyTaskActive()) { - PX4_INFO("Running, active flight task: %i", _flight_tasks.getActiveTask()); - - } else { - PX4_INFO("Running, no flight task active"); - } - - perf_print_counter(_cycle_perf); - - return 0; -} - void MulticopterPositionControl::Run() { if (should_exit()) { @@ -309,87 +292,16 @@ void MulticopterPositionControl::Run() _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now); - // switch to the required flighttask - start_flight_task(); + vehicle_local_position_setpoint_s setpoint; // check if any task is active - if (_flight_tasks.isAnyTaskActive()) { - // setpoints and constraints for the position controller from flighttask or failsafe - vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint; - vehicle_constraints_s constraints = FlightTask::empty_constraints; - - _flight_tasks.setYawHandler(_wv_controller); - - // update task - if (!_flight_tasks.update()) { - // FAILSAFE - // Task was not able to update correctly. Do Failsafe. - failsafe(time_stamp_now, setpoint, _states, false, !was_in_failsafe); - - } else { - setpoint = _flight_tasks.getPositionSetpoint(); - constraints = _flight_tasks.getConstraints(); - - _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now); - } - - // publish trajectory setpoint - _traj_sp_pub.publish(setpoint); + if (_trajectory_setpoint_sub.update(&setpoint)) { + vehicle_constraints_s constraints; + _vehicle_constraints_sub.update(&constraints); // check if all local states are valid and map accordingly set_vehicle_states(setpoint.vz); - // fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN - // TODO: this should get obsolete once the takeoff limiting moves into the flight tasks - if (!PX4_ISFINITE(constraints.speed_up) || (constraints.speed_up > _param_mpc_z_vel_max_up.get())) { - constraints.speed_up = _param_mpc_z_vel_max_up.get(); - } - - // limit tilt during takeoff ramupup - if (_takeoff.getTakeoffState() < TakeoffState::flight) { - constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); - setpoint.acceleration[2] = NAN; - } - - // limit altitude only if local position is valid - if (PX4_ISFINITE(_states.position(2))) { - limit_altitude(setpoint); - } - - // handle smooth takeoff - _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, - constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now); - constraints.speed_up = _takeoff.updateRamp(dt, constraints.speed_up); - - const bool not_taken_off = _takeoff.getTakeoffState() < TakeoffState::rampup; - const bool flying = _takeoff.getTakeoffState() >= TakeoffState::flight; - const bool flying_but_ground_contact = flying && _vehicle_land_detected.ground_contact; - - if (flying) { - _control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get()); - - } else { - // allow zero thrust when taking off and landing - _control.setThrustLimits(0.f, _param_mpc_thr_max.get()); - } - - if (not_taken_off || flying_but_ground_contact) { - // we are not flying yet and need to avoid any corrections - reset_setpoint_to_nan(setpoint); - Vector3f(0.f, 0.f, 100.f).copyTo(setpoint.acceleration); // High downwards acceleration to make sure there's no thrust - // set yaw-sp to current yaw - // TODO: we need a clean way to disable yaw control - setpoint.yaw = _states.yaw; - setpoint.yawspeed = 0.f; - // prevent any integrator windup - _control.resetIntegral(); - } - - if (not_taken_off) { - // reactivate the task which will reset the setpoint to current state - _flight_tasks.reActivate(); - } - // Run position control _control.setState(_states); _control.setConstraints(constraints); @@ -424,11 +336,6 @@ void MulticopterPositionControl::Run() // This message will be used by other modules (such as Landdetector) to determine vehicle intention. _local_pos_sp_pub.publish(local_pos_sp); - // Inform FlightTask about the input and output of the velocity controller - // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock) - _flight_tasks.updateVelocityControllerIO(Vector3f(local_pos_sp.vx, local_pos_sp.vy, local_pos_sp.vz), - Vector3f(local_pos_sp.acceleration)); - vehicle_attitude_setpoint_s attitude_setpoint{}; attitude_setpoint.timestamp = time_stamp_now; _control.getAttitudeSetpoint(attitude_setpoint); @@ -442,18 +349,6 @@ void MulticopterPositionControl::Run() _wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z(); - // if there's any change in landing gear setpoint publish it - landing_gear_s landing_gear = _flight_tasks.getGear(); - - if (landing_gear.landing_gear != _old_landing_gear_position - && landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) { - - landing_gear.timestamp = hrt_absolute_time(); - _landing_gear_pub.publish(landing_gear); - } - - _old_landing_gear_position = landing_gear.landing_gear; - } else { // reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation _vel_x_deriv.reset(); @@ -465,218 +360,6 @@ void MulticopterPositionControl::Run() perf_end(_cycle_perf); } -void MulticopterPositionControl::start_flight_task() -{ - bool task_failure = false; - bool should_disable_task = true; - int prev_failure_count = _task_failure_count; - - // Do not run any flight task for VTOLs in fixed-wing mode - if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - _flight_tasks.switchTask(FlightTaskIndex::None); - 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.nav_state - && _vehicle_status.nav_state != vehicle_status_s::NAVIGATION_STATE_ORBIT) { - _flight_tasks.switchTask(FlightTaskIndex::None); - } - - if (_vehicle_status.in_transition_mode) { - should_disable_task = false; - FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Transition); - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Transition activation failed with error: %s", _flight_tasks.errorToString(error)); - } - - task_failure = true; - _task_failure_count++; - - } else { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; - } - - return; - } - - // offboard - if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD - && (_control_mode.flag_control_altitude_enabled || - _control_mode.flag_control_position_enabled || - _control_mode.flag_control_climb_rate_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_acceleration_enabled)) { - - should_disable_task = false; - FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Offboard); - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Offboard activation failed with error: %s", _flight_tasks.errorToString(error)); - } - - task_failure = true; - _task_failure_count++; - - } else { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; - } - } - - // Auto-follow me - if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) { - should_disable_task = false; - FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe); - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Follow-Me activation failed with error: %s", _flight_tasks.errorToString(error)); - } - - task_failure = true; - _task_failure_count++; - - } else { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; - } - - } else if (_control_mode.flag_control_auto_enabled) { - // Auto related maneuvers - should_disable_task = false; - FlightTaskError error = FlightTaskError::NoError; - - error = _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel); - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Auto activation failed with error: %s", _flight_tasks.errorToString(error)); - } - - task_failure = true; - _task_failure_count++; - - } else { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; - } - - } else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) { - - // Emergency descend - should_disable_task = false; - FlightTaskError error = FlightTaskError::NoError; - - error = _flight_tasks.switchTask(FlightTaskIndex::Descend); - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Descend activation failed with error: %s", _flight_tasks.errorToString(error)); - } - - task_failure = true; - _task_failure_count++; - - } else { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; - } - - } - - // manual position control - if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure) { - should_disable_task = false; - FlightTaskError error = FlightTaskError::NoError; - - switch (_param_mpc_pos_mode.get()) { - 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::ManualAcceleration); - break; - } - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Position-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error)); - } - - task_failure = true; - _task_failure_count++; - - } else { - check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_POSCTL); - task_failure = false; - } - } - - // manual altitude control - if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) { - should_disable_task = false; - FlightTaskError error = FlightTaskError::NoError; - - switch (_param_mpc_pos_mode.get()) { - case 0: - error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude); - break; - - case 3: - default: - error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmoothVel); - break; - } - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Altitude-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error)); - } - - task_failure = true; - _task_failure_count++; - - } else { - check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_ALTCTL); - task_failure = false; - } - } - - if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) { - should_disable_task = false; - } - - // check task failure - if (task_failure) { - - // for some reason no flighttask was able to start. - // go into failsafe flighttask - FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Failsafe); - - if (error != FlightTaskError::NoError) { - // No task was activated. - _flight_tasks.switchTask(FlightTaskIndex::None); - } - - } else if (should_disable_task) { - _flight_tasks.switchTask(FlightTaskIndex::None); - } - - _last_vehicle_nav_state = _vehicle_status.nav_state; -} - void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force, bool warn) { @@ -740,58 +423,6 @@ void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_se setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN; } -void MulticopterPositionControl::check_failure(bool task_failure, uint8_t nav_state) -{ - if (!task_failure) { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; - - } else if (_task_failure_count > NUM_FAILURE_TRIES) { - // tell commander to switch mode - PX4_WARN("Previous flight task failed, switching to mode %d", nav_state); - send_vehicle_cmd_do(nav_state); - _task_failure_count = 0; // avoid immediate resending of a vehicle command in the next iteration - } -} - -void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state) -{ - vehicle_command_s command{}; - command.timestamp = hrt_absolute_time(); - command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - command.param1 = (float)1; // base mode - command.param3 = (float)0; // sub mode - command.target_system = 1; - command.target_component = 1; - command.source_system = 1; - command.source_component = 1; - command.confirmation = false; - command.from_external = false; - - // set the main mode - switch (nav_state) { - case vehicle_status_s::NAVIGATION_STATE_STAB: - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_STABILIZED; - break; - - case vehicle_status_s::NAVIGATION_STATE_ALTCTL: - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_ALTCTL; - break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; - command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - break; - - default: //vehicle_status_s::NAVIGATION_STATE_POSCTL - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_POSCTL; - break; - } - - // publish the vehicle command - _pub_vehicle_command.publish(command); -} - int MulticopterPositionControl::task_spawn(int argc, char *argv[]) { bool vtol = false; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 46f331eef7..aeee26d2ff 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -92,36 +92,30 @@ public: bool init(); - /** @see ModuleBase::print_status() */ - int print_status() override; - private: void Run() override; Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ uORB::Publication _vehicle_attitude_setpoint_pub; - uORB::Publication _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */ orb_advert_t _mavlink_log_pub{nullptr}; - uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ - uORB::Publication _traj_sp_pub{ORB_ID(trajectory_setpoint)}; /**< trajectory setpoints publication */ uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position */ uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)}; hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ int _task_failure_count{0}; /**< counter for task failures */ - vehicle_status_s _vehicle_status{}; /**< vehicle status */ /**< vehicle-land-detection: initialze to landed */ vehicle_land_detected_s _vehicle_land_detected = { .timestamp = 0, @@ -175,7 +169,6 @@ private: control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ - FlightTasks _flight_tasks; /**< class generating position controller setpoints depending on vehicle task */ PositionControl _control; /**< class for core PID position control */ PositionControlStates _states{}; /**< structure containing vehicle state information for position control */ @@ -187,8 +180,6 @@ private: /** Timeout in us for trajectory data to get considered invalid */ static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; - /** number of tries before switching to a failsafe flight task */ - static constexpr int NUM_FAILURE_TRIES = 10; /** If Flighttask fails, keep 0.2 seconds the current setpoint before going into failsafe land */ static constexpr uint64_t LOITER_TIME_BEFORE_DESCEND = 200_ms; /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off ant tilt is limited */ @@ -203,8 +194,6 @@ private: perf_counter_t _cycle_perf; - uint8_t _last_vehicle_nav_state{0}; - /** * Update our local parameter cache. * Parameter update can be forced when argument is true. @@ -236,12 +225,6 @@ private: */ void limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint); - /** - * Start flightasks based on navigation state. - * This methods activates a task based on the navigation state. - */ - void start_flight_task(); - /** * Failsafe. * If flighttask fails for whatever reason, then do failsafe. This could @@ -256,14 +239,4 @@ private: * Reset setpoints to NAN */ void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint); - - /** - * check if task should be switched because of failsafe - */ - void check_failure(bool task_failure, uint8_t nav_state); - - /** - * send vehicle command to inform commander about failsafe - */ - void send_vehicle_cmd_do(uint8_t nav_state); }; diff --git a/src/modules/mc_pos_control/Takeoff/CMakeLists.txt b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt index 67f02ddd16..b641ed936b 100644 --- a/src/modules/mc_pos_control/Takeoff/CMakeLists.txt +++ b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt @@ -31,10 +31,10 @@ # ############################################################################ -px4_add_library(Takeoff +px4_add_library(Takeoff2 Takeoff.cpp ) -target_include_directories(Takeoff PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(Takeoff PUBLIC hysteresis) +target_include_directories(Takeoff2 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(Takeoff2 PUBLIC hysteresis) -px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) +px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff2)