new release for OpenIMU330RI

examples/OpenIMU300RI/IMU/lib/Core/.piopm

@@ -0,0 +1 @@
+{"type": "library", "name": "Core", "version": "1.0.4", "spec": {"owner": null, "id": null, "name": "Lib", "requirements": null, "url": "file://../../../Lib"}}

examples/OpenIMU300RI/IMU/lib/Core/Algorithm/algorithmAPI.h

@@ -36,7 +36,7 @@ limitations under the License.
  *
  * @retval N/A
  *****************************************************************************/
-void InitializeAlgorithmStruct(uint8_t callingFreq);
+void InitializeAlgorithmStruct(uint8_t callingFreq, const enumIMUType imuType);
 
 /******************************************************************************
  * @brief Get algorithm status.
@@ -93,6 +93,11 @@ uint32_t getAlgorithmTimer();
 uint16_t getAlgorithmCounter();
 uint16_t getAlgorithmFrequency();
 uint32_t getAlgorithmITOW();
+BOOL     getAlgorithmLinAccelDetectMode();
+BOOL     getAlgorithmAccelPredictMode();
+float    getAlgorithmCoefOfReduceQ();
+float    getAlgorithmAccelSwitchDelay();
+float    getAlgorithmRateIntegrationTime();    
 
 
 /******************************************************************************

examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/EKF_Algorithm.h

@@ -172,6 +172,7 @@ void EKF_GetOperationalSwitches(uint8_t *EKF_LinAccelSwitch, uint8_t *EKF_TurnSw
 void EKF_SetInputStruct(double *accels, double *rates, double *mags,
                         gpsDataStruct_t *gps, odoDataStruct_t *odo,
                         BOOL ppsDetected);
+void EKF_SetSteeringAngle(real angle, uint16_t states);
 void EKF_SetOutputStruct(void);
 
 #endif /* _EKF_ALGORITHM_H_ */

examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/MotionStatus.h

