From 5eb7d8ec5fc20f7a41c568b6fae5359aa7d9cf3c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 26 Mar 2018 08:19:57 +0200
Subject: [PATCH] flight tasks smoothing: refactor param_find -> Param
Plus some style fixes
---
.../tasks/FlightTaskManualAltitudeSmooth.cpp | 2 +-
.../tasks/FlightTaskManualPositionSmooth.cpp | 4 +-
.../tasks/Utility/ManualSmoothingXY.cpp | 60 ++++-------------
.../tasks/Utility/ManualSmoothingXY.hpp | 65 ++++++++-----------
.../tasks/Utility/ManualSmoothingZ.cpp | 55 ++++------------
.../tasks/Utility/ManualSmoothingZ.hpp | 43 +++++-------
src/systemcmds/tests/test_smooth_z.cpp | 8 +--
7 files changed, 80 insertions(+), 157 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp
index 91a488c4e7..6c00b3bf5b 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp
@@ -39,8 +39,8 @@
using namespace matrix;
- _smoothing(_velocity(2), _sticks(2))
FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth() :
+ _smoothing(this, _velocity(2), _sticks(2))
{}
void FlightTaskManualAltitudeSmooth::_updateSetpoints()
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
index b4892c2d2e..edb033d000 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
@@ -40,8 +40,8 @@
using namespace matrix;
FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth() :
- _smoothingXY(matrix::Vector2f(&_velocity(0))),
- _smoothingZ(_velocity(2), _sticks(2))
+ _smoothingXY(this, matrix::Vector2f(&_velocity(0))),
+ _smoothingZ(this, _velocity(2), _sticks(2))
{}
void FlightTaskManualPositionSmooth::_updateSetpoints()
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
index 8f17e0bb0d..34bd925425 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
@@ -40,26 +40,16 @@
#include <mathlib/mathlib.h>
#include <float.h>
-ManualSmoothingXY::ManualSmoothingXY(const matrix::Vector2f &vel) :
+ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel) :
+ ModuleParams(parent),
_vel_sp_prev(vel)
{
- _acc_hover_h = param_find("MPC_ACC_HOR_MAX");
- _acc_xy_max_h = param_find("MPC_ACC_HOR");
- _dec_xy_min_h = param_find("DEC_HOR_SLOW");
- _jerk_max_h = param_find("MPC_JERK_MAX");
- _jerk_min_h = param_find("MPC_JERK_MIN");
- _vel_manual_h = param_find("MPC_VEL_MANUAL");
-
- /* Load the params the very first time */
- _setParams();
}
void
ManualSmoothingXY::smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
const float &yawrate_sp, const float dt)
{
- _updateParams();
-
_updateAcceleration(vel_sp, vel, yaw, yawrate_sp, dt);
_velocitySlewRate(vel_sp, dt);
@@ -67,30 +57,6 @@ ManualSmoothingXY::smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector
_vel_sp_prev = vel_sp;
}
-void
-ManualSmoothingXY::_updateParams()
-{
- bool updated;
- parameter_update_s param_update;
- orb_check(_parameter_sub, &updated);
-
- if (updated) {
- orb_copy(ORB_ID(parameter_update), _parameter_sub, ¶m_update);
- _setParams();
- }
-}
-
-void
-ManualSmoothingXY::_setParams()
-{
- param_get(_acc_hover_h, &_acc_hover);
- param_get(_acc_xy_max_h, &_acc_xy_max);
- param_get(_dec_xy_min_h, &_dec_xy_min);
- param_get(_jerk_max_h, &_jerk_max);
- param_get(_jerk_min_h, &_jerk_min);
- param_get(_vel_manual_h, &_vel_manual);
-}
-
void
ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
const float &yawrate_sp, const float dt)
@@ -140,7 +106,7 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V
* Only use direction change if not aligned, no yawspeed demand, demand larger than 0.7 of max speed and velocity larger than 2m/s.
* Only use deceleration if stick input is lower than previous setpoint, aligned and no yawspeed demand. */
bool yawspeed_demand = fabsf(yawrate_sp) > 0.05f && PX4_ISFINITE(yawrate_sp);
- bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_manual) && !yawspeed_demand
+ bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_manual.get()) && !yawspeed_demand
&& (vel.length() > 2.0f);
bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand;
@@ -176,7 +142,7 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
} else if (intention != _intention) {
/* we start braking with lowest acceleration
* This make stopping smoother. */
- _acc_state_dependent = _dec_xy_min;
+ _acc_state_dependent = _dec_xy_min.get();
/* Adjust jerk based on current velocity, This ensures
* that the vehicle will stop much quicker at large speed but
@@ -184,10 +150,10 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
*/
_jerk_state_dependent = 1e6f; // default
- if (_jerk_max > _jerk_min) {
+ if (_jerk_max.get() > _jerk_min.get()) {
- _jerk_state_dependent = math::min((_jerk_max - _jerk_min)
- / _vel_manual * vel.length() + _jerk_min, _jerk_max);
+ _jerk_state_dependent = math::min((_jerk_max.get() - _jerk_min.get())
+ / _vel_manual.get() * vel.length() + _jerk_min.get(), _jerk_max.get());
}
/* Since user wants to brake smoothly but NOT continuing to fly
@@ -197,14 +163,14 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
}
/* limit jerk when braking to zero */
- float jerk = (_acc_hover - _acc_state_dependent) / dt;
+ float jerk = (_acc_hover.get() - _acc_state_dependent) / dt;
if (jerk > _jerk_state_dependent) {
_acc_state_dependent = _jerk_state_dependent * dt
+ _acc_state_dependent;
} else {
- _acc_state_dependent = _acc_hover;
+ _acc_state_dependent = _acc_hover.get();
}
break;
@@ -221,20 +187,20 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
* slewrate will have no effect. Nonetheless, just set
* acceleration to maximum.
*/
- _acc_state_dependent = _acc_xy_max;
+ _acc_state_dependent = _acc_xy_max.get();
break;
}
case Intention::acceleration: {
/* Limit acceleration linearly based on velocity setpoint.*/
- _acc_state_dependent = (_acc_xy_max - _dec_xy_min)
- / _vel_manual * vel_sp.length() + _dec_xy_min;
+ _acc_state_dependent = (_acc_xy_max.get() - _dec_xy_min.get())
+ / _vel_manual.get() * vel_sp.length() + _dec_xy_min.get();
break;
}
case Intention::deceleration: {
- _acc_state_dependent = _dec_xy_min;
+ _acc_state_dependent = _dec_xy_min.get();
break;
}
}
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
index b3d626ca3d..5307e3da23 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp
@@ -39,16 +39,17 @@
#pragma once
-#include <systemlib/param/param.h>
+#include <px4_module_params.h>
#include <matrix/matrix/math.hpp>
-class ManualSmoothingXY
+class ManualSmoothingXY : public ModuleParams
{
public:
- ManualSmoothingXY(const matrix::Vector2f &vel);
- ~ManualSmoothingXY() {};
+ ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel);
+ ~ManualSmoothingXY() = default;
- /* Smoothing of velocity setpoint horizontally based
+ /**
+ * Smoothing of velocity setpoint horizontally based
* on flight direction.
* @param vel_sp: velocity setpoint in xy
* @param dt: time delta in seconds
@@ -70,13 +71,22 @@ public:
/* Overwrite methods:
* Needed if different parameter values than default required.
*/
- void overwriteHoverAcceleration(float acc) {_acc_hover = acc;};
- void overwriteMaxAcceleration(float acc) {_acc_xy_max = acc;};
- void overwriteDecelerationMin(float dec) {_dec_xy_min = dec;};
- void overwriteJerkMax(float jerk) {_jerk_max = jerk;};
- void overwriteJerkMin(float jerk) {_jerk_min = jerk;};
+ void overwriteHoverAcceleration(float acc) { _acc_hover.set(acc); }
+ void overwriteMaxAcceleration(float acc) { _acc_xy_max.set(acc); }
+ void overwriteDecelerationMin(float dec) { _dec_xy_min.set(dec); }
+ void overwriteJerkMax(float jerk) { _jerk_max.set(jerk); }
+ void overwriteJerkMin(float jerk) { _jerk_min.set(jerk); }
private:
+ void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+ const float &yawrate_sp, const float dt);
+ Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+ const float &yawrate_sp);
+ void _getStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention,
+ const float dt);
+ void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt);
+ matrix::Vector2f _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw);
+ matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw);
/* User intention: brake or acceleration */
Intention _intention{Intention::acceleration};
@@ -90,32 +100,13 @@ private:
/* Previous setpoints */
matrix::Vector2f _vel_sp_prev{}; // previous velocity setpoint
- /* Params */
- param_t _acc_hover_h{PARAM_INVALID};
- param_t _acc_xy_max_h{PARAM_INVALID};
- param_t _dec_xy_min_h{PARAM_INVALID};
- param_t _jerk_max_h{PARAM_INVALID};
- param_t _jerk_min_h{PARAM_INVALID};
- param_t _vel_manual_h{PARAM_INVALID};
- float _acc_hover{50.0f}; // acceleration in hover
- float _acc_xy_max{10.0f}; // acceleration in flight
- float _dec_xy_min{1.0f}; // deceleration in flight
- float _jerk_max{15.0f}; // jerk max during brake
- float _jerk_min{1.0f}; // jerk min during brake
- float _vel_manual{}; //maximum velocity in manual controlled mode
- int _parameter_sub{-1};
-
- /* Helper methods */
- void _setParams();
- void _updateParams();
- void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
- const float &yawrate_sp, const float dt);
- Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
- const float &yawrate_sp);
- void _getStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention,
- const float dt);
- void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt);
- matrix::Vector2f _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw) ;
- matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw);
+ DEFINE_PARAMETERS(
+ (ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _acc_hover, ///< acceleration in hover
+ (ParamFloat<px4::params::MPC_ACC_HOR>) _acc_xy_max, ///< acceleration in flight
+ (ParamFloat<px4::params::MPC_DEC_HOR_SLOW>) _dec_xy_min, ///< deceleration in flight
+ (ParamFloat<px4::params::MPC_JERK_MIN>) _jerk_min, ///< jerk min during brake
+ (ParamFloat<px4::params::MPC_JERK_MAX>) _jerk_max, ///< jerk max during brake
+ (ParamFloat<px4::params::MPC_VEL_MANUAL>) _vel_manual ///< maximum velocity in manual controlled mode
+ )
};
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp
index ed72dc82d7..da98c06904 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp
@@ -40,15 +40,10 @@
#include <mathlib/mathlib.h>
#include <float.h>
-ManualSmoothingZ::ManualSmoothingZ(const float &vel, const float &stick) :
+ManualSmoothingZ::ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick) :
+ ModuleParams(parent),
_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");
- _jerk_max_h = param_find("MPC_JERK_MAX");
-
- /* Load the params the very first time */
- setParams();
}
/* in manual altitude control apply acceleration limit based on stick input
@@ -59,33 +54,11 @@ ManualSmoothingZ::ManualSmoothingZ(const float &vel, const float &stick) :
void
ManualSmoothingZ::smoothVelFromSticks(float &vel_sp, const float dt)
{
- updateParams();
updateAcceleration(vel_sp, dt);
velocitySlewRate(vel_sp, dt);
_vel_sp_prev = vel_sp;
}
-void
-ManualSmoothingZ::updateParams()
-{
- bool updated;
- parameter_update_s param_update;
- orb_check(_parameter_sub, &updated);
-
- if (updated) {
- orb_copy(ORB_ID(parameter_update), _parameter_sub, ¶m_update);
- setParams();
- }
-}
-
-void
-ManualSmoothingZ::setParams()
-{
- param_get(_acc_max_up_h, &_acc_max_up);
- param_get(_acc_max_down_h, &_acc_max_down);
- param_get(_jerk_max_h, &_jerk_max);
-}
-
void
ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
{
@@ -109,7 +82,7 @@ ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) {
/* we start with lowest acceleration */
- _acc_state_dependent = _acc_max_down;
+ _acc_state_dependent = _acc_max_down.get();
/* reset slew-rate: this ensures that there
* is no delay present when user demands to brake
@@ -123,13 +96,13 @@ ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
case ManualIntentionZ::brake: {
/* limit jerk when braking to zero */
- float jerk = (_acc_max_up - _acc_state_dependent) / dt;
+ float jerk = (_acc_max_up.get() - _acc_state_dependent) / dt;
- if (jerk > _jerk_max) {
- _acc_state_dependent = _jerk_max * dt + _acc_state_dependent;
+ if (jerk > _jerk_max.get()) {
+ _acc_state_dependent = _jerk_max.get() * dt + _acc_state_dependent;
} else {
- _acc_state_dependent = _acc_max_up;
+ _acc_state_dependent = _acc_max_up.get();
}
break;
@@ -137,8 +110,8 @@ ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
case ManualIntentionZ::acceleration: {
- _acc_state_dependent = (getMaxAcceleration() - _acc_max_down)
- * fabsf(_stick) + _acc_max_down;
+ _acc_state_dependent = (getMaxAcceleration() - _acc_max_down.get())
+ * fabsf(_stick) + _acc_max_down.get();
break;
}
}
@@ -153,11 +126,11 @@ ManualSmoothingZ::setMaxAcceleration()
if (_stick < -FLT_EPSILON) {
/* accelerating upward */
- _max_acceleration = _acc_max_up;
+ _max_acceleration = _acc_max_up.get();
} else if (_stick > FLT_EPSILON) {
/* accelerating downward */
- _max_acceleration = _acc_max_down;
+ _max_acceleration = _acc_max_down.get();
} else {
@@ -165,15 +138,15 @@ ManualSmoothingZ::setMaxAcceleration()
if (fabsf(_vel_sp_prev) < FLT_EPSILON) {
/* at rest */
- _max_acceleration = _acc_max_up;
+ _max_acceleration = _acc_max_up.get();
} else if (_vel_sp_prev < 0.0f) {
/* braking downward */
- _max_acceleration = _acc_max_down;
+ _max_acceleration = _acc_max_down.get();
} else {
/* braking upward */
- _max_acceleration = _acc_max_up;
+ _max_acceleration = _acc_max_up.get();
}
}
}
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp
index cc26fe59ac..a5d7c1da31 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp
@@ -39,7 +39,7 @@
#pragma once
-#include <systemlib/param/param.h>
+#include <px4_module_params.h>
/* User intention: brake or acceleration */
enum class ManualIntentionZ {
@@ -47,13 +47,14 @@ enum class ManualIntentionZ {
acceleration,
};
-class ManualSmoothingZ
+class ManualSmoothingZ : public ModuleParams
{
public:
- ManualSmoothingZ(const float &vel, const float &stick);
- ~ManualSmoothingZ() {};
+ ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick);
+ ~ManualSmoothingZ() = default;
- /* Smooths velocity setpoint based
+ /**
+ * 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
@@ -70,11 +71,14 @@ public:
/* 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;};
+ void overwriteAccelerationUp(float acc_max_up) { _acc_max_up.set(acc_max_up); }
+ void overwriteAccelerationDown(float acc_max_down) {_acc_max_down.set(acc_max_down); }
+ void overwriteJerkMax(float jerk_max) {_jerk_max.set(jerk_max); }
private:
+ void velocitySlewRate(float &vel_sp, const float dt);
+ void updateAcceleration(float &vel_sp, const float dt);
+ void setMaxAcceleration();
/* User intention: brake or acceleration */
ManualIntentionZ _intention{ManualIntentionZ::acceleration};
@@ -90,22 +94,11 @@ private:
*/
float _acc_state_dependent{0.0f};
float _vel_sp_prev;
- float _max_acceleration;
-
- /* 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};
- int _parameter_sub{-1};
-
- /* Helper methods */
- void velocitySlewRate(float &vel_sp, const float dt);
- void setParams();
- void updateParams();
- void updateAcceleration(float &vel_sp, const float dt);
- void setMaxAcceleration();
+ float _max_acceleration{0.f};
+ DEFINE_PARAMETERS(
+ (ParamFloat<px4::params::MPC_ACC_UP_MAX>) _acc_max_up,
+ (ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _acc_max_down,
+ (ParamFloat<px4::params::MPC_JERK_MAX>) _jerk_max
+ )
};
diff --git a/src/systemcmds/tests/test_smooth_z.cpp b/src/systemcmds/tests/test_smooth_z.cpp
index 09c47488ce..3e90c2dfc0 100644
--- a/src/systemcmds/tests/test_smooth_z.cpp
+++ b/src/systemcmds/tests/test_smooth_z.cpp
@@ -34,7 +34,7 @@ bool SmoothZTest::brakeUpward()
float acc_max_up = 5.0f;
float acc_max_down = 2.0f;
- ManualSmoothingZ smooth(vel, stick_current);
+ ManualSmoothingZ smooth(nullptr, vel, stick_current);
/* overwrite parameters since they might change depending on configuration */
smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
@@ -86,7 +86,7 @@ bool SmoothZTest::brakeDownward()
float acc_max_up = 5.0f;
float acc_max_down = 2.0f;
- ManualSmoothingZ smooth(vel, stick_current);
+ ManualSmoothingZ smooth(nullptr, vel, stick_current);
/* overwrite parameters since they might change depending on configuration */
smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
@@ -143,7 +143,7 @@ bool SmoothZTest::accelerateUpwardFromBrake()
float acc_max_up = 5.0f;
float acc_max_down = 2.0f;
- ManualSmoothingZ smooth(vel, stick_current);
+ ManualSmoothingZ smooth(nullptr, vel, stick_current);
/* overwrite parameters since they might change depending on configuration */
smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
@@ -192,7 +192,7 @@ bool SmoothZTest::accelerateDownwardFromBrake()
float acc_max_up = 5.0f;
float acc_max_down = 2.0f;
- ManualSmoothingZ smooth(vel, stick_current);
+ ManualSmoothingZ smooth(nullptr, vel, stick_current);
/* overwrite parameters since they might change depending on configuration */
smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
--
GitLab