fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 01:20:34 +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-18 16:57:01 +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/fw_pos_control_l1/FixedwingPositionControl.cpp
+3 -6
View File
@@ -2135,8 +2135,7 @@ FixedwingPositionControl::Run()
_pos_sp_triplet.current.lon = static_cast<double>(NAN);
_pos_sp_triplet.current.alt = NAN;
if (PX4_ISFINITE(trajectory_setpoint.position[0]) && PX4_ISFINITE(trajectory_setpoint.position[1])
&& PX4_ISFINITE(trajectory_setpoint.position[2])) {
if (Vector3f(trajectory_setpoint.position).isAllFinite()) {
if (_global_local_proj_ref.isInitialized()) {
double lat;
double lon;
@@ -2150,16 +2149,14 @@ FixedwingPositionControl::Run()
}
if (PX4_ISFINITE(trajectory_setpoint.velocity[0]) && PX4_ISFINITE(trajectory_setpoint.velocity[1])
&& PX4_ISFINITE(trajectory_setpoint.velocity[2])) {
if (Vector3f(trajectory_setpoint.velocity).isAllFinite()) {
valid_setpoint = true;
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_pos_sp_triplet.current.vx = trajectory_setpoint.velocity[0];
_pos_sp_triplet.current.vy = trajectory_setpoint.velocity[1];
_pos_sp_triplet.current.vz = trajectory_setpoint.velocity[2];
if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1])
&& PX4_ISFINITE(trajectory_setpoint.acceleration[2])) {
if (Vector3f(trajectory_setpoint.acceleration).isAllFinite()) {
Vector2f velocity_sp_2d(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
Vector2f normalized_velocity_sp_2d = velocity_sp_2d.normalized();
Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]);