Skip to content
Snippets Groups Projects
Commit 5eb7d8ec authored by Beat Küng's avatar Beat Küng
Browse files

flight tasks smoothing: refactor param_find -> Param

Plus some style fixes
parent 398d511f
No related branches found
No related tags found
No related merge requests found
......@@ -39,8 +39,8 @@
using namespace matrix;
_smoothing(_velocity(2), _sticks(2))
FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth() :
_smoothing(this, _velocity(2), _sticks(2))
{}
void FlightTaskManualAltitudeSmooth::_updateSetpoints()
......
......@@ -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()
......
......@@ -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, &param_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;
}
}
......
......@@ -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
)
};
......@@ -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, &param_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();
}
}
}
......
......@@ -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
)
};
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment