diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 7fff71f9ffe..4ed455806a4 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -320,7 +320,7 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position) { // If the task fails sned out empty NAN setpoints and the controller will emergency failsafe - trajectory_setpoint_s setpoint = FlightTask::empty_setpoint; + trajectory_setpoint_s setpoint = FlightTask::empty_trajectory_setpoint; vehicle_constraints_s constraints = FlightTask::empty_constraints; if (_current_task.task->updateInitialize() && _current_task.task->update()) { @@ -392,7 +392,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) } // Save current setpoints for the next FlightTask - trajectory_setpoint_s last_setpoint = FlightTask::empty_setpoint; + trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint; ekf_reset_counters_s last_reset_counters{}; if (isAnyTaskActive()) { diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index 726b1e2c006..c4b96490463 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -3,9 +3,7 @@ #include constexpr uint64_t FlightTask::_timeout; -// First index of empty_setpoint corresponds to time-stamp and requires a finite number. -const trajectory_setpoint_s FlightTask::empty_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; - +const trajectory_setpoint_s FlightTask::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, false, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; @@ -21,7 +19,7 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint) void FlightTask::reActivate() { // Preserve vertical velocity while on the ground to allow descending by stick for reliable land detection - trajectory_setpoint_s setpoint_preserve_vertical{empty_setpoint}; + trajectory_setpoint_s setpoint_preserve_vertical{empty_trajectory_setpoint}; setpoint_preserve_vertical.velocity[2] = _velocity_setpoint(2); activate(setpoint_preserve_vertical); } diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 3126c1ba642..c76a83206ad 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -138,10 +138,9 @@ public: const vehicle_trajectory_waypoint_s &getAvoidanceWaypoint() { return _desired_waypoint; } /** - * Empty setpoint. - * All setpoints are set to NAN. + * All setpoints are set to NAN (uncontrolled). Timestampt zero. */ - static const trajectory_setpoint_s empty_setpoint; + static const trajectory_setpoint_s empty_trajectory_setpoint; /** * Empty constraints. diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 64cd3d2b428..503c6b60433 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -2119,7 +2119,7 @@ FixedwingPositionControl::Run() } if (_control_mode.flag_control_offboard_enabled) { - vehicle_local_position_setpoint_s trajectory_setpoint; + trajectory_setpoint_s trajectory_setpoint; if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { bool valid_setpoint = false; @@ -2135,31 +2135,32 @@ FixedwingPositionControl::Run() _pos_sp_triplet.current.lon = static_cast(NAN); _pos_sp_triplet.current.alt = NAN; - if (PX4_ISFINITE(trajectory_setpoint.x) && PX4_ISFINITE(trajectory_setpoint.y) && PX4_ISFINITE(trajectory_setpoint.z)) { + if (PX4_ISFINITE(trajectory_setpoint.position[0]) && PX4_ISFINITE(trajectory_setpoint.position[1]) + && PX4_ISFINITE(trajectory_setpoint.position[2])) { if (_global_local_proj_ref.isInitialized()) { double lat; double lon; - _global_local_proj_ref.reproject(trajectory_setpoint.x, trajectory_setpoint.y, lat, lon); + _global_local_proj_ref.reproject(trajectory_setpoint.position[0], trajectory_setpoint.position[1], lat, lon); valid_setpoint = true; _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; _pos_sp_triplet.current.lat = lat; _pos_sp_triplet.current.lon = lon; - _pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z; + _pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.position[2]; } } - if (PX4_ISFINITE(trajectory_setpoint.vx) && PX4_ISFINITE(trajectory_setpoint.vx) - && PX4_ISFINITE(trajectory_setpoint.vz)) { + if (PX4_ISFINITE(trajectory_setpoint.velocity[0]) && PX4_ISFINITE(trajectory_setpoint.velocity[1]) + && PX4_ISFINITE(trajectory_setpoint.velocity[2])) { valid_setpoint = true; _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - _pos_sp_triplet.current.vx = trajectory_setpoint.vx; - _pos_sp_triplet.current.vy = trajectory_setpoint.vy; - _pos_sp_triplet.current.vz = trajectory_setpoint.vz; + _pos_sp_triplet.current.vx = trajectory_setpoint.velocity[0]; + _pos_sp_triplet.current.vy = trajectory_setpoint.velocity[1]; + _pos_sp_triplet.current.vz = trajectory_setpoint.velocity[2]; if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1]) && PX4_ISFINITE(trajectory_setpoint.acceleration[2])) { - Vector2f velocity_sp_2d(trajectory_setpoint.vx, trajectory_setpoint.vy); + Vector2f velocity_sp_2d(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]); Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(velocity_sp_2d) * velocity_sp_2d.normalized(); diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index eb21bb3bbbc..ab569a20d7e 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -212,12 +212,12 @@ bool MulticopterLandDetector::_get_ground_contact_state() // if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present, // we then can assume that the vehicle hit ground if (_flag_control_climb_rate_enabled) { - vehicle_local_position_setpoint_s trajectory_setpoint; + trajectory_setpoint_s trajectory_setpoint; if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { // Setpoints can be NAN - _in_descend = PX4_ISFINITE(trajectory_setpoint.vz) - && (trajectory_setpoint.vz >= crawl_speed_threshold); + _in_descend = PX4_ISFINITE(trajectory_setpoint.velocity[2]) + && (trajectory_setpoint.velocity[2] >= crawl_speed_threshold); } // ground contact requires commanded descent until landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 855d6ff52d2..1bbd7c7315f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -46,7 +46,6 @@ #include #include #include -#include #include #include "LandDetector.h" diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ba43ff3fc01..f239b2b4efb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -106,7 +106,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 0151a98affd..4744e7e4d7f 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -52,7 +52,6 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) : { parameters_update(true); _tilt_limit_slew_rate.setSlewRate(.2f); - reset_setpoint_to_nan(_setpoint); _takeoff_status_pub.advertise(); } @@ -355,7 +354,7 @@ void MulticopterPositionControl::Run() } else if (previous_position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) { // clear existing setpoint when controller is no longer active - reset_setpoint_to_nan(_setpoint); + _setpoint = PositionControl::empty_trajectory_setpoint; } } } @@ -372,43 +371,26 @@ void MulticopterPositionControl::Run() } } - if (_trajectory_setpoint_sub.updated()) { - trajectory_setpoint_s trajectory_setpoint; - - if (_trajectory_setpoint_sub.copy(&trajectory_setpoint)) { - _setpoint.timestamp = trajectory_setpoint.timestamp; - _setpoint.x = trajectory_setpoint.position[0]; - _setpoint.y = trajectory_setpoint.position[1]; - _setpoint.z = trajectory_setpoint.position[2]; - _setpoint.vx = trajectory_setpoint.velocity[0]; - _setpoint.vy = trajectory_setpoint.velocity[1]; - _setpoint.vz = trajectory_setpoint.velocity[2]; - _setpoint.acceleration[0] = trajectory_setpoint.acceleration[0]; - _setpoint.acceleration[1] = trajectory_setpoint.acceleration[1]; - _setpoint.acceleration[2] = trajectory_setpoint.acceleration[2]; - _setpoint.yaw = trajectory_setpoint.yaw; - _setpoint.yawspeed = trajectory_setpoint.yawspeed; - } - } + _trajectory_setpoint_sub.update(&_setpoint); // adjust existing (or older) setpoint with any EKF reset deltas if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) { if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) { - _setpoint.vx += vehicle_local_position.delta_vxy[0]; - _setpoint.vy += vehicle_local_position.delta_vxy[1]; + _setpoint.velocity[0] += vehicle_local_position.delta_vxy[0]; + _setpoint.velocity[1] += vehicle_local_position.delta_vxy[1]; } if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) { - _setpoint.vz += vehicle_local_position.delta_vz; + _setpoint.velocity[2] += vehicle_local_position.delta_vz; } if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) { - _setpoint.x += vehicle_local_position.delta_xy[0]; - _setpoint.y += vehicle_local_position.delta_xy[1]; + _setpoint.position[0] += vehicle_local_position.delta_xy[0]; + _setpoint.position[1] += vehicle_local_position.delta_xy[1]; } if (vehicle_local_position.z_reset_counter != _z_reset_counter) { - _setpoint.z += vehicle_local_position.delta_z; + _setpoint.position[2] += vehicle_local_position.delta_z; } if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) { @@ -463,13 +445,13 @@ void MulticopterPositionControl::Run() const bool want_takeoff = _vehicle_control_mode.flag_armed && (vehicle_local_position.timestamp_sample < _setpoint.timestamp + 1_s); - if (want_takeoff && PX4_ISFINITE(_setpoint.z) - && (_setpoint.z < states.position(2))) { + if (want_takeoff && PX4_ISFINITE(_setpoint.position[2]) + && (_setpoint.position[2] < states.position(2))) { _vehicle_constraints.want_takeoff = true; - } else if (want_takeoff && PX4_ISFINITE(_setpoint.vz) - && (_setpoint.vz < 0.f)) { + } else if (want_takeoff && PX4_ISFINITE(_setpoint.velocity[2]) + && (_setpoint.velocity[2] < 0.f)) { _vehicle_constraints.want_takeoff = true; @@ -507,7 +489,7 @@ void MulticopterPositionControl::Run() if (not_taken_off || flying_but_ground_contact) { // we are not flying yet and need to avoid any corrections - reset_setpoint_to_nan(_setpoint); + _setpoint = PositionControl::empty_trajectory_setpoint; _setpoint.timestamp = vehicle_local_position.timestamp_sample; Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust @@ -544,15 +526,15 @@ void MulticopterPositionControl::Run() _control.setInputSetpoint(_setpoint); // update states - if (!PX4_ISFINITE(_setpoint.z) - && PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON) + if (!PX4_ISFINITE(_setpoint.position[2]) + && PX4_ISFINITE(_setpoint.velocity[2]) && (fabsf(_setpoint.velocity[2]) > FLT_EPSILON) && PX4_ISFINITE(vehicle_local_position.z_deriv) && vehicle_local_position.z_valid && vehicle_local_position.v_z_valid) { // A change in velocity is demanded and the altitude is not controlled. // 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); + float weighting = fminf(fabsf(_setpoint.velocity[2]) / _param_mpc_land_speed.get(), 1.f); states.velocity(2) = vehicle_local_position.z_deriv * weighting + vehicle_local_position.vz * (1.f - weighting); } @@ -603,7 +585,7 @@ void MulticopterPositionControl::Run() perf_end(_cycle_perf); } -vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const hrt_abstime &now, +trajectory_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states) { // do not warn while we are disarmed, as we might not have valid setpoints yet @@ -614,13 +596,12 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe _last_warn = now; } - vehicle_local_position_setpoint_s failsafe_setpoint{}; - reset_setpoint_to_nan(failsafe_setpoint); + trajectory_setpoint_s failsafe_setpoint = PositionControl::empty_trajectory_setpoint; failsafe_setpoint.timestamp = now; if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) { // don't move along xy - failsafe_setpoint.vx = failsafe_setpoint.vy = 0.f; + failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = 0.f; if (warn) { PX4_WARN("Failsafe: stop and wait"); @@ -629,7 +610,7 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe } else { // descend with land speed since we can't stop failsafe_setpoint.acceleration[0] = failsafe_setpoint.acceleration[1] = 0.f; - failsafe_setpoint.vz = _param_mpc_land_speed.get(); + failsafe_setpoint.velocity[2] = _param_mpc_land_speed.get(); if (warn) { PX4_WARN("Failsafe: blind land"); @@ -638,13 +619,13 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe if (PX4_ISFINITE(states.velocity(2))) { // don't move along z if we can stop in all dimensions - if (!PX4_ISFINITE(failsafe_setpoint.vz)) { - failsafe_setpoint.vz = 0.f; + if (!PX4_ISFINITE(failsafe_setpoint.velocity[2])) { + failsafe_setpoint.velocity[2] = 0.f; } } else { // emergency descend with a bit below hover thrust - failsafe_setpoint.vz = NAN; + failsafe_setpoint.velocity[2] = NAN; failsafe_setpoint.acceleration[2] = .3f; if (warn) { @@ -655,16 +636,6 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe return failsafe_setpoint; } -void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint) -{ - setpoint.timestamp = 0; - setpoint.x = setpoint.y = setpoint.z = NAN; - setpoint.yaw = setpoint.yawspeed = NAN; - setpoint.vx = setpoint.vy = setpoint.vz = NAN; - setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN; - setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN; -} - 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 7a70ae0b13e..7a6be18ccd4 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -109,8 +109,8 @@ private: hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ hrt_abstime _time_position_control_enabled{0}; - vehicle_local_position_setpoint_s _setpoint {}; - vehicle_control_mode_s _vehicle_control_mode {}; + trajectory_setpoint_s _setpoint{PositionControl::empty_trajectory_setpoint}; + vehicle_control_mode_s _vehicle_control_mode{}; vehicle_constraints_s _vehicle_constraints { .timestamp = 0, @@ -223,10 +223,5 @@ private: * Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid. * This should only happen briefly when transitioning and never during mode operation or by design. */ - vehicle_local_position_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states); - - /** - * Reset setpoints to NAN - */ - void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint); + trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states); }; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index b770ab5a68d..5abbc2fbcec 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -44,6 +44,8 @@ using namespace matrix; +const trajectory_setpoint_s PositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; + void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) { _gain_vel_p = P; @@ -93,10 +95,10 @@ void PositionControl::setState(const PositionControlStates &states) _vel_dot = states.acceleration; } -void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint) +void PositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint) { - _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); - _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); + _pos_sp = Vector3f(setpoint.position); + _vel_sp = Vector3f(setpoint.velocity); _acc_sp = Vector3f(setpoint.acceleration); _yaw_sp = setpoint.yaw; _yawspeed_sp = setpoint.yawspeed; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index fa2fd35f805..124fd08b034 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -141,9 +142,9 @@ public: /** * Pass the desired setpoints * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint. - * @param setpoint a vehicle_local_position_setpoint_s structure + * @param setpoint setpoints including feed-forwards to execute in update() */ - void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint); + void setInputSetpoint(const trajectory_setpoint_s &setpoint); /** * Apply P-position and PID-velocity controller that updates the member @@ -178,6 +179,11 @@ public: */ void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const; + /** + * All setpoints are set to NAN (uncontrolled). Timestampt zero. + */ + static const trajectory_setpoint_s empty_trajectory_setpoint; + private: bool _inputValid(); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 8be0f44fca4..384646afd49 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -79,22 +79,6 @@ public: _position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN); _position_control.setTiltLimit(1.f); _position_control.setHoverThrust(.5f); - - resetInputSetpoint(); - } - - void resetInputSetpoint() - { - _input_setpoint.x = NAN; - _input_setpoint.y = NAN; - _input_setpoint.z = NAN; - _input_setpoint.yaw = NAN; - _input_setpoint.yawspeed = NAN; - _input_setpoint.vx = NAN; - _input_setpoint.vy = NAN; - _input_setpoint.vz = NAN; - Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.acceleration); - Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.thrust); } bool runController() @@ -107,7 +91,7 @@ public: } PositionControl _position_control; - vehicle_local_position_setpoint_s _input_setpoint{}; + trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint}; vehicle_local_position_setpoint_s _output_setpoint{}; vehicle_attitude_setpoint_s _attitude{}; @@ -134,27 +118,21 @@ public: TEST_F(PositionControlBasicDirectionTest, PositionDirection) { - _input_setpoint.x = .1f; - _input_setpoint.y = .1f; - _input_setpoint.z = -.1f; + Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.position); EXPECT_TRUE(runController()); checkDirection(); } TEST_F(PositionControlBasicDirectionTest, VelocityDirection) { - _input_setpoint.vx = .1f; - _input_setpoint.vy = .1f; - _input_setpoint.vz = -.1f; + Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.velocity); EXPECT_TRUE(runController()); checkDirection(); } TEST_F(PositionControlBasicTest, TiltLimit) { - _input_setpoint.x = 10.f; - _input_setpoint.y = 10.f; - _input_setpoint.z = -0.f; + Vector3f(10.f, 10.f, 0.f).copyTo(_input_setpoint.position); EXPECT_TRUE(runController()); Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); @@ -174,9 +152,7 @@ TEST_F(PositionControlBasicTest, TiltLimit) TEST_F(PositionControlBasicTest, VelocityLimit) { - _input_setpoint.x = 10.f; - _input_setpoint.y = 10.f; - _input_setpoint.z = -10.f; + Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); EXPECT_TRUE(runController()); Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy); @@ -187,9 +163,7 @@ TEST_F(PositionControlBasicTest, VelocityLimit) TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) { // Given a setpoint that drives the controller into vertical and horizontal saturation - _input_setpoint.x = 10.f; - _input_setpoint.y = 10.f; - _input_setpoint.z = -10.f; + Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); // When you run it for one iteration runController(); @@ -218,9 +192,7 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) { - _input_setpoint.x = 10.f; - _input_setpoint.y = 0.f; - _input_setpoint.z = 10.f; + Vector3f(10.f, 0.f, 10.f).copyTo(_input_setpoint.position); runController(); Vector3f thrust(_output_setpoint.thrust); @@ -234,9 +206,8 @@ TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) TEST_F(PositionControlBasicTest, FailsafeInput) { - _input_setpoint.vz = .1f; - _input_setpoint.thrust[0] = _input_setpoint.thrust[1] = 0.f; _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f; + _input_setpoint.velocity[2] = .1f; EXPECT_TRUE(runController()); EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f); @@ -255,14 +226,12 @@ TEST_F(PositionControlBasicTest, IdleThrustInput) EXPECT_TRUE(runController()); EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f); EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f); - EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); // minimum thrust } TEST_F(PositionControlBasicTest, InputCombinationsPosition) { - _input_setpoint.x = .1f; - _input_setpoint.y = .2f; - _input_setpoint.z = .3f; + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); EXPECT_TRUE(runController()); EXPECT_FLOAT_EQ(_output_setpoint.x, .1f); @@ -278,13 +247,13 @@ TEST_F(PositionControlBasicTest, InputCombinationsPosition) TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity) { - _input_setpoint.vx = .1f; - _input_setpoint.vy = .2f; - _input_setpoint.z = .3f; // altitude + _input_setpoint.velocity[0] = .1f; + _input_setpoint.velocity[1] = .2f; + _input_setpoint.position[2] = .3f; // altitude EXPECT_TRUE(runController()); - // EXPECT_TRUE(isnan(_output_setpoint.x)); - // EXPECT_TRUE(isnan(_output_setpoint.y)); + EXPECT_TRUE(isnan(_output_setpoint.x)); + EXPECT_TRUE(isnan(_output_setpoint.y)); EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f); EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f); @@ -297,9 +266,9 @@ TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity) TEST_F(PositionControlBasicTest, SetpointValiditySimple) { EXPECT_FALSE(runController()); - _input_setpoint.x = .1f; + _input_setpoint.position[0] = .1f; EXPECT_FALSE(runController()); - _input_setpoint.y = .2f; + _input_setpoint.position[1] = .2f; EXPECT_FALSE(runController()); _input_setpoint.acceleration[2] = .3f; EXPECT_TRUE(runController()); @@ -308,13 +277,13 @@ TEST_F(PositionControlBasicTest, SetpointValiditySimple) TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) { // This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly - float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.acceleration[0], - &_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.acceleration[1], - &_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.acceleration[2] + float *const setpoint_loop_access_map[] = {&_input_setpoint.position[0], &_input_setpoint.velocity[0], &_input_setpoint.acceleration[0], + &_input_setpoint.position[1], &_input_setpoint.velocity[1], &_input_setpoint.acceleration[1], + &_input_setpoint.position[2], &_input_setpoint.velocity[2], &_input_setpoint.acceleration[2] }; for (int combination = 0; combination < 512; combination++) { - resetInputSetpoint(); + _input_setpoint = PositionControl::empty_trajectory_setpoint; for (int j = 0; j < 9; j++) { if (combination & (1 << j)) { @@ -333,8 +302,10 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl << "input" << std::endl - << "position " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl - << "velocity " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << std::endl + << "position " << _input_setpoint.position[0] << ", " + << _input_setpoint.position[1] << ", " << _input_setpoint.position[2] << std::endl + << "velocity " << _input_setpoint.velocity[0] << ", " + << _input_setpoint.velocity[1] << ", " << _input_setpoint.velocity[2] << std::endl << "acceleration " << _input_setpoint.acceleration[0] << ", " << _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl << "output" << std::endl @@ -347,9 +318,7 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) TEST_F(PositionControlBasicTest, InvalidState) { - _input_setpoint.x = .1f; - _input_setpoint.y = .2f; - _input_setpoint.z = .3f; + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); PositionControlStates states{}; states.position(0) = NAN; @@ -377,9 +346,7 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust) const float hover_thrust = 0.6f; _position_control.setHoverThrust(hover_thrust); - _input_setpoint.vx = 0.f; - _input_setpoint.vy = 0.f; - _input_setpoint.vz = -0.f; + Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); // WHEN: we run the controller EXPECT_TRUE(runController()); @@ -400,16 +367,14 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust) TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint) { // GIVEN: the controller was ran with an invalid setpoint containing some valid values - _input_setpoint.x = .1f; - _input_setpoint.y = .2f; + _input_setpoint.position[0] = .1f; + _input_setpoint.position[1] = .2f; // all z-axis setpoints stay NAN EXPECT_FALSE(runController()); // WHEN: we run the controller with a valid setpoint - resetInputSetpoint(); - _input_setpoint.vx = 0.f; - _input_setpoint.vy = 0.f; - _input_setpoint.vz = 0.f; + _input_setpoint = PositionControl::empty_trajectory_setpoint; + Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); EXPECT_TRUE(runController()); // THEN: the integral did not wind up and produce unexpected deviation diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index 4fd29bffb17..1de486e3253 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -73,8 +73,7 @@ public: /** * Update the state for the takeoff. - * @param setpoint a vehicle_local_position_setpoint_s structure - * @return true if setpoint has updated correctly + * Has to be called also when not flying altitude controlled to skip the takeoff and not do it in flight when switching mode. */ void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us);