diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp index 0cb416a8b8..a903271ed6 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp @@ -58,14 +58,10 @@ void FlightTaskManualAltitudeSmooth::_updateSetpoints() /* Get yaw, thrust */ FlightTaskManualAltitude::_updateSetpoints(); - /* Smooth velocity setpoint */ - float vel_sp[2] = {_vel_sp_z, _vel_sp_prev_z}; - _smoothing.smoothVelFromSticks(vel_sp, _deltatime); - _vel_sp_z = vel_sp[0]; + /* Smooth velocity in z*/ + _smoothing.smoothVelFromSticks(_vel_sp_z, _deltatime); /* Check for altitude lock*/ _updateAltitudeLock(); - /* Update previous velocity setpoint for next smoothing iteration */ - _vel_sp_prev_z = _vel_sp_z; } diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp index c6e6dca282..2567c22a70 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp @@ -14,7 +14,7 @@ * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software - * without specific prior written permission. + * without spec{fic prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT @@ -41,7 +41,7 @@ #include ManualSmoothingZ::ManualSmoothingZ(const float &vel, const float &stick) : - _vel(vel), _stick(stick) + _vel(vel), _stick(stick), _vel_sp_prev(vel) { _acc_max_up_h = param_find("MPC_ACC_UP_MAX"); _acc_max_down_h = param_find("MPC_ACC_DOWN_MAX"); @@ -57,7 +57,7 @@ ManualSmoothingZ::ManualSmoothingZ(const float &vel, const float &stick) : * 2.) accelerate */ void -ManualSmoothingZ::smoothVelFromSticks(float vel_sp[2], const float dt) +ManualSmoothingZ::smoothVelFromSticks(float &vel_sp, const float dt) { updateParams(); @@ -65,6 +65,8 @@ ManualSmoothingZ::smoothVelFromSticks(float vel_sp[2], const float dt) velocitySlewRate(vel_sp, dt); + _vel_sp_prev = vel_sp; + } void @@ -90,8 +92,11 @@ ManualSmoothingZ::setParams() } void -ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt) +ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt) { + /* Check for max acceleration */ + setMaxAcceleration(); + /* check if zero input stick */ const bool is_current_zero = (fabsf(_stick) <= FLT_EPSILON); @@ -115,7 +120,7 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt) * is no delay present when user demands to brake */ - vel_sp[1] = _vel; + _vel_sp_prev = _vel; } @@ -137,7 +142,7 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt) case ManualIntentionZ::acceleration: { - _acc_state_dependent = (getMaxAcceleration(vel_sp) - _acc_max_down) + _acc_state_dependent = (getMaxAcceleration() - _acc_max_down) * fabsf(_stick) + _acc_max_down; break; } @@ -146,46 +151,46 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt) _intention = intention; } -float -ManualSmoothingZ::getMaxAcceleration(float vel_sp[2]) +void +ManualSmoothingZ::setMaxAcceleration() { /* Note: NED frame */ - if (_stick < 0.0f) { + if (_stick < -FLT_EPSILON) { /* accelerating upward */ - return _acc_max_up; + _max_acceleration = _acc_max_up; - } else if (_stick > 0.0f) { + } else if (_stick > FLT_EPSILON) { /* accelerating downward */ - return _acc_max_down; + _max_acceleration = _acc_max_down; } else { /* want to brake */ - if (fabsf(vel_sp[0] - vel_sp[1]) < FLT_EPSILON) { + if (fabsf(_vel_sp_prev) < FLT_EPSILON) { /* at rest */ - return _acc_max_up; + _max_acceleration = _acc_max_up; - } else if (vel_sp[0] < 0.0f) { + } else if (_vel_sp_prev < 0.0f) { /* braking downward */ - return _acc_max_down; + _max_acceleration = _acc_max_down; } else { /* braking upward */ - return _acc_max_up; + _max_acceleration = _acc_max_up; } } } void -ManualSmoothingZ::velocitySlewRate(float vel_sp[2], const float dt) +ManualSmoothingZ::velocitySlewRate(float &vel_sp, const float dt) { /* limit vertical acceleration */ - float acc = (vel_sp[0] - vel_sp[1]) / dt; + float acc = (vel_sp - _vel_sp_prev) / dt; float max_acc = (acc < 0.0f) ? -_acc_state_dependent : _acc_state_dependent; if (fabsf(acc) > fabsf(max_acc)) { - vel_sp[0] = max_acc * dt + vel_sp[1]; + vel_sp = max_acc * dt + _vel_sp_prev; } } diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp index d69a61af3d..b3fb8a1ee7 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp @@ -60,11 +60,11 @@ public: * vel_sp will contain smoothed current previous set-point. * @param dt: time delta in seconds */ - void smoothVelFromSticks(float vel_sp[2], const float dt); + void smoothVelFromSticks(float &vel_sp, const float dt); /* Getter methods */ - float getMaxAcceleration(float vel_sp[2]); + float getMaxAcceleration() {return _max_acceleration;}; ManualIntentionZ getIntention() {return _intention;}; /* Overwrite methods: @@ -89,6 +89,8 @@ private: * _acc_max_down <= _acc_state_dependent <= _acc_max_up */ float _acc_state_dependent{0.0f}; + float _vel_sp_prev; + float _max_acceleration; /* Params */ param_t _acc_max_up_h{PARAM_INVALID}; @@ -100,9 +102,10 @@ private: int _parameter_sub{-1}; /* Helper methods */ - void velocitySlewRate(float vel_sp[2], const float dt); + void velocitySlewRate(float &vel_sp, const float dt); void setParams(); void updateParams(); - void updateAcceleration(float vel_sp[2], const float dt); + void updateAcceleration(float &vel_sp, const float dt); + void setMaxAcceleration(); }; diff --git a/src/systemcmds/tests/test_smooth_z.cpp b/src/systemcmds/tests/test_smooth_z.cpp index ef276877b5..02d10db0f1 100644 --- a/src/systemcmds/tests/test_smooth_z.cpp +++ b/src/systemcmds/tests/test_smooth_z.cpp @@ -34,7 +34,6 @@ bool SmoothZTest::brakeUpward() float acc_max_up = 5.0f; float acc_max_down = 2.0f; - float vel_sp[] = {vel_sp_current, vel_sp_previous}; ManualSmoothingZ smooth(vel, stick_current); /* overwrite parameters since they might change depending on configuration */ @@ -49,17 +48,17 @@ bool SmoothZTest::brakeUpward() for (int i = 0; i < 100; i++) { - smooth.smoothVelFromSticks(vel_sp, dt); + smooth.smoothVelFromSticks(vel_sp_current, dt); /* Test if intention is brake */ ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake); /* we should always use upward acceleration */ - ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON)); + ut_assert_true((smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON)); /* New setpoint has to be lower than previous setpoint (NED frame) or equal 0. 0 velocity * occurs once the vehicle is at perfect rest. */ - ut_assert_true((vel_sp[0] < vel_sp[1]) || (fabsf(vel_sp[0]) < FLT_EPSILON)); + ut_assert_true((vel_sp_current < vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON)); /* We reset the previou setpoint to newest setpoint @@ -67,9 +66,10 @@ bool SmoothZTest::brakeUpward() * We also set vel to previous setpoint where we make the assumption that * the vehicle can perfectly track the setpoints. */ - vel_sp[1] = vel_sp[0]; - vel_sp[0] = 0.0f; - vel = vel_sp[1]; + vel_sp_previous = vel_sp_current; + vel_sp_current = 0.0f; + vel = vel_sp_previous; + } @@ -86,7 +86,6 @@ bool SmoothZTest::brakeDownward() float acc_max_up = 5.0f; float acc_max_down = 2.0f; - float vel_sp[] = { vel_sp_current, vel_sp_previous }; ManualSmoothingZ smooth(vel, stick_current); /* overwrite parameters since they might change depending on configuration */ @@ -101,21 +100,21 @@ bool SmoothZTest::brakeDownward() for (int i = 0; i < 100; i++) { - smooth.smoothVelFromSticks(vel_sp, dt); + smooth.smoothVelFromSticks(vel_sp_current, dt); /* Test if intention is brake */ ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake); /* New setpoint has to be larger than previous setpoint (NED frame) or equal 0. 0 velocity * occurs once the vehicle is at perfect rest. */ - ut_assert_true((vel_sp[0] > vel_sp[1]) || (fabsf(vel_sp[0]) < FLT_EPSILON)); + ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON)); /* we should always use downward acceleration except when vehicle is at rest*/ - if (fabsf(vel_sp[0]) < FLT_EPSILON) { - ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON)); + if (fabsf(vel_sp_current) < FLT_EPSILON) { + ut_assert_true(fabsf((smooth.getMaxAcceleration() - acc_max_up) < FLT_EPSILON)); } else { - ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_down < FLT_EPSILON)); + ut_assert_true(fabsf((smooth.getMaxAcceleration() - acc_max_down) < FLT_EPSILON)); } @@ -124,9 +123,9 @@ bool SmoothZTest::brakeDownward() * We also set vel to previous setpoint where we make the assumption that * the vehicle can perfectly track the setpoints. */ - vel_sp[1] = vel_sp[0]; - vel_sp[0] = 0.0f; - vel = vel_sp[1]; + vel_sp_previous = vel_sp_current; + vel_sp_current = 0.0f; + vel = vel_sp_previous; } @@ -137,13 +136,13 @@ bool SmoothZTest::accelerateUpwardFromBrake() { /* Downward flight and want to stop */ float stick_current = -1.0f; // sticks are at full upward position - float vel_sp_current = -5.0f; // desired velocity is at -5m/s + float vel_sp_target = -5.0f; // desired velocity is at -5m/s + float vel_sp_current = vel_sp_target; float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint float acc_max_up = 5.0f; float acc_max_down = 2.0f; - float vel_sp[] = {vel_sp_current, vel_sp_previous}; ManualSmoothingZ smooth(vel, stick_current); /* overwrite parameters since they might change depending on configuration */ @@ -155,27 +154,27 @@ bool SmoothZTest::accelerateUpwardFromBrake() for (int i = 0; i < 100; i++) { - smooth.smoothVelFromSticks(vel_sp, dt); + smooth.smoothVelFromSticks(vel_sp_current, dt); /* Test if intention is acceleration */ ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration); /* we should always use upward acceleration */ - ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON)); + ut_assert_true((smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON)); /* New setpoint has to be larger than previous setpoint or equal to target velocity * vel_sp_current. The negative sign is because of NED frame. */ - ut_assert_true((-vel_sp[0] > -vel_sp[1]) || (fabsf(vel_sp[0] - vel_sp_current) < FLT_EPSILON)); + ut_assert_true((-vel_sp_current > -vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_previous) < FLT_EPSILON)); /* We reset the previous setpoint to newest setpoint and reset the current setpoint. * We also set the current velocity to the previous setpoint with the assumption that * the vehicle does perfect tracking. */ - vel_sp[1] = vel_sp[0]; - vel_sp[0] = vel_sp_current; - vel = vel_sp[1]; + vel_sp_previous = vel_sp_current; + vel_sp_current = vel_sp_target; + vel = vel_sp_previous; } @@ -186,13 +185,13 @@ bool SmoothZTest::accelerateDownwardFromBrake() { /* Downward flight and want to stop */ float stick_current = 1.0f; // sticks are at full downward position - float vel_sp_current = 5.0f; // desired velocity is at 5m/s + float vel_sp_target = 5.0f; // desired velocity is at 5m/s + float vel_sp_current = vel_sp_target; float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint float acc_max_up = 5.0f; float acc_max_down = 2.0f; - float vel_sp[] = {vel_sp_current, vel_sp_previous}; ManualSmoothingZ smooth(vel, stick_current); /* overwrite parameters since they might change depending on configuration */ @@ -204,32 +203,32 @@ bool SmoothZTest::accelerateDownwardFromBrake() for (int i = 0; i < 100; i++) { - smooth.smoothVelFromSticks(vel_sp, dt); + smooth.smoothVelFromSticks(vel_sp_current, dt); /* Test if intention is acceleration */ ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration); /* we should always use downward acceleration except when target velocity is reached */ - if (fabsf(vel_sp[0] - vel_sp_current) < FLT_EPSILON) { - ut_assert_true(smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON); + if (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON) { + ut_assert_true(smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON); } else { - ut_assert_true(smooth.getMaxAcceleration(vel_sp) - acc_max_down < FLT_EPSILON); + ut_assert_true(smooth.getMaxAcceleration() - acc_max_down < FLT_EPSILON); } /* New setpoint has to be larger than previous setpoint or equal to target velocity * vel_sp_current (NED frame). */ - ut_assert_true((vel_sp[0] > vel_sp[1]) || (fabsf(vel_sp[0] - vel_sp_current) < FLT_EPSILON)); + ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON)); /* We reset the previous setpoint to newest setpoint and reset the current setpoint. * We also set the current velocity to the previous setpoint with the assumption that * the vehicle does perfect tracking. */ - vel_sp[1] = vel_sp[0]; - vel_sp[0] = vel_sp_current; - vel = vel_sp[1]; + vel_sp_previous = vel_sp_current; + vel_sp_current = vel_sp_target; + vel = vel_sp_previous; }