@@ -50,10 +50,11 @@ void MotionStatusImu(real *gyro, real *accel, ImuStatsStruct *imuStats, BOOL res
  * @param [in] staticDelay      A Counter. When static period detected, delay [staticDelay] samples before
  *                              lowering accel error. [staticDelay] is also used to reset initial accel that
  *                              is propagated using gyro to estimate future accel.
+ * @param [in] rateBias         Input angular rate bias, rad/s.
  * @param [out] gAccelStats     A struct for results storage.
  * @retval None.
 ******************************************************************************/
-void EstimateAccelError(real *accel, real *w, real dt, uint32_t staticDelay, ImuStatsStruct *gAccelStats);
+void EstimateAccelError(real *accel, real *w, real dt, uint32_t staticDelay, real *rateBias, ImuStatsStruct *imuStats);
 
 /******************************************************************************
  * @brief Detect motion according to the difference between measured accel magnitude and 1g.

examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/algorithm.h

@@ -80,7 +80,7 @@ typedef struct {
 
 	real accelSwitch;
 	uint32_t linAccelSwitchDelay;
-
+	uint32_t rateIntegrationTime;
 	InnovationStruct       Innov;
 } LimitStruct;
 
@@ -192,6 +192,8 @@ typedef struct {
 
     uint8_t linAccelLPFType;
     uint8_t useRawAccToDetectLinAccel;
+    uint8_t useRawRateToPredAccel;
+    real coefOfReduceQ;
 
     uint8_t callingFreq;
     real    dt;
@@ -215,6 +217,8 @@ typedef struct {
 
     DurationStruct    Duration;
     LimitStruct       Limit;
+    enumIMUType       imuType;
+
 } AlgorithmStruct;
 
 extern AlgorithmStruct gAlgorithm;

examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/EKF_Algorithm.c

@@ -94,6 +94,7 @@ void EKF_Algorithm(void)
                                 gEKFInput.angRate_B,
                                 gAlgorithm.dt,
                                 gAlgorithm.Limit.linAccelSwitchDelay,
+                                gKalmanFilter.rateBias_B,
                                 &gImuStats);
         }
         else
@@ -102,6 +103,7 @@ void EKF_Algorithm(void)
                                 gEKFInput.angRate_B,
                                 gAlgorithm.dt,
                                 gAlgorithm.Limit.linAccelSwitchDelay,
+                                gKalmanFilter.rateBias_B,
                                 &gImuStats);
         }
 

examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/MotionStatus.c

@@ -463,7 +463,7 @@ static void LowPassFilter(real *in, Buffer *bfIn, Buffer *bfOut, real *b, real *
     bfPut(bfIn, in);
 }
 
-void EstimateAccelError(real *accel, real *w, real dt, uint32_t staticDelay, ImuStatsStruct *imuStats)
+void EstimateAccelError(real *accel, real *w, real dt, uint32_t staticDelay, real *rateBias, ImuStatsStruct *imuStats)
 {
     static BOOL bIni = false;               // indicate if the procedure is initialized
     static real lastAccel[3];               // accel input of last step
@@ -504,11 +504,20 @@ void EstimateAccelError(real *accel, real *w, real dt, uint32_t staticDelay, Imu
         lastEstimatedAccel[2] = lastAccel[2];
     }
     counter++;
-    if (counter == staticDelay)
+    if (counter == gAlgorithm.Limit.rateIntegrationTime )
     {
         counter = 0;
     }
 
+    //use corrected rate to predict acceleration
+    if(!gAlgorithm.useRawRateToPredAccel)
+    {
+        // Remove rate bias from raw rate sensor data.
+        lastGyro[X_AXIS] -= rateBias[X_AXIS];
+        lastGyro[Y_AXIS] -= rateBias[Y_AXIS];
+        lastGyro[Z_AXIS] -= rateBias[Z_AXIS];
+    }
+
     // propagate accel using gyro
     //  a(k) = a(k-1) -w x a(k-1)*dt
     real ae[3];
@@ -639,4 +648,4 @@ BOOL DetectStaticGnssVelocity(double *vNED, real threshold, BOOL gnssValid)
 BOOL DetectStaticOdo(real odo)
 {
     return FALSE;
-}
+}

examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/PredictFunctions.c

@@ -732,9 +732,9 @@ static void _UpdateProcessCovariance(void)
              * drive-test).  Note: this is only called upon the first-entry
              * into low-gain mode.
              */
-            /*gKalmanFilter.Q[STATE_WBX] = (real)1.0e-3 * gKalmanFilter.Q[STATE_WBX];
-            gKalmanFilter.Q[STATE_WBY] = (real)1.0e-3 * gKalmanFilter.Q[STATE_WBY];
-            gKalmanFilter.Q[STATE_WBZ] = (real)1.0e-3 * gKalmanFilter.Q[STATE_WBZ];*/
+            gKalmanFilter.Q[STATE_WBX] = gAlgorithm.coefOfReduceQ * gKalmanFilter.Q[STATE_WBX];
+            gKalmanFilter.Q[STATE_WBY] = gAlgorithm.coefOfReduceQ * gKalmanFilter.Q[STATE_WBY];
+            gKalmanFilter.Q[STATE_WBZ] = gAlgorithm.coefOfReduceQ * gKalmanFilter.Q[STATE_WBZ];
         }
     }
 

examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/UpdateFunctions.c

@@ -577,7 +577,7 @@ void _GenerateObservationCovariance_AHRS(void)
      * After accel in the algorithm is changed to [m/s/s],
      * 40000*Rnom(g^2) = 40000*Rnom([m/s/s]^2)/gravity/gravity = 400*Rnom([m/s/s]^2)
      */
-    real maxR = 400.0f * Rnom;
+    real maxR = 4.0f * Rnom;
     if (gKalmanFilter.R[STATE_ROLL] > maxR)
     {
         gKalmanFilter.R[STATE_ROLL] = maxR;

examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/algorithm.c

@@ -27,8 +27,17 @@ AlgorithmStruct gAlgorithm;
 AlgoStatus      gAlgoStatus;
 
 
-void InitializeAlgorithmStruct(uint8_t callingFreq)
+void InitializeAlgorithmStruct(uint8_t callingFreq, const enumIMUType imuTypeIn)
 {
+
+#ifndef IMU_ONLY
+    enumIMUType imuType = imuTypeIn;
+
+    if(imuType == CurrentIMU){
+        // Reuse previously initialized IMU type
+        imuType = gAlgorithm.imuType;
+    }
+
     memset(&gAlgorithm, 0, sizeof(AlgorithmStruct));
 
     //----------------------------algortihm config-----------------------------
@@ -59,7 +68,7 @@ void InitializeAlgorithmStruct(uint8_t callingFreq)
     // Set up other timing variables
     gAlgorithm.dtOverTwo = (real)(0.5) * gAlgorithm.dt;
     gAlgorithm.dtSquared = gAlgorithm.dt * gAlgorithm.dt;
-    gAlgorithm.sqrtDt = sqrtf(gAlgorithm.dt);
+    gAlgorithm.sqrtDt    = sqrtf(gAlgorithm.dt);
 
     // Set the algorithm duration periods
     gAlgorithm.Duration.Stabilize_System = (uint32_t)(gAlgorithm.callingFreq * STABILIZE_SYSTEM_DURATION);
@@ -97,8 +106,11 @@ void InitializeAlgorithmStruct(uint8_t callingFreq)
     gAlgorithm.Limit.Free_Integration_Cntr = gAlgorithm.callingFreq * LIMIT_FREE_INTEGRATION_CNTR;
 
     // Linear acceleration switch limits (level and time)
-    gAlgorithm.Limit.accelSwitch = (real)(0.012);   // [g]
-    gAlgorithm.Limit.linAccelSwitchDelay = (uint32_t)(2.0 * gAlgorithm.callingFreq);
+    gAlgorithm.Limit.accelSwitch         = (real)(0.012);   // [g]
+    float tmp = getAlgorithmAccelSwitchDelay();
+    gAlgorithm.Limit.linAccelSwitchDelay  = (uint32_t)(tmp * gAlgorithm.callingFreq);
+    tmp = getAlgorithmRateIntegrationTime();
+    gAlgorithm.Limit.rateIntegrationTime  = (uint32_t)(tmp * gAlgorithm.callingFreq);
 
     // Innovation error limits for EKF states
     gAlgorithm.Limit.Innov.positionError = (real)270.0;
@@ -113,7 +125,18 @@ void InitializeAlgorithmStruct(uint8_t callingFreq)
     // Uing raw accel to detect linear acceleration has lower failure rate in small
     //  and smooth linear acceleration. But on some platform, there is large vibration,
     //  uing raw accel to detect linear acceleration will always detect linear accel.
-    gAlgorithm.useRawAccToDetectLinAccel = TRUE;
+    gAlgorithm.useRawAccToDetectLinAccel = getAlgorithmLinAccelDetectMode();    // TRUE: raw accel, FALSE: filtered accel.
+
+    // The gyro data normally has just a small bias after factory calibration 
+    // and the accuracy is good enough to detect linear acceleration. 
+    // However, the gyro_x with a big bias, and the rate integration time default
+    // setting of 2 seconds combined to not be accurate enough predicting the 
+    // next acceleration measurement. So in most of situations, 
+    // corrected rate should be used to predict next accel.
+    gAlgorithm.useRawRateToPredAccel     = getAlgorithmAccelPredictMode();   // FALSE: corrected rate, TRUE: raw rate.
+
+    // Coefficient of reducing Q.
+    gAlgorithm.coefOfReduceQ             = getAlgorithmCoefOfReduceQ();
 
     // Set the turn-switch threshold to a default value in [deg/sec]
     gAlgorithm.turnSwitchThreshold = 6.0;
@@ -130,6 +153,29 @@ void InitializeAlgorithmStruct(uint8_t callingFreq)
     gAlgorithm.velocityAlwaysAlongBodyX = TRUE;
 
     // get IMU specifications
+    switch (imuType)
+    {
+    case OpenIMU330:
+    case OpenIMU335RI:
+        {
+            //0.2deg/sqrt(Hr) = 0.2 / 60 * D2R = 5.8177640741e-05rad/sqrt(s)
+            gAlgorithm.imuSpec.arw      = (real)5.82e-5; 
+            gAlgorithm.imuSpec.sigmaW   = (real)(1.25 * 5.82e-5 / sqrt(1.0/RW_ODR));
+            //1.5deg/Hr = 1.5 / 3600 * D2R = 7.272205093e-06rad/s
+            gAlgorithm.imuSpec.biW      = (real)7.27e-6;
+            gAlgorithm.imuSpec.maxBiasW = (real)MAX_BW;
+            //0.04m/s/sqrt(Hr) = 0.04 / 60 = 6.67e-04 m/s/sqrt(s)
+            gAlgorithm.imuSpec.vrw      = (real)6.67e-04;
+            gAlgorithm.imuSpec.sigmaA   = (real)(1.25 * 6.67e-04 / sqrt(1.0/RW_ODR));
+             //20ug = 20.0e-6g * GRAVITY
+            gAlgorithm.imuSpec.biA      = (real)(20.0e-6 * GRAVITY);
+            gAlgorithm.imuSpec.maxBiasA = (real)MAX_BA;            
+        }
+        break;
+    case OpenIMU300ZI:
+    case OpenIMU300RI:
+    default:
+        {
     gAlgorithm.imuSpec.arw = (real)ARW_300ZA;
     gAlgorithm.imuSpec.sigmaW = (real)(1.25 * ARW_300ZA / sqrt(1.0/RW_ODR));
     gAlgorithm.imuSpec.biW = (real)BIW_300ZA;
@@ -138,6 +184,9 @@ void InitializeAlgorithmStruct(uint8_t callingFreq)
     gAlgorithm.imuSpec.sigmaA = (real)(1.25 * VRW_300ZA / sqrt(1.0/RW_ODR));
     gAlgorithm.imuSpec.biA = (real)BIA_300ZA;
     gAlgorithm.imuSpec.maxBiasA = (real)MAX_BA;
+        }
+        break;
+    }
 
     // default noise level multiplier for static detection
     gAlgorithm.staticDetectParam.staticVarGyro = (real)(gAlgorithm.imuSpec.sigmaW * gAlgorithm.imuSpec.sigmaW);
@@ -152,6 +201,8 @@ void InitializeAlgorithmStruct(uint8_t callingFreq)
 
     //----------------------------algorithm states-----------------------------
     memset(&gAlgoStatus, 0, sizeof(gAlgoStatus));
+#endif
+
 }
 
 void GetAlgoStatus(AlgoStatus *algoStatus)

examples/OpenIMU300RI/IMU/lib/Core/Common/include/GlobalConstants.h

@@ -149,6 +149,14 @@ typedef enum{
 	UNKNOWN                 = 0xFF
 } enumGPSProtocol;
 
+// Choices for IMU type
+typedef enum{
+	CurrentIMU              = -1,
+	OpenIMU300RI            =  0,
+	OpenIMU300ZI            =  1,
+	OpenIMU330              =  2,
+	OpenIMU335RI            =  3
+} enumIMUType;
 
 // Algorithm specifiers
 #define  IMU   0

examples/OpenIMU300RI/IMU/lib/Core/Common/include/ucb_packet_struct.h

@@ -64,6 +64,9 @@ typedef struct {
     uint8_t         packetCode[2];
 }usr_packet_t;
 
+#define SYSTEM_TYPE_MASTER   0
+#define SYSTEM_TYPE_SLAVE    1
+
 
 typedef struct {
      uint8_t       packetType;      // 0

examples/OpenIMU300RI/IMU/lib/Core/GPS/gpsAPI.h

@@ -92,5 +92,7 @@ extern gpsDataStruct_t gGPS, gCanGps;
 void  GetGPSData(gpsDataStruct_t *data);
 BOOL  SetGpsBaudRate(int rate, int fApply);
 BOOL  SetGpsProtocol(int protocol, int fApply);
+void  ProcGps();
+void  InitGpsSerialCommunication(int baudrate, BOOL fInit);
 
 #endif /* GPS_API_H */