diff --git a/msg/takeoff_status.msg b/msg/takeoff_status.msg index 346de764eb..9b55b08f4f 100644 --- a/msg/takeoff_status.msg +++ b/msg/takeoff_status.msg @@ -2,10 +2,11 @@ uint64 timestamp # time since system start (microseconds) -uint8 TAKEOFF_STATE_DISARMED = 0 -uint8 TAKEOFF_STATE_SPOOLUP = 1 -uint8 TAKEOFF_STATE_READY_FOR_TAKEOFF = 2 -uint8 TAKEOFF_STATE_RAMPUP = 3 -uint8 TAKEOFF_STATE_FLIGHT = 4 +uint8 TAKEOFF_STATE_UNINITIALIZED = 0 +uint8 TAKEOFF_STATE_DISARMED = 1 +uint8 TAKEOFF_STATE_SPOOLUP = 2 +uint8 TAKEOFF_STATE_READY_FOR_TAKEOFF = 3 +uint8 TAKEOFF_STATE_RAMPUP = 4 +uint8 TAKEOFF_STATE_FLIGHT = 5 uint8 takeoff_state diff --git a/msg/vehicle_constraints.msg b/msg/vehicle_constraints.msg index fabfce997e..0e624c0370 100644 --- a/msg/vehicle_constraints.msg +++ b/msg/vehicle_constraints.msg @@ -3,15 +3,8 @@ uint64 timestamp # time since system start (microseconds) -float32 yawspeed # in radians/sec float32 speed_xy # in meters/sec float32 speed_up # in meters/sec float32 speed_down # in meters/sec -float32 tilt # in radians [0, PI] -float32 min_distance_to_ground # in meters -float32 max_distance_to_ground # in meters -bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight) -bool reset_integral # tells controller to reset integrators e.g. since we know the vehicle is not in air -float32 minimum_thrust # tell controller what the minimum collective output thrust should be -uint32 flight_task +bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight) diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 991f564be1..3cedcd92f3 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -108,8 +108,6 @@ add_compile_options( # add subdirectory containing all tasks add_subdirectory(tasks) -add_subdirectory(Takeoff) - px4_add_module( MODULE modules__flight_mode_manager MAIN flight_mode_manager @@ -124,7 +122,6 @@ px4_add_module( ${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.hpp ${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.cpp DEPENDS - Takeoff px4_work_queue WeatherVane ) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 01f0669f4e..4b872ecd60 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -115,10 +115,6 @@ void FlightModeManager::Run() } } - // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes - _takeoff.updateTakeoffState(_vehicle_control_mode_sub.get().flag_armed, _vehicle_land_detected_sub.get().landed, false, - 10.f, !_vehicle_control_mode_sub.get().flag_control_climb_rate_enabled, time_stamp_now); - // activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy // that turns the nose of the vehicle into the wind if (_wv_controller != nullptr) { @@ -162,10 +158,6 @@ void FlightModeManager::updateParams() _current_task.task->handleParameterUpdate(); } - _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); - _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); - _takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get()); - if (_wv_controller != nullptr) { _wv_controller->update_parameters(); } @@ -517,21 +509,15 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt, // limit altitude according to land detector limitAltitude(setpoint, vehicle_local_position); - 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_sub.get().ground_contact; + if (_takeoff_status_sub.updated()) { + takeoff_status_s takeoff_status; - 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 - setpoint.yawspeed = 0.f; - // prevent any integrator windup - constraints.reset_integral = true; + if (_takeoff_status_sub.copy(&takeoff_status)) { + _takeoff_state = takeoff_status.takeoff_state; + } } - if (not_taken_off) { + if (_takeoff_state < takeoff_status_s::TAKEOFF_STATE_RAMPUP) { // reactivate the task which will reset the setpoint to current state _current_task.task->reActivate(); } @@ -540,38 +526,9 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt, setpoint.timestamp = hrt_absolute_time(); _trajectory_setpoint_pub.publish(setpoint); - - // Allow ramping from zero thrust on takeoff - if (flying) { - constraints.minimum_thrust = _param_mpc_thr_min.get(); - - } else { - // allow zero thrust when taking off and landing - constraints.minimum_thrust = 0.f; - } - - // 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()); - } - - // handle smooth takeoff - _takeoff.updateTakeoffState(_vehicle_control_mode_sub.get().flag_armed, _vehicle_land_detected_sub.get().landed, - constraints.want_takeoff, constraints.speed_up, !_vehicle_control_mode_sub.get().flag_control_climb_rate_enabled, - _time_stamp_last_loop); - constraints.speed_up = _takeoff.updateRamp(dt, constraints.speed_up); - - constraints.flight_task = static_cast(_current_task.index); constraints.timestamp = hrt_absolute_time(); _vehicle_constraints_pub.publish(constraints); - // if there's any change in landing gear setpoint publish it landing_gear_s landing_gear = _current_task.task->getGear(); @@ -583,16 +540,6 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt, } _old_landing_gear_position = landing_gear.landing_gear; - - // Publish takeoff status - takeoff_status_s takeoff_status; - takeoff_status.takeoff_state = static_cast(_takeoff.getTakeoffState()); - - if (takeoff_status.takeoff_state != _old_takeoff_state) { - takeoff_status.timestamp = hrt_absolute_time(); - _takeoff_status_pub.publish(takeoff_status); - _old_takeoff_state = takeoff_status.takeoff_state; - } } void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint, @@ -614,15 +561,6 @@ void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoin } } -void FlightModeManager::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint) -{ - setpoint.x = setpoint.y = setpoint.z = NAN; - setpoint.vx = setpoint.vy = setpoint.vz = NAN; - setpoint.yaw = setpoint.yawspeed = NAN; - setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN; - setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN; -} - FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) { // switch to the running task, nothing to do diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 8c1278dcbe..b77fafdd20 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -56,8 +56,6 @@ #include #include -#include "Takeoff/Takeoff.hpp" - #include enum class FlightTaskError : int { @@ -95,7 +93,6 @@ private: void handleCommand(); void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position); void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position); - void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint); /** * Switch to a specific task (for normal usage) @@ -133,10 +130,9 @@ private: FlightTaskIndex index{FlightTaskIndex::None}; } _current_task{}; - Takeoff _takeoff; ///< state machine and ramp to bring the vehicle off the ground without a jump WeatherVane *_wv_controller{nullptr}; int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP}; - uint8_t _old_takeoff_state{0}; + uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED}; int _task_failure_count{0}; uint8_t _last_vehicle_nav_state{0}; @@ -145,6 +141,7 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; @@ -156,19 +153,12 @@ private: uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; - uORB::Publication _takeoff_status_pub{ORB_ID(takeoff_status)}; uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::Publication _vehicle_constraints_pub{ORB_ID(vehicle_constraints)}; DEFINE_PARAMETERS( - (ParamInt) _param_mpc_pos_mode, - (ParamFloat) _param_mpc_tiltmax_lnd, - (ParamFloat) _param_mpc_z_vel_max_up, - (ParamFloat) _param_mpc_spoolup_time, - (ParamFloat) _param_mpc_tko_ramp_t, - (ParamFloat) _param_mpc_z_vel_p_acc, - (ParamFloat) _param_mpc_thr_min + (ParamInt) _param_mpc_pos_mode ); }; diff --git a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp index 50f34db88a..678d45f6b1 100644 --- a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -65,7 +65,6 @@ protected: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, (ParamFloat) _param_mpc_land_speed, - (ParamFloat) _param_mpc_tiltmax_lnd, (ParamInt) _param_mpc_land_rc_help, (ParamFloat) _param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index 5593adf4d5..577b24f98d 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -6,7 +6,7 @@ constexpr uint64_t FlightTask::_timeout; // First index of empty_setpoint corresponds to time-stamp and requires a finite number. const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}}; -const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}}; +const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint) @@ -172,9 +172,6 @@ void FlightTask::_setDefaultConstraints() _constraints.speed_xy = _param_mpc_xy_vel_max.get(); _constraints.speed_up = _param_mpc_z_vel_max_up.get(); _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); - _constraints.tilt = NAN; - _constraints.min_distance_to_ground = NAN; - _constraints.max_distance_to_ground = NAN; _constraints.want_takeoff = false; } diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 827ea71dc6..8c823196e1 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -227,6 +227,9 @@ protected: float _dist_to_bottom{}; /**< current height above ground level */ float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */ + float _min_distance_to_ground{-INFINITY}; /**< min distance to ground constraint */ + float _max_distance_to_ground{INFINITY}; /**< max distance to ground constraint */ + /** * Setpoints which the position controller has to execute. * Setpoints that are set to NAN are not controlled. Not all setpoints can be set at the same time. diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 215fe0cf5d..c13035f363 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -81,17 +81,17 @@ bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s void FlightTaskManualAltitude::_updateConstraintsFromEstimator() { if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) { - _constraints.min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min; + _min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min; } else { - _constraints.min_distance_to_ground = -INFINITY; + _min_distance_to_ground = -INFINITY; } if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) { - _constraints.max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max; + _max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max; } else { - _constraints.max_distance_to_ground = INFINITY; + _max_distance_to_ground = INFINITY; } } @@ -184,7 +184,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() // Ensure that minimum altitude is respected if // there is a distance sensor and distance to bottom is below minimum. - if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _constraints.min_distance_to_ground) { + if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _min_distance_to_ground) { _terrainFollowing(apply_brake, stopped); } else { @@ -210,14 +210,10 @@ void FlightTaskManualAltitude::_updateAltitudeLock() void FlightTaskManualAltitude::_respectMinAltitude() { - const bool respectAlt = PX4_ISFINITE(_dist_to_bottom) - && _dist_to_bottom < _constraints.min_distance_to_ground; - // Height above ground needs to be limited (flow / range-finder) - if (respectAlt) { + if (PX4_ISFINITE(_dist_to_bottom) && (_dist_to_bottom < _min_distance_to_ground)) { // increase altitude to minimum flow distance - _position_setpoint(2) = _position(2) - - (_constraints.min_distance_to_ground - _dist_to_bottom); + _position_setpoint(2) = _position(2) - (_min_distance_to_ground - _dist_to_bottom); } } @@ -253,8 +249,8 @@ void FlightTaskManualAltitude::_respectMaxAltitude() // if there is a valid maximum distance to ground, linearly increase speed limit with distance // below the maximum, preserving control loop vertical position error gain. - if (PX4_ISFINITE(_constraints.max_distance_to_ground)) { - _constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_constraints.max_distance_to_ground - _dist_to_bottom), + if (PX4_ISFINITE(_max_distance_to_ground)) { + _constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom), -_max_speed_down, _max_speed_up); } else { @@ -262,17 +258,16 @@ void FlightTaskManualAltitude::_respectMaxAltitude() } // if distance to bottom exceeded maximum distance, slowly approach maximum distance - if (_dist_to_bottom > _constraints.max_distance_to_ground) { + if (_dist_to_bottom > _max_distance_to_ground) { // difference between current distance to ground and maximum distance to ground - const float delta_distance_to_max = _dist_to_bottom - _constraints.max_distance_to_ground; + const float delta_distance_to_max = _dist_to_bottom - _max_distance_to_ground; // set position setpoint to maximum distance to ground - _position_setpoint(2) = _position(2) + delta_distance_to_max; + _position_setpoint(2) = _position(2) + delta_distance_to_max; // limit speed downwards to 0.7m/s _constraints.speed_down = math::min(_max_speed_down, 0.7f); } else { _constraints.speed_down = _max_speed_down; - } } } diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index fb321a26c4..ab9999b185 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ add_subdirectory(PositionControl) +add_subdirectory(Takeoff) px4_add_module( MODULE modules__mc_pos_control @@ -42,6 +43,7 @@ px4_add_module( MulticopterPositionControl.hpp DEPENDS PositionControl + Takeoff controllib git_ecl ecl_geo diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 88f4a82eb0..eb50190c92 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -36,24 +36,26 @@ #include #include #include +#include "PositionControl/ControlMath.hpp" using namespace matrix; MulticopterPositionControl::MulticopterPositionControl(bool vtol) : SuperBlock(nullptr, "MPC"), ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _vel_x_deriv(this, "VELD"), _vel_y_deriv(this, "VELD"), - _vel_z_deriv(this, "VELD"), - _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")) + _vel_z_deriv(this, "VELD") { // fetch initial parameter values parameters_update(true); // set failsafe hysteresis _failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND); + + reset_setpoint_to_nan(_setpoint); } MulticopterPositionControl::~MulticopterPositionControl() @@ -68,10 +70,8 @@ bool MulticopterPositionControl::init() return false; } - // limit to every other vehicle_local_position update (50 Hz) - _local_pos_sub.set_interval_us(20_ms); - _time_stamp_last_loop = hrt_absolute_time(); + ScheduleNow(); return true; } @@ -156,9 +156,6 @@ int MulticopterPositionControl::parameters_update(bool force) Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()), Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()), Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get())); - _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); - _control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get()); - _control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians! // Check that the design parameters are inside the absolute maximum constraints if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) { @@ -189,79 +186,68 @@ int MulticopterPositionControl::parameters_update(bool force) // 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())); + + _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); + _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); + _takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get()); } return OK; } -void MulticopterPositionControl::poll_subscriptions() +PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehicle_local_position_s &local_pos) { - _control_mode_sub.update(&_control_mode); + PositionControlStates states; - if (_param_mpc_use_hte.get()) { - hover_thrust_estimate_s hte; - - if (_hover_thrust_estimate_sub.update(&hte)) { - if (hte.valid) { - _control.updateHoverThrust(hte.hover_thrust); - } - } - } -} - -void MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) -{ // only set position states if valid and finite - if (PX4_ISFINITE(_local_pos.x) && PX4_ISFINITE(_local_pos.y) && _local_pos.xy_valid) { - _states.position(0) = _local_pos.x; - _states.position(1) = _local_pos.y; + if (PX4_ISFINITE(local_pos.x) && PX4_ISFINITE(local_pos.y) && local_pos.xy_valid) { + states.position(0) = local_pos.x; + states.position(1) = local_pos.y; } else { - _states.position(0) = _states.position(1) = NAN; + states.position(0) = NAN; + states.position(1) = NAN; } - if (PX4_ISFINITE(_local_pos.z) && _local_pos.z_valid) { - _states.position(2) = _local_pos.z; + if (PX4_ISFINITE(local_pos.z) && local_pos.z_valid) { + states.position(2) = local_pos.z; } else { - _states.position(2) = NAN; + states.position(2) = NAN; } - if (PX4_ISFINITE(_local_pos.vx) && PX4_ISFINITE(_local_pos.vy) && _local_pos.v_xy_valid) { - _states.velocity(0) = _local_pos.vx; - _states.velocity(1) = _local_pos.vy; - _states.acceleration(0) = _vel_x_deriv.update(_states.velocity(0)); - _states.acceleration(1) = _vel_y_deriv.update(_states.velocity(1)); + if (PX4_ISFINITE(local_pos.vx) && PX4_ISFINITE(local_pos.vy) && local_pos.v_xy_valid) { + states.velocity(0) = local_pos.vx; + states.velocity(1) = local_pos.vy; + states.acceleration(0) = _vel_x_deriv.update(local_pos.vx); + states.acceleration(1) = _vel_y_deriv.update(local_pos.vy); } else { - _states.velocity(0) = _states.velocity(1) = NAN; - _states.acceleration(0) = _states.acceleration(1) = NAN; + states.velocity(0) = NAN; + states.velocity(1) = NAN; + states.acceleration(0) = NAN; + states.acceleration(1) = NAN; + // reset derivatives to prevent acceleration spikes when regaining velocity _vel_x_deriv.reset(); _vel_y_deriv.reset(); } - if (PX4_ISFINITE(_local_pos.vz) && _local_pos.v_z_valid) { - _states.velocity(2) = _local_pos.vz; - - if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) { - // A change in velocity is demanded. Set velocity to the derivative of position - // because it has less bias but blend it in across the landing speed range - float weighting = fminf(fabsf(vel_sp_z) / _param_mpc_land_speed.get(), 1.0f); - _states.velocity(2) = _local_pos.z_deriv * weighting + _local_pos.vz * (1.0f - weighting); - } - - _states.acceleration(2) = _vel_z_deriv.update(_states.velocity(2)); + if (PX4_ISFINITE(local_pos.vz) && local_pos.v_z_valid) { + states.velocity(2) = local_pos.vz; + states.acceleration(2) = _vel_z_deriv.update(states.velocity(2)); } else { - _states.velocity(2) = _states.acceleration(2) = NAN; + states.velocity(2) = NAN; + states.acceleration(2) = NAN; + // reset derivative to prevent acceleration spikes when regaining velocity _vel_z_deriv.reset(); } - if (PX4_ISFINITE(_local_pos.heading)) { - _states.yaw = _local_pos.heading; - } + states.yaw = local_pos.heading; + + return states; } void MulticopterPositionControl::Run() @@ -272,14 +258,16 @@ void MulticopterPositionControl::Run() return; } + // reschedule backup + ScheduleDelayed(100_ms); + + parameters_update(false); + perf_begin(_cycle_perf); + vehicle_local_position_s local_pos; - if (_local_pos_sub.update(&_local_pos)) { - - poll_subscriptions(); - parameters_update(false); - - const hrt_abstime time_stamp_now = _local_pos.timestamp; + if (_local_pos_sub.update(&local_pos)) { + const hrt_abstime time_stamp_now = local_pos.timestamp; const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); _time_stamp_last_loop = time_stamp_now; @@ -289,26 +277,130 @@ void MulticopterPositionControl::Run() const bool was_in_failsafe = _in_failsafe; _in_failsafe = false; - vehicle_local_position_setpoint_s setpoint; + _control_mode_sub.update(&_control_mode); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); - // check if any task is active - if (_trajectory_setpoint_sub.update(&setpoint)) { - _control.setInputSetpoint(setpoint); + if (_param_mpc_use_hte.get()) { + hover_thrust_estimate_s hte; - // check if all local states are valid and map accordingly - set_vehicle_states(setpoint.vz); - _control.setState(_states); - - vehicle_constraints_s constraints; - - if (_vehicle_constraints_sub.update(&constraints)) { - _control.setConstraints(constraints); - _control.setThrustLimits(constraints.minimum_thrust, _param_mpc_thr_max.get()); - - if (constraints.reset_integral) { - _control.resetIntegral(); + if (_hover_thrust_estimate_sub.update(&hte)) { + if (hte.valid) { + _control.updateHoverThrust(hte.hover_thrust); } } + } + + PositionControlStates states{set_vehicle_states(local_pos)}; + + if (_control_mode.flag_control_climb_rate_enabled) { + + _trajectory_setpoint_sub.update(&_setpoint); + + // adjust existing (or older) setpoint with any EKF reset deltas + if (_setpoint.timestamp < local_pos.timestamp) { + if (local_pos.vxy_reset_counter != _vxy_reset_counter) { + if (PX4_ISFINITE(_setpoint.vx)) { + _setpoint.vx += local_pos.delta_vxy[0]; + } + + if (PX4_ISFINITE(_setpoint.vy)) { + _setpoint.vy += local_pos.delta_vxy[1]; + } + } + + if (local_pos.vz_reset_counter != _vz_reset_counter) { + if (PX4_ISFINITE(_setpoint.vz)) { + _setpoint.vz += local_pos.delta_vz; + } + } + + if (local_pos.xy_reset_counter != _xy_reset_counter) { + if (PX4_ISFINITE(_setpoint.x)) { + _setpoint.x += local_pos.delta_xy[0]; + } + + if (PX4_ISFINITE(_setpoint.y)) { + _setpoint.y += local_pos.delta_xy[1]; + } + } + + if (local_pos.z_reset_counter != _z_reset_counter) { + if (PX4_ISFINITE(_setpoint.z)) { + _setpoint.z += local_pos.delta_z; + } + } + + if (local_pos.heading_reset_counter != _heading_reset_counter) { + if (PX4_ISFINITE(_setpoint.yaw)) { + _setpoint.yaw += local_pos.delta_heading; + } + } + } + + // update vehicle constraints and handle smooth takeoff + _vehicle_constraints_sub.update(&_vehicle_constraints); + + // 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(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())) { + _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get(); + } + + // handle smooth takeoff + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, _vehicle_constraints.want_takeoff, + _vehicle_constraints.speed_up, false, time_stamp_now); + + 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 (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 + + // prevent any integrator windup + _control.resetIntegral(); + } + + // limit tilt during takeoff ramupup + if (_takeoff.getTakeoffState() < TakeoffState::flight) { + _control.setTiltLimit(math::radians(_param_mpc_tiltmax_lnd.get())); + + } else { + _control.setTiltLimit(math::radians(_param_mpc_tiltmax_air.get())); + } + + const float speed_up = _takeoff.updateRamp(dt, _vehicle_constraints.speed_up); + const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down : + _param_mpc_z_vel_max_dn.get(); + const float speed_horizontal = PX4_ISFINITE(_vehicle_constraints.speed_xy) ? _vehicle_constraints.speed_xy : + _param_mpc_xy_vel_max.get(); + + // Allow ramping from zero thrust on takeoff + const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f; + + _control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get()); + + _control.setVelocityLimits( + math::constrain(speed_horizontal, 0.f, _param_mpc_xy_vel_max.get()), + math::constrain(speed_up, 0.f, _param_mpc_z_vel_max_up.get()), + math::constrain(speed_down, 0.f, _param_mpc_z_vel_max_dn.get())); + + _control.setInputSetpoint(_setpoint); + + // update states + if (PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON) + && PX4_ISFINITE(local_pos.z_deriv) && local_pos.z_valid && local_pos.v_z_valid) { + // A change in velocity is demanded. Set velocity to the derivative of position + // because it has less bias but blend it in across the landing speed range + // < MPC_LAND_SPEED: ramp up using altitude derivative without a step + // >= MPC_LAND_SPEED: use altitude derivative + float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f); + states.velocity(2) = local_pos.z_deriv * weighting + local_pos.vz * (1.f - weighting); + } + + _control.setState(states); // Run position control if (_control.update(dt)) { @@ -321,13 +413,13 @@ void MulticopterPositionControl::Run() _last_warn = time_stamp_now; } - failsafe(time_stamp_now, setpoint, _states, !was_in_failsafe); + failsafe(time_stamp_now, _setpoint, states, !was_in_failsafe); - _control.setInputSetpoint(setpoint); - - constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}}; - _control.setConstraints(constraints); + // reset constraints + _vehicle_constraints = {0, NAN, NAN, NAN, false, {}}; + _control.setInputSetpoint(_setpoint); + _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); _control.update(dt); } @@ -335,26 +427,39 @@ void MulticopterPositionControl::Run() // on top of the input/feed-forward setpoints these containt the PID corrections // This message is used by other modules (such as Landdetector) to determine vehicle intention. vehicle_local_position_setpoint_s local_pos_sp{}; - local_pos_sp.timestamp = time_stamp_now; _control.getLocalPositionSetpoint(local_pos_sp); + local_pos_sp.timestamp = hrt_absolute_time(); _local_pos_sp_pub.publish(local_pos_sp); // Publish attitude setpoint output - // It's important to publish also when disarmed otheriwse the attitude setpoint stays uninitialized. - // Not publishing when not running a flight task - // in stabilized mode attitude setpoints get ignored - // in offboard with attitude setpoints they come from MAVLink directly vehicle_attitude_setpoint_s attitude_setpoint{}; - attitude_setpoint.timestamp = time_stamp_now; _control.getAttitudeSetpoint(attitude_setpoint); + attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); } else { - // reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation - _vel_x_deriv.reset(); - _vel_y_deriv.reset(); - _vel_z_deriv.reset(); + // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true, time_stamp_now); } + + // Publish takeoff status + const uint8_t takeoff_state = static_cast(_takeoff.getTakeoffState()); + + if (takeoff_state != _old_takeoff_state) { + takeoff_status_s takeoff_status{}; + takeoff_status.takeoff_state = takeoff_state; + takeoff_status.timestamp = hrt_absolute_time(); + _takeoff_status_pub.publish(takeoff_status); + + _old_takeoff_state = takeoff_state; + } + + // save latest reset counters + _vxy_reset_counter = local_pos.vxy_reset_counter; + _vz_reset_counter = local_pos.vz_reset_counter; + _xy_reset_counter = local_pos.xy_reset_counter; + _z_reset_counter = local_pos.z_reset_counter; + _heading_reset_counter = local_pos.heading_reset_counter; } perf_end(_cycle_perf); @@ -374,7 +479,7 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_ if (_failsafe_land_hysteresis.get_state()) { reset_setpoint_to_nan(setpoint); - if (PX4_ISFINITE(_states.velocity(0)) && PX4_ISFINITE(_states.velocity(1))) { + if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) { // don't move along xy setpoint.vx = setpoint.vy = 0.f; @@ -392,7 +497,7 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_ } } - if (PX4_ISFINITE(_states.velocity(2))) { + if (PX4_ISFINITE(states.velocity(2))) { // don't move along z if we can stop in all dimensions if (!PX4_ISFINITE(setpoint.vz)) { setpoint.vz = 0.f; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 2f5313b7d1..2141686446 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -37,7 +37,9 @@ #pragma once -#include +#include "PositionControl/PositionControl.hpp" +#include "Takeoff/Takeoff.hpp" + #include #include #include @@ -50,23 +52,22 @@ #include #include #include - +#include #include #include -#include #include #include #include +#include #include +#include #include #include -#include "PositionControl/PositionControl.hpp" - using namespace time_literals; class MulticopterPositionControl : public ModuleBase, public control::SuperBlock, - public ModuleParams, public px4::WorkItem + public ModuleParams, public px4::ScheduledWorkItem { public: MulticopterPositionControl(bool vtol = false); @@ -86,26 +87,46 @@ public: private: void Run() override; - uORB::Publication _vehicle_attitude_setpoint_pub; + Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ + orb_advert_t _mavlink_log_pub{nullptr}; - uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + uORB::Publication _takeoff_status_pub{ORB_ID(takeoff_status)}; + uORB::Publication _vehicle_attitude_setpoint_pub; + uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; 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_control_mode_s _control_mode{}; /**< vehicle control mode */ - vehicle_local_position_s _local_pos{}; /**< vehicle local position */ + vehicle_control_mode_s _control_mode{}; + vehicle_local_position_setpoint_s _setpoint{}; + vehicle_constraints_s _vehicle_constraints{ + .timestamp = 0, + .speed_xy = NAN, + .speed_up = NAN, + .speed_down = NAN, + .want_takeoff = false, + }; + + vehicle_land_detected_s _vehicle_land_detected { + .timestamp = 0, + .alt_max = -1.0f, + .freefall = false, + .ground_contact = true, + .maybe_landed = true, + .landed = true, + }; DEFINE_PARAMETERS( // Position Control @@ -125,6 +146,8 @@ private: (ParamBool) _param_mpc_use_hte, // Takeoff / Land + (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ + (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ (ParamFloat) _param_mpc_tko_speed, (ParamFloat) _param_mpc_land_speed, @@ -157,11 +180,10 @@ private: control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ PositionControl _control; /**< class for core PID position control */ - PositionControlStates _states{}; /**< structure containing vehicle state information for position control */ - hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */ + hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */ - bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */ + bool _in_failsafe{false}; /**< true if failsafe was entered within current cycle */ bool _hover_thrust_initialized{false}; @@ -176,7 +198,15 @@ private: systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ - perf_counter_t _cycle_perf; + uint8_t _old_takeoff_state{}; + + uint8_t _vxy_reset_counter{0}; + uint8_t _vz_reset_counter{0}; + uint8_t _xy_reset_counter{0}; + uint8_t _z_reset_counter{0}; + uint8_t _heading_reset_counter{0}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; /** * Update our local parameter cache. @@ -185,16 +215,10 @@ private: */ int parameters_update(bool force); - /** - * Check for changes in subscribed topics. - */ - void poll_subscriptions(); - /** * Check for validity of positon/velocity states. - * @param vel_sp_z velocity setpoint in z-direction */ - void set_vehicle_states(const float &vel_sp_z); + PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos); /** * Adjust the setpoint during landing. diff --git a/src/modules/mc_pos_control/PositionControl/CMakeLists.txt b/src/modules/mc_pos_control/PositionControl/CMakeLists.txt index 2ccb43d6ff..4ebbe5d801 100644 --- a/src/modules/mc_pos_control/PositionControl/CMakeLists.txt +++ b/src/modules/mc_pos_control/PositionControl/CMakeLists.txt @@ -32,13 +32,12 @@ ############################################################################ px4_add_library(PositionControl - PositionControl.cpp ControlMath.cpp + ControlMath.hpp + PositionControl.cpp + PositionControl.hpp ) -target_include_directories(PositionControl - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} -) +target_include_directories(PositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ) px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl) px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 8cdafc2847..caca1c85a4 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -88,27 +88,6 @@ void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s & _yawspeed_sp = setpoint.yawspeed; } -void PositionControl::setConstraints(const vehicle_constraints_s &constraints) -{ - _constraints = constraints; - - // For safety check if adjustable constraints are below global constraints. If they are not stricter than global - // constraints, then just use global constraints for the limits. - if (!PX4_ISFINITE(constraints.tilt) || (constraints.tilt > _lim_tilt)) { - _constraints.tilt = _lim_tilt; - } - - if (!PX4_ISFINITE(constraints.speed_up) || (constraints.speed_up > _lim_vel_up)) { - _constraints.speed_up = _lim_vel_up; - } - - if (!PX4_ISFINITE(constraints.speed_down) || (constraints.speed_down > _lim_vel_down)) { - _constraints.speed_down = _lim_vel_down; - } - - // ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion -} - bool PositionControl::update(const float dt) { // x and y input setpoints always have to come in pairs @@ -138,7 +117,7 @@ void PositionControl::_positionControl() // the desired position setpoint over the feed-forward term. _vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal); // Constrain velocity in z-direction. - _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down); + _vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down); } void PositionControl::_velocityControl(const float dt) @@ -198,7 +177,7 @@ void PositionControl::_accelerationControl() { // Assume standard acceleration due to gravity in vertical direction for attitude generation Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized(); - ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt); + ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt); // Scale thrust assuming hover thrust produces standard gravity float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; // Project thrust to planned body attitude diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 4986c93a86..08720d49f3 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -41,7 +41,6 @@ #include #include -#include #include struct PositionControlStates { @@ -139,13 +138,6 @@ public: */ void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint); - /** - * Pass constraints that are stricter than the global limits - * Note: NAN value means no constraint, take maximum limit of controller. - * @param constraints a PositionControl structure with supported constraints - */ - void setConstraints(const vehicle_constraints_s &constraints); - /** * Apply P-position and PID-velocity controller that updates the member * thrust, yaw- and yawspeed-setpoints. @@ -209,8 +201,6 @@ private: matrix::Vector3f _vel_int; /**< integral term of the velocity controller */ float _yaw{}; /**< current heading */ - vehicle_constraints_s _constraints{}; /**< variable constraints */ - // Setpoints matrix::Vector3f _pos_sp; /**< desired position */ matrix::Vector3f _vel_sp; /**< desired velocity */ diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 1facfc9234..f2bb5cf8e1 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -82,11 +82,6 @@ public: _position_control.setTiltLimit(1.f); _position_control.setHoverThrust(.5f); - _contraints.tilt = 1.f; - _contraints.speed_xy = NAN; - _contraints.speed_up = NAN; - _contraints.speed_down = NAN; - resetInputSetpoint(); } @@ -106,7 +101,6 @@ public: bool runController() { - _position_control.setConstraints(_contraints); _position_control.setInputSetpoint(_input_setpoint); const bool ret = _position_control.update(.1f); _position_control.getLocalPositionSetpoint(_output_setpoint); @@ -115,7 +109,6 @@ public: } PositionControl _position_control; - vehicle_constraints_s _contraints{}; vehicle_local_position_setpoint_s _input_setpoint{}; vehicle_local_position_setpoint_s _output_setpoint{}; vehicle_attitude_setpoint_s _attitude{}; @@ -168,12 +161,14 @@ TEST_F(PositionControlBasicTest, TiltLimit) EXPECT_GT(angle, 0.f); EXPECT_LE(angle, 1.f); - _contraints.tilt = .5f; + _position_control.setTiltLimit(0.5f); EXPECT_TRUE(runController()); body_z = Quatf(_attitude.q_d).dcm_z(); angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); EXPECT_GT(angle, 0.f); EXPECT_LE(angle, .50001f); + + _position_control.setTiltLimit(1.f); // restore original } TEST_F(PositionControlBasicTest, VelocityLimit) diff --git a/src/modules/flight_mode_manager/Takeoff/CMakeLists.txt b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt similarity index 96% rename from src/modules/flight_mode_manager/Takeoff/CMakeLists.txt rename to src/modules/mc_pos_control/Takeoff/CMakeLists.txt index 67f02ddd16..e088a69c87 100644 --- a/src/modules/flight_mode_manager/Takeoff/CMakeLists.txt +++ b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt @@ -33,7 +33,9 @@ px4_add_library(Takeoff Takeoff.cpp + Takeoff.hpp ) +#target_compile_definitions(Takeoff PRIVATE DEBUG_BUILD) target_include_directories(Takeoff PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(Takeoff PUBLIC hysteresis) diff --git a/src/modules/flight_mode_manager/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp similarity index 100% rename from src/modules/flight_mode_manager/Takeoff/Takeoff.cpp rename to src/modules/mc_pos_control/Takeoff/Takeoff.cpp diff --git a/src/modules/flight_mode_manager/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp similarity index 100% rename from src/modules/flight_mode_manager/Takeoff/Takeoff.hpp rename to src/modules/mc_pos_control/Takeoff/Takeoff.hpp diff --git a/src/modules/flight_mode_manager/Takeoff/TakeoffTest.cpp b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp similarity index 100% rename from src/modules/flight_mode_manager/Takeoff/TakeoffTest.cpp rename to src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp