dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

RateControl: allow setting individual saturation flags 

This helps for more complicated cases where certain axes are controlled
through and get feedback from a different allocator.
This commit is contained in:
Matthias Grob 2023-06-14 18:42:58 +02:00 committed by Silvan Fuhrer
parent 11436f4109
commit ce8c4094a8
2 changed files with 26 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/lib/rate_control/rate_control.cpp
View File

@ -47,13 +47,27 @@ void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f
_gain_d = D;
}
void RateControl::setSaturationStatus(const Vector<bool, 3> &saturation_positive,
const Vector<bool, 3> &saturation_negative)
void RateControl::setSaturationStatus(const Vector3<bool> &saturation_positive,
const Vector3<bool> &saturation_negative)
{
_control_allocator_saturation_positive = saturation_positive;
_control_allocator_saturation_negative = saturation_negative;
}
void RateControl::setPositiveSaturationFlag(size_t axis, bool is_saturated)
{
if (axis < 3) {
_control_allocator_saturation_positive(axis) = is_saturated;
}
}
void RateControl::setNegativeSaturationFlag(size_t axis, bool is_saturated)
{
if (axis < 3) {
_control_allocator_saturation_negative(axis) = is_saturated;
}
}
Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel,
const float dt, const bool landed)
{

12
src/lib/rate_control/rate_control.hpp
View File

@ -75,8 +75,16 @@ public:
* Set saturation status
* @param control saturation vector from control allocator
*/
void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
const matrix::Vector<bool, 3> &saturation_negative);
void setSaturationStatus(const matrix::Vector3<bool> &saturation_positive,
const matrix::Vector3<bool> &saturation_negative);
/**
* Set individual saturation flags
* @param axis 0 roll, 1 pitch, 2 yaw
* @param is_saturated value to update the flag with
*/
void setPositiveSaturationFlag(size_t axis, bool is_saturated);
void setNegativeSaturationFlag(size_t axis, bool is_saturated);
/**
* Run one control loop cycle calculation