diff --git a/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/driverGPS.h b/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/driverGPS.h index c7681a8..552c24b 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/driverGPS.h +++ b/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/driverGPS.h @@ -218,8 +218,8 @@ typedef struct { int lon_deg; int lon_min; double lon_min_fraction; - char latSign; - char lonSign; + int8_t latSign; + int8_t lonSign; } NmeaLatLonSTRUCT; // delta struct diff --git a/examples/OpenIMU300RI/IMU/platformio.ini b/examples/OpenIMU300RI/IMU/platformio.ini index 452378f..6935b53 100644 --- a/examples/OpenIMU300RI/IMU/platformio.ini +++ b/examples/OpenIMU300RI/IMU/platformio.ini @@ -15,7 +15,7 @@ platform = aceinna_imu lib_archive = false board = OpenIMU300 ;lib_deps = ../../../openIMU300-lib -lib_deps = OpenIMU300-base-library@~1.1.11 +lib_deps = OpenIMU300-base-library@1.1.13 build_flags = ; -D CLI -D IMU_ONLY diff --git a/examples/OpenIMU300RI/IMU/src/appVersion.h b/examples/OpenIMU300RI/IMU/src/appVersion.h index 734abdd..6f23af3 100644 --- a/examples/OpenIMU300RI/IMU/src/appVersion.h +++ b/examples/OpenIMU300RI/IMU/src/appVersion.h @@ -26,7 +26,7 @@ limitations under the License. #ifndef _IMU_APP_VERSION_H #define _IMU_APP_VERSION_H -#define APP_VERSION_STRING "IMU_J1939 03.01.09" +#define APP_VERSION_STRING "OpenIMU300RI IMU_J1939 03.01.10" #endif diff --git a/examples/OpenIMU300RI/IMU/src/user/UserConfigurationUart.c b/examples/OpenIMU300RI/IMU/src/user/UserConfigurationUart.c index 4152c3b..24aee9b 100644 --- a/examples/OpenIMU300RI/IMU/src/user/UserConfigurationUart.c +++ b/examples/OpenIMU300RI/IMU/src/user/UserConfigurationUart.c @@ -50,6 +50,7 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) { BOOL result = TRUE; uint64_t *ptr = (uint64_t *)pUserUartConfig; + uint16_t orientOut; if(number < USER_UART_CRC || number >= USER_UART_MAX_PARAM ){ return FALSE; @@ -89,9 +90,9 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) case USER_UART_ORIENTATION: { uint64_t tmp = data; - result = platformSetOrientation((uint16_t*)&data, fApply); + result = platformSetOrientation((uint16_t*)&data, &orientOut, fApply); if(fApply && result == TRUE){ - UpdateEcuOrientationSettings((uint16_t)data); + UpdateEcuOrientationSettings(orientOut); data = tmp; } } diff --git a/examples/OpenIMU300RI/INS/platformio.ini b/examples/OpenIMU300RI/INS/platformio.ini index 2294490..ce10484 100644 --- a/examples/OpenIMU300RI/INS/platformio.ini +++ b/examples/OpenIMU300RI/INS/platformio.ini @@ -15,7 +15,7 @@ platform = aceinna_imu lib_archive = false board = OpenIMU300 ;lib_deps = ../../../openIMU300-lib -lib_deps = OpenIMU300-base-library@~1.1.11 +lib_deps = OpenIMU300-base-library@1.1.13 build_flags = -D GPS -D GPS_OVER_CAN diff --git a/examples/OpenIMU300RI/INS/src/appVersion.h b/examples/OpenIMU300RI/INS/src/appVersion.h index 49c548f..8bb91da 100644 --- a/examples/OpenIMU300RI/INS/src/appVersion.h +++ b/examples/OpenIMU300RI/INS/src/appVersion.h @@ -26,7 +26,7 @@ limitations under the License. #ifndef _IMU_APP_VERSION_H #define _IMU_APP_VERSION_H -#define APP_VERSION_STRING "INS_J1939 03.01.09" +#define APP_VERSION_STRING "OpenIMU300RI INS_J1939 03.01.10" #endif diff --git a/examples/OpenIMU300RI/INS/src/user/UserConfigurationUart.c b/examples/OpenIMU300RI/INS/src/user/UserConfigurationUart.c index 7ee463d..21895cb 100644 --- a/examples/OpenIMU300RI/INS/src/user/UserConfigurationUart.c +++ b/examples/OpenIMU300RI/INS/src/user/UserConfigurationUart.c @@ -50,7 +50,7 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) { BOOL result = TRUE; uint64_t *ptr = (uint64_t *)pUserUartConfig; - + uint16_t orientOut; if(number < USER_UART_CRC || number >= USER_UART_MAX_PARAM ){ return FALSE; } @@ -89,9 +89,9 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) case USER_UART_ORIENTATION: { uint64_t tmp = data; - result = platformSetOrientation((uint16_t*)&data, fApply); + result = platformSetOrientation((uint16_t*)&data, &orientOut, fApply); if(fApply && result == TRUE){ - UpdateEcuOrientationSettings((uint16_t)data); + UpdateEcuOrientationSettings(orientOut); data = tmp; } } diff --git a/examples/OpenIMU300RI/VG_AHRS/platformio.ini b/examples/OpenIMU300RI/VG_AHRS/platformio.ini index 41fbdb1..d5faebf 100644 --- a/examples/OpenIMU300RI/VG_AHRS/platformio.ini +++ b/examples/OpenIMU300RI/VG_AHRS/platformio.ini @@ -15,7 +15,7 @@ platform = aceinna_imu lib_archive = false board = OpenIMU300 ;lib_deps = ../../../openIMU300-lib -lib_deps = OpenIMU300-base-library@~1.1.11 +lib_deps = OpenIMU300-base-library@1.1.13 build_flags = ; -D CLI ; Comment next line for VG algorithm diff --git a/examples/OpenIMU300RI/VG_AHRS/src/appVersion.h b/examples/OpenIMU300RI/VG_AHRS/src/appVersion.h index 57e5cff..8c3f408 100644 --- a/examples/OpenIMU300RI/VG_AHRS/src/appVersion.h +++ b/examples/OpenIMU300RI/VG_AHRS/src/appVersion.h @@ -26,7 +26,7 @@ limitations under the License. #ifndef _IMU_APP_VERSION_H #define _IMU_APP_VERSION_H -#define APP_VERSION_STRING "VG_AHRS_J1939 03.01.09" +#define APP_VERSION_STRING "OpenIMU300RI VG_AHRS_J1939 03.01.10" #endif diff --git a/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfigurationUart.c b/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfigurationUart.c index 7ee463d..877b48c 100644 --- a/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfigurationUart.c +++ b/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfigurationUart.c @@ -89,9 +89,10 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) case USER_UART_ORIENTATION: { uint64_t tmp = data; - result = platformSetOrientation((uint16_t*)&data, fApply); + uint16_t orientOut; + result = platformSetOrientation((uint16_t*)&data, &orientOut, fApply); if(fApply && result == TRUE){ - UpdateEcuOrientationSettings((uint16_t)data); + UpdateEcuOrientationSettings(orientOut); data = tmp; } } diff --git a/examples/OpenIMU300ZI/IMU/src/FreeRTOSConfig.h b/examples/OpenIMU300ZI/IMU/include/FreeRTOSConfig.h similarity index 100% rename from examples/OpenIMU300ZI/IMU/src/FreeRTOSConfig.h rename to examples/OpenIMU300ZI/IMU/include/FreeRTOSConfig.h diff --git a/examples/OpenIMU300ZI/IMU/src/osresources.h b/examples/OpenIMU300ZI/IMU/include/osresources.h similarity index 100% rename from examples/OpenIMU300ZI/IMU/src/osresources.h rename to examples/OpenIMU300ZI/IMU/include/osresources.h diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/algorithmAPI.h b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/algorithmAPI.h index 51705e4..c68ed69 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/algorithmAPI.h +++ b/examples/OpenIMU300ZI/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. @@ -68,6 +68,22 @@ void enableGpsInAlgorithm(BOOL enable); BOOL gpsUsedInAlgorithm(); +void enableOdoInAlgorithm(BOOL enable); + +BOOL odoUsedInAlgorithm(); + +void enableFreeIntegration(BOOL enable); + +BOOL freeIntegrationEnabled(); + +void enableStationaryLockYaw(BOOL enable); + +BOOL stationaryLockYawEnabled(); + +void enableImuStaticDetect(BOOL enable); + +BOOL imuStaticDetectEnabled(); + void SetAlgorithmUseDgps(BOOL d); void updateAlgorithmTimings(int corr, uint32_t tmrVal ); @@ -77,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(); /****************************************************************************** diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/EKF_Algorithm.h b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/EKF_Algorithm.h index b789f6a..4028640 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/EKF_Algorithm.h +++ b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/EKF_Algorithm.h @@ -14,6 +14,7 @@ #include "StateIndices.h" #include "gpsAPI.h" // For gpsDataStruct_t in EKF setter +#include "odoAPI.h" #include "Indices.h" @@ -64,10 +65,23 @@ typedef struct { real R[9]; real K[NUMBER_OF_EKF_STATES][3]; real stateUpdate[NUMBER_OF_EKF_STATES]; + real deltaP_tmp[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; // The following two are used in more than one functions, so they are pre-computed. real wTrueTimesDtOverTwo[NUM_AXIS]; real turnSwitchMultiplier; + + // saved states when pps comes in + real ppsP[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; + real phi[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; + real dQ[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; + real ppsPosition_N[NUM_AXIS]; + real ppsVelocity_N[NUM_AXIS]; + //real ppsQuaternion[4]; + real ppsEulerAngles[NUM_AXIS]; + //real ppsRateBias_B[NUM_AXIS]; + //real ppsAccelBias_B[NUM_AXIS]; + uint32_t ppsITow; } KalmanFilterStruct; extern KalmanFilterStruct gKalmanFilter; @@ -96,6 +110,13 @@ typedef struct { * This is valid only when there is gps udpate. */ BOOL gpsUpdate; // Indicate if GNSS measurement is updated. + + // odometer + BOOL odoUpdate; // indicate if odo measurement is updated + real odoVelocity; // velocity from odo, [m/s] + + // 1PPS from GNSS receiver + BOOL ppsDetected; } EKF_InputDataStruct; extern EKF_InputDataStruct gEKFInput; @@ -148,7 +169,10 @@ void EKF_GetOperationalMode(uint8_t *EKF_OperMode); void EKF_GetOperationalSwitches(uint8_t *EKF_LinAccelSwitch, uint8_t *EKF_TurnSwitch); // Setter functions -void EKF_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStruct_t *gps); +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_ */ diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/MotionStatus.h b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/MotionStatus.h index 785ac68..57e4fcf 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/MotionStatus.h +++ b/examples/OpenIMU300ZI/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. diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/TimingVars.h b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/TimingVars.h index 1799cbc..5f545f0 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/TimingVars.h +++ b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/TimingVars.h @@ -33,6 +33,7 @@ float TimingVars_GetTime(void); void TimingVars_SetTMin(float tMin); float TimingVars_GetTMin(void); +void TimingVars_dacqFrequency(int freq); void TimingVars_DisplayTimerVars(signed long timeStep); diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/algorithm.h b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/algorithm.h index 1f479d8..a9a578b 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/algorithm.h +++ b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/include/algorithm.h @@ -77,24 +77,24 @@ typedef struct { int32_t Declination_Expiration_Time; // [msec] uint16_t Free_Integration_Cntr; // [count] -//#define LIMIT_MAX_REST_TIME_BEFORE_DROP_TO_AHRS 60000 // 60000 [ msec ] = 60 [ sec ] -//#define LIMIT_DECL_EXPIRATION_TIME 60000 // 60,000 [ counts ] = 10 [ min ] real accelSwitch; uint32_t linAccelSwitchDelay; - + uint32_t rateIntegrationTime; InnovationStruct Innov; } LimitStruct; /// specifying how the user sets up the device algorithm -struct algoBehavior_BITS { /// bits description - uint16_t freeIntegrate : 1; /// 0 - uint16_t useMag : 1; /// 1 - uint16_t useGPS : 1; /// 2 - Not used yet - uint16_t stationaryLockYaw : 1; /// 3 - Not used yet - uint16_t restartOnOverRange : 1; /// 4 - uint16_t dynamicMotion : 1; /// 5 - Not used - uint16_t rsvd : 10; /// 6:15 +struct algoBehavior_BITS { // bits description + uint16_t freeIntegrate : 1; // 0 + uint16_t useMag : 1; // 1 + uint16_t useGPS : 1; // 2 + uint16_t useOdo : 1; // 3 + uint16_t enableStationaryLockYaw : 1; // 4 + uint16_t restartOnOverRange : 1; // 5 + uint16_t dynamicMotion : 1; // 6 + uint16_t enableImuStaticDetect : 1; // 7 + uint16_t rsvd : 8; // 8:15 }; union AlgoBehavior @@ -110,11 +110,21 @@ struct ALGO_STATUS_BITS uint16_t highGain : 1; // 1 high gain mode uint16_t attitudeOnlyAlgorithm : 1; // 2 attitude only algorithm uint16_t turnSwitch : 1; // 3 turn switch - uint16_t noAirdataAiding : 1; // 4 airdata aiding - uint16_t noMagnetometerheading : 1; // 5 magnetometer heading - uint16_t noGPSTrackReference : 1; // 6 GPS track - uint16_t gpsUpdate : 1; // 7 GPS measurement update - uint16_t rsvd : 8; // 8:15 + uint16_t linearAccel : 1; // 4 Linear acceleration detected by difference from gravity + uint16_t staticImu : 1; // 5 zero velocity detected by IMU + uint16_t gpsHeadingValid : 1; /* 6 When GPS velocity is above a certain threshold, + * this is set to TRUE, and GPS heading measurement + * is used, otherwise, this is set to FALSE and magnetic + * heading (if available) is used. + */ + uint16_t stationaryYawLock : 1; // 7 Yaw is locked when unit is static + uint16_t ppsAvailable : 1; /* 8. + * This will be initialized as FALSE. + * This will become TRUE when PPS is detected for the first time. + * This will become FALSE when PPS has not been detected for more than 2sec. + * This will become FALSE when GNSS measurement is refrshed and used. + */ + uint16_t rsvd : 7; // 9:15 }; typedef union ALGO_STATUS @@ -182,6 +192,8 @@ typedef struct { uint8_t linAccelLPFType; uint8_t useRawAccToDetectLinAccel; + uint8_t useRawRateToPredAccel; + real coefOfReduceQ; uint8_t callingFreq; real dt; @@ -205,8 +217,14 @@ typedef struct { DurationStruct Duration; LimitStruct Limit; + enumIMUType imuType; + } AlgorithmStruct; extern AlgorithmStruct gAlgorithm; +// Update IMU specs. +void UpdateImuSpec(real rwOdr, real arw, real biw, real maxBiasW, + real vrw, real bia, real maxBiasA); + #endif diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/EKF_Algorithm.c b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/EKF_Algorithm.c index 6081705..2874e27 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/EKF_Algorithm.c +++ b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/EKF_Algorithm.c @@ -61,6 +61,9 @@ ImuStatsStruct gImuStats; static void RemoveLeverArm(double *lla, double *vNed, real *w, real *leverArmB, real *rn2e, double *ecef); +static void HandlePps(); +static void SaveKfStateAtPps(); + //============================================================================= /* This routine is called at either 100 or 200 Hz based upon the system configuration: @@ -91,6 +94,7 @@ void EKF_Algorithm(void) gEKFInput.angRate_B, gAlgorithm.dt, gAlgorithm.Limit.linAccelSwitchDelay, + gKalmanFilter.rateBias_B, &gImuStats); } else @@ -99,11 +103,20 @@ void EKF_Algorithm(void) gEKFInput.angRate_B, gAlgorithm.dt, gAlgorithm.Limit.linAccelSwitchDelay, + gKalmanFilter.rateBias_B, &gImuStats); } // Detect if the unit is static or dynamic DetectMotionFromAccel(gImuStats.accelNorm, 0); + gAlgoStatus.bit.linearAccel = !gAlgorithm.linAccelSwitch; + + // if zero velocity detection by IMU is not enabled, detection result is always false. + if (!gAlgorithm.Behavior.bit.enableImuStaticDetect) + { + gImuStats.bStaticIMU = FALSE; + } + gAlgoStatus.bit.staticImu = gImuStats.bStaticIMU; } // Compute the EKF solution if past the stabilization and initialization stages @@ -115,6 +128,11 @@ void EKF_Algorithm(void) // Perform EKF Prediction EKF_PredictionStage(gImuStats.lpfAccel); + if (gAlgorithm.state == INS_SOLUTION) + { + HandlePps(); + } + /* Update the predicted states if not freely integrating * NOTE: free- integration is not applicable in HG AHRS mode. */ @@ -222,42 +240,65 @@ void EKF_Algorithm(void) DynamicMotion(); } -void enableFreeIntegration(BOOL enable) +static void HandlePps() { - gAlgorithm.Behavior.bit.freeIntegrate = enable; -} - - -BOOL freeIntegrationEnabled() -{ - return (BOOL)gAlgorithm.Behavior.bit.freeIntegrate; -} - -void enableMagInAlgorithm(BOOL enable) -{ - if(1) + // PPS not detected for a long time? + int32_t timeSinceLastPps = (int32_t)(gAlgorithm.itow - gKalmanFilter.ppsITow); + if (timeSinceLastPps < 0) { - gAlgorithm.Behavior.bit.useMag = enable; + timeSinceLastPps = timeSinceLastPps + MAX_ITOW; } - else + if (timeSinceLastPps > 2000) { - gAlgorithm.Behavior.bit.useMag = FALSE; + gAlgoStatus.bit.ppsAvailable = FALSE; + } + // PPS detected + if (gEKFInput.ppsDetected) + { + // PPS is available from now. + gAlgoStatus.bit.ppsAvailable = TRUE; + // save states when pps detected + SaveKfStateAtPps(); } } -BOOL magUsedInAlgorithm() -{ - return gAlgorithm.Behavior.bit.useMag != 0; -} - -BOOL gpsUsedInAlgorithm(void) -{ - return (BOOL)gAlgorithm.Behavior.bit.useGPS; -} - -void enableGpsInAlgorithm(BOOL enable) +static void SaveKfStateAtPps() { - gAlgorithm.Behavior.bit.useGPS = enable; + // save time + gKalmanFilter.ppsITow = gAlgorithm.itow; + + // save states + gKalmanFilter.ppsPosition_N[0] = gKalmanFilter.Position_N[0]; + gKalmanFilter.ppsPosition_N[1] = gKalmanFilter.Position_N[1]; + gKalmanFilter.ppsPosition_N[2] = gKalmanFilter.Position_N[2]; + gKalmanFilter.ppsVelocity_N[0] = gKalmanFilter.Velocity_N[0]; + gKalmanFilter.ppsVelocity_N[1] = gKalmanFilter.Velocity_N[1]; + gKalmanFilter.ppsVelocity_N[2] = gKalmanFilter.Velocity_N[2]; + //gKalmanFilter.ppsQuaternion[0] = gKalmanFilter.quaternion[0]; + //gKalmanFilter.ppsQuaternion[1] = gKalmanFilter.quaternion[1]; + //gKalmanFilter.ppsQuaternion[2] = gKalmanFilter.quaternion[2]; + //gKalmanFilter.ppsQuaternion[3] = gKalmanFilter.quaternion[3]; + gKalmanFilter.ppsEulerAngles[0] = gKalmanFilter.eulerAngles[0]; + gKalmanFilter.ppsEulerAngles[1] = gKalmanFilter.eulerAngles[1]; + gKalmanFilter.ppsEulerAngles[2] = gKalmanFilter.eulerAngles[2]; + //gKalmanFilter.ppsRateBias_B[0] = gKalmanFilter.rateBias_B[0]; + //gKalmanFilter.ppsRateBias_B[1] = gKalmanFilter.rateBias_B[1]; + //gKalmanFilter.ppsRateBias_B[2] = gKalmanFilter.rateBias_B[2]; + //gKalmanFilter.ppsAccelBias_B[0] = gKalmanFilter.accelBias_B[0]; + //gKalmanFilter.ppsAccelBias_B[1] = gKalmanFilter.accelBias_B[1]; + //gKalmanFilter.ppsAccelBias_B[2] = gKalmanFilter.accelBias_B[2]; + // save state covariance matrix + memcpy(&gKalmanFilter.ppsP[0][0], &gKalmanFilter.P[0][0], sizeof(gKalmanFilter.P)); + + /* reset state transition matrix and state covariance matrix increment between pps and update + * phi is reset to an identity matrix, and dQ is reset to a zero matrix. + */ + memset(&gKalmanFilter.phi[0][0], 0, sizeof(gKalmanFilter.phi)); + memset(&gKalmanFilter.dQ[0][0], 0, sizeof(gKalmanFilter.dQ)); + for (int i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + gKalmanFilter.phi[i][i] = 1.0; + } } // Getters based on results structure passed to WriteResultsIntoOutputStream() @@ -397,7 +438,9 @@ void EKF_GetOperationalSwitches(uint8_t *EKF_LinAccelSwitch, uint8_t *EKF_TurnSw // SETTERS: for EKF input and output structures // Populate the EKF input structure with sensor and GPS data (if used) -void EKF_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStruct_t *gps) +void EKF_SetInputStruct(double *accels, double *rates, double *mags, + gpsDataStruct_t *gps, odoDataStruct_t *odo, + BOOL ppsDetected) { // Accelerometer signal is in [m/s/s] gEKFInput.accel_B[X_AXIS] = (real)accels[X_AXIS] * GRAVITY; @@ -450,8 +493,8 @@ void EKF_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStru gEKFInput.vNedAnt[Z_AXIS] = gps->vNed[Z_AXIS]; // Course and velocity data - gEKFInput.rawGroundSpeed = (real)sqrt(SQUARE(gEKFInput.vNed[0]) + - SQUARE(gEKFInput.vNed[1]));// gps->rawGroundSpeed; + gEKFInput.rawGroundSpeed = (real)sqrt(SQUARE(gps->vNed[0]) + + SQUARE(gps->vNed[1]));// gps->rawGroundSpeed; gEKFInput.trueCourse = (real)gps->trueCourse; /* Remove lever arm effects in LLA/Velocity. To do this requires transformation matrix @@ -490,6 +533,13 @@ void EKF_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStru &gKalmanFilter.rGPS_N[0]); } } + + // odometer + gEKFInput.odoUpdate = odo->update; + gEKFInput.odoVelocity = odo->v; + + // 1PPS signal from GNSS receiver + gEKFInput.ppsDetected = ppsDetected; } @@ -732,4 +782,3 @@ static void RemoveLeverArm(double *lla, double *vNed, real *w, real *leverArmB, ecef[Y_AXIS] = tmp * sinLon; ecef[Z_AXIS] = ((E_MINOR_OVER_MAJOR_SQ * (rn)) + lla[ALT]) * sinLat; } - diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/MotionStatus.c b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/MotionStatus.c index 220f0cd..017d8e9 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/MotionStatus.c +++ b/examples/OpenIMU300ZI/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; -} \ No newline at end of file +} diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/PredictFunctions.c b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/PredictFunctions.c index 3647db5..8f56b75 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/PredictFunctions.c +++ b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/PredictFunctions.c @@ -78,7 +78,7 @@ uint8_t RLE_Q[ROWS_IN_F][2] = { { STATE_RX, STATE_RX }, // Local functions static void _PredictStateEstimate(void); static void _PredictCovarianceEstimate(void); - +static void StateCovarianceChange(); static void _UpdateProcessJacobian(void); static void _UpdateProcessCovariance(void); @@ -101,6 +101,19 @@ void EKF_PredictionStage(real *filteredAccel) _PredictStateEstimate(); // x(k+1) = x(k) + f(x(k), u(k)) _PredictCovarianceEstimate(); // P = F*P*FTrans + Q + /* calculate state transition matrix and state covarariance increment + *P(k) = F(k)*( F(k-1) * P(k-1) * F(k-1)' )*F(k)' + Q(k) + * = F(k)...F(1)*P(1)*F(1)'...F(k)' + F(k)...F(2)*Q1*F(2)'...F(k)' + ... + F(k)*Q(k-1)*F(k)' + Q(k) + * = phi(k)*P(1)*phi(k)' + dQ(k) + * phi(k) = F(k)*phi(k-1), phi(1) = eye(N) + * dQ(k) = F(k)*dQ(k-1)*F(k)' + Q(k), dQ(1) = zeros(N) + */ + if (gAlgoStatus.bit.ppsAvailable) + { + // This is done only when pps is detected. + StateCovarianceChange(); + } + // Extract the predicted Euler angles from the predicted quaternion QuaternionToEulerAngles( gKalmanFilter.eulerAngles, gKalmanFilter.quaternion ); @@ -664,7 +677,6 @@ static void _UpdateProcessCovariance(void) biSq[Y_AXIS] = biSq[X_AXIS]; biSq[Z_AXIS] = biSq[X_AXIS]; - /* Rate-bias terms (computed once as it does not change with attitude). * sigDriftDot = (2*pi/ln(2)) * BI^2 / ARW * 2*pi/ln(2) = 9.064720283654388 @@ -680,9 +692,16 @@ static void _UpdateProcessCovariance(void) gKalmanFilter.Q[STATE_WBZ] = sigDriftDot * biSq[Z_AXIS] * gAlgorithm.dt; gKalmanFilter.Q[STATE_WBZ] = gKalmanFilter.Q[STATE_WBZ] * gKalmanFilter.Q[STATE_WBZ]; - gKalmanFilter.Q[STATE_ABX] = (real)1.0e-12; - gKalmanFilter.Q[STATE_ABY] = (real)1.0e-12; - gKalmanFilter.Q[STATE_ABZ] = (real)1.0e-12; + // Accel bias + biSq[X_AXIS] = gAlgorithm.imuSpec.biA * gAlgorithm.imuSpec.biA; + biSq[Y_AXIS] = biSq[X_AXIS]; + biSq[Z_AXIS] = biSq[X_AXIS]; + sigDriftDot = (real)9.064720283654388 / gAlgorithm.imuSpec.vrw; + gKalmanFilter.Q[STATE_ABX] = sigDriftDot * biSq[X_AXIS] * gAlgorithm.dt; + gKalmanFilter.Q[STATE_ABX] = gKalmanFilter.Q[STATE_ABX] * gKalmanFilter.Q[STATE_ABX]; + //gKalmanFilter.Q[STATE_ABX] = (real)1.0e-12; + gKalmanFilter.Q[STATE_ABY] = gKalmanFilter.Q[STATE_ABX]; + gKalmanFilter.Q[STATE_ABZ] = gKalmanFilter.Q[STATE_ABX]; /* Precalculate the multiplier applied to the Q terms associated with * attitude. sigRate = ARW / sqrt( dt ) @@ -713,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]; } } @@ -803,13 +822,92 @@ void GenerateProcessCovariance(void) gKalmanFilter.Q[STATE_VX] = 100 * dtSigAccelSq;//(real)1e-10; gKalmanFilter.Q[STATE_VY] = 100 * dtSigAccelSq; gKalmanFilter.Q[STATE_VZ] = 100 * dtSigAccelSq; - - // Acceleration - bias - gKalmanFilter.Q[STATE_ABX] = (real)5e-11; //(real)1e-10; // dtSigAccelSq; //%1e-8 %sigmaAccelBiasSq; - gKalmanFilter.Q[STATE_ABY] = (real)5e-11; //(real)1e-10; //dtSigAccelSq; //%sigmaAccelBiasSq; - gKalmanFilter.Q[STATE_ABZ] = (real)5e-11; //(real)1e-10; //dtSigAccelSq; //%sigmaAccelBiasSq; } +static void StateCovarianceChange() +{ + uint8_t rowNum, colNum, multIndex; + // 1) phi(k) = F(k)*phi(k-1) + // deltaP_tmp is used to hold F(k)*phi(k-1) + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); + for (rowNum = 0; rowNum < 16; rowNum++) + { + for (colNum = 0; colNum < 16; colNum++) + { + for (multIndex = RLE_F[rowNum][0]; multIndex <= RLE_F[rowNum][1]; multIndex++) + { + gKalmanFilter.deltaP_tmp[rowNum][colNum] += + gKalmanFilter.F[rowNum][multIndex] * gKalmanFilter.phi[multIndex][colNum]; + } + } + } + memcpy(&gKalmanFilter.phi[0][0], &gKalmanFilter.deltaP_tmp[0][0], sizeof(gKalmanFilter.phi)); + + // 2) dQ(k) = F(k)*dQ(k-1)*F(k)' + Q(k) + // 2.1) deltaP_tmp is used to hold F(k)*dQ(k-1) + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); + for (rowNum = 0; rowNum < 16; rowNum++) + { + for (colNum = 0; colNum < 16; colNum++) + { + for (multIndex = RLE_F[rowNum][0]; multIndex <= RLE_F[rowNum][1]; multIndex++) + { + gKalmanFilter.deltaP_tmp[rowNum][colNum] += + gKalmanFilter.F[rowNum][multIndex] * gKalmanFilter.dQ[multIndex][colNum]; + } + } + } + // 2.2) Calculate F(k)*dQ(k-1)*F(k)' = deltaP_tmp*F(k)' + memset(&gKalmanFilter.dQ[0][0], 0, sizeof(gKalmanFilter.dQ)); + for (rowNum = 0; rowNum < ROWS_IN_F; rowNum++) + { + for (colNum = rowNum; colNum < ROWS_IN_F; colNum++) + { + for (multIndex = RLE_F[colNum][0]; multIndex <= RLE_F[colNum][1]; multIndex++) + { + gKalmanFilter.dQ[rowNum][colNum] = gKalmanFilter.dQ[rowNum][colNum] + + gKalmanFilter.deltaP_tmp[rowNum][multIndex] * gKalmanFilter.F[colNum][multIndex]; + } + gKalmanFilter.dQ[colNum][rowNum] = gKalmanFilter.dQ[rowNum][colNum]; + } + } + // 2.3) Calculate F(k)*dQ(k-1)*F(k)' + Q(k) + gKalmanFilter.dQ[STATE_RX][STATE_RX] += gKalmanFilter.Q[STATE_RX]; + gKalmanFilter.dQ[STATE_RY][STATE_RY] += gKalmanFilter.Q[STATE_RY]; + gKalmanFilter.dQ[STATE_RZ][STATE_RZ] += gKalmanFilter.Q[STATE_RZ]; + + gKalmanFilter.dQ[STATE_VX][STATE_VX] += gKalmanFilter.Q[STATE_VX]; + gKalmanFilter.dQ[STATE_VY][STATE_VY] += gKalmanFilter.Q[STATE_VY]; + gKalmanFilter.dQ[STATE_VZ][STATE_VZ] += gKalmanFilter.Q[STATE_VZ]; + + gKalmanFilter.dQ[STATE_Q0][STATE_Q0] += gKalmanFilter.Q[STATE_Q0]; + gKalmanFilter.dQ[STATE_Q0][STATE_Q1] += gKalmanFilter.Qq[0]; + gKalmanFilter.dQ[STATE_Q0][STATE_Q2] += gKalmanFilter.Qq[1]; + gKalmanFilter.dQ[STATE_Q0][STATE_Q3] += gKalmanFilter.Qq[2]; + + gKalmanFilter.dQ[STATE_Q1][STATE_Q0] = gKalmanFilter.dQ[STATE_Q0][STATE_Q1]; + gKalmanFilter.dQ[STATE_Q1][STATE_Q1] += gKalmanFilter.Q[STATE_Q1]; + gKalmanFilter.dQ[STATE_Q1][STATE_Q2] += gKalmanFilter.Qq[3]; + gKalmanFilter.dQ[STATE_Q1][STATE_Q3] += gKalmanFilter.Qq[4]; + + gKalmanFilter.dQ[STATE_Q2][STATE_Q0] = gKalmanFilter.dQ[STATE_Q0][STATE_Q2]; + gKalmanFilter.dQ[STATE_Q2][STATE_Q1] = gKalmanFilter.dQ[STATE_Q1][STATE_Q2]; + gKalmanFilter.dQ[STATE_Q2][STATE_Q2] += gKalmanFilter.Q[STATE_Q2]; + gKalmanFilter.dQ[STATE_Q2][STATE_Q3] += gKalmanFilter.Qq[5]; + + gKalmanFilter.dQ[STATE_Q3][STATE_Q0] = gKalmanFilter.dQ[STATE_Q0][STATE_Q3]; + gKalmanFilter.dQ[STATE_Q3][STATE_Q1] = gKalmanFilter.dQ[STATE_Q1][STATE_Q3]; + gKalmanFilter.dQ[STATE_Q3][STATE_Q2] = gKalmanFilter.dQ[STATE_Q2][STATE_Q3]; + gKalmanFilter.dQ[STATE_Q3][STATE_Q3] += gKalmanFilter.Q[STATE_Q3]; + + gKalmanFilter.dQ[STATE_WBX][STATE_WBX] += gKalmanFilter.Q[STATE_WBX]; + gKalmanFilter.dQ[STATE_WBY][STATE_WBY] += gKalmanFilter.Q[STATE_WBY]; + gKalmanFilter.dQ[STATE_WBZ][STATE_WBZ] += gKalmanFilter.Q[STATE_WBZ]; + + gKalmanFilter.dQ[STATE_ABX][STATE_ABX] += gKalmanFilter.Q[STATE_ABX]; + gKalmanFilter.dQ[STATE_ABY][STATE_ABY] += gKalmanFilter.Q[STATE_ABY]; + gKalmanFilter.dQ[STATE_ABZ][STATE_ABZ] += gKalmanFilter.Q[STATE_ABZ]; +} /** **************************************************************************** * @name: firstOrderLowPass_float implements a low pass yaw axis filter diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/SelectState.c b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/SelectState.c index d4906ed..0fe20ee 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/SelectState.c +++ b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/SelectState.c @@ -311,6 +311,8 @@ void LG_To_INS_Transition_Test(void) // Transit to INS solution gAlgorithm.state = INS_SOLUTION; + + gAlgoStatus.bit.attitudeOnlyAlgorithm = FALSE; // Initialize the algorithm with GNSS position and lever arm InitINSFilter(); diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/TimingVars.c b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/TimingVars.c index 2942dfb..f4de8d9 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/TimingVars.c +++ b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/TimingVars.c @@ -162,3 +162,7 @@ void Initialize_Timing(void) timer.dacqFrequency = DACQ_200_HZ; // default } +void TimingVars_dacqFrequency(int freq) +{ + timer.dacqFrequency = freq; +} diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/UpdateFunctions.c b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/UpdateFunctions.c index c6a45e5..967e5e3 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/UpdateFunctions.c +++ b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/UpdateFunctions.c @@ -62,6 +62,8 @@ static real _UnwrapAttitudeError( real attitudeError ); static real _LimitValue( real value, real limit ); static BOOL _CheckForUpdateTrigger(uint8_t updateRate); +static void ApplyGpsDealyCorrForStateCov(); + /****************************************************************************** * @brief Initializa heading using GNSS heading. * If the GNSS heading is valid and the vehicle is drving forward, the GNSS @@ -149,8 +151,31 @@ void EKF_UpdateStage(void) */ if( gEKFInput.gpsUpdate ) { - // Sync the algorithm itow to the GPS value + /* Sync the algorithm itow to the GPS value. GPS time is the time of pps. + * It is delayed by (gAlgorithm.itow-gEKFInput.itow). If there is loss of + * PPS detection or GPS measuremetn, gEKFInput.itow equals gKalmanFilter.ppsITow. + */ + if (gAlgoStatus.bit.ppsAvailable) + { + /* If GPS itow is above algorithm itow, something is wrong. + * If algorithm itow is more than 1sec ahead of GPS itow, GPS delay is + * too large and measurement is not used. + */ + int32_t gnssDelay = gAlgorithm.itow - gEKFInput.itow; + if (gnssDelay < 0 || gnssDelay > 1000) + { + gAlgoStatus.bit.ppsAvailable = FALSE; + } + // sync with GPS time + gAlgorithm.itow = gEKFInput.itow + gAlgorithm.itow - gKalmanFilter.ppsITow; + } + else + { gAlgorithm.itow = gEKFInput.itow; + } + // update pps detection time + gKalmanFilter.ppsITow = gEKFInput.itow; + // Resync timer timer.tenHertzCntr = 0; timer.subFrameCntr = 0; @@ -159,7 +184,8 @@ void EKF_UpdateStage(void) if (gEKFInput.gpsFixType) { // GPS heading valid? - useGpsHeading = (gEKFInput.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING); + gAlgoStatus.bit.gpsHeadingValid = (gEKFInput.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING); + useGpsHeading = gAlgoStatus.bit.gpsHeadingValid; /* If GNSS outage is longer than a threshold (maxReliableDRTime), DR results get unreliable * So, when GNSS comes back, the EKF is reinitialized. Otherwise, the DR results are still @@ -186,11 +212,13 @@ void EKF_UpdateStage(void) // reset the "last good reading" time gAlgorithm.timeOfLastGoodGPSReading = gEKFInput.itow; } - //else + + // apply motion constraints if (gAlgorithm.velocityAlwaysAlongBodyX && gAlgorithm.headingIni>HEADING_UNINITIALIZED) { Update_PseudoMeasurement(); } + // At 1 Hz mark, update when GPS data is valid, else do an AHRS-update runInsUpdate = 1; } @@ -198,7 +226,12 @@ void EKF_UpdateStage(void) { Update_Att(); runInsUpdate = 0; // set up for next pass - useGpsHeading = 0; + // handle P when PPS is available + if (gAlgoStatus.bit.ppsAvailable) + { + ApplyGpsDealyCorrForStateCov(); + gAlgoStatus.bit.ppsAvailable = FALSE; + } } } } @@ -207,9 +240,18 @@ void EKF_UpdateStage(void) void ComputeSystemInnovation_Pos(void) { // Position error + if (gAlgoStatus.bit.ppsAvailable) + { + gKalmanFilter.nu[STATE_RX] = gKalmanFilter.rGPS_N[X_AXIS] - gKalmanFilter.ppsPosition_N[X_AXIS]; + gKalmanFilter.nu[STATE_RY] = gKalmanFilter.rGPS_N[Y_AXIS] - gKalmanFilter.ppsPosition_N[Y_AXIS]; + gKalmanFilter.nu[STATE_RZ] = gKalmanFilter.rGPS_N[Z_AXIS] - gKalmanFilter.ppsPosition_N[Z_AXIS]; + } + else + { gKalmanFilter.nu[STATE_RX] = gKalmanFilter.rGPS_N[X_AXIS] - gKalmanFilter.Position_N[X_AXIS]; gKalmanFilter.nu[STATE_RY] = gKalmanFilter.rGPS_N[Y_AXIS] - gKalmanFilter.Position_N[Y_AXIS]; gKalmanFilter.nu[STATE_RZ] = gKalmanFilter.rGPS_N[Z_AXIS] - gKalmanFilter.Position_N[Z_AXIS]; + } gKalmanFilter.nu[STATE_RX] = _LimitValue(gKalmanFilter.nu[STATE_RX], gAlgorithm.Limit.Innov.positionError); gKalmanFilter.nu[STATE_RY] = _LimitValue(gKalmanFilter.nu[STATE_RY], gAlgorithm.Limit.Innov.positionError); @@ -221,9 +263,18 @@ void ComputeSystemInnovation_Pos(void) void ComputeSystemInnovation_Vel(void) { // Velocity error + if (gAlgoStatus.bit.ppsAvailable) + { + gKalmanFilter.nu[STATE_VX] = (real)gEKFInput.vNed[X_AXIS] - gKalmanFilter.ppsVelocity_N[X_AXIS]; + gKalmanFilter.nu[STATE_VY] = (real)gEKFInput.vNed[Y_AXIS] - gKalmanFilter.ppsVelocity_N[Y_AXIS]; + gKalmanFilter.nu[STATE_VZ] = (real)gEKFInput.vNed[Z_AXIS] - gKalmanFilter.ppsVelocity_N[Z_AXIS]; + } + else + { gKalmanFilter.nu[STATE_VX] = (real)gEKFInput.vNed[X_AXIS] - gKalmanFilter.Velocity_N[X_AXIS]; gKalmanFilter.nu[STATE_VY] = (real)gEKFInput.vNed[Y_AXIS] - gKalmanFilter.Velocity_N[Y_AXIS]; gKalmanFilter.nu[STATE_VZ] = (real)gEKFInput.vNed[Z_AXIS] - gKalmanFilter.Velocity_N[Z_AXIS]; + } gKalmanFilter.nu[STATE_VX] = _LimitValue(gKalmanFilter.nu[STATE_VX], gAlgorithm.Limit.Innov.velocityError); gKalmanFilter.nu[STATE_VY] = _LimitValue(gKalmanFilter.nu[STATE_VY], gAlgorithm.Limit.Innov.velocityError); @@ -254,9 +305,17 @@ void ComputeSystemInnovation_Att(void) { if (gAlgorithm.headingIni >= HEADING_GNSS_LOW) // heading already initialized with GNSS heading { + if (gAlgoStatus.bit.ppsAvailable) + { + gKalmanFilter.nu[STATE_YAW] = gEKFInput.trueCourse * (real)DEG_TO_RAD - + gKalmanFilter.ppsEulerAngles[YAW]; + } + else + { gKalmanFilter.nu[STATE_YAW] = gEKFInput.trueCourse * (real)DEG_TO_RAD - gKalmanFilter.eulerAngles[YAW]; } + } else { gKalmanFilter.nu[STATE_YAW] = 0.0; @@ -518,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; @@ -657,7 +716,6 @@ uint8_t rowNum, colNum, multIndex; real S_3x3[3][3], SInverse_3x3[3][3]; real PxHTranspose[ROWS_IN_P][ROWS_IN_H]; real KxH[NUMBER_OF_EKF_STATES][COLS_IN_H] = {{ 0.0 }}; -real deltaP_tmp[ROWS_IN_P][COLS_IN_P]; void Update_Att(void) { @@ -675,33 +733,59 @@ void Update_Att(void) _GenerateObservationJacobian_AHRS(); // gKF.H: 3x16 _GenerateObservationCovariance_AHRS(); // gKF.R: 3x3 - // In INS mode, do not do pitch and roll update while heading update is kept + // In INS mode, do not do pitch and roll update while heading update is kept. if (gAlgorithm.state == INS_SOLUTION) { + // reset this state. + gAlgoStatus.bit.stationaryYawLock = FALSE; + + /* Heading measurement is invalid, check if static yaw lock should take effect. + * Even if IMU static detection fails and the vehicle runs at a certain speed, + * static yaw lock should not take effect. This is guaranteed by R[STATE_YAW], which + * is set to 1.0 when vehicle speed is below a certian threshold. + * The risk is that the mechnism fails when the vehicle is below the threshold but not + * static. + */ + if (gKalmanFilter.R[STATE_YAW] > 0.9) + { if (!gImuStats.bStaticIMU) { - // If neither mag or GPS headig is available, update measuremnt matrix H to 2x16 - if (gKalmanFilter.R[STATE_YAW] > 0.9) - { + /* Heading measurement is invaid and IMU is not static, yaw lock should not + * take effect. That is, there is no heading update + */ for (colNum = 0; colNum < COLS_IN_H; colNum++) { gKalmanFilter.H[2][colNum] = 0.0; } - } lastYaw = 7.0; } - else + else if (gAlgorithm.Behavior.bit.enableStationaryLockYaw) { + /* IMU is static and static yaw lock is enabled. The first time when the algo runs here, + * the staic yaw is acquired. And yaw will be locked to this value. + */ if (lastYaw > TWO_PI) { lastYaw = gKalmanFilter.eulerAngles[YAW]; } else { - gKalmanFilter.nu[STATE_YAW] = lastYaw - gKalmanFilter.eulerAngles[YAW]; - gKalmanFilter.R[STATE_YAW] = 1e-4; + real diff = lastYaw - gKalmanFilter.eulerAngles[YAW]; + // if angle change exceeds max bias, it is not static + if (fabs(diff) > gAlgorithm.imuSpec.maxBiasW) + { + lastYaw = 7.0; + } + else + { + gKalmanFilter.nu[STATE_YAW] = diff; + gKalmanFilter.R[STATE_YAW] = 1e-8; + gAlgoStatus.bit.stationaryYawLock = TRUE; + } } } + } + // Do not perform roll and pitch update for (colNum = 0; colNum < COLS_IN_H; colNum++) { gKalmanFilter.H[0][colNum] = 0.0; @@ -813,7 +897,7 @@ void Update_Att(void) } // deltaP = KxH * gKF.P; - memset(deltaP_tmp, 0, sizeof(deltaP_tmp)); + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); /* deltaP is symmetric so only need to multiply one half and reflect the values * across the diagonal */ @@ -824,10 +908,10 @@ void Update_Att(void) { for (multIndex = RLE_KxH[rowNum][0]; multIndex <= RLE_KxH[rowNum][1]; multIndex++) { - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + KxH[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; } - deltaP_tmp[colNum][rowNum] = deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; } } #else @@ -849,10 +933,10 @@ void Update_Att(void) { continue; } - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + KxH[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; } - deltaP_tmp[colNum][rowNum] = deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; } } #endif @@ -864,7 +948,7 @@ void Update_Att(void) for (colNum = rowNum; colNum < COLS_IN_P; colNum++) { gKalmanFilter.P[rowNum][colNum] = gKalmanFilter.P[rowNum][colNum] - - deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[rowNum][colNum]; gKalmanFilter.P[colNum][rowNum] = gKalmanFilter.P[rowNum][colNum]; } } @@ -936,7 +1020,7 @@ void Update_Pos(void) // Compute the intermediate state update, stateUpdate AxB(&gKalmanFilter.K[0][0], &gKalmanFilter.nu[STATE_RX], NUMBER_OF_EKF_STATES, 3, 1, &gKalmanFilter.stateUpdate[0]); - memset(deltaP_tmp, 0, sizeof(deltaP_tmp)); + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); // Update the intermediate covariance estimate for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) { @@ -959,14 +1043,15 @@ void Update_Pos(void) { continue; } - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + gKalmanFilter.K[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; } - deltaP_tmp[colNum][rowNum] = deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; } } - AMinusB(&gKalmanFilter.P[0][0], &deltaP_tmp[0][0], NUMBER_OF_EKF_STATES, NUMBER_OF_EKF_STATES, &gKalmanFilter.P[0][0]); + AMinusB(&gKalmanFilter.P[0][0], &gKalmanFilter.deltaP_tmp[0][0], + NUMBER_OF_EKF_STATES, NUMBER_OF_EKF_STATES, &gKalmanFilter.P[0][0]); // Update states gKalmanFilter.Position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_RX]; @@ -1060,7 +1145,7 @@ void Update_Vel(void) // Compute the intermediate state update AxB(&gKalmanFilter.K[0][0], &gKalmanFilter.nu[STATE_VX], NUMBER_OF_EKF_STATES, 3, 1, &gKalmanFilter.stateUpdate[0]); - memset(deltaP_tmp, 0, sizeof(deltaP_tmp)); + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); // Update the intermediate covariance estimate for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) { @@ -1079,15 +1164,15 @@ void Update_Vel(void) */ for (multIndex = STATE_VX; multIndex <= STATE_VZ; multIndex++) { - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + gKalmanFilter.K[rowNum][multIndex - STATE_VX] * gKalmanFilter.P[multIndex][colNum]; } - deltaP_tmp[colNum][rowNum] = deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; } } // P2 = P2 - KxHxP2 - AMinusB(&gKalmanFilter.P[0][0], &deltaP_tmp[0][0], NUMBER_OF_EKF_STATES, NUMBER_OF_EKF_STATES, &gKalmanFilter.P[0][0]); + AMinusB(&gKalmanFilter.P[0][0], &gKalmanFilter.deltaP_tmp[0][0], NUMBER_OF_EKF_STATES, NUMBER_OF_EKF_STATES, &gKalmanFilter.P[0][0]); // ++++++++++++++++++++++ END OF VELOCITY ++++++++++++++++++++++ // Update states @@ -1114,7 +1199,6 @@ void Update_Vel(void) gKalmanFilter.accelBias_B[X_AXIS] += gKalmanFilter.stateUpdate[STATE_ABX]; gKalmanFilter.accelBias_B[Y_AXIS] += gKalmanFilter.stateUpdate[STATE_ABY]; gKalmanFilter.accelBias_B[Z_AXIS] += gKalmanFilter.stateUpdate[STATE_ABZ]; - } static void Update_GPS(void) @@ -1130,6 +1214,11 @@ static void Update_GPS(void) /* Compute the system error: z = meas, h = pred = q, nu - z - h * Do this at the same time even if the update is spread across time-steps */ + if (gAlgoStatus.bit.ppsAvailable) + { + // use P saved when PPS is detected if PPS is available. + memcpy(&gKalmanFilter.P[0][0], &gKalmanFilter.ppsP[0][0], sizeof(gKalmanFilter.P)); + } ComputeSystemInnovation_Pos(); Update_Pos(); ComputeSystemInnovation_Vel(); @@ -1192,17 +1281,16 @@ static void Update_PseudoMeasurement(void) * Zero velocity detection result has a higher priority to determine the front velocity because * odometer is also used for zero velocity detection when odometer is available. */ - BOOL hasOdo = FALSE; BOOL frontVelMeaValid = FALSE; real frontVelMea = 0.0; /* Front velocity is first determined by odometer. If odometer is not available, zero velocity * detection results are used to determine if front velocity is zero. If neither odometer is * available nor zero velocity detected, front velocity measurement is not valid. */ - if (hasOdo) + if (odoUsedInAlgorithm()) { frontVelMeaValid = TRUE; - frontVelMea = 0.0; // replace with real odo output + frontVelMea = gEKFInput.odoVelocity; r[0] = 1.0e-4; // variance of front velocity measurement should be from odo spec } else if (gImuStats.bStaticIMU) @@ -1349,7 +1437,7 @@ static void Update_PseudoMeasurement(void) } // deltaP = KxH * gKF.P; - memset(deltaP_tmp, 0, sizeof(deltaP_tmp)); + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); /* deltaP is symmetric so only need to multiply one half and reflect the values * across the diagonal */ @@ -1365,10 +1453,10 @@ static void Update_PseudoMeasurement(void) { continue; } - deltaP_tmp[rowNum][colNum] = gKalmanFilter.K[rowNum][0] * PxHTranspose[colNum][0] + + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.K[rowNum][0] * PxHTranspose[colNum][0] + gKalmanFilter.K[rowNum][1] * PxHTranspose[colNum][1] + gKalmanFilter.K[rowNum][2] * PxHTranspose[colNum][2]; - deltaP_tmp[colNum][rowNum] = deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; } } @@ -1380,7 +1468,7 @@ static void Update_PseudoMeasurement(void) for (colNum = rowNum; colNum < COLS_IN_P; colNum++) { gKalmanFilter.P[rowNum][colNum] = gKalmanFilter.P[rowNum][colNum] - - deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[rowNum][colNum]; gKalmanFilter.P[colNum][rowNum] = gKalmanFilter.P[rowNum][colNum]; } } @@ -1869,3 +1957,37 @@ static void InitializeEkfHeading() #endif } + +static void ApplyGpsDealyCorrForStateCov() +{ + uint8_t i, j, k; + + // P = phi * P * phi' + dQ + // 1) use deltaP_tmp to hold phi * p + AxB(&gKalmanFilter.phi[0][0], &gKalmanFilter.P[0][0], NUMBER_OF_EKF_STATES, NUMBER_OF_EKF_STATES, + NUMBER_OF_EKF_STATES, &gKalmanFilter.deltaP_tmp[0][0]); + + // 2) phi * p * phi' = deltaP_tmp * phi' + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + for (j = i; j < NUMBER_OF_EKF_STATES; j++) + { + gKalmanFilter.P[i][j] = 0; + for (k = 0; k < NUMBER_OF_EKF_STATES; k++) + { + gKalmanFilter.P[i][j] += gKalmanFilter.deltaP_tmp[i][k] * gKalmanFilter.phi[j][k]; + } + gKalmanFilter.P[j][i] = gKalmanFilter.P[i][j]; + } + } + + // 3) add dQ + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + for (j = i; j < NUMBER_OF_EKF_STATES; j++) + { + gKalmanFilter.P[i][j] += gKalmanFilter.dQ[i][j]; + gKalmanFilter.P[j][i] = gKalmanFilter.P[i][j]; + } + } +} diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/algorithm.c b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/algorithm.c index cacba34..f77904a 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/algorithm.c +++ b/examples/OpenIMU300ZI/IMU/lib/Core/Algorithm/src/algorithm.c @@ -15,6 +15,8 @@ ******************************************************************************/ #include +#include + #include "SensorNoiseParameters.h" #include "platformAPI.h" #include "algorithm.h" @@ -25,9 +27,20 @@ AlgorithmStruct gAlgorithm; AlgoStatus gAlgoStatus; -void InitializeAlgorithmStruct(uint8_t callingFreq) +void InitializeAlgorithmStruct(uint8_t callingFreq, const enumIMUType imuTypeIn) { - gAlgorithm.Behavior.bit.freeIntegrate = FALSE; + +#ifndef IMU_ONLY + enumIMUType imuType = imuTypeIn; + + if(imuType == CurrentIMU){ + // Reuse previously initialized IMU type + imuType = gAlgorithm.imuType; + } + + memset(&gAlgorithm, 0, sizeof(AlgorithmStruct)); + + //----------------------------algortihm config----------------------------- // The calling frequency drives the execution rate of the EKF and dictates // the algorithm constants if(callingFreq == 0){ @@ -94,7 +107,10 @@ void InitializeAlgorithmStruct(uint8_t callingFreq) // Linear acceleration switch limits (level and time) gAlgorithm.Limit.accelSwitch = (real)(0.012); // [g] - gAlgorithm.Limit.linAccelSwitchDelay = (uint32_t)(2.0 * gAlgorithm.callingFreq); + 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; @@ -109,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; @@ -126,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; @@ -134,15 +184,25 @@ 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); gAlgorithm.staticDetectParam.staticVarAccel = (real)(gAlgorithm.imuSpec.sigmaA * gAlgorithm.imuSpec.sigmaA); - gAlgorithm.staticDetectParam.maxGyroBias = MAX_BW; + gAlgorithm.staticDetectParam.maxGyroBias = gAlgorithm.imuSpec.maxBiasW; gAlgorithm.staticDetectParam.staticGnssVel = 0.2; gAlgorithm.staticDetectParam.staticNoiseMultiplier[0] = 4.0; gAlgorithm.staticDetectParam.staticNoiseMultiplier[1] = 4.0; gAlgorithm.staticDetectParam.staticNoiseMultiplier[2] = 1.0; + + gAlgorithm.Behavior.bit.dynamicMotion = 1; + + //----------------------------algorithm states----------------------------- + memset(&gAlgoStatus, 0, sizeof(gAlgoStatus)); +#endif + } void GetAlgoStatus(AlgoStatus *algoStatus) @@ -199,4 +259,90 @@ void setPointOfInterest( real poiBx, real poiBy, real poiBz ) gAlgorithm.pointOfInterestB[0] = poiBx; gAlgorithm.pointOfInterestB[1] = poiBy; gAlgorithm.pointOfInterestB[2] = poiBz; +} + +void UpdateImuSpec(real rwOdr, real arw, real biw, real maxBiasW, + real vrw, real bia, real maxBiasA) +{ + // Update IMU specifications + gAlgorithm.imuSpec.arw = arw; + gAlgorithm.imuSpec.sigmaW = (real)(1.25 * arw / sqrt(1.0 / rwOdr)); + gAlgorithm.imuSpec.biW = biw; + gAlgorithm.imuSpec.maxBiasW = maxBiasW; + gAlgorithm.imuSpec.vrw = vrw; + gAlgorithm.imuSpec.sigmaA = (real)(1.25 * vrw / sqrt(1.0 / rwOdr)); + gAlgorithm.imuSpec.biA = bia; + gAlgorithm.imuSpec.maxBiasA = maxBiasA; + + // Update affected params related to zero velocity detection + gAlgorithm.staticDetectParam.staticVarGyro = (real)(gAlgorithm.imuSpec.sigmaW * gAlgorithm.imuSpec.sigmaW); + gAlgorithm.staticDetectParam.staticVarAccel = (real)(gAlgorithm.imuSpec.sigmaA * gAlgorithm.imuSpec.sigmaA); + gAlgorithm.staticDetectParam.maxGyroBias = gAlgorithm.imuSpec.maxBiasW; +} + +void enableMagInAlgorithm(BOOL enable) +{ + if (1) + { + gAlgorithm.Behavior.bit.useMag = enable; + } + else + { + gAlgorithm.Behavior.bit.useMag = FALSE; + } +} + +void enableGpsInAlgorithm(BOOL enable) +{ + gAlgorithm.Behavior.bit.useGPS = enable; +} + +void enableOdoInAlgorithm(BOOL enable) +{ + gAlgorithm.Behavior.bit.useOdo = enable; +} + +BOOL magUsedInAlgorithm() +{ + return gAlgorithm.Behavior.bit.useMag != 0; +} + +BOOL gpsUsedInAlgorithm(void) +{ + return (BOOL)gAlgorithm.Behavior.bit.useGPS; +} + +BOOL odoUsedInAlgorithm(void) +{ + return (BOOL)gAlgorithm.Behavior.bit.useOdo; +} + +void enableFreeIntegration(BOOL enable) +{ + gAlgorithm.Behavior.bit.freeIntegrate = enable; +} + +BOOL freeIntegrationEnabled() +{ + return (BOOL)gAlgorithm.Behavior.bit.freeIntegrate; +} + +void enableStationaryLockYaw(BOOL enable) +{ + gAlgorithm.Behavior.bit.enableStationaryLockYaw = enable; +} + +BOOL stationaryLockYawEnabled() +{ + return (BOOL)gAlgorithm.Behavior.bit.enableStationaryLockYaw; +} + +void enableImuStaticDetect(BOOL enable) +{ + gAlgorithm.Behavior.bit.enableImuStaticDetect = enable; +} + +BOOL imuStaticDetectEnabled() +{ + return (BOOL)gAlgorithm.Behavior.bit.enableImuStaticDetect; } \ No newline at end of file diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Common/include/GlobalConstants.h b/examples/OpenIMU300ZI/IMU/lib/Core/Common/include/GlobalConstants.h index b7ec1e5..afdbdf0 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Common/include/GlobalConstants.h +++ b/examples/OpenIMU300ZI/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 diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Common/include/ucb_packet_struct.h b/examples/OpenIMU300ZI/IMU/lib/Core/Common/include/ucb_packet_struct.h index 066ee3f..506a31d 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/Common/include/ucb_packet_struct.h +++ b/examples/OpenIMU300ZI/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 diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/GPS/gpsAPI.h b/examples/OpenIMU300ZI/IMU/lib/Core/GPS/gpsAPI.h index de2dcb9..0815ab5 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/GPS/gpsAPI.h +++ b/examples/OpenIMU300ZI/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 */ diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/GPS/include/driverGPS.h b/examples/OpenIMU300ZI/IMU/lib/Core/GPS/include/driverGPS.h index c7681a8..552c24b 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/GPS/include/driverGPS.h +++ b/examples/OpenIMU300ZI/IMU/lib/Core/GPS/include/driverGPS.h @@ -218,8 +218,8 @@ typedef struct { int lon_deg; int lon_min; double lon_min_fraction; - char latSign; - char lonSign; + int8_t latSign; + int8_t lonSign; } NmeaLatLonSTRUCT; // delta struct diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/driverGPS.c b/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/driverGPS.c index 89416a6..daac33e 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/driverGPS.c +++ b/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/driverGPS.c @@ -37,8 +37,9 @@ limitations under the License. #include "debug.h" #endif -int gpsSerialChan = UART_CHANNEL_NONE; // undefined, needs to be explicitly defined - +int gpsChan = UART_CHANNEL_NONE; // undefined, needs to be explicitly defined +BOOL _handleGpsMessages(GpsData_t *GPSData); +BOOL gpsUartInitialized = FALSE; /// GPS data struct // to change to NMEA then pass GPS out debug port: un-comment this and @@ -60,6 +61,11 @@ int16_t _getIndexLineFeed(uint16_t numInBuff, unsigned *msgLength); void _setGPSMessageSignature(GpsData_t* GPSData); void _userCmdBaudProtcol(GpsData_t* GPSData); +/******************************************* + * @brief + * + * @param GPSData == +********************************************/ void loadGpsCommSettings(GpsData_t* GPSData) { GPSData->GPSAUTOSetting = 0; @@ -67,6 +73,7 @@ void loadGpsCommSettings(GpsData_t* GPSData) GPSData->GPSConfigureOK = 0; } + // extern_port.c extern void GetGpsExternUartChannel(unsigned int* uartChannel); @@ -77,8 +84,8 @@ extern void GetGpsExternUartChannel(unsigned int* uartChannel); ******************************************************************************/ int initGpsUart(int baudrate) { - gpsSerialChan = platformGetSerialChannel(GPS_SERIAL_PORT); - return uart_init(gpsSerialChan, baudrate); + gpsChan = platformGetSerialChannel(GPS_SERIAL_PORT, FALSE); + return uart_init(gpsChan, baudrate); } @@ -90,7 +97,7 @@ int initGpsUart(int baudrate) ******************************************************************************/ int gpsBytesAvailable() { - return uart_rxBytesAvailable(gpsSerialChan); + return uart_rxBytesAvailable(gpsChan); } /** **************************************************************************** @@ -100,7 +107,7 @@ int gpsBytesAvailable() ******************************************************************************/ void flushGPSRecBuf(void) { - uart_flushRecBuffer(gpsSerialChan); + uart_flushRecBuffer(gpsChan); } /** **************************************************************************** @@ -110,7 +117,7 @@ void flushGPSRecBuf(void) ******************************************************************************/ BOOL isGpsTxEmpty(void) { - return uart_txBytesRemains(gpsSerialChan) == 0; + return uart_txBytesRemains(gpsChan) == 0; } /** **************************************************************************** @@ -122,11 +129,11 @@ uint16_t delBytesGpsBuf(uint16_t numToPop) { int16_t numInBuffer; - numInBuffer = uart_rxBytesAvailable(gpsSerialChan); + numInBuffer = uart_rxBytesAvailable(gpsChan); if (numInBuffer < numToPop) { numToPop = numInBuffer; } - uart_removeRxBytes(gpsSerialChan, numToPop); + uart_removeRxBytes(gpsChan, numToPop); // COM_buf_delete(&(gGpsUartPtr->rec_buf), // numToPop); return ( numInBuffer - numToPop ); ///< unscanned bytes @@ -158,7 +165,7 @@ uint8_t peekByteGpsBuf(uint16_t index) { uint8_t output = 0; - uart_copyBytes(gpsSerialChan, index, 1, &output); + uart_copyBytes(gpsChan, index, 1, &output); return output; } @@ -181,7 +188,7 @@ unsigned long peekGPSmsgHeader(uint16_t index, unsigned long GPSHeader; headerLength = GPSData->GPSMsgSignature.GPSheaderLength; - uart_copyBytes(gpsSerialChan, index, headerLength, header); + uart_copyBytes(gpsChan, index, headerLength, header); GPSHeader = 0; for (i = 0, j = headerLength - 1; i < headerLength; i++, j--) { @@ -203,8 +210,8 @@ unsigned long peekGPSmsgHeader(uint16_t index, ******************************************************************************/ int16_t retrieveGpsMsg(uint16_t numBytes, GpsData_t *GPSData, uint8_t *outBuffer) { - uart_read(gpsSerialChan, outBuffer, numBytes); - return uart_rxBytesAvailable(gpsSerialChan); + uart_read(gpsChan, outBuffer, numBytes); + return uart_rxBytesAvailable(gpsChan); } /** **************************************************************************** @@ -226,10 +233,10 @@ int16_t findHeader(uint16_t numInBuff, int num; do { - uart_copyBytes(gpsSerialChan,0,1,&byte); + uart_copyBytes(gpsChan,0,1,&byte); if (byte == GPSData->GPSMsgSignature.startByte) { - uart_copyBytes(gpsSerialChan,1,GPSData->GPSMsgSignature.GPSheaderLength - 1, buf); + uart_copyBytes(gpsChan,1,GPSData->GPSMsgSignature.GPSheaderLength - 1, buf); header = byte << (GPSData->GPSMsgSignature.GPSheaderLength - 1) * 8; switch (GPSData->GPSMsgSignature.GPSheaderLength) { case 2: // SiRF 0xa0a2 @@ -242,13 +249,13 @@ int16_t findHeader(uint16_t numInBuff, if ( header == GPSData->GPSMsgSignature.GPSheader ) { exit = 1; } else { - num = uart_removeRxBytes(gpsSerialChan, 1); + num = uart_removeRxBytes(gpsChan, 1); if(num){ numInBuff--; } } } else { - num = uart_removeRxBytes(gpsSerialChan, 1); + num = uart_removeRxBytes(gpsChan, 1); if(num){ numInBuff--; } @@ -267,7 +274,7 @@ int16_t findHeader(uint16_t numInBuff, ******************************************************************************/ int writeGps(char *data, uint16_t len) { - return uart_write(gpsSerialChan, (uint8_t*)data, len); + return uart_write(gpsChan, (uint8_t*)data, len); } @@ -400,10 +407,19 @@ void initGPSHandler(void) gGpsDataPtr->GPSTopLevelConfig |= (1 << HZ2); // update to change internal (ublox) GPS to 2Hz /// Configure GPS structure, from Flash (EEPROM) loadGpsCommSettings(gGpsDataPtr); + if(!gpsUartInitialized){ + gpsUartInitialized = TRUE; initGpsUart(gGpsDataPtr->GPSbaudRate); + } #endif } +void InitGpsSerialCommunication(int baudrate, BOOL fInit) +{ + gGpsDataPtr->GPSbaudRate = baudrate; + gpsChan = platformGetSerialChannel(GPS_SERIAL_PORT, FALSE); + gpsUartInitialized = fInit; +} /** **************************************************************************** @@ -422,6 +438,7 @@ void initGPSDataStruct(void) } +#ifndef NMEA_ONLY BOOL _handleGpsMessages(GpsData_t *GPSData) { static uint8_t gpsUartBuf[100]; @@ -433,7 +450,7 @@ BOOL _handleGpsMessages(GpsData_t *GPSData) while(1){ if(!bytesInBuffer){ - bytesInBuffer = uart_read(gpsSerialChan, gpsUartBuf, sizeof (gpsUartBuf)); + bytesInBuffer = uart_read(gpsChan, gpsUartBuf, sizeof (gpsUartBuf)); if(!bytesInBuffer){ return 0; // nothing to do } @@ -457,6 +474,42 @@ BOOL _handleGpsMessages(GpsData_t *GPSData) } } /* end _handleGpsMessages */ +#endif + + +BOOL _handleGpsNMEAMessages(GpsData_t *GPSData) +{ + static uint8_t gpsUartBuf[100]; + static uint8_t gpsMsg[MAX_MSG_LENGTH]; + static int bytesInBuffer = 0; + unsigned char tmp; + unsigned static int pos = 0; + + // refresh GPS channel + gpsChan = platformGetSerialChannel(GPS_SERIAL_PORT, FALSE); + while(1){ + if(!bytesInBuffer){ + bytesInBuffer = uart_read(gpsChan, gpsUartBuf, sizeof (gpsUartBuf)); + if(!bytesInBuffer){ + return 0; // nothing to do + } + pos = 0; +#ifdef GNSS_TRAFFIC_RELAY + if(platformIsGnssTrafficRelayEnabled()){ + int masterChan = platformGetSerialChannel(MASTER_SERIAL_PORT, TRUE); + uart_write(masterChan, gpsUartBuf, bytesInBuffer); + return 0; + } +#endif + } + tmp = gpsUartBuf[pos++]; + bytesInBuffer--; + platformDetectPingMessageFromGpsDriver(tmp); + parseNMEAMessage(tmp, gpsMsg, GPSData); + } +} +/* end _handleGpsMessages */ + /** **************************************************************************** * @name GPSHandler GPS stream data and return GPS data for NAV algorithms @@ -469,13 +522,16 @@ void GPSHandler(void) { gGpsDataPtr->Timer100Hz10ms = getSystemTime(); ///< get system clock ticks gGpsDataPtr->isGPSBaudrateKnown = 1; - +#ifndef NMEA_ONLY // parse moved to top since is always called after init if (gGpsDataPtr->GPSConfigureOK > 0 ) { // Configuration is completed _handleGpsMessages(gGpsDataPtr); } else { ///< configure GPS receiver if needed: OK < 0 _configGPSReceiver(gGpsDataPtr); } +#else + _handleGpsNMEAMessages(gGpsDataPtr); +#endif } /** **************************************************************************** diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/processNMEAGPS.c b/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/processNMEAGPS.c index f51ada5..6f08c1d 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/processNMEAGPS.c +++ b/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/processNMEAGPS.c @@ -40,6 +40,7 @@ limitations under the License. char _parseGPGGA(char *msgBody, GpsData_t* GPSData); char _parseVTG(char *msgBody, GpsData_t* GPSData); char _parseRMC(char *msgBody, GpsData_t* GPSData); +char _parseZDA(char *msgBody, GpsData_t* GPSData); void _handleNMEAmsg(char *msgID, char *msgBody,GpsData_t* GPSData); void _NMEA2UbloxAndLLA(NmeaLatLonSTRUCT* NmeaData, GpsData_t* GPSData); void _smoothVerticalData(GpsData_t* GPSData); @@ -185,6 +186,10 @@ void _handleNMEAmsg(char *msgID, GPSData->totalVTG++; _parseVTG(msgBody, GPSData); } + if( strncmp(ptr, "ZDA", 3) == 0 ) { + GPSData->totalVTG++; + _parseZDA(msgBody, GPSData); + } if( strncmp(ptr, "RMC", 3) == 0 ) { _parseRMC(msgBody, GPSData); } @@ -312,7 +317,7 @@ char _parseGPGGA(char *msgBody, if (field[6] == '.') { // Some messages have second fraction .SS skip the '.' - GPSData->GPSSecondFraction = ( (field[7] - '0') / 10) + (field[8] - '0') / 100; + GPSData->GPSSecondFraction = ( (field[7] - '0') / 10.0) + (field[8] - '0') / 100.0; } } else status = 1; @@ -501,6 +506,54 @@ char _parseRMC(char *msgBody, return status; } +/** **************************************************************************** + * @name: _parseZDA LOCAL parse GPZDA message. duplicates nav data but has + * calander data needed for WMM (Worl Magnetic Model) lookup + * @author Dong An + * @param [in] msgBody - data to parse + * @param [in] GPSData - data structure to parst he data into + * @retval status + * @details $GPZDA (Recommended Minimum sentence C) + * 172809 Fix taken at 17:28:09 UTC + * 12 Day + * 07 Month + * 1996 Year + * 00 Offset from GMT Hours + * 00 Offset from GMT Minutes + * *45 The checksum data, always begins with '*' + * 07/01/15 dkh convert directly from ascii to decimal + * $GPZDA,172809.456,12,07,1996,00,00*45 + ******************************************************************************/ +char _parseZDA(char *msgBody, + GpsData_t *GPSData) +{ + char field[NMEA_MSG_MAX_FIELD]; + char status = 0; + int parseReset = 1; + + /// Date field 8 + + if( extractNMEAfield(msgBody, field, 1, parseReset) ) { + GPSData->GPSday = ( (field[0] - '0') * 10) + field[1] - '0'; /// month + } else + status = 1; + + if( extractNMEAfield(msgBody, field, 2, parseReset) ) { + GPSData->GPSmonth = ((field[0] - '0') * 10) + field[1] - '0'; /// day + } else + status = 1; + + if( extractNMEAfield(msgBody, field, 3, parseReset) ) { + GPSData->GPSyear = ( (field[2] - '0') * 10) + field[3] - '0'; /// year + } else + status = 1; + + + return status; + +} + + /** **************************************************************************** * @name: computeNMEAChecksum compute the checksum of a NMEA message. * @author Dong An diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/taskGps.c b/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/taskGps.c index 56a62a1..43412e0 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/taskGps.c +++ b/examples/OpenIMU300ZI/IMU/lib/Core/GPS/src/taskGps.c @@ -51,7 +51,7 @@ void TaskGps(void const *argument) int bytesAvailable; static uint32_t updateHDOP, pollSirfCnt; - while(gpsSerialChan == UART_CHANNEL_NONE) { + while(gpsChan == UART_CHANNEL_NONE) { // nothing to do untill port decided OS_Delay( 1000); } @@ -93,4 +93,29 @@ void TaskGps(void const *argument) } } + +/** **************************************************************************** + * @name ProcGps + * @brief task callback with the main loop for handle GPS data, make sure the + * GPS handling function gets called on a regular basis;. + * gCalibration.productConfiguration.bit.hasGps = 1; by setting: + * true and true in name_IMU380.xml file + * @param N/A + * @retval N/A + ******************************************************************************/ +void ProcGps() +{ + static BOOL firstTime = TRUE; + + if(firstTime){ + // start out with the DOP high + firstTime = 0; + gGpsDataPtr->HDOP = 50.0; + initGPSHandler(); + } + + GPSHandler(); + +} + #endif // GPS \ No newline at end of file diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/Odo/odoAPI.h b/examples/OpenIMU300ZI/IMU/lib/Core/Odo/odoAPI.h new file mode 100644 index 0000000..458560b --- /dev/null +++ b/examples/OpenIMU300ZI/IMU/lib/Core/Odo/odoAPI.h @@ -0,0 +1,40 @@ +/** *************************************************************************** + * @file odoApi.h Odometer interface. + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * @brief This is a generalized odometer interface. + *****************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef ODO_API_H +#define ODO_API_H + +#include +#include "GlobalConstants.h" + +typedef struct { + uint8_t update; // 1 if contains new data + real v; // velocity measured by odometer, [m/s] +} odoDataStruct_t; + +extern odoDataStruct_t gOdo; + +#endif /* ODO_API_H */ diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/SteerAlgo/steerAPI.h b/examples/OpenIMU300ZI/IMU/lib/Core/SteerAlgo/steerAPI.h new file mode 100644 index 0000000..c10a0a4 --- /dev/null +++ b/examples/OpenIMU300ZI/IMU/lib/Core/SteerAlgo/steerAPI.h @@ -0,0 +1,43 @@ +/** *************************************************************************** + * @file odoApi.h Odometer interface. + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * @brief This is a generalized odometer interface. + *****************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef STEER_API_H +#define STEER_API_H + +#include +#include "GlobalConstants.h" + +typedef struct { + uint8_t update; // 1 if contains new data + real angle; // streering angle floating point + int16_t intAngle; // streering angle as message - weighted + int16_t sRate; // stering angle rate + uint16_t steerAlgoStates; +} steerDataStruct_t; + +extern steerDataStruct_t gSteerAngle; + +#endif /* ODO_API_H */ diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/UARTComm/CommonMessages.c b/examples/OpenIMU300ZI/IMU/lib/Core/UARTComm/CommonMessages.c index 09cbf0b..6fdfa44 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/UARTComm/CommonMessages.c +++ b/examples/OpenIMU300ZI/IMU/lib/Core/UARTComm/CommonMessages.c @@ -31,6 +31,9 @@ limitations under the License. #include "platformAPI.h" #include "sensorsAPI.h" #include "appVersion.h" +#include "ucb_packet_struct.h" +#include "magAPI.h" +#include "magAlign.h" #include "CommonMessages.h" #include "algorithm.h" @@ -360,3 +363,144 @@ BOOL Fill_e2PacketPayload(uint8_t *payload, uint8_t *payloadLen) return TRUE; } +BOOL Fill_MagAlignResponsePayload(int8_t state, UcbPacketStruct *ptrUcbPacket) +{ + int8_t estimatedMagAlignVals[8] = {0}; + int8_t magAlignVals[8] = {0}; + BOOL valid = TRUE; + + switch (state - 1) + { + // + uint8_t len; + + case 0: // Return the Mag-Align status + //uint8_t *model = (uint8_t*)unitVersionString(); + //uint8_t *rev = (uint8_t*)platformBuildInfo(); + //unsigned int serialNum = unitSerialNumber(); + //len = snprintf((char*)ptrUcbPacket->payload, 250, "%s %s SN:%u", model, rev, serialNum ); + //ptrUcbPacket->payloadLength = len; + if (gMagAlign.state == MAG_ALIGN_STATUS_START_CAL_WITH_AUTOEND) + { + // Start (auto end) + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)0x1); + } + else if (gMagAlign.state == MAG_ALIGN_STATUS_START_CAL_WITHOUT_AUTOEND) + { + // Start (manual end) + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)0x2); + } + else + { + // Start (manual end) + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)0x0); + } + + ptrUcbPacket->payloadLength = len; + break; + + case 1: // Start mag-align w/ autoend + case 2: // Start mag-align w/ autoend + case 3: // Stop mag-align w/ autoend + case 4: // Accept results + case 5: // Accept results and write to EEPROM + case 6: // Abort Mag-Align or reject results + case 8: // Restore default mag-align values + case 9: // Restore default mag-align values and save in EEPROM + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)state - 1); + ptrUcbPacket->payloadLength = len; + break; + + case 7: // Return stored mag-align values +#if 0 + // Test values: + gMagAlign.estParams.hardIronBias[X_AXIS] = 0.1; + gMagAlign.estParams.hardIronBias[Y_AXIS] = -0.2; + gMagAlign.estParams.softIronScaleRatio = 0.98; + gMagAlign.estParams.softIronAngle = -270.0 * DEG_TO_RAD; +#endif + + // Bias can be +/- 8.0 [g] (full scale of sensor) + // SF = 2^15 / maxVal = 2^15 / 8.0 = 4096 + magAlignVals[0] = (char)(((int16_t)(gMagAlign.hardIronBias[X_AXIS] * (float)4096.0) >> 8) & 0xFF); + magAlignVals[1] = (char)(((int16_t)(gMagAlign.hardIronBias[X_AXIS] * (float)4096.0) >> 0) & 0xFF); + magAlignVals[2] = (char)(((int16_t)(gMagAlign.hardIronBias[Y_AXIS] * (float)4096.0) >> 8) & 0xFF); + magAlignVals[3] = (char)(((int16_t)(gMagAlign.hardIronBias[Y_AXIS] * (float)4096.0) >> 0) & 0xFF); + + // Ratio can be 0 --> 1 + // SF = (2^16-1) / maxVal = (2^16-1) / 1.0 = 65535 + magAlignVals[4] = (char)(((int16_t)(gMagAlign.softIronScaleRatio * (float)65535.0) >> 8) & 0xFF); + magAlignVals[5] = (char)(((int16_t)(gMagAlign.softIronScaleRatio * (float)65535.0) >> 0) & 0xFF); + + // SF = 2^15 / maxVal = 2^15 / pi = 10430.37835047045 + magAlignVals[6] = (char)(((int16_t)(gMagAlign.softIronAngle * (float)10430.37835047046) >> 8) & 0xFF); + magAlignVals[7] = (char)(((int16_t)(gMagAlign.softIronAngle * (float)10430.37835047046) >> 0) & 0xFF); + + // Bias can be +/- 8.0 [g] (full scale of sensor) + // SF = 2^15 / maxVal = 2^15 / 8.0 = 4096 + estimatedMagAlignVals[0] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[X_AXIS] * (float)4096.0) >> 8) & 0xFF); + estimatedMagAlignVals[1] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[X_AXIS] * (float)4096.0) >> 0) & 0xFF); + estimatedMagAlignVals[2] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[Y_AXIS] * (float)4096.0) >> 8) & 0xFF); + estimatedMagAlignVals[3] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[Y_AXIS] * (float)4096.0) >> 0) & 0xFF); + + // Ratio can be 0 --> 1 + // SF = (2^16-1) / maxVal = (2^16-1) / 1.0 = 65535 + estimatedMagAlignVals[4] = (char)(((int16_t)(gMagAlign.estParams.softIronScaleRatio * (float)65535.0) >> 8) & 0xFF); + estimatedMagAlignVals[5] = (char)(((int16_t)(gMagAlign.estParams.softIronScaleRatio * (float)65535.0) >> 0) & 0xFF); + + // Angle can be +/- pi (in radians) + // Correct for angles that exceed +/-180 + if (gMagAlign.estParams.softIronAngle > PI) + { + gMagAlign.estParams.softIronAngle = (float)PI - gMagAlign.estParams.softIronAngle; + } + else if (gMagAlign.estParams.softIronAngle < -PI) + { + gMagAlign.estParams.softIronAngle = TWO_PI + gMagAlign.estParams.softIronAngle; + } + + // SF = 2^15 / maxVal = 2^15 / pi = 10430.37835047045 + estimatedMagAlignVals[6] = (char)(((int16_t)(gMagAlign.estParams.softIronAngle * (float)10430.37835047046) >> 8) & 0xFF); + estimatedMagAlignVals[7] = (char)(((int16_t)(gMagAlign.estParams.softIronAngle * (float)10430.37835047046) >> 0) & 0xFF); + +#if 0 + DebugPrintFloat(" ", (float)estimatedMagAlignVals[0], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[1], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[2], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[3], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[4], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[5], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[6], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[7], 1); + DebugPrintEndline(); +#endif + + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", (char)magAlignVals[0], + (char)magAlignVals[1], + (char)magAlignVals[2], + (char)magAlignVals[3], + (char)magAlignVals[4], + (char)magAlignVals[5], + (char)magAlignVals[6], + (char)magAlignVals[7], + (char)estimatedMagAlignVals[0], + (char)estimatedMagAlignVals[1], + (char)estimatedMagAlignVals[2], + (char)estimatedMagAlignVals[3], + (char)estimatedMagAlignVals[4], + (char)estimatedMagAlignVals[5], + (char)estimatedMagAlignVals[6], + (char)estimatedMagAlignVals[7]); + + ptrUcbPacket->payloadLength = len; + break; + + case 10: // Load user computed mag-align values + break; + + default: + valid = FALSE; + break; + } + return valid; +} diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/UARTComm/CommonMessages.h b/examples/OpenIMU300ZI/IMU/lib/Core/UARTComm/CommonMessages.h index 0f373f0..b425784 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/UARTComm/CommonMessages.h +++ b/examples/OpenIMU300ZI/IMU/lib/Core/UARTComm/CommonMessages.h @@ -122,5 +122,6 @@ BOOL Fill_a2PacketPayload(uint8_t *payload, uint8_t *payloadLen); BOOL Fill_s1PacketPayload(uint8_t *payload, uint8_t *payloadLen); BOOL Fill_e1PacketPayload(uint8_t *payload, uint8_t *payloadLen); BOOL Fill_e2PacketPayload(uint8_t *payload, uint8_t *payloadLen); +BOOL Fill_MagAlignResponsePayload(int8_t state, UcbPacketStruct *ptrUcbPacket); #endif diff --git a/examples/OpenIMU300ZI/IMU/lib/Core/library.json b/examples/OpenIMU300ZI/IMU/lib/Core/library.json index 152eebf..cea1786 100644 --- a/examples/OpenIMU300ZI/IMU/lib/Core/library.json +++ b/examples/OpenIMU300ZI/IMU/lib/Core/library.json @@ -1,6 +1,6 @@ { "name": "Core", - "version": "1.0.5", + "version": "1.0.6", "description": "Algorithm and GPS libraries", "build": { "flags": [ @@ -12,6 +12,10 @@ "-I Common/include", "-I GPS", "-I GPS/include", + "-I Odo", + "-I Odo/include", + "-I SteerAlgo", + "-I SteerAlgo/include", "-I Math", "-I Math/include", "-I SPIComm", diff --git a/examples/OpenIMU300ZI/IMU/openimu.json b/examples/OpenIMU300ZI/IMU/openimu.json index 735192e..d9bc352 100644 --- a/examples/OpenIMU300ZI/IMU/openimu.json +++ b/examples/OpenIMU300ZI/IMU/openimu.json @@ -1,5 +1,5 @@ { - "appVersion": "OpenIMU300ZI IMU 1.1.3", + "appVersion": "OpenIMU300ZI IMU 04.01.04", "description": "9-axis OpenIMU with triaxial rate, acceleration, and magnetic measurement", "userConfiguration": [ { "paramId": 0, "category": "General", "paramType": "disabled", "type": "uint64", "name": "Data CRC" }, diff --git a/examples/OpenIMU300ZI/IMU/platformio.ini b/examples/OpenIMU300ZI/IMU/platformio.ini index 4d5822b..2ccf0a1 100644 --- a/examples/OpenIMU300ZI/IMU/platformio.ini +++ b/examples/OpenIMU300ZI/IMU/platformio.ini @@ -14,14 +14,16 @@ description = An IMU provide calibrated inertial sensor data to the user. platform = aceinna_imu lib_archive = false board = OpenIMU300 -lib_deps = OpenIMU300-base-library@1.0.9 -;lib_deps= ../../openIMU300-lib +;lib_deps = ../../../openIMU300-lib +lib_deps = OpenIMU300-base-library@1.1.13 build_flags = ; -D CLI + -D IMU_ONLY -D __FPU_PRESENT -D ARM_MATH_CM4 -D SPI_BUS_COMM -I src/user + -I include -I src -Og ; -Wno-comment @@ -33,7 +35,10 @@ build_flags = ;monitor_port = COM38 ;monitor_speed = 38400 -;uncomment next lines if jlink tool used ;upload_protocol = jlink ;debug_tool = jlink + +;debug_tool = custom +;debug_port = :4242 +;debug_server = $PLATFORMIO_HOME_DIR/packages/tool-stlink/bin/st-util diff --git a/examples/OpenIMU300ZI/IMU/src/appVersion.h b/examples/OpenIMU300ZI/IMU/src/appVersion.h index 8865789..4d37adb 100644 --- a/examples/OpenIMU300ZI/IMU/src/appVersion.h +++ b/examples/OpenIMU300ZI/IMU/src/appVersion.h @@ -26,8 +26,8 @@ limitations under the License. #ifndef _IMU_APP_VERSION_H #define _IMU_APP_VERSION_H -#define APP_VERSION_STRING "OpenIMU300ZI IMU 1.1.3" -#define APP_SPI_SW_VERSION 113 +#define APP_VERSION_STRING "OpenIMU300ZI IMU 04.01.04" +#define APP_SPI_SW_VERSION 115 #endif diff --git a/examples/OpenIMU300ZI/IMU/src/main.c b/examples/OpenIMU300ZI/IMU/src/main.c index be40679..5321536 100644 --- a/examples/OpenIMU300ZI/IMU/src/main.c +++ b/examples/OpenIMU300ZI/IMU/src/main.c @@ -81,7 +81,7 @@ void DebugInterfaceInit(void) { char status[100]; - int debugChannel = platformGetSerialChannel(DEBUG_SERIAL_PORT); + int debugChannel = platformGetSerialChannel(DEBUG_SERIAL_PORT, TRUE); if(debugChannel == UART_CHANNEL_NONE){ //nothing to do @@ -89,7 +89,7 @@ void DebugInterfaceInit(void) } // Initialize the DEBUG USART (serial) port - InitDebugSerialCommunication(38400); + InitDebugSerialCommunication(115200); BoardGetResetStatus(status, sizeof(status)); diff --git a/examples/OpenIMU300ZI/IMU/src/taskDataAcquisition.c b/examples/OpenIMU300ZI/IMU/src/taskDataAcquisition.c index 4f6c5a0..328e830 100644 --- a/examples/OpenIMU300ZI/IMU/src/taskDataAcquisition.c +++ b/examples/OpenIMU300ZI/IMU/src/taskDataAcquisition.c @@ -80,11 +80,11 @@ void TaskDataAcquisition(void const *argument) // ***************************************************************** // NOTE: This task loop runs at 200 Hz // ***************************************************************** - + // Handle Timing vard, watchdog and BIT PrepareToNewDacqTick(); // Wait for next tick - // Upon timeout of TIM2 (or user sync), let the process continue + // Upon timeout or user sync let the process continue res = osSemaphoreWait(dataAcqSem, 1000); if(res != osOK){ // Wait timeout expired. Something wrong wit the dacq system @@ -96,9 +96,8 @@ void TaskDataAcquisition(void const *argument) // in case of UART communication interface sets pin IO2 high if(platformGetUnitCommunicationType() != UART_COMM){ setDataReadyPin(1); - }else{ - setIO2Pin (1); } + setIO2Pin (1); // Get calibrated sensor data: @@ -144,6 +143,7 @@ void TaskDataAcquisition(void const *argument) //applyNewScaledSensorsData(); //***************************************************************** + setIO2Pin (0); if(platformHasMag() ) { // Mag Alignment (follows Kalman filter or user algorithm as the // innovation routine calculates the euler angles and the magnetic @@ -169,8 +169,6 @@ void TaskDataAcquisition(void const *argument) }else { // Process user commands and output continuous packets to UART // Processing of user commands always goes first - // Inform user, that new data set is ready (if required) - setIO2Pin (0); ProcessUserCommands (); SendContinuousPacket(200); } diff --git a/examples/OpenIMU300ZI/IMU/src/user/UserConfiguration.c b/examples/OpenIMU300ZI/IMU/src/user/UserConfiguration.c index 171be90..d763edf 100644 --- a/examples/OpenIMU300ZI/IMU/src/user/UserConfiguration.c +++ b/examples/OpenIMU300ZI/IMU/src/user/UserConfiguration.c @@ -46,6 +46,8 @@ const UserConfigurationStruct gDefaultUserConfig = { .lpfRateFilterFreq = 25, .orientation = "+X+Y+Z", // add default parameter values here, if desired + .uartGpsBaudRate = 0, + .uartGpsProtocol = 0, .hardIron_X = 0.0, .hardIron_Y = 0.0, .softIron_Ratio = 1.0, @@ -152,6 +154,7 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) { BOOL result = TRUE; uint64_t *ptr = (uint64_t *)&gUserConfiguration; + uint16_t orientOut; if(number < USER_CRC || number >= USER_MAX_PARAM ){ return FALSE; @@ -174,7 +177,7 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) result = platformSelectLPFilter(RATE_SENSOR, (uint32_t)data, fApply); break; case USER_ORIENTATION: - result = platformSetOrientation((uint16_t*)&data, fApply); + result = platformSetOrientation((uint16_t*)&data, &orientOut, fApply); break; case USER_CRC: case USER_DATA_SIZE: diff --git a/examples/OpenIMU300ZI/IMU/src/user/UserConfiguration.h b/examples/OpenIMU300ZI/IMU/src/user/UserConfiguration.h index c7bea7a..52b8e48 100644 --- a/examples/OpenIMU300ZI/IMU/src/user/UserConfiguration.h +++ b/examples/OpenIMU300ZI/IMU/src/user/UserConfiguration.h @@ -76,6 +76,19 @@ typedef struct { // parameter size should even to 8 bytes // Add parameter offset in UserConfigParamOffset structure if validation or // special processing required + int64_t uartGpsBaudRate; /// baudrate of GPS UART, bps. + /// valid options are: + /// 4800 + /// 9600 + /// 19200 + /// 38400 + /// 57600 + /// 115200 + /// 230400 + int64_t uartGpsProtocol; /// protocol of GPS receicer. + /// so far valid options are: + /// NMEA_TEXT + /// NOVATEL_BINARY double hardIron_X; double hardIron_Y; @@ -157,6 +170,8 @@ typedef enum{ USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, //***************************************************************************************** // add parameter enumerator here while adding new parameter in user UserConfigurationStruct + USER_GPS_BAUD_RATE , // 8 + USER_GPS_PROTOCOL , // 9 USER_HARD_IRON_X , USER_HARD_IRON_Y , USER_SOFT_IRON_RATIO , diff --git a/examples/OpenIMU300ZI/IMU/src/user/UserMessagingUART.c b/examples/OpenIMU300ZI/IMU/src/user/UserMessagingUART.c index 0eacf1b..4b513f8 100644 --- a/examples/OpenIMU300ZI/IMU/src/user/UserMessagingUART.c +++ b/examples/OpenIMU300ZI/IMU/src/user/UserMessagingUART.c @@ -38,6 +38,7 @@ limitations under the License. #include "Indices.h" // For X_AXIS, etc #include "CommonMessages.h" +#include "magAPI.h" // Declare the IMU data structure IMUDataStruct gIMU; @@ -63,8 +64,9 @@ usr_packet_t userInputPackets[] = { {USR_IN_GET_ALL, "gA"}, {USR_IN_GET_VERSION, "gV"}, {USR_IN_RESET, "rS"}, - {USR_IN_UPDATE_BIAS, "uB"}, +// {USR_IN_UPDATE_BIAS, "uB"}, // place new input packet code here, before USR_IN_MAX + {USR_IN_MAG_ALIGN, "ma"}, // 0x6D 0x61 {USR_IN_MAX, {0xff, 0xff}}, // "" }; @@ -220,6 +222,7 @@ int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) { BOOL valid = TRUE; int ret = USER_PACKET_OK; + uint8_t retVal; // userPacket *pkt = (userPacket *)ptrUcbPacket->payload; /// call appropriate function based on packet type @@ -275,8 +278,9 @@ int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) valid = FALSE; } break; - case USR_IN_UPDATE_BIAS: - UpdateBias(); + case USR_IN_MAG_ALIGN: + retVal = ProcessMagAlignCmds((magAlignCmdPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + valid = Fill_MagAlignResponsePayload(retVal, ptrUcbPacket); break; default: /// default handler - unknown packet diff --git a/examples/OpenIMU300ZI/IMU/src/user/UserMessagingUART.h b/examples/OpenIMU300ZI/IMU/src/user/UserMessagingUART.h index 142ed24..311f24f 100644 --- a/examples/OpenIMU300ZI/IMU/src/user/UserMessagingUART.h +++ b/examples/OpenIMU300ZI/IMU/src/user/UserMessagingUART.h @@ -67,9 +67,9 @@ typedef enum { USR_IN_GET_ALL , USR_IN_GET_VERSION , USR_IN_RESET , - USR_IN_UPDATE_BIAS , // add new packet type here, before USR_IN_MAX - USR_IN_MAX , + USR_IN_MAG_ALIGN , + USR_IN_MAX }UserInPacketType; // User output packet codes, change at will diff --git a/examples/OpenIMU300ZI/IMU/src/user/dataProcessingAndPresentation.c b/examples/OpenIMU300ZI/IMU/src/user/dataProcessingAndPresentation.c index 8f71404..c8689bd 100644 --- a/examples/OpenIMU300ZI/IMU/src/user/dataProcessingAndPresentation.c +++ b/examples/OpenIMU300ZI/IMU/src/user/dataProcessingAndPresentation.c @@ -36,7 +36,6 @@ limitations under the License. static void _IncrementIMUTimer(uint16_t dacqRate); static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); static void _IMUDebugMessage(void); -BOOL fUpdateBias = FALSE; /* * diff --git a/examples/OpenIMU300ZI/INS/lib/Core/.pio/.piopkgmanager.json b/examples/OpenIMU300ZI/INS/lib/Core/.pio/.piopkgmanager.json deleted file mode 100644 index 771f01d..0000000 --- a/examples/OpenIMU300ZI/INS/lib/Core/.pio/.piopkgmanager.json +++ /dev/null @@ -1 +0,0 @@ -{"name": "Lib", "url": "file://../../../Lib", "requirements": null} \ No newline at end of file diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/algorithmAPI.h b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/algorithmAPI.h index 302c9ab..d1c8e18 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/algorithmAPI.h +++ b/examples/OpenIMU300ZI/INS/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. @@ -72,6 +72,18 @@ void enableOdoInAlgorithm(BOOL enable); BOOL odoUsedInAlgorithm(); +void enableFreeIntegration(BOOL enable); + +BOOL freeIntegrationEnabled(); + +void enableStationaryLockYaw(BOOL enable); + +BOOL stationaryLockYawEnabled(); + +void enableImuStaticDetect(BOOL enable); + +BOOL imuStaticDetectEnabled(); + void SetAlgorithmUseDgps(BOOL d); void updateAlgorithmTimings(int corr, uint32_t tmrVal ); @@ -81,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(); /****************************************************************************** diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/EKF_Algorithm.h b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/EKF_Algorithm.h index 6c0e946..4028640 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/EKF_Algorithm.h +++ b/examples/OpenIMU300ZI/INS/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_ */ diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/MotionStatus.h b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/MotionStatus.h index 785ac68..57e4fcf 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/MotionStatus.h +++ b/examples/OpenIMU300ZI/INS/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. diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/TimingVars.h b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/TimingVars.h index 1799cbc..5f545f0 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/TimingVars.h +++ b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/TimingVars.h @@ -33,6 +33,7 @@ float TimingVars_GetTime(void); void TimingVars_SetTMin(float tMin); float TimingVars_GetTMin(void); +void TimingVars_dacqFrequency(int freq); void TimingVars_DisplayTimerVars(signed long timeStep); diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/algorithm.h b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/algorithm.h index 8b92c36..28bae01 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/include/algorithm.h +++ b/examples/OpenIMU300ZI/INS/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; diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/EKF_Algorithm.c b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/EKF_Algorithm.c index e3fdb87..2874e27 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/EKF_Algorithm.c +++ b/examples/OpenIMU300ZI/INS/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); } @@ -299,54 +301,6 @@ static void SaveKfStateAtPps() } } -void enableFreeIntegration(BOOL enable) -{ - gAlgorithm.Behavior.bit.freeIntegrate = enable; -} - - -BOOL freeIntegrationEnabled() -{ - return (BOOL)gAlgorithm.Behavior.bit.freeIntegrate; -} - -void enableMagInAlgorithm(BOOL enable) -{ - if(1) - { - gAlgorithm.Behavior.bit.useMag = enable; - } - else - { - gAlgorithm.Behavior.bit.useMag = FALSE; - } -} - -BOOL magUsedInAlgorithm() -{ - return gAlgorithm.Behavior.bit.useMag != 0; -} - -BOOL gpsUsedInAlgorithm(void) -{ - return (BOOL)gAlgorithm.Behavior.bit.useGPS; -} - -void enableGpsInAlgorithm(BOOL enable) -{ - gAlgorithm.Behavior.bit.useGPS = enable; -} - -BOOL odoUsedInAlgorithm(void) -{ - return (BOOL)gAlgorithm.Behavior.bit.useOdo; -} - -void enableOdoInAlgorithm(BOOL enable) -{ - gAlgorithm.Behavior.bit.useOdo = enable; -} - // Getters based on results structure passed to WriteResultsIntoOutputStream() /* Extract the attitude (expressed as Euler-angles) of the body-frame (B) diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/MotionStatus.c b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/MotionStatus.c index 220f0cd..017d8e9 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/MotionStatus.c +++ b/examples/OpenIMU300ZI/INS/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; -} \ No newline at end of file +} diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/PredictFunctions.c b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/PredictFunctions.c index e728dca..8f56b75 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/PredictFunctions.c +++ b/examples/OpenIMU300ZI/INS/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]; } } diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/TimingVars.c b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/TimingVars.c index 2942dfb..f4de8d9 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/TimingVars.c +++ b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/TimingVars.c @@ -162,3 +162,7 @@ void Initialize_Timing(void) timer.dacqFrequency = DACQ_200_HZ; // default } +void TimingVars_dacqFrequency(int freq) +{ + timer.dacqFrequency = freq; +} diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/UpdateFunctions.c b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/UpdateFunctions.c index 2759483..5519909 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/UpdateFunctions.c +++ b/examples/OpenIMU300ZI/INS/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; diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/algorithm.c b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/algorithm.c index 99be594..e53ca66 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/algorithm.c +++ b/examples/OpenIMU300ZI/INS/lib/Core/Algorithm/src/algorithm.c @@ -27,10 +27,20 @@ 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----------------------------- - gAlgorithm.Behavior.bit.freeIntegrate = FALSE; // The calling frequency drives the execution rate of the EKF and dictates // the algorithm constants if(callingFreq == 0){ @@ -97,7 +107,10 @@ void InitializeAlgorithmStruct(uint8_t callingFreq) // Linear acceleration switch limits (level and time) gAlgorithm.Limit.accelSwitch = (real)(0.012); // [g] - gAlgorithm.Limit.linAccelSwitchDelay = (uint32_t)(2.0 * gAlgorithm.callingFreq); + 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; @@ -112,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; @@ -128,13 +152,30 @@ void InitializeAlgorithmStruct(uint8_t callingFreq) // For most vehicles, the velocity is always along the body x axis gAlgorithm.velocityAlwaysAlongBodyX = TRUE; - // enable yaw lock when vehicle is static - gAlgorithm.Behavior.bit.enableStationaryLockYaw = TRUE; - - // enable zero velocity detection by IMU - gAlgorithm.Behavior.bit.enableImuStaticDetect = 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; @@ -143,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); @@ -153,8 +197,12 @@ void InitializeAlgorithmStruct(uint8_t callingFreq) gAlgorithm.staticDetectParam.staticNoiseMultiplier[1] = 4.0; gAlgorithm.staticDetectParam.staticNoiseMultiplier[2] = 1.0; + gAlgorithm.Behavior.bit.dynamicMotion = 1; + //----------------------------algorithm states----------------------------- memset(&gAlgoStatus, 0, sizeof(gAlgoStatus)); +#endif + } void GetAlgoStatus(AlgoStatus *algoStatus) @@ -230,4 +278,71 @@ void UpdateImuSpec(real rwOdr, real arw, real biw, real maxBiasW, gAlgorithm.staticDetectParam.staticVarGyro = (real)(gAlgorithm.imuSpec.sigmaW * gAlgorithm.imuSpec.sigmaW); gAlgorithm.staticDetectParam.staticVarAccel = (real)(gAlgorithm.imuSpec.sigmaA * gAlgorithm.imuSpec.sigmaA); gAlgorithm.staticDetectParam.maxGyroBias = gAlgorithm.imuSpec.maxBiasW; +} + +void enableMagInAlgorithm(BOOL enable) +{ + if (1) + { + gAlgorithm.Behavior.bit.useMag = enable; + } + else + { + gAlgorithm.Behavior.bit.useMag = FALSE; + } +} + +void enableGpsInAlgorithm(BOOL enable) +{ + gAlgorithm.Behavior.bit.useGPS = enable; +} + +void enableOdoInAlgorithm(BOOL enable) +{ + gAlgorithm.Behavior.bit.useOdo = enable; +} + +BOOL magUsedInAlgorithm() +{ + return gAlgorithm.Behavior.bit.useMag != 0; +} + +BOOL gpsUsedInAlgorithm(void) +{ + return (BOOL)gAlgorithm.Behavior.bit.useGPS; +} + +BOOL odoUsedInAlgorithm(void) +{ + return (BOOL)gAlgorithm.Behavior.bit.useOdo; +} + +void enableFreeIntegration(BOOL enable) +{ + gAlgorithm.Behavior.bit.freeIntegrate = enable; +} + +BOOL freeIntegrationEnabled() +{ + return (BOOL)gAlgorithm.Behavior.bit.freeIntegrate; +} + +void enableStationaryLockYaw(BOOL enable) +{ + gAlgorithm.Behavior.bit.enableStationaryLockYaw = enable; +} + +BOOL stationaryLockYawEnabled() +{ + return (BOOL)gAlgorithm.Behavior.bit.enableStationaryLockYaw; +} + +void enableImuStaticDetect(BOOL enable) +{ + gAlgorithm.Behavior.bit.enableImuStaticDetect = enable; +} + +BOOL imuStaticDetectEnabled() +{ + return (BOOL)gAlgorithm.Behavior.bit.enableImuStaticDetect; } \ No newline at end of file diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Common/include/GlobalConstants.h b/examples/OpenIMU300ZI/INS/lib/Core/Common/include/GlobalConstants.h index b7ec1e5..afdbdf0 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Common/include/GlobalConstants.h +++ b/examples/OpenIMU300ZI/INS/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 diff --git a/examples/OpenIMU300ZI/INS/lib/Core/Common/include/ucb_packet_struct.h b/examples/OpenIMU300ZI/INS/lib/Core/Common/include/ucb_packet_struct.h index 066ee3f..506a31d 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/Common/include/ucb_packet_struct.h +++ b/examples/OpenIMU300ZI/INS/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 diff --git a/examples/OpenIMU300ZI/INS/lib/Core/GPS/gpsAPI.h b/examples/OpenIMU300ZI/INS/lib/Core/GPS/gpsAPI.h index 3a24ea4..04091d0 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/GPS/gpsAPI.h +++ b/examples/OpenIMU300ZI/INS/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 */ diff --git a/examples/OpenIMU300ZI/INS/lib/Core/GPS/include/driverGPS.h b/examples/OpenIMU300ZI/INS/lib/Core/GPS/include/driverGPS.h index c7681a8..552c24b 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/GPS/include/driverGPS.h +++ b/examples/OpenIMU300ZI/INS/lib/Core/GPS/include/driverGPS.h @@ -218,8 +218,8 @@ typedef struct { int lon_deg; int lon_min; double lon_min_fraction; - char latSign; - char lonSign; + int8_t latSign; + int8_t lonSign; } NmeaLatLonSTRUCT; // delta struct diff --git a/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/driverGPS.c b/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/driverGPS.c index 89416a6..daac33e 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/driverGPS.c +++ b/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/driverGPS.c @@ -37,8 +37,9 @@ limitations under the License. #include "debug.h" #endif -int gpsSerialChan = UART_CHANNEL_NONE; // undefined, needs to be explicitly defined - +int gpsChan = UART_CHANNEL_NONE; // undefined, needs to be explicitly defined +BOOL _handleGpsMessages(GpsData_t *GPSData); +BOOL gpsUartInitialized = FALSE; /// GPS data struct // to change to NMEA then pass GPS out debug port: un-comment this and @@ -60,6 +61,11 @@ int16_t _getIndexLineFeed(uint16_t numInBuff, unsigned *msgLength); void _setGPSMessageSignature(GpsData_t* GPSData); void _userCmdBaudProtcol(GpsData_t* GPSData); +/******************************************* + * @brief + * + * @param GPSData == +********************************************/ void loadGpsCommSettings(GpsData_t* GPSData) { GPSData->GPSAUTOSetting = 0; @@ -67,6 +73,7 @@ void loadGpsCommSettings(GpsData_t* GPSData) GPSData->GPSConfigureOK = 0; } + // extern_port.c extern void GetGpsExternUartChannel(unsigned int* uartChannel); @@ -77,8 +84,8 @@ extern void GetGpsExternUartChannel(unsigned int* uartChannel); ******************************************************************************/ int initGpsUart(int baudrate) { - gpsSerialChan = platformGetSerialChannel(GPS_SERIAL_PORT); - return uart_init(gpsSerialChan, baudrate); + gpsChan = platformGetSerialChannel(GPS_SERIAL_PORT, FALSE); + return uart_init(gpsChan, baudrate); } @@ -90,7 +97,7 @@ int initGpsUart(int baudrate) ******************************************************************************/ int gpsBytesAvailable() { - return uart_rxBytesAvailable(gpsSerialChan); + return uart_rxBytesAvailable(gpsChan); } /** **************************************************************************** @@ -100,7 +107,7 @@ int gpsBytesAvailable() ******************************************************************************/ void flushGPSRecBuf(void) { - uart_flushRecBuffer(gpsSerialChan); + uart_flushRecBuffer(gpsChan); } /** **************************************************************************** @@ -110,7 +117,7 @@ void flushGPSRecBuf(void) ******************************************************************************/ BOOL isGpsTxEmpty(void) { - return uart_txBytesRemains(gpsSerialChan) == 0; + return uart_txBytesRemains(gpsChan) == 0; } /** **************************************************************************** @@ -122,11 +129,11 @@ uint16_t delBytesGpsBuf(uint16_t numToPop) { int16_t numInBuffer; - numInBuffer = uart_rxBytesAvailable(gpsSerialChan); + numInBuffer = uart_rxBytesAvailable(gpsChan); if (numInBuffer < numToPop) { numToPop = numInBuffer; } - uart_removeRxBytes(gpsSerialChan, numToPop); + uart_removeRxBytes(gpsChan, numToPop); // COM_buf_delete(&(gGpsUartPtr->rec_buf), // numToPop); return ( numInBuffer - numToPop ); ///< unscanned bytes @@ -158,7 +165,7 @@ uint8_t peekByteGpsBuf(uint16_t index) { uint8_t output = 0; - uart_copyBytes(gpsSerialChan, index, 1, &output); + uart_copyBytes(gpsChan, index, 1, &output); return output; } @@ -181,7 +188,7 @@ unsigned long peekGPSmsgHeader(uint16_t index, unsigned long GPSHeader; headerLength = GPSData->GPSMsgSignature.GPSheaderLength; - uart_copyBytes(gpsSerialChan, index, headerLength, header); + uart_copyBytes(gpsChan, index, headerLength, header); GPSHeader = 0; for (i = 0, j = headerLength - 1; i < headerLength; i++, j--) { @@ -203,8 +210,8 @@ unsigned long peekGPSmsgHeader(uint16_t index, ******************************************************************************/ int16_t retrieveGpsMsg(uint16_t numBytes, GpsData_t *GPSData, uint8_t *outBuffer) { - uart_read(gpsSerialChan, outBuffer, numBytes); - return uart_rxBytesAvailable(gpsSerialChan); + uart_read(gpsChan, outBuffer, numBytes); + return uart_rxBytesAvailable(gpsChan); } /** **************************************************************************** @@ -226,10 +233,10 @@ int16_t findHeader(uint16_t numInBuff, int num; do { - uart_copyBytes(gpsSerialChan,0,1,&byte); + uart_copyBytes(gpsChan,0,1,&byte); if (byte == GPSData->GPSMsgSignature.startByte) { - uart_copyBytes(gpsSerialChan,1,GPSData->GPSMsgSignature.GPSheaderLength - 1, buf); + uart_copyBytes(gpsChan,1,GPSData->GPSMsgSignature.GPSheaderLength - 1, buf); header = byte << (GPSData->GPSMsgSignature.GPSheaderLength - 1) * 8; switch (GPSData->GPSMsgSignature.GPSheaderLength) { case 2: // SiRF 0xa0a2 @@ -242,13 +249,13 @@ int16_t findHeader(uint16_t numInBuff, if ( header == GPSData->GPSMsgSignature.GPSheader ) { exit = 1; } else { - num = uart_removeRxBytes(gpsSerialChan, 1); + num = uart_removeRxBytes(gpsChan, 1); if(num){ numInBuff--; } } } else { - num = uart_removeRxBytes(gpsSerialChan, 1); + num = uart_removeRxBytes(gpsChan, 1); if(num){ numInBuff--; } @@ -267,7 +274,7 @@ int16_t findHeader(uint16_t numInBuff, ******************************************************************************/ int writeGps(char *data, uint16_t len) { - return uart_write(gpsSerialChan, (uint8_t*)data, len); + return uart_write(gpsChan, (uint8_t*)data, len); } @@ -400,10 +407,19 @@ void initGPSHandler(void) gGpsDataPtr->GPSTopLevelConfig |= (1 << HZ2); // update to change internal (ublox) GPS to 2Hz /// Configure GPS structure, from Flash (EEPROM) loadGpsCommSettings(gGpsDataPtr); + if(!gpsUartInitialized){ + gpsUartInitialized = TRUE; initGpsUart(gGpsDataPtr->GPSbaudRate); + } #endif } +void InitGpsSerialCommunication(int baudrate, BOOL fInit) +{ + gGpsDataPtr->GPSbaudRate = baudrate; + gpsChan = platformGetSerialChannel(GPS_SERIAL_PORT, FALSE); + gpsUartInitialized = fInit; +} /** **************************************************************************** @@ -422,6 +438,7 @@ void initGPSDataStruct(void) } +#ifndef NMEA_ONLY BOOL _handleGpsMessages(GpsData_t *GPSData) { static uint8_t gpsUartBuf[100]; @@ -433,7 +450,7 @@ BOOL _handleGpsMessages(GpsData_t *GPSData) while(1){ if(!bytesInBuffer){ - bytesInBuffer = uart_read(gpsSerialChan, gpsUartBuf, sizeof (gpsUartBuf)); + bytesInBuffer = uart_read(gpsChan, gpsUartBuf, sizeof (gpsUartBuf)); if(!bytesInBuffer){ return 0; // nothing to do } @@ -457,6 +474,42 @@ BOOL _handleGpsMessages(GpsData_t *GPSData) } } /* end _handleGpsMessages */ +#endif + + +BOOL _handleGpsNMEAMessages(GpsData_t *GPSData) +{ + static uint8_t gpsUartBuf[100]; + static uint8_t gpsMsg[MAX_MSG_LENGTH]; + static int bytesInBuffer = 0; + unsigned char tmp; + unsigned static int pos = 0; + + // refresh GPS channel + gpsChan = platformGetSerialChannel(GPS_SERIAL_PORT, FALSE); + while(1){ + if(!bytesInBuffer){ + bytesInBuffer = uart_read(gpsChan, gpsUartBuf, sizeof (gpsUartBuf)); + if(!bytesInBuffer){ + return 0; // nothing to do + } + pos = 0; +#ifdef GNSS_TRAFFIC_RELAY + if(platformIsGnssTrafficRelayEnabled()){ + int masterChan = platformGetSerialChannel(MASTER_SERIAL_PORT, TRUE); + uart_write(masterChan, gpsUartBuf, bytesInBuffer); + return 0; + } +#endif + } + tmp = gpsUartBuf[pos++]; + bytesInBuffer--; + platformDetectPingMessageFromGpsDriver(tmp); + parseNMEAMessage(tmp, gpsMsg, GPSData); + } +} +/* end _handleGpsMessages */ + /** **************************************************************************** * @name GPSHandler GPS stream data and return GPS data for NAV algorithms @@ -469,13 +522,16 @@ void GPSHandler(void) { gGpsDataPtr->Timer100Hz10ms = getSystemTime(); ///< get system clock ticks gGpsDataPtr->isGPSBaudrateKnown = 1; - +#ifndef NMEA_ONLY // parse moved to top since is always called after init if (gGpsDataPtr->GPSConfigureOK > 0 ) { // Configuration is completed _handleGpsMessages(gGpsDataPtr); } else { ///< configure GPS receiver if needed: OK < 0 _configGPSReceiver(gGpsDataPtr); } +#else + _handleGpsNMEAMessages(gGpsDataPtr); +#endif } /** **************************************************************************** diff --git a/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/processNMEAGPS.c b/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/processNMEAGPS.c index f51ada5..6f08c1d 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/processNMEAGPS.c +++ b/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/processNMEAGPS.c @@ -40,6 +40,7 @@ limitations under the License. char _parseGPGGA(char *msgBody, GpsData_t* GPSData); char _parseVTG(char *msgBody, GpsData_t* GPSData); char _parseRMC(char *msgBody, GpsData_t* GPSData); +char _parseZDA(char *msgBody, GpsData_t* GPSData); void _handleNMEAmsg(char *msgID, char *msgBody,GpsData_t* GPSData); void _NMEA2UbloxAndLLA(NmeaLatLonSTRUCT* NmeaData, GpsData_t* GPSData); void _smoothVerticalData(GpsData_t* GPSData); @@ -185,6 +186,10 @@ void _handleNMEAmsg(char *msgID, GPSData->totalVTG++; _parseVTG(msgBody, GPSData); } + if( strncmp(ptr, "ZDA", 3) == 0 ) { + GPSData->totalVTG++; + _parseZDA(msgBody, GPSData); + } if( strncmp(ptr, "RMC", 3) == 0 ) { _parseRMC(msgBody, GPSData); } @@ -312,7 +317,7 @@ char _parseGPGGA(char *msgBody, if (field[6] == '.') { // Some messages have second fraction .SS skip the '.' - GPSData->GPSSecondFraction = ( (field[7] - '0') / 10) + (field[8] - '0') / 100; + GPSData->GPSSecondFraction = ( (field[7] - '0') / 10.0) + (field[8] - '0') / 100.0; } } else status = 1; @@ -501,6 +506,54 @@ char _parseRMC(char *msgBody, return status; } +/** **************************************************************************** + * @name: _parseZDA LOCAL parse GPZDA message. duplicates nav data but has + * calander data needed for WMM (Worl Magnetic Model) lookup + * @author Dong An + * @param [in] msgBody - data to parse + * @param [in] GPSData - data structure to parst he data into + * @retval status + * @details $GPZDA (Recommended Minimum sentence C) + * 172809 Fix taken at 17:28:09 UTC + * 12 Day + * 07 Month + * 1996 Year + * 00 Offset from GMT Hours + * 00 Offset from GMT Minutes + * *45 The checksum data, always begins with '*' + * 07/01/15 dkh convert directly from ascii to decimal + * $GPZDA,172809.456,12,07,1996,00,00*45 + ******************************************************************************/ +char _parseZDA(char *msgBody, + GpsData_t *GPSData) +{ + char field[NMEA_MSG_MAX_FIELD]; + char status = 0; + int parseReset = 1; + + /// Date field 8 + + if( extractNMEAfield(msgBody, field, 1, parseReset) ) { + GPSData->GPSday = ( (field[0] - '0') * 10) + field[1] - '0'; /// month + } else + status = 1; + + if( extractNMEAfield(msgBody, field, 2, parseReset) ) { + GPSData->GPSmonth = ((field[0] - '0') * 10) + field[1] - '0'; /// day + } else + status = 1; + + if( extractNMEAfield(msgBody, field, 3, parseReset) ) { + GPSData->GPSyear = ( (field[2] - '0') * 10) + field[3] - '0'; /// year + } else + status = 1; + + + return status; + +} + + /** **************************************************************************** * @name: computeNMEAChecksum compute the checksum of a NMEA message. * @author Dong An diff --git a/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/taskGps.c b/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/taskGps.c index 56a62a1..43412e0 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/taskGps.c +++ b/examples/OpenIMU300ZI/INS/lib/Core/GPS/src/taskGps.c @@ -51,7 +51,7 @@ void TaskGps(void const *argument) int bytesAvailable; static uint32_t updateHDOP, pollSirfCnt; - while(gpsSerialChan == UART_CHANNEL_NONE) { + while(gpsChan == UART_CHANNEL_NONE) { // nothing to do untill port decided OS_Delay( 1000); } @@ -93,4 +93,29 @@ void TaskGps(void const *argument) } } + +/** **************************************************************************** + * @name ProcGps + * @brief task callback with the main loop for handle GPS data, make sure the + * GPS handling function gets called on a regular basis;. + * gCalibration.productConfiguration.bit.hasGps = 1; by setting: + * true and true in name_IMU380.xml file + * @param N/A + * @retval N/A + ******************************************************************************/ +void ProcGps() +{ + static BOOL firstTime = TRUE; + + if(firstTime){ + // start out with the DOP high + firstTime = 0; + gGpsDataPtr->HDOP = 50.0; + initGPSHandler(); + } + + GPSHandler(); + +} + #endif // GPS \ No newline at end of file diff --git a/examples/OpenIMU300ZI/INS/lib/Core/SteerAlgo/steerAPI.h b/examples/OpenIMU300ZI/INS/lib/Core/SteerAlgo/steerAPI.h new file mode 100644 index 0000000..c10a0a4 --- /dev/null +++ b/examples/OpenIMU300ZI/INS/lib/Core/SteerAlgo/steerAPI.h @@ -0,0 +1,43 @@ +/** *************************************************************************** + * @file odoApi.h Odometer interface. + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * @brief This is a generalized odometer interface. + *****************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef STEER_API_H +#define STEER_API_H + +#include +#include "GlobalConstants.h" + +typedef struct { + uint8_t update; // 1 if contains new data + real angle; // streering angle floating point + int16_t intAngle; // streering angle as message - weighted + int16_t sRate; // stering angle rate + uint16_t steerAlgoStates; +} steerDataStruct_t; + +extern steerDataStruct_t gSteerAngle; + +#endif /* ODO_API_H */ diff --git a/examples/OpenIMU300ZI/INS/lib/Core/UARTComm/CommonMessages.c b/examples/OpenIMU300ZI/INS/lib/Core/UARTComm/CommonMessages.c index 09cbf0b..6fdfa44 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/UARTComm/CommonMessages.c +++ b/examples/OpenIMU300ZI/INS/lib/Core/UARTComm/CommonMessages.c @@ -31,6 +31,9 @@ limitations under the License. #include "platformAPI.h" #include "sensorsAPI.h" #include "appVersion.h" +#include "ucb_packet_struct.h" +#include "magAPI.h" +#include "magAlign.h" #include "CommonMessages.h" #include "algorithm.h" @@ -360,3 +363,144 @@ BOOL Fill_e2PacketPayload(uint8_t *payload, uint8_t *payloadLen) return TRUE; } +BOOL Fill_MagAlignResponsePayload(int8_t state, UcbPacketStruct *ptrUcbPacket) +{ + int8_t estimatedMagAlignVals[8] = {0}; + int8_t magAlignVals[8] = {0}; + BOOL valid = TRUE; + + switch (state - 1) + { + // + uint8_t len; + + case 0: // Return the Mag-Align status + //uint8_t *model = (uint8_t*)unitVersionString(); + //uint8_t *rev = (uint8_t*)platformBuildInfo(); + //unsigned int serialNum = unitSerialNumber(); + //len = snprintf((char*)ptrUcbPacket->payload, 250, "%s %s SN:%u", model, rev, serialNum ); + //ptrUcbPacket->payloadLength = len; + if (gMagAlign.state == MAG_ALIGN_STATUS_START_CAL_WITH_AUTOEND) + { + // Start (auto end) + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)0x1); + } + else if (gMagAlign.state == MAG_ALIGN_STATUS_START_CAL_WITHOUT_AUTOEND) + { + // Start (manual end) + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)0x2); + } + else + { + // Start (manual end) + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)0x0); + } + + ptrUcbPacket->payloadLength = len; + break; + + case 1: // Start mag-align w/ autoend + case 2: // Start mag-align w/ autoend + case 3: // Stop mag-align w/ autoend + case 4: // Accept results + case 5: // Accept results and write to EEPROM + case 6: // Abort Mag-Align or reject results + case 8: // Restore default mag-align values + case 9: // Restore default mag-align values and save in EEPROM + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)state - 1); + ptrUcbPacket->payloadLength = len; + break; + + case 7: // Return stored mag-align values +#if 0 + // Test values: + gMagAlign.estParams.hardIronBias[X_AXIS] = 0.1; + gMagAlign.estParams.hardIronBias[Y_AXIS] = -0.2; + gMagAlign.estParams.softIronScaleRatio = 0.98; + gMagAlign.estParams.softIronAngle = -270.0 * DEG_TO_RAD; +#endif + + // Bias can be +/- 8.0 [g] (full scale of sensor) + // SF = 2^15 / maxVal = 2^15 / 8.0 = 4096 + magAlignVals[0] = (char)(((int16_t)(gMagAlign.hardIronBias[X_AXIS] * (float)4096.0) >> 8) & 0xFF); + magAlignVals[1] = (char)(((int16_t)(gMagAlign.hardIronBias[X_AXIS] * (float)4096.0) >> 0) & 0xFF); + magAlignVals[2] = (char)(((int16_t)(gMagAlign.hardIronBias[Y_AXIS] * (float)4096.0) >> 8) & 0xFF); + magAlignVals[3] = (char)(((int16_t)(gMagAlign.hardIronBias[Y_AXIS] * (float)4096.0) >> 0) & 0xFF); + + // Ratio can be 0 --> 1 + // SF = (2^16-1) / maxVal = (2^16-1) / 1.0 = 65535 + magAlignVals[4] = (char)(((int16_t)(gMagAlign.softIronScaleRatio * (float)65535.0) >> 8) & 0xFF); + magAlignVals[5] = (char)(((int16_t)(gMagAlign.softIronScaleRatio * (float)65535.0) >> 0) & 0xFF); + + // SF = 2^15 / maxVal = 2^15 / pi = 10430.37835047045 + magAlignVals[6] = (char)(((int16_t)(gMagAlign.softIronAngle * (float)10430.37835047046) >> 8) & 0xFF); + magAlignVals[7] = (char)(((int16_t)(gMagAlign.softIronAngle * (float)10430.37835047046) >> 0) & 0xFF); + + // Bias can be +/- 8.0 [g] (full scale of sensor) + // SF = 2^15 / maxVal = 2^15 / 8.0 = 4096 + estimatedMagAlignVals[0] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[X_AXIS] * (float)4096.0) >> 8) & 0xFF); + estimatedMagAlignVals[1] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[X_AXIS] * (float)4096.0) >> 0) & 0xFF); + estimatedMagAlignVals[2] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[Y_AXIS] * (float)4096.0) >> 8) & 0xFF); + estimatedMagAlignVals[3] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[Y_AXIS] * (float)4096.0) >> 0) & 0xFF); + + // Ratio can be 0 --> 1 + // SF = (2^16-1) / maxVal = (2^16-1) / 1.0 = 65535 + estimatedMagAlignVals[4] = (char)(((int16_t)(gMagAlign.estParams.softIronScaleRatio * (float)65535.0) >> 8) & 0xFF); + estimatedMagAlignVals[5] = (char)(((int16_t)(gMagAlign.estParams.softIronScaleRatio * (float)65535.0) >> 0) & 0xFF); + + // Angle can be +/- pi (in radians) + // Correct for angles that exceed +/-180 + if (gMagAlign.estParams.softIronAngle > PI) + { + gMagAlign.estParams.softIronAngle = (float)PI - gMagAlign.estParams.softIronAngle; + } + else if (gMagAlign.estParams.softIronAngle < -PI) + { + gMagAlign.estParams.softIronAngle = TWO_PI + gMagAlign.estParams.softIronAngle; + } + + // SF = 2^15 / maxVal = 2^15 / pi = 10430.37835047045 + estimatedMagAlignVals[6] = (char)(((int16_t)(gMagAlign.estParams.softIronAngle * (float)10430.37835047046) >> 8) & 0xFF); + estimatedMagAlignVals[7] = (char)(((int16_t)(gMagAlign.estParams.softIronAngle * (float)10430.37835047046) >> 0) & 0xFF); + +#if 0 + DebugPrintFloat(" ", (float)estimatedMagAlignVals[0], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[1], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[2], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[3], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[4], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[5], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[6], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[7], 1); + DebugPrintEndline(); +#endif + + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", (char)magAlignVals[0], + (char)magAlignVals[1], + (char)magAlignVals[2], + (char)magAlignVals[3], + (char)magAlignVals[4], + (char)magAlignVals[5], + (char)magAlignVals[6], + (char)magAlignVals[7], + (char)estimatedMagAlignVals[0], + (char)estimatedMagAlignVals[1], + (char)estimatedMagAlignVals[2], + (char)estimatedMagAlignVals[3], + (char)estimatedMagAlignVals[4], + (char)estimatedMagAlignVals[5], + (char)estimatedMagAlignVals[6], + (char)estimatedMagAlignVals[7]); + + ptrUcbPacket->payloadLength = len; + break; + + case 10: // Load user computed mag-align values + break; + + default: + valid = FALSE; + break; + } + return valid; +} diff --git a/examples/OpenIMU300ZI/INS/lib/Core/UARTComm/CommonMessages.h b/examples/OpenIMU300ZI/INS/lib/Core/UARTComm/CommonMessages.h index 0f373f0..b425784 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/UARTComm/CommonMessages.h +++ b/examples/OpenIMU300ZI/INS/lib/Core/UARTComm/CommonMessages.h @@ -122,5 +122,6 @@ BOOL Fill_a2PacketPayload(uint8_t *payload, uint8_t *payloadLen); BOOL Fill_s1PacketPayload(uint8_t *payload, uint8_t *payloadLen); BOOL Fill_e1PacketPayload(uint8_t *payload, uint8_t *payloadLen); BOOL Fill_e2PacketPayload(uint8_t *payload, uint8_t *payloadLen); +BOOL Fill_MagAlignResponsePayload(int8_t state, UcbPacketStruct *ptrUcbPacket); #endif diff --git a/examples/OpenIMU300ZI/INS/lib/Core/library.json b/examples/OpenIMU300ZI/INS/lib/Core/library.json index f5babce..30e45c6 100644 --- a/examples/OpenIMU300ZI/INS/lib/Core/library.json +++ b/examples/OpenIMU300ZI/INS/lib/Core/library.json @@ -14,6 +14,8 @@ "-I GPS/include", "-I Odo", "-I Odo/include", + "-I SteerAlgo", + "-I SteerAlgo/include", "-I Math", "-I Math/include", "-I SPIComm", diff --git a/examples/OpenIMU300ZI/INS/openimu.json b/examples/OpenIMU300ZI/INS/openimu.json index 7427c4e..ffc63b5 100644 --- a/examples/OpenIMU300ZI/INS/openimu.json +++ b/examples/OpenIMU300ZI/INS/openimu.json @@ -1,5 +1,5 @@ { - "appVersion": "OpenIMU300ZI INS 1.1.1", + "appVersion": "OpenIMU300ZI INS 04.01.04", "description": "9-axis OpenIMU with triaxial rate, acceleration, and magnetic measurement", "userConfiguration": [ { "paramId": 0,"category": "General", "paramType": "disabled", "type": "uint64", "name": "Data CRC" }, diff --git a/examples/OpenIMU300ZI/INS/platformio.ini b/examples/OpenIMU300ZI/INS/platformio.ini index 4271e75..948e880 100644 --- a/examples/OpenIMU300ZI/INS/platformio.ini +++ b/examples/OpenIMU300ZI/INS/platformio.ini @@ -20,8 +20,8 @@ description = platform = aceinna_imu lib_archive = false board = OpenIMU300 -;lib_deps= ../../OpenIMU300-lib -lib_deps = OpenIMU300-base-library@1.0.9 +;lib_deps = ../../../openIMU300-lib +lib_deps = OpenIMU300-base-library@1.1.13 build_flags = -D CLI -D GPS @@ -30,7 +30,6 @@ build_flags = -D DISPLAY_DIAGNOSTIC_MSG -D ARM_MATH_CM4 -I include - -I include/API -I src/user -I src ; -L ldscripts diff --git a/examples/OpenIMU300ZI/INS/src/appVersion.h b/examples/OpenIMU300ZI/INS/src/appVersion.h index a891dab..de70dfa 100644 --- a/examples/OpenIMU300ZI/INS/src/appVersion.h +++ b/examples/OpenIMU300ZI/INS/src/appVersion.h @@ -26,7 +26,8 @@ limitations under the License. #ifndef _INS_APP_VERSION_H #define _INS_APP_VERSION_H -#define APP_VERSION_STRING "OpenIMU300ZI INS 1.1.1" +#define APP_VERSION_STRING "OpenIMU300ZI INS 04.01.04" +#define APP_SPI_SW_VERSION 115 #endif diff --git a/examples/OpenIMU300ZI/INS/src/main.c b/examples/OpenIMU300ZI/INS/src/main.c index e1457ee..38e5daf 100644 --- a/examples/OpenIMU300ZI/INS/src/main.c +++ b/examples/OpenIMU300ZI/INS/src/main.c @@ -58,7 +58,6 @@ char buildInfo[] = {__DATE__"," __TIME__}; BOOL fSPI = FALSE; BOOL fGPS = FALSE; - /** *************************************************************************** * @name getBuildInfo - provides the pointer to the build date and time string * @brief @@ -88,7 +87,7 @@ void DebugInterfaceInit(void) return; // no resources } - int debugChannel = platformGetSerialChannel(DEBUG_SERIAL_PORT); + int debugChannel = platformGetSerialChannel(DEBUG_SERIAL_PORT, TRUE); if(debugChannel == UART_CHANNEL_NONE){ //nothing to do @@ -97,14 +96,12 @@ void DebugInterfaceInit(void) // Initialize the DEBUG USART (serial) port InitDebugSerialCommunication(230400); // debug_usart.c - //DEBUG_STRING("\r\nDMU380 System\r\n"); BoardGetResetStatus(status, sizeof(status)); - //ERROR_STRING(status); +// ERROR_STRING(status); } - void CreateTasks(void) { osThreadId iD; diff --git a/examples/OpenIMU300ZI/INS/src/taskDataAcquisition.c b/examples/OpenIMU300ZI/INS/src/taskDataAcquisition.c index db614da..2274c52 100644 --- a/examples/OpenIMU300ZI/INS/src/taskDataAcquisition.c +++ b/examples/OpenIMU300ZI/INS/src/taskDataAcquisition.c @@ -31,14 +31,14 @@ limitations under the License. #include "magAPI.h" #include "platformAPI.h" #include "userAPI.h" +#include "commAPI.h" #include "spiAPI.h" +#include "UserMessagingSPI.h" #include "taskDataAcquisition.h" #include "osapi.h" #include "osresources.h" -#include "commAPI.h" - /** *************************************************************************** * @name TaskDataAcquisition() CALLBACK main loop @@ -53,6 +53,7 @@ void TaskDataAcquisition(void const *argument) // int res; uint16_t dacqRate; + int spiRateRef = 0, spiRateDiv = 0; #pragma GCC diagnostic ignored "-Wunused-but-set-variable" BOOL overRange = FALSE; //uncomment this line if overrange processing required @@ -161,7 +162,7 @@ void TaskDataAcquisition(void const *argument) // in case of UART communication interface clears pin IO2 // in case of SPI communication interface clears pin DRDY if(platformGetUnitCommunicationType() != UART_COMM){ - setDataReadyPin(0); + setDataReadyPin(1); }else{ setIO2Pin (0); } @@ -176,6 +177,18 @@ void TaskDataAcquisition(void const *argument) if(platformGetUnitCommunicationType() != UART_COMM){ // Perform interface - specific processing here FillSPIBurstDataBuffer(); + if(spiRateRef){ + spiRateDiv++; + if(spiRateDiv >= spiRateRef){ + // Inform user, that new data set is ready (if required) + setDataReadyPin(0); // activate data ready - set low + spiRateDiv = 0; + spiRateRef = GetSpiPacketRateDivider(); + } + }else { + spiRateRef = GetSpiPacketRateDivider(); + } + UpdateSpiUserConfig(); } else { // Process commands and output continuous packets to UART // Processing of user commands always goes first diff --git a/examples/OpenIMU300ZI/INS/include/taskDataAcquisition.h b/examples/OpenIMU300ZI/INS/src/taskDataAcquisition.h similarity index 100% rename from examples/OpenIMU300ZI/INS/include/taskDataAcquisition.h rename to examples/OpenIMU300ZI/INS/src/taskDataAcquisition.h diff --git a/examples/OpenIMU300ZI/INS/src/user/UserAlgorithm.c b/examples/OpenIMU300ZI/INS/src/user/UserAlgorithm.c index 20c39c4..9f71e79 100644 --- a/examples/OpenIMU300ZI/INS/src/user/UserAlgorithm.c +++ b/examples/OpenIMU300ZI/INS/src/user/UserAlgorithm.c @@ -37,7 +37,6 @@ limitations under the License. #endif #include "UserConfiguration.h" - #include "algorithmAPI.h" #include "algorithm.h" #include "UserAlgorithm.h" @@ -54,15 +53,24 @@ odoDataStruct_t gOdo; // static void _Algorithm(); -static void _InitAlgo(uint8_t algoType); -// Initialize GPS algorithm variables +// Initialize algorithm variables void InitUserAlgorithm() { // Initialize built-in algorithm structure - InitializeAlgorithmStruct(FREQ_100_HZ); + InitializeAlgorithmStruct(FREQ_100_HZ, OpenIMU300ZI); // place additional required initialization here + /* Set the configuration variables for INS solution. + * Enable GPS + * Enable stationary yaw lock + * Enable zero velocity detection by IMU + */ + enableMagInAlgorithm(FALSE); + enableGpsInAlgorithm(TRUE); + enableOdoInAlgorithm(FALSE); + enableStationaryLockYaw(TRUE); // enable yaw lock when vehicle is static + enableImuStaticDetect(TRUE); // enable zero velocity detection by IMU setLeverArm( (real)gUserConfiguration.leverArmBx, (real)gUserConfiguration.leverArmBy, (real)gUserConfiguration.leverArmBz ); @@ -75,12 +83,6 @@ void InitUserAlgorithm() void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, gpsDataStruct_t *gps, odoDataStruct_t *odo, BOOL ppsDetected) { - // This can be set at startup based on the packet type selected - static uint8_t algoType = INS; - - // Initialize variable related to the UserNavAlgorithm - _InitAlgo(algoType); - // Populate the EKF input data structure EKF_SetInputStruct(accels, rates, mags, gps, odo, ppsDetected); @@ -99,55 +101,37 @@ void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, // -static void _InitAlgo(uint8_t algoType) +static void _Algorithm() { - // Initialize the timer variables - static uint8_t initAlgo = 1; - if(initAlgo) { - // Reset 'initAlgo' so this is not executed more than once. This - // prevents the algorithm from being switched during run-time. - initAlgo = 0; - - // Set the configuration variables for a VG-type solution - // (useMags = 0 forces the VG solution) - gAlgorithm.Behavior.bit.freeIntegrate = 0; - gAlgorithm.Behavior.bit.useMag = 0; - gAlgorithm.Behavior.bit.useGPS = 0; - gAlgorithm.Behavior.bit.useOdo = 0; - gAlgorithm.Behavior.bit.restartOnOverRange = 0; - gAlgorithm.Behavior.bit.dynamicMotion = 1; - - // Set the system configuration based on system type - switch( algoType ) { - case VG: - // Nothing additional to do (already configured for a VG - // solution) - break; - case AHRS: - // Set the configuration variables for AHRS solution - // (useMags = 1 and enable mags) - enableMagInAlgorithm(TRUE); - break; - case INS: - /* Set the configuration variables for INS solution. - * (Enable GPS and set algorithm calling frequency to 100Hz) - */ - enableMagInAlgorithm(FALSE); - enableGpsInAlgorithm(TRUE); - enableOdoInAlgorithm(FALSE); - gAlgorithm.callingFreq = FREQ_100_HZ; // redundant; set above - break; - default: - // Nothing to do - break; - } - } + // Aceinna VG/AHRS/INS algorithm + EKF_Algorithm(); } -// -static void _Algorithm() +BOOL getAlgorithmLinAccelDetectMode() { - // Aceinna VG/AHRS/INS algorithm - EKF_Algorithm(); + return TRUE; +} + +BOOL getAlgorithmAccelPredictMode() +{ + return FALSE; +} + +float getAlgorithmCoefOfReduceQ() +{ + // 0.0001 to 1 (1 to 10000) + return (float)10/10000; +} + +float getAlgorithmAccelSwitchDelay() +{ + // 0.01 to 10 (100 to 10000) + return (float)2000/1000; +} + +float getAlgorithmRateIntegrationTime() +{ + // 0.01 to 10 (100 to 10000) + return (float)2000/1000; } diff --git a/examples/OpenIMU300ZI/INS/src/user/UserConfiguration.c b/examples/OpenIMU300ZI/INS/src/user/UserConfiguration.c index edc3647..ba901c6 100644 --- a/examples/OpenIMU300ZI/INS/src/user/UserConfiguration.c +++ b/examples/OpenIMU300ZI/INS/src/user/UserConfiguration.c @@ -42,15 +42,15 @@ limitations under the License. const UserConfigurationStruct gDefaultUserConfig = { .dataCRC = 0, .dataSize = sizeof(UserConfigurationStruct), - .userUartBaudRate = 230400, + .userUartBaudRate = 115200, .userPacketType = "e2", - .userPacketRate = 100, + .userPacketRate = 50, .lpfAccelFilterFreq = 25, .lpfRateFilterFreq = 25, - .orientation = "+X+Y+Z", - .gpsBaudRate = 115200, - .gpsProtocol = UBLOX_BINARY, + .orientation = "+X+Y+Z", // The EVB connector pointing forward // add default parameter values here, if desired + .uartGpsBaudRate = 115200, + .uartGpsProtocol = UBLOX_BINARY, .hardIron_X = 0.0, .hardIron_Y = 0.0, .softIron_Ratio = 1.0, @@ -61,6 +61,12 @@ const UserConfigurationStruct gDefaultUserConfig = { .pointOfInterestBx = 0.0, .pointOfInterestBy = 0.0, .pointOfInterestBz = 0.0, + .appBehavior = APP_BEHAVIOR_USE_EXT_SYNC, + .spiOrientation = 0x0000, //+X +Y +Z + .spiSyncRate = 1, // 200Hz + .extSyncFreq = 1, // 1Hz + .spiAccelLpfType = IIR_20HZ_LPF, // Butterworth 20Hz + .spiGyroLpfType = IIR_20HZ_LPF, // Butterworth 20Hz }; UserConfigurationStruct gUserConfiguration; @@ -97,22 +103,20 @@ void userInitConfigureUnit() while(1); } - if(EEPROM_IsAppStartedFirstTime()) { + if(EEPROM_IsAppStartedFirstTime()){ // comment next line if want to keep previously stored in EEPROM parameters // after rebuilding and/or reloading new application - RestoreDefaultUserConfig(); //JSM - Commented out so the mag-align values - // won't be overwritten each time the - // firmware is reloaded + RestoreDefaultUserConfig(); } // Validate checksum of user configuration structure configValid = EEPROM_ValidateUserConfig(&size); - if(configValid == TRUE) { + if(configValid == TRUE){ // Here we have validated User configuration image. // Load it from eeprom into ram on top of the default configuration EEPROM_LoadUserConfig((void*)&gUserConfiguration, &size); - } else{ + }else{ memset((void*)&gUserConfiguration, 0xff, sizeof(gUserConfiguration)); } @@ -147,7 +151,7 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) { BOOL result = TRUE; uint64_t *ptr = (uint64_t *)&gUserConfiguration; - + uint16_t orientOut; if(number < USER_CRC || number >= USER_MAX_PARAM ){ return FALSE; } @@ -169,7 +173,7 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) result = platformSelectLPFilter(RATE_SENSOR, (uint32_t)data, fApply); break; case USER_ORIENTATION: - result = platformSetOrientation((uint16_t*)&data, fApply); + result = platformSetOrientation((uint16_t*)&data, &orientOut, fApply); break; case USER_CRC: case USER_DATA_SIZE: @@ -206,7 +210,7 @@ BOOL UpdateUserParameter(uint32_t number, uint64_t data, BOOL fApply) BOOL result; uint64_t *ptr = (uint64_t *)&gUserConfiguration; - if(number <= USER_LAST_SYSTEM_PARAM || number >= USER_MAX_PARAM ){ + if(number <= USER_LAST_SYSTEM_PARAM || number > USER_LAST_UART_PARAM){ return FALSE; } @@ -276,7 +280,7 @@ BOOL UpdateUserConfig(userConfigPayload* pld, uint8_t *payloadLen) BOOL ret = FALSE; int32_t result = 0; - maxParam = sizeof(UserConfigurationStruct)/8; + maxParam = USER_LAST_UART_PARAM; // Validate parameters numbers and quantity if(pld->numParams > MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET){ @@ -343,7 +347,7 @@ BOOL UpdateUserParam(userParamPayload* pld, uint8_t *payloadLen) BOOL ret = TRUE; int32_t result = 0; - maxParam = sizeof(UserConfigurationStruct)/8; + maxParam = USER_LAST_UART_PARAM; offsetValid = pld->paramNum < maxParam; if(offsetValid){ @@ -397,7 +401,7 @@ BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) int32_t result = 0; int numParams = (*payloadLen)/8; - maxParam = sizeof(UserConfigurationStruct)/8; + maxParam = USER_LAST_UART_PARAM; if(numParams > MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET){ lenValid = FALSE; @@ -460,7 +464,7 @@ BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen) BOOL lenValid = TRUE; uint64_t *ptr = (uint64_t *)&gUserConfiguration; - maxParam = sizeof(UserConfigurationStruct)/8; + maxParam = USER_LAST_UART_PARAM; offsetValid = pld->paramOffset < maxParam; @@ -499,7 +503,7 @@ BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen) BOOL offsetValid; uint64_t *ptr = (uint64_t *)&gUserConfiguration; - maxParam = sizeof(UserConfigurationStruct)/8; + maxParam = USER_LAST_UART_PARAM; offsetValid = pld->paramNum < maxParam; if(offsetValid){ @@ -530,7 +534,7 @@ BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) uint32_t offset, i, numParams; uint64_t *ptr = (uint64_t *)&gUserConfiguration; - numParams = sizeof(UserConfigurationStruct)/8; + numParams = USER_LAST_UART_PARAM; offset = 0; for (i = 0; i < numParams; i++, offset++){ @@ -605,3 +609,35 @@ BOOL RestoreDefaultUserConfig(void) } return valid; } + +BOOL ExtSyncEnabled() +{ + return (gUserConfiguration.appBehavior & APP_BEHAVIOR_USE_EXT_SYNC) != 0; +} + +int ExtSyncFrequency() +{ + return gUserConfiguration.extSyncFreq; +} + + +uint8_t SpiSyncRate() +{ + return (uint8_t)gUserConfiguration.spiSyncRate; +} + +uint8_t SpiAccelLpfType() +{ + return (uint8_t)gUserConfiguration.spiAccelLpfType; +} + +uint8_t SpiGyroLpfType() +{ + return (uint8_t)gUserConfiguration.spiGyroLpfType; +} + +uint16_t SpiOrientation() +{ + return (uint16_t)gUserConfiguration.spiOrientation; +} + diff --git a/examples/OpenIMU300ZI/INS/src/user/UserConfiguration.h b/examples/OpenIMU300ZI/INS/src/user/UserConfiguration.h index d2445ce..f6ec4dd 100644 --- a/examples/OpenIMU300ZI/INS/src/user/UserConfiguration.h +++ b/examples/OpenIMU300ZI/INS/src/user/UserConfiguration.h @@ -25,6 +25,7 @@ limitations under the License. #include "GlobalConstants.h" #include "UserMessagingUART.h" +#include "filter.h" /// User defined configuration strucrture ///Please notice, that parameters are 64 bit to accomodate double types as well as longer string types @@ -72,7 +73,7 @@ typedef struct { // here is the border between arbitrary parameters and platform configuration parameters //*************************************************************************************** - int64_t gpsBaudRate; /// baudrate of GPS UART, bps. + int64_t uartGpsBaudRate; /// baudrate of GPS UART, bps. /// valid options are: /// 4800 /// 9600 @@ -81,7 +82,7 @@ typedef struct { /// 57600 /// 115200 /// 230400 - int64_t gpsProtocol; /// protocol of GPS receicer. + int64_t uartGpsProtocol; /// protocol of GPS receicer. /// so far valid options are: /// NMEA_TEXT /// NOVATEL_BINARY @@ -100,12 +101,72 @@ typedef struct { double pointOfInterestBx; double pointOfInterestBy; double pointOfInterestBz; + uint64_t appBehavior; + + //*************************************************************************************** + // SPI-Related specific parameters + //*************************************************************************************** + int64_t spiSyncRate; /// SPI data ready rate + /// 0 - 0 Hz + /// 1 - 200 Hz + /// 2 - 100 Hz + /// 3 - 50 Hz + /// 4 - 25 Hz + /// 5 - 20 Hz + /// 6 - 10 Hz + /// 7 - 5 Hz + /// 8 - 4 Hz + /// 9 - 2 Hz + + int64_t spiOrientation; /// orientation for SPI mode + //0x0000 +Ux +Uy +Uz + //0x0009 -Ux -Uy +Uz + //0x0023 -Uy +Ux +Uz + //0x002A +Uy -Ux +Uz + //0x0041 -Ux +Uy -Uz + //0x0048 +Ux -Uy -Uz + //0x0062 +Uy +Ux -Uz + //0x006B -Uy -Ux -Uz + //0x0085 -Uz +Uy +Ux + //0x008C +Uz -Uy +Ux + //0x0092 +Uy +Uz +Ux + //0x009B -Uy -Uz +Ux + //0x00C4 +Uz +Uy -Ux + //0x00CD -Uz -Uy -Ux + //0x00D3 -Uy +Uz -Ux + //0x00DA +Uy -Uz -Ux + //0x0111 -Ux +Uz +Uy + //0x0118 +Ux -Uz +Uy + //0x0124 +Uz +Ux +Uy + //0x012D -Uz -Ux +Uy + //0x0150 +Ux +Uz -Uy + //0x0159 -Ux -Uz -Uy + //0x0165 -Uz +Ux -Uy + //0x016C +Uz -Ux -Uy + + int64_t spiAccelLpfType; /// built-in lpf filter cutoff frequency selection for accelerometers + int64_t spiGyroLpfType; /// built-in lpf filter cutoff frequency selection for rate sensors + /// Options are: + // UNFILTERED = 0x00, + // FIR_40HZ_LPF = 0x03, // Bartlett LPF 40HZ + // FIR_20HZ_LPF = 0x04, // Bartlett LPF 20HZ + // FIR_10HZ_LPF = 0x05, // Bartlett LPF 10HZ + // FIR_05HZ_LPF = 0x06, // Bartlett LPF 5HZ + // IIR_50HZ_LPF = 0x30, // Butterworth LPF 50HZ + // IIR_20HZ_LPF = 0x40, // Butterworth LPF 20HZ + // IIR_10HZ_LPF = 0x50, // Butterworth LPF 10HZ + // IIR_05HZ_LPF = 0x60, // Butterworth LPF 5HZ + // IIR_02HZ_LPF = 0x70, // Butterworth LPF 2HZ + // IIR_25HZ_LPF = 0x80, // Butterworth LPF 25HZ + // IIR_40HZ_LPF = 0x90, // Butterworth LPF 40HZ + uint64_t extSyncFreq; /// external sync frequency + } UserConfigurationStruct; typedef enum { //***************************************************************************************** // add system parameters here and reassign USER_LAST_SYSTEM_PARAM (DO NOT CHANGE THIS!!!) - USER_CRC = 0, + USER_CRC = 0, // 0 USER_DATA_SIZE , // 1 USER_USER_BAUD_RATE , // 2 order of next 4 parameters USER_USER_PACKET_TYPE , // 3 of required unit output bandwidth @@ -113,21 +174,29 @@ typedef enum { USER_LPF_ACCEL_TYPE , // 5 prefered LPF filter type for accelerometer USER_LPF_RATE_TYPE , // 6 prefered LPF filter type for rate sensor USER_ORIENTATION , // 7 unit orientation - USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, + USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, // 7 //***************************************************************************************** // add parameter enumerator here while adding new parameter in user UserConfigurationStruct - USER_GPS_BAUD_RATE , - USER_GPS_PROTOCOL , - USER_HARD_IRON_X , - USER_HARD_IRON_Y , - USER_SOFT_IRON_RATIO , - USER_SOFT_IRON_ANGLE , + USER_GPS_BAUD_RATE , // 8 + USER_GPS_PROTOCOL , // 9 + USER_HARD_IRON_X , // 10 + USER_HARD_IRON_Y , // 11 + USER_SOFT_IRON_RATIO , // 12 + USER_SOFT_IRON_ANGLE , // 13 USER_LEVER_ARM_BX , USER_LEVER_ARM_BY , USER_LEVER_ARM_BZ , USER_POINT_OF_INTEREST_BX , USER_POINT_OF_INTEREST_BY , USER_POINT_OF_INTEREST_BZ , + USER_LAST_UART_PARAM , // 14 + USER_APPLICATION_BEHAVIOR , +// spi-specific parameters + USER_SPI_SYNC_RATE , // SPI data ready rate + USER_SPI_ORIENTATION , // SPI mode orientation + USER_SPI_ACCEl_LPF , // SPI mode accel lpf + USER_SPI_RATE_LPF , // SPI mode gyro lpf + USER_EXT_SYNC_FREQ , // Extern sync frequency applied tp SYNC/1PPS input USER_MAX_PARAM } UserConfigParamNumber; @@ -138,6 +207,7 @@ extern int userPacketOut; #define INVALID_PARAM -1 #define INVALID_VALUE -2 #define INVALID_PAYLOAD_SIZE -3 +#define APP_BEHAVIOR_USE_EXT_SYNC 0x0000000000000001LL @@ -164,6 +234,13 @@ extern BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLe extern BOOL UpdateUserParameter(uint32_t number, uint64_t data, BOOL fApply); extern BOOL UpdateSystemParameter(uint32_t offset, uint64_t data, BOOL fApply); BOOL setUserPacketType(uint8_t* type, BOOL fApply); +extern BOOL ExtSyncEnabled(); +extern int ExtSyncFrequency(); + +uint16_t SpiOrientation(); +uint8_t SpiSyncRate(); +uint8_t SpiAccelLpfType(); +uint8_t SpiGyroLpfType(); #endif /* USER_CONFIGURATION_H */ diff --git a/examples/OpenIMU300ZI/INS/src/user/UserMessagingSPI.c b/examples/OpenIMU300ZI/INS/src/user/UserMessagingSPI.c index 7f54e37..28f1f5a 100644 --- a/examples/OpenIMU300ZI/INS/src/user/UserMessagingSPI.c +++ b/examples/OpenIMU300ZI/INS/src/user/UserMessagingSPI.c @@ -34,7 +34,9 @@ limitations under the License. #include "sensorsAPI.h" #include "spiAPI.h" #include "magAPI.h" - +#include "appVersion.h" +#include "BitStatus.h" +#include "EKF_Algorithm.h" uint8_t _spiDataBuf[2][SPI_DATA_BUF_LEN]; uint8_t *_activeSpiDataBufPtr = _spiDataBuf[0]; @@ -43,10 +45,11 @@ uint8_t _activeSpiDataBufIdx = 0; uint8_t _spiRegs[NUM_SPI_REGS]; +int16_t _attitude[3]; BOOL _readOp = FALSE; uint8_t _regAddr = 0; - +BOOL fSaveSpiConfig = FALSE; // Initialize SPI registers here // For default register assignments refer to // UserMessaging.h @@ -81,11 +84,42 @@ void InitUserSPIRegisters() _spiRegs[SPI_REG_PROD_ID_REQUEST+1] = 0x00; // product code LO _spiRegs[SPI_REG_SERIAL_NUM_REQUEST] = arr[7]; _spiRegs[SPI_REG_SERIAL_NUM_REQUEST+1] = (arr[8] << 4) | arr[9]; + // fill up orientation + uint16_t tmp = SpiOrientation(); + _spiRegs[SPI_REG_ORIENTATION_LSB_CTRL] = tmp & 0xff; + _spiRegs[SPI_REG_ORIENTATION_MSB_CTRL] = (tmp >> 8) & 0xff; + // Fill up default DRDY rate + _spiRegs[SPI_REG_DRDY_RATE_CTRL] = SpiSyncRate(); + _spiRegs[SPI_REG_DRDY_CTRL] = 0x04; + _spiRegs[SPI_REG_ACCEL_FILTER_TYPE_CTRL] = SpiAccelLpfType(); + _spiRegs[SPI_REG_RATE_FILTER_TYPE_CTRL] = SpiGyroLpfType(); + _spiRegs[SPI_REG_ACCEL_SENSOR_RANGE_CTRL] = SPI_ACCEL_SENSOR_RANGE_8G; + _spiRegs[SPI_REG_RATE_SENSOR_RANGE_CTRL] = SPI_RATE_SENSOR_RANGE_500; + _spiRegs[SPI_REG_MAG_SENSOR_RANGE_CTRL] = SPI_MAG_SENSOR_RANGE_2G; + // Fill up sensors data scale factor on the SPI bus + _spiRegs[SPI_REG_ACCEL_SENSOR_SCALE] = 4; // 4000 counts per 1 G -> 8G + _spiRegs[SPI_REG_RATE_SENSOR_SCALE] = 64; // 64 counts per dps -> 500 dps + _spiRegs[SPI_REG_MAG_SENSOR_SCALE] = 16; // 16384 64 counts per Gauss // Fill up HW/SW versions _spiRegs[SPI_REG_HW_VERSION_REQUEST] = ReadUnitHwConfiguration(); - _spiRegs[SPI_REG_SW_VERSION_REQUEST] = unitSpiSwVersion(); - _spiRegs[SPI_REG_ACCEL_FILTER_TYPE_CTRL] = IIR_05HZ_LPF; - _spiRegs[SPI_REG_RATE_FILTER_TYPE_CTRL] = IIR_05HZ_LPF; + _spiRegs[SPI_REG_SW_VERSION_REQUEST] = APP_SPI_SW_VERSION; + { + uint8_t regOffset; + magAlignUserParams_t params; + int16_t tmp[4]; + getUserMagAlignParams(¶ms); + regOffset = SPI_REG_HARD_IRON_BIAS_X_MSB; + tmp[0] = (int16_t)((params.hardIron_X/10) * 32768 ); // +-10 Gauss + tmp[1] = (int16_t)((params.hardIron_Y/10) * 32768 ); // +-10 Gauss + tmp[2] = (int16_t)(params.softIron_Ratio * 32768 ); // + tmp[3] = (int16_t)((params.softIron_Angle/3.141592) * 32768 ); // +-3.141592 rad + for(int i = 0; i < 4; i++){ + _spiRegs[regOffset++] = (tmp[i] >> 8) & 0xff; + _spiRegs[regOffset++] = tmp[i] & 0xff; + } + } + + } @@ -93,15 +127,52 @@ void InitUserSPIRegisters() // Each new data set get loaded into new buffer, while pervious // data set remains in old buffer. After loading active buffer get switched // When user requests data from outside - dtata from active buffer will be transmitted - BOOL loadSPIBurstData(spi_burst_data_t *data) +BOOL loadSPIBurstData(ext_spi_burst_data_t *data) { - if(sizeof(spi_burst_data_t) > SPI_DATA_BUF_LEN){ - return FALSE; + if(sizeof(ext_spi_burst_data_t) > SPI_DATA_BUF_LEN){ + while(1); // catch it here } - memcpy(_spiDataBuf[_emptySpiDataBufIdx], data, sizeof(spi_burst_data_t)); + memcpy(_spiDataBuf[_emptySpiDataBufIdx], data, sizeof(ext_spi_burst_data_t)); _activeSpiDataBufPtr = _spiDataBuf[_emptySpiDataBufIdx]; _activeSpiDataBufIdx = _emptySpiDataBufIdx; //active buffer; _emptySpiDataBufIdx ^= 1; //new buffer; + +// Accelerometer data + _spiRegs[SPI_REG_XACCEL_REQUEST] = data->accels[0] & 0xff; + _spiRegs[SPI_REG_XACCEL_REQUEST+1] = (data->accels[0] >> 8) & 0xff; + _spiRegs[SPI_REG_YACCEL_REQUEST] = data->accels[1] & 0xff; + _spiRegs[SPI_REG_YACCEL_REQUEST+1] = (data->accels[1] >> 8) & 0xff; + _spiRegs[SPI_REG_ZACCEL_REQUEST] = data->accels[2] & 0xff; + _spiRegs[SPI_REG_ZACCEL_REQUEST+1] = (data->accels[2] >> 8) & 0xff; +// Gyro data + _spiRegs[SPI_REG_XRATE_REQUEST] = data->rates[0] & 0xff; + _spiRegs[SPI_REG_XRATE_REQUEST+1] = (data->rates[0] >> 8) & 0xff; + _spiRegs[SPI_REG_YRATE_REQUEST] = data->rates[1] & 0xff; + _spiRegs[SPI_REG_YRATE_REQUEST+1] = (data->rates[1] >> 8) & 0xff; + _spiRegs[SPI_REG_ZRATE_REQUEST] = data->rates[2] & 0xff; + _spiRegs[SPI_REG_ZRATE_REQUEST+1] = (data->rates[2] >> 8) & 0xff; +// Mag data + _spiRegs[SPI_REG_XMAG_REQUEST] = data->mags[0] & 0xff; + _spiRegs[SPI_REG_XMAG_REQUEST+1] = (data->mags[0] >> 8) & 0xff; + _spiRegs[SPI_REG_YMAG_REQUEST] = data->mags[1] & 0xff; + _spiRegs[SPI_REG_YMAG_REQUEST+1] = (data->mags[1] >> 8) & 0xff; + _spiRegs[SPI_REG_ZMAG_REQUEST] = data->mags[2] & 0xff; + _spiRegs[SPI_REG_ZMAG_REQUEST+1] = (data->mags[2] >> 8) & 0xff; +// Temp data + _spiRegs[SPI_REG_RTEMP_REQUEST] = data->temp & 0xff; + _spiRegs[SPI_REG_RTEMP_REQUEST+1] = (data->temp >> 8) & 0xff; + _spiRegs[SPI_REG_BTEMP_REQUEST] = data->temp & 0xff; + _spiRegs[SPI_REG_BTEMP_REQUEST+1] = (data->temp >> 8) & 0xff; +// Master Status + _spiRegs[SPI_REG_MASTER_STATUS_REQUEST] = data->status & 0xff; + _spiRegs[SPI_REG_MASTER_STATUS_REQUEST+1] = (data->status >> 8) & 0xff; +// HW Status + _spiRegs[SPI_REG_HW_STATUS_REQUEST] = gBitStatus.hwBIT.all & 0xff; + _spiRegs[SPI_REG_HW_STATUS_REQUEST+1] = (gBitStatus.hwBIT.all >> 8) & 0xff; +// SW Status + _spiRegs[SPI_REG_SW_STATUS_REQUEST] = gBitStatus.swBIT.all & 0xff; + _spiRegs[SPI_REG_SW_STATUS_REQUEST+1] = (gBitStatus.swBIT.all >> 8) & 0xff; + return TRUE; } @@ -120,7 +191,6 @@ uint8_t bufPtr = 0; void SPI_ProcessCommand(uint8_t cmd) { static uint8_t regData[2] = {0,0}; -// ext_spi_burst_data_t *bptr = (ext_spi_burst_data_t*)_activeSpiDataBufPtr; _readOp = (cmd & 0x80) == 0; _regAddr = 0xff; // reset reg addr // command in cmd" @@ -133,6 +203,22 @@ void SPI_ProcessCommand(uint8_t cmd) bufIdx[bufPtr++] = _activeSpiDataBufIdx; UserSPIPrepareForDataTransmit(_activeSpiDataBufPtr, sizeof(spi_burst_data_t)); break; + case 0x3f: + // ext sensors data + burstCnt++; + bufIdx[bufPtr++] = _activeSpiDataBufIdx; + UserSPIPrepareForDataTransmit(_activeSpiDataBufPtr, sizeof(ext_spi_burst_data_t)); + break; + case 0x3D: + // ext sensors data + burstCnt++; + bufIdx[bufPtr++] = _activeSpiDataBufIdx; + att_spi_burst_data_t *pld = (att_spi_burst_data_t*)_activeSpiDataBufPtr; + for(int i = 0; i < 3; i++){ + pld->attitude[i] = _attitude[i]; + } + UserSPIPrepareForDataTransmit(_activeSpiDataBufPtr, sizeof(att_spi_burst_data_t)); + break; case SPI_REG_MAG_ALIGN_CTRL: { uint8_t state, regOffset; @@ -164,11 +250,39 @@ void SPI_ProcessCommand(uint8_t cmd) } } +BOOL CheckSpiPacketRateDivider(uint8_t dataRate ) +{ + switch( dataRate ) + { + case OUTPUT_DATA_RATE_ZERO : + case OUTPUT_DATA_RATE_200_HZ: + case OUTPUT_DATA_RATE_100_HZ: + case OUTPUT_DATA_RATE_50_HZ : + case OUTPUT_DATA_RATE_25_HZ : + case OUTPUT_DATA_RATE_20_HZ : + case OUTPUT_DATA_RATE_10_HZ : + case OUTPUT_DATA_RATE_5_HZ : + case OUTPUT_DATA_RATE_4_HZ : + case OUTPUT_DATA_RATE_2_HZ : + case OUTPUT_DATA_RATE_1_HZ : + return TRUE; + break; + default: + return FALSE; + break; + } +} /* end CheckPacketRateDivider */ + BOOL CheckSpiSensorFilterType(uint8_t type) { switch(type){ case UNFILTERED: + case FIR_40HZ_LPF: + case FIR_20HZ_LPF: + case FIR_10HZ_LPF: + case FIR_05HZ_LPF: + case IIR_02HZ_LPF: case IIR_05HZ_LPF: case IIR_10HZ_LPF: case IIR_20HZ_LPF: @@ -181,6 +295,60 @@ BOOL CheckSpiSensorFilterType(uint8_t type) } }; +BOOL CheckSpiSyncRate(uint8_t rate) +{ + if(rate < 10){ + return TRUE; + } + return FALSE; +} + +uint16_t GetSpiOrientationFromRegs() +{ + uint16_t tmp; + + tmp = (_spiRegs[SPI_REG_ORIENTATION_MSB_CTRL] << 8) | _spiRegs[SPI_REG_ORIENTATION_LSB_CTRL]; + return tmp; +} + +void SaveParam(uint8_t paramId) +{ + switch(paramId){ + case 0: + case 0xff: + // save all parameters + gUserConfiguration.spiOrientation = GetSpiOrientationFromRegs(); + gUserConfiguration.spiGyroLpfType = _spiRegs[SPI_REG_RATE_FILTER_TYPE_CTRL]; + gUserConfiguration.spiAccelLpfType = _spiRegs[SPI_REG_ACCEL_FILTER_TYPE_CTRL]; + case SPI_REG_DRDY_RATE_CTRL: + gUserConfiguration.spiSyncRate = _spiRegs[SPI_REG_DRDY_RATE_CTRL]; + break; + case SPI_REG_ACCEL_FILTER_TYPE_CTRL: + gUserConfiguration.spiAccelLpfType = _spiRegs[SPI_REG_ACCEL_FILTER_TYPE_CTRL]; + break; + case SPI_REG_RATE_FILTER_TYPE_CTRL: + gUserConfiguration.spiGyroLpfType = _spiRegs[SPI_REG_RATE_FILTER_TYPE_CTRL]; + break; + case SPI_REG_ORIENTATION_MSB_CTRL: + case SPI_REG_ORIENTATION_LSB_CTRL: + gUserConfiguration.spiOrientation = GetSpiOrientationFromRegs(); + break; + default: + return; + } + + fSaveSpiConfig = TRUE; + +} + +void UpdateSpiUserConfig() +{ + if(fSaveSpiConfig){ + SaveUserConfig(); + fSaveSpiConfig = FALSE; + } +} + // Here SPI write transaction is processed // based on register address assigned in processSPICommand function @@ -193,25 +361,27 @@ void SPI_ProcessData(uint8_t* in) { uint16_t tmp; static uint8_t orientShadow = 0xff; + static uint8_t resetShadow = 0x00; if(!_readOp || _regAddr != 0xff){ switch(_regAddr){ - // Place read only registers here - case SPI_REG_MANUF_CODE_REQUEST: - case SPI_REG_MANUF_LOC_REQUEST: - case SPI_REG_UNIT_CODE_REQUEST: - case SPI_REG_UNIT_CODE_REQUEST+1: - case SPI_REG_PROD_ID_REQUEST: - case SPI_REG_PROD_ID_REQUEST+1: - case SPI_REG_SERIAL_NUM_REQUEST: - case SPI_REG_SERIAL_NUM_REQUEST+1: - case SPI_REG_HW_VERSION_REQUEST: - case SPI_REG_SW_VERSION_REQUEST: + case SPI_REG_RESET_MSB_CTRL: + if(in[1] == 0x55){ + resetShadow = 1; + }else { + resetShadow = 1; + } break; - case SPI_REG_MAG_ALIGN_CTRL: - { - uint8_t len = 1; - ProcessMagAlignCmds((magAlignCmdPayload*)&in[1], &len); + case SPI_REG_RESET_LSB_CTRL: + if(in[1] == 0xaa && resetShadow){ + Reset(); + }else { + resetShadow = 0; + } + break; + case SPI_REG_DRDY_RATE_CTRL: + if(CheckSpiSyncRate(in[1])){ + _spiRegs[_regAddr] = in[1]; } break; case SPI_REG_ORIENTATION_MSB_CTRL: @@ -239,8 +409,16 @@ void SPI_ProcessData(uint8_t* in) platformUpdateRateFilterType(in[1]); } break; + case SPI_REG_MAG_ALIGN_CTRL: + { + uint8_t len = 1; + ProcessMagAlignCmds((magAlignCmdPayload*)&in[1], &len); + } + break; + case SPI_REG_SAVE_CFG_CTRL: + SaveParam(in[1]); + break; default: - _spiRegs[_regAddr] = in[1]; break; } } @@ -268,7 +446,7 @@ uint16_t overRange = 0; ******************************************************************************/ int16_t prepare_sensor_value( float dataIn, float limit, float scaleFactor) { - static uint16_t UpperLimit = 32760; + static uint16_t UpperLimit = 32766; float tempFloat = 0.0; int sign; static int16_t tempInt16 = 0x0000; @@ -306,14 +484,18 @@ int16_t prepare_sensor_value( float dataIn, float limit, float scaleFactor) // Fills SPI burst data buffer with latest samples void FillSPIBurstDataBuffer() { - spi_burst_data_t data; - double Rates[3], Accels[3]; + ext_spi_burst_data_t data; + double Rates[3], Accels[3], Mags[3]; double temp; + real EulerAngles[3]; float t1; GetRateData_degPerSec(Rates); GetAccelData_g(Accels); GetBoardTempData(&temp); + GetMagData_G(Mags); + EKF_GetAttitude_EA_RAD(EulerAngles); + data.status = 0; overRange = 0; @@ -321,37 +503,53 @@ void FillSPIBurstDataBuffer() for(int i = 0; i < 3; i++){ data.accels[i] = prepare_sensor_value(Accels[i], GetSpiAccelLimit(), GetSpiAccelScaleFactor()); data.rates[i] = prepare_sensor_value(Rates[i], GetSpiRateLimit(), GetSpiRateScaleFactor()); + t1 = Mags[i] * 16384; + data.mags[i] = swap16((int16_t)t1); + t1 = EulerAngles[i] * 10430.378; // 65536/2PI + _attitude[i] = swap16((int16_t)t1); } if(overRange){ data.status |= SENSOR_OVER_RANGE_STATUS_MASK; } - t1 = temp; + t1 = (temp - 31.0)/0.073111172849435; data.temp = swap16((int16_t)t1); loadSPIBurstData(&data); } -int GetSpiPacketRateDivider() -{ - return _spiRegs[SPI_REG_DRDY_RATE_CTRL]; -} - float GetSpiAccelScaleFactor() { - return 4000.0; // so far limited to 5G + return 4000.0; // so far limited to 8G } float GetSpiRateScaleFactor() { - return 100.0; // 100 LSB/deg/s + return 64; } float GetSpiRateLimit() { - return 300.0; // deg/sec + return 500.0; } float GetSpiAccelLimit() { - return 4.5; // so far limited to 4,5G + return 8.0; // so far limited to 8G +} + +int GetSpiPacketRateDivider() +{ + switch(_spiRegs[SPI_REG_DRDY_RATE_CTRL]){ + case 0: return 0; // 0 Hz + case 2: return 2; // 100 Hz + case 3: return 4; // 50 Hz + case 4: return 8; // 25 Hz + case 5: return 10; // 20 Hz + case 6: return 20; // 10 Hz + case 7: return 40; // 5 Hz + case 8: return 50; // 4 Hz + case 9: return 100; // 2 Hz + default: + return 1; // 200 Hz + } } diff --git a/examples/OpenIMU300ZI/INS/src/user/UserMessagingSPI.h b/examples/OpenIMU300ZI/INS/src/user/UserMessagingSPI.h index 128b8fa..777c8e6 100644 --- a/examples/OpenIMU300ZI/INS/src/user/UserMessagingSPI.h +++ b/examples/OpenIMU300ZI/INS/src/user/UserMessagingSPI.h @@ -40,39 +40,76 @@ typedef struct{ int16_t temp; }spi_burst_data_t; +#pragma pack(1) typedef struct{ uint16_t status; int16_t rates[3]; int16_t accels[3]; int16_t temp; - int16_t sensToPps; - int16_t ppsToDrdy; + int16_t mags[3]; }ext_spi_burst_data_t; +typedef struct{ + uint16_t status; + int16_t rates[3]; + int16_t accels[3]; + int16_t temp; + int16_t attitude[3]; +}att_spi_burst_data_t; +#pragma pack() #define SPI_DATA_BUF_LEN 32 #define SPI_BURST_DATA_SIZE 16 #define NUM_SPI_REGS 128 -// functions of SPI interface registers -#define SPI_REG_DRDY_RATE_CTRL 0x37 // DATA READY PIN output frequency divider (from 200Hz) -#define SPI_REG_ORIENTATION_LSB_CTRL 0x75 // Orientation LSB -#define SPI_REG_ORIENTATION_MSB_CTRL 0x74 // Orientation MSB -#define SPI_REG_ACCEL_FILTER_TYPE_CTRL 0x38 // Built-in LPF selection -#define SPI_REG_RATE_FILTER_TYPE_CTRL 0x39 // Built-in LPF selection +#define SPI_REG_RESET_LSB_CTRL 0x02 +#define SPI_REG_RESET_MSB_CTRL 0x03 + +#define SPI_REG_DRDY_RATE_CTRL 0x37 +#define SPI_REG_ACCEL_FILTER_TYPE_CTRL 0x38 +#define SPI_REG_ACCEL_SENSOR_RANGE_CTRL 0x70 +#define SPI_REG_RATE_SENSOR_RANGE_CTRL 0x71 +#define SPI_REG_MAG_SENSOR_RANGE_CTRL 0x72 +#define SPI_REG_ORIENTATION_LSB_CTRL 0x75 +#define SPI_REG_ORIENTATION_MSB_CTRL 0x74 +#define SPI_REG_SAVE_CFG_CTRL 0x76 +#define SPI_REG_RATE_FILTER_TYPE_CTRL 0x78 + +#define SPI_REG_SELF_TEST_CTRL 0x35 +#define SPI_REG_CLOB_CTRL 0x3E +#define SPI_REG_DRDY_CTRL 0x34 #define SPI_REG_MAG_ALIGN_CTRL 0x50 // mag alignment command #define SPI_REG_MAG_ALIGN_CTRL2 0x51 // mag alignment command/status -#define SPI_REG_BURST_MSG_REQUEST 0x3E // burst message request -#define SPI_REG_EXT_BURST_MSG_REQUEST 0x3F // estended burst message request -#define SPI_REG_MANUF_CODE_REQUEST 0x52 // manufacturer code -#define SPI_REG_MANUF_LOC_REQUEST 0x53 // manufacturel location -#define SPI_REG_UNIT_CODE_REQUEST 0x54 // unit code -#define SPI_REG_PROD_ID_REQUEST 0x56 // product ID - 0x3830(default) -#define SPI_REG_SERIAL_NUM_REQUEST 0x58 // serial number request -#define SPI_REG_MASTER_STATUS_REQUEST 0x5A // master status request -#define SPI_REG_HW_VERSION_REQUEST 0x7E // HW version request -#define SPI_REG_SW_VERSION_REQUEST 0x7F // SW version request +#define SPI_REG_XRATE_REQUEST 0x04 +#define SPI_REG_YRATE_REQUEST 0x06 +#define SPI_REG_ZRATE_REQUEST 0x08 +#define SPI_REG_XACCEL_REQUEST 0x0A +#define SPI_REG_YACCEL_REQUEST 0x0C +#define SPI_REG_ZACCEL_REQUEST 0x0E +#define SPI_REG_XMAG_REQUEST 0x10 +#define SPI_REG_YMAG_REQUEST 0x12 +#define SPI_REG_ZMAG_REQUEST 0x14 +#define SPI_REG_RTEMP_REQUEST 0x16 +#define SPI_REG_BTEMP_REQUEST 0x18 + +#define SPI_REG_BURST_MSG_REQUEST 0x3E + + + +#define SPI_REG_MAG_SENSOR_SCALE 0x32 +#define SPI_REG_ACCEL_SENSOR_SCALE 0x46 +#define SPI_REG_RATE_SENSOR_SCALE 0x47 +#define SPI_REG_MANUF_CODE_REQUEST 0x52 +#define SPI_REG_MANUF_LOC_REQUEST 0x53 +#define SPI_REG_UNIT_CODE_REQUEST 0x54 +#define SPI_REG_PROD_ID_REQUEST 0x56 // 0x3830 +#define SPI_REG_SERIAL_NUM_REQUEST 0x58 +#define SPI_REG_MASTER_STATUS_REQUEST 0x5A +#define SPI_REG_HW_STATUS_REQUEST 0x5C +#define SPI_REG_SW_STATUS_REQUEST 0x5E +#define SPI_REG_HW_VERSION_REQUEST 0x7E +#define SPI_REG_SW_VERSION_REQUEST 0x7F #define SPI_REG_HARD_IRON_BIAS_X_MSB 0x48 // Estimated hard iron bias X #define SPI_REG_HARD_IRON_BIAS_X_LSB 0x49 // Estimated hard iron bias X @@ -83,6 +120,7 @@ typedef struct{ #define SPI_REG_SOFT_IRON_ANGLE_MSB 0x4E // Estimated soft iron angle #define SPI_REG_SOFT_IRON_ANGLE_LSB 0x4F // Estimated soft iron angle + // options for DATA READY signal divider enum output_drdy_rate { OUTPUT_DATA_RATE_ZERO = 0, @@ -98,9 +136,21 @@ enum output_drdy_rate { OUTPUT_DATA_RATE_1_HZ = 200 }; +enum output_rate_sensor_dyn_range{ + SPI_RATE_SENSOR_RANGE_500 = 0x08, +}; + +enum output_accel_sensor_dyn_range{ + SPI_ACCEL_SENSOR_RANGE_8G = 8, +}; + +enum output_mag_sensor_dyn_range{ + SPI_MAG_SENSOR_RANGE_2G = 2, +}; + #define SENSOR_OVER_RANGE_STATUS_MASK 0x0010 -BOOL loadSPIBurstData(spi_burst_data_t *data); +BOOL loadSPIBurstData(ext_spi_burst_data_t *data); void InitUserSPIRegisters(); int16_t prepare_sensor_value( float dataIn, float limit, float scaleFactor); uint16_t GetSpiFilterType(); @@ -111,6 +161,7 @@ float GetSpiAccelLimit(); float GetSpiRateLimit(); void fillSPIDataBuffer(); int UserSPIPrepareForDataTransmit(uint8_t *out, int outLen); +void UpdateSpiUserConfig(); diff --git a/examples/OpenIMU300ZI/INS/src/user/UserMessagingUART.c b/examples/OpenIMU300ZI/INS/src/user/UserMessagingUART.c index acdbb8b..573b2f6 100644 --- a/examples/OpenIMU300ZI/INS/src/user/UserMessagingUART.c +++ b/examples/OpenIMU300ZI/INS/src/user/UserMessagingUART.c @@ -105,6 +105,7 @@ usr_packet_t userOutputPackets[] = { {USR_OUT_SCALED1, "s1"}, {USR_OUT_EKF1, "e1"}, {USR_OUT_EKF2, "e2"}, + {USR_OUT_ID, "id"}, {USR_OUT_MAX, {0xff, 0xff}}, // "" }; @@ -160,7 +161,7 @@ void userPacketTypeToBytes(uint8_t bytes[]) // continuous packet bytes[0] = userOutputPackets[_outputPacketType].packetCode[0]; bytes[1] = userOutputPackets[_outputPacketType].packetCode[1]; - } else { + }else { bytes[0] = 0; bytes[1] = 0; } @@ -218,6 +219,10 @@ BOOL setUserPacketType(uint8_t *data, BOOL fApply) _outputPacketType = type; _userPayloadLen = USR_OUT_EKF2_PAYLOAD_LEN; break; + case USR_OUT_ID: + _outputPacketType = type; + _userPayloadLen = USR_OUT_ID_PAYLOAD_LEN; + break; default: result = FALSE; break; @@ -538,6 +543,115 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) *payloadLen = len; } break; + // break; + case USR_OUT_ID: + { + // The payload length (NumOfBytes) is based on the following: + // 1 uint32_t (4 bytes) = 4 bytes timer + // 1 float (4 bytes) = 4 bytes GPS heading + // 1 uint32_t (4 bytes) = 4 bytes GPS itow + // 3 floats (4 bytes) = 12 bytes ea + // 3 floats (4 bytes) = 12 bytes a + // 3 floats (4 bytes) = 12 bytes aBias + // 3 floats (4 bytes) = 12 bytes w + // 3 floats (4 bytes) = 12 bytes wBias + // 3 floats (4 bytes) = 12 bytes v + // 3 floats (4 bytes) = 12 bytes gps NED velocity + // 3 double (8 bytes) = 24 bytes lla + // 3 double (8 bytes) = 24 bytes gps LLA + // 1 uint8_t (1 byte) = 1 bytes + // 1 uint8_t (1 byte) = 1 bytes + // 1 uint8_t (1 byte) = 1 bytes + // ================================= + // NumOfBytes = 147 bytes + *payloadLen = USR_OUT_ID_PAYLOAD_LEN; + + // Output time as represented by gLeveler.timerCntr (uint32_t + // incremented at each call of the algorithm) + uint32_t *algoData_1 = (uint32_t*)(payload); + *algoData_1++ = gIMU.timerCntr; + + // Set the pointer of the algoData array to the payload + float *algoData_2_1 = (float*)(algoData_1); + *algoData_2_1++ = (float)gEKFInput.trueCourse; + uint32_t *algoData_2 = (uint32_t*)(algoData_2_1); + // *algoData_2++ = (double)( 0.001 * gIMU.timerCntr ); + *algoData_2++ = gEKFInput.itow; + + // Set the pointer of the algoData array to the payload + float *algoData_3 = (float*)(algoData_2); + real EulerAngles[NUM_AXIS]; + EKF_GetAttitude_EA(EulerAngles); + *algoData_3++ = (float)EulerAngles[ROLL]; + *algoData_3++ = (float)EulerAngles[PITCH]; + *algoData_3++ = (float)EulerAngles[YAW]; + + // double accels[NUM_AXIS]; + *algoData_3++ = (float)gIMU.accel_g[X_AXIS]; + *algoData_3++ = (float)gIMU.accel_g[Y_AXIS]; + *algoData_3++ = (float)gIMU.accel_g[Z_AXIS]; + + // float accelBias[NUM_AXIS]; + // EKF_GetEstimatedAccelBias(accelBias); + // *algoData_3++ = (float)accelBias[X_AXIS]; + // *algoData_3++ = (float)accelBias[Y_AXIS]; + // *algoData_3++ = (float)accelBias[Z_AXIS]; + *algoData_3++ = (float)gEKFInput.HDOP; + *algoData_3++ = (float)gEKFInput.GPSHorizAcc; + *algoData_3++ = (float)gEKFInput.GPSVertAcc; + + // double rates[NUM_AXIS]; + *algoData_3++ = (float)gIMU.rate_degPerSec[X_AXIS]; + *algoData_3++ = (float)gIMU.rate_degPerSec[Y_AXIS]; + *algoData_3++ = (float)gIMU.rate_degPerSec[Z_AXIS]; + + float rateBias[NUM_AXIS]; + EKF_GetEstimatedAngRateBias(rateBias); + *algoData_3++ = (float)rateBias[X_AXIS]; + *algoData_3++ = (float)rateBias[Y_AXIS]; + *algoData_3++ = (float)rateBias[Z_AXIS]; + + float vel[NUM_AXIS]; + EKF_GetEstimatedVelocity(vel); + *algoData_3++ = (float)vel[X_AXIS]; + *algoData_3++ = (float)vel[Y_AXIS]; + *algoData_3++ = (float)vel[Z_AXIS]; + + // double mags[NUM_AXIS]; + // GetMagData_G(mags); + *algoData_3++ = (float)gEKFInput.vNedAnt[0]; + *algoData_3++ = (float)gEKFInput.vNedAnt[1]; + *algoData_3++ = (float)gEKFInput.vNedAnt[2]; + + // Set the pointer of the algoData array to the payload + double *algoData_4 = (double*)(algoData_3); + double lla[NUM_AXIS]; + EKF_GetEstimatedLLA(lla); + *algoData_4++ = (double)lla[LAT]; + *algoData_4++ = (double)lla[LON]; + *algoData_4++ = (double)lla[ALT]; + + // debug + *algoData_4++ = (double)gEKFInput.llaAnt[LAT] * R2D; + *algoData_4++ = (double)gEKFInput.llaAnt[LON] * R2D; + *algoData_4++ = (double)gEKFInput.llaAnt[ALT]; + + // Set the pointer of the algoData array to the payload + uint8_t *algoData_5 = (uint8_t*)(algoData_4); + uint8_t opMode, linAccelSw, turnSw; + EKF_GetOperationalMode(&opMode); + EKF_GetOperationalSwitches(&linAccelSw, &turnSw); + turnSw = turnSw << 1; + turnSw |= gEKFInput.ppsDetected; + turnSw = turnSw << 1; + turnSw |= gEKFInput.gpsFixType; + turnSw = turnSw << 1; + turnSw |= gEKFInput.gpsUpdate; + *algoData_5++ = opMode; + *algoData_5++ = gEKFInput.numSatellites; + *algoData_5++ = turnSw; + } + break; // place additional user packet preparing calls here // case USR_OUT_XXXX: diff --git a/examples/OpenIMU300ZI/INS/src/user/UserMessagingUART.h b/examples/OpenIMU300ZI/INS/src/user/UserMessagingUART.h index 70d230c..f1f00f2 100644 --- a/examples/OpenIMU300ZI/INS/src/user/UserMessagingUART.h +++ b/examples/OpenIMU300ZI/INS/src/user/UserMessagingUART.h @@ -44,7 +44,7 @@ typedef enum { PACKET_RATE_5HZ = 5, // packet rate 5 Hz PACKET_RATE_2HZ = 2, // packet rate 2 Hz PACKET_RATE_1HZ = 1, // packet rate 1 Hz -} packet_rate_t; +}packet_rate_t; // User Input packet payload has next structure: @@ -70,7 +70,7 @@ typedef enum { // add new packet type here, before USR_IN_MAX USR_IN_MAG_ALIGN , USR_IN_MAX -} UserInPacketType; +}UserInPacketType; // User output packet codes, change at will typedef enum { @@ -83,6 +83,7 @@ typedef enum { USR_OUT_SCALED1, USR_OUT_EKF1, USR_OUT_EKF2, + USR_OUT_ID, USR_OUT_MAX } UserOutPacketType; @@ -91,7 +92,7 @@ typedef enum { #pragma pack(1) typedef struct { uint8_t packetPayload[252]; // maximum 252 bytes -} userPacket; +}userPacket; #define MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET 30 #define FIRST_30_PARAMS 0xFFFFFFFF @@ -115,7 +116,6 @@ typedef struct { uint64_t parameters[MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET]; // up to 30 64-bit parameters (little endian) }allUserParamsPayload; - #pragma pack() @@ -126,6 +126,7 @@ typedef struct { #define USR_OUT_SCALED1_PAYLOAD_LEN (52) #define USR_OUT_EKF1_PAYLOAD_LEN (75) #define USR_OUT_EKF2_PAYLOAD_LEN (123) +#define USR_OUT_ID_PAYLOAD_LEN (147) #define USER_OK 0x00 #define USER_NAK 0x80 diff --git a/examples/OpenIMU300ZI/INS/src/user/dataProcessingAndPresentation.c b/examples/OpenIMU300ZI/INS/src/user/dataProcessingAndPresentation.c index e82080c..cefcab0 100644 --- a/examples/OpenIMU300ZI/INS/src/user/dataProcessingAndPresentation.c +++ b/examples/OpenIMU300ZI/INS/src/user/dataProcessingAndPresentation.c @@ -43,7 +43,9 @@ BOOL fAlgorithmSynced; static void _IncrementIMUTimer(uint16_t dacqRate); static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); static void _IMUDebugMessage(void); +#ifdef GPS static void _GPSDebugMessage(void); +#endif /* * ****************** @@ -84,11 +86,6 @@ void inertialAndPositionDataProcessing(uint16_t dacqRate) // void *results; - // if 1PPS event detection required - use next two functions - // solutionTstamp is extrapolated Itow with 1uS precision - double solutionTstamp = platformGetSolutionTstampAsDouble(); - - // Increment the IMU timer by the calling rate of the data-acquisition task _IncrementIMUTimer(dacqRate); @@ -144,7 +141,9 @@ void inertialAndPositionDataProcessing(uint16_t dacqRate) GetBoardTempData(&gIMU.temp_C); // Obtain GPS data (lat/lon: deg, alt: meters, vel: m/s, ITOW: msec, ) +#ifdef GPS GetGPSData(&gGPS); +#endif // check if pps is detected right before this excution of the task. BOOL ppsDetected = platformGetPpsFlag(TRUE); @@ -187,7 +186,6 @@ static void _IncrementIMUTimer(uint16_t dacqRate) gIMU.timerCntr = gIMU.timerCntr + gIMU.dTimerCntr; } - // static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) { @@ -219,7 +217,7 @@ static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) // to the algorithm isn't set yet. // Create message here - static uint8_t msgType = 1; + static uint8_t msgType = 2; switch( msgType ) { case 0: @@ -231,10 +229,12 @@ static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) _IMUDebugMessage(); break; +#ifdef GPS case 2: // GPS data _GPSDebugMessage(); break; +#endif } } } @@ -258,6 +258,7 @@ static void _IMUDebugMessage(void) DebugPrintEndline(); } +#ifdef GPS static void _GPSDebugMessage(void) { @@ -294,3 +295,4 @@ static void _GPSDebugMessage(void) #endif DebugPrintEndline(); } +#endif diff --git a/examples/OpenIMU300ZI/INS/include/API/userAPI.h b/examples/OpenIMU300ZI/INS/src/user/userAPI.h similarity index 100% rename from examples/OpenIMU300ZI/INS/include/API/userAPI.h rename to examples/OpenIMU300ZI/INS/src/user/userAPI.h diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/FreeRTOSConfig.h b/examples/OpenIMU300ZI/VG_AHRS/include/FreeRTOSConfig.h similarity index 100% rename from examples/OpenIMU300ZI/VG_AHRS/src/FreeRTOSConfig.h rename to examples/OpenIMU300ZI/VG_AHRS/include/FreeRTOSConfig.h diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/osresources.h b/examples/OpenIMU300ZI/VG_AHRS/include/osresources.h similarity index 100% rename from examples/OpenIMU300ZI/VG_AHRS/src/osresources.h rename to examples/OpenIMU300ZI/VG_AHRS/include/osresources.h diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/algorithmAPI.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/algorithmAPI.h index 51705e4..c68ed69 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/algorithmAPI.h +++ b/examples/OpenIMU300ZI/VG_AHRS/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. @@ -68,6 +68,22 @@ void enableGpsInAlgorithm(BOOL enable); BOOL gpsUsedInAlgorithm(); +void enableOdoInAlgorithm(BOOL enable); + +BOOL odoUsedInAlgorithm(); + +void enableFreeIntegration(BOOL enable); + +BOOL freeIntegrationEnabled(); + +void enableStationaryLockYaw(BOOL enable); + +BOOL stationaryLockYawEnabled(); + +void enableImuStaticDetect(BOOL enable); + +BOOL imuStaticDetectEnabled(); + void SetAlgorithmUseDgps(BOOL d); void updateAlgorithmTimings(int corr, uint32_t tmrVal ); @@ -77,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(); /****************************************************************************** diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/EKF_Algorithm.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/EKF_Algorithm.h index b789f6a..4028640 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/EKF_Algorithm.h +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/EKF_Algorithm.h @@ -14,6 +14,7 @@ #include "StateIndices.h" #include "gpsAPI.h" // For gpsDataStruct_t in EKF setter +#include "odoAPI.h" #include "Indices.h" @@ -64,10 +65,23 @@ typedef struct { real R[9]; real K[NUMBER_OF_EKF_STATES][3]; real stateUpdate[NUMBER_OF_EKF_STATES]; + real deltaP_tmp[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; // The following two are used in more than one functions, so they are pre-computed. real wTrueTimesDtOverTwo[NUM_AXIS]; real turnSwitchMultiplier; + + // saved states when pps comes in + real ppsP[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; + real phi[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; + real dQ[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; + real ppsPosition_N[NUM_AXIS]; + real ppsVelocity_N[NUM_AXIS]; + //real ppsQuaternion[4]; + real ppsEulerAngles[NUM_AXIS]; + //real ppsRateBias_B[NUM_AXIS]; + //real ppsAccelBias_B[NUM_AXIS]; + uint32_t ppsITow; } KalmanFilterStruct; extern KalmanFilterStruct gKalmanFilter; @@ -96,6 +110,13 @@ typedef struct { * This is valid only when there is gps udpate. */ BOOL gpsUpdate; // Indicate if GNSS measurement is updated. + + // odometer + BOOL odoUpdate; // indicate if odo measurement is updated + real odoVelocity; // velocity from odo, [m/s] + + // 1PPS from GNSS receiver + BOOL ppsDetected; } EKF_InputDataStruct; extern EKF_InputDataStruct gEKFInput; @@ -148,7 +169,10 @@ void EKF_GetOperationalMode(uint8_t *EKF_OperMode); void EKF_GetOperationalSwitches(uint8_t *EKF_LinAccelSwitch, uint8_t *EKF_TurnSwitch); // Setter functions -void EKF_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStruct_t *gps); +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_ */ diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/MotionStatus.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/MotionStatus.h index 785ac68..57e4fcf 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/MotionStatus.h +++ b/examples/OpenIMU300ZI/VG_AHRS/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. diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/TimingVars.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/TimingVars.h index 1799cbc..5f545f0 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/TimingVars.h +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/TimingVars.h @@ -33,6 +33,7 @@ float TimingVars_GetTime(void); void TimingVars_SetTMin(float tMin); float TimingVars_GetTMin(void); +void TimingVars_dacqFrequency(int freq); void TimingVars_DisplayTimerVars(signed long timeStep); diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/algorithm.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/algorithm.h index 1f479d8..a9a578b 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/algorithm.h +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/include/algorithm.h @@ -77,24 +77,24 @@ typedef struct { int32_t Declination_Expiration_Time; // [msec] uint16_t Free_Integration_Cntr; // [count] -//#define LIMIT_MAX_REST_TIME_BEFORE_DROP_TO_AHRS 60000 // 60000 [ msec ] = 60 [ sec ] -//#define LIMIT_DECL_EXPIRATION_TIME 60000 // 60,000 [ counts ] = 10 [ min ] real accelSwitch; uint32_t linAccelSwitchDelay; - + uint32_t rateIntegrationTime; InnovationStruct Innov; } LimitStruct; /// specifying how the user sets up the device algorithm -struct algoBehavior_BITS { /// bits description - uint16_t freeIntegrate : 1; /// 0 - uint16_t useMag : 1; /// 1 - uint16_t useGPS : 1; /// 2 - Not used yet - uint16_t stationaryLockYaw : 1; /// 3 - Not used yet - uint16_t restartOnOverRange : 1; /// 4 - uint16_t dynamicMotion : 1; /// 5 - Not used - uint16_t rsvd : 10; /// 6:15 +struct algoBehavior_BITS { // bits description + uint16_t freeIntegrate : 1; // 0 + uint16_t useMag : 1; // 1 + uint16_t useGPS : 1; // 2 + uint16_t useOdo : 1; // 3 + uint16_t enableStationaryLockYaw : 1; // 4 + uint16_t restartOnOverRange : 1; // 5 + uint16_t dynamicMotion : 1; // 6 + uint16_t enableImuStaticDetect : 1; // 7 + uint16_t rsvd : 8; // 8:15 }; union AlgoBehavior @@ -110,11 +110,21 @@ struct ALGO_STATUS_BITS uint16_t highGain : 1; // 1 high gain mode uint16_t attitudeOnlyAlgorithm : 1; // 2 attitude only algorithm uint16_t turnSwitch : 1; // 3 turn switch - uint16_t noAirdataAiding : 1; // 4 airdata aiding - uint16_t noMagnetometerheading : 1; // 5 magnetometer heading - uint16_t noGPSTrackReference : 1; // 6 GPS track - uint16_t gpsUpdate : 1; // 7 GPS measurement update - uint16_t rsvd : 8; // 8:15 + uint16_t linearAccel : 1; // 4 Linear acceleration detected by difference from gravity + uint16_t staticImu : 1; // 5 zero velocity detected by IMU + uint16_t gpsHeadingValid : 1; /* 6 When GPS velocity is above a certain threshold, + * this is set to TRUE, and GPS heading measurement + * is used, otherwise, this is set to FALSE and magnetic + * heading (if available) is used. + */ + uint16_t stationaryYawLock : 1; // 7 Yaw is locked when unit is static + uint16_t ppsAvailable : 1; /* 8. + * This will be initialized as FALSE. + * This will become TRUE when PPS is detected for the first time. + * This will become FALSE when PPS has not been detected for more than 2sec. + * This will become FALSE when GNSS measurement is refrshed and used. + */ + uint16_t rsvd : 7; // 9:15 }; typedef union ALGO_STATUS @@ -182,6 +192,8 @@ typedef struct { uint8_t linAccelLPFType; uint8_t useRawAccToDetectLinAccel; + uint8_t useRawRateToPredAccel; + real coefOfReduceQ; uint8_t callingFreq; real dt; @@ -205,8 +217,14 @@ typedef struct { DurationStruct Duration; LimitStruct Limit; + enumIMUType imuType; + } AlgorithmStruct; extern AlgorithmStruct gAlgorithm; +// Update IMU specs. +void UpdateImuSpec(real rwOdr, real arw, real biw, real maxBiasW, + real vrw, real bia, real maxBiasA); + #endif diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/EKF_Algorithm.c b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/EKF_Algorithm.c index 6081705..2874e27 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/EKF_Algorithm.c +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/EKF_Algorithm.c @@ -61,6 +61,9 @@ ImuStatsStruct gImuStats; static void RemoveLeverArm(double *lla, double *vNed, real *w, real *leverArmB, real *rn2e, double *ecef); +static void HandlePps(); +static void SaveKfStateAtPps(); + //============================================================================= /* This routine is called at either 100 or 200 Hz based upon the system configuration: @@ -91,6 +94,7 @@ void EKF_Algorithm(void) gEKFInput.angRate_B, gAlgorithm.dt, gAlgorithm.Limit.linAccelSwitchDelay, + gKalmanFilter.rateBias_B, &gImuStats); } else @@ -99,11 +103,20 @@ void EKF_Algorithm(void) gEKFInput.angRate_B, gAlgorithm.dt, gAlgorithm.Limit.linAccelSwitchDelay, + gKalmanFilter.rateBias_B, &gImuStats); } // Detect if the unit is static or dynamic DetectMotionFromAccel(gImuStats.accelNorm, 0); + gAlgoStatus.bit.linearAccel = !gAlgorithm.linAccelSwitch; + + // if zero velocity detection by IMU is not enabled, detection result is always false. + if (!gAlgorithm.Behavior.bit.enableImuStaticDetect) + { + gImuStats.bStaticIMU = FALSE; + } + gAlgoStatus.bit.staticImu = gImuStats.bStaticIMU; } // Compute the EKF solution if past the stabilization and initialization stages @@ -115,6 +128,11 @@ void EKF_Algorithm(void) // Perform EKF Prediction EKF_PredictionStage(gImuStats.lpfAccel); + if (gAlgorithm.state == INS_SOLUTION) + { + HandlePps(); + } + /* Update the predicted states if not freely integrating * NOTE: free- integration is not applicable in HG AHRS mode. */ @@ -222,42 +240,65 @@ void EKF_Algorithm(void) DynamicMotion(); } -void enableFreeIntegration(BOOL enable) +static void HandlePps() { - gAlgorithm.Behavior.bit.freeIntegrate = enable; -} - - -BOOL freeIntegrationEnabled() -{ - return (BOOL)gAlgorithm.Behavior.bit.freeIntegrate; -} - -void enableMagInAlgorithm(BOOL enable) -{ - if(1) + // PPS not detected for a long time? + int32_t timeSinceLastPps = (int32_t)(gAlgorithm.itow - gKalmanFilter.ppsITow); + if (timeSinceLastPps < 0) { - gAlgorithm.Behavior.bit.useMag = enable; + timeSinceLastPps = timeSinceLastPps + MAX_ITOW; } - else + if (timeSinceLastPps > 2000) { - gAlgorithm.Behavior.bit.useMag = FALSE; + gAlgoStatus.bit.ppsAvailable = FALSE; + } + // PPS detected + if (gEKFInput.ppsDetected) + { + // PPS is available from now. + gAlgoStatus.bit.ppsAvailable = TRUE; + // save states when pps detected + SaveKfStateAtPps(); } } -BOOL magUsedInAlgorithm() -{ - return gAlgorithm.Behavior.bit.useMag != 0; -} - -BOOL gpsUsedInAlgorithm(void) -{ - return (BOOL)gAlgorithm.Behavior.bit.useGPS; -} - -void enableGpsInAlgorithm(BOOL enable) +static void SaveKfStateAtPps() { - gAlgorithm.Behavior.bit.useGPS = enable; + // save time + gKalmanFilter.ppsITow = gAlgorithm.itow; + + // save states + gKalmanFilter.ppsPosition_N[0] = gKalmanFilter.Position_N[0]; + gKalmanFilter.ppsPosition_N[1] = gKalmanFilter.Position_N[1]; + gKalmanFilter.ppsPosition_N[2] = gKalmanFilter.Position_N[2]; + gKalmanFilter.ppsVelocity_N[0] = gKalmanFilter.Velocity_N[0]; + gKalmanFilter.ppsVelocity_N[1] = gKalmanFilter.Velocity_N[1]; + gKalmanFilter.ppsVelocity_N[2] = gKalmanFilter.Velocity_N[2]; + //gKalmanFilter.ppsQuaternion[0] = gKalmanFilter.quaternion[0]; + //gKalmanFilter.ppsQuaternion[1] = gKalmanFilter.quaternion[1]; + //gKalmanFilter.ppsQuaternion[2] = gKalmanFilter.quaternion[2]; + //gKalmanFilter.ppsQuaternion[3] = gKalmanFilter.quaternion[3]; + gKalmanFilter.ppsEulerAngles[0] = gKalmanFilter.eulerAngles[0]; + gKalmanFilter.ppsEulerAngles[1] = gKalmanFilter.eulerAngles[1]; + gKalmanFilter.ppsEulerAngles[2] = gKalmanFilter.eulerAngles[2]; + //gKalmanFilter.ppsRateBias_B[0] = gKalmanFilter.rateBias_B[0]; + //gKalmanFilter.ppsRateBias_B[1] = gKalmanFilter.rateBias_B[1]; + //gKalmanFilter.ppsRateBias_B[2] = gKalmanFilter.rateBias_B[2]; + //gKalmanFilter.ppsAccelBias_B[0] = gKalmanFilter.accelBias_B[0]; + //gKalmanFilter.ppsAccelBias_B[1] = gKalmanFilter.accelBias_B[1]; + //gKalmanFilter.ppsAccelBias_B[2] = gKalmanFilter.accelBias_B[2]; + // save state covariance matrix + memcpy(&gKalmanFilter.ppsP[0][0], &gKalmanFilter.P[0][0], sizeof(gKalmanFilter.P)); + + /* reset state transition matrix and state covariance matrix increment between pps and update + * phi is reset to an identity matrix, and dQ is reset to a zero matrix. + */ + memset(&gKalmanFilter.phi[0][0], 0, sizeof(gKalmanFilter.phi)); + memset(&gKalmanFilter.dQ[0][0], 0, sizeof(gKalmanFilter.dQ)); + for (int i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + gKalmanFilter.phi[i][i] = 1.0; + } } // Getters based on results structure passed to WriteResultsIntoOutputStream() @@ -397,7 +438,9 @@ void EKF_GetOperationalSwitches(uint8_t *EKF_LinAccelSwitch, uint8_t *EKF_TurnSw // SETTERS: for EKF input and output structures // Populate the EKF input structure with sensor and GPS data (if used) -void EKF_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStruct_t *gps) +void EKF_SetInputStruct(double *accels, double *rates, double *mags, + gpsDataStruct_t *gps, odoDataStruct_t *odo, + BOOL ppsDetected) { // Accelerometer signal is in [m/s/s] gEKFInput.accel_B[X_AXIS] = (real)accels[X_AXIS] * GRAVITY; @@ -450,8 +493,8 @@ void EKF_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStru gEKFInput.vNedAnt[Z_AXIS] = gps->vNed[Z_AXIS]; // Course and velocity data - gEKFInput.rawGroundSpeed = (real)sqrt(SQUARE(gEKFInput.vNed[0]) + - SQUARE(gEKFInput.vNed[1]));// gps->rawGroundSpeed; + gEKFInput.rawGroundSpeed = (real)sqrt(SQUARE(gps->vNed[0]) + + SQUARE(gps->vNed[1]));// gps->rawGroundSpeed; gEKFInput.trueCourse = (real)gps->trueCourse; /* Remove lever arm effects in LLA/Velocity. To do this requires transformation matrix @@ -490,6 +533,13 @@ void EKF_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStru &gKalmanFilter.rGPS_N[0]); } } + + // odometer + gEKFInput.odoUpdate = odo->update; + gEKFInput.odoVelocity = odo->v; + + // 1PPS signal from GNSS receiver + gEKFInput.ppsDetected = ppsDetected; } @@ -732,4 +782,3 @@ static void RemoveLeverArm(double *lla, double *vNed, real *w, real *leverArmB, ecef[Y_AXIS] = tmp * sinLon; ecef[Z_AXIS] = ((E_MINOR_OVER_MAJOR_SQ * (rn)) + lla[ALT]) * sinLat; } - diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/MotionStatus.c b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/MotionStatus.c index 220f0cd..017d8e9 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/MotionStatus.c +++ b/examples/OpenIMU300ZI/VG_AHRS/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; -} \ No newline at end of file +} diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/PredictFunctions.c b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/PredictFunctions.c index 3647db5..8f56b75 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/PredictFunctions.c +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/PredictFunctions.c @@ -78,7 +78,7 @@ uint8_t RLE_Q[ROWS_IN_F][2] = { { STATE_RX, STATE_RX }, // Local functions static void _PredictStateEstimate(void); static void _PredictCovarianceEstimate(void); - +static void StateCovarianceChange(); static void _UpdateProcessJacobian(void); static void _UpdateProcessCovariance(void); @@ -101,6 +101,19 @@ void EKF_PredictionStage(real *filteredAccel) _PredictStateEstimate(); // x(k+1) = x(k) + f(x(k), u(k)) _PredictCovarianceEstimate(); // P = F*P*FTrans + Q + /* calculate state transition matrix and state covarariance increment + *P(k) = F(k)*( F(k-1) * P(k-1) * F(k-1)' )*F(k)' + Q(k) + * = F(k)...F(1)*P(1)*F(1)'...F(k)' + F(k)...F(2)*Q1*F(2)'...F(k)' + ... + F(k)*Q(k-1)*F(k)' + Q(k) + * = phi(k)*P(1)*phi(k)' + dQ(k) + * phi(k) = F(k)*phi(k-1), phi(1) = eye(N) + * dQ(k) = F(k)*dQ(k-1)*F(k)' + Q(k), dQ(1) = zeros(N) + */ + if (gAlgoStatus.bit.ppsAvailable) + { + // This is done only when pps is detected. + StateCovarianceChange(); + } + // Extract the predicted Euler angles from the predicted quaternion QuaternionToEulerAngles( gKalmanFilter.eulerAngles, gKalmanFilter.quaternion ); @@ -664,7 +677,6 @@ static void _UpdateProcessCovariance(void) biSq[Y_AXIS] = biSq[X_AXIS]; biSq[Z_AXIS] = biSq[X_AXIS]; - /* Rate-bias terms (computed once as it does not change with attitude). * sigDriftDot = (2*pi/ln(2)) * BI^2 / ARW * 2*pi/ln(2) = 9.064720283654388 @@ -680,9 +692,16 @@ static void _UpdateProcessCovariance(void) gKalmanFilter.Q[STATE_WBZ] = sigDriftDot * biSq[Z_AXIS] * gAlgorithm.dt; gKalmanFilter.Q[STATE_WBZ] = gKalmanFilter.Q[STATE_WBZ] * gKalmanFilter.Q[STATE_WBZ]; - gKalmanFilter.Q[STATE_ABX] = (real)1.0e-12; - gKalmanFilter.Q[STATE_ABY] = (real)1.0e-12; - gKalmanFilter.Q[STATE_ABZ] = (real)1.0e-12; + // Accel bias + biSq[X_AXIS] = gAlgorithm.imuSpec.biA * gAlgorithm.imuSpec.biA; + biSq[Y_AXIS] = biSq[X_AXIS]; + biSq[Z_AXIS] = biSq[X_AXIS]; + sigDriftDot = (real)9.064720283654388 / gAlgorithm.imuSpec.vrw; + gKalmanFilter.Q[STATE_ABX] = sigDriftDot * biSq[X_AXIS] * gAlgorithm.dt; + gKalmanFilter.Q[STATE_ABX] = gKalmanFilter.Q[STATE_ABX] * gKalmanFilter.Q[STATE_ABX]; + //gKalmanFilter.Q[STATE_ABX] = (real)1.0e-12; + gKalmanFilter.Q[STATE_ABY] = gKalmanFilter.Q[STATE_ABX]; + gKalmanFilter.Q[STATE_ABZ] = gKalmanFilter.Q[STATE_ABX]; /* Precalculate the multiplier applied to the Q terms associated with * attitude. sigRate = ARW / sqrt( dt ) @@ -713,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]; } } @@ -803,13 +822,92 @@ void GenerateProcessCovariance(void) gKalmanFilter.Q[STATE_VX] = 100 * dtSigAccelSq;//(real)1e-10; gKalmanFilter.Q[STATE_VY] = 100 * dtSigAccelSq; gKalmanFilter.Q[STATE_VZ] = 100 * dtSigAccelSq; - - // Acceleration - bias - gKalmanFilter.Q[STATE_ABX] = (real)5e-11; //(real)1e-10; // dtSigAccelSq; //%1e-8 %sigmaAccelBiasSq; - gKalmanFilter.Q[STATE_ABY] = (real)5e-11; //(real)1e-10; //dtSigAccelSq; //%sigmaAccelBiasSq; - gKalmanFilter.Q[STATE_ABZ] = (real)5e-11; //(real)1e-10; //dtSigAccelSq; //%sigmaAccelBiasSq; } +static void StateCovarianceChange() +{ + uint8_t rowNum, colNum, multIndex; + // 1) phi(k) = F(k)*phi(k-1) + // deltaP_tmp is used to hold F(k)*phi(k-1) + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); + for (rowNum = 0; rowNum < 16; rowNum++) + { + for (colNum = 0; colNum < 16; colNum++) + { + for (multIndex = RLE_F[rowNum][0]; multIndex <= RLE_F[rowNum][1]; multIndex++) + { + gKalmanFilter.deltaP_tmp[rowNum][colNum] += + gKalmanFilter.F[rowNum][multIndex] * gKalmanFilter.phi[multIndex][colNum]; + } + } + } + memcpy(&gKalmanFilter.phi[0][0], &gKalmanFilter.deltaP_tmp[0][0], sizeof(gKalmanFilter.phi)); + + // 2) dQ(k) = F(k)*dQ(k-1)*F(k)' + Q(k) + // 2.1) deltaP_tmp is used to hold F(k)*dQ(k-1) + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); + for (rowNum = 0; rowNum < 16; rowNum++) + { + for (colNum = 0; colNum < 16; colNum++) + { + for (multIndex = RLE_F[rowNum][0]; multIndex <= RLE_F[rowNum][1]; multIndex++) + { + gKalmanFilter.deltaP_tmp[rowNum][colNum] += + gKalmanFilter.F[rowNum][multIndex] * gKalmanFilter.dQ[multIndex][colNum]; + } + } + } + // 2.2) Calculate F(k)*dQ(k-1)*F(k)' = deltaP_tmp*F(k)' + memset(&gKalmanFilter.dQ[0][0], 0, sizeof(gKalmanFilter.dQ)); + for (rowNum = 0; rowNum < ROWS_IN_F; rowNum++) + { + for (colNum = rowNum; colNum < ROWS_IN_F; colNum++) + { + for (multIndex = RLE_F[colNum][0]; multIndex <= RLE_F[colNum][1]; multIndex++) + { + gKalmanFilter.dQ[rowNum][colNum] = gKalmanFilter.dQ[rowNum][colNum] + + gKalmanFilter.deltaP_tmp[rowNum][multIndex] * gKalmanFilter.F[colNum][multIndex]; + } + gKalmanFilter.dQ[colNum][rowNum] = gKalmanFilter.dQ[rowNum][colNum]; + } + } + // 2.3) Calculate F(k)*dQ(k-1)*F(k)' + Q(k) + gKalmanFilter.dQ[STATE_RX][STATE_RX] += gKalmanFilter.Q[STATE_RX]; + gKalmanFilter.dQ[STATE_RY][STATE_RY] += gKalmanFilter.Q[STATE_RY]; + gKalmanFilter.dQ[STATE_RZ][STATE_RZ] += gKalmanFilter.Q[STATE_RZ]; + + gKalmanFilter.dQ[STATE_VX][STATE_VX] += gKalmanFilter.Q[STATE_VX]; + gKalmanFilter.dQ[STATE_VY][STATE_VY] += gKalmanFilter.Q[STATE_VY]; + gKalmanFilter.dQ[STATE_VZ][STATE_VZ] += gKalmanFilter.Q[STATE_VZ]; + + gKalmanFilter.dQ[STATE_Q0][STATE_Q0] += gKalmanFilter.Q[STATE_Q0]; + gKalmanFilter.dQ[STATE_Q0][STATE_Q1] += gKalmanFilter.Qq[0]; + gKalmanFilter.dQ[STATE_Q0][STATE_Q2] += gKalmanFilter.Qq[1]; + gKalmanFilter.dQ[STATE_Q0][STATE_Q3] += gKalmanFilter.Qq[2]; + + gKalmanFilter.dQ[STATE_Q1][STATE_Q0] = gKalmanFilter.dQ[STATE_Q0][STATE_Q1]; + gKalmanFilter.dQ[STATE_Q1][STATE_Q1] += gKalmanFilter.Q[STATE_Q1]; + gKalmanFilter.dQ[STATE_Q1][STATE_Q2] += gKalmanFilter.Qq[3]; + gKalmanFilter.dQ[STATE_Q1][STATE_Q3] += gKalmanFilter.Qq[4]; + + gKalmanFilter.dQ[STATE_Q2][STATE_Q0] = gKalmanFilter.dQ[STATE_Q0][STATE_Q2]; + gKalmanFilter.dQ[STATE_Q2][STATE_Q1] = gKalmanFilter.dQ[STATE_Q1][STATE_Q2]; + gKalmanFilter.dQ[STATE_Q2][STATE_Q2] += gKalmanFilter.Q[STATE_Q2]; + gKalmanFilter.dQ[STATE_Q2][STATE_Q3] += gKalmanFilter.Qq[5]; + + gKalmanFilter.dQ[STATE_Q3][STATE_Q0] = gKalmanFilter.dQ[STATE_Q0][STATE_Q3]; + gKalmanFilter.dQ[STATE_Q3][STATE_Q1] = gKalmanFilter.dQ[STATE_Q1][STATE_Q3]; + gKalmanFilter.dQ[STATE_Q3][STATE_Q2] = gKalmanFilter.dQ[STATE_Q2][STATE_Q3]; + gKalmanFilter.dQ[STATE_Q3][STATE_Q3] += gKalmanFilter.Q[STATE_Q3]; + + gKalmanFilter.dQ[STATE_WBX][STATE_WBX] += gKalmanFilter.Q[STATE_WBX]; + gKalmanFilter.dQ[STATE_WBY][STATE_WBY] += gKalmanFilter.Q[STATE_WBY]; + gKalmanFilter.dQ[STATE_WBZ][STATE_WBZ] += gKalmanFilter.Q[STATE_WBZ]; + + gKalmanFilter.dQ[STATE_ABX][STATE_ABX] += gKalmanFilter.Q[STATE_ABX]; + gKalmanFilter.dQ[STATE_ABY][STATE_ABY] += gKalmanFilter.Q[STATE_ABY]; + gKalmanFilter.dQ[STATE_ABZ][STATE_ABZ] += gKalmanFilter.Q[STATE_ABZ]; +} /** **************************************************************************** * @name: firstOrderLowPass_float implements a low pass yaw axis filter diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/SelectState.c b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/SelectState.c index d4906ed..0fe20ee 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/SelectState.c +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/SelectState.c @@ -311,6 +311,8 @@ void LG_To_INS_Transition_Test(void) // Transit to INS solution gAlgorithm.state = INS_SOLUTION; + + gAlgoStatus.bit.attitudeOnlyAlgorithm = FALSE; // Initialize the algorithm with GNSS position and lever arm InitINSFilter(); diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/TimingVars.c b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/TimingVars.c index 2942dfb..f4de8d9 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/TimingVars.c +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/TimingVars.c @@ -162,3 +162,7 @@ void Initialize_Timing(void) timer.dacqFrequency = DACQ_200_HZ; // default } +void TimingVars_dacqFrequency(int freq) +{ + timer.dacqFrequency = freq; +} diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/UpdateFunctions.c b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/UpdateFunctions.c index c6a45e5..2b5da5f 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/UpdateFunctions.c +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/UpdateFunctions.c @@ -62,6 +62,8 @@ static real _UnwrapAttitudeError( real attitudeError ); static real _LimitValue( real value, real limit ); static BOOL _CheckForUpdateTrigger(uint8_t updateRate); +static void ApplyGpsDealyCorrForStateCov(); + /****************************************************************************** * @brief Initializa heading using GNSS heading. * If the GNSS heading is valid and the vehicle is drving forward, the GNSS @@ -149,8 +151,31 @@ void EKF_UpdateStage(void) */ if( gEKFInput.gpsUpdate ) { - // Sync the algorithm itow to the GPS value + /* Sync the algorithm itow to the GPS value. GPS time is the time of pps. + * It is delayed by (gAlgorithm.itow-gEKFInput.itow). If there is loss of + * PPS detection or GPS measuremetn, gEKFInput.itow equals gKalmanFilter.ppsITow. + */ + if (gAlgoStatus.bit.ppsAvailable) + { + /* If GPS itow is above algorithm itow, something is wrong. + * If algorithm itow is more than 1sec ahead of GPS itow, GPS delay is + * too large and measurement is not used. + */ + int32_t gnssDelay = gAlgorithm.itow - gEKFInput.itow; + if (gnssDelay < 0 || gnssDelay > 1000) + { + gAlgoStatus.bit.ppsAvailable = FALSE; + } + // sync with GPS time + gAlgorithm.itow = gEKFInput.itow + gAlgorithm.itow - gKalmanFilter.ppsITow; + } + else + { gAlgorithm.itow = gEKFInput.itow; + } + // update pps detection time + gKalmanFilter.ppsITow = gEKFInput.itow; + // Resync timer timer.tenHertzCntr = 0; timer.subFrameCntr = 0; @@ -159,7 +184,8 @@ void EKF_UpdateStage(void) if (gEKFInput.gpsFixType) { // GPS heading valid? - useGpsHeading = (gEKFInput.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING); + gAlgoStatus.bit.gpsHeadingValid = (gEKFInput.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING); + useGpsHeading = gAlgoStatus.bit.gpsHeadingValid; /* If GNSS outage is longer than a threshold (maxReliableDRTime), DR results get unreliable * So, when GNSS comes back, the EKF is reinitialized. Otherwise, the DR results are still @@ -186,11 +212,13 @@ void EKF_UpdateStage(void) // reset the "last good reading" time gAlgorithm.timeOfLastGoodGPSReading = gEKFInput.itow; } - //else + + // apply motion constraints if (gAlgorithm.velocityAlwaysAlongBodyX && gAlgorithm.headingIni>HEADING_UNINITIALIZED) { Update_PseudoMeasurement(); } + // At 1 Hz mark, update when GPS data is valid, else do an AHRS-update runInsUpdate = 1; } @@ -198,7 +226,12 @@ void EKF_UpdateStage(void) { Update_Att(); runInsUpdate = 0; // set up for next pass - useGpsHeading = 0; + // handle P when PPS is available + if (gAlgoStatus.bit.ppsAvailable) + { + ApplyGpsDealyCorrForStateCov(); + gAlgoStatus.bit.ppsAvailable = FALSE; + } } } } @@ -207,9 +240,18 @@ void EKF_UpdateStage(void) void ComputeSystemInnovation_Pos(void) { // Position error + if (gAlgoStatus.bit.ppsAvailable) + { + gKalmanFilter.nu[STATE_RX] = gKalmanFilter.rGPS_N[X_AXIS] - gKalmanFilter.ppsPosition_N[X_AXIS]; + gKalmanFilter.nu[STATE_RY] = gKalmanFilter.rGPS_N[Y_AXIS] - gKalmanFilter.ppsPosition_N[Y_AXIS]; + gKalmanFilter.nu[STATE_RZ] = gKalmanFilter.rGPS_N[Z_AXIS] - gKalmanFilter.ppsPosition_N[Z_AXIS]; + } + else + { gKalmanFilter.nu[STATE_RX] = gKalmanFilter.rGPS_N[X_AXIS] - gKalmanFilter.Position_N[X_AXIS]; gKalmanFilter.nu[STATE_RY] = gKalmanFilter.rGPS_N[Y_AXIS] - gKalmanFilter.Position_N[Y_AXIS]; gKalmanFilter.nu[STATE_RZ] = gKalmanFilter.rGPS_N[Z_AXIS] - gKalmanFilter.Position_N[Z_AXIS]; + } gKalmanFilter.nu[STATE_RX] = _LimitValue(gKalmanFilter.nu[STATE_RX], gAlgorithm.Limit.Innov.positionError); gKalmanFilter.nu[STATE_RY] = _LimitValue(gKalmanFilter.nu[STATE_RY], gAlgorithm.Limit.Innov.positionError); @@ -221,9 +263,18 @@ void ComputeSystemInnovation_Pos(void) void ComputeSystemInnovation_Vel(void) { // Velocity error + if (gAlgoStatus.bit.ppsAvailable) + { + gKalmanFilter.nu[STATE_VX] = (real)gEKFInput.vNed[X_AXIS] - gKalmanFilter.ppsVelocity_N[X_AXIS]; + gKalmanFilter.nu[STATE_VY] = (real)gEKFInput.vNed[Y_AXIS] - gKalmanFilter.ppsVelocity_N[Y_AXIS]; + gKalmanFilter.nu[STATE_VZ] = (real)gEKFInput.vNed[Z_AXIS] - gKalmanFilter.ppsVelocity_N[Z_AXIS]; + } + else + { gKalmanFilter.nu[STATE_VX] = (real)gEKFInput.vNed[X_AXIS] - gKalmanFilter.Velocity_N[X_AXIS]; gKalmanFilter.nu[STATE_VY] = (real)gEKFInput.vNed[Y_AXIS] - gKalmanFilter.Velocity_N[Y_AXIS]; gKalmanFilter.nu[STATE_VZ] = (real)gEKFInput.vNed[Z_AXIS] - gKalmanFilter.Velocity_N[Z_AXIS]; + } gKalmanFilter.nu[STATE_VX] = _LimitValue(gKalmanFilter.nu[STATE_VX], gAlgorithm.Limit.Innov.velocityError); gKalmanFilter.nu[STATE_VY] = _LimitValue(gKalmanFilter.nu[STATE_VY], gAlgorithm.Limit.Innov.velocityError); @@ -254,9 +305,17 @@ void ComputeSystemInnovation_Att(void) { if (gAlgorithm.headingIni >= HEADING_GNSS_LOW) // heading already initialized with GNSS heading { + if (gAlgoStatus.bit.ppsAvailable) + { + gKalmanFilter.nu[STATE_YAW] = gEKFInput.trueCourse * (real)DEG_TO_RAD - + gKalmanFilter.ppsEulerAngles[YAW]; + } + else + { gKalmanFilter.nu[STATE_YAW] = gEKFInput.trueCourse * (real)DEG_TO_RAD - gKalmanFilter.eulerAngles[YAW]; } + } else { gKalmanFilter.nu[STATE_YAW] = 0.0; @@ -518,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; @@ -657,7 +716,6 @@ uint8_t rowNum, colNum, multIndex; real S_3x3[3][3], SInverse_3x3[3][3]; real PxHTranspose[ROWS_IN_P][ROWS_IN_H]; real KxH[NUMBER_OF_EKF_STATES][COLS_IN_H] = {{ 0.0 }}; -real deltaP_tmp[ROWS_IN_P][COLS_IN_P]; void Update_Att(void) { @@ -675,33 +733,59 @@ void Update_Att(void) _GenerateObservationJacobian_AHRS(); // gKF.H: 3x16 _GenerateObservationCovariance_AHRS(); // gKF.R: 3x3 - // In INS mode, do not do pitch and roll update while heading update is kept + // In INS mode, do not do pitch and roll update while heading update is kept. if (gAlgorithm.state == INS_SOLUTION) { + // reset this state. + gAlgoStatus.bit.stationaryYawLock = FALSE; + + /* Heading measurement is invalid, check if static yaw lock should take effect. + * Even if IMU static detection fails and the vehicle runs at a certain speed, + * static yaw lock should not take effect. This is guaranteed by R[STATE_YAW], which + * is set to 1.0 when vehicle speed is below a certian threshold. + * The risk is that the mechnism fails when the vehicle is below the threshold but not + * static. + */ + if (gKalmanFilter.R[STATE_YAW] > 0.9) + { if (!gImuStats.bStaticIMU) { - // If neither mag or GPS headig is available, update measuremnt matrix H to 2x16 - if (gKalmanFilter.R[STATE_YAW] > 0.9) - { + /* Heading measurement is invaid and IMU is not static, yaw lock should not + * take effect. That is, there is no heading update + */ for (colNum = 0; colNum < COLS_IN_H; colNum++) { gKalmanFilter.H[2][colNum] = 0.0; } - } lastYaw = 7.0; } - else + else if (gAlgorithm.Behavior.bit.enableStationaryLockYaw) { + /* IMU is static and static yaw lock is enabled. The first time when the algo runs here, + * the staic yaw is acquired. And yaw will be locked to this value. + */ if (lastYaw > TWO_PI) { lastYaw = gKalmanFilter.eulerAngles[YAW]; } else { - gKalmanFilter.nu[STATE_YAW] = lastYaw - gKalmanFilter.eulerAngles[YAW]; - gKalmanFilter.R[STATE_YAW] = 1e-4; + real diff = lastYaw - gKalmanFilter.eulerAngles[YAW]; + // if angle change exceeds max bias, it is not static + if (fabs(diff) > gAlgorithm.imuSpec.maxBiasW) + { + lastYaw = 7.0; + } + else + { + gKalmanFilter.nu[STATE_YAW] = diff; + gKalmanFilter.R[STATE_YAW] = 1e-8; + gAlgoStatus.bit.stationaryYawLock = TRUE; } } + } + } + // Do not perform roll and pitch update for (colNum = 0; colNum < COLS_IN_H; colNum++) { gKalmanFilter.H[0][colNum] = 0.0; @@ -813,7 +897,7 @@ void Update_Att(void) } // deltaP = KxH * gKF.P; - memset(deltaP_tmp, 0, sizeof(deltaP_tmp)); + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); /* deltaP is symmetric so only need to multiply one half and reflect the values * across the diagonal */ @@ -824,10 +908,10 @@ void Update_Att(void) { for (multIndex = RLE_KxH[rowNum][0]; multIndex <= RLE_KxH[rowNum][1]; multIndex++) { - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + KxH[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; } - deltaP_tmp[colNum][rowNum] = deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; } } #else @@ -849,10 +933,10 @@ void Update_Att(void) { continue; } - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + KxH[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; } - deltaP_tmp[colNum][rowNum] = deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; } } #endif @@ -864,7 +948,7 @@ void Update_Att(void) for (colNum = rowNum; colNum < COLS_IN_P; colNum++) { gKalmanFilter.P[rowNum][colNum] = gKalmanFilter.P[rowNum][colNum] - - deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[rowNum][colNum]; gKalmanFilter.P[colNum][rowNum] = gKalmanFilter.P[rowNum][colNum]; } } @@ -936,7 +1020,7 @@ void Update_Pos(void) // Compute the intermediate state update, stateUpdate AxB(&gKalmanFilter.K[0][0], &gKalmanFilter.nu[STATE_RX], NUMBER_OF_EKF_STATES, 3, 1, &gKalmanFilter.stateUpdate[0]); - memset(deltaP_tmp, 0, sizeof(deltaP_tmp)); + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); // Update the intermediate covariance estimate for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) { @@ -959,14 +1043,15 @@ void Update_Pos(void) { continue; } - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + gKalmanFilter.K[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; } - deltaP_tmp[colNum][rowNum] = deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; } } - AMinusB(&gKalmanFilter.P[0][0], &deltaP_tmp[0][0], NUMBER_OF_EKF_STATES, NUMBER_OF_EKF_STATES, &gKalmanFilter.P[0][0]); + AMinusB(&gKalmanFilter.P[0][0], &gKalmanFilter.deltaP_tmp[0][0], + NUMBER_OF_EKF_STATES, NUMBER_OF_EKF_STATES, &gKalmanFilter.P[0][0]); // Update states gKalmanFilter.Position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_RX]; @@ -1060,7 +1145,7 @@ void Update_Vel(void) // Compute the intermediate state update AxB(&gKalmanFilter.K[0][0], &gKalmanFilter.nu[STATE_VX], NUMBER_OF_EKF_STATES, 3, 1, &gKalmanFilter.stateUpdate[0]); - memset(deltaP_tmp, 0, sizeof(deltaP_tmp)); + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); // Update the intermediate covariance estimate for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) { @@ -1079,15 +1164,15 @@ void Update_Vel(void) */ for (multIndex = STATE_VX; multIndex <= STATE_VZ; multIndex++) { - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + gKalmanFilter.K[rowNum][multIndex - STATE_VX] * gKalmanFilter.P[multIndex][colNum]; } - deltaP_tmp[colNum][rowNum] = deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; } } // P2 = P2 - KxHxP2 - AMinusB(&gKalmanFilter.P[0][0], &deltaP_tmp[0][0], NUMBER_OF_EKF_STATES, NUMBER_OF_EKF_STATES, &gKalmanFilter.P[0][0]); + AMinusB(&gKalmanFilter.P[0][0], &gKalmanFilter.deltaP_tmp[0][0], NUMBER_OF_EKF_STATES, NUMBER_OF_EKF_STATES, &gKalmanFilter.P[0][0]); // ++++++++++++++++++++++ END OF VELOCITY ++++++++++++++++++++++ // Update states @@ -1114,7 +1199,6 @@ void Update_Vel(void) gKalmanFilter.accelBias_B[X_AXIS] += gKalmanFilter.stateUpdate[STATE_ABX]; gKalmanFilter.accelBias_B[Y_AXIS] += gKalmanFilter.stateUpdate[STATE_ABY]; gKalmanFilter.accelBias_B[Z_AXIS] += gKalmanFilter.stateUpdate[STATE_ABZ]; - } static void Update_GPS(void) @@ -1130,6 +1214,11 @@ static void Update_GPS(void) /* Compute the system error: z = meas, h = pred = q, nu - z - h * Do this at the same time even if the update is spread across time-steps */ + if (gAlgoStatus.bit.ppsAvailable) + { + // use P saved when PPS is detected if PPS is available. + memcpy(&gKalmanFilter.P[0][0], &gKalmanFilter.ppsP[0][0], sizeof(gKalmanFilter.P)); + } ComputeSystemInnovation_Pos(); Update_Pos(); ComputeSystemInnovation_Vel(); @@ -1192,17 +1281,16 @@ static void Update_PseudoMeasurement(void) * Zero velocity detection result has a higher priority to determine the front velocity because * odometer is also used for zero velocity detection when odometer is available. */ - BOOL hasOdo = FALSE; BOOL frontVelMeaValid = FALSE; real frontVelMea = 0.0; /* Front velocity is first determined by odometer. If odometer is not available, zero velocity * detection results are used to determine if front velocity is zero. If neither odometer is * available nor zero velocity detected, front velocity measurement is not valid. */ - if (hasOdo) + if (odoUsedInAlgorithm()) { frontVelMeaValid = TRUE; - frontVelMea = 0.0; // replace with real odo output + frontVelMea = gEKFInput.odoVelocity; r[0] = 1.0e-4; // variance of front velocity measurement should be from odo spec } else if (gImuStats.bStaticIMU) @@ -1349,7 +1437,7 @@ static void Update_PseudoMeasurement(void) } // deltaP = KxH * gKF.P; - memset(deltaP_tmp, 0, sizeof(deltaP_tmp)); + memset(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); /* deltaP is symmetric so only need to multiply one half and reflect the values * across the diagonal */ @@ -1365,10 +1453,10 @@ static void Update_PseudoMeasurement(void) { continue; } - deltaP_tmp[rowNum][colNum] = gKalmanFilter.K[rowNum][0] * PxHTranspose[colNum][0] + + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.K[rowNum][0] * PxHTranspose[colNum][0] + gKalmanFilter.K[rowNum][1] * PxHTranspose[colNum][1] + gKalmanFilter.K[rowNum][2] * PxHTranspose[colNum][2]; - deltaP_tmp[colNum][rowNum] = deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; } } @@ -1380,7 +1468,7 @@ static void Update_PseudoMeasurement(void) for (colNum = rowNum; colNum < COLS_IN_P; colNum++) { gKalmanFilter.P[rowNum][colNum] = gKalmanFilter.P[rowNum][colNum] - - deltaP_tmp[rowNum][colNum]; + gKalmanFilter.deltaP_tmp[rowNum][colNum]; gKalmanFilter.P[colNum][rowNum] = gKalmanFilter.P[rowNum][colNum]; } } @@ -1869,3 +1957,37 @@ static void InitializeEkfHeading() #endif } + +static void ApplyGpsDealyCorrForStateCov() +{ + uint8_t i, j, k; + + // P = phi * P * phi' + dQ + // 1) use deltaP_tmp to hold phi * p + AxB(&gKalmanFilter.phi[0][0], &gKalmanFilter.P[0][0], NUMBER_OF_EKF_STATES, NUMBER_OF_EKF_STATES, + NUMBER_OF_EKF_STATES, &gKalmanFilter.deltaP_tmp[0][0]); + + // 2) phi * p * phi' = deltaP_tmp * phi' + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + for (j = i; j < NUMBER_OF_EKF_STATES; j++) + { + gKalmanFilter.P[i][j] = 0; + for (k = 0; k < NUMBER_OF_EKF_STATES; k++) + { + gKalmanFilter.P[i][j] += gKalmanFilter.deltaP_tmp[i][k] * gKalmanFilter.phi[j][k]; + } + gKalmanFilter.P[j][i] = gKalmanFilter.P[i][j]; + } + } + + // 3) add dQ + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + for (j = i; j < NUMBER_OF_EKF_STATES; j++) + { + gKalmanFilter.P[i][j] += gKalmanFilter.dQ[i][j]; + gKalmanFilter.P[j][i] = gKalmanFilter.P[i][j]; + } + } +} diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/algorithm.c b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/algorithm.c index cacba34..f77904a 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/algorithm.c +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Algorithm/src/algorithm.c @@ -15,6 +15,8 @@ ******************************************************************************/ #include +#include + #include "SensorNoiseParameters.h" #include "platformAPI.h" #include "algorithm.h" @@ -25,9 +27,20 @@ AlgorithmStruct gAlgorithm; AlgoStatus gAlgoStatus; -void InitializeAlgorithmStruct(uint8_t callingFreq) +void InitializeAlgorithmStruct(uint8_t callingFreq, const enumIMUType imuTypeIn) { - gAlgorithm.Behavior.bit.freeIntegrate = FALSE; + +#ifndef IMU_ONLY + enumIMUType imuType = imuTypeIn; + + if(imuType == CurrentIMU){ + // Reuse previously initialized IMU type + imuType = gAlgorithm.imuType; + } + + memset(&gAlgorithm, 0, sizeof(AlgorithmStruct)); + + //----------------------------algortihm config----------------------------- // The calling frequency drives the execution rate of the EKF and dictates // the algorithm constants if(callingFreq == 0){ @@ -94,7 +107,10 @@ void InitializeAlgorithmStruct(uint8_t callingFreq) // Linear acceleration switch limits (level and time) gAlgorithm.Limit.accelSwitch = (real)(0.012); // [g] - gAlgorithm.Limit.linAccelSwitchDelay = (uint32_t)(2.0 * gAlgorithm.callingFreq); + 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; @@ -109,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; @@ -126,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; @@ -134,15 +184,25 @@ 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); gAlgorithm.staticDetectParam.staticVarAccel = (real)(gAlgorithm.imuSpec.sigmaA * gAlgorithm.imuSpec.sigmaA); - gAlgorithm.staticDetectParam.maxGyroBias = MAX_BW; + gAlgorithm.staticDetectParam.maxGyroBias = gAlgorithm.imuSpec.maxBiasW; gAlgorithm.staticDetectParam.staticGnssVel = 0.2; gAlgorithm.staticDetectParam.staticNoiseMultiplier[0] = 4.0; gAlgorithm.staticDetectParam.staticNoiseMultiplier[1] = 4.0; gAlgorithm.staticDetectParam.staticNoiseMultiplier[2] = 1.0; + + gAlgorithm.Behavior.bit.dynamicMotion = 1; + + //----------------------------algorithm states----------------------------- + memset(&gAlgoStatus, 0, sizeof(gAlgoStatus)); +#endif + } void GetAlgoStatus(AlgoStatus *algoStatus) @@ -199,4 +259,90 @@ void setPointOfInterest( real poiBx, real poiBy, real poiBz ) gAlgorithm.pointOfInterestB[0] = poiBx; gAlgorithm.pointOfInterestB[1] = poiBy; gAlgorithm.pointOfInterestB[2] = poiBz; +} + +void UpdateImuSpec(real rwOdr, real arw, real biw, real maxBiasW, + real vrw, real bia, real maxBiasA) +{ + // Update IMU specifications + gAlgorithm.imuSpec.arw = arw; + gAlgorithm.imuSpec.sigmaW = (real)(1.25 * arw / sqrt(1.0 / rwOdr)); + gAlgorithm.imuSpec.biW = biw; + gAlgorithm.imuSpec.maxBiasW = maxBiasW; + gAlgorithm.imuSpec.vrw = vrw; + gAlgorithm.imuSpec.sigmaA = (real)(1.25 * vrw / sqrt(1.0 / rwOdr)); + gAlgorithm.imuSpec.biA = bia; + gAlgorithm.imuSpec.maxBiasA = maxBiasA; + + // Update affected params related to zero velocity detection + gAlgorithm.staticDetectParam.staticVarGyro = (real)(gAlgorithm.imuSpec.sigmaW * gAlgorithm.imuSpec.sigmaW); + gAlgorithm.staticDetectParam.staticVarAccel = (real)(gAlgorithm.imuSpec.sigmaA * gAlgorithm.imuSpec.sigmaA); + gAlgorithm.staticDetectParam.maxGyroBias = gAlgorithm.imuSpec.maxBiasW; +} + +void enableMagInAlgorithm(BOOL enable) +{ + if (1) + { + gAlgorithm.Behavior.bit.useMag = enable; + } + else + { + gAlgorithm.Behavior.bit.useMag = FALSE; + } +} + +void enableGpsInAlgorithm(BOOL enable) +{ + gAlgorithm.Behavior.bit.useGPS = enable; +} + +void enableOdoInAlgorithm(BOOL enable) +{ + gAlgorithm.Behavior.bit.useOdo = enable; +} + +BOOL magUsedInAlgorithm() +{ + return gAlgorithm.Behavior.bit.useMag != 0; +} + +BOOL gpsUsedInAlgorithm(void) +{ + return (BOOL)gAlgorithm.Behavior.bit.useGPS; +} + +BOOL odoUsedInAlgorithm(void) +{ + return (BOOL)gAlgorithm.Behavior.bit.useOdo; +} + +void enableFreeIntegration(BOOL enable) +{ + gAlgorithm.Behavior.bit.freeIntegrate = enable; +} + +BOOL freeIntegrationEnabled() +{ + return (BOOL)gAlgorithm.Behavior.bit.freeIntegrate; +} + +void enableStationaryLockYaw(BOOL enable) +{ + gAlgorithm.Behavior.bit.enableStationaryLockYaw = enable; +} + +BOOL stationaryLockYawEnabled() +{ + return (BOOL)gAlgorithm.Behavior.bit.enableStationaryLockYaw; +} + +void enableImuStaticDetect(BOOL enable) +{ + gAlgorithm.Behavior.bit.enableImuStaticDetect = enable; +} + +BOOL imuStaticDetectEnabled() +{ + return (BOOL)gAlgorithm.Behavior.bit.enableImuStaticDetect; } \ No newline at end of file diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Common/include/GlobalConstants.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Common/include/GlobalConstants.h index b7ec1e5..afdbdf0 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Common/include/GlobalConstants.h +++ b/examples/OpenIMU300ZI/VG_AHRS/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 diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Common/include/ucb_packet_struct.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Common/include/ucb_packet_struct.h index 066ee3f..506a31d 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Common/include/ucb_packet_struct.h +++ b/examples/OpenIMU300ZI/VG_AHRS/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 diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/gpsAPI.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/gpsAPI.h index 3a24ea4..04091d0 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/gpsAPI.h +++ b/examples/OpenIMU300ZI/VG_AHRS/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 */ diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/include/driverGPS.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/include/driverGPS.h index c7681a8..552c24b 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/include/driverGPS.h +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/include/driverGPS.h @@ -218,8 +218,8 @@ typedef struct { int lon_deg; int lon_min; double lon_min_fraction; - char latSign; - char lonSign; + int8_t latSign; + int8_t lonSign; } NmeaLatLonSTRUCT; // delta struct diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/driverGPS.c b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/driverGPS.c index 89416a6..daac33e 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/driverGPS.c +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/driverGPS.c @@ -37,8 +37,9 @@ limitations under the License. #include "debug.h" #endif -int gpsSerialChan = UART_CHANNEL_NONE; // undefined, needs to be explicitly defined - +int gpsChan = UART_CHANNEL_NONE; // undefined, needs to be explicitly defined +BOOL _handleGpsMessages(GpsData_t *GPSData); +BOOL gpsUartInitialized = FALSE; /// GPS data struct // to change to NMEA then pass GPS out debug port: un-comment this and @@ -60,6 +61,11 @@ int16_t _getIndexLineFeed(uint16_t numInBuff, unsigned *msgLength); void _setGPSMessageSignature(GpsData_t* GPSData); void _userCmdBaudProtcol(GpsData_t* GPSData); +/******************************************* + * @brief + * + * @param GPSData == +********************************************/ void loadGpsCommSettings(GpsData_t* GPSData) { GPSData->GPSAUTOSetting = 0; @@ -67,6 +73,7 @@ void loadGpsCommSettings(GpsData_t* GPSData) GPSData->GPSConfigureOK = 0; } + // extern_port.c extern void GetGpsExternUartChannel(unsigned int* uartChannel); @@ -77,8 +84,8 @@ extern void GetGpsExternUartChannel(unsigned int* uartChannel); ******************************************************************************/ int initGpsUart(int baudrate) { - gpsSerialChan = platformGetSerialChannel(GPS_SERIAL_PORT); - return uart_init(gpsSerialChan, baudrate); + gpsChan = platformGetSerialChannel(GPS_SERIAL_PORT, FALSE); + return uart_init(gpsChan, baudrate); } @@ -90,7 +97,7 @@ int initGpsUart(int baudrate) ******************************************************************************/ int gpsBytesAvailable() { - return uart_rxBytesAvailable(gpsSerialChan); + return uart_rxBytesAvailable(gpsChan); } /** **************************************************************************** @@ -100,7 +107,7 @@ int gpsBytesAvailable() ******************************************************************************/ void flushGPSRecBuf(void) { - uart_flushRecBuffer(gpsSerialChan); + uart_flushRecBuffer(gpsChan); } /** **************************************************************************** @@ -110,7 +117,7 @@ void flushGPSRecBuf(void) ******************************************************************************/ BOOL isGpsTxEmpty(void) { - return uart_txBytesRemains(gpsSerialChan) == 0; + return uart_txBytesRemains(gpsChan) == 0; } /** **************************************************************************** @@ -122,11 +129,11 @@ uint16_t delBytesGpsBuf(uint16_t numToPop) { int16_t numInBuffer; - numInBuffer = uart_rxBytesAvailable(gpsSerialChan); + numInBuffer = uart_rxBytesAvailable(gpsChan); if (numInBuffer < numToPop) { numToPop = numInBuffer; } - uart_removeRxBytes(gpsSerialChan, numToPop); + uart_removeRxBytes(gpsChan, numToPop); // COM_buf_delete(&(gGpsUartPtr->rec_buf), // numToPop); return ( numInBuffer - numToPop ); ///< unscanned bytes @@ -158,7 +165,7 @@ uint8_t peekByteGpsBuf(uint16_t index) { uint8_t output = 0; - uart_copyBytes(gpsSerialChan, index, 1, &output); + uart_copyBytes(gpsChan, index, 1, &output); return output; } @@ -181,7 +188,7 @@ unsigned long peekGPSmsgHeader(uint16_t index, unsigned long GPSHeader; headerLength = GPSData->GPSMsgSignature.GPSheaderLength; - uart_copyBytes(gpsSerialChan, index, headerLength, header); + uart_copyBytes(gpsChan, index, headerLength, header); GPSHeader = 0; for (i = 0, j = headerLength - 1; i < headerLength; i++, j--) { @@ -203,8 +210,8 @@ unsigned long peekGPSmsgHeader(uint16_t index, ******************************************************************************/ int16_t retrieveGpsMsg(uint16_t numBytes, GpsData_t *GPSData, uint8_t *outBuffer) { - uart_read(gpsSerialChan, outBuffer, numBytes); - return uart_rxBytesAvailable(gpsSerialChan); + uart_read(gpsChan, outBuffer, numBytes); + return uart_rxBytesAvailable(gpsChan); } /** **************************************************************************** @@ -226,10 +233,10 @@ int16_t findHeader(uint16_t numInBuff, int num; do { - uart_copyBytes(gpsSerialChan,0,1,&byte); + uart_copyBytes(gpsChan,0,1,&byte); if (byte == GPSData->GPSMsgSignature.startByte) { - uart_copyBytes(gpsSerialChan,1,GPSData->GPSMsgSignature.GPSheaderLength - 1, buf); + uart_copyBytes(gpsChan,1,GPSData->GPSMsgSignature.GPSheaderLength - 1, buf); header = byte << (GPSData->GPSMsgSignature.GPSheaderLength - 1) * 8; switch (GPSData->GPSMsgSignature.GPSheaderLength) { case 2: // SiRF 0xa0a2 @@ -242,13 +249,13 @@ int16_t findHeader(uint16_t numInBuff, if ( header == GPSData->GPSMsgSignature.GPSheader ) { exit = 1; } else { - num = uart_removeRxBytes(gpsSerialChan, 1); + num = uart_removeRxBytes(gpsChan, 1); if(num){ numInBuff--; } } } else { - num = uart_removeRxBytes(gpsSerialChan, 1); + num = uart_removeRxBytes(gpsChan, 1); if(num){ numInBuff--; } @@ -267,7 +274,7 @@ int16_t findHeader(uint16_t numInBuff, ******************************************************************************/ int writeGps(char *data, uint16_t len) { - return uart_write(gpsSerialChan, (uint8_t*)data, len); + return uart_write(gpsChan, (uint8_t*)data, len); } @@ -400,10 +407,19 @@ void initGPSHandler(void) gGpsDataPtr->GPSTopLevelConfig |= (1 << HZ2); // update to change internal (ublox) GPS to 2Hz /// Configure GPS structure, from Flash (EEPROM) loadGpsCommSettings(gGpsDataPtr); + if(!gpsUartInitialized){ + gpsUartInitialized = TRUE; initGpsUart(gGpsDataPtr->GPSbaudRate); + } #endif } +void InitGpsSerialCommunication(int baudrate, BOOL fInit) +{ + gGpsDataPtr->GPSbaudRate = baudrate; + gpsChan = platformGetSerialChannel(GPS_SERIAL_PORT, FALSE); + gpsUartInitialized = fInit; +} /** **************************************************************************** @@ -422,6 +438,7 @@ void initGPSDataStruct(void) } +#ifndef NMEA_ONLY BOOL _handleGpsMessages(GpsData_t *GPSData) { static uint8_t gpsUartBuf[100]; @@ -433,7 +450,7 @@ BOOL _handleGpsMessages(GpsData_t *GPSData) while(1){ if(!bytesInBuffer){ - bytesInBuffer = uart_read(gpsSerialChan, gpsUartBuf, sizeof (gpsUartBuf)); + bytesInBuffer = uart_read(gpsChan, gpsUartBuf, sizeof (gpsUartBuf)); if(!bytesInBuffer){ return 0; // nothing to do } @@ -457,6 +474,42 @@ BOOL _handleGpsMessages(GpsData_t *GPSData) } } /* end _handleGpsMessages */ +#endif + + +BOOL _handleGpsNMEAMessages(GpsData_t *GPSData) +{ + static uint8_t gpsUartBuf[100]; + static uint8_t gpsMsg[MAX_MSG_LENGTH]; + static int bytesInBuffer = 0; + unsigned char tmp; + unsigned static int pos = 0; + + // refresh GPS channel + gpsChan = platformGetSerialChannel(GPS_SERIAL_PORT, FALSE); + while(1){ + if(!bytesInBuffer){ + bytesInBuffer = uart_read(gpsChan, gpsUartBuf, sizeof (gpsUartBuf)); + if(!bytesInBuffer){ + return 0; // nothing to do + } + pos = 0; +#ifdef GNSS_TRAFFIC_RELAY + if(platformIsGnssTrafficRelayEnabled()){ + int masterChan = platformGetSerialChannel(MASTER_SERIAL_PORT, TRUE); + uart_write(masterChan, gpsUartBuf, bytesInBuffer); + return 0; + } +#endif + } + tmp = gpsUartBuf[pos++]; + bytesInBuffer--; + platformDetectPingMessageFromGpsDriver(tmp); + parseNMEAMessage(tmp, gpsMsg, GPSData); + } +} +/* end _handleGpsMessages */ + /** **************************************************************************** * @name GPSHandler GPS stream data and return GPS data for NAV algorithms @@ -469,13 +522,16 @@ void GPSHandler(void) { gGpsDataPtr->Timer100Hz10ms = getSystemTime(); ///< get system clock ticks gGpsDataPtr->isGPSBaudrateKnown = 1; - +#ifndef NMEA_ONLY // parse moved to top since is always called after init if (gGpsDataPtr->GPSConfigureOK > 0 ) { // Configuration is completed _handleGpsMessages(gGpsDataPtr); } else { ///< configure GPS receiver if needed: OK < 0 _configGPSReceiver(gGpsDataPtr); } +#else + _handleGpsNMEAMessages(gGpsDataPtr); +#endif } /** **************************************************************************** diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/processNMEAGPS.c b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/processNMEAGPS.c index f51ada5..6f08c1d 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/processNMEAGPS.c +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/processNMEAGPS.c @@ -40,6 +40,7 @@ limitations under the License. char _parseGPGGA(char *msgBody, GpsData_t* GPSData); char _parseVTG(char *msgBody, GpsData_t* GPSData); char _parseRMC(char *msgBody, GpsData_t* GPSData); +char _parseZDA(char *msgBody, GpsData_t* GPSData); void _handleNMEAmsg(char *msgID, char *msgBody,GpsData_t* GPSData); void _NMEA2UbloxAndLLA(NmeaLatLonSTRUCT* NmeaData, GpsData_t* GPSData); void _smoothVerticalData(GpsData_t* GPSData); @@ -185,6 +186,10 @@ void _handleNMEAmsg(char *msgID, GPSData->totalVTG++; _parseVTG(msgBody, GPSData); } + if( strncmp(ptr, "ZDA", 3) == 0 ) { + GPSData->totalVTG++; + _parseZDA(msgBody, GPSData); + } if( strncmp(ptr, "RMC", 3) == 0 ) { _parseRMC(msgBody, GPSData); } @@ -312,7 +317,7 @@ char _parseGPGGA(char *msgBody, if (field[6] == '.') { // Some messages have second fraction .SS skip the '.' - GPSData->GPSSecondFraction = ( (field[7] - '0') / 10) + (field[8] - '0') / 100; + GPSData->GPSSecondFraction = ( (field[7] - '0') / 10.0) + (field[8] - '0') / 100.0; } } else status = 1; @@ -501,6 +506,54 @@ char _parseRMC(char *msgBody, return status; } +/** **************************************************************************** + * @name: _parseZDA LOCAL parse GPZDA message. duplicates nav data but has + * calander data needed for WMM (Worl Magnetic Model) lookup + * @author Dong An + * @param [in] msgBody - data to parse + * @param [in] GPSData - data structure to parst he data into + * @retval status + * @details $GPZDA (Recommended Minimum sentence C) + * 172809 Fix taken at 17:28:09 UTC + * 12 Day + * 07 Month + * 1996 Year + * 00 Offset from GMT Hours + * 00 Offset from GMT Minutes + * *45 The checksum data, always begins with '*' + * 07/01/15 dkh convert directly from ascii to decimal + * $GPZDA,172809.456,12,07,1996,00,00*45 + ******************************************************************************/ +char _parseZDA(char *msgBody, + GpsData_t *GPSData) +{ + char field[NMEA_MSG_MAX_FIELD]; + char status = 0; + int parseReset = 1; + + /// Date field 8 + + if( extractNMEAfield(msgBody, field, 1, parseReset) ) { + GPSData->GPSday = ( (field[0] - '0') * 10) + field[1] - '0'; /// month + } else + status = 1; + + if( extractNMEAfield(msgBody, field, 2, parseReset) ) { + GPSData->GPSmonth = ((field[0] - '0') * 10) + field[1] - '0'; /// day + } else + status = 1; + + if( extractNMEAfield(msgBody, field, 3, parseReset) ) { + GPSData->GPSyear = ( (field[2] - '0') * 10) + field[3] - '0'; /// year + } else + status = 1; + + + return status; + +} + + /** **************************************************************************** * @name: computeNMEAChecksum compute the checksum of a NMEA message. * @author Dong An diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/taskGps.c b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/taskGps.c index 56a62a1..43412e0 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/taskGps.c +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/GPS/src/taskGps.c @@ -51,7 +51,7 @@ void TaskGps(void const *argument) int bytesAvailable; static uint32_t updateHDOP, pollSirfCnt; - while(gpsSerialChan == UART_CHANNEL_NONE) { + while(gpsChan == UART_CHANNEL_NONE) { // nothing to do untill port decided OS_Delay( 1000); } @@ -93,4 +93,29 @@ void TaskGps(void const *argument) } } + +/** **************************************************************************** + * @name ProcGps + * @brief task callback with the main loop for handle GPS data, make sure the + * GPS handling function gets called on a regular basis;. + * gCalibration.productConfiguration.bit.hasGps = 1; by setting: + * true and true in name_IMU380.xml file + * @param N/A + * @retval N/A + ******************************************************************************/ +void ProcGps() +{ + static BOOL firstTime = TRUE; + + if(firstTime){ + // start out with the DOP high + firstTime = 0; + gGpsDataPtr->HDOP = 50.0; + initGPSHandler(); + } + + GPSHandler(); + +} + #endif // GPS \ No newline at end of file diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Odo/odoAPI.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Odo/odoAPI.h new file mode 100644 index 0000000..458560b --- /dev/null +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/Odo/odoAPI.h @@ -0,0 +1,40 @@ +/** *************************************************************************** + * @file odoApi.h Odometer interface. + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * @brief This is a generalized odometer interface. + *****************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef ODO_API_H +#define ODO_API_H + +#include +#include "GlobalConstants.h" + +typedef struct { + uint8_t update; // 1 if contains new data + real v; // velocity measured by odometer, [m/s] +} odoDataStruct_t; + +extern odoDataStruct_t gOdo; + +#endif /* ODO_API_H */ diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/SteerAlgo/steerAPI.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/SteerAlgo/steerAPI.h new file mode 100644 index 0000000..c10a0a4 --- /dev/null +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/SteerAlgo/steerAPI.h @@ -0,0 +1,43 @@ +/** *************************************************************************** + * @file odoApi.h Odometer interface. + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * @brief This is a generalized odometer interface. + *****************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef STEER_API_H +#define STEER_API_H + +#include +#include "GlobalConstants.h" + +typedef struct { + uint8_t update; // 1 if contains new data + real angle; // streering angle floating point + int16_t intAngle; // streering angle as message - weighted + int16_t sRate; // stering angle rate + uint16_t steerAlgoStates; +} steerDataStruct_t; + +extern steerDataStruct_t gSteerAngle; + +#endif /* ODO_API_H */ diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/UARTComm/CommonMessages.c b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/UARTComm/CommonMessages.c index 09cbf0b..6fdfa44 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/UARTComm/CommonMessages.c +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/UARTComm/CommonMessages.c @@ -31,6 +31,9 @@ limitations under the License. #include "platformAPI.h" #include "sensorsAPI.h" #include "appVersion.h" +#include "ucb_packet_struct.h" +#include "magAPI.h" +#include "magAlign.h" #include "CommonMessages.h" #include "algorithm.h" @@ -360,3 +363,144 @@ BOOL Fill_e2PacketPayload(uint8_t *payload, uint8_t *payloadLen) return TRUE; } +BOOL Fill_MagAlignResponsePayload(int8_t state, UcbPacketStruct *ptrUcbPacket) +{ + int8_t estimatedMagAlignVals[8] = {0}; + int8_t magAlignVals[8] = {0}; + BOOL valid = TRUE; + + switch (state - 1) + { + // + uint8_t len; + + case 0: // Return the Mag-Align status + //uint8_t *model = (uint8_t*)unitVersionString(); + //uint8_t *rev = (uint8_t*)platformBuildInfo(); + //unsigned int serialNum = unitSerialNumber(); + //len = snprintf((char*)ptrUcbPacket->payload, 250, "%s %s SN:%u", model, rev, serialNum ); + //ptrUcbPacket->payloadLength = len; + if (gMagAlign.state == MAG_ALIGN_STATUS_START_CAL_WITH_AUTOEND) + { + // Start (auto end) + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)0x1); + } + else if (gMagAlign.state == MAG_ALIGN_STATUS_START_CAL_WITHOUT_AUTOEND) + { + // Start (manual end) + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)0x2); + } + else + { + // Start (manual end) + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)0x0); + } + + ptrUcbPacket->payloadLength = len; + break; + + case 1: // Start mag-align w/ autoend + case 2: // Start mag-align w/ autoend + case 3: // Stop mag-align w/ autoend + case 4: // Accept results + case 5: // Accept results and write to EEPROM + case 6: // Abort Mag-Align or reject results + case 8: // Restore default mag-align values + case 9: // Restore default mag-align values and save in EEPROM + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c", (char)state - 1); + ptrUcbPacket->payloadLength = len; + break; + + case 7: // Return stored mag-align values +#if 0 + // Test values: + gMagAlign.estParams.hardIronBias[X_AXIS] = 0.1; + gMagAlign.estParams.hardIronBias[Y_AXIS] = -0.2; + gMagAlign.estParams.softIronScaleRatio = 0.98; + gMagAlign.estParams.softIronAngle = -270.0 * DEG_TO_RAD; +#endif + + // Bias can be +/- 8.0 [g] (full scale of sensor) + // SF = 2^15 / maxVal = 2^15 / 8.0 = 4096 + magAlignVals[0] = (char)(((int16_t)(gMagAlign.hardIronBias[X_AXIS] * (float)4096.0) >> 8) & 0xFF); + magAlignVals[1] = (char)(((int16_t)(gMagAlign.hardIronBias[X_AXIS] * (float)4096.0) >> 0) & 0xFF); + magAlignVals[2] = (char)(((int16_t)(gMagAlign.hardIronBias[Y_AXIS] * (float)4096.0) >> 8) & 0xFF); + magAlignVals[3] = (char)(((int16_t)(gMagAlign.hardIronBias[Y_AXIS] * (float)4096.0) >> 0) & 0xFF); + + // Ratio can be 0 --> 1 + // SF = (2^16-1) / maxVal = (2^16-1) / 1.0 = 65535 + magAlignVals[4] = (char)(((int16_t)(gMagAlign.softIronScaleRatio * (float)65535.0) >> 8) & 0xFF); + magAlignVals[5] = (char)(((int16_t)(gMagAlign.softIronScaleRatio * (float)65535.0) >> 0) & 0xFF); + + // SF = 2^15 / maxVal = 2^15 / pi = 10430.37835047045 + magAlignVals[6] = (char)(((int16_t)(gMagAlign.softIronAngle * (float)10430.37835047046) >> 8) & 0xFF); + magAlignVals[7] = (char)(((int16_t)(gMagAlign.softIronAngle * (float)10430.37835047046) >> 0) & 0xFF); + + // Bias can be +/- 8.0 [g] (full scale of sensor) + // SF = 2^15 / maxVal = 2^15 / 8.0 = 4096 + estimatedMagAlignVals[0] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[X_AXIS] * (float)4096.0) >> 8) & 0xFF); + estimatedMagAlignVals[1] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[X_AXIS] * (float)4096.0) >> 0) & 0xFF); + estimatedMagAlignVals[2] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[Y_AXIS] * (float)4096.0) >> 8) & 0xFF); + estimatedMagAlignVals[3] = (char)(((int16_t)(gMagAlign.estParams.hardIronBias[Y_AXIS] * (float)4096.0) >> 0) & 0xFF); + + // Ratio can be 0 --> 1 + // SF = (2^16-1) / maxVal = (2^16-1) / 1.0 = 65535 + estimatedMagAlignVals[4] = (char)(((int16_t)(gMagAlign.estParams.softIronScaleRatio * (float)65535.0) >> 8) & 0xFF); + estimatedMagAlignVals[5] = (char)(((int16_t)(gMagAlign.estParams.softIronScaleRatio * (float)65535.0) >> 0) & 0xFF); + + // Angle can be +/- pi (in radians) + // Correct for angles that exceed +/-180 + if (gMagAlign.estParams.softIronAngle > PI) + { + gMagAlign.estParams.softIronAngle = (float)PI - gMagAlign.estParams.softIronAngle; + } + else if (gMagAlign.estParams.softIronAngle < -PI) + { + gMagAlign.estParams.softIronAngle = TWO_PI + gMagAlign.estParams.softIronAngle; + } + + // SF = 2^15 / maxVal = 2^15 / pi = 10430.37835047045 + estimatedMagAlignVals[6] = (char)(((int16_t)(gMagAlign.estParams.softIronAngle * (float)10430.37835047046) >> 8) & 0xFF); + estimatedMagAlignVals[7] = (char)(((int16_t)(gMagAlign.estParams.softIronAngle * (float)10430.37835047046) >> 0) & 0xFF); + +#if 0 + DebugPrintFloat(" ", (float)estimatedMagAlignVals[0], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[1], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[2], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[3], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[4], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[5], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[6], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[7], 1); + DebugPrintEndline(); +#endif + + len = snprintf((char *)ptrUcbPacket->payload, 250, "%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", (char)magAlignVals[0], + (char)magAlignVals[1], + (char)magAlignVals[2], + (char)magAlignVals[3], + (char)magAlignVals[4], + (char)magAlignVals[5], + (char)magAlignVals[6], + (char)magAlignVals[7], + (char)estimatedMagAlignVals[0], + (char)estimatedMagAlignVals[1], + (char)estimatedMagAlignVals[2], + (char)estimatedMagAlignVals[3], + (char)estimatedMagAlignVals[4], + (char)estimatedMagAlignVals[5], + (char)estimatedMagAlignVals[6], + (char)estimatedMagAlignVals[7]); + + ptrUcbPacket->payloadLength = len; + break; + + case 10: // Load user computed mag-align values + break; + + default: + valid = FALSE; + break; + } + return valid; +} diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/UARTComm/CommonMessages.h b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/UARTComm/CommonMessages.h index 0f373f0..b425784 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/UARTComm/CommonMessages.h +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/UARTComm/CommonMessages.h @@ -122,5 +122,6 @@ BOOL Fill_a2PacketPayload(uint8_t *payload, uint8_t *payloadLen); BOOL Fill_s1PacketPayload(uint8_t *payload, uint8_t *payloadLen); BOOL Fill_e1PacketPayload(uint8_t *payload, uint8_t *payloadLen); BOOL Fill_e2PacketPayload(uint8_t *payload, uint8_t *payloadLen); +BOOL Fill_MagAlignResponsePayload(int8_t state, UcbPacketStruct *ptrUcbPacket); #endif diff --git a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/library.json b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/library.json index 497943a..0227568 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/lib/Core/library.json +++ b/examples/OpenIMU300ZI/VG_AHRS/lib/Core/library.json @@ -12,6 +12,10 @@ "-I Common/include", "-I GPS", "-I GPS/include", + "-I Odo", + "-I Odo/include", + "-I SteerAlgo", + "-I SteerAlgo/include", "-I Math", "-I Math/include", "-I SPIComm", diff --git a/examples/OpenIMU300ZI/VG_AHRS/openimu.json b/examples/OpenIMU300ZI/VG_AHRS/openimu.json index 395d2c8..e035391 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/openimu.json +++ b/examples/OpenIMU300ZI/VG_AHRS/openimu.json @@ -1,5 +1,5 @@ { - "appVersion": "OpenIMU300ZI VG_AHRS 1.1.3", + "appVersion": "OpenIMU300ZI VG_AHRS 04.01.04", "type": "openimu", "description": "9-axis OpenIMU with triaxial rate, acceleration, and magnetic measurement", "userConfiguration": [ diff --git a/examples/OpenIMU300ZI/VG_AHRS/platformio.ini b/examples/OpenIMU300ZI/VG_AHRS/platformio.ini index 520d579..e0b229c 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/platformio.ini +++ b/examples/OpenIMU300ZI/VG_AHRS/platformio.ini @@ -14,8 +14,8 @@ description = A Kalman filter based algorithm that uses rate-sensors to propagat platform = aceinna_imu lib_archive = false board = OpenIMU300 -;lib_deps= ../../openIMU300-lib -lib_deps = OpenIMU300-base-library@1.0.9 +;lib_deps = ../../../openIMU300-lib +lib_deps = OpenIMU300-base-library@1.1.13 build_flags = -g3 ; -D CLI @@ -23,6 +23,7 @@ build_flags = -D SPI_BUS_COMM -D __FPU_PRESENT -D ARM_MATH_CM4 + -I include -I src/user -I src -Og diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/appVersion.h b/examples/OpenIMU300ZI/VG_AHRS/src/appVersion.h index 98d7c31..bbfde12 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/src/appVersion.h +++ b/examples/OpenIMU300ZI/VG_AHRS/src/appVersion.h @@ -26,7 +26,8 @@ limitations under the License. #ifndef _VG_APP_VERSION_H #define _VG_APP_VERSION_H -#define APP_VERSION_STRING "OpenIMU300ZI VG_AHRS 1.1.3" +#define APP_VERSION_STRING "OpenIMU300ZI VG_AHRS 04.01.04" +#define APP_SPI_SW_VERSION 115 #endif diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/main.c b/examples/OpenIMU300ZI/VG_AHRS/src/main.c index cb291c4..3575ddf 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/src/main.c +++ b/examples/OpenIMU300ZI/VG_AHRS/src/main.c @@ -87,7 +87,7 @@ void DebugInterfaceInit(void) return; // no resources } - int debugChannel = platformGetSerialChannel(DEBUG_SERIAL_PORT); + int debugChannel = platformGetSerialChannel(DEBUG_SERIAL_PORT, TRUE); if(debugChannel == UART_CHANNEL_NONE){ //nothing to do @@ -99,7 +99,6 @@ void DebugInterfaceInit(void) BoardGetResetStatus(status, sizeof(status)); -// ERROR_STRING(status); } void CreateTasks(void) @@ -212,7 +211,7 @@ int main(void) // Initialize OS and create required tasks CreateTasks(); - // Initialize the DEBUG serial port + // Initialize the DEBUG USART (serial) port DebugInterfaceInit(); //InitTimer_Watchdog( ENABLE ); diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/taskDataAcquisition.c b/examples/OpenIMU300ZI/VG_AHRS/src/taskDataAcquisition.c index 060d481..d81c4a8 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/src/taskDataAcquisition.c +++ b/examples/OpenIMU300ZI/VG_AHRS/src/taskDataAcquisition.c @@ -33,6 +33,7 @@ limitations under the License. #include "userAPI.h" #include "commAPI.h" #include "spiAPI.h" +#include "UserMessagingSPI.h" #include "taskDataAcquisition.h" @@ -52,6 +53,7 @@ void TaskDataAcquisition(void const *argument) // int res; uint16_t dacqRate; + int spiRateRef = 0, spiRateDiv = 0; #pragma GCC diagnostic ignored "-Wunused-but-set-variable" BOOL overRange = FALSE; //uncomment this line if overrange processing required @@ -87,9 +89,9 @@ void TaskDataAcquisition(void const *argument) // ***************************************************************** // Handle Timing vard, watchdog and BIT PrepareToNewDacqTick(); - // Wait for next tick - // Upon timeout of TIM5 (or user sync), let the process continue + // Wait for next tick + // Upon timeout or user sync let the process continue res = osSemaphoreWait(dataAcqSem, 1000); if(res != osOK){ // Wait timeout expired. Something wrong wit the dacq system @@ -101,17 +103,14 @@ void TaskDataAcquisition(void const *argument) // in case of UART communication interface sets pin IO2 high if(platformGetUnitCommunicationType() != UART_COMM){ setDataReadyPin(1); - }else{ - setIO2Pin (1); } + setIO2Pin (1); + // Get calibrated sensor data: // Inside this function the sensor data is filtered by a second-order low-pass // Butterworth filter, with a cutoff frequency selected by the user (or zero to - // disable). The cutoff is selected using the following: - // - // Select_LP_filter(rawSensor_e sensorType, eFilterType filterType) - // + // disable). // Refer to UserConfiguration.c for implementation and to the enumerator structure // 'eFilterType' in file filter.h for available selections. // @@ -136,7 +135,7 @@ void TaskDataAcquisition(void const *argument) // Temperature - degrees C //****************************************************************** - // BIT status. May have inadvertently changed this during an update. + // Update BIT status updateBITandStatus(); // ********************** Algorithm ************************ @@ -150,22 +149,14 @@ void TaskDataAcquisition(void const *argument) // loop. inertialAndPositionDataProcessing(dacqRate); - // Uncomment next line if there is intention of using S0 or S1 xbow + // Uncomment next line if there is intention of using S0 or S1 legacy // packets for continuous data output //***************************************************************** - // applyNewScaledSensorsData(); + //applyNewScaledSensorsData(); //***************************************************************** - // Inform user, that new data set is ready (if required) - // in case of UART communication interface clears pin IO2 - // in case of SPI communication interface clears pin DRDY - if(platformGetUnitCommunicationType() != UART_COMM){ - setDataReadyPin(0); - }else{ setIO2Pin (0); - } - - if (platformHasMag() ) { + if(platformHasMag() ) { // Mag Alignment (follows Kalman filter or user algorithm as the // innovation routine calculates the euler angles and the magnetic // vector in the NED-frame) @@ -175,8 +166,20 @@ void TaskDataAcquisition(void const *argument) if(platformGetUnitCommunicationType() != UART_COMM){ // Perform interface - specific processing here FillSPIBurstDataBuffer(); - } else { - // Process commands and output continuous packets to UART + if(spiRateRef){ + spiRateDiv++; + if(spiRateDiv >= spiRateRef){ + // Inform user, that new data set is ready (if required) + setDataReadyPin(0); // activate data ready - set low + spiRateDiv = 0; + spiRateRef = GetSpiPacketRateDivider(); + } + }else { + spiRateRef = GetSpiPacketRateDivider(); + } + UpdateSpiUserConfig(); + }else { + // Process user commands and output continuous packets to UART // Processing of user commands always goes first ProcessUserCommands (); SendContinuousPacket(dacqRate); diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserAlgorithm.c b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserAlgorithm.c index 8782dfe..b83e501 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserAlgorithm.c +++ b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserAlgorithm.c @@ -24,6 +24,7 @@ limitations under the License. *******************************************************************************/ #include +#include #include "platformAPI.h" #include "userAPI.h" @@ -35,6 +36,7 @@ limitations under the License. #include "arm_math.h" #endif +#include "UserConfiguration.h" #include "algorithmAPI.h" #include "algorithm.h" #include "UserAlgorithm.h" @@ -47,258 +49,75 @@ limitations under the License. // Declare the IMU data structure IMUDataStruct gIMU; gpsDataStruct_t gGPS; +odoDataStruct_t gOdo; // -static void _Algorithm(uint16_t dacqRate, uint8_t algoType); -static void _InitAlgo(uint8_t algoType); +static void _Algorithm(); -#ifndef INS_OFFLINE -static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); -#endif // !INS_OFFLINE - -// Initialize GPS algorithm variables +// Initialize algorithm variables void InitUserAlgorithm() { // Initialize built-in algorithm structure - InitializeAlgorithmStruct(FREQ_200_HZ); + InitializeAlgorithmStruct(FREQ_200_HZ, OpenIMU300ZI); // place additional required initialization here + /* Set the configuration variables for AHRS solution + * Enable mag + */ + enableMagInAlgorithm(TRUE); } -void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, gpsDataStruct_t *gps, uint16_t dacqRate) +void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, + gpsDataStruct_t *gps, odoDataStruct_t *odo, BOOL ppsDetected) { - // This can be set at startup based on the packet type selected - static uint8_t algoType = AHRS; - - // Initialize variable related to the UserNavAlgorithm - _InitAlgo(algoType); - // Populate the EKF input data structure - EKF_SetInputStruct(accels, rates, mags, gps); + EKF_SetInputStruct(accels, rates, mags, gps, odo, ppsDetected); // Call the desired algorithm based on the EKF with different // calling rates and different settings. - _Algorithm(dacqRate, algoType); + _Algorithm(); // Fill the output data structure with the EKF states and other // desired information EKF_SetOutputStruct(); -#ifndef INS_OFFLINE - // Generate a debug message that provides algorithm output to verify the - // algorithm is generating the proper output. - _GenerateDebugMessage(dacqRate, ZERO_HZ); -#endif // !INS_OFFLINE - // The returned value from this function is unused by external functions. The // NULL pointer is returned instead of a data structure. return NULL; } - // -static void _InitAlgo(uint8_t algoType) +static void _Algorithm() { - // Initialize the timer variables - static uint8_t initAlgo = 1; - if(initAlgo) { - // Reset 'initAlgo' so this is not executed more than once. This - // prevents the algorithm from being switched during run-time. - initAlgo = 0; - - // Set the configuration variables for a VG-type solution - // (useMags = 0 forces the VG solution) - gAlgorithm.Behavior.bit.freeIntegrate = 0; - gAlgorithm.Behavior.bit.useMag = 0; - gAlgorithm.Behavior.bit.useGPS = 0; - gAlgorithm.Behavior.bit.stationaryLockYaw = 0; - gAlgorithm.Behavior.bit.restartOnOverRange = 0; - gAlgorithm.Behavior.bit.dynamicMotion = 1; - - // Set the system configuration based on system type - switch( algoType ) { - case VG: - // Nothing additional to do (already configured for a VG - // solution) - break; - case AHRS: - // Set the configuration variables for AHRS solution - // (useMags = 1 and enable mags) - enableMagInAlgorithm(TRUE); - break; - case INS: - /* Set the configuration variables for INS solution. - * (Enable GPS and set algorithm calling frequency to 100Hz) - */ - enableMagInAlgorithm(FALSE); - enableGpsInAlgorithm(TRUE); - gAlgorithm.callingFreq = FREQ_100_HZ; // redundant; set above - break; - default: - // Nothing to do - break; - } - } + // Aceinna VG/AHRS/INS algorithm + EKF_Algorithm(); } - -// -static void _Algorithm(uint16_t dacqRate, uint8_t algoType) +BOOL getAlgorithmLinAccelDetectMode() { - // - static uint8_t algoCntr = 0, algoCntrLimit = 0; - - // Initialize the configuration variables needed to make the system - // generate a VG-type solution. - static uint8_t initAlgo = 1; - if(initAlgo) { - // Reset 'initAlgo' so this is not executed more than once. This - // prevents the algorithm from being switched during run-time. - initAlgo = 0; + return TRUE; +} - // Set the variables that control the algorithm execution rate - algoCntrLimit = (uint8_t)( (float)dacqRate / (float)gAlgorithm.callingFreq + 0.5 ); - if( algoCntrLimit < 1 ) { - // If this logic is reached, also need to adjust the algorithm - // parameters to match the modified calling freq (or stop the - // program to indicate that the user must adjust the program) - algoCntrLimit = 1; - } - algoCntr = algoCntrLimit; - } - - // Increment the counter. If greater than or equal to the limit, reset - // the counter to cause the algorithm to run on the next pass through. - algoCntr++; - if(algoCntr >= algoCntrLimit) { - // Reset counter - algoCntr = 0; +BOOL getAlgorithmAccelPredictMode() +{ + return FALSE; +} - // Log GPS measurement update - if (gEKFInput.gpsUpdate) - { - gAlgoStatus.bit.gpsUpdate = 1; - } - else - { - gAlgoStatus.bit.gpsUpdate = 0; - } - // Aceinna VG/AHRS/INS algorithm - EKF_Algorithm(); - } +float getAlgorithmCoefOfReduceQ() +{ + // 0.0001 to 1 (1 to 10000) + return (float)10/10000; } -#ifndef INS_OFFLINE -// -static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) +float getAlgorithmAccelSwitchDelay() { - // Variables that control the output frequency of the debug statement - static uint8_t debugOutputCntr, debugOutputCntrLimit; - - // Check debug flag. If set then generate the debug message to verify - // the output of the EKF algorithm - if( debugOutputFreq > ZERO_HZ ) { - // Initialize variables used to control the output of the debug messages - static int initFlag = 1; - if(initFlag) { - // Reset 'initFlag' so this section is not executed more than once. - initFlag = 0; - - // Set the variables that control the debug-message output-rate (based on - // the desired calling frequency of the debug output) - debugOutputCntrLimit = (uint8_t)( (float)dacqRate / (float)debugOutputFreq + 0.5 ); - debugOutputCntr = debugOutputCntrLimit; - } - - debugOutputCntr++; - if(debugOutputCntr >= debugOutputCntrLimit) { - debugOutputCntr = 0; -#if 1 - // For testing, the output packet should contain: - // 1) ITOW - // 2) Euler Angles - // 3) Ang-rate (meas) - // 4) Est ang-rate bias - // 5) Accel (meas) - // 6) Est accel bias - // 7) LLA (est) - // 8) NED-Vel (est) - // 9) Oper mode - - DebugPrintLongInt("", gGPS.itow ); - - DebugPrintFloat(", ", gEKFOutput.eulerAngs_BinN[ROLL], 4); - DebugPrintFloat(", ", gEKFOutput.eulerAngs_BinN[PITCH], 4); - DebugPrintFloat(", ", gEKFOutput.eulerAngs_BinN[YAW], 4); + // 0.01 to 10 (100 to 10000) + return (float)2000/1000; +} - DebugPrintFloat(", ", gEKFInput.angRate_B[X_AXIS], 5); - DebugPrintFloat(", ", gEKFInput.angRate_B[Y_AXIS], 5); - DebugPrintFloat(", ", gEKFInput.angRate_B[Z_AXIS], 5); - - DebugPrintFloat(", ", gEKFOutput.angRateBias_B[X_AXIS], 5); - DebugPrintFloat(", ", gEKFOutput.angRateBias_B[Y_AXIS], 5); - DebugPrintFloat(", ", gEKFOutput.angRateBias_B[Z_AXIS], 5); - - DebugPrintFloat(", ", gEKFInput.accel_B[X_AXIS], 5); - DebugPrintFloat(", ", gEKFInput.accel_B[Y_AXIS], 5); - DebugPrintFloat(", ", gEKFInput.accel_B[Z_AXIS], 5); - - DebugPrintFloat(", ", gEKFOutput.accelBias_B[X_AXIS], 5); - DebugPrintFloat(", ", gEKFOutput.accelBias_B[Y_AXIS], 5); - DebugPrintFloat(", ", gEKFOutput.accelBias_B[Z_AXIS], 5); - - DebugPrintFloat(", ", gEKFOutput.llaDeg[LAT], 8); - DebugPrintFloat(", ", gEKFOutput.llaDeg[LON], 8); - DebugPrintFloat(", ", gEKFOutput.llaDeg[ALT], 5); - - DebugPrintFloat(", ", gEKFOutput.velocity_N[X_AXIS], 5); - DebugPrintFloat(", ", gEKFOutput.velocity_N[Y_AXIS], 5); - DebugPrintFloat(", ", gEKFOutput.velocity_N[Z_AXIS], 5); -#if 1 - DebugPrintInt(", ", gEKFOutput.opMode); -#else - switch(gEKFOutputData.opMode) { - case 0: - DebugPrintString(", Stab"); - break; - case 1: - DebugPrintString(", Init"); - break; - case 2: - DebugPrintString(", HG"); - break; - case 3: - DebugPrintString(", LG"); - break; - case 4: - DebugPrintString(", INS"); - break; - } -#endif - DebugPrintEndline(); -#else - // - DebugPrintLongInt("ITOW: ", gGPS.itow ); - // LLA - DebugPrintFloat(", valid: ", (float)gGPS.gpsValid, 1); - // LLA - DebugPrintFloat(", Lat: ", (float)gGPS.latitude, 8); - DebugPrintFloat(", Lon: ", (float)gGPS.longitude, 8); - DebugPrintFloat(", Alt: ", (float)gGPS.altitude, 5); - // Velocity - //DebugPrintFloat(", vN: ", (float)gGPS.vNed[X_AXIS], 5); - //DebugPrintFloat(", vE: ", (float)gGPS.vNed[Y_AXIS], 5); - //DebugPrintFloat(", vD: ", (float)gGPS.vNed[Z_AXIS], 5); - // v^2 - //double vSquared = gGPS.vNed[X_AXIS] * gGPS.vNed[X_AXIS] + - // gGPS.vNed[Y_AXIS] * gGPS.vNed[Y_AXIS]; - //DebugPrintFloat(", vSq: ", (float)vSquared, 5); - //DebugPrintFloat(", vSq: ", (float)gGPS.rawGroundSpeed * (float)gGPS.rawGroundSpeed, 5); - // Newline - DebugPrintEndline(); -#endif - } - } +float getAlgorithmRateIntegrationTime() +{ + // 0.01 to 10 (100 to 10000) + return (float)2000/1000; } -#endif // !INS_OFFLINE diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserConfiguration.c b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserConfiguration.c index 8341b95..c12bac3 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserConfiguration.c +++ b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserConfiguration.c @@ -47,10 +47,18 @@ const UserConfigurationStruct gDefaultUserConfig = { .lpfRateFilterFreq = 25, .orientation = "+X+Y+Z", // The EVB connector pointing forward // add default parameter values here, if desired + .uartGpsBaudRate = 0, + .uartGpsProtocol = 0, .hardIron_X = 0.0, .hardIron_Y = 0.0, .softIron_Ratio = 1.0, - .softIron_Angle = 0.0 + .softIron_Angle = 0.0, + .appBehavior = APP_BEHAVIOR_USE_EXT_SYNC, + .spiOrientation = 0x0000, //+X +Y +Z + .spiSyncRate = 1, // 200Hz + .extSyncFreq = 1, // 1Hz + .spiAccelLpfType = IIR_20HZ_LPF, // Butterworth 20Hz + .spiGyroLpfType = IIR_20HZ_LPF, // Butterworth 20Hz }; UserConfigurationStruct gUserConfiguration; @@ -87,7 +95,7 @@ void userInitConfigureUnit() while(1); } - if(EEPROM_IsAppStartedFirstTime()) { + if(EEPROM_IsAppStartedFirstTime()){ // comment next line if want to keep previously stored in EEPROM parameters // after rebuilding and/or reloading new application RestoreDefaultUserConfig(); @@ -135,7 +143,7 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) { BOOL result = TRUE; uint64_t *ptr = (uint64_t *)&gUserConfiguration; - + uint16_t orientOut; if(number < USER_CRC || number >= USER_MAX_PARAM ){ return FALSE; } @@ -157,7 +165,7 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) result = platformSelectLPFilter(RATE_SENSOR, (uint32_t)data, fApply); break; case USER_ORIENTATION: - result = platformSetOrientation((uint16_t*)&data, fApply); + result = platformSetOrientation((uint16_t*)&data, &orientOut, fApply); break; case USER_CRC: case USER_DATA_SIZE: @@ -194,7 +202,7 @@ BOOL UpdateUserParameter(uint32_t number, uint64_t data, BOOL fApply) BOOL result; uint64_t *ptr = (uint64_t *)&gUserConfiguration; - if(number <= USER_LAST_SYSTEM_PARAM || number >= USER_MAX_PARAM ){ + if(number <= USER_LAST_SYSTEM_PARAM || number > USER_LAST_UART_PARAM){ return FALSE; } @@ -233,7 +241,7 @@ BOOL UpdateUserConfig(userConfigPayload* pld, uint8_t *payloadLen) BOOL ret = FALSE; int32_t result = 0; - maxParam = sizeof(UserConfigurationStruct)/8; + maxParam = USER_LAST_UART_PARAM; // Validate parameters numbers and quantity if(pld->numParams > MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET){ @@ -300,7 +308,7 @@ BOOL UpdateUserParam(userParamPayload* pld, uint8_t *payloadLen) BOOL ret = TRUE; int32_t result = 0; - maxParam = sizeof(UserConfigurationStruct)/8; + maxParam = USER_LAST_UART_PARAM; offsetValid = pld->paramNum < maxParam; if(offsetValid){ @@ -354,7 +362,7 @@ BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) int32_t result = 0; int numParams = (*payloadLen)/8; - maxParam = sizeof(UserConfigurationStruct)/8; + maxParam = USER_LAST_UART_PARAM; if(numParams > MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET){ lenValid = FALSE; @@ -417,7 +425,7 @@ BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen) BOOL lenValid = TRUE; uint64_t *ptr = (uint64_t *)&gUserConfiguration; - maxParam = sizeof(UserConfigurationStruct)/8; + maxParam = USER_LAST_UART_PARAM; offsetValid = pld->paramOffset < maxParam; @@ -456,7 +464,7 @@ BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen) BOOL offsetValid; uint64_t *ptr = (uint64_t *)&gUserConfiguration; - maxParam = sizeof(UserConfigurationStruct)/8; + maxParam = USER_LAST_UART_PARAM; offsetValid = pld->paramNum < maxParam; if(offsetValid){ @@ -487,7 +495,7 @@ BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) uint32_t offset, i, numParams; uint64_t *ptr = (uint64_t *)&gUserConfiguration; - numParams = sizeof(UserConfigurationStruct)/8; + numParams = USER_LAST_UART_PARAM; offset = 0; for (i = 0; i < numParams; i++, offset++){ @@ -562,3 +570,35 @@ BOOL RestoreDefaultUserConfig(void) } return valid; } + +BOOL ExtSyncEnabled() +{ + return (gUserConfiguration.appBehavior & APP_BEHAVIOR_USE_EXT_SYNC) != 0; +} + +int ExtSyncFrequency() +{ + return gUserConfiguration.extSyncFreq; +} + + +uint8_t SpiSyncRate() +{ + return (uint8_t)gUserConfiguration.spiSyncRate; +} + +uint8_t SpiAccelLpfType() +{ + return (uint8_t)gUserConfiguration.spiAccelLpfType; +} + +uint8_t SpiGyroLpfType() +{ + return (uint8_t)gUserConfiguration.spiGyroLpfType; +} + +uint16_t SpiOrientation() +{ + return (uint16_t)gUserConfiguration.spiOrientation; +} + diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserConfiguration.h b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserConfiguration.h index a6a74b3..9b97ee7 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserConfiguration.h +++ b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserConfiguration.h @@ -25,6 +25,7 @@ limitations under the License. #include "GlobalConstants.h" #include "UserMessagingUART.h" +#include "filter.h" /// User defined configuration strucrture ///Please notice, that parameters are 64 bit to accomodate double types as well as longer string types @@ -72,7 +73,7 @@ typedef struct { // here is the border between arbitrary parameters and platform configuration parameters //*************************************************************************************** - int64_t gpsBaudRate; /// baudrate of GPS UART, bps. + int64_t uartGpsBaudRate; /// baudrate of GPS UART, bps. /// valid options are: /// 4800 /// 9600 @@ -81,7 +82,7 @@ typedef struct { /// 57600 /// 115200 /// 230400 - int64_t gpsProtocol; /// protocol of GPS receicer. + int64_t uartGpsProtocol; /// protocol of GPS receicer. /// so far valid options are: /// NMEA_TEXT /// NOVATEL_BINARY @@ -94,12 +95,72 @@ typedef struct { double hardIron_Y; double softIron_Ratio; double softIron_Angle; + uint64_t appBehavior; + + //*************************************************************************************** + // SPI-Related specific parameters + //*************************************************************************************** + int64_t spiSyncRate; /// SPI data ready rate + /// 0 - 0 Hz + /// 1 - 200 Hz + /// 2 - 100 Hz + /// 3 - 50 Hz + /// 4 - 25 Hz + /// 5 - 20 Hz + /// 6 - 10 Hz + /// 7 - 5 Hz + /// 8 - 4 Hz + /// 9 - 2 Hz + + int64_t spiOrientation; /// orientation for SPI mode + //0x0000 +Ux +Uy +Uz + //0x0009 -Ux -Uy +Uz + //0x0023 -Uy +Ux +Uz + //0x002A +Uy -Ux +Uz + //0x0041 -Ux +Uy -Uz + //0x0048 +Ux -Uy -Uz + //0x0062 +Uy +Ux -Uz + //0x006B -Uy -Ux -Uz + //0x0085 -Uz +Uy +Ux + //0x008C +Uz -Uy +Ux + //0x0092 +Uy +Uz +Ux + //0x009B -Uy -Uz +Ux + //0x00C4 +Uz +Uy -Ux + //0x00CD -Uz -Uy -Ux + //0x00D3 -Uy +Uz -Ux + //0x00DA +Uy -Uz -Ux + //0x0111 -Ux +Uz +Uy + //0x0118 +Ux -Uz +Uy + //0x0124 +Uz +Ux +Uy + //0x012D -Uz -Ux +Uy + //0x0150 +Ux +Uz -Uy + //0x0159 -Ux -Uz -Uy + //0x0165 -Uz +Ux -Uy + //0x016C +Uz -Ux -Uy + + int64_t spiAccelLpfType; /// built-in lpf filter cutoff frequency selection for accelerometers + int64_t spiGyroLpfType; /// built-in lpf filter cutoff frequency selection for rate sensors + /// Options are: + // UNFILTERED = 0x00, + // FIR_40HZ_LPF = 0x03, // Bartlett LPF 40HZ + // FIR_20HZ_LPF = 0x04, // Bartlett LPF 20HZ + // FIR_10HZ_LPF = 0x05, // Bartlett LPF 10HZ + // FIR_05HZ_LPF = 0x06, // Bartlett LPF 5HZ + // IIR_50HZ_LPF = 0x30, // Butterworth LPF 50HZ + // IIR_20HZ_LPF = 0x40, // Butterworth LPF 20HZ + // IIR_10HZ_LPF = 0x50, // Butterworth LPF 10HZ + // IIR_05HZ_LPF = 0x60, // Butterworth LPF 5HZ + // IIR_02HZ_LPF = 0x70, // Butterworth LPF 2HZ + // IIR_25HZ_LPF = 0x80, // Butterworth LPF 25HZ + // IIR_40HZ_LPF = 0x90, // Butterworth LPF 40HZ + uint64_t extSyncFreq; /// external sync frequency + } UserConfigurationStruct; typedef enum { //***************************************************************************************** // add system parameters here and reassign USER_LAST_SYSTEM_PARAM (DO NOT CHANGE THIS!!!) - USER_CRC = 0, + USER_CRC = 0, // 0 USER_DATA_SIZE , // 1 USER_USER_BAUD_RATE , // 2 order of next 4 parameters USER_USER_PACKET_TYPE , // 3 of required unit output bandwidth @@ -107,15 +168,23 @@ typedef enum { USER_LPF_ACCEL_TYPE , // 5 prefered LPF filter type for accelerometer USER_LPF_RATE_TYPE , // 6 prefered LPF filter type for rate sensor USER_ORIENTATION , // 7 unit orientation - USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, + USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, // 7 //***************************************************************************************** // add parameter enumerator here while adding new parameter in user UserConfigurationStruct - USER_GPS_BAUD_RATE , - USER_GPS_PROTOCOL , - USER_HARD_IRON_X , - USER_HARD_IRON_Y , - USER_SOFT_IRON_RATIO , - USER_SOFT_IRON_ANGLE , + USER_GPS_BAUD_RATE , // 8 + USER_GPS_PROTOCOL , // 9 + USER_HARD_IRON_X , // 10 + USER_HARD_IRON_Y , // 11 + USER_SOFT_IRON_RATIO , // 12 + USER_SOFT_IRON_ANGLE , // 13 + USER_LAST_UART_PARAM , // 14 + USER_APPLICATION_BEHAVIOR , +// spi-specific parameters + USER_SPI_SYNC_RATE , // SPI data ready rate + USER_SPI_ORIENTATION , // SPI mode orientation + USER_SPI_ACCEl_LPF , // SPI mode accel lpf + USER_SPI_RATE_LPF , // SPI mode gyro lpf + USER_EXT_SYNC_FREQ , // Extern sync frequency applied tp SYNC/1PPS input USER_MAX_PARAM } UserConfigParamNumber; @@ -126,6 +195,7 @@ extern int userPacketOut; #define INVALID_PARAM -1 #define INVALID_VALUE -2 #define INVALID_PAYLOAD_SIZE -3 +#define APP_BEHAVIOR_USE_EXT_SYNC 0x0000000000000001LL @@ -152,6 +222,13 @@ extern BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLe extern BOOL UpdateUserParameter(uint32_t number, uint64_t data, BOOL fApply); extern BOOL UpdateSystemParameter(uint32_t offset, uint64_t data, BOOL fApply); BOOL setUserPacketType(uint8_t* type, BOOL fApply); +extern BOOL ExtSyncEnabled(); +extern int ExtSyncFrequency(); + +uint16_t SpiOrientation(); +uint8_t SpiSyncRate(); +uint8_t SpiAccelLpfType(); +uint8_t SpiGyroLpfType(); #endif /* USER_CONFIGURATION_H */ diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingSPI.c b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingSPI.c index 60f49d1..28f1f5a 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingSPI.c +++ b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingSPI.c @@ -34,7 +34,9 @@ limitations under the License. #include "sensorsAPI.h" #include "spiAPI.h" #include "magAPI.h" - +#include "appVersion.h" +#include "BitStatus.h" +#include "EKF_Algorithm.h" uint8_t _spiDataBuf[2][SPI_DATA_BUF_LEN]; uint8_t *_activeSpiDataBufPtr = _spiDataBuf[0]; @@ -43,10 +45,11 @@ uint8_t _activeSpiDataBufIdx = 0; uint8_t _spiRegs[NUM_SPI_REGS]; +int16_t _attitude[3]; BOOL _readOp = FALSE; uint8_t _regAddr = 0; - +BOOL fSaveSpiConfig = FALSE; // Initialize SPI registers here // For default register assignments refer to // UserMessaging.h @@ -81,11 +84,42 @@ void InitUserSPIRegisters() _spiRegs[SPI_REG_PROD_ID_REQUEST+1] = 0x00; // product code LO _spiRegs[SPI_REG_SERIAL_NUM_REQUEST] = arr[7]; _spiRegs[SPI_REG_SERIAL_NUM_REQUEST+1] = (arr[8] << 4) | arr[9]; + // fill up orientation + uint16_t tmp = SpiOrientation(); + _spiRegs[SPI_REG_ORIENTATION_LSB_CTRL] = tmp & 0xff; + _spiRegs[SPI_REG_ORIENTATION_MSB_CTRL] = (tmp >> 8) & 0xff; + // Fill up default DRDY rate + _spiRegs[SPI_REG_DRDY_RATE_CTRL] = SpiSyncRate(); + _spiRegs[SPI_REG_DRDY_CTRL] = 0x04; + _spiRegs[SPI_REG_ACCEL_FILTER_TYPE_CTRL] = SpiAccelLpfType(); + _spiRegs[SPI_REG_RATE_FILTER_TYPE_CTRL] = SpiGyroLpfType(); + _spiRegs[SPI_REG_ACCEL_SENSOR_RANGE_CTRL] = SPI_ACCEL_SENSOR_RANGE_8G; + _spiRegs[SPI_REG_RATE_SENSOR_RANGE_CTRL] = SPI_RATE_SENSOR_RANGE_500; + _spiRegs[SPI_REG_MAG_SENSOR_RANGE_CTRL] = SPI_MAG_SENSOR_RANGE_2G; + // Fill up sensors data scale factor on the SPI bus + _spiRegs[SPI_REG_ACCEL_SENSOR_SCALE] = 4; // 4000 counts per 1 G -> 8G + _spiRegs[SPI_REG_RATE_SENSOR_SCALE] = 64; // 64 counts per dps -> 500 dps + _spiRegs[SPI_REG_MAG_SENSOR_SCALE] = 16; // 16384 64 counts per Gauss // Fill up HW/SW versions _spiRegs[SPI_REG_HW_VERSION_REQUEST] = ReadUnitHwConfiguration(); - _spiRegs[SPI_REG_SW_VERSION_REQUEST] = unitSpiSwVersion(); - _spiRegs[SPI_REG_ACCEL_FILTER_TYPE_CTRL] = IIR_05HZ_LPF; - _spiRegs[SPI_REG_RATE_FILTER_TYPE_CTRL] = IIR_05HZ_LPF; + _spiRegs[SPI_REG_SW_VERSION_REQUEST] = APP_SPI_SW_VERSION; + { + uint8_t regOffset; + magAlignUserParams_t params; + int16_t tmp[4]; + getUserMagAlignParams(¶ms); + regOffset = SPI_REG_HARD_IRON_BIAS_X_MSB; + tmp[0] = (int16_t)((params.hardIron_X/10) * 32768 ); // +-10 Gauss + tmp[1] = (int16_t)((params.hardIron_Y/10) * 32768 ); // +-10 Gauss + tmp[2] = (int16_t)(params.softIron_Ratio * 32768 ); // + tmp[3] = (int16_t)((params.softIron_Angle/3.141592) * 32768 ); // +-3.141592 rad + for(int i = 0; i < 4; i++){ + _spiRegs[regOffset++] = (tmp[i] >> 8) & 0xff; + _spiRegs[regOffset++] = tmp[i] & 0xff; + } + } + + } @@ -93,15 +127,52 @@ void InitUserSPIRegisters() // Each new data set get loaded into new buffer, while pervious // data set remains in old buffer. After loading active buffer get switched // When user requests data from outside - dtata from active buffer will be transmitted - BOOL loadSPIBurstData(spi_burst_data_t *data) +BOOL loadSPIBurstData(ext_spi_burst_data_t *data) { - if(sizeof(spi_burst_data_t) > SPI_DATA_BUF_LEN){ - return FALSE; + if(sizeof(ext_spi_burst_data_t) > SPI_DATA_BUF_LEN){ + while(1); // catch it here } - memcpy(_spiDataBuf[_emptySpiDataBufIdx], data, sizeof(spi_burst_data_t)); + memcpy(_spiDataBuf[_emptySpiDataBufIdx], data, sizeof(ext_spi_burst_data_t)); _activeSpiDataBufPtr = _spiDataBuf[_emptySpiDataBufIdx]; _activeSpiDataBufIdx = _emptySpiDataBufIdx; //active buffer; _emptySpiDataBufIdx ^= 1; //new buffer; + +// Accelerometer data + _spiRegs[SPI_REG_XACCEL_REQUEST] = data->accels[0] & 0xff; + _spiRegs[SPI_REG_XACCEL_REQUEST+1] = (data->accels[0] >> 8) & 0xff; + _spiRegs[SPI_REG_YACCEL_REQUEST] = data->accels[1] & 0xff; + _spiRegs[SPI_REG_YACCEL_REQUEST+1] = (data->accels[1] >> 8) & 0xff; + _spiRegs[SPI_REG_ZACCEL_REQUEST] = data->accels[2] & 0xff; + _spiRegs[SPI_REG_ZACCEL_REQUEST+1] = (data->accels[2] >> 8) & 0xff; +// Gyro data + _spiRegs[SPI_REG_XRATE_REQUEST] = data->rates[0] & 0xff; + _spiRegs[SPI_REG_XRATE_REQUEST+1] = (data->rates[0] >> 8) & 0xff; + _spiRegs[SPI_REG_YRATE_REQUEST] = data->rates[1] & 0xff; + _spiRegs[SPI_REG_YRATE_REQUEST+1] = (data->rates[1] >> 8) & 0xff; + _spiRegs[SPI_REG_ZRATE_REQUEST] = data->rates[2] & 0xff; + _spiRegs[SPI_REG_ZRATE_REQUEST+1] = (data->rates[2] >> 8) & 0xff; +// Mag data + _spiRegs[SPI_REG_XMAG_REQUEST] = data->mags[0] & 0xff; + _spiRegs[SPI_REG_XMAG_REQUEST+1] = (data->mags[0] >> 8) & 0xff; + _spiRegs[SPI_REG_YMAG_REQUEST] = data->mags[1] & 0xff; + _spiRegs[SPI_REG_YMAG_REQUEST+1] = (data->mags[1] >> 8) & 0xff; + _spiRegs[SPI_REG_ZMAG_REQUEST] = data->mags[2] & 0xff; + _spiRegs[SPI_REG_ZMAG_REQUEST+1] = (data->mags[2] >> 8) & 0xff; +// Temp data + _spiRegs[SPI_REG_RTEMP_REQUEST] = data->temp & 0xff; + _spiRegs[SPI_REG_RTEMP_REQUEST+1] = (data->temp >> 8) & 0xff; + _spiRegs[SPI_REG_BTEMP_REQUEST] = data->temp & 0xff; + _spiRegs[SPI_REG_BTEMP_REQUEST+1] = (data->temp >> 8) & 0xff; +// Master Status + _spiRegs[SPI_REG_MASTER_STATUS_REQUEST] = data->status & 0xff; + _spiRegs[SPI_REG_MASTER_STATUS_REQUEST+1] = (data->status >> 8) & 0xff; +// HW Status + _spiRegs[SPI_REG_HW_STATUS_REQUEST] = gBitStatus.hwBIT.all & 0xff; + _spiRegs[SPI_REG_HW_STATUS_REQUEST+1] = (gBitStatus.hwBIT.all >> 8) & 0xff; +// SW Status + _spiRegs[SPI_REG_SW_STATUS_REQUEST] = gBitStatus.swBIT.all & 0xff; + _spiRegs[SPI_REG_SW_STATUS_REQUEST+1] = (gBitStatus.swBIT.all >> 8) & 0xff; + return TRUE; } @@ -120,22 +191,37 @@ uint8_t bufPtr = 0; void SPI_ProcessCommand(uint8_t cmd) { static uint8_t regData[2] = {0,0}; -// ext_spi_burst_data_t *bptr = (ext_spi_burst_data_t*)_activeSpiDataBufPtr; _readOp = (cmd & 0x80) == 0; _regAddr = 0xff; // reset reg addr // command in cmd" // place result in "out", place len into *len if(_readOp){ switch(cmd & 0x7F){ - case SPI_REG_BURST_MSG_REQUEST: + case 0x3e: // sensors data burstCnt++; bufIdx[bufPtr++] = _activeSpiDataBufIdx; UserSPIPrepareForDataTransmit(_activeSpiDataBufPtr, sizeof(spi_burst_data_t)); break; + case 0x3f: + // ext sensors data + burstCnt++; + bufIdx[bufPtr++] = _activeSpiDataBufIdx; + UserSPIPrepareForDataTransmit(_activeSpiDataBufPtr, sizeof(ext_spi_burst_data_t)); + break; + case 0x3D: + // ext sensors data + burstCnt++; + bufIdx[bufPtr++] = _activeSpiDataBufIdx; + att_spi_burst_data_t *pld = (att_spi_burst_data_t*)_activeSpiDataBufPtr; + for(int i = 0; i < 3; i++){ + pld->attitude[i] = _attitude[i]; + } + UserSPIPrepareForDataTransmit(_activeSpiDataBufPtr, sizeof(att_spi_burst_data_t)); + break; case SPI_REG_MAG_ALIGN_CTRL: { - uint8_t len = 1, state, regOffset; + uint8_t state, regOffset; real params[4]; int16_t tmp[4]; state = GetMagAlignEstimatedParams(params); @@ -164,11 +250,39 @@ void SPI_ProcessCommand(uint8_t cmd) } } +BOOL CheckSpiPacketRateDivider(uint8_t dataRate ) +{ + switch( dataRate ) + { + case OUTPUT_DATA_RATE_ZERO : + case OUTPUT_DATA_RATE_200_HZ: + case OUTPUT_DATA_RATE_100_HZ: + case OUTPUT_DATA_RATE_50_HZ : + case OUTPUT_DATA_RATE_25_HZ : + case OUTPUT_DATA_RATE_20_HZ : + case OUTPUT_DATA_RATE_10_HZ : + case OUTPUT_DATA_RATE_5_HZ : + case OUTPUT_DATA_RATE_4_HZ : + case OUTPUT_DATA_RATE_2_HZ : + case OUTPUT_DATA_RATE_1_HZ : + return TRUE; + break; + default: + return FALSE; + break; + } +} /* end CheckPacketRateDivider */ + BOOL CheckSpiSensorFilterType(uint8_t type) { switch(type){ case UNFILTERED: + case FIR_40HZ_LPF: + case FIR_20HZ_LPF: + case FIR_10HZ_LPF: + case FIR_05HZ_LPF: + case IIR_02HZ_LPF: case IIR_05HZ_LPF: case IIR_10HZ_LPF: case IIR_20HZ_LPF: @@ -181,6 +295,60 @@ BOOL CheckSpiSensorFilterType(uint8_t type) } }; +BOOL CheckSpiSyncRate(uint8_t rate) +{ + if(rate < 10){ + return TRUE; + } + return FALSE; +} + +uint16_t GetSpiOrientationFromRegs() +{ + uint16_t tmp; + + tmp = (_spiRegs[SPI_REG_ORIENTATION_MSB_CTRL] << 8) | _spiRegs[SPI_REG_ORIENTATION_LSB_CTRL]; + return tmp; +} + +void SaveParam(uint8_t paramId) +{ + switch(paramId){ + case 0: + case 0xff: + // save all parameters + gUserConfiguration.spiOrientation = GetSpiOrientationFromRegs(); + gUserConfiguration.spiGyroLpfType = _spiRegs[SPI_REG_RATE_FILTER_TYPE_CTRL]; + gUserConfiguration.spiAccelLpfType = _spiRegs[SPI_REG_ACCEL_FILTER_TYPE_CTRL]; + case SPI_REG_DRDY_RATE_CTRL: + gUserConfiguration.spiSyncRate = _spiRegs[SPI_REG_DRDY_RATE_CTRL]; + break; + case SPI_REG_ACCEL_FILTER_TYPE_CTRL: + gUserConfiguration.spiAccelLpfType = _spiRegs[SPI_REG_ACCEL_FILTER_TYPE_CTRL]; + break; + case SPI_REG_RATE_FILTER_TYPE_CTRL: + gUserConfiguration.spiGyroLpfType = _spiRegs[SPI_REG_RATE_FILTER_TYPE_CTRL]; + break; + case SPI_REG_ORIENTATION_MSB_CTRL: + case SPI_REG_ORIENTATION_LSB_CTRL: + gUserConfiguration.spiOrientation = GetSpiOrientationFromRegs(); + break; + default: + return; + } + + fSaveSpiConfig = TRUE; + +} + +void UpdateSpiUserConfig() +{ + if(fSaveSpiConfig){ + SaveUserConfig(); + fSaveSpiConfig = FALSE; + } +} + // Here SPI write transaction is processed // based on register address assigned in processSPICommand function @@ -193,25 +361,27 @@ void SPI_ProcessData(uint8_t* in) { uint16_t tmp; static uint8_t orientShadow = 0xff; + static uint8_t resetShadow = 0x00; if(!_readOp || _regAddr != 0xff){ switch(_regAddr){ - // Place read only registers here - case SPI_REG_MANUF_CODE_REQUEST: - case SPI_REG_MANUF_LOC_REQUEST: - case SPI_REG_UNIT_CODE_REQUEST: - case SPI_REG_UNIT_CODE_REQUEST+1: - case SPI_REG_PROD_ID_REQUEST: - case SPI_REG_PROD_ID_REQUEST+1: - case SPI_REG_SERIAL_NUM_REQUEST: - case SPI_REG_SERIAL_NUM_REQUEST+1: - case SPI_REG_HW_VERSION_REQUEST: - case SPI_REG_SW_VERSION_REQUEST: + case SPI_REG_RESET_MSB_CTRL: + if(in[1] == 0x55){ + resetShadow = 1; + }else { + resetShadow = 1; + } break; - case SPI_REG_MAG_ALIGN_CTRL: - { - uint8_t len = 1; - ProcessMagAlignCmds((magAlignCmdPayload*)&in[1], &len); + case SPI_REG_RESET_LSB_CTRL: + if(in[1] == 0xaa && resetShadow){ + Reset(); + }else { + resetShadow = 0; + } + break; + case SPI_REG_DRDY_RATE_CTRL: + if(CheckSpiSyncRate(in[1])){ + _spiRegs[_regAddr] = in[1]; } break; case SPI_REG_ORIENTATION_MSB_CTRL: @@ -239,8 +409,16 @@ void SPI_ProcessData(uint8_t* in) platformUpdateRateFilterType(in[1]); } break; + case SPI_REG_MAG_ALIGN_CTRL: + { + uint8_t len = 1; + ProcessMagAlignCmds((magAlignCmdPayload*)&in[1], &len); + } + break; + case SPI_REG_SAVE_CFG_CTRL: + SaveParam(in[1]); + break; default: - _spiRegs[_regAddr] = in[1]; break; } } @@ -268,7 +446,7 @@ uint16_t overRange = 0; ******************************************************************************/ int16_t prepare_sensor_value( float dataIn, float limit, float scaleFactor) { - static uint16_t UpperLimit = 32760; + static uint16_t UpperLimit = 32766; float tempFloat = 0.0; int sign; static int16_t tempInt16 = 0x0000; @@ -306,14 +484,18 @@ int16_t prepare_sensor_value( float dataIn, float limit, float scaleFactor) // Fills SPI burst data buffer with latest samples void FillSPIBurstDataBuffer() { - spi_burst_data_t data; - double Rates[3], Accels[3]; + ext_spi_burst_data_t data; + double Rates[3], Accels[3], Mags[3]; double temp; + real EulerAngles[3]; float t1; GetRateData_degPerSec(Rates); GetAccelData_g(Accels); GetBoardTempData(&temp); + GetMagData_G(Mags); + EKF_GetAttitude_EA_RAD(EulerAngles); + data.status = 0; overRange = 0; @@ -321,37 +503,53 @@ void FillSPIBurstDataBuffer() for(int i = 0; i < 3; i++){ data.accels[i] = prepare_sensor_value(Accels[i], GetSpiAccelLimit(), GetSpiAccelScaleFactor()); data.rates[i] = prepare_sensor_value(Rates[i], GetSpiRateLimit(), GetSpiRateScaleFactor()); + t1 = Mags[i] * 16384; + data.mags[i] = swap16((int16_t)t1); + t1 = EulerAngles[i] * 10430.378; // 65536/2PI + _attitude[i] = swap16((int16_t)t1); } if(overRange){ data.status |= SENSOR_OVER_RANGE_STATUS_MASK; } - t1 = temp; + t1 = (temp - 31.0)/0.073111172849435; data.temp = swap16((int16_t)t1); loadSPIBurstData(&data); } -int GetSpiPacketRateDivider() -{ - return _spiRegs[SPI_REG_DRDY_RATE_CTRL]; -} - float GetSpiAccelScaleFactor() { - return 4000.0; // so far limited to 5G + return 4000.0; // so far limited to 8G } float GetSpiRateScaleFactor() { - return 100; + return 64; } float GetSpiRateLimit() { - return 300.0; + return 500.0; } float GetSpiAccelLimit() { - return 4.5; // so far limited to 4,5G + return 8.0; // so far limited to 8G +} + +int GetSpiPacketRateDivider() +{ + switch(_spiRegs[SPI_REG_DRDY_RATE_CTRL]){ + case 0: return 0; // 0 Hz + case 2: return 2; // 100 Hz + case 3: return 4; // 50 Hz + case 4: return 8; // 25 Hz + case 5: return 10; // 20 Hz + case 6: return 20; // 10 Hz + case 7: return 40; // 5 Hz + case 8: return 50; // 4 Hz + case 9: return 100; // 2 Hz + default: + return 1; // 200 Hz + } } diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingSPI.h b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingSPI.h index d476716..777c8e6 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingSPI.h +++ b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingSPI.h @@ -40,40 +40,76 @@ typedef struct{ int16_t temp; }spi_burst_data_t; +#pragma pack(1) typedef struct{ uint16_t status; int16_t rates[3]; int16_t accels[3]; int16_t temp; - int16_t sensToPps; - int16_t ppsToDrdy; + int16_t mags[3]; }ext_spi_burst_data_t; +typedef struct{ + uint16_t status; + int16_t rates[3]; + int16_t accels[3]; + int16_t temp; + int16_t attitude[3]; +}att_spi_burst_data_t; +#pragma pack() #define SPI_DATA_BUF_LEN 32 #define SPI_BURST_DATA_SIZE 16 #define NUM_SPI_REGS 128 -// functions of SPI interface registers -#define SPI_REG_DRDY_RATE_CTRL 0x37 // DATA READY PIN output frequency divider (from 200Hz) -#define SPI_REG_ORIENTATION_LSB_CTRL 0x75 // Orientation LSB -#define SPI_REG_ORIENTATION_MSB_CTRL 0x74 // Orientation MSB -#define SPI_REG_ACCEL_FILTER_TYPE_CTRL 0x38 // Built-in LPF selection -#define SPI_REG_RATE_FILTER_TYPE_CTRL 0x39 // Built-in LPF selection +#define SPI_REG_RESET_LSB_CTRL 0x02 +#define SPI_REG_RESET_MSB_CTRL 0x03 + +#define SPI_REG_DRDY_RATE_CTRL 0x37 +#define SPI_REG_ACCEL_FILTER_TYPE_CTRL 0x38 +#define SPI_REG_ACCEL_SENSOR_RANGE_CTRL 0x70 +#define SPI_REG_RATE_SENSOR_RANGE_CTRL 0x71 +#define SPI_REG_MAG_SENSOR_RANGE_CTRL 0x72 +#define SPI_REG_ORIENTATION_LSB_CTRL 0x75 +#define SPI_REG_ORIENTATION_MSB_CTRL 0x74 +#define SPI_REG_SAVE_CFG_CTRL 0x76 +#define SPI_REG_RATE_FILTER_TYPE_CTRL 0x78 + +#define SPI_REG_SELF_TEST_CTRL 0x35 +#define SPI_REG_CLOB_CTRL 0x3E +#define SPI_REG_DRDY_CTRL 0x34 #define SPI_REG_MAG_ALIGN_CTRL 0x50 // mag alignment command #define SPI_REG_MAG_ALIGN_CTRL2 0x51 // mag alignment command/status -#define SPI_REG_BURST_MSG_REQUEST 0x3E // burst message request -#define SPI_REG_EXT_BURST_MSG_REQUEST 0x3F // estended burst message request -#define SPI_REG_MAG_ALIGNMENT_STATUS 0x51 // mag align status -#define SPI_REG_MANUF_CODE_REQUEST 0x52 // manufacturer code -#define SPI_REG_MANUF_LOC_REQUEST 0x53 // manufacturel location -#define SPI_REG_UNIT_CODE_REQUEST 0x54 // unit code -#define SPI_REG_PROD_ID_REQUEST 0x56 // product ID - 0x3830(default) -#define SPI_REG_SERIAL_NUM_REQUEST 0x58 // serial number request -#define SPI_REG_MASTER_STATUS_REQUEST 0x5A // master status request -#define SPI_REG_HW_VERSION_REQUEST 0x7E // HW version request -#define SPI_REG_SW_VERSION_REQUEST 0x7F // SW version request +#define SPI_REG_XRATE_REQUEST 0x04 +#define SPI_REG_YRATE_REQUEST 0x06 +#define SPI_REG_ZRATE_REQUEST 0x08 +#define SPI_REG_XACCEL_REQUEST 0x0A +#define SPI_REG_YACCEL_REQUEST 0x0C +#define SPI_REG_ZACCEL_REQUEST 0x0E +#define SPI_REG_XMAG_REQUEST 0x10 +#define SPI_REG_YMAG_REQUEST 0x12 +#define SPI_REG_ZMAG_REQUEST 0x14 +#define SPI_REG_RTEMP_REQUEST 0x16 +#define SPI_REG_BTEMP_REQUEST 0x18 + +#define SPI_REG_BURST_MSG_REQUEST 0x3E + + + +#define SPI_REG_MAG_SENSOR_SCALE 0x32 +#define SPI_REG_ACCEL_SENSOR_SCALE 0x46 +#define SPI_REG_RATE_SENSOR_SCALE 0x47 +#define SPI_REG_MANUF_CODE_REQUEST 0x52 +#define SPI_REG_MANUF_LOC_REQUEST 0x53 +#define SPI_REG_UNIT_CODE_REQUEST 0x54 +#define SPI_REG_PROD_ID_REQUEST 0x56 // 0x3830 +#define SPI_REG_SERIAL_NUM_REQUEST 0x58 +#define SPI_REG_MASTER_STATUS_REQUEST 0x5A +#define SPI_REG_HW_STATUS_REQUEST 0x5C +#define SPI_REG_SW_STATUS_REQUEST 0x5E +#define SPI_REG_HW_VERSION_REQUEST 0x7E +#define SPI_REG_SW_VERSION_REQUEST 0x7F #define SPI_REG_HARD_IRON_BIAS_X_MSB 0x48 // Estimated hard iron bias X #define SPI_REG_HARD_IRON_BIAS_X_LSB 0x49 // Estimated hard iron bias X @@ -100,9 +136,21 @@ enum output_drdy_rate { OUTPUT_DATA_RATE_1_HZ = 200 }; +enum output_rate_sensor_dyn_range{ + SPI_RATE_SENSOR_RANGE_500 = 0x08, +}; + +enum output_accel_sensor_dyn_range{ + SPI_ACCEL_SENSOR_RANGE_8G = 8, +}; + +enum output_mag_sensor_dyn_range{ + SPI_MAG_SENSOR_RANGE_2G = 2, +}; + #define SENSOR_OVER_RANGE_STATUS_MASK 0x0010 -BOOL loadSPIBurstData(spi_burst_data_t *data); +BOOL loadSPIBurstData(ext_spi_burst_data_t *data); void InitUserSPIRegisters(); int16_t prepare_sensor_value( float dataIn, float limit, float scaleFactor); uint16_t GetSpiFilterType(); @@ -113,6 +161,7 @@ float GetSpiAccelLimit(); float GetSpiRateLimit(); void fillSPIDataBuffer(); int UserSPIPrepareForDataTransmit(uint8_t *out, int outLen); +void UpdateSpiUserConfig(); diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingUART.h b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingUART.h index 56bb3a7..9148a8c 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingUART.h +++ b/examples/OpenIMU300ZI/VG_AHRS/src/user/UserMessagingUART.h @@ -18,8 +18,8 @@ See the License for the specific language governing permissions and limitations under the License. *******************************************************************************/ -#ifndef USER_MESSAGING_H -#define USER_MESSAGING_H +#ifndef _USER_MESSAGING_UART_H +#define _USER_MESSAGING_UART_H #include @@ -44,7 +44,7 @@ typedef enum { PACKET_RATE_5HZ = 5, // packet rate 5 Hz PACKET_RATE_2HZ = 2, // packet rate 2 Hz PACKET_RATE_1HZ = 1, // packet rate 1 Hz -}packet_rate_t; +} packet_rate_t; // User Input packet payload has next structure: @@ -90,7 +90,7 @@ typedef enum { #pragma pack(1) typedef struct { uint8_t packetPayload[252]; // maximum 252 bytes -}userPacket; +} userPacket; #define MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET 30 #define FIRST_30_PARAMS 0xFFFFFFFF diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/user/dataProcessingAndPresentation.c b/examples/OpenIMU300ZI/VG_AHRS/src/user/dataProcessingAndPresentation.c index a53c311..ac7ca8c 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/src/user/dataProcessingAndPresentation.c +++ b/examples/OpenIMU300ZI/VG_AHRS/src/user/dataProcessingAndPresentation.c @@ -28,23 +28,24 @@ limitations under the License. #include "userAPI.h" #include "sensorsAPI.h" - -#ifdef GPS #include "gpsAPI.h" -static void _GPSDebugMessage(void); -#endif - #include "UserAlgorithm.h" #include "algorithm.h" #include "algorithmAPI.h" +#include "platformAPI.h" #include "Indices.h" // For X_AXIS and Z_AXIS #include "debug.h" // For debug commands +BOOL fAlgorithmSynced; + // Local-function prototypes static void _IncrementIMUTimer(uint16_t dacqRate); static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); static void _IMUDebugMessage(void); +#ifdef GPS +static void _GPSDebugMessage(void); +#endif /* * ****************** @@ -69,6 +70,8 @@ Data * Built-in * Raw Data * * Data * User Filter s* * Al void initUserDataProcessingEngine() { InitUserAlgorithm(); // default implementation located in file user_algorithm.c +// init PPS sync engine + platformEnableGpsPps(TRUE); } @@ -86,6 +89,44 @@ void inertialAndPositionDataProcessing(uint16_t dacqRate) // Increment the IMU timer by the calling rate of the data-acquisition task _IncrementIMUTimer(dacqRate); + static uint8_t initAlgo = 1; + static uint8_t algoCntr = 0, algoCntrLimit = 0; + static BOOL firstTimeSync = TRUE; + if (initAlgo) + { + // Reset 'initAlgo' so this is not executed more than once. This + // prevents the algorithm from being switched during run-time. + initAlgo = 0; + + // Set the variables that control the algorithm execution rate + algoCntrLimit = (uint8_t)((float)dacqRate / (float)gAlgorithm.callingFreq + 0.5); + if (algoCntrLimit < 1) + { + // If this logic is reached, also need to adjust the algorithm + // parameters to match the modified calling freq (or stop the + // program to indicate that the user must adjust the program) + algoCntrLimit = 1; + } + algoCntr = algoCntrLimit; + } + + // call the algorithm + algoCntr++; + // syn the calling of the algorithm with PPS. This is done only once. + if(firstTimeSync) + { + if (platformGetPpsFlag(FALSE)) // do not reset pps detection state + { + firstTimeSync = FALSE; + fAlgorithmSynced = TRUE; + algoCntr = algoCntrLimit; + } + } + if (algoCntr >= algoCntrLimit) + { + // Reset counter + algoCntr = 0; + // Obtain accelerometer data [g] GetAccelData_g(gIMU.accel_g); @@ -104,12 +145,11 @@ void inertialAndPositionDataProcessing(uint16_t dacqRate) GetGPSData(&gGPS); #endif - // Generate a debug message that provide sensor data in order to verify the - // algorithm input data is as expected. - _GenerateDebugMessage(dacqRate, ZERO_HZ); + // check if pps is detected right before this excution of the task. + BOOL ppsDetected = platformGetPpsFlag(TRUE); // Execute user algorithm (default implementation located in file user_algorithm.c) - results = RunUserNavAlgorithm(gIMU.accel_g, gIMU.rate_radPerSec, gIMU.mag_G, &gGPS, dacqRate); + results = RunUserNavAlgorithm(gIMU.accel_g, gIMU.rate_radPerSec, gIMU.mag_G, &gGPS, &gOdo, ppsDetected); // Get algorithm status, and set the state in system status AlgoStatus algoStatus; @@ -118,7 +158,12 @@ void inertialAndPositionDataProcessing(uint16_t dacqRate) gBitStatus.swAlgBIT.bit.initialization = algoStatus.bit.algorithmInit; // add current result to output queue for subsequent sending out as continuous packet // returns pointer to user-defined results structure - WriteResultsIntoOutputStream(results) ; // default implementation located in file file UserMessaging.c + WriteResultsIntoOutputStream(results); // default implementation located in file file UserMessaging.c + } + + // Generate a debug message that provide sensor data in order to verify the + // algorithm input data is as expected. + _GenerateDebugMessage(dacqRate, ZERO_HZ); } @@ -224,6 +269,8 @@ static void _GPSDebugMessage(void) DebugPrintFloat(", Lon: ", (float)gGPS.longitude, 8); DebugPrintFloat(", Alt: ", (float)gGPS.altitude, 5); DebugPrintLongInt(", ITOW: ", gGPS.itow ); + DebugPrintLongInt(", EstITOW: ", platformGetEstimatedITOW() ); + DebugPrintLongInt(", SolutionTstamp: ", (uint64_t)platformGetSolutionTstampAsDouble()); #else // DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); diff --git a/examples/OpenIMU300ZI/VG_AHRS/src/userAPI.h b/examples/OpenIMU300ZI/VG_AHRS/src/user/userAPI.h similarity index 91% rename from examples/OpenIMU300ZI/VG_AHRS/src/userAPI.h rename to examples/OpenIMU300ZI/VG_AHRS/src/user/userAPI.h index 88b2ad4..86ca868 100644 --- a/examples/OpenIMU300ZI/VG_AHRS/src/userAPI.h +++ b/examples/OpenIMU300ZI/VG_AHRS/src/user/userAPI.h @@ -41,10 +41,12 @@ limitations under the License. #include #include "gpsAPI.h" +#include "odoAPI.h" void inertialAndPositionDataProcessing(uint16_t dacqRate); -void *RunUserNavAlgorithm(double *accels, double *rates, double* mags, gpsDataStruct_t *gps, uint16_t dacqRate); +void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, + gpsDataStruct_t *gps, odoDataStruct_t *odo, BOOL ppsDetected); void WriteResultsIntoOutputStream(void *results) ; void InitUserDataStructures(); void InitUserFilters(); diff --git a/platform.json b/platform.json index bdbaf19..6040782 100644 --- a/platform.json +++ b/platform.json @@ -20,7 +20,7 @@ "type": "git", "url": "https://github.com/aceinna/platform-aceinna_imu.git" }, - "version": "1.3.3", + "version": "1.3.4", "packages": { "toolchain-gccarmnoneeabi": { "type": "toolchain",