From 54a5e177f3e4d0a89628933b1f1f19954e1932aa Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Wed, 13 Dec 2017 14:53:33 +0100
Subject: [PATCH] ManualSmoothingZ: comments cleanup and style fix
---
.../tasks/Utility/ManualSmoothingZ.cpp | 21 ++++----
.../tasks/Utility/ManualSmoothingZ.hpp | 51 +++++++++++++++----
2 files changed, 50 insertions(+), 22 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp
index f2ce930571..878b26b77d 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp
@@ -34,7 +34,7 @@
/**
* @file ManualSmoothingZ.hpp
*
- * This Class is used for smoothing the velocity setpoints in Z-direction.
+ * This Class is used for smoothing the velocity set-points in z-direction NED frame.
*/
#include "ManualSmoothingZ.hpp"
@@ -47,7 +47,7 @@ ManualSmoothingZ::ManualSmoothingZ(const float &vel, const float &stick) :
{
_acc_max_up_h = param_find("MPC_ACC_UP_MAX");
_acc_max_down_h = param_find("MPC_ACC_DOWN_MAX");
- _jerk_max = param_find("MPC_JERK_MAX");
+ _jerk_max_h = param_find("MPC_JERK_MAX");
/* Load the params the very first time */
setParams();
@@ -98,23 +98,23 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt)
const bool is_current_zero = (fabsf(_stick) <= FLT_EPSILON);
/* default is acceleration */
- Intention intention = Intention::acceleration;
+ ManualIntentionZ intention = ManualIntentionZ::acceleration;
/* check zero input stick */
if (is_current_zero) {
- intention = Intention::brake;
+ intention = ManualIntentionZ::brake;
}
/*
* update intention
*/
- if ((_intention != Intention::brake) && (intention == Intention::brake)) {
+ if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) {
/* we start with lowest acceleration */
_acc_state_dependent = _acc_max_down;
- /* reset slewrate: this ensures that there
- * is no delay present because of the slewrate
+ /* reset slew-rate: this ensures that there
+ * is no delay present when user demands to brake
*/
vel_sp[1] = _vel;
@@ -122,7 +122,7 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt)
}
switch (intention) {
- case Intention::brake: {
+ case ManualIntentionZ::brake: {
/* limit jerk when braking to zero */
float jerk = (_acc_max_up - _acc_state_dependent) / dt;
@@ -137,10 +137,9 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt)
break;
}
- case Intention::acceleration: {
+ 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;
}
@@ -170,7 +169,7 @@ ManualSmoothingZ::getMaxAcceleration(float vel_sp[2])
/* at rest */
return _acc_max_up;
- } else if (vel_sp[0] < 0.0f ) {
+ } else if (vel_sp[0] < 0.0f) {
/* braking downward */
return _acc_max_down;
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp
index 45746ff895..d69a61af3d 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp
@@ -41,39 +41,68 @@
#include <systemlib/param/param.h>
+/* User intention: brake or acceleration */
+enum class ManualIntentionZ {
+ brake,
+ acceleration,
+};
class ManualSmoothingZ
{
public:
ManualSmoothingZ(const float &vel, const float &stick);
- ~ManualSmoothingZ();
+ ~ManualSmoothingZ() {};
+ /* Smooths velocity setpoint based
+ * on flight direction.
+ * @param vel_sp[2] array: vel_sp[0] = current velocity set-point;
+ * vel_sp[1] = previous velocity set-point
+ * vel_sp will contain smoothed current previous set-point.
+ * @param dt: time delta in seconds
+ */
void smoothVelFromSticks(float vel_sp[2], const float dt);
+
+ /* Getter methods */
+ float getMaxAcceleration(float vel_sp[2]);
+ ManualIntentionZ getIntention() {return _intention;};
+
+ /* Overwrite methods:
+ * Needed if different parameter values than default required.
+ */
+ void overwriteAccelerationUp(float acc_max_up) {_acc_max_up = acc_max_up;};
+ void overwriteAccelerationDown(float acc_max_down) {_acc_max_down = acc_max_down;};
+ void overwriteJerkMax(float jerk_max) {_jerk_max = jerk_max;};
+
private:
- enum class Intention {
- brake,
- acceleration,
- };
- Intention _intention{Intention::brake};
+ /* User intention: brake or acceleration */
+ ManualIntentionZ _intention{ManualIntentionZ::acceleration};
+ /* Dependency injection: vehicle velocity in z-direction;
+ * stick input in z-direction
+ */
const float &_vel;
const float &_stick;
+ /* Acceleration that depends on vehicle state
+ * _acc_max_down <= _acc_state_dependent <= _acc_max_up
+ */
+ float _acc_state_dependent{0.0f};
+
+ /* Params */
param_t _acc_max_up_h{PARAM_INVALID};
param_t _acc_max_down_h{PARAM_INVALID};
param_t _jerk_max_h{PARAM_INVALID};
float _acc_max_up{0.0f};
float _acc_max_down{0.0f};
float _jerk_max{10000.0f};
- float _acc_state_dependent{0.0f};
int _parameter_sub{-1};
-
+ /* Helper methods */
void velocitySlewRate(float vel_sp[2], const float dt);
- void updateParams();
- void updateAcceleration(float &vel_sp_prev, const float dt);
void setParams();
- float getMaxAcceleration();
+ void updateParams();
+ void updateAcceleration(float vel_sp[2], const float dt);
+
};
--
GitLab