Butchered position estimator from Damian Aregger into shape, publishes now global position estimate as well. Compiling, needs HIL testing

2026-10-30 21:43:34 +08:00 · 2013-03-23 22:39:54 +01:00
parent 0dc96dbd89
commit 482cada59b
56 changed files with 3475 additions and 1 deletions
@@ -0,0 +1,96 @@
+/*
+ * rtGetNaN.c
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ *       MATLAB for code generation function to initialize non-finite, NaN
+ */
+#include "rtGetNaN.h"
+#define NumBitsPerChar	8U
+
+/* Function: rtGetNaN ==================================================
+ * Abstract:
+ * Initialize rtNaN needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetNaN(void)
+{
+  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+  real_T nan = 0.0;
+  if (bitsPerReal == 32U) {
+    nan = rtGetNaNF();
+  } else {
+  uint16_T one = 1U;
+  enum {
+    LittleEndian,
+    BigEndian
+  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+  switch (machByteOrder) {
+   case LittleEndian:
+    {
+      union {
+        LittleEndianIEEEDouble bitVal;
+        real_T fltVal;
+      } tmpVal;
+
+      tmpVal.bitVal.words.wordH = 0xFFF80000U;
+      tmpVal.bitVal.words.wordL = 0x00000000U;
+      nan = tmpVal.fltVal;
+      break;
+    }
+
+   case BigEndian:
+    {
+        union {
+          BigEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
+        tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
+        nan = tmpVal.fltVal;
+        break;
+      }
+    }
+  }
+
+  return nan;
+}
+
+/* Function: rtGetNaNF ==================================================
+ * Abstract:
+ * Initialize rtNaNF needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetNaNF(void)
+{
+  IEEESingle nanF = { { 0 } };
+  uint16_T one = 1U;
+  enum {
+    LittleEndian,
+    BigEndian
+  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+  switch (machByteOrder) {
+   case LittleEndian:
+    {
+      nanF.wordL.wordLuint = 0xFFC00000U;
+      break;
+    }
+
+   case BigEndian:
+    {
+      nanF.wordL.wordLuint = 0x7FFFFFFFU;
+      break;
+    }
+  }
+
+  return nanF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetNaN.c) */