fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 15:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use isAllFinite() in all places that check finiteness on entire vectors or matrices

Browse Source
This commit is contained in:
Matthias Grob
2022-10-19 19:09:20 +02:00
parent 93de9567a5
commit 5ca28dd6dc
38 changed files with 128 additions and 252 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+2 -2
View File
@@ -177,7 +177,7 @@ void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration)
void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt)
{
const bool moving = _velocity_setpoint.norm_squared() > FLT_EPSILON;
const bool position_locked = PX4_ISFINITE(_position_setpoint(0)) || PX4_ISFINITE(_position_setpoint(1));
const bool position_locked = Vector2f(_position_setpoint).isAllFinite();
// lock position
if (!moving && !position_locked) {
@@ -189,7 +189,7 @@ void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector
_position_setpoint.setNaN();
// avoid velocity setpoint jump caused by ignoring remaining position error
if (PX4_ISFINITE(vel_sp_feedback(0)) && PX4_ISFINITE(vel_sp_feedback(1))) {
if (vel_sp_feedback.isAllFinite()) {
_velocity_setpoint = vel_sp_feedback;
}
}
src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
+1 -6
View File
@@ -64,12 +64,7 @@ bool Sticks::checkAndUpdateStickInputs()
_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
// valid stick inputs are required
const bool valid_sticks = PX4_ISFINITE(_positions(0))
&& PX4_ISFINITE(_positions(1))
&& PX4_ISFINITE(_positions(2))
&& PX4_ISFINITE(_positions(3));
_input_available = valid_sticks;
_input_available = _positions.isAllFinite();
} else {
failsafe_flags_s failsafe_flags;