fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 11:03: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/lib/motion_planning/PositionSmoothing.cpp
+3 -4
View File
@@ -172,8 +172,8 @@ const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &posi
// If a velocity is specified, that is used as a feedforward to track the position setpoint
// (ie. it assumes the position setpoint is moving at the specified velocity)
// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
auto &target = waypoints[1];
const bool xy_target_valid = PX4_ISFINITE(target(0)) && PX4_ISFINITE(target(1));
const Vector3f &target = waypoints[1];
const bool xy_target_valid = Vector2f(target).isAllFinite();
const bool z_target_valid = PX4_ISFINITE(target(2));
Vector3f velocity_setpoint = feedforward_velocity_setpoint;
@@ -269,8 +269,7 @@ void PositionSmoothing::_generateTrajectory(
float delta_time,
PositionSmoothingSetpoints &out_setpoints)
{
if (!PX4_ISFINITE(velocity_setpoint(0)) || !PX4_ISFINITE(velocity_setpoint(1))
|| !PX4_ISFINITE(velocity_setpoint(2))) {
if (!velocity_setpoint.isAllFinite()) {
return;
}