support EF3Lite

2020-09-23 17:27:03 +08:00
parent a9a1664902
commit e8940d1a31
26 changed files with 3052 additions and 0 deletions
@@ -0,0 +1,119 @@
+
+/*
+ * Include Files
+ *
+ */
+#if defined(MATLAB_MEX_FILE)
+#include "tmwtypes.h"
+#include "simstruc_types.h"
+#else
+#include "rtwtypes.h"
+#endif
+
+#include "hal_api.h"
+
+
+/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
+#ifdef HAL_IMPL
+/* TODO include files */
+#include "fuckQvar.h"
+uint32_t g_ins_seq = 0;
+
+#endif
+#include <stdint.h>
+/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
+#define y_width 1
+
+/*
+ * Create external references here.  
+ *
+ */
+/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
+int ins_inited[1];
+
+void initialize_ins(uint8_t id)
+{
+  switch (id)
+  {
+  case 0:
+    if (!ins_inited[0])
+    {
+#ifdef HAL_IMPL
+      // TODO initialize GPS No.0
+      ins_inited[0] = 1;
+#endif
+    }
+    break;
+  default:
+    break;
+  }
+}
+/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
+
+/*
+ * Start function
+ *
+ */
+void hal_ins_Start_wrapper(const uint8_T *id, const int_T p_width0)
+{
+/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
+initialize_ins(id[0]);
+/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
+}
+/*
+ * Output function
+ *
+ */
+void hal_ins_Outputs_wrapper(HAL_INS_SI_t *ins,
+			int32_T *ErrorCode,
+			const uint8_T *id, const int_T p_width0)
+{
+/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
+ErrorCode[0] = -1;
+  switch (id[0])
+  {
+  case 0:
+    if (ins_inited[0])
+    {
+#ifdef HAL_IMPL
+      // TODO read pressure
+      ins->latitude = Pack_EKF[EKF_for_FC].alldata.gps_latti;  ///< Unit is degree
+      ins->longitude = Pack_EKF[EKF_for_FC].alldata.gps_longi;
+      ins->altitude = Pack_EKF[EKF_for_FC].alldata.gps_high;
+
+      ins->vel_north = Pack_EKF[EKF_for_FC].xEKF[3];  ///< Unit is meter per second, in NED axes
+      ins->vel_east = Pack_EKF[EKF_for_FC].xEKF[4];
+      ins->vel_down = Pack_EKF[EKF_for_FC].xEKF[5];
+
+      ins->roll = Pack_EKF[EKF_for_FC].alldata.true_roll/ deg;  ///< Unit is radian
+      ins->pitch = Pack_EKF[EKF_for_FC].alldata.true_pitch/ deg;
+      ins->yaw = Pack_EKF[EKF_for_FC].alldata.true_yaw / deg;
+
+      ins->seq = g_ins_seq;
+
+      /** ÏÂÃæÕâ¼¸Ïî´ýÊµÏÖ */
+      ins->status = 0xff;  ///< working status
+                           ///< bit0 - attitude valid
+                           ///< bit1 - speed valid
+                           ///< bit2 - position valid
+                           ///< bit7 - calibration
+
+      ins->integration_status = 0xff; ///< integration method
+                                      ///< 0 - pure Inertial mode
+                                      ///< bit0(1) - GPS assist
+                                      ///< bit1(2) - baro assist
+                                      ///< bit2(4) - radar assist
+      ins->BIT = 0;   ///< health status 0-health; otherwise-unhealth.
+
+
+      ErrorCode[0] = 0;
+#endif
+    }
+    break;
+  default:
+    break;
+  }
+/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
+}
+
+