FailureDetector - New class, first working simple implementation. Outputs PX4_ERR messages if roll or pitch is exceeded.

src/modules/commander/Commander.cpp

@@ -1425,6 +1425,7 @@ Commander::run()
 			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
 
 			updateParams();
+			_failure_detector.update_params();
 
 			/* update parameters */
 			if (!armed.armed) {
@@ -2180,6 +2181,16 @@ Commander::run()
 			}
 		}
 
+		if (_failure_detector.update()) {
+			const auto _failure_status = _failure_detector.get();
+			if (_failure_status.roll) {
+				PX4_ERR("Roll angle exceeded");
+			}
+			if (_failure_status.pitch) {
+				PX4_ERR("Pitch angle exceeded");
+			}
+		}
+
 		/* Reset main state to loiter or auto-mission after takeoff is completed.
 		 * Sometimes, the mission result topic is outdated and the mission is still signaled
 		 * as finished even though we only just started with the takeoff. Therefore, we also