ControlMath: add addIfNotNan helper functions

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -215,4 +215,32 @@ bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r,
 		return false;
 	}
 }
+
+void addIfNotNan(float &setpoint, const float addition)
+{
+	if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(addition)) {
+		// No NAN, add to the setpoint
+		setpoint += addition;
+
+	} else if (!PX4_ISFINITE(setpoint)) {
+		// Setpoint NAN, take addition
+		setpoint = addition;
+	}
+
+	// Addition is NAN or both are NAN, nothing to do
 }
+
+void addIfNotNanVector3f(Vector3f &setpoint, const Vector3f &addition)
+{
+	for (int i = 0; i < 3; i++) {
+		addIfNotNan(setpoint(i), addition(i));
+	}
+}
+
+void setZeroIfNanVector3f(Vector3f &vector)
+{
+	// Adding zero vector overwrites elements that are NaN with zero
+	addIfNotNanVector3f(vector, Vector3f());
+}
+
+} // ControlMath

src/modules/mc_pos_control/PositionControl/ControlMath.hpp

@@ -86,4 +86,24 @@ matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f
  */
 bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r, const matrix::Vector3f &line_a,
 		       const matrix::Vector3f &line_b, matrix::Vector3f &res);
+
+/**
+ * Adds e.g. feed-forward to the setpoint making sure existing or added NANs have no influence on control.
+ * This function is udeful to support all the different setpoint combinations of position, velocity, acceleration with NAN representing an uncommited value.
+ * @param setpoint existing possibly NAN setpoint to add to
+ * @param addition value/NAN to add to the setpoint
+ */
+void addIfNotNan(float &setpoint, const float addition);
+
+/**
+ * _addIfNotNan for Vector3f treating each element individually
+ * @see _addIfNotNan
+ */
+void addIfNotNanVector3f(matrix::Vector3f &setpoint, const matrix::Vector3f &addition);
+
+/**
+ * Overwrites elements of a Vector3f which are NaN with zero
+ * @param vector possibly containing NAN elements
+ */
+void setZeroIfNanVector3f(matrix::Vector3f &vector);
 }

src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp

@@ -177,3 +177,21 @@ TEST(ControlMathTest, CrossSphereLine)
 	EXPECT_FALSE(retval);
 	EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f));
 }
+
+TEST(ControlMathTest, addIfNotNan)
+{
+	float v = 1.f;
+	// regular addition
+	ControlMath::addIfNotNan(v, 2.f);
+	EXPECT_EQ(v, 3.f);
+	// addition is NAN and has no influence
+	ControlMath::addIfNotNan(v, NAN);
+	EXPECT_EQ(v, 3.f);
+	v = NAN;
+	// both summands are NAN
+	ControlMath::addIfNotNan(v, NAN);
+	EXPECT_TRUE(isnan(v));
+	// regular value gets added to NAN and overwrites it
+	ControlMath::addIfNotNan(v, 3.f);
+	EXPECT_EQ(v, 3.f);
+}