diff --git a/examples/OpenIMU300RI/IMU/board/OpenIMU300.json b/examples/OpenIMU300RI/IMU/board/OpenIMU300.json deleted file mode 100644 index cacb4ca..0000000 --- a/examples/OpenIMU300RI/IMU/board/OpenIMU300.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "build": { - "core": "stm32", - "cpu": "cortex-m4", - "extra_flags": [ - "-mcpu=cortex-m4", - "-mfloat-abi=softfp", - "-mfpu=fpv4-sp-d16", - "-DSTM32F4", - "-DSTM32F405RG", - "-DSTM32F405xx" - ], - "ldscript": "stm32f40x.ld", - "f_cpu": "120000000L", - "mcu": "stm32f405rg" - }, - "debug": { - "default_tools": ["stlink"], - "openocd_target": "stm32f4x", - "jlink_device": "STM32F405RG", - "svd_path": "STM32F40x.svd" - }, - "frameworks": [], - "name": "Aceinna OpenIMU 300", - "upload": { - "offset_address": "0x08010000", - "maximum_ram_size": 131072, - "maximum_size": 1048576, - "protocols": [ - "blackmagic", - "stlink", - "jlink" - ], - "protocol": "stlink" - }, - "url": "https://www.aceinna.com/inertial-systems/", - "vendor": "Aceinna" -} diff --git a/examples/OpenIMU300RI/IMU/src/FreeRTOSConfig.h b/examples/OpenIMU300RI/IMU/include/FreeRTOSConfig.h similarity index 100% rename from examples/OpenIMU300RI/IMU/src/FreeRTOSConfig.h rename to examples/OpenIMU300RI/IMU/include/FreeRTOSConfig.h diff --git a/examples/OpenIMU300RI/IMU/src/osresources.h b/examples/OpenIMU300RI/IMU/include/osresources.h similarity index 100% rename from examples/OpenIMU300RI/IMU/src/osresources.h rename to examples/OpenIMU300RI/IMU/include/osresources.h diff --git a/examples/OpenIMU300RI/IMU/ldscripts/stm32f40x.ld b/examples/OpenIMU300RI/IMU/ldscripts/stm32f40x.ld deleted file mode 100644 index 1c0c9f6..0000000 --- a/examples/OpenIMU300RI/IMU/ldscripts/stm32f40x.ld +++ /dev/null @@ -1,202 +0,0 @@ -/* -***************************************************************************** -** - -** File : LinkerScript.ld -** -** Abstract : Linker script for STM32F405RGx Device with -** 1024KByte FLASH, 128KByte RAM -** -** Set heap size, stack size and stack location according -** to application requirements. -** -** Set memory bank area and size if external memory is used. -** -** Target : STMicroelectronics STM32 -** -** -** Distribution: The file is distributed as is, without any warranty -** of any kind. -** -***************************************************************************** -** @attention -** -**

© COPYRIGHT(c) 2014 Ac6

-** -** Redistribution and use in source and binary forms, with or without modification, -** are permitted provided that the following conditions are met: -** 1. Redistributions of source code must retain the above copyright notice, -** this list of conditions and the following disclaimer. -** 2. Redistributions in binary form must reproduce the above copyright notice, -** this list of conditions and the following disclaimer in the documentation -** and/or other materials provided with the distribution. -** 3. Neither the name of Ac6 nor the names of its contributors -** may be used to endorse or promote products derived from this software -** without specific prior written permission. -** -** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -** -***************************************************************************** -*/ - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* Highest address of the user mode stack */ -_estack = 0x2001ff00; /* end of RAM */ -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0x2000; /* required amount of heap */ -_Min_Stack_Size = 0x2000; /* required amount of stack */ - -/* Specify the memory areas */ -MEMORY -{ -FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 448K -EEPROM (rw) : ORIGIN = 0x08008000, LENGTH = 16K -USER_EEPROM(rw) : ORIGIN = 0x0800C000, LENGTH = 16K -RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K -} - -/* Define output sections */ -SECTIONS -{ - /* The startup code goes first into FLASH */ - .isr_vector : - { - . = ALIGN(4); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - } >FLASH - - /* The program code and other data goes into FLASH */ - .text : - { - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) - - KEEP (*(.init)) - KEEP (*(.fini)) - - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH - - /* Constant data goes into FLASH */ - .rodata : - { - . = ALIGN(4); - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - . = ALIGN(4); - } >FLASH - - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH - .ARM : { - __exidx_start = .; - *(.ARM.exidx*) - __exidx_end = .; - } >FLASH - - .preinit_array : - { - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP (*(.preinit_array*)) - PROVIDE_HIDDEN (__preinit_array_end = .); - } >FLASH - .init_array : - { - PROVIDE_HIDDEN (__init_array_start = .); - KEEP (*(SORT(.init_array.*))) - KEEP (*(.init_array*)) - PROVIDE_HIDDEN (__init_array_end = .); - } >FLASH - .fini_array : - { - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP (*(SORT(.fini_array.*))) - KEEP (*(.fini_array*)) - PROVIDE_HIDDEN (__fini_array_end = .); - } >FLASH - - /* used by the startup to initialize data */ - _sidata = LOADADDR(.data); - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { - . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT> FLASH - - - /* Uninitialized data section */ - . = ALIGN(4); - .bss : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(.bss*) - *(COMMON) - - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM - - /* User_heap_stack section, used to check that there is enough RAM left */ - ._user_heap_stack : - { - . = ALIGN(8); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(8); - } >RAM - - .eeprom 0x08008000: - { - . = ALIGN(4); - KEEP(*(.xbowEeprom)) /* keep my variable even if not referenced */ - . = ALIGN(4); - _eeeprom = .; /* define a global symbols at end of code */ - } >EEPROM - - .usereeprom 0x0800C000: - { - . = ALIGN(4); - KEEP(*(.userEeprom)) /* keep my variable even if not referenced */ - . = ALIGN(4); - _eusereeprom = .; /* define a global symbols at end of code */ - } >USER_EEPROM - - /* Remove information from the standard libraries */ - /DISCARD/ : - { - libc.a ( * ) - libm.a ( * ) - libgcc.a ( * ) - } - - .ARM.attributes 0 : { *(.ARM.attributes) } -} diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/algorithmAPI.h b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/algorithmAPI.h index 80b664b..95f0185 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/algorithmAPI.h +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/algorithmAPI.h @@ -36,7 +36,7 @@ limitations under the License. * * @retval N/A *****************************************************************************/ -void InitializeAlgorithmStruct(uint16_t callingFreq); +void InitializeAlgorithmStruct(uint8_t callingFreq); /****************************************************************************** * @brief Get algorithm status. @@ -68,7 +68,23 @@ void enableGpsInAlgorithm(BOOL enable); BOOL gpsUsedInAlgorithm(); -void SetAlgorithmUseDgps(BOOL d); +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 ); diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/AlgorithmLimits.h b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/AlgorithmLimits.h index 0aa7590..6a1262c 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/AlgorithmLimits.h +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/AlgorithmLimits.h @@ -39,12 +39,13 @@ limitations under the License. #define LIMIT_BIAS_RATE_UPDATE_INS 5.0e-4 #define LIMIT_MIN_GPS_VELOCITY_HEADING 0.45 //0.45 m/s ~= 1.0 mph +#define RELIABLE_GPS_VELOCITY_HEADING 1.0 // velocity of 1.0m/s should provide reliable GNSS heading #define LIMIT_OBS_JACOBIAN_DENOM 1e-3; // The following times are compared against ITOW (units in [msec]) -//#define LIMIT_MAX_GPS_DROP_TIME 3 // 3 [sec] -#define LIMIT_MAX_GPS_DROP_TIME 30 // [sec] +#define LIMIT_MAX_GPS_DROP_TIME 300 // [sec] +#define LIMIT_RELIABLE_DR_TIME 10 // [sec] #define LIMIT_MAX_REST_TIME_BEFORE_DROP_TO_AHRS 60000 // 60000 [ msec ] = 60 [ sec ] #define LIMIT_MAX_REST_TIME_BEFORE_HEADING_INVALID 120000 // 120sec, heading drifts much slower than pos #define LIMIT_DECL_EXPIRATION_TIME 60000 // 60,000 [ counts ] = 10 [ min ] diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/EKF_Algorithm.h b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/EKF_Algorithm.h index f6bc5b0..6c0e946 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/EKF_Algorithm.h +++ b/examples/OpenIMU300RI/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" @@ -30,8 +31,6 @@ typedef struct { real rateBias_B[NUM_AXIS]; real accelBias_B[NUM_AXIS]; - real stateUpdate[NUMBER_OF_EKF_STATES]; - // Prediction variables: P = FxPxFTranspose + Q real F[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; real P[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; @@ -42,27 +41,47 @@ typedef struct { * is symmetric, only 6 off-diagonal terms need stored. */ - real correctedRate_B[NUM_AXIS]; - real correctedAccel_B[NUM_AXIS]; - real aCorr_N[NUM_AXIS]; + real correctedRate_B[NUM_AXIS]; // [rad/s] + real correctedAccel_B[NUM_AXIS]; // [m/s/s] + real linearAccel_B[NUM_AXIS]; // [m/s/s], linear acceleration in body frame, used to detect drive position + + /* Algorithm results. Velocity states are directly used as results for output. + * The following two are calculated from state + */ + real eulerAngles[NUM_AXIS]; + double llaDeg[NUM_AXIS]; - real R_BinN[NUM_AXIS][NUM_AXIS]; // convert body to NED - real eulerAngles[NUM_AXIS], measuredEulerAngles[NUM_AXIS]; + // measurements + real R_BinN[3][3]; // convert body to NED + real Rn2e[3][3]; // Coordinate tranformation matrix from NED to ECEF + real measuredEulerAngles[3]; // Euler angles measurements + real rGPS_N[3]; // current IMU position w.r.t rGPS0_E in NED. + double rGPS0_E[3]; // Initial IMU ECEF position when first entering INS state. + double rGPS_E[3]; // current IMU ECEF position // Update variables: S = HxPxHTranspose + R - // DEBUG H is 3x16 or 9x16 (AHRS vs INS) real nu[9]; - real H[3][NUMBER_OF_EKF_STATES]; 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]; - double llaDeg[NUM_AXIS]; -// double Position_E[NUM_AXIS]; - + // 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; @@ -70,27 +89,37 @@ extern KalmanFilterStruct gKalmanFilter; /* Global Algorithm structure */ typedef struct { // Sensor readings in the body-frame (B) - real accel_B[NUM_AXIS]; // [g] - real angRate_B[NUM_AXIS]; // [rad/s] - real magField_B[NUM_AXIS]; // [G] + real accel_B[NUM_AXIS]; // [m/s/s] + real angRate_B[NUM_AXIS]; // [rad/s] + real magField_B[NUM_AXIS]; // [G] // GPS information - BOOL gpsValid; - BOOL gpsUpdate; - - double llaRad[3]; - double vNed[NUM_AXIS]; - - double trueCourse; - double rawGroundSpeed; - uint32_t itow; - - float GPSHorizAcc, GPSVertAcc; + double llaAnt[3]; // Antenna Lat, Lon, ellipsoid Altitude, [rad, rad, meter] + double vNedAnt[NUM_AXIS]; // Antenna NED velocity, [m/s, m/s, m/s] + double lla[3]; // IMU Lat, Lon, ellipsoid Altitude, [rad, rad, meter] + double vNed[3]; // IMU NED velocity, [m/s, m/s, m/s] + float geoidAboveEllipsoid; // [m] + real trueCourse; // Antenna heading, [deg] + real rawGroundSpeed; // IMU ground speed, calculated from vNed, [m/s] + float GPSHorizAcc; // [m] + float GPSVertAcc; // [m] float HDOP; + uint8_t gpsFixType; // Indicate if this GNSS measurement is valid + uint8_t numSatellites; /* Num of satellites in this GNSS measurement. + * 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 gEKFInputData; +extern EKF_InputDataStruct gEKFInput; /* Global Algorithm structure */ @@ -113,21 +142,13 @@ typedef struct { uint8_t opMode; uint8_t turnSwitchFlag; uint8_t linAccelSwitch; - uint8_t gpsMeasurementUpdate; } EKF_OutputDataStruct; -extern EKF_OutputDataStruct gEKFOutputData; +extern EKF_OutputDataStruct gEKFOutput; + -typedef struct -{ - BOOL bValid; // tell if stats are valid - BOOL accelErrLimit; // accelErr is set to max/min limit - real lpfAccel[3]; // low-pass filtered accel - real accelNorm; // magnitude of current accel - real accelMean[3]; // average of past n accel - real accelVar[3]; // variance of past n accel - real accelErr[3]; // estimated accel error -} AccelStatsStruct; +// Initialize Kalman filter parameters of the INS app +uint8_t InitINSFilter(void); void EKF_Algorithm(void); void enableFreeIntegration(BOOL enable); @@ -148,7 +169,9 @@ 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_SetOutputStruct(void); #endif /* _EKF_ALGORITHM_H_ */ diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/MotionStatus.h b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/MotionStatus.h new file mode 100644 index 0000000..785ac68 --- /dev/null +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/MotionStatus.h @@ -0,0 +1,90 @@ +/***************************************************************************** + * @file MotionStatus.h + * @brief Calculate sensor stats, and detect motion status using IMU/ODO/GNSS + * @author Dong Xiaoguang + * @version 1.0 + * @date 20190801 + *****************************************************************************/ + +#ifndef MOTION_STATUS_H_INCLUDED +#define MOTION_STATUS_H_INCLUDED + +#include +#include "GlobalConstants.h" +#include "algorithm.h" + + +typedef struct +{ + BOOL bValid; // tell if stats are valid + BOOL bStaticIMU; // Static period detected by IMU + BOOL accelErrLimit; // accelErr is set to max/min limit + real lpfAccel[3]; // [m/s/s], low-pass filtered accel + real accelNorm; // [m/s/s], magnitude of current accel + real accelMean[3]; // [m/s/s], average of past n accel samples + real accelVar[3]; // [m/s/s]^2, variance of past n accel samples + real accelErr[3]; // [m/s/s], estimated accel error + real lpfGyro[3]; // [rad/s], low-pass filtered gyro + real gyroMean[3]; // [rad/s], average of past n gyro samples + real gyroVar[3]; // [rad/s]^2, variance of past n gyro samples +} ImuStatsStruct; + +/****************************************************************************** + * @brief Calculate IMU data stats, and detect zero velocity. + * TRACE: + * @param [in] gyro [rad/s] + * @param [in] accel [m/s/s] + * @param [in] reset TRUE to reset this process + * @param [Out] imuStats results + * @retval None. +******************************************************************************/ +void MotionStatusImu(real *gyro, real *accel, ImuStatsStruct *imuStats, BOOL reset); + +/****************************************************************************** + * @brief Using gyro propagation to estimate accel error. + * g_dot = -cross(w, g), g is gravity and w is angular rate. + * TRACE: + * @param [in] accel Input accel, m/s/s. + * @param [in] w Input angular rate, rad/s. + * @param [in] dt Sampling interval, sec. + * @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 [out] gAccelStats A struct for results storage. + * @retval None. +******************************************************************************/ +void EstimateAccelError(real *accel, real *w, real dt, uint32_t staticDelay, ImuStatsStruct *gAccelStats); + +/****************************************************************************** + * @brief Detect motion according to the difference between measured accel magnitude and 1g. + * Set gAlgorithm.linAccelSwitch to be True if being static for a while. + * This result is no longer used in current algorithm. + * TRACE: + * @param [in] accelNorm Input accel magnitude, g. + * @param [in] iReset Reset the procedure. + * @retval Always true. +******************************************************************************/ +BOOL DetectMotionFromAccel(real accelNorm, int iReset); + +/****************************************************************************** + * @brief Detect zero velocity using GNSS speed. + * TRACE: + * @param [in] vNED NED velocity measured by GNSS [m/s] + * @param [in] gnssValid Indicate if GNSS measurement is valid. + * If valid, vNED will be used to detect zero velocity. + * If not, detection will be reset and FALSE is always + * returned. + * @retval TRUE if static, otherwise FALSE. +******************************************************************************/ +BOOL DetectStaticGnssVelocity(double *vNED, real threshold, BOOL gnssValid); + +/****************************************************************************** + * @brief Detect zero velocity using odometer data. + * TRACE: + * @param [in] odo velocity measured by odometer [m/s] + * @retval TRUE if static, otherwise FALSE. +******************************************************************************/ +BOOL DetectStaticOdo(real odo); + +#endif // MOTION_STATUS_H_INCLUDED + diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/SensorNoiseParameters.h b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/SensorNoiseParameters.h index 462161a..5b509d2 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/SensorNoiseParameters.h +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/SensorNoiseParameters.h @@ -10,13 +10,30 @@ #include "GlobalConstants.h" - // Add 25% uncertainty to the noise parameter -#define SENSOR_NOISE_RATE_STD_DEV (1.25 * 1e-1 * D2R) -#define SENSOR_NOISE_ACCEL_STD_DEV 1.25 * 7.0e-4 * GRAVITY -#define SENSOR_NOISE_MAG_STD_DEV 0.00 // todo tm20160603 - ?check this value ? - -#define R_VALS_GPS_TRACK 0.1 - +// IMU spec +#define RW_ODR 100.0 /* [Hz], The data sampling rate when calculate ARW and VRW. + * ARW = sigma * sqrt(dt) = sigma * sqrt(1/ODR) + */ +#define ARW_300ZA 8.73e-5 /* [rad/sqrt(s)], gyro angular random walk, sampled at 100Hz + * 0.3deg/sqrt(Hr) = 0.3 / 60 * D2R = 8.72664625997165e-05rad/sqrt(s) + */ +#define BIW_300ZA 2.91e-5 /* [rad/s], gyro bias instability + * 6.0deg/Hr = 6.0 / 3600 * D2R = 2.90888208665722e-05rad/s + */ +#define MAX_BW 8.73e-3 /* [rad/s], max possible gyro bias + * 0.5deg/s = 0.5 * D2R = 0.00872664625997165rad/s + */ +#define VRW_300ZA 1.0e-3 /* [m/s/sqrt(s)], accel velocity random walk, sampled at 100Hz + * 0.06m/s/sqrt(Hr) = 0.06 / 60 = 0.001m/s/sqrt(s) + */ +#define BIA_300ZA 10.0e-6 * GRAVITY /* [m/s/s], accel bias instability + * 10ug = 10.0e-6g * GRAVITY + */ +#define MAX_BA 3.0e-3 * GRAVITY /* [m/s/s], max possible accel bias + * 3mg = 3.0e-3g * GRAVITY + */ + +// GNSS spec #define R_VALS_GPS_POS_X 5.0 #define R_VALS_GPS_POS_Y 5.0 #define R_VALS_GPS_POS_Z 7.5 diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/TimingVars.h b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/TimingVars.h index 1799cbc..5f545f0 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/TimingVars.h +++ b/examples/OpenIMU300RI/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/OpenIMU300RI/IMU/lib/Core/Algorithm/include/algorithm.h b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/algorithm.h index 47e11f3..8b92c36 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/algorithm.h +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/include/algorithm.h @@ -47,6 +47,12 @@ limitations under the License. #define HIGH_GAIN_AHRS_DURATION 30.0 // 60.0 * SAMPLING_RATE #define LOW_GAIN_AHRS_DURATION 30.0 // 30.0 * SAMPLING_RATE +// Define heading initialization reliability +#define HEADING_UNINITIALIZED 0 +#define HEADING_MAG 1 +#define HEADING_GNSS_LOW 2 +#define HEADING_GNSS_HIGH 3 + typedef struct { uint32_t Stabilize_System; // SAMPLING_RATE * 0.36 uint32_t Initialize_Attitude; // SAMPLING_RATE * ( 1.0 - 0.36 ) @@ -61,13 +67,16 @@ typedef struct { } InnovationStruct; typedef struct { - int32_t Max_GPS_Drop_Time; // [msec] + int32_t maxGpsDropTime; // [msec] + int32_t maxReliableDRTime; /* [msec] When GPS outage duration exceeds this limit, + * the position and velocity will be reinitialized when GPS + * is available again. Otherwise, the fusion algorithm will + * gradually correct the position and velocity to the GPS. + */ int32_t Max_Rest_Time_Before_Drop_To_AHRS; // [msec] 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; @@ -76,14 +85,16 @@ typedef struct { } 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 @@ -95,15 +106,25 @@ union AlgoBehavior // Algorithm states struct ALGO_STATUS_BITS { - uint16_t algorithmInit : 1; // 0 algorithm initialization - uint16_t highGain : 1; // 1 high gain mode + uint16_t algorithmInit : 1; // 0 algorithm initialization + 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 turnSwitch : 1; // 3 turn switch + 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 @@ -114,57 +135,92 @@ typedef union ALGO_STATUS extern AlgoStatus gAlgoStatus; +typedef struct +{ + real arw; // [rad/sqrt(s)], gyro angle random walk + real sigmaW; // [rad/s], gyro noise std + real biW; // [rad/s], gyro bias instability + real maxBiasW; // [rad/s], max possible gyro bias + real vrw; // [m/s/sqrt(s)], accel velocity random walk + real sigmaA; // [m/s/s], accel noise std + real biA; // [m/s/s], accel bias instability + real maxBiasA; // [m/s/s], max possible accel bias +} IMU_SPEC; + +typedef struct +{ + real staticVarGyro; // [rad/s]^2 + real staticVarAccel; // [m/s/s]^2 + real maxGyroBias; // [rad/s] + real staticGnssVel; // [m/s] + real staticNoiseMultiplier[3]; /* Use IMU noise level and gyro output to detect static period. + * The nominal noise level and max gyro bias of an IMU is defined in + * SensorNoiseParameters.h. These parameters are determined by IMU + * output when static and are hightly related to ARW and VRW. + * When IMU is installed on a vehicle, its noise level when + * vehicle is static could be higher than the nominal noise + * level due to vibration. This setting is used to scale + * the nominal noise level and gyro bias for static detection. + * [scale_gyro_var, scale_accel_var, scale_gyro_bias] + */ +} STATIC_DETECT_SETTING; + + /* Global Algorithm structure */ typedef struct { - uint32_t itow; - uint32_t dITOW; + uint32_t itow; + uint32_t dITOW; + + // control the stage of operation for the algorithms + uint32_t stateTimer; + uint8_t state; // takes values from HARDWARE_STABILIZE to INIT_ATTITUDE to HG_AHRS - // control the stage of operation for the algorithms - uint32_t stateTimer; - uint8_t state; // takes values from HARDWARE_STABILIZE to INIT_ATTITUDE to HG_AHRS + uint8_t insFirstTime; + uint8_t headingIni; + uint8_t applyDeclFlag; - uint8_t insFirstTime; - uint8_t gnssHeadingFirstTime; - uint8_t applyDeclFlag; + int32_t timeOfLastSufficientGPSVelocity; + int32_t timeOfLastGoodGPSReading; - int32_t timeOfLastSufficientGPSVelocity; - int32_t timeOfLastGoodGPSReading; + real filteredYawRate; // Yaw-Rate (Turn-Switch) filter - real rGPS_N[3]; // current IMU position (lever-arm) w.r.t rGPS0_E in NED. - real R_NinE[3][3]; // convert NED to ECEF - double rGPS0_E[3]; // Initial antenna ECEF position when first entering INS state - double rGPS_E[3]; // current antenna ECEF position + /* The following variables are used to increase the Kalman filter gain when the + * acceleration is very close to one (i.e. the system is at rest) + */ + uint32_t linAccelSwitchCntr; + uint8_t linAccelSwitch; - real filteredYawRate; // Yaw-Rate (Turn-Switch) filter + uint8_t linAccelLPFType; + uint8_t useRawAccToDetectLinAccel; - /* The following variables are used to increase the Kalman filter gain when the - * acceleration is very close to one (i.e. the system is at rest) - */ - uint32_t linAccelSwitchCntr; - uint8_t linAccelSwitch; + uint8_t callingFreq; + real dt; + real dtOverTwo; + real dtSquared; + real sqrtDt; - uint8_t linAccelLPFType; - uint8_t useRawAccToDetectLinAccel; + volatile uint32_t timer; // timer since power up (ms) + volatile uint16_t counter; // inc. with every continuous mode output packet - uint8_t callingFreq; - real dt; - real dtOverTwo; - real dtSquared; - real sqrtDt; + union AlgoBehavior Behavior; + float turnSwitchThreshold; // 0, 0.4, 10 driving, 1 flying [deg/sec] 0x000d - volatile uint32_t timer; // timer since power up (ms) - volatile uint16_t counter; // inc. with every continuous mode output packet + real leverArmB[3]; // Antenna position w.r.t IMU in vehicle body frame + real pointOfInterestB[3]; // Point of interest position w.r.t IMU in vehicle body frame - union AlgoBehavior Behavior; - float turnSwitchThreshold; // 0, 0.4, 10 driving, 1 flying [deg/sec] 0x000d + BOOL velocityAlwaysAlongBodyX; // enable zero velocity update - real leverArmB[3]; // Antenna position w.r.t IMU in vehicle body frame - real pointOfInterestB[3]; // Point of interest position w.r.t IMU in vehicle body frame + IMU_SPEC imuSpec; // IMU specifications + STATIC_DETECT_SETTING staticDetectParam; // params used for static detection - DurationStruct Duration; - LimitStruct Limit; + DurationStruct Duration; + LimitStruct Limit; } 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/OpenIMU300RI/IMU/lib/Core/Algorithm/src/EKF_Algorithm.c b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/EKF_Algorithm.c index c3faadf..8e79f6b 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/EKF_Algorithm.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/EKF_Algorithm.c @@ -7,17 +7,19 @@ #include // std::abs #include // EXIT_FAILURE #include +#include // memset #include "GlobalConstants.h" // TRUE, FALSE, etc #include "platformAPI.h" #include "Indices.h" // IND #include "MagAlign.h" +#include "QuaternionMath.h" #include "algorithm.h" // gAlgorithm #include "AlgorithmLimits.h" #include "TransformationMath.h" #include "VectorMath.h" -#include "buffer.h" +#include "MotionStatus.h" #include "SelectState.h" #include "EKF_Algorithm.h" #include "PredictFunctions.h" @@ -31,88 +33,36 @@ #endif #endif -KalmanFilterStruct gKalmanFilter; -EKF_InputDataStruct gEKFInputData; -EKF_OutputDataStruct gEKFOutputData; -AccelStatsStruct gAccelStats; - -//============================================================================= -// Filter variables (Third-Order BWF w/ default 5 Hz Cutoff) -#define FILTER_ORDER 3 - -#define CURRENT 0 -#define PASTx1 1 -#define PASTx2 2 -#define PASTx3 3 -/* Replace this with a fuction that will compute the coefficients so the - * input is the cutoff frequency in Hertz - */ -#define NO_LPF 0 -#define TWO_HZ_LPF 1 -#define FIVE_HZ_LPF 2 -#define TEN_HZ_LPF 3 -#define TWENTY_HZ_LPF 4 -#define TWENTY_FIVE_HZ_LPF 5 -#define N_LPF 6 - -/****************************************************************************** - * @brief Get filter coefficients of a 3rd Butterworth low-pass filter. - * For now only a few specific cut-off frequencies are supported. - * TRACE: - * @param [in] lpfType Low-pass filter cut-off frequency. - * @param [in] callingFreq Sampling frequency, only 100Hz and 200Hz are supported. - * @param [out] b coefficients of the numerator of the filter. - * @param [out] a coefficients of the denominator of the filter. - * @retval None. -******************************************************************************/ -static void _PopulateFilterCoefficients( uint8_t lpfType, uint8_t callingFreq, real *b, real *a ); - -/****************************************************************************** - * @brief Process input data through a low-pass Butterworth filter. - * TRACE: - * @param [in] accel Input accel - * @param [in] lpfType Low-pass filter cut-off frequency. - * @param [in] callingFreq Sampling frequency, only 100Hz and 200Hz are supported. - * @param [out] filteredAccel Filter accel. - * @retval None. -******************************************************************************/ -static void _AccelLPF( real *accel, uint8_t lpfType, uint8_t callingFreq, real *filteredAccel ); - -#define DATA_NUM_FOR_STATS 20 // 20 samples can give a relative good estimate of var -/****************************************************************************** - * @brief Compute mean and var of the input data. - * A recursive mean calculation and a naive var calculation are implemented. A recursive var - * calculation was also tested but abandoned due to its inherent numerical stability. - * TRACE: - * @param [in] accel Input accel, g. - * @param [in] iReset Reset the procedure. - * @param [out] gAccelStats A struct for results storage. - * @retval None. -******************************************************************************/ -static void ComputeAccelStats( real *accel, AccelStatsStruct *gAccelStats, int iReset ); +KalmanFilterStruct gKalmanFilter; +EKF_InputDataStruct gEKFInput; +EKF_OutputDataStruct gEKFOutput; +ImuStatsStruct gImuStats; /****************************************************************************** - * @brief Using gyro propagation to estimate accel error. - * g_dot = -cross(w, g), g is gravity and w is angular rate. + * @brief Remove lever arm in position and velocity. + * GNSS measured position is the position of the antenna. For GNSS/INS integration, + * the position of IMU is needed. Before using the GNSS position and velocity, + * those shold be first converted to the IMU position and velocity by removing + * lever arm effects. Lever arm introduces an offset in position. This offset + * can be directly canceled by substracting lever arm. Combined with angular + * velocity, lever arm further introduces relative velocity. * TRACE: - * @param [in] accel Input accel, g. - * @param [in] w Input angular rate, rad/s. - * @param [in] dt Sampling interval, sec. - * @param [out] gAccelStats A struct for results storage. - * @retval None. + * @param [in/out] LLA [rad, rad, m], Lat, lon and alt of the antenna, and + * will be converted to LLA of the IMU. + * @param [in/out] vNed [m/s], NED velocity of the antenna, and will be + * converted to NED velocity of the IMU. + * @param [in] w [rad/s], angular velocity of the vehicle relative to + * ECEF in the body frame. + * @param [in] leverArmB [m], lever arm in the body frame. + * @param [out] rn2e Transformation matrix from NED to ECEF. + * @param [out] ecef [m], ECEF position without lever arm. + * @retval TRUE if heading initialized, FALSE if not. ******************************************************************************/ -static void EstimateAccelError( real *accel, real *w, real dt, AccelStatsStruct *gAccelStats); +static void RemoveLeverArm(double *lla, double *vNed, real *w, real *leverArmB, + real *rn2e, double *ecef); -/****************************************************************************** - * @brief Detect motion according to the difference between measured accel magnitude and 1g. - * Set gAlgorithm.linAccelSwitch to be True if being static for a while. - * This result is no longer used in current algorithm. - * TRACE: - * @param [in] accelNorm Input accel magnitude, g. - * @param [in] iReset Reset the procedure. - * @retval Always true. -******************************************************************************/ -static BOOL DetectMotionFromAccel(real accelNorm, int iReset); +static void HandlePps(); +static void SaveKfStateAtPps(); //============================================================================= @@ -131,39 +81,40 @@ void EKF_Algorithm(void) */ if ( gAlgorithm.state > STABILIZE_SYSTEM ) { - // Low-pass filter - _AccelLPF( gEKFInputData.accel_B, - gAlgorithm.linAccelLPFType, - gAlgorithm.callingFreq, - gAccelStats.lpfAccel ); - // Compute accel norm using raw accel data. The norm will be used to detect static periods. - gAccelStats.accelNorm = sqrtf(gEKFInputData.accel_B[X_AXIS] * gEKFInputData.accel_B[X_AXIS] + - gEKFInputData.accel_B[Y_AXIS] * gEKFInputData.accel_B[Y_AXIS] + - gEKFInputData.accel_B[Z_AXIS] * gEKFInputData.accel_B[Z_AXIS] ); - /* Compute accel mean/var. - * Compute this after STABILIZE_SYSTEM so there is enough data to fill the buffer - * Before buffer is full, results are not accurate and not used indeed. + /* Compute IMU mean/var, filter IMU data (optional), detect static. + * After STABILIZE_SYSTEM, each IMU sample is pushed into a buffer. + * Before the buffer is full, results are not accurate should not be used. */ - ComputeAccelStats(gAccelStats.lpfAccel, &gAccelStats, 0); + MotionStatusImu(gEKFInput.angRate_B, gEKFInput.accel_B, &gImuStats, FALSE); // estimate accel error if (gAlgorithm.useRawAccToDetectLinAccel) { - EstimateAccelError( gEKFInputData.accel_B, - gEKFInputData.angRate_B, + EstimateAccelError( gEKFInput.accel_B, + gEKFInput.angRate_B, gAlgorithm.dt, - &gAccelStats); + gAlgorithm.Limit.linAccelSwitchDelay, + &gImuStats); } else { - EstimateAccelError( gAccelStats.lpfAccel, - gEKFInputData.angRate_B, + EstimateAccelError( gImuStats.lpfAccel, + gEKFInput.angRate_B, gAlgorithm.dt, - &gAccelStats); + gAlgorithm.Limit.linAccelSwitchDelay, + &gImuStats); } // Detect if the unit is static or dynamic - DetectMotionFromAccel(gAccelStats.accelNorm, 0); + 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 @@ -173,7 +124,12 @@ void EKF_Algorithm(void) gAlgorithm.itow = gAlgorithm.itow + gAlgorithm.dITOW; // Perform EKF Prediction - EKF_PredictionStage(gAccelStats.lpfAccel); + 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. @@ -207,6 +163,7 @@ void EKF_Algorithm(void) // Perform EKF Update EKF_UpdateStage(); } + /* Save the past attitude quaternion before updating (for use in the * covariance estimation calculations). */ @@ -214,6 +171,41 @@ void EKF_Algorithm(void) gKalmanFilter.quaternion_Past[Q1] = gKalmanFilter.quaternion[Q1]; gKalmanFilter.quaternion_Past[Q2] = gKalmanFilter.quaternion[Q2]; gKalmanFilter.quaternion_Past[Q3] = gKalmanFilter.quaternion[Q3]; + + /* Generate the transformation matrix (R_BinN) based on the past value of + * the attitude quaternion (prior to prediction at the new time-step) + */ + QuaternionToR321(gKalmanFilter.quaternion, &gKalmanFilter.R_BinN[0][0]); + + /* Euler angels are not calcualted here because it is used as a propagation resutls + * to calculate system innovation. So, Euler angles are updated in the prediction + * stage. In theory, Euler angles should be updated after each measurement update. + * However, after EKF converges, it does not matter. + */ + + // Update LLA + if ((gAlgorithm.insFirstTime == FALSE)) + { + double r_E[NUM_AXIS]; + float pointOfInterestN[3]; + pointOfInterestN[0] = gKalmanFilter.R_BinN[0][0] * gAlgorithm.pointOfInterestB[0] + + gKalmanFilter.R_BinN[0][1] * gAlgorithm.pointOfInterestB[1] + + gKalmanFilter.R_BinN[0][2] * gAlgorithm.pointOfInterestB[2]; + pointOfInterestN[1] = gKalmanFilter.R_BinN[1][0] * gAlgorithm.pointOfInterestB[0] + + gKalmanFilter.R_BinN[1][1] * gAlgorithm.pointOfInterestB[1] + + gKalmanFilter.R_BinN[1][2] * gAlgorithm.pointOfInterestB[2]; + pointOfInterestN[2] = gKalmanFilter.R_BinN[2][0] * gAlgorithm.pointOfInterestB[0] + + gKalmanFilter.R_BinN[2][1] * gAlgorithm.pointOfInterestB[1] + + gKalmanFilter.R_BinN[2][2] * gAlgorithm.pointOfInterestB[2]; + pointOfInterestN[0] += gKalmanFilter.Position_N[0]; + pointOfInterestN[1] += gKalmanFilter.Position_N[1]; + pointOfInterestN[2] += gKalmanFilter.Position_N[2]; + PosNED_To_PosECEF(pointOfInterestN, &gKalmanFilter.rGPS0_E[0], &gKalmanFilter.Rn2e[0][0], &r_E[0]); + // 100 Hz generated once 1 Hz 100 Hz + // output variable (ned used for anything else); this is in [ deg, deg, m ] + ECEF_To_LLA(&gKalmanFilter.llaDeg[LAT], &r_E[X_AXIS]); + // 100 Hz 100 Hz + } } /* Select the algorithm state based upon the present state as well as @@ -239,14 +231,6 @@ void EKF_Algorithm(void) INS_To_AHRS_Transition_Test(); break; default: -#ifdef DISPLAY_DIAGNOSTIC_MSG -#ifdef INS_OFFLINE - // Shouldn't be able to make it here - TimingVars_DiagnosticMsg("Uh-oh! Invalid algorithm state in EKF_Algorithm.cpp"); - std::cout << "Press enter to finish ..."; - std::cin.get(); -#endif -#endif return; } @@ -254,46 +238,67 @@ void EKF_Algorithm(void) DynamicMotion(); } -void enableFreeIntegration(BOOL enable) -{ - gAlgorithm.Behavior.bit.freeIntegrate = enable; -} - - -BOOL freeIntegrationEnabled() +static void HandlePps() { - return 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 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() /* Extract the attitude (expressed as Euler-angles) of the body-frame (B) @@ -302,9 +307,9 @@ void enableGpsInAlgorithm(BOOL enable) void EKF_GetAttitude_EA(real *EulerAngles) { // Euler-angles in [deg] - EulerAngles[ROLL] = (real)gEKFOutputData.eulerAngs_BinN[ROLL]; - EulerAngles[PITCH] = (real)gEKFOutputData.eulerAngs_BinN[PITCH]; - EulerAngles[YAW] = (real)gEKFOutputData.eulerAngs_BinN[YAW]; + EulerAngles[ROLL] = (real)gEKFOutput.eulerAngs_BinN[ROLL]; + EulerAngles[PITCH] = (real)gEKFOutput.eulerAngs_BinN[PITCH]; + EulerAngles[YAW] = (real)gEKFOutput.eulerAngs_BinN[YAW]; } @@ -322,10 +327,10 @@ void EKF_GetAttitude_EA_RAD(real *EulerAngles) */ void EKF_GetAttitude_Q(real *Quaternions) { - Quaternions[Q0] = (real)gEKFOutputData.quaternion_BinN[Q0]; - Quaternions[Q1] = (real)gEKFOutputData.quaternion_BinN[Q1]; - Quaternions[Q2] = (real)gEKFOutputData.quaternion_BinN[Q2]; - Quaternions[Q3] = (real)gEKFOutputData.quaternion_BinN[Q3]; + Quaternions[Q0] = (real)gEKFOutput.quaternion_BinN[Q0]; + Quaternions[Q1] = (real)gEKFOutput.quaternion_BinN[Q1]; + Quaternions[Q2] = (real)gEKFOutput.quaternion_BinN[Q2]; + Quaternions[Q3] = (real)gEKFOutput.quaternion_BinN[Q3]; } @@ -335,9 +340,9 @@ void EKF_GetAttitude_Q(real *Quaternions) void EKF_GetCorrectedAngRates(real *CorrAngRates_B) { // Angular-rate in [deg/s] - CorrAngRates_B[X_AXIS] = (real)gEKFOutputData.corrAngRates_B[X_AXIS]; - CorrAngRates_B[Y_AXIS] = (real)gEKFOutputData.corrAngRates_B[Y_AXIS]; - CorrAngRates_B[Z_AXIS] = (real)gEKFOutputData.corrAngRates_B[Z_AXIS]; + CorrAngRates_B[X_AXIS] = (real)gEKFOutput.corrAngRates_B[X_AXIS]; + CorrAngRates_B[Y_AXIS] = (real)gEKFOutput.corrAngRates_B[Y_AXIS]; + CorrAngRates_B[Z_AXIS] = (real)gEKFOutput.corrAngRates_B[Z_AXIS]; } @@ -347,9 +352,9 @@ void EKF_GetCorrectedAngRates(real *CorrAngRates_B) void EKF_GetCorrectedAccels(real *CorrAccels_B) { // Acceleration in [m/s^2] - CorrAccels_B[X_AXIS] = (real)gEKFOutputData.corrAccel_B[X_AXIS]; - CorrAccels_B[Y_AXIS] = (real)gEKFOutputData.corrAccel_B[Y_AXIS]; - CorrAccels_B[Z_AXIS] = (real)gEKFOutputData.corrAccel_B[Z_AXIS]; + CorrAccels_B[X_AXIS] = (real)gEKFOutput.corrAccel_B[X_AXIS]; + CorrAccels_B[Y_AXIS] = (real)gEKFOutput.corrAccel_B[Y_AXIS]; + CorrAccels_B[Z_AXIS] = (real)gEKFOutput.corrAccel_B[Z_AXIS]; } @@ -359,9 +364,9 @@ void EKF_GetCorrectedAccels(real *CorrAccels_B) void EKF_GetEstimatedAngRateBias(real *AngRateBias_B) { // Angular-rate bias in [deg/sec] - AngRateBias_B[X_AXIS] = (real)gEKFOutputData.angRateBias_B[X_AXIS]; - AngRateBias_B[Y_AXIS] = (real)gEKFOutputData.angRateBias_B[Y_AXIS]; - AngRateBias_B[Z_AXIS] = (real)gEKFOutputData.angRateBias_B[Z_AXIS]; + AngRateBias_B[X_AXIS] = (real)gEKFOutput.angRateBias_B[X_AXIS]; + AngRateBias_B[Y_AXIS] = (real)gEKFOutput.angRateBias_B[Y_AXIS]; + AngRateBias_B[Z_AXIS] = (real)gEKFOutput.angRateBias_B[Z_AXIS]; } @@ -371,9 +376,9 @@ void EKF_GetEstimatedAngRateBias(real *AngRateBias_B) void EKF_GetEstimatedAccelBias(real *AccelBias_B) { // Acceleration-bias in [m/s^2] - AccelBias_B[X_AXIS] = (real)gEKFOutputData.accelBias_B[X_AXIS]; - AccelBias_B[Y_AXIS] = (real)gEKFOutputData.accelBias_B[Y_AXIS]; - AccelBias_B[Z_AXIS] = (real)gEKFOutputData.accelBias_B[Z_AXIS]; + AccelBias_B[X_AXIS] = (real)gEKFOutput.accelBias_B[X_AXIS]; + AccelBias_B[Y_AXIS] = (real)gEKFOutput.accelBias_B[Y_AXIS]; + AccelBias_B[Z_AXIS] = (real)gEKFOutput.accelBias_B[Z_AXIS]; } @@ -381,9 +386,9 @@ void EKF_GetEstimatedAccelBias(real *AccelBias_B) void EKF_GetEstimatedPosition(real *Position_N) { // Position in [m] - Position_N[X_AXIS] = (real)gEKFOutputData.position_N[X_AXIS]; - Position_N[Y_AXIS] = (real)gEKFOutputData.position_N[Y_AXIS]; - Position_N[Z_AXIS] = (real)gEKFOutputData.position_N[Z_AXIS]; + Position_N[X_AXIS] = (real)gEKFOutput.position_N[X_AXIS]; + Position_N[Y_AXIS] = (real)gEKFOutput.position_N[Y_AXIS]; + Position_N[Z_AXIS] = (real)gEKFOutput.position_N[Z_AXIS]; } @@ -391,9 +396,9 @@ void EKF_GetEstimatedPosition(real *Position_N) void EKF_GetEstimatedVelocity(real *Velocity_N) { // Velocity in [m/s] - Velocity_N[X_AXIS] = (real)gEKFOutputData.velocity_N[X_AXIS]; - Velocity_N[Y_AXIS] = (real)gEKFOutputData.velocity_N[Y_AXIS]; - Velocity_N[Z_AXIS] = (real)gEKFOutputData.velocity_N[Z_AXIS]; + Velocity_N[X_AXIS] = (real)gEKFOutput.velocity_N[X_AXIS]; + Velocity_N[Y_AXIS] = (real)gEKFOutput.velocity_N[Y_AXIS]; + Velocity_N[Z_AXIS] = (real)gEKFOutput.velocity_N[Z_AXIS]; } @@ -401,9 +406,9 @@ void EKF_GetEstimatedVelocity(real *Velocity_N) void EKF_GetEstimatedLLA(double *LLA) { // Velocity in [m/s] - LLA[X_AXIS] = (double)gEKFOutputData.llaDeg[X_AXIS]; - LLA[Y_AXIS] = (double)gEKFOutputData.llaDeg[Y_AXIS]; - LLA[Z_AXIS] = (double)gEKFOutputData.llaDeg[Z_AXIS]; + LLA[X_AXIS] = (double)gEKFOutput.llaDeg[X_AXIS]; + LLA[Y_AXIS] = (double)gEKFOutput.llaDeg[Y_AXIS]; + LLA[Z_AXIS] = (double)gEKFOutput.llaDeg[Z_AXIS]; } @@ -416,69 +421,123 @@ void EKF_GetEstimatedLLA(double *LLA) */ void EKF_GetOperationalMode(uint8_t *EKF_OperMode) { - *EKF_OperMode = gEKFOutputData.opMode; + *EKF_OperMode = gEKFOutput.opMode; } // Extract the linear-acceleration and turn-switch flags void EKF_GetOperationalSwitches(uint8_t *EKF_LinAccelSwitch, uint8_t *EKF_TurnSwitch) { - *EKF_LinAccelSwitch = gEKFOutputData.linAccelSwitch; - *EKF_TurnSwitch = gEKFOutputData.turnSwitchFlag; + *EKF_LinAccelSwitch = gEKFOutput.linAccelSwitch; + *EKF_TurnSwitch = gEKFOutput.turnSwitchFlag; } // 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 [g] - gEKFInputData.accel_B[X_AXIS] = (real)accels[X_AXIS]; - gEKFInputData.accel_B[Y_AXIS] = (real)accels[Y_AXIS]; - gEKFInputData.accel_B[Z_AXIS] = (real)accels[Z_AXIS]; + // Accelerometer signal is in [m/s/s] + gEKFInput.accel_B[X_AXIS] = (real)accels[X_AXIS] * GRAVITY; + gEKFInput.accel_B[Y_AXIS] = (real)accels[Y_AXIS] * GRAVITY; + gEKFInput.accel_B[Z_AXIS] = (real)accels[Z_AXIS] * GRAVITY; // Angular-rate signal is in [rad/s] - gEKFInputData.angRate_B[X_AXIS] = (real)rates[X_AXIS]; - gEKFInputData.angRate_B[Y_AXIS] = (real)rates[Y_AXIS]; - gEKFInputData.angRate_B[Z_AXIS] = (real)rates[Z_AXIS]; + gEKFInput.angRate_B[X_AXIS] = (real)rates[X_AXIS]; + gEKFInput.angRate_B[Y_AXIS] = (real)rates[Y_AXIS]; + gEKFInput.angRate_B[Z_AXIS] = (real)rates[Z_AXIS]; // Magnetometer signal is in [G] - gEKFInputData.magField_B[X_AXIS] = (real)mags[X_AXIS]; - gEKFInputData.magField_B[Y_AXIS] = (real)mags[Y_AXIS]; - gEKFInputData.magField_B[Z_AXIS] = (real)mags[Z_AXIS]; + gEKFInput.magField_B[X_AXIS] = (real)mags[X_AXIS]; + gEKFInput.magField_B[Y_AXIS] = (real)mags[Y_AXIS]; + gEKFInput.magField_B[Z_AXIS] = (real)mags[Z_AXIS]; real tmp[2]; - tmp[X_AXIS] = gEKFInputData.magField_B[X_AXIS] - gMagAlign.hardIronBias[X_AXIS]; - tmp[Y_AXIS] = gEKFInputData.magField_B[Y_AXIS] - gMagAlign.hardIronBias[Y_AXIS]; - gEKFInputData.magField_B[X_AXIS] = gMagAlign.SF[0] * tmp[X_AXIS] + gMagAlign.SF[1] * tmp[Y_AXIS]; - gEKFInputData.magField_B[Y_AXIS] = gMagAlign.SF[2] * tmp[X_AXIS] + gMagAlign.SF[3] * tmp[Y_AXIS]; + tmp[X_AXIS] = gEKFInput.magField_B[X_AXIS] - gMagAlign.hardIronBias[X_AXIS]; + tmp[Y_AXIS] = gEKFInput.magField_B[Y_AXIS] - gMagAlign.hardIronBias[Y_AXIS]; + gEKFInput.magField_B[X_AXIS] = gMagAlign.SF[0] * tmp[X_AXIS] + gMagAlign.SF[1] * tmp[Y_AXIS]; + gEKFInput.magField_B[Y_AXIS] = gMagAlign.SF[2] * tmp[X_AXIS] + gMagAlign.SF[3] * tmp[Y_AXIS]; // ----- Input from the GPS goes here ----- - // Validity data - gEKFInputData.gpsValid = (BOOL)gps->gpsValid; - gEKFInputData.gpsUpdate = gps->gpsUpdate; - - // Lat/Lon/Alt data - gEKFInputData.llaRad[LAT] = gps->latitude * D2R; - gEKFInputData.llaRad[LON] = gps->longitude * D2R; - gEKFInputData.llaRad[ALT] = gps->altitude; - - // Velocity data - gEKFInputData.vNed[X_AXIS] = gps->vNed[X_AXIS]; - gEKFInputData.vNed[Y_AXIS] = gps->vNed[Y_AXIS]; - gEKFInputData.vNed[Z_AXIS] = gps->vNed[Z_AXIS]; - - // Course and velocity data - gEKFInputData.rawGroundSpeed = gps->rawGroundSpeed; - gEKFInputData.trueCourse = gps->trueCourse; - - // ITOW data - gEKFInputData.itow = gps->itow; - - // Data quality measures - gEKFInputData.GPSHorizAcc = gps->GPSHorizAcc; - gEKFInputData.GPSVertAcc = gps->GPSVertAcc; - gEKFInputData.HDOP = gps->HDOP; + gEKFInput.gpsUpdate = gps->gpsUpdate; + + if (gEKFInput.gpsUpdate) + { + // Validity data + gEKFInput.gpsFixType = gps->gpsFixType; + + // num of satellites + gEKFInput.numSatellites = gps->numSatellites; + + // ITOW data + gEKFInput.itow = gps->itow; + + // Data quality measures + gEKFInput.GPSHorizAcc = gps->GPSHorizAcc; + gEKFInput.GPSVertAcc = gps->GPSVertAcc; + gEKFInput.HDOP = gps->HDOP; + + // Lat/Lon/Alt data + gEKFInput.llaAnt[LAT] = gps->latitude * D2R; + gEKFInput.llaAnt[LON] = gps->longitude * D2R; + gEKFInput.llaAnt[ALT] = gps->altitude; + gEKFInput.geoidAboveEllipsoid = gps->geoidAboveEllipsoid; + + // Velocity data + gEKFInput.vNedAnt[X_AXIS] = gps->vNed[X_AXIS]; + gEKFInput.vNedAnt[Y_AXIS] = gps->vNed[Y_AXIS]; + gEKFInput.vNedAnt[Z_AXIS] = gps->vNed[Z_AXIS]; + + // Course and velocity data + 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 + * from the body frame to the NED frame. Before heading initialized, lever arm cannot + * be correctly removed. After heading initialized, there would be position jump if + * initial heading is different from uninitlized one and the lever arm is large. + * After heading intialized, the position/velocity could also be reinitialized, and + * lever arm effects on the position/velocity are not corrected removed. + * LLA without lever arm is used to update Rn2e/ECEF postion, and calculate relative + * position in NED + */ + if (gEKFInput.gpsFixType) + { + gEKFInput.lla[LAT] = gEKFInput.llaAnt[LAT]; + gEKFInput.lla[LON] = gEKFInput.llaAnt[LON]; + gEKFInput.lla[ALT] = gEKFInput.llaAnt[ALT]; + gEKFInput.vNed[0] = gEKFInput.vNedAnt[0]; + gEKFInput.vNed[1] = gEKFInput.vNedAnt[1]; + gEKFInput.vNed[2] = gEKFInput.vNedAnt[2]; + /* remove lever arm. Indeed, corrected angular rate should be used. Considering angular + * bias is small, raw angular rate is used. + */ + RemoveLeverArm( gEKFInput.lla, + gEKFInput.vNed, + gEKFInput.angRate_B, + gAlgorithm.leverArmB, + &gKalmanFilter.Rn2e[0][0], + gKalmanFilter.rGPS_E); + + /* Calculate relative position in the NED frame. The initial position is rGPS0_E which. + * is determined when the algorithm first enters the INS mode (InitINSFilter). + */ + ECEF_To_Base( &gKalmanFilter.rGPS0_E[0], + &gKalmanFilter.rGPS_E[0], + &gKalmanFilter.Rn2e[0][0], + &gKalmanFilter.rGPS_N[0]); + } + } + + // odometer + gEKFInput.odoUpdate = odo->update; + gEKFInput.odoVelocity = odo->v; + + // 1PPS signal from GNSS receiver + gEKFInput.ppsDetected = ppsDetected; } @@ -488,563 +547,236 @@ void EKF_SetOutputStruct(void) // ------------------ States ------------------ // Position in [m] - gEKFOutputData.position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS]; - gEKFOutputData.position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS]; - gEKFOutputData.position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS]; + gEKFOutput.position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS]; + gEKFOutput.position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS]; + gEKFOutput.position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS]; // Velocity in [m/s] - gEKFOutputData.velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS]; - gEKFOutputData.velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS]; - gEKFOutputData.velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS]; + gEKFOutput.velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS]; + gEKFOutput.velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS]; + gEKFOutput.velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS]; // Position in [N/A] - gEKFOutputData.quaternion_BinN[Q0] = gKalmanFilter.quaternion[Q0]; - gEKFOutputData.quaternion_BinN[Q1] = gKalmanFilter.quaternion[Q1]; - gEKFOutputData.quaternion_BinN[Q2] = gKalmanFilter.quaternion[Q2]; - gEKFOutputData.quaternion_BinN[Q3] = gKalmanFilter.quaternion[Q3]; + gEKFOutput.quaternion_BinN[Q0] = gKalmanFilter.quaternion[Q0]; + gEKFOutput.quaternion_BinN[Q1] = gKalmanFilter.quaternion[Q1]; + gEKFOutput.quaternion_BinN[Q2] = gKalmanFilter.quaternion[Q2]; + gEKFOutput.quaternion_BinN[Q3] = gKalmanFilter.quaternion[Q3]; // Angular-rate bias in [deg/sec] - gEKFOutputData.angRateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] * RAD_TO_DEG; - gEKFOutputData.angRateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] * RAD_TO_DEG; - gEKFOutputData.angRateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] * RAD_TO_DEG; + gEKFOutput.angRateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] * RAD_TO_DEG; + gEKFOutput.angRateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] * RAD_TO_DEG; + gEKFOutput.angRateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] * RAD_TO_DEG; // Acceleration-bias in [m/s^2] - gEKFOutputData.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS]; - gEKFOutputData.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS]; - gEKFOutputData.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS]; + gEKFOutput.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS]; + gEKFOutput.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS]; + gEKFOutput.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS]; // ------------------ Derived variables ------------------ // Euler-angles in [deg] - gEKFOutputData.eulerAngs_BinN[ROLL] = gKalmanFilter.eulerAngles[ROLL] * RAD_TO_DEG; - gEKFOutputData.eulerAngs_BinN[PITCH] = gKalmanFilter.eulerAngles[PITCH] * RAD_TO_DEG; - gEKFOutputData.eulerAngs_BinN[YAW] = gKalmanFilter.eulerAngles[YAW] * RAD_TO_DEG; + gEKFOutput.eulerAngs_BinN[ROLL] = gKalmanFilter.eulerAngles[ROLL] * RAD_TO_DEG; + gEKFOutput.eulerAngs_BinN[PITCH] = gKalmanFilter.eulerAngles[PITCH] * RAD_TO_DEG; + gEKFOutput.eulerAngs_BinN[YAW] = gKalmanFilter.eulerAngles[YAW] * RAD_TO_DEG; // Angular-rate in [deg/s] - gEKFOutputData.corrAngRates_B[X_AXIS] = ( gEKFInputData.angRate_B[X_AXIS] - + gEKFOutput.corrAngRates_B[X_AXIS] = ( gEKFInput.angRate_B[X_AXIS] - gKalmanFilter.rateBias_B[X_AXIS] ) * RAD_TO_DEG; - gEKFOutputData.corrAngRates_B[Y_AXIS] = ( gEKFInputData.angRate_B[Y_AXIS] - + gEKFOutput.corrAngRates_B[Y_AXIS] = ( gEKFInput.angRate_B[Y_AXIS] - gKalmanFilter.rateBias_B[Y_AXIS] ) * RAD_TO_DEG; - gEKFOutputData.corrAngRates_B[Z_AXIS] = ( gEKFInputData.angRate_B[Z_AXIS] - + gEKFOutput.corrAngRates_B[Z_AXIS] = ( gEKFInput.angRate_B[Z_AXIS] - gKalmanFilter.rateBias_B[Z_AXIS] ) * RAD_TO_DEG; // Acceleration in [m/s^2] - gEKFOutputData.corrAccel_B[X_AXIS] = gEKFInputData.accel_B[X_AXIS] * GRAVITY - - gKalmanFilter.accelBias_B[X_AXIS]; - gEKFOutputData.corrAccel_B[Y_AXIS] = gEKFInputData.accel_B[Y_AXIS] * GRAVITY - - gKalmanFilter.accelBias_B[Y_AXIS]; - gEKFOutputData.corrAccel_B[Z_AXIS] = gEKFInputData.accel_B[Z_AXIS] * GRAVITY - - gKalmanFilter.accelBias_B[Z_AXIS]; + gEKFOutput.corrAccel_B[X_AXIS] = gEKFInput.accel_B[X_AXIS] - gKalmanFilter.accelBias_B[X_AXIS]; + gEKFOutput.corrAccel_B[Y_AXIS] = gEKFInput.accel_B[Y_AXIS] - gKalmanFilter.accelBias_B[Y_AXIS]; + gEKFOutput.corrAccel_B[Z_AXIS] = gEKFInput.accel_B[Z_AXIS] - gKalmanFilter.accelBias_B[Z_AXIS]; // ------------------ Algorithm flags ------------------ - gEKFOutputData.opMode = gAlgorithm.state; - gEKFOutputData.linAccelSwitch = gAlgorithm.linAccelSwitch; - gEKFOutputData.turnSwitchFlag = gAlgoStatus.bit.turnSwitch; - gEKFOutputData.gpsMeasurementUpdate = gAlgoStatus.bit.gpsUpdate; + gEKFOutput.opMode = gAlgorithm.state; + gEKFOutput.linAccelSwitch = gAlgorithm.linAccelSwitch; + gEKFOutput.turnSwitchFlag = gAlgoStatus.bit.turnSwitch; // ------------------ Latitude and Longitude Data ------------------ - gEKFOutputData.llaDeg[LAT] = gKalmanFilter.llaDeg[LAT]; - gEKFOutputData.llaDeg[LON] = gKalmanFilter.llaDeg[LON]; - gEKFOutputData.llaDeg[ALT] = gKalmanFilter.llaDeg[ALT]; + gEKFOutput.llaDeg[LAT] = gKalmanFilter.llaDeg[LAT]; + gEKFOutput.llaDeg[LON] = gKalmanFilter.llaDeg[LON]; + gEKFOutput.llaDeg[ALT] = gKalmanFilter.llaDeg[ALT]; } -static void EstimateAccelError( real *accel, real *w, real dt, AccelStatsStruct *gAccelStats) + +// +uint8_t InitINSFilter(void) { - static BOOL bIni = false; // indicate if the procedure is initialized - static real lastAccel[3]; // accel input of last step - static real lastGyro[3]; // gyro input of last step - static float lastEstimatedAccel[3]; // propagated accel of last step - static uint32_t counter = 0; // propagation counter - static uint32_t t[3]; - // initialize - if ( !bIni ) - { - bIni = true; - lastAccel[0] = accel[0]; - lastAccel[1] = accel[1]; - lastAccel[2] = accel[2]; - lastGyro[0] = w[0]; - lastGyro[1] = w[1]; - lastGyro[2] = w[2]; - t[0] = 0; - t[1] = 0; - t[2] = 0; - gAccelStats->accelErr[0] = 0.0; - gAccelStats->accelErr[1] = 0.0; - gAccelStats->accelErr[2] = 0.0; - return; - } + real tmp[7][7]; + int rowNum, colNum; + +#ifdef INS_OFFLINE + printf("reset INS filter.\n"); +#endif // INS_OFFLINE + + gAlgorithm.insFirstTime = FALSE; + + // Sync the algorithm and GPS ITOW + gAlgorithm.itow = gEKFInput.itow; - /* Using gyro to propagate accel and then to detect accel error can give valid result for a - * short period of time because the inhere long-term drift of integrating gyro data. - * So, after this period of time, a new accel input will be selected. - * Beside, this method cannot detect long-time smooth linear acceleration. In this case, we - * can only hope the linear acceleration is large enough to make an obvious diffeerence from - * the Earth gravity 1g. + /* We have a good GPS reading now - set this variable so we + * don't drop into INS right away */ - if ( counter==0 ) - { - lastEstimatedAccel[0] = lastAccel[0]; - lastEstimatedAccel[1] = lastAccel[1]; - lastEstimatedAccel[2] = lastAccel[2]; - } - counter++; - if ( counter == gAlgorithm.Limit.linAccelSwitchDelay ) - { - counter = 0; - } - - // propagate accel using gyro - // a(k) = a(k-1) -w x a(k-1)*dt - real ae[3]; - lastGyro[0] *= -dt; - lastGyro[1] *= -dt; - lastGyro[2] *= -dt; - cross(lastGyro, lastEstimatedAccel, ae); - ae[0] += lastEstimatedAccel[0]; - ae[1] += lastEstimatedAccel[1]; - ae[2] += lastEstimatedAccel[2]; - - // save this estimated accel - lastEstimatedAccel[0] = ae[0]; - lastEstimatedAccel[1] = ae[1]; - lastEstimatedAccel[2] = ae[2]; - - // err = a(k) - am - ae[0] -= accel[0]; - ae[1] -= accel[1]; - ae[2] -= accel[2]; - - /* If the difference between the propagted accel and the input accel exceeds some threshold, - * we assume there is linear acceleration and set .accelErr to be a large value (0.1g). - * If the difference has been within the threshold for a period of time, we start to decrease - * estimated accel error .accelErr. + gAlgorithm.timeOfLastGoodGPSReading = gEKFInput.itow; + + /* Upon the first entry into INS, save off the base position and reset the + * Kalman filter variables. */ - int j; - gAccelStats->accelErrLimit = false; - for(j=0; j<3; j++) - { - if ( fabs(ae[j]) > 1.0e-2 ) // linear accel detected - { - t[j] = 0; - gAccelStats->accelErr[j] = 0.1f; - } - else // no linear accel detected, start to decrease estimated accel error - { - if ( t[j] > gAlgorithm.Limit.linAccelSwitchDelay ) // decrase error - { - gAccelStats->accelErr[j] *= 0.9f; - gAccelStats->accelErr[j] += 0.1f * ae[j]; - } - else // keep previous error value - { - t[j]++; - // gAccelStats->accelErr[j]; - } - } - // limit error, not taking effect here since the max accelErr should be 0.1 - if ( gAccelStats->accelErr[j] > 0.5 ) - { - gAccelStats->accelErr[j] = 0.5; - gAccelStats->accelErrLimit = true; - } - if ( gAccelStats->accelErr[j] < -0.5 ) - { - gAccelStats->accelErr[j] = -0.5; - gAccelStats->accelErrLimit = true; - } - } - // record accel for next step - lastAccel[0] = accel[0]; - lastAccel[1] = accel[1]; - lastAccel[2] = accel[2]; - lastGyro[0] = w[0]; - lastGyro[1] = w[1]; - lastGyro[2] = w[2]; -} + // Save off the base ECEF location + gKalmanFilter.rGPS0_E[X_AXIS] = gKalmanFilter.rGPS_E[X_AXIS]; + gKalmanFilter.rGPS0_E[Y_AXIS] = gKalmanFilter.rGPS_E[Y_AXIS]; + gKalmanFilter.rGPS0_E[Z_AXIS] = gKalmanFilter.rGPS_E[Z_AXIS]; -static void ComputeAccelStats( real *accel, AccelStatsStruct *gAccelStats, int iReset ) -{ - static BOOL bIni = false; // indicate the routine is initialized or not - static real data[3][DATA_NUM_FOR_STATS]; // a section in memoty to store buffer data - static Buffer bf; // a ring buffer using the above memory section - - // reset the calculation of motion stats - if ( iReset ) - { - bIni = false; - } + // Reset the gps position (as position is relative to starting location) + gKalmanFilter.rGPS_N[X_AXIS] = 0.0; + gKalmanFilter.rGPS_N[Y_AXIS] = 0.0; + gKalmanFilter.rGPS_N[Z_AXIS] = 0.0; - // initialization - if ( !bIni ) - { - bIni = true; - // reset stats - gAccelStats->bValid = false; - gAccelStats->accelMean[0] = 0.0; - gAccelStats->accelMean[1] = 0.0; - gAccelStats->accelMean[2] = 0.0; - gAccelStats->accelVar[0] = 0.0; - gAccelStats->accelVar[1] = 0.0; - gAccelStats->accelVar[2] = 0.0; - // create/reset buffer - bfNew(&bf, &data[0][0], 3, DATA_NUM_FOR_STATS); - } + // Reset prediction values. Position_N is also IMU position. + gKalmanFilter.Position_N[X_AXIS] = (real)0.0; + gKalmanFilter.Position_N[Y_AXIS] = (real)0.0; + gKalmanFilter.Position_N[Z_AXIS] = (real)0.0; + + gKalmanFilter.Velocity_N[X_AXIS] = (real)gEKFInput.vNed[X_AXIS]; + gKalmanFilter.Velocity_N[Y_AXIS] = (real)gEKFInput.vNed[Y_AXIS]; + gKalmanFilter.Velocity_N[Z_AXIS] = (real)gEKFInput.vNed[Z_AXIS]; + + gKalmanFilter.accelBias_B[X_AXIS] = (real)0.0; + gKalmanFilter.accelBias_B[Y_AXIS] = (real)0.0; + gKalmanFilter.accelBias_B[Z_AXIS] = (real)0.0; - /* Compute mean/variance from input data. - * When the input data buffer is full, the stats can be assumed valid. + gKalmanFilter.linearAccel_B[X_AXIS] = (real)0.0; + + /* Extract the Quaternion and rate-bias values from the matrix before + * resetting */ - // save last stats for recursive calculation - real lastMean[3]; - real lastVar[3]; - lastMean[0] = gAccelStats->accelMean[0]; - lastMean[1] = gAccelStats->accelMean[1]; - lastMean[2] = gAccelStats->accelMean[2]; - lastVar[0] = gAccelStats->accelVar[0]; - lastVar[1] = gAccelStats->accelVar[1]; - lastVar[2] = gAccelStats->accelVar[2]; - if ( bf.full ) + // Save off the quaternion and rate-bias covariance values + for (rowNum = Q0; rowNum <= Q3 + Z_AXIS + 1; rowNum++) { - // Enough data collected, stats can be assumed valid - gAccelStats->bValid = true; - /* when buffer is full, the var and mean are computed from - * all data in the buffer. From now on, the var and mean - * should be computed by removing the oldest data and including - * the latest data. - */ - // Get the oldest data which will be removed by following bfPut - real oldest[3]; - bfGet(&bf, oldest, bf.num-1); - // put this accel into buffer - bfPut(&bf, accel); - // mean(n+1) = ( mean(n) * n - x(1) + x(n+1) ) / n - gAccelStats->accelMean[0] = lastMean[0] + ( accel[0] - oldest[0] ) / (real)(bf.num); - gAccelStats->accelMean[1] = lastMean[1] + ( accel[1] - oldest[1] ) / (real)(bf.num); - gAccelStats->accelMean[2] = lastMean[2] + ( accel[2] - oldest[2] ) / (real)(bf.num); - - // naive var calculation is adopted because recursive method is numerically instable - real tmpVar[3]; - tmpVar[0] = vecVar(&bf.d[0], gAccelStats->accelMean[0], bf.num); - tmpVar[1] = vecVar(&bf.d[bf.n], gAccelStats->accelMean[1], bf.num); - tmpVar[2] = vecVar(&bf.d[2*bf.n], gAccelStats->accelMean[2], bf.num); - // make var estimation smooth - real k = 0.99f; - int i; - for ( i=0; i<3; i++ ) + for (colNum = Q0; colNum <= Q3 + Z_AXIS + 1; colNum++) { - if ( tmpVar[i] >= gAccelStats->accelVar[i] ) - { - gAccelStats->accelVar[i] = tmpVar[i]; - } - else - { - gAccelStats->accelVar[i] = k*gAccelStats->accelVar[i] + (1.0f-k)*tmpVar[i]; - } + tmp[rowNum][colNum] = gKalmanFilter.P[rowNum + STATE_Q0][colNum + STATE_Q0]; } } - else - { - // put this accel into buffer - bfPut(&bf, accel); - /* Recursivly include new accel. The data num used to compute mean and - * var are increasing. - */ - // mean(n+1) = mean(n) *n / (n+1) + x(n+1) / (n+1) - gAccelStats->accelMean[0] = lastMean[0] + ( accel[0] - lastMean[0] ) / (real)(bf.num); - gAccelStats->accelMean[1] = lastMean[1] + ( accel[1] - lastMean[1] ) / (real)(bf.num); - gAccelStats->accelMean[2] = lastMean[2] + ( accel[2] - lastMean[2] ) / (real)(bf.num); - gAccelStats->accelVar[0] = lastVar[0] + lastMean[0]*lastMean[0] - - gAccelStats->accelMean[0] * gAccelStats->accelMean[0] + - ( accel[0]*accel[0] - lastVar[0] -lastMean[0]*lastMean[0] ) / (real)(bf.num); - gAccelStats->accelVar[1] = lastVar[1] + lastMean[1]*lastMean[1] - - gAccelStats->accelMean[1] * gAccelStats->accelMean[1] + - ( accel[1]*accel[1] - lastVar[1] -lastMean[1]*lastMean[1] ) / (real)(bf.num); - gAccelStats->accelVar[2] = lastVar[2] + lastMean[2]*lastMean[2] - - gAccelStats->accelMean[2] * gAccelStats->accelMean[2] + - ( accel[2]*accel[2] - lastVar[2] -lastMean[2]*lastMean[2] ) / (real)(bf.num); - } -} - -static BOOL DetectMotionFromAccel(real accelNorm, int iReset) -{ - if( iReset ) + // Reset P + memset(gKalmanFilter.P, 0, sizeof(gKalmanFilter.P)); + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) { - gAlgorithm.linAccelSwitch = false; - gAlgorithm.linAccelSwitchCntr = 0; + gKalmanFilter.P[rowNum][rowNum] = (real)INIT_P_INS; } - /* Check for times when the acceleration is 'close' to 1 [g]. When thisoccurs, - * increment a counter. When it exceeds a threshold (indicating that the system - * has been at rest for a given period) then decrease the R-values (in the - * update stage of the EKF), effectively increasing the Kalman gain. - */ - if (fabs( 1.0 - accelNorm ) < gAlgorithm.Limit.accelSwitch ) + + // Repopulate the P matrix with the quaternion and rate-bias values + for (rowNum = Q0; rowNum <= Q3 + Z_AXIS + 1; rowNum++) { - gAlgorithm.linAccelSwitchCntr++; - if ( gAlgorithm.linAccelSwitchCntr >= gAlgorithm.Limit.linAccelSwitchDelay ) + for (colNum = Q0; colNum <= Q3 + Z_AXIS + 1; colNum++) { - gAlgorithm.linAccelSwitch = TRUE; + gKalmanFilter.P[rowNum + STATE_Q0][colNum + STATE_Q0] = tmp[rowNum][colNum]; } - else - { - gAlgorithm.linAccelSwitch = FALSE; - } - } - else - { - gAlgorithm.linAccelSwitchCntr = 0; - gAlgorithm.linAccelSwitch = FALSE; } - return true; -} + /* Use the GPS-provided horizontal and vertical accuracy values to populate + * the covariance values. + */ + gKalmanFilter.P[STATE_RX][STATE_RX] = gEKFInput.GPSHorizAcc * gEKFInput.GPSHorizAcc; + gKalmanFilter.P[STATE_RY][STATE_RY] = gKalmanFilter.P[STATE_RX][STATE_RX]; + gKalmanFilter.P[STATE_RZ][STATE_RZ] = gEKFInput.GPSVertAcc * gEKFInput.GPSVertAcc; -/* Set the accelerometer filter coefficients, which are used to filter the - * accelerometer readings prior to determining the setting of the linear- - * acceleration switch and computing the roll and pitch from accelerometer - * readings. - */ -static void _PopulateFilterCoefficients( uint8_t lpfType, uint8_t callingFreq, real *b, real *a ) -{ - switch( lpfType ) + /* Scale the best velocity error by HDOP then multiply by the z-axis angular + * rate PLUS one (to prevent the number from being zero) so the velocity + * update during high-rate turns is reduced. + */ + float temp = (real)0.0625 * gEKFInput.HDOP; // 0.0625 = 0.05 / 0.8 + real absFilteredYawRate = (real)fabs(gAlgorithm.filteredYawRate); + if (absFilteredYawRate > TEN_DEGREES_IN_RAD) { - case NO_LPF: - b[0] = (real)(1.0); - b[1] = (real)(0.0); - b[2] = (real)(0.0); - b[3] = (real)(0.0); - - a[0] = (real)(0.0); - a[1] = (real)(0.0); - a[2] = (real)(0.0); - a[3] = (real)(0.0); - break; - case TWO_HZ_LPF: - if( callingFreq == FREQ_100_HZ) - { - b[0] = (real)(2.19606211225382e-4); - b[1] = (real)(6.58818633676145e-4); - b[2] = (real)(6.58818633676145e-4); - b[3] = (real)(2.19606211225382e-4); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-2.748835809214676); - a[2] = (real)( 2.528231219142559); - a[3] = (real)(-0.777638560238080); - } - else - { - b[0] = (real)(2.91464944656705e-5); - b[1] = (real)(8.74394833970116e-5); - b[2] = (real)(8.74394833970116e-5); - b[3] = (real)(2.91464944656705e-5); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-2.874356892677485); - a[2] = (real)( 2.756483195225695); - a[3] = (real)(-0.881893130592486); - } - break; - case FIVE_HZ_LPF: - if( callingFreq == FREQ_100_HZ) - { - b[0] = (real)( 0.002898194633721); - b[1] = (real)( 0.008694583901164); - b[2] = (real)( 0.008694583901164); - b[3] = (real)( 0.002898194633721); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-2.374094743709352); - a[2] = (real)( 1.929355669091215); - a[3] = (real)(-0.532075368312092); - } - else - { - b[0] = (real)( 0.000416546139076); - b[1] = (real)( 0.001249638417227); - b[2] = (real)( 0.001249638417227); - b[3] = (real)( 0.000416546139076); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-2.686157396548143); - a[2] = (real)( 2.419655110966473); - a[3] = (real)(-0.730165345305723); - } - break; - case TWENTY_HZ_LPF: - if (callingFreq == FREQ_100_HZ) - { - // [B,A] = butter(3,20/(100/2)) - b[0] = (real)( 0.098531160923927); - b[1] = (real)( 0.295593482771781); - b[2] = (real)( 0.295593482771781); - b[3] = (real)( 0.098531160923927); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-0.577240524806303); - a[2] = (real)( 0.421787048689562); - a[3] = (real)(-0.056297236491843); - } - else - { - // [B,A] = butter(3,20/(200/2)) - b[0] = (real)( 0.018098933007514); - b[1] = (real)( 0.054296799022543); - b[2] = (real)( 0.054296799022543); - b[3] = (real)( 0.018098933007514); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-1.760041880343169); - a[2] = (real)( 1.182893262037831); - a[3] = (real)(-0.278059917634546); - } - break; - case TWENTY_FIVE_HZ_LPF: - if (callingFreq == FREQ_100_HZ) - { - b[0] = (real)( 0.166666666666667); - b[1] = (real)( 0.500000000000000); - b[2] = (real)( 0.500000000000000); - b[3] = (real)( 0.166666666666667); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-0.000000000000000); - a[2] = (real)( 0.333333333333333); - a[3] = (real)(-0.000000000000000); - } - else - { - b[0] = (real)( 0.031689343849711); - b[1] = (real)( 0.095068031549133); - b[2] = (real)( 0.095068031549133); - b[3] = (real)( 0.031689343849711); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-1.459029062228061); - a[2] = (real)( 0.910369000290069); - a[3] = (real)(-0.197825187264319); - } - break; - case TEN_HZ_LPF: - default: - if( callingFreq == FREQ_100_HZ) - { - b[0] = (real)( 0.0180989330075144); - b[1] = (real)( 0.0542967990225433); - b[2] = (real)( 0.0542967990225433); - b[3] = (real)( 0.0180989330075144); - - a[0] = (real)( 1.0000000000000000); - a[1] = (real)(-1.7600418803431690); - a[2] = (real)( 1.1828932620378310); - a[3] = (real)(-0.2780599176345460); - } - else - { - b[0] = (real)( 0.002898194633721); - b[1] = (real)( 0.008694583901164); - b[2] = (real)( 0.008694583901164); - b[3] = (real)( 0.002898194633721); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-2.374094743709352); - a[2] = (real)( 1.929355669091215); - a[3] = (real)(-0.532075368312092); - } - break; + temp *= (1.0f + absFilteredYawRate); } -} - -static void _AccelLPF( real *accel, uint8_t lpfType, uint8_t callingFreq, real *filteredAccel ) -{ - // Floating-point filter variables - static real accelFilt[FILTER_ORDER+1][NUM_AXIS]; - static real accelReading[FILTER_ORDER+1][NUM_AXIS]; + gKalmanFilter.P[STATE_VX][STATE_VX] = temp;// *((real)1.0 + fabs(gAlgorithm.filteredYawRate) * (real)RAD_TO_DEG); + gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_VX] * gKalmanFilter.P[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.P[STATE_VX][STATE_VX]; - // filter coefficients. y/x = b/a - static real b_AccelFilt[FILTER_ORDER+1]; - static real a_AccelFilt[FILTER_ORDER+1]; + // z-axis velocity isn't really a function of yaw-rate and hdop + //gKalmanFilter.R[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VZ][STATE_VZ] = (float)(0.1 * 0.1); - // Compute filter coefficients and initialize data buffer - static BOOL initAccelFilt = true; - if (initAccelFilt) - { - initAccelFilt = false; - - // Set the filter coefficients based on selected cutoff frequency and sampling rate - _PopulateFilterCoefficients( lpfType, callingFreq, b_AccelFilt, a_AccelFilt); - - // Initialize the filter variables (do not need to populate the 0th element - // as it is never used) - for( int i = PASTx1; i <= PASTx3; i++ ) - { - accelReading[i][X_AXIS] = accel[X_AXIS]; - accelReading[i][Y_AXIS] = accel[Y_AXIS]; - accelReading[i][Z_AXIS] = accel[Z_AXIS]; - - accelFilt[i][X_AXIS] = accel[X_AXIS]; - accelFilt[i][Y_AXIS] = accel[Y_AXIS]; - accelFilt[i][Z_AXIS] = accel[Z_AXIS]; - } - } + return 1; +} - /* Filter accelerometer readings (Note: a[0] = 1.0 and the filter coefficients are symmetric) - * y = filtered output; x = raw input; - */ - /* a[0]*y(k) + a[1]*y(k-1) + a[2]*y(k-2) + a[3]*y(k-3) = - * b[0]*x(k) + b[1]*x(k-1) + b[2]*x(k-2) + b[3]*x(k-3) = - * b[0]*( x(k) + x(k-3) ) + b[1]*( x(k-1) + x(k-2) ) +static void RemoveLeverArm(double *lla, double *vNed, real *w, real *leverArmB, real *rn2e, double *ecef) +{ + // Using position with lever arm to calculate rm and rn + double sinLat = sin(lla[LAT]); + double cosLat = cos(lla[LAT]); + double tmp = 1.0 - (E_ECC_SQ * sinLat * sinLat); + double sqrtTmp = sqrt(tmp); + double rn = E_MAJOR / sqrtTmp; // radius of Curvature [meters] + double rm = rn * (1.0 - E_ECC_SQ) / tmp; + // Remove lever arm from position + real leverArmN[3]; // lever arm in the NED frame + leverArmN[0] = gKalmanFilter.R_BinN[0][0] * leverArmB[0] + + gKalmanFilter.R_BinN[0][1] * leverArmB[1] + + gKalmanFilter.R_BinN[0][2] * leverArmB[2]; + leverArmN[1] = gKalmanFilter.R_BinN[1][0] * leverArmB[0] + + gKalmanFilter.R_BinN[1][1] * leverArmB[1] + + gKalmanFilter.R_BinN[1][2] * leverArmB[2]; + leverArmN[2] = gKalmanFilter.R_BinN[2][0] * leverArmB[0] + + gKalmanFilter.R_BinN[2][1] * leverArmB[1] + + gKalmanFilter.R_BinN[2][2] * leverArmB[2]; + lla[0] -= leverArmN[0] / rm; + lla[1] -= leverArmN[1] / rn / cosLat; + lla[2] += leverArmN[2]; /* Notice: lever arm is now in NED frame while altitude is + * in the opposite direction of the z axis of NED frame. + */ + + /* Remove lever arm effects from velocity + * v_gnss = v_imu + C_b2n * cross(wB, leverArmB) */ - accelFilt[CURRENT][X_AXIS] = b_AccelFilt[CURRENT] * accel[X_AXIS] + - b_AccelFilt[PASTx1] * ( accelReading[PASTx1][X_AXIS] + - accelReading[PASTx2][X_AXIS] ) + - b_AccelFilt[PASTx3] * accelReading[PASTx3][X_AXIS] - - a_AccelFilt[PASTx1] * accelFilt[PASTx1][X_AXIS] - - a_AccelFilt[PASTx2] * accelFilt[PASTx2][X_AXIS] - - a_AccelFilt[PASTx3] * accelFilt[PASTx3][X_AXIS]; - accelFilt[CURRENT][Y_AXIS] = b_AccelFilt[CURRENT] * accel[Y_AXIS] + - b_AccelFilt[PASTx1] * ( accelReading[PASTx1][Y_AXIS] + - accelReading[PASTx2][Y_AXIS] ) + - b_AccelFilt[PASTx3] * accelReading[PASTx3][Y_AXIS] - - a_AccelFilt[PASTx1] * accelFilt[PASTx1][Y_AXIS] - - a_AccelFilt[PASTx2] * accelFilt[PASTx2][Y_AXIS] - - a_AccelFilt[PASTx3] * accelFilt[PASTx3][Y_AXIS]; - accelFilt[CURRENT][Z_AXIS] = b_AccelFilt[CURRENT] * accel[Z_AXIS] + - b_AccelFilt[PASTx1] * ( accelReading[PASTx1][Z_AXIS] + - accelReading[PASTx2][Z_AXIS] ) + - b_AccelFilt[PASTx3] * accelReading[PASTx3][Z_AXIS] - - a_AccelFilt[PASTx1] * accelFilt[PASTx1][Z_AXIS] - - a_AccelFilt[PASTx2] * accelFilt[PASTx2][Z_AXIS] - - a_AccelFilt[PASTx3] * accelFilt[PASTx3][Z_AXIS]; - - // Update past readings - accelReading[PASTx3][X_AXIS] = accelReading[PASTx2][X_AXIS]; - accelReading[PASTx2][X_AXIS] = accelReading[PASTx1][X_AXIS]; - accelReading[PASTx1][X_AXIS] = accel[X_AXIS]; - - accelReading[PASTx3][Y_AXIS] = accelReading[PASTx2][Y_AXIS]; - accelReading[PASTx2][Y_AXIS] = accelReading[PASTx1][Y_AXIS]; - accelReading[PASTx1][Y_AXIS] = accel[Y_AXIS]; - - accelReading[PASTx3][Z_AXIS] = accelReading[PASTx2][Z_AXIS]; - accelReading[PASTx2][Z_AXIS] = accelReading[PASTx1][Z_AXIS]; - accelReading[PASTx1][Z_AXIS] = accel[Z_AXIS]; - - accelFilt[PASTx3][X_AXIS] = accelFilt[PASTx2][X_AXIS]; - accelFilt[PASTx2][X_AXIS] = accelFilt[PASTx1][X_AXIS]; - accelFilt[PASTx1][X_AXIS] = accelFilt[CURRENT][X_AXIS]; - - accelFilt[PASTx3][Y_AXIS] = accelFilt[PASTx2][Y_AXIS]; - accelFilt[PASTx2][Y_AXIS] = accelFilt[PASTx1][Y_AXIS]; - accelFilt[PASTx1][Y_AXIS] = accelFilt[CURRENT][Y_AXIS]; - - accelFilt[PASTx3][Z_AXIS] = accelFilt[PASTx2][Z_AXIS]; - accelFilt[PASTx2][Z_AXIS] = accelFilt[PASTx1][Z_AXIS]; - accelFilt[PASTx1][Z_AXIS] = accelFilt[CURRENT][Z_AXIS]; - - // Output filtered accel - filteredAccel[X_AXIS] = accelFilt[CURRENT][X_AXIS]; - filteredAccel[Y_AXIS] = accelFilt[CURRENT][Y_AXIS]; - filteredAccel[Z_AXIS] = accelFilt[CURRENT][Z_AXIS]; + cross(w, leverArmB, leverArmN); // use leverArmN to temporatily hold w x leverArmB in body frame + vNed[0] -= gKalmanFilter.R_BinN[0][0] * leverArmN[0] + + gKalmanFilter.R_BinN[0][1] * leverArmN[1] + + gKalmanFilter.R_BinN[0][2] * leverArmN[2]; + vNed[1] -= gKalmanFilter.R_BinN[1][0] * leverArmN[0] + + gKalmanFilter.R_BinN[1][1] * leverArmN[1] + + gKalmanFilter.R_BinN[1][2] * leverArmN[2]; + vNed[2] -= gKalmanFilter.R_BinN[2][0] * leverArmN[0] + + gKalmanFilter.R_BinN[2][1] * leverArmN[1] + + gKalmanFilter.R_BinN[2][2] * leverArmN[2]; + + // calcualte transfromation matrix from NED to ECEF + sinLat = sin(lla[LAT]); // recalculate with LLA without lever arm + cosLat = cos(lla[LAT]); + double sinLon = sin(lla[LON]); + double cosLon = cos(lla[LON]); + + real sinLat_r = (real)sinLat; + real cosLat_r = (real)cosLat; + real sinLon_r = (real)sinLon; + real cosLon_r = (real)cosLon; + + // Form the transformation matrix from NED to ECEF + // First row + *(rn2e + 0 * 3 + 0) = -sinLat_r * cosLon_r; + *(rn2e + 0 * 3 + 1) = -sinLon_r; + *(rn2e + 0 * 3 + 2) = -cosLat_r * cosLon_r; + // Second row + *(rn2e + 1 * 3 + 0) = -sinLat_r * sinLon_r; + *(rn2e + 1 * 3 + 1) = cosLon_r; + *(rn2e + 1 * 3 + 2) = -cosLat_r * sinLon_r; + // Third row + *(rn2e + 2 * 3 + 0) = cosLat_r; + *(rn2e + 2 * 3 + 1) = 0.0; + *(rn2e + 2 * 3 + 2) = -sinLat_r; + + // calculate ECEF position + tmp = (rn + lla[ALT]) * cosLat; + ecef[X_AXIS] = tmp * cosLon; + ecef[Y_AXIS] = tmp * sinLon; + ecef[Z_AXIS] = ((E_MINOR_OVER_MAJOR_SQ * (rn)) + lla[ALT]) * sinLat; } diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/MotionStatus.c b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/MotionStatus.c new file mode 100644 index 0000000..220f0cd --- /dev/null +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/MotionStatus.c @@ -0,0 +1,642 @@ +/***************************************************************************** + * @file MotionStatus.c + * @brief Calculate sensor stats, and detect motion status using IMU/ODO/GNSS + * @author Dong Xiaoguang + * @version 1.0 + * @date 20190801 + *****************************************************************************/ +#include +#include "MotionStatus.h" +#include "buffer.h" +#include "VectorMath.h" + + +//============================================================================= +// Filter variables (Third-Order BWF w/ default 5 Hz Cutoff) +#define FILTER_ORDER 3 + +#define CURRENT 0 +#define PASTx1 1 +#define PASTx2 2 +#define PASTx3 3 +/* Replace this with a fuction that will compute the coefficients so the + * input is the cutoff frequency in Hertz + */ +#define NO_LPF 0 +#define TWO_HZ_LPF 1 +#define FIVE_HZ_LPF 2 +#define TEN_HZ_LPF 3 +#define TWENTY_HZ_LPF 4 +#define TWENTY_FIVE_HZ_LPF 5 +#define N_LPF 6 + +#define SAMPLES_FOR_STATS 20 /* 20 samples can give a relative good estimate of var + * This value should not be below FILTER_ORDER. + */ + + /****************************************************************************** + * @brief Get filter coefficients of a 3rd Butterworth low-pass filter. + * For now only a few specific cut-off frequencies are supported. + * TRACE: + * @param [in] lpfType Low-pass filter cut-off frequency. + * @param [in] callingFreq Sampling frequency, only 100Hz and 200Hz are supported. + * @param [out] b coefficients of the numerator of the filter. + * @param [out] a coefficients of the denominator of the filter. + * @retval None. + ******************************************************************************/ +static void _PopulateFilterCoefficients(uint8_t lpfType, uint8_t callingFreq, real *b, real *a); + +/****************************************************************************** + * @brief Process input data through a low-pass Butterworth filter. + * TRACE: + * @param [in] in Input data + * @param [in] bfIn Input data buffer + * @param [in] bfOut Output data buffer + * @param [in] b Numerator coef of the filter + * @param [in] a Denominator coef of the filter + * + * @param [out] filtered Filtered IMU data. + * @retval None. +******************************************************************************/ +static void LowPassFilter(real *in, Buffer *bfIn, Buffer *bfOut, real *b, real *a, real *filtered); + +/****************************************************************************** + * @brief Compute mean and var of the input data. + * Calculate mena and var of the latest n samples. + * TRACE: + * @param [in] bf The buffer to hold the latest n samples. + * @param [in] latest The latest sample. + * @param [in/out] mean Mean of samples already in buffer is used as input, and + * mean of latest samples (remove oldest and include latest) + * is returned as output. + * @param [in/out] var Var of samples already in buffer is used as input, and + * var of latest samples (remove oldest and include latest) + * is returned as output. + * @retval None. +******************************************************************************/ +static void ComputeStats(Buffer *bf, real *latest, real *mean, real *var); + +/****************************************************************************** + * @brief Detect zero velocity using IMU data. + * TRACE: + * @param [in] gyroVar variance of gyro [rad/s]^2 + * @param [in] gyroMean mean of gyro [rad/s] + * @param [in] accelVar variance of accel [m/s/s]^2 + * @param [in] threshold threshold to detect zero velocity + * @retval TRUE if static, otherwise FALSE. +******************************************************************************/ +BOOL DetectStaticIMU(real *gyroVar, real *gyroMean, real *accelVar, STATIC_DETECT_SETTING *threshold); + + +void MotionStatusImu(real *gyro, real *accel, ImuStatsStruct *imuStats, BOOL reset) +{ + static BOOL bIni = false; // indicate the routine is initialized or not + // Buffer to store input IMU data. Data in buffer is ued to calcualte filtered IMU dat. + static real dAccel[3][FILTER_ORDER]; // a section in memory as buffer to store accel data + static real dGyro[3][FILTER_ORDER]; // a section in memory as buffer to store gyro data + static Buffer bfAccel; // a ring buffer of accel + static Buffer bfGyro; // a ring buffer of gyro + // Buffer to store filtered IMU data. Data in buffer is used to calculate IMU stats. + static real dLpfAccel[3][SAMPLES_FOR_STATS]; // a section in memory as buffer to store accel data + static real dLpfGyro[3][SAMPLES_FOR_STATS]; // a section in memory as buffer to store gyro data + static Buffer bfLpfAccel; // a ring buffer of accel + static Buffer bfLpfGyro; // a ring buffer of gyro + // filter coefficients. y/x = b/a + static real b_AccelFilt[FILTER_ORDER + 1]; + static real a_AccelFilt[FILTER_ORDER + 1]; + + // reset the calculation of motion stats + if (reset) + { + bIni = false; + } + + // initialization + if (!bIni) + { + bIni = true; + // reset stats + imuStats->bValid = false; + imuStats->accelMean[0] = 0.0; + imuStats->accelMean[1] = 0.0; + imuStats->accelMean[2] = 0.0; + imuStats->accelVar[0] = 0.0; + imuStats->accelVar[1] = 0.0; + imuStats->accelVar[2] = 0.0; + imuStats->gyroMean[0] = 0.0; + imuStats->gyroMean[1] = 0.0; + imuStats->gyroMean[2] = 0.0; + imuStats->gyroVar[0] = 0.0; + imuStats->gyroVar[1] = 0.0; + imuStats->gyroVar[2] = 0.0; + // create/reset buffer + bfNew(&bfGyro, &dGyro[0][0], 3, FILTER_ORDER); + bfNew(&bfAccel, &dAccel[0][0], 3, FILTER_ORDER); + bfNew(&bfLpfGyro, &dLpfGyro[0][0], 3, SAMPLES_FOR_STATS); + bfNew(&bfLpfAccel, &dLpfAccel[0][0], 3, SAMPLES_FOR_STATS); + // Set the filter coefficients based on selected cutoff frequency and sampling rate + _PopulateFilterCoefficients(gAlgorithm.linAccelLPFType, gAlgorithm.callingFreq, b_AccelFilt, a_AccelFilt); + } + + /* Low-pass filter. + * The input IMU data is put into the buffer, and then filtered. + */ + LowPassFilter(gyro, &bfGyro, &bfLpfGyro, b_AccelFilt, a_AccelFilt, imuStats->lpfGyro); + LowPassFilter(accel, &bfAccel, &bfLpfAccel, b_AccelFilt, a_AccelFilt, imuStats->lpfAccel); + + /* Compute accel norm using raw accel data. + * The norm will be used to detect static periods via magnitude. + */ + imuStats->accelNorm = sqrtf(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]); + + /* Compute mean/variance from input data. + * The latest will be put into the buffer, and stats of data in buffer is then calculated. + * When the input data buffer is full, the stats can be assumed valid. + */ + ComputeStats(&bfLpfGyro, imuStats->lpfGyro, imuStats->gyroMean, imuStats->gyroVar); + ComputeStats(&bfLpfAccel, imuStats->lpfAccel, imuStats->accelMean, imuStats->accelVar); + imuStats->bValid = bfLpfGyro.full && bfLpfAccel.full; + + // Detect static period using var calculated above. + imuStats->bStaticIMU = DetectStaticIMU( imuStats->gyroVar, + imuStats->gyroMean, + imuStats->accelVar, + &gAlgorithm.staticDetectParam); +} + +static void ComputeStats(Buffer *bf, real *latest, real *mean, real *var) +{ + real lastMean[3]; + real lastVar[3]; + lastMean[0] = mean[0]; + lastMean[1] = mean[1]; + lastMean[2] = mean[2]; + lastVar[0] = var[0]; + lastVar[1] = var[1]; + lastVar[2] = var[2]; + if (bf->full) + { + /* when buffer is full, the var and mean are computed from + * all data in the buffer. From now on, the var and mean + * should be computed by removing the oldest data and including + * the latest data. + */ + // Get the oldest data which will be removed by following bfPut + real oldest[3]; + bfGet(bf, oldest, bf->num - 1); + // put this accel into buffer + bfPut(bf, latest); + // mean(n+1) = ( mean(n) * n - x(1) + x(n+1) ) / n + mean[0] += (latest[0] - oldest[0]) / (real)(bf->num); + mean[1] += (latest[1] - oldest[1]) / (real)(bf->num); + mean[2] += (latest[2] - oldest[2]) / (real)(bf->num); + + // naive var calculation is adopted because recursive method is numerically instable + real tmpVar[3]; + tmpVar[0] = vecVar(&(bf->d[0]), mean[0], bf->num); + tmpVar[1] = vecVar(&(bf->d[bf->n]), mean[1], bf->num); + tmpVar[2] = vecVar(&(bf->d[2 * bf->n]), mean[2], bf->num); + // make var estimation smooth + real k = 0.96f; + int i; + for (i = 0; i < 3; i++) + { + if (tmpVar[i] >= var[i]) + { + var[i] = tmpVar[i]; + } + else + { + var[i] = k * var[i] + (1.0f - k)*tmpVar[i]; + } + } + } + else + { + // put this accel into buffer + bfPut(bf, latest); + /* Recursivly include new accel. The data num used to compute mean and + * var are increasing. + */ + // mean(n+1) = mean(n) *n / (n+1) + x(n+1) / (n+1) + mean[0] = lastMean[0] + (latest[0] - lastMean[0]) / (real)(bf->num); + mean[1] = lastMean[1] + (latest[1] - lastMean[1]) / (real)(bf->num); + mean[2] = lastMean[2] + (latest[2] - lastMean[2]) / (real)(bf->num); + var[0] = lastVar[0] + lastMean[0] * lastMean[0] - mean[0] * mean[0] + + (latest[0] * latest[0] - lastVar[0] - lastMean[0] * lastMean[0]) / (real)(bf->num); + var[1] = lastVar[1] + lastMean[1] * lastMean[1] - mean[1] * mean[1] + + (latest[1] * latest[1] - lastVar[1] - lastMean[1] * lastMean[1]) / (real)(bf->num); + var[2] = lastVar[2] + lastMean[2] * lastMean[2] - mean[2] * mean[2] + + (latest[2] * latest[2] - lastVar[2] - lastMean[2] * lastMean[2]) / (real)(bf->num); + } +} + +BOOL DetectStaticIMU(real *gyroVar, real *gyroMean, real *accelVar, STATIC_DETECT_SETTING *threshold) +{ + BOOL bStatic = TRUE; + int i; + static float multiplier[3] = { 0.0, 0.0, 0.0 }; + static float gyroVarThreshold = 0.0; + static float accelVarThreshold = 0.0; + static float gyroBiasThreshold = 0.0; + // Update threshold + if (multiplier[0] != threshold->staticNoiseMultiplier[0]) + { + multiplier[0] = threshold->staticNoiseMultiplier[0]; + gyroVarThreshold = multiplier[0] * threshold->staticVarGyro; + } + if (multiplier[1] != threshold->staticNoiseMultiplier[1]) + { + multiplier[1] = threshold->staticNoiseMultiplier[1]; + accelVarThreshold = multiplier[1] * threshold->staticVarAccel; + } + if (multiplier[2] != threshold->staticNoiseMultiplier[2]) + { + multiplier[2] = threshold->staticNoiseMultiplier[2]; + gyroBiasThreshold = multiplier[2] * threshold->maxGyroBias; + } + + for (i = 0; i < 3; i++) + { + if (gyroVar[i] > gyroVarThreshold || + accelVar[i] > accelVarThreshold || + fabs(gyroMean[i]) > gyroBiasThreshold) + { + bStatic = FALSE; + break; + } + } + + return bStatic; +} + +/* Set the accelerometer filter coefficients, which are used to filter the + * accelerometer readings prior to determining the setting of the linear- + * acceleration switch and computing the roll and pitch from accelerometer + * readings. + */ +static void _PopulateFilterCoefficients(uint8_t lpfType, uint8_t callingFreq, real *b, real *a) +{ + switch (lpfType) + { + case NO_LPF: + b[0] = (real)(1.0); + b[1] = (real)(0.0); + b[2] = (real)(0.0); + b[3] = (real)(0.0); + + a[0] = (real)(0.0); + a[1] = (real)(0.0); + a[2] = (real)(0.0); + a[3] = (real)(0.0); + break; + case TWO_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(2.19606211225382e-4); + b[1] = (real)(6.58818633676145e-4); + b[2] = (real)(6.58818633676145e-4); + b[3] = (real)(2.19606211225382e-4); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.748835809214676); + a[2] = (real)(2.528231219142559); + a[3] = (real)(-0.777638560238080); + } + else + { + b[0] = (real)(2.91464944656705e-5); + b[1] = (real)(8.74394833970116e-5); + b[2] = (real)(8.74394833970116e-5); + b[3] = (real)(2.91464944656705e-5); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.874356892677485); + a[2] = (real)(2.756483195225695); + a[3] = (real)(-0.881893130592486); + } + break; + case FIVE_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(0.002898194633721); + b[1] = (real)(0.008694583901164); + b[2] = (real)(0.008694583901164); + b[3] = (real)(0.002898194633721); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.374094743709352); + a[2] = (real)(1.929355669091215); + a[3] = (real)(-0.532075368312092); + } + else + { + b[0] = (real)(0.000416546139076); + b[1] = (real)(0.001249638417227); + b[2] = (real)(0.001249638417227); + b[3] = (real)(0.000416546139076); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.686157396548143); + a[2] = (real)(2.419655110966473); + a[3] = (real)(-0.730165345305723); + } + break; + case TWENTY_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + // [B,A] = butter(3,20/(100/2)) + b[0] = (real)(0.098531160923927); + b[1] = (real)(0.295593482771781); + b[2] = (real)(0.295593482771781); + b[3] = (real)(0.098531160923927); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-0.577240524806303); + a[2] = (real)(0.421787048689562); + a[3] = (real)(-0.056297236491843); + } + else + { + // [B,A] = butter(3,20/(200/2)) + b[0] = (real)(0.018098933007514); + b[1] = (real)(0.054296799022543); + b[2] = (real)(0.054296799022543); + b[3] = (real)(0.018098933007514); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-1.760041880343169); + a[2] = (real)(1.182893262037831); + a[3] = (real)(-0.278059917634546); + } + break; + case TWENTY_FIVE_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(0.166666666666667); + b[1] = (real)(0.500000000000000); + b[2] = (real)(0.500000000000000); + b[3] = (real)(0.166666666666667); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-0.000000000000000); + a[2] = (real)(0.333333333333333); + a[3] = (real)(-0.000000000000000); + } + else + { + b[0] = (real)(0.031689343849711); + b[1] = (real)(0.095068031549133); + b[2] = (real)(0.095068031549133); + b[3] = (real)(0.031689343849711); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-1.459029062228061); + a[2] = (real)(0.910369000290069); + a[3] = (real)(-0.197825187264319); + } + break; + case TEN_HZ_LPF: + default: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(0.0180989330075144); + b[1] = (real)(0.0542967990225433); + b[2] = (real)(0.0542967990225433); + b[3] = (real)(0.0180989330075144); + + a[0] = (real)(1.0000000000000000); + a[1] = (real)(-1.7600418803431690); + a[2] = (real)(1.1828932620378310); + a[3] = (real)(-0.2780599176345460); + } + else + { + b[0] = (real)(0.002898194633721); + b[1] = (real)(0.008694583901164); + b[2] = (real)(0.008694583901164); + b[3] = (real)(0.002898194633721); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.374094743709352); + a[2] = (real)(1.929355669091215); + a[3] = (real)(-0.532075368312092); + } + break; + } +} + +static void LowPassFilter(real *in, Buffer *bfIn, Buffer *bfOut, real *b, real *a, real *filtered) +{ + // Fill the buffer with the first input. + if (!bfIn->full) + { + bfPut(bfIn, in); + filtered[0] = in[0]; + filtered[1] = in[1]; + filtered[2] = in[2]; + return; + } + + /* Filter accelerometer readings (Note: a[0] = 1.0 and the filter coefficients are symmetric) + * y = filtered output; x = raw input; + * a[0]*y(k) + a[1]*y(k-1) + a[2]*y(k-2) + a[3]*y(k-3) = + * b[0]*x(k) + b[1]*x(k-1) + b[2]*x(k-2) + b[3]*x(k-3) = + * b[0]*( x(k) + x(k-3) ) + b[1]*( x(k-1) + x(k-2) ) + */ + int i; + real tmpIn[3]; + real tmpOut[3]; + filtered[0] = b[CURRENT] * in[0]; + filtered[1] = b[CURRENT] * in[1]; + filtered[2] = b[CURRENT] * in[2]; + for (i = PASTx1; i <= PASTx3; i++) + { + bfGet(bfIn, tmpIn, i-1); + bfGet(bfOut, tmpOut, i-1); + filtered[0] += b[i] * tmpIn[0] - a[i] * tmpOut[0]; + filtered[1] += b[i] * tmpIn[1] - a[i] * tmpOut[1]; + filtered[2] += b[i] * tmpIn[2] - a[i] * tmpOut[2]; + } + + // New data into buffer + bfPut(bfIn, in); +} + +void EstimateAccelError(real *accel, real *w, real dt, uint32_t staticDelay, ImuStatsStruct *imuStats) +{ + static BOOL bIni = false; // indicate if the procedure is initialized + static real lastAccel[3]; // accel input of last step + static real lastGyro[3]; // gyro input of last step + static float lastEstimatedAccel[3]; // propagated accel of last step + static uint32_t counter = 0; // propagation counter + static uint32_t t[3]; + // initialize + if (!bIni) + { + bIni = true; + lastAccel[0] = accel[0]; + lastAccel[1] = accel[1]; + lastAccel[2] = accel[2]; + lastGyro[0] = w[0]; + lastGyro[1] = w[1]; + lastGyro[2] = w[2]; + t[0] = 0; + t[1] = 0; + t[2] = 0; + imuStats->accelErr[0] = 0.0; + imuStats->accelErr[1] = 0.0; + imuStats->accelErr[2] = 0.0; + return; + } + + /* Using gyro to propagate accel and then to detect accel error can give valid result for a + * short period of time because the inhere long-term drift of integrating gyro data. + * So, after this period of time, a new accel input will be selected. + * Beside, this method cannot detect long-time smooth linear acceleration. In this case, we + * can only hope the linear acceleration is large enough to make an obvious diffeerence from + * the Earth gravity 1g. + */ + if (counter == 0) + { + lastEstimatedAccel[0] = lastAccel[0]; + lastEstimatedAccel[1] = lastAccel[1]; + lastEstimatedAccel[2] = lastAccel[2]; + } + counter++; + if (counter == staticDelay) + { + counter = 0; + } + + // propagate accel using gyro + // a(k) = a(k-1) -w x a(k-1)*dt + real ae[3]; + lastGyro[0] *= -dt; + lastGyro[1] *= -dt; + lastGyro[2] *= -dt; + cross(lastGyro, lastEstimatedAccel, ae); + ae[0] += lastEstimatedAccel[0]; + ae[1] += lastEstimatedAccel[1]; + ae[2] += lastEstimatedAccel[2]; + + // save this estimated accel + lastEstimatedAccel[0] = ae[0]; + lastEstimatedAccel[1] = ae[1]; + lastEstimatedAccel[2] = ae[2]; + + // err = a(k) - am + ae[0] -= accel[0]; + ae[1] -= accel[1]; + ae[2] -= accel[2]; + + /* If the difference between the propagted accel and the input accel exceeds some threshold, + * we assume there is linear acceleration and set .accelErr to be a large value (0.1g). + * If the difference has been within the threshold for a period of time, we start to decrease + * estimated accel error .accelErr. + */ + int j; + imuStats->accelErrLimit = false; + for (j = 0; j < 3; j++) + { + if (fabs(ae[j]) > 0.0980665) // linear accel detected, 0.01g + { + t[j] = 0; + imuStats->accelErr[j] = (real)0.980665; // 0.1g + } + else // no linear accel detected, start to decrease estimated accel error + { + if (t[j] > staticDelay) // decrease error + { + imuStats->accelErr[j] *= 0.9f; + imuStats->accelErr[j] += 0.1f * ae[j]; + } + else // keep previous error value + { + t[j]++; + // imuStats->accelErr[j]; + } + } + // limit error, not taking effect here since the max accelErr should be 0.1g + if (imuStats->accelErr[j] > 5.0) // 0.5g + { + imuStats->accelErr[j] = 5.0; + imuStats->accelErrLimit = true; + } + if (imuStats->accelErr[j] < -5.0) + { + imuStats->accelErr[j] = -5.0; + imuStats->accelErrLimit = true; + } + } + // record accel for next step + lastAccel[0] = accel[0]; + lastAccel[1] = accel[1]; + lastAccel[2] = accel[2]; + lastGyro[0] = w[0]; + lastGyro[1] = w[1]; + lastGyro[2] = w[2]; +} + +BOOL DetectMotionFromAccel(real accelNorm, int iReset) +{ + if (iReset) + { + gAlgorithm.linAccelSwitch = false; + gAlgorithm.linAccelSwitchCntr = 0; + } + /* Check for times when the acceleration is 'close' to 1 [g]. When this occurs, + * increment a counter. When it exceeds a threshold (indicating that the system + * has been at rest for a given period) then decrease the R-values (in the + * update stage of the EKF), effectively increasing the Kalman gain. + */ + if (fabs(1.0 - accelNorm/GRAVITY) < gAlgorithm.Limit.accelSwitch) + { + gAlgorithm.linAccelSwitchCntr++; + if (gAlgorithm.linAccelSwitchCntr >= gAlgorithm.Limit.linAccelSwitchDelay) + { + gAlgorithm.linAccelSwitch = TRUE; + } + else + { + gAlgorithm.linAccelSwitch = FALSE; + } + } + else + { + gAlgorithm.linAccelSwitchCntr = 0; + gAlgorithm.linAccelSwitch = FALSE; + } + + return TRUE; +} + +BOOL DetectStaticGnssVelocity(double *vNED, real threshold, BOOL gnssValid) +{ + static uint8_t cntr = 0; + if (gnssValid) + { + if (fabs(vNED[0]) < threshold && fabs(vNED[1]) < threshold && fabs(vNED[2]) < threshold) + { + if (cntr < 3) + { + cntr++; + } + } + else + { + cntr = 0; + } + } + else + { + cntr = 0; + } + + return cntr >= 3; +} + +BOOL DetectStaticOdo(real odo) +{ + return FALSE; +} \ No newline at end of file diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/PredictFunctions.c b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/PredictFunctions.c index 3ac9131..e728dca 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/PredictFunctions.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/PredictFunctions.c @@ -22,19 +22,20 @@ #include "algorithm.h" #include "algorithmAPI.h" #include "SensorNoiseParameters.h" +#include "MotionStatus.h" #include "EKF_Algorithm.h" #include "PredictFunctions.h" #include "AlgorithmLimits.h" #include "WorldMagneticModel.h" -#ifdef INS_OFFLINE -#include "SimulationParameters.h" -#else +#ifndef INS_OFFLINE #ifdef DISPLAY_DIAGNOSTIC_MSG #include "debug.h" #endif #endif +extern ImuStatsStruct gImuStats; + /* F is sparse and has elements in the following locations... * There may be some more efficient ways of implementing this as this method * still performs multiplication with zero values. (Ask Andrey) @@ -77,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); @@ -100,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 ); @@ -117,9 +131,9 @@ void EKF_PredictionStage(real *filteredAccel) */ if(magUsedInAlgorithm()) { - magFieldVector[X_AXIS] = (real)gEKFInputData.magField_B[X_AXIS]; - magFieldVector[Y_AXIS] = (real)gEKFInputData.magField_B[Y_AXIS]; - magFieldVector[Z_AXIS] = (real)gEKFInputData.magField_B[Z_AXIS]; + magFieldVector[X_AXIS] = (real)gEKFInput.magField_B[X_AXIS]; + magFieldVector[Y_AXIS] = (real)gEKFInput.magField_B[Y_AXIS]; + magFieldVector[Z_AXIS] = (real)gEKFInput.magField_B[Z_AXIS]; } else { @@ -184,10 +198,6 @@ static void _PredictStateEstimate(void) { real aCorr_N[3]; real deltaQuaternion[4]; - /* Generate the transformation matrix (R_BinN) based on the past value of - * the attitude quaternion (prior to prediction at the new time-step) - */ - QuaternionToR321(gKalmanFilter.quaternion, &gKalmanFilter.R_BinN[0][0]); if( gAlgorithm.state > LOW_GAIN_AHRS ) { @@ -202,34 +212,45 @@ static void _PredictStateEstimate(void) // ================= NED Velocity (v_N) ================= // aCorr_B = aMeas_B - aBias_B - // gEKFInputData.accel_B in g's, convert to m/s^2 for integration - gKalmanFilter.correctedAccel_B[X_AXIS] = (real)(gEKFInputData.accel_B[X_AXIS] * GRAVITY) - + // gEKFInput.accel_B in g's, convert to m/s^2 for integration + gKalmanFilter.correctedAccel_B[X_AXIS] = gEKFInput.accel_B[X_AXIS] - gKalmanFilter.accelBias_B[X_AXIS]; - gKalmanFilter.correctedAccel_B[Y_AXIS] = (real)(gEKFInputData.accel_B[Y_AXIS] * GRAVITY) - + gKalmanFilter.correctedAccel_B[Y_AXIS] = gEKFInput.accel_B[Y_AXIS] - gKalmanFilter.accelBias_B[Y_AXIS]; - gKalmanFilter.correctedAccel_B[Z_AXIS] = (real)(gEKFInputData.accel_B[Z_AXIS] * GRAVITY) - + gKalmanFilter.correctedAccel_B[Z_AXIS] = gEKFInput.accel_B[Z_AXIS] - gKalmanFilter.accelBias_B[Z_AXIS]; - // Transform the corrected acceleration vector from the body to the NED-frame - // a_N = R_BinN * a_B - aCorr_N[X_AXIS] = gKalmanFilter.R_BinN[X_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + - gKalmanFilter.R_BinN[X_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + - gKalmanFilter.R_BinN[X_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; - aCorr_N[Y_AXIS] = gKalmanFilter.R_BinN[Y_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + - gKalmanFilter.R_BinN[Y_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + - gKalmanFilter.R_BinN[Y_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; - aCorr_N[Z_AXIS] = gKalmanFilter.R_BinN[Z_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + - gKalmanFilter.R_BinN[Z_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + - gKalmanFilter.R_BinN[Z_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; - - // Determine the acceleration of the system by removing the gravity vector - // v_N(k+1) = v_N(k) + dV = v_N(k) + aMotion_N*DT = v_N(k) + ( a_N - g_N )*DT - gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + - ( aCorr_N[X_AXIS] ) * gAlgorithm.dt; - gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + - ( aCorr_N[Y_AXIS] ) * gAlgorithm.dt; - gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + - ( aCorr_N[Z_AXIS] + (real)GRAVITY ) * gAlgorithm.dt; + /* Transform the corrected acceleration vector from the body to the NED-frame and remove gravity + * a_N = R_BinN * a_B + */ + aCorr_N[X_AXIS] = + gKalmanFilter.R_BinN[X_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[X_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[X_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; + aCorr_N[Y_AXIS] = + gKalmanFilter.R_BinN[Y_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[Y_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[Y_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; + aCorr_N[Z_AXIS] = + gKalmanFilter.R_BinN[Z_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS] + + (real)GRAVITY; + + /* Determine the acceleration of the system by removing the gravity vector + * v_N(k+1) = v_N(k) + dV = v_N(k) + aMotion_N*DT = v_N(k) + ( a_N - g_N )*DT + */ + gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + aCorr_N[X_AXIS] * gAlgorithm.dt; + gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + aCorr_N[Y_AXIS] * gAlgorithm.dt; + gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + aCorr_N[Z_AXIS] * gAlgorithm.dt; + + // Calculate linear acceleration in the body frame. + gKalmanFilter.linearAccel_B[X_AXIS] += (gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][X_AXIS] * (real)GRAVITY)*gAlgorithm.dt; + gKalmanFilter.linearAccel_B[Y_AXIS] = gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Y_AXIS] * (real)GRAVITY; + gKalmanFilter.linearAccel_B[Z_AXIS] = gKalmanFilter.correctedAccel_B[Z_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Z_AXIS] * (real)GRAVITY; } else { @@ -243,18 +264,18 @@ static void _PredictStateEstimate(void) gKalmanFilter.Velocity_N[Z_AXIS] = (real)0.0; // what should this be??? - gKalmanFilter.correctedAccel_B[XACCEL] = (real)gEKFInputData.accel_B[X_AXIS] * (real)GRAVITY; - gKalmanFilter.correctedAccel_B[YACCEL] = (real)gEKFInputData.accel_B[Y_AXIS] * (real)GRAVITY; - gKalmanFilter.correctedAccel_B[ZACCEL] = (real)gEKFInputData.accel_B[Z_AXIS] * (real)GRAVITY; + gKalmanFilter.correctedAccel_B[XACCEL] = gEKFInput.accel_B[X_AXIS]; + gKalmanFilter.correctedAccel_B[YACCEL] = gEKFInput.accel_B[Y_AXIS]; + gKalmanFilter.correctedAccel_B[ZACCEL] = gEKFInput.accel_B[Z_AXIS]; } // ================= Attitude quaternion ================= // Find the 'true' angular rate (wTrue_B = wCorr_B = wMeas_B - wBias_B) - gKalmanFilter.correctedRate_B[X_AXIS] = gEKFInputData.angRate_B[X_AXIS] - + gKalmanFilter.correctedRate_B[X_AXIS] = gEKFInput.angRate_B[X_AXIS] - gKalmanFilter.rateBias_B[X_AXIS]; - gKalmanFilter.correctedRate_B[Y_AXIS] = gEKFInputData.angRate_B[Y_AXIS] - + gKalmanFilter.correctedRate_B[Y_AXIS] = gEKFInput.angRate_B[Y_AXIS] - gKalmanFilter.rateBias_B[Y_AXIS]; - gKalmanFilter.correctedRate_B[Z_AXIS] = gEKFInputData.angRate_B[Z_AXIS] - + gKalmanFilter.correctedRate_B[Z_AXIS] = gEKFInput.angRate_B[Z_AXIS] - gKalmanFilter.rateBias_B[Z_AXIS]; // Placed in gKalmanFilter as wTrueTimesDtOverTwo is used to compute the Jacobian (F) @@ -637,7 +658,7 @@ static void _UpdateProcessJacobian(void) static void _UpdateProcessCovariance(void) { // Variables used to initially populate the Q-matrix - real arw, biSq[3] = {(real)1.0e-10, (real)1.0e-10, (real)1.0e-10}; + real biSq[3] = {(real)1.0e-10, (real)1.0e-10, (real)1.0e-10}; real sigDriftDot; // Variables used to populate the Q-matrix each time-step @@ -651,83 +672,16 @@ static void _UpdateProcessCovariance(void) { initQ_HG = FALSE; -#ifdef INS_OFFLINE - /* This value is set based on the version string specified in the - * simulation configuration file, ekfSim.cfg - */ - uint8_t sysRange = gSimulation.sysRange; - uint8_t rsType = gSimulation.rsType; -#else - // This value is set based on the version string loaded into the unit - // via the system configuration load - uint8_t sysRange = platformGetSysRange(); // from system config - uint8_t rsType = BMI_RS; -#endif - - /* Set the matrix, Q, based on whether the system is a -200 or -400 - * Q-values are based on the rate-sensor's ARW (noise) and BI values - * passed through the process model. - */ - switch (sysRange) - { - case _200_DPS_RANGE: // same as default - // Bias-stability value for the rate-sensors - if (rsType == BMI_RS) - { - // BMI: 1.508e-3 [deg/sec] = 2.63e-5 [rad/sec] - biSq[X_AXIS] = (real)(6.92e-10); // (2.63e-5)^2 - biSq[Y_AXIS] = biSq[X_AXIS]; - biSq[Z_AXIS] = biSq[X_AXIS]; - } - else - { - // Maxim x/y: 1.85e-3 [deg/sec] = 3.24E-05 [rad/sec] - // Maxim z: 7.25e-4 [deg/sec] = 1.27E-05 [rad/sec] - biSq[X_AXIS] = (real)(1.05e-9); // (3.25e-5)^2 - biSq[Y_AXIS] = biSq[X_AXIS]; - biSq[Z_AXIS] = (real)(1.61e-10); // (1.27e-5)^2 - } - break; - - // -400 values - case _400_DPS_RANGE: - // bi in [rad] - if (rsType == BMI_RS) - { - // BMI: 6 [deg/hr] = 1.7e-3 [deg/sec] = 2.91e-5 [rad/sec] - biSq[X_AXIS] = (real)(8.46e-10); // (2.91e-5)^2 - biSq[Y_AXIS] = biSq[X_AXIS]; - biSq[Z_AXIS] = biSq[X_AXIS]; - } - else - { - // Maxim x/y: 2.16e-3 [deg/sec] = 3.24E-05 [rad/sec] - // Maxim z: 1.07e-3 [deg/sec] = 1.86E-05 [rad/sec] - biSq[X_AXIS] = (real)(1.41e-09); // (3.24e-5)^2 - biSq[Y_AXIS] = biSq[X_AXIS]; - biSq[Z_AXIS] = (real)(3.46e-10); // (1.86e-5)^2 - } - break; - } - - // ARW is unaffected by range setting, 200/400 (or it seems). Value is in - // units of [rad/rt-sec] - if (rsType == BMI_RS) - { - // BMI: 4.39e-3 [deg/rt-sec] = 7.66e-5 [rad/rt-sec] - arw = (real)7.66e-5; - } - else - { - // Maxim: 5.63e-3 [deg/rt-sec] = 9.82E-05 [rad/rt-sec] - arw = (real)9.82e-5; - } + // squated gyro bias instability + biSq[X_AXIS] = gAlgorithm.imuSpec.biW * gAlgorithm.imuSpec.biW; + 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 */ - sigDriftDot = (real)9.064720283654388 / arw; + sigDriftDot = (real)9.064720283654388 / gAlgorithm.imuSpec.arw; // Rate-bias terms (Q is ultimately the squared value, which is done in the second line of the assignment) gKalmanFilter.Q[STATE_WBX] = sigDriftDot * biSq[X_AXIS] * gAlgorithm.dt; @@ -738,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)6.839373353251920e-10; - gKalmanFilter.Q[STATE_ABY] = (real)6.839373353251920e-10; - gKalmanFilter.Q[STATE_ABZ] = (real)6.839373353251920e-10; + // 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 ) @@ -748,7 +709,7 @@ static void _UpdateProcessCovariance(void) * = 0.5 * sqrt(dt) * sqrt(dt) * ( ARW / sqrt(dt) ) * = 0.5 * sqrt(dt) * ARW */ - multiplier_Q = (real)(0.5) * gAlgorithm.sqrtDt * arw; + multiplier_Q = (real)(0.5) * gAlgorithm.sqrtDt * gAlgorithm.imuSpec.arw; multiplier_Q_Sq = multiplier_Q * multiplier_Q; } @@ -771,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_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_WBZ] = (real)1.0e-3 * gKalmanFilter.Q[STATE_WBZ];*/ } } @@ -782,6 +743,17 @@ static void _UpdateProcessCovariance(void) * block of the Q-matrix). The rest of the elements in the matrix are set * during the transition into and between EKF states (high-gain, low-gain, * etc) or above (upon first entry into this function). + * The process cov matrix of quaternion is + * [1-q0*q0 -q0*q1 -q0*q2 -q0*q3; + * -q0*q1 1-q1*q1 -q1*q2 -q1*q3; + * -q0*q2 -q1*q2 1-q2*q2 -q2*q3; + * -q0*q3 -q1*q3 -q2*q3 1-q3*q3] * (0.5*dt*sigma_gyro)^2 + * The eigenvalue of the matrix is [1 1 1 1-q0^2-q1^2-q2^2-q3^2], which means it + * is not positive defintie when quaternion norm is above or equal to 1. Quaternion + * norm can be above 1 due to numerical accuray. A scale factor 0.99 is added here to + * make sure the positive definiteness of the covariance matrix. The eigenvalues now + * are [1 1 1 1-0.99*(q0^2+q1^2+q2^2+q3^2)]. Even if there is numerical accuracy issue, + * the cov matrix is still positive definite. */ real q0q0 = gKalmanFilter.quaternion_Past[Q0] * gKalmanFilter.quaternion_Past[Q0] * 0.99f; real q0q1 = gKalmanFilter.quaternion_Past[Q0] * gKalmanFilter.quaternion_Past[Q1] * 0.99f; @@ -799,9 +771,13 @@ static void _UpdateProcessCovariance(void) // Note: this block of the covariance matrix is symmetric real tmpQMultiplier = multiplier_Q_Sq; - if ( gAlgorithm.state == INS_SOLUTION ) + /* Only considering gyro noise can underestimate the cov of the quaternion. + * A scale factor 100 is added here. This is mainly for faster convergence + * of the heading angle in the INS solution. + */ + if (gAlgorithm.state == INS_SOLUTION) { - tmpQMultiplier = 100.0 * multiplier_Q_Sq; + tmpQMultiplier = 1.0f * multiplier_Q_Sq; } gKalmanFilter.Q[STATE_Q0] = ((real)1.0 - q0q0) * tmpQMultiplier; gKalmanFilter.Qq[0] = (-q0q1) * tmpQMultiplier; @@ -831,7 +807,7 @@ void GenerateProcessCovariance(void) */ // Acceleration based values - real dtSigAccelSq = (real)(gAlgorithm.dt * SENSOR_NOISE_ACCEL_STD_DEV); + real dtSigAccelSq = (real)(gAlgorithm.dt * gAlgorithm.imuSpec.sigmaA); dtSigAccelSq = dtSigAccelSq * dtSigAccelSq; // Position @@ -839,17 +815,99 @@ void GenerateProcessCovariance(void) gKalmanFilter.Q[STATE_RY] = gAlgorithm.dtSquared * dtSigAccelSq; gKalmanFilter.Q[STATE_RZ] = gAlgorithm.dtSquared * dtSigAccelSq; - // Velocity + /* Velocity, todo. 100 is to take under-estimated accel bias, gyro bias and + * attitude error since none of them is Gaussian. Non-Gaussian error produces + * velocity drift. High-freq vibration can also be handled by this. + */ 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/OpenIMU300RI/IMU/lib/Core/Algorithm/src/SelectState.c b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/SelectState.c index 3513336..0fe20ee 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/SelectState.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/SelectState.c @@ -11,6 +11,7 @@ #include "GlobalConstants.h" #include "platformAPI.h" +#include "TimingVars.h" #include "algorithm.h" // gAlgorithm #include "algorithmAPI.h" @@ -22,7 +23,6 @@ #include "TransformationMath.h" // FieldVectorsToEulerAngles #include "EKF_Algorithm.h" #include "PredictFunctions.h" -#include "TimingVars.h" #ifndef INS_OFFLINE #ifdef DISPLAY_DIAGNOSTIC_MSG @@ -45,8 +45,6 @@ static BOOL _AverageFieldVectors(uint16_t pointsToAverage); static void _DropToHighGainAHRS(void); static void _ResetAlgorithm(void); -static uint8_t _InitINSFilter(real* leverArmN); - // StabilizeSystem: Run for a prescribed period to let the sensors settle. void StabilizeSystem(void) { @@ -60,11 +58,11 @@ void StabilizeSystem(void) */ if (gAlgorithm.stateTimer == 0) { #ifdef INS_OFFLINE - printf("To ini att. %u\n", gEKFInputData.itow); + printf("To ini att. %u\n", gEKFInput.itow); #else #ifdef DISPLAY_DIAGNOSTIC_MSG DebugPrintString("To ini att. "); - DebugPrintInt("", gEKFInputData.itow); + DebugPrintInt("", gEKFInput.itow); DebugPrintEndline(); #endif #endif @@ -111,9 +109,9 @@ void InitializeAttitude(void) /* Quasi-static check: check for motion over threshold. If detected, reset * the accumulation variables and restart initialization phase. */ - if ((fabs(gEKFInputData.angRate_B[X_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE) || - (fabs(gEKFInputData.angRate_B[Y_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE) || - (fabs(gEKFInputData.angRate_B[Z_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE)) + if ((fabs(gEKFInput.angRate_B[X_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE) || + (fabs(gEKFInput.angRate_B[Y_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE) || + (fabs(gEKFInput.angRate_B[Z_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE)) { accumulatedAccelVector[X_AXIS] = (real)0.0; accumulatedAccelVector[Y_AXIS] = (real)0.0; @@ -136,11 +134,11 @@ void InitializeAttitude(void) */ if (gAlgorithm.stateTimer == 0) { #ifdef INS_OFFLINE - printf("To HG. %u\n", gEKFInputData.itow); + printf("To HG. %u\n", gEKFInput.itow); #else #ifdef DISPLAY_DIAGNOSTIC_MSG DebugPrintString("To HG. "); - DebugPrintInt("", gEKFInputData.itow); + DebugPrintInt("", gEKFInput.itow); DebugPrintEndline(); #endif #endif @@ -187,6 +185,10 @@ void InitializeAttitude(void) */ EulerAnglesToQuaternion( gKalmanFilter.measuredEulerAngles, gKalmanFilter.quaternion ); + gKalmanFilter.quaternion_Past[0] = gKalmanFilter.quaternion[0]; + gKalmanFilter.quaternion_Past[1] = gKalmanFilter.quaternion[1]; + gKalmanFilter.quaternion_Past[2] = gKalmanFilter.quaternion[2]; + gKalmanFilter.quaternion_Past[3] = gKalmanFilter.quaternion[3]; // Euler angles from the initial measurement (DEBUG: initial output of the system) gKalmanFilter.eulerAngles[ROLL] = gKalmanFilter.measuredEulerAngles[ROLL]; gKalmanFilter.eulerAngles[PITCH] = gKalmanFilter.measuredEulerAngles[PITCH]; @@ -242,11 +244,11 @@ void HG_To_LG_Transition_Test(void) */ if (gAlgorithm.stateTimer == 0) { #ifdef INS_OFFLINE - printf("To LG. %u\n", gEKFInputData.itow); + printf("To LG. %u\n", gEKFInput.itow); #else #ifdef DISPLAY_DIAGNOSTIC_MSG DebugPrintString("To LG. "); - DebugPrintInt("", gEKFInputData.itow); + DebugPrintInt("", gEKFInput.itow); DebugPrintEndline(); #endif #endif @@ -292,14 +294,14 @@ void LG_To_INS_Transition_Test(void) /* If GPS output is valid (GPS providing data with a good signal lock) * then transit to INS mode. */ - if ( gpsUsedInAlgorithm() && gEKFInputData.gpsUpdate ) + if ( gpsUsedInAlgorithm() && gEKFInput.gpsUpdate && gEKFInput.gpsFixType ) { #ifdef INS_OFFLINE - printf("To INS. %u\n", gEKFInputData.itow); + printf("To INS. %u\n", gEKFInput.itow); #else #ifdef DISPLAY_DIAGNOSTIC_MSG DebugPrintString("To INS. "); - DebugPrintInt("", gEKFInputData.itow); + DebugPrintInt("", gEKFInput.itow); DebugPrintEndline(); #endif #endif @@ -310,43 +312,10 @@ void LG_To_INS_Transition_Test(void) // Transit to INS solution gAlgorithm.state = INS_SOLUTION; - // Sync the algorithm and GPS ITOW - gAlgorithm.itow = gEKFInputData.itow; - - /* We have a good GPS reading now - set this variable so we - * don't drop into INS right away - */ - gAlgorithm.timeOfLastGoodGPSReading = gEKFInputData.itow; - - /* Prepare for INS. - * R_NinE and rGPS_E are used later. rGPS_E is used in _InitINSFilter to initialize rGPS0_E. - * R_NinE is in UpdateFunctions.c to convert position to ECEF. R_NinE is also calculated in - * UpdateFunctions.c. But it is necessary here because it is needed to calculate position - * after switching to INS and before the next GNSS measurement to trigger a Kalman update. - */ - LLA_To_Base(&gEKFInputData.llaRad[0], - &gAlgorithm.rGPS0_E[0], - &gAlgorithm.rGPS_N[0], - &gAlgorithm.R_NinE[0][0], - &gAlgorithm.rGPS_E[0]); - - /* Lever-arm is antenna position w.r.t to IMU in body. rGPS_N is antenna positive - * w.r.t to initial point in NED. IMU positive w.r.t initial point is - * rGPS_N - R_b_to_N * lever-arm - */ - float leverArmN[3]; - leverArmN[0] = gKalmanFilter.R_BinN[0][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[0][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[0][2] * gAlgorithm.leverArmB[2]; - leverArmN[1] = gKalmanFilter.R_BinN[1][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[1][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[1][2] * gAlgorithm.leverArmB[2]; - leverArmN[2] = gKalmanFilter.R_BinN[2][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[2][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[2][2] * gAlgorithm.leverArmB[2]; + gAlgoStatus.bit.attitudeOnlyAlgorithm = FALSE; // Initialize the algorithm with GNSS position and lever arm - _InitINSFilter(leverArmN); + InitINSFilter(); // Set linear-acceleration switch variables gAlgorithm.linAccelSwitchCntr = 0; @@ -354,103 +323,6 @@ void LG_To_INS_Transition_Test(void) } } - - - -// -static uint8_t _InitINSFilter(real* leverArmN) -{ - real tmp[7][7]; - int rowNum, colNum; - - gAlgorithm.insFirstTime = FALSE; - - /* Upon the first entry into INS, save off the base position and reset the - * Kalman filter variables. - */ - // Save off the base ECEF location - gAlgorithm.rGPS0_E[X_AXIS] = gAlgorithm.rGPS_E[X_AXIS]; - gAlgorithm.rGPS0_E[Y_AXIS] = gAlgorithm.rGPS_E[Y_AXIS]; - gAlgorithm.rGPS0_E[Z_AXIS] = gAlgorithm.rGPS_E[Z_AXIS]; - - /* Reset the gps position (as position is relative to starting location) - * rGPS_N is the IMU position, while starting location is the antenna position. - * rGPS_N is reset to lever-arm. - */ - gAlgorithm.rGPS_N[X_AXIS] = -leverArmN[0]; - gAlgorithm.rGPS_N[Y_AXIS] = -leverArmN[1]; - gAlgorithm.rGPS_N[Z_AXIS] = -leverArmN[2]; - - // Reset prediction values. Position_N is also IMU position. - gKalmanFilter.Position_N[X_AXIS] = (real)-leverArmN[0]; - gKalmanFilter.Position_N[Y_AXIS] = (real)-leverArmN[1]; - gKalmanFilter.Position_N[Z_AXIS] = (real)-leverArmN[2]; - - gKalmanFilter.Velocity_N[X_AXIS] = (real)gEKFInputData.vNed[X_AXIS]; - gKalmanFilter.Velocity_N[Y_AXIS] = (real)gEKFInputData.vNed[Y_AXIS]; - gKalmanFilter.Velocity_N[Z_AXIS] = (real)gEKFInputData.vNed[Z_AXIS]; - - gKalmanFilter.accelBias_B[X_AXIS] = (real)0.0; - gKalmanFilter.accelBias_B[Y_AXIS] = (real)0.0; - gKalmanFilter.accelBias_B[Z_AXIS] = (real)0.0; - - /* Extract the Quaternion and rate-bias values from the matrix before - * resetting - */ - // Save off the quaternion and rate-bias covariance values - for (rowNum = Q0; rowNum <= Q3 + Z_AXIS + 1; rowNum++) - { - for (colNum = Q0; colNum <= Q3 + Z_AXIS + 1; colNum++) - { - tmp[rowNum][colNum] = gKalmanFilter.P[rowNum + STATE_Q0][colNum + STATE_Q0]; - } - } - - // Reset P - memset(gKalmanFilter.P, 0, sizeof(gKalmanFilter.P)); - for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) - { - gKalmanFilter.P[rowNum][rowNum] = (real)INIT_P_INS; - } - - // Repopulate the P matrix with the quaternion and rate-bias values - for (rowNum = Q0; rowNum <= Q3 + Z_AXIS + 1; rowNum++) - { - for (colNum = Q0; colNum <= Q3 + Z_AXIS + 1; colNum++) - { - gKalmanFilter.P[rowNum + STATE_Q0][colNum + STATE_Q0] = tmp[rowNum][colNum]; - } - } - - /* Use the GPS-provided horizontal and vertical accuracy values to populate - * the covariance values. - */ - gKalmanFilter.P[STATE_RX][STATE_RX] = gEKFInputData.GPSHorizAcc * gEKFInputData.GPSHorizAcc; - gKalmanFilter.P[STATE_RY][STATE_RY] = gKalmanFilter.P[STATE_RX][STATE_RX]; - gKalmanFilter.P[STATE_RZ][STATE_RZ] = gEKFInputData.GPSVertAcc * gEKFInputData.GPSVertAcc; - - /* Scale the best velocity error by HDOP then multiply by the z-axis angular - * rate PLUS one (to prevent the number from being zero) so the velocity - * update during high-rate turns is reduced. - */ - float temp = (real)0.0625 * gEKFInputData.HDOP; // 0.0625 = 0.05 / 0.8 - real absFilteredYawRate = (real)fabs(gAlgorithm.filteredYawRate); - if (absFilteredYawRate > TEN_DEGREES_IN_RAD) - { - temp *= (1.0f + absFilteredYawRate); - } - gKalmanFilter.P[STATE_VX][STATE_VX] = temp;// *((real)1.0 + fabs(gAlgorithm.filteredYawRate) * (real)RAD_TO_DEG); - gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_VX] * gKalmanFilter.P[STATE_VX][STATE_VX]; - gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.P[STATE_VX][STATE_VX]; - - // z-axis velocity isn't really a function of yaw-rate and hdop - //gKalmanFilter.R[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VX][STATE_VX]; - gKalmanFilter.P[STATE_VZ][STATE_VZ] = (float)(0.1 * 0.1); - - return 1; -} - - /* INS_To_AHRS_Transition_Test: Drop back to LG AHRS operation if... * 1) GPS drops out for more than 3 seconds * 2) magnetometer data not available AND at rest too long @@ -459,21 +331,25 @@ static uint8_t _InitINSFilter(real* leverArmN) void INS_To_AHRS_Transition_Test(void) { // Record last GPS velocity large enough to give a good heading measurement - if (gEKFInputData.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING) + if (gEKFInput.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING) { - gAlgorithm.timeOfLastSufficientGPSVelocity = (int32_t)gEKFInputData.itow; + gAlgorithm.timeOfLastSufficientGPSVelocity = (int32_t)gEKFInput.itow; } /* Determine the length of time it has been since the system 'moved' -- * only linear motion considered (rotations ignored). */ - int32_t timeSinceRestBegan = (int32_t)gEKFInputData.itow - gAlgorithm.timeOfLastSufficientGPSVelocity; + int32_t timeSinceRestBegan = (int32_t)gEKFInput.itow - gAlgorithm.timeOfLastSufficientGPSVelocity; if (timeSinceRestBegan < 0) { timeSinceRestBegan = timeSinceRestBegan + MAX_ITOW; } - if (timeSinceRestBegan > LIMIT_MAX_REST_TIME_BEFORE_HEADING_INVALID) + if (timeSinceRestBegan > LIMIT_MAX_REST_TIME_BEFORE_HEADING_INVALID && gAlgorithm.headingIni != HEADING_UNINITIALIZED) { - gAlgorithm.gnssHeadingFirstTime = TRUE; + gAlgorithm.headingIni = HEADING_GNSS_LOW; +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("Rest for too long."); + DebugPrintEndline(); +#endif } // compute time since the last good GPS reading @@ -482,11 +358,15 @@ void INS_To_AHRS_Transition_Test(void) timeSinceLastGoodGPSReading = timeSinceLastGoodGPSReading + MAX_ITOW; } - if ( timeSinceLastGoodGPSReading > gAlgorithm.Limit.Max_GPS_Drop_Time ) + if ( timeSinceLastGoodGPSReading > gAlgorithm.Limit.maxGpsDropTime ) { +#ifdef INS_OFFLINE + printf("GPS outage too long\n"); +#endif // INS_OFFLINE + // Currently in INS mode but requiring a transition to AHRS / VG gAlgorithm.insFirstTime = TRUE; - gAlgorithm.gnssHeadingFirstTime = TRUE; + gAlgorithm.headingIni = HEADING_UNINITIALIZED; /* The transition from INS to AHRS and back to INS does not seem to * generate a stable solution if we transition to LG AHRS for only 30 @@ -511,7 +391,6 @@ void INS_To_AHRS_Transition_Test(void) gAlgoStatus.bit.highGain = ( gAlgorithm.state == HIGH_GAIN_AHRS ); gAlgoStatus.bit.attitudeOnlyAlgorithm = TRUE; } - } @@ -581,22 +460,22 @@ static void _DropToHighGainAHRS(void) static BOOL _AccumulateFieldVectors(void) { // Accumulate the acceleration vector readings (accels in g's) - accumulatedAccelVector[X_AXIS] += (real)gEKFInputData.accel_B[X_AXIS]; - accumulatedAccelVector[Y_AXIS] += (real)gEKFInputData.accel_B[Y_AXIS]; - accumulatedAccelVector[Z_AXIS] += (real)gEKFInputData.accel_B[Z_AXIS]; + accumulatedAccelVector[X_AXIS] += (real)gEKFInput.accel_B[X_AXIS]; + accumulatedAccelVector[Y_AXIS] += (real)gEKFInput.accel_B[Y_AXIS]; + accumulatedAccelVector[Z_AXIS] += (real)gEKFInput.accel_B[Z_AXIS]; // Accumulate the gyroscope vector readings (accels in rad/s) - accumulatedGyroVector[X_AXIS] += gEKFInputData.angRate_B[X_AXIS]; - accumulatedGyroVector[Y_AXIS] += gEKFInputData.angRate_B[Y_AXIS]; - accumulatedGyroVector[Z_AXIS] += gEKFInputData.angRate_B[Z_AXIS]; + accumulatedGyroVector[X_AXIS] += gEKFInput.angRate_B[X_AXIS]; + accumulatedGyroVector[Y_AXIS] += gEKFInput.angRate_B[Y_AXIS]; + accumulatedGyroVector[Z_AXIS] += gEKFInput.angRate_B[Z_AXIS]; // Accumulate the magnetic-field vector readings (or set to zero if the // product does not have magnetometers) if (magUsedInAlgorithm() ) { - accumulatedMagVector[X_AXIS] += (real)gEKFInputData.magField_B[X_AXIS]; - accumulatedMagVector[Y_AXIS] += (real)gEKFInputData.magField_B[Y_AXIS]; - accumulatedMagVector[Z_AXIS] += (real)gEKFInputData.magField_B[Z_AXIS]; + accumulatedMagVector[X_AXIS] += (real)gEKFInput.magField_B[X_AXIS]; + accumulatedMagVector[Y_AXIS] += (real)gEKFInput.magField_B[Y_AXIS]; + accumulatedMagVector[Z_AXIS] += (real)gEKFInput.magField_B[Z_AXIS]; } else { accumulatedMagVector[X_AXIS] = (real)0.0; accumulatedMagVector[Y_AXIS] = (real)0.0; diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/TimingVars.c b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/TimingVars.c index 00ba3dc..f4de8d9 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/TimingVars.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/TimingVars.c @@ -19,9 +19,6 @@ limitations under the License. #include "TimingVars.h" -#ifdef INS_OFFLINE -#include "SimulationParameters.h" -#endif TimingVars timer; // for InitTimingVars @@ -161,14 +158,11 @@ void Initialize_Timing(void) // toggles between 0 and 1 at 200 Hz (currently used in firmware) timer.oneHundredHertzFlag = 0; -#ifdef INS_OFFLINE - // This value is set based on the version string specified in the - // simulation configuration file, ekfSim.cfg - // IMU: 0/1, VG: 2, AHRS: 3, Aided-VG: 4, Aided-AHRS: 5, INS: 6 - uint8_t sysType = gSimulation.sysType; -#endif - // Override execution rate of taskDataAcquisition() based on the configuration timer.dacqFrequency = DACQ_200_HZ; // default } +void TimingVars_dacqFrequency(int freq) +{ + timer.dacqFrequency = freq; +} diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/UpdateFunctions.c b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/UpdateFunctions.c index ab4beb1..2759483 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/UpdateFunctions.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/UpdateFunctions.c @@ -24,20 +24,19 @@ #include "algorithm.h" #include "algorithmAPI.h" #include "AlgorithmLimits.h" +#include "MotionStatus.h" #include "EKF_Algorithm.h" #include "UpdateFunctions.h" #include "SensorNoiseParameters.h" -extern AccelStatsStruct gAccelStats; +extern ImuStatsStruct gImuStats; #if FAST_MATH #include "arm_math.h" #endif // FAST_MATH -#ifdef INS_OFFLINE -#include "SimulationParameters.h" -#else +#ifndef INS_OFFLINE #ifdef DISPLAY_DIAGNOSTIC_MSG #include "debug.h" #endif @@ -63,20 +62,36 @@ 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 + * heading is considered valid, and the eading will be initialized to be + * gEKFInput.trueCourse, and velocity will also be initiazlied as the + * corresponding NED speed. After this, the quaternion (q0 and q3) and velocity + * terms in the state covariance matrix P will be reset. Non-diagonal terms will be + * set as 0s, and diagonal terms will be set according to estimated variance. The + * cov(quaternion, velocity) should also be updated. But the positive-definiteness + * is not guaranteed this way. + * TRACE: + * @retval TRUE if heading initialized/reinitialized, FALSE if not. +******************************************************************************/ +static int InitializeHeadingFromGnss(); + /****************************************************************************** - * @brief Initializa heading when GPS velocity is above a certain threshhold. - * If the GPS velocity is above a certain threshold, a global variable useGpsHeading - * will be set to 1. And in this function, the heading will be initialized to be - * gEKFInputData.trueCourse, and velocity will also be initiazlied as the + * @brief When heading is ready for initialization, the heading angle (yaw, and + * indeed quaternion in the Kalman filter) is initialized to match the value of + * gEKFInput.trueCourse, and velocity will also be initiazlied as the * corresponding NED speed. After this, the quaternion (q0 and q3) and velocity * terms in the state covariance matrix P will be reset. Non-diagonal terms will be * set as 0s, and diagonal terms will be set according to estimated variance. The * cov(quaternion, velocity) should also be updated. But the positive-definiteness * is not guaranteed this way. * TRACE: - * @retval TRUE if heading initialized, FALSE if not. + * @retval None. ******************************************************************************/ -static int InitializeGnssHeading(); +static void InitializeEkfHeading(); // Update rates #define TEN_HERTZ_UPDATE 10 @@ -85,25 +100,28 @@ static int InitializeGnssHeading(); #define FIFTY_HERTZ_UPDATE 50 #define ONE_HUNDRED_HERTZ_UPDATE 100 -static uint32_t updateCntr[2] = { 0, 0 }; static BOOL useGpsHeading = 0; /* When GPS velocity is above a certain threshold, * this is set to 1, and GPS heading measurement * is used, otherwise, this is set to 0 and magnetic - * heading is used. */ + * heading is used. + */ static int runInsUpdate = 0; /* To enable the update to be broken up into * two sequential calculations in two sucessive - * 100 Hz periods. */ + * 100 Hz periods. + */ // Uncomment to run only AHRS-type updates //#define ATT_UPDATE_ONLY static void Update_GPS(void); +static void Update_PseudoMeasurement(void); +static void GenPseudoMeasCov(real *r); // EKF_UpdateStage.m void EKF_UpdateStage(void) { /* Perform a VG/AHRS update, regardless of GPS availability or health, - * when the state is HG AHRS or LG AHRS. Once GPS becomes healthy + * when the state is HG AHRS or LG AHRS. Once GPS becomes healthy * (and the right conditions are met) perform an INS or reduced-order GPS update. */ if( gAlgorithm.state <= LOW_GAIN_AHRS ) @@ -131,12 +149,76 @@ void EKF_UpdateStage(void) * Check for 'new GPS data'. If new, and GPS is valid, perform a * GPS-Based update and reset timer values to resync the attitude updates. */ - if( gEKFInputData.gpsUpdate ) - { - if (gEKFInputData.gpsValid) + if( gEKFInput.gpsUpdate ) + { + /* 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; + + // GNSS update + if (gEKFInput.gpsFixType) + { + // GPS heading valid? + 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 + * good, just correct the filter states with input GNSS measurement. + */ + int32_t timeSinceLastGoodGPSReading = (int32_t)gAlgorithm.itow - gAlgorithm.timeOfLastGoodGPSReading; + if (timeSinceLastGoodGPSReading < 0) + { + timeSinceLastGoodGPSReading = timeSinceLastGoodGPSReading + MAX_ITOW; + } + if (timeSinceLastGoodGPSReading > gAlgorithm.Limit.maxReliableDRTime) + { +#ifdef INS_OFFLINE + printf("GPS relocked.\n"); +#endif // INS_OFFLINE + // Since a relative long time has passed since DR begins, INS states need reinitialized. + InitINSFilter(); + } + else + { + // DR for a relative short time, no need to reinitialize the filter. + Update_GPS(); + } + // reset the "last good reading" time + gAlgorithm.timeOfLastGoodGPSReading = gEKFInput.itow; + } + + // apply motion constraints + if (gAlgorithm.velocityAlwaysAlongBodyX && gAlgorithm.headingIni>HEADING_UNINITIALIZED) { - Update_GPS(); + Update_PseudoMeasurement(); } + // At 1 Hz mark, update when GPS data is valid, else do an AHRS-update runInsUpdate = 1; } @@ -144,33 +226,12 @@ void EKF_UpdateStage(void) { Update_Att(); runInsUpdate = 0; // set up for next pass - useGpsHeading = 0; - } - - // Update LLA at 100/200 Hz - if ( gEKFInputData.gpsValid && ( gAlgorithm.insFirstTime == FALSE ) ) - { - //r_E = Base_To_ECEF( &gKalmanFilter.Position_N[0], &gAlgorithm.rGPS0_E[0], &R_NinE[0][0] ); // - double r_E[NUM_AXIS]; - float pointOfInterestN[3]; - pointOfInterestN[0] = gKalmanFilter.R_BinN[0][0] * gAlgorithm.pointOfInterestB[0] + - gKalmanFilter.R_BinN[0][1] * gAlgorithm.pointOfInterestB[1] + - gKalmanFilter.R_BinN[0][2] * gAlgorithm.pointOfInterestB[2]; - pointOfInterestN[1] = gKalmanFilter.R_BinN[1][0] * gAlgorithm.pointOfInterestB[0] + - gKalmanFilter.R_BinN[1][1] * gAlgorithm.pointOfInterestB[1] + - gKalmanFilter.R_BinN[1][2] * gAlgorithm.pointOfInterestB[2]; - pointOfInterestN[2] = gKalmanFilter.R_BinN[2][0] * gAlgorithm.pointOfInterestB[0] + - gKalmanFilter.R_BinN[2][1] * gAlgorithm.pointOfInterestB[1] + - gKalmanFilter.R_BinN[2][2] * gAlgorithm.pointOfInterestB[2]; - pointOfInterestN[0] += gKalmanFilter.Position_N[0]; - pointOfInterestN[1] += gKalmanFilter.Position_N[1]; - pointOfInterestN[2] += gKalmanFilter.Position_N[2]; - PosNED_To_PosECEF(pointOfInterestN, &gAlgorithm.rGPS0_E[0], &gAlgorithm.R_NinE[0][0], &r_E[0] ); - // 100 Hz generated once 1 Hz 100 Hz - - //gKalmanFilter.llaDeg[LAT_IDX] = ECEF2LLA( r_E ); // output variable (ned used for anything else); this is in [ deg, deg, m ] - ECEF_To_LLA(&gKalmanFilter.llaDeg[LAT], &r_E[X_AXIS]); - // 100 Hz 100 Hz + // handle P when PPS is available + if (gAlgoStatus.bit.ppsAvailable) + { + ApplyGpsDealyCorrForStateCov(); + gAlgoStatus.bit.ppsAvailable = FALSE; + } } } } @@ -179,9 +240,18 @@ void EKF_UpdateStage(void) void ComputeSystemInnovation_Pos(void) { // Position error - gKalmanFilter.nu[STATE_RX] = gAlgorithm.rGPS_N[X_AXIS] - gKalmanFilter.Position_N[X_AXIS]; - gKalmanFilter.nu[STATE_RY] = gAlgorithm.rGPS_N[Y_AXIS] - gKalmanFilter.Position_N[Y_AXIS]; - gKalmanFilter.nu[STATE_RZ] = gAlgorithm.rGPS_N[Z_AXIS] - gKalmanFilter.Position_N[Z_AXIS]; + 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); @@ -193,9 +263,18 @@ void ComputeSystemInnovation_Pos(void) void ComputeSystemInnovation_Vel(void) { // Velocity error - gKalmanFilter.nu[STATE_VX] = (real)gEKFInputData.vNed[X_AXIS] - gKalmanFilter.Velocity_N[X_AXIS]; - gKalmanFilter.nu[STATE_VY] = (real)gEKFInputData.vNed[Y_AXIS] - gKalmanFilter.Velocity_N[Y_AXIS]; - gKalmanFilter.nu[STATE_VZ] = (real)gEKFInputData.vNed[Z_AXIS] - gKalmanFilter.Velocity_N[Z_AXIS]; + 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); @@ -224,8 +303,24 @@ void ComputeSystemInnovation_Att(void) // CHANGED TO SWITCH BETWEEN GPS AND MAG UPDATES if ( useGpsHeading ) { - gKalmanFilter.nu[STATE_YAW] = (real)gEKFInputData.trueCourse * (real)DEG_TO_RAD - - gKalmanFilter.eulerAngles[YAW]; + 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; + } + } else if ( magUsedInAlgorithm() && gAlgorithm.state <= LOW_GAIN_AHRS ) { @@ -368,7 +463,6 @@ uint8_t _GenerateObservationJacobian_AHRS(void) // void _GenerateObservationCovariance_AHRS(void) { - // static real Rnom; // Only need to compute certain elements of R once @@ -382,48 +476,12 @@ void _GenerateObservationCovariance_AHRS(void) */ memset(gKalmanFilter.R, 0, sizeof(gKalmanFilter.R)); -#ifdef INS_OFFLINE - /* This value is set based on the version string specified in the - * simulation configuration file, ekfSim.cfg - */ - uint8_t sysRange = gSimulation.sysRange; -#else - /* This value is set based on the version string loaded into the unit - * via the system configuration load - */ - uint8_t sysRange = platformGetSysRange(); // from system config -#endif - - /* Set the matrix, R, based on whether the system is operating in high - * or low-gain (is the acceleration above or below the acceleration threshold) - * - * R-values are based on the variance of the roll and pitch angles - * generated from the sensor noise passed through the measurement - * model. The values are multiplied by dt^2 as well as vary with the - * angle. The value can be made large enough to work with all angles - * but it may slow the response. - * - * These values are found by passing the accelerometer VRW values - * (determined from a very limited data set) through a Matlab - * script which generates the roll and pitch noise based on the - * sensor noise. The value below is the 1-sigma value at 0 - * degrees. The quadratic correction below is meant to increase - * R as the angle increases (due to the geometry and - * mathematical function used to compute the angle). - * - * Matlab script: R_Versus_Theta.m + /* Calculate accel var when static from IMU specs. + * This accel var is the min accel var. If real-time accel var is below this value, + * the min accel var is used. + * Accel var is further converted to Euler angels measurement var. */ - switch (sysRange) - { - case _200_DPS_RANGE: - // -200 VRW value (average x/y/z): 7.2e-4 [(m/sec)/rt-sec] - Rnom = (real)9.82e-07; // (9.91e-4)^2 - break; - case _400_DPS_RANGE: - // -400 VRW value (average x/y/z): 8.8e-4 [(m/sec)/rt-sec] - Rnom = (real)1.54e-06; // (1.24e-3)^2 - break; - } + Rnom = gAlgorithm.imuSpec.sigmaA * gAlgorithm.imuSpec.sigmaA; } /* Dynamically tune measurement covariance matrix R to get proper Kalman filter @@ -433,13 +491,13 @@ void _GenerateObservationCovariance_AHRS(void) */ // Rnom, accel var and accel error - real totalAccelVar[3]; + real totalAccelVar[3]; // [m/s/s]^2 for (int i = 0; i < 3; i++) { // replace sensor noise var with vibration var - if (gAccelStats.accelVar[i] > Rnom) + if (gImuStats.accelVar[i] > Rnom) { - totalAccelVar[i] = gAccelStats.accelVar[i]; + totalAccelVar[i] = gImuStats.accelVar[i]; } else { @@ -447,7 +505,7 @@ void _GenerateObservationCovariance_AHRS(void) } // linear accel? (including noise and vibration) real errSqr; - errSqr = gAccelStats.accelErr[i] * gAccelStats.accelErr[i]; + errSqr = gImuStats.accelErr[i] * gImuStats.accelErr[i]; if (errSqr > totalAccelVar[i]) { totalAccelVar[i] = errSqr; @@ -458,13 +516,13 @@ void _GenerateObservationCovariance_AHRS(void) * Notice: totalAccelVarSum just approximates accel norm var */ real totalAccelVarSum = totalAccelVar[X_AXIS] + totalAccelVar[Y_AXIS] + totalAccelVar[Z_AXIS]; - real diff = gAccelStats.accelNorm - 1.0f; + real diff = gImuStats.accelNorm - GRAVITY; diff *= diff; real additionalR = 0.0; /* if diff is larger than estimated accel err and the estimated accel err does * not reach limit, diff will be used as additional measurement noise var. */ - if (diff > 4.0*totalAccelVarSum && gAccelStats.accelErrLimit == false) + if (diff > 4.0*totalAccelVarSum && gImuStats.accelErrLimit == false) { // the magnitude of diff is too big, there is linear acceleration additionalR = diff; @@ -481,9 +539,9 @@ void _GenerateObservationCovariance_AHRS(void) * Notice: var(kx) = k*k*var(x) */ // Get ax^2, ay^2 and az^2 of normalized accel - real axSqr = gAccelStats.lpfAccel[0] * gAccelStats.lpfAccel[0]; - real aySqr = gAccelStats.lpfAccel[1] * gAccelStats.lpfAccel[1]; - real azSqr = gAccelStats.lpfAccel[2] * gAccelStats.lpfAccel[2]; + real axSqr = gImuStats.lpfAccel[0] * gImuStats.lpfAccel[0]; + real aySqr = gImuStats.lpfAccel[1] * gImuStats.lpfAccel[1]; + real azSqr = gImuStats.lpfAccel[2] * gImuStats.lpfAccel[2]; real sumSqr = axSqr + aySqr + azSqr; axSqr /= sumSqr; aySqr /= sumSqr; @@ -505,7 +563,20 @@ void _GenerateObservationCovariance_AHRS(void) gKalmanFilter.R[STATE_ROLL] += additionalR; gKalmanFilter.R[STATE_PITCH] += additionalR; - // limit R + /* We are indeed using var of multiple accel samples to estimate the var of Euler + * angles. From the formula above, accel var should be var of normalized accel. + * However, we choose GRAVITY instead of real accel norm to normalize the accel. + * Besides, accel var is only an estimate of Euler angles var, and Euler angels + * var is indeed not Gaussian. + */ + gKalmanFilter.R[STATE_ROLL] /= GRAVITY * GRAVITY; + gKalmanFilter.R[STATE_PITCH] /= GRAVITY * GRAVITY; + + /* limit R + * In previous version, Rnom is in untis of [g]^2, and maxR = 40000.0f*Rnom. + * 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; if (gKalmanFilter.R[STATE_ROLL] > maxR) { @@ -550,7 +621,7 @@ void _GenerateObservationCovariance_AHRS(void) */ if ( useGpsHeading ) { - float temp = (float)atan( 0.05 / gEKFInputData.rawGroundSpeed ); + float temp = (float)atan( 0.05 / gEKFInput.rawGroundSpeed ); gKalmanFilter.R[STATE_YAW] = temp * temp; if (gAlgoStatus.bit.turnSwitch) { @@ -583,7 +654,7 @@ void _GenerateObservationCovariance_AHRS(void) } else { - gKalmanFilter.R[STATE_YAW] = (real)0.1; + gKalmanFilter.R[STATE_YAW] = (real)1.0; } } @@ -608,15 +679,15 @@ void _GenerateObservationCovariance_INS(void) /* Use the GPS-provided horizontal and vertical accuracy values to populate * the covariance values. */ - gKalmanFilter.R[STATE_RX] = gEKFInputData.GPSHorizAcc * gEKFInputData.GPSHorizAcc; + gKalmanFilter.R[STATE_RX] = gEKFInput.GPSHorizAcc * gEKFInput.GPSHorizAcc; gKalmanFilter.R[STATE_RY] = gKalmanFilter.R[STATE_RX]; - gKalmanFilter.R[STATE_RZ] = gEKFInputData.GPSVertAcc * gEKFInputData.GPSVertAcc; + gKalmanFilter.R[STATE_RZ] = gEKFInput.GPSVertAcc * gEKFInput.GPSVertAcc; /* Scale the best velocity error by HDOP then multiply by the z-axis angular * rate PLUS one (to prevent the number from being zero) so the velocity * update during high-rate turns is reduced. */ - float temp = (real)0.0625 * gEKFInputData.HDOP; // 0.0625 = 0.05 / 0.8 + float temp = (real)0.0625 * gEKFInput.HDOP; // 0.0625 = 0.05 / 0.8 real absFilteredYawRate = (real)fabs(gAlgorithm.filteredYawRate); if (absFilteredYawRate > TEN_DEGREES_IN_RAD) { @@ -625,6 +696,14 @@ void _GenerateObservationCovariance_INS(void) gKalmanFilter.R[STATE_VX] = temp;// *((real)1.0 + fabs(gAlgorithm.filteredYawRate) * (real)RAD_TO_DEG); gKalmanFilter.R[STATE_VX] = gKalmanFilter.R[STATE_VX] * gKalmanFilter.R[STATE_VX]; gKalmanFilter.R[STATE_VY] = gKalmanFilter.R[STATE_VX]; + if (gAlgorithm.headingIni == HEADING_UNINITIALIZED) + { + /* When heading is not initialized, velocity measurement is not able to correct + * attitude/rate bias/accel bias, the larger the velocity, the more uncertain it is. + */ + gKalmanFilter.R[STATE_VX] += SQUARE(gEKFInput.vNed[0]) + SQUARE(gEKFInput.vNed[1]); + gKalmanFilter.R[STATE_VY] += gKalmanFilter.R[STATE_VX]; + } // z-axis velocity isn't really a function of yaw-rate and hdop gKalmanFilter.R[STATE_VZ] = (float)(0.1 * 0.1); @@ -637,10 +716,10 @@ 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) { + static real lastYaw = 7.0; // a values larger than 2pi means this yaw is invalid // which state is updated in Update_Att() uint8_t updatedStatesAtt[16] = { 1, 1, 1, // Positions are not updated 1, 1, 1, // Velocities are not updated @@ -653,14 +732,67 @@ void Update_Att(void) */ _GenerateObservationJacobian_AHRS(); // gKF.H: 3x16 _GenerateObservationCovariance_AHRS(); // gKF.R: 3x3 - // If neither mag or GPS headig is available, update measuremnt matrix H to 2x16 - if (gKalmanFilter.R[STATE_YAW] > 0.9) + + // 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) + { + /* 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 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 + { + 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[2][colNum] = 0.0; + gKalmanFilter.H[0][colNum] = 0.0; + gKalmanFilter.H[1][colNum] = 0.0; } } + /* This solution consists of an integrated roll/pitch/yaw solution * S = H*P*HTrans + R (However the matrix math can be simplified since * H is very sparse! P is fully populated) @@ -751,9 +883,6 @@ void Update_Att(void) /* Update covariance: P = P + DP = P - K*H*P * KxH = gKF.K * gKF.H; */ - /* 2) Use gKalmanFilter.P as a temporary variable to hold FxPxFTranspose - * to reduce the number of "large" variables on the heap - */ memset(KxH, 0, sizeof(KxH)); for (rowNum = 0; rowNum < ROWS_IN_K; rowNum++) { @@ -768,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 */ @@ -779,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 @@ -804,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 @@ -819,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]; } } @@ -831,7 +960,7 @@ void Update_Att(void) */ void Update_Pos(void) { - // which state is updated in Update_Att() + // which state is updated in Update_Pos() uint8_t updatedStatesPos[16] = { 1, 1, 1, // Positions are updated 1, 1, 1, // Velocities are updated 1, 1, 1, 1, // Quaternions are NOT updated @@ -891,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++) { @@ -914,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]; @@ -932,21 +1062,21 @@ void Update_Pos(void) gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_VY]; gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_VZ]; - //gKalmanFilter.quaternion[Q0] = gKalmanFilter.quaternion[Q0] + gKalmanFilter.stateUpdate[STATE_Q0]; - //gKalmanFilter.quaternion[Q1] = gKalmanFilter.quaternion[Q1] + gKalmanFilter.stateUpdate[STATE_Q1]; - //gKalmanFilter.quaternion[Q2] = gKalmanFilter.quaternion[Q2] + gKalmanFilter.stateUpdate[STATE_Q2]; - //gKalmanFilter.quaternion[Q3] = gKalmanFilter.quaternion[Q3] + gKalmanFilter.stateUpdate[STATE_Q3]; - // - //// Normalize quaternion and force q0 to be positive - //QuatNormalize(gKalmanFilter.quaternion); - // - //gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; - //gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; - //gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; - // - //gKalmanFilter.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_ABX]; - //gKalmanFilter.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_ABY]; - //gKalmanFilter.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_ABZ]; + gKalmanFilter.quaternion[Q0] = gKalmanFilter.quaternion[Q0] + gKalmanFilter.stateUpdate[STATE_Q0]; + gKalmanFilter.quaternion[Q1] = gKalmanFilter.quaternion[Q1] + gKalmanFilter.stateUpdate[STATE_Q1]; + gKalmanFilter.quaternion[Q2] = gKalmanFilter.quaternion[Q2] + gKalmanFilter.stateUpdate[STATE_Q2]; + gKalmanFilter.quaternion[Q3] = gKalmanFilter.quaternion[Q3] + gKalmanFilter.stateUpdate[STATE_Q3]; + + // Normalize quaternion and force q0 to be positive + QuatNormalize(gKalmanFilter.quaternion); + + gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; + gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; + gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; + + gKalmanFilter.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_ABX]; + gKalmanFilter.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_ABY]; + gKalmanFilter.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_ABZ]; } @@ -955,7 +1085,7 @@ void Update_Pos(void) */ void Update_Vel(void) { - // which state is updated in Update_Att() + // which state is updated in Update_Vel() uint8_t updatedStatesVel[16] = { 1, 1, 1, // Positions are NOT updated 1, 1, 1, // Velocities are updated 1, 1, 1, 1, // Quaternions are updated @@ -1015,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++) { @@ -1034,21 +1164,21 @@ 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 - //gKalmanFilter.Position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_RX]; - //gKalmanFilter.Position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_RY]; - //gKalmanFilter.Position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_RZ]; + gKalmanFilter.Position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_RX]; + gKalmanFilter.Position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_RY]; + gKalmanFilter.Position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_RZ]; gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_VX]; gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_VY]; @@ -1062,59 +1192,20 @@ void Update_Vel(void) // Normalize quaternion and force q0 to be positive QuatNormalize(gKalmanFilter.quaternion); - //gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; - //gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; - //gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; - - gKalmanFilter.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_ABX]; - gKalmanFilter.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_ABY]; - gKalmanFilter.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_ABZ]; + gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; + gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; + gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; + + 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) -{ - // Sync the algorithm itow to the GPS value - gAlgorithm.itow = gEKFInputData.itow; - - // Resync timer - timer.tenHertzCntr = 0; - timer.subFrameCntr = 0; - - // Debugging counter: updateCntr[0] should be about 9x less than updateCntr[1] - updateCntr[0] = updateCntr[0] + 1; - - // reset the "last good reading" time - gAlgorithm.timeOfLastGoodGPSReading = gEKFInputData.itow; - - // GPS heading valid? - useGpsHeading = (gEKFInputData.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING); +{ + // Calculate the R-values for the INS measurements + _GenerateObservationCovariance_INS(); - /* Extract what's common between the following function and the - * routines below so we aren't repeating calculations - */ - LLA_To_Base(&gEKFInputData.llaRad[0], - &gAlgorithm.rGPS0_E[0], - &gAlgorithm.rGPS_N[0], - &gAlgorithm.R_NinE[0][0], - &gAlgorithm.rGPS_E[0]); - - /* Lever-arm is antenna position w.r.t to IMU in body. rGPS_N is antenna positive - * w.r.t to initial point in NED. IMU positive w.r.t initial point is - * rGPS_N - R_b_to_N * lever-arm - */ - float leverArmN[3]; - leverArmN[0] = gKalmanFilter.R_BinN[0][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[0][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[0][2] * gAlgorithm.leverArmB[2]; - leverArmN[1] = gKalmanFilter.R_BinN[1][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[1][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[1][2] * gAlgorithm.leverArmB[2]; - leverArmN[2] = gKalmanFilter.R_BinN[2][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[2][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[2][2] * gAlgorithm.leverArmB[2]; - gAlgorithm.rGPS_N[0] -= leverArmN[0]; - gAlgorithm.rGPS_N[1] -= leverArmN[1]; - gAlgorithm.rGPS_N[2] -= leverArmN[2]; /* This Sequential-Filter (three-stage approach) is nearly as * good as the full implementation -- we also can split it * across multiple iterations to not exceed 10 ms execution on @@ -1123,22 +1214,25 @@ 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(); - ComputeSystemInnovation_Vel(); - ComputeSystemInnovation_Att(); - - // Calculate the R-values for the INS measurements - _GenerateObservationCovariance_INS(); - Update_Pos(); - + ComputeSystemInnovation_Vel(); Update_Vel(); + ComputeSystemInnovation_Att(); // Initialize heading. If getting initial heading at this step, do not update att - if (gAlgorithm.gnssHeadingFirstTime) + if (gAlgorithm.headingIni < HEADING_GNSS_HIGH) { - if (InitializeGnssHeading()) + if (InitializeHeadingFromGnss()) { + // Heading is initialized. Related elements in the EKF also need intializing. + InitializeEkfHeading(); + /* This heading measurement is used to initialize heading, and should not be * used to update heading. */ @@ -1147,6 +1241,268 @@ static void Update_GPS(void) } } +static void Update_PseudoMeasurement(void) +{ + // which state is updated in Update_Vel() + uint8_t updatedStatesPseudo[16] = { 1, 1, 1, // Positions are NOT updated + 1, 1, 1, // Velocities are updated + 1, 1, 1, 1, // Quaternions are updated + 1, 1, 1, // Gyro biases are updated + 1, 1, 1 }; // Accel biases are upated + + /* Get current rb2n. + * gKalmanFilter.R_BinN is updated every time the algo enters _PredictStateEstimate + * After prediction and GPS update, this matrix needs updated. + */ + real rb2n[3][3]; + QuaternionToR321(gKalmanFilter.quaternion, &rb2n[0][0]); + + // detect zero velocity using GNSS vNED + BOOL staticGnss = DetectStaticGnssVelocity(gEKFInput.vNed, + gAlgorithm.staticDetectParam.staticGnssVel, + gEKFInput.gpsFixType); + + // measurement cov + real r[3] = { 1.0e-4f, 1.0e-4f, 1.0e-4f }; + if (!gImuStats.bStaticIMU) + { + /* If zero velocity is not detected by IMU, the covariance for the lateral and + * vertical velocity measurement should be increased. + */ + GenPseudoMeasCov(r); + r[1] = 1.0e-1; + r[2] = 1.0e-1; + } + + /* Compute innovation (measured - estimated) of velocity expressed in the body frame: + * innovation = [odo/0.0, 0.0, 0.0] - Rn2b * v_ned + * When odometer is available, front velocity measurement is given by odometer. + * When zero velocity detected, front velocity measurement is 0. + * 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 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 (odoUsedInAlgorithm()) + { + frontVelMeaValid = TRUE; + frontVelMea = gEKFInput.odoVelocity; + r[0] = 1.0e-4; // variance of front velocity measurement should be from odo spec + } + else if (gImuStats.bStaticIMU) + { + /* Only when GNSS is invalid or zero velocity is also detected by GNSS, zero velocity + * detected by IMU (and GNSS) can be used to determine the along-track velocity. + * When front velocity measurement is not available, it is not necessary to readjust + * its variance since it will not be used. + */ + if ((!gEKFInput.gpsFixType) || staticGnss) + { + frontVelMeaValid = TRUE; + frontVelMea = 0.0; + } + } + // front vel error + gKalmanFilter.nu[STATE_VX] = frontVelMea + -rb2n[0][0] * gKalmanFilter.Velocity_N[0] + -rb2n[1][0] * gKalmanFilter.Velocity_N[1] + -rb2n[2][0] * gKalmanFilter.Velocity_N[2]; + // lateral (right) vel error + gKalmanFilter.nu[STATE_VY] = + -rb2n[0][1] * gKalmanFilter.Velocity_N[0] + -rb2n[1][1] * gKalmanFilter.Velocity_N[1] + -rb2n[2][1] * gKalmanFilter.Velocity_N[2]; + // vertical (downwards) vel erro + gKalmanFilter.nu[STATE_VZ] = + -rb2n[0][2] * gKalmanFilter.Velocity_N[0] + -rb2n[1][2] * gKalmanFilter.Velocity_N[1] + -rb2n[2][2] * gKalmanFilter.Velocity_N[2]; + gKalmanFilter.nu[STATE_VY] = _LimitValue(gKalmanFilter.nu[STATE_VY], gAlgorithm.Limit.Innov.velocityError); + gKalmanFilter.nu[STATE_VZ] = _LimitValue(gKalmanFilter.nu[STATE_VZ], gAlgorithm.Limit.Innov.velocityError); + + // p*H'. PxHTranspose is 16x3, only the last two columns are used when only lateral and vertical measurements + memset(PxHTranspose, 0, sizeof(PxHTranspose)); + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + for (colNum = 0; colNum < 3; colNum++) + { + PxHTranspose[rowNum][colNum] = + gKalmanFilter.P[rowNum][3] * rb2n[0][colNum] + + gKalmanFilter.P[rowNum][4] * rb2n[1][colNum] + + gKalmanFilter.P[rowNum][5] * rb2n[2][colNum]; + } + } + + // s = H*P*H' + R + for (rowNum = 0; rowNum < 3; rowNum++) + { + for (colNum = rowNum; colNum < 3; colNum++) + { + S_3x3[rowNum][colNum] = rb2n[0][rowNum] * PxHTranspose[3][colNum] + + rb2n[1][rowNum] * PxHTranspose[4][colNum] + + rb2n[2][rowNum] * PxHTranspose[5][colNum]; + S_3x3[colNum][rowNum] = S_3x3[rowNum][colNum]; + } + S_3x3[rowNum][rowNum] += r[rowNum]; + } + + // Calculate inv(H*P*H'+R) according to if front velocity measurement is available + if (frontVelMeaValid) + { + matrixInverse_3x3(&S_3x3[0][0], &SInverse_3x3[0][0]); + } + else + { + S_3x3[0][0] = 1.0; + S_3x3[0][1] = 0.0; + S_3x3[0][2] = 0.0; + S_3x3[1][0] = 0.0; + S_3x3[2][0] = 0.0; + matrixInverse_3x3(&S_3x3[0][0], &SInverse_3x3[0][0]); + SInverse_3x3[0][0] = 0.0; + } + + // K = P*H' * inv(H*P*H' + R). gKalmanFilter.K is 16x3, only the last two columns are used. + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + for (colNum = 0; colNum < 3; colNum++) + { + gKalmanFilter.K[rowNum][colNum] = + PxHTranspose[rowNum][0] * SInverse_3x3[0][colNum] + + PxHTranspose[rowNum][1] * SInverse_3x3[1][colNum] + + PxHTranspose[rowNum][2] * SInverse_3x3[2][colNum]; + } + } + // force uncorrected terms in K to be 0 + for (rowNum = STATE_RX; rowNum <= STATE_ABZ; rowNum++) + { + if (!updatedStatesPseudo[rowNum]) + { + for (colNum = 0; colNum < 3; colNum++) + { + gKalmanFilter.K[rowNum][colNum] = 0.0; + } + } + } + + // dx = k * nu + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + gKalmanFilter.stateUpdate[rowNum] = + gKalmanFilter.K[rowNum][0] * gKalmanFilter.nu[STATE_VX] + + gKalmanFilter.K[rowNum][1] * gKalmanFilter.nu[STATE_VY] + + gKalmanFilter.K[rowNum][2] * gKalmanFilter.nu[STATE_VZ]; + } + + // update state + //gKalmanFilter.Position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_RX]; + //gKalmanFilter.Position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_RY]; + //gKalmanFilter.Position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_RZ]; + + gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_VX]; + gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_VY]; + gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_VZ]; + + gKalmanFilter.quaternion[Q0] = gKalmanFilter.quaternion[Q0] + gKalmanFilter.stateUpdate[STATE_Q0]; + gKalmanFilter.quaternion[Q1] = gKalmanFilter.quaternion[Q1] + gKalmanFilter.stateUpdate[STATE_Q1]; + gKalmanFilter.quaternion[Q2] = gKalmanFilter.quaternion[Q2] + gKalmanFilter.stateUpdate[STATE_Q2]; + gKalmanFilter.quaternion[Q3] = gKalmanFilter.quaternion[Q3] + gKalmanFilter.stateUpdate[STATE_Q3]; + + // Normalize quaternion and force q0 to be positive + QuatNormalize(gKalmanFilter.quaternion); + + gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; + gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; + gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; + + 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]; + + // Update covariance: P = P + DP = P - K*H*P + // Use transpose(PxHTranspose) to hold H*P (3x16). + // Only the last two columns are used when only lateral and vertical measurements + for (colNum = 0; colNum < NUMBER_OF_EKF_STATES; colNum++) + { + for (rowNum = 0; rowNum < 3; rowNum++) + { + PxHTranspose[colNum][rowNum] = rb2n[0][rowNum] * gKalmanFilter.P[3][colNum] + + rb2n[1][rowNum] * gKalmanFilter.P[4][colNum] + + rb2n[2][rowNum] * gKalmanFilter.P[5][colNum]; + } + } + + // deltaP = KxH * gKF.P; + 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 + */ + for (rowNum = 0; rowNum <= STATE_ABZ; rowNum++) + { + if (!updatedStatesPseudo[rowNum]) + { + continue; + } + for (colNum = rowNum; colNum <= STATE_ABZ; colNum++) + { + if (!updatedStatesPseudo[colNum]) + { + continue; + } + 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]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; + } + } + + /* P is symmetric so only need to multiply one half and reflect the values + * across the diagonal + */ + for (rowNum = 0; rowNum < ROWS_IN_P; rowNum++) + { + for (colNum = rowNum; colNum < COLS_IN_P; colNum++) + { + gKalmanFilter.P[rowNum][colNum] = gKalmanFilter.P[rowNum][colNum] - + gKalmanFilter.deltaP_tmp[rowNum][colNum]; + gKalmanFilter.P[colNum][rowNum] = gKalmanFilter.P[rowNum][colNum]; + } + } +} + +static void GenPseudoMeasCov(real *r) +{ + real absYawRate = (real)fabs(gEKFInput.angRate_B[2]); + r[1] = absYawRate; + r[2] = absYawRate; + + real minVar = 1e-4; + real maxVar = 1e-2; + if (r[1] < minVar) + { + r[1] = minVar; + } + if (r[1] > maxVar) + { + r[1] = maxVar; + } + + if (r[2] < minVar) + { + r[2] = minVar; + } + if (r[2] > maxVar) + { + r[2] = maxVar; + } + //printf("rr: %f,%f\n", r[1], r[2]); +} + + /* Conversion from turn-rate threshold (values loaded into gConfiguration) to * decimal value in [rad/sec]: * @@ -1185,7 +1541,6 @@ static void _TurnSwitch(void) if ((gAlgorithm.state > HIGH_GAIN_AHRS) && (absYawRate > minSwitch)) { gAlgoStatus.bit.turnSwitch = TRUE; - // std::cout << "TurnSwitch (INS): Activated\n"; /* When the rate is below the maximum rate defined by turnSwitchThreshold, * then generate a scale-factor that is between ( 1.0 - G ) and 0.0 (based on absYawRate). @@ -1252,7 +1607,7 @@ static BOOL _CheckForUpdateTrigger(uint8_t updateRate) { // uint8_t oneHundredHzCntr; - uint8_t gpsUpdate = 0; + uint8_t updateFlag = 0; // switch( updateRate ) @@ -1261,7 +1616,7 @@ static BOOL _CheckForUpdateTrigger(uint8_t updateRate) case 10: if( timer.subFrameCntr == 0 ) { - gpsUpdate = 1; + updateFlag = 1; } break; @@ -1269,7 +1624,7 @@ static BOOL _CheckForUpdateTrigger(uint8_t updateRate) case 20: if( timer.subFrameCntr == 0 || timer.subFrameCntr == 5 ) { - gpsUpdate = 1; + updateFlag = 1; } break; @@ -1304,7 +1659,7 @@ static BOOL _CheckForUpdateTrigger(uint8_t updateRate) oneHundredHzCntr == 92 || oneHundredHzCntr == 96 ) { - gpsUpdate = 1; + updateFlag = 1; } break; @@ -1316,142 +1671,323 @@ static BOOL _CheckForUpdateTrigger(uint8_t updateRate) timer.subFrameCntr == 6 || timer.subFrameCntr == 8 ) { - gpsUpdate = 1; + updateFlag = 1; } break; // fifty-hertz update case 100: - gpsUpdate = 1; + updateFlag = 1; break; } - return gpsUpdate; + return updateFlag; } - -static int InitializeGnssHeading() +static int InitializeHeadingFromGnss() { /* enable declination correction, but the corrected magnetic yaw will not * be used if GPS is available. */ gAlgorithm.applyDeclFlag = TRUE; - if (useGpsHeading)// && gEKFInputData.rawGroundSpeed>1.0) + /* backward drive detection for heading initialization using GNSS heading. + * Detection happends every second. Velocity increment is relatively reliable + * if it is accumulated for 1sec. + */ + static real lastVelBxGnss = 0; + static uint8_t forwardDriveConfidence = 0; + static uint32_t lastTOW = 0; + uint32_t timeSinceLastDetection = gAlgorithm.itow - lastTOW; + if (timeSinceLastDetection < 0) { - // Heading is initialized with GNSS - gAlgorithm.gnssHeadingFirstTime = FALSE; + timeSinceLastDetection = timeSinceLastDetection + MAX_ITOW; + } + if (timeSinceLastDetection > 950) // 950ms is set as the threshold for 1sec + { + lastTOW = gAlgorithm.itow; + /* assume velocity is always along the body x axis. otherwise, GNSS heading + * cannot be used to initialize fusion heading + */ + real velBx = sqrtf(SQUARE(gEKFInput.vNed[0]) + SQUARE(gEKFInput.vNed[1]) + SQUARE(gEKFInput.vNed[2])); + velBx = fabs(velBx); + real dv = velBx - lastVelBxGnss; + if ((dv * gKalmanFilter.linearAccel_B[X_AXIS]) > 0.0 && fabs(gKalmanFilter.linearAccel_B[X_AXIS]) > 0.2) + { + if (forwardDriveConfidence < 255) + { + forwardDriveConfidence++; + } + } + else + { + forwardDriveConfidence = 0; + } + // record this velocity along body x axis for next run + lastVelBxGnss = velBx; + // reset accumulated x body axis velocity change. + gKalmanFilter.linearAccel_B[X_AXIS] = 0.0; + } + + // detect if GNSS heading is reliable + static uint8_t gnssHeadingGoodCntr = 0; + static float lastGnssHeading = 0.0; + static float lastFusionHeading = 0.0; + BOOL gnssHeadingGood = 0; + float angleDiff = 0.0; + if (useGpsHeading) + { + float calculatedGnssHeading = (float)(atan2(gEKFInput.vNed[1], gEKFInput.vNed[0]) * R2D); + float diffHeading = AngleErrDeg(gEKFInput.trueCourse - calculatedGnssHeading); + // input GNSS heading matches heading calculated from vNED + if (fabs(diffHeading) < 5.0) + { + // GNSS heading change matches fusion yaw angle + float gnssHeadingChange = gEKFInput.trueCourse - lastGnssHeading; + float fusionHeadingChange = gKalmanFilter.eulerAngles[2] * (float)R2D - lastFusionHeading; + angleDiff = (float)fabs( AngleErrDeg(gnssHeadingChange - fusionHeadingChange) ); + if (angleDiff < 5.0) + { + gnssHeadingGood = TRUE; + } + } + lastGnssHeading = gEKFInput.trueCourse; + lastFusionHeading = gKalmanFilter.eulerAngles[2] * (float)R2D; + } + if (gnssHeadingGood) + { + gnssHeadingGoodCntr++; + } + else + { + gnssHeadingGoodCntr = 0; + } + + // Heading initialization when drive forward and GNSS heading is reliable + BOOL thisHeadingUsedForIni = FALSE; + if (gAlgorithm.headingIni < HEADING_GNSS_LOW) // heading is immediately but maybe unreliably initialized + { + if (gnssHeadingGoodCntr >= 1 && forwardDriveConfidence >= 1) // Only one sample is checked, so heading may be unreliable + { + gnssHeadingGoodCntr = 0; + // Heading is initialized with GNSS + gAlgorithm.headingIni = HEADING_GNSS_LOW; #ifdef INS_OFFLINE - printf("gps heading: %f\n", gEKFInputData.trueCourse); + printf("quick gps heading: %f\n", gEKFInput.trueCourse); #else #ifdef DISPLAY_DIAGNOSTIC_MSG - DebugPrintString("gps heading"); - DebugPrintFloat(": ", gEKFInputData.trueCourse, 9); - DebugPrintEndline(); + DebugPrintString("quick gps heading"); + DebugPrintFloat(": ", gEKFInput.trueCourse, 9); + DebugPrintEndline(); #endif #endif - // initialize yaw angle with GPS heading - gKalmanFilter.eulerAngles[YAW] = (real)(gEKFInputData.trueCourse * D2R); - if (gKalmanFilter.eulerAngles[YAW] > PI) + thisHeadingUsedForIni = TRUE; + } + } + else + { + /* Three points are checked, and the latest ground speed is above a certian threshold. + * The latest GNSS heading should be reliable. + */ + if (gnssHeadingGoodCntr >= 3 && + forwardDriveConfidence >= 5 && + gEKFInput.rawGroundSpeed > RELIABLE_GPS_VELOCITY_HEADING) { - gKalmanFilter.eulerAngles[YAW] -= (real)TWO_PI; + gnssHeadingGoodCntr = 0; + forwardDriveConfidence = 0; + gAlgorithm.headingIni = HEADING_GNSS_HIGH; +#ifdef INS_OFFLINE + printf("reliable gps heading: %f\n", gEKFInput.trueCourse); +#else +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("reliable gps heading"); + DebugPrintFloat(": ", gEKFInput.trueCourse, 9); + DebugPrintEndline(); +#endif +#endif + thisHeadingUsedForIni = TRUE; } - EulerAnglesToQuaternion(gKalmanFilter.eulerAngles, gKalmanFilter.quaternion); - - // reinitialize NED velocity - gKalmanFilter.Velocity_N[X_AXIS] = (real)gEKFInputData.vNed[X_AXIS]; - gKalmanFilter.Velocity_N[Y_AXIS] = (real)gEKFInputData.vNed[Y_AXIS]; - gKalmanFilter.Velocity_N[Z_AXIS] = (real)gEKFInputData.vNed[Z_AXIS]; - + } + + return thisHeadingUsedForIni; +} + +static void InitializeEkfHeading() +{ + /* Compare the reliable heading with Kalamn filter heading. If the difference exceeds + * a certain threshold, this means the immediate heading initialization is unreliable, + * and the Kalman filter needs reinitialized with the reliable one. + */ + float angleDiff = (float)fabs(AngleErrDeg(gEKFInput.trueCourse - + gKalmanFilter.eulerAngles[2] * (float)R2D)); + if (angleDiff <= 2.0) + { + return; + } + +#ifdef INS_OFFLINE + printf("Reinitialize KF: %f\n", angleDiff); +#else +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("Reinitialize KF: "); + DebugPrintFloat("", angleDiff, 9); + DebugPrintEndline(); +#endif +#endif + + // initialize yaw angle with GPS heading + gKalmanFilter.eulerAngles[YAW] = (gEKFInput.trueCourse * D2R); + if (gKalmanFilter.eulerAngles[YAW] > PI) + { + gKalmanFilter.eulerAngles[YAW] -= (real)TWO_PI; + } + EulerAnglesToQuaternion(gKalmanFilter.eulerAngles, gKalmanFilter.quaternion); + + // reinitialize NED position + gKalmanFilter.Position_N[0] = (real)gKalmanFilter.rGPS_N[0]; + gKalmanFilter.Position_N[1] = (real)gKalmanFilter.rGPS_N[1]; + gKalmanFilter.Position_N[2] = (real)gKalmanFilter.rGPS_N[2]; + + // reinitialize NED velocity + gKalmanFilter.Velocity_N[X_AXIS] = (real)gEKFInput.vNed[X_AXIS]; + gKalmanFilter.Velocity_N[Y_AXIS] = (real)gEKFInput.vNed[Y_AXIS]; + gKalmanFilter.Velocity_N[Z_AXIS] = (real)gEKFInput.vNed[Z_AXIS]; + #if 1 // mod, DXG - // reset quaternion and velocity terms in the P matrix - int i, j; - // vel row - for (i = STATE_VX; i < STATE_VZ; i++) + // reset quaternion and velocity terms in the P matrix + int i, j; + // pos row + gKalmanFilter.P[STATE_RX][STATE_RX] = gKalmanFilter.R[STATE_RX]; + gKalmanFilter.P[STATE_RY][STATE_RY] = gKalmanFilter.R[STATE_RY]; + gKalmanFilter.P[STATE_RZ][STATE_RZ] = gKalmanFilter.R[STATE_RZ]; + for (i = STATE_RX; i < STATE_RZ; i++) + { + for (j = 0; j < NUMBER_OF_EKF_STATES; j++) { - for (j = 0; j < NUMBER_OF_EKF_STATES; j++) + if (i != j) { - if (i != j) - { - gKalmanFilter.P[i][j] = 0; - gKalmanFilter.P[j][i] = 0; - } + gKalmanFilter.P[i][j] = 0; + gKalmanFilter.P[j][i] = 0; } } - // q0 row - for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + } + // vel row + gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.R[STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.R[STATE_VY]; + gKalmanFilter.P[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VZ]; + for (i = STATE_VX; i < STATE_VZ; i++) + { + for (j = 0; j < NUMBER_OF_EKF_STATES; j++) { - if (i != STATE_Q0) + if (i != j) { - gKalmanFilter.P[STATE_Q0][i] = 0; - gKalmanFilter.P[i][STATE_Q0] = 0; + gKalmanFilter.P[i][j] = 0; + gKalmanFilter.P[j][i] = 0; } } - // q3 row - for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + } + // q0 row + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + if (i != STATE_Q0) { - if (i != STATE_Q3) - { - gKalmanFilter.P[STATE_Q3][i] = 0; - gKalmanFilter.P[i][STATE_Q3] = 0; - } + gKalmanFilter.P[STATE_Q0][i] = 0; + gKalmanFilter.P[i][STATE_Q0] = 0; } - - // the initial covariance of the quaternion is estimated from ground speed. - float temp = (float)atan(0.05 / gEKFInputData.rawGroundSpeed); - temp *= temp; // heading var - if (gAlgoStatus.bit.turnSwitch) + } + // q3 row + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + if (i != STATE_Q3) { - temp *= 10.0; // when rotating, heading var increases + gKalmanFilter.P[STATE_Q3][i] = 0; + gKalmanFilter.P[i][STATE_Q3] = 0; } - temp /= 4.0; // sin(heading/2) or cos(heading/2) - float sinYawSqr = (real)sin(gKalmanFilter.eulerAngles[YAW] / 2.0f); - sinYawSqr *= sinYawSqr; - // Assume roll and pitch are close to 0deg - gKalmanFilter.P[STATE_Q0][STATE_Q0] = temp * sinYawSqr; - gKalmanFilter.P[STATE_Q3][STATE_Q3] = temp * (1.0f - sinYawSqr); + } + + // the initial covariance of the quaternion is estimated from ground speed. + float temp = (float)atan(0.05 / gEKFInput.rawGroundSpeed); + temp *= temp; // heading var + if (gAlgoStatus.bit.turnSwitch) + { + temp *= 10.0; // when rotating, heading var increases + } + temp /= 4.0; // sin(heading/2) or cos(heading/2) + float sinYawSqr = (real)sin(gKalmanFilter.eulerAngles[YAW] / 2.0f); + sinYawSqr *= sinYawSqr; + // Assume roll and pitch are close to 0deg + gKalmanFilter.P[STATE_Q0][STATE_Q0] = temp * sinYawSqr; + gKalmanFilter.P[STATE_Q3][STATE_Q3] = temp * (1.0f - sinYawSqr); - gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.R[STATE_VX]; - gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.R[STATE_VY]; - gKalmanFilter.P[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VZ]; + gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.R[STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.R[STATE_VY]; + gKalmanFilter.P[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VZ]; #if 0 - // reset velocity and quaternion terms in the P matrix - real v2 = gEKFInputData.rawGroundSpeed * gEKFInputData.rawGroundSpeed; - real v3by4 = 4.0 * v2 * gEKFInputData.rawGroundSpeed; - real vn2 = gEKFInputData.vNed[0] * gEKFInputData.vNed[0]; - real q0q0 = gKalmanFilter.quaternion[0] * gKalmanFilter.quaternion[0]; - real q3q3 = gKalmanFilter.quaternion[3] * gKalmanFilter.quaternion[3]; - if (q0q0 < 1.0e-3) - { - q0q0 = 1.0e-3; - } - if (q3q3 < 1.0e-3) - { - q3q3 = 1.0e-3; - } - real multiplerQVn = (v2 - vn2) / v3by4; - multiplerQVn *= multiplerQVn; - real multiplerQVe = (gEKFInputData.vNed[0] * gEKFInputData.vNed[1]) / v3by4; - multiplerQVe *= multiplerQVe; - gKalmanFilter.P[STATE_VX][STATE_Q0] = multiplerQVn / q0q0 * gKalmanFilter.R[STATE_VX][STATE_VX]; - gKalmanFilter.P[STATE_VX][STATE_Q3] = multiplerQVn / q3q3 * gKalmanFilter.R[STATE_VX][STATE_VX]; - gKalmanFilter.P[STATE_VY][STATE_Q0] = multiplerQVe / q0q0 * gKalmanFilter.R[STATE_VY][STATE_VY]; - gKalmanFilter.P[STATE_VY][STATE_Q3] = multiplerQVe / q3q3 * gKalmanFilter.R[STATE_VY][STATE_VY]; - - - gKalmanFilter.P[STATE_Q0][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_Q0]; - gKalmanFilter.P[STATE_Q3][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_Q3]; - gKalmanFilter.P[STATE_Q0][STATE_VY] = gKalmanFilter.P[STATE_VY][STATE_Q0]; - gKalmanFilter.P[STATE_Q3][STATE_VY] = gKalmanFilter.P[STATE_VY][STATE_Q3]; + // reset velocity and quaternion terms in the P matrix + real v2 = gEKFInput.rawGroundSpeed * gEKFInput.rawGroundSpeed; + real v3by4 = 4.0 * v2 * gEKFInput.rawGroundSpeed; + real vn2 = gEKFInput.vNed[0] * gEKFInput.vNed[0]; + real q0q0 = gKalmanFilter.quaternion[0] * gKalmanFilter.quaternion[0]; + real q3q3 = gKalmanFilter.quaternion[3] * gKalmanFilter.quaternion[3]; + if (q0q0 < 1.0e-3) + { + q0q0 = 1.0e-3; + } + if (q3q3 < 1.0e-3) + { + q3q3 = 1.0e-3; + } + real multiplerQVn = (v2 - vn2) / v3by4; + multiplerQVn *= multiplerQVn; + real multiplerQVe = (gEKFInput.vNed[0] * gEKFInput.vNed[1]) / v3by4; + multiplerQVe *= multiplerQVe; + gKalmanFilter.P[STATE_VX][STATE_Q0] = multiplerQVn / q0q0 * gKalmanFilter.R[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VX][STATE_Q3] = multiplerQVn / q3q3 * gKalmanFilter.R[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_Q0] = multiplerQVe / q0q0 * gKalmanFilter.R[STATE_VY][STATE_VY]; + gKalmanFilter.P[STATE_VY][STATE_Q3] = multiplerQVe / q3q3 * gKalmanFilter.R[STATE_VY][STATE_VY]; + + + gKalmanFilter.P[STATE_Q0][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_Q0]; + gKalmanFilter.P[STATE_Q3][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_Q3]; + gKalmanFilter.P[STATE_Q0][STATE_VY] = gKalmanFilter.P[STATE_VY][STATE_Q0]; + gKalmanFilter.P[STATE_Q3][STATE_VY] = gKalmanFilter.P[STATE_VY][STATE_Q3]; #endif #endif - return 1; +} + +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]; + } } - else + + // 3) add dQ + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) { - return 0; + 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]; + } } -} \ No newline at end of file +} diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/WorldMagneticModel.c b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/WorldMagneticModel.c index e9b20dc..c3f9403 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/WorldMagneticModel.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/WorldMagneticModel.c @@ -157,7 +157,7 @@ void TaskWorldMagneticModel(void const *argument) while(1) { OS_Delay( 5000 ); #ifdef GPS - if( gGpsDataPtr->gpsValid ) + if( gGpsDataPtr->gpsFixType ) { // caution - safety checks on the dates are fine, but we don't want // to rely on these hard-coded dates in operation @@ -204,7 +204,7 @@ void TaskWorldMagneticModel(void const *argument) // /* // % WorldMagneticModel.m // -// if( gGpsDataPtr.GPSValid ), +// if( gGpsDataPtr.gpsFixType ), // % WMM here // declinationAngle_rad = ( 13 + 36 / 60 + 43/3600 ) * CONV.DEG_TO_RAD; // @@ -216,7 +216,7 @@ void TaskWorldMagneticModel(void const *argument) // */ // // // VERY SIMPLE WMM (declination near San Jose only) -//// if (gGpsDataPtr->gpsValid) { +//// if (gGpsDataPtr->gpsFixType) { //// gWorldMagModel.decl_rad = (real)( (13.0 + 40.0 / 60.0 + 29.0 / 3600.0) * D2R ); //// } else { //// gWorldMagModel.decl_rad = (real)0.0; diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/algorithm.c b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/algorithm.c index 13fb6c5..c5a2087 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/algorithm.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/Algorithm/src/algorithm.c @@ -15,6 +15,9 @@ ******************************************************************************/ #include +#include + +#include "SensorNoiseParameters.h" #include "platformAPI.h" #include "algorithm.h" #include "AlgorithmLimits.h" @@ -24,11 +27,17 @@ AlgorithmStruct gAlgorithm; AlgoStatus gAlgoStatus; -void InitializeAlgorithmStruct(uint16_t callingFreq) +void InitializeAlgorithmStruct(uint8_t callingFreq) { - gAlgorithm.Behavior.bit.freeIntegrate = FALSE; + 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){ + // IMU case + callingFreq = FREQ_200_HZ; + } gAlgorithm.callingFreq = callingFreq; // Set dt based on the calling frequency of the EKF @@ -50,7 +59,7 @@ void InitializeAlgorithmStruct(uint16_t callingFreq) // Set up other timing variables gAlgorithm.dtOverTwo = (real)(0.5) * gAlgorithm.dt; gAlgorithm.dtSquared = gAlgorithm.dt * gAlgorithm.dt; - gAlgorithm.sqrtDt = sqrt(gAlgorithm.dt); + gAlgorithm.sqrtDt = sqrtf(gAlgorithm.dt); // Set the algorithm duration periods gAlgorithm.Duration.Stabilize_System = (uint32_t)(gAlgorithm.callingFreq * STABILIZE_SYSTEM_DURATION); @@ -72,6 +81,7 @@ void InitializeAlgorithmStruct(uint16_t callingFreq) gAlgorithm.applyDeclFlag = FALSE; gAlgorithm.insFirstTime = TRUE; + gAlgorithm.headingIni = HEADING_UNINITIALIZED; //gAlgorithm.magAlignUnderway = FALSE; // Set and reset in mag-align code @@ -79,7 +89,8 @@ void InitializeAlgorithmStruct(uint16_t callingFreq) gAlgorithm.itow = 0; // Limit is compared to ITOW. Time must be in [msec]. - gAlgorithm.Limit.Max_GPS_Drop_Time = LIMIT_MAX_GPS_DROP_TIME * 1000; + gAlgorithm.Limit.maxGpsDropTime = LIMIT_MAX_GPS_DROP_TIME * 1000; + gAlgorithm.Limit.maxReliableDRTime = LIMIT_RELIABLE_DR_TIME * 1000; // Limit is compared to count (incremented upon loop through // taskDataAcquisition). Time must be in [count] based on ODR. @@ -105,15 +116,42 @@ void InitializeAlgorithmStruct(uint16_t callingFreq) gAlgorithm.useRawAccToDetectLinAccel = TRUE; // Set the turn-switch threshold to a default value in [deg/sec] - gAlgorithm.turnSwitchThreshold = 2.0; + gAlgorithm.turnSwitchThreshold = 6.0; // default lever arm and point of interest - gAlgorithm.leverArmB[X_AXIS] = 0.0; - gAlgorithm.leverArmB[Y_AXIS] = 0.0; - gAlgorithm.leverArmB[Z_AXIS] = 0.0; - gAlgorithm.pointOfInterestB[X_AXIS] = 0.0; - gAlgorithm.pointOfInterestB[Y_AXIS] = 0.0; - gAlgorithm.pointOfInterestB[Z_AXIS] = 0.0; + gAlgorithm.leverArmB[X_AXIS] = 0.0; + gAlgorithm.leverArmB[Y_AXIS] = 0.0; + gAlgorithm.leverArmB[Z_AXIS] = 0.0; + gAlgorithm.pointOfInterestB[X_AXIS] = 0.0; + gAlgorithm.pointOfInterestB[Y_AXIS] = 0.0; + gAlgorithm.pointOfInterestB[Z_AXIS] = 0.0; + + // For most vehicles, the velocity is always along the body x axis + gAlgorithm.velocityAlwaysAlongBodyX = TRUE; + + // get IMU specifications + 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; + gAlgorithm.imuSpec.maxBiasW = (real)MAX_BW; + gAlgorithm.imuSpec.vrw = (real)VRW_300ZA; + 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; + + // 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 = 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)); } void GetAlgoStatus(AlgoStatus *algoStatus) @@ -170,4 +208,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/OpenIMU300RI/IMU/lib/Core/GPS/gpsAPI.h b/examples/OpenIMU300RI/IMU/lib/Core/GPS/gpsAPI.h index aef45ab..3a24ea4 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/GPS/gpsAPI.h +++ b/examples/OpenIMU300RI/IMU/lib/Core/GPS/gpsAPI.h @@ -43,33 +43,45 @@ void TaskGps(void const *argument); } #endif +// Fix type +#define INVALID 0 // GNSS not fixed yet +#define SPP 1 // Single Point Positioning +#define DGPS 2 // Pseudorange Differential +#define PPP 3 // Precise Point Positioning +#define RTK_FIX 4 // RTK Fixed +#define RTK_FLOAT 5 // RTK Float +#define DEAD_REC 6 // Dead Reckoning (will be considered as INVALID) +#define MANUAL 7 // Manual Input Mode (will be considered as INVALID) +#define SIMULATION 8 // Simulation Mode (will be considered as INVALID) + typedef struct { - int gpsValid; // 1 if data is valid - uint8_t gpsUpdate; // 1 if contains new data - - double latitude; // latitude , degrees - double longitude; // longitude, degrees - double vNed[3]; // velocities, m/s NED (North East Down) x, y, z - double trueCourse; // [deg] - double rawGroundSpeed; // NMEA kph, SiRf m/s - change to m/s - double altitude; // above ellipsoid [m] - double GPSSecondFraction; - float altEllipsoid; // [km] altitude above ellipsoid for WMM - - uint32_t itow; // gps Time Of Week, miliseconds + uint8_t gpsFixType; // 1 if data is valid + uint8_t gpsUpdate; // 1 if contains new data + uint8_t numSatellites; // num of satellites in the solution + uint32_t itow; // gps Time Of Week, miliseconds + + double latitude; // latitude , degrees + double longitude; // longitude, degrees + double vNed[3]; // velocities, m/s NED (North East Down) x, y, z + double trueCourse; // [deg] + double rawGroundSpeed; // [m/s] + double altitude; // above WGS84 ellipsoid [m] + double GPSSecondFraction; + - uint8_t GPSmonth; // mm - uint8_t GPSday; // dd - uint8_t GPSyear; // yy last two digits of year - char GPSHour; // hh - char GPSMinute; // mm - char GPSSecond; // ss - - float GPSHorizAcc, GPSVertAcc; - float HDOP; + uint8_t GPSmonth; // mm + uint8_t GPSday; // dd + uint8_t GPSyear; // yy last two digits of year + char GPSHour; // hh + char GPSMinute; // mm + char GPSSecond; // ss + + float GPSHorizAcc, GPSVertAcc; + float HDOP; + float geoidAboveEllipsoid; // [m] Height of geoid (mean sea level) above WGS84 ellipsoid } gpsDataStruct_t; -extern gpsDataStruct_t gGPS; +extern gpsDataStruct_t gGPS, gCanGps; /** **************************************************************************** * @name GetGPSData diff --git a/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/GpsData.h b/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/GpsData.h index 10a514b..cb21530 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/GpsData.h +++ b/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/GpsData.h @@ -43,35 +43,36 @@ This Data structure is not only used for all GPS interface and process but also used to be accessed by other modules rather than GPS files. */ typedef struct { - BOOL gpsValid; - long double lat; // concatinated from int components [deg.dec] - long double lon; - double vNed[3]; // NED North East Down [m/s] x, y, z - uint32_t itow; ///< gps milisecond Interval Time Of Week - - int updateFlagForEachCall; /// changed to 16 bits - int totalGGA; - int totalVTG; - - double trueCourse; // [deg] - double rawGroundSpeed; // NMEA kph, SiRf m/s - - double alt; // above mean sea level [m] - double filteredAlt; // FIXME should this be local? - float altEllipsoid; // [km] altitude above ellipsoid for WMM - uint8_t GPSmonth; // mm - uint8_t GPSday; // dd - uint8_t GPSyear; // yy last two digits of year - char GPSHour; // hh - char GPSMinute; // mm - char GPSSecond; // ss - double GPSSecondFraction; // FIXME used? + uint8_t gpsFixType; // GPS solution type: invalid, spp, dgps, spp, rtk_float, rtk_fix + uint8_t numSatellites; + + uint8_t GPSmonth; // mm + uint8_t GPSday; // dd + uint8_t GPSyear; // yy last two digits of year + char GPSHour; // hh + char GPSMinute; // mm + char GPSSecond; // ss + double GPSSecondFraction; // FIXME used? + + uint32_t itow; ///< gps milisecond Interval Time Of Week + int updateFlagForEachCall; /// changed to 16 bits + + int totalGGA; + int totalVTG; + + double lat; // [deg], latitude + double lon; // [lon], longitude + double alt; // [m] above WGS84 ellipsoid + double vNed[3]; // NED North East Down [m/s] x, y, z + double trueCourse; // [deg] + double rawGroundSpeed; // [m/s] + + float geoidAboveEllipsoid; // [m] Height of geoid (mean sea level) above WGS84 ellipsoid /// compatible with Ublox driver FIXME should these be seperate data structure? unsigned char ubloxClassID; unsigned char ubloxMsgID; signed long LonLatH[3]; // SiRF Lat Lon[deg] * 10^7 Alt ellipse [m]*100 <-- UNUSED - char GPSFix; float HDOP; // Horizontal Dilution Of Precision x.x double GPSVelAcc; unsigned short GPSStatusWord; /// will replace GPSfix @@ -114,9 +115,6 @@ typedef struct { float GPSHorizAcc; float GPSVertAcc; - - int numSatelites; - } GpsData_t; extern GpsData_t *gGpsDataPtr; // definition in driverGPSAllentrance.c diff --git a/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/driverGPS.h b/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/driverGPS.h index f114857..c7681a8 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/driverGPS.h +++ b/examples/OpenIMU300RI/IMU/lib/Core/GPS/include/driverGPS.h @@ -159,23 +159,33 @@ typedef struct uint8_t hour; uint8_t min; uint8_t sec; - uint8_t valid; + uint8_t valid; // Validity flags uint32_t tAcc; // time accuracy estimate (UTC), ns int32_t nano; // fraction of second, range -1e9..1e9 (UTC), ns - uint8_t fixType; // GNSS fix type - // 0: no fix - // 1: dead reckoning only - // 2: 2D-fix - // 3: 3D-fix - // 4: GNSS + dead reckoning combined - // 5: time only fix - uint8_t flags; + uint8_t fixType; /* GNSS fix type + * 0: no fix + * 1: dead reckoning only + * 2: 2D-fix + * 3: 3D-fix + * 4: GNSS + dead reckoning combined + * 5: time only fix + */ + uint8_t flags; /* Fix status flags + * bit0: gnssFixOK, 1 = valid fix (i.e within DOP & accuracy masks) + * bit1: diffSoln, 1 = differential corrections were applied + * bit2-4: psmState, Power Save Mode state + * bit5: headVehValid, 1 = heading of vehicle is valid + * bit6-7: carrSoln, Carrier phase range solution status: + * 0: no carrier phase range solution + * 1: float solution + * 2: fixed solution + */ uint8_t flags2; uint8_t numSV; // number of satellites in Nav solution; int32_t lon; // deg, scaling is 1e-7 int32_t lat; // deg, scaling is 1e-7 int32_t height; // mm, height above ellipsoid; - int32_t hMSL; + int32_t hMSL; // mm, hegiht above mean seal level uint32_t hAcc; // mm, horizontal accuracy estimate uint32_t vAcc; // mm, vertical accuracy estimate int32_t velN; // mm/s, north velocity diff --git a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/driverGPS.c b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/driverGPS.c index 2cf7a92..89416a6 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/driverGPS.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/driverGPS.c @@ -273,7 +273,6 @@ int writeGps(char *data, uint16_t len) void GetGPSData(gpsDataStruct_t *data) { - data->gpsValid = gGpsDataPtr->gpsValid; data->gpsUpdate = ( gGpsDataPtr->updateFlagForEachCall >> GOT_VTG_MSG ) & 0x00000001 && ( gGpsDataPtr->updateFlagForEachCall >> GOT_GGA_MSG ) & 0x00000001; // gGpsDataPtr->updateFlagForEachCall &= 0xFFFFFFFD; @@ -281,33 +280,36 @@ void GetGPSData(gpsDataStruct_t *data) { // Reset GPS update flag only when pos and vel are both available gGpsDataPtr->updateFlagForEachCall &= 0xFFFFFFFC; - } - data->latitude = gGpsDataPtr->lat; - data->longitude = gGpsDataPtr->lon; - data->altitude = gGpsDataPtr->alt; + data->gpsFixType = gGpsDataPtr->gpsFixType; + data->numSatellites = gGpsDataPtr->numSatellites; + + data->latitude = gGpsDataPtr->lat; + data->longitude = gGpsDataPtr->lon; + data->altitude = gGpsDataPtr->alt; - data->vNed[0] = gGpsDataPtr->vNed[0]; - data->vNed[1] = gGpsDataPtr->vNed[1]; - data->vNed[2] = gGpsDataPtr->vNed[2]; + data->vNed[0] = gGpsDataPtr->vNed[0]; + data->vNed[1] = gGpsDataPtr->vNed[1]; + data->vNed[2] = gGpsDataPtr->vNed[2]; - data->trueCourse = gGpsDataPtr->trueCourse; - data->rawGroundSpeed = gGpsDataPtr->rawGroundSpeed; + data->trueCourse = gGpsDataPtr->trueCourse; + data->rawGroundSpeed = gGpsDataPtr->rawGroundSpeed; - data->GPSSecondFraction = gGpsDataPtr->GPSSecondFraction; - data->altEllipsoid = gGpsDataPtr->altEllipsoid; + data->GPSSecondFraction = gGpsDataPtr->GPSSecondFraction; - data->itow = gGpsDataPtr->itow; - data->GPSmonth = gGpsDataPtr->GPSmonth; - data->GPSday = gGpsDataPtr->GPSday; - data->GPSyear = gGpsDataPtr->GPSyear; - data->GPSHour = gGpsDataPtr->GPSHour; - data->GPSMinute = gGpsDataPtr->GPSMinute; - data->GPSSecond = gGpsDataPtr->GPSSecond; + data->itow = gGpsDataPtr->itow; + data->GPSmonth = gGpsDataPtr->GPSmonth; + data->GPSday = gGpsDataPtr->GPSday; + data->GPSyear = gGpsDataPtr->GPSyear; + data->GPSHour = gGpsDataPtr->GPSHour; + data->GPSMinute = gGpsDataPtr->GPSMinute; + data->GPSSecond = gGpsDataPtr->GPSSecond; - data->HDOP = gGpsDataPtr->HDOP; - data->GPSHorizAcc = gGpsDataPtr->GPSHorizAcc; - data->GPSVertAcc = gGpsDataPtr->GPSVertAcc; + data->HDOP = gGpsDataPtr->HDOP; + data->GPSHorizAcc = gGpsDataPtr->GPSHorizAcc; + data->GPSVertAcc = gGpsDataPtr->GPSVertAcc; + data->geoidAboveEllipsoid = gGpsDataPtr->geoidAboveEllipsoid; + } } @@ -439,9 +441,6 @@ BOOL _handleGpsMessages(GpsData_t *GPSData) } tmp = gpsUartBuf[pos++]; bytesInBuffer--; -#ifdef DETECT_USER_SERIAL_CMD - platformDetectUserSerialCmd(tmp); -#endif switch(GPSData->GPSProtocol){ case NMEA_TEXT: parseNMEAMessage(tmp, gpsMsg, GPSData); diff --git a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processNMEAGPS.c b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processNMEAGPS.c index 00e31d0..f51ada5 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processNMEAGPS.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processNMEAGPS.c @@ -33,6 +33,7 @@ limitations under the License. #include #include +#include "gpsAPI.h" #include "driverGPS.h" #include "BITStatus.h" @@ -42,6 +43,9 @@ char _parseRMC(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); + +int dayofweek(int day, int month, int year); + int crcError = 0; int starError = 0; /** **************************************************************************** @@ -181,6 +185,9 @@ void _handleNMEAmsg(char *msgID, GPSData->totalVTG++; _parseVTG(msgBody, GPSData); } + if( strncmp(ptr, "RMC", 3) == 0 ) { + _parseRMC(msgBody, GPSData); + } } /** **************************************************************************** @@ -244,6 +251,8 @@ char extractNMEAfield(char *msgBody, break; } } + outField[outIndex] = '\x0'; + // Leave the currentField as the one returned so the next search passes // the currrent doesn't match requested test // Back up one so the next search picks up the end ',' @@ -252,7 +261,7 @@ char extractNMEAfield(char *msgBody, } /** **************************************************************************** - * @name: _parseGPGGA LOCAL parse GPGGA message. Tiem, position, fix data + * @name: _parseGPGGA LOCAL parse GPGGA message. Time, position, fix data * @author Dong An * @param [in] msgBody - data to parse * @param [in] GPSData - data structure to parst he data into @@ -285,6 +294,7 @@ char extractNMEAfield(char *msgBody, * decimal * 02/16/15 dkh move convert to after check for flag ******************************************************************************/ +#include "debug.h" char _parseGPGGA(char *msgBody, GpsData_t *GPSData) { @@ -292,8 +302,6 @@ char _parseGPGGA(char *msgBody, char status = 0; int parseReset = true; NmeaLatLonSTRUCT nmeaLatLon; - // this is the local using the NMEA convention - char GPSFix = 0; // no fix memset(&nmeaLatLon, 0, sizeof(nmeaLatLon) ); /// Time - convert from ascii digits to decimal '0' = 48d @@ -344,7 +352,17 @@ char _parseGPGGA(char *msgBody, /// GPS quality if( extractNMEAfield(msgBody, field, 5, parseReset) ) { - GPSFix = field[0] - '0'; // convert ascii digit to decimal + GPSData->gpsFixType = field[0] - '0'; // convert ascii digit to decimal + if (GPSData->gpsFixType >= DEAD_REC) // DR, manual and simulation is considered invalid + { + GPSData->gpsFixType = INVALID; + } + } else + status = 1; + + // Number of satellites + if( extractNMEAfield(msgBody, field, 6, parseReset) ) { + GPSData->numSatellites = atoi((char *)field); } else status = 1; @@ -354,32 +372,37 @@ char _parseGPGGA(char *msgBody, } else status = 1; - if(GPSFix >= 1) { - convertItow(GPSData); // create pseudo ITOW - gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked - gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track - } else { - gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // no signal lock - gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track - } - - /// Altitude if( extractNMEAfield(msgBody, field, 8, parseReset) ) { - GPSData->alt = atof((char *)field); + GPSData->alt = atof((char *)field); // altitude above MSL + } else{ + status = 1; + } + if( extractNMEAfield(msgBody, field, 10, parseReset) ) { + GPSData->geoidAboveEllipsoid = atof((char *)field); } else{ status = 1; } - if(GPSFix >= 1) { + // if fixed, convert data + if(GPSData->gpsFixType > INVALID) + { + // Convert deg/min/sec to xxx.xxx deg _NMEA2UbloxAndLLA(&nmeaLatLon, GPSData); + // convert geiod height to ellipsoid height + GPSData->alt += GPSData->geoidAboveEllipsoid; + // create pseudo ITOW + convertItow(GPSData); + + gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked + gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track } - // flip the sense from the NMEA convention to match the propiatery binary messages - if (GPSData->HDOP < 20) { // the threshold may get set lower in algorithm.c - GPSData->GPSFix = 0; // fix - } else { - GPSData->GPSFix = 1; // no fix + else + { + gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // no signal lock + gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track } + if( status == 0) { GPSData->updateFlagForEachCall |= 1 << GOT_GGA_MSG; GPSData->LLHCounter++; @@ -422,6 +445,7 @@ char _parseVTG(char *msgBody, /// speed over ground [km/hr] if( extractNMEAfield(msgBody, field, 6, parseReset) ) { GPSData->rawGroundSpeed = atof((char *)field); // double [kph] + GPSData->rawGroundSpeed *= 0.2777777777778; // convert km/hr to m/s // the heading of the VTG of some receiver is empty when velocity is low. // a default zero will be set to the heading value. if ( status == 1 ) { @@ -474,7 +498,6 @@ char _parseRMC(char *msgBody, GPSData->GPSyear = ( (field[4] - '0') * 10) + field[5] - '0'; /// year } else status = 1; - return status; } @@ -553,10 +576,8 @@ void _NMEA2UbloxAndLLA(NmeaLatLonSTRUCT *NmeaData, ******************************************************************************/ void convertNorhEastVelocity(GpsData_t* GPSData) { - GPSData->vNed[0] = (GPSData->rawGroundSpeed * 0.2777777777778) * - cos(D2R * GPSData->trueCourse); // 0.277 = 1000/3600 kph -> m/s - GPSData->vNed[1] = (GPSData->rawGroundSpeed * 0.2777777777778) * - sin(D2R * GPSData->trueCourse); + GPSData->vNed[0] = GPSData->rawGroundSpeed * cos(D2R * GPSData->trueCourse); + GPSData->vNed[1] = GPSData->rawGroundSpeed * sin(D2R * GPSData->trueCourse); _smoothVerticalData(GPSData); // synthesize and filter down velocity } @@ -603,7 +624,6 @@ void _smoothVerticalData(GpsData_t* GPSData) } if ( OutAltCounter >= 5 ) { firstFlag = 0; // reset - GPSData->filteredAlt = filteredAlt ; GPSData->vNed[2] = filteredVd ; return; } @@ -623,7 +643,6 @@ void _smoothVerticalData(GpsData_t* GPSData) CurrentVd = -(tmpDouble / delta_T); ///down is positive } else { /// may be NMEA is lost firstFlag = 0; /// reset - GPSData->filteredAlt = filteredAlt ; GPSData->vNed[2] = filteredVd ; return; } @@ -643,7 +662,6 @@ void _smoothVerticalData(GpsData_t* GPSData) if (OutVelCounter >= 5) { firstFlag = 0; /// reset - GPSData->filteredAlt = filteredAlt ; GPSData->vNed[2] = filteredVd ; return; } @@ -655,8 +673,7 @@ void _smoothVerticalData(GpsData_t* GPSData) // save the current time local to calc delta T next time LastTotalTime = TotalTime; - /// output the filtered alt and vel values - GPSData->filteredAlt = filteredAlt; + /// output the vel GPSData->vNed[2] = filteredVd; } @@ -668,14 +685,30 @@ void _smoothVerticalData(GpsData_t* GPSData) ******************************************************************************/ void convertItow(GpsData_t* GPSData) { - long double tmp; - - tmp = ((long double)GPSData->GPSHour) * 3600.0 + ((long double)GPSData->GPSMinute) * 60.0; - tmp += (long double)GPSData->GPSSecond + (long double)GPSData->GPSSecondFraction; + double tmp; + // calculate day of week + int tow = dayofweek(GPSData->GPSday, GPSData->GPSmonth, GPSData->GPSyear+2000); + // calculate second of week + tmp = (double)tow * 86400.0; + tmp += ((double)GPSData->GPSHour) * 3600.0 + ((double)GPSData->GPSMinute) * 60.0; + tmp += (double)GPSData->GPSSecond + (double)GPSData->GPSSecondFraction; /// converting UTC to GPS time is impossible without GPS satellite /// Navigation message. using UTC time directly by scaling to ms. GPSData->itow = (unsigned long) (tmp * 1000); } +int dayofweek(int day, int month, int year) +{ + + int adjustment, mm, yy; + + adjustment = (14 - month) / 12; + mm = month + 12 * adjustment - 2; + yy = year - adjustment; + + return (day + (13 * mm - 1) / 5 + + yy + yy / 4 - yy / 100 + yy / 400) % 7; +} + #endif // GPS diff --git a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processNovAtelGPS.c b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processNovAtelGPS.c index a385ac9..6722855 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processNovAtelGPS.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processNovAtelGPS.c @@ -221,16 +221,17 @@ void _parseBestPosB_Fast(logBestPosB *bestPosB, { GPSData->lat = bestPosB->Lat; GPSData->lon = bestPosB->Lon; - GPSData->alt = bestPosB->hgt; ///alt + GPSData->alt = bestPosB->hgt + bestPosB->undulation; // altitude above ellipsoid + GPSData->geoidAboveEllipsoid = bestPosB->undulation; // geoid above ellipsoid if (bestPosB->sol_status != 0) // zero is good fix anything else is { // enumeration for bad fix - GPSData->GPSFix = 0;// PosVelType + GPSData->gpsFixType = 0;// PosVelType gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // locked gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track gGpsDataPtr->HDOP = 21.0f; // force to above threshold } else { - GPSData->GPSFix = bestPosB->pos_type; + GPSData->gpsFixType = bestPosB->pos_type; gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track gGpsDataPtr->HDOP = 1.0f; // force to below threshold @@ -269,7 +270,7 @@ void _parseBestPosB_Fast(logBestPosB *bestPosB, GPSData->GPSHorizAcc = sqrtf( bestPosB->lat_sigma * bestPosB->lat_sigma + bestPosB->lon_sigma * bestPosB->lon_sigma ); // [m] GPSData->GPSVertAcc = bestPosB->hgt_sigma; // [m] - GPSData->numSatelites = bestPosB->num_obs; + GPSData->numSatellites = bestPosB->num_obs; } /** **************************************************************************** @@ -329,7 +330,7 @@ void _parseBestPosB(char *completeMessage, gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track } - GPSData->GPSFix = SolStatus[0]; + GPSData->gpsFixType = SolStatus[0]; memcpy(&GPSData->itow, &completeMessage[16], 4); ///Itow @@ -371,12 +372,12 @@ void _parseBestVelB_Fast(logBestVelB *bestVelB, if (bestVelB->sol_status != 0) // zero is good fix anything else is { // enumeration for bad fix - GPSData->GPSFix = bestVelB->vel_type; + GPSData->gpsFixType = bestVelB->vel_type; gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // not locked gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track gGpsDataPtr->HDOP = 21.0f; // } else { - GPSData->GPSFix = 0; + GPSData->gpsFixType = 0; gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // not locked gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track gGpsDataPtr->HDOP = 1.0f; // @@ -437,7 +438,7 @@ void _parseBestVelB(char *completeMessage, gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // not locked gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track } - GPSData->GPSFix = SolStatus[0]; + GPSData->gpsFixType = SolStatus[0]; memcpy(&GPSData->itow, &completeMessage[16], 4); ///Itow diff --git a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processSiRFGPS.c b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processSiRFGPS.c index 681af14..56c579d 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processSiRFGPS.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processSiRFGPS.c @@ -398,12 +398,12 @@ static void _parseSiRFGeodeticNavMsg(char *msg, // Switched at the sazme time as PPS 5 satellites if ( geo->navValid > 0) { GPSData->HDOP = 50.0; - GPSData->GPSFix = 1; // zero is valid anything else is degraded + GPSData->gpsFixType = 1; // zero is valid anything else is degraded gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // no signal lock gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track } else { GPSData->HDOP = geo->hdop; - GPSData->GPSFix = 0; + GPSData->gpsFixType = 0; gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track } @@ -422,7 +422,7 @@ static void _parseSiRFGeodeticNavMsg(char *msg, GPSData->GPSday = geo->utcDay; // dd GPSData->GPSyear = byteSwap16(geo->utcYear) - 2000; // last two digits of year - GPSData->altEllipsoid = (float)byteSwap32(geo->altitudeEllipsoid) / 100.; // 0x2221201f in m * 10^2 + GPSData->geoidAboveEllipsoid = (float)byteSwap32(geo->altitudeEllipsoid - geo->altitudeMSL) / 100.; // 0x2221201f in m * 10^2 GPSData->GPSHour = geo->utcHour; GPSData->GPSMinute = geo->utcMinute; diff --git a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processUbloxGPS.c b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processUbloxGPS.c index 2e75a53..9354476 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processUbloxGPS.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/processUbloxGPS.c @@ -32,6 +32,7 @@ limitations under the License. #include "driverGPS.h" #include "platformAPI.h" +#include "gpsAPI.h" #ifdef DISPLAY_DIAGNOSTIC_MSG #include "debug.h" @@ -312,7 +313,7 @@ void processUbloxBinaryMessage(char *msg, GPSData->VELCounter++; break; case UBLOX_NAV_STATUS: ///NAV_STATUS - GPSData->GPSFix = msg[10]; ///fix indicator + GPSData->gpsFixType = msg[10]; ///fix indicator if((msg[11]&0x02) == 0x02) ///6+5 GPSData->GPSStatusWord |=1 << DGPS_ON; ///on else @@ -422,10 +423,34 @@ void processUbloxBinaryMessage(char *msg, void decodeNavPvt(char *msg, GpsData_t *GPSData) { + // Decode ubloxNavPvtSTRUCT *navPvt; navPvt = (ubloxNavPvtSTRUCT*)(&msg[UBLOX_BINAERY_HEADER_LEN]); + + // Get decoded results // fix type - GPSData->gpsValid = navPvt->fixType == 3; + GPSData->gpsFixType = INVALID; + if (navPvt->flags & 0x01) + { + GPSData->gpsFixType = SPP; + if (navPvt->flags & 0x02) + { + GPSData->gpsFixType = DGPS; + } + uint8_t carrSoln = navPvt->flags >> 6; + if (carrSoln == 1) + { + GPSData->gpsFixType = RTK_FLOAT; + } + else if(carrSoln == 2) + { + GPSData->gpsFixType = RTK_FIX; + } + } + + // num of satellites + GPSData->numSatellites = navPvt->numSV; + // lat GPSData->lat = navPvt->lat * 1.0e-7; @@ -444,9 +469,12 @@ void decodeNavPvt(char *msg, GpsData_t *GPSData) // fractional second GPSData->GPSSecondFraction = navPvt->nano * 1.0e-9; // ellipsoid height - GPSData->altEllipsoid = navPvt->height * 1.0e-3; + GPSData->geoidAboveEllipsoid = (navPvt->height - navPvt->hMSL) * 1.0e-3; // itow GPSData->itow = navPvt->iTOW; + if(GPSData->itow%1000 != 0){ + GPSData->itow+=1; // correction of numerical issue + } platformUpdateITOW(GPSData->itow); // year month day hour min sec GPSData->GPSmonth = navPvt->month; diff --git a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/taskGps.c b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/taskGps.c index 3854b95..56a62a1 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/taskGps.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/GPS/src/taskGps.c @@ -89,16 +89,6 @@ void TaskGps(void const *argument) pollSirfCnt++; } } - - // Set the GPS validity, based on the Horizontal Dilution of Precision - if (gGpsDataPtr->gpsValid && gGpsDataPtr->HDOP > 15.0) - { - gGpsDataPtr->gpsValid = false; - } - else if (gGpsDataPtr->HDOP < 10.0) - { - gGpsDataPtr->gpsValid = true; - } } } } diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Math/include/TransformationMath.h b/examples/OpenIMU300RI/IMU/lib/Core/Math/include/TransformationMath.h index 910b5fc..2a01980 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Math/include/TransformationMath.h +++ b/examples/OpenIMU300RI/IMU/lib/Core/Math/include/TransformationMath.h @@ -73,12 +73,28 @@ real UnitGravityAndMagToYaw(real *unitGravityVector, real *magFieldVector); ******************************************************************************/ real RollPitchAndMagToYaw(real roll, real pitch, real *magFieldVector); +/****************************************************************************** +* @brief Limit angle error to be [-180, 180]. +* TRACE: +* @param [in] aErr in [deg]. +* @retval aErr within [-180, 180]deg. +******************************************************************************/ +real AngleErrDeg(real aErr); + +/****************************************************************************** +* @brief Limit angle error to be [-PI, PI]. +* TRACE: +* @param [in] aErr in [rad]. +* @retval aErr within [-2*PI, 2*PI]. +******************************************************************************/ +real AngleErrRad(real aErr); // BOOL LLA_To_R_EinN( double* llaRad, real* R_EinN); BOOL LLA_To_R_NinE( double* llaRad, real* R_NinE); -BOOL LLA_To_Base( double* llaRad, double* rECEF_Init, - real* dr_N, - real* R_NinE, double* rECEF ); +BOOL ECEF_To_Base( double* rECEF_Init, + double* rECEF , + real* R_NinE, + real* dr_N); BOOL LLA_To_ECEF( double* lla_Rad, double* ecef_m); BOOL PosNED_To_PosECEF( real* r_N, double* rECEF_Init, real* R_NinE, double* rECEF ); @@ -86,10 +102,9 @@ BOOL ECEF_To_LLA( double* llaDeg, double* ecef_m ); BOOL VelECEF_To_VelNED( double* LLA, real* VelECEF, real* VelNED ); + void printMtx(float *a, int m, int n); void printVec(float *v, int n); -int realSymmetricMtxEig(float *a, int n, float *v, float eps, int jt); - #endif /* TRANSFORMATIONMATH_H */ diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Math/src/TransformationMath.c b/examples/OpenIMU300RI/IMU/lib/Core/Math/src/TransformationMath.c index 9fc4b2c..9158f9b 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Math/src/TransformationMath.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/Math/src/TransformationMath.c @@ -204,68 +204,32 @@ BOOL LLA_To_R_NinE( double* llaRad, ///** *************************************************************************** -//* @name LLA2Base Express LLA in a local NED Base Frame +//* @name Calculate NED relative position of two ECEF positions. //* @details Pre calculated all non-changing constants and unfolded the matrices -//* @param [in] LLA - array with the current Latitude, Longitude and Altitude [rad] -//* @param [in] BaseECEF - start of frame position -//* @param [in] Rne - rotation matrix from ECEF to NED -//* @param [out] NED - output of the position in North East and Down coords -//* @param [out] newECEF - current position in ECEF from LLA +//* @param [in] rECEF_Init - start of frame position +//* @param [in] rECEF - current position in ECEF from LLA +//* @param [in] R_NinE - rotation matrix from NED to ECEF +//* @param [out] dr_N - output of the position in North East and Down coords //* @retval always 1 //******************************************************************************/ -BOOL LLA_To_Base( double* llaRad, // in - double* rECEF_Init, // in - real* dr_N, //NED, +BOOL ECEF_To_Base( double* rECEF_Init, + double* rECEF, real* R_NinE, - double* rECEF) // out + real* dr_N) { real dr_E[NUM_AXIS]; - double N; - double sinLat = sin(llaRad[LAT]); - double cosLat = cos(llaRad[LAT]); - double sinLon = sin(llaRad[LON]); - double cosLon = cos(llaRad[LON]); - - real sinLat_r = (real)sinLat; - real cosLat_r = (real)cosLat; - real sinLon_r = (real)sinLon; - real cosLon_r = (real)cosLon; - - N = E_MAJOR / sqrt(1.0 - (E_ECC_SQ * sinLat * sinLat)); // radius of Curvature [meters] - - //LLA_To_ECEF(llaRad, rECEF); - double temp_d = (N + llaRad[ALT]) * cosLat; - rECEF[X_AXIS] = temp_d * cosLon; - rECEF[Y_AXIS] = temp_d * sinLon; - rECEF[Z_AXIS] = ((E_MINOR_OVER_MAJOR_SQ * N) + llaRad[ALT]) * sinLat; - dr_E[X_AXIS] = (real)( rECEF[X_AXIS] - *(rECEF_Init + X_AXIS) ); dr_E[Y_AXIS] = (real)( rECEF[Y_AXIS] - *(rECEF_Init + Y_AXIS) ); dr_E[Z_AXIS] = (real)( rECEF[Z_AXIS] - *(rECEF_Init + Z_AXIS) ); - // Form R_NinE - // First row - *(R_NinE + 0 * 3 + 0) = -sinLat_r * cosLon_r; - *(R_NinE + 0 * 3 + 1) = -sinLon_r; - *(R_NinE + 0 * 3 + 2) = -cosLat_r * cosLon_r; - - // Second row - *(R_NinE + 1 * 3 + 0) = -sinLat_r * sinLon_r; - *(R_NinE + 1 * 3 + 1) = cosLon_r; - *(R_NinE + 1 * 3 + 2) = -cosLat_r * sinLon_r; - - // Third row - *(R_NinE + 2 * 3 + 0) = cosLat_r; - *(R_NinE + 2 * 3 + 1) = 0.0; - *(R_NinE + 2 * 3 + 2) = -sinLat_r; - - // Convert from delta-position in the ECEF-frame to the NED-frame (the transpose - // in the equations that followed is handled in the formulation) - // - // N E ( E N )T - // dr_N = R * dr_E = ( R ) * dr_E - // ( ) + /* Convert from delta-position in the ECEF-frame to the NED-frame (the transpose + * in the equations that followed is handled in the formulation) + * + * N E ( E N )T + * dr_N = R * dr_E = ( R ) * dr_E + * ( ) + */ dr_N[X_AXIS] = *(R_NinE + X_AXIS * 3 + X_AXIS) * dr_E[X_AXIS] + *(R_NinE + Y_AXIS * 3 + X_AXIS) * dr_E[Y_AXIS] + *(R_NinE + Z_AXIS * 3 + X_AXIS) * dr_E[Z_AXIS]; @@ -438,97 +402,36 @@ void printVec(float *v, int n) printf("%.9g\n", v[i]); } -int realSymmetricMtxEig(float *a, int n, float *v, float eps, int jt) +real AngleErrDeg(real aErr) { - int i, j, p, q, u, w, t, s, l; - double fm, cn, sn, omega, x, y, d; - l = 1; - p = 0; q = 0; // initialized AB - for (i = 0; i <= n - 1; i++) - { - v[i*n + i] = 1.0; - for (j = 0; j <= n - 1; j++) - { - if (i != j) - { - v[i*n + j] = 0.0; - } - } - } - while (1) + while (fabs(aErr) > 180.0) { - fm = 0.0; - for (i = 0; i <= n - 1; i++) + if (aErr > 180.0) { - for (j = 0; j <= n - 1; j++) - { - d = fabs(a[i*n + j]); - if ((i != j) && (d > fm)) - { - fm = d; - p = i; - q = j; - } - } + aErr -= (real)360.0; } - if (fm < eps) + else if (aErr < -180.0) { - return(1); + aErr += (real)360.0; } - if (l > jt) - { - return(-1); - } - l = l + 1; - u = p * n + q; - w = p * n + p; - t = q * n + p; - s = q * n + q; - x = -a[u]; - y = (a[s] - a[w]) / 2.0; - omega = x / sqrt(x*x + y * y); - if (y < 0.0) - { - omega = -omega; - } - sn = 1.0 + sqrt(1.0 - omega * omega); - sn = omega / sqrt(2.0*sn); - cn = sqrt(1.0 - sn * sn); - fm = a[w]; - a[w] = fm * cn*cn + a[s] * sn*sn + a[u] * omega; - a[s] = fm * sn*sn + a[s] * cn*cn - a[u] * omega; - a[u] = 0.0; - a[t] = 0.0; - for (j = 0; j <= n - 1; j++) - { - if ((j != p) && (j != q)) - { - u = p * n + j; - w = q * n + j; - fm = a[u]; - a[u] = fm * cn + a[w] * sn; - a[w] = -fm * sn + a[w] * cn; - } - } - for (i = 0; i <= n - 1; i++) + } + + return aErr; +} + +real AngleErrRad(real aErr) +{ + while (fabs(aErr) > PI) + { + if (aErr > PI) { - if ((i != p) && (i != q)) - { - u = i * n + p; - w = i * n + q; - fm = a[u]; - a[u] = fm * cn + a[w] * sn; - a[w] = -fm * sn + a[w] * cn; - } + aErr -= (real)TWO_PI; } - for (i = 0; i <= n - 1; i++) + else if (aErr < -PI) { - u = i * n + p; - w = i * n + q; - fm = v[u]; - v[u] = fm * cn + v[w] * sn; - v[w] = -fm * sn + v[w] * cn; + aErr += (real)TWO_PI; } } - return(1); -} \ No newline at end of file + + return aErr; +} diff --git a/examples/OpenIMU300RI/IMU/lib/Core/Common/src/dummy.c b/examples/OpenIMU300RI/IMU/lib/Core/Odo/odoAPI.h similarity index 74% rename from examples/OpenIMU300RI/IMU/lib/Core/Common/src/dummy.c rename to examples/OpenIMU300RI/IMU/lib/Core/Odo/odoAPI.h index 395b50f..458560b 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/Common/src/dummy.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/Odo/odoAPI.h @@ -1,12 +1,13 @@ /** *************************************************************************** - * @file spi_dummy.c + * @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 @@ -22,21 +23,18 @@ 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 SPI_BUS_COMM -#include -#include - -// Dummy placeholder function -void SPI_ProcessCommand(uint8_t cmd) -{ +#ifndef ODO_API_H +#define ODO_API_H -} +#include +#include "GlobalConstants.h" -// Dummy placeholder function -void SPI_ProcessData(uint8_t* in) -{ +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 +#endif /* ODO_API_H */ diff --git a/examples/OpenIMU300RI/IMU/lib/Core/UARTComm/CommonMessages.c b/examples/OpenIMU300RI/IMU/lib/Core/UARTComm/CommonMessages.c index 4812cfb..09cbf0b 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/UARTComm/CommonMessages.c +++ b/examples/OpenIMU300RI/IMU/lib/Core/UARTComm/CommonMessages.c @@ -253,7 +253,7 @@ BOOL Fill_e1PacketPayload(uint8_t *payload, uint8_t *payloadLen) *payloadLen = sizeof(ekf1_payload_t); pld->tstmp = platformGetIMUCounter(); - pld->dbTstmp = (double)platformGetSolutionTstamp() * 0.000001; // seconds + pld->dbTstmp = platformGetSolutionTstampAsDouble() * 0.000001; // seconds EKF_GetAttitude_EA(EulerAngles); pld->roll = (float)EulerAngles[ROLL]; @@ -306,8 +306,7 @@ BOOL Fill_e2PacketPayload(uint8_t *payload, uint8_t *payloadLen) *payloadLen = sizeof(ekf2_payload_t); pld->tstmp = platformGetIMUCounter(); - pld->dbTstmp = (double)platformGetSolutionTstamp() * 0.000001; // seconds - + pld->dbTstmp = platformGetSolutionTstampAsDouble() * 0.000001; // seconds EKF_GetAttitude_EA(rData); pld->roll = (float)rData[ROLL]; @@ -347,7 +346,7 @@ BOOL Fill_e2PacketPayload(uint8_t *payload, uint8_t *payloadLen) EKF_GetEstimatedLLA(dData); for(int i = 0; i < NUM_AXIS; i++){ - pld->pos[i] = (float)dData[i]; + pld->pos[i] = dData[i]; } uint8_t opMode, linAccelSw, turnSw; diff --git a/examples/OpenIMU300RI/IMU/lib/Core/library.json b/examples/OpenIMU300RI/IMU/lib/Core/library.json index 497943a..f5babce 100644 --- a/examples/OpenIMU300RI/IMU/lib/Core/library.json +++ b/examples/OpenIMU300RI/IMU/lib/Core/library.json @@ -1,22 +1,24 @@ { - "name": "Core", - "version": "1.0.4", - "description": "Algorithm and GPS libraries", - "build": { - "flags": [ - "-I Algorithm", - "-I Algorithm/include", - "-I Buffer", - "-I Buffer/include", - "-I Common", - "-I Common/include", - "-I GPS", - "-I GPS/include", - "-I Math", - "-I Math/include", - "-I SPIComm", - "-I UARTComm", - "-I ." + "name": "Core", + "version": "1.0.4", + "description": "Algorithm and GPS libraries", + "build": { + "flags": [ + "-I Algorithm", + "-I Algorithm/include", + "-I Buffer", + "-I Buffer/include", + "-I Common", + "-I Common/include", + "-I GPS", + "-I GPS/include", + "-I Odo", + "-I Odo/include", + "-I Math", + "-I Math/include", + "-I SPIComm", + "-I UARTComm", + "-I ." ] } } diff --git a/examples/OpenIMU300RI/IMU/platformio.ini b/examples/OpenIMU300RI/IMU/platformio.ini index 3008242..cc4f0c1 100644 --- a/examples/OpenIMU300RI/IMU/platformio.ini +++ b/examples/OpenIMU300RI/IMU/platformio.ini @@ -14,8 +14,8 @@ description = IMU Application example for CAN bus with J1939 protocol. platform = aceinna_imu lib_archive = false board = OpenIMU300 -;lib_deps= ../../openIMU300-lib -lib_deps = OpenIMU300-base-library@1.0.8 +;lib_deps = ../../openIMU300-lib/openIMU300-lib +lib_deps = OpenIMU300-base-library@1.1.6 build_flags = ; -D CLI -D SAE_J1939 @@ -23,7 +23,6 @@ build_flags = -D __FPU_PRESENT -D ARM_MATH_CM4 -I include - -I include/API -I src/user -I src -I lib/CAN/include diff --git a/examples/OpenIMU300RI/IMU/src/appVersion.h b/examples/OpenIMU300RI/IMU/src/appVersion.h index 1ade20a..e5eb59d 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 1.0.1" +#define APP_VERSION_STRING "IMU_J1939 1.1.3" #endif diff --git a/examples/OpenIMU300RI/IMU/src/main.c b/examples/OpenIMU300RI/IMU/src/main.c index 89e3d3d..6f7dec8 100644 --- a/examples/OpenIMU300RI/IMU/src/main.c +++ b/examples/OpenIMU300RI/IMU/src/main.c @@ -138,7 +138,7 @@ int main(void) // initializa debug interface if desired // Messages will be output from user serial port - DebugInterfaceInit(); +// DebugInterfaceInit(); // Apply factory configuration platformInitConfigureUnit(); diff --git a/examples/OpenIMU300RI/IMU/src/user/UserConfiguration.c b/examples/OpenIMU300RI/IMU/src/user/UserConfiguration.c index 3ee9b41..8536b3e 100644 --- a/examples/OpenIMU300RI/IMU/src/user/UserConfiguration.c +++ b/examples/OpenIMU300RI/IMU/src/user/UserConfiguration.c @@ -50,7 +50,7 @@ const UserConfigurationStruct gDefaultUserConfig = { .ecuPacketRate = 1, // 100Hz .ecuFilterFreqAccel = 25, .ecuFilterFreqRate = 25, - .ecuPacketType = ( ACEINNA_SAE_J1939_PACKET_SLOPE_SENSOR | + .ecuPacketType = ( ACEINNA_SAE_J1939_PACKET_ANGULAR_RATE | ACEINNA_SAE_J1939_PACKET_ACCELERATION | ACEINNA_SAE_J1939_PACKET_MAG diff --git a/examples/OpenIMU300RI/IMU/src/user/UserMessagingCAN.c b/examples/OpenIMU300RI/IMU/src/user/UserMessagingCAN.c index d1da2d6..ee69f53 100644 --- a/examples/OpenIMU300RI/IMU/src/user/UserMessagingCAN.c +++ b/examples/OpenIMU300RI/IMU/src/user/UserMessagingCAN.c @@ -357,6 +357,8 @@ void ProcessEcuCommands(void * command, uint8_t ps, uint8_t addr) } + + /** *************************************************************************** * @name ProcessDataPAcketss * @brief decode and process incoming Vehicle data packets diff --git a/examples/OpenIMU300RI/IMU/src/user/UserMessagingUART.c b/examples/OpenIMU300RI/IMU/src/user/UserMessagingUART.c index 13c5ec7..cc6e93e 100644 --- a/examples/OpenIMU300RI/IMU/src/user/UserMessagingUART.c +++ b/examples/OpenIMU300RI/IMU/src/user/UserMessagingUART.c @@ -323,9 +323,8 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) pld->c = 'A'; pld->s = 1234; pld->i = -5; - if(platformGetPpsFlag()){ + if(platformGetPpsFlag(TRUE)){ ppsTstamp = platformGetPpsTimeStamp(); - platformSetPpsFlag(FALSE); } // pld->ll = platformGetDacqTimeStamp(); // in microseconds; pld->ll = ppsTstamp; // time stamp of last PPS edge in microseconds from system start diff --git a/examples/OpenIMU300RI/INS/.gitignore b/examples/OpenIMU300RI/INS/.gitignore new file mode 100644 index 0000000..f53cace --- /dev/null +++ b/examples/OpenIMU300RI/INS/.gitignore @@ -0,0 +1,9 @@ +.pio/** +.pio/libdeps/** +.vscode/** +.pioenvs/** +.piolibdeps/** +*.map +.settings +.cproject +.project diff --git a/examples/OpenIMU300RI/INS/.travis.yml b/examples/OpenIMU300RI/INS/.travis.yml new file mode 100644 index 0000000..52072ef --- /dev/null +++ b/examples/OpenIMU300RI/INS/.travis.yml @@ -0,0 +1,55 @@ +# Continuous Integration (CI) is the practice, in software +# engineering, of merging all developer working copies with a shared mainline +# several times a day < http://docs.platformio.org/page/ci/index.html > +# +# Documentation: +# +# * Travis CI Embedded Builds with PlatformIO +# < https://docs.travis-ci.com/user/integration/platformio/ > +# +# * PlatformIO integration with Travis CI +# < http://docs.platformio.org/page/ci/travis.html > +# +# * User Guide for `platformio ci` command +# < http://docs.platformio.org/page/userguide/cmd_ci.html > +# +# +# Please choice one of the following templates (proposed below) and uncomment +# it (remove "# " before each line) or use own configuration according to the +# Travis CI documentation (see above). +# + + +# +# Template #1: General project. Test it using existing `platformio.ini`. +# + +# language: python +# python: +# - "2.7" +# +# install: +# - pip install -U platformio +# +# script: +# - platformio run + + +# +# Template #2: The project is intended to by used as a library with examples +# + +# language: python +# python: +# - "2.7" +# +# env: +# - PLATFORMIO_CI_SRC=path/to/test/file.c +# - PLATFORMIO_CI_SRC=examples/file.ino +# - PLATFORMIO_CI_SRC=path/to/test/directory +# +# install: +# - pip install -U platformio +# +# script: +# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N diff --git a/examples/OpenIMU300RI/VG_AHRS/src/FreeRTOSConfig.h b/examples/OpenIMU300RI/INS/include/FreeRTOSConfig.h similarity index 100% rename from examples/OpenIMU300RI/VG_AHRS/src/FreeRTOSConfig.h rename to examples/OpenIMU300RI/INS/include/FreeRTOSConfig.h diff --git a/examples/OpenIMU300RI/VG_AHRS/src/osresources.h b/examples/OpenIMU300RI/INS/include/osresources.h similarity index 100% rename from examples/OpenIMU300RI/VG_AHRS/src/osresources.h rename to examples/OpenIMU300RI/INS/include/osresources.h diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/algorithmAPI.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/algorithmAPI.h new file mode 100644 index 0000000..95f0185 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/algorithmAPI.h @@ -0,0 +1,121 @@ +/** ****************************************************************************** + * @file algorithmAPI.h API functions for Interfacing with lgorithm + * + * 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. + * + *****************************************************************************/ +/******************************************************************************* +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 _ALGORITHM_API_H +#define _ALGORITHM_API_H + +#include +#include "GlobalConstants.h" +#include "algorithm.h" + +/****************************************************************************** + * @name Initialize_AlgorithmStruct + * @brief initializes the values in the AlgorithmStruct + * + * @retval N/A + *****************************************************************************/ +void InitializeAlgorithmStruct(uint8_t callingFreq); + +/****************************************************************************** + * @brief Get algorithm status. + * Refer to the declaration of struct ALGO_STATUS_BITS to see the details about + * algorithm status. + * @param [in] algoStatus: to store the returned values. + * @param [out] algoStatus + * @retval None. +******************************************************************************/ +void GetAlgoStatus(AlgoStatus *algoStatus); + +void setAlgorithmStateStabilizeSystem(); + +uint16_t getAlgorithmCounter(); + +/****************************************************************************** + * @brief Set the calling frequency of the algorithm. + * @param [in] + * @param [out] + * @retval None. +******************************************************************************/ +void setAlgorithmExeFreq(int freq); + +void enableMagInAlgorithm(BOOL enable); + +BOOL magUsedInAlgorithm(); + +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 ); + +uint32_t getAlgorithmTimer(); + +uint16_t getAlgorithmCounter(); +uint16_t getAlgorithmFrequency(); +uint32_t getAlgorithmITOW(); + + +/****************************************************************************** + * @name setLeverArm + * @brief Set the position of the antenna relative to the IMU in the vehicle + * body frame. Lever arm will be substracted from the GNSS measurement to get + * the position of the IMU. + * @param [in] leverArmBx/y/z: lever arm in the vehicle body frame, in unti of meters. + * @param [out] + * @retval None. + *****************************************************************************/ +void setLeverArm( real leverArmBx, real leverArmBy, real leverArmBz ); + +/****************************************************************************** + * @name setPointOfInterest + * @brief Set the position of the point of interest relative to the IMU in the + * vehicle body frame. This value will be added to the position of the IMU to get + * the absolute position of the point of interest. + * @param [in] poiBx/y/z: position of the point of interest in the vehicle + * body frame, in unti of meters. + * @param [out] + * @retval None. + *****************************************************************************/ +void setPointOfInterest( real poiBx, real poiBy, real poiBz ); + +#endif diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/AlgorithmLimits.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/AlgorithmLimits.h new file mode 100644 index 0000000..6a1262c --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/AlgorithmLimits.h @@ -0,0 +1,58 @@ +/*************************************************************************** + * File: AlgorithmLimits.h + ***************************************************************************/ +/******************************************************************************* +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 _ALGORITHM_LIMITS_H_ +#define _ALGORITHM_LIMITS_H_ + +#include "GlobalConstants.h" + +#define INIT_P_Q 1.0e-5; +#define INIT_P_WB 1.0e-5; +#define INIT_P_INS 1.0e-3; + +// Declare the limits +#define LIMIT_P 500.0 + +#define LIMIT_ANG_ERROR_ROLL 0.017453292519943 // ONE_DEGREE_IN_RAD +#define LIMIT_ANG_ERROR_PITCH 0.017453292519943 // ONE_DEGREE_IN_RAD +#define LIMIT_ANG_ERROR_YAW 0.043633231299858 //(2.5 * ONE_DEGREE_IN_RAD) +#define LIMIT_YAW_RATE_SQ 0.001903858873667 // ( 2.5 [ deg/sec ] * DEG_TO_RAD )^2 + +#define LIMIT_BIAS_RATE_UPDATE_AHRS 5.0e-3 +#define LIMIT_BIAS_RATE_UPDATE_INS 5.0e-4 + +#define LIMIT_MIN_GPS_VELOCITY_HEADING 0.45 //0.45 m/s ~= 1.0 mph +#define RELIABLE_GPS_VELOCITY_HEADING 1.0 // velocity of 1.0m/s should provide reliable GNSS heading + +#define LIMIT_OBS_JACOBIAN_DENOM 1e-3; + +// The following times are compared against ITOW (units in [msec]) +#define LIMIT_MAX_GPS_DROP_TIME 300 // [sec] +#define LIMIT_RELIABLE_DR_TIME 10 // [sec] +#define LIMIT_MAX_REST_TIME_BEFORE_DROP_TO_AHRS 60000 // 60000 [ msec ] = 60 [ sec ] +#define LIMIT_MAX_REST_TIME_BEFORE_HEADING_INVALID 120000 // 120sec, heading drifts much slower than pos +#define LIMIT_DECL_EXPIRATION_TIME 60000 // 60,000 [ counts ] = 10 [ min ] + +// The following is compared against a counter (in units of the calling frequency of the EKF) +#define LIMIT_FREE_INTEGRATION_CNTR 60 // 60 [ sec ] + +#define LIMIT_QUASI_STATIC_STARTUP_RATE 0.087266462599716 // (5.0 * ONE_DEGREE_IN_RAD) + +#endif /* _ALGORITHM_LIMITS_H_ */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/EKF_Algorithm.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/EKF_Algorithm.h new file mode 100644 index 0000000..6c0e946 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/EKF_Algorithm.h @@ -0,0 +1,178 @@ +/* + * File: EKF_Algorithm.h + * Author: joemotyka + * + * Created on May 8, 2016, 12:23 AM + */ + +#ifndef _EKF_ALGORITHM_H_ +#define _EKF_ALGORITHM_H_ + +#include + +#include "GlobalConstants.h" +#include "StateIndices.h" + +#include "gpsAPI.h" // For gpsDataStruct_t in EKF setter +#include "odoAPI.h" + +#include "Indices.h" + +// Changed to 1e-2 on Sep 13, 2016 +#define INIT_P 0.01 + + +// Global Kalman Filter structure +typedef struct { + // States + real Velocity_N[NUM_AXIS]; + real Position_N[NUM_AXIS]; + real quaternion[4], quaternion_Past[4]; + real rateBias_B[NUM_AXIS]; + real accelBias_B[NUM_AXIS]; + + // Prediction variables: P = FxPxFTranspose + Q + real F[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; + real P[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; + real Q[NUMBER_OF_EKF_STATES]; + real Qq[6]; /* The process cov matrix of quaternion should be 4x4. + * Its 4 diagonal terms are stored in Q. + * Its off-diagonol terms are stored in Qq. Because the matrix + * is symmetric, only 6 off-diagonal terms need stored. + */ + + real correctedRate_B[NUM_AXIS]; // [rad/s] + real correctedAccel_B[NUM_AXIS]; // [m/s/s] + real linearAccel_B[NUM_AXIS]; // [m/s/s], linear acceleration in body frame, used to detect drive position + + /* Algorithm results. Velocity states are directly used as results for output. + * The following two are calculated from state + */ + real eulerAngles[NUM_AXIS]; + double llaDeg[NUM_AXIS]; + + // measurements + real R_BinN[3][3]; // convert body to NED + real Rn2e[3][3]; // Coordinate tranformation matrix from NED to ECEF + real measuredEulerAngles[3]; // Euler angles measurements + real rGPS_N[3]; // current IMU position w.r.t rGPS0_E in NED. + double rGPS0_E[3]; // Initial IMU ECEF position when first entering INS state. + double rGPS_E[3]; // current IMU ECEF position + + // Update variables: S = HxPxHTranspose + R + real nu[9]; + real H[3][NUMBER_OF_EKF_STATES]; + 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; + +/* Global Algorithm structure */ +typedef struct { + // Sensor readings in the body-frame (B) + real accel_B[NUM_AXIS]; // [m/s/s] + real angRate_B[NUM_AXIS]; // [rad/s] + real magField_B[NUM_AXIS]; // [G] + + // GPS information + uint32_t itow; + double llaAnt[3]; // Antenna Lat, Lon, ellipsoid Altitude, [rad, rad, meter] + double vNedAnt[NUM_AXIS]; // Antenna NED velocity, [m/s, m/s, m/s] + double lla[3]; // IMU Lat, Lon, ellipsoid Altitude, [rad, rad, meter] + double vNed[3]; // IMU NED velocity, [m/s, m/s, m/s] + float geoidAboveEllipsoid; // [m] + real trueCourse; // Antenna heading, [deg] + real rawGroundSpeed; // IMU ground speed, calculated from vNed, [m/s] + float GPSHorizAcc; // [m] + float GPSVertAcc; // [m] + float HDOP; + uint8_t gpsFixType; // Indicate if this GNSS measurement is valid + uint8_t numSatellites; /* Num of satellites in this GNSS measurement. + * 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; + + +/* Global Algorithm structure */ +typedef struct { + // Algorithm states (15 states) + double position_N[NUM_AXIS]; + double velocity_N[NUM_AXIS]; + double quaternion_BinN[4]; + double angRateBias_B[NUM_AXIS]; + double accelBias_B[NUM_AXIS]; + + double llaDeg[NUM_AXIS]; + + // Derived variables + double eulerAngs_BinN[NUM_AXIS]; + double corrAngRates_B[NUM_AXIS]; + double corrAccel_B[NUM_AXIS]; + + // Operational states + uint8_t opMode; + uint8_t turnSwitchFlag; + uint8_t linAccelSwitch; +} EKF_OutputDataStruct; + +extern EKF_OutputDataStruct gEKFOutput; + + +// Initialize Kalman filter parameters of the INS app +uint8_t InitINSFilter(void); + +void EKF_Algorithm(void); +void enableFreeIntegration(BOOL enable); + +// Getters for data extraction from the EKF output data structure +void EKF_GetAttitude_EA(real *EulerAngles); +void EKF_GetAttitude_EA_RAD(real *EulerAngles); +void EKF_GetAttitude_Q(real *Quaternions); +void EKF_GetCorrectedAngRates(real *CorrAngRates_B); +void EKF_GetCorrectedAccels(real *CorrAccels_B); +void EKF_GetEstimatedAngRateBias(real *AngRateBias_B); +void EKF_GetEstimatedAccelBias(real *AccelBias_B); +void EKF_GetEstimatedPosition(real *Position_N); +void EKF_GetEstimatedVelocity(real *Velocity_N); +void EKF_GetEstimatedLLA(double *LLA); + +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, odoDataStruct_t *odo, + BOOL ppsDetected); +void EKF_SetOutputStruct(void); + +#endif /* _EKF_ALGORITHM_H_ */ + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/MagAlign.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/MagAlign.h new file mode 100644 index 0000000..7dbb4e6 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/MagAlign.h @@ -0,0 +1,57 @@ +/****************************************************************************** + * File: MagAlign.h + *******************************************************************************/ +/******************************************************************************* +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 MAGALIGN_H +#define MAGALIGN_H + +#include "GlobalConstants.h" + +/* new mag-align progress status */ +#define MAG_ALIGN_STATUS_LEVEL_START 0x6 +#define MAG_ALIGN_STATUS_LEVEL_END 0xF +#define MAG_ALIGN_STATUS_SAVE2EEPROM 0xE +#define MAG_ALIGN_STATUS_START_CAL_WITHOUT_AUTOEND 0x9 +#define MAG_ALIGN_STATUS_START_CAL_WITH_AUTOEND 0xC +#define MAG_ALIGN_STATUS_TERMINATION 0xB +#define MAG_ALIGN_STATUS_COMPLETE 0xD +#define MAG_ALIGN_STATUS_ACCEPTED 0xA +#define MAG_ALIGN_STATUS_IDLE 0x0 + +#define ONE_DEGREE_Q29 9370165 + +typedef struct { + real hardIronBias[2]; + real softIronAngle; + real softIronScaleRatio; +} ComputedMagneticParams; + +typedef struct { + real hardIronBias[2]; + real softIronAngle; + real softIronScaleRatio; + real SF[4]; + + ComputedMagneticParams estParams; + + volatile int state; +} MagAlignStruct; + +extern MagAlignStruct gMagAlign; + +#endif /* MAGALIGN_H */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/MotionStatus.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/MotionStatus.h new file mode 100644 index 0000000..785ac68 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/MotionStatus.h @@ -0,0 +1,90 @@ +/***************************************************************************** + * @file MotionStatus.h + * @brief Calculate sensor stats, and detect motion status using IMU/ODO/GNSS + * @author Dong Xiaoguang + * @version 1.0 + * @date 20190801 + *****************************************************************************/ + +#ifndef MOTION_STATUS_H_INCLUDED +#define MOTION_STATUS_H_INCLUDED + +#include +#include "GlobalConstants.h" +#include "algorithm.h" + + +typedef struct +{ + BOOL bValid; // tell if stats are valid + BOOL bStaticIMU; // Static period detected by IMU + BOOL accelErrLimit; // accelErr is set to max/min limit + real lpfAccel[3]; // [m/s/s], low-pass filtered accel + real accelNorm; // [m/s/s], magnitude of current accel + real accelMean[3]; // [m/s/s], average of past n accel samples + real accelVar[3]; // [m/s/s]^2, variance of past n accel samples + real accelErr[3]; // [m/s/s], estimated accel error + real lpfGyro[3]; // [rad/s], low-pass filtered gyro + real gyroMean[3]; // [rad/s], average of past n gyro samples + real gyroVar[3]; // [rad/s]^2, variance of past n gyro samples +} ImuStatsStruct; + +/****************************************************************************** + * @brief Calculate IMU data stats, and detect zero velocity. + * TRACE: + * @param [in] gyro [rad/s] + * @param [in] accel [m/s/s] + * @param [in] reset TRUE to reset this process + * @param [Out] imuStats results + * @retval None. +******************************************************************************/ +void MotionStatusImu(real *gyro, real *accel, ImuStatsStruct *imuStats, BOOL reset); + +/****************************************************************************** + * @brief Using gyro propagation to estimate accel error. + * g_dot = -cross(w, g), g is gravity and w is angular rate. + * TRACE: + * @param [in] accel Input accel, m/s/s. + * @param [in] w Input angular rate, rad/s. + * @param [in] dt Sampling interval, sec. + * @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 [out] gAccelStats A struct for results storage. + * @retval None. +******************************************************************************/ +void EstimateAccelError(real *accel, real *w, real dt, uint32_t staticDelay, ImuStatsStruct *gAccelStats); + +/****************************************************************************** + * @brief Detect motion according to the difference between measured accel magnitude and 1g. + * Set gAlgorithm.linAccelSwitch to be True if being static for a while. + * This result is no longer used in current algorithm. + * TRACE: + * @param [in] accelNorm Input accel magnitude, g. + * @param [in] iReset Reset the procedure. + * @retval Always true. +******************************************************************************/ +BOOL DetectMotionFromAccel(real accelNorm, int iReset); + +/****************************************************************************** + * @brief Detect zero velocity using GNSS speed. + * TRACE: + * @param [in] vNED NED velocity measured by GNSS [m/s] + * @param [in] gnssValid Indicate if GNSS measurement is valid. + * If valid, vNED will be used to detect zero velocity. + * If not, detection will be reset and FALSE is always + * returned. + * @retval TRUE if static, otherwise FALSE. +******************************************************************************/ +BOOL DetectStaticGnssVelocity(double *vNED, real threshold, BOOL gnssValid); + +/****************************************************************************** + * @brief Detect zero velocity using odometer data. + * TRACE: + * @param [in] odo velocity measured by odometer [m/s] + * @retval TRUE if static, otherwise FALSE. +******************************************************************************/ +BOOL DetectStaticOdo(real odo); + +#endif // MOTION_STATUS_H_INCLUDED + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/PredictFunctions.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/PredictFunctions.h new file mode 100644 index 0000000..c2dde5f --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/PredictFunctions.h @@ -0,0 +1,15 @@ +#ifndef EKF_ALGORITHM_H +#define EKF_ALGORITHM_H + +void EKF_PredictionStage(real *filteredAccel); +void GenerateProcessCovariance(void); +void GenerateProcessJacobian(void); + +// Size of EKF matrices +#define ROWS_IN_P 16 +#define COLS_IN_P 16 + +#define ROWS_IN_F 16 +#define COLS_IN_F 16 + +#endif /* EKF_ALGORITHM_H */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/SelectState.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/SelectState.h new file mode 100644 index 0000000..380f0da --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/SelectState.h @@ -0,0 +1,19 @@ +/* + * File: SelectState.h + * Author: joemotyka + * + * Created on May 6, 2016, 11:37 PM + */ + +#ifndef SELECTSTATE_H +#define SELECTSTATE_H + +void StabilizeSystem(void); +void InitializeAttitude(void); +void HG_To_LG_Transition_Test(void); +void LG_To_INS_Transition_Test(void); +void INS_To_AHRS_Transition_Test(void); + +void DynamicMotion(void); + +#endif /* SELECTSTATE_H */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/SensorNoiseParameters.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/SensorNoiseParameters.h new file mode 100644 index 0000000..5b509d2 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/SensorNoiseParameters.h @@ -0,0 +1,46 @@ +/* +* File: SensorNoiseParameters.h +* Author: joemotyka +* +* Created on April 10, 2016, 11:37 PM +*/ + +#ifndef _SENSOR_NOISE_PARAMETERS_H_ +#define _SENSOR_NOISE_PARAMETERS_H_ + +#include "GlobalConstants.h" + +// IMU spec +#define RW_ODR 100.0 /* [Hz], The data sampling rate when calculate ARW and VRW. + * ARW = sigma * sqrt(dt) = sigma * sqrt(1/ODR) + */ +#define ARW_300ZA 8.73e-5 /* [rad/sqrt(s)], gyro angular random walk, sampled at 100Hz + * 0.3deg/sqrt(Hr) = 0.3 / 60 * D2R = 8.72664625997165e-05rad/sqrt(s) + */ +#define BIW_300ZA 2.91e-5 /* [rad/s], gyro bias instability + * 6.0deg/Hr = 6.0 / 3600 * D2R = 2.90888208665722e-05rad/s + */ +#define MAX_BW 8.73e-3 /* [rad/s], max possible gyro bias + * 0.5deg/s = 0.5 * D2R = 0.00872664625997165rad/s + */ +#define VRW_300ZA 1.0e-3 /* [m/s/sqrt(s)], accel velocity random walk, sampled at 100Hz + * 0.06m/s/sqrt(Hr) = 0.06 / 60 = 0.001m/s/sqrt(s) + */ +#define BIA_300ZA 10.0e-6 * GRAVITY /* [m/s/s], accel bias instability + * 10ug = 10.0e-6g * GRAVITY + */ +#define MAX_BA 3.0e-3 * GRAVITY /* [m/s/s], max possible accel bias + * 3mg = 3.0e-3g * GRAVITY + */ + +// GNSS spec +#define R_VALS_GPS_POS_X 5.0 +#define R_VALS_GPS_POS_Y 5.0 +#define R_VALS_GPS_POS_Z 7.5 + +#define R_VALS_GPS_VEL_X 0.025 +#define R_VALS_GPS_VEL_Y 0.025 +#define R_VALS_GPS_VEL_Z 0.025 + + +#endif /* _SENSOR_NOISE_PARAMETERS_H_ */ \ No newline at end of file diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/StateIndices.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/StateIndices.h new file mode 100644 index 0000000..e43b06f --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/StateIndices.h @@ -0,0 +1,35 @@ +/* + * File: StateIndices.h + * Author: joemotyka + * + * Created on April 10, 2016, 11:09 PM + */ + +#ifndef STATEINDICES_H +#define STATEINDICES_H + +#define NUMBER_OF_EKF_STATES 16 + + +#define STATE_RX 0 +#define STATE_RY 1 +#define STATE_RZ 2 +#define STATE_VX 3 +#define STATE_VY 4 +#define STATE_VZ 5 +#define STATE_Q0 6 +#define STATE_Q1 7 +#define STATE_Q2 8 +#define STATE_Q3 9 +#define STATE_WBX 10 +#define STATE_WBY 11 +#define STATE_WBZ 12 +#define STATE_ABX 13 +#define STATE_ABY 14 +#define STATE_ABZ 15 + +#define STATE_ROLL 6 +#define STATE_PITCH 7 +#define STATE_YAW 8 + +#endif /* STATEINDICES_H */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/TimingVars.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/TimingVars.h new file mode 100644 index 0000000..5f545f0 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/TimingVars.h @@ -0,0 +1,76 @@ +/****************************************************************************** + * @file TimingVars.h + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + + + +#include // uint8_t, etc. + + +#ifndef TIMINGVARS_H +#define TIMINGVARS_H + +#include "GlobalConstants.h" + +// +void TimingVars_Increment(void); +float TimingVars_GetTime(void); + +void TimingVars_SetTMin(float tMin); +float TimingVars_GetTMin(void); +void TimingVars_dacqFrequency(int freq); + +void TimingVars_DisplayTimerVars(signed long timeStep); + +#ifdef DISPLAY_DIAGNOSTIC_MSG + #ifdef INS_OFFLINE + #include // std::cin, std::cout + #include // std::ifstream + #include + #include + void TimingVars_DiagnosticMsg(std::string msg); + #else + #include "debug.h" + void TimingVars_DiagnosticMsg(char *msg); + #endif +#endif + + +uint32_t TimingVars_GetTimeStep(void); + +typedef struct { + uint8_t dacqFrequency; + uint32_t secondCntr; + uint8_t tenHertzCntr; + int8_t subFrameCntr; + uint16_t basicFrameCounter; + + // toggles between 0 and 1 at 200 Hz (currently used in firmware) + int oneHundredHertzFlag; + + // + float time; + float tMin; +} TimingVars; + +extern TimingVars timer; + +void Initialize_Timing(void); + +#endif /* TIMINGVARS_H */ + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/UpdateFunctions.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/UpdateFunctions.h new file mode 100644 index 0000000..71f27f2 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/UpdateFunctions.h @@ -0,0 +1,46 @@ +/* + * File: UpdateFunctions.h + * Author: joemotyka + * + * Created on May 8, 2016, 12:35 AM + */ + +#ifndef UPDATEFUNCTIONS_H +#define UPDATEFUNCTIONS_H + +#include // for uint8_t, ... +#include "GlobalConstants.h" + +// Make these #defines +#define ROWS_IN_P 16 +#define COLS_IN_P 16 + +#define ROWS_IN_H 3 +#define COLS_IN_H 16 + +#define ROWS_IN_R 3 +#define COLS_IN_R ROWS_IN_R + +#define ROWS_IN_K 16 + +void EKF_UpdateStage(void); + +// Functions to split the INS update across multiple iterations, so the update can +// complete in the required 10 ms +void Update_Pos(void); +void Update_Vel(void); +void Update_Att(void); + +void ComputeSystemInnovation_Att(void); +void ComputeSystemInnovation_Pos(void); +void ComputeSystemInnovation_Vel(void); + + +uint8_t _GenerateObservationJacobian_AHRS(void); + +void _GenerateObservationCovariance_AHRS(void); +void _GenerateObservationCovariance_INS(void); + + +#endif /* UPDATEFUNCTIONS_H */ + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/WMMInternal.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/WMMInternal.h new file mode 100644 index 0000000..37d3bda --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/WMMInternal.h @@ -0,0 +1,180 @@ +/** +****************************************************************************** +* @file WMMInternal.h +* +*****************************************************************************/ +/******************************************************************************* +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 WMMINTERNAL_H_ +#define WMMINTERNAL_H_ + +#include + +// internal constants +#define WMM_MAX_MODEL_DEGREES 12 +#define WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES 12 +#define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2); +#define NUMPCUP 92 // NUMTERMS +1 +#define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1 + +// internal structure definitions +typedef struct { + float EditionDate; + float epoch; //Base time of Geomagnetic model epoch (yrs) + char ModelName[20]; + float Main_Field_Coeff_G[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) + float Main_Field_Coeff_H[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) + float Secular_Var_Coeff_G[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) + float Secular_Var_Coeff_H[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) + uint16_t nMax; // Maximum degree of spherical harmonic model + uint16_t nMaxSecVar; // Maxumum degree of spherical harmonic secular model + uint16_t SecularVariationUsed; // Whether or not the magnetic secular variation vector will be needed by program +} WMMtype_MagneticModel; + +typedef struct { + float a; // semi-major axis of the ellipsoid + float b; // semi-minor axis of the ellipsoid + float fla; // flattening + float epssq; // first eccentricity squared + float eps; // first eccentricity + float re; // mean radius of ellipsoid +} WMMtype_Ellipsoid; + +typedef struct { + float lambda; // longitude + float phi; // geodetic latitude + float HeightAboveEllipsoid; // height above the ellipsoid (HaE) +} WMMtype_CoordGeodetic; + +typedef struct { + float lambda; // longitude + float phig; // geocentric latitude + float r; // distance from the center of the ellipsoid +} WMMtype_CoordSpherical; + +typedef struct { + uint16_t Year; + uint16_t Month; + uint16_t Day; + float DecimalYear; +} WMMtype_Date; + +typedef struct { + float Pcup[NUMPCUP]; // Legendre Function + float dPcup[NUMPCUP]; // Derivative of Lagendre fn +} WMMtype_LegendreFunction; + +typedef struct { + float Bx; // North + float By; // East + float Bz; // Down +} WMMtype_MagneticResults; + +typedef struct { + float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES + 1]; // [earth_reference_radius_km / sph. radius ]^n + float cos_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // cp(m) - cosine of (m*spherical coord. longitude + float sin_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // sp(m) - sine of (m*spherical coord. longitude) +} WMMtype_SphericalHarmonicVariables; + +typedef struct { + float Decl; // 1. Angle between the magnetic field vector and true north, positive east + float Incl; // 2. Angle between the magnetic field vector and the horizontal plane, positive down + float F; // 3. Magnetic Field Strength + float H; // 4. Horizontal Magnetic Field Strength + float X; // 5. Northern component of the magnetic field vector + float Y; // 6. Eastern component of the magnetic field vector + float Z; // 7. Downward component of the magnetic field vector + float GV; // 8. The Grid Variation + float Decldot; // 9. Yearly Rate of change in declination + float Incldot; // 10. Yearly Rate of change in inclination + float Fdot; // 11. Yearly rate of change in Magnetic field strength + float Hdot; // 12. Yearly rate of change in horizontal field strength + float Xdot; // 13. Yearly rate of change in the northern component + float Ydot; // 14. Yearly rate of change in the eastern component + float Zdot; // 15. Yearly rate of change in the downward component + float GVdot; // 16. Yearly rate of chnage in grid variation +} WMMtype_GeoMagneticElements; + +// Internal Function Prototypes +void WMM_Set_Coeff_Array( float coeffs[][6]); +void WMM_GeodeticToSpherical( WMMtype_Ellipsoid Ellip, + WMMtype_CoordGeodetic CoordGeodetic, + WMMtype_CoordSpherical *CoordSpherical); +uint16_t WMM_DateToYear( WMMtype_Date *CalendarDate, char *Error); +void WMM_TimelyModifyMagneticModel( WMMtype_Date UserDate, + WMMtype_MagneticModel *MagneticModel, + WMMtype_MagneticModel *TimedMagneticModel ); + +uint16_t WMM_Geomag( WMMtype_Ellipsoid Ellip, + WMMtype_CoordSpherical CoordSpherical, + WMMtype_CoordGeodetic CoordGeodetic, + WMMtype_MagneticModel *TimedMagneticModel, + WMMtype_GeoMagneticElements *GeoMagneticElements ); + +uint16_t WMM_AssociatedLegendreFunction( WMMtype_CoordSpherical CoordSpherical, + uint16_t nMax, + WMMtype_LegendreFunction *LegendreFunction ); + +uint16_t WMM_CalculateGeoMagneticElements( WMMtype_MagneticResults *MagneticResultsGeo, + WMMtype_GeoMagneticElements *GeoMagneticElements); + +uint16_t WMM_CalculateSecularVariation( WMMtype_MagneticResults MagneticVariation, + WMMtype_GeoMagneticElements *MagneticElements); + +uint16_t WMM_ComputeSphericalHarmonicVariables( WMMtype_Ellipsoid Ellip, + WMMtype_CoordSpherical CoordSpherical, + uint16_t nMax, + WMMtype_SphericalHarmonicVariables *SphVariables); + +uint16_t WMM_PcupLow( float *Pcup, + float *dPcup, + float x, + uint16_t nMax ); + +uint16_t WMM_PcupHigh( float *Pcup, + float *dPcup, + float x, + uint16_t nMax ); + +uint16_t WMM_RotateMagneticVector( WMMtype_CoordSpherical, + WMMtype_CoordGeodetic CoordGeodetic, + WMMtype_MagneticResults MagneticResultsSph, + WMMtype_MagneticResults *MagneticResultsGeo ); + +uint16_t WMM_SecVarSummation( WMMtype_LegendreFunction *LegendreFunction, + WMMtype_MagneticModel *MagneticModel, + WMMtype_SphericalHarmonicVariables SphVariables, + WMMtype_CoordSpherical CoordSpherical, + WMMtype_MagneticResults *MagneticResults ); + +uint16_t WMM_SecVarSummationSpecial( WMMtype_MagneticModel *MagneticModel, + WMMtype_SphericalHarmonicVariables SphVariables, + WMMtype_CoordSpherical CoordSpherical, + WMMtype_MagneticResults *MagneticResults ); + +uint16_t WMM_Summation( WMMtype_LegendreFunction *LegendreFunction, + WMMtype_MagneticModel *MagneticModel, + WMMtype_SphericalHarmonicVariables SphVariables, + WMMtype_CoordSpherical CoordSpherical, + WMMtype_MagneticResults *MagneticResults ); + +uint16_t WMM_SummationSpecial( WMMtype_MagneticModel *MagneticModel, + WMMtype_SphericalHarmonicVariables SphVariables, + WMMtype_CoordSpherical CoordSpherical, + WMMtype_MagneticResults *MagneticResults ); + +#endif /* WMMINTERNAL_H_ */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/WorldMagneticModel.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/WorldMagneticModel.h new file mode 100644 index 0000000..6b7db8f --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/WorldMagneticModel.h @@ -0,0 +1,52 @@ +/****************************************************************************** +* File: WorldMagneticModel.h +* Created on May 16, 2016, 8:22 PM +*******************************************************************************/ +/******************************************************************************* +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 WORLDMAGNETICMODEL_H +#define WORLDMAGNETICMODEL_H + +#include "GlobalConstants.h" + +#include + +// at 1000 mph at the equator this needs to be run once every couple of seconds +// at general aviation speed this needs to be run once every couple of minutes +void WMM_Initialize(void); + +void WMM_GetMagVector( float Lat, + float Lon, + float AltEllipsoid, + uint16_t Month, + uint16_t Day, + uint16_t Year, + float* B, + float* wmmDecl); // X - N, Y - E, Z - D + +// +typedef struct { + uint32_t timeOfLastSoln; + BOOL validSoln; + + float decl_rad; +} WorldMagModelStruct; + +extern WorldMagModelStruct gWorldMagModel; + +#endif /* MAGALIGN_H */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/algorithm.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/algorithm.h new file mode 100644 index 0000000..8b92c36 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/include/algorithm.h @@ -0,0 +1,226 @@ +/***************************************************************************** +* @name algorihm.h +* +* +* 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. +* +* -Algorithm data structure used in sensor calibration and communication +* protocols with outside world. +******************************************************************************/ +/******************************************************************************* +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 ALGORITHM_H +#define ALGORITHM_H + +#include "GlobalConstants.h" + +#include "Indices.h" + +// Define the algorithm states +#define STABILIZE_SYSTEM 0 +#define INITIALIZE_ATTITUDE 1 +#define HIGH_GAIN_AHRS 2 +#define LOW_GAIN_AHRS 3 +#define INS_SOLUTION 4 + + +// Specify the minimum state times (in seconds) +#define STABILIZE_SYSTEM_DURATION 0.36 // [sec] +#define INITIALIZE_ATTITUDE_DURATION 0.64 // ( 1.0 - 0.36 ) [sec] +#define HIGH_GAIN_AHRS_DURATION 30.0 // 60.0 * SAMPLING_RATE +#define LOW_GAIN_AHRS_DURATION 30.0 // 30.0 * SAMPLING_RATE + +// Define heading initialization reliability +#define HEADING_UNINITIALIZED 0 +#define HEADING_MAG 1 +#define HEADING_GNSS_LOW 2 +#define HEADING_GNSS_HIGH 3 + +typedef struct { + uint32_t Stabilize_System; // SAMPLING_RATE * 0.36 + uint32_t Initialize_Attitude; // SAMPLING_RATE * ( 1.0 - 0.36 ) + uint32_t High_Gain_AHRS; // 60.0 * SAMPLING_RATE + uint32_t Low_Gain_AHRS; // 30.0 * SAMPLING_RATE +} DurationStruct; + +typedef struct { + real positionError; + real velocityError; + real attitudeError; +} InnovationStruct; + +typedef struct { + int32_t maxGpsDropTime; // [msec] + int32_t maxReliableDRTime; /* [msec] When GPS outage duration exceeds this limit, + * the position and velocity will be reinitialized when GPS + * is available again. Otherwise, the fusion algorithm will + * gradually correct the position and velocity to the GPS. + */ + int32_t Max_Rest_Time_Before_Drop_To_AHRS; // [msec] + int32_t Declination_Expiration_Time; // [msec] + + uint16_t Free_Integration_Cntr; // [count] + + real accelSwitch; + uint32_t linAccelSwitchDelay; + + 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 + 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 +{ + uint16_t all; + struct algoBehavior_BITS bit; +}; + +// Algorithm states +struct ALGO_STATUS_BITS +{ + uint16_t algorithmInit : 1; // 0 algorithm initialization + 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 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 +{ + uint16_t all; + struct ALGO_STATUS_BITS bit; +} AlgoStatus; + +extern AlgoStatus gAlgoStatus; + +typedef struct +{ + real arw; // [rad/sqrt(s)], gyro angle random walk + real sigmaW; // [rad/s], gyro noise std + real biW; // [rad/s], gyro bias instability + real maxBiasW; // [rad/s], max possible gyro bias + real vrw; // [m/s/sqrt(s)], accel velocity random walk + real sigmaA; // [m/s/s], accel noise std + real biA; // [m/s/s], accel bias instability + real maxBiasA; // [m/s/s], max possible accel bias +} IMU_SPEC; + +typedef struct +{ + real staticVarGyro; // [rad/s]^2 + real staticVarAccel; // [m/s/s]^2 + real maxGyroBias; // [rad/s] + real staticGnssVel; // [m/s] + real staticNoiseMultiplier[3]; /* Use IMU noise level and gyro output to detect static period. + * The nominal noise level and max gyro bias of an IMU is defined in + * SensorNoiseParameters.h. These parameters are determined by IMU + * output when static and are hightly related to ARW and VRW. + * When IMU is installed on a vehicle, its noise level when + * vehicle is static could be higher than the nominal noise + * level due to vibration. This setting is used to scale + * the nominal noise level and gyro bias for static detection. + * [scale_gyro_var, scale_accel_var, scale_gyro_bias] + */ +} STATIC_DETECT_SETTING; + + +/* Global Algorithm structure */ +typedef struct { + uint32_t itow; + uint32_t dITOW; + + // control the stage of operation for the algorithms + uint32_t stateTimer; + uint8_t state; // takes values from HARDWARE_STABILIZE to INIT_ATTITUDE to HG_AHRS + + uint8_t insFirstTime; + uint8_t headingIni; + uint8_t applyDeclFlag; + + int32_t timeOfLastSufficientGPSVelocity; + int32_t timeOfLastGoodGPSReading; + + real filteredYawRate; // Yaw-Rate (Turn-Switch) filter + + /* The following variables are used to increase the Kalman filter gain when the + * acceleration is very close to one (i.e. the system is at rest) + */ + uint32_t linAccelSwitchCntr; + uint8_t linAccelSwitch; + + uint8_t linAccelLPFType; + uint8_t useRawAccToDetectLinAccel; + + uint8_t callingFreq; + real dt; + real dtOverTwo; + real dtSquared; + real sqrtDt; + + volatile uint32_t timer; // timer since power up (ms) + volatile uint16_t counter; // inc. with every continuous mode output packet + + union AlgoBehavior Behavior; + float turnSwitchThreshold; // 0, 0.4, 10 driving, 1 flying [deg/sec] 0x000d + + real leverArmB[3]; // Antenna position w.r.t IMU in vehicle body frame + real pointOfInterestB[3]; // Point of interest position w.r.t IMU in vehicle body frame + + BOOL velocityAlwaysAlongBodyX; // enable zero velocity update + + IMU_SPEC imuSpec; // IMU specifications + STATIC_DETECT_SETTING staticDetectParam; // params used for static detection + + DurationStruct Duration; + LimitStruct Limit; +} 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/OpenIMU300RI/INS/lib/Core/Algorithm/magAPI.h b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/magAPI.h new file mode 100644 index 0000000..c66df16 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/magAPI.h @@ -0,0 +1,123 @@ +/** *************************************************************************** + * @file magAPI.h API functions for Magnitometer functionality + * + * 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. + * + *****************************************************************************/ +/******************************************************************************* +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 _MAG_API_H +#define _MAG_API_H + +#include +#include "GlobalConstants.h" + +/** *************************************************************************** + * @name InitMagAlignParams + * @brief initialize the parameters for magnetic heading calculation + * taskDataAcquisition.c and HardSoftIronCalibration() + * Trace: + * [SDD_INIT_EXT_MAG_CONFIG <-- SRC_INIT_MAGALIGN_RESULT] + * @param N/A + * @retval None + ******************************************************************************/ +void InitMagAlignParams(void); + +/** **************************************************************************** + * @name MagAlign API call to collect data for Hard/Soft Iron calibration + * @brief called every frame in taskDataAcquisition.c + * + * Trace: + * + * @param N/A + * @retval always returns 1 + ******************************************************************************/ +uint8_t MagAlign( void ); + + +/** *************************************************************************** + * @name TaskDataAcquisition() CALLBACK main loop + * @brief Get the sensor data at the specified frequency (based on the + * configuration of the accelerometer rate-sensor). Process and provide + * information to the user via the UART or SPI. + * @param N/A + * @retval N/A + ******************************************************************************/ +void TaskWorldMagneticModel(void const *argument); + + +/***************************************************************************** + * @name SetMagAlignState + * @brief Sets current state of mag calibration + * @param [in] state currend calibration state + * @retval N/A + ******************************************************************************/ +void SetMagAlignState(int state); + +/***************************************************************************** + * @name GetMagAlignState + * @brief Returns current state of mag calibration + * @param [out ] state currend calibration state + * @retval N/A + ******************************************************************************/ +int GetMagAlignState(void); + +/***************************************************************************** + * @name GetMagAlignEstimatedParams + * @brief Returns current state of mag calibration and estimated values of the parameters + * @param [out ] state currend calibration state + * @param [in ] structure to fill estimated values + * @retval N/A + ******************************************************************************/ +uint8_t GetMagAlignEstimatedParams(real *params); + +#pragma pack(1) +// example of user payload structure +typedef struct { + uint8_t parameter[8]; +} magAlignCmdPayload; +#pragma pack() + +typedef struct{ + real hardIron_X; + real hardIron_Y; + real softIron_Ratio; + real softIron_Angle; +}magAlignUserParams_t; + +/***** + * + * + ****/ +uint8_t ProcessMagAlignCmds(magAlignCmdPayload* pld, uint8_t *payloadLen); + +/***** + * + * + ****/ +void getUserMagAlignParams(magAlignUserParams_t *params); + +/***** + * + * + ****/ +void setUserMagAlignParams(magAlignUserParams_t *params); + +#endif diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/EKF_Algorithm.c b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/EKF_Algorithm.c new file mode 100644 index 0000000..8e79f6b --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/EKF_Algorithm.c @@ -0,0 +1,782 @@ +/* + * File: EKF_Algorithms.c + * Author: joemotyka + * + * Created on May 8, 2016, 12:35 AM + */ +#include // std::abs +#include // EXIT_FAILURE +#include +#include // memset + +#include "GlobalConstants.h" // TRUE, FALSE, etc +#include "platformAPI.h" +#include "Indices.h" // IND + +#include "MagAlign.h" +#include "QuaternionMath.h" +#include "algorithm.h" // gAlgorithm +#include "AlgorithmLimits.h" +#include "TransformationMath.h" +#include "VectorMath.h" +#include "MotionStatus.h" +#include "SelectState.h" +#include "EKF_Algorithm.h" +#include "PredictFunctions.h" +#include "UpdateFunctions.h" +#include "TimingVars.h" + + +#ifndef INS_OFFLINE +#ifdef DISPLAY_DIAGNOSTIC_MSG +#include "debug.h" +#endif +#endif + +KalmanFilterStruct gKalmanFilter; +EKF_InputDataStruct gEKFInput; +EKF_OutputDataStruct gEKFOutput; +ImuStatsStruct gImuStats; + +/****************************************************************************** + * @brief Remove lever arm in position and velocity. + * GNSS measured position is the position of the antenna. For GNSS/INS integration, + * the position of IMU is needed. Before using the GNSS position and velocity, + * those shold be first converted to the IMU position and velocity by removing + * lever arm effects. Lever arm introduces an offset in position. This offset + * can be directly canceled by substracting lever arm. Combined with angular + * velocity, lever arm further introduces relative velocity. + * TRACE: + * @param [in/out] LLA [rad, rad, m], Lat, lon and alt of the antenna, and + * will be converted to LLA of the IMU. + * @param [in/out] vNed [m/s], NED velocity of the antenna, and will be + * converted to NED velocity of the IMU. + * @param [in] w [rad/s], angular velocity of the vehicle relative to + * ECEF in the body frame. + * @param [in] leverArmB [m], lever arm in the body frame. + * @param [out] rn2e Transformation matrix from NED to ECEF. + * @param [out] ecef [m], ECEF position without lever arm. + * @retval TRUE if heading initialized, FALSE if not. +******************************************************************************/ +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: + * -- Unaided soln: 200 Hz + * -- Aided soln: 100 Hz + */ +void EKF_Algorithm(void) +{ + static uint16_t freeIntegrationCounter = 0; + + /* After STABILIZE_SYSTEM, the accel data will first pass a low-pass filter. + * Stats of the filter accel will then be calculated. + * According to gAlgorithm.useRawAccToDetectLinAccel, + * raw or filtered accel is used to detect linear accel. + */ + if ( gAlgorithm.state > STABILIZE_SYSTEM ) + { + /* Compute IMU mean/var, filter IMU data (optional), detect static. + * After STABILIZE_SYSTEM, each IMU sample is pushed into a buffer. + * Before the buffer is full, results are not accurate should not be used. + */ + MotionStatusImu(gEKFInput.angRate_B, gEKFInput.accel_B, &gImuStats, FALSE); + + // estimate accel error + if (gAlgorithm.useRawAccToDetectLinAccel) + { + EstimateAccelError( gEKFInput.accel_B, + gEKFInput.angRate_B, + gAlgorithm.dt, + gAlgorithm.Limit.linAccelSwitchDelay, + &gImuStats); + } + else + { + EstimateAccelError( gImuStats.lpfAccel, + gEKFInput.angRate_B, + gAlgorithm.dt, + gAlgorithm.Limit.linAccelSwitchDelay, + &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 + if( gAlgorithm.state > INITIALIZE_ATTITUDE ) + { + // Increment the algorithm itow + gAlgorithm.itow = gAlgorithm.itow + gAlgorithm.dITOW; + + // 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. + */ + if (gAlgorithm.Behavior.bit.freeIntegrate && (gAlgorithm.state > HIGH_GAIN_AHRS)) + { + /* Limit the free-integration time before reverting to the complete + * EKF solution (including updates). + */ + freeIntegrationCounter = freeIntegrationCounter + 1; // [cycles] + if (freeIntegrationCounter >= gAlgorithm.Limit.Free_Integration_Cntr) + { + freeIntegrationCounter = 0; + enableFreeIntegration(FALSE); + +#ifdef DISPLAY_DIAGNOSTIC_MSG + // Display the time at the end of the free-integration period + TimingVars_DiagnosticMsg("Free integration period ended"); +#endif + } + // Restart the system in LG AHRS after free integration is complete + gAlgorithm.insFirstTime = TRUE; + gAlgorithm.state = LOW_GAIN_AHRS; + gAlgorithm.stateTimer = gAlgorithm.Duration.Low_Gain_AHRS; + } + else + { + enableFreeIntegration(FALSE); + freeIntegrationCounter = 0; + + // Perform EKF Update + EKF_UpdateStage(); + } + + /* Save the past attitude quaternion before updating (for use in the + * covariance estimation calculations). + */ + gKalmanFilter.quaternion_Past[Q0] = gKalmanFilter.quaternion[Q0]; + gKalmanFilter.quaternion_Past[Q1] = gKalmanFilter.quaternion[Q1]; + gKalmanFilter.quaternion_Past[Q2] = gKalmanFilter.quaternion[Q2]; + gKalmanFilter.quaternion_Past[Q3] = gKalmanFilter.quaternion[Q3]; + + /* Generate the transformation matrix (R_BinN) based on the past value of + * the attitude quaternion (prior to prediction at the new time-step) + */ + QuaternionToR321(gKalmanFilter.quaternion, &gKalmanFilter.R_BinN[0][0]); + + /* Euler angels are not calcualted here because it is used as a propagation resutls + * to calculate system innovation. So, Euler angles are updated in the prediction + * stage. In theory, Euler angles should be updated after each measurement update. + * However, after EKF converges, it does not matter. + */ + + // Update LLA + if ((gAlgorithm.insFirstTime == FALSE)) + { + double r_E[NUM_AXIS]; + float pointOfInterestN[3]; + pointOfInterestN[0] = gKalmanFilter.R_BinN[0][0] * gAlgorithm.pointOfInterestB[0] + + gKalmanFilter.R_BinN[0][1] * gAlgorithm.pointOfInterestB[1] + + gKalmanFilter.R_BinN[0][2] * gAlgorithm.pointOfInterestB[2]; + pointOfInterestN[1] = gKalmanFilter.R_BinN[1][0] * gAlgorithm.pointOfInterestB[0] + + gKalmanFilter.R_BinN[1][1] * gAlgorithm.pointOfInterestB[1] + + gKalmanFilter.R_BinN[1][2] * gAlgorithm.pointOfInterestB[2]; + pointOfInterestN[2] = gKalmanFilter.R_BinN[2][0] * gAlgorithm.pointOfInterestB[0] + + gKalmanFilter.R_BinN[2][1] * gAlgorithm.pointOfInterestB[1] + + gKalmanFilter.R_BinN[2][2] * gAlgorithm.pointOfInterestB[2]; + pointOfInterestN[0] += gKalmanFilter.Position_N[0]; + pointOfInterestN[1] += gKalmanFilter.Position_N[1]; + pointOfInterestN[2] += gKalmanFilter.Position_N[2]; + PosNED_To_PosECEF(pointOfInterestN, &gKalmanFilter.rGPS0_E[0], &gKalmanFilter.Rn2e[0][0], &r_E[0]); + // 100 Hz generated once 1 Hz 100 Hz + // output variable (ned used for anything else); this is in [ deg, deg, m ] + ECEF_To_LLA(&gKalmanFilter.llaDeg[LAT], &r_E[X_AXIS]); + // 100 Hz 100 Hz + } + } + + /* Select the algorithm state based upon the present state as well as + * operational conditions (time, sensor health, etc). Note: This is called + * after the the above code-block to prevent the transition from occuring + * until the next time step. + */ + switch( gAlgorithm.state ) + { + case STABILIZE_SYSTEM: + StabilizeSystem(); + break; + case INITIALIZE_ATTITUDE: + InitializeAttitude(); + break; + case HIGH_GAIN_AHRS: + HG_To_LG_Transition_Test(); + break; + case LOW_GAIN_AHRS: + LG_To_INS_Transition_Test(); + break; + case INS_SOLUTION: + INS_To_AHRS_Transition_Test(); + break; + default: + return; + } + + // Dynamic motion logic (to revert back to HG AHRS) + DynamicMotion(); +} + +static void HandlePps() +{ + // PPS not detected for a long time? + int32_t timeSinceLastPps = (int32_t)(gAlgorithm.itow - gKalmanFilter.ppsITow); + if (timeSinceLastPps < 0) + { + timeSinceLastPps = timeSinceLastPps + MAX_ITOW; + } + if (timeSinceLastPps > 2000) + { + gAlgoStatus.bit.ppsAvailable = FALSE; + } + // PPS detected + if (gEKFInput.ppsDetected) + { + // PPS is available from now. + gAlgoStatus.bit.ppsAvailable = TRUE; + // save states when pps detected + SaveKfStateAtPps(); + } +} + +static void SaveKfStateAtPps() +{ + // 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() + +/* Extract the attitude (expressed as Euler-angles) of the body-frame (B) + * in the NED-frame (N) in [deg]. + */ +void EKF_GetAttitude_EA(real *EulerAngles) +{ + // Euler-angles in [deg] + EulerAngles[ROLL] = (real)gEKFOutput.eulerAngs_BinN[ROLL]; + EulerAngles[PITCH] = (real)gEKFOutput.eulerAngs_BinN[PITCH]; + EulerAngles[YAW] = (real)gEKFOutput.eulerAngs_BinN[YAW]; +} + + +void EKF_GetAttitude_EA_RAD(real *EulerAngles) +{ + // Euler-angles in [rad] + EulerAngles[ROLL] = (real)gKalmanFilter.eulerAngles[ROLL]; + EulerAngles[PITCH] = (real)gKalmanFilter.eulerAngles[PITCH]; + EulerAngles[YAW] = (real) gKalmanFilter.eulerAngles[YAW]; +} + + +/* Extract the attitude (expressed by quaternion-elements) of the body- + * frame (B) in the NED-frame (N). + */ +void EKF_GetAttitude_Q(real *Quaternions) +{ + Quaternions[Q0] = (real)gEKFOutput.quaternion_BinN[Q0]; + Quaternions[Q1] = (real)gEKFOutput.quaternion_BinN[Q1]; + Quaternions[Q2] = (real)gEKFOutput.quaternion_BinN[Q2]; + Quaternions[Q3] = (real)gEKFOutput.quaternion_BinN[Q3]; +} + + +/* Extract the angular-rate of the body (corrected for estimated rate-bias) + * measured in the body-frame (B). + */ +void EKF_GetCorrectedAngRates(real *CorrAngRates_B) +{ + // Angular-rate in [deg/s] + CorrAngRates_B[X_AXIS] = (real)gEKFOutput.corrAngRates_B[X_AXIS]; + CorrAngRates_B[Y_AXIS] = (real)gEKFOutput.corrAngRates_B[Y_AXIS]; + CorrAngRates_B[Z_AXIS] = (real)gEKFOutput.corrAngRates_B[Z_AXIS]; +} + + +/* Extract the acceleration of the body (corrected for estimated + * accelerometer-bias) measured in the body-frame (B). + */ +void EKF_GetCorrectedAccels(real *CorrAccels_B) +{ + // Acceleration in [m/s^2] + CorrAccels_B[X_AXIS] = (real)gEKFOutput.corrAccel_B[X_AXIS]; + CorrAccels_B[Y_AXIS] = (real)gEKFOutput.corrAccel_B[Y_AXIS]; + CorrAccels_B[Z_AXIS] = (real)gEKFOutput.corrAccel_B[Z_AXIS]; +} + + +/* Extract the acceleration of the body (corrected for estimated + * accelerometer-bias) measured in the body-frame (B). + */ +void EKF_GetEstimatedAngRateBias(real *AngRateBias_B) +{ + // Angular-rate bias in [deg/sec] + AngRateBias_B[X_AXIS] = (real)gEKFOutput.angRateBias_B[X_AXIS]; + AngRateBias_B[Y_AXIS] = (real)gEKFOutput.angRateBias_B[Y_AXIS]; + AngRateBias_B[Z_AXIS] = (real)gEKFOutput.angRateBias_B[Z_AXIS]; +} + + +/* Extract the acceleration of the body (corrected for estimated + * accelerometer-bias) measured in the body-frame (B). + */ +void EKF_GetEstimatedAccelBias(real *AccelBias_B) +{ + // Acceleration-bias in [m/s^2] + AccelBias_B[X_AXIS] = (real)gEKFOutput.accelBias_B[X_AXIS]; + AccelBias_B[Y_AXIS] = (real)gEKFOutput.accelBias_B[Y_AXIS]; + AccelBias_B[Z_AXIS] = (real)gEKFOutput.accelBias_B[Z_AXIS]; +} + + +// Extract the Position of the body measured in the NED-frame (N) +void EKF_GetEstimatedPosition(real *Position_N) +{ + // Position in [m] + Position_N[X_AXIS] = (real)gEKFOutput.position_N[X_AXIS]; + Position_N[Y_AXIS] = (real)gEKFOutput.position_N[Y_AXIS]; + Position_N[Z_AXIS] = (real)gEKFOutput.position_N[Z_AXIS]; +} + + +// Extract the Position of the body measured in the NED-frame (N) +void EKF_GetEstimatedVelocity(real *Velocity_N) +{ + // Velocity in [m/s] + Velocity_N[X_AXIS] = (real)gEKFOutput.velocity_N[X_AXIS]; + Velocity_N[Y_AXIS] = (real)gEKFOutput.velocity_N[Y_AXIS]; + Velocity_N[Z_AXIS] = (real)gEKFOutput.velocity_N[Z_AXIS]; +} + + +// Extract the Position of the body measured in the NED-frame (N) +void EKF_GetEstimatedLLA(double *LLA) +{ + // Velocity in [m/s] + LLA[X_AXIS] = (double)gEKFOutput.llaDeg[X_AXIS]; + LLA[Y_AXIS] = (double)gEKFOutput.llaDeg[Y_AXIS]; + LLA[Z_AXIS] = (double)gEKFOutput.llaDeg[Z_AXIS]; +} + + +/* Extract the Operational Mode of the Algorithm: + * 0: Stabilize + * 1: Initialize + * 2: High-Gain VG/AHRS mode + * 3: Low-Gain VG/AHRS mode + * 4: INS operation + */ +void EKF_GetOperationalMode(uint8_t *EKF_OperMode) +{ + *EKF_OperMode = gEKFOutput.opMode; +} + + +// Extract the linear-acceleration and turn-switch flags +void EKF_GetOperationalSwitches(uint8_t *EKF_LinAccelSwitch, uint8_t *EKF_TurnSwitch) +{ + *EKF_LinAccelSwitch = gEKFOutput.linAccelSwitch; + *EKF_TurnSwitch = gEKFOutput.turnSwitchFlag; +} + + +// 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, odoDataStruct_t *odo, + BOOL ppsDetected) +{ + // Accelerometer signal is in [m/s/s] + gEKFInput.accel_B[X_AXIS] = (real)accels[X_AXIS] * GRAVITY; + gEKFInput.accel_B[Y_AXIS] = (real)accels[Y_AXIS] * GRAVITY; + gEKFInput.accel_B[Z_AXIS] = (real)accels[Z_AXIS] * GRAVITY; + + // Angular-rate signal is in [rad/s] + gEKFInput.angRate_B[X_AXIS] = (real)rates[X_AXIS]; + gEKFInput.angRate_B[Y_AXIS] = (real)rates[Y_AXIS]; + gEKFInput.angRate_B[Z_AXIS] = (real)rates[Z_AXIS]; + + // Magnetometer signal is in [G] + gEKFInput.magField_B[X_AXIS] = (real)mags[X_AXIS]; + gEKFInput.magField_B[Y_AXIS] = (real)mags[Y_AXIS]; + gEKFInput.magField_B[Z_AXIS] = (real)mags[Z_AXIS]; + real tmp[2]; + tmp[X_AXIS] = gEKFInput.magField_B[X_AXIS] - gMagAlign.hardIronBias[X_AXIS]; + tmp[Y_AXIS] = gEKFInput.magField_B[Y_AXIS] - gMagAlign.hardIronBias[Y_AXIS]; + gEKFInput.magField_B[X_AXIS] = gMagAlign.SF[0] * tmp[X_AXIS] + gMagAlign.SF[1] * tmp[Y_AXIS]; + gEKFInput.magField_B[Y_AXIS] = gMagAlign.SF[2] * tmp[X_AXIS] + gMagAlign.SF[3] * tmp[Y_AXIS]; + + // ----- Input from the GPS goes here ----- + gEKFInput.gpsUpdate = gps->gpsUpdate; + + if (gEKFInput.gpsUpdate) + { + // Validity data + gEKFInput.gpsFixType = gps->gpsFixType; + + // num of satellites + gEKFInput.numSatellites = gps->numSatellites; + + // ITOW data + gEKFInput.itow = gps->itow; + + // Data quality measures + gEKFInput.GPSHorizAcc = gps->GPSHorizAcc; + gEKFInput.GPSVertAcc = gps->GPSVertAcc; + gEKFInput.HDOP = gps->HDOP; + + // Lat/Lon/Alt data + gEKFInput.llaAnt[LAT] = gps->latitude * D2R; + gEKFInput.llaAnt[LON] = gps->longitude * D2R; + gEKFInput.llaAnt[ALT] = gps->altitude; + gEKFInput.geoidAboveEllipsoid = gps->geoidAboveEllipsoid; + + // Velocity data + gEKFInput.vNedAnt[X_AXIS] = gps->vNed[X_AXIS]; + gEKFInput.vNedAnt[Y_AXIS] = gps->vNed[Y_AXIS]; + gEKFInput.vNedAnt[Z_AXIS] = gps->vNed[Z_AXIS]; + + // Course and velocity data + 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 + * from the body frame to the NED frame. Before heading initialized, lever arm cannot + * be correctly removed. After heading initialized, there would be position jump if + * initial heading is different from uninitlized one and the lever arm is large. + * After heading intialized, the position/velocity could also be reinitialized, and + * lever arm effects on the position/velocity are not corrected removed. + * LLA without lever arm is used to update Rn2e/ECEF postion, and calculate relative + * position in NED + */ + if (gEKFInput.gpsFixType) + { + gEKFInput.lla[LAT] = gEKFInput.llaAnt[LAT]; + gEKFInput.lla[LON] = gEKFInput.llaAnt[LON]; + gEKFInput.lla[ALT] = gEKFInput.llaAnt[ALT]; + gEKFInput.vNed[0] = gEKFInput.vNedAnt[0]; + gEKFInput.vNed[1] = gEKFInput.vNedAnt[1]; + gEKFInput.vNed[2] = gEKFInput.vNedAnt[2]; + /* remove lever arm. Indeed, corrected angular rate should be used. Considering angular + * bias is small, raw angular rate is used. + */ + RemoveLeverArm( gEKFInput.lla, + gEKFInput.vNed, + gEKFInput.angRate_B, + gAlgorithm.leverArmB, + &gKalmanFilter.Rn2e[0][0], + gKalmanFilter.rGPS_E); + + /* Calculate relative position in the NED frame. The initial position is rGPS0_E which. + * is determined when the algorithm first enters the INS mode (InitINSFilter). + */ + ECEF_To_Base( &gKalmanFilter.rGPS0_E[0], + &gKalmanFilter.rGPS_E[0], + &gKalmanFilter.Rn2e[0][0], + &gKalmanFilter.rGPS_N[0]); + } + } + + // odometer + gEKFInput.odoUpdate = odo->update; + gEKFInput.odoVelocity = odo->v; + + // 1PPS signal from GNSS receiver + gEKFInput.ppsDetected = ppsDetected; +} + + +// Populate the EKF output structure with algorithm results +void EKF_SetOutputStruct(void) +{ + // ------------------ States ------------------ + + // Position in [m] + gEKFOutput.position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS]; + gEKFOutput.position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS]; + gEKFOutput.position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS]; + + // Velocity in [m/s] + gEKFOutput.velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS]; + gEKFOutput.velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS]; + gEKFOutput.velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS]; + + // Position in [N/A] + gEKFOutput.quaternion_BinN[Q0] = gKalmanFilter.quaternion[Q0]; + gEKFOutput.quaternion_BinN[Q1] = gKalmanFilter.quaternion[Q1]; + gEKFOutput.quaternion_BinN[Q2] = gKalmanFilter.quaternion[Q2]; + gEKFOutput.quaternion_BinN[Q3] = gKalmanFilter.quaternion[Q3]; + + // Angular-rate bias in [deg/sec] + gEKFOutput.angRateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] * RAD_TO_DEG; + gEKFOutput.angRateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] * RAD_TO_DEG; + gEKFOutput.angRateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] * RAD_TO_DEG; + + // Acceleration-bias in [m/s^2] + gEKFOutput.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS]; + gEKFOutput.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS]; + gEKFOutput.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS]; + + // ------------------ Derived variables ------------------ + + // Euler-angles in [deg] + gEKFOutput.eulerAngs_BinN[ROLL] = gKalmanFilter.eulerAngles[ROLL] * RAD_TO_DEG; + gEKFOutput.eulerAngs_BinN[PITCH] = gKalmanFilter.eulerAngles[PITCH] * RAD_TO_DEG; + gEKFOutput.eulerAngs_BinN[YAW] = gKalmanFilter.eulerAngles[YAW] * RAD_TO_DEG; + + // Angular-rate in [deg/s] + gEKFOutput.corrAngRates_B[X_AXIS] = ( gEKFInput.angRate_B[X_AXIS] - + gKalmanFilter.rateBias_B[X_AXIS] ) * RAD_TO_DEG; + gEKFOutput.corrAngRates_B[Y_AXIS] = ( gEKFInput.angRate_B[Y_AXIS] - + gKalmanFilter.rateBias_B[Y_AXIS] ) * RAD_TO_DEG; + gEKFOutput.corrAngRates_B[Z_AXIS] = ( gEKFInput.angRate_B[Z_AXIS] - + gKalmanFilter.rateBias_B[Z_AXIS] ) * RAD_TO_DEG; + + // Acceleration in [m/s^2] + gEKFOutput.corrAccel_B[X_AXIS] = gEKFInput.accel_B[X_AXIS] - gKalmanFilter.accelBias_B[X_AXIS]; + gEKFOutput.corrAccel_B[Y_AXIS] = gEKFInput.accel_B[Y_AXIS] - gKalmanFilter.accelBias_B[Y_AXIS]; + gEKFOutput.corrAccel_B[Z_AXIS] = gEKFInput.accel_B[Z_AXIS] - gKalmanFilter.accelBias_B[Z_AXIS]; + + + // ------------------ Algorithm flags ------------------ + gEKFOutput.opMode = gAlgorithm.state; + gEKFOutput.linAccelSwitch = gAlgorithm.linAccelSwitch; + gEKFOutput.turnSwitchFlag = gAlgoStatus.bit.turnSwitch; + + // ------------------ Latitude and Longitude Data ------------------ + gEKFOutput.llaDeg[LAT] = gKalmanFilter.llaDeg[LAT]; + gEKFOutput.llaDeg[LON] = gKalmanFilter.llaDeg[LON]; + gEKFOutput.llaDeg[ALT] = gKalmanFilter.llaDeg[ALT]; +} + + +// +uint8_t InitINSFilter(void) +{ + real tmp[7][7]; + int rowNum, colNum; + +#ifdef INS_OFFLINE + printf("reset INS filter.\n"); +#endif // INS_OFFLINE + + gAlgorithm.insFirstTime = FALSE; + + // Sync the algorithm and GPS ITOW + gAlgorithm.itow = gEKFInput.itow; + + /* We have a good GPS reading now - set this variable so we + * don't drop into INS right away + */ + gAlgorithm.timeOfLastGoodGPSReading = gEKFInput.itow; + + /* Upon the first entry into INS, save off the base position and reset the + * Kalman filter variables. + */ + // Save off the base ECEF location + gKalmanFilter.rGPS0_E[X_AXIS] = gKalmanFilter.rGPS_E[X_AXIS]; + gKalmanFilter.rGPS0_E[Y_AXIS] = gKalmanFilter.rGPS_E[Y_AXIS]; + gKalmanFilter.rGPS0_E[Z_AXIS] = gKalmanFilter.rGPS_E[Z_AXIS]; + + // Reset the gps position (as position is relative to starting location) + gKalmanFilter.rGPS_N[X_AXIS] = 0.0; + gKalmanFilter.rGPS_N[Y_AXIS] = 0.0; + gKalmanFilter.rGPS_N[Z_AXIS] = 0.0; + + // Reset prediction values. Position_N is also IMU position. + gKalmanFilter.Position_N[X_AXIS] = (real)0.0; + gKalmanFilter.Position_N[Y_AXIS] = (real)0.0; + gKalmanFilter.Position_N[Z_AXIS] = (real)0.0; + + gKalmanFilter.Velocity_N[X_AXIS] = (real)gEKFInput.vNed[X_AXIS]; + gKalmanFilter.Velocity_N[Y_AXIS] = (real)gEKFInput.vNed[Y_AXIS]; + gKalmanFilter.Velocity_N[Z_AXIS] = (real)gEKFInput.vNed[Z_AXIS]; + + gKalmanFilter.accelBias_B[X_AXIS] = (real)0.0; + gKalmanFilter.accelBias_B[Y_AXIS] = (real)0.0; + gKalmanFilter.accelBias_B[Z_AXIS] = (real)0.0; + + gKalmanFilter.linearAccel_B[X_AXIS] = (real)0.0; + + /* Extract the Quaternion and rate-bias values from the matrix before + * resetting + */ + // Save off the quaternion and rate-bias covariance values + for (rowNum = Q0; rowNum <= Q3 + Z_AXIS + 1; rowNum++) + { + for (colNum = Q0; colNum <= Q3 + Z_AXIS + 1; colNum++) + { + tmp[rowNum][colNum] = gKalmanFilter.P[rowNum + STATE_Q0][colNum + STATE_Q0]; + } + } + + // Reset P + memset(gKalmanFilter.P, 0, sizeof(gKalmanFilter.P)); + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + gKalmanFilter.P[rowNum][rowNum] = (real)INIT_P_INS; + } + + // Repopulate the P matrix with the quaternion and rate-bias values + for (rowNum = Q0; rowNum <= Q3 + Z_AXIS + 1; rowNum++) + { + for (colNum = Q0; colNum <= Q3 + Z_AXIS + 1; colNum++) + { + gKalmanFilter.P[rowNum + STATE_Q0][colNum + STATE_Q0] = tmp[rowNum][colNum]; + } + } + + /* Use the GPS-provided horizontal and vertical accuracy values to populate + * the covariance values. + */ + gKalmanFilter.P[STATE_RX][STATE_RX] = gEKFInput.GPSHorizAcc * gEKFInput.GPSHorizAcc; + gKalmanFilter.P[STATE_RY][STATE_RY] = gKalmanFilter.P[STATE_RX][STATE_RX]; + gKalmanFilter.P[STATE_RZ][STATE_RZ] = gEKFInput.GPSVertAcc * gEKFInput.GPSVertAcc; + + /* Scale the best velocity error by HDOP then multiply by the z-axis angular + * rate PLUS one (to prevent the number from being zero) so the velocity + * update during high-rate turns is reduced. + */ + float temp = (real)0.0625 * gEKFInput.HDOP; // 0.0625 = 0.05 / 0.8 + real absFilteredYawRate = (real)fabs(gAlgorithm.filteredYawRate); + if (absFilteredYawRate > TEN_DEGREES_IN_RAD) + { + temp *= (1.0f + absFilteredYawRate); + } + gKalmanFilter.P[STATE_VX][STATE_VX] = temp;// *((real)1.0 + fabs(gAlgorithm.filteredYawRate) * (real)RAD_TO_DEG); + gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_VX] * gKalmanFilter.P[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.P[STATE_VX][STATE_VX]; + + // z-axis velocity isn't really a function of yaw-rate and hdop + //gKalmanFilter.R[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VZ][STATE_VZ] = (float)(0.1 * 0.1); + + return 1; +} + +static void RemoveLeverArm(double *lla, double *vNed, real *w, real *leverArmB, real *rn2e, double *ecef) +{ + // Using position with lever arm to calculate rm and rn + double sinLat = sin(lla[LAT]); + double cosLat = cos(lla[LAT]); + double tmp = 1.0 - (E_ECC_SQ * sinLat * sinLat); + double sqrtTmp = sqrt(tmp); + double rn = E_MAJOR / sqrtTmp; // radius of Curvature [meters] + double rm = rn * (1.0 - E_ECC_SQ) / tmp; + // Remove lever arm from position + real leverArmN[3]; // lever arm in the NED frame + leverArmN[0] = gKalmanFilter.R_BinN[0][0] * leverArmB[0] + + gKalmanFilter.R_BinN[0][1] * leverArmB[1] + + gKalmanFilter.R_BinN[0][2] * leverArmB[2]; + leverArmN[1] = gKalmanFilter.R_BinN[1][0] * leverArmB[0] + + gKalmanFilter.R_BinN[1][1] * leverArmB[1] + + gKalmanFilter.R_BinN[1][2] * leverArmB[2]; + leverArmN[2] = gKalmanFilter.R_BinN[2][0] * leverArmB[0] + + gKalmanFilter.R_BinN[2][1] * leverArmB[1] + + gKalmanFilter.R_BinN[2][2] * leverArmB[2]; + lla[0] -= leverArmN[0] / rm; + lla[1] -= leverArmN[1] / rn / cosLat; + lla[2] += leverArmN[2]; /* Notice: lever arm is now in NED frame while altitude is + * in the opposite direction of the z axis of NED frame. + */ + + /* Remove lever arm effects from velocity + * v_gnss = v_imu + C_b2n * cross(wB, leverArmB) + */ + cross(w, leverArmB, leverArmN); // use leverArmN to temporatily hold w x leverArmB in body frame + vNed[0] -= gKalmanFilter.R_BinN[0][0] * leverArmN[0] + + gKalmanFilter.R_BinN[0][1] * leverArmN[1] + + gKalmanFilter.R_BinN[0][2] * leverArmN[2]; + vNed[1] -= gKalmanFilter.R_BinN[1][0] * leverArmN[0] + + gKalmanFilter.R_BinN[1][1] * leverArmN[1] + + gKalmanFilter.R_BinN[1][2] * leverArmN[2]; + vNed[2] -= gKalmanFilter.R_BinN[2][0] * leverArmN[0] + + gKalmanFilter.R_BinN[2][1] * leverArmN[1] + + gKalmanFilter.R_BinN[2][2] * leverArmN[2]; + + // calcualte transfromation matrix from NED to ECEF + sinLat = sin(lla[LAT]); // recalculate with LLA without lever arm + cosLat = cos(lla[LAT]); + double sinLon = sin(lla[LON]); + double cosLon = cos(lla[LON]); + + real sinLat_r = (real)sinLat; + real cosLat_r = (real)cosLat; + real sinLon_r = (real)sinLon; + real cosLon_r = (real)cosLon; + + // Form the transformation matrix from NED to ECEF + // First row + *(rn2e + 0 * 3 + 0) = -sinLat_r * cosLon_r; + *(rn2e + 0 * 3 + 1) = -sinLon_r; + *(rn2e + 0 * 3 + 2) = -cosLat_r * cosLon_r; + // Second row + *(rn2e + 1 * 3 + 0) = -sinLat_r * sinLon_r; + *(rn2e + 1 * 3 + 1) = cosLon_r; + *(rn2e + 1 * 3 + 2) = -cosLat_r * sinLon_r; + // Third row + *(rn2e + 2 * 3 + 0) = cosLat_r; + *(rn2e + 2 * 3 + 1) = 0.0; + *(rn2e + 2 * 3 + 2) = -sinLat_r; + + // calculate ECEF position + tmp = (rn + lla[ALT]) * cosLat; + ecef[X_AXIS] = tmp * cosLon; + ecef[Y_AXIS] = tmp * sinLon; + ecef[Z_AXIS] = ((E_MINOR_OVER_MAJOR_SQ * (rn)) + lla[ALT]) * sinLat; +} diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/MagAlign.c b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/MagAlign.c new file mode 100644 index 0000000..2f688c8 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/MagAlign.c @@ -0,0 +1,20 @@ +/****************************************************************************** + * @file MagAlign.c + * @brief 2D soft iron and hard iron algorithm. + * @author Dong Xiaoguang + * @date 2019.05.10 + * @version V1.0.0 + *----------------------------------------------------------------------------- + * Change History + * | | | + * ---------------------------------------------------------------------------- + * 2019.05.09 | v1.0.0 | Dong Xiaoguang | Create file + * ---------------------------------------------------------------------------- +******************************************************************************/ + +#include "MagAlign.h" +#include "WorldMagneticModel.h" + + +MagAlignStruct gMagAlign; +WorldMagModelStruct gWorldMagModel; diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/MotionStatus.c b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/MotionStatus.c new file mode 100644 index 0000000..220f0cd --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/MotionStatus.c @@ -0,0 +1,642 @@ +/***************************************************************************** + * @file MotionStatus.c + * @brief Calculate sensor stats, and detect motion status using IMU/ODO/GNSS + * @author Dong Xiaoguang + * @version 1.0 + * @date 20190801 + *****************************************************************************/ +#include +#include "MotionStatus.h" +#include "buffer.h" +#include "VectorMath.h" + + +//============================================================================= +// Filter variables (Third-Order BWF w/ default 5 Hz Cutoff) +#define FILTER_ORDER 3 + +#define CURRENT 0 +#define PASTx1 1 +#define PASTx2 2 +#define PASTx3 3 +/* Replace this with a fuction that will compute the coefficients so the + * input is the cutoff frequency in Hertz + */ +#define NO_LPF 0 +#define TWO_HZ_LPF 1 +#define FIVE_HZ_LPF 2 +#define TEN_HZ_LPF 3 +#define TWENTY_HZ_LPF 4 +#define TWENTY_FIVE_HZ_LPF 5 +#define N_LPF 6 + +#define SAMPLES_FOR_STATS 20 /* 20 samples can give a relative good estimate of var + * This value should not be below FILTER_ORDER. + */ + + /****************************************************************************** + * @brief Get filter coefficients of a 3rd Butterworth low-pass filter. + * For now only a few specific cut-off frequencies are supported. + * TRACE: + * @param [in] lpfType Low-pass filter cut-off frequency. + * @param [in] callingFreq Sampling frequency, only 100Hz and 200Hz are supported. + * @param [out] b coefficients of the numerator of the filter. + * @param [out] a coefficients of the denominator of the filter. + * @retval None. + ******************************************************************************/ +static void _PopulateFilterCoefficients(uint8_t lpfType, uint8_t callingFreq, real *b, real *a); + +/****************************************************************************** + * @brief Process input data through a low-pass Butterworth filter. + * TRACE: + * @param [in] in Input data + * @param [in] bfIn Input data buffer + * @param [in] bfOut Output data buffer + * @param [in] b Numerator coef of the filter + * @param [in] a Denominator coef of the filter + * + * @param [out] filtered Filtered IMU data. + * @retval None. +******************************************************************************/ +static void LowPassFilter(real *in, Buffer *bfIn, Buffer *bfOut, real *b, real *a, real *filtered); + +/****************************************************************************** + * @brief Compute mean and var of the input data. + * Calculate mena and var of the latest n samples. + * TRACE: + * @param [in] bf The buffer to hold the latest n samples. + * @param [in] latest The latest sample. + * @param [in/out] mean Mean of samples already in buffer is used as input, and + * mean of latest samples (remove oldest and include latest) + * is returned as output. + * @param [in/out] var Var of samples already in buffer is used as input, and + * var of latest samples (remove oldest and include latest) + * is returned as output. + * @retval None. +******************************************************************************/ +static void ComputeStats(Buffer *bf, real *latest, real *mean, real *var); + +/****************************************************************************** + * @brief Detect zero velocity using IMU data. + * TRACE: + * @param [in] gyroVar variance of gyro [rad/s]^2 + * @param [in] gyroMean mean of gyro [rad/s] + * @param [in] accelVar variance of accel [m/s/s]^2 + * @param [in] threshold threshold to detect zero velocity + * @retval TRUE if static, otherwise FALSE. +******************************************************************************/ +BOOL DetectStaticIMU(real *gyroVar, real *gyroMean, real *accelVar, STATIC_DETECT_SETTING *threshold); + + +void MotionStatusImu(real *gyro, real *accel, ImuStatsStruct *imuStats, BOOL reset) +{ + static BOOL bIni = false; // indicate the routine is initialized or not + // Buffer to store input IMU data. Data in buffer is ued to calcualte filtered IMU dat. + static real dAccel[3][FILTER_ORDER]; // a section in memory as buffer to store accel data + static real dGyro[3][FILTER_ORDER]; // a section in memory as buffer to store gyro data + static Buffer bfAccel; // a ring buffer of accel + static Buffer bfGyro; // a ring buffer of gyro + // Buffer to store filtered IMU data. Data in buffer is used to calculate IMU stats. + static real dLpfAccel[3][SAMPLES_FOR_STATS]; // a section in memory as buffer to store accel data + static real dLpfGyro[3][SAMPLES_FOR_STATS]; // a section in memory as buffer to store gyro data + static Buffer bfLpfAccel; // a ring buffer of accel + static Buffer bfLpfGyro; // a ring buffer of gyro + // filter coefficients. y/x = b/a + static real b_AccelFilt[FILTER_ORDER + 1]; + static real a_AccelFilt[FILTER_ORDER + 1]; + + // reset the calculation of motion stats + if (reset) + { + bIni = false; + } + + // initialization + if (!bIni) + { + bIni = true; + // reset stats + imuStats->bValid = false; + imuStats->accelMean[0] = 0.0; + imuStats->accelMean[1] = 0.0; + imuStats->accelMean[2] = 0.0; + imuStats->accelVar[0] = 0.0; + imuStats->accelVar[1] = 0.0; + imuStats->accelVar[2] = 0.0; + imuStats->gyroMean[0] = 0.0; + imuStats->gyroMean[1] = 0.0; + imuStats->gyroMean[2] = 0.0; + imuStats->gyroVar[0] = 0.0; + imuStats->gyroVar[1] = 0.0; + imuStats->gyroVar[2] = 0.0; + // create/reset buffer + bfNew(&bfGyro, &dGyro[0][0], 3, FILTER_ORDER); + bfNew(&bfAccel, &dAccel[0][0], 3, FILTER_ORDER); + bfNew(&bfLpfGyro, &dLpfGyro[0][0], 3, SAMPLES_FOR_STATS); + bfNew(&bfLpfAccel, &dLpfAccel[0][0], 3, SAMPLES_FOR_STATS); + // Set the filter coefficients based on selected cutoff frequency and sampling rate + _PopulateFilterCoefficients(gAlgorithm.linAccelLPFType, gAlgorithm.callingFreq, b_AccelFilt, a_AccelFilt); + } + + /* Low-pass filter. + * The input IMU data is put into the buffer, and then filtered. + */ + LowPassFilter(gyro, &bfGyro, &bfLpfGyro, b_AccelFilt, a_AccelFilt, imuStats->lpfGyro); + LowPassFilter(accel, &bfAccel, &bfLpfAccel, b_AccelFilt, a_AccelFilt, imuStats->lpfAccel); + + /* Compute accel norm using raw accel data. + * The norm will be used to detect static periods via magnitude. + */ + imuStats->accelNorm = sqrtf(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]); + + /* Compute mean/variance from input data. + * The latest will be put into the buffer, and stats of data in buffer is then calculated. + * When the input data buffer is full, the stats can be assumed valid. + */ + ComputeStats(&bfLpfGyro, imuStats->lpfGyro, imuStats->gyroMean, imuStats->gyroVar); + ComputeStats(&bfLpfAccel, imuStats->lpfAccel, imuStats->accelMean, imuStats->accelVar); + imuStats->bValid = bfLpfGyro.full && bfLpfAccel.full; + + // Detect static period using var calculated above. + imuStats->bStaticIMU = DetectStaticIMU( imuStats->gyroVar, + imuStats->gyroMean, + imuStats->accelVar, + &gAlgorithm.staticDetectParam); +} + +static void ComputeStats(Buffer *bf, real *latest, real *mean, real *var) +{ + real lastMean[3]; + real lastVar[3]; + lastMean[0] = mean[0]; + lastMean[1] = mean[1]; + lastMean[2] = mean[2]; + lastVar[0] = var[0]; + lastVar[1] = var[1]; + lastVar[2] = var[2]; + if (bf->full) + { + /* when buffer is full, the var and mean are computed from + * all data in the buffer. From now on, the var and mean + * should be computed by removing the oldest data and including + * the latest data. + */ + // Get the oldest data which will be removed by following bfPut + real oldest[3]; + bfGet(bf, oldest, bf->num - 1); + // put this accel into buffer + bfPut(bf, latest); + // mean(n+1) = ( mean(n) * n - x(1) + x(n+1) ) / n + mean[0] += (latest[0] - oldest[0]) / (real)(bf->num); + mean[1] += (latest[1] - oldest[1]) / (real)(bf->num); + mean[2] += (latest[2] - oldest[2]) / (real)(bf->num); + + // naive var calculation is adopted because recursive method is numerically instable + real tmpVar[3]; + tmpVar[0] = vecVar(&(bf->d[0]), mean[0], bf->num); + tmpVar[1] = vecVar(&(bf->d[bf->n]), mean[1], bf->num); + tmpVar[2] = vecVar(&(bf->d[2 * bf->n]), mean[2], bf->num); + // make var estimation smooth + real k = 0.96f; + int i; + for (i = 0; i < 3; i++) + { + if (tmpVar[i] >= var[i]) + { + var[i] = tmpVar[i]; + } + else + { + var[i] = k * var[i] + (1.0f - k)*tmpVar[i]; + } + } + } + else + { + // put this accel into buffer + bfPut(bf, latest); + /* Recursivly include new accel. The data num used to compute mean and + * var are increasing. + */ + // mean(n+1) = mean(n) *n / (n+1) + x(n+1) / (n+1) + mean[0] = lastMean[0] + (latest[0] - lastMean[0]) / (real)(bf->num); + mean[1] = lastMean[1] + (latest[1] - lastMean[1]) / (real)(bf->num); + mean[2] = lastMean[2] + (latest[2] - lastMean[2]) / (real)(bf->num); + var[0] = lastVar[0] + lastMean[0] * lastMean[0] - mean[0] * mean[0] + + (latest[0] * latest[0] - lastVar[0] - lastMean[0] * lastMean[0]) / (real)(bf->num); + var[1] = lastVar[1] + lastMean[1] * lastMean[1] - mean[1] * mean[1] + + (latest[1] * latest[1] - lastVar[1] - lastMean[1] * lastMean[1]) / (real)(bf->num); + var[2] = lastVar[2] + lastMean[2] * lastMean[2] - mean[2] * mean[2] + + (latest[2] * latest[2] - lastVar[2] - lastMean[2] * lastMean[2]) / (real)(bf->num); + } +} + +BOOL DetectStaticIMU(real *gyroVar, real *gyroMean, real *accelVar, STATIC_DETECT_SETTING *threshold) +{ + BOOL bStatic = TRUE; + int i; + static float multiplier[3] = { 0.0, 0.0, 0.0 }; + static float gyroVarThreshold = 0.0; + static float accelVarThreshold = 0.0; + static float gyroBiasThreshold = 0.0; + // Update threshold + if (multiplier[0] != threshold->staticNoiseMultiplier[0]) + { + multiplier[0] = threshold->staticNoiseMultiplier[0]; + gyroVarThreshold = multiplier[0] * threshold->staticVarGyro; + } + if (multiplier[1] != threshold->staticNoiseMultiplier[1]) + { + multiplier[1] = threshold->staticNoiseMultiplier[1]; + accelVarThreshold = multiplier[1] * threshold->staticVarAccel; + } + if (multiplier[2] != threshold->staticNoiseMultiplier[2]) + { + multiplier[2] = threshold->staticNoiseMultiplier[2]; + gyroBiasThreshold = multiplier[2] * threshold->maxGyroBias; + } + + for (i = 0; i < 3; i++) + { + if (gyroVar[i] > gyroVarThreshold || + accelVar[i] > accelVarThreshold || + fabs(gyroMean[i]) > gyroBiasThreshold) + { + bStatic = FALSE; + break; + } + } + + return bStatic; +} + +/* Set the accelerometer filter coefficients, which are used to filter the + * accelerometer readings prior to determining the setting of the linear- + * acceleration switch and computing the roll and pitch from accelerometer + * readings. + */ +static void _PopulateFilterCoefficients(uint8_t lpfType, uint8_t callingFreq, real *b, real *a) +{ + switch (lpfType) + { + case NO_LPF: + b[0] = (real)(1.0); + b[1] = (real)(0.0); + b[2] = (real)(0.0); + b[3] = (real)(0.0); + + a[0] = (real)(0.0); + a[1] = (real)(0.0); + a[2] = (real)(0.0); + a[3] = (real)(0.0); + break; + case TWO_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(2.19606211225382e-4); + b[1] = (real)(6.58818633676145e-4); + b[2] = (real)(6.58818633676145e-4); + b[3] = (real)(2.19606211225382e-4); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.748835809214676); + a[2] = (real)(2.528231219142559); + a[3] = (real)(-0.777638560238080); + } + else + { + b[0] = (real)(2.91464944656705e-5); + b[1] = (real)(8.74394833970116e-5); + b[2] = (real)(8.74394833970116e-5); + b[3] = (real)(2.91464944656705e-5); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.874356892677485); + a[2] = (real)(2.756483195225695); + a[3] = (real)(-0.881893130592486); + } + break; + case FIVE_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(0.002898194633721); + b[1] = (real)(0.008694583901164); + b[2] = (real)(0.008694583901164); + b[3] = (real)(0.002898194633721); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.374094743709352); + a[2] = (real)(1.929355669091215); + a[3] = (real)(-0.532075368312092); + } + else + { + b[0] = (real)(0.000416546139076); + b[1] = (real)(0.001249638417227); + b[2] = (real)(0.001249638417227); + b[3] = (real)(0.000416546139076); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.686157396548143); + a[2] = (real)(2.419655110966473); + a[3] = (real)(-0.730165345305723); + } + break; + case TWENTY_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + // [B,A] = butter(3,20/(100/2)) + b[0] = (real)(0.098531160923927); + b[1] = (real)(0.295593482771781); + b[2] = (real)(0.295593482771781); + b[3] = (real)(0.098531160923927); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-0.577240524806303); + a[2] = (real)(0.421787048689562); + a[3] = (real)(-0.056297236491843); + } + else + { + // [B,A] = butter(3,20/(200/2)) + b[0] = (real)(0.018098933007514); + b[1] = (real)(0.054296799022543); + b[2] = (real)(0.054296799022543); + b[3] = (real)(0.018098933007514); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-1.760041880343169); + a[2] = (real)(1.182893262037831); + a[3] = (real)(-0.278059917634546); + } + break; + case TWENTY_FIVE_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(0.166666666666667); + b[1] = (real)(0.500000000000000); + b[2] = (real)(0.500000000000000); + b[3] = (real)(0.166666666666667); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-0.000000000000000); + a[2] = (real)(0.333333333333333); + a[3] = (real)(-0.000000000000000); + } + else + { + b[0] = (real)(0.031689343849711); + b[1] = (real)(0.095068031549133); + b[2] = (real)(0.095068031549133); + b[3] = (real)(0.031689343849711); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-1.459029062228061); + a[2] = (real)(0.910369000290069); + a[3] = (real)(-0.197825187264319); + } + break; + case TEN_HZ_LPF: + default: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(0.0180989330075144); + b[1] = (real)(0.0542967990225433); + b[2] = (real)(0.0542967990225433); + b[3] = (real)(0.0180989330075144); + + a[0] = (real)(1.0000000000000000); + a[1] = (real)(-1.7600418803431690); + a[2] = (real)(1.1828932620378310); + a[3] = (real)(-0.2780599176345460); + } + else + { + b[0] = (real)(0.002898194633721); + b[1] = (real)(0.008694583901164); + b[2] = (real)(0.008694583901164); + b[3] = (real)(0.002898194633721); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.374094743709352); + a[2] = (real)(1.929355669091215); + a[3] = (real)(-0.532075368312092); + } + break; + } +} + +static void LowPassFilter(real *in, Buffer *bfIn, Buffer *bfOut, real *b, real *a, real *filtered) +{ + // Fill the buffer with the first input. + if (!bfIn->full) + { + bfPut(bfIn, in); + filtered[0] = in[0]; + filtered[1] = in[1]; + filtered[2] = in[2]; + return; + } + + /* Filter accelerometer readings (Note: a[0] = 1.0 and the filter coefficients are symmetric) + * y = filtered output; x = raw input; + * a[0]*y(k) + a[1]*y(k-1) + a[2]*y(k-2) + a[3]*y(k-3) = + * b[0]*x(k) + b[1]*x(k-1) + b[2]*x(k-2) + b[3]*x(k-3) = + * b[0]*( x(k) + x(k-3) ) + b[1]*( x(k-1) + x(k-2) ) + */ + int i; + real tmpIn[3]; + real tmpOut[3]; + filtered[0] = b[CURRENT] * in[0]; + filtered[1] = b[CURRENT] * in[1]; + filtered[2] = b[CURRENT] * in[2]; + for (i = PASTx1; i <= PASTx3; i++) + { + bfGet(bfIn, tmpIn, i-1); + bfGet(bfOut, tmpOut, i-1); + filtered[0] += b[i] * tmpIn[0] - a[i] * tmpOut[0]; + filtered[1] += b[i] * tmpIn[1] - a[i] * tmpOut[1]; + filtered[2] += b[i] * tmpIn[2] - a[i] * tmpOut[2]; + } + + // New data into buffer + bfPut(bfIn, in); +} + +void EstimateAccelError(real *accel, real *w, real dt, uint32_t staticDelay, ImuStatsStruct *imuStats) +{ + static BOOL bIni = false; // indicate if the procedure is initialized + static real lastAccel[3]; // accel input of last step + static real lastGyro[3]; // gyro input of last step + static float lastEstimatedAccel[3]; // propagated accel of last step + static uint32_t counter = 0; // propagation counter + static uint32_t t[3]; + // initialize + if (!bIni) + { + bIni = true; + lastAccel[0] = accel[0]; + lastAccel[1] = accel[1]; + lastAccel[2] = accel[2]; + lastGyro[0] = w[0]; + lastGyro[1] = w[1]; + lastGyro[2] = w[2]; + t[0] = 0; + t[1] = 0; + t[2] = 0; + imuStats->accelErr[0] = 0.0; + imuStats->accelErr[1] = 0.0; + imuStats->accelErr[2] = 0.0; + return; + } + + /* Using gyro to propagate accel and then to detect accel error can give valid result for a + * short period of time because the inhere long-term drift of integrating gyro data. + * So, after this period of time, a new accel input will be selected. + * Beside, this method cannot detect long-time smooth linear acceleration. In this case, we + * can only hope the linear acceleration is large enough to make an obvious diffeerence from + * the Earth gravity 1g. + */ + if (counter == 0) + { + lastEstimatedAccel[0] = lastAccel[0]; + lastEstimatedAccel[1] = lastAccel[1]; + lastEstimatedAccel[2] = lastAccel[2]; + } + counter++; + if (counter == staticDelay) + { + counter = 0; + } + + // propagate accel using gyro + // a(k) = a(k-1) -w x a(k-1)*dt + real ae[3]; + lastGyro[0] *= -dt; + lastGyro[1] *= -dt; + lastGyro[2] *= -dt; + cross(lastGyro, lastEstimatedAccel, ae); + ae[0] += lastEstimatedAccel[0]; + ae[1] += lastEstimatedAccel[1]; + ae[2] += lastEstimatedAccel[2]; + + // save this estimated accel + lastEstimatedAccel[0] = ae[0]; + lastEstimatedAccel[1] = ae[1]; + lastEstimatedAccel[2] = ae[2]; + + // err = a(k) - am + ae[0] -= accel[0]; + ae[1] -= accel[1]; + ae[2] -= accel[2]; + + /* If the difference between the propagted accel and the input accel exceeds some threshold, + * we assume there is linear acceleration and set .accelErr to be a large value (0.1g). + * If the difference has been within the threshold for a period of time, we start to decrease + * estimated accel error .accelErr. + */ + int j; + imuStats->accelErrLimit = false; + for (j = 0; j < 3; j++) + { + if (fabs(ae[j]) > 0.0980665) // linear accel detected, 0.01g + { + t[j] = 0; + imuStats->accelErr[j] = (real)0.980665; // 0.1g + } + else // no linear accel detected, start to decrease estimated accel error + { + if (t[j] > staticDelay) // decrease error + { + imuStats->accelErr[j] *= 0.9f; + imuStats->accelErr[j] += 0.1f * ae[j]; + } + else // keep previous error value + { + t[j]++; + // imuStats->accelErr[j]; + } + } + // limit error, not taking effect here since the max accelErr should be 0.1g + if (imuStats->accelErr[j] > 5.0) // 0.5g + { + imuStats->accelErr[j] = 5.0; + imuStats->accelErrLimit = true; + } + if (imuStats->accelErr[j] < -5.0) + { + imuStats->accelErr[j] = -5.0; + imuStats->accelErrLimit = true; + } + } + // record accel for next step + lastAccel[0] = accel[0]; + lastAccel[1] = accel[1]; + lastAccel[2] = accel[2]; + lastGyro[0] = w[0]; + lastGyro[1] = w[1]; + lastGyro[2] = w[2]; +} + +BOOL DetectMotionFromAccel(real accelNorm, int iReset) +{ + if (iReset) + { + gAlgorithm.linAccelSwitch = false; + gAlgorithm.linAccelSwitchCntr = 0; + } + /* Check for times when the acceleration is 'close' to 1 [g]. When this occurs, + * increment a counter. When it exceeds a threshold (indicating that the system + * has been at rest for a given period) then decrease the R-values (in the + * update stage of the EKF), effectively increasing the Kalman gain. + */ + if (fabs(1.0 - accelNorm/GRAVITY) < gAlgorithm.Limit.accelSwitch) + { + gAlgorithm.linAccelSwitchCntr++; + if (gAlgorithm.linAccelSwitchCntr >= gAlgorithm.Limit.linAccelSwitchDelay) + { + gAlgorithm.linAccelSwitch = TRUE; + } + else + { + gAlgorithm.linAccelSwitch = FALSE; + } + } + else + { + gAlgorithm.linAccelSwitchCntr = 0; + gAlgorithm.linAccelSwitch = FALSE; + } + + return TRUE; +} + +BOOL DetectStaticGnssVelocity(double *vNED, real threshold, BOOL gnssValid) +{ + static uint8_t cntr = 0; + if (gnssValid) + { + if (fabs(vNED[0]) < threshold && fabs(vNED[1]) < threshold && fabs(vNED[2]) < threshold) + { + if (cntr < 3) + { + cntr++; + } + } + else + { + cntr = 0; + } + } + else + { + cntr = 0; + } + + return cntr >= 3; +} + +BOOL DetectStaticOdo(real odo) +{ + return FALSE; +} \ No newline at end of file diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/PredictFunctions.c b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/PredictFunctions.c new file mode 100644 index 0000000..e728dca --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/PredictFunctions.c @@ -0,0 +1,941 @@ +/* + * File: EKF_PredictionStage.cpp + * Author: joemotyka + * + * Created on May 8, 2016, 12:23 AM + */ + + +#include // memset +#include // pow + +#include "GlobalConstants.h" +#include "platformAPI.h" +#include "Indices.h" // IND +#include "StateIndices.h" // STATE_IND + +#include "MatrixMath.h" +#include "VectorMath.h" +#include "QuaternionMath.h" // QuatNormalize +#include "TransformationMath.h" + +#include "algorithm.h" +#include "algorithmAPI.h" +#include "SensorNoiseParameters.h" +#include "MotionStatus.h" +#include "EKF_Algorithm.h" +#include "PredictFunctions.h" +#include "AlgorithmLimits.h" +#include "WorldMagneticModel.h" + +#ifndef INS_OFFLINE +#ifdef DISPLAY_DIAGNOSTIC_MSG +#include "debug.h" +#endif +#endif + +extern ImuStatsStruct gImuStats; + +/* F is sparse and has elements in the following locations... + * There may be some more efficient ways of implementing this as this method + * still performs multiplication with zero values. (Ask Andrey) + */ +uint8_t RLE_F[ROWS_IN_F][2] = { { STATE_RX, STATE_VX }, // Row 0: cols 0,3 + { STATE_RY, STATE_VY }, // Row 1: cols 1,4 + { STATE_RZ, STATE_VZ }, // Row 2: cols 2,5 + { STATE_VX, STATE_ABZ }, // Row 3: cols 3,6:9,13:15 + { STATE_VY, STATE_ABZ }, // Row 4: cols 4,6:9,13:15 + { STATE_VZ, STATE_ABZ }, // Row 5: cols 5,6:9,13:15 + { STATE_Q0, STATE_WBZ }, // Row 6: cols 6:12 + { STATE_Q0, STATE_WBZ }, // Row 7: cols 6:12 + { STATE_Q0, STATE_WBZ }, // Row 8: cols 6:12 + { STATE_Q0, STATE_WBZ }, // Row 9: cols 6:12 + { STATE_WBX, STATE_WBX }, // Row 10: cols 10 + { STATE_WBY, STATE_WBY }, // Row 11: cols 11 + { STATE_WBZ, STATE_WBZ }, // Row 12: cols 12 + { STATE_ABX, STATE_ABX }, // Row 13: cols 13 + { STATE_ABY, STATE_ABY }, // Row 14: cols 14 + { STATE_ABZ, STATE_ABZ } }; // Row 15: cols 15 + +// Q is sparse and has elements in the following locations... +uint8_t RLE_Q[ROWS_IN_F][2] = { { STATE_RX, STATE_RX }, + { STATE_RY, STATE_RY }, + { STATE_RZ, STATE_RZ }, + { STATE_VX, STATE_VX }, + { STATE_VY, STATE_VY }, + { STATE_VZ, STATE_VZ }, + { STATE_Q0, STATE_Q3 }, + { STATE_Q0, STATE_Q3 }, + { STATE_Q0, STATE_Q3 }, + { STATE_Q0, STATE_Q3 }, + { STATE_WBX, STATE_WBX }, + { STATE_WBY, STATE_WBY }, + { STATE_WBZ, STATE_WBZ }, + { STATE_ABX, STATE_ABX }, + { STATE_ABY, STATE_ABY }, + { STATE_ABZ, STATE_ABZ } }; + +// Local functions +static void _PredictStateEstimate(void); +static void _PredictCovarianceEstimate(void); +static void StateCovarianceChange(); +static void _UpdateProcessJacobian(void); +static void _UpdateProcessCovariance(void); + +// todo tm20160603 - use filters from filter.h, or move this filter there (Ask Andrey) +void _FirstOrderLowPass(real *output, real input); + +/* 16 States: [ STATE_RX, STATE_RY, STATE_RZ, ... + * STATE_VX, STATE_VY, STATE_VZ, ... + * STATE_Q0, STATE_Q1, STATE_Q2, STATE_Q3, ... + * STATE_WBX, STATE_WBY, STATE_WBZ, ... + * STATE_ABX, STATE_ABY, STATE_ABZ ] + */ +//============================================================================= +//EKF_PredictionStage.m +void EKF_PredictionStage(real *filteredAccel) +{ + real magFieldVector[3]; + + // Propagate the state (22 usec) and covariance (1.82 msec) estimates + _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 ); + + /* Filter the yaw-rate at 200 Hz for the TURN-SWITCH (used in the + * update stage only -- since that is a ten-hertz routine). The way this + * is coded, the filter function can only be used for filtering yaw-rate + * data as the previous input state is saved as a static in the function. + */ + _FirstOrderLowPass( &gAlgorithm.filteredYawRate, + gKalmanFilter.correctedRate_B[Z_AXIS] ); + + /* Extract the magnetometer readings (set to zero if the magnetometer is not + * present or unused). + */ + if(magUsedInAlgorithm()) + { + magFieldVector[X_AXIS] = (real)gEKFInput.magField_B[X_AXIS]; + magFieldVector[Y_AXIS] = (real)gEKFInput.magField_B[Y_AXIS]; + magFieldVector[Z_AXIS] = (real)gEKFInput.magField_B[Z_AXIS]; + } + else + { + magFieldVector[X_AXIS] = magFieldVector[Y_AXIS] = magFieldVector[Z_AXIS] = (real)0.0; + } + + /* Compute the measured Euler angles from gravity and magnetic field data + * ( phiMeas, thetaMeas, psiMeas ) = f( g_B, mMeas_B ). Adjust for declination. + */ + // Compute the unit gravity vector (-accel) in the body frame + real unitGravityVector[3] = {0.0f}; + UnitGravity(filteredAccel, unitGravityVector); + // Compute roll and pitch from the unit gravity vector. + UnitGravityToEulerAngles(unitGravityVector, gKalmanFilter.measuredEulerAngles); + /* Compute measured yaw. + * If mag is not in use, yaw is the predicted yaw in the Kalman filter + * If mag is in use, predicted roll and pitch are used to project mag and compute yaw in LG (chage to >=LG?), + * measured pitch and roll (indeed the unit gravity vector from measured accel) are used to project + * mag and compute yaw in other cases. + */ + if ( magUsedInAlgorithm() ) + { + // Transform the magnetic field vector from the body-frame to the plane normal to the gravity vector + if ( gAlgorithm.state == LOW_GAIN_AHRS ) + { + // Using predicted pitch and roll to project the mag measurement + gKalmanFilter.measuredEulerAngles[YAW] = + RollPitchAndMagToYaw( gKalmanFilter.eulerAngles[ROLL], + gKalmanFilter.eulerAngles[PITCH], + magFieldVector ); + } + else + { + // Using accel measurement to project the mag measurement + gKalmanFilter.measuredEulerAngles[YAW] = + UnitGravityAndMagToYaw( unitGravityVector, + magFieldVector ); + } + } + else + { + // For VG, set the measured heading to the predicted heading (this + // forces the error to zero) + gKalmanFilter.measuredEulerAngles[YAW] = gKalmanFilter.eulerAngles[YAW]; + } + + + // Adjust for declination if the GPS signal is good + if( gAlgorithm.applyDeclFlag ) + { + gKalmanFilter.measuredEulerAngles[YAW] = gKalmanFilter.measuredEulerAngles[YAW] + + gWorldMagModel.decl_rad; + } +} + + +/* Predict the EKF states at 100 Hz based on readings from the: + * - accelerometer + * - angular-rate sensors + */ +static void _PredictStateEstimate(void) +{ + real aCorr_N[3]; + real deltaQuaternion[4]; + + if( gAlgorithm.state > LOW_GAIN_AHRS ) + { + // ================= NED Position (r_N) ================= + // r_N(k+1) = r_N(k) + dV_N = r_N(k) + v_N*DT + gKalmanFilter.Position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS] + + gKalmanFilter.Velocity_N[X_AXIS] * gAlgorithm.dt; + gKalmanFilter.Position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS] + + gKalmanFilter.Velocity_N[Y_AXIS] * gAlgorithm.dt; + gKalmanFilter.Position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS] + + gKalmanFilter.Velocity_N[Z_AXIS] * gAlgorithm.dt; + + // ================= NED Velocity (v_N) ================= + // aCorr_B = aMeas_B - aBias_B + // gEKFInput.accel_B in g's, convert to m/s^2 for integration + gKalmanFilter.correctedAccel_B[X_AXIS] = gEKFInput.accel_B[X_AXIS] - + gKalmanFilter.accelBias_B[X_AXIS]; + gKalmanFilter.correctedAccel_B[Y_AXIS] = gEKFInput.accel_B[Y_AXIS] - + gKalmanFilter.accelBias_B[Y_AXIS]; + gKalmanFilter.correctedAccel_B[Z_AXIS] = gEKFInput.accel_B[Z_AXIS] - + gKalmanFilter.accelBias_B[Z_AXIS]; + + /* Transform the corrected acceleration vector from the body to the NED-frame and remove gravity + * a_N = R_BinN * a_B + */ + aCorr_N[X_AXIS] = + gKalmanFilter.R_BinN[X_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[X_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[X_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; + aCorr_N[Y_AXIS] = + gKalmanFilter.R_BinN[Y_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[Y_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[Y_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; + aCorr_N[Z_AXIS] = + gKalmanFilter.R_BinN[Z_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS] + + (real)GRAVITY; + + /* Determine the acceleration of the system by removing the gravity vector + * v_N(k+1) = v_N(k) + dV = v_N(k) + aMotion_N*DT = v_N(k) + ( a_N - g_N )*DT + */ + gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + aCorr_N[X_AXIS] * gAlgorithm.dt; + gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + aCorr_N[Y_AXIS] * gAlgorithm.dt; + gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + aCorr_N[Z_AXIS] * gAlgorithm.dt; + + // Calculate linear acceleration in the body frame. + gKalmanFilter.linearAccel_B[X_AXIS] += (gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][X_AXIS] * (real)GRAVITY)*gAlgorithm.dt; + gKalmanFilter.linearAccel_B[Y_AXIS] = gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Y_AXIS] * (real)GRAVITY; + gKalmanFilter.linearAccel_B[Z_AXIS] = gKalmanFilter.correctedAccel_B[Z_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Z_AXIS] * (real)GRAVITY; + } + else + { + // GPS not valid yet, do not propagate the position or velocity + gKalmanFilter.Position_N[X_AXIS] = (real)0.0; + gKalmanFilter.Position_N[Y_AXIS] = (real)0.0; + gKalmanFilter.Position_N[Z_AXIS] = (real)0.0; + + gKalmanFilter.Velocity_N[X_AXIS] = (real)0.0; + gKalmanFilter.Velocity_N[Y_AXIS] = (real)0.0; + gKalmanFilter.Velocity_N[Z_AXIS] = (real)0.0; + + // what should this be??? + gKalmanFilter.correctedAccel_B[XACCEL] = gEKFInput.accel_B[X_AXIS]; + gKalmanFilter.correctedAccel_B[YACCEL] = gEKFInput.accel_B[Y_AXIS]; + gKalmanFilter.correctedAccel_B[ZACCEL] = gEKFInput.accel_B[Z_AXIS]; + } + + // ================= Attitude quaternion ================= + // Find the 'true' angular rate (wTrue_B = wCorr_B = wMeas_B - wBias_B) + gKalmanFilter.correctedRate_B[X_AXIS] = gEKFInput.angRate_B[X_AXIS] - + gKalmanFilter.rateBias_B[X_AXIS]; + gKalmanFilter.correctedRate_B[Y_AXIS] = gEKFInput.angRate_B[Y_AXIS] - + gKalmanFilter.rateBias_B[Y_AXIS]; + gKalmanFilter.correctedRate_B[Z_AXIS] = gEKFInput.angRate_B[Z_AXIS] - + gKalmanFilter.rateBias_B[Z_AXIS]; + + // Placed in gKalmanFilter as wTrueTimesDtOverTwo is used to compute the Jacobian (F) + gKalmanFilter.wTrueTimesDtOverTwo[X_AXIS] = gKalmanFilter.correctedRate_B[X_AXIS] * gAlgorithm.dtOverTwo; + gKalmanFilter.wTrueTimesDtOverTwo[Y_AXIS] = gKalmanFilter.correctedRate_B[Y_AXIS] * gAlgorithm.dtOverTwo; + gKalmanFilter.wTrueTimesDtOverTwo[Z_AXIS] = gKalmanFilter.correctedRate_B[Z_AXIS] * gAlgorithm.dtOverTwo; + + // Find the attitude change based on angular rate data + deltaQuaternion[Q0] = -gKalmanFilter.wTrueTimesDtOverTwo[X_AXIS] * gKalmanFilter.quaternion[Q1] + + -gKalmanFilter.wTrueTimesDtOverTwo[Y_AXIS] * gKalmanFilter.quaternion[Q2] + + -gKalmanFilter.wTrueTimesDtOverTwo[Z_AXIS] * gKalmanFilter.quaternion[Q3]; + deltaQuaternion[Q1] = gKalmanFilter.wTrueTimesDtOverTwo[X_AXIS] * gKalmanFilter.quaternion[Q0] + + gKalmanFilter.wTrueTimesDtOverTwo[Z_AXIS] * gKalmanFilter.quaternion[Q2] + + -gKalmanFilter.wTrueTimesDtOverTwo[Y_AXIS] * gKalmanFilter.quaternion[Q3]; + deltaQuaternion[Q2] = gKalmanFilter.wTrueTimesDtOverTwo[Y_AXIS] * gKalmanFilter.quaternion[Q0] + + -gKalmanFilter.wTrueTimesDtOverTwo[Z_AXIS] * gKalmanFilter.quaternion[Q1] + + gKalmanFilter.wTrueTimesDtOverTwo[X_AXIS] * gKalmanFilter.quaternion[Q3]; + deltaQuaternion[Q3] = gKalmanFilter.wTrueTimesDtOverTwo[Z_AXIS] * gKalmanFilter.quaternion[Q0] + + gKalmanFilter.wTrueTimesDtOverTwo[Y_AXIS] * gKalmanFilter.quaternion[Q1] + + -gKalmanFilter.wTrueTimesDtOverTwo[X_AXIS] * gKalmanFilter.quaternion[Q2]; + + // Update the attitude + // q_BinN(k+1) = q_BinN(k) + dq = q_BinN(k) + OMEGA*q_BinN(k) + gKalmanFilter.quaternion[Q0] = gKalmanFilter.quaternion[Q0] + deltaQuaternion[Q0]; + gKalmanFilter.quaternion[Q1] = gKalmanFilter.quaternion[Q1] + deltaQuaternion[Q1]; + gKalmanFilter.quaternion[Q2] = gKalmanFilter.quaternion[Q2] + deltaQuaternion[Q2]; + gKalmanFilter.quaternion[Q3] = gKalmanFilter.quaternion[Q3] + deltaQuaternion[Q3]; + + // Normalize quaternion and force q0 to be positive + QuatNormalize(gKalmanFilter.quaternion); + + // ================= Angular-rate bias: wBias(k+1) = wBias(k) ================= + // N/A (predicted state is same as past state) + + // ================= Linear-acceleration bias: aBias(k+1) = aBias(k) ================= + // N/A (predicted state is same as past state) +} + + +// Define variables that reside on the heap +real PxFTranspose[ROWS_IN_P][ROWS_IN_F], FxPxFTranspose[ROWS_IN_F][ROWS_IN_F]; + +// PredictCovarianceEstimate.m +static void _PredictCovarianceEstimate(void) +{ + uint8_t rowNum, colNum, multIndex; + + /* Compute the F and Q matrices used in the prediction stage (only certain + * elements in the process-covariance, Q, change with each time-step) + */ + _UpdateProcessJacobian(); // gKF.F (16x16) + _UpdateProcessCovariance(); // gKF.Q (16x16) + + // Update P from the P, F, and Q matrices: P = FxPxFTranspose + Q + // 1) PxFTranspose is computed first + memset(PxFTranspose, 0, sizeof(PxFTranspose)); + for (rowNum = 0; rowNum < ROWS_IN_P; rowNum++) + { + for (colNum = 0; colNum < ROWS_IN_F; colNum++) + { + for (multIndex = RLE_F[colNum][0]; multIndex <= RLE_F[colNum][1]; multIndex++) + { + PxFTranspose[rowNum][colNum] = PxFTranspose[rowNum][colNum] + + gKalmanFilter.P[rowNum][multIndex] * gKalmanFilter.F[colNum][multIndex]; + } + } + } + +#if 0 + // 2) Use gKalmanFilter.P as a temporary variable to hold FxPxFTranspose + // to reduce the number of "large" variables on the heap + for (rowNum = 0; rowNum < 16; rowNum++) + { + for (colNum = 0; colNum < 16; colNum++) + { + gKalmanFilter.P[rowNum][colNum] = 0.0; + for (multIndex = RLE_F[rowNum][0]; multIndex <= RLE_F[rowNum][1]; multIndex++) + { + gKalmanFilter.P[rowNum][colNum] = gKalmanFilter.P[rowNum][colNum] + + gKalmanFilter.F[rowNum][multIndex] * PxFTranspose[multIndex][colNum]; + } + } + } + + // P is a fully populated matrix (nominally) so all the elements of the matrix have to be + // considered when working with it. + LimitValuesAndForceMatrixSymmetry_noAvg(&gKalmanFilter.P[0][0], (real)LIMIT_P, ROWS_IN_P, COLS_IN_P); +#else + /* 2) Use gKalmanFilter.P as a temporary variable to hold FxPxFTranspose + * to reduce the number of "large" variables on the heap. + * This matrix is symmetric so only need to multiply one half and reflect the values + * across the diagonal + */ + memset(gKalmanFilter.P, 0, sizeof(gKalmanFilter.P)); + for (rowNum = 0; rowNum < 16; rowNum++) + { + for (colNum = rowNum; colNum < 16; colNum++) + { + //gKalmanFilter.P[rowNum][colNum] = 0.0; + for (multIndex = RLE_F[rowNum][0]; multIndex <= RLE_F[rowNum][1]; multIndex++) + { + gKalmanFilter.P[rowNum][colNum] = gKalmanFilter.P[rowNum][colNum] + + gKalmanFilter.F[rowNum][multIndex] * PxFTranspose[multIndex][colNum]; + } + gKalmanFilter.P[colNum][rowNum] = gKalmanFilter.P[rowNum][colNum]; + //Limit values in P + if(gKalmanFilter.P[rowNum][colNum] > (real)LIMIT_P) + { + gKalmanFilter.P[rowNum][colNum] = (real)LIMIT_P; + } + else if(gKalmanFilter.P[rowNum][colNum] < -(real)LIMIT_P) + { + gKalmanFilter.P[rowNum][colNum] = -(real)LIMIT_P; + } + } + } +#endif + + /* 3) Finally, add Q to FxPxFTranspose (P) to get the final value for + * gKalmanFilter.P (only the quaternion elements of Q have nonzero off- + * diagonal terms) + */ + gKalmanFilter.P[STATE_RX][STATE_RX] = gKalmanFilter.P[STATE_RX][STATE_RX] + gKalmanFilter.Q[STATE_RX]; + gKalmanFilter.P[STATE_RY][STATE_RY] = gKalmanFilter.P[STATE_RY][STATE_RY] + gKalmanFilter.Q[STATE_RY]; + gKalmanFilter.P[STATE_RZ][STATE_RZ] = gKalmanFilter.P[STATE_RZ][STATE_RZ] + gKalmanFilter.Q[STATE_RZ]; + + gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_VX] + gKalmanFilter.Q[STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.P[STATE_VY][STATE_VY] + gKalmanFilter.Q[STATE_VY]; + gKalmanFilter.P[STATE_VZ][STATE_VZ] = gKalmanFilter.P[STATE_VZ][STATE_VZ] + gKalmanFilter.Q[STATE_VZ]; + + gKalmanFilter.P[STATE_Q0][STATE_Q0] = gKalmanFilter.P[STATE_Q0][STATE_Q0] + gKalmanFilter.Q[STATE_Q0]; + gKalmanFilter.P[STATE_Q0][STATE_Q1] = gKalmanFilter.P[STATE_Q0][STATE_Q1] + gKalmanFilter.Qq[0]; + gKalmanFilter.P[STATE_Q0][STATE_Q2] = gKalmanFilter.P[STATE_Q0][STATE_Q2] + gKalmanFilter.Qq[1]; + gKalmanFilter.P[STATE_Q0][STATE_Q3] = gKalmanFilter.P[STATE_Q0][STATE_Q3] + gKalmanFilter.Qq[2]; + + //gKalmanFilter.P[STATE_Q1][STATE_Q0] = gKalmanFilter.P[STATE_Q1][STATE_Q0] + gKalmanFilter.Q[STATE_Q1][STATE_Q0]; + gKalmanFilter.P[STATE_Q1][STATE_Q0] = gKalmanFilter.P[STATE_Q0][STATE_Q1]; + gKalmanFilter.P[STATE_Q1][STATE_Q1] = gKalmanFilter.P[STATE_Q1][STATE_Q1] + gKalmanFilter.Q[STATE_Q1]; + gKalmanFilter.P[STATE_Q1][STATE_Q2] = gKalmanFilter.P[STATE_Q1][STATE_Q2] + gKalmanFilter.Qq[3]; + gKalmanFilter.P[STATE_Q1][STATE_Q3] = gKalmanFilter.P[STATE_Q1][STATE_Q3] + gKalmanFilter.Qq[4]; + + //gKalmanFilter.P[STATE_Q2][STATE_Q0] = gKalmanFilter.P[STATE_Q2][STATE_Q0] + gKalmanFilter.Q[STATE_Q2][STATE_Q0]; + //gKalmanFilter.P[STATE_Q2][STATE_Q1] = gKalmanFilter.P[STATE_Q2][STATE_Q1] + gKalmanFilter.Q[STATE_Q2][STATE_Q1]; + gKalmanFilter.P[STATE_Q2][STATE_Q0] = gKalmanFilter.P[STATE_Q0][STATE_Q2]; + gKalmanFilter.P[STATE_Q2][STATE_Q1] = gKalmanFilter.P[STATE_Q1][STATE_Q2]; + gKalmanFilter.P[STATE_Q2][STATE_Q2] = gKalmanFilter.P[STATE_Q2][STATE_Q2] + gKalmanFilter.Q[STATE_Q2]; + gKalmanFilter.P[STATE_Q2][STATE_Q3] = gKalmanFilter.P[STATE_Q2][STATE_Q3] + gKalmanFilter.Qq[5]; + + //gKalmanFilter.P[STATE_Q3][STATE_Q0] = gKalmanFilter.P[STATE_Q3][STATE_Q0] + gKalmanFilter.Q[STATE_Q3][STATE_Q0]; + //gKalmanFilter.P[STATE_Q3][STATE_Q1] = gKalmanFilter.P[STATE_Q3][STATE_Q1] + gKalmanFilter.Q[STATE_Q3][STATE_Q1]; + //gKalmanFilter.P[STATE_Q3][STATE_Q2] = gKalmanFilter.P[STATE_Q3][STATE_Q2] + gKalmanFilter.Q[STATE_Q3][STATE_Q2]; + gKalmanFilter.P[STATE_Q3][STATE_Q0] = gKalmanFilter.P[STATE_Q0][STATE_Q3]; + gKalmanFilter.P[STATE_Q3][STATE_Q1] = gKalmanFilter.P[STATE_Q1][STATE_Q3]; + gKalmanFilter.P[STATE_Q3][STATE_Q2] = gKalmanFilter.P[STATE_Q2][STATE_Q3]; + gKalmanFilter.P[STATE_Q3][STATE_Q3] = gKalmanFilter.P[STATE_Q3][STATE_Q3] + gKalmanFilter.Q[STATE_Q3]; + + gKalmanFilter.P[STATE_WBX][STATE_WBX] = gKalmanFilter.P[STATE_WBX][STATE_WBX] + gKalmanFilter.Q[STATE_WBX]; + gKalmanFilter.P[STATE_WBY][STATE_WBY] = gKalmanFilter.P[STATE_WBY][STATE_WBY] + gKalmanFilter.Q[STATE_WBY]; + gKalmanFilter.P[STATE_WBZ][STATE_WBZ] = gKalmanFilter.P[STATE_WBZ][STATE_WBZ] + gKalmanFilter.Q[STATE_WBZ]; + + gKalmanFilter.P[STATE_ABX][STATE_ABX] = gKalmanFilter.P[STATE_ABX][STATE_ABX] + gKalmanFilter.Q[STATE_ABX]; + gKalmanFilter.P[STATE_ABY][STATE_ABY] = gKalmanFilter.P[STATE_ABY][STATE_ABY] + gKalmanFilter.Q[STATE_ABY]; + gKalmanFilter.P[STATE_ABZ][STATE_ABZ] = gKalmanFilter.P[STATE_ABZ][STATE_ABZ] + gKalmanFilter.Q[STATE_ABZ]; +} + + +// GenerateProcessJacobian.m: Set the elements of F that DO NOT change with each time-step +void GenerateProcessJacobian(void) +{ + // Initialize the Process Jacobian matrix (F) + memset(gKalmanFilter.F, 0, sizeof(gKalmanFilter.F)); + + // Form the process Jacobian + + // ---------- Rows corresponding to POSITION ---------- + gKalmanFilter.F[STATE_RX][STATE_VX] = gAlgorithm.dt; + gKalmanFilter.F[STATE_RY][STATE_VY] = gAlgorithm.dt; + gKalmanFilter.F[STATE_RZ][STATE_VZ] = gAlgorithm.dt; + + // ---------- Rows corresponding to VELOCITY ---------- + // N/A (other than diagonal, set below, all other terms changes with attitude) + + // ---------- Rows corresponding to ATTITUDE ---------- + // N/A (other than diagonal, set below, all other terms changes with attitude) + + // ---------- Rows corresponding to RATE-BIAS ---------- + // N/A (no terms changes with attitude) + + // ---------- Rows corresponding to ACCELERATION-BIAS ---------- + // N/A (no terms changes with attitude) + + // ---------- Add to I16 to get final formulation of F ---------- + // Populate the diagonals of F with 1.0 + gKalmanFilter.F[STATE_RX][STATE_RX] = (real)1.0; + gKalmanFilter.F[STATE_RY][STATE_RY] = (real)1.0; + gKalmanFilter.F[STATE_RZ][STATE_RZ] = (real)1.0; + + gKalmanFilter.F[STATE_VX][STATE_VX] = (real)1.0; + gKalmanFilter.F[STATE_VY][STATE_VY] = (real)1.0; + gKalmanFilter.F[STATE_VZ][STATE_VZ] = (real)1.0; + + gKalmanFilter.F[STATE_Q0][STATE_Q0] = (real)1.0; + gKalmanFilter.F[STATE_Q1][STATE_Q1] = (real)1.0; + gKalmanFilter.F[STATE_Q2][STATE_Q2] = (real)1.0; + gKalmanFilter.F[STATE_Q3][STATE_Q3] = (real)1.0; + + gKalmanFilter.F[STATE_WBX][STATE_WBX] = (real)1.0; + gKalmanFilter.F[STATE_WBY][STATE_WBY] = (real)1.0; + gKalmanFilter.F[STATE_WBZ][STATE_WBZ] = (real)1.0; + + gKalmanFilter.F[STATE_ABX][STATE_ABX] = (real)1.0; + gKalmanFilter.F[STATE_ABY][STATE_ABY] = (real)1.0; + gKalmanFilter.F[STATE_ABZ][STATE_ABZ] = (real)1.0; +} + + +// _UpdateProcessJacobian.m: Update the elements of F that change with each time-step +static void _UpdateProcessJacobian(void) +{ + real q0aXdT, q1aXdT, q2aXdT, q3aXdT; + real q0aYdT, q1aYdT, q2aYdT, q3aYdT; + real q0aZdT, q1aZdT, q2aZdT, q3aZdT; + real q0DtOver2, q1DtOver2, q2DtOver2, q3DtOver2; + + // ---------- Rows corresponding to POSITION ---------- + // No updates + + // ---------- Rows corresponding to VELOCITY ---------- + // Columns corresponding to the attitude-quaternion states + q0aXdT = gKalmanFilter.quaternion_Past[Q0] * gKalmanFilter.correctedAccel_B[X_AXIS] * gAlgorithm.dt; + q1aXdT = gKalmanFilter.quaternion_Past[Q1] * gKalmanFilter.correctedAccel_B[X_AXIS] * gAlgorithm.dt; + q2aXdT = gKalmanFilter.quaternion_Past[Q2] * gKalmanFilter.correctedAccel_B[X_AXIS] * gAlgorithm.dt; + q3aXdT = gKalmanFilter.quaternion_Past[Q3] * gKalmanFilter.correctedAccel_B[X_AXIS] * gAlgorithm.dt; + + q0aYdT = gKalmanFilter.quaternion_Past[Q0] * gKalmanFilter.correctedAccel_B[Y_AXIS] * gAlgorithm.dt; + q1aYdT = gKalmanFilter.quaternion_Past[Q1] * gKalmanFilter.correctedAccel_B[Y_AXIS] * gAlgorithm.dt; + q2aYdT = gKalmanFilter.quaternion_Past[Q2] * gKalmanFilter.correctedAccel_B[Y_AXIS] * gAlgorithm.dt; + q3aYdT = gKalmanFilter.quaternion_Past[Q3] * gKalmanFilter.correctedAccel_B[Y_AXIS] * gAlgorithm.dt; + + q0aZdT = gKalmanFilter.quaternion_Past[Q0] * gKalmanFilter.correctedAccel_B[Z_AXIS] * gAlgorithm.dt; + q1aZdT = gKalmanFilter.quaternion_Past[Q1] * gKalmanFilter.correctedAccel_B[Z_AXIS] * gAlgorithm.dt; + q2aZdT = gKalmanFilter.quaternion_Past[Q2] * gKalmanFilter.correctedAccel_B[Z_AXIS] * gAlgorithm.dt; + q3aZdT = gKalmanFilter.quaternion_Past[Q3] * gKalmanFilter.correctedAccel_B[Z_AXIS] * gAlgorithm.dt; + +#if 1 + // mod, DXG + gKalmanFilter.F[STATE_VX][STATE_Q0] = (real)2.0 * ( q0aXdT - q3aYdT + q2aZdT - + gKalmanFilter.R_BinN[0][0]*q0aXdT - + gKalmanFilter.R_BinN[0][1]*q0aYdT - + gKalmanFilter.R_BinN[0][2]*q0aZdT); + gKalmanFilter.F[STATE_VX][STATE_Q1] = (real)2.0 * ( q1aXdT + q2aYdT + q3aZdT - + gKalmanFilter.R_BinN[0][0]*q1aXdT - + gKalmanFilter.R_BinN[0][1]*q1aYdT - + gKalmanFilter.R_BinN[0][2]*q1aZdT); + gKalmanFilter.F[STATE_VX][STATE_Q2] = (real)2.0 * ( -q2aXdT + q1aYdT + q0aZdT - + gKalmanFilter.R_BinN[0][0]*q2aXdT - + gKalmanFilter.R_BinN[0][1]*q2aYdT - + gKalmanFilter.R_BinN[0][2]*q2aZdT); + gKalmanFilter.F[STATE_VX][STATE_Q3] = (real)2.0 * ( -q3aXdT - q0aYdT + q1aZdT - + gKalmanFilter.R_BinN[0][0]*q3aXdT - + gKalmanFilter.R_BinN[0][1]*q3aYdT - + gKalmanFilter.R_BinN[0][2]*q3aZdT); + + gKalmanFilter.F[STATE_VY][STATE_Q0] = (real)2.0 * ( q3aXdT + q0aYdT - q1aZdT - + gKalmanFilter.R_BinN[1][0]*q0aXdT - + gKalmanFilter.R_BinN[1][1]*q0aYdT - + gKalmanFilter.R_BinN[1][2]*q0aZdT); + gKalmanFilter.F[STATE_VY][STATE_Q1] = (real)2.0 * ( q2aXdT - q1aYdT - q0aZdT - + gKalmanFilter.R_BinN[1][0]*q1aXdT - + gKalmanFilter.R_BinN[1][1]*q1aYdT - + gKalmanFilter.R_BinN[1][2]*q1aZdT); + gKalmanFilter.F[STATE_VY][STATE_Q2] = (real)2.0 * ( q1aXdT + q2aYdT + q3aZdT - + gKalmanFilter.R_BinN[1][0]*q2aXdT - + gKalmanFilter.R_BinN[1][1]*q2aYdT - + gKalmanFilter.R_BinN[1][2]*q2aZdT); + gKalmanFilter.F[STATE_VY][STATE_Q3] = (real)2.0 * ( q0aXdT - q3aYdT + q2aZdT - + gKalmanFilter.R_BinN[1][0]*q3aXdT - + gKalmanFilter.R_BinN[1][1]*q3aYdT - + gKalmanFilter.R_BinN[1][2]*q3aZdT); + + gKalmanFilter.F[STATE_VZ][STATE_Q0] = (real)2.0 * ( -q2aXdT + q1aYdT + q0aZdT - + gKalmanFilter.R_BinN[2][0]*q0aXdT - + gKalmanFilter.R_BinN[2][1]*q0aYdT - + gKalmanFilter.R_BinN[2][2]*q0aZdT); + gKalmanFilter.F[STATE_VZ][STATE_Q1] = (real)2.0 * ( q3aXdT + q0aYdT - q1aZdT - + gKalmanFilter.R_BinN[2][0]*q1aXdT - + gKalmanFilter.R_BinN[2][1]*q1aYdT - + gKalmanFilter.R_BinN[2][2]*q1aZdT); + gKalmanFilter.F[STATE_VZ][STATE_Q2] = (real)2.0 * ( -q0aXdT + q3aYdT - q2aZdT - + gKalmanFilter.R_BinN[2][0]*q2aXdT - + gKalmanFilter.R_BinN[2][1]*q2aYdT - + gKalmanFilter.R_BinN[2][2]*q2aZdT); + gKalmanFilter.F[STATE_VZ][STATE_Q3] = (real)2.0 * ( q1aXdT + q2aYdT + q3aZdT - + gKalmanFilter.R_BinN[2][0]*q3aXdT - + gKalmanFilter.R_BinN[2][1]*q3aYdT - + gKalmanFilter.R_BinN[2][2]*q3aZdT); +#else + gKalmanFilter.F[STATE_VX][STATE_Q0] = (real)2.0 * (q0aXdT - q3aYdT + q2aZdT); + gKalmanFilter.F[STATE_VX][STATE_Q1] = (real)2.0 * (q1aXdT + q2aYdT + q3aZdT); + gKalmanFilter.F[STATE_VX][STATE_Q2] = (real)2.0 * (-q2aXdT + q1aYdT + q0aZdT); + gKalmanFilter.F[STATE_VX][STATE_Q3] = (real)2.0 * (-q3aXdT - q0aYdT + q1aZdT); + + gKalmanFilter.F[STATE_VY][STATE_Q0] = (real)2.0 * (q3aXdT + q0aYdT - q1aZdT); + gKalmanFilter.F[STATE_VY][STATE_Q1] = (real)2.0 * (q2aXdT - q1aYdT - q0aZdT); + gKalmanFilter.F[STATE_VY][STATE_Q2] = (real)2.0 * (q1aXdT + q2aYdT + q3aZdT); + gKalmanFilter.F[STATE_VY][STATE_Q3] = (real)2.0 * (q0aXdT - q3aYdT + q2aZdT); + + gKalmanFilter.F[STATE_VZ][STATE_Q0] = (real)2.0 * (-q2aXdT + q1aYdT + q0aZdT); + gKalmanFilter.F[STATE_VZ][STATE_Q1] = (real)2.0 * (q3aXdT + q0aYdT - q1aZdT); + gKalmanFilter.F[STATE_VZ][STATE_Q2] = (real)2.0 * (-q0aXdT + q3aYdT - q2aZdT); + gKalmanFilter.F[STATE_VZ][STATE_Q3] = (real)2.0 * (q1aXdT + q2aYdT + q3aZdT); +#endif + + // Columns corresponding to the acceleration-bias state (-R_BinN*DT) + gKalmanFilter.F[STATE_VX][STATE_ABX] = -gKalmanFilter.R_BinN[0][0] * gAlgorithm.dt; + gKalmanFilter.F[STATE_VX][STATE_ABY] = -gKalmanFilter.R_BinN[0][1] * gAlgorithm.dt; + gKalmanFilter.F[STATE_VX][STATE_ABZ] = -gKalmanFilter.R_BinN[0][2] * gAlgorithm.dt; + + gKalmanFilter.F[STATE_VY][STATE_ABX] = -gKalmanFilter.R_BinN[1][0] * gAlgorithm.dt; + gKalmanFilter.F[STATE_VY][STATE_ABY] = -gKalmanFilter.R_BinN[1][1] * gAlgorithm.dt; + gKalmanFilter.F[STATE_VY][STATE_ABZ] = -gKalmanFilter.R_BinN[1][2] * gAlgorithm.dt; + + gKalmanFilter.F[STATE_VZ][STATE_ABX] = -gKalmanFilter.R_BinN[2][0] * gAlgorithm.dt; + gKalmanFilter.F[STATE_VZ][STATE_ABY] = -gKalmanFilter.R_BinN[2][1] * gAlgorithm.dt; + gKalmanFilter.F[STATE_VZ][STATE_ABZ] = -gKalmanFilter.R_BinN[2][2] * gAlgorithm.dt; + + // ---------- Rows corresponding to attitude-QUATERNION ---------- + // Columns corresponding to the attitude-quaternion state (0.5*Omega*DT) + //gKalmanFilter.F[STATE_Q0][STATE_Q0] = 0; + gKalmanFilter.F[STATE_Q0][STATE_Q1] = -gKalmanFilter.wTrueTimesDtOverTwo[X_AXIS]; + gKalmanFilter.F[STATE_Q0][STATE_Q2] = -gKalmanFilter.wTrueTimesDtOverTwo[Y_AXIS]; + gKalmanFilter.F[STATE_Q0][STATE_Q3] = -gKalmanFilter.wTrueTimesDtOverTwo[Z_AXIS]; + + gKalmanFilter.F[STATE_Q1][STATE_Q0] = gKalmanFilter.wTrueTimesDtOverTwo[X_AXIS]; + //gKalmanFilter.F[STATE_Q1][STATE_Q1] = 0; + gKalmanFilter.F[STATE_Q1][STATE_Q2] = gKalmanFilter.wTrueTimesDtOverTwo[Z_AXIS]; + gKalmanFilter.F[STATE_Q1][STATE_Q3] = -gKalmanFilter.wTrueTimesDtOverTwo[Y_AXIS]; + + gKalmanFilter.F[STATE_Q2][STATE_Q0] = gKalmanFilter.wTrueTimesDtOverTwo[Y_AXIS]; + gKalmanFilter.F[STATE_Q2][STATE_Q1] = -gKalmanFilter.wTrueTimesDtOverTwo[Z_AXIS]; + //gKalmanFilter.F[STATE_Q2][STATE_Q2] = 0; + gKalmanFilter.F[STATE_Q2][STATE_Q3] = gKalmanFilter.wTrueTimesDtOverTwo[X_AXIS]; + + gKalmanFilter.F[STATE_Q3][STATE_Q0] = gKalmanFilter.wTrueTimesDtOverTwo[Z_AXIS]; + gKalmanFilter.F[STATE_Q3][STATE_Q1] = gKalmanFilter.wTrueTimesDtOverTwo[Y_AXIS]; + gKalmanFilter.F[STATE_Q3][STATE_Q2] = -gKalmanFilter.wTrueTimesDtOverTwo[X_AXIS]; + //gKalmanFilter.F[STATE_Q3,STATE.Q3] = 0; + + // Columns corresponding to the rate-bias state (-0.5*Xi*DT) + q0DtOver2 = gKalmanFilter.quaternion_Past[Q0] * gAlgorithm.dtOverTwo; + q1DtOver2 = gKalmanFilter.quaternion_Past[Q1] * gAlgorithm.dtOverTwo; + q2DtOver2 = gKalmanFilter.quaternion_Past[Q2] * gAlgorithm.dtOverTwo; + q3DtOver2 = gKalmanFilter.quaternion_Past[Q3] * gAlgorithm.dtOverTwo; + + // + gKalmanFilter.F[STATE_Q0][STATE_WBX] = q1DtOver2; + gKalmanFilter.F[STATE_Q0][STATE_WBY] = q2DtOver2; + gKalmanFilter.F[STATE_Q0][STATE_WBZ] = q3DtOver2; + + gKalmanFilter.F[STATE_Q1][STATE_WBX] = -q0DtOver2; + gKalmanFilter.F[STATE_Q1][STATE_WBY] = q3DtOver2; + gKalmanFilter.F[STATE_Q1][STATE_WBZ] = -q2DtOver2; + + gKalmanFilter.F[STATE_Q2][STATE_WBX] = -q3DtOver2; + gKalmanFilter.F[STATE_Q2][STATE_WBY] = -q0DtOver2; + gKalmanFilter.F[STATE_Q2][STATE_WBZ] = q1DtOver2; + + gKalmanFilter.F[STATE_Q3][STATE_WBX] = q2DtOver2; + gKalmanFilter.F[STATE_Q3][STATE_WBY] = -q1DtOver2; + gKalmanFilter.F[STATE_Q3][STATE_WBZ] = -q0DtOver2; + + // ---------- Rows corresponding to RATE-BIAS ---------- + // All zeros + + // ---------- Rows corresponding to ACCELERATION-BIAS ---------- + // All zeros +} + +// +static void _UpdateProcessCovariance(void) +{ + // Variables used to initially populate the Q-matrix + real biSq[3] = {(real)1.0e-10, (real)1.0e-10, (real)1.0e-10}; + real sigDriftDot; + + // Variables used to populate the Q-matrix each time-step + static real multiplier_Q, multiplier_Q_Sq; + + static BOOL initQ_HG = TRUE; + static BOOL initQ_LG = TRUE; + + // Only need to generate Q-Bias values once + if (initQ_HG == TRUE) + { + initQ_HG = FALSE; + + // squated gyro bias instability + biSq[X_AXIS] = gAlgorithm.imuSpec.biW * gAlgorithm.imuSpec.biW; + 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 + */ + sigDriftDot = (real)9.064720283654388 / gAlgorithm.imuSpec.arw; + + // Rate-bias terms (Q is ultimately the squared value, which is done in the second line of the assignment) + gKalmanFilter.Q[STATE_WBX] = sigDriftDot * biSq[X_AXIS] * gAlgorithm.dt; + gKalmanFilter.Q[STATE_WBX] = gKalmanFilter.Q[STATE_WBX] * gKalmanFilter.Q[STATE_WBX]; + + gKalmanFilter.Q[STATE_WBY] = gKalmanFilter.Q[STATE_WBX]; + + gKalmanFilter.Q[STATE_WBZ] = sigDriftDot * biSq[Z_AXIS] * gAlgorithm.dt; + gKalmanFilter.Q[STATE_WBZ] = gKalmanFilter.Q[STATE_WBZ] * gKalmanFilter.Q[STATE_WBZ]; + + // 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 ) + * mult = 0.5 * dt * sigRate + * = 0.5 * sqrt(dt) * sqrt(dt) * ( ARW / sqrt(dt) ) + * = 0.5 * sqrt(dt) * ARW + */ + multiplier_Q = (real)(0.5) * gAlgorithm.sqrtDt * gAlgorithm.imuSpec.arw; + multiplier_Q_Sq = multiplier_Q * multiplier_Q; + } + + /* Attempt to solve the rate-bias problem: Decrease the process covariance, + * Q, upon transition to low-gain mode to limit the change in the rate-bias + * estimate. + */ + if( initQ_LG == TRUE ) + { + if( gAlgorithm.state == LOW_GAIN_AHRS ) + { + initQ_LG = FALSE; + + /* After running drive-test data through the AHRS simulation, + * reducing Q by 1000x reduced the changeability of the rate-bias + * estimate (the estimate was less affected by errors). This + * seems to have improved the solution as much of the errors + * was due to rapid changes in the rate-bias estimate. This + * seemed to result in a better than nominal solution (for the + * 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];*/ + } + } + + /* Update the elements of the process covariance matrix, Q, that change + * with each time-step (the elements that correspond to the quaternion- + * block of the Q-matrix). The rest of the elements in the matrix are set + * during the transition into and between EKF states (high-gain, low-gain, + * etc) or above (upon first entry into this function). + * The process cov matrix of quaternion is + * [1-q0*q0 -q0*q1 -q0*q2 -q0*q3; + * -q0*q1 1-q1*q1 -q1*q2 -q1*q3; + * -q0*q2 -q1*q2 1-q2*q2 -q2*q3; + * -q0*q3 -q1*q3 -q2*q3 1-q3*q3] * (0.5*dt*sigma_gyro)^2 + * The eigenvalue of the matrix is [1 1 1 1-q0^2-q1^2-q2^2-q3^2], which means it + * is not positive defintie when quaternion norm is above or equal to 1. Quaternion + * norm can be above 1 due to numerical accuray. A scale factor 0.99 is added here to + * make sure the positive definiteness of the covariance matrix. The eigenvalues now + * are [1 1 1 1-0.99*(q0^2+q1^2+q2^2+q3^2)]. Even if there is numerical accuracy issue, + * the cov matrix is still positive definite. + */ + real q0q0 = gKalmanFilter.quaternion_Past[Q0] * gKalmanFilter.quaternion_Past[Q0] * 0.99f; + real q0q1 = gKalmanFilter.quaternion_Past[Q0] * gKalmanFilter.quaternion_Past[Q1] * 0.99f; + real q0q2 = gKalmanFilter.quaternion_Past[Q0] * gKalmanFilter.quaternion_Past[Q2] * 0.99f; + real q0q3 = gKalmanFilter.quaternion_Past[Q0] * gKalmanFilter.quaternion_Past[Q3] * 0.99f; + + real q1q1 = gKalmanFilter.quaternion_Past[Q1] * gKalmanFilter.quaternion_Past[Q1] * 0.99f; + real q1q2 = gKalmanFilter.quaternion_Past[Q1] * gKalmanFilter.quaternion_Past[Q2] * 0.99f; + real q1q3 = gKalmanFilter.quaternion_Past[Q1] * gKalmanFilter.quaternion_Past[Q3] * 0.99f; + + real q2q2 = gKalmanFilter.quaternion_Past[Q2] * gKalmanFilter.quaternion_Past[Q2] * 0.99f; + real q2q3 = gKalmanFilter.quaternion_Past[Q2] * gKalmanFilter.quaternion_Past[Q3] * 0.99f; + + real q3q3 = gKalmanFilter.quaternion_Past[Q3] * gKalmanFilter.quaternion_Past[Q3] * 0.99f; + + // Note: this block of the covariance matrix is symmetric + real tmpQMultiplier = multiplier_Q_Sq; + /* Only considering gyro noise can underestimate the cov of the quaternion. + * A scale factor 100 is added here. This is mainly for faster convergence + * of the heading angle in the INS solution. + */ + if (gAlgorithm.state == INS_SOLUTION) + { + tmpQMultiplier = 1.0f * multiplier_Q_Sq; + } + gKalmanFilter.Q[STATE_Q0] = ((real)1.0 - q0q0) * tmpQMultiplier; + gKalmanFilter.Qq[0] = (-q0q1) * tmpQMultiplier; + gKalmanFilter.Qq[1] = (-q0q2) * tmpQMultiplier; + gKalmanFilter.Qq[2] = (-q0q3) * tmpQMultiplier; + + gKalmanFilter.Q[STATE_Q1] = ((real)1.0 - q1q1) * tmpQMultiplier; + gKalmanFilter.Qq[3] = (-q1q2) * tmpQMultiplier; + gKalmanFilter.Qq[4] = (-q1q3) * tmpQMultiplier; + + gKalmanFilter.Q[STATE_Q2] = ((real)1.0 - q2q2) * tmpQMultiplier; + gKalmanFilter.Qq[5] = (-q2q3) * tmpQMultiplier; + + gKalmanFilter.Q[STATE_Q3] = ((real)1.0 - q3q3) * tmpQMultiplier; +} + + +// +// GenerateProcessCovarMatrix.m +void GenerateProcessCovariance(void) +{ + // Initialize the Process Covariance (Q) matrix with values that do not change + memset(gKalmanFilter.Q, 0, sizeof(gKalmanFilter.Q)); + + /* THE FOLLOWING COVARIANCE VALUES AREN'T CORRECT, JUST SELECTED SO THE + * PROGRAM COULD RUN + */ + + // Acceleration based values + real dtSigAccelSq = (real)(gAlgorithm.dt * gAlgorithm.imuSpec.sigmaA); + dtSigAccelSq = dtSigAccelSq * dtSigAccelSq; + + // Position + gKalmanFilter.Q[STATE_RX] = gAlgorithm.dtSquared * dtSigAccelSq; + gKalmanFilter.Q[STATE_RY] = gAlgorithm.dtSquared * dtSigAccelSq; + gKalmanFilter.Q[STATE_RZ] = gAlgorithm.dtSquared * dtSigAccelSq; + + /* Velocity, todo. 100 is to take under-estimated accel bias, gyro bias and + * attitude error since none of them is Gaussian. Non-Gaussian error produces + * velocity drift. High-freq vibration can also be handled by this. + */ + gKalmanFilter.Q[STATE_VX] = 100 * dtSigAccelSq;//(real)1e-10; + gKalmanFilter.Q[STATE_VY] = 100 * dtSigAccelSq; + gKalmanFilter.Q[STATE_VZ] = 100 * dtSigAccelSq; +} + +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 + * @brief floating point version + * TRACE: + * @param [out] - output pointer to the filtered value + * @param [in] - input pointer to a new raw value + * @retval N/A + * @details This is a replacement for 'lowPass2Pole' found in the 440 code. + * Note, the implementation in the 440 SW is not a second-order filter + * but is an implementation of a first-order filter. + ******************************************************************************/ +void _FirstOrderLowPass( real *output, // <-- INITIALLY THIS IS OUTPUT PAST (FILTERED VALUED IS RETURNED HERE) + real input ) // <-- CURRENT VALUE OF SIGNAL TO BE FILTERED +{ + static real inputPast; + + // 0.25 Hz LPF + if( gAlgorithm.callingFreq == FREQ_100_HZ ) + { + *output = (real)(0.984414127416097) * (*output) + + (real)(0.007792936291952) * (input + inputPast); + } + else + { + *output = (real)(0.992176700177507) * (*output) + + (real)(0.003911649911247) * (input + inputPast); + } + + inputPast = input; +} diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/SelectState.c b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/SelectState.c new file mode 100644 index 0000000..0fe20ee --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/SelectState.c @@ -0,0 +1,582 @@ +/* + * File: SelectState.c + * Author: joemotyka + * + * Created on May 8, 2016, 12:35 AM + */ + +#include +#include // memset +#include // fabs + +#include "GlobalConstants.h" +#include "platformAPI.h" +#include "TimingVars.h" + +#include "algorithm.h" // gAlgorithm +#include "algorithmAPI.h" +#include "Indices.h" // IND +#include "StateIndices.h" // STATE_IND +#include "AlgorithmLimits.h" // LIMIT +#include "VectorMath.h" // VectorNormalize +#include "QuaternionMath.h" // EulerAnglesToQuaternion, QuaternionToEulerAngles +#include "TransformationMath.h" // FieldVectorsToEulerAngles +#include "EKF_Algorithm.h" +#include "PredictFunctions.h" + +#ifndef INS_OFFLINE +#ifdef DISPLAY_DIAGNOSTIC_MSG +#include "debug.h" +#endif +#endif + + +static real accumulatedAccelVector[3]; +static real accumulatedGyroVector[3]; +static real accumulatedMagVector[3]; +static real averagedAccelVector[3]; +static real averagedGyroVector[3]; +static real averagedMagVector[3]; + +// Local functions +static BOOL _AccumulateFieldVectors(void); +static BOOL _AverageFieldVectors(uint16_t pointsToAverage); + +static void _DropToHighGainAHRS(void); +static void _ResetAlgorithm(void); + +// StabilizeSystem: Run for a prescribed period to let the sensors settle. +void StabilizeSystem(void) +{ + // Decrement timer (initial value is set based on the calling frequency of + // the EKF) + gAlgorithm.stateTimer = gAlgorithm.stateTimer - 1; + + /* Upon timeout prepare for transition to the next stage of the EKF + * (initialization) by resetting the state and state-timer and + * initializing the accumulation vectors. + */ + if (gAlgorithm.stateTimer == 0) { + #ifdef INS_OFFLINE + printf("To ini att. %u\n", gEKFInput.itow); + #else +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("To ini att. "); + DebugPrintInt("", gEKFInput.itow); + DebugPrintEndline(); +#endif + #endif + // Set new state and timer + gAlgorithm.state = INITIALIZE_ATTITUDE; + gAlgorithm.stateTimer = gAlgorithm.Duration.Initialize_Attitude; + + // Initialize the vectors + accumulatedAccelVector[X_AXIS] = (real)0.0; + accumulatedAccelVector[Y_AXIS] = (real)0.0; + accumulatedAccelVector[Z_AXIS] = (real)0.0; + + accumulatedGyroVector[X_AXIS] = (real)0.0; + accumulatedGyroVector[Y_AXIS] = (real)0.0; + accumulatedGyroVector[Z_AXIS] = (real)0.0; + + accumulatedMagVector[X_AXIS] = (real)0.0; + accumulatedMagVector[Y_AXIS] = (real)0.0; + accumulatedMagVector[Z_AXIS] = (real)0.0; + +#ifdef DISPLAY_DIAGNOSTIC_MSG + TimingVars_DiagnosticMsg("Transitioning to initialization mode"); +#endif + } + + // Set the bit to indicate initialization + gAlgoStatus.bit.algorithmInit = TRUE; +} + + +/* InitializeAttitude: Initialize the algorithm by collecting sensor data for + * a prescribed period and averaging it. + */ +void InitializeAttitude(void) +{ + // Decrement timer + gAlgorithm.stateTimer = gAlgorithm.stateTimer - 1; + + /* Sum the acceleration and magnetic-field vectors (from the end of the + * initialization stage) + */ + _AccumulateFieldVectors(); + + /* Quasi-static check: check for motion over threshold. If detected, reset + * the accumulation variables and restart initialization phase. + */ + if ((fabs(gEKFInput.angRate_B[X_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE) || + (fabs(gEKFInput.angRate_B[Y_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE) || + (fabs(gEKFInput.angRate_B[Z_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE)) + { + accumulatedAccelVector[X_AXIS] = (real)0.0; + accumulatedAccelVector[Y_AXIS] = (real)0.0; + accumulatedAccelVector[Z_AXIS] = (real)0.0; + + accumulatedGyroVector[X_AXIS] = (real)0.0; + accumulatedGyroVector[Y_AXIS] = (real)0.0; + accumulatedGyroVector[Z_AXIS] = (real)0.0; + + accumulatedMagVector[X_AXIS] = (real)0.0; + accumulatedMagVector[Y_AXIS] = (real)0.0; + accumulatedMagVector[Z_AXIS] = (real)0.0; + + gAlgorithm.stateTimer = gAlgorithm.Duration.Initialize_Attitude; + } + + /* Timeout... Prepare for the transition to the next stage of the EKF + * (High-Gain AHRS) then determine the system's Initial Conditions by + * averaging the accumulated vectors. + */ + if (gAlgorithm.stateTimer == 0) { + #ifdef INS_OFFLINE + printf("To HG. %u\n", gEKFInput.itow); + #else +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("To HG. "); + DebugPrintInt("", gEKFInput.itow); + DebugPrintEndline(); +#endif + #endif +#ifdef DISPLAY_DIAGNOSTIC_MSG + if (magUsedInAlgorithm()) { + TimingVars_DiagnosticMsg("Transitioning to high-gain AHRS mode"); + } else { + TimingVars_DiagnosticMsg("Transitioning to high-gain VG mode"); + } +#endif + + // Set new state and timer + gAlgorithm.state = HIGH_GAIN_AHRS; + gAlgorithm.stateTimer = gAlgorithm.Duration.High_Gain_AHRS; + + /* Average acceleration and magnetic field-vectors to determine the + * initial attitude of the system. + */ + _AverageFieldVectors(gAlgorithm.Duration.Initialize_Attitude); + + /* Compute the measured Euler Angles and associated quaternion + * Euler angles are computed from averaged field vectors + * (correct for hard/soft-iron effects) + */ + // Unit gravity vector in the body frame + real unitGravityVector[3] = {0.0f}; + UnitGravity( averagedAccelVector, unitGravityVector ); + // Roll and pitch + UnitGravityToEulerAngles( unitGravityVector, gKalmanFilter.measuredEulerAngles ); + // Yaw + if ( magUsedInAlgorithm() ) + { + gKalmanFilter.measuredEulerAngles[YAW] = UnitGravityAndMagToYaw( unitGravityVector, + averagedMagVector ); + } + else + { + gKalmanFilter.measuredEulerAngles[YAW] = 0.0f; // start from 0 + } + + /* Initial attitude quaternion is generated using Euler angles from + * averaged gravity and magnetic fields. (DEBUG: This is used to + * initialize the EKF state) + */ + EulerAnglesToQuaternion( gKalmanFilter.measuredEulerAngles, + gKalmanFilter.quaternion ); + gKalmanFilter.quaternion_Past[0] = gKalmanFilter.quaternion[0]; + gKalmanFilter.quaternion_Past[1] = gKalmanFilter.quaternion[1]; + gKalmanFilter.quaternion_Past[2] = gKalmanFilter.quaternion[2]; + gKalmanFilter.quaternion_Past[3] = gKalmanFilter.quaternion[3]; + // Euler angles from the initial measurement (DEBUG: initial output of the system) + gKalmanFilter.eulerAngles[ROLL] = gKalmanFilter.measuredEulerAngles[ROLL]; + gKalmanFilter.eulerAngles[PITCH] = gKalmanFilter.measuredEulerAngles[PITCH]; + gKalmanFilter.eulerAngles[YAW] = gKalmanFilter.measuredEulerAngles[YAW]; + + // Initialize the Kalman filter variables + _ResetAlgorithm(); + + // Set linear-acceleration switch variables + gAlgorithm.linAccelSwitchCntr = 0; + + /// Update the system status + gAlgoStatus.bit.algorithmInit = FALSE; + gAlgoStatus.bit.highGain = TRUE; + gAlgoStatus.bit.attitudeOnlyAlgorithm = TRUE; + } +} + + +/* HG_To_LG_Transition_Test: Transition from high-gain to low-gain. Only check + * is that the bias isn't greater than 10 deg/sec (this is probably not a good check). + */ +void HG_To_LG_Transition_Test(void) +{ + /* Decrement timer if 'dynamicMotion' TRUE (setting FALSE will cause the + * system to revert to high - gain mode once out of high - gain mode -- need + * to set flag high to transition out of high - gain mode once this is done) + */ + /* dynamic-motion flag switch from high-gain to low-gain AHRS. if not set + * timer will not decrement the transition to LG AHRS will not occur. + * set at system configuration or (Nav-View) interface + */ + if (gAlgorithm.Behavior.bit.dynamicMotion) { + gAlgorithm.stateTimer--; + } + + /* Startup check (if the estimated bias is large the software never + * transitions to the LG AHRS mode. NOTE: this seems incorrect, instead + * the SW should check if the bias has converged, not if it is above a + * threshold -- it is possible that the system could have a large bias.) + * However, this seems wrong too + */ + if ((fabs(gKalmanFilter.rateBias_B[X_AXIS]) > TEN_DEGREES_IN_RAD) && + (fabs(gKalmanFilter.rateBias_B[Y_AXIS]) > TEN_DEGREES_IN_RAD) && + (fabs(gKalmanFilter.rateBias_B[Z_AXIS]) > TEN_DEGREES_IN_RAD)) + { + gAlgorithm.stateTimer = gAlgorithm.Duration.High_Gain_AHRS; + } + + /* Timeout... Prepare for the transition to the next stage of the EKF + * (Low-Gain AHRS) and populate the values in Q that do not change with + * each iteration. + */ + if (gAlgorithm.stateTimer == 0) { + #ifdef INS_OFFLINE + printf("To LG. %u\n", gEKFInput.itow); + #else +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("To LG. "); + DebugPrintInt("", gEKFInput.itow); + DebugPrintEndline(); +#endif + #endif +#ifdef DISPLAY_DIAGNOSTIC_MSG + if (magUsedInAlgorithm()) { + TimingVars_DiagnosticMsg("Transitioning to low-gain AHRS mode"); + } else { + TimingVars_DiagnosticMsg("Transitioning to low-gain VG mode"); + } +#endif + + gAlgorithm.state = LOW_GAIN_AHRS; + gAlgorithm.stateTimer = gAlgorithm.Duration.Low_Gain_AHRS; + + gAlgoStatus.bit.highGain = FALSE; + } +} + + +/* This logic is only called upon transition to INS from LG_AHRS then it is + * not called unless the algorithm reverts back to HG_AHRS, which will + * cause the system to pass through LG_AHRS on its way to INS. + */ +void LG_To_INS_Transition_Test(void) +{ +#ifdef DISPLAY_DIAGNOSTIC_MSG + // Display the diagnostic message once upon transition (DEBUG: Remove in firmware) + static int oneTime = TRUE; +#endif + + if (gAlgorithm.stateTimer > 0) { + // Stay in LG mode until timeout occurs then begin check for INS transition + gAlgorithm.stateTimer = gAlgorithm.stateTimer - 1; + } else { + // Upon timeout, begin check for INS transition (remove msg in firmware) +#ifdef DISPLAY_DIAGNOSTIC_MSG + if (oneTime) { + TimingVars_DiagnosticMsg("Begin check for INS transition"); + oneTime = FALSE; + } +#endif + + /* If GPS output is valid (GPS providing data with a good signal lock) + * then transit to INS mode. + */ + if ( gpsUsedInAlgorithm() && gEKFInput.gpsUpdate && gEKFInput.gpsFixType ) + { + #ifdef INS_OFFLINE + printf("To INS. %u\n", gEKFInput.itow); + #else +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("To INS. "); + DebugPrintInt("", gEKFInput.itow); + DebugPrintEndline(); +#endif + #endif +#ifdef DISPLAY_DIAGNOSTIC_MSG + TimingVars_DiagnosticMsg("Transitioning to INS mode"); +#endif + + // Transit to INS solution + gAlgorithm.state = INS_SOLUTION; + + gAlgoStatus.bit.attitudeOnlyAlgorithm = FALSE; + + // Initialize the algorithm with GNSS position and lever arm + InitINSFilter(); + + // Set linear-acceleration switch variables + gAlgorithm.linAccelSwitchCntr = 0; + } + } +} + +/* INS_To_AHRS_Transition_Test: Drop back to LG AHRS operation if... + * 1) GPS drops out for more than 3 seconds + * 2) magnetometer data not available AND at rest too long + * 3) magnetic alignment being performed + */ +void INS_To_AHRS_Transition_Test(void) +{ + // Record last GPS velocity large enough to give a good heading measurement + if (gEKFInput.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING) + { + gAlgorithm.timeOfLastSufficientGPSVelocity = (int32_t)gEKFInput.itow; + } + /* Determine the length of time it has been since the system 'moved' -- + * only linear motion considered (rotations ignored). + */ + int32_t timeSinceRestBegan = (int32_t)gEKFInput.itow - gAlgorithm.timeOfLastSufficientGPSVelocity; + if (timeSinceRestBegan < 0) + { + timeSinceRestBegan = timeSinceRestBegan + MAX_ITOW; + } + if (timeSinceRestBegan > LIMIT_MAX_REST_TIME_BEFORE_HEADING_INVALID && gAlgorithm.headingIni != HEADING_UNINITIALIZED) + { + gAlgorithm.headingIni = HEADING_GNSS_LOW; +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("Rest for too long."); + DebugPrintEndline(); +#endif + } + + // compute time since the last good GPS reading + int32_t timeSinceLastGoodGPSReading = (int32_t)gAlgorithm.itow - gAlgorithm.timeOfLastGoodGPSReading; + if (timeSinceLastGoodGPSReading < 0) { + timeSinceLastGoodGPSReading = timeSinceLastGoodGPSReading + MAX_ITOW; + } + + if ( timeSinceLastGoodGPSReading > gAlgorithm.Limit.maxGpsDropTime ) + { +#ifdef INS_OFFLINE + printf("GPS outage too long\n"); +#endif // INS_OFFLINE + + // Currently in INS mode but requiring a transition to AHRS / VG + gAlgorithm.insFirstTime = TRUE; + gAlgorithm.headingIni = HEADING_UNINITIALIZED; + + /* The transition from INS to AHRS and back to INS does not seem to + * generate a stable solution if we transition to LG AHRS for only 30 + * seconds.The interval needs to be longer(~1min).However, to + * mitigate any unforseen issues with the transition, HG AHRS with the + * nominal timing(1 min in HG, 30 seconds in LG) will be selected. + */ + gAlgorithm.state = LOW_GAIN_AHRS; // HIGH_GAIN_AHRS; + gAlgorithm.stateTimer = gAlgorithm.Duration.Low_Gain_AHRS; // gAlgorithm.Duration.High_Gain_AHRS; + + // Set linear-acceleration switch variables + gAlgorithm.linAccelSwitchCntr = 0; + +#ifdef DISPLAY_DIAGNOSTIC_MSG + if (magUsedInAlgorithm()) { + TimingVars_DiagnosticMsg("Transitioning to low-gain AHRS mode"); + } else { + TimingVars_DiagnosticMsg("Transitioning to low-gain VG mode"); + } +#endif + + gAlgoStatus.bit.highGain = ( gAlgorithm.state == HIGH_GAIN_AHRS ); + gAlgoStatus.bit.attitudeOnlyAlgorithm = TRUE; + } +} + + +/* Dynamic motion logic: + * 0) When dynamicMotion is FALSE, remain in high-gain AHRS (do not decrement + * counter in 'HG_To_LG_Transition_Test') + * 1) If dynamicMotion is selected then proceed to other filter states upon + * timeout (else, stay in HG mode) + * 2) When in LG or INS mode... if dynamicMotion is set FALSE then transition + * to HG AHRS + * 3) Once dynamicMotion is reset TRUE (by user), the system should begin + * transition to LG AHRS as if beginning from nominal startup + */ +void DynamicMotion(void) +{ + static BOOL enterStatic = FALSE; // should this be true or false? + + /* If dynamicMotion is FALSE then transition to high-gain AHRS. The + * system stays in HG until dynamicMotion is set high. + */ + if (gAlgorithm.state > HIGH_GAIN_AHRS) { + if (gAlgorithm.Behavior.bit.dynamicMotion) { + enterStatic = FALSE; + } else { + if (enterStatic == FALSE) { + enterStatic = TRUE; + _DropToHighGainAHRS(); + +#ifdef DISPLAY_DIAGNOSTIC_MSG + /* Question: what if DM flag is set (by user) at the moment of + * transition? Should the FW initialize enterStatic + * in HG_To_LG_Transition_Test? + */ + TimingVars_DiagnosticMsg("Transitioning to High-Gain AHRS -- dynamic-motion flag set"); +#endif + } + } + } +} + + +// +static void _DropToHighGainAHRS(void) +{ + gAlgorithm.state = HIGH_GAIN_AHRS; + gAlgorithm.stateTimer = gAlgorithm.Duration.High_Gain_AHRS; + + gAlgoStatus.bit.highGain = TRUE; + gAlgoStatus.bit.attitudeOnlyAlgorithm = TRUE; + + // Reset flag in case drop is from INS + gAlgorithm.insFirstTime = TRUE; +} + + +/****************************************************************************** +* @name: _AccumulateFieldVectors Update the running sum of the acceleration and +* @brief magnetic field vectors (the accumulation variables are 64-bits long). +* The total is averaged to form the system ICs. +* +* @brief called in main.cpp and processUpdateAlgorithm() in algorithm.cpp +* TRACE: +* +* @param N/A +* @retval 1 if magnetometers are used, otherwise it returns a zero. +******************************************************************************/ +static BOOL _AccumulateFieldVectors(void) +{ + // Accumulate the acceleration vector readings (accels in g's) + accumulatedAccelVector[X_AXIS] += (real)gEKFInput.accel_B[X_AXIS]; + accumulatedAccelVector[Y_AXIS] += (real)gEKFInput.accel_B[Y_AXIS]; + accumulatedAccelVector[Z_AXIS] += (real)gEKFInput.accel_B[Z_AXIS]; + + // Accumulate the gyroscope vector readings (accels in rad/s) + accumulatedGyroVector[X_AXIS] += gEKFInput.angRate_B[X_AXIS]; + accumulatedGyroVector[Y_AXIS] += gEKFInput.angRate_B[Y_AXIS]; + accumulatedGyroVector[Z_AXIS] += gEKFInput.angRate_B[Z_AXIS]; + + // Accumulate the magnetic-field vector readings (or set to zero if the + // product does not have magnetometers) + if (magUsedInAlgorithm() ) + { + accumulatedMagVector[X_AXIS] += (real)gEKFInput.magField_B[X_AXIS]; + accumulatedMagVector[Y_AXIS] += (real)gEKFInput.magField_B[Y_AXIS]; + accumulatedMagVector[Z_AXIS] += (real)gEKFInput.magField_B[Z_AXIS]; + } else { + accumulatedMagVector[X_AXIS] = (real)0.0; + accumulatedMagVector[Y_AXIS] = (real)0.0; + accumulatedMagVector[Z_AXIS] = (real)0.0; + } + + return(magUsedInAlgorithm()); +} + + +/****************************************************************************** +* @name: _AverageFieldVectors Average the accumulated field vectors by shifting +* the sum to the right +* Note: the number of samples that are summed must be a multiple of 2: +* Number of points accumulated, N = 2^bitsToShift +* +* TRACE: +* +* @param [in] bitsToShift +* @brief global data structure changes: +* Input: gKalmanFilter.AccumulatedAccelVector +* gKalmanFilter.AccumulatedMagVector +* Output: gKalmanFilter.AveragedAccelVector +* gKalmanFilter.AveragedMagVector +* @retval 1 if magnetometers are used, otherwise it returns a zero. +******************************************************************************/ +static BOOL _AverageFieldVectors(uint16_t pointsToAverage) +{ + real mult = (real)(1.0 / (real)pointsToAverage); + + // Average the accumulated acceleration vector + averagedAccelVector[X_AXIS] = accumulatedAccelVector[X_AXIS] * mult; + averagedAccelVector[Y_AXIS] = accumulatedAccelVector[Y_AXIS] * mult; + averagedAccelVector[Z_AXIS] = accumulatedAccelVector[Z_AXIS] * mult; + + // Average the accumulated angular rate vector + averagedGyroVector[X_AXIS] = accumulatedGyroVector[X_AXIS] * mult; + averagedGyroVector[Y_AXIS] = accumulatedGyroVector[Y_AXIS] * mult; + averagedGyroVector[Z_AXIS] = accumulatedGyroVector[Z_AXIS] * mult; + + /* Average the accumulated magnetic-field vector (or set to zero if + * magnetometer is not in use.) + */ + if (magUsedInAlgorithm()) + { + averagedMagVector[X_AXIS] = accumulatedMagVector[X_AXIS] * mult; + averagedMagVector[Y_AXIS] = accumulatedMagVector[Y_AXIS] * mult; + averagedMagVector[Z_AXIS] = accumulatedMagVector[Z_AXIS] * mult; + } + else + { + averagedMagVector[X_AXIS] = (real)0.0; + averagedMagVector[Y_AXIS] = (real)0.0; + averagedMagVector[Z_AXIS] = (real)0.0; + } + + return (magUsedInAlgorithm()); +} + + +// +static void _ResetAlgorithm(void) +{ + int elemNum; + + // Reset P + memset(gKalmanFilter.P, 0, sizeof(gKalmanFilter.P)); + float s1 = 1.0f; + float s2 = 0.01f; // s1=0.01, s2=0.1, HG=160sec can make ini_pitch=40 stable ,DXG + gKalmanFilter.P[STATE_Q0][STATE_Q0] = s1 * (real)INIT_P_Q; + gKalmanFilter.P[STATE_Q1][STATE_Q1] = s1 * (real)INIT_P_Q; + gKalmanFilter.P[STATE_Q2][STATE_Q2] = s1 * (real)INIT_P_Q; + gKalmanFilter.P[STATE_Q3][STATE_Q3] = s1 * (real)INIT_P_Q; + + gKalmanFilter.P[STATE_WBX][STATE_WBX] = s2 * (real)INIT_P_WB; + gKalmanFilter.P[STATE_WBY][STATE_WBY] = s2 * (real)INIT_P_WB; + gKalmanFilter.P[STATE_WBZ][STATE_WBZ] = s2 * (real)INIT_P_WB; + + + // Reset the rate-bias and corrected-rate variables (in the body-frame) + for (elemNum = X_AXIS; elemNum <= Z_AXIS; elemNum++) + { + /* Initialize gyro rate bias with averaged gyro output + * If averaged gyro output is above 1deg/s, this means there should be rotation + * during initializatoin and it cannot be considered as bias. A default zero bias + * is used instead. + */ + if (fabs(averagedGyroVector[elemNum]) < ONE_DEGREE_IN_RAD) + { + gKalmanFilter.rateBias_B[elemNum] = (real)averagedGyroVector[elemNum]; + } + else + { + gKalmanFilter.rateBias_B[elemNum] = 0.0; + } + + gKalmanFilter.correctedRate_B[elemNum] = (real)0.0; + } + + + GenerateProcessJacobian(); + GenerateProcessCovariance(); +} + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/TimingVars.c b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/TimingVars.c new file mode 100644 index 0000000..f4de8d9 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/TimingVars.c @@ -0,0 +1,168 @@ +/****************************************************************************** + * File: TimingVars.c + *******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + + +#include "TimingVars.h" + +TimingVars timer; // for InitTimingVars + +void TimingVars_Increment(void) +{ + // IncrementTimingVars.m + // + // Purpose: Increment timing variables that control the operation + // of the Extended Kalman Filter (in particular, the + // update stage of the EKF). + // + // Output: secondCntr -- increments once per secondCntr (1 Hz); not reset. + // tenHertzCntr -- increments ten times per second (10 Hz); reset once + // it reached 10. + // subFrameCntr -- increments one hundred times per second (100 Hz); + // reset once it reaches 10. + // t -- time generated from the three counters + // + + if(timer.dacqFrequency == DACQ_100_HZ){ + timer.oneHundredHertzFlag = 1; + }else{ + timer.oneHundredHertzFlag++; + timer.oneHundredHertzFlag &= 1; // toggle 1Hz flag + } + + if ( timer.basicFrameCounter >= timer.dacqFrequency) { + timer.basicFrameCounter = 0; + } else { + timer.basicFrameCounter++; + } + + if (timer.oneHundredHertzFlag == 1) { + timer.subFrameCntr = timer.subFrameCntr + 1; + if (timer.subFrameCntr >= 10) { + timer.subFrameCntr = 0; + + timer.tenHertzCntr = timer.tenHertzCntr + 1; + if (timer.tenHertzCntr >= 10) { + timer.tenHertzCntr = 0; + + timer.secondCntr = timer.secondCntr + 1; + } + } + } +} + +float TimingVars_GetTime(void) +{ + timer.time = (float)( 1.00*timer.secondCntr + + 0.10*timer.tenHertzCntr + + 0.01*timer.subFrameCntr); + //timing.t = timing.t + timing.tMin; + + return(timer.time); +} + +void TimingVars_SetTMin(float tMin) +{ + timer.tMin = tMin; +} + +float TimingVars_GetTMin(void) +{ + return(timer.tMin); +} + +#ifdef DISPLAY_DIAGNOSTIC_MSG +#ifdef INS_OFFLINE +void TimingVars_DisplayTimerVars(signed long timeStep) +{ + std::cout << "Iter " << timeStep << ": " << timer.secondCntr << ", " + << timer.tenHertzCntr << ", " + << timer.subFrameCntr << ", (t = " + << TimingVars_GetTime() << ")\n"; +} + +void TimingVars_DiagnosticMsg( std::string msg ) +{ + std::cout << msg << " (t = " << TimingVars_GetTime() + << ", k = " << TimingVars_GetTimeStep() + << ")\n"; +} +#else +void TimingVars_DisplayTimerVars(signed long timeStep) +{ + DEBUG_INT("Iter", timeStep); + DEBUG_INT(",", timer.secondCntr); + DEBUG_INT(",", timer.tenHertzCntr); + DEBUG_INT(",", timer.subFrameCntr); + DEBUG_INT(",(", TimingVars_GetTime()); + DEBUG_STRING(")\r\n,"); +} + +void TimingVars_DiagnosticMsg( char *msg ) +{ + DEBUG_STRING(msg); + DEBUG_INT(" (t = ", TimingVars_GetTime()); + DEBUG_INT(", k = ", TimingVars_GetTimeStep()); + DEBUG_STRING(")\r\n"); +} +#endif +#endif + + +uint32_t TimingVars_GetTimeStep(void) +{ + return( 100 * timer.secondCntr + + 10 * timer.tenHertzCntr + + 1 * timer.subFrameCntr ); +} + +void Initialize_Timing(void) +{ + // InitTimingVars.m + // + // Purpose: Initialize the timing variables that control operation of the + // Extended Kalman Filter + // + // Output: secondCntr -- increments once per simulated second (1 Hz); not reset. + // tenHertzCntr -- increments ten times per second (10 Hz); reset + // once it reached 10. + // subFrameCntr -- increments one hundred times per second (100 Hz); + // reset once it reaches 10. + // time -- time generated from the three counters + // + + // Initialize timing variables + timer.secondCntr = 0; + timer.tenHertzCntr = 0; + timer.subFrameCntr = -1; + timer.basicFrameCounter = 0; // increments at 100 or 200 Hz and reset after 1 second + + timer.time = 0.0; + timer.tMin = 0.0; + + // toggles between 0 and 1 at 200 Hz (currently used in firmware) + timer.oneHundredHertzFlag = 0; + + // Override execution rate of taskDataAcquisition() based on the configuration + timer.dacqFrequency = DACQ_200_HZ; // default +} + +void TimingVars_dacqFrequency(int freq) +{ + timer.dacqFrequency = freq; +} diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/UpdateFunctions.c b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/UpdateFunctions.c new file mode 100644 index 0000000..2759483 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/UpdateFunctions.c @@ -0,0 +1,1993 @@ +/* + * File: UpdateFunctions.c + * Author: joemotyka + * + * Created on May 8, 2016, 12:35 AM + */ + +#include +#include +#include + +#include "GlobalConstants.h" // TRUE, FALSE, NUMBER_OF_EKF_STATES, ... +#include "Indices.h" +#include "StateIndices.h" + +#include "platformAPI.h" +#include "TimingVars.h" + +#include "VectorMath.h" +#include "MatrixMath.h" +#include "QuaternionMath.h" +#include "TransformationMath.h" + +#include "algorithm.h" +#include "algorithmAPI.h" +#include "AlgorithmLimits.h" +#include "MotionStatus.h" +#include "EKF_Algorithm.h" +#include "UpdateFunctions.h" +#include "SensorNoiseParameters.h" + + +extern ImuStatsStruct gImuStats; + +#if FAST_MATH +#include "arm_math.h" +#endif // FAST_MATH + +#ifndef INS_OFFLINE +#ifdef DISPLAY_DIAGNOSTIC_MSG +#include "debug.h" +#endif +#endif // INS_OFFLINE + + +// H is sparse and has elements in the following locations... +uint8_t RLE_H[ROWS_IN_H][2] = { {STATE_Q0, STATE_Q3}, + {STATE_Q0, STATE_Q3}, + {STATE_Q0, STATE_Q3} }; + +// KxH is sparse with elements only in cols 6 through 9 +uint8_t RLE_KxH[ROWS_IN_K][2] = { {STATE_Q0, STATE_Q3}, {STATE_Q0, STATE_Q3}, {STATE_Q0, STATE_Q3}, + {STATE_Q0, STATE_Q3}, {STATE_Q0, STATE_Q3}, {STATE_Q0, STATE_Q3}, + {STATE_Q0, STATE_Q3}, {STATE_Q0, STATE_Q3}, {STATE_Q0, STATE_Q3}, {STATE_Q0, STATE_Q3}, + {STATE_Q0, STATE_Q3}, {STATE_Q0, STATE_Q3}, {STATE_Q0, STATE_Q3}, + {STATE_Q0, STATE_Q3}, {STATE_Q0, STATE_Q3}, {STATE_Q0, STATE_Q3} }; + +// Local functions +static void _TurnSwitch(void); + +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 + * heading is considered valid, and the eading will be initialized to be + * gEKFInput.trueCourse, and velocity will also be initiazlied as the + * corresponding NED speed. After this, the quaternion (q0 and q3) and velocity + * terms in the state covariance matrix P will be reset. Non-diagonal terms will be + * set as 0s, and diagonal terms will be set according to estimated variance. The + * cov(quaternion, velocity) should also be updated. But the positive-definiteness + * is not guaranteed this way. + * TRACE: + * @retval TRUE if heading initialized/reinitialized, FALSE if not. +******************************************************************************/ +static int InitializeHeadingFromGnss(); + +/****************************************************************************** + * @brief When heading is ready for initialization, the heading angle (yaw, and + * indeed quaternion in the Kalman filter) is initialized to match the value of + * gEKFInput.trueCourse, and velocity will also be initiazlied as the + * corresponding NED speed. After this, the quaternion (q0 and q3) and velocity + * terms in the state covariance matrix P will be reset. Non-diagonal terms will be + * set as 0s, and diagonal terms will be set according to estimated variance. The + * cov(quaternion, velocity) should also be updated. But the positive-definiteness + * is not guaranteed this way. + * TRACE: + * @retval None. +******************************************************************************/ +static void InitializeEkfHeading(); + +// Update rates +#define TEN_HERTZ_UPDATE 10 +#define TWENTY_HERTZ_UPDATE 20 +#define TWENTY_FIVE_HERTZ_UPDATE 25 +#define FIFTY_HERTZ_UPDATE 50 +#define ONE_HUNDRED_HERTZ_UPDATE 100 + +static BOOL useGpsHeading = 0; /* When GPS velocity is above a certain threshold, + * this is set to 1, and GPS heading measurement + * is used, otherwise, this is set to 0 and magnetic + * heading is used. + */ +static int runInsUpdate = 0; /* To enable the update to be broken up into + * two sequential calculations in two sucessive + * 100 Hz periods. + */ + +// Uncomment to run only AHRS-type updates +//#define ATT_UPDATE_ONLY + +static void Update_GPS(void); +static void Update_PseudoMeasurement(void); +static void GenPseudoMeasCov(real *r); + +// EKF_UpdateStage.m +void EKF_UpdateStage(void) +{ + /* Perform a VG/AHRS update, regardless of GPS availability or health, + * when the state is HG AHRS or LG AHRS. Once GPS becomes healthy + * (and the right conditions are met) perform an INS or reduced-order GPS update. + */ + if( gAlgorithm.state <= LOW_GAIN_AHRS ) + { + // Only allow the algorithm to be called on 100 Hz marks + if(timer.oneHundredHertzFlag == 1) + { + // Update the AHRS solution at a 10 Hz update rate + // Subframe counter counts to 10 before it is reset + if( _CheckForUpdateTrigger(TEN_HERTZ_UPDATE) ) + { + /* The AHRS/VG solution is handled inside FieldVectorsToEulerAngles + * (called from the prediction function EKF_PredictionStage) + */ + ComputeSystemInnovation_Att(); + Update_Att(); + } + } + } + else + { + /* GPS-type Updates (with magnetometers: true-heading = mag-heading + mag-decl) + * Perform the EKF update at 10 Hz (split nine mag-only updates for for every GPS/mag update) + * + * Check for 'new GPS data'. If new, and GPS is valid, perform a + * GPS-Based update and reset timer values to resync the attitude updates. + */ + if( gEKFInput.gpsUpdate ) + { + /* 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; + + // GNSS update + if (gEKFInput.gpsFixType) + { + // GPS heading valid? + 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 + * good, just correct the filter states with input GNSS measurement. + */ + int32_t timeSinceLastGoodGPSReading = (int32_t)gAlgorithm.itow - gAlgorithm.timeOfLastGoodGPSReading; + if (timeSinceLastGoodGPSReading < 0) + { + timeSinceLastGoodGPSReading = timeSinceLastGoodGPSReading + MAX_ITOW; + } + if (timeSinceLastGoodGPSReading > gAlgorithm.Limit.maxReliableDRTime) + { +#ifdef INS_OFFLINE + printf("GPS relocked.\n"); +#endif // INS_OFFLINE + // Since a relative long time has passed since DR begins, INS states need reinitialized. + InitINSFilter(); + } + else + { + // DR for a relative short time, no need to reinitialize the filter. + Update_GPS(); + } + // reset the "last good reading" time + gAlgorithm.timeOfLastGoodGPSReading = gEKFInput.itow; + } + + // 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; + } + else if( runInsUpdate ) + { + Update_Att(); + runInsUpdate = 0; // set up for next pass + // handle P when PPS is available + if (gAlgoStatus.bit.ppsAvailable) + { + ApplyGpsDealyCorrForStateCov(); + gAlgoStatus.bit.ppsAvailable = FALSE; + } + } + } +} + +// ----Compute the innovation vector, nu---- +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); + gKalmanFilter.nu[STATE_RZ] = _LimitValue(gKalmanFilter.nu[STATE_RZ], gAlgorithm.Limit.Innov.positionError); +} + + +// ----Compute the innovation vector, nu---- +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); + gKalmanFilter.nu[STATE_VZ] = _LimitValue(gKalmanFilter.nu[STATE_VZ], gAlgorithm.Limit.Innov.velocityError); +} + + +/* Compute the innovation, nu, between measured and predicted attitude. + * Correct for wrap-around. Then limit the error. + */ +void ComputeSystemInnovation_Att(void) +{ + // ----- Roll ----- + gKalmanFilter.nu[STATE_ROLL] = gKalmanFilter.measuredEulerAngles[ROLL] - + gKalmanFilter.eulerAngles[ROLL]; + gKalmanFilter.nu[STATE_ROLL] = _UnwrapAttitudeError(gKalmanFilter.nu[STATE_ROLL]); + gKalmanFilter.nu[STATE_ROLL] = _LimitValue(gKalmanFilter.nu[STATE_ROLL], gAlgorithm.Limit.Innov.attitudeError); + + // ----- Pitch ----- + gKalmanFilter.nu[STATE_PITCH] = gKalmanFilter.measuredEulerAngles[PITCH] - + gKalmanFilter.eulerAngles[PITCH]; + gKalmanFilter.nu[STATE_PITCH] = _UnwrapAttitudeError(gKalmanFilter.nu[STATE_PITCH]); + gKalmanFilter.nu[STATE_PITCH] = _LimitValue(gKalmanFilter.nu[STATE_PITCH], gAlgorithm.Limit.Innov.attitudeError); + + // ----- Yaw ----- + // CHANGED TO SWITCH BETWEEN GPS AND MAG UPDATES + if ( useGpsHeading ) + { + 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; + } + + } + else if ( magUsedInAlgorithm() && gAlgorithm.state <= LOW_GAIN_AHRS ) + { + gKalmanFilter.nu[STATE_YAW] = gKalmanFilter.measuredEulerAngles[YAW] - + gKalmanFilter.eulerAngles[YAW]; + } + else + { + gKalmanFilter.nu[STATE_YAW] = (real)0.0; + } + gKalmanFilter.nu[STATE_YAW] = _UnwrapAttitudeError(gKalmanFilter.nu[STATE_YAW]); + gKalmanFilter.nu[STATE_YAW] = _LimitValue(gKalmanFilter.nu[STATE_YAW], gAlgorithm.Limit.Innov.attitudeError); + + /* When the filtered yaw-rate is above certain thresholds then reduce the + * attitude-errors used to update roll and pitch. + */ + _TurnSwitch(); + + // + gKalmanFilter.nu[STATE_ROLL] = gKalmanFilter.turnSwitchMultiplier * gKalmanFilter.nu[STATE_ROLL]; + gKalmanFilter.nu[STATE_PITCH] = gKalmanFilter.turnSwitchMultiplier * gKalmanFilter.nu[STATE_PITCH]; + //gKalmanFilter.nu[STATE_YAW] = gKalmanFilter.turnSwitchMultiplier * gKalmanFilter.nu[STATE_YAW]; +} + + +/****************************************************************************** + * @name: _GenerateObservationJacobian_RollAndPitch roll and pitch elements of + * the measurement Jacobian (H) + * @brief + * TRACE: + * @param N/A + * @retval 1 +******************************************************************************/ +uint8_t _GenerateObservationJacobian_AHRS(void) +{ + real xPhi, yPhi; + real uTheta; + real xPsi, yPsi; + real denom = 1.0; + real multiplier = 1.0; + + // Set the values in DP to zero + static BOOL initH = TRUE; + if( initH ) + { + initH = FALSE; + memset(gKalmanFilter.H, 0, sizeof(gKalmanFilter.H)); + } + + /// Note: H is 3x7 + /// Roll + yPhi = (real)2.0 * ( gKalmanFilter.quaternion[Q2] * gKalmanFilter.quaternion[Q3] + + gKalmanFilter.quaternion[Q0] * gKalmanFilter.quaternion[Q1] ); + xPhi = gKalmanFilter.quaternion[Q0] * gKalmanFilter.quaternion[Q0] + + -gKalmanFilter.quaternion[Q1] * gKalmanFilter.quaternion[Q1] + + -gKalmanFilter.quaternion[Q2] * gKalmanFilter.quaternion[Q2] + + gKalmanFilter.quaternion[Q3] * gKalmanFilter.quaternion[Q3]; + + denom = yPhi*yPhi + xPhi*xPhi; + if (denom < 1e-3) + { + /* Based on drive-test data, the minimum value seen was 0.98 but the minimum + * values based on Matlab analysis is 1e-7. + */ + denom = (real)1e-3; + } + multiplier = (real)2.0 / denom; + + /// Derivative of the roll-angle wrt quaternions + gKalmanFilter.H[ROLL][STATE_Q0] = multiplier * ( xPhi*gKalmanFilter.quaternion[Q1] + + -yPhi*gKalmanFilter.quaternion[Q0]); + gKalmanFilter.H[ROLL][STATE_Q1] = multiplier * ( xPhi*gKalmanFilter.quaternion[Q0] + + yPhi*gKalmanFilter.quaternion[Q1]); + gKalmanFilter.H[ROLL][STATE_Q2] = multiplier * ( xPhi*gKalmanFilter.quaternion[Q3] + + yPhi*gKalmanFilter.quaternion[Q2]); + gKalmanFilter.H[ROLL][STATE_Q3] = multiplier * ( xPhi*gKalmanFilter.quaternion[Q2] + + -yPhi*gKalmanFilter.quaternion[Q3]); + + /* Pitch (including modifications for |q| = 1 constraint) + * mu = 2*( q(1)*q(3) - q(2)*q(4) ); + * % 2/sqrt(1-mu*mu) * [q(3); -q(4); q(1); -q(2)] + * 2/sqrt(1-mu*mu) * [q(3) - mu*q(1); + * -q(4) - mu*q(2); + * q(1)- mu*q(3); + * -q(2)- mu*q(4)] + */ + uTheta = (real)2.0 * ( gKalmanFilter.quaternion[Q1] * gKalmanFilter.quaternion[Q3] - + gKalmanFilter.quaternion[Q0] * gKalmanFilter.quaternion[Q2] ); + // account for numerical accuracy to make sure abs(uTheta) <= 1 + if (uTheta > 1.0) + { + uTheta = 1.0; + } + if (uTheta < -1.0) + { + uTheta = -1.0; + } + denom = (real)sqrt(1.0f - uTheta*uTheta); + if (denom < 1e-3) + { + denom = (real)1e-3; + } + multiplier = (real)2.0 / denom; + + gKalmanFilter.H[PITCH][STATE_Q0] = multiplier * ( gKalmanFilter.quaternion[Q2] + + uTheta * gKalmanFilter.quaternion[Q0] ); + gKalmanFilter.H[PITCH][STATE_Q1] = multiplier * (-gKalmanFilter.quaternion[Q3] + + uTheta * gKalmanFilter.quaternion[Q1] ); + gKalmanFilter.H[PITCH][STATE_Q2] = multiplier * ( gKalmanFilter.quaternion[Q0] + + uTheta * gKalmanFilter.quaternion[Q2] ); + gKalmanFilter.H[PITCH][STATE_Q3] = multiplier * (-gKalmanFilter.quaternion[Q1] + + uTheta * gKalmanFilter.quaternion[Q3] ); + + /// Yaw + yPsi = (real)2.0 * ( gKalmanFilter.quaternion[Q1] * gKalmanFilter.quaternion[Q2] + + gKalmanFilter.quaternion[Q0] * gKalmanFilter.quaternion[Q3] ); + xPsi = (real)1.0 - (real)2.0 * ( gKalmanFilter.quaternion[Q2] * gKalmanFilter.quaternion[Q2] + + gKalmanFilter.quaternion[Q3] * gKalmanFilter.quaternion[Q3] ); + denom = yPsi*yPsi + xPsi*xPsi; + if (denom < 1e-3) + { + denom = (real)1e-3; + } + multiplier = (real)2.0 / denom; + + /// Derivative of the yaw-angle wrt quaternions + gKalmanFilter.H[YAW][STATE_Q0] = multiplier * ( xPsi*gKalmanFilter.quaternion[Q3] + + -yPsi*gKalmanFilter.quaternion[Q0]); + gKalmanFilter.H[YAW][STATE_Q1] = multiplier * ( xPsi*gKalmanFilter.quaternion[Q2] + + -yPsi*gKalmanFilter.quaternion[Q1]); + gKalmanFilter.H[YAW][STATE_Q2] = multiplier * ( xPsi*gKalmanFilter.quaternion[Q1] + + yPsi*gKalmanFilter.quaternion[Q2]); + gKalmanFilter.H[YAW][STATE_Q3] = multiplier * ( xPsi*gKalmanFilter.quaternion[Q0] + + yPsi*gKalmanFilter.quaternion[Q3]); + + return 1; +} + + +// +void _GenerateObservationCovariance_AHRS(void) +{ + static real Rnom; + + // Only need to compute certain elements of R once + static BOOL initR = TRUE; + if (initR) + { + initR = FALSE; + + /* Clear the values in R (in AHRS mode, there are 3 rows in the Jacobian) + * Initialize the Process Covariance (Q) matrix + */ + memset(gKalmanFilter.R, 0, sizeof(gKalmanFilter.R)); + + /* Calculate accel var when static from IMU specs. + * This accel var is the min accel var. If real-time accel var is below this value, + * the min accel var is used. + * Accel var is further converted to Euler angels measurement var. + */ + Rnom = gAlgorithm.imuSpec.sigmaA * gAlgorithm.imuSpec.sigmaA; + } + + /* Dynamically tune measurement covariance matrix R to get proper Kalman filter + * gain. The R consists of four parts. First, its min value is Rnom; Secodd, + * the online estimation of accel var; Thrid, the online estimation of accel + * error; Four, the different between accel magnitude and 1g. + */ + + // Rnom, accel var and accel error + real totalAccelVar[3]; // [m/s/s]^2 + for (int i = 0; i < 3; i++) + { + // replace sensor noise var with vibration var + if (gImuStats.accelVar[i] > Rnom) + { + totalAccelVar[i] = gImuStats.accelVar[i]; + } + else + { + totalAccelVar[i] = Rnom; + } + // linear accel? (including noise and vibration) + real errSqr; + errSqr = gImuStats.accelErr[i] * gImuStats.accelErr[i]; + if (errSqr > totalAccelVar[i]) + { + totalAccelVar[i] = errSqr; + } + } + + /* consider magnitude to further increase R + * Notice: totalAccelVarSum just approximates accel norm var + */ + real totalAccelVarSum = totalAccelVar[X_AXIS] + totalAccelVar[Y_AXIS] + totalAccelVar[Z_AXIS]; + real diff = gImuStats.accelNorm - GRAVITY; + diff *= diff; + real additionalR = 0.0; + /* if diff is larger than estimated accel err and the estimated accel err does + * not reach limit, diff will be used as additional measurement noise var. + */ + if (diff > 4.0*totalAccelVarSum && gImuStats.accelErrLimit == false) + { + // the magnitude of diff is too big, there is linear acceleration + additionalR = diff; + } + else + { + // the magnitude of diff is within noise level, no additional var + additionalR = 0.0; + } + + /* convert accel measurement var to pitch and roll var + * d(pitch) = 1/sqrt(1-ax^2) * d(ax) = 1/sqrt(ay^2+az^2) * d(ax) + * d(roll) = (az^2/(ay^2+az^2)) * d(ay) + (-ay/(ay^2+az^2)) * d(az) + * Notice: var(kx) = k*k*var(x) + */ + // Get ax^2, ay^2 and az^2 of normalized accel + real axSqr = gImuStats.lpfAccel[0] * gImuStats.lpfAccel[0]; + real aySqr = gImuStats.lpfAccel[1] * gImuStats.lpfAccel[1]; + real azSqr = gImuStats.lpfAccel[2] * gImuStats.lpfAccel[2]; + real sumSqr = axSqr + aySqr + azSqr; + axSqr /= sumSqr; + aySqr /= sumSqr; + azSqr /= sumSqr; + // pitch var + real mult = 1.0f - axSqr; + if (mult < 1.0e-2) + { + mult = 1.0e-2f; + } + mult = 1.0f / mult; // mult = 1 / (1-ax^2) = 1 / (ay^2 + az^2) + gKalmanFilter.R[STATE_PITCH] = mult * totalAccelVar[X_AXIS]; + // roll var + mult *= mult; // multi = 1 / (ay^2 + az^2)^2 + gKalmanFilter.R[STATE_ROLL] = mult * azSqr * azSqr * totalAccelVar[Y_AXIS] + + mult * aySqr * totalAccelVar[Z_AXIS]; + + // additional R + gKalmanFilter.R[STATE_ROLL] += additionalR; + gKalmanFilter.R[STATE_PITCH] += additionalR; + + /* We are indeed using var of multiple accel samples to estimate the var of Euler + * angles. From the formula above, accel var should be var of normalized accel. + * However, we choose GRAVITY instead of real accel norm to normalize the accel. + * Besides, accel var is only an estimate of Euler angles var, and Euler angels + * var is indeed not Gaussian. + */ + gKalmanFilter.R[STATE_ROLL] /= GRAVITY * GRAVITY; + gKalmanFilter.R[STATE_PITCH] /= GRAVITY * GRAVITY; + + /* limit R + * In previous version, Rnom is in untis of [g]^2, and maxR = 40000.0f*Rnom. + * 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; + if (gKalmanFilter.R[STATE_ROLL] > maxR) + { + gKalmanFilter.R[STATE_ROLL] = maxR; + } + if (gKalmanFilter.R[STATE_PITCH] > maxR) + { + gKalmanFilter.R[STATE_PITCH] = maxR; + } + + /* Yaw + * ------ From NovAtel's description of BESTVEL: ------ + * Velocity (speed and direction) calculations are computed from either + * Doppler or carrier phase measurements rather than from pseudorange + * measurements. Typical speed accuracies are around 0.03m/s (0.07 mph, + * 0.06 knots). + * + * Direction accuracy is derived as a function of the vehicle speed. A + * simple approach would be to assume a worst case 0.03 m/s cross-track + * velocity that would yield a direction error function something like: + * + * d (speed) = tan-1(0.03/speed) + * + * For example, if you are flying in an airplane at a speed of 120 knots + * or 62 m/s, the approximate directional error will be: + * + * tan-1 (0.03/62) = 0.03 degrees + * + * Consider another example applicable to hiking at an average walking + * speed of 3 knots or 1.5 m/s. Using the same error function yields a + * direction error of about 1.15 degrees. + * + * You can see from both examples that a faster vehicle speed allows for a + * more accurate heading indication. As the vehicle slows down, the + * velocity information becomes less and less accurate. If the vehicle is + * stopped, a GNSS receiver still outputs some kind of movement at speeds + * between 0 and 0.5 m/s in random and changing directions. This + * represents the noise and error of the static position. + + * ----- Yaw ----- + * CHANGED TO SWITCH BETWEEN GPS AND MAG UPDATES + */ + if ( useGpsHeading ) + { + float temp = (float)atan( 0.05 / gEKFInput.rawGroundSpeed ); + gKalmanFilter.R[STATE_YAW] = temp * temp; + if (gAlgoStatus.bit.turnSwitch) + { + gKalmanFilter.R[STATE_YAW] *= 10.0; + } + } + else if ( magUsedInAlgorithm() && gAlgorithm.state <= LOW_GAIN_AHRS ) + { + // todo: need to further distinguish between if mag is used + // MAGNETOMETERS + if( (gAlgorithm.state == HIGH_GAIN_AHRS) || + (gAlgorithm.linAccelSwitch == TRUE) ) + { + // --- High-Gain --- + gKalmanFilter.R[STATE_YAW] = (real)1.0e-2; // jun4 + } else { + // --- Low-Gain --- + gKalmanFilter.R[STATE_YAW] = (real)1.0e-1; // v14.6 values + } + + /* For 'large' roll/pitch angles, increase R-yaw to decrease the effect + * of update due to potential uncompensated z-axis magnetometer + * readings from affecting the yaw-update. + */ + if( ( gKalmanFilter.eulerAngles[ROLL] > TEN_DEGREES_IN_RAD ) || + ( gKalmanFilter.eulerAngles[PITCH] > TEN_DEGREES_IN_RAD ) ) + { + gKalmanFilter.R[STATE_YAW] = (real)0.2; + } + } + else + { + gKalmanFilter.R[STATE_YAW] = (real)1.0; + } +} + +// +void _GenerateObservationCovariance_INS(void) +{ + // Only need to compute certain elements of R once + static BOOL initR = TRUE; + if (initR) + { + initR = FALSE; + + gKalmanFilter.R[STATE_RX] = (real)R_VALS_GPS_POS_X; + gKalmanFilter.R[STATE_RY] = gKalmanFilter.R[STATE_RX]; + gKalmanFilter.R[STATE_RZ] = gKalmanFilter.R[STATE_RX]; + + gKalmanFilter.R[STATE_VX] = (real)R_VALS_GPS_VEL_X; + gKalmanFilter.R[STATE_VY] = gKalmanFilter.R[STATE_VX]; + gKalmanFilter.R[STATE_VZ] = gKalmanFilter.R[STATE_VX]; + } + + /* Use the GPS-provided horizontal and vertical accuracy values to populate + * the covariance values. + */ + gKalmanFilter.R[STATE_RX] = gEKFInput.GPSHorizAcc * gEKFInput.GPSHorizAcc; + gKalmanFilter.R[STATE_RY] = gKalmanFilter.R[STATE_RX]; + gKalmanFilter.R[STATE_RZ] = gEKFInput.GPSVertAcc * gEKFInput.GPSVertAcc; + + /* Scale the best velocity error by HDOP then multiply by the z-axis angular + * rate PLUS one (to prevent the number from being zero) so the velocity + * update during high-rate turns is reduced. + */ + float temp = (real)0.0625 * gEKFInput.HDOP; // 0.0625 = 0.05 / 0.8 + real absFilteredYawRate = (real)fabs(gAlgorithm.filteredYawRate); + if (absFilteredYawRate > TEN_DEGREES_IN_RAD) + { + temp *= (1.0f + absFilteredYawRate); + } + gKalmanFilter.R[STATE_VX] = temp;// *((real)1.0 + fabs(gAlgorithm.filteredYawRate) * (real)RAD_TO_DEG); + gKalmanFilter.R[STATE_VX] = gKalmanFilter.R[STATE_VX] * gKalmanFilter.R[STATE_VX]; + gKalmanFilter.R[STATE_VY] = gKalmanFilter.R[STATE_VX]; + if (gAlgorithm.headingIni == HEADING_UNINITIALIZED) + { + /* When heading is not initialized, velocity measurement is not able to correct + * attitude/rate bias/accel bias, the larger the velocity, the more uncertain it is. + */ + gKalmanFilter.R[STATE_VX] += SQUARE(gEKFInput.vNed[0]) + SQUARE(gEKFInput.vNed[1]); + gKalmanFilter.R[STATE_VY] += gKalmanFilter.R[STATE_VX]; + } + + // z-axis velocity isn't really a function of yaw-rate and hdop + gKalmanFilter.R[STATE_VZ] = (float)(0.1 * 0.1); +} + + +// +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 }}; + +void Update_Att(void) +{ + static real lastYaw = 7.0; // a values larger than 2pi means this yaw is invalid + // which state is updated in Update_Att() + uint8_t updatedStatesAtt[16] = { 1, 1, 1, // Positions are not updated + 1, 1, 1, // Velocities are not updated + 1, 1, 1, 1, // Quaternions are updated + 1, 1, 1, // Gyro biases are updated + 1, 1, 1}; // Accel biases are not upated + + /* Calculate the elements in the H and R matrices + * Matrix sizes for an Euler-angle based AHRS solution: + */ + _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. + 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) + { + /* 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 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 + { + 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; + gKalmanFilter.H[1][colNum] = 0.0; + } + } + + /* This solution consists of an integrated roll/pitch/yaw solution + * S = H*P*HTrans + R (However the matrix math can be simplified since + * H is very sparse! P is fully populated) + * Update P from the P, H, and R matrices: P = HxPxHTranspose + R + */ + // 1) PxHTranspose is computed first + memset(PxHTranspose, 0, sizeof(PxHTranspose)); + for (rowNum = 0; rowNum < ROWS_IN_P; rowNum++) + { + for (colNum = 0; colNum < ROWS_IN_H; colNum++) + { + for (multIndex = RLE_H[colNum][0]; multIndex <= RLE_H[colNum][1]; multIndex++) + { + PxHTranspose[rowNum][colNum] = PxHTranspose[rowNum][colNum] + + gKalmanFilter.P[rowNum][multIndex] * gKalmanFilter.H[colNum][multIndex]; + } + } + } + + /* HPH' is symmetric so only need to multiply one half and reflect the values + * across the diagonal. S_3x3 is to hold values of HPH'. + */ + for (rowNum = 0; rowNum < ROWS_IN_H; rowNum++) + { + for (colNum = rowNum; colNum < ROWS_IN_H; colNum++) + { + S_3x3[rowNum][colNum] = 0.0; + for (multIndex = RLE_H[rowNum][0]; multIndex <= RLE_H[rowNum][1]; multIndex++) + { + S_3x3[rowNum][colNum] = S_3x3[rowNum][colNum] + + gKalmanFilter.H[rowNum][multIndex] * PxHTranspose[multIndex][colNum]; + } + S_3x3[colNum][rowNum] = S_3x3[rowNum][colNum]; + } + } + + // S = HxPxHTranspose + R (rows 7:10 and cols 7:10 of P PLUS diagonal of R) + S_3x3[ROLL][ROLL] += gKalmanFilter.R[STATE_ROLL]; + S_3x3[PITCH][PITCH] += gKalmanFilter.R[STATE_PITCH]; + S_3x3[YAW][YAW] += gKalmanFilter.R[STATE_YAW]; + + // Invert the S-Matrix (replace with sequential update) + matrixInverse_3x3(&S_3x3[0][0], &SInverse_3x3[0][0]); + + // Compute the Kalman gain: K = P*HTrans*SInv + AxB( &PxHTranspose[0][0], + &SInverse_3x3[0][0], + ROWS_IN_P, ROWS_IN_H, ROWS_IN_H, + &gKalmanFilter.K[0][0] ); + + // force unupdated terms in K to be 0 + for (rowNum = STATE_RX; rowNum <= STATE_ABZ; rowNum++) + { + if (!updatedStatesAtt[rowNum]) + { + for (colNum = 0; colNum < 3; colNum++) + { + gKalmanFilter.K[rowNum][colNum] = 0.0; + } + } + } + + /* Compute attitude-quaternion updates: Dx = K*nu + * NOTE: Can access nu in the elements that the attitude error is stored BUT the + * value of ROWS_IN_H must be correct or the multiplication will be wrong + */ + AxV( &gKalmanFilter.K[0][0], + &gKalmanFilter.nu[STATE_ROLL], + NUMBER_OF_EKF_STATES, ROWS_IN_H, + &gKalmanFilter.stateUpdate[0] ); + + // Update states based on computed deltas + // --- attitude quaternions (q = q + Dq) --- + gKalmanFilter.quaternion[Q0] = gKalmanFilter.quaternion[Q0] + gKalmanFilter.stateUpdate[STATE_Q0]; + gKalmanFilter.quaternion[Q1] = gKalmanFilter.quaternion[Q1] + gKalmanFilter.stateUpdate[STATE_Q1]; + gKalmanFilter.quaternion[Q2] = gKalmanFilter.quaternion[Q2] + gKalmanFilter.stateUpdate[STATE_Q2]; + gKalmanFilter.quaternion[Q3] = gKalmanFilter.quaternion[Q3] + gKalmanFilter.stateUpdate[STATE_Q3]; + + // Normalize q + QuatNormalize(&gKalmanFilter.quaternion[0]); + + // --- Angular-rate bias (wBias = wBias = DwBias) --- + // If magnetometers are not used then set the rate bias to zero??? + gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; + gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; + gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; + + /* Update covariance: P = P + DP = P - K*H*P + * KxH = gKF.K * gKF.H; + */ + memset(KxH, 0, sizeof(KxH)); + for (rowNum = 0; rowNum < ROWS_IN_K; rowNum++) + { + for (colNum = RLE_KxH[rowNum][0]; colNum <= RLE_KxH[rowNum][1]; colNum++) + { + for (multIndex = 0; multIndex < ROWS_IN_H; multIndex++) + { + KxH[rowNum][colNum] = KxH[rowNum][colNum] + + gKalmanFilter.K[rowNum][multIndex] * gKalmanFilter.H[multIndex][colNum]; + } + } + } + + // deltaP = KxH * gKF.P; + 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 + */ +#if 0 + for (rowNum = 0; rowNum < ROWS_IN_K; rowNum++) + { + for (colNum = rowNum; colNum < COLS_IN_P; colNum++) + { + for (multIndex = RLE_KxH[rowNum][0]; multIndex <= RLE_KxH[rowNum][1]; multIndex++) + { + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + + KxH[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; + } + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; + } + } +#else + for (rowNum = 0; rowNum <= STATE_ABZ; rowNum++) + { + if (!updatedStatesAtt[rowNum]) + { + continue; + } + for (colNum = rowNum; colNum <= STATE_ABZ; colNum++) + { + if (!updatedStatesAtt[colNum]) + { + continue; + } + for (multIndex = RLE_KxH[rowNum][0]; multIndex <= RLE_KxH[rowNum][1]; multIndex++) + { + if (!updatedStatesAtt[multIndex]) + { + continue; + } + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + + KxH[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; + } + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; + } + } +#endif + /* P is symmetric so only need to multiply one half and reflect the values + * across the diagonal + */ + for (rowNum = 0; rowNum < ROWS_IN_P; rowNum++) + { + for (colNum = rowNum; colNum < COLS_IN_P; colNum++) + { + gKalmanFilter.P[rowNum][colNum] = gKalmanFilter.P[rowNum][colNum] - + gKalmanFilter.deltaP_tmp[rowNum][colNum]; + gKalmanFilter.P[colNum][rowNum] = gKalmanFilter.P[rowNum][colNum]; + } + } +} + + +/* The position update only allows us to update the position and velocity states (along + * with Pr and Pv). Want to verify this... + */ +void Update_Pos(void) +{ + // which state is updated in Update_Pos() + uint8_t updatedStatesPos[16] = { 1, 1, 1, // Positions are updated + 1, 1, 1, // Velocities are updated + 1, 1, 1, 1, // Quaternions are NOT updated + 1, 1, 1, // Gyro biases are NOT updated + 1, 1, 1 }; // Accel biases are NOT upated + + + /* S1 = H1*gKF.P*H1' + R1; Top 3 rows of the first 3 cols of P + R + * K1 = gKF.P*H1'*inv(S1); ( first 3 cols of P ) * S1Inverse + * P1 = (eye(16) - K1*H1) * gKF.P; + * H2 = [I3x3 0_3x3 0_3x4 0_3x3 0_3x3] + */ + + // S1 = H1*gKF.P*H1' + R1; + S_3x3[0][0] = gKalmanFilter.P[STATE_RX][STATE_RX] + gKalmanFilter.R[STATE_RX]; + S_3x3[0][1] = gKalmanFilter.P[STATE_RX][STATE_RY]; + S_3x3[0][2] = gKalmanFilter.P[STATE_RX][STATE_RZ]; + + S_3x3[1][0] = gKalmanFilter.P[STATE_RY][STATE_RX]; + S_3x3[1][1] = gKalmanFilter.P[STATE_RY][STATE_RY] + gKalmanFilter.R[STATE_RY]; + S_3x3[1][2] = gKalmanFilter.P[STATE_RY][STATE_RZ]; + + S_3x3[2][0] = gKalmanFilter.P[STATE_RZ][STATE_RX]; + S_3x3[2][1] = gKalmanFilter.P[STATE_RZ][STATE_RY]; + S_3x3[2][2] = gKalmanFilter.P[STATE_RZ][STATE_RZ] + gKalmanFilter.R[STATE_RZ]; + + // S1_Inverse + matrixInverse_3x3(&S_3x3[0][0], &SInverse_3x3[0][0]); + + // Compute K1 = ( gKF.P*H1' ) * S1Inverse = ( first 3 cols of P ) * S1Inverse + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + for (colNum = X_AXIS; colNum <= Z_AXIS; colNum++) + { + gKalmanFilter.K[rowNum][colNum] = 0.0; + // H is sparse so only the columns of P associated with the position states are used + // in the calculation + for (multIndex = STATE_RX; multIndex <= STATE_RZ; multIndex++) + { + gKalmanFilter.K[rowNum][colNum] = gKalmanFilter.K[rowNum][colNum] + + gKalmanFilter.P[rowNum][multIndex] * SInverse_3x3[multIndex - STATE_RX][colNum]; + } + } + } + + // force uncorrected terms in K to be 0 + for (rowNum = STATE_RX; rowNum <= STATE_ABZ; rowNum++) + { + if (!updatedStatesPos[rowNum]) + { + for (colNum = 0; colNum < 3; colNum++) + { + gKalmanFilter.K[rowNum][colNum] = 0.0; + } + } + } + // 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(&gKalmanFilter.deltaP_tmp[0][0], 0, sizeof(gKalmanFilter.deltaP_tmp)); + // Update the intermediate covariance estimate + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + if (!updatedStatesPos[rowNum]) + { + continue; + } + for (colNum = rowNum; colNum < NUMBER_OF_EKF_STATES; colNum++) + { + if (!updatedStatesPos[colNum]) + { + continue; + } + /* H is sparse so only the columns of P associated with the position states are used + * in the calculation + */ + for (multIndex = STATE_RX; multIndex <= STATE_RZ; multIndex++) + { + if (!updatedStatesPos[multIndex]) + { + continue; + } + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + + gKalmanFilter.K[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; + } + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; + } + } + + 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]; + gKalmanFilter.Position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_RY]; + gKalmanFilter.Position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_RZ]; + + gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_VX]; + gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_VY]; + gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_VZ]; + + gKalmanFilter.quaternion[Q0] = gKalmanFilter.quaternion[Q0] + gKalmanFilter.stateUpdate[STATE_Q0]; + gKalmanFilter.quaternion[Q1] = gKalmanFilter.quaternion[Q1] + gKalmanFilter.stateUpdate[STATE_Q1]; + gKalmanFilter.quaternion[Q2] = gKalmanFilter.quaternion[Q2] + gKalmanFilter.stateUpdate[STATE_Q2]; + gKalmanFilter.quaternion[Q3] = gKalmanFilter.quaternion[Q3] + gKalmanFilter.stateUpdate[STATE_Q3]; + + // Normalize quaternion and force q0 to be positive + QuatNormalize(gKalmanFilter.quaternion); + + gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; + gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; + gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; + + gKalmanFilter.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_ABX]; + gKalmanFilter.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_ABY]; + gKalmanFilter.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_ABZ]; +} + + +/* The velocity update only allows us to update the velocity, attitude, and acceleration- + * bias states (along with Pv, Pq, and Pab). Wwant to verify this... + */ +void Update_Vel(void) +{ + // which state is updated in Update_Vel() + uint8_t updatedStatesVel[16] = { 1, 1, 1, // Positions are NOT updated + 1, 1, 1, // Velocities are updated + 1, 1, 1, 1, // Quaternions are updated + 1, 1, 1, // Gyro biases are NOT updated + 1, 1, 1 }; // Accel biases are upated + + /* S2 = H2*P1*H2' + R2; (4th, 5th, and 6th rows of the 4th, 5th, and 6th cols of P1) + * K2 = P1*H2'*inv(S2); + * P2 = (eye(16) - K2*H2) * P1; + * H2 = [0_3x3 I3x3 0_3x4 0_3x3 0_3x3] + */ + + // S2 = H2*P1*H2' + R2; + S_3x3[0][0] = gKalmanFilter.P[STATE_VX][STATE_VX] + gKalmanFilter.R[STATE_VX]; + S_3x3[0][1] = gKalmanFilter.P[STATE_VX][STATE_VY]; + S_3x3[0][2] = gKalmanFilter.P[STATE_VX][STATE_VZ]; + + S_3x3[1][0] = gKalmanFilter.P[STATE_VY][STATE_VX]; + S_3x3[1][1] = gKalmanFilter.P[STATE_VY][STATE_VY] + gKalmanFilter.R[STATE_VY]; + S_3x3[1][2] = gKalmanFilter.P[STATE_VY][STATE_VZ]; + + S_3x3[2][0] = gKalmanFilter.P[STATE_VZ][STATE_VX]; + S_3x3[2][1] = gKalmanFilter.P[STATE_VZ][STATE_VY]; + S_3x3[2][2] = gKalmanFilter.P[STATE_VZ][STATE_VZ] + gKalmanFilter.R[STATE_VZ]; + + // S2_Inverse + matrixInverse_3x3(&S_3x3[0][0], &SInverse_3x3[0][0]); + + // Compute K2 = ( P1*H2' ) * S2Inverse = ( 4th, 5th, and 6th cols of P1 ) * S2Inverse + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + for (colNum = X_AXIS; colNum <= Z_AXIS; colNum++) + { + gKalmanFilter.K[rowNum][colNum] = 0.0; + /* H is sparse so only the columns of P associated with the velocity states are used + * in the calculation + */ + for (multIndex = STATE_VX; multIndex <= STATE_VZ; multIndex++) + { + gKalmanFilter.K[rowNum][colNum] = gKalmanFilter.K[rowNum][colNum] + + gKalmanFilter.P[rowNum][multIndex] * SInverse_3x3[multIndex - STATE_VX][colNum]; + } + } + } + + // force uncorrected terms in K to be 0 + for (rowNum = STATE_RX; rowNum <= STATE_ABZ; rowNum++) + { + if (!updatedStatesVel[rowNum]) + { + for (colNum = 0; colNum < 3; colNum++) + { + gKalmanFilter.K[rowNum][colNum] = 0.0; + } + } + } + // Compute the intermediate state update + AxB(&gKalmanFilter.K[0][0], &gKalmanFilter.nu[STATE_VX], NUMBER_OF_EKF_STATES, 3, 1, &gKalmanFilter.stateUpdate[0]); + + 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++) + { + if (!updatedStatesVel[rowNum]) + { + continue; + } + for (colNum = rowNum; colNum < NUMBER_OF_EKF_STATES; colNum++) + { + if (!updatedStatesVel[colNum]) + { + continue; + } + /* H is sparse so only the columns of P associated with the velocity states are used + * in the calculation + */ + for (multIndex = STATE_VX; multIndex <= STATE_VZ; multIndex++) + { + gKalmanFilter.deltaP_tmp[rowNum][colNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum] + + gKalmanFilter.K[rowNum][multIndex - STATE_VX] * gKalmanFilter.P[multIndex][colNum]; + } + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; + } + } + + // P2 = P2 - KxHxP2 + 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 + gKalmanFilter.Position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_RX]; + gKalmanFilter.Position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_RY]; + gKalmanFilter.Position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_RZ]; + + gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_VX]; + gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_VY]; + gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_VZ]; + + gKalmanFilter.quaternion[Q0] = gKalmanFilter.quaternion[Q0] + gKalmanFilter.stateUpdate[STATE_Q0]; + gKalmanFilter.quaternion[Q1] = gKalmanFilter.quaternion[Q1] + gKalmanFilter.stateUpdate[STATE_Q1]; + gKalmanFilter.quaternion[Q2] = gKalmanFilter.quaternion[Q2] + gKalmanFilter.stateUpdate[STATE_Q2]; + gKalmanFilter.quaternion[Q3] = gKalmanFilter.quaternion[Q3] + gKalmanFilter.stateUpdate[STATE_Q3]; + + // Normalize quaternion and force q0 to be positive + QuatNormalize(gKalmanFilter.quaternion); + + gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; + gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; + gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; + + 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) +{ + // Calculate the R-values for the INS measurements + _GenerateObservationCovariance_INS(); + + /* This Sequential-Filter (three-stage approach) is nearly as + * good as the full implementation -- we also can split it + * across multiple iterations to not exceed 10 ms execution on + * the embedded 380 + */ + /* 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(); + Update_Vel(); + ComputeSystemInnovation_Att(); + + // Initialize heading. If getting initial heading at this step, do not update att + if (gAlgorithm.headingIni < HEADING_GNSS_HIGH) + { + if (InitializeHeadingFromGnss()) + { + // Heading is initialized. Related elements in the EKF also need intializing. + InitializeEkfHeading(); + + /* This heading measurement is used to initialize heading, and should not be + * used to update heading. + */ + useGpsHeading = FALSE; + } + } +} + +static void Update_PseudoMeasurement(void) +{ + // which state is updated in Update_Vel() + uint8_t updatedStatesPseudo[16] = { 1, 1, 1, // Positions are NOT updated + 1, 1, 1, // Velocities are updated + 1, 1, 1, 1, // Quaternions are updated + 1, 1, 1, // Gyro biases are updated + 1, 1, 1 }; // Accel biases are upated + + /* Get current rb2n. + * gKalmanFilter.R_BinN is updated every time the algo enters _PredictStateEstimate + * After prediction and GPS update, this matrix needs updated. + */ + real rb2n[3][3]; + QuaternionToR321(gKalmanFilter.quaternion, &rb2n[0][0]); + + // detect zero velocity using GNSS vNED + BOOL staticGnss = DetectStaticGnssVelocity(gEKFInput.vNed, + gAlgorithm.staticDetectParam.staticGnssVel, + gEKFInput.gpsFixType); + + // measurement cov + real r[3] = { 1.0e-4f, 1.0e-4f, 1.0e-4f }; + if (!gImuStats.bStaticIMU) + { + /* If zero velocity is not detected by IMU, the covariance for the lateral and + * vertical velocity measurement should be increased. + */ + GenPseudoMeasCov(r); + r[1] = 1.0e-1; + r[2] = 1.0e-1; + } + + /* Compute innovation (measured - estimated) of velocity expressed in the body frame: + * innovation = [odo/0.0, 0.0, 0.0] - Rn2b * v_ned + * When odometer is available, front velocity measurement is given by odometer. + * When zero velocity detected, front velocity measurement is 0. + * 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 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 (odoUsedInAlgorithm()) + { + frontVelMeaValid = TRUE; + frontVelMea = gEKFInput.odoVelocity; + r[0] = 1.0e-4; // variance of front velocity measurement should be from odo spec + } + else if (gImuStats.bStaticIMU) + { + /* Only when GNSS is invalid or zero velocity is also detected by GNSS, zero velocity + * detected by IMU (and GNSS) can be used to determine the along-track velocity. + * When front velocity measurement is not available, it is not necessary to readjust + * its variance since it will not be used. + */ + if ((!gEKFInput.gpsFixType) || staticGnss) + { + frontVelMeaValid = TRUE; + frontVelMea = 0.0; + } + } + // front vel error + gKalmanFilter.nu[STATE_VX] = frontVelMea + -rb2n[0][0] * gKalmanFilter.Velocity_N[0] + -rb2n[1][0] * gKalmanFilter.Velocity_N[1] + -rb2n[2][0] * gKalmanFilter.Velocity_N[2]; + // lateral (right) vel error + gKalmanFilter.nu[STATE_VY] = + -rb2n[0][1] * gKalmanFilter.Velocity_N[0] + -rb2n[1][1] * gKalmanFilter.Velocity_N[1] + -rb2n[2][1] * gKalmanFilter.Velocity_N[2]; + // vertical (downwards) vel erro + gKalmanFilter.nu[STATE_VZ] = + -rb2n[0][2] * gKalmanFilter.Velocity_N[0] + -rb2n[1][2] * gKalmanFilter.Velocity_N[1] + -rb2n[2][2] * gKalmanFilter.Velocity_N[2]; + gKalmanFilter.nu[STATE_VY] = _LimitValue(gKalmanFilter.nu[STATE_VY], gAlgorithm.Limit.Innov.velocityError); + gKalmanFilter.nu[STATE_VZ] = _LimitValue(gKalmanFilter.nu[STATE_VZ], gAlgorithm.Limit.Innov.velocityError); + + // p*H'. PxHTranspose is 16x3, only the last two columns are used when only lateral and vertical measurements + memset(PxHTranspose, 0, sizeof(PxHTranspose)); + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + for (colNum = 0; colNum < 3; colNum++) + { + PxHTranspose[rowNum][colNum] = + gKalmanFilter.P[rowNum][3] * rb2n[0][colNum] + + gKalmanFilter.P[rowNum][4] * rb2n[1][colNum] + + gKalmanFilter.P[rowNum][5] * rb2n[2][colNum]; + } + } + + // s = H*P*H' + R + for (rowNum = 0; rowNum < 3; rowNum++) + { + for (colNum = rowNum; colNum < 3; colNum++) + { + S_3x3[rowNum][colNum] = rb2n[0][rowNum] * PxHTranspose[3][colNum] + + rb2n[1][rowNum] * PxHTranspose[4][colNum] + + rb2n[2][rowNum] * PxHTranspose[5][colNum]; + S_3x3[colNum][rowNum] = S_3x3[rowNum][colNum]; + } + S_3x3[rowNum][rowNum] += r[rowNum]; + } + + // Calculate inv(H*P*H'+R) according to if front velocity measurement is available + if (frontVelMeaValid) + { + matrixInverse_3x3(&S_3x3[0][0], &SInverse_3x3[0][0]); + } + else + { + S_3x3[0][0] = 1.0; + S_3x3[0][1] = 0.0; + S_3x3[0][2] = 0.0; + S_3x3[1][0] = 0.0; + S_3x3[2][0] = 0.0; + matrixInverse_3x3(&S_3x3[0][0], &SInverse_3x3[0][0]); + SInverse_3x3[0][0] = 0.0; + } + + // K = P*H' * inv(H*P*H' + R). gKalmanFilter.K is 16x3, only the last two columns are used. + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + for (colNum = 0; colNum < 3; colNum++) + { + gKalmanFilter.K[rowNum][colNum] = + PxHTranspose[rowNum][0] * SInverse_3x3[0][colNum] + + PxHTranspose[rowNum][1] * SInverse_3x3[1][colNum] + + PxHTranspose[rowNum][2] * SInverse_3x3[2][colNum]; + } + } + // force uncorrected terms in K to be 0 + for (rowNum = STATE_RX; rowNum <= STATE_ABZ; rowNum++) + { + if (!updatedStatesPseudo[rowNum]) + { + for (colNum = 0; colNum < 3; colNum++) + { + gKalmanFilter.K[rowNum][colNum] = 0.0; + } + } + } + + // dx = k * nu + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + gKalmanFilter.stateUpdate[rowNum] = + gKalmanFilter.K[rowNum][0] * gKalmanFilter.nu[STATE_VX] + + gKalmanFilter.K[rowNum][1] * gKalmanFilter.nu[STATE_VY] + + gKalmanFilter.K[rowNum][2] * gKalmanFilter.nu[STATE_VZ]; + } + + // update state + //gKalmanFilter.Position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_RX]; + //gKalmanFilter.Position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_RY]; + //gKalmanFilter.Position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_RZ]; + + gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_VX]; + gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_VY]; + gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_VZ]; + + gKalmanFilter.quaternion[Q0] = gKalmanFilter.quaternion[Q0] + gKalmanFilter.stateUpdate[STATE_Q0]; + gKalmanFilter.quaternion[Q1] = gKalmanFilter.quaternion[Q1] + gKalmanFilter.stateUpdate[STATE_Q1]; + gKalmanFilter.quaternion[Q2] = gKalmanFilter.quaternion[Q2] + gKalmanFilter.stateUpdate[STATE_Q2]; + gKalmanFilter.quaternion[Q3] = gKalmanFilter.quaternion[Q3] + gKalmanFilter.stateUpdate[STATE_Q3]; + + // Normalize quaternion and force q0 to be positive + QuatNormalize(gKalmanFilter.quaternion); + + gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; + gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; + gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; + + 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]; + + // Update covariance: P = P + DP = P - K*H*P + // Use transpose(PxHTranspose) to hold H*P (3x16). + // Only the last two columns are used when only lateral and vertical measurements + for (colNum = 0; colNum < NUMBER_OF_EKF_STATES; colNum++) + { + for (rowNum = 0; rowNum < 3; rowNum++) + { + PxHTranspose[colNum][rowNum] = rb2n[0][rowNum] * gKalmanFilter.P[3][colNum] + + rb2n[1][rowNum] * gKalmanFilter.P[4][colNum] + + rb2n[2][rowNum] * gKalmanFilter.P[5][colNum]; + } + } + + // deltaP = KxH * gKF.P; + 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 + */ + for (rowNum = 0; rowNum <= STATE_ABZ; rowNum++) + { + if (!updatedStatesPseudo[rowNum]) + { + continue; + } + for (colNum = rowNum; colNum <= STATE_ABZ; colNum++) + { + if (!updatedStatesPseudo[colNum]) + { + continue; + } + 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]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; + } + } + + /* P is symmetric so only need to multiply one half and reflect the values + * across the diagonal + */ + for (rowNum = 0; rowNum < ROWS_IN_P; rowNum++) + { + for (colNum = rowNum; colNum < COLS_IN_P; colNum++) + { + gKalmanFilter.P[rowNum][colNum] = gKalmanFilter.P[rowNum][colNum] - + gKalmanFilter.deltaP_tmp[rowNum][colNum]; + gKalmanFilter.P[colNum][rowNum] = gKalmanFilter.P[rowNum][colNum]; + } + } +} + +static void GenPseudoMeasCov(real *r) +{ + real absYawRate = (real)fabs(gEKFInput.angRate_B[2]); + r[1] = absYawRate; + r[2] = absYawRate; + + real minVar = 1e-4; + real maxVar = 1e-2; + if (r[1] < minVar) + { + r[1] = minVar; + } + if (r[1] > maxVar) + { + r[1] = maxVar; + } + + if (r[2] < minVar) + { + r[2] = minVar; + } + if (r[2] > maxVar) + { + r[2] = maxVar; + } + //printf("rr: %f,%f\n", r[1], r[2]); +} + + +/* Conversion from turn-rate threshold (values loaded into gConfiguration) to + * decimal value in [rad/sec]: + * + * thresh_rad = ( 10.0 * pi/180 ); % 0.1745 + * thresh_counts = floor( thresh_rad * ( 2^16 / (2*pi) ) ); % 1820 + * thresh_rad = thresh_counts * ( 2*pi / 2^16 ) % 1820 * (2*pi) / 2^16 = 0.1745 + */ +real TILT_YAW_SWITCH_GAIN = (real)0.05; + +// TurnSwitch.m +static void _TurnSwitch(void) +{ + static real minSwitch = (real)0.0, maxSwitch = (real)0.0; + static real turnSwitchThresholdPast = (real)0.0; + static real linInterpSF; + + real absYawRate; + real turnSwitchScaleFactor; + + // gKF.filteredYawRate (calculated in the prediction stage) + absYawRate = (real)fabs(gAlgorithm.filteredYawRate); + + // In case the user changes the TST during operation + if (gAlgorithm.turnSwitchThreshold != turnSwitchThresholdPast) + { + turnSwitchThresholdPast = gAlgorithm.turnSwitchThreshold; + + // Example conversion: ( 1820*12868 / 2^27 ) * ( 180/pi ) + minSwitch = gAlgorithm.turnSwitchThreshold * (real)(DEG_TO_RAD); // angle in radians + maxSwitch = (real)2.0 * minSwitch; // angle in radians + + linInterpSF = ((real)1.0 - TILT_YAW_SWITCH_GAIN) / (maxSwitch - minSwitch); + } + + // Linear interpolation if the yawRate is above the specified threshold + if ((gAlgorithm.state > HIGH_GAIN_AHRS) && (absYawRate > minSwitch)) + { + gAlgoStatus.bit.turnSwitch = TRUE; + + /* When the rate is below the maximum rate defined by turnSwitchThreshold, + * then generate a scale-factor that is between ( 1.0 - G ) and 0.0 (based on absYawRate). + * If it is above 'maxSwitch' then the SF is zero. + */ + if (absYawRate < maxSwitch) + { + turnSwitchScaleFactor = linInterpSF * (maxSwitch - absYawRate); + } + else + { + // yaw-rate is above maxSwitch ==> no gain + turnSwitchScaleFactor = (real)0.0; + } + + // Specify the multiplier so it is between G and 1.0 + gKalmanFilter.turnSwitchMultiplier = TILT_YAW_SWITCH_GAIN + turnSwitchScaleFactor; + } + else + { + gAlgoStatus.bit.turnSwitch = FALSE; + gKalmanFilter.turnSwitchMultiplier = (real)1.0; + } +} + + +static real _UnwrapAttitudeError(real attitudeError) +{ + while (fabs(attitudeError) > PI) + { + if (attitudeError > PI) + { + attitudeError = attitudeError - (real)TWO_PI; + } + else if (attitudeError < -PI) + { + attitudeError = attitudeError + (real)TWO_PI; + } + } + + return attitudeError; +} + + +static real _LimitValue(real value, real limit) +{ + if (value > limit) + { + return limit; + } + else if (value < -limit) + { + return -limit; + } + + return value; +} + + +/* Returns true when the system is ready to update (based on the timer values + * and the desired update rate) + */ +static BOOL _CheckForUpdateTrigger(uint8_t updateRate) +{ + // + uint8_t oneHundredHzCntr; + uint8_t updateFlag = 0; + + // + switch( updateRate ) + { + // ten-hertz update + case 10: + if( timer.subFrameCntr == 0 ) + { + updateFlag = 1; + } + break; + + // twenty-hertz update + case 20: + if( timer.subFrameCntr == 0 || timer.subFrameCntr == 5 ) + { + updateFlag = 1; + } + break; + + // twenty-hertz update + case 25: + oneHundredHzCntr = 10 * timer.tenHertzCntr + timer.subFrameCntr; + + // + if( oneHundredHzCntr == 0 || + oneHundredHzCntr == 4 || + oneHundredHzCntr == 8 || + oneHundredHzCntr == 12 || + oneHundredHzCntr == 16 || + oneHundredHzCntr == 20 || + oneHundredHzCntr == 24 || + oneHundredHzCntr == 28 || + oneHundredHzCntr == 32 || + oneHundredHzCntr == 36 || + oneHundredHzCntr == 40 || + oneHundredHzCntr == 44 || + oneHundredHzCntr == 48 || + oneHundredHzCntr == 52 || + oneHundredHzCntr == 56 || + oneHundredHzCntr == 60 || + oneHundredHzCntr == 64 || + oneHundredHzCntr == 68 || + oneHundredHzCntr == 72 || + oneHundredHzCntr == 76 || + oneHundredHzCntr == 80 || + oneHundredHzCntr == 84 || + oneHundredHzCntr == 88 || + oneHundredHzCntr == 92 || + oneHundredHzCntr == 96 ) + { + updateFlag = 1; + } + break; + + // fifty-hertz update + case 50: + if( timer.subFrameCntr == 0 || + timer.subFrameCntr == 2 || + timer.subFrameCntr == 4 || + timer.subFrameCntr == 6 || + timer.subFrameCntr == 8 ) + { + updateFlag = 1; + } + break; + + // fifty-hertz update + case 100: + updateFlag = 1; + break; + } + + return updateFlag; +} + +static int InitializeHeadingFromGnss() +{ + /* enable declination correction, but the corrected magnetic yaw will not + * be used if GPS is available. + */ + gAlgorithm.applyDeclFlag = TRUE; + + /* backward drive detection for heading initialization using GNSS heading. + * Detection happends every second. Velocity increment is relatively reliable + * if it is accumulated for 1sec. + */ + static real lastVelBxGnss = 0; + static uint8_t forwardDriveConfidence = 0; + static uint32_t lastTOW = 0; + uint32_t timeSinceLastDetection = gAlgorithm.itow - lastTOW; + if (timeSinceLastDetection < 0) + { + timeSinceLastDetection = timeSinceLastDetection + MAX_ITOW; + } + if (timeSinceLastDetection > 950) // 950ms is set as the threshold for 1sec + { + lastTOW = gAlgorithm.itow; + /* assume velocity is always along the body x axis. otherwise, GNSS heading + * cannot be used to initialize fusion heading + */ + real velBx = sqrtf(SQUARE(gEKFInput.vNed[0]) + SQUARE(gEKFInput.vNed[1]) + SQUARE(gEKFInput.vNed[2])); + velBx = fabs(velBx); + real dv = velBx - lastVelBxGnss; + if ((dv * gKalmanFilter.linearAccel_B[X_AXIS]) > 0.0 && fabs(gKalmanFilter.linearAccel_B[X_AXIS]) > 0.2) + { + if (forwardDriveConfidence < 255) + { + forwardDriveConfidence++; + } + } + else + { + forwardDriveConfidence = 0; + } + // record this velocity along body x axis for next run + lastVelBxGnss = velBx; + // reset accumulated x body axis velocity change. + gKalmanFilter.linearAccel_B[X_AXIS] = 0.0; + } + + // detect if GNSS heading is reliable + static uint8_t gnssHeadingGoodCntr = 0; + static float lastGnssHeading = 0.0; + static float lastFusionHeading = 0.0; + BOOL gnssHeadingGood = 0; + float angleDiff = 0.0; + if (useGpsHeading) + { + float calculatedGnssHeading = (float)(atan2(gEKFInput.vNed[1], gEKFInput.vNed[0]) * R2D); + float diffHeading = AngleErrDeg(gEKFInput.trueCourse - calculatedGnssHeading); + // input GNSS heading matches heading calculated from vNED + if (fabs(diffHeading) < 5.0) + { + // GNSS heading change matches fusion yaw angle + float gnssHeadingChange = gEKFInput.trueCourse - lastGnssHeading; + float fusionHeadingChange = gKalmanFilter.eulerAngles[2] * (float)R2D - lastFusionHeading; + angleDiff = (float)fabs( AngleErrDeg(gnssHeadingChange - fusionHeadingChange) ); + if (angleDiff < 5.0) + { + gnssHeadingGood = TRUE; + } + } + lastGnssHeading = gEKFInput.trueCourse; + lastFusionHeading = gKalmanFilter.eulerAngles[2] * (float)R2D; + } + if (gnssHeadingGood) + { + gnssHeadingGoodCntr++; + } + else + { + gnssHeadingGoodCntr = 0; + } + + // Heading initialization when drive forward and GNSS heading is reliable + BOOL thisHeadingUsedForIni = FALSE; + if (gAlgorithm.headingIni < HEADING_GNSS_LOW) // heading is immediately but maybe unreliably initialized + { + if (gnssHeadingGoodCntr >= 1 && forwardDriveConfidence >= 1) // Only one sample is checked, so heading may be unreliable + { + gnssHeadingGoodCntr = 0; + // Heading is initialized with GNSS + gAlgorithm.headingIni = HEADING_GNSS_LOW; + +#ifdef INS_OFFLINE + printf("quick gps heading: %f\n", gEKFInput.trueCourse); +#else +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("quick gps heading"); + DebugPrintFloat(": ", gEKFInput.trueCourse, 9); + DebugPrintEndline(); +#endif +#endif + thisHeadingUsedForIni = TRUE; + } + } + else + { + /* Three points are checked, and the latest ground speed is above a certian threshold. + * The latest GNSS heading should be reliable. + */ + if (gnssHeadingGoodCntr >= 3 && + forwardDriveConfidence >= 5 && + gEKFInput.rawGroundSpeed > RELIABLE_GPS_VELOCITY_HEADING) + { + gnssHeadingGoodCntr = 0; + forwardDriveConfidence = 0; + gAlgorithm.headingIni = HEADING_GNSS_HIGH; +#ifdef INS_OFFLINE + printf("reliable gps heading: %f\n", gEKFInput.trueCourse); +#else +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("reliable gps heading"); + DebugPrintFloat(": ", gEKFInput.trueCourse, 9); + DebugPrintEndline(); +#endif +#endif + thisHeadingUsedForIni = TRUE; + } + } + + return thisHeadingUsedForIni; +} + +static void InitializeEkfHeading() +{ + /* Compare the reliable heading with Kalamn filter heading. If the difference exceeds + * a certain threshold, this means the immediate heading initialization is unreliable, + * and the Kalman filter needs reinitialized with the reliable one. + */ + float angleDiff = (float)fabs(AngleErrDeg(gEKFInput.trueCourse - + gKalmanFilter.eulerAngles[2] * (float)R2D)); + if (angleDiff <= 2.0) + { + return; + } + +#ifdef INS_OFFLINE + printf("Reinitialize KF: %f\n", angleDiff); +#else +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("Reinitialize KF: "); + DebugPrintFloat("", angleDiff, 9); + DebugPrintEndline(); +#endif +#endif + + // initialize yaw angle with GPS heading + gKalmanFilter.eulerAngles[YAW] = (gEKFInput.trueCourse * D2R); + if (gKalmanFilter.eulerAngles[YAW] > PI) + { + gKalmanFilter.eulerAngles[YAW] -= (real)TWO_PI; + } + EulerAnglesToQuaternion(gKalmanFilter.eulerAngles, gKalmanFilter.quaternion); + + // reinitialize NED position + gKalmanFilter.Position_N[0] = (real)gKalmanFilter.rGPS_N[0]; + gKalmanFilter.Position_N[1] = (real)gKalmanFilter.rGPS_N[1]; + gKalmanFilter.Position_N[2] = (real)gKalmanFilter.rGPS_N[2]; + + // reinitialize NED velocity + gKalmanFilter.Velocity_N[X_AXIS] = (real)gEKFInput.vNed[X_AXIS]; + gKalmanFilter.Velocity_N[Y_AXIS] = (real)gEKFInput.vNed[Y_AXIS]; + gKalmanFilter.Velocity_N[Z_AXIS] = (real)gEKFInput.vNed[Z_AXIS]; + +#if 1 // mod, DXG + // reset quaternion and velocity terms in the P matrix + int i, j; + // pos row + gKalmanFilter.P[STATE_RX][STATE_RX] = gKalmanFilter.R[STATE_RX]; + gKalmanFilter.P[STATE_RY][STATE_RY] = gKalmanFilter.R[STATE_RY]; + gKalmanFilter.P[STATE_RZ][STATE_RZ] = gKalmanFilter.R[STATE_RZ]; + for (i = STATE_RX; i < STATE_RZ; i++) + { + for (j = 0; j < NUMBER_OF_EKF_STATES; j++) + { + if (i != j) + { + gKalmanFilter.P[i][j] = 0; + gKalmanFilter.P[j][i] = 0; + } + } + } + // vel row + gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.R[STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.R[STATE_VY]; + gKalmanFilter.P[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VZ]; + for (i = STATE_VX; i < STATE_VZ; i++) + { + for (j = 0; j < NUMBER_OF_EKF_STATES; j++) + { + if (i != j) + { + gKalmanFilter.P[i][j] = 0; + gKalmanFilter.P[j][i] = 0; + } + } + } + // q0 row + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + if (i != STATE_Q0) + { + gKalmanFilter.P[STATE_Q0][i] = 0; + gKalmanFilter.P[i][STATE_Q0] = 0; + } + } + // q3 row + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + if (i != STATE_Q3) + { + gKalmanFilter.P[STATE_Q3][i] = 0; + gKalmanFilter.P[i][STATE_Q3] = 0; + } + } + + // the initial covariance of the quaternion is estimated from ground speed. + float temp = (float)atan(0.05 / gEKFInput.rawGroundSpeed); + temp *= temp; // heading var + if (gAlgoStatus.bit.turnSwitch) + { + temp *= 10.0; // when rotating, heading var increases + } + temp /= 4.0; // sin(heading/2) or cos(heading/2) + float sinYawSqr = (real)sin(gKalmanFilter.eulerAngles[YAW] / 2.0f); + sinYawSqr *= sinYawSqr; + // Assume roll and pitch are close to 0deg + gKalmanFilter.P[STATE_Q0][STATE_Q0] = temp * sinYawSqr; + gKalmanFilter.P[STATE_Q3][STATE_Q3] = temp * (1.0f - sinYawSqr); + + gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.R[STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.R[STATE_VY]; + gKalmanFilter.P[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VZ]; + +#if 0 + // reset velocity and quaternion terms in the P matrix + real v2 = gEKFInput.rawGroundSpeed * gEKFInput.rawGroundSpeed; + real v3by4 = 4.0 * v2 * gEKFInput.rawGroundSpeed; + real vn2 = gEKFInput.vNed[0] * gEKFInput.vNed[0]; + real q0q0 = gKalmanFilter.quaternion[0] * gKalmanFilter.quaternion[0]; + real q3q3 = gKalmanFilter.quaternion[3] * gKalmanFilter.quaternion[3]; + if (q0q0 < 1.0e-3) + { + q0q0 = 1.0e-3; + } + if (q3q3 < 1.0e-3) + { + q3q3 = 1.0e-3; + } + real multiplerQVn = (v2 - vn2) / v3by4; + multiplerQVn *= multiplerQVn; + real multiplerQVe = (gEKFInput.vNed[0] * gEKFInput.vNed[1]) / v3by4; + multiplerQVe *= multiplerQVe; + gKalmanFilter.P[STATE_VX][STATE_Q0] = multiplerQVn / q0q0 * gKalmanFilter.R[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VX][STATE_Q3] = multiplerQVn / q3q3 * gKalmanFilter.R[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_Q0] = multiplerQVe / q0q0 * gKalmanFilter.R[STATE_VY][STATE_VY]; + gKalmanFilter.P[STATE_VY][STATE_Q3] = multiplerQVe / q3q3 * gKalmanFilter.R[STATE_VY][STATE_VY]; + + + gKalmanFilter.P[STATE_Q0][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_Q0]; + gKalmanFilter.P[STATE_Q3][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_Q3]; + gKalmanFilter.P[STATE_Q0][STATE_VY] = gKalmanFilter.P[STATE_VY][STATE_Q0]; + gKalmanFilter.P[STATE_Q3][STATE_VY] = gKalmanFilter.P[STATE_VY][STATE_Q3]; +#endif + +#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/OpenIMU300RI/INS/lib/Core/Algorithm/src/WorldMagneticModel.c b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/WorldMagneticModel.c new file mode 100644 index 0000000..c3f9403 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/WorldMagneticModel.c @@ -0,0 +1,1285 @@ +/****************************************************************************** +* @file WorldMagneticModel.c +* +*******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#include +#include +#include +#include +#include +#include "osapi.h" +#include "WorldMagneticModel.h" +#include "WMMInternal.h" +#include "GpsData.h" + +#include "GlobalConstants.h" + + +WMMtype_Ellipsoid Ellip; +WMMtype_MagneticModel MagneticModel; + +// WMM Coefficients valid from 2015-2020 +float coeffs[91][6] = { { 0, 0, 0.0f, 0.0f, 0.0f, 0.0f }, // 1 + { 1, 0, -29438.5f, 0.0f, 10.7f, 0.0f }, // 2 + { 1, 1, -1501.1f, 4796.2f, 17.9f, -26.8f }, // 3 + { 2, 0, -2445.3f, 0.0f, -8.6f, 0.0f }, // 4 + { 2, 1, 3012.5f, -2845.6f, -3.3f, -27.1f }, // 5 + { 2, 2, 1676.6f, -642.0f, 2.4f, -13.3f }, // 6 + { 3, 0, 1351.1f, 0.0f, 3.1f, 0.0f }, // 7 + { 3, 1, -2352.3f, -115.3f, -6.2f, 8.4f }, // 8 + { 3, 2, 1225.6f, 245.0f, -0.4f, -0.4f }, // 9 + { 3, 3, 581.9f, -538.3f, -10.4f, 2.3f }, // 10 + { 4, 0, 907.2f, 0.0f, -0.4f, 0.0f }, // 1 + { 4, 1, 813.7f, 283.4f, 0.8f, -0.6f }, // 2 + { 4, 2, 120.3f, -188.6f, -9.2f, 5.3f }, // 3 + { 4, 3, -335.0f, 180.9f, 4.0f, 3.0f }, // 4 + { 4, 4, 70.3f, -329.5f, -4.2f, -5.3f }, // 5 + { 5, 0, -232.6f, 0.0f, -0.2f, 0.0f }, // 6 + { 5, 1, 360.1f, 47.4f, 0.1f, 0.4f }, // 7 + { 5, 2, 192.4f, 196.9f, -1.4f, 1.6f }, // 8 + { 5, 3, -141.0f, -119.4f, 0.0f, -1.1f }, // 9 + { 5, 4, -157.4f, 16.1f, 1.3f, 3.3f }, // 20 + { 5, 5, 4.3f, 100.1f, 3.8f, 0.1f }, // 1 + { 6, 0, 69.5f, 0.0f, -0.5f, 0.0f }, // 2 + { 6, 1, 67.4f, -20.7f, -0.2f, 0.0f }, // 3 + { 6, 2, 72.8f, 33.2f, -0.6f, -2.2f }, // 4 + { 6, 3, -129.8f, 58.8f, 2.4f, -0.7f }, // 5 + { 6, 4, -29.0f, -66.5f, -1.1f, 0.1f }, // 6 + { 6, 5, 13.2f, 7.3f, 0.3f, 1.0f }, // 7 + { 6, 6, -70.9f, 62.5f, 1.5f, 1.3f }, // 8 + { 7, 0, 81.6f, 0.0f, 0.2f, 0.0f }, // 9 + { 7, 1, -76.1f, -54.1f, -0.2f, 0.7f }, // 30 + { 7, 2, -6.8f, -19.4f, -0.4f, 0.5f }, // 1 + { 7, 3, 51.9f, 5.6f, 1.3f, -0.2f }, // 2 + { 7, 4, 15.0f, 24.4f, 0.2f, -0.1f }, // 3 + { 7, 5, 9.3f, 3.3f, -0.4f, -0.7f }, // 4 + { 7, 6, -2.8f, -27.5f, -0.9f, 0.1f }, // 5 + { 7, 7, 6.7f, -2.3f, 0.3f, 0.1f }, // 6 + { 8, 0, 24.0f, 0.0f, 0.0f, 0.0f }, // 7 + { 8, 1, 8.6f, 10.2f, 0.1f, -0.3f }, // 8 + { 8, 2, -16.9f, -18.1f, -0.5f, 0.3f }, // 9 + { 8, 3, -3.2f, 13.2f, 0.5f, 0.3f }, // 40 + { 8, 4, -20.6f, -14.6f, -0.2f, 0.6f }, // 1 + { 8, 5, 13.3f, 16.2f, 0.4f, -0.1f }, // 2 + { 8, 6, 11.7f, 5.7f, 0.2f, -0.2f }, // 3 + { 8, 7, -16.0f, -9.1f, -0.4f, 0.3f }, // 4 + { 8, 8, -2.0f, 2.2f, 0.3f, 0.0f }, // 5 + { 9, 0, 5.4f, 0.0f, 0.0f, 0.0f }, // 6 + { 9, 1, 8.8f, -21.6f, -0.1f, -0.2f }, // 7 + { 9, 2, 3.1f, 10.8f, -0.1f, -0.1f }, // 8 + { 9, 3, -3.1f, 11.7f, 0.4f, -0.2f }, // 9 + { 9, 4, 0.6f, -6.8f, -0.5f, 0.1f }, // 50 + { 9, 5, -13.3f, -6.9f, -0.2f, 0.1f }, // 1 + { 9, 6, -0.1f, 7.8f, 0.1f, 0.0f }, // 2 + { 9, 7, 8.7f, 1.0f, 0.0f, -0.2f }, // 3 + { 9, 8, -9.1f, -3.9f, -0.2f, 0.4f }, // 4 + { 9, 9, -10.5f, 8.5f, -0.1f, 0.3f }, // 5 + { 10, 0, -1.9f, 0.0f, 0.0f, 0.0f }, // 6 + { 10, 1, -6.5f, 3.3f, 0.0f, 0.1f }, // 7 + { 10, 2, 0.2f, -0.3f, -0.1f, -0.1f }, // 8 + { 10, 3, 0.6f, 4.6f, 0.3f, 0.0f }, // 9 + { 10, 4, -0.6f, 4.4f, -0.1f, 0.0f }, // 60 + { 10, 5, 1.7f, -7.9f, -0.1f, -0.2f }, // 1 + { 10, 6, -0.7f, -0.6f, -0.1f, 0.1f }, // 2 + { 10, 7, 2.1f, -4.1f, 0.0f, -0.1f }, // 3 + { 10, 8, 2.3f, -2.8f, -0.2f, -0.2f }, // 4 + { 10, 9, -1.8f, -1.1f, -0.1f, 0.1f }, // 5 + { 10, 10, -3.6f, -8.7f, -0.2f, -0.1f }, // 6 + { 11, 0, 3.1f, 0.0f, 0.0f, 0.0f }, // 7 + { 11, 1, -1.5f, -0.1f, 0.0f, 0.0f }, // 8 + { 11, 2, -2.3f, 2.1f, -0.1f, 0.1f }, // 9 + { 11, 3, 2.1f, -0.7f, 0.1f, 0.0f }, // 70 + { 11, 4, -0.9f, -1.1f, 0.0f, 0.1f }, // 1 + { 11, 5, 0.6f, 0.7f, 0.0f, 0.0f }, // 2 + { 11, 6, -0.7f, -0.2f, 0.0f, 0.0f }, // 3 + { 11, 7, 0.2f, -2.1f, 0.0f, 0.1f }, // 4 + { 11, 8, 1.7f, -1.5f, 0.0f, 0.0f }, // 5 + { 11, 9, -0.2f, -2.5f, 0.0f, -0.1f }, // 6 + { 11, 10, 0.4f, -2.0f, -0.1f, 0.0f }, // 7 + { 11, 11, 3.5f, -2.3f, -0.1f, -0.1f }, // 8 + { 12, 0, -2.0f, 0.0f, 0.1f, 0.0f }, // 9 + { 12, 1, -0.3f, -1.0f, 0.0f, 0.0f }, // 80 + { 12, 2, 0.4f, 0.5f, 0.0f, 0.0f }, // 1 + { 12, 3, 1.3f, 1.8f, 0.1f, -0.1f }, // 2 + { 12, 4, -0.9f, -2.2f, -0.1f, 0.0f }, // 3 + { 12, 5, 0.9f, 0.3f, 0.0f, 0.0f }, // 4 + { 12, 6, 0.1f, 0.7f, 0.1f, 0.0f }, // 5 + { 12, 7, 0.5f, -0.1f, 0.0f, 0.0f }, // 6 + { 12, 8, -0.4f, 0.3f, 0.0f, 0.0f }, // 7 + { 12, 9, -0.4f, 0.2f, 0.0f, 0.0f }, // 8 + { 12, 10, 0.2f, -0.9f, 0.0f, 0.0f }, // 9 + { 12, 11, -0.9f, -0.2f, 0.0f, 0.0f }, // 90 + { 12, 12, 0.0f, 0.7f, 0.0f, 0.0f } }; // 1 + +/** *************************************************************************** + * @name TaskDataAcquisition() CALLBACK main loop + * @brief Get the sensor data at the specified frequency (based on the + * configuration of the accelerometer rate-sensor). Process and provide + * information to the user via the UART or SPI. + * @param N/A + * @retval N/A + ******************************************************************************/ +void TaskWorldMagneticModel(void const *argument) +{ + // +#ifdef GPS + float B[3]; + float decl_deg; +#endif + + WMM_Initialize(); + + gWorldMagModel.decl_rad = (real)0.0; + gWorldMagModel.validSoln = FALSE; + gWorldMagModel.timeOfLastSoln = 0; + +// Can set this in "LG_To_INS_..." +// gWorldMagModel.timeOfLastSoln = LIMIT_DECL_EXPIRATION_TIME - +// gGpsDataPtr->itow; + + // + while(1) { + OS_Delay( 5000 ); +#ifdef GPS + if( gGpsDataPtr->gpsFixType ) + { + // caution - safety checks on the dates are fine, but we don't want + // to rely on these hard-coded dates in operation + // todo - get the GPS date from the external Novatel GPS + int month = gGpsDataPtr->GPSmonth; + int day = gGpsDataPtr->GPSday; + int year = gGpsDataPtr->GPSyear; + if ( year < 16 || year > 20 ) { + month = 6; + day = 21; + year = 17; + } else if ( day < 1 || day > 31 ) { + month = 6; + day = 21; + year = 17; + } else if ( month < 1 || month > 12 ) { + month = 6; + day = 21; + year = 17; + } + + WMM_GetMagVector( (float)gGpsDataPtr->lat, // [ deg ] + (float)gGpsDataPtr->lon, // [ deg ] + (float)(gGpsDataPtr->alt * 0.001), // [ km ] + month, + day, + year + 2000, + &B[0], // [ nT ] + &decl_deg ); // [ deg ] + gWorldMagModel.decl_rad = decl_deg * (real)DEG_TO_RAD; // [ rad ] + + gWorldMagModel.validSoln = TRUE; + gWorldMagModel.timeOfLastSoln = gGpsDataPtr->itow; + } +#endif + } + +} + + +//void WorldMagneticModel(void) +//{ +// // WorldMagneticModel; +// /* +// % WorldMagneticModel.m +// +// if( gGpsDataPtr.gpsFixType ), +// % WMM here +// declinationAngle_rad = ( 13 + 36 / 60 + 43/3600 ) * CONV.DEG_TO_RAD; +// +// initLat = gGpsDataPtr.pos(IND.X_AXIS); +// initLon = gGpsDataPtr.pos(IND.Y_AXIS); +// initAlt = gGpsDataPtr.pos(IND.Z_AXIS); +// +// end +// */ +// +// // VERY SIMPLE WMM (declination near San Jose only) +//// if (gGpsDataPtr->gpsFixType) { +//// gWorldMagModel.decl_rad = (real)( (13.0 + 40.0 / 60.0 + 29.0 / 3600.0) * D2R ); +//// } else { +//// gWorldMagModel.decl_rad = (real)0.0; +//// } +//} + + + + + +/** **************************************************************************** +* @name WMM_Initialize Sets default values for WMM subroutines. +* Trace: +* @param N/A +* @retval +* @brief use - very simple - only two exposed functions +* WMM_GetMagVector(float Lat, float Lon, float Alt, uint16_t Month, uint16_t +* Day, uint16_t Year, float B[3]); +* e.g. Iceland in may of 2012 = WMM_GetMagVector(65.0, -20.0, 0.0, 5, 5, 2012, B); +* Alt is above the WGS-84 Ellipsoid +* B is the NED (XYZ) magnetic vector in nTesla +* UPDATES : Ellip and MagneticModel +******************************************************************************/ +void WMM_Initialize(void) +{ + uint16_t i = 0; + + /// Sets WGS-84 parameters + Ellip.a = 6378.137f; ///< semi-major axis of the ellipsoid in km + Ellip.b = 6356.7523142f; ///< semi-minor axis of the ellipsoid in km + Ellip.fla = 1 / 298.257223563f; // flattening + Ellip.eps = (float)sqrt(1 - (Ellip.b*Ellip.b) / (Ellip.a*Ellip.a)); ///< first eccentricity + Ellip.epssq = (Ellip.eps * Ellip.eps); ///< first eccentricity squared + Ellip.re = 6371.2f; ///< Earth's radius in km + + /// Sets Magnetic Model parameters + MagneticModel.nMax = WMM_MAX_MODEL_DEGREES; + MagneticModel.nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; + MagneticModel.SecularVariationUsed = 0; + + // TODO: Really, Really needs to be read from a file - out of date in 2015 at latest + MagneticModel.EditionDate = (float)5.7863328170559505e-307; + MagneticModel.epoch = 2015.0f; + sprintf(MagneticModel.ModelName, "WMM-2015"); + + for (i = 0; i < NUMTERMS; i++){ + MagneticModel.Main_Field_Coeff_G[i] = coeffs[i][2]; + MagneticModel.Main_Field_Coeff_H[i] = coeffs[i][3]; + MagneticModel.Secular_Var_Coeff_G[i] = coeffs[i][4]; + MagneticModel.Secular_Var_Coeff_H[i] = coeffs[i][5]; + } +} + + +/** **************************************************************************** +* @name WMM_GetMagVector +* Trace: +* @param [in] Lat latitude +* @param [in] Lon longitude +* @param [in] AltEllipsoid - height above ellipsoid (km) +* @param [in] Month of the year mm +* @param [in] Day day of month dd +* @param [in] Year yyyy +* @param [out] B pointer to the GeoMagneticElements +* @retval N/A +******************************************************************************/ +WMMtype_MagneticModel TimedMagneticModel; +WMMtype_GeoMagneticElements GeoMagneticElements; +WMMtype_CoordSpherical CoordSpherical; +WMMtype_CoordGeodetic CoordGeodetic; +WMMtype_Date Date; + +void WMM_GetMagVector( float Lat, + float Lon, + float AltEllipsoid, + uint16_t Month, + uint16_t Day, + uint16_t Year, + float* B, + float* wmmDecl ) +{ + char Error_Message[255]; + + // WMMtype_MagneticModel TimedMagneticModel; + // WMMtype_CoordSpherical CoordSpherical; + // WMMtype_CoordGeodetic CoordGeodetic; + // WMMtype_Date Date; + // WMMtype_GeoMagneticElements GeoMagneticElements; + + CoordGeodetic.lambda = Lon; + CoordGeodetic.phi = Lat; + CoordGeodetic.HeightAboveEllipsoid = AltEllipsoid; + /// geodetic -> Spherical Equations: 17-18, WMM Technical report + WMM_GeodeticToSpherical( Ellip, + CoordGeodetic, + &CoordSpherical ); + + Date.Month = Month; + Date.Day = Day; + Date.Year = Year; + WMM_DateToYear(&Date, Error_Message); + + /// Time adjust the coefficients, Equation 19, WMM Technical report + WMM_TimelyModifyMagneticModel( Date, + &MagneticModel, + &TimedMagneticModel ); + /// Computes the geoMagnetic field elements and their time change + WMM_Geomag( Ellip, + CoordSpherical, + CoordGeodetic, + &TimedMagneticModel, + &GeoMagneticElements ); + + *(B+0) = GeoMagneticElements.X; // North + *(B+1) = GeoMagneticElements.Y; // East + *(B+2) = GeoMagneticElements.Z; // Vertical + // GeoMagneticElements.F is the field strength + + *wmmDecl = GeoMagneticElements.Decl; +} + +/** **************************************************************************** +* @name WMM_Geomag API +* @detail +* The main subroutine that calls a sequence of WMM sub-functions to calculate +* the magnetic field elements for a single point. The function expects the +* model coefficients and point coordinates as input and returns the magnetic +* field elements and their rate of change. Though, this subroutine can be +* called successively to calculate a time series, profile or grid of magnetic +* field, these are better achieved by the subroutine WMM_Grid. +* Trace: +* @param [in] Ellip +* @param [in] CoordSpherical +* @param [in] CoordGeodetic +* @param [in] TimedMagneticModel +* @param [out] GeoMagneticElements +* @retval N/A +* @brief CALLS: +* WMM_ComputeSphericalHarmonicVariables( ); Compute Spherical Harmonic variables +* WMM_AssociatedLegendreFunction(); Compute ALF +* WMM_Summation(); Accumulate the spherical harmonic coefficients +* WMM_SecVarSummation(); Sum the Secular Variation Coefficients +* WMM_RotateMagneticVector(); Map the computed Magnetic fields to Geodetic coordinates +* WMM_RotateMagneticVector(); Map the secular variation field components to Geodetic coordinates +* WMM_CalculateGeoMagneticElements(); Geomagnetic elements +* WMM_CalculateSecularVariation() secular variation of each of the Geomagnetic elements +******************************************************************************/ +WMMtype_LegendreFunction LegendreAllocate; +WMMtype_LegendreFunction *LegendreFunction = &LegendreAllocate; +WMMtype_SphericalHarmonicVariables SphVariables; +WMMtype_MagneticResults MagneticResultsSph; +WMMtype_MagneticResults MagneticResultsGeo; +WMMtype_MagneticResults MagneticResultsSphVar; +WMMtype_MagneticResults MagneticResultsGeoVar; + +uint16_t WMM_Geomag( WMMtype_Ellipsoid Ellip, + WMMtype_CoordSpherical CoordSpherical, + WMMtype_CoordGeodetic CoordGeodetic, + WMMtype_MagneticModel *TimedMagneticModel, + WMMtype_GeoMagneticElements *GeoMagneticElements ) +{ + WMM_ComputeSphericalHarmonicVariables( Ellip, + CoordSpherical, + TimedMagneticModel->nMax, + &SphVariables ); ///< Compute Spherical Harmonic variables + WMM_AssociatedLegendreFunction( CoordSpherical, + TimedMagneticModel->nMax, + LegendreFunction ); ///< Compute ALF + WMM_Summation( LegendreFunction, + TimedMagneticModel, + SphVariables, + CoordSpherical, + &MagneticResultsSph ); ///< Accumulate the spherical harmonic coefficients + WMM_SecVarSummation( LegendreFunction, + TimedMagneticModel, + SphVariables, + CoordSpherical, + &MagneticResultsSphVar ); ///< Sum the Secular Variation Coefficients + WMM_RotateMagneticVector( CoordSpherical, + CoordGeodetic, + MagneticResultsSph, + &MagneticResultsGeo ); ///< Map the computed Magnetic fields to Geodeitic coordinates + WMM_RotateMagneticVector( CoordSpherical, + CoordGeodetic, + MagneticResultsSphVar, + &MagneticResultsGeoVar ); ///< Map the secular variation field components to Geodetic coordinates + WMM_CalculateGeoMagneticElements( &MagneticResultsGeo, + GeoMagneticElements ); ///< Calculate the Geomagnetic elements, Equation 18 , WMM Technical report + WMM_CalculateSecularVariation( MagneticResultsGeoVar, + GeoMagneticElements ); ///< Calculate the secular variation of each of the Geomagnetic elements + + return TRUE; +} + +/** **************************************************************************** +* @name WMM_ComputeSphericalHarmonicVariables Computes Spherical variables +* @detail Variables computed are (a/r)^(n+2), cos_m(lambda) and sin_m(lambda) +* for spherical harmonic summations. (Equations 10-12 in the WMM +* Technical Report) +* Trace: +* @param [in] Ellipsoid struct a - semi-major axis of the ellipsoid +b - semi-minor axis of the ellipsoid +fla flattening +epssq first eccentricity squared +eps first eccentricity +re mean radius of ellipsoid +* @param [in] CoordSpherical struct: lambda - longitude +phig - geocentric latitude +r - distance from the center of the ellipsoid +* @param [in] nMax Maximum degree of spherical harmonic secular model +* @param [out] SphVariables Point to data structure with the following elements +* float RelativeRadiusPower[]; [earth_reference_radius_km sph. radius ]^n +* float cos_mlambda[]; cp(m) - cosine of (mspherical coord. longitude) +* float sin_mlambda[]; sp(m) - sine of (mspherical coord. longitude) +* @retval N/A +* @brief CALLS: none +******************************************************************************/ +uint16_t WMM_ComputeSphericalHarmonicVariables( WMMtype_Ellipsoid Ellip, + WMMtype_CoordSpherical CoordSpherical, + uint16_t nMax, + WMMtype_SphericalHarmonicVariables *SphVariables ) +{ + float cos_lambda; + float sin_lambda; + uint16_t m; + uint16_t n; + + // cos_lambda = (float)cos(DEG2RAD(CoordSpherical.lambda)); + // sin_lambda = (float)sin(DEG2RAD(CoordSpherical.lambda)); + cos_lambda = (float)cos(DEG_TO_RAD * CoordSpherical.lambda); + sin_lambda = (float)sin(DEG_TO_RAD * CoordSpherical.lambda); + /** for n = 0 ... model_order, compute (Radius of Earth / Spherical radius r)^(n+2) + for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ + SphVariables->RelativeRadiusPower[0] = (Ellip.re / CoordSpherical.r) * (Ellip.re / CoordSpherical.r); + for (n = 1; n <= nMax; n++) { + SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * + (Ellip.re / CoordSpherical.r); + } + + /** @brief Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax + cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b) + sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b) */ + SphVariables->cos_mlambda[0] = 1.0; + SphVariables->sin_mlambda[0] = 0.0; + + SphVariables->cos_mlambda[1] = cos_lambda; + SphVariables->sin_mlambda[1] = sin_lambda; + for (m = 2; m <= nMax; m++) + { + SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m - 1] * cos_lambda - + SphVariables->sin_mlambda[m - 1] * sin_lambda; + SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m - 1] * sin_lambda + + SphVariables->sin_mlambda[m - 1] * cos_lambda; + } + + return TRUE; +} /*WMM_ComputeSphericalHarmonicVariables*/ + +/** **************************************************************************** +* @name WMM_AssociatedLegendreFunction +* @brief Computes all of the Schmidt-semi normalized associated Legendre +* functions up to degree nMax. If nMax <= 16, function WMM_PcupLow is +* used. Otherwise WMM_PcupHigh is called. +* @param [in] CoordSpherical struct: lambda - longitude +* phig geocentric latitude +* r - distance from the center of the ellipsoid +* @param [in] nMax - Maximum degree of spherical harmonic secular model +* @param [in] LegendreFunction Ptr struct: +* Pcup - pointer to store Legendre Function ) +* dPcup - pointer to store Derivative of Lagendre function +* @param [out] LegendreFunction Calculated Legendre variables in the data structure +* @return always true +******************************************************************************/ +uint16_t WMM_AssociatedLegendreFunction( WMMtype_CoordSpherical CoordSpherical, + uint16_t nMax, + WMMtype_LegendreFunction *LegendreFunction ) +{ + float sin_phi; + uint16_t FLAG = 1; + + // sin_phi = (float)sin ( DEG2RAD( CoordSpherical.phig ) ); ///< sin (geocentric latitude) + sin_phi = (float)sin(DEG_TO_RAD * CoordSpherical.phig); ///< sin (geocentric latitude) + + if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10) ///< If nMax is less than 16 or at the poles + FLAG = WMM_PcupLow(LegendreFunction->Pcup, + LegendreFunction->dPcup, + sin_phi, + nMax); + else + FLAG = WMM_PcupHigh(LegendreFunction->Pcup, + LegendreFunction->dPcup, + sin_phi, + nMax); + if (FLAG == 0) // Error while computing Legendre variables + return FALSE; + + return TRUE; +} /*WMM_AssociatedLegendreFunction */ + +/** **************************************************************************** +* @name WMM_Summation +* @details Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate +* system using spherical harmonic summation. +* The vector Magnetic field is given by -grad V, where V is Geomagnetic +* scalar potential The gradient in spherical coordinates is given by: +*
	 dV ^     1 dV ^        1     dV ^
+*  grad V = -- r  +  - -- t  +  -------- -- p
+*			 dr       r dt       r sin(t) dp               
+* @param [in] LegendreFunction +* @param [in] MagneticModel +* @param [in] SphVariables +* @param [in] CoordSpherical +* @param [out] MagneticResults +* @return always true +* @brief CALLS : WMM_SummationSpecial +* @author Manoj Nair +* @date June, 2009 Manoj.C.Nair@Noaa.Gov +******************************************************************************/ +uint16_t WMM_Summation( WMMtype_LegendreFunction *LegendreFunction, + WMMtype_MagneticModel *MagneticModel, + WMMtype_SphericalHarmonicVariables SphVariables, + WMMtype_CoordSpherical CoordSpherical, + WMMtype_MagneticResults *MagneticResults ) +{ + uint16_t m, n, index; + float cos_phi; + + MagneticResults->Bz = 0.0; + MagneticResults->By = 0.0; + MagneticResults->Bx = 0.0; + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + + /*
	 nMax  	(n+2) 	  n     m            m           m
+            Bz =   -SUM (a/r)   (n+1) SUM  [g cos(m p) + h sin(m p)] P (sin(phi))
+            n=1      	      m=0   n            n           n  
*/ + /// Equation 12 in the WMM Technical report. Derivative with respect to radius. + MagneticResults->Bz -= SphVariables.RelativeRadiusPower[n] * + (MagneticModel->Main_Field_Coeff_G[index] * + SphVariables.cos_mlambda[m] + + MagneticModel->Main_Field_Coeff_H[index] * + SphVariables.sin_mlambda[m]) * + (float)(n + 1) * + LegendreFunction->Pcup[index]; + + /*
1 nMax  (n+2)    n     m            m           m
+            By =    SUM (a/r) (m)  SUM  [g cos(m p) + h sin(m p)] dP (sin(phi))
+            n=1             m=0   n            n           n  
*/ + /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ + MagneticResults->By += SphVariables.RelativeRadiusPower[n] * + (MagneticModel->Main_Field_Coeff_G[index] * + SphVariables.sin_mlambda[m] - + MagneticModel->Main_Field_Coeff_H[index] * + SphVariables.cos_mlambda[m]) * + (float)(m)* + LegendreFunction->Pcup[index]; + /*
   nMax  (n+2) n     m            m           m
+            Bx = - SUM (a/r)   SUM  [g cos(m p) + h sin(m p)] dP (sin(phi))
+            n=1         m=0   n            n           n  
*/ + /// Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. + + MagneticResults->Bx -= SphVariables.RelativeRadiusPower[n] * + (MagneticModel->Main_Field_Coeff_G[index] * + SphVariables.cos_mlambda[m] + + MagneticModel->Main_Field_Coeff_H[index] * + SphVariables.sin_mlambda[m]) * + LegendreFunction->dPcup[index]; + } + } + + // cos_phi = (float)cos( DEG2RAD( CoordSpherical.phig ) ); + cos_phi = (float)cos(DEG_TO_RAD * CoordSpherical.phig); + if (fabs(cos_phi) > 1.0e-10) { + MagneticResults->By = MagneticResults->By / cos_phi; + } + else { + /** @brief Special calculation for component - By - at Geographic poles. + * If the user wants to avoid using this function, please make sure that + * the latitude is not exactly +/-90. An option is to make use the function + * WMM_CheckGeographicPoles. */ + WMM_SummationSpecial(MagneticModel, + SphVariables, + CoordSpherical, + MagneticResults); + } + return TRUE; +}/*WMM_Summation */ + +/** **************************************************************************** +* @name WMM_SecVarSummation +* @details This Function sums the secular variation coefficients to get the secular +* variation of the Magnetic vector. +* @paaram [in] LegendreFunction +* @paaram [in] MagneticModel +* @paaram [in] SphVariables +* @paaram [in] CoordSpherical +* @paaram [out] MagneticResults +* @return always true +* @brief CALLS : WMM_SecVarSummationSpecial +******************************************************************************/ +uint16_t WMM_SecVarSummation( WMMtype_LegendreFunction *LegendreFunction, + WMMtype_MagneticModel *MagneticModel, + WMMtype_SphericalHarmonicVariables SphVariables, + WMMtype_CoordSpherical CoordSpherical, + WMMtype_MagneticResults *MagneticResults ) +{ + uint16_t m, n, index; + float cos_phi; + + MagneticModel->SecularVariationUsed = TRUE; + MagneticResults->Bz = 0.0; + MagneticResults->By = 0.0; + MagneticResults->Bx = 0.0; + + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + + /**
	 nMax  	(n+2) 	  n     m            m           m
+            Bz =   -SUM (a/r)   (n+1) SUM  [g cos(m p) + h sin(m p)] P (sin(phi))
+            n=1      	      m=0   n            n           n  
*/ + /* Derivative with respect to radius.*/ + MagneticResults->Bz -= SphVariables.RelativeRadiusPower[n] * + (MagneticModel->Secular_Var_Coeff_G[index] * + SphVariables.cos_mlambda[m] + + MagneticModel->Secular_Var_Coeff_H[index] * + SphVariables.sin_mlambda[m]) * + (float)(n + 1) * + LegendreFunction->Pcup[index]; + + /**
 1 nMax  (n+2)    n     m            m           m
+            By =    SUM (a/r) (m)  SUM  [g cos(m p) + h sin(m p)] dP (sin(phi))
+            n=1             m=0   n            n           n  
*/ + /// Derivative with respect to longitude, divided by radius. + MagneticResults->By += SphVariables.RelativeRadiusPower[n] * + (MagneticModel->Secular_Var_Coeff_G[index] * + SphVariables.sin_mlambda[m] - + MagneticModel->Secular_Var_Coeff_H[index] * + SphVariables.cos_mlambda[m]) * (float)(m)* + LegendreFunction->Pcup[index]; + /**
  nMax  (n+2) n     m            m           m
+            Bx = - SUM (a/r)   SUM  [g cos(m p) + h sin(m p)] dP (sin(phi))
+            n=1         m=0   n            n           n  
*/ + /// Derivative with respect to latitude, divided by radius. + MagneticResults->Bx -= SphVariables.RelativeRadiusPower[n] * + (MagneticModel->Secular_Var_Coeff_G[index] * + SphVariables.cos_mlambda[m] + + MagneticModel->Secular_Var_Coeff_H[index] * + SphVariables.sin_mlambda[m]) * + LegendreFunction->dPcup[index]; + } + } + // cos_phi = (float)cos( DEG2RAD( CoordSpherical.phig ) ); + cos_phi = (float)cos(DEG_TO_RAD * CoordSpherical.phig); + if (fabs(cos_phi) > 1.0e-10) { + MagneticResults->By = MagneticResults->By / cos_phi; + } + else { + /// Special calculation for component By at Geographic poles + WMM_SecVarSummationSpecial(MagneticModel, + SphVariables, + CoordSpherical, + MagneticResults); + } + return TRUE; +} /*WMM_SecVarSummation*/ + +/** **************************************************************************** +* @name WMM_RotateMagneticVector +* @details Rotate the Magnetic Vectors to Geodetic Coordinates +* @author Manoj Nair +* @date June, 2009 Manoj.C.Nair@Noaa.Gov +* @details Equation 16, WMM Technical report +* #param [in] CoordSpherical struct WMMtype_CoordSpherical: +* lambda - longitude +* phig - geocentric latitude +* r - distance from the center of the ellipsoid +* @param [in] CoordGeodetic struct WMMtype_CoordGeodetic: +* lambda - longitude +* phi - geodetic latitude +* HeightAboveEllipsoid - height above the ellipsoid (HaE) +* HeightAboveGeoid - height above the Geoid +* @param [in] MagneticResultsSph structure WMMtype_MagneticResults: +* Bx - North +* By - East +* Bz - Down +* @param[out] MagneticResultsGeo Ptr struct WMMtype_MagneticResults: +* Bx - North +* By - East +* Bz - Down +* @brief CALLS : none +******************************************************************************/ +uint16_t WMM_RotateMagneticVector( WMMtype_CoordSpherical CoordSpherical, + WMMtype_CoordGeodetic CoordGeodetic, + WMMtype_MagneticResults MagneticResultsSph, + WMMtype_MagneticResults *MagneticResultsGeo ) +{ + float Psi; + /// Difference between the spherical and Geodetic latitudes + Psi = (float)DEG_TO_RAD * (CoordSpherical.phig - CoordGeodetic.phi); + + /// Rotate spherical field components to the Geodeitic system + MagneticResultsGeo->Bz = MagneticResultsSph.Bx * (float)sin(Psi) + MagneticResultsSph.Bz * (float)cos(Psi); + MagneticResultsGeo->Bx = MagneticResultsSph.Bx * (float)cos(Psi) - MagneticResultsSph.Bz * (float)sin(Psi); + MagneticResultsGeo->By = MagneticResultsSph.By; + + return TRUE; +} /*WMM_RotateMagneticVector*/ + +/** **************************************************************************** +* @name WMM_CalculateGeoMagneticElements +* @brief Calculate all the Geomagnetic elements from X,Y and Z components +* @param [in] MagneticResultsGeo Ptr struct: +* Bx North +* By East +* Bz Down +* @param [out] GeoMagneticElements Ptr struct: +* Decl - Angle between the magnetic field vector and true north, + east +* Incl - Angle between the magnetic field vector and the horizontal plane, + down +* F - Magnetic Field Strength +* H - Horizontal Magnetic Field Strength +* X - Northern component of the magnetic field vector +* Y - Eastern component of the magnetic field vector +* Z - Downward component of the magnetic field vector +* @brief CALLS : none +******************************************************************************/ +uint16_t WMM_CalculateGeoMagneticElements( WMMtype_MagneticResults *MagneticResultsGeo, + WMMtype_GeoMagneticElements *GeoMagneticElements ) +{ + GeoMagneticElements->X = MagneticResultsGeo->Bx; + GeoMagneticElements->Y = MagneticResultsGeo->By; + GeoMagneticElements->Z = MagneticResultsGeo->Bz; + + GeoMagneticElements->H = (float)sqrt( MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + + MagneticResultsGeo->By * MagneticResultsGeo->By ); + GeoMagneticElements->F = (float)sqrt( GeoMagneticElements->H * GeoMagneticElements->H + + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz ); + // GeoMagneticElements->Decl = (float)RAD2DEG(atan2 (GeoMagneticElements->Y , GeoMagneticElements->X)); + // GeoMagneticElements->Incl = (float)RAD2DEG(atan2 (GeoMagneticElements->Z , GeoMagneticElements->H)); + GeoMagneticElements->Decl = (float)RAD_TO_DEG * atan2(GeoMagneticElements->Y, GeoMagneticElements->X); + GeoMagneticElements->Incl = (float)RAD_TO_DEG * atan2(GeoMagneticElements->Z, GeoMagneticElements->H); + + return TRUE; +} /*WMM_CalculateGeoMagneticElements */ + +/** **************************************************************************** +* @name WMM_CalculateSecularVariation +* @details Takes the Magnetic Variation in x, y, and z and uses it to calculate +* the secular variation of each of the Geomagnetic elements. +* @param [in] MagneticVariation struct +* Bx North +* By East +* Bz Down +* @param [out] MagneticElements Ptr struct +* Decldot - Yearly Rate of change in declination +* Incldot - Yearly Rate of change in inclination +* Fdot - Yearly rate of change in Magnetic field strength +* Hdot - Yearly rate of change in horizontal field strength +* Xdot - Yearly rate of change in the northern component +* Ydot - Yearly rate of change in the eastern component +* Zdot - Yearly rate of change in the downward component +* GVdot - Yearly rate of change in grid variation +* @brief CALLS : none +******************************************************************************/ +uint16_t WMM_CalculateSecularVariation( WMMtype_MagneticResults MagneticVariation, + WMMtype_GeoMagneticElements *MagneticElements ) +{ + MagneticElements->Xdot = MagneticVariation.Bx; + MagneticElements->Ydot = MagneticVariation.By; + MagneticElements->Zdot = MagneticVariation.Bz; + MagneticElements->Hdot = ( MagneticElements->X * MagneticElements->Xdot + + MagneticElements->Y * MagneticElements->Ydot ) / MagneticElements->H; ///< See equation 19 in the WMM technical report + MagneticElements->Fdot = ( MagneticElements->X * MagneticElements->Xdot + + MagneticElements->Y * MagneticElements->Ydot + + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F; + MagneticElements->Decldot = (float)RAD_TO_DEG * ( MagneticElements->X * MagneticElements->Ydot - + MagneticElements->Y * MagneticElements->Xdot ) / + ( MagneticElements->H * MagneticElements->H ); + MagneticElements->Incldot = (float)RAD_TO_DEG * ( MagneticElements->H * MagneticElements->Zdot - + MagneticElements->Z * MagneticElements->Hdot ) / + ( MagneticElements->F * MagneticElements->F ); + MagneticElements->GVdot = MagneticElements->Decldot; + + return TRUE; +} /*WMM_CalculateSecularVariation*/ + +/** *************************************************************************** +* @brief WMM_PcupHigh +* @details Evaluates all of the Schmidt-semi normalized associated Legendre +* functions up to degree nMax. The functions are initially scaled by +* 10^280 sin^m in order to minimize the effects of underflow at large m +* near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). +* Note that this function performs the same operation as WMM_PcupLow. +* However this function also can be used for high degree (large nMax) models. +* param [in] nMax - Maximum spherical harmonic degree to compute. +* @param [in] x - cos(latitude) or sin(latitude). +* @param [out] Pcup - A vector of all associated Legendgre polynomials evaluated +* at x up to nMax. The length must by greater or equal to +* (nMax+1)*(nMax+2)/2. +* @param [out] dPcup - Derivative of Pcup(x) with respect to latitude +* @brief CALLS : none +* @details Notes: +* Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. +* Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov +* Change from the previous version +* The previous version computes the derivatives as +* dP(n,m)(x)/dx, where x = sin(latitude) (or cos(colatitude) ). +* However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. +* Hence the derivatives are multiplied by sin(latitude). +* Removed the options for CS phase and normalizations. +* +* Note: In geomagnetism, the derivatives of ALF are usually found with +* respect to the colatitudes. Here the derivatives are found with respect +* to the latitude. The difference is a sign reversal for the derivative of +* the Associated Legendre Functions. +* +* The derivatives can't be computed for latitude = |90| degrees. +******************************************************************************/ +float f1[NUMPCUP], f2[NUMPCUP], PreSqr[NUMPCUP]; + +uint16_t WMM_PcupHigh( float *Pcup, + float *dPcup, + float x, + uint16_t nMax ) +{ + float pm2, pm1, pmm, plm, rescalem, z, scalef; + // float f1[NUMPCUP], f2[NUMPCUP], PreSqr[NUMPCUP]; + uint16_t k, kstart, m, n; + + if (fabs(x) == 1.0) { + // printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); + return FALSE; + } + + scalef = (float)1.0e-280; + + for (n = 0; n <= 2 * nMax + 1; ++n) { + PreSqr[n] = (float)sqrt((float)(n)); + } + + k = 2; + + for (n = 2; n <= nMax; n++) { + k = k + 1; + f1[k] = (float)(2 * n - 1) / (float)(n); + f2[k] = (float)(n - 1) / (float)(n); + for (m = 1; m <= n - 2; m++) { + k++; + f1[k] = (float)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - m]; + f2[k] = PreSqr[n - m - 1] * PreSqr[n + m - 1] / PreSqr[n + m] / PreSqr[n - m]; + } + k = k + 2; + } + + /// z = sin (geocentric latitude) + z = (float)sqrt((1.0f - x) * (1.0f + x)); + pm2 = 1.0f; + Pcup[0] = 1.0f; + dPcup[0] = 0.0f; + if (nMax == 0) + return FALSE; + pm1 = x; + Pcup[1] = pm1; + dPcup[1] = z; + k = 1; + + for (n = 2; n <= nMax; n++) { + k += n; + plm = f1[k] * x * pm1 - f2[k] * pm2; + Pcup[k] = plm; + dPcup[k] = (float)(n)* (pm1 - x * plm) / z; + pm2 = pm1; + pm1 = plm; + } + + pmm = PreSqr[2] * scalef; + rescalem = 1.0f / scalef; + kstart = 0; + + for (m = 1; m <= nMax - 1; ++m) { + rescalem *= z; + + /// Calculate Pcup(m,m) + kstart = kstart + m + 1; + pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m]; + Pcup[kstart] = pmm*rescalem / PreSqr[2 * m + 1]; + dPcup[kstart] = -((float)(m)* x * Pcup[kstart] / z); + pm2 = pmm / PreSqr[2 * m + 1]; + /// Calculate Pcup(m+1,m) + k = kstart + m + 1; + pm1 = x * PreSqr[2 * m + 1] * pm2; + Pcup[k] = pm1 * rescalem; + dPcup[k] = ((pm2*rescalem) * PreSqr[2 * m + 1] - x * (float)(m + 1) * Pcup[k]) / z; + /// Calculate Pcup(n,m) + for (n = m + 2; n <= nMax; ++n) { + k += n; + plm = x * f1[k] * pm1 - f2[k] * pm2; + Pcup[k] = plm * rescalem; + dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (float)(n)* x * Pcup[k]) / z; + pm2 = pm1; + pm1 = plm; + } + } + + /// Calculate Pcup(nMax,nMax) + rescalem = rescalem * z; + kstart = kstart + m + 1; + pmm = pmm / PreSqr[2 * nMax]; + Pcup[kstart] = pmm * rescalem; + dPcup[kstart] = -(float)(nMax)* x * Pcup[kstart] / z; + + return TRUE; +} /* WMM_PcupHigh */ + +/** *************************************************************************** +* @name WMM_PcupLow +* @brief function evaluates all of the Schmidt-semi normalized associated Legendre +* functions up to degree nMax. +* @param [in] nMax -Maximum spherical harmonic degree to compute. +* @param [in] x - cos(colatitude) or sin(latitude). +* @param [out] Pcup - A vector of all associated Legendgre polynomials +* evaluated at x up to nMax. +* @param [in] dPcup - Derivative of Pcup(x) with respect to latitude +* @details Notes: Overflow may occur if nMax > 20 , especially for +* high-latitudes. Use WMM_PcupHigh for large nMax. +* @author Manoj Nair +* @date June, 2009 . Manoj.C.Nair@Noaa.Gov. +* @brief +* Note: In geomagnetism, the derivatives of ALF are usually found with +* respect to the colatitudes. Here the derivatives are found with respect +* to the latitude. The difference is a sign reversal for the derivative of +* the Associated Legendre Functions. +******************************************************************************/ +float schmidtQuasiNorm[NUMPCUP]; + +uint16_t WMM_PcupLow( float *Pcup, + float *dPcup, + float x, + uint16_t nMax ) +{ + uint16_t n, m, index, index1, index2; + float k, z; + // float schmidtQuasiNorm[NUMPCUP]; + + Pcup[0] = 1.0; + dPcup[0] = 0.0; + ///sin (geocentric latitude) - sin_phi + z = (float)sqrt((1.0f - x) * (1.0f + x)); + + /// First, Compute the Gauss-normalized associated Legendre functions + for (n = 1; n <= nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + if (n == m) { + index1 = (n - 1) * n / 2 + m - 1; + Pcup[index] = z * Pcup[index1]; + dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; + } + else if (n == 1 && m == 0) { + index1 = (n - 1) * n / 2 + m; + Pcup[index] = x * Pcup[index1]; + dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; + } + else if (n > 1 && n != m) { + index1 = (n - 2) * (n - 1) / 2 + m; + index2 = (n - 1) * n / 2 + m; + if (m > n - 2) { + Pcup[index] = x * Pcup[index2]; + dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; + } + else { + k = (float)(((n - 1) * (n - 1)) - (m * m)) / + (float)((2 * n - 1) * (2 * n - 3)); + Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; + dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; + } + } + } + } + + /** Compute the ration between the Gauss-normalized associated Legendre + functions and the Schmidt quasi-normalized version. This is equivalent to + sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ + schmidtQuasiNorm[0] = 1.0; + for (n = 1; n <= nMax; n++) { + index = (n * (n + 1) / 2); + index1 = (n - 1) * n / 2; + /* for m = 0 */ + schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (float)(2 * n - 1) / (float)n; + + for (m = 1; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + index1 = (n * (n + 1) / 2 + m - 1); + schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (float)sqrt((float)((n - m + 1) * (m == 1 ? 2 : 1)) / + (float)(n + m)); + } + } + + /** Converts the Gauss-normalized associated Legendre + functions to the Schmidt quasi-normalized version using pre-computed + relation stored in the variable schmidtQuasiNorm */ + for (n = 1; n <= nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; + dPcup[index] = -dPcup[index] * schmidtQuasiNorm[index]; + /** The sign is changed since the new WMM routines use derivative + with respect to latitude instead of co-latitude */ + } + } + + return TRUE; +} /*WMM_PcupLow */ + +/** *************************************************************************** +* @name WMM_SummationSpecial +* @brief Special calculation for the component By at Geographic poles. +* @author Manoj Nair +* @date June, 2009 manoj.c.nair@noaa.gov +* @param [in] MagneticModel +* @param [in] SphVariables +* @param [in] CoordSpherical +* @param [out] MagneticResults +* @briefCALLS : none +* @details See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM +* Technical report +******************************************************************************/ +float PcupS[NUMPCUPS]; +uint16_t WMM_SummationSpecial( WMMtype_MagneticModel *MagneticModel, + WMMtype_SphericalHarmonicVariables SphVariables, + WMMtype_CoordSpherical CoordSpherical, + WMMtype_MagneticResults *MagneticResults ) +{ + uint16_t n, index; + float k, sin_phi, schmidtQuasiNorm1, schmidtQuasiNorm2, schmidtQuasiNorm3; + // float PcupS[NUMPCUPS]; + + PcupS[0] = 1; + schmidtQuasiNorm1 = 1.0; + MagneticResults->By = 0.0; + // sin_phi = (float)sin( (float)DEG2RAD( CoordSpherical.phig ) ); + sin_phi = (float)sin((float)DEG_TO_RAD * CoordSpherical.phig); + + for (n = 1; n <= MagneticModel->nMax; n++) + { + /** Compute the ratio between the Gauss-normalized associated Legendre + functions and the Schmidt quasi-normalized version. This is + equivalent to sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ + index = (n * (n + 1) / 2 + 1); + schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; + schmidtQuasiNorm3 = schmidtQuasiNorm2 * (float)sqrt((float)(n * 2) / (float)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } + else { + k = (float)(((n - 1) * (n - 1)) - 1) / + (float)((2 * n - 1) * (2 * n - 3)); + PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; + } + + /**
 1 nMax  (n+2)    n     m            m           m
+        By =    SUM (a/r) (m)  SUM  [g cos(m p) + h sin(m p)] dP (sin(phi))
+        n=1             m=0   n            n           n  
*/ + /* Equation 11 in the WMM Technical report. Derivative with respect to + longitude, divided by radius. */ + MagneticResults->By += SphVariables.RelativeRadiusPower[n] * + (MagneticModel->Main_Field_Coeff_G[index] * + SphVariables.sin_mlambda[1] - + MagneticModel->Main_Field_Coeff_H[index] * + SphVariables.cos_mlambda[1]) * + PcupS[n] * + schmidtQuasiNorm3; + } + return TRUE; +}/*WMM_SummationSpecial */ + +/** *************************************************************************** +* @name WMM_SecVarSummationSpecial +* @brief Special calculation for the secular variation summation at the poles. +* @param [in] MagneticModel +* @param [in] SphVariables +* @param [in] CoordSpherical +* @param [out] MagneticResults +* @brief CALLS : none +******************************************************************************/ +//float PcupS[NUMPCUPS]; +uint16_t WMM_SecVarSummationSpecial( WMMtype_MagneticModel *MagneticModel, + WMMtype_SphericalHarmonicVariables SphVariables, + WMMtype_CoordSpherical CoordSpherical, + WMMtype_MagneticResults *MagneticResults ) +{ + uint16_t n, index; + float k, sin_phi, schmidtQuasiNorm1, schmidtQuasiNorm2, schmidtQuasiNorm3; + // float PcupS[NUMPCUPS]; + + PcupS[0] = 1; + schmidtQuasiNorm1 = 1.0; + MagneticResults->By = 0.0; + // sin_phi = (float)sin( (float)DEG2RAD( CoordSpherical.phig ) ); + sin_phi = (float)sin((float)DEG_TO_RAD * CoordSpherical.phig); + + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { + index = (n * (n + 1) / 2 + 1); + schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; + schmidtQuasiNorm3 = schmidtQuasiNorm2 * (float)sqrt((float)(n * 2) / (float)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } + else { + k = (float)(((n - 1) * (n - 1)) - 1) / + (float)((2 * n - 1) * (2 * n - 3)); + PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; + } + + /**
  1 nMax  (n+2)    n     m            m           m
+        By =    SUM (a/r) (m)  SUM  [g cos(m p) + h sin(m p)] dP (sin(phi))
+        n=1             m=0   n            n           n  
*/ + /// Derivative with respect to longitude, divided by radius. + MagneticResults->By += SphVariables.RelativeRadiusPower[n] * + (MagneticModel->Secular_Var_Coeff_G[index] * + SphVariables.sin_mlambda[1] - + MagneticModel->Secular_Var_Coeff_H[index] * + SphVariables.cos_mlambda[1]) * + PcupS[n] * + schmidtQuasiNorm3; + } + return TRUE; +}/*SecVarSummationSpecial*/ + + +/** *************************************************************************** +* @name WMM_TimelyModifyMagneticModel +* @details Time change the Model coefficients from the base year of the model using +* secular variation coefficients. Store the coefficients of the static model +* with their values advanced from epoch t0 to epoch t. Copy the SV +* coefficients. If input "t" is the same as "t0", then this is merely a copy +* operation. If the address of "TimedMagneticModel" is the same as the address +* of "MagneticModel", then this procedure overwrites the given item "MagneticModel". +* @param [in] UserDate +* @param [in] MagneticModel +* @param [out] TimedMagneticModel +* @return N/A +* @brief CALLS : none +******************************************************************************/ +void WMM_TimelyModifyMagneticModel( WMMtype_Date UserDate, + WMMtype_MagneticModel *MagneticModel, + WMMtype_MagneticModel *TimedMagneticModel ) +{ + uint16_t n, m, index, a, b; + + TimedMagneticModel->EditionDate = MagneticModel->EditionDate; + TimedMagneticModel->epoch = MagneticModel->epoch; + TimedMagneticModel->nMax = MagneticModel->nMax; + TimedMagneticModel->nMaxSecVar = MagneticModel->nMaxSecVar; + a = TimedMagneticModel->nMaxSecVar; + b = (a * (a + 1) / 2 + a); + strcpy(TimedMagneticModel->ModelName, MagneticModel->ModelName); + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + if (index <= b) { + TimedMagneticModel->Main_Field_Coeff_H[index] = MagneticModel->Main_Field_Coeff_H[index] + + (UserDate.DecimalYear - MagneticModel->epoch) * + MagneticModel->Secular_Var_Coeff_H[index]; + TimedMagneticModel->Main_Field_Coeff_G[index] = MagneticModel->Main_Field_Coeff_G[index] + + (UserDate.DecimalYear - MagneticModel->epoch) * + MagneticModel->Secular_Var_Coeff_G[index]; + TimedMagneticModel->Secular_Var_Coeff_H[index] = MagneticModel->Secular_Var_Coeff_H[index]; // We need a copy of the secular var coeff to calculate secular change + TimedMagneticModel->Secular_Var_Coeff_G[index] = MagneticModel->Secular_Var_Coeff_G[index]; + } + else { + TimedMagneticModel->Main_Field_Coeff_H[index] = MagneticModel->Main_Field_Coeff_H[index]; + TimedMagneticModel->Main_Field_Coeff_G[index] = MagneticModel->Main_Field_Coeff_G[index]; + } + } + } +} /* WMM_TimelyModifyMagneticModel */ + +/** *************************************************************************** +* @name WMM_DateToYear +* @details Converts a given calendar date into a decimal year +* @param [out] CalendarDate +* @param [out] Error +* @param [out] TimedMagneticModel +* @return always 1 +* @brief CALLS : none +******************************************************************************/ +uint16_t MonthDays[13] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; + +uint16_t WMM_DateToYear( WMMtype_Date *CalendarDate, + char *Error ) +{ + uint16_t temp = 0; /// Total number of days + uint16_t ExtraDay = 0; + uint16_t i; + + if ( (CalendarDate->Year % 4 == 0 && CalendarDate->Year % 100 != 0) || + CalendarDate->Year % 400 == 0) + { + ExtraDay = 1; + } else { + ExtraDay = 0; + } + MonthDays[2] += ExtraDay; + + /******************Validation********************************/ + if (CalendarDate->Month <= 0 || CalendarDate->Month > 12) + { + strcpy(Error, "\nError: The Month entered is invalid, valid months are '1 to 12'\n"); + return 0; + } + + if (CalendarDate->Day <= 0 || CalendarDate->Day > MonthDays[CalendarDate->Month]) + { + // printf("\nThe number of days in month %d is %d\n", CalendarDate->Month, MonthDays[CalendarDate->Month]); + strcpy(Error, "\nError: The day entered is invalid\n"); + return 0; + } + /****************Calculation of t***************************/ + for (i = 1; i <= CalendarDate->Month; i++) { + temp += MonthDays[i - 1]; + } + temp += CalendarDate->Day; + CalendarDate->DecimalYear = CalendarDate->Year + (temp - 1) / (365.0f + ExtraDay); + + return 1; +} /*WMM_DateToYear*/ + +/** *************************************************************************** +* @name WMM_GeodeticToSpherical +* @details Converts Geodetic coordinates to Spherical coordinates Convert +* geodetic coordinates, (defined by the WGS-84 reference ellipsoid), to Earth +* Centred Earth Fixed Cartesian coordinates, and then to spherical coordinates. +* @param [in] Ellip +* @param [in] CoordGeodetic +* @param [out] CoordSpherical +* @return always 1 +* @brief CALLS : none +******************************************************************************/ +void WMM_GeodeticToSpherical( WMMtype_Ellipsoid Ellip, + WMMtype_CoordGeodetic CoordGeodetic, + WMMtype_CoordSpherical *CoordSpherical ) +{ + float CosLat, SinLat, rc, xp, zp; ///< all local variables + + // CosLat = (float)cos( (float)DEG2RAD(CoordGeodetic.phi) ); + // SinLat = (float)sin( (float)DEG2RAD(CoordGeodetic.phi) ); + CosLat = (float)cos((float)DEG_TO_RAD * CoordGeodetic.phi); + SinLat = (float)sin((float)DEG_TO_RAD * CoordGeodetic.phi); + + /// compute the local radius of curvature on the WGS-84 reference ellipsoid + rc = Ellip.a / (float)sqrt(1.0f - Ellip.epssq * SinLat * SinLat); // [ km ] + + /// compute ECEF Cartesian coordinates of specified point (for longitude=0) + xp = (rc + CoordGeodetic.HeightAboveEllipsoid) * CosLat; + zp = (rc * (1.0f - Ellip.epssq) + CoordGeodetic.HeightAboveEllipsoid) * SinLat; + + /// compute spherical radius and angle lambda and phi of specified point + CoordSpherical->r = (float)sqrt(xp*xp + zp*zp); + // CoordSpherical->phig = (float)RAD2DEG(asin(zp / CoordSpherical->r)); ///< geocentric latitude + CoordSpherical->phig = (float)RAD_TO_DEG * asin(zp / CoordSpherical->r); ///< geocentric latitude + CoordSpherical->lambda = CoordGeodetic.lambda; ///< longitude +}// WMM_GeodeticToSpherical diff --git a/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/algorithm.c b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/algorithm.c new file mode 100644 index 0000000..c5a2087 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Algorithm/src/algorithm.c @@ -0,0 +1,297 @@ +/****************************************************************************** + * @file algorithm.c + * @brief Top level algorithm configurations and functions. + * All top-level algorithm configurations and functions are here, including + * algorithm state, algorithm configurations, algorithm input and output. + * @author Dong Xiaoguang + * @date 2019.05.09 + * @version V1.0.0 + *----------------------------------------------------------------------------- + * Change History + * | | | + * ---------------------------------------------------------------------------- + * 2019.05.09 | v1.0.0 | Dong Xiaoguang | Create file + * ---------------------------------------------------------------------------- +******************************************************************************/ + +#include +#include + +#include "SensorNoiseParameters.h" +#include "platformAPI.h" +#include "algorithm.h" +#include "AlgorithmLimits.h" +#include "algorithmAPI.h" + +AlgorithmStruct gAlgorithm; +AlgoStatus gAlgoStatus; + + +void InitializeAlgorithmStruct(uint8_t callingFreq) +{ + 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){ + // IMU case + callingFreq = FREQ_200_HZ; + } + gAlgorithm.callingFreq = callingFreq; + + // Set dt based on the calling frequency of the EKF + if (gAlgorithm.callingFreq == FREQ_100_HZ) + { + gAlgorithm.dt = (real)(0.01); + gAlgorithm.dITOW = 10; + } + else if (gAlgorithm.callingFreq == FREQ_200_HZ) + { + gAlgorithm.dt = (real)(0.005); + gAlgorithm.dITOW = 5; + } + else + { + while (1); + } + + // Set up other timing variables + gAlgorithm.dtOverTwo = (real)(0.5) * gAlgorithm.dt; + gAlgorithm.dtSquared = gAlgorithm.dt * gAlgorithm.dt; + gAlgorithm.sqrtDt = sqrtf(gAlgorithm.dt); + + // Set the algorithm duration periods + gAlgorithm.Duration.Stabilize_System = (uint32_t)(gAlgorithm.callingFreq * STABILIZE_SYSTEM_DURATION); + gAlgorithm.Duration.Initialize_Attitude = (uint32_t)(gAlgorithm.callingFreq * INITIALIZE_ATTITUDE_DURATION); + gAlgorithm.Duration.High_Gain_AHRS = (uint32_t)(gAlgorithm.callingFreq * HIGH_GAIN_AHRS_DURATION); + gAlgorithm.Duration.Low_Gain_AHRS = (uint32_t)(gAlgorithm.callingFreq * LOW_GAIN_AHRS_DURATION); + + // Set the initial state of the EKF + gAlgorithm.state = STABILIZE_SYSTEM; + gAlgorithm.stateTimer = gAlgorithm.Duration.Stabilize_System; + + // Turn-switch variable + gAlgorithm.filteredYawRate = (real)0.0; + + // Tell the algorithm to apply the declination correction to the heading + // (at startup in AHRS, do not apply. After INS becomes healthy, apply, + // even in AHRS, but this condition shouldn't last forever. Question: + // how long to keep this set TRUE after GPS in invalid?) + gAlgorithm.applyDeclFlag = FALSE; + + gAlgorithm.insFirstTime = TRUE; + gAlgorithm.headingIni = HEADING_UNINITIALIZED; + + //gAlgorithm.magAlignUnderway = FALSE; // Set and reset in mag-align code + + // Increment at 100 Hz in EKF_Algorithm; sync with GPS itow when valid. + gAlgorithm.itow = 0; + + // Limit is compared to ITOW. Time must be in [msec]. + gAlgorithm.Limit.maxGpsDropTime = LIMIT_MAX_GPS_DROP_TIME * 1000; + gAlgorithm.Limit.maxReliableDRTime = LIMIT_RELIABLE_DR_TIME * 1000; + + // Limit is compared to count (incremented upon loop through + // taskDataAcquisition). Time must be in [count] based on ODR. + gAlgorithm.Limit.Free_Integration_Cntr = gAlgorithm.callingFreq * LIMIT_FREE_INTEGRATION_CNTR; + + // Linear acceleration switch limits (level and time) + gAlgorithm.Limit.accelSwitch = (real)(0.012); // [g] + gAlgorithm.Limit.linAccelSwitchDelay = (uint32_t)(2.0 * gAlgorithm.callingFreq); + + // Innovation error limits for EKF states + gAlgorithm.Limit.Innov.positionError = (real)270.0; + gAlgorithm.Limit.Innov.velocityError = (real)27.0; + gAlgorithm.Limit.Innov.attitudeError = (real)SIX_DEGREES_IN_RAD; + + // Five-hertz LPF (corresponding integer value found in PredictFunctions.c) + // Replace with a function that computes the coefficients. Value (below) will + // then be the cutoff frequency. + gAlgorithm.linAccelLPFType = 1; + + // 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; + + // Set the turn-switch threshold to a default value in [deg/sec] + gAlgorithm.turnSwitchThreshold = 6.0; + + // default lever arm and point of interest + gAlgorithm.leverArmB[X_AXIS] = 0.0; + gAlgorithm.leverArmB[Y_AXIS] = 0.0; + gAlgorithm.leverArmB[Z_AXIS] = 0.0; + gAlgorithm.pointOfInterestB[X_AXIS] = 0.0; + gAlgorithm.pointOfInterestB[Y_AXIS] = 0.0; + gAlgorithm.pointOfInterestB[Z_AXIS] = 0.0; + + // For most vehicles, the velocity is always along the body x axis + gAlgorithm.velocityAlwaysAlongBodyX = TRUE; + + // get IMU specifications + 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; + gAlgorithm.imuSpec.maxBiasW = (real)MAX_BW; + gAlgorithm.imuSpec.vrw = (real)VRW_300ZA; + 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; + + // 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 = 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)); +} + +void GetAlgoStatus(AlgoStatus *algoStatus) +{ + algoStatus->all = gAlgoStatus.all; +} + + +void setAlgorithmExeFreq(int freq) +{ + gAlgorithm.callingFreq = freq; + +} + +void updateAlgorithmTimings(int corr, uint32_t tmrVal ) +{ + // Set the counter to a value that corresponds to seconds after TIM2 is + // called and just after the sensors are read. + gAlgorithm.counter = (uint16_t)( 1.25e-3 * ( ( corr + (uint16_t)(1.334489891239070E-05 * tmrVal) ) << 16 ) ); + // Increment the timer output value (FIXME: is this in the right spot? + // Should it be in the algorithm if-statement below?) + gAlgorithm.timer = gAlgorithm.timer + gAlgorithm.dITOW; +} + +uint32_t getAlgorithmTimer() +{ + return gAlgorithm.timer; +} + +uint16_t getAlgorithmCounter() +{ + return gAlgorithm.counter; +} + +uint16_t getAlgorithmFrequency() +{ + return gAlgorithm.callingFreq; +} + +uint32_t getAlgorithmITOW() +{ + return gAlgorithm.itow; +} + +void setLeverArm( real leverArmBx, real leverArmBy, real leverArmBz ) +{ + gAlgorithm.leverArmB[0] = leverArmBx; + gAlgorithm.leverArmB[1] = leverArmBy; + gAlgorithm.leverArmB[2] = leverArmBz; +} + +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/OpenIMU300RI/INS/lib/Core/Buffer/include/buffer.h b/examples/OpenIMU300RI/INS/lib/Core/Buffer/include/buffer.h new file mode 100644 index 0000000..678144b --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Buffer/include/buffer.h @@ -0,0 +1,62 @@ +/***************************************************************************** + * buffer operations + * @file buffer.h + * @brief ring buffer + * @author Dong Xiaoguang + * @version 1.0 + * @date 20180813 + *****************************************************************************/ + + +#ifndef BUFFER_H_INCLUDED +#define BUFFER_H_INCLUDED + +#include "GlobalConstants.h" + + +//======================data struct definitions====================== +typedef struct _buffer +{ + real *d; // data storage, each column represents a set of data + int m; //row number + int n; //column number + int i; //index for data being put into the buffer, -1 means buffer is empty + int full; //1 means buffer is full, 0 not + int num; //data number in the buffer +} Buffer; + +//------------------------------------------------------------------- +// new a buffer +// input: bf--pointer to the buffer +// d--pointer to the memory for data storage +// m--row number(length of each set of data) +// n--column number(number of sets of data) +// output: +// return: +void bfNew(Buffer *bf, real*d, int m, int n); + +//------------------------------------------------------------------- +// put data into the buffer +// input: bf--buffer pointer +// d--pointer to the data being put into the buffer +// output: +// return: +void bfPut(Buffer *bf, real* d); + +//------------------------------------------------------------------- +// read data from the buffer +// input: bf--buffer pointer +// idx--data index, idx=0 means the latest data, idx=1 means data before the latest... +// output: +// return: 1 menas OK, 0 menas idx out of bound +int bfGet(Buffer *bf,real *d, int idx); + +//------------------------------------------------------------------- +// clear the buffer +// input: bf--buffer pointer +// output: +// return: +void bfClear(Buffer *bf); + + +#endif // BUFFER_H_INCLUDED diff --git a/examples/OpenIMU300RI/INS/lib/Core/Buffer/src/buffer.c b/examples/OpenIMU300RI/INS/lib/Core/Buffer/src/buffer.c new file mode 100644 index 0000000..0b7ff40 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Buffer/src/buffer.c @@ -0,0 +1,83 @@ +#include "buffer.h" + + +void bfNew(Buffer *bf, real*d, int m, int n) +{ + bf->d = d; + bf->m = m; + bf->n = n; + bf->i = -1; + bf->full = 0; + bf->num = 0; +} + +void bfPut(Buffer *bf, real* d) +{ + int i; + int m = bf->m; // row number + int n = bf->n; // column number + + bf->i += 1; // move to the next position + if(bf->full == 0) // buffer is not full + { + bf->num = bf->i + 1; + if(bf->i == n-1) // buffer become full + { + bf->full = 1; + } + } + if(bf->i==n) //buffer is full, overwrite the oldest data + { + bf->i = 0; + } + // put data into buffer + for(i=0;id[i*n+bf->i] = d[i]; + } +} + +int bfGet(Buffer *bf,real *d, int idx) +{ + int i; + int m,n; + m = bf->m; + n = bf->n; + if(idx>=n) // idx exceeds the max column number + { + for(i=0;ifull)// buffer is full + { + idx = bf->i - idx; + if(idx<0) + idx += n; + for(i=0;id[i*n+idx]; + } + else//buffer is not full + { + idx = bf->i - idx; + if(idx<0)// idx exceeds the storage range + { + for(i=0;id[i*n+idx]; + } + } + return 1; +} + +void bfClear(Buffer *bf) +{ + bf->i = -1; + bf->full = 0; + bf->num = 0; +} diff --git a/examples/OpenIMU300RI/INS/lib/Core/Common/include/GlobalConstants.h b/examples/OpenIMU300RI/INS/lib/Core/Common/include/GlobalConstants.h new file mode 100644 index 0000000..b7ec1e5 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Common/include/GlobalConstants.h @@ -0,0 +1,192 @@ +/******************************************************************************* + * File: GlobalConstants.h + *******************************************************************************/ +/******************************************************************************* +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 GLOBALCONSTANTS_H +#define GLOBALCONSTANTS_H + +#define BMI_RS 1 +#define MAXIM_RS 2 + +#ifndef NULL +#define NULL 0 +#endif + +#define APPLY 1 +#define REMOVE 0 + +typedef unsigned char bool; +typedef unsigned char BOOL; + +#ifndef true +#define true 1 +#define false 0 +#endif + +#ifndef TRUE +#define TRUE 1 +#define FALSE 0 +#endif + +#define TWO_POW_16 65536UL + +// Constants +#define RAD_TO_DEG 57.29577951308232 +#define DEG_TO_RAD 0.017453292519943 +#define D2R ( 0.017453292519943 ) ///< ( PI/180.0 ) = 0.017453292519943 +#define R2D 57.29577951308232 + +#define DEG2RAD(d) ( (d) * D2R ) +#define RAD2DEG(r) ( (r) * R2D ) + +#define SIGMA 1.0e-8 +#define KNOT2MPSEC 5.144444444e-1 +#define SQUARE(x) ((x)*(x)) + +#define ROLL_INCIDENCE_LIMIT 0x1000 +#define PITCH_INCIDENCE_LIMIT 0x1000 +#define HARD_IRON_LIMIT 8192 // 0.25 G +#define SOFT_IRON_LIMIT 6554 // 20% +/// hard and soft iron resolution (2^16 / 2) +#define IRON_SCALE 32768 + + +// For fast inverse trigonometric functions +#define TAN15DEG 0.26794919243F +#define TAN30DEG 0.57735026919F + +// The following is the acceleration due to gravity at the calibration location +#define GRAVITY 9.80665 +#define ACCEL_DUE_TO_GRAV 9.794259 +#define g_TO_M_SEC_SQ 9.80655 + +#define MAX_ITOW 604800000 // Second in a week (assuming exactly 24 hours in a day) + +// PI and related values +#define TWO_PI 6.283185307179586 +#define PI 3.141592653589793 +#define PI_OVER_TWO 1.570796326794897 +#define PI_OVER_FOUR 0.785398163397448 +#define PI_OVER_SIX 0.523598775598299 + +#define ONE_OVER_PI 0.318309886183791 +#define ONE_OVER_TWO_PI 0.159154943091895 + +// Specify constants used to limit variables in the algorithm +#define ONE_DEGREE_IN_RAD (0.017453292519943) +#define TWO_DEGREES_IN_RAD (0.034906585039887) +#define TWO_PT_FIVE_DEGREES_IN_RAD (0.043633231299858) +#define THREE_DEGREES_IN_RAD (0.052359877559830) +#define FIVE_DEGREES_IN_RAD (0.087266462599716) +#define SIX_DEGREES_IN_RAD (0.104719755119660) +#define TEN_DEGREES_IN_RAD (0.17453292519943) +#define TWENTY_DEGREES_IN_RAD (0.349065850398866) +#define THREE_HUNDRED_EIGHTY_DEGREES_IN_RAD (6.632251157578453) + +#define FAST_MATH 0 + +#define MIN_TO_MILLISECONDS 60000.0 + +/// Specify the data acquisition task rate of the system in Hz. Due to the way data is collected, +/// this is different than the sampling rate of the sensors. Note: must be 100 or 200. +#define DACQ_100_HZ 100 +#define DACQ_200_HZ 200 +#define DACQ_RATE_INVALID 0 + +/// Specify the algorithm execution frequency in Hz. +/// So far only 100 and 200 +#define FREQ_50_HZ 50 +#define FREQ_100_HZ 100 +#define FREQ_200_HZ 200 +#define FREQ_INVALID 0 + +// Choices for user communication interface +#define UART_COMM 0 +#define SPI_COMM 1 +#define CAN_BUS 2 +//#define UNVALID_COMM 100 + +// Choices for sensors range +#define _200_DPS_RANGE 0 +#define _400_DPS_RANGE 1 +#define _1000_DPS_RANGE 2 + +// Choices for user system type +#define IMU_6DOF_SYS 0 // IMU 6 degrees of freedom - Only accelerometers and gyros +#define IMU_9DOF_SYS 1 // default IMU 9 degrees of freedom - Accelerometers, gyros, magnetometer +#define UNAIDED_VG_SYS 2 +#define UNAIDED_AHRS_SYS 3 +#define AIDED_VG_SYS 4 +#define AIDED_AHRS_SYS 5 +#define INS_SYS 6 + +// Choices for GPS protocol type +typedef enum{ + AUTODETECT = -1, + UBLOX_BINARY = 0, + NOVATEL_BINARY = 1, + NOVATEL_ASCII = 2, + NMEA_TEXT = 3, + DEFAULT_SEARCH_PROTOCOL = NMEA_TEXT, // 3 + SIRF_BINARY = 4, + INIT_SEARCH_PROTOCOL = SIRF_BINARY, ///< 4 max value, goes through each until we hit AUTODETECT + UNKNOWN = 0xFF +} enumGPSProtocol; + + +// Algorithm specifiers +#define IMU 0 +#define AHRS 1 +#define VG 2 +#define INS 3 + +//#define USE_DOUBLE +#ifdef USE_DOUBLE +#define real double +#else +#define real float +#endif + +// Force Q0 positive by flipping the sign on all quaternion-elements when q0 < 0. This +// seems to reduce the errors in the system although (in theory) it shouldn't affect +// the result. +#define FORCE_Q0_POSITIVE + +// some value limits +#define INT16_LIMIT 32765 +#define INT12_LIMIT 2045 + +#define USER_PACKET_OK 0 +#define UNKNOWN_USER_PACKET 1 +#define USER_PACKET_ERROR 2 + +#define USER_OK 0x00 +#define USER_NAK 0x80 +#define USER_INVALID 0x81 + +#define MAXUINT32 4294967295 ///< max unsigned 32 bit int=> ((2^32)-1) +#define MAXUINT16 65535 ///< max unsigned 16 bit int=> ((2^16)-1) +#define MAXINT16 ( 32767) ///< max signed 16 bit int=> ((2^15)-1) +#define MININT16 (-32768) ///< max negative signed 16 bit int=> (-(2^15)) +#define MAXINT32 ( 0x7fffffff) ///< max signed 32 bit int +#define MININT32 ( 0x80000000) ///< max negative signed 32 bit int + + +#endif /* GLOBALCONSTANTS_H */ + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Common/include/scaling.h b/examples/OpenIMU300RI/INS/lib/Core/Common/include/scaling.h new file mode 100644 index 0000000..9734b9b --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Common/include/scaling.h @@ -0,0 +1,91 @@ +/** *************************************************************************** + * @file scaling.h + * + * 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. + * + * macros to define various power of 2, fixed point and constant scalsing + ******************************************************************************/ +/******************************************************************************* +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 SCALING_H +#define SCALING_H + + +/// pre-computed values +#define SCALE_BY_8(value) ( (value) * 8.0 ) +#define SCALE_BY_2POW16_OVER_2PI(value) ( (value) * 10430.37835047045 ) // 65536 / (2.0 * PI) +#define SCALE_BY_2POW16_OVER_7PI(value) ( (value) * 2980.108100134415 ) // 65536 / (7.0 * PI) +#define SCALE_BY_2POW16_OVER_2(value) ( (value) * 32768.0 ) // 65536 / 2 +#define SCALE_BY_2POW16_OVER_16(value) ( (value) * 4096.0 ) // 5536 / 16 +#define SCALE_BY_2POW16_OVER_20(value) ( (value) * 3276.8 ) // 65536 / 20 +#define SCALE_BY_2POW16_OVER_64(value) ( (value) * 1024.0 ) // 65536 / 64 +#define SCALE_BY_2POW16_OVER_128(value) ( (value) * 512.0 ) // 65536 / 128 +#define SCALE_BY_2POW16_OVER_200(value) ( (value) * 327.68 ) // 65536 / 200 +#define SCALE_BY_2POW16_OVER_512(value) ( (value) * 128.0 ) // 65536 / 512 + +#define SCALE_BY_2POW32_OVER_2PI(value) ( (value) * 683565287.23678304) // 4294967296 / 2 * PI +#define SCALE_BY_2POW16_OVER_2POW14(value) ( (value) * 4.0 ) // 65536 / 16384 + +#define TWO_OVER_TWO_POW16_q30 32768 +#define TWENTY_OVER_TWO_POW16_q30 327680 +#define TWOPI_OVER_MAXINT16_q30 205894 + +#define TWO_POW16_OVER_7PI_q19 1562434916 // floor( 2^16/( 7*pi ) * 2^19 ) +#define TWO_POW16_OVER_20_q19 1717986918 // floor( 2^16/20 * 2^19 ) +#define TWO_POW16_OVER_2_q15 1073741824 // floor( 2^16/2 * 2^15 ) +#define TWO_POW16_OVER_200_q22 1374389534 // floor( 2^16/200 * 2^22 ) +//#define TWO_POW16_OVER_20_q19 1717986918 // floor( 2^16/20 * 2^19 ) +#define TWO_POW16_TIMES_100_OVER_7PI_q12 298011 // floor( 2^16/( 7*pi ) * 2^12 ) + +#define TWO_POW16_OVER_128_q21 1073741824 // floor( 2^16/200 * 2^22 ) +#define TWO_POW16_OVER_512_q23 128 + +#define TWO_POW16_OVER_2PI_q17 1367130551 +#define TWO_POW19_OVER_7PI_q16 23841 + +#define MAXUINT16_OVER_2PI 10430.21919552736 +#define DEGREES_TO_RADS 0.017453292519943 +#define RADS_TO_DEGREES 57.29577951308232 +#define ITAR_RATE_LIMIT 7.15585 // 410 dps * DEGREES_TO_RADS + +#define MAXINT16_OVER_2PI 5215.030020292134 //( MAXINT16 / TWOPI) +#define MAXUINT16_OVER_512 127.9980468750000 // ( MAXUINT16 / 512.0) +#define MAXUINT16_OVER_2 32768.0 //( MAXUINT16 / 2.0) + +// For magCal() +#define MAXINT16_OVER_2PI_q18 1367088830 + +#define MAXINT32_20BIT_OVER131072M 8 // 2^20/(2^17) + +/// INT32_TO_MISALIGN_SCALING = 1/2^( 32 - 5 ) as per the definition of misalign +/// in DMU Serial Interface Spec +#define INT32_TO_MISALIGN_SCALING 7.450580596923828e-09 + +/// BOARD_TEMP_SCALE_FACTOR = 1/256 = 0.00390625 from the TMP102 datasheet +/// ( shift 4-bits due to buffer and 4-bits for scaling) +#define BOARD_TEMP_SCALE_FACTOR 0.00390625 +#define BOARD_TEMP_SCALE_FACTOR_q30 419430 + +/// GYRO_TEMP_SCALE_FACTOR = 1/256 = 0.00390625 from the Maxim21000 datasheet +#define MAXIM21000_TEMP_SCALE_FACTOR 0.00390625 +#define BMI160_TEMP_SCALE_FACTOR 0.001953125 +#define BMI160_TEMP_OFFSET 23.0 +#endif + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Common/include/ucb_packet_struct.h b/examples/OpenIMU300RI/INS/lib/Core/Common/include/ucb_packet_struct.h new file mode 100644 index 0000000..066ee3f --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Common/include/ucb_packet_struct.h @@ -0,0 +1,89 @@ +/** *************************************************************************** + * @file ucb_packet_struct.h utility functions for interfacing with Aceinna proprietary + * UCB (unified code base) packets. UCB packet structure + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY O ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * @brief these are in ucb_packet_types.def on the 440 these were in + * xbowProtocol.h + *****************************************************************************/ +/******************************************************************************* +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 UCB_PACKET_STRUCT_H +#define UCB_PACKET_STRUCT_H +#include + +#define UCB_SYNC_LENGTH 2 +#define UCB_PACKET_TYPE_LENGTH 2 +#define UCB_PAYLOAD_LENGTH_LENGTH 1 +#define UCB_CRC_LENGTH 2 + +#define UCB_SYNC_INDEX 0 +#define UCB_PACKET_TYPE_INDEX (UCB_SYNC_INDEX + UCB_SYNC_LENGTH) +#define UCB_PAYLOAD_LENGTH_INDEX (UCB_PACKET_TYPE_INDEX + UCB_PACKET_TYPE_LENGTH) + +/// preamble sync bytes +static const uint8_t UCB_SYNC [UCB_SYNC_LENGTH] = { 0x55, 0x55 }; + +/// packet field type definitions +typedef uint16_t UcbPacketCodeType; +typedef uint16_t UcbPacketCrcType; + + +#define UCB_MAX_PAYLOAD_LENGTH 255 +#define UCB_USER_IN 200 +#define UCB_USER_OUT 201 +#define UCB_ERROR_INVALID_TYPE 202 + + + +typedef struct { + uint16_t packetType; + uint16_t packetCode; +}ucb_packet_t; + +typedef struct { + uint16_t packetType; + uint8_t packetCode[2]; +}usr_packet_t; + + +typedef struct { + uint8_t packetType; // 0 + uint8_t systemType; // 1 + uint8_t spiAddress; // 2 + uint8_t sync_MSB; // 3 + uint8_t sync_LSB; // 4 + uint8_t code_MSB; // 5 + uint8_t code_LSB; // 6 + uint8_t payloadLength; // 7 + uint8_t payload[UCB_MAX_PAYLOAD_LENGTH + 3]; // aligned to 4 bytes +} UcbPacketStruct; + +#define NUM_ARRAY_ITEMS(a) (sizeof(a) / sizeof(a[0])) +#define CODE(first, second) (((first << 8) | second) & 0xffff) +#define SWAP(x) (((x >> 8) & 0xff) | ((x << 8) & 0xff00)) + +extern int getUserPayloadLength(void); +extern int checkUserPacketType(UcbPacketCodeType receivedCode); +extern void userPacketTypeToBytes(uint8_t bytes[]); + + +#endif diff --git a/examples/OpenIMU300RI/INS/lib/Core/Common/include/utilities.h b/examples/OpenIMU300RI/INS/lib/Core/Common/include/utilities.h new file mode 100644 index 0000000..0e09559 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Common/include/utilities.h @@ -0,0 +1,38 @@ +/** *************************************************************************** + * @file utilities.h + * + * 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. + * + * local version of string functions + ******************************************************************************/ +/******************************************************************************* +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 UTILITIES_H +#define UTILITIES_H +#include + +extern char* strtok_r1(char *str, char const token, char **cursor); +extern void strrep(char *str, const char find, const char rep); +extern int strcmpi(char const *a, char const *b); +//extern void itoa(int value, char *sp, int radix); +uint16_t byteSwap16(uint16_t b); +uint32_t byteSwap32( uint32_t val ); + +#endif diff --git a/examples/OpenIMU300RI/INS/lib/Core/Common/src/utilities.c b/examples/OpenIMU300RI/INS/lib/Core/Common/src/utilities.c new file mode 100644 index 0000000..0ffe768 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Common/src/utilities.c @@ -0,0 +1,224 @@ +/** *************************************************************************** + * @file utilities.c + * + * 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. + * + * string functions + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + + +#include "utilities.h" + +/** **************************************************************************** + * @name: tolower + * @brief reimplementation of library function: + * change uppercase letters to lowercase + * (This is just a helper function for strcmpi.) + * @param [in] c - character of interest + * @retval the character of interest (lowercase if it was upper) + ******************************************************************************/ +//char tolower(char c) { +char tlwr(char c) { + if ('A' <= c && c <= 'Z') { + return (char)((c-'A')+'a'); + } + return c; +} + +/** **************************************************************************** + * @name: strcmpi + * @brief reimplementation of library function: + * compare two string without regard for case + * + * @param [in] two strings to be compared (a and b) + * @retval strcmp returns 0 when the strings are equal, a negative + * integer when s1 is less than s2, or a positive integer if s1 is greater + * than s2, according to the lexicographical order. + ******************************************************************************/ +int strcmpi(char const *a, + char const *b) { + + while ((*a) && (*b) && tlwr(*a) == tlwr(*b)) { + a++; + b++; + } + if (*a > *b) { + return 1; + } else if (*a < *b) { + return -1; + } else { //(*a == *b) + return 0; + } +} + +/** **************************************************************************** + * @name: strrep + * @brief replace a character inside a string, for example to + * replace newline characters with string termination characters. + * + * @param [in] str - string to be modified + * @param [in] find - character to be replaced + * @param [in] rep - replacement character + * @retval None. +*******************************************************************************/ +void strrep(char *str, + const char find, + const char rep) { + while (*str) { + if (find == *str) { + *str = rep; + } + str++; + } +} + +/** **************************************************************************** + * @name: strtok_r + * @brief reimplementation of library function: + * parses a string into a sequence of tokens + * The strtok_r() function is a reentrant version of strtok(). + * It gets the next token from string s1, where tokens are strings separated + * by characters from s2. To get the first token from s1, strtok_r() is + * called with s1 as its first parameter. Remaining tokens from s1 are + * obtained by calling strtok_r() with a null pointer for the first + * parameter. The string of delimiters, s2, can differ from call to call. + * + * @param [in] str - string to be searched + * @param [in] token - to use for separation + * @param [in] cursor - points to the last found token + * @retval returns the next token to be used. +*****************************************************************************/ +char* strtok_r1(char *str, + char const token, + char **cursor) { + char *first; + char *last; + + /// On the first call, we initialize from str + /// On subsequent calls, str can be NULL + if (str) { + first = str; + } else { + first = *cursor; + if (!first) { + return 0; + } + } + /// Skip past any initial tokens + while (*first && (*first == token)) { + first++; + } + /// If string ends before first token is found, abort + if (0 == *first) { + return 0; + } + + /// Now start moving last + last = first + 1; + while (*last && (*last != token)) { + last++; + } + /// Found either end-of-token or end of string + if (*last) { + /// End of token + *cursor = last + 1; + *last = 0; + } else { + *cursor = 0; + } + return first; +} + + +/***************************************************************************** + ** Function name: itoa + ** Descriptions: reimplementation of library function: + ** Converts an integer value to a null-terminated string using the + ** specified base and stores the result in the array given by str parameter. + ** parameters: value to be converted + ** string to put it in + ** base of the numer + ** Returned value: none. +*****************************************************************************/ +#define MAX_INT32_STRING 11 // 10 digits plus a NULL +#define RADIX_DECIMAL 10 + +#ifdef FL +void itoa(int value, char *sp, int radix) +{ + char tmp[MAX_INT32_STRING]; + char *tp = tmp; + int i; + unsigned v; + int sign; + + sign = (radix == RADIX_DECIMAL && value < 0); + if (sign) { v = -value; } + else { v = (unsigned)value; } + + while (v || tp == tmp) + { + i = v % radix; + v /= radix; + if (i < RADIX_DECIMAL) { + *tp++ = i+'0'; + } else { + *tp++ = i + 'A' - RADIX_DECIMAL; + } + } + + if (radix != RADIX_DECIMAL) { // zero fill non decimal values + while (tp < &(tmp[4])) { + *tp++ = '0'; + } + } + + if (sign) { *sp++ = '-'; } + // reverse and put in output buffer + while (tp > tmp) { + tp--; + *sp = *tp; + sp++; + } + *sp = '\0'; + +} +#endif + +/** **************************************************************************** + * @name byteSwap16 + * @param [in] b - short word to swap endieness + * @retval byte swappeed word + ******************************************************************************/ +uint16_t byteSwap16(uint16_t b) +{ return (b << 8) | (b >> 8); } + +/** **************************************************************************** + * @name byteSwap32 + * @param [in] b - short word to swap endieness + * @retval byte swappeed word + ******************************************************************************/ +uint32_t byteSwap32( uint32_t val ) +{ + val = ((val << 8) & 0xFF00FF00 ) | ((val >> 8) & 0xFF00FF ); + return (val << 16) | (val >> 16); +} + diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/gpsAPI.h b/examples/OpenIMU300RI/INS/lib/Core/GPS/gpsAPI.h new file mode 100644 index 0000000..3a24ea4 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/gpsAPI.h @@ -0,0 +1,96 @@ +/** *************************************************************************** + * @file gps.h GPS Driver for Inertial/GPS NAV. + * + * 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 GPS interface, taken loosely from the DMU440 + * project possibly implemented using the Origin ORG4475 GPS module (or NovAtel + * or uBlox) the GPS may communicated via SPI or UART, that is passed in on init + * 03.2007 DA Cleaned up, Doxygenized, and finalized for NAV440 release. + *****************************************************************************/ +/******************************************************************************* +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 GPS_API_H +#define GPS_API_H + +#include +#include "GlobalConstants.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void TaskGps(void const *argument); + +#ifdef __cplusplus +} +#endif + +// Fix type +#define INVALID 0 // GNSS not fixed yet +#define SPP 1 // Single Point Positioning +#define DGPS 2 // Pseudorange Differential +#define PPP 3 // Precise Point Positioning +#define RTK_FIX 4 // RTK Fixed +#define RTK_FLOAT 5 // RTK Float +#define DEAD_REC 6 // Dead Reckoning (will be considered as INVALID) +#define MANUAL 7 // Manual Input Mode (will be considered as INVALID) +#define SIMULATION 8 // Simulation Mode (will be considered as INVALID) + +typedef struct { + uint8_t gpsFixType; // 1 if data is valid + uint8_t gpsUpdate; // 1 if contains new data + uint8_t numSatellites; // num of satellites in the solution + uint32_t itow; // gps Time Of Week, miliseconds + + double latitude; // latitude , degrees + double longitude; // longitude, degrees + double vNed[3]; // velocities, m/s NED (North East Down) x, y, z + double trueCourse; // [deg] + double rawGroundSpeed; // [m/s] + double altitude; // above WGS84 ellipsoid [m] + double GPSSecondFraction; + + + uint8_t GPSmonth; // mm + uint8_t GPSday; // dd + uint8_t GPSyear; // yy last two digits of year + char GPSHour; // hh + char GPSMinute; // mm + char GPSSecond; // ss + + float GPSHorizAcc, GPSVertAcc; + float HDOP; + float geoidAboveEllipsoid; // [m] Height of geoid (mean sea level) above WGS84 ellipsoid +} gpsDataStruct_t; + +extern gpsDataStruct_t gGPS, gCanGps; + +/** **************************************************************************** + * @name GetGPSData + * @brief Get GPS data + * @param [in] data - pointer to external GPS structure + * @retval N/A + ******************************************************************************/ +void GetGPSData(gpsDataStruct_t *data); +BOOL SetGpsBaudRate(int rate, int fApply); +BOOL SetGpsProtocol(int protocol, int fApply); + +#endif /* GPS_API_H */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/include/GpsData.h b/examples/OpenIMU300RI/INS/lib/Core/GPS/include/GpsData.h new file mode 100644 index 0000000..cb21530 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/include/GpsData.h @@ -0,0 +1,123 @@ +/******************************************************************************* +* File: GPS_Data.h +********************************************************************************/ +/******************************************************************************* +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 GPS_DATA_H +#define GPS_DATA_H + +#include +#include "GlobalConstants.h" + +/** \struct universalMSGSpec +\brief specify an universal message spec. +*/ +typedef struct { + unsigned long GPSheader; ///< could be 1, 2, 3, or 4 bytes - see message headers ^ + unsigned char GPSheaderLength; + unsigned short lengthOfHeaderIDLength; + unsigned char crcLength; + unsigned char binaryOrAscii; // 1 = ascii, 0 binary + unsigned char startByte; // pulled out of header +} universalMSGSpec; + +/** @struct GPSDataSTRUCT +@brief global data structure for all GPS interface and process. + +This Data structure is not only used for all GPS interface and process +but also used to be accessed by other modules rather than GPS files. +*/ +typedef struct { + uint8_t gpsFixType; // GPS solution type: invalid, spp, dgps, spp, rtk_float, rtk_fix + uint8_t numSatellites; + + uint8_t GPSmonth; // mm + uint8_t GPSday; // dd + uint8_t GPSyear; // yy last two digits of year + char GPSHour; // hh + char GPSMinute; // mm + char GPSSecond; // ss + double GPSSecondFraction; // FIXME used? + + uint32_t itow; ///< gps milisecond Interval Time Of Week + int updateFlagForEachCall; /// changed to 16 bits + + int totalGGA; + int totalVTG; + + double lat; // [deg], latitude + double lon; // [lon], longitude + double alt; // [m] above WGS84 ellipsoid + double vNed[3]; // NED North East Down [m/s] x, y, z + double trueCourse; // [deg] + double rawGroundSpeed; // [m/s] + + float geoidAboveEllipsoid; // [m] Height of geoid (mean sea level) above WGS84 ellipsoid + + /// compatible with Ublox driver FIXME should these be seperate data structure? + unsigned char ubloxClassID; + unsigned char ubloxMsgID; + signed long LonLatH[3]; // SiRF Lat Lon[deg] * 10^7 Alt ellipse [m]*100 <-- UNUSED + float HDOP; // Horizontal Dilution Of Precision x.x + double GPSVelAcc; + unsigned short GPSStatusWord; /// will replace GPSfix + unsigned char isGPSFWVerKnown; + unsigned char isGPSBaudrateKnown; + unsigned long Timer100Hz10ms; + unsigned char ubloxOldVersion; + float UbloxSoftwareVer; + unsigned int navCFGword; + unsigned int nav2CFGword; + char GPSConfigureOK; /// always needs to be initialized as -1 + + unsigned char reClassID; + unsigned char reMsgID; + + unsigned long LLHCounter; + unsigned long VELCounter; + unsigned long STATUSCounter; // UBLOX - or first flag SiRF + unsigned long SBASCounter; + unsigned long firewallCounter; + unsigned long firewallRunCounter; + unsigned long reconfigGPSCounter; + + /// GPS Baudrate and protocal: -1, 0,1, 2, 3 corresponding to + int GPSbaudRate; /// 4800, 9600, 19200, 38400, 57600, 115200, etc + /// AutoDect, Ublox Binary, NovAtel binary, NovAtel ASCII, NMEA + enumGPSProtocol GPSProtocol; + + universalMSGSpec GPSMsgSignature; + unsigned char GPSAUTOSetting; + unsigned char GPSTopLevelConfig; // UBLOX + unsigned char resetAutoBaud; + unsigned char autoBaudCounter; + + //uint8_t sirfInitialized; + //float latQ; + //float lonQ; + //float hgtQ; + //uint8_t useSigmas; + + float GPSHorizAcc; + float GPSVertAcc; +} GpsData_t; + +extern GpsData_t *gGpsDataPtr; // definition in driverGPSAllentrance.c + + +#endif /* GPS_DATA_H */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/include/NovAtelPacketFormats.h b/examples/OpenIMU300RI/INS/lib/Core/GPS/include/NovAtelPacketFormats.h new file mode 100644 index 0000000..fb9b0f2 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/include/NovAtelPacketFormats.h @@ -0,0 +1,128 @@ +/****************************************************************************** + * @file NovatelPacketFormats.h +*******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + + +#include "driverGPS.h" +#include + +#pragma pack(1) +typedef struct { // 7 32 bit words + uint8_t preamble0; // 0xAA + uint8_t preamble1; // 0x44 + uint8_t preamble2; // 0x12 + uint8_t headerLength; // bytes 0x1c 28d + + uint16_t msgID; // 0x0100 0x2a 42d, 0x63 99d + uint8_t msgType; // + uint8_t portAddress; // 0x20 COM1 + + uint16_t msgLength; // bytes payload only + uint16_t sequence; // 0 or 1 + + uint8_t idleTime; + uint8_t timeStatus; + uint16_t week; // GPS week number + + uint32_t milliseconds; + + uint32_t recStatus; + + uint16_t reserved; + uint16_t swVersion; +} novatelBinaryHeader; +#pragma pack() + +#pragma pack(1) +typedef struct { + novatelBinaryHeader header; // #1 7 32 bit words + uint32_t sol_status; // #2 + uint32_t pos_type; // #3 + double Lat; // #4 + double Lon; // #5 + double hgt; // #6 above msl + float undulation; // #7 + uint32_t datum_id_num; // #8 + float lat_sigma; // #9 + float lon_sigma; // #10 + float hgt_sigma; // #11 + uint32_t stn_id; // #12 + float diff_age; // #13 + float sol_age; // #14 + uint8_t num_obs; // #15 + uint8_t num_GPSL1; // #16 + uint8_t num_L1; // #17 + uint8_t num_L2; // #18 + uint32_t reserved; // #19 - #22 + uint32_t Crc; // #23 + uint16_t terminator; // #24 /CR /LF +} logBestPosB; +#pragma pack() + +#pragma pack(1) +typedef struct { + novatelBinaryHeader header; // #1 7 32 bit words + uint32_t sol_status; // #2 + uint32_t vel_type; // #3 + float latency; // #4 + float age; // #5 + double hor_spd; // #6 [m/s] + double trk_gnd; // #7 True [deg] + double vert_spd; // #8 +up [m/s] + float reserved; // #9 + uint32_t Crc; // #10 + uint16_t terminator; // #11 /CR /LF +} logBestVelB; +#pragma pack() + +unsigned long _CalculateBlockCRC32(unsigned long ulCount, + unsigned char *ucBuffer); + +unsigned long _CRC32Value(int i); +void processNovAtelBinaryMsg(char *msg, + unsigned int *msgLength, + GpsData_t *GPSData); + +void _parseBestPosB_Fast(logBestPosB *completeMessage, + GpsData_t *GPSData); + +void _parseBestPosB(char *completeMessage, + unsigned char *headerLength, + unsigned short *bodyLength, + GpsData_t *GPSData); + +void _parseBestVelB(char *completeMessage, + unsigned char *headerLength, + unsigned short *bodyLength, + GpsData_t *GPSData); +void _parseBestVelB_Fast(logBestVelB *completeMessage, + GpsData_t *GPSData); + + +void _decodeSelectedNovAtelASCIImsg(char *msgID, + char *msgBody, + unsigned int *msgBodyLength, + GpsData_t *GPSData); + +char _parseNoVatelPOSVELNAVDOPA(char *msgBody, + unsigned int *msgBodyLength, + GpsData_t *GPSData); + +long double _assembyToLongDouble(unsigned char *doubleBytes); + +unsigned int _decodePositionTypeAscii(char *positionType); diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/include/SiRFPacketFormats.h b/examples/OpenIMU300RI/INS/lib/Core/GPS/include/SiRFPacketFormats.h new file mode 100644 index 0000000..707c06a --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/include/SiRFPacketFormats.h @@ -0,0 +1,236 @@ +/** *************************************************************************** + * @file SiRFPacketFormat.h SiRF GPS receiver. + * + * 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. + * @details + * This file includes all specific processing, including configuring, + * for SiRF GPS receiver. + *****************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#include "Indices.h" + +#define SIRF_NAVIGATION_DATA 0x02 +#define SIRF_MEASURED_TRACKER_DATA 0x04 +#define SIRF_SOFTWRE_VERSION_STRING 0x06 +#define SIRF_SOFTWARE_COMMAND_ACK 0x0B // ID 12 +#define SIRF_SOFTWARE_COMMAND_NACK 0x0C // ID 13 +#define SIRF_GEODETIC_NAVIGATION_DATA 0x29 // ID 41 +#define SIRF_TRACKER_MESSAGES 0x41 // ID 65 'A' also ERROR +#define SIRF_SBAS_PARAMETERS 0x32 +#define SIRF_EPHEMERIS 0x38 // ID 56 +#define SIRF_STATISICS 0xE1 // ID 225 +#define SIRF_STATIC_NAVIGATION 0x8f // ID 143 + +#define SIRF_POLL_SOFTWARE_VERSION 0x84 // ID 132 +#define SIRF_SET_DGPS_SOURCE 0x85 // ID 133 +#define SIRF_SET_DGPS_CTRL 0x8A // ID 138 +#define SIRF_SET_MESSAGE_RATE 0xA6 // ID 166 +#define SIRF_SET_SBAS_PARAMETERS 0xAA // ID 170 +#define SIRF_SET_SERIAL_PORT 0x86 // ID 134 + +#define SIRF_SET_NAV_MODE 0x88 // ID 136 + +#define MAX_MESSAGE_RATE 30 +#define NUM_CNO_VALUES 10 + +#define TEN_TO_THE_SEVENTH 10000000 +#define ONE_OVER_TEN_TO_THE_SEVENTH 1e-7 + +#define MILLISECOND_TO_SECOND 1000 +#define SECOND_TO_MILLISECOND 0.001 + +#pragma pack(1) ///< structure will appear in memory as they do here +typedef struct { + int32_t pos[NUM_AXIS]; + int16_t vel[NUM_AXIS]; + uint8_t mode1; + uint8_t hdop; + uint8_t mode2; + uint16_t gpsWeek; + uint32_t tow; + uint8_t prn[12]; +} tSiRFNav; + +#define MAX_NUM_TRACKER_CHANNELS 12 +typedef struct { + uint8_t id; + uint8_t azimuth; + uint8_t elevation; + uint8_t state; + uint8_t cno[NUM_CNO_VALUES]; +} tSiRFTrackerData; + +typedef struct { + uint16_t gpsWeek; + uint32_t tow; + uint8_t numChannels; + tSiRFTrackerData channel[MAX_NUM_TRACKER_CHANNELS]; +} tSiRFTracker; + +typedef struct { + uint8_t id; + uint8_t mode; + uint8_t messageIdToBeSet; + uint8_t updateRate; + uint32_t reserved; +} tSiRFSetMessageRate; + +typedef struct { + uint8_t id; + uint16_t reserved; + uint8_t degMode; + uint8_t posCalcMode; + uint8_t deadRecogEnable; + int16_t altitudeInput; + uint8_t altHoldMode; + uint8_t altHoldSource; + uint8_t coastTimeout; + uint8_t degradedTimeout; + uint8_t deadRecogTimeout; + uint8_t measAndTrackSmoothing; +} tSiRFSetModeControl; + + +typedef struct { + uint8_t id; + uint8_t sbasPRN; + uint8_t sbasMode; + uint8_t sbasBits; + uint16_t reserved; +} tSiRFSetSbasParams; + + +typedef struct { + uint8_t id; + uint8_t dgpsSource; + uint32_t reserved; + uint8_t reserved2; +} tSiRFSetDgpsSource; + + +typedef struct { + uint8_t id; + uint8_t dgpsSelection; + uint8_t timeout; +} tSiRFSetDgpsControl; + + +typedef struct { + uint8_t id; + uint8_t reserved; +} tSiRFPollVersion; + +typedef struct { + uint8_t id; // 0x8f 143d + uint8_t flag; // 1 enable, 0 disable +} tSiRFStaticNavigation; + +typedef struct { + uint8_t id; + uint32_t baud; + uint8_t databits; + uint8_t stopbits; + uint8_t parity; + uint8_t reserved; +} tSiRFPSetSerialPort; + +enum eNavValidBitmask { // only useful info in bit 0 + NAV_COMPLETELY_VALID = 0x0000, // best case + SOLUTION_NOT_YET_OVERDETERMINED = 0x01, +}; + +enum eGpsPositionFixType { + NO_NAV_FIX = 0, + ONE_SV_KV_SLN = 1, + TWO_SV_KV_SLN = 2, + THREE_SV_KV_SLN = 3, + FOUR_PLUS_SV_KV_SLN = 4, + TWO_D_LEAST_SQUARES_SLN = 5, + THREE_D_LEAST_SQUARES_SLN = 6, + DR_SOLUTION = 7 +}; + +enum eAltitudeHoldStatus { + NO_ALT_HOLD = 0, + ALT_HOLD_FROM_KF = 1, + ALT_HOLD_FROM_USER = 2, + ALWAYS_HOLD_ALT_FROM_USER = 3 +}; + +#pragma pack(1) +typedef struct { + // zero valid, non zero degraded + uint16_t navValid; // 0x0201 enum eNavValidBitmask + union { + uint16_t all; + struct { + uint16_t fixType :3; // enum eGpsPositionFixType + uint16_t tricklePowerInUse :1; + uint16_t altitudeHoldStatus :2; // enum eAltitudeHoldStatus + uint16_t dopLimitsExceeeded :1; + uint16_t dgpsCorrectionsApplied :1; + uint16_t sensorDrSolutionType :1; // SiRF Drive only + uint16_t navigationSolutionOverDetermined:1; + uint16_t velocityDr2TimeoutExceeded :1; + uint16_t fixEditedByMiFunctions :1; + uint16_t invalidVelocity :1; + uint16_t altitudeHoldDisabled :1; + uint16_t sensorDRErrorStatus :2; // SiRF Drive only + } bits; + } navType; // 0x0403 + uint16_t extendedWeekNumber; // 0x0605 from 06.01.1980 (Jan 6) + uint32_t tow; // 0x0a090807 [sec] * 10^3 + uint16_t utcYear; // 0x0c0b yyyy + uint8_t utcMonth; // 0x0d mm + uint8_t utcDay; // 0x0e dd + uint8_t utcHour; // 0x0f hh + uint8_t utcMinute; // 0x10 mm + uint16_t utcSecond; // 0x1211 ss + uint32_t satelliteIdBitmap; // 0x16151413 + int32_t latitude; // 0x1a191817 [degrees] * 10^7, + = N + int32_t longitude; // 0x1e1d1c1b [degrees] * 10^7, + = E + int32_t altitudeEllipsoid; // 0x2221201f [m] * 10^2 + int32_t altitudeMSL; // 0x26252423 [m] * 10^2 + int8_t mapDatum; // 0x27 + uint16_t speedOverGround; // 0x2928 [m/s] *10^2 + uint16_t courseOverGround; // 0x2b2a [degrees] clockwise from true north x 10^2 + int16_t magneticVariation; // 0x2d2c not implemented + int16_t climbRate; // 0x2f2e [m/s] *10^2 + int16_t headingRate; // 0x3130 SiRFDrive only + uint32_t estHorizPosErrror; // 0x35343332 EHPE [meters] x 10^2 + uint32_t estVertPosErrror; // 0x39383736 EVPE [meters] x 10^2 + uint32_t estTimeError; // 0x3d3c3b3a ETE [s] 10^2 // SiRFDrive only + uint16_t estHorizVelError; // 0x3f3e EHVE [m/s] x 102 (SiRFDRive only) + int32_t clockBias; // 0x43424140 [m] x 10^2 + uint32_t clockBiasError; // 0x47464544 [m] x 10^2 (SiRFDRive only) + int32_t clockDrift; // 0x4b4a4948 [m/s] x 10^2 + uint32_t clockDriftError; // 0x4f4e4d4c [m/s] x 10^2 (SiRFDRive only) + uint32_t distance; // 0x53525150 [meters] Distance traveled since reset (SiRFDRive only) + uint16_t distanceError; // 0x5554 [meters] (SiRFDRive only) + uint16_t headingError; // 0x5756 [degrees] x 10^2 (SiRFDRive only) + uint8_t numSvsInFix; // 0x58 see also satelliteIdBitmap + uint8_t hdop; // 0x59 Horizontal Dilution of Precision x 5 (0.2 resolution) + uint8_t additionalModeInfo; // 0x5a see SiRF manual +} tSiRFGeoNav; // 90 bytes + 1 for additional mode Info +#pragma pack() + +// reorderBytes defined in processSIRFGPS.cpp +#define SET_VALUE(val, m, idx) {(val) = reorderBytes(m, (idx), sizeof(val));} diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/include/driverGPS.h b/examples/OpenIMU300RI/INS/lib/Core/GPS/include/driverGPS.h new file mode 100644 index 0000000..c7681a8 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/include/driverGPS.h @@ -0,0 +1,299 @@ +/** *************************************************************************** + * @file driverGPS.h header file for all GPS interface and process files. + * + * 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. + * @details + * This file defines constants, data structures, and function + * prototypes. To enable GPS the config file must have true + * and true in name_IMU380.xml file. This version runs the + * Origin ORG4475 SiRF based GPS module. + *****************************************************************************/ +/******************************************************************************* +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 DRIVERGPS_H +#define DRIVERGPS_H + +#include "GlobalConstants.h" +#include "GpsData.h" + +/// bit position of updateFlagForEachCall; +#define GOT_GGA_MSG 0 +#define GOT_VTG_MSG 1 +#define W_CONTROL_POINT_EEPROM 2 +#define W_GPS_DELAY_EEPROM 3 +#define W_VTG_CONF_EEPROM 4 +#define W_BYPASS_CONF_EEPROM 5 +#define GOT_UBLOX_ACK 8 /// MSB byte for ublox specific +#define GOT_UBLOX_VNED 9 +#define GOT_UBLOX_NAVSBAS 10 + +/// bit position of GPSStatusWord; +#define DGPS_ON 0 +#define WAAS_ON 1 +#define EGNOS_ON 2 +#define MSAS_ON 3 +#define CLEAR_DGPS_SOURCES 0x01 + +/// GPSAutoSetting +#define AUTOBAUD 1 +#define AUTOPROTOCOL 2 + +/// bit position of GPSTopLevelConfig +#define HZ2 0 +#define GPSPORT_SUPPORT420 1 +#define EXTERNAL_GPS 2 + +/// max message length, field length +#define MAX_MSG_LENGTH 400 /// for sharing with other binary messages + /// must be less than SCIA&B buffer size + /// NovAtel Ascii could be big. PosVelNavDopa is 350 bytes + /// should be <= max SCIBR buffer size +#define MAX_UBLOX_BODY_LENGTH 100 + +#define NMEA_MSG_BUF_SIZE 100 ///< Max length of NMEA +#define NMEA_MSG_MAX_ID_LENGTH 8 +#define NMEA_MSG_MAX_DATA_LENGTH NMEA_MSG_BUF_SIZE-3 +#define NMEA_MSG_MAX_FIELD 25 ///< Max length of Field + +/// maxmum GPS velocity accuracy allowed of ublox +#define MAX_GPS_VELOCITY_ACCURACY 3.0 ///< m/s + +/// ublox message ID +#define UBLOX_NAV_PVT 0x0107 +#define UBLOX_NAV_POSLLH 0x0102 +#define UBLOX_NAV_STATUS 0x0103 +#define UBLOX_NAV_VELNED 0x0112 +#define UBLOX_NAV_SBAS 0x0132 +#define UBLOX_ACK_ACK 0x0501 +#define UBLOX_CFG_CFG 0x0609 +#define UBLOX_CFG_PRT 0x0600 +#define UBLOX_CFG_NAV 0x0603 +#define UBLOX_CFG_NAV2 0x061A +#define UBLOX_CFG_RXM 0x0611 +#define UBLOX_CFG_SBAS 0x0616 +#define UBLOX_CFG_MSG 0x0601 +#define UBLOX_CFG_RATE 0x0608 +#define UBLOX_NMEA_GLL 0xF001 +#define UBLOX_NMEA_GSA 0xF002 +#define UBLOX_NMEA_GSV 0xF003 +#define UBLOX_NMEA_ZDA 0xF008 +#define UBLOX_NMEA_VTG 0xF005 +#define UBLOX_NMEA_GGA 0xF000 +#define UBLOX_NMEA_RMC 0xF004 +#define UBLOX_MON_VER 0x0A04 +#define CFG_DONE 0xFFFF + +/// some definition for ublox plug-play +#define QUERY_TIMEOUT 1000 ///< ms: 1 seconds + +#define TEST_ID_RECORD 10 //ublox + +/// message headers +#define UBLOX_BINAERY_SYNC 0xB562 +#define UBLOX_BINAERY_HEADER_LEN 6 +#define NOVATEL_ASCII_HEADER 0x23504F ///< "#PO" +#define NMEA_HEADER 0x244750 ///< "$GP" +#define NOVATEL_OME4_BINARY_HEADER 0xAA4412 +#define SIRF_BINARY_HEADER 0xA0A2 +#define SIRF_END_SEQUENCE 0xB0B3 +#define MAX_HEADER_LEN 4 +#define NMEA_SYNC_1 0x00244750 // $GP +#define NMEA_SYNC_2 0x0024474E // $GN + +/// maximum waiting time for a GPS message to complete +//#define GPS_PACKET_RECEIVE_TIMEOUT 500 ///< 0.5 second +#define GPS_PACKET_RECEIVE_TIMEOUT 100 /// < 100ms for 5Hz data rate + +/// UBLOX msg Rate +#define NAV_SBAS_RATE_RATIO 40 ///< 40*0.25=10 sec. 0.1Hz +///constants for monitoring ublox message rate. +#define TIME_INTERVAL_FOR_RATE_CALCU 20000 ///< ms: 20 seconds +#define TICKS_TO_SECONDS 0.001 +#define MIN_POSLLH_RATE 0.2 ///< 0.2 Hz timeout +#define MIN_VELNED_RATE 0.2 ///< 0.2 Hz timeout +#define MIN_STATUS_RATE 0.2 ///< 0.2 Hz timeout +#define MIN_SBAS_RATE 0.0 ///< completely stop +#define MAX_LOW_RATE_COUNT 3 + +/// ublox FW version threshold +#define NAV2_ENABLED_VERSION 3.04 + +/// used for autoProtocol +#define MAX_PROTOCOL_SEARCHTIME 2000 ///< 2 seconds +#define INIT_SEARCH_BAUD 3 ///< 3 - BAUD_57600 + +///for NMEA vertical process +#define ALT_FILTER_GAIN 0.2 ///< 1/(4+1) +#define VD_FILTER_GAIN 0.0476 ///< 1/(20+1) +#define MAX_ALTI_JUMP 10.0 ///< meters +#define MIN_DELTA_T 1.e-5 ///< seconds 0.01 ms +#define MAX_DELTA_T 2 ///< seconds +#define MAX_VEL_JUMP 5 + +/** struct ubloxIDTypeSTRUCT + @brief specify ublox message ID and associated message rate and target port. + */ +typedef struct +{ + uint32_t iTOW; // ms + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; // Validity flags + uint32_t tAcc; // time accuracy estimate (UTC), ns + int32_t nano; // fraction of second, range -1e9..1e9 (UTC), ns + uint8_t fixType; /* GNSS fix type + * 0: no fix + * 1: dead reckoning only + * 2: 2D-fix + * 3: 3D-fix + * 4: GNSS + dead reckoning combined + * 5: time only fix + */ + uint8_t flags; /* Fix status flags + * bit0: gnssFixOK, 1 = valid fix (i.e within DOP & accuracy masks) + * bit1: diffSoln, 1 = differential corrections were applied + * bit2-4: psmState, Power Save Mode state + * bit5: headVehValid, 1 = heading of vehicle is valid + * bit6-7: carrSoln, Carrier phase range solution status: + * 0: no carrier phase range solution + * 1: float solution + * 2: fixed solution + */ + uint8_t flags2; + uint8_t numSV; // number of satellites in Nav solution; + int32_t lon; // deg, scaling is 1e-7 + int32_t lat; // deg, scaling is 1e-7 + int32_t height; // mm, height above ellipsoid; + int32_t hMSL; // mm, hegiht above mean seal level + uint32_t hAcc; // mm, horizontal accuracy estimate + uint32_t vAcc; // mm, vertical accuracy estimate + int32_t velN; // mm/s, north velocity + int32_t velE; // mm/s, east velocity + int32_t velD; // mm/s, dow velocity + int32_t gSpeed; // mm/s, groud speed (2-D) + int32_t headMot; // deg, scaling is 1e-5, heading of motion (2-D) + uint32_t sAcc; // mm/s, speed accuracy estimate + uint32_t headAcc; // deg, scaling is 1e-5, heading accuracy estimate + uint16_t pDOP; // scaling is 0.01, position DOP + uint8_t reserved1[6]; + int32_t headVeh; // deg, scaling is 1e-5, heading of vehicle (2-D) + int16_t magDec; // deg, scaling is 1e-2, magnetic declination + uint16_t magAcc; // deg, scaling is 1e-2, magnetic declination accuracy +} ubloxNavPvtSTRUCT; + +typedef struct { + char classID; + char msgID; + char cfgClassID; + char cfgMsgID; + char rateRatio; + char whatTagert; +}ubloxIDTypeSTRUCT; + +typedef struct { + int lat_deg; + int lat_min; + double lat_min_fraction; + int lon_deg; + int lon_min; + double lon_min_fraction; + char latSign; + char lonSign; +} NmeaLatLonSTRUCT; + +// delta struct +typedef struct { + double oldValues[3]; // moving window of delta values + double sum; + double last; // Last whole value +} gpsDeltaStruct; + +double avgDeltaSmoother(double in, gpsDeltaStruct* data); +void thresholdSmoother( double dataIn[3], float dataOut[3]); + + +/// function prototypes + +/// interface with algorithms +// these are duplicated in gps.h +// driverGPSAllEntrance.cpp +void GPSHandler(void); + +void initGPSHandler(void); /// note: should pass in SPI or UART + +void initGPSDataStruct(void); /// note: should pass in SPI or UART + +/// ublox binary processUbloxGPS.cpp +void configUbloxGPSReceiver(int *bytesFromBuffer, GpsData_t* GPSData); +void processUbloxBinaryMessage(char *msg,unsigned int *msgLength, GpsData_t *GPSData); +unsigned char configurateUBloxGPSPerformance (GpsData_t* GPSData); +unsigned char configurateUBloxGPSIOMsgRate (GpsData_t* GPSData); +unsigned char getConnectedWithUnknownStatusUbloxGPS(GpsData_t* GPSData); +void pollUbloxMsg(ubloxIDTypeSTRUCT *IDInput, GpsData_t *GPSData); + +/// NMEA processNMEAGPS.cpp +void processNMEAMessage(char *msg, unsigned int *msgLength, GpsData_t* GPSData); +char extractNMEAfield(char *msgBody, char *fieldOutput, int fieldIndex, int MAXFieldLength); +char computeNMEAChecksum(char *NMEAMess, unsigned int *lengthBeforeStar); +void convertNorhEastVelocity(GpsData_t* GPSData); +void convertItow(GpsData_t* GPSData); + +/// SiRF binary processsSiRFGPS.cpp +void processSiRFBinaryMessage(char *msg,unsigned int *msgLength, GpsData_t *GPSData); +void pollSiRFVersionMsg(void); +void sirfBinarySwitchBaud(uint32_t baud); +void configSiRFGPSReceiver(GpsData_t* GPSData, int goalBaudRate); + +/// NovAtel binary processNovAtelGPS.cpp +void processNovAtelBinaryMsg(char *msg,unsigned int *msgLength, GpsData_t* GPSData); +void processNovAtelBinaryMsg_Fast(char *msg, unsigned int *msgLength, GpsData_t *GPSData); + +// driver interface driverGPS.cpp +extern uint32_t baudEnumToBaudRate(int baudRate); + +int gpsBytesAvailable(); +int initGpsUart(int baudrate); +void initOnePpsUart( void ); +//void setExternalPortPtr(port_struct* gGpsPortPtr); +//void flushGPSRecBuf(void); +//BOOL isGpsTxEmpty(void); +//uint16_t delBytesGpsBuf(uint16_t numToPop); +//uint16_t peekWordGpsBuf(uint16_t index); +//uint8_t peekByteGpsBuf(uint16_t index); +//unsigned long peekGPSmsgHeader(uint16_t index, GpsData_t *GPSData); +int writeGps(char *send, uint16_t len); +//int16_t findHeader(uint16_t numInBuff, GpsData_t *GPSData); +//int16_t retrieveGpsMsg(uint16_t numBytes, GpsData_t *GPSData, uint8_t *outBuffer); +//unsigned char autobaud(GpsData_t* GPSData); +BOOL HandleGps(GpsData_t *GPSData); +int parseNMEAMessage(uint8_t inByte, uint8_t *gpsMsg, GpsData_t *GPSData); +int parseNovotelBinaryMessage(uint8_t inByte, uint8_t *gpsMsg, GpsData_t *GPSData); +int parseUbloBinaryMessage(uint8_t inByte, uint8_t *gpsMsg, GpsData_t *GPSData); + + + +#define GPS_INTERFACE_RX_BUFFER_SIZE 512 +#define GPS_INTERFACE_TX_BUFFER_SIZE 30 + +#endif /// END OF DRIVERGPS_H diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/include/gps.h b/examples/OpenIMU300RI/INS/lib/Core/GPS/include/gps.h new file mode 100644 index 0000000..d2dbb2d --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/include/gps.h @@ -0,0 +1,37 @@ +/** *************************************************************************** + * @file gps.h GPS Driver for Inertial/GPS NAV. + * @author Dong An + * @date 2009-04-10 23:20:59Z + * @ver 8719 + * @copyright (c) 2013, 2014 All Rights Reserved. + * @section LICENSE + * 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 GPS interface, taken loosely from the DMU440 + * project possibly implemented using the Origin ORG4475 GPS module (or NovAtel + * or uBlox) the GPS may communicated via SPI or UART, that is passed in on init + * 03.2007 DA Cleaned up, Doxygenized, and finalized for NAV440 release. + *****************************************************************************/ +#ifndef GPS_H +#define GPS_H + +#include + + +// debugging stream out the debug port +#define GPS_NO_STREAM 0 +#define GPS_NMEA_DEBUG_STREAM 1 + +#ifndef STREAM_GPS +//#define STREAM_GPS GPS_NO_STREAM +#define STREAM_GPS GPS_NMEA_DEBUG_STREAM +#endif + +#define NMEA_SYNC_1 0x00244750 // $GP +#define NMEA_SYNC_2 0x0024474E // $GN + + +#endif /* GPS_H */ \ No newline at end of file diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/src/GPSgroudSpeedFilter.c b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/GPSgroudSpeedFilter.c new file mode 100644 index 0000000..0c584b3 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/GPSgroudSpeedFilter.c @@ -0,0 +1,155 @@ +/** *************************************************************************** + * @file GPSgroudSpeedFilter.c GPS Driver for Intertial/GPS NAV.speed smoother + * + * 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. + * + * @briefsmoothing data using a moving average filtering with a robust weight + * in order to exclude outliers. The maximum delay occurred in the smoothed + * data is 400 msecs. This technique is essentially same as a low pass filter + * except it is time-independent. + * + * This module provides high level GPS management, based on product type and + * GPS receiver. This module is interfaced with NAV processing and other + * specific GPS process files. The functions are provided in this files are + * common for all GPS protocol. Protocol-specific process is provided in other + * GPS files + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ +#ifdef GPS + +#include +#include +#include "driverGPS.h" + +#define MEDIAN_FILTER_DATA_SIZE 3 +#define MOVING_WINDOW_LENGTH 3 +#define TRUST_FACTOR 6 + +/** **************************************************************************** + * @name: _median LOCAL routine for calculating a median value + * @author JJANG + * @param [in] in - new value + * @retval actual baud rate value + ******************************************************************************/ +float _median1(float *in) +{ + int i; + int j; + float temp; + float out; + float dta[MEDIAN_FILTER_DATA_SIZE]; + + for(i = 0; i < MEDIAN_FILTER_DATA_SIZE; i++) + dta[i] = in[i]; + + for (i = 1; i < MEDIAN_FILTER_DATA_SIZE; i++) { + temp = dta[i]; + j = i; + while( dta[j - 1] > temp) { + dta[j] = dta[j - 1]; + j--; + if(j == 0) + break; + } + dta[j] = temp; + } + out = ( !(MEDIAN_FILTER_DATA_SIZE % 2) ) ? + (dta[MEDIAN_FILTER_DATA_SIZE / 2 - 1] + dta[MEDIAN_FILTER_DATA_SIZE / 2]) / 2 : + dta[(MEDIAN_FILTER_DATA_SIZE - 1) / 2]; + return out; +} + +/** **************************************************************************** + * @name: lowpass_filter LOCAL routine for smoothing data using a moving average + * with a robust weight + * @author JJANG + * @param [in] in - new value + * @retval actual baud rate value + * ccalled in processNMEAGPS.cpp + ******************************************************************************/ +float lowpass_filter(float *in) +{ + static int weight[MOVING_WINDOW_LENGTH]; + static float dtapoint[MOVING_WINDOW_LENGTH]; + static float r[MOVING_WINDOW_LENGTH]; + static unsigned char initialFlag = 0; + int i; + int j; + int sum; + float out; + float MAD; + + if (initialFlag == 0) { + initialFlag = 1; + + for(i = 0; i < MOVING_WINDOW_LENGTH; i++) { + weight[i] = 1; + dtapoint[i] = 0; + r[i] = 0; + } + } + dtapoint[MOVING_WINDOW_LENGTH - 1] = *in; + + ///compute moving average + out = 0; + sum = 0; + for(j = 0;j < MOVING_WINDOW_LENGTH; j++) { + out += weight[j] * dtapoint[j]; + sum += weight[j]; + } + + if (sum == 0) + sum = 1; + out /= (float)sum; + + ///absolute value of residual of ith data + r[MEDIAN_FILTER_DATA_SIZE - 1] = (float)fabs(dtapoint[MOVING_WINDOW_LENGTH - 1] - out); + ///median value of the residuals over window + MAD = _median1(r); + + ///if the residual is larger than a given threshold + ///exclude data (6 * the median) + if (r[MOVING_WINDOW_LENGTH - 1] > TRUST_FACTOR * MAD) + weight[MOVING_WINDOW_LENGTH - 1] = 0; + else + weight[MOVING_WINDOW_LENGTH - 1] = 1; + + ///do moving avergae with new weight + out = 0; + sum = 0; + for(j = 0;j < MOVING_WINDOW_LENGTH; j++) { + out += weight[j] * dtapoint[j]; + sum += weight[j]; + } + if (sum == 0) + sum = 1; + out /= (float)sum; + + for (i = 0; i < (MOVING_WINDOW_LENGTH - 1); i++) { + dtapoint[i] = dtapoint[i + 1]; + r[i] = r[i + 1]; + weight[i] = weight[i + 1]; + } + weight[MOVING_WINDOW_LENGTH - 1] = 1; + + return out; +} + +#endif // GPS \ No newline at end of file diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/src/driverGPS.c b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/driverGPS.c new file mode 100644 index 0000000..89416a6 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/driverGPS.c @@ -0,0 +1,542 @@ +/** *************************************************************************** + * @file driverGPS.c serial interface and buffer functions + * + * 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. + *****************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ +#ifdef GPS + +#include +#include + +#include "osapi.h" +#include "driverGPS.h" +#include "uart.h" +#include "gpsAPI.h" +#include "math.h" +#include "platformAPI.h" + +#ifdef DISPLAY_DIAGNOSTIC_MSG +#include "debug.h" +#endif + +int gpsSerialChan = UART_CHANNEL_NONE; // undefined, needs to be explicitly defined + + +/// GPS data struct +// to change to NMEA then pass GPS out debug port: un-comment this and +// set LOGGING_LEVEL to LEVEL_STREAM. Nugget must have GPS config file loaded +// output stream is at 115200 baud even though the internal gps may be at 4800 +#define STREAM_GPS GPS_NMEA_DEBUG_STREAM // [1] +GpsData_t gGpsData = { + .GPSbaudRate = 4800, + .GPSProtocol = NMEA_TEXT, + //.sirfInitialized = FALSE, +}; + +GpsData_t *gGpsDataPtr = &gGpsData; + +// local functions +void _configGPSReceiver(GpsData_t* GPSData); +void _parseGPSMsg(int *numInBuffPrt, GpsData_t *GPSData); +int16_t _getIndexLineFeed(uint16_t numInBuff, unsigned *msgLength); +void _setGPSMessageSignature(GpsData_t* GPSData); +void _userCmdBaudProtcol(GpsData_t* GPSData); + +void loadGpsCommSettings(GpsData_t* GPSData) +{ + GPSData->GPSAUTOSetting = 0; + _setGPSMessageSignature(GPSData); + GPSData->GPSConfigureOK = 0; +} + +// extern_port.c +extern void GetGpsExternUartChannel(unsigned int* uartChannel); + +/** **************************************************************************** + * @name: initGpsUart set up baudrate and GPS UART + * @param [in] gGpsDataPtr - GPS control structure + * @retval N/A + ******************************************************************************/ +int initGpsUart(int baudrate) +{ + gpsSerialChan = platformGetSerialChannel(GPS_SERIAL_PORT); + return uart_init(gpsSerialChan, baudrate); +} + + +/** **************************************************************************** + * @name: gpsBytesAvailable bytes in Gps circ buffer API only used in + * driverGPSAllEntrance + * @param N/A + * @retval number of bytes in Gps circ buffer + ******************************************************************************/ +int gpsBytesAvailable() +{ + return uart_rxBytesAvailable(gpsSerialChan); +} + +/** **************************************************************************** + * @name: flushGPSRecBuf empty GPS interface's software buffer. + * @param N/A + * @retval N/A + ******************************************************************************/ +void flushGPSRecBuf(void) +{ + uart_flushRecBuffer(gpsSerialChan); +} + +/** **************************************************************************** + * @name isGpsTxEmpty check if transmit buffer is empty... + * @param N/A + * @retval N/A + ******************************************************************************/ +BOOL isGpsTxEmpty(void) +{ + return uart_txBytesRemains(gpsSerialChan) == 0; +} + +/** **************************************************************************** + * @name delBytesGpsBuf will pop bytes off in the receive FIFO. + * @param [in] numToPop is the number of bytes to pop off + * @retval the number of bytes remaining in the FIFO. + ******************************************************************************/ +uint16_t delBytesGpsBuf(uint16_t numToPop) +{ + int16_t numInBuffer; + + numInBuffer = uart_rxBytesAvailable(gpsSerialChan); + if (numInBuffer < numToPop) { + numToPop = numInBuffer; + } + uart_removeRxBytes(gpsSerialChan, numToPop); +// COM_buf_delete(&(gGpsUartPtr->rec_buf), +// numToPop); + return ( numInBuffer - numToPop ); ///< unscanned bytes +} + +/** **************************************************************************** + * @name peekWordGpsBuf will return a word without popping the FIFO. + * @brief No error checking on the range of index is performed. + * @param [in] index represents the byte offset into the FIFO to start looking at. + * @retval the 16-bit word in the FIFO starting at index offset from end. + ******************************************************************************/ +/* +uint16_t peekWordGpsBuf(uint16_t index) +{ + uint16_t word; + + word = peekByteGpsBuf(index) << 8 | peekByteGpsBuf(index + 1); + return word; +} +*/ +/** **************************************************************************** + * @name peekByteGpsBuf peekByteSCIA will return a byte without popping + * the FIFO. + * @brief No error checking on the range of index is performed. + * @param [in] index represents the byte offset into the FIFO to look at. + * @retval the byte in the FIFO pointed to by index. + ******************************************************************************/ +uint8_t peekByteGpsBuf(uint16_t index) +{ + uint8_t output = 0; + + uart_copyBytes(gpsSerialChan, index, 1, &output); + + return output; +} + +/** **************************************************************************** + * @name peekGPSmsgHeader search and return GPS message header + * @brief No error checking on the range of index or start of sentence word is + * performed. + * @param [in] index represents the byte offset into the FIFO to look at. + * @param [in] GPSData - data structure containng the message data + * @retval the message header. + ******************************************************************************/ +unsigned long peekGPSmsgHeader(uint16_t index, + GpsData_t *GPSData) +{ + uint8_t header[MAX_HEADER_LEN]; + int i; + int j; + int headerLength; + unsigned long GPSHeader; + + headerLength = GPSData->GPSMsgSignature.GPSheaderLength; + uart_copyBytes(gpsSerialChan, index, headerLength, header); + GPSHeader = 0; + + for (i = 0, j = headerLength - 1; i < headerLength; i++, j--) { + GPSHeader |= header[i] << (j * 8); + } + + return GPSHeader; +} + +/** **************************************************************************** + * @name retrieveGpsMsg search for start of GPS message header + * @brief COM_buf_get does a check for underflow the returns the message or + * nothing + * @param [in] numBytes - to retrieve. + * @param [in] GPSData - data structure containing the message information + * @param [out] outBuffer - pointer to the output buffer + * @retval The number of bytes left in the buffer. Or Zero - finished or + * buffer had exactly the number of bytes in the complete messsage + ******************************************************************************/ +int16_t retrieveGpsMsg(uint16_t numBytes, GpsData_t *GPSData, uint8_t *outBuffer) +{ + uart_read(gpsSerialChan, outBuffer, numBytes); + return uart_rxBytesAvailable(gpsSerialChan); +} + +/** **************************************************************************** + * @name findHeader search for start of GPS message header + * @brief Does a simple check for the start byte then peeks at enough bytes + * to compare with signature header. + * @param [in] numInBuff - bytes available in input buffer. + * @param [in] GPSData - data structure containing the message information + * @retval The number of bytes left in the buffer including the header. Can + * be less than the complete messsage + ******************************************************************************/ +int16_t findHeader(uint16_t numInBuff, + GpsData_t *GPSData) +{ + uint8_t buf[MAX_HEADER_LEN]; + uint8_t byte; + uint32_t header = 0; + volatile uint8_t exit = 0; + int num; + + do { + uart_copyBytes(gpsSerialChan,0,1,&byte); + + if (byte == GPSData->GPSMsgSignature.startByte) { + uart_copyBytes(gpsSerialChan,1,GPSData->GPSMsgSignature.GPSheaderLength - 1, buf); + header = byte << (GPSData->GPSMsgSignature.GPSheaderLength - 1) * 8; + switch (GPSData->GPSMsgSignature.GPSheaderLength) { + case 2: // SiRF 0xa0a2 + header |= (buf[0]); + break; + case 3: // NMEA "$GP", NovAtel binary 0xAA4412 + header |= (buf[0] << 8) | (buf[1]); + break; + } + if ( header == GPSData->GPSMsgSignature.GPSheader ) { + exit = 1; + } else { + num = uart_removeRxBytes(gpsSerialChan, 1); + if(num){ + numInBuff--; + } + } + } else { + num = uart_removeRxBytes(gpsSerialChan, 1); + if(num){ + numInBuff--; + } + } + } while ( (exit == 0) && (numInBuff > 0) ); + return numInBuff; +} + +/** **************************************************************************** + * @name writeGps send data to the GPS + * @param [in] data - to go out + * @param [in] len - length in bytes + * @retval N/A + * @details gConfiguration.userBehavior.bit.useGPS (use USART) determines + * which pipeline to send command through + ******************************************************************************/ +int writeGps(char *data, uint16_t len) +{ + return uart_write(gpsSerialChan, (uint8_t*)data, len); + +} + +void GetGPSData(gpsDataStruct_t *data) +{ + data->gpsUpdate = ( gGpsDataPtr->updateFlagForEachCall >> GOT_VTG_MSG ) & 0x00000001 && + ( gGpsDataPtr->updateFlagForEachCall >> GOT_GGA_MSG ) & 0x00000001; + // gGpsDataPtr->updateFlagForEachCall &= 0xFFFFFFFD; + if(data->gpsUpdate) + { + // Reset GPS update flag only when pos and vel are both available + gGpsDataPtr->updateFlagForEachCall &= 0xFFFFFFFC; + + data->gpsFixType = gGpsDataPtr->gpsFixType; + data->numSatellites = gGpsDataPtr->numSatellites; + + data->latitude = gGpsDataPtr->lat; + data->longitude = gGpsDataPtr->lon; + data->altitude = gGpsDataPtr->alt; + + data->vNed[0] = gGpsDataPtr->vNed[0]; + data->vNed[1] = gGpsDataPtr->vNed[1]; + data->vNed[2] = gGpsDataPtr->vNed[2]; + + data->trueCourse = gGpsDataPtr->trueCourse; + data->rawGroundSpeed = gGpsDataPtr->rawGroundSpeed; + + data->GPSSecondFraction = gGpsDataPtr->GPSSecondFraction; + + data->itow = gGpsDataPtr->itow; + data->GPSmonth = gGpsDataPtr->GPSmonth; + data->GPSday = gGpsDataPtr->GPSday; + data->GPSyear = gGpsDataPtr->GPSyear; + data->GPSHour = gGpsDataPtr->GPSHour; + data->GPSMinute = gGpsDataPtr->GPSMinute; + data->GPSSecond = gGpsDataPtr->GPSSecond; + + data->HDOP = gGpsDataPtr->HDOP; + data->GPSHorizAcc = gGpsDataPtr->GPSHorizAcc; + data->GPSVertAcc = gGpsDataPtr->GPSVertAcc; + data->geoidAboveEllipsoid = gGpsDataPtr->geoidAboveEllipsoid; + } +} + + +/** **************************************************************************** + * @name: avgDeltaSmoother API routine using data structure to hold instance data + * for smoothing gps using a 3 item moving average of the change in values + * (deltas) that removes data "spikes" over 6 times the average + * @author + * @param [in] in - new value to add to the average + * @param [in] data - filter data for lat, lon or alt + * @retval last value if > than 6x avg or pass the value back out + ******************************************************************************/ +double avgDeltaSmoother( double rawData, gpsDeltaStruct *delta) +{ + double thisDelta; + double returnVal; + + thisDelta = delta->last - rawData; + + delta->sum -= delta->oldValues[2]; // pop the old value off of the sum + delta->sum += thisDelta; // push the new value in the sum + + // push the data down the hold stack + delta->oldValues[2] = delta->oldValues[1]; + delta->oldValues[1] = delta->oldValues[0]; + delta->oldValues[0] = thisDelta; + if ( fabs(thisDelta) > 6.0 * fabs( delta->sum / 3.0 ) ) { + returnVal = delta->last; // filter (omit) the value + } else + returnVal = rawData; // send the input value back out + + delta->last = rawData; // hold input value for next entry + return returnVal; +} + +/****************************************************************************** + * @name: thresholdSmoother API threshold filter that uses total speed from Vned + * for smoothing gps "glitches" (in delta speed) "spikes" over 15m/s + * @author + * @param [in] in - new value to compare to threshold + * @param [in] data - return last good data or current good data + * @retval N/A + ******************************************************************************/ +void thresholdSmoother( double vNedIn[3], + float vNedOut[3]) +{ + double speedSqCurr; + double SPEED_SQ_LIMIT = 225; // limit the change to 15 m/s + static double speedSqPast; + static double vNedPast[3]; + + // "glitch" filter + // Compute the speed + speedSqCurr = vNedIn[0]*vNedIn[0] + vNedIn[1]*vNedIn[1] + vNedIn[2]*vNedIn[2]; + + // "Filter" the velocity signal if the delta is greater than 15 [ m/s ] + // (This is an 8% margin over expected system dynamics) + if( fabs( speedSqCurr - speedSqPast ) > SPEED_SQ_LIMIT ) { + // If a glitch is encountered, set output to last "good" value. + // Do not update past with current. + vNedOut[0] = (float)vNedPast[0]; + vNedOut[1] = (float)vNedPast[1]; + vNedOut[2] = (float)vNedPast[2]; + } else { + // Do not change GPS velocity, update past values with current values + speedSqPast = speedSqCurr; + vNedPast[0] = vNedIn[0]; + vNedPast[1] = vNedIn[1]; + vNedPast[2] = vNedIn[2]; + + vNedOut[0] = (float)vNedIn[0]; + vNedOut[1] = (float)vNedIn[1]; + vNedOut[2] = (float)vNedIn[2]; + } +} + +/** **************************************************************************** + * @name initGPSHandler initializes data GPS structure using all configuration + * structure that is read from EEPROM. + * @athor Dong An + * @param [in] baudRate - enumeration to translate + * @retval actual baud rate value + ******************************************************************************/ +void initGPSHandler(void) +{ + #ifdef GPS + gGpsDataPtr->HDOP = 50.0f; + gGpsDataPtr->GPSTopLevelConfig |= (1 << HZ2); // update to change internal (ublox) GPS to 2Hz + /// Configure GPS structure, from Flash (EEPROM) + loadGpsCommSettings(gGpsDataPtr); + initGpsUart(gGpsDataPtr->GPSbaudRate); +#endif +} + + + +/** **************************************************************************** + * @name initGPSDataStruct initializes data GPS structure using all configuration + * structure that is read from EEPROM. + * @athor Dong An + * @param [in] baudRate - enumeration to translate + * @retval actual baud rate value + ******************************************************************************/ +void initGPSDataStruct(void) +{ + gGpsDataPtr = &gGpsData; + memset((void*)&gGpsData, 0, sizeof(GpsData_t)); + gGpsData.GPSbaudRate = 4800; + gGpsData.GPSProtocol = NMEA_TEXT; +} + + +BOOL _handleGpsMessages(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; + + + while(1){ + if(!bytesInBuffer){ + bytesInBuffer = uart_read(gpsSerialChan, gpsUartBuf, sizeof (gpsUartBuf)); + if(!bytesInBuffer){ + return 0; // nothing to do + } + pos = 0; + } + tmp = gpsUartBuf[pos++]; + bytesInBuffer--; + switch(GPSData->GPSProtocol){ + case NMEA_TEXT: + parseNMEAMessage(tmp, gpsMsg, GPSData); + break; + case NOVATEL_BINARY: + parseNovotelBinaryMessage(tmp, gpsMsg, GPSData); + break; + case UBLOX_BINARY: + parseUbloBinaryMessage(tmp, gpsMsg, GPSData); + break; + default: + break; + } + } +} +/* end _handleGpsMessages */ + +/** **************************************************************************** + * @name GPSHandler GPS stream data and return GPS data for NAV algorithms + * @athor Dong An + * @retval N/A + * @brief The _configGPSReceiver() has a 2 second delay in it that causes a + * DAQ restart in the data acquisition task + ******************************************************************************/ +void GPSHandler(void) +{ + gGpsDataPtr->Timer100Hz10ms = getSystemTime(); ///< get system clock ticks + gGpsDataPtr->isGPSBaudrateKnown = 1; + + // 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); + } +} + +/** **************************************************************************** + * @name _configGPSReceiver LOCAL configure GPS parses GPS stream data and fills + * GPS data into NAV filter structure. + * @brief All GPS receiver configuration commands should be sent here if needed. + * @author Dong An + * @param [out] bytesFromBuffer - data from GPS data buffer + * @param [out] GPSData - gps data structure + * @retval N/A + ******************************************************************************/ +void _configGPSReceiver(GpsData_t *GPSData) +{ + + _handleGpsMessages(GPSData); // load GPSdata structure + + if (GPSData->GPSProtocol == SIRF_BINARY) { ///< 0 - internal GPS receiver is Origin SiRF binary or NMEA + configSiRFGPSReceiver(GPSData, 230400); + _setGPSMessageSignature(GPSData); + } else { + // assuming that rate and protocol configured properly from the beginning + GPSData->GPSConfigureOK = 1; + } +} + +/** **************************************************************************** + * @name _setGPSMessageSignature LOCAL input GPS message spec (or feature) into + * GPS structure + * @author Dong An + * @param [in] GPSData - GPS data + * @retval status + ******************************************************************************/ +void _setGPSMessageSignature(GpsData_t* GPSData) +{ + switch(GPSData->GPSProtocol) { + case SIRF_BINARY : + GPSData->GPSMsgSignature.GPSheader = SIRF_BINARY_HEADER; // 0xa0a2 + GPSData->GPSMsgSignature.GPSheaderLength = 2; + GPSData->GPSMsgSignature.lengthOfHeaderIDLength = 4; // header len + 2 payload len + GPSData->GPSMsgSignature.crcLength = 2; + GPSData->GPSMsgSignature.binaryOrAscii = 0; + break; + case NOVATEL_BINARY : + GPSData->GPSMsgSignature.GPSheader = NOVATEL_OME4_BINARY_HEADER; // 0xAA4412 + GPSData->GPSMsgSignature.GPSheaderLength = 3; + GPSData->GPSMsgSignature.lengthOfHeaderIDLength = 5; + GPSData->GPSMsgSignature.binaryOrAscii = 0; + GPSData->GPSMsgSignature.crcLength = 4; + break; + case NMEA_TEXT : + GPSData->GPSMsgSignature.GPSheader = NMEA_HEADER; // "$GP" + GPSData->GPSMsgSignature.GPSheaderLength = 3; + GPSData->GPSMsgSignature.binaryOrAscii = 1; + GPSData->GPSMsgSignature.lengthOfHeaderIDLength = 3; + break; + default: + break; + } + GPSData->GPSMsgSignature.startByte = GPSData->GPSMsgSignature.GPSheader >> ( (GPSData->GPSMsgSignature.GPSheaderLength * 8) - 8); +} + +#endif // GPS + + diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/src/driverGPSAllEntrance.c b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/driverGPSAllEntrance.c new file mode 100644 index 0000000..d46557b --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/driverGPSAllEntrance.c @@ -0,0 +1,83 @@ +/** *************************************************************************** + * @file driverGPSAllentrance.c GPS Driver for Internal/GPS NAV. + * + * 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. + * @details + * This module provides high level GPS management, + * based on product type and GPS receiver. This module + * is interfaced with NAV processing and other specific GPS + * process files. The functions are provided in this files are common + * for all GPS protocol. Protocol-specific process is provided in other GPS files + *****************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ +#ifdef GPS + + +#include + +#include "GpsData.h" +#include "gpsAPI.h" + +float getGpsHdop() {return gGpsDataPtr->HDOP;} + + +BOOL SetGpsBaudRate(int rate, int fApply) +{ + switch (rate) + { + case 4800: + case 9600: + case 19200: + case 38400: + case 57600: + case 115200: + case 230400: + break; + default: + return FALSE; + } + if(fApply) + { + gGpsDataPtr->GPSbaudRate = rate; + } + + return TRUE; +} + +BOOL SetGpsProtocol(int protocol, int fApply) +{ + switch(protocol) + { + case NMEA_TEXT: + case NOVATEL_BINARY: + case UBLOX_BINARY: + break; + default: + return FALSE; + } + if(fApply) + { + gGpsDataPtr->GPSProtocol = protocol; + } + + return TRUE; +} + +#endif // GPS diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processNMEAGPS.c b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processNMEAGPS.c new file mode 100644 index 0000000..f51ada5 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processNMEAGPS.c @@ -0,0 +1,714 @@ +/** *************************************************************************** + * @file processNMEAGPS.c + * + * 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. + * @betails Processing for GPS NMEA protocol. + * + * This includes all specific processing for NMEA protocol. + *****************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ +#ifdef GPS + +#include +#include +#include +#include +#include +#include + +#include "gpsAPI.h" +#include "driverGPS.h" +#include "BITStatus.h" + +char _parseGPGGA(char *msgBody, GpsData_t* GPSData); +char _parseVTG(char *msgBody, GpsData_t* GPSData); +char _parseRMC(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); + +int dayofweek(int day, int month, int year); + +int crcError = 0; +int starError = 0; +/** **************************************************************************** + * @name: processNMEAMessage parse a complete NMEA message. + * @author Dong An + * @param [in] msg - data to parse + * @param [in] msgLength - length of data to parse + * @param [in] GPSData - data structure to parse into + * @retval N/A + * @details Already found the header, checks for '*' delimeter and checksum + * before calling decode + * 07/01/15 dkh removed redundant checks and simplified conversion from ascii + * to int + ******************************************************************************/ +void processNMEAMessage(char *msg, + unsigned int *msgLength, + GpsData_t *GPSData) +{ + volatile char checksumRcvd; + volatile char checksumCalc; + unsigned int lengthBeforeStar; + uint8_t tmp; + + if (msg[*msgLength - 5]!= '*'){ + starError++; + return; /// no '*' delimiter before checksum + + } + // convert hex "digits" from ascii to int + tmp = msg[*msgLength - 4]; + checksumRcvd = tmp > '9'? tmp - '7' : tmp - '0'; + checksumRcvd <<= 4; + tmp = msg[*msgLength - 3]; + checksumRcvd |= tmp > '9'? tmp - '7' : tmp - '0'; + + lengthBeforeStar = *msgLength - 5; + checksumCalc = computeNMEAChecksum(msg, &lengthBeforeStar); + if (checksumCalc == checksumRcvd) { + _handleNMEAmsg(msg, // header "$GP..." + &msg[7], // payload + GPSData); + }else{ + crcError++; + checksumCalc = computeNMEAChecksum(msg, &lengthBeforeStar); + } +} + +/** **************************************************************************** + * @name: parseNMEAMessage parse a complete NMEA message. + * @author Dong An + * @param [in] inByte - next byte from serial stream + * @param [in] gpsMsg - GPS message buffer + * @param [in] GPSData - data structure to parse into + * @retval N/A + * @details Extract message from serial byte stream and process + ******************************************************************************/ +int parseNMEAMessage(uint8_t inByte, uint8_t *gpsMsg, GpsData_t *GPSData) +{ + static int state = 0; + static unsigned int len = 0; + static uint8_t *ptr; + static uint32_t sync = 0; + unsigned static int synced = 0; + + sync = (sync << 8) | inByte; + synced = 0; + + if ((sync & 0x00ffffff) == NMEA_SYNC_1) + { + synced = 1; + gpsMsg[0] = '$'; + gpsMsg[1] = 'G'; + gpsMsg[2] = 'P'; + } else if ((sync & 0x00ffffff) == NMEA_SYNC_2) { + synced = 1; + gpsMsg[0] = '$'; + gpsMsg[1] = 'G'; + gpsMsg[2] = 'N'; + } + + // After getting synced, start to wait for the full message + if (synced){ + state = 1; + ptr = &gpsMsg[3]; + len = 3; + return 0; + } + + if(state == 0){ + return 0; + } + + // After getting synced, put each input byte into gpsMsg + *ptr++ = inByte; + len++; + + if (len >= MAX_MSG_LENGTH){ + // overflow - reset packet engine + state = 0; + return 1; + } + + if (inByte == 0x0a) + { // LF - last character + // got full message - process it + processNMEAMessage((char *)gpsMsg, &len, GPSData); + state = 0; + } + + return 0; +} + + + +/** **************************************************************************** + * @name: _handleNMEAmsg decode specific GPS NMEA messages. + * @author Dong An + * @param [in] msgId - message ID + * @param [in] msgBody - data to parse + * @param [in] GPSData - data structure to parse into + * @retval actual baud rate value + * @details GGA is fix data + * VTG Vector track ofer the Ground + * 07/01/15 dkh changed to strncmp so don't need to add '\0' + ******************************************************************************/ +void _handleNMEAmsg(char *msgID, + char *msgBody, + GpsData_t *GPSData) +{ + char *ptr = (char *)&msgID[3]; + + if( strncmp(ptr, "GGA", 3) == 0 ) { + GPSData->totalGGA++; + _parseGPGGA(msgBody, GPSData); + } + if( strncmp(ptr, "VTG", 3) == 0 ) { + GPSData->totalVTG++; + _parseVTG(msgBody, GPSData); + } + if( strncmp(ptr, "RMC", 3) == 0 ) { + _parseRMC(msgBody, GPSData); + } +} + +/** **************************************************************************** + * @name: extractNMEAfield extract a field from NMEA body. + * @author Dong An + * @param [in] msgBody - data to parse + * @param [out] outField - the data field extracted + * @param [in] fieldIndex - index of the field for the data + * @param [in] resetFlag - sets the indeies to zero + * @retval status + * @details NMEA messages are delimited with ',' and '*' to end message before + * the checksum byte + * 07/01/15 dkh changed to continue search starting from last found field + ******************************************************************************/ +char extractNMEAfield(char *msgBody, + char *outField, + int fieldIndex, + int resetFlag) +{ + static int searchIndex = 0; // input array index + static int currentField = 0; // delimeter ',' or '*' number + int outIndex = 0; // output array index + + if(msgBody == NULL || outField == NULL || NMEA_MSG_MAX_FIELD <= 0) { + return 0; // bad pointer check + } + // Continues indecies from the last search to avoid going through the entire + // sentence every time. After done parsing a sentance needs to be reset. + if (resetFlag == 1) { + searchIndex = 0; + currentField = 0; + } + + /// Scan through until the local field number matches the requested one + while(currentField != fieldIndex && msgBody[searchIndex]) { + if(msgBody[searchIndex] == ',') { + currentField++; // count delimters + } + searchIndex++; + } + + /// Next element in the array is a delimter or empty: return a empty buffer + if( (msgBody[searchIndex] == ',' || + msgBody[searchIndex] == '*' || + msgBody[searchIndex] == '\x0') ) { + outField[0] = '\x0'; + return 0; // check for end of buffer + } + + /// Scan and load output buffer until the next delimter is found. outIndex + /// starts at 0 here + while(msgBody[searchIndex] != ',' && + msgBody[searchIndex] != '*' && + msgBody[searchIndex]) { + outField[outIndex] = msgBody[searchIndex]; + outIndex++; + searchIndex++; + + if(outIndex >= NMEA_MSG_MAX_FIELD) { + outIndex = NMEA_MSG_MAX_FIELD - 1; + break; + } + } + outField[outIndex] = '\x0'; + + // Leave the currentField as the one returned so the next search passes + // the currrent doesn't match requested test + // Back up one so the next search picks up the end ',' + searchIndex--; + return 1; +} + +/** **************************************************************************** + * @name: _parseGPGGA LOCAL parse GPGGA message. Time, position, fix data + * @author Dong An + * @param [in] msgBody - data to parse + * @param [in] GPSData - data structure to parst he data into + * @retval status + * @details + * $GPGGA Global Positioning System Fix Data + * 123519.00 Fix taken at 12:35:19.00 UTC + * 4807.038,N Latitude 48 deg 07.038' N + * 01131.000,E Longitude 11 deg 31.000' E + * 1 Fix quality: 0 = invalid + * 1 = GPS fix (SPS) + * 2 = DGPS fix + * 3 = PPS fix + * 4 = Real Time Kinematic + * 5 = Float RTK + * 6 = estimated (dead reckoning) (2.3 feature) + * 7 = Manual input mode + * 8 = Simulation mode + * 08 Number of satellites being tracked + * 0.9 Horizontal dilution of position + * 545.4,M Altitude, Meters, above mean sea level + * 46.9,M Height of geoid (mean sea level) above WGS84 + * ellipsoid + * (empty field) time in seconds since last DGPS update + * (empty field) DGPS station ID number + * *47 the checksum data, always begins with * + * $GPGGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh + * $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47 + * 01/07/15 dkh load data structure directly, convert directly from ascii to + * decimal + * 02/16/15 dkh move convert to after check for flag + ******************************************************************************/ +#include "debug.h" +char _parseGPGGA(char *msgBody, + GpsData_t *GPSData) +{ + char field[NMEA_MSG_MAX_FIELD]; + char status = 0; + int parseReset = true; + NmeaLatLonSTRUCT nmeaLatLon; + + memset(&nmeaLatLon, 0, sizeof(nmeaLatLon) ); + /// Time - convert from ascii digits to decimal '0' = 48d + if( extractNMEAfield(msgBody, field, 0, parseReset) ) { + GPSData->GPSHour = ( (field[0] - '0') * 10) + field[1] - '0'; /// Hour + GPSData->GPSMinute = ( (field[2] - '0') * 10) + field[3] - '0'; /// minute + GPSData->GPSSecond = ( (field[4] - '0') * 10) + field[5] - '0'; /// Second + + if (field[6] == '.') { + // Some messages have second fraction .SS skip the '.' + GPSData->GPSSecondFraction = ( (field[7] - '0') / 10) + (field[8] - '0') / 100; + } + } else + status = 1; + + parseReset = false; + /// Latitude + if( extractNMEAfield(msgBody, field, 1, parseReset) ) { + nmeaLatLon.lat_min_fraction = atof((char *)field + 4); + nmeaLatLon.lat_min = ( (field[2] - '0') * 10) + field[3] - '0'; + nmeaLatLon.lat_deg = ( (field[0] - '0') * 10) + field[1] - '0'; + } else + status = 1; + + if( extractNMEAfield(msgBody, field, 2, parseReset) ) { + if(field[0] == 'S') // // sign + nmeaLatLon.latSign = -1; + else + nmeaLatLon.latSign = 1; + } else + status = 1; + + /// Longitude + if( extractNMEAfield(msgBody, field, 3, parseReset) ) { + nmeaLatLon.lon_min_fraction = atof((char *)field + 5); + nmeaLatLon.lon_min = ( (field[3] - '0') * 10) + field[4] - '0'; + nmeaLatLon.lon_deg = ( (field[0] - '0') * 100) + ((field[1] - '0') * 10) + (field[2] - '0') ; + } else + status = 1; + + if( extractNMEAfield(msgBody, field, 4, parseReset) ) { + if(field[0] == 'W') // sign + nmeaLatLon.lonSign = -1; + else + nmeaLatLon.lonSign = 1; + } else + status = 1; + + /// GPS quality + if( extractNMEAfield(msgBody, field, 5, parseReset) ) { + GPSData->gpsFixType = field[0] - '0'; // convert ascii digit to decimal + if (GPSData->gpsFixType >= DEAD_REC) // DR, manual and simulation is considered invalid + { + GPSData->gpsFixType = INVALID; + } + } else + status = 1; + + // Number of satellites + if( extractNMEAfield(msgBody, field, 6, parseReset) ) { + GPSData->numSatellites = atoi((char *)field); + } else + status = 1; + + /// HDOP x.x + if( extractNMEAfield(msgBody, field, 7, parseReset) ) { + GPSData->HDOP = (field[0] - '0') + (field[2] - '0') * 0.1f ;// convert ascii digit to decimal + } else + status = 1; + + /// Altitude + if( extractNMEAfield(msgBody, field, 8, parseReset) ) { + GPSData->alt = atof((char *)field); // altitude above MSL + } else{ + status = 1; + } + if( extractNMEAfield(msgBody, field, 10, parseReset) ) { + GPSData->geoidAboveEllipsoid = atof((char *)field); + } else{ + status = 1; + } + + // if fixed, convert data + if(GPSData->gpsFixType > INVALID) + { + // Convert deg/min/sec to xxx.xxx deg + _NMEA2UbloxAndLLA(&nmeaLatLon, GPSData); + // convert geiod height to ellipsoid height + GPSData->alt += GPSData->geoidAboveEllipsoid; + // create pseudo ITOW + convertItow(GPSData); + + gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked + gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track + } + else + { + gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // no signal lock + gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track + } + + if( status == 0) { + GPSData->updateFlagForEachCall |= 1 << GOT_GGA_MSG; + GPSData->LLHCounter++; + GPSData->GPSHorizAcc = GPSData->HDOP * 3.0; // just an approximation + GPSData->GPSVertAcc = GPSData->GPSHorizAcc * 1.5; // just another approximation + } + + return status; +} + +/** **************************************************************************** + * @name: _parseVTG LOCAL parse GPVTG message. track good speed over ground + * @author Dong An + * @param [in] msgBody - data to parse + * @param [in] GPSData - data structure to parst he data into + * @retval status + * @details $GPVTG Track made good and ground speed + * 054.7,T Course (degrees) + * 034.4,M Magnetic track made good + * 005.5,N Ground speed, knots + * 010.2,K Ground speed, Kilometers per hour + * *48 Checksum + * $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 + * 07/01/15 dkh load data structutre directly skipping local variables + ******************************************************************************/ +char _parseVTG(char *msgBody, + GpsData_t *GPSData) +{ + char field[NMEA_MSG_MAX_FIELD]; // [25] + char status = 0; + int parseReset = 1; + + ///true course [deg] + if( extractNMEAfield(msgBody, field, 0, parseReset) ) { + GPSData->trueCourse = atof((char *)field); // double + } else + status = 1; + + parseReset = false; + /// speed over ground [km/hr] + if( extractNMEAfield(msgBody, field, 6, parseReset) ) { + GPSData->rawGroundSpeed = atof((char *)field); // double [kph] + GPSData->rawGroundSpeed *= 0.2777777777778; // convert km/hr to m/s + // the heading of the VTG of some receiver is empty when velocity is low. + // a default zero will be set to the heading value. + if ( status == 1 ) { + status = 0; + GPSData->trueCourse = 0.0; + } + } else + status = 1; + + if( status == 0 ) { + /// Fill into user buffer + GPSData->updateFlagForEachCall |= 1 << GOT_VTG_MSG; + GPSData->VELCounter++; + convertNorhEastVelocity(GPSData); // split ground speed to NED + } + return status; +} + +/** **************************************************************************** + * @name: _parseRMC LOCAL parse GPRMC 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 $GPRMC (Recommended Minimum sentence C) + * 123519 Fix taken at 12:35:19 UTC + * A Status A=active or V=Void. + * 4807.038,N Latitude 48 deg 07.038' N + * 01131.000,E Longitude 11 deg 31.000' E + * 022.4 Speed over the ground in knots + * 084.4 Track angle in degrees True + * 230394 Date - 23rd of March 1994 ddmmyy + * 003.1,W Magnetic Variation + * *6A The checksum data, always begins with '*' + * 07/01/15 dkh convert directly from ascii to decimal + * $GPRMC,123519.000,A,4807.0380,N,01131.0000,E,022.4,084.4,230294,,,A*6A + ******************************************************************************/ +char _parseRMC(char *msgBody, + GpsData_t *GPSData) +{ + char field[NMEA_MSG_MAX_FIELD]; + char status = 0; + int parseReset = 1; + + /// Date field 8 + if( extractNMEAfield(msgBody, field, 8, parseReset) ) { + GPSData->GPSday = ( (field[0] - '0') * 10) + field[1] - '0'; /// day + GPSData->GPSmonth = ( (field[2] - '0') * 10) + field[3] - '0'; /// month + GPSData->GPSyear = ( (field[4] - '0') * 10) + field[5] - '0'; /// year + } else + status = 1; + return status; + +} +/** **************************************************************************** + * @name: computeNMEAChecksum compute the checksum of a NMEA message. + * @author Dong An + * @param [in] msgBody - data to parse + * @param [in] GPSData - data structure to parst he data into + * @retval status + ******************************************************************************/ +char computeNMEAChecksum(char *NMEAMsg, + unsigned int *lengthBeforeStar) +{ + unsigned int i; + char msgChecksumComputed = 0; + + for (i = 1; i < (*lengthBeforeStar); i++) { +// if(NMEAMsg[i] == ','){ +// continue; +// } + msgChecksumComputed ^= NMEAMsg[i]; ///skip '$' + } + return msgChecksumComputed; +} + +/** **************************************************************************** + * @name: _NMEA2UbloxAndLLA convert NMEA LLH into ublox LLH format. + * @author Dong An + * @param [in] GPSData - GPS data strutcture to process + * @retval N/A + * @details called in driverGPSAllEntrance.cpp INS380 uses the decimal + * GPSData->lat and GPSData->lon + * NMEA format is dddmm.mmmmmm d = degrees, m = minutes + * lat / lon = dd.dddddddd + * LatLonH = lat or lon * 1000000 (decimal shift to unsigned integer) + ******************************************************************************/ +void _NMEA2UbloxAndLLA(NmeaLatLonSTRUCT *NmeaData, + GpsData_t *GPSData) +{ + long double tmp1; + long double tmp2; + long double tmp3; + + // Longitude + tmp1 = (long double) NmeaData->lon_deg; + tmp2 = (long double) NmeaData->lon_min; + tmp3 = (long double) NmeaData->lon_min_fraction; + GPSData->lon = (tmp1 + (tmp2 + tmp3) / (long double)(60.0)); + if (NmeaData->lonSign == -1) + { + GPSData->lon = -GPSData->lon; + } +// GPSData->LonLatH[0] = (signed long)(GPSData->lon * 1.e7); + + // Latitude + tmp1 = (long double) NmeaData->lat_deg; + tmp2 = (long double) NmeaData->lat_min; + tmp3 = (long double) NmeaData->lat_min_fraction; + + GPSData->lat = (tmp1 + (tmp2 + tmp3) / (long double)(60.0)); + if (NmeaData->latSign == -1) + { + GPSData->lat = -GPSData->lat; + } +// GPSData->LonLatH[1] = (signed long)(GPSData->lat * 1.e7); + // Height (note: inconsistency in this equation based on other equations (should be 100 not 1000) +// GPSData->LonLatH[2] = (signed long)(GPSData->alt * 1000.0); +} + +/** **************************************************************************** + * @name: convertNorhEastVelocity API convert ground speed into NED velocity. + * @brief note: down velocity is synthesized from delta alt + * @author Dong An + * @param [in] GPSData - GPS data strutcture to process + * @retval status + ******************************************************************************/ +void convertNorhEastVelocity(GpsData_t* GPSData) +{ + GPSData->vNed[0] = GPSData->rawGroundSpeed * cos(D2R * GPSData->trueCourse); + GPSData->vNed[1] = GPSData->rawGroundSpeed * sin(D2R * GPSData->trueCourse); + + _smoothVerticalData(GPSData); // synthesize and filter down velocity +} + +/** **************************************************************************** + * @name processGPSVerticalData SYNTHESIZE and filter vertical velocity from alt + * @brief perform differential of altitude to obtain + * vertical velocity when using NME along with filtering of the data. + * @author Dong An + * @param [in] GPSData - GPS data + * @retval status + * @details EMA with a threshold to "filter" (omit) any value over the limit + ******************************************************************************/ +void _smoothVerticalData(GpsData_t* GPSData) +{ + static unsigned char firstFlag = 0; + static unsigned char firstFlagVd = 0; + static unsigned int OutAltCounter = 0; + static unsigned int OutVelCounter = 0; + static float filteredAlt; + static float filteredVd; + static double LastTotalTime = 0.0; + + double TotalTime = 0.0; + float temp1; // used for both alt and vel + double tmpDouble; + float lastAlt = filteredAlt; + float CurrentAlt = GPSData->alt; + float CurrentVd; + double delta_T; + + if (firstFlag == 0) { + firstFlag = 1; + filteredAlt = CurrentAlt; + filteredVd = 0; + firstFlagVd = 0; + } else { + temp1 = CurrentAlt - filteredAlt; + if( fabs( temp1 ) > MAX_ALTI_JUMP ) { // 10.0 [m] + temp1 = 0.0 ; + OutAltCounter++; + } else { + OutAltCounter = 0; + } + if ( OutAltCounter >= 5 ) { + firstFlag = 0; // reset + GPSData->vNed[2] = filteredVd ; + return; + } + + filteredAlt = filteredAlt + temp1 * ALT_FILTER_GAIN; // gain 0.2 [m] + + TotalTime = GPSData->GPSHour * 3600.0 + GPSData->GPSMinute * 60.0 + + GPSData->GPSSecond + GPSData->GPSSecondFraction; + delta_T = TotalTime - LastTotalTime; + + tmpDouble = filteredAlt - lastAlt; + if ( delta_T < MIN_DELTA_T) { // MIN 1.e-5 [s] + CurrentVd = filteredVd; + } else { + if (delta_T < MAX_DELTA_T) { // MAX 2.0 [s] +// CurrentVd = tmpDouble - tmpDouble / delta_T; ///down is positive + CurrentVd = -(tmpDouble / delta_T); ///down is positive + } else { /// may be NMEA is lost + firstFlag = 0; /// reset + GPSData->vNed[2] = filteredVd ; + return; + } + } ///end of enough delta T + + if( firstFlagVd == 0) { + firstFlagVd = 1; + filteredVd = CurrentVd; + } else { + temp1 = CurrentVd - filteredVd; + + if( temp1 > MAX_VEL_JUMP) { // 5.0 [m/s] + temp1 = 0.0 ; + OutVelCounter++; + } else + OutVelCounter = 0; + + if (OutVelCounter >= 5) { + firstFlag = 0; /// reset + GPSData->vNed[2] = filteredVd ; + return; + } + // do you need to filter the velcity? It is derived from altitude which is filtered + filteredVd = filteredVd + temp1 * VD_FILTER_GAIN; // gain 0.0476 [m/s] + } + } /// end of not first + + // save the current time local to calc delta T next time + LastTotalTime = TotalTime; + + /// output the vel + GPSData->vNed[2] = filteredVd; +} + +/** **************************************************************************** + * @name convertITOW convert GPS NMEA's UTC time tag into pseudo GPS ITOW number. + * @author Dong An + * @param [in] GPSData - GPS data + * @retval status + ******************************************************************************/ +void convertItow(GpsData_t* GPSData) +{ + double tmp; + // calculate day of week + int tow = dayofweek(GPSData->GPSday, GPSData->GPSmonth, GPSData->GPSyear+2000); + // calculate second of week + tmp = (double)tow * 86400.0; + tmp += ((double)GPSData->GPSHour) * 3600.0 + ((double)GPSData->GPSMinute) * 60.0; + tmp += (double)GPSData->GPSSecond + (double)GPSData->GPSSecondFraction; + + /// converting UTC to GPS time is impossible without GPS satellite + /// Navigation message. using UTC time directly by scaling to ms. + GPSData->itow = (unsigned long) (tmp * 1000); +} + +int dayofweek(int day, int month, int year) +{ + + int adjustment, mm, yy; + + adjustment = (14 - month) / 12; + mm = month + 12 * adjustment - 2; + yy = year - adjustment; + + return (day + (13 * mm - 1) / 5 + + yy + yy / 4 - yy / 100 + yy / 400) % 7; +} + +#endif // GPS diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processNovAtelGPS.c b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processNovAtelGPS.c new file mode 100644 index 0000000..6722855 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processNovAtelGPS.c @@ -0,0 +1,523 @@ +/** *************************************************************************** + * @file processNovAtelGPS.c interface NovAtel OEM4 GPS receiver. + * + * 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. + * @details + * This file provides parsing functions for NovAtel OEM4 GPS receiver. + *****************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#ifdef GPS + +#include +#include +#include +#include +#include + +#include "driverGPS.h" +#include "NovAtelPacketFormats.h" +#include "BITStatus.h" + +#define CRC32_POLYNOMIAL 0xEDB88320 + +static gpsDeltaStruct downVelDelta; + + +/** **************************************************************************** + * @name processNovAtelBinaryMsg parse a complete NovAtel binary message. + * + * @param [in] msg - input buffer + * @param [in] msgLen - input buffer length + * @param [in] GPSData structure to parse into + * @retval N/A + ******************************************************************************/ +void processNovAtelBinaryMsg_Fast(char *msg, + unsigned int *msgLength, + GpsData_t *GPSData) +{ + unsigned long calcuCRC; + uint32_t* pMsgCRC; + + calcuCRC = _CalculateBlockCRC32((unsigned long)(*msgLength - 4), + (unsigned char *)&msg[0]); + pMsgCRC = (uint32_t*)&msg[*msgLength - 4]; + + if (calcuCRC == *pMsgCRC) { + switch ( *(unsigned short*)&msg[4] ) { // msgID + case 42: /// 0x2a bestPosB + case 47: /// 0x2b PSRPosB - PSeudoRange + _parseBestPosB_Fast((logBestPosB*)msg, + GPSData); + break; + case 99: /// 0x63 bestVelB + case 100: /// 0x64 PSRVelB - PSeudoRange + _parseBestVelB_Fast((logBestVelB*)msg, + GPSData); + break; + default:; + } + } + // bad CRC just return +} + +/** **************************************************************************** + * @name parseNovAtelBinaryMsg parse a complete NovAtel binary message. + * + * @param [in] inByte - next byte from serial stream + * @param [in] gpsMsg - GPS message buffer + * @param [in] GPSData structure to parse into + * @retval N/A + ******************************************************************************/ +int parseNovotelBinaryMessage(uint8_t inByte, uint8_t *gpsMsg, GpsData_t *GPSData) +{ + static int state = 0; + static unsigned int len = 0, headerLen = 0; + static uint8_t *ptr; + static uint32_t sync = 0; + unsigned static int totalLen = 0, msgLen = 0; + + + sync = (sync << 8) | inByte; + if ((sync & 0x00ffffff) == NOVATEL_OME4_BINARY_HEADER){ + gpsMsg[0] = (NOVATEL_OME4_BINARY_HEADER >> 16) & 0xff; + gpsMsg[1] = (NOVATEL_OME4_BINARY_HEADER >> 8) & 0xff; + gpsMsg[2] = NOVATEL_OME4_BINARY_HEADER & 0xff; + state = 1; + ptr = &gpsMsg[3]; + len = 3; + headerLen = 0; + return 0; + } + + if(state == 0){ + return 0; + } + + *ptr++ = inByte; + len++; + + if (len >= MAX_MSG_LENGTH){ + // overflow - reset packet engine + state = 0; + return 1; + } + switch (state){ + case 1: + // header processing + if (headerLen == 0){ + headerLen = inByte; + }else if (len == headerLen){ + msgLen = *((uint16_t *)&gpsMsg[8]); + totalLen = msgLen + headerLen + 4; // crc included + // data next + state = 2; + } + break; + case 2: + if (len == totalLen){ + processNovAtelBinaryMsg_Fast((char *)gpsMsg, &len, GPSData); + state = 0; + } + break; + default: + break; + } + return 0; +} + + + +void processNovAtelBinaryMsg(char *msg, + unsigned int *msgLength, + GpsData_t *GPSData) +{ + unsigned char headerLength = 0; + unsigned short bodyLength = 0; + unsigned short msgID = 0; + unsigned long calcuCRC; + uint32_t* pRecCRC; + + calcuCRC = _CalculateBlockCRC32( (unsigned long)(*msgLength - 4), + (unsigned char *)&msg[0]); + + pRecCRC = (uint32_t*)&msg[*msgLength - 4]; + + if (calcuCRC == *pRecCRC) { + headerLength = msg[3]; + bodyLength = *(unsigned short*)&msg[8]; + + msgID = *(unsigned short*)&msg[4]; + switch (msgID) { + case 42: /// 0x2a bestPosB + case 47: /// 0x2b PSRPosB - PSeudoRange + _parseBestPosB(msg, + &headerLength, + &bodyLength, + GPSData); + break; + case 99: /// 0x63 bestVelB + case 100: /// 0x64 PSRVelB - PSeudoRange + _parseBestVelB(msg, + &headerLength, + &bodyLength, + GPSData); + break; + default:; + } + } + // bad CRC just return +} + +/** **************************************************************************** + * @name _parseBestPosB LOCAL parse a BESTPOSB message. + * @author Doug Hiranaka + * @param [in] bestPosB - input buffer cast to best pos B + * @param [in] GPSData structure to parse into + * @retval N/A + 1 BESTPOS header Log header H 0 + 2 sol stat Solution status, see Table 85 on page 408 Enum 4 H + 3 pos type Position type, see Table 84 on page 407 Enum 4 H+4 + 4 lat Latitude 9 (degrees) Double 8 H+8 + 5 lon Longitude (degrees) Double 8 H+16 + 6 hgt Height above mean sea level (metres) Double 8 H+24 + 7 undulation - relationship between the geoid and ellipsoid (m) of the datum a Float 4 H+32 + 8 datum id# Datum ID number Enum 4 H+36 + 9 lat ? Latitude standard deviation (m) Float 4 H+40 + 10 lon ? Longitude standard deviation (m) Float 4 H+44 + 11 hgt ? Height standard deviation (m) Float 4 H+48 + 12 stn id Base station ID Char[4] 4 H+52 + 13 diff_age Differential age in seconds Float 4 H+56 + 14 sol_age Solution age in seconds Float 4 H+60 + 15 #SVs Number of satellites tracked Uchar 1 H+64 + 16 #solnSVs Number of satellites used in solution Uchar 1 H+65 + 17 #solnL1SVs Number of satellites with L1/E1/B1 used in solution Uchar 1 H+66 + 18 #solnMultiSVs # satellites with multi-frequency signals used in solution Uchar 1 H+67 + 19 Reserved Hex 1 H+68 + 20 ext sol stat Extended solution status Hex 1 H+69 + 21 Galileo and BeiDou sig mask Hex 1 H+70 + 22 GPS and GLONASS sig mask Hex 1 H+71 + 23 xxxx 32-bit CRC (ASCII and Binary only) Hex 4 H+72 + ******************************************************************************/ +void _parseBestPosB_Fast(logBestPosB *bestPosB, + GpsData_t *GPSData) +{ + GPSData->lat = bestPosB->Lat; + GPSData->lon = bestPosB->Lon; + GPSData->alt = bestPosB->hgt + bestPosB->undulation; // altitude above ellipsoid + GPSData->geoidAboveEllipsoid = bestPosB->undulation; // geoid above ellipsoid + + if (bestPosB->sol_status != 0) // zero is good fix anything else is + { // enumeration for bad fix + GPSData->gpsFixType = 0;// PosVelType + gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // locked + gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track + gGpsDataPtr->HDOP = 21.0f; // force to above threshold + } else { + GPSData->gpsFixType = bestPosB->pos_type; + gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked + gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track + gGpsDataPtr->HDOP = 1.0f; // force to below threshold + } + + //0xFFFFFFFC = b11111111111111111111111111111100 + //0xFFFFFFFd = b11111111111111111111111111111101 + //0xFFFFFFFe = b11111111111111111111111111111110 + // If ITOW is the same as the value stored in GPSData->itow then it seems + // that velocity has already been measured at the current time step. If + // so then set the update flag accordingly, if not then set the GGA value + // only. + if( (bestPosB->header.milliseconds == GPSData->itow ) && + (GPSData->updateFlagForEachCall & 0x02 ) ) ///GOT_GGA_MSG + { + GPSData->updateFlagForEachCall |= 3; //1 << GOT_GGA_MSG; + } else { + GPSData->updateFlagForEachCall = 1; //GPSData->updateFlagForEachCall & 0xFFFFFFFd; + } + GPSData->itow = (uint32_t) bestPosB->header.milliseconds; + + GPSData->LLHCounter++; + + float tmp = (float)1.9230769944E-2 * (float)bestPosB->header.week; + float tmpYear = (uint8_t)tmp; + GPSData->GPSyear = (uint8_t)( tmpYear - 20 ); + GPSData->GPSmonth = (uint8_t)( ( tmp - tmpYear ) * 12 ); + if( GPSData->GPSmonth < 1 ) { + GPSData->GPSmonth = 1; + } else if( GPSData->GPSmonth > 12 ) { + GPSData->GPSmonth = 12; + } + GPSData->GPSday = 1; + + // Accuracy measurements + GPSData->GPSHorizAcc = sqrtf( bestPosB->lat_sigma * bestPosB->lat_sigma + bestPosB->lon_sigma * bestPosB->lon_sigma ); // [m] + GPSData->GPSVertAcc = bestPosB->hgt_sigma; // [m] + + GPSData->numSatellites = bestPosB->num_obs; +} + +/** **************************************************************************** +* @name _parseBestPosB LOCAL parse a BESTPOSB message. +* @author Dong An +* @param [in] completeMessage - input buffer +* @param [in] headerLength - header length +* @param [in] bodyLength - input buffer length +* @param [in] GPSData structure to parse into +* @retval N/A + 1 BESTPOS header Log header H 0 + 2 sol stat Solution status, see Table 85 on page 408 Enum 4 H + 3 pos type Position type, see Table 84 on page 407 Enum 4 H+4 + 4 lat Latitude 9 (degrees) Double 8 H+8 + 5 lon Longitude (degrees) Double 8 H+16 + 6 hgt Height above mean sea level (metres) Double 8 H+24 + 7 undulation - relationship between the geoid and ellipsoid (m) of the datum a Float 4 H+32 + 8 datum id# Datum ID number Enum 4 H+36 + 9 lat ? Latitude standard deviation (m) Float 4 H+40 + 10 lon ? Longitude standard deviation (m) Float 4 H+44 + 11 hgt ? Height standard deviation (m) Float 4 H+48 + 12 stn id Base station ID Char[4] 4 H+52 + 13 diff_age Differential age in seconds Float 4 H+56 + 14 sol_age Solution age in seconds Float 4 H+60 + 15 #SVs Number of satellites tracked Uchar 1 H+64 + 16 #solnSVs Number of satellites used in solution Uchar 1 H+65 + 17 #solnL1SVs Number of satellites with L1/E1/B1 used in solution Uchar 1 H+66 + 18 #solnMultiSVs # satellites with multi-frequency signals used in solution Uchar 1 H+67 + 19 Reserved Hex 1 H+68 + 20 ext sol stat Extended solution status Hex 1 H+69 + 21 Galileo and BeiDou sig mask Hex 1 H+70 + 22 GPS and GLONASS sig mask Hex 1 H+71 + 23 xxxx 32-bit CRC (ASCII and Binary only) Hex 4 H+72 +******************************************************************************/ +void _parseBestPosB(char *completeMessage, + unsigned char *headerLength, + unsigned short *bodyLength, + GpsData_t *GPSData) +{ + char SolStatus[4]; + + // these are uint16_t but only looking at first byte + SolStatus[0] = completeMessage[ *headerLength]; + + GPSData->lat = _assembyToLongDouble((unsigned char*)&completeMessage[*headerLength + 8]); // lat + GPSData->lon = _assembyToLongDouble((unsigned char*)&completeMessage[*headerLength + 16]); // lon + GPSData->alt = _assembyToLongDouble((unsigned char*)&completeMessage[*headerLength + 24]); ///alt + + GPSData->updateFlagForEachCall |= 1 << GOT_GGA_MSG; + + if (SolStatus[0] != 0) { // zero is good fix anything else is enumeratio for bad fix + gGpsDataPtr->HDOP = 21.0f; // force to above threshold + gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // not locked + gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track + } else { + gGpsDataPtr->HDOP = 1.0f; // force to below threshold + gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked + gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track + } + GPSData->gpsFixType = SolStatus[0]; + + memcpy(&GPSData->itow, &completeMessage[16], 4); ///Itow + + GPSData->LLHCounter++; +} + +/** **************************************************************************** +* @name _parseBestVelB LOCAL parse a BESTVELB message. +* @author Dong An +* @param [in] bestVelB - input buffer cast to bestVelB +* @param [in] GPSData structure to parse into +* @retval N/A + 1 BESTVEL header Log header H 0 + 2 sol status Solution status (enum) 4 H + 3 vel type Velocity type, see Table 84, Position or Velocity Type 4 H+4 + 4 latency in the velocity time tag in seconds. subrtrac from timeFloat 4 H+8 + 5 age Differential age in seconds Float 4 H+12 + 6 hor spd Horizontal speed over ground [m/s] Double 8 H+16 + 7 trk gnd (track over ground) True North, [deg] Double 8 H+24 + 8 vert spd Vertical speed, [m/s] + (up) - (down) Double 8 H+32 + 9 Reserved Float 4 H+40 + 10 xxxx 32-bit CRC (ASCII and Binary only) Hex 4 H+44 +******************************************************************************/ +void _parseBestVelB_Fast(logBestVelB *bestVelB, + GpsData_t *GPSData) +{ + double radTrueCourse = 0.0; + + GPSData->trueCourse = bestVelB->trk_gnd; ///COG wrt true north + radTrueCourse = GPSData->trueCourse * D2R; // [rad] + + // Velocity information + GPSData->rawGroundSpeed = bestVelB->hor_spd; // [m/s] + + // + GPSData->vNed[0] = bestVelB->hor_spd * cos(radTrueCourse); // [m/s] N + GPSData->vNed[1] = bestVelB->hor_spd * sin(radTrueCourse); // [m/s] E + GPSData->vNed[2] = avgDeltaSmoother( -bestVelB->vert_spd, &downVelDelta); + + if (bestVelB->sol_status != 0) // zero is good fix anything else is + { // enumeration for bad fix + GPSData->gpsFixType = bestVelB->vel_type; + gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // not locked + gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track + gGpsDataPtr->HDOP = 21.0f; // + } else { + GPSData->gpsFixType = 0; + gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // not locked + gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track + gGpsDataPtr->HDOP = 1.0f; // + } + + // If ITOW is the same as the value stored in GPSData->itow then it seems + // that velocity has already been measured at the current time step. If + // so then set the update flag accordingly, if not then set the GGA value + // only. + if( ((uint32_t)bestVelB->header.milliseconds == GPSData->itow ) && + (GPSData->updateFlagForEachCall & 0x01 ) ) + { + GPSData->updateFlagForEachCall |= 3; //1 << GOT_VTG_MSG; + } else { + GPSData->updateFlagForEachCall = 2; //GPSData->updateFlagForEachCall & 0xFFFFFFFe; + } + GPSData->itow = (uint32_t) bestVelB->header.milliseconds; + + GPSData->VELCounter++; +} + + +/** **************************************************************************** + * @name _parseBestVelB LOCAL parse a BESTVELB message. + * @author Dong An + * @param [in] completeMessage - input buffer + * @param [in] headerLength - header length + * @param [in] bodyLength - input buffer length + * @param [in] GPSData structure to parse into + * @retval N/A + ******************************************************************************/ +void _parseBestVelB(char *completeMessage, + unsigned char *headerLength, + unsigned short *bodyLength, + GpsData_t *GPSData) +{ + char SolStatus[4]; + double radTrueCourse = 0.0; + + SolStatus[0] = completeMessage[ *headerLength]; + + GPSData->rawGroundSpeed = _assembyToLongDouble((unsigned char*)&completeMessage[*headerLength + 16]); // ground speed [m/s] + GPSData->trueCourse = _assembyToLongDouble((unsigned char*)&completeMessage[*headerLength + 24]); ///COG [deg] + radTrueCourse = GPSData->trueCourse * D2R; // [rad] + GPSData->vNed[0] = GPSData->rawGroundSpeed * cos(radTrueCourse); // [m/s] N + GPSData->vNed[1] = GPSData->rawGroundSpeed * sin(radTrueCourse); // [m/s] E + GPSData->vNed[2] = -_assembyToLongDouble((unsigned char*)&completeMessage[*headerLength + 32]); ///vertical Vel [m/s] + up + + GPSData->updateFlagForEachCall |= 1 << GOT_VTG_MSG; + + if (SolStatus[0] != 0) // zero is good fix anything else is + { // enumeration for bad fix + gGpsDataPtr->HDOP = 21.0f; // + gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // not locked + gBitStatus.swStatus.bit.noGPSTrackReference = 1; // GPS track + } else { + gGpsDataPtr->HDOP = 1.0f; // + gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // not locked + gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track + } + GPSData->gpsFixType = SolStatus[0]; + + memcpy(&GPSData->itow, &completeMessage[16], 4); ///Itow + + GPSData->VELCounter++; +} + + +/** **************************************************************************** + * @name assembyToLongDouble LOCAL put 8 bytes IEEE double precision into + * "long double" of c2000. + * @author Dong An + * @param [in] doubleBytes - input + * @retval long double version of the number + ******************************************************************************/ +long double _assembyToLongDouble(unsigned char *inByte) +{ + union { + long long dataLong; + long double dataDouble; + } long2Double; + + memcpy(&long2Double.dataLong, inByte, 8); + return long2Double.dataDouble; +} + +/** **************************************************************************** + * @name _CRC32Value LOCAL subroutine of CRC computation for NovAtel binary + * message of c2000. + * @author Dong An + * @param [in] i - input value + * @retval N/A + ******************************************************************************/ +unsigned long _CRC32Value(int i) +{ + int j; + unsigned long ulCRC; + + ulCRC = i; + for(j = 8; j > 0; j--) { + if (ulCRC & 1) + ulCRC = (ulCRC >> 1) ^ CRC32_POLYNOMIAL; + else + ulCRC >>= 1; + } + return ulCRC; +} + +/** **************************************************************************** + * @name _CalculateBlockCRC32 LOCAL compute CRC for a complete NovAtel binary + * message of c2000. + * @author Dong An + * @param [in] ulCount - input size 32bit words + * @param [in] ucBuffer - input value + * @retval the calulated CRC + ******************************************************************************/ +unsigned long _CalculateBlockCRC32(unsigned long ulCount, + unsigned char *ucBuffer) +{ + unsigned long ulTemp1; + unsigned long ulTemp2; + unsigned long ulCRC = 0; + + while (ulCount-- != 0) { + ulTemp1 = (ulCRC >> 8) & 0x00FFFFFFL; + ulTemp2 = _CRC32Value( ((int)ulCRC ^ *ucBuffer++) & 0xff); + ulCRC = ulTemp1 ^ ulTemp2; + } + return ulCRC; +} + +void sendNovAtelBinaryCmdMsg(void) +{ + uint8_t baudCmd[] = { "COM COM1,115200,N,8,1,N,OFF,ON\r" }; + uint8_t posBCmd[] = { "log com1 BESTPOSB ontime 1\r" }; + uint8_t velBCmd[] = { "log com1 BESTVELB ontime 1\r" }; + + writeGps((char*)posBCmd, strlen((char*)posBCmd)); + writeGps((char*)velBCmd, strlen((char*)velBCmd)); + writeGps((char*)baudCmd, strlen((char*)baudCmd)); +} + +#endif // GPS \ No newline at end of file diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processSiRFGPS.c b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processSiRFGPS.c new file mode 100644 index 0000000..56c579d --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processSiRFGPS.c @@ -0,0 +1,583 @@ +/** *************************************************************************** + * @file processSiRFGPS.c Processing for SiRF binary GPS receiver. + * + * 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. + * @details This file includes all specific processing, including configuring, + * for SiRF GPS receiver. + *****************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#ifdef GPS + +#include + +#include +#define LOGGING_LEVEL LEVEL_DEBUG +#include "driverGPS.h" +#include "SiRFPacketFormats.h" +#include "utilities.h" +#include +#include "BITStatus.h" +#include "osapi.h" + +static gpsDeltaStruct downVelDelta; + +void _siRFStaticNavMsg(uint8_t flag); + +/** **************************************************************************** + * @name _computeSirfChecksum LOCAL calculate a checksum for SiRF + * @param [in] payload - input buffer + * @param [in] payloadLen - input buffer length + * @retval calculated checksum + ******************************************************************************/ +static uint16_t _computeSirfChecksum(uint8_t *payload, + unsigned int payloadLen) +{ + unsigned int i; + uint16_t checksum = 0; + + for ( i = 0; i < payloadLen; i++) { + checksum += payload[i]; + } + return checksum; +} + +/** **************************************************************************** + * @name reorderBytes used in SiRFPacketFormats.h + * @param [in] in - input buffer + * @param [in] index - byte array index + * @param [in] numBytes - number of bytes to process + * @retval word with bytes swapped + * used in SET_VALUE() macro + ******************************************************************************/ +static uint32_t reorderBytes(char *in, + int *index, + int numBytes) +{ + uint32_t out = 0; + int i; + + in += *index; + for ( i = 0 ; i < numBytes; i++ ) { + out = (out << 8) + *in; + in++; + } + *index += numBytes; + return out; +} + +/** **************************************************************************** + * @name _sirfSwitchToBinary LOCAL configure SiRF GPS receiver to binary + * @brief configure SiRF GPS receiver using a NMEA $PSRF100 to go faster and send + * binary packets + * Send message 100: SetSerialPort, Set PORT A parameters and protocol + * $PSRF100, + * 0, where 0 = binary, 1 = NMEA + * 9600, baud rate + * 8, data bits + * 1, stop bits + * 0 parity (0=none, 1=odd, 2=even) + * *0C checksum + * \r\n end message + * @author Dong An + * @param [out] realBaudRate - + * @retval N/A + ******************************************************************************/ +void _sirfSwitchToBinary(uint32_t realBaudRate) +{ + char msg[GPS_INTERFACE_TX_BUFFER_SIZE]; + char check; + char nibble; + unsigned int msgIndex = 0; + + memset(msg, 0, GPS_INTERFACE_TX_BUFFER_SIZE); + + strcpy(msg, "$PSRF100"); + msgIndex = strlen(msg); + + /// SiRF binary mode + strcpy(&(msg[msgIndex]), ",0,"); + msgIndex = strlen(msg); + + itoa(realBaudRate, &(msg[msgIndex]), 10); + msgIndex = strlen(msg); + + /// SiRF protocol is only valid for 8 data bits, 1 stop bit, and no parity. + strcpy(&(msg[msgIndex]), ",8,1,0"); + msgIndex = strlen(msg); + + check = computeNMEAChecksum(msg, &msgIndex); + msg[msgIndex] = '*'; msgIndex++; + /// short itoa for hex-ing the checksum + nibble = (check >> 4) & 0xF; + if (nibble < 10) { + nibble += '0'; + } else { + nibble += 'A' - 10; + } + + msg[msgIndex] = nibble; msgIndex++; + nibble = check & 0xF; + if (nibble < 10) { + nibble += '0'; + } else { + nibble += 'A' - 10; + } + msg[msgIndex] = nibble; + msgIndex++; + + strcpy(&(msg[msgIndex]), "\r\n"); + msgIndex += 2; + writeGps(msg, msgIndex); +} + +/** **************************************************************************** + * @name configSiRFGPSReceiver API configure SiRF GPS receiver to go faster, + * send bin packets + * @author Dong An + * @param [out] GPSData - gps data structure + * @param [out] goalBaudRate - data from GPS data buffer + * @retval N/A + ******************************************************************************/ +void configSiRFGPSReceiver(GpsData_t * GPSData, int goalBaudRate) +{ +/* + static int timeout = 5; // + + if (!GPSData->sirfInitialized) { + _sirfSwitchToBinary(goalBaudRate); + GPSData->sirfInitialized = 1; + timeout = 0; + ///< wait for message to finish DEBUG: Can get stuck here if SiRF not attached + while ( !isGpsTxEmpty() ) {}; // spin + OS_Delay(100); + initGpsUart(goalBaudRate); + OS_Delay(100); ///< clear incoming remaining packets + flushGPSRecBuf(); + pollSiRFVersionMsg(); // send request for version - if (binary) version recieved set msg rate + GPSData->GPSFix = 1; + }else if (++timeout > 5) { // 5 cycles of GPS task + GPSData->sirfInitialized = 0; + GPSData->GPSbaudRate = 4800; + while ( !isGpsTxEmpty() ) {}; // spin + initGpsUart(GPSData->GPSbaudRate); // initial GPS receiver baudrate + OS_Delay(100); ///< clear incoming remaining packets + GPSData->GPSFix = 0; + } +*/ + +} + + +/** **************************************************************************** + * @name _SiRFSendMessage LOCAL format and send a message to SiRF + * @param [in] msg - dat to xmit + * @param [in] messageLength - length of send buffer + * @retval byte swappeed word + ******************************************************************************/ +static void _SiRFSendMessage(uint8_t *msg, + int messageLength) +{ + uint16_t tmp; + + tmp = byteSwap16(SIRF_BINARY_HEADER); + writeGps((char*)&tmp, + sizeof(tmp)); + + tmp = byteSwap16(messageLength); + writeGps((char*)&tmp, + sizeof(tmp)); + + writeGps((char*)msg, + messageLength); + + tmp = _computeSirfChecksum((uint8_t*)msg, + messageLength); + tmp = byteSwap16(tmp); + writeGps((char*)&tmp, + sizeof(tmp)); + + tmp = SIRF_END_SEQUENCE; + tmp = byteSwap16(tmp); + writeGps((char*)&tmp, + sizeof(tmp)); +} + +/** **************************************************************************** + * @name pollSiRFVersionMsg format and send a binary version request message + * to SiRF + * @param N/A + * @retval N/A + ******************************************************************************/ +void pollSiRFVersionMsg() +{ + tSiRFPollVersion msg; + + msg.id = SIRF_POLL_SOFTWARE_VERSION; // 0x84 + msg.reserved = 0; + _SiRFSendMessage((uint8_t *)&msg, + sizeof(msg)); +} + +/** **************************************************************************** + * @name siRFStaticNavMsg format and send a binary message to enable or disable + * static nvigationto SiRF + * @param [in] flag 1 = enable, 0 = disable + * @retval N/A + * @details Static navigation is a position filter for use with motor vehicle + * applications. When the vehicle�s speed falls below a threshold, the + * position and heading are frozen, and speed is set to 0. This + * condition continues until the computed speed rises above 1.2 times + * the threshold, or until the computed position is at least a set + * distance from the frozen place. The threshold speed and set distance + * may vary with software versions. + ******************************************************************************/ +void _siRFStaticNavMsg(uint8_t flag) +{ + tSiRFStaticNavigation msg; + + msg.id = SIRF_STATIC_NAVIGATION; // 0x8f ID 143 + msg.flag = flag; + _SiRFSendMessage((uint8_t *)&msg, + sizeof(msg)); +} + + +/** **************************************************************************** + * @name sirfBinarySwitchBaud format and send a version request message to SiRF + * @param [in] baud - the baudrate to set + * @retval N/A + ******************************************************************************/ +void sirfBinarySwitchBaud(uint32_t baud) +{ + tSiRFPSetSerialPort msg; + + msg.id = SIRF_SET_SERIAL_PORT; + msg.baud = baud; + msg.databits = 8; + msg.stopbits = 1; + msg.parity = 0; + msg.reserved = 0; + _SiRFSendMessage((uint8_t *)&msg, + sizeof(msg)); +} + +/** **************************************************************************** + * @name _SiRFSendMessageRateMsg LOCAL format and send a message to SiRF + * @param [in] mode - mode to set + * @param [in] messageIdToBeSet - + * @param [in] rate - new rate to be set + * @retval byte swappeed word + ******************************************************************************/ +static void _SiRFSendMessageRateMsg(uint8_t mode, + uint8_t messageIdToBeSet, + uint8_t rate) +{ + tSiRFSetMessageRate msg; + + msg.id = SIRF_SET_MESSAGE_RATE; // 0xA6 166d + msg.mode = mode; + msg.messageIdToBeSet = messageIdToBeSet; + msg.updateRate = rate; + msg.reserved = 0; + _SiRFSendMessage((uint8_t *)&msg, + sizeof(msg)); +} + + +/** **************************************************************************** + * @name _SiRFSendMessageRateMsg LOCAL format and send a message to SiRF + * @param [in] mode - mode to set + * @param [in] messageIdToBeSet - + * @param [in] rate - new rate to be set + * @retval byte swappeed word + ******************************************************************************/ +/* +static void _SiRFSendModeControlMsg(void) +{ + tSiRFSetModeControl msg; + + msg.id = SIRF_SET_NAV_MODE; // 0x88 (mid 136) + msg.reserved = 0x0000; + msg.degMode = 0x04; // Disable degraded-mode operation + msg.posCalcMode = 0x04; // Set 5 Hz output + msg.deadRecogEnable = 0x00; + msg.altitudeInput = 0x0000; + msg.altHoldMode = 0x00; + msg.altHoldSource = 0x00; + msg.coastTimeout = 0x00; + msg.degradedTimeout = 0x00; + msg.deadRecogTimeout = 0x0F; + msg.measAndTrackSmoothing = 0x02; + + _SiRFSendMessage((uint8_t *)&msg, + sizeof(msg)); +} +*/ + +//#define FIVE_HZ_GPS + +/** **************************************************************************** + * @name _configureSiRFBinaryMessages LOCAL format and send a request message to + * SiRF + * @param N/A + * @retval N/A + * @details msg SIRF_GEODETIC_NAVIGATION_DATA [0x29] 41d + ******************************************************************************/ +static void _configureSiRFBinaryMessages() +{ + _SiRFSendMessageRateMsg(0x02, 0, 0); // disable all messages + OS_Delay(10); + _SiRFSendMessageRateMsg(0x00, SIRF_GEODETIC_NAVIGATION_DATA, 1); // 1 [sec] +#ifdef FIVE_HZ_GPS + _SiRFSendModeControlMsg(); +#endif +} + + +/** **************************************************************************** + * @name _parseSiRFGeodeticNavMsg LOCAL parse message for nav data + * @param [in] msg - [0x29] 41d data to be parsed + * @param [in] msgLength - length of the message to be parsed + * @param [in] GPSData - GPS data structure to parse message into + * @retval N/A + * @details The data in the input msg array is byte swapped + * 12/01/15 dkh cast the input buffer to a packed structure to reduce parsing + ******************************************************************************/ +static void _parseSiRFGeodeticNavMsg(char *msg, + unsigned int msgLength, + GpsData_t *GPSData) +{ + tSiRFGeoNav *geo = (tSiRFGeoNav *)msg; + double latitude = 0.0; + double longitude = 0.0; + double radTrueCourse = 0.0; + + if (msgLength < sizeof(tSiRFGeoNav)) + return; // error + + /******* packet parsed, now put it in GPSData as best as makes sense ****/ + GPSData->totalGGA++; + GPSData->totalVTG++; + + // done this way to match up to NMEA which is abs value and N or E + latitude = (int32_t)byteSwap32(geo->latitude); // [degrees] * 10^7, + = N + GPSData->lat = latitude * ONE_OVER_TEN_TO_THE_SEVENTH; /// TEN_TO_THE_SEVENTH; + + longitude = (int32_t)byteSwap32(geo->longitude); + GPSData->lon = longitude * ONE_OVER_TEN_TO_THE_SEVENTH; /// TEN_TO_THE_SEVENTH; + + GPSData->alt = 0.01 * (int32_t)byteSwap32(geo->altitudeEllipsoid); // / 100; // [m] * 10^2 + +// Unused in FW +// GPSData->LonLatH[0] = (int32_t)byteSwap32(geo->latitude); // [deg]*10^7 + = N +// GPSData->LonLatH[1] = (int32_t)byteSwap32(geo->longitude); // [deg]*10^7 + = E +// GPSData->LonLatH[2] = (int32_t)byteSwap32(geo->altitudeEllipsoid); // [m] ellipsoid *100 + + // 0x0000 = valid navigation (any bit set implies navigation solution is not optimal); + // Bit 0: + // On = solution not yet overdetermined(1) (< 5 SVs), + // Off = solution overdetermined(1) (> = 5 SV) + // Switched at the sazme time as PPS 5 satellites + if ( geo->navValid > 0) { + GPSData->HDOP = 50.0; + GPSData->gpsFixType = 1; // zero is valid anything else is degraded + gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // no signal lock + gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track + } else { + GPSData->HDOP = geo->hdop; + GPSData->gpsFixType = 0; + gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked + gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track + } + + GPSData->trueCourse = 0.01 * byteSwap16(geo->courseOverGround); // / 100.0; // [deg] + GPSData->rawGroundSpeed = 0.01 * byteSwap16(geo->speedOverGround); // / 100.0; + + radTrueCourse = (0.01 * byteSwap16(geo->courseOverGround)) * 0.017453292519943; // * PI / 180.0; // [rad] + GPSData->vNed[0] = GPSData->rawGroundSpeed * cos(radTrueCourse); // [m/s] * 10^2 N + GPSData->vNed[1] = GPSData->rawGroundSpeed * sin(radTrueCourse); // [m/s] * 10^2 E + + // filtered down velocity + GPSData->vNed[2] = avgDeltaSmoother(-((int16_t)byteSwap16(geo->climbRate) * 0.01), &downVelDelta); + + GPSData->GPSmonth = geo->utcMonth; // mm + GPSData->GPSday = geo->utcDay; // dd + GPSData->GPSyear = byteSwap16(geo->utcYear) - 2000; // last two digits of year + + GPSData->geoidAboveEllipsoid = (float)byteSwap32(geo->altitudeEllipsoid - geo->altitudeMSL) / 100.; // 0x2221201f in m * 10^2 + + GPSData->GPSHour = geo->utcHour; + GPSData->GPSMinute = geo->utcMinute; + GPSData->GPSSecondFraction = byteSwap16(geo->utcSecond); + GPSData->GPSSecondFraction *= 0.001; //MILLISECOND_TO_SECOND; MILLISECOND_TO_SECOND 1000 + GPSData->GPSSecond = (char) floor(GPSData->GPSSecondFraction); + GPSData->GPSSecondFraction -= GPSData->GPSSecond; + + GPSData->itow = byteSwap32(geo->tow); +} + +// nut used +/** **************************************************************************** + * @name _processSiRFNavigationData LOCAL process message from SiRF + * @param [in] msg - [0x02] data to be parsed + * @param [in] msgLength - length of the message to be parsed + * @param [in] GPSData - GPS data structure to parse message into + * @retval N/A + ******************************************************************************/ +static void _processSiRFNavigationData(char *msg, + unsigned int msgLength, + GpsData_t *GPSData) +{ + tSiRFNav nav; + int axis; + int index; + unsigned int i; + + if (msgLength < sizeof(nav)) + return; // error + + index = 0; // X, Y, Z [m] + for ( axis = 0; axis < NUM_AXIS; axis++) { + SET_VALUE(nav.pos[axis], msg, &index); + } + // X, Y, Z [m/sec] + for ( axis = 0; axis < NUM_AXIS; axis++) { + SET_VALUE(nav.vel[axis], msg, &index); + } + + nav.mode1 = msg[index]; index++; + nav.hdop = msg[index]; index++; + nav.mode2 = msg[index]; index++; + + SET_VALUE(nav.gpsWeek, msg, &index); + SET_VALUE(nav.tow, msg, &index); + + for ( i = 0 ; i < sizeof(nav.prn); i++ ) { + nav.prn[i] = msg[index]; + index++; + } + + /******* packet parsed, now put it in GPSData as best as makes sense ****/ + GPSData->updateFlagForEachCall |= 1 << GOT_GGA_MSG; + GPSData->totalGGA++; + for ( axis = 0; axis < NUM_AXIS; axis++) { + GPSData->vNed[axis] = 0.125 * (double)nav.vel[axis]; // now in m/s + } +} + +/** **************************************************************************** + * @name _processSiRFMeasuredTrackerData LOCAL process message from SiRF + * @param [in] msg - [0x04] data to be parsed + * @param [in] msgLength - length of the message to be parsed + * @param [in] GPSData - GPS data structure to parse message into + * @retval N/A + ******************************************************************************/ +static void _processSiRFMeasuredTrackerData(char *msg, + unsigned int msgLength, + GpsData_t *GPSData) +{ + tSiRFTracker tracker; + int index = 0; + unsigned int i; + unsigned int c; + + if (msgLength < (sizeof(tracker) - sizeof(tracker.channel)) ) { + return; + } + SET_VALUE(tracker.gpsWeek, msg, &index); + SET_VALUE(tracker.tow, msg, &index); // [s] + + tracker.numChannels = msg[index]; index++; + + for ( i = 0; i < tracker.numChannels && (unsigned int)index < msgLength; i++) { + tracker.channel[i].id = msg[index]; index++; + tracker.channel[i].azimuth = msg[index]; index++; + tracker.channel[i].state = msg[index]; index++; + for ( c = 0; c < (unsigned int)NUM_CNO_VALUES && (unsigned int)index < msgLength; c++) { + tracker.channel[i].cno[c] = msg[index]; index++; + } + } +} + +static uint8_t configSirf = FALSE; + +/** **************************************************************************** + * @name processSiRFBinaryMessage process message from SiRF + * @param [in] msg - data to be parsed + * @param [in] msgLength - length of the message to be parsed + * @param [in] GPSData - GPS data structure to parse message into + * @retval N/A + ******************************************************************************/ +void processSiRFBinaryMessage(char *msg, + unsigned int *msgLength, + GpsData_t *GPSData) +{ + int index = GPSData->GPSMsgSignature.lengthOfHeaderIDLength; + uint8_t msgId = msg[index]; index++; + static char sirfSoftVer[128]; + uint32_t len = *msgLength; + static uint32_t sirfSoftVerCnt; + + switch (msgId) { + case SIRF_GEODETIC_NAVIGATION_DATA: // 0x29, 41 + _parseSiRFGeodeticNavMsg(&msg[index], + *msgLength - index, + GPSData); + break; + case SIRF_NAVIGATION_DATA: // 0x02 + _processSiRFNavigationData(&msg[index], + *msgLength - index, + GPSData); + break; + case SIRF_MEASURED_TRACKER_DATA: // 0x04 + _processSiRFMeasuredTrackerData(&msg[index], + *msgLength - index, + GPSData); + break; + case SIRF_SOFTWRE_VERSION_STRING: // 0x06 + if (len < 128) { + memcpy(sirfSoftVer, msg, len); + sirfSoftVerCnt++; + } + if (configSirf == FALSE) + _configureSiRFBinaryMessages(); // set output data rate + + configSirf = TRUE; + GPSData->GPSConfigureOK = 1; + break; + case SIRF_SOFTWARE_COMMAND_ACK: // 0x0b + case SIRF_SOFTWARE_COMMAND_NACK: // 0x0c + // just to get rid of warnings: +// case SIRF_EPHEMERIS: // 0x38 = 56 +// case SIRF_STATISICS: // 0xE1 = 225 +// case SIRF_TRACKER_MESSAGES: // 0x41 = 65 + // do nothing, essentially we can't turn this off + break; + default: // got unexpected message, send a disable for the message + _SiRFSendMessageRateMsg(0x00, + msgId, + 0); // turn off this message + break; + } + GPSData->GPSConfigureOK = 1; +} + +#endif // GPS \ No newline at end of file diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processUbloxGPS.c b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processUbloxGPS.c new file mode 100644 index 0000000..9354476 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/processUbloxGPS.c @@ -0,0 +1,1570 @@ +/** *************************************************************************** + * @file processUbloxGPS.c Processing for ublox GPS receiver. + * + * 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. + * @details + * This file includes all specific processing, including configuring, .., + * for ublox GPS receiver. + *****************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ +#ifdef GPS + + +#include +#include + +#include "driverGPS.h" +#include "platformAPI.h" +#include "gpsAPI.h" + +#ifdef DISPLAY_DIAGNOSTIC_MSG +#include "debug.h" +#endif + +/// constants used for connecting with a unknown ublox +#define AUTO_BAUD_PROCESS 0 +#define RESTORE_TO_FACTORY 1 +#define CONFIG_TO_57600 2 +#define QUERY_VERSION 3 + +/** @struct NAV_POSLLH_PAYLOAD_STRUCT + @brief NAV_POSLLH payload. + */ +typedef struct { + unsigned long itow; + signed long LonLatH[3]; + unsigned long HMSL; + unsigned long Hacc; + unsigned long Vacc; + +} NAV_POSLLH_PAYLOAD_STRUCT; + +/** @union NAV_POSLLH_PAYLOAD_UNION + @brief NAV_POSLLH payload. + */ +union NAV_POSLLHPayLoad +{ char myChar[28]; + NAV_POSLLH_PAYLOAD_STRUCT myData; +} NAV_POSLLH_PAYLOAD_UNION; + + +/** @struct NAV_VELNED_PAYLOAD_STRUCT + @brief NAV_VELNED payload. + */ +typedef struct { + unsigned long itow; + signed long Vel_NED[3]; + unsigned long Speed; + unsigned long GSpeed; + signed long Heading; + unsigned long SAcc; + unsigned long CAcc; + +} NAV_VELNED_PAYLOAD_STRUCT; + +/** @union NAV_VELNED_PAYLOAD_UNION + @brief NAV_VELNED payload. + */ +union NAV_VELNEDPayload +{ char myChar[36]; + NAV_VELNED_PAYLOAD_STRUCT myData; +} NAV_VELNED_PAYLOAD_UNION; + +/** @struct NAV_STATUS_PAYLOAD_STRUCT + @brief NAV_STATUS payload. + */ +typedef struct { + unsigned long itow; + unsigned char GPSfix; + unsigned char Flags; + unsigned char Diffs; + unsigned char res; + unsigned long TTFF; + unsigned long MSSS; +} NAV_STATUS_PAYLOAD_STRUCT; + +/** @union NAV_STATUS_PAYLOAD_UNION + @brief NAV_STATUS payload. + */ +union NAV_STATUSPayload +{ char myChar[16]; + NAV_STATUS_PAYLOAD_STRUCT myData; +} NAV_STATUS_PAYLOAD_UNION; + + +char msgIDTurnedOFF[TEST_ID_RECORD][2]; +char msgIDOn[TEST_ID_RECORD][2]; +unsigned int totalUnwantedMsg = 0; +unsigned char IDCounter1 = 0; +unsigned char IDCounter2 = 0; +unsigned char processedIDcounter = 0; + +//void decodeUbloxGPSBinary(int *numInBuffPrt, GpsData_t* GPSData); +//unsigned char configurate_uBloxGPS (GpsData_t* GPSData); + +//void pollUbloxMsg(ubloxIDTypeSTRUCT *IDInput, GpsData_t *GPSData); +void _computeUbloxCheckSumCrc(char *msg, + unsigned int *msgLength, + unsigned int *cCKA, + unsigned int *cCKB); +void _decodeVersionMsg(char *msg, + unsigned int *msgLength, + GpsData_t *GPSData); +unsigned char _sendAcknowlegdeProcess(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData, + unsigned char *firstCall, + void (*pt2MyFunc)(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData)); +void _generateSendCFG_MSG(ubloxIDTypeSTRUCT *IDInput, + GpsData_t *GPSData); +void _generateSendCFG_PRT(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData); +void _generateSendCFG_CFG(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData); + +void (*pt2TmpFunc)(ubloxIDTypeSTRUCT *cfgID, GpsData_t *GPSData); +void (*pt2TmpFunc1)(ubloxIDTypeSTRUCT *cfgID, GpsData_t *GPSData); + +void decodeNavPvt(char *msg, GpsData_t *GPSData); + +/** **************************************************************************** + * @name configUbloxGPSReceiver LOCAL NOT CALLED configure ublox GPS receiver + * @brief All GPS receiver configuration commands should be sent here if needed. + * @author Dong An + * @param [out] bytesFromBuffer - data from GPS data buffer + * @param [out] GPSData - gps data structure + * @retval N/A + ******************************************************************************/ +void configUbloxGPSReceiver(int *bytesFromBuffer, + GpsData_t *GPSData) +{ + static unsigned char versionKnown = 0; + static unsigned char ubloxPerCfgDone = 0; + static unsigned char ubloxIOCfgDone = 0; + + if (versionKnown == 0) + versionKnown = getConnectedWithUnknownStatusUbloxGPS( GPSData ); + else /// GPS FW version is identified + if(ubloxPerCfgDone == 0) + ubloxPerCfgDone = configurateUBloxGPSPerformance( GPSData ); + else if (ubloxIOCfgDone == 0) + ubloxIOCfgDone = configurateUBloxGPSIOMsgRate(GPSData); /// configure ublox + + if(ubloxIOCfgDone == 1) { /// count after config + GPSData->GPSConfigureOK = 1; + GPSData->LLHCounter = 0; + GPSData->VELCounter = 0; + GPSData->STATUSCounter = 0; + GPSData->SBASCounter = 0; + versionKnown = 0; /// release for next time + ubloxPerCfgDone = 0; + ubloxIOCfgDone = 0; + } +} +/** **************************************************************************** + * @name ubloxGPSMsgSender send out GPS configuration message + * @author Dong An + * @param [in] msgID - message identifier + * @param [in] msgpayLoad - input buffer + * @param [in] msgpayLoadlength - length + * @param [in] GPSData structure to parse into + * @retval N/A + ******************************************************************************/ +void ubloxGPSMsgSender(ubloxIDTypeSTRUCT *msgID, + char *MsgPayLoad, + int *msgPayLoadLength, + GpsData_t *GPSData) +{ + char ubloxMsg[MAX_UBLOX_BODY_LENGTH+8]; + int i; + static unsigned int j; ///need static for optimizer. + ///Otherwise, it could be not incremented at the end + unsigned int msgLength; + unsigned int CK_A_C; + unsigned int CK_B_C; + short payloadLength = *msgPayLoadLength; + + j = 0; + ubloxMsg[j++] = 0xB5; ///header1 + ubloxMsg[j++] = 0x62; ///header2 + + ubloxMsg[j++] = msgID->classID; + ubloxMsg[j++] = msgID->msgID; + + ubloxMsg[j++] = payloadLength&0xFF; + ubloxMsg[j++] = (payloadLength >> 8)&0xFF; + + for (i = 0; i < *msgPayLoadLength; i++) + ubloxMsg[j++] = MsgPayLoad[i]; + msgLength = j + 2; ///include 2 bytes crc + _computeUbloxCheckSumCrc(ubloxMsg, + &msgLength, + &CK_A_C, + &CK_B_C); + + ubloxMsg[j++] = CK_A_C; + ubloxMsg[j++] = CK_B_C; + ubloxMsg[j++] = 0; + writeGps(ubloxMsg, (uint16_t)j); +} + +/** **************************************************************************** + * @name processUbloxBinaryMessage parse a complete ubblox binary message. + * @author Dong An + * @param [in] msgID - message identifier + * @param [in] msgpayLoad - input buffer + * @param [in] msgpayLoadlength - length + * @param [in] GPSData structure to parse into + * @retval N/A + ******************************************************************************/ +void processUbloxBinaryMessage(char *msg, + unsigned int *msgLength, + GpsData_t *GPSData) +{ + unsigned int checksumACalcu; + unsigned int checksumBCalcu; + unsigned int checksumARec; + unsigned int checksumBRec; + unsigned int i; + int j; + ubloxIDTypeSTRUCT msgID; + unsigned short classIDMessID; + static unsigned char firstFlag = 0; + static unsigned char firstReceived = 0; + + ///testing only + msgIDOn[IDCounter2][0] = msg[2]; + msgIDOn[IDCounter2][1] = msg[3]; + IDCounter2++; + if (IDCounter2 == TEST_ID_RECORD) + IDCounter2=0; + + ///checksum + checksumARec = msg[*msgLength - 2]; + checksumBRec = msg[*msgLength - 1]; + _computeUbloxCheckSumCrc(msg, + msgLength, + &checksumACalcu, + &checksumBCalcu); + + if( checksumACalcu!=checksumARec || checksumBCalcu !=checksumBRec) + return; + + classIDMessID = msg[2]; + classIDMessID = classIDMessID << 8 |(msg[3] & 0xFF); + + switch (classIDMessID) { + case UBLOX_NAV_PVT: + { + decodeNavPvt(msg, GPSData); + break; + } + case UBLOX_NAV_POSLLH: ///LLH + j = 6; + for (i = 0; i < 28; i++) + { + NAV_POSLLH_PAYLOAD_UNION.myChar[i] = msg[i+j]; + } + + ///similiar to NMEA GGA + GPSData->updateFlagForEachCall |= 1<totalGGA++; + GPSData->lat = (double)NAV_POSLLH_PAYLOAD_UNION.myData.LonLatH[0]*1e-7; ///deg (*1e7) + GPSData->lon = (double)NAV_POSLLH_PAYLOAD_UNION.myData.LonLatH[1]*1e-7; /// deg (*1e7) + GPSData->alt = NAV_POSLLH_PAYLOAD_UNION.myData.LonLatH[2]*1e-3 ; ///meters (*1e-3) +// GPSData->LonLatH[2] = NAV_POSLLH_PAYLOAD_UNION.myData.LonLatH[2]*10e-3 ; ///meters (*1e-3) + GPSData->itow = NAV_POSLLH_PAYLOAD_UNION.myData.itow; + GPSData->LLHCounter++; + break; + case UBLOX_NAV_VELNED: ///VEL-NED + j = 6; + for (i = 0; i < 36; i++) + { + NAV_VELNED_PAYLOAD_UNION.myChar[i] = msg[j + i] ; + } + ///NOT similiar to NMEA VTG message (speed and heading) + // this is NOT FILTERED + GPSData->updateFlagForEachCall |= 1<totalVTG++; + GPSData->vNed[0] = (double)NAV_VELNED_PAYLOAD_UNION.myData.Vel_NED[0] * 1e-2; ///m/s + GPSData->vNed[1] = (double)NAV_VELNED_PAYLOAD_UNION.myData.Vel_NED[1] * 1e-2; + GPSData->vNed[2] = (double)NAV_VELNED_PAYLOAD_UNION.myData.Vel_NED[2] * 1e-2; + GPSData->itow = NAV_VELNED_PAYLOAD_UNION.myData.itow; + GPSData->GPSVelAcc = (double)NAV_VELNED_PAYLOAD_UNION.myData.SAcc * 1e-2; ///changed to m/s + + GPSData->updateFlagForEachCall |= 1 << GOT_UBLOX_VNED; + GPSData->VELCounter++; + break; + case UBLOX_NAV_STATUS: ///NAV_STATUS + GPSData->gpsFixType = msg[10]; ///fix indicator + if((msg[11]&0x02) == 0x02) ///6+5 + GPSData->GPSStatusWord |=1 << DGPS_ON; ///on + else + GPSData->GPSStatusWord &= !(1 << DGPS_ON); ///off + GPSData->STATUSCounter++; + break; + case UBLOX_CFG_NAV: ///CFG NAV-Mess + break; + case UBLOX_NAV_SBAS: ///CFG NAV-SBAS + GPSData->GPSStatusWord &= CLEAR_DGPS_SOURCES; ///clear + switch(msg[12]) { + case 0: + GPSData->GPSStatusWord |=1<GPSStatusWord |=1<GPSStatusWord |=1<updateFlagForEachCall |= (1 << GOT_UBLOX_NAVSBAS); + GPSData->SBASCounter++; + break; + case UBLOX_ACK_ACK: ///ACK message + GPSData->reClassID = msg[6]; + GPSData->reMsgID = msg[7]; + break; + case UBLOX_MON_VER: + _decodeVersionMsg(msg, msgLength, GPSData); + GPSData->isGPSFWVerKnown = 1; + break; + case UBLOX_CFG_NAV2: + break; + default: ///other Messages + ///firewall: only after GPS cfg is done, because this may + /// disturb the GPS cfg process + if ((GPSData->GPSTopLevelConfig & (1<GPSConfigureOK != 0) { + if (firstReceived == 0) { + msgIDTurnedOFF[IDCounter1][0] = msg[2]; + msgIDTurnedOFF[IDCounter1][1] = msg[3]; + firstReceived = 1; + IDCounter1++; + totalUnwantedMsg++; + if (IDCounter1 == TEST_ID_RECORD) + IDCounter1=0; + } else { + j = 0; + for (i = 0; i < totalUnwantedMsg; i++) { + if (msgIDTurnedOFF[j][0] != msg[2] || + msgIDTurnedOFF[j][1] != msg[3]) { ///new one + msgIDTurnedOFF[IDCounter1][0] = msg[2]; + msgIDTurnedOFF[IDCounter1][1] = msg[3]; + IDCounter1++; + if (IDCounter1 == TEST_ID_RECORD) + IDCounter1=0; + totalUnwantedMsg++; + break; + }///end of new one + j++; + if (j == TEST_ID_RECORD) + j = 0; + }///end of scanning + }///end of not first time + if (processedIDcounter!=IDCounter1) { + if ((msgIDTurnedOFF[processedIDcounter][0]&0xFF)!=0x04 && + (msgIDTurnedOFF[processedIDcounter][1]&0xFF)!=0x01) { + // if it is a non-warning message + msgID.classID = (UBLOX_CFG_MSG >> 8) & 0xFF; + msgID.msgID = UBLOX_CFG_MSG & 0xFF; + + msgID.cfgClassID = msgIDTurnedOFF[processedIDcounter][0]; + msgID.cfgMsgID = msgIDTurnedOFF[processedIDcounter][1]; + msgID.rateRatio = 0; + msgID.whatTagert = 0; ///current port + + pt2TmpFunc =&_generateSendCFG_MSG; + + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, GPSData, &firstFlag,pt2TmpFunc); + firstFlag = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, GPSData, &firstFlag, pt2TmpFunc)) { + firstFlag = 0; ///reset for next time + processedIDcounter++; + if (processedIDcounter == TEST_ID_RECORD) + processedIDcounter = 0; + GPSData->firewallCounter++; + } + }/// end non-first time + GPSData->firewallRunCounter++; + } else { //skip if it is a warning message + processedIDcounter++; + if (processedIDcounter == TEST_ID_RECORD) + processedIDcounter=0; + } + }///end of "processedIDcounter!=IDCounter1" + }/// end of not configurating + break; + } ///end of switch +} + +void decodeNavPvt(char *msg, GpsData_t *GPSData) +{ + // Decode + ubloxNavPvtSTRUCT *navPvt; + navPvt = (ubloxNavPvtSTRUCT*)(&msg[UBLOX_BINAERY_HEADER_LEN]); + + // Get decoded results + // fix type + GPSData->gpsFixType = INVALID; + if (navPvt->flags & 0x01) + { + GPSData->gpsFixType = SPP; + if (navPvt->flags & 0x02) + { + GPSData->gpsFixType = DGPS; + } + uint8_t carrSoln = navPvt->flags >> 6; + if (carrSoln == 1) + { + GPSData->gpsFixType = RTK_FLOAT; + } + else if(carrSoln == 2) + { + GPSData->gpsFixType = RTK_FIX; + } + } + + // num of satellites + GPSData->numSatellites = navPvt->numSV; + + // lat + GPSData->lat = navPvt->lat * 1.0e-7; + + // lon + GPSData->lon = navPvt->lon * 1.0e-7; + + // altitude + GPSData->alt = navPvt->height * 1.0e-3; + // NED velocity + GPSData->vNed[0] = navPvt->velN * 1.0e-3; + GPSData->vNed[1] = navPvt->velE * 1.0e-3; + GPSData->vNed[2] = navPvt->velD * 1.0e-3; + // heading and ground speed + GPSData->trueCourse = navPvt->headMot * 1.0e-5; + GPSData->rawGroundSpeed = navPvt->gSpeed * 1.0e-3; + // fractional second + GPSData->GPSSecondFraction = navPvt->nano * 1.0e-9; + // ellipsoid height + GPSData->geoidAboveEllipsoid = (navPvt->height - navPvt->hMSL) * 1.0e-3; + // itow + GPSData->itow = navPvt->iTOW; + if(GPSData->itow%1000 != 0){ + GPSData->itow+=1; // correction of numerical issue + } + platformUpdateITOW(GPSData->itow); + // year month day hour min sec + GPSData->GPSmonth = navPvt->month; + GPSData->GPSday = navPvt->day; + GPSData->GPSyear = navPvt->year % 100; + GPSData->GPSHour = navPvt->hour; + GPSData->GPSMinute = navPvt->min; + GPSData->GPSSecond = navPvt->sec; + // accuracy + GPSData->HDOP = navPvt->pDOP * 7.0e-3; // scaling is 0.01 for pdop, hdop^2 is less than half of pdop^2 + // hdop is less than sqrt(2) (about 0.7) x pdop + GPSData->GPSHorizAcc = navPvt->hAcc * 1.0e-3; + GPSData->GPSVertAcc = navPvt->vAcc * 1.0e-3; + // update flag + GPSData->updateFlagForEachCall |= 1 << GOT_GGA_MSG; + GPSData->updateFlagForEachCall |= 1 << GOT_VTG_MSG; +} + +/** **************************************************************************** + * @name: parseNMEAMessage parse a complete NMEA message. + * @author Dong An + * @param [in] inByte - next byte from serial stream + * @param [in] gpsMsg - GPS message buffer + * @param [in] GPSData - data structure to parse into + * @retval N/A + * @details Extract message from serial byte stream and process + ******************************************************************************/ +int parseUbloBinaryMessage(uint8_t inByte, uint8_t *gpsMsg, GpsData_t *GPSData) +{ + static unsigned int len = 0; + static uint8_t *ptr; + static uint32_t sync = 0; + unsigned static int synced = 0; + unsigned static int totalLen = 0; + + // wait for ublox binary header to sync + sync = (sync << 8) | inByte; + if (synced == 0) + { + if ((sync & 0x0000ffff) == UBLOX_BINAERY_SYNC) + { + gpsMsg[0] = (UBLOX_BINAERY_SYNC >> 8) & 0xff; + gpsMsg[1] = UBLOX_BINAERY_SYNC & 0xff; + synced = 1; + ptr = &gpsMsg[2]; + len = 2; + } + return 0; + } + // synced, wait for a complete packet + // class, ID, length + *ptr++ = inByte; + len++; + // header bytes are all received + if (len == UBLOX_BINAERY_HEADER_LEN) + { + // total length = headerlen + payload length + 2bytes CRC + int payloadLen = gpsMsg[5]; + payloadLen = payloadLen << 8 | gpsMsg[4]; + totalLen = UBLOX_BINAERY_HEADER_LEN + payloadLen + 2; + if (totalLen > MAX_MSG_LENGTH) + { + len = 0; + totalLen = 0; + synced = 0; + return 0; + } + } + // a complete packet is received + if (len == totalLen) + { + processUbloxBinaryMessage((char *)gpsMsg, &len, GPSData); + len = 0; + totalLen = 0; + synced = 0; + } + + + return 0; +} + +/** **************************************************************************** + * @name _computeUbloxCheckSumCrc compute checksum of a complete ubblox binary + * message. + * @author Dong An + * @param [in] msg- message + * @param [in] msgLength - input buffer length + * @param [out] cCKA - checksum A + * @param [out] cCKB - checksum B + * @retval N/A + ******************************************************************************/ +void _computeUbloxCheckSumCrc(char *msg, + unsigned int *msgLength, + unsigned int *cCKA, + unsigned int *cCKB) +{ + int i; + int CKRange; + unsigned int checksumACalcu = 0; + unsigned int checksumBCalcu = 0; + + CKRange = *msgLength - 4; ///exclude header and crc itself + for (i = 0; i < CKRange; i++) { + checksumACalcu = checksumACalcu + (msg[i+2] & 0xFF); + checksumACalcu = checksumACalcu& 0xFF; + + checksumBCalcu = checksumBCalcu + checksumACalcu; + checksumBCalcu = checksumBCalcu & 0xFF; + } + *cCKA = checksumACalcu; + *cCKB = checksumBCalcu; +} + +/** **************************************************************************** + * @name generateSendCFG_NAV generate and send out CFG-NAV message. + * @author Dong An + * @param [in] cfgID- message + * @param [in] GPSData - GPS data structure to pull message from + * @retval N/A + ******************************************************************************/ +void generateSendCFG_NAV(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData) +{ + int i; + char tmpMsgPayload[MAX_UBLOX_BODY_LENGTH ]; + ubloxIDTypeSTRUCT tmpMsgID; + + tmpMsgID.classID = cfgID->classID; + tmpMsgID.msgID = cfgID->msgID; + + i = 0; ///offset + tmpMsgPayload[i++] = 0x3; ///0: Platform, default=0x03=Automotive; + ///configured to 0x6=airborne with <2g + //changed to 0x03 Automotive for UCB 2.2.1 + tmpMsgPayload[i++] = 0x04; ///1: MinSVs, default=0x03 + ///configured to 04 + tmpMsgPayload[i++] = 0x10; ///2: MaxSVs, default=0x10 + tmpMsgPayload[i++] = 0x18; ///3: MinCNO,default=0x18 + tmpMsgPayload[i++] = 0x14; ///4: AbsCNO, default=0x14 + tmpMsgPayload[i++] = 0x05; ///5: MinELE, defautl=0x05 + tmpMsgPayload[i++] = 0x00; ///6: DGPSTTR, default=0x00 + tmpMsgPayload[i++] = 0x3C; ///7: DGPSTO, default=0x3C + tmpMsgPayload[i++] = 0x3C; ///8: PRCAGE, default=0x3C + tmpMsgPayload[i++] = 0x14; ///9: CPCAGE, default=0x14 + tmpMsgPayload[i++] = 0xE8; ///10: U2, MinCLT, default=0xE8 + tmpMsgPayload[i++] = 0x03; ///11: default=0x03 + tmpMsgPayload[i++] = 0x00; ///12: U2, AbsCLT, default=0x00 + tmpMsgPayload[i++] = 0x00; ///13: default=0x00 + tmpMsgPayload[i++] = 0x00; ///14: MaxDR, default=0x00 + tmpMsgPayload[i++] = 0x07; ///15: NAVOPT, default=0x17 + ///configured to 0x07 + tmpMsgPayload[i++] = 0x64; ///16: U2, PDOP, default=0xFA + ///configured to 0x64 + tmpMsgPayload[i++] = 0x00; ///17: Default=0x00 + tmpMsgPayload[i++] = 0x64; ///18: U2, TDOP, default=0xFA + ///configured to 0x64 + tmpMsgPayload[i++] = 0x00; ///19: default=0x00 + tmpMsgPayload[i++] = 0x64; ///20: U2, PACC, default=0x64 + tmpMsgPayload[i++] = 0x00; ///21: default=0x00 + tmpMsgPayload[i++] = 0x2C; ///22: U2, TACC, default=0x2C + tmpMsgPayload[i++] = 0x01; ///23: default=0x01 + tmpMsgPayload[i++] = 0x0F; ///24: U2, FACC, default=0F + tmpMsgPayload[i++] = 0x00; ///25: default=0x00 + tmpMsgPayload[i++] = 0x00; ///26: StaticThres, default=0x00 + tmpMsgPayload[i++] = 0x00; ///27: reserved. + + ubloxGPSMsgSender(&tmpMsgID, + tmpMsgPayload, + &i, + GPSData); +} + +/** **************************************************************************** + * @name generateSendCFG_NAV2 generate and send out CFG-NAV2 message. + * @author Dong An + * @param [in] cfgID- message + * @param [in] GPSData - GPS data structure to pull message from + * @retval N/A + ******************************************************************************/ +void generateSendCFG_NAV2(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData) +{ + int i; + int j; + char tmpMsgPayload[MAX_UBLOX_BODY_LENGTH ]; + ubloxIDTypeSTRUCT tmpMsgID; + + + tmpMsgID.classID = cfgID->classID; + tmpMsgID.msgID = cfgID->msgID; + + i = 0; ///offset + tmpMsgPayload[i++] = 0x3; ///0: Platform; default=0x03=automotive; + ///configured to 0x6=airborne with <2g + //changed to automotive for UCB 2.1.1 + for (j = 0; j < 3; j++) + tmpMsgPayload[i++] = 0x00; /// reserved 1-3 + tmpMsgPayload[i++] = 0x03; ///4: MinSVInitial, default=0x03 + tmpMsgPayload[i++] = 0x04; //5 MinSVs, default=0x03; + ///configured to 0x04 + tmpMsgPayload[i++] = 0x10; ///6: MaxSVs, default=0x10; + tmpMsgPayload[i++] = 0x02; ///7: FixMode, default=0x02; + tmpMsgPayload[i++] = 0x50; ///8: I4, FixedAltitude, default=0x50; + tmpMsgPayload[i++] = 0xC3; ///9: default=0xC3 + tmpMsgPayload[i++] = 0x00; ///10: default=0x00 + tmpMsgPayload[i++] = 0x00; ///11: default=0x00 + tmpMsgPayload[i++] = 0x18; ///12: MinCN0Initial, default=0x18 + tmpMsgPayload[i++] = 0x14; ///13: MinCN0After, default=0x14 + tmpMsgPayload[i++] = 0x05; ///14: MinElE default=0x05 + tmpMsgPayload[i++] = 0x3C; ///15: DGPSTO, default=0x3C + tmpMsgPayload[i++] = 0x00; ///16: MaxDR, default=0x00 + tmpMsgPayload[i++] = 0x01; ///17: NAVOPT, default=0x01 + for (j = 0; j < 2; j++) + tmpMsgPayload[i++] = 0x00; ///18-19: U2 reserved + tmpMsgPayload[i++] = 0x64; ///20: U2,PDOP, default=0xFA + /// configured to 0x64 + tmpMsgPayload[i++] = 0x00; ///21: default=0x00 + tmpMsgPayload[i++] = 0x64; ///22: U2, TDOP, default=0xFA + ///configured to 0x64 + tmpMsgPayload[i++] = 0x00; ///23: default=00 + tmpMsgPayload[i++] = 0x64; ///24: U2, PACC, defautl=0x64 + tmpMsgPayload[i++] = 0x00; ///25: default=0x00 + tmpMsgPayload[i++] = 0x2C; ///26: U2, TACC, default=0x2C + tmpMsgPayload[i++] = 0x01; ///27: default=0x01 + tmpMsgPayload[i++] = 0x00; ///28: StaticThres, default=0x00 + for (j = 0; j < 11; j++) + tmpMsgPayload[i++] = 0x00; /// reserved 29-39 + + ubloxGPSMsgSender(&tmpMsgID, + tmpMsgPayload, + &i, + GPSData); +} + +/** **************************************************************************** + * @name GenerateSendCFG_RXM generate and send out CFG-RXM message. + * @author Dong An + * @param [in] cfgID- message + * @param [in] GPSData - GPS data structure to pull message from + * @retval N/A + ******************************************************************************/ +void GenerateSendCFG_RXM(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData) +{ + int i; + char tmpMsgPayload[MAX_UBLOX_BODY_LENGTH ]; + ubloxIDTypeSTRUCT tmpMsgID; + + + tmpMsgID.classID = cfgID->classID; + tmpMsgID.msgID = cfgID->msgID; + + i = 0; + tmpMsgPayload[i++] = 1; + tmpMsgPayload[i++] = 0; + + ubloxGPSMsgSender(&tmpMsgID, + tmpMsgPayload, + &i, + GPSData); +} + +/** **************************************************************************** + * @name generateSendCFG_SBAS generate and send out CFG-SBAS message. + * @author Dong An + * @param [in] cfgID- message + * @param [in] GPSData - GPS data structure to pull message from + * @retval N/A + ******************************************************************************/ +void generateSendCFG_SBAS(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData) +{ + int i; + char tmpMsgPayload[MAX_UBLOX_BODY_LENGTH ]; + ubloxIDTypeSTRUCT tmpMsgID; + + tmpMsgID.classID = cfgID->classID; + tmpMsgID.msgID = cfgID->msgID; + + i = 0; + tmpMsgPayload[i++] = 0x1; ///SBAS Enabled(1) + tmpMsgPayload[i++] = 0x7; ///Bit 0: Use SBAS GEOs as a ranging source (for navigation) + ///Bit 1: Use SBAS Differential Corrections + ///Bit 2: Use SBAS Integrity Information + tmpMsgPayload[i++] = 0x1; ///1 SBAS search channel (valid range: 0 - 3) to use + tmpMsgPayload[i++] = 0; ///reserved + tmpMsgPayload[i++] = 0; ///auto-search + tmpMsgPayload[i++] = 0; + tmpMsgPayload[i++] = 0; + tmpMsgPayload[i++] = 0; + + ubloxGPSMsgSender(&tmpMsgID, + tmpMsgPayload, + &i, + GPSData); +} + +/** **************************************************************************** + * @name generateSendCFG_RATE generate and send out CFG-RATE message. + * @author Dong An + * @param [in] cfgID- message + * @param [in] GPSData - GPS data structure to pull message from + * @retval N/A + ******************************************************************************/ +void generateSendCFG_RATE(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData) +{ + int i; + char tmpMsgPayload[MAX_UBLOX_BODY_LENGTH ]; + ubloxIDTypeSTRUCT tmpMsgID; + unsigned short Meas; + unsigned short Nav; + unsigned short Time; + + tmpMsgID.classID = cfgID->classID; + tmpMsgID.msgID = cfgID->msgID; + + if( (GPSData->GPSTopLevelConfig & (1 << HZ2)) == (1 << HZ2)) + Meas = 500; ///ms + else + Meas = 250; ///ms + + Nav = 1; + Time = 0; + i = 0; + tmpMsgPayload[i++] = Meas & 0xFF; + tmpMsgPayload[i++] = (Meas >> 8) & 0xFF; + + tmpMsgPayload[i++] = Nav & 0xFF; + tmpMsgPayload[i++] = (Nav >> 8) & 0xFF; + + tmpMsgPayload[i++] = Time & 0xFF; + tmpMsgPayload[i++] = (Time >> 8) & 0xFF; + + ubloxGPSMsgSender(&tmpMsgID, + tmpMsgPayload, + &i, + GPSData); +} + +/** **************************************************************************** + * @name pollUbloxMsg poll a ublox message. + * @author Dong An + * @param [in] cfgID- message + * @param [in] GPSData - GPS data structure to pull message from + * @retval N/A + ******************************************************************************/ +void pollUbloxMsg(ubloxIDTypeSTRUCT *IDInput, + GpsData_t *GPSData) +{ + int i; + char tmpMsgPayload[MAX_UBLOX_BODY_LENGTH ]; + ubloxIDTypeSTRUCT tmpMsgID; + + tmpMsgID.classID = IDInput->classID; + tmpMsgID.msgID = IDInput->msgID; + + i = 0; /// o bytes in payload + ubloxGPSMsgSender(&tmpMsgID, + tmpMsgPayload, + &i, + GPSData); +} + +/** **************************************************************************** + * @name _generateSendCFG_MSG LOCAL generate and send out CFG-MSG message. + * @author Dong An + * @param [in] cfgID- message + * @param [in] GPSData - GPS data structure to pull message from + * @retval N/A + ******************************************************************************/ +void _generateSendCFG_MSG(ubloxIDTypeSTRUCT *IDInput, + GpsData_t *GPSData) +{ + int i; + char tmpMsgPayload[MAX_UBLOX_BODY_LENGTH ]; + ubloxIDTypeSTRUCT tmpMsgID; + + tmpMsgID.classID = (UBLOX_CFG_MSG >> 8) & 0xFF; + tmpMsgID.msgID = UBLOX_CFG_MSG & 0xFF; + + i = 0; + switch (IDInput->whatTagert) { + case 0: ///current + tmpMsgPayload[i++] = IDInput->cfgClassID; + tmpMsgPayload[i++] = IDInput->cfgMsgID; + + tmpMsgPayload[i++] = IDInput->rateRatio; ///Target 0 + break; + case 4:/// all target + tmpMsgPayload[i++] = IDInput->cfgClassID; + tmpMsgPayload[i++] = IDInput->cfgMsgID; + + tmpMsgPayload[i++] = IDInput->rateRatio; ///Target 0 + tmpMsgPayload[i++] = IDInput->rateRatio; ///Target 1 + tmpMsgPayload[i++] = IDInput->rateRatio; ///Target 2 + tmpMsgPayload[i++] = IDInput->rateRatio; ///Target 3 + break; + default: + break; + } + + ubloxGPSMsgSender(&tmpMsgID,tmpMsgPayload,&i, GPSData); + +} + +/** **************************************************************************** + * @name configurateUBloxGPSPerformance configure ublox NAV performance. + * @author Dong An + * @param [in] GPSData - GPS data structure to pull message from + * @retval N/A + ******************************************************************************/ +unsigned char configurateUBloxGPSPerformance (GpsData_t* GPSData) +{ + static unsigned char firstFlag1 = 0; + static int CFGIndex1 = UBLOX_CFG_RXM; + ubloxIDTypeSTRUCT msgID; + unsigned char finish = 0; + + switch (CFGIndex1) { + case UBLOX_CFG_RXM: ///CFG RXM + msgID.classID = (UBLOX_CFG_RXM >> 8) & 0xFF; + msgID.msgID = UBLOX_CFG_RXM & 0xFF; + pt2TmpFunc1 = &GenerateSendCFG_RXM; ///fast aquisition mode + if(firstFlag1 == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag1, + pt2TmpFunc1); + firstFlag1 = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag1, + pt2TmpFunc1)) { + firstFlag1 = 0; //reset for next case + CFGIndex1 = UBLOX_CFG_SBAS; ///exit + } + }/// end non-first time + break; + case UBLOX_CFG_SBAS: ///CFG-SBAS + msgID.classID = (UBLOX_CFG_SBAS >> 8) & 0xFF; + msgID.msgID = UBLOX_CFG_SBAS & 0xFF; + pt2TmpFunc1 = &generateSendCFG_SBAS; + if(firstFlag1 == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag1, + pt2TmpFunc1); + firstFlag1 = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag1, + pt2TmpFunc1)) { + firstFlag1 = 0; ///reset for next case + CFGIndex1 = UBLOX_CFG_NAV; ///exit + } + }/// end non-first time + break; + case UBLOX_CFG_NAV: ///CFG-NAV or CFG-NAV2 + if (GPSData->ubloxOldVersion == 0) { /// old version + msgID.classID = (UBLOX_CFG_NAV >> 8) & 0xFF; + msgID.msgID = UBLOX_CFG_NAV & 0xFF; + pt2TmpFunc1 = &generateSendCFG_NAV; + if(firstFlag1 == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag1, + pt2TmpFunc1); + firstFlag1 = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag1, + pt2TmpFunc1)) { + firstFlag1 = 0; ///reset for next case + CFGIndex1 = UBLOX_CFG_RATE; ///exit + } + }/// end non-first time + } else {/// for the latest ublox version + msgID.classID = (UBLOX_CFG_NAV2 >> 8) & 0xFF; + msgID.msgID = UBLOX_CFG_NAV2 & 0xFF; + pt2TmpFunc1 = &generateSendCFG_NAV2; + if(firstFlag1 == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag1, + pt2TmpFunc1); + firstFlag1 = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag1, + pt2TmpFunc1)) { + firstFlag1 = 0; //reset for next case + CFGIndex1 = UBLOX_CFG_RATE; //exit + } + }/// end non-first time + } ///end of "for latest version" + break; + case UBLOX_CFG_RATE:/// CFG-RATE (last increase the processor load) + msgID.classID = (UBLOX_CFG_RATE >> 8) & 0xFF; + msgID.msgID = UBLOX_CFG_RATE & 0xFF; + pt2TmpFunc1 = &generateSendCFG_RATE; + if(firstFlag1 == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag1, + pt2TmpFunc1); + firstFlag1 = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag1, + pt2TmpFunc1)) { + firstFlag1 = 0; ///reset for next case + CFGIndex1 = CFG_DONE; ///exit + } + }/// end non-first time + break; + case CFG_DONE: + finish = 1; + CFGIndex1 = UBLOX_CFG_RXM; + break; + } + return finish; +} + +/** **************************************************************************** + * @name configurateUBloxGPSIOMsgRate configure ublox I/O message. + * @author Dong An + * @param [in] GPSData - GPS data structure to pull message from + * @retval N/A + ******************************************************************************/ +unsigned char configurateUBloxGPSIOMsgRate (GpsData_t* GPSData) +{ + static unsigned char firstFlag = 0; + static unsigned int CFGIndex = UBLOX_NAV_POSLLH; + ubloxIDTypeSTRUCT msgID; + unsigned char finish = 0; + + msgID.classID = (UBLOX_CFG_MSG >> 8) & 0xFF;/// All cases use the same CFG-MSG(set message rate) + msgID.msgID = UBLOX_CFG_MSG & 0xFF; + + switch (CFGIndex) { + ///the following for binary message AHRS actually needs + case UBLOX_NAV_POSLLH: + msgID.cfgClassID = (UBLOX_NAV_POSLLH >> 8) & 0xFF; + msgID.cfgMsgID = UBLOX_NAV_POSLLH & 0xFF; + msgID.rateRatio = 1; + msgID.whatTagert = 0; ///current port + pt2TmpFunc = &_generateSendCFG_MSG; + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc); + firstFlag = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc)) { + firstFlag = 0; ///reset for next case + CFGIndex = UBLOX_NAV_VELNED; ///exit + } + }/// end non-first time + break; + case UBLOX_NAV_VELNED: + msgID.cfgClassID = (UBLOX_NAV_VELNED >> 8) & 0xFF; + msgID.cfgMsgID = UBLOX_NAV_VELNED & 0xFF; + msgID.rateRatio = 1; + msgID.whatTagert = 0; ///current port + pt2TmpFunc = &_generateSendCFG_MSG; + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc); + firstFlag = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc)) { + firstFlag = 0; ///reset for next case + CFGIndex = UBLOX_NAV_STATUS; ///exit + } + }/// end non-first time + break; + case UBLOX_NAV_STATUS: + msgID.cfgClassID = (UBLOX_NAV_STATUS >> 8) & 0xFF; + msgID.cfgMsgID = UBLOX_NAV_STATUS & 0xFF; + msgID.rateRatio = 1; + msgID.whatTagert = 0; ///current port + pt2TmpFunc = &_generateSendCFG_MSG; + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc); + firstFlag = 1; + } + else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc)) { + firstFlag = 0; ///reset for next case + CFGIndex = UBLOX_NAV_SBAS; ///exit + } + }/// end non-first time + break; + case UBLOX_NAV_SBAS: + msgID.cfgClassID = (UBLOX_NAV_SBAS >> 8) & 0xFF; + msgID.cfgMsgID = UBLOX_NAV_SBAS & 0xFF; + msgID.rateRatio = NAV_SBAS_RATE_RATIO; + msgID.whatTagert = 0; ///current port + pt2TmpFunc = &_generateSendCFG_MSG; + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc); + firstFlag = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc)) { + firstFlag = 0; ///reset for next case + CFGIndex = UBLOX_NMEA_GLL; ///exit + } + }/// end non-first time + break; + ///the following for NMEA + case UBLOX_NMEA_GLL: ///CFG RXM + msgID.cfgClassID = (UBLOX_NMEA_GLL >> 8) & 0xFF; + msgID.cfgMsgID = UBLOX_NMEA_GLL & 0xFF; + msgID.rateRatio = 0; + msgID.whatTagert = 4; ///GPS port + pt2TmpFunc = &_generateSendCFG_MSG; + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc); + firstFlag = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc)) { + firstFlag = 0; ///reset for next case + CFGIndex = UBLOX_NMEA_GSA; ///exit + } + }/// end non-first time + break; + case UBLOX_NMEA_GSA: + ///NMEA Messages: GSA + msgID.cfgClassID = (UBLOX_NMEA_GSA >> 8) & 0xFF; + msgID.cfgMsgID = UBLOX_NMEA_GSA & 0xFF; + msgID.rateRatio = 0; + msgID.whatTagert = 4; ///GPS port + pt2TmpFunc = &_generateSendCFG_MSG; + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc); + firstFlag = 1; + } else { //after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc)) { + firstFlag = 0; ///reset for next case + CFGIndex = UBLOX_NMEA_GSV; ///exit + } + }/// end non-first time + break; + case UBLOX_NMEA_GSV: + ///NMEA Messages: GSV + msgID.cfgClassID = (UBLOX_NMEA_GSV >> 8) & 0xFF; + msgID.cfgMsgID = UBLOX_NMEA_GSV & 0xFF; + msgID.rateRatio = 0; + msgID.whatTagert = 4; ///GPS port + pt2TmpFunc = &_generateSendCFG_MSG; + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc); + firstFlag = 1; + } + else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc)) { + firstFlag = 0; ///reset for next case + CFGIndex = UBLOX_NMEA_ZDA; ///exit + } + }/// end non-first time + break; + case UBLOX_NMEA_ZDA: + msgID.cfgClassID = (UBLOX_NMEA_ZDA >> 8) & 0xFF; + msgID.cfgMsgID = UBLOX_NMEA_ZDA & 0xFF; + msgID.rateRatio = 0; + msgID.whatTagert = 4; ///GPS port + pt2TmpFunc = &_generateSendCFG_MSG; + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc); + firstFlag = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc)) { + firstFlag = 0; ///reset for next case + CFGIndex = UBLOX_NMEA_VTG; ///exit + } + }/// end non-first time + break; + case UBLOX_NMEA_VTG: + ///NMEA Messages: VTG + msgID.cfgClassID = (UBLOX_NMEA_VTG >> 8) & 0xFF; + msgID.cfgMsgID = UBLOX_NMEA_VTG & 0xFF; + msgID.rateRatio = 0; + msgID.whatTagert = 4; ///GPS port + msgID.rateRatio = 1; + pt2TmpFunc = &_generateSendCFG_MSG; + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc); + firstFlag = 1; + } else { //after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc)) { + firstFlag = 0; ///reset for next case + CFGIndex = UBLOX_NMEA_GGA; ///exit + } + }/// end non-first time + break; + case UBLOX_NMEA_GGA: + msgID.cfgClassID = (UBLOX_NMEA_GGA >> 8) & 0xFF; + msgID.cfgMsgID = UBLOX_NMEA_GGA & 0xFF; + msgID.rateRatio = 0; + msgID.whatTagert = 4; //GPS port + msgID.rateRatio = 1; + pt2TmpFunc = &_generateSendCFG_MSG; + + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc); + firstFlag = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc)) { + firstFlag = 0; ///reset for next case + CFGIndex = UBLOX_NMEA_RMC; ///exit + } + }/// end non-first time + break; + case UBLOX_NMEA_RMC: + msgID.cfgClassID = (UBLOX_NMEA_RMC >> 8) & 0xFF; + msgID.cfgMsgID = UBLOX_NMEA_RMC & 0xFF; + msgID.rateRatio = 0; + msgID.whatTagert = 4; ///GPS port + + if ( (GPSData->GPSTopLevelConfig & (1 << GPSPORT_SUPPORT420)) == (1 << GPSPORT_SUPPORT420)) { + msgID.rateRatio = 1; + } else { + msgID.rateRatio = 0; + } + + pt2TmpFunc = &_generateSendCFG_MSG; + + if(firstFlag == 0) { + _sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc); + firstFlag = 1; + } else { ///after first time + if (_sendAcknowlegdeProcess(&msgID, + GPSData, + &firstFlag, + pt2TmpFunc)) { + firstFlag = 0; ///reset for next case + CFGIndex = CFG_DONE; ///exit + } + }/// end non-first time + break; + case CFG_DONE: + default: + finish = 1; + CFGIndex = UBLOX_NAV_POSLLH; + break; + } + return finish; +} + +/** **************************************************************************** + * @name getConnectedWithUnknownStatusUbloxGPS performs configuring steps for + * getting connected with an unknown ublox. + * @author Dong An + * @param [in] GPSData - GPS data structure to pull message from + * @retval known version + ******************************************************************************/ +unsigned char getConnectedWithUnknownStatusUbloxGPS(GpsData_t* GPSData) +{ + unsigned char knownVersion = 0; +/* + static unsigned char firstFlag = 0; + ubloxIDTypeSTRUCT msgID; + static unsigned long lastSentoutTime = 0; + long deltaFromLastSendout; + static unsigned char sendCFGFlag = 0; + static unsigned char sendPRTFlag = 0; + static unsigned int squenceProcess = AUTO_BAUD_PROCESS; + + msgID.classID = (UBLOX_MON_VER >> 8) & 0xFF; + msgID.msgID = UBLOX_MON_VER & 0xFF; + + switch (squenceProcess) { + case AUTO_BAUD_PROCESS: + if ( autobaud(GPSData) ) { + if (sendCFGFlag == 0) + squenceProcess = RESTORE_TO_FACTORY; + else + squenceProcess = CONFIG_TO_57600; + } + break; + case RESTORE_TO_FACTORY: + if(sendCFGFlag == 0) { + msgID.classID = (UBLOX_CFG_CFG >> 8) & 0xFF; + msgID.msgID = UBLOX_CFG_CFG & 0xFF; + _generateSendCFG_CFG(&msgID, + GPSData); + sendCFGFlag = 1; + } + + if ( isGpsTxEmpty() ) { + firstFlag = 0;/// reset + squenceProcess = AUTO_BAUD_PROCESS; + } + break; + case CONFIG_TO_57600: + if( GPSData->GPSbaudRate == 3 ) { + squenceProcess = QUERY_VERSION; + firstFlag = 0; + } else { + if( sendPRTFlag == 0 ) { + msgID.classID = (UBLOX_CFG_PRT >> 8) & 0xFF; + msgID.msgID = UBLOX_CFG_PRT & 0xFF; + _generateSendCFG_PRT(&msgID, + GPSData); + sendPRTFlag = 1; + } + + if ( isGpsTxEmpty() ) { + firstFlag = 0;/// reset + sendPRTFlag = 0; + squenceProcess = AUTO_BAUD_PROCESS; + } + } + break; + case QUERY_VERSION: + if(firstFlag == 0 && GPSData->isGPSFWVerKnown != 1) { + flushGPSRecBuf(); ///clean up the buffer + GPSData->isGPSFWVerKnown = 0; + pollUbloxMsg(&msgID, + GPSData); ///reuest Version Message + firstFlag = 1; + lastSentoutTime = GPSData->Timer100Hz10ms; + } else { + deltaFromLastSendout = GPSData->Timer100Hz10ms - lastSentoutTime; + if ( GPSData->isGPSFWVerKnown != 1 ) { + if( deltaFromLastSendout>QUERY_TIMEOUT ) { ///try again + flushGPSRecBuf(); ///clean up the buffer + pollUbloxMsg(&msgID, + GPSData); ///reuest Version Message + lastSentoutTime = GPSData->Timer100Hz10ms; + }/// end of timeout and try again + }/// end of non-version received + else { + firstFlag = 0; + knownVersion = 1; + squenceProcess = AUTO_BAUD_PROCESS; + } + } /// end of non-first + break; + } +*/ + return knownVersion; +} + +/** **************************************************************************** + * @name decodeVersionMsg LOCAL parse ublox FW version message. + * @author Dong An + * @param [in] msg - message to parse + * @param [in] msgLength - length of buffer + * @param [in] GPSData - GPS data structure to pull message from + * @retval N/A + ******************************************************************************/ +void _decodeVersionMsg(char *msg, + unsigned int *msgLength, + GpsData_t *GPSData) +{ + char tmpBuff[10]; + int i; + + for (i = 0; i < 4; i++) + tmpBuff[i] = msg[6 + i]; + tmpBuff[i]='\0'; + + GPSData->UbloxSoftwareVer = atof(tmpBuff); + + if ( GPSData->UbloxSoftwareVer < NAV2_ENABLED_VERSION ) + GPSData->ubloxOldVersion = 0; + else + GPSData->ubloxOldVersion = 1; ///0: old 1:new, CFG-NAV2 allowed +} + +/** **************************************************************************** + * @name _sendAcknowlegdeProcess performs sending CFG message and receiving ACK + * message. + * @author Dong An + * @param [in] cfgID - classs id + * @param [in] GPSData - GPS data structure to pull message from + * @param [in] firstCall - flag + * @param [in] pt2MyFunc - callback function pointer + * @retval acknowledge + ******************************************************************************/ +unsigned char _sendAcknowlegdeProcess(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData, + unsigned char *firstCall, + void (*pt2MyFunc)(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData)) +{ + unsigned char sendAcknowlegde = 0; +/* + static unsigned long lastSentoutTime = 0; + unsigned char firstFlag = *firstCall; + long deltaFromLastSendout; + + if(firstFlag == 0) { + GPSData->ubloxClassID = cfgID->classID;/// register the ID + GPSData->ubloxMsgID = cfgID->msgID; + GPSData->reClassID = 0; ///clear the received ID + GPSData->reMsgID = 0; + if (GPSData->GPSConfigureOK <= 0 ) + flushGPSRecBuf(); ///clean up the buffer only during configuring + + (*pt2MyFunc)(cfgID, GPSData); ///calling the specific CFG-Msg + + lastSentoutTime = GPSData->Timer100Hz10ms; + sendAcknowlegde = 0; + } else { ///after first time + deltaFromLastSendout = GPSData->Timer100Hz10ms - lastSentoutTime; + + if( GPSData->ubloxClassID == GPSData->reClassID && + GPSData->ubloxMsgID == GPSData->reMsgID ){ + GPSData->updateFlagForEachCall &= !(1 << GOT_UBLOX_ACK); ///reset + sendAcknowlegde = 1; + } else { + if( deltaFromLastSendout > QUERY_TIMEOUT ) { ///timeout try again + if (GPSData->GPSConfigureOK <= 0 ) + flushGPSRecBuf(); ///clean up the buffer only during configuring + (*pt2MyFunc)(cfgID, GPSData); ///calling the specific CFG-Msg + lastSentoutTime = GPSData->Timer100Hz10ms; + } + }/// end of "not received ACK" + }/// end if "after first time" send +*/ + return sendAcknowlegde; +} + +/** **************************************************************************** + * @name _generateSendCFG_PRT generate and send out CFG-PRT message. + * @author Dong An + * @param [in] cfgID - classs id + * @param [in] GPSData - GPS data structure to pull message from + * @param [in] firstCall - flag + * @param [in] pt2MyFunc - callback function pointer + * @retval N/A + ******************************************************************************/ +void _generateSendCFG_PRT(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData) +{ + int i; + char tmpMsgPayload[MAX_UBLOX_BODY_LENGTH ]; + ubloxIDTypeSTRUCT tmpMsgID; + + tmpMsgID.classID = cfgID->classID; + tmpMsgID.msgID = cfgID->msgID; + + i = 0; ///offset + tmpMsgPayload[i++] = 2; /// 0:Port ID: 2 for the port connected with DSP + tmpMsgPayload[i++] = 0; /// 1:reserved + tmpMsgPayload[i++] = 0; /// 2:reserved + tmpMsgPayload[i++] = 0; /// 3:reserved + unsigned long tmp = 0; + tmp |=(0x1 << 4) | (0x3 << 6) | (0x4 << 9) | (0x0 << 12); + ///unknown/8 bits /no parity/1 stop/ LSB first/16X over sampling + tmpMsgPayload[i++] = tmp & 0xFF; ///4: + tmpMsgPayload[i++] = (tmp >> 8) & 0xFF; ///5: + tmpMsgPayload[i++] = (tmp >> 16) & 0xFF; ///6: + tmpMsgPayload[i++] = (tmp >> 24) & 0xFF; ///7: + + tmp = 57600; + tmpMsgPayload[i++] = tmp & 0xFF; ///4: + tmpMsgPayload[i++] = (tmp >> 8) & 0xFF; ///5: + tmpMsgPayload[i++] = (tmp >> 16) & 0xFF; ///6: + tmpMsgPayload[i++] = (tmp >> 24) & 0xFF; ///7: + + unsigned short tmp1 = 0x07; + tmpMsgPayload[i++] = tmp1&0xFF; ///4: + tmpMsgPayload[i++] = (tmp1 >> 8) & 0xFF; ///5: + + tmp1 = 0x03; + tmpMsgPayload[i++] = tmp1 & 0xFF; ///4: + tmpMsgPayload[i++] = (tmp1 >> 8) & 0xFF; ///5: + + tmp1 = 0x0; + tmpMsgPayload[i++] = tmp1 & 0xFF; ///4: + tmpMsgPayload[i++] = (tmp1 >> 8) & 0xFF; ///5: + + tmp1 = 0x0; + tmpMsgPayload[i++] = tmp1 & 0xFF; ///4: + tmpMsgPayload[i++] = (tmp1 >> 8) & 0xFF; ///5: + + ubloxGPSMsgSender(&tmpMsgID, + tmpMsgPayload, + &i, + GPSData); +} + +/** **************************************************************************** + * @name _generateSendCFG_CFG generate and send out CFG-CFG message. + * @author Dong An + * @param [in] cfgID - classs id + * @param [in] GPSData - GPS data structure to pull message from + * @param [in] firstCall - flag + * @param [in] pt2MyFunc - callback function pointer + * @retval N/A + ******************************************************************************/ +void _generateSendCFG_CFG(ubloxIDTypeSTRUCT *cfgID, + GpsData_t *GPSData) +{ + int i; + char tmpMsgPayload[MAX_UBLOX_BODY_LENGTH ]; + ubloxIDTypeSTRUCT tmpMsgID; + + tmpMsgID.classID = cfgID->classID; + tmpMsgID.msgID = cfgID->msgID; + + i = 0; ///offset + tmpMsgPayload[i++] = 0xFF; /// 0:Port ID: 2 for the port connected with DSP + tmpMsgPayload[i++] = 0xFF; /// 1:reserved + tmpMsgPayload[i++] = 0xFF; /// 2:reserved + tmpMsgPayload[i++] = 0xFF; /// 3:reserved + + tmpMsgPayload[i++] = 0x0; /// 0:Port ID: 2 for the port connected with DSP + tmpMsgPayload[i++] = 0x0; /// 1:reserved + tmpMsgPayload[i++] = 0x0; /// 2:reserved + tmpMsgPayload[i++] = 0x0; /// 3:reserved + + tmpMsgPayload[i++] = 0xFF; /// 0:Port ID: 2 for the port connected with DSP + tmpMsgPayload[i++] = 0xFF; /// 1:reserved + tmpMsgPayload[i++] = 0xFF; /// 2:reserved + tmpMsgPayload[i++] = 0xFF; /// 3:reserved + + ubloxGPSMsgSender(&tmpMsgID, + tmpMsgPayload, + &i, + GPSData); +} + +#endif // GPS diff --git a/examples/OpenIMU300RI/INS/lib/Core/GPS/src/taskGps.c b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/taskGps.c new file mode 100644 index 0000000..56a62a1 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/GPS/src/taskGps.c @@ -0,0 +1,96 @@ +/** *************************************************************************** + * @file taskGps.c handle GPS data, make sure the GPS handling function gets + * called on a regular basis + * + * 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. + *****************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#ifdef GPS + +#include +#include + +#include "driverGPS.h" +#include "uart.h" +#include "gps.h" +#include "osapi.h" +#include "platformAPI.h" +#include "gpsAPI.h" + +int gpsIdleCnt = 0; + +/** **************************************************************************** + * @name TaskGps + * @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 TaskGps(void const *argument) +{ + int bytesAvailable; + static uint32_t updateHDOP, pollSirfCnt; + + while(gpsSerialChan == UART_CHANNEL_NONE) { + // nothing to do untill port decided + OS_Delay( 1000); + } + + // start out with the DOP high + gGpsDataPtr->HDOP = 50.0; + + initGPSHandler(); + GPSHandler(); + + while (1) + { + + // if (gpsUsedInAlgorithm()) // useGPS so far only external GPS + if (1) // useGPS so far only external GPS + { + bytesAvailable = gpsBytesAvailable(); + if(!bytesAvailable) + { + gpsIdleCnt++; + OS_Delay(20); + continue; + } + GPSHandler(); + uart_BIT(GPS_SERIAL_PORT); // + // or signal other tasks based on these params? + + if(gGpsDataPtr->GPSProtocol == SIRF_BINARY) + { + if (((getSystemTime() / 1000) - updateHDOP) > 600) + { + gGpsDataPtr->HDOP = 50.0; + updateHDOP = (getSystemTime() / 1000); + pollSiRFVersionMsg(); + pollSirfCnt++; + } + } + } + } +} + +#endif // GPS \ No newline at end of file diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/include/FastInvTrigFuncs.h b/examples/OpenIMU300RI/INS/lib/Core/Math/include/FastInvTrigFuncs.h new file mode 100644 index 0000000..00fb494 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/include/FastInvTrigFuncs.h @@ -0,0 +1,20 @@ + + +#ifndef FASTINVTRIGFUNCS_H +#define FASTINVTRIGFUNCS_H + +// Functions that return results in rad +float fatan_rad(float x); +float fatan2_rad(float y, float x); +float fatan_15deg_rad(float x); +float fasin_rad(float x); +float facos_rad(float x); + +// Functions that return results in deg +float fatan_deg(float x); +float fatan2_deg(float y, float x); +float fatan_15deg(float x); +float fasin_deg(float x); +float facos_deg(float x); + +#endif /* FASTINVTRIGFUNCS_H */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/include/MatrixMath.h b/examples/OpenIMU300RI/INS/lib/Core/Math/include/MatrixMath.h new file mode 100644 index 0000000..f4eae23 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/include/MatrixMath.h @@ -0,0 +1,42 @@ +/** *************************************************************************** +* @file matrixMath.h real precision Linear algebra calculation functions +* @author +* @date September, 2008 +* @copyright (c) 2013, 2014 All Rights Reserved. +* @section LICENSE +* 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 +*****************************************************************************/ + +#ifndef MATRIXMATH_H +#define MATRIXMATH_H + +#include // uint8_t, etc. +#include "GlobalConstants.h" // for real + +// Function declarations +uint8_t AxBTranspose(real *A, real *B, uint8_t rowsInA, uint8_t colsInA, uint8_t rowsInB, real *C); +uint8_t AxB(real *A, real *B, uint8_t rowsInA, uint8_t colsInA, uint8_t colsInB, real *C); +uint8_t AxV(real *A, real *V, uint8_t rowsInA, uint8_t colsInA, real *C); +uint8_t APlusB(real *A, real *B, uint8_t rowsInA, uint8_t colsInA, real *C); +uint8_t AMinusB(real *A, real *B, uint8_t rowsInA, uint8_t colsInA, real *C); +uint8_t AxScalar(real *A, real c, uint8_t rowsInA, uint8_t colsInA, real *C); +real DotProduct(real *V1, real *V2, uint8_t rowsInA); +void ForceMatrixSymmetry(real *A, uint8_t rowsInA, uint8_t colsInA); +void ForceMatrixSymmetry_avg(real *A, uint8_t rowsInA, uint8_t colsInA); +void LimitMatrixValues(real* A, real limit, uint8_t rowsInA, uint8_t colsInA); +void LimitValuesAndForceMatrixSymmetry_avg(real* A, real limit, uint8_t rowsInA, uint8_t colsInA); +void LimitValuesAndForceMatrixSymmetry_noAvg(real* A, real limit, uint8_t rowsInA, uint8_t colsInA); + +uint8_t matrixInverse_2x2(real *A, real *AInverse); +uint8_t matrixInverse_3x3(real* A, real* AInverse); + +// Used when quaternion-measurements were computed from accelerometer and magnetometer +// readings. Now update is based on Euler-angles; 4x4 matrix inverse unused. +// int MatrixInverse_4x4(real* A, real* AInverse); + +// RLE implementation +#endif /* MATRIXMATH_H */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/include/QuaternionMath.h b/examples/OpenIMU300RI/INS/lib/Core/Math/include/QuaternionMath.h new file mode 100644 index 0000000..d5211b2 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/include/QuaternionMath.h @@ -0,0 +1,20 @@ +/* + * File: QuaternionMath.h + * Author: joemotyka + * + * Created on May 7, 2016, 5:03 PM + */ + +#ifndef QUATERNIONMATH_H +#define QUATERNIONMATH_H + +#include "GlobalConstants.h" + +BOOL EulerAnglesToQuaternion(real* EulerAngles, real* Quaternion); +BOOL EulerAnglesToR321(real* EulerAngles, real* R321); +BOOL QuatNormalize(real *Quat); +BOOL QuaternionToEulerAngles( real* EulerAngles, real* Quaternion ); +BOOL QuaternionToR321(real* Quaternion, real* R321); + +#endif /* QUATERNIONMATH_H */ + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/include/TransformationMath.h b/examples/OpenIMU300RI/INS/lib/Core/Math/include/TransformationMath.h new file mode 100644 index 0000000..2a01980 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/include/TransformationMath.h @@ -0,0 +1,112 @@ +/* + * File: TransformationMath.h + * Author: joemotyka + * + * Created on May 8, 2016, 12:35 AM + */ + +#ifndef TRANSFORMATIONMATH_H +#define TRANSFORMATIONMATH_H + +#include "GlobalConstants.h" +#include + +#define E_MAJOR 6.378137e+6 // semi major axis a +#define E_MINOR 6.356752314245216e+6 // semi minor axis b +#define E_ECC 0.08181919084255 // first eccentricity +#define E_ECC_SQ 0.006694379990130 // (first eccentricity)^2 +#define E_MAJOR_SQ 4.068063159076900e+013 // E_MAJOR^2 +#define E_MINOR_SQ 4.040829998466191e+013 // E_MINOR2 +#define EP_SQ 4.284131151324081e+004 // (E_MAJOR2 - E_MINOR2)/E_MINOR + +#define E_MINOR_OVER_MAJOR_SQ 0.993305620009870 // E_MAJOR_SQ / E_MINOR_SQ +#define E_MAJOR_OVER_MINOR_SQ 1.006739496742265 // E_MINOR_SQ / E_MAJOR_SQ + +#define E_MAJOR_OVER_MINOR 1.003364089820971 // E_MAJOR / E_MINOR + +#define E_ECC_SQxE_MAJOR 42697.67270710779 + + //============================================================================= + /****************************************************************************** + * @brief Compute unit gravity vector in the body frame from accel measurement. + * Accelerometer measurement = -gravity when there is no linear acceleration. + * If acceleromter measurement is [0; 0; -1], the unit gravity vector should + * be [0; 0; 1].* TRACE: + * @param [in] accel accelerometer measurement. + * @param [out] unitGravityVector unit gravity vector in the body frame. + * @retval + ******************************************************************************/ +void UnitGravity(real *accel, real *unitGravityVector); + +/****************************************************************************** +* @brief Compute pitch and roll angle from unit gravity vector in the body frame. +* @param [in] unitGravityVector unit gravity vector in the body frame. +* @param [out] eulerAngles Euler angles in order [roll pitch yaw]. +* roll is put in eulerAngle[0], and pitch in eulerAngle[1]. +* @retval +******************************************************************************/ +void UnitGravityToEulerAngles(real *unitGravityVector, real* eulerAngles); + +/****************************************************************************** +* @brief Compute yaw angle from unit gravity vector and magnetic measurement. +* The unit gravity vector in the body frame is used to project the magnetic +* measurement onto a perpendicular frame. The projected x and y component of +* the magnetic measurement in this perpendicular frame are used to compute the +* yaw angle. +* TRACE: +* @param [in] unitGravityVector unit gravity vector in the body frame. +* @param [in] magFieldVector magnetic vector in the body frame. +* @retval yaw angle in [rad]. +******************************************************************************/ +real UnitGravityAndMagToYaw(real *unitGravityVector, real *magFieldVector); + +/****************************************************************************** +* @brief Compute yaw angle from pitch, roll and magnetic measurement. +* The pitch and roll angles are used to project the magneticmeasurement onto a +* perpendicular frame. The projected x and y component of the magnetic measurement +* in this perpendicular frame are used to compute the yaw angle. +* TRACE: +* @param [in] roll roll angle in [rad]. +* @param [in] pitch pitch angle in [rad]. +* @param [in] magFieldVector magnetic vector in the body frame. +* @retval yaw angle in [rad]. +******************************************************************************/ +real RollPitchAndMagToYaw(real roll, real pitch, real *magFieldVector); + +/****************************************************************************** +* @brief Limit angle error to be [-180, 180]. +* TRACE: +* @param [in] aErr in [deg]. +* @retval aErr within [-180, 180]deg. +******************************************************************************/ +real AngleErrDeg(real aErr); + +/****************************************************************************** +* @brief Limit angle error to be [-PI, PI]. +* TRACE: +* @param [in] aErr in [rad]. +* @retval aErr within [-2*PI, 2*PI]. +******************************************************************************/ +real AngleErrRad(real aErr); +// +BOOL LLA_To_R_EinN( double* llaRad, real* R_EinN); +BOOL LLA_To_R_NinE( double* llaRad, real* R_NinE); +BOOL ECEF_To_Base( double* rECEF_Init, + double* rECEF , + real* R_NinE, + real* dr_N); +BOOL LLA_To_ECEF( double* lla_Rad, double* ecef_m); +BOOL PosNED_To_PosECEF( real* r_N, double* rECEF_Init, + real* R_NinE, double* rECEF ); +BOOL ECEF_To_LLA( double* llaDeg, double* ecef_m ); + +BOOL VelECEF_To_VelNED( double* LLA, real* VelECEF, real* VelNED ); + + +void printMtx(float *a, int m, int n); +void printVec(float *v, int n); + +#endif /* TRANSFORMATIONMATH_H */ + + + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/include/VectorMath.h b/examples/OpenIMU300RI/INS/lib/Core/Math/include/VectorMath.h new file mode 100644 index 0000000..1dd09e3 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/include/VectorMath.h @@ -0,0 +1,38 @@ +/* +* File: VectorMath.h +* Author: joemotyka +* +* Created on May 7, 2016, 12:50 AM +*/ + +#ifndef VECTORMATH_H +#define VECTORMATH_H + +#include "GlobalConstants.h" + +// Declare the function definitions for real +void VectorNormalize(real *vectorIn, real *vectorOut); +real VectorMag(real *vectorIn); +void VectorCrossProduct(real *vect1, real *vect2, real *vectOut); +real VectorDotProduct(real *vect1, real *vect2); + +/****************************************************************************** +* @brief Compute the variance of a vector +* @param [in] a pointer to the vector. +* @param [in] am mean value of the vector. +* @param [in] n number of elements in the vector. +* @param [out] +* @retval variance of the vector +******************************************************************************/ +real vecVar(real *a, real am, int n); + +/****************************************************************************** +* @brief vector coross product +* @param [in] a 3D vector +* @param [in] b 3D vector +* @param [out] c c = axb +* return: +******************************************************************************/ +void cross(real* a, real* b, real* axb); + +#endif /* VECTORMATH_H */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/include/arm_common_tables.h b/examples/OpenIMU300RI/INS/lib/Core/Math/include/arm_common_tables.h new file mode 100644 index 0000000..8742a56 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/include/arm_common_tables.h @@ -0,0 +1,136 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2014 ARM Limited. All rights reserved. +* +* $Date: 19. October 2015 +* $Revision: V.1.4.5 a +* +* Project: CMSIS DSP Library +* Title: arm_common_tables.h +* +* Description: This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions +* +* Target Processor: Cortex-M4/Cortex-M3 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* -------------------------------------------------------------------- */ + +#ifndef _ARM_COMMON_TABLES_H +#define _ARM_COMMON_TABLES_H + +#include "arm_math.h" + +extern const uint16_t armBitRevTable[1024]; +extern const q15_t armRecipTableQ15[64]; +extern const q31_t armRecipTableQ31[64]; +/* extern const q31_t realCoefAQ31[1024]; */ +/* extern const q31_t realCoefBQ31[1024]; */ +extern const float32_t twiddleCoef_16[32]; +extern const float32_t twiddleCoef_32[64]; +extern const float32_t twiddleCoef_64[128]; +extern const float32_t twiddleCoef_128[256]; +extern const float32_t twiddleCoef_256[512]; +extern const float32_t twiddleCoef_512[1024]; +extern const float32_t twiddleCoef_1024[2048]; +extern const float32_t twiddleCoef_2048[4096]; +extern const float32_t twiddleCoef_4096[8192]; +#define twiddleCoef twiddleCoef_4096 +extern const q31_t twiddleCoef_16_q31[24]; +extern const q31_t twiddleCoef_32_q31[48]; +extern const q31_t twiddleCoef_64_q31[96]; +extern const q31_t twiddleCoef_128_q31[192]; +extern const q31_t twiddleCoef_256_q31[384]; +extern const q31_t twiddleCoef_512_q31[768]; +extern const q31_t twiddleCoef_1024_q31[1536]; +extern const q31_t twiddleCoef_2048_q31[3072]; +extern const q31_t twiddleCoef_4096_q31[6144]; +extern const q15_t twiddleCoef_16_q15[24]; +extern const q15_t twiddleCoef_32_q15[48]; +extern const q15_t twiddleCoef_64_q15[96]; +extern const q15_t twiddleCoef_128_q15[192]; +extern const q15_t twiddleCoef_256_q15[384]; +extern const q15_t twiddleCoef_512_q15[768]; +extern const q15_t twiddleCoef_1024_q15[1536]; +extern const q15_t twiddleCoef_2048_q15[3072]; +extern const q15_t twiddleCoef_4096_q15[6144]; +extern const float32_t twiddleCoef_rfft_32[32]; +extern const float32_t twiddleCoef_rfft_64[64]; +extern const float32_t twiddleCoef_rfft_128[128]; +extern const float32_t twiddleCoef_rfft_256[256]; +extern const float32_t twiddleCoef_rfft_512[512]; +extern const float32_t twiddleCoef_rfft_1024[1024]; +extern const float32_t twiddleCoef_rfft_2048[2048]; +extern const float32_t twiddleCoef_rfft_4096[4096]; + + +/* floating-point bit reversal tables */ +#define ARMBITREVINDEXTABLE__16_TABLE_LENGTH ((uint16_t)20 ) +#define ARMBITREVINDEXTABLE__32_TABLE_LENGTH ((uint16_t)48 ) +#define ARMBITREVINDEXTABLE__64_TABLE_LENGTH ((uint16_t)56 ) +#define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208 ) +#define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440 ) +#define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448 ) +#define ARMBITREVINDEXTABLE1024_TABLE_LENGTH ((uint16_t)1800) +#define ARMBITREVINDEXTABLE2048_TABLE_LENGTH ((uint16_t)3808) +#define ARMBITREVINDEXTABLE4096_TABLE_LENGTH ((uint16_t)4032) + +extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH]; + +/* fixed-point bit reversal tables */ +#define ARMBITREVINDEXTABLE_FIXED___16_TABLE_LENGTH ((uint16_t)12 ) +#define ARMBITREVINDEXTABLE_FIXED___32_TABLE_LENGTH ((uint16_t)24 ) +#define ARMBITREVINDEXTABLE_FIXED___64_TABLE_LENGTH ((uint16_t)56 ) +#define ARMBITREVINDEXTABLE_FIXED__128_TABLE_LENGTH ((uint16_t)112 ) +#define ARMBITREVINDEXTABLE_FIXED__256_TABLE_LENGTH ((uint16_t)240 ) +#define ARMBITREVINDEXTABLE_FIXED__512_TABLE_LENGTH ((uint16_t)480 ) +#define ARMBITREVINDEXTABLE_FIXED_1024_TABLE_LENGTH ((uint16_t)992 ) +#define ARMBITREVINDEXTABLE_FIXED_2048_TABLE_LENGTH ((uint16_t)1984) +#define ARMBITREVINDEXTABLE_FIXED_4096_TABLE_LENGTH ((uint16_t)4032) + +extern const uint16_t armBitRevIndexTable_fixed_16[ARMBITREVINDEXTABLE_FIXED___16_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable_fixed_32[ARMBITREVINDEXTABLE_FIXED___32_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable_fixed_64[ARMBITREVINDEXTABLE_FIXED___64_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable_fixed_128[ARMBITREVINDEXTABLE_FIXED__128_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable_fixed_256[ARMBITREVINDEXTABLE_FIXED__256_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable_fixed_512[ARMBITREVINDEXTABLE_FIXED__512_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable_fixed_1024[ARMBITREVINDEXTABLE_FIXED_1024_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable_fixed_2048[ARMBITREVINDEXTABLE_FIXED_2048_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable_fixed_4096[ARMBITREVINDEXTABLE_FIXED_4096_TABLE_LENGTH]; + +/* Tables for Fast Math Sine and Cosine */ +extern const float32_t sinTable_f32[FAST_MATH_TABLE_SIZE + 1]; +extern const q31_t sinTable_q31[FAST_MATH_TABLE_SIZE + 1]; +extern const q15_t sinTable_q15[FAST_MATH_TABLE_SIZE + 1]; + +#endif /* ARM_COMMON_TABLES_H */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/include/arm_const_structs.h b/examples/OpenIMU300RI/INS/lib/Core/Math/include/arm_const_structs.h new file mode 100644 index 0000000..726d06e --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/include/arm_const_structs.h @@ -0,0 +1,79 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2014 ARM Limited. All rights reserved. +* +* $Date: 19. March 2015 +* $Revision: V.1.4.5 +* +* Project: CMSIS DSP Library +* Title: arm_const_structs.h +* +* Description: This file has constant structs that are initialized for +* user convenience. For example, some can be given as +* arguments to the arm_cfft_f32() function. +* +* Target Processor: Cortex-M4/Cortex-M3 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* -------------------------------------------------------------------- */ + +#ifndef _ARM_CONST_STRUCTS_H +#define _ARM_CONST_STRUCTS_H + +#include "arm_math.h" +#include "arm_common_tables.h" + + extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len16; + extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len32; + extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len64; + extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len128; + extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len256; + extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len512; + extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024; + extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048; + extern const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096; + + extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len16; + extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len32; + extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len64; + extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len128; + extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len256; + extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len512; + extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len1024; + extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len2048; + extern const arm_cfft_instance_q31 arm_cfft_sR_q31_len4096; + + extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len16; + extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len32; + extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len64; + extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len128; + extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len256; + extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len512; + extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len1024; + extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len2048; + extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len4096; + +#endif diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/include/arm_math.h b/examples/OpenIMU300RI/INS/lib/Core/Math/include/arm_math.h new file mode 100644 index 0000000..cb996c8 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/include/arm_math.h @@ -0,0 +1,7156 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2015 ARM Limited. All rights reserved. +* +* $Date: 20. October 2015 +* $Revision: V1.4.5 b +* +* Project: CMSIS DSP Library +* Title: arm_math.h +* +* Description: Public header file for CMSIS DSP Library +* +* Target Processor: Cortex-M7/Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------- */ + +/** + \mainpage CMSIS DSP Software Library + * + * Introduction + * ------------ + * + * This user manual describes the CMSIS DSP software library, + * a suite of common signal processing functions for use on Cortex-M processor based devices. + * + * The library is divided into a number of functions each covering a specific category: + * - Basic math functions + * - Fast math functions + * - Complex math functions + * - Filters + * - Matrix functions + * - Transforms + * - Motor control functions + * - Statistical functions + * - Support functions + * - Interpolation functions + * + * The library has separate functions for operating on 8-bit integers, 16-bit integers, + * 32-bit integer and 32-bit floating-point values. + * + * Using the Library + * ------------ + * + * The library installer contains prebuilt versions of the libraries in the Lib folder. + * - arm_cortexM7lfdp_math.lib (Little endian and Double Precision Floating Point Unit on Cortex-M7) + * - arm_cortexM7bfdp_math.lib (Big endian and Double Precision Floating Point Unit on Cortex-M7) + * - arm_cortexM7lfsp_math.lib (Little endian and Single Precision Floating Point Unit on Cortex-M7) + * - arm_cortexM7bfsp_math.lib (Big endian and Single Precision Floating Point Unit on Cortex-M7) + * - arm_cortexM7l_math.lib (Little endian on Cortex-M7) + * - arm_cortexM7b_math.lib (Big endian on Cortex-M7) + * - arm_cortexM4lf_math.lib (Little endian and Floating Point Unit on Cortex-M4) + * - arm_cortexM4bf_math.lib (Big endian and Floating Point Unit on Cortex-M4) + * - arm_cortexM4l_math.lib (Little endian on Cortex-M4) + * - arm_cortexM4b_math.lib (Big endian on Cortex-M4) + * - arm_cortexM3l_math.lib (Little endian on Cortex-M3) + * - arm_cortexM3b_math.lib (Big endian on Cortex-M3) + * - arm_cortexM0l_math.lib (Little endian on Cortex-M0 / CortexM0+) + * - arm_cortexM0b_math.lib (Big endian on Cortex-M0 / CortexM0+) + * + * The library functions are declared in the public file arm_math.h which is placed in the Include folder. + * Simply include this file and link the appropriate library in the application and begin calling the library functions. The Library supports single + * public header file arm_math.h for Cortex-M7/M4/M3/M0/M0+ with little endian and big endian. Same header file will be used for floating point unit(FPU) variants. + * Define the appropriate pre processor MACRO ARM_MATH_CM7 or ARM_MATH_CM4 or ARM_MATH_CM3 or + * ARM_MATH_CM0 or ARM_MATH_CM0PLUS depending on the target processor in the application. + * + * Examples + * -------- + * + * The library ships with a number of examples which demonstrate how to use the library functions. + * + * Toolchain Support + * ------------ + * + * The library has been developed and tested with MDK-ARM version 5.14.0.0 + * The library is being tested in GCC and IAR toolchains and updates on this activity will be made available shortly. + * + * Building the Library + * ------------ + * + * The library installer contains a project file to re build libraries on MDK-ARM Tool chain in the CMSIS\\DSP_Lib\\Source\\ARM folder. + * - arm_cortexM_math.uvprojx + * + * + * The libraries can be built by opening the arm_cortexM_math.uvprojx project in MDK-ARM, selecting a specific target, and defining the optional pre processor MACROs detailed above. + * + * Pre-processor Macros + * ------------ + * + * Each library project have differant pre-processor macros. + * + * - UNALIGNED_SUPPORT_DISABLE: + * + * Define macro UNALIGNED_SUPPORT_DISABLE, If the silicon does not support unaligned memory access + * + * - ARM_MATH_BIG_ENDIAN: + * + * Define macro ARM_MATH_BIG_ENDIAN to build the library for big endian targets. By default library builds for little endian targets. + * + * - ARM_MATH_MATRIX_CHECK: + * + * Define macro ARM_MATH_MATRIX_CHECK for checking on the input and output sizes of matrices + * + * - ARM_MATH_ROUNDING: + * + * Define macro ARM_MATH_ROUNDING for rounding on support functions + * + * - ARM_MATH_CMx: + * + * Define macro ARM_MATH_CM4 for building the library on Cortex-M4 target, ARM_MATH_CM3 for building library on Cortex-M3 target + * and ARM_MATH_CM0 for building library on Cortex-M0 target, ARM_MATH_CM0PLUS for building library on Cortex-M0+ target, and + * ARM_MATH_CM7 for building the library on cortex-M7. + * + * - __FPU_PRESENT: + * + * Initialize macro __FPU_PRESENT = 1 when building on FPU supported Targets. Enable this macro for M4bf and M4lf libraries + * + *
+ * CMSIS-DSP in ARM::CMSIS Pack + * ----------------------------- + * + * The following files relevant to CMSIS-DSP are present in the ARM::CMSIS Pack directories: + * |File/Folder |Content | + * |------------------------------|------------------------------------------------------------------------| + * |\b CMSIS\\Documentation\\DSP | This documentation | + * |\b CMSIS\\DSP_Lib | Software license agreement (license.txt) | + * |\b CMSIS\\DSP_Lib\\Examples | Example projects demonstrating the usage of the library functions | + * |\b CMSIS\\DSP_Lib\\Source | Source files for rebuilding the library | + * + *
+ * Revision History of CMSIS-DSP + * ------------ + * Please refer to \ref ChangeLog_pg. + * + * Copyright Notice + * ------------ + * + * Copyright (C) 2010-2015 ARM Limited. All rights reserved. + */ + + +/** + * @defgroup groupMath Basic Math Functions + */ + +/** + * @defgroup groupFastMath Fast Math Functions + * This set of functions provides a fast approximation to sine, cosine, and square root. + * As compared to most of the other functions in the CMSIS math library, the fast math functions + * operate on individual values and not arrays. + * There are separate functions for Q15, Q31, and floating-point data. + * + */ + +/** + * @defgroup groupCmplxMath Complex Math Functions + * This set of functions operates on complex data vectors. + * The data in the complex arrays is stored in an interleaved fashion + * (real, imag, real, imag, ...). + * In the API functions, the number of samples in a complex array refers + * to the number of complex values; the array contains twice this number of + * real values. + */ + +/** + * @defgroup groupFilters Filtering Functions + */ + +/** + * @defgroup groupMatrix Matrix Functions + * + * This set of functions provides basic matrix math operations. + * The functions operate on matrix data structures. For example, + * the type + * definition for the floating-point matrix structure is shown + * below: + *
+ *     typedef struct
+ *     {
+ *       uint16_t numRows;     // number of rows of the matrix.
+ *       uint16_t numCols;     // number of columns of the matrix.
+ *       float32_t *pData;     // points to the data of the matrix.
+ *     } arm_matrix_instance_f32;
+ * 
+ * There are similar definitions for Q15 and Q31 data types. + * + * The structure specifies the size of the matrix and then points to + * an array of data. The array is of size numRows X numCols + * and the values are arranged in row order. That is, the + * matrix element (i, j) is stored at: + *
+ *     pData[i*numCols + j]
+ * 
+ * + * \par Init Functions + * There is an associated initialization function for each type of matrix + * data structure. + * The initialization function sets the values of the internal structure fields. + * Refer to the function arm_mat_init_f32(), arm_mat_init_q31() + * and arm_mat_init_q15() for floating-point, Q31 and Q15 types, respectively. + * + * \par + * Use of the initialization function is optional. However, if initialization function is used + * then the instance structure cannot be placed into a const data section. + * To place the instance structure in a const data + * section, manually initialize the data structure. For example: + *
+ * arm_matrix_instance_f32 S = {nRows, nColumns, pData};
+ * arm_matrix_instance_q31 S = {nRows, nColumns, pData};
+ * arm_matrix_instance_q15 S = {nRows, nColumns, pData};
+ * 
+ * where nRows specifies the number of rows, nColumns + * specifies the number of columns, and pData points to the + * data array. + * + * \par Size Checking + * By default all of the matrix functions perform size checking on the input and + * output matrices. For example, the matrix addition function verifies that the + * two input matrices and the output matrix all have the same number of rows and + * columns. If the size check fails the functions return: + *
+ *     ARM_MATH_SIZE_MISMATCH
+ * 
+ * Otherwise the functions return + *
+ *     ARM_MATH_SUCCESS
+ * 
+ * There is some overhead associated with this matrix size checking. + * The matrix size checking is enabled via the \#define + *
+ *     ARM_MATH_MATRIX_CHECK
+ * 
+ * within the library project settings. By default this macro is defined + * and size checking is enabled. By changing the project settings and + * undefining this macro size checking is eliminated and the functions + * run a bit faster. With size checking disabled the functions always + * return ARM_MATH_SUCCESS. + */ + +/** + * @defgroup groupTransforms Transform Functions + */ + +/** + * @defgroup groupController Controller Functions + */ + +/** + * @defgroup groupStats Statistics Functions + */ +/** + * @defgroup groupSupport Support Functions + */ + +/** + * @defgroup groupInterpolation Interpolation Functions + * These functions perform 1- and 2-dimensional interpolation of data. + * Linear interpolation is used for 1-dimensional data and + * bilinear interpolation is used for 2-dimensional data. + */ + +/** + * @defgroup groupExamples Examples + */ +#ifndef _ARM_MATH_H +#define _ARM_MATH_H + +#include "stdint.h" + +/* ignore some GCC warnings */ +#if defined ( __GNUC__ ) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wsign-conversion" +#pragma GCC diagnostic ignored "-Wconversion" +#pragma GCC diagnostic ignored "-Wunused-parameter" +#endif + +#define __CMSIS_GENERIC /* disable NVIC and Systick functions */ + +#if defined(ARM_MATH_CM7) + #include "core_cm7.h" +#elif defined (ARM_MATH_CM4) + #include "core_cm4.h" +#elif defined (ARM_MATH_CM3) + #include "core_cm3.h" +#elif defined (ARM_MATH_CM0) + #include "core_cm0.h" + #define ARM_MATH_CM0_FAMILY +#elif defined (ARM_MATH_CM0PLUS) + #include "core_cm0plus.h" + #define ARM_MATH_CM0_FAMILY +#else + #error "Define according the used Cortex core ARM_MATH_CM7, ARM_MATH_CM4, ARM_MATH_CM3, ARM_MATH_CM0PLUS or ARM_MATH_CM0" +#endif + +#undef __CMSIS_GENERIC /* enable NVIC and Systick functions */ +#include "string.h" +#include "math.h" +#ifdef __cplusplus +extern "C" +{ +#endif + + + /** + * @brief Macros required for reciprocal calculation in Normalized LMS + */ + +#define DELTA_Q31 (0x100) +#define DELTA_Q15 0x5 +#define INDEX_MASK 0x0000003F +#ifndef PI +#define PI 3.14159265358979f +#endif + + /** + * @brief Macros required for SINE and COSINE Fast math approximations + */ + +#define FAST_MATH_TABLE_SIZE 512 +#define FAST_MATH_Q31_SHIFT (32 - 10) +#define FAST_MATH_Q15_SHIFT (16 - 10) +#define CONTROLLER_Q31_SHIFT (32 - 9) +#define TABLE_SIZE 256 +#define TABLE_SPACING_Q31 0x400000 +#define TABLE_SPACING_Q15 0x80 + + /** + * @brief Macros required for SINE and COSINE Controller functions + */ + /* 1.31(q31) Fixed value of 2/360 */ + /* -1 to +1 is divided into 360 values so total spacing is (2/360) */ +#define INPUT_SPACING 0xB60B61 + + /** + * @brief Macro for Unaligned Support + */ +#ifndef UNALIGNED_SUPPORT_DISABLE + #define ALIGN4 +#else + #if defined (__GNUC__) + #define ALIGN4 __attribute__((aligned(4))) + #else + #define ALIGN4 __align(4) + #endif +#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ + + /** + * @brief Error status returned by some functions in the library. + */ + + typedef enum + { + ARM_MATH_SUCCESS = 0, /**< No error */ + ARM_MATH_ARGUMENT_ERROR = -1, /**< One or more arguments are incorrect */ + ARM_MATH_LENGTH_ERROR = -2, /**< Length of data buffer is incorrect */ + ARM_MATH_SIZE_MISMATCH = -3, /**< Size of matrices is not compatible with the operation. */ + ARM_MATH_NANINF = -4, /**< Not-a-number (NaN) or infinity is generated */ + ARM_MATH_SINGULAR = -5, /**< Generated by matrix inversion if the input matrix is singular and cannot be inverted. */ + ARM_MATH_TEST_FAILURE = -6 /**< Test Failed */ + } arm_status; + + /** + * @brief 8-bit fractional data type in 1.7 format. + */ + typedef int8_t q7_t; + + /** + * @brief 16-bit fractional data type in 1.15 format. + */ + typedef int16_t q15_t; + + /** + * @brief 32-bit fractional data type in 1.31 format. + */ + typedef int32_t q31_t; + + /** + * @brief 64-bit fractional data type in 1.63 format. + */ + typedef int64_t q63_t; + + /** + * @brief 32-bit floating-point type definition. + */ + typedef float float32_t; + + /** + * @brief 64-bit floating-point type definition. + */ + typedef double float64_t; + + /** + * @brief definition to read/write two 16 bit values. + */ +#if defined __CC_ARM + #define __SIMD32_TYPE int32_t __packed + #define CMSIS_UNUSED __attribute__((unused)) + +#elif defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) + #define __SIMD32_TYPE int32_t + #define CMSIS_UNUSED __attribute__((unused)) + +#elif defined __GNUC__ + #define __SIMD32_TYPE int32_t + #define CMSIS_UNUSED __attribute__((unused)) + +#elif defined __ICCARM__ + #define __SIMD32_TYPE int32_t __packed + #define CMSIS_UNUSED + +#elif defined __CSMC__ + #define __SIMD32_TYPE int32_t + #define CMSIS_UNUSED + +#elif defined __TASKING__ + #define __SIMD32_TYPE __unaligned int32_t + #define CMSIS_UNUSED + +#else + #error Unknown compiler +#endif + +#define __SIMD32(addr) (*(__SIMD32_TYPE **) & (addr)) +#define __SIMD32_CONST(addr) ((__SIMD32_TYPE *)(addr)) +#define _SIMD32_OFFSET(addr) (*(__SIMD32_TYPE *) (addr)) +#define __SIMD64(addr) (*(int64_t **) & (addr)) + +#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) + /** + * @brief definition to pack two 16 bit values. + */ +#define __PKHBT(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) << 0) & (int32_t)0x0000FFFF) | \ + (((int32_t)(ARG2) << ARG3) & (int32_t)0xFFFF0000) ) +#define __PKHTB(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) << 0) & (int32_t)0xFFFF0000) | \ + (((int32_t)(ARG2) >> ARG3) & (int32_t)0x0000FFFF) ) + +#endif + + + /** + * @brief definition to pack four 8 bit values. + */ +#ifndef ARM_MATH_BIG_ENDIAN + +#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v0) << 0) & (int32_t)0x000000FF) | \ + (((int32_t)(v1) << 8) & (int32_t)0x0000FF00) | \ + (((int32_t)(v2) << 16) & (int32_t)0x00FF0000) | \ + (((int32_t)(v3) << 24) & (int32_t)0xFF000000) ) +#else + +#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v3) << 0) & (int32_t)0x000000FF) | \ + (((int32_t)(v2) << 8) & (int32_t)0x0000FF00) | \ + (((int32_t)(v1) << 16) & (int32_t)0x00FF0000) | \ + (((int32_t)(v0) << 24) & (int32_t)0xFF000000) ) + +#endif + + + /** + * @brief Clips Q63 to Q31 values. + */ + static __INLINE q31_t clip_q63_to_q31( + q63_t x) + { + return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ? + ((0x7FFFFFFF ^ ((q31_t) (x >> 63)))) : (q31_t) x; + } + + /** + * @brief Clips Q63 to Q15 values. + */ + static __INLINE q15_t clip_q63_to_q15( + q63_t x) + { + return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ? + ((0x7FFF ^ ((q15_t) (x >> 63)))) : (q15_t) (x >> 15); + } + + /** + * @brief Clips Q31 to Q7 values. + */ + static __INLINE q7_t clip_q31_to_q7( + q31_t x) + { + return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ? + ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x; + } + + /** + * @brief Clips Q31 to Q15 values. + */ + static __INLINE q15_t clip_q31_to_q15( + q31_t x) + { + return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ? + ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x; + } + + /** + * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format. + */ + + static __INLINE q63_t mult32x64( + q63_t x, + q31_t y) + { + return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) + + (((q63_t) (x >> 32) * y))); + } + +/* + #if defined (ARM_MATH_CM0_FAMILY) && defined ( __CC_ARM ) + #define __CLZ __clz + #endif + */ +/* note: function can be removed when all toolchain support __CLZ for Cortex-M0 */ +#if defined (ARM_MATH_CM0_FAMILY) && ((defined (__ICCARM__)) ) + static __INLINE uint32_t __CLZ( + q31_t data); + + static __INLINE uint32_t __CLZ( + q31_t data) + { + uint32_t count = 0; + uint32_t mask = 0x80000000; + + while((data & mask) == 0) + { + count += 1u; + mask = mask >> 1u; + } + + return (count); + } +#endif + + /** + * @brief Function to Calculates 1/in (reciprocal) value of Q31 Data type. + */ + + static __INLINE uint32_t arm_recip_q31( + q31_t in, + q31_t * dst, + q31_t * pRecipTable) + { + q31_t out; + uint32_t tempVal; + uint32_t index, i; + uint32_t signBits; + + if(in > 0) + { + signBits = ((uint32_t) (__CLZ( in) - 1)); + } + else + { + signBits = ((uint32_t) (__CLZ(-in) - 1)); + } + + /* Convert input sample to 1.31 format */ + in = (in << signBits); + + /* calculation of index for initial approximated Val */ + index = (uint32_t)(in >> 24); + index = (index & INDEX_MASK); + + /* 1.31 with exp 1 */ + out = pRecipTable[index]; + + /* calculation of reciprocal value */ + /* running approximation for two iterations */ + for (i = 0u; i < 2u; i++) + { + tempVal = (uint32_t) (((q63_t) in * out) >> 31); + tempVal = 0x7FFFFFFFu - tempVal; + /* 1.31 with exp 1 */ + /* out = (q31_t) (((q63_t) out * tempVal) >> 30); */ + out = clip_q63_to_q31(((q63_t) out * tempVal) >> 30); + } + + /* write output */ + *dst = out; + + /* return num of signbits of out = 1/in value */ + return (signBits + 1u); + } + + + /** + * @brief Function to Calculates 1/in (reciprocal) value of Q15 Data type. + */ + static __INLINE uint32_t arm_recip_q15( + q15_t in, + q15_t * dst, + q15_t * pRecipTable) + { + q15_t out = 0; + uint32_t tempVal = 0; + uint32_t index = 0, i = 0; + uint32_t signBits = 0; + + if(in > 0) + { + signBits = ((uint32_t)(__CLZ( in) - 17)); + } + else + { + signBits = ((uint32_t)(__CLZ(-in) - 17)); + } + + /* Convert input sample to 1.15 format */ + in = (in << signBits); + + /* calculation of index for initial approximated Val */ + index = (uint32_t)(in >> 8); + index = (index & INDEX_MASK); + + /* 1.15 with exp 1 */ + out = pRecipTable[index]; + + /* calculation of reciprocal value */ + /* running approximation for two iterations */ + for (i = 0u; i < 2u; i++) + { + tempVal = (uint32_t) (((q31_t) in * out) >> 15); + tempVal = 0x7FFFu - tempVal; + /* 1.15 with exp 1 */ + out = (q15_t) (((q31_t) out * tempVal) >> 14); + /* out = clip_q31_to_q15(((q31_t) out * tempVal) >> 14); */ + } + + /* write output */ + *dst = out; + + /* return num of signbits of out = 1/in value */ + return (signBits + 1); + } + + + /* + * @brief C custom defined intrinisic function for only M0 processors + */ +#if defined(ARM_MATH_CM0_FAMILY) + static __INLINE q31_t __SSAT( + q31_t x, + uint32_t y) + { + int32_t posMax, negMin; + uint32_t i; + + posMax = 1; + for (i = 0; i < (y - 1); i++) + { + posMax = posMax * 2; + } + + if(x > 0) + { + posMax = (posMax - 1); + + if(x > posMax) + { + x = posMax; + } + } + else + { + negMin = -posMax; + + if(x < negMin) + { + x = negMin; + } + } + return (x); + } +#endif /* end of ARM_MATH_CM0_FAMILY */ + + + /* + * @brief C custom defined intrinsic function for M3 and M0 processors + */ +#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) + + /* + * @brief C custom defined QADD8 for M3 and M0 processors + */ + static __INLINE uint32_t __QADD8( + uint32_t x, + uint32_t y) + { + q31_t r, s, t, u; + + r = __SSAT(((((q31_t)x << 24) >> 24) + (((q31_t)y << 24) >> 24)), 8) & (int32_t)0x000000FF; + s = __SSAT(((((q31_t)x << 16) >> 24) + (((q31_t)y << 16) >> 24)), 8) & (int32_t)0x000000FF; + t = __SSAT(((((q31_t)x << 8) >> 24) + (((q31_t)y << 8) >> 24)), 8) & (int32_t)0x000000FF; + u = __SSAT(((((q31_t)x ) >> 24) + (((q31_t)y ) >> 24)), 8) & (int32_t)0x000000FF; + + return ((uint32_t)((u << 24) | (t << 16) | (s << 8) | (r ))); + } + + + /* + * @brief C custom defined QSUB8 for M3 and M0 processors + */ + static __INLINE uint32_t __QSUB8( + uint32_t x, + uint32_t y) + { + q31_t r, s, t, u; + + r = __SSAT(((((q31_t)x << 24) >> 24) - (((q31_t)y << 24) >> 24)), 8) & (int32_t)0x000000FF; + s = __SSAT(((((q31_t)x << 16) >> 24) - (((q31_t)y << 16) >> 24)), 8) & (int32_t)0x000000FF; + t = __SSAT(((((q31_t)x << 8) >> 24) - (((q31_t)y << 8) >> 24)), 8) & (int32_t)0x000000FF; + u = __SSAT(((((q31_t)x ) >> 24) - (((q31_t)y ) >> 24)), 8) & (int32_t)0x000000FF; + + return ((uint32_t)((u << 24) | (t << 16) | (s << 8) | (r ))); + } + + + /* + * @brief C custom defined QADD16 for M3 and M0 processors + */ + static __INLINE uint32_t __QADD16( + uint32_t x, + uint32_t y) + { +/* q31_t r, s; without initialisation 'arm_offset_q15 test' fails but 'intrinsic' tests pass! for armCC */ + q31_t r = 0, s = 0; + + r = __SSAT(((((q31_t)x << 16) >> 16) + (((q31_t)y << 16) >> 16)), 16) & (int32_t)0x0000FFFF; + s = __SSAT(((((q31_t)x ) >> 16) + (((q31_t)y ) >> 16)), 16) & (int32_t)0x0000FFFF; + + return ((uint32_t)((s << 16) | (r ))); + } + + + /* + * @brief C custom defined SHADD16 for M3 and M0 processors + */ + static __INLINE uint32_t __SHADD16( + uint32_t x, + uint32_t y) + { + q31_t r, s; + + r = (((((q31_t)x << 16) >> 16) + (((q31_t)y << 16) >> 16)) >> 1) & (int32_t)0x0000FFFF; + s = (((((q31_t)x ) >> 16) + (((q31_t)y ) >> 16)) >> 1) & (int32_t)0x0000FFFF; + + return ((uint32_t)((s << 16) | (r ))); + } + + + /* + * @brief C custom defined QSUB16 for M3 and M0 processors + */ + static __INLINE uint32_t __QSUB16( + uint32_t x, + uint32_t y) + { + q31_t r, s; + + r = __SSAT(((((q31_t)x << 16) >> 16) - (((q31_t)y << 16) >> 16)), 16) & (int32_t)0x0000FFFF; + s = __SSAT(((((q31_t)x ) >> 16) - (((q31_t)y ) >> 16)), 16) & (int32_t)0x0000FFFF; + + return ((uint32_t)((s << 16) | (r ))); + } + + + /* + * @brief C custom defined SHSUB16 for M3 and M0 processors + */ + static __INLINE uint32_t __SHSUB16( + uint32_t x, + uint32_t y) + { + q31_t r, s; + + r = (((((q31_t)x << 16) >> 16) - (((q31_t)y << 16) >> 16)) >> 1) & (int32_t)0x0000FFFF; + s = (((((q31_t)x ) >> 16) - (((q31_t)y ) >> 16)) >> 1) & (int32_t)0x0000FFFF; + + return ((uint32_t)((s << 16) | (r ))); + } + + + /* + * @brief C custom defined QASX for M3 and M0 processors + */ + static __INLINE uint32_t __QASX( + uint32_t x, + uint32_t y) + { + q31_t r, s; + + r = __SSAT(((((q31_t)x << 16) >> 16) - (((q31_t)y ) >> 16)), 16) & (int32_t)0x0000FFFF; + s = __SSAT(((((q31_t)x ) >> 16) + (((q31_t)y << 16) >> 16)), 16) & (int32_t)0x0000FFFF; + + return ((uint32_t)((s << 16) | (r ))); + } + + + /* + * @brief C custom defined SHASX for M3 and M0 processors + */ + static __INLINE uint32_t __SHASX( + uint32_t x, + uint32_t y) + { + q31_t r, s; + + r = (((((q31_t)x << 16) >> 16) - (((q31_t)y ) >> 16)) >> 1) & (int32_t)0x0000FFFF; + s = (((((q31_t)x ) >> 16) + (((q31_t)y << 16) >> 16)) >> 1) & (int32_t)0x0000FFFF; + + return ((uint32_t)((s << 16) | (r ))); + } + + + /* + * @brief C custom defined QSAX for M3 and M0 processors + */ + static __INLINE uint32_t __QSAX( + uint32_t x, + uint32_t y) + { + q31_t r, s; + + r = __SSAT(((((q31_t)x << 16) >> 16) + (((q31_t)y ) >> 16)), 16) & (int32_t)0x0000FFFF; + s = __SSAT(((((q31_t)x ) >> 16) - (((q31_t)y << 16) >> 16)), 16) & (int32_t)0x0000FFFF; + + return ((uint32_t)((s << 16) | (r ))); + } + + + /* + * @brief C custom defined SHSAX for M3 and M0 processors + */ + static __INLINE uint32_t __SHSAX( + uint32_t x, + uint32_t y) + { + q31_t r, s; + + r = (((((q31_t)x << 16) >> 16) + (((q31_t)y ) >> 16)) >> 1) & (int32_t)0x0000FFFF; + s = (((((q31_t)x ) >> 16) - (((q31_t)y << 16) >> 16)) >> 1) & (int32_t)0x0000FFFF; + + return ((uint32_t)((s << 16) | (r ))); + } + + + /* + * @brief C custom defined SMUSDX for M3 and M0 processors + */ + static __INLINE uint32_t __SMUSDX( + uint32_t x, + uint32_t y) + { + return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y ) >> 16)) - + ((((q31_t)x ) >> 16) * (((q31_t)y << 16) >> 16)) )); + } + + /* + * @brief C custom defined SMUADX for M3 and M0 processors + */ + static __INLINE uint32_t __SMUADX( + uint32_t x, + uint32_t y) + { + return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y ) >> 16)) + + ((((q31_t)x ) >> 16) * (((q31_t)y << 16) >> 16)) )); + } + + + /* + * @brief C custom defined QADD for M3 and M0 processors + */ + static __INLINE int32_t __QADD( + int32_t x, + int32_t y) + { + return ((int32_t)(clip_q63_to_q31((q63_t)x + (q31_t)y))); + } + + + /* + * @brief C custom defined QSUB for M3 and M0 processors + */ + static __INLINE int32_t __QSUB( + int32_t x, + int32_t y) + { + return ((int32_t)(clip_q63_to_q31((q63_t)x - (q31_t)y))); + } + + + /* + * @brief C custom defined SMLAD for M3 and M0 processors + */ + static __INLINE uint32_t __SMLAD( + uint32_t x, + uint32_t y, + uint32_t sum) + { + return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y << 16) >> 16)) + + ((((q31_t)x ) >> 16) * (((q31_t)y ) >> 16)) + + ( ((q31_t)sum ) ) )); + } + + + /* + * @brief C custom defined SMLADX for M3 and M0 processors + */ + static __INLINE uint32_t __SMLADX( + uint32_t x, + uint32_t y, + uint32_t sum) + { + return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y ) >> 16)) + + ((((q31_t)x ) >> 16) * (((q31_t)y << 16) >> 16)) + + ( ((q31_t)sum ) ) )); + } + + + /* + * @brief C custom defined SMLSDX for M3 and M0 processors + */ + static __INLINE uint32_t __SMLSDX( + uint32_t x, + uint32_t y, + uint32_t sum) + { + return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y ) >> 16)) - + ((((q31_t)x ) >> 16) * (((q31_t)y << 16) >> 16)) + + ( ((q31_t)sum ) ) )); + } + + + /* + * @brief C custom defined SMLALD for M3 and M0 processors + */ + static __INLINE uint64_t __SMLALD( + uint32_t x, + uint32_t y, + uint64_t sum) + { +/* return (sum + ((q15_t) (x >> 16) * (q15_t) (y >> 16)) + ((q15_t) x * (q15_t) y)); */ + return ((uint64_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y << 16) >> 16)) + + ((((q31_t)x ) >> 16) * (((q31_t)y ) >> 16)) + + ( ((q63_t)sum ) ) )); + } + + + /* + * @brief C custom defined SMLALDX for M3 and M0 processors + */ + static __INLINE uint64_t __SMLALDX( + uint32_t x, + uint32_t y, + uint64_t sum) + { +/* return (sum + ((q15_t) (x >> 16) * (q15_t) y)) + ((q15_t) x * (q15_t) (y >> 16)); */ + return ((uint64_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y ) >> 16)) + + ((((q31_t)x ) >> 16) * (((q31_t)y << 16) >> 16)) + + ( ((q63_t)sum ) ) )); + } + + + /* + * @brief C custom defined SMUAD for M3 and M0 processors + */ + static __INLINE uint32_t __SMUAD( + uint32_t x, + uint32_t y) + { + return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y << 16) >> 16)) + + ((((q31_t)x ) >> 16) * (((q31_t)y ) >> 16)) )); + } + + + /* + * @brief C custom defined SMUSD for M3 and M0 processors + */ + static __INLINE uint32_t __SMUSD( + uint32_t x, + uint32_t y) + { + return ((uint32_t)(((((q31_t)x << 16) >> 16) * (((q31_t)y << 16) >> 16)) - + ((((q31_t)x ) >> 16) * (((q31_t)y ) >> 16)) )); + } + + + /* + * @brief C custom defined SXTB16 for M3 and M0 processors + */ + static __INLINE uint32_t __SXTB16( + uint32_t x) + { + return ((uint32_t)(((((q31_t)x << 24) >> 24) & (q31_t)0x0000FFFF) | + ((((q31_t)x << 8) >> 8) & (q31_t)0xFFFF0000) )); + } + +#endif /* defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) */ + + + /** + * @brief Instance structure for the Q7 FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of filter coefficients in the filter. */ + q7_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + } arm_fir_instance_q7; + + /** + * @brief Instance structure for the Q15 FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of filter coefficients in the filter. */ + q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + } arm_fir_instance_q15; + + /** + * @brief Instance structure for the Q31 FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of filter coefficients in the filter. */ + q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + } arm_fir_instance_q31; + + /** + * @brief Instance structure for the floating-point FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of filter coefficients in the filter. */ + float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + } arm_fir_instance_f32; + + + /** + * @brief Processing function for the Q7 FIR filter. + * @param[in] S points to an instance of the Q7 FIR filter structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_fir_q7( + const arm_fir_instance_q7 * S, + q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q7 FIR filter. + * @param[in,out] S points to an instance of the Q7 FIR structure. + * @param[in] numTaps Number of filter coefficients in the filter. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] blockSize number of samples that are processed. + */ + void arm_fir_init_q7( + arm_fir_instance_q7 * S, + uint16_t numTaps, + q7_t * pCoeffs, + q7_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q15 FIR filter. + * @param[in] S points to an instance of the Q15 FIR structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_fir_q15( + const arm_fir_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Processing function for the fast Q15 FIR filter for Cortex-M3 and Cortex-M4. + * @param[in] S points to an instance of the Q15 FIR filter structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_fir_fast_q15( + const arm_fir_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q15 FIR filter. + * @param[in,out] S points to an instance of the Q15 FIR filter structure. + * @param[in] numTaps Number of filter coefficients in the filter. Must be even and greater than or equal to 4. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] blockSize number of samples that are processed at a time. + * @return The function returns ARM_MATH_SUCCESS if initialization was successful or ARM_MATH_ARGUMENT_ERROR if + * numTaps is not a supported value. + */ + arm_status arm_fir_init_q15( + arm_fir_instance_q15 * S, + uint16_t numTaps, + q15_t * pCoeffs, + q15_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q31 FIR filter. + * @param[in] S points to an instance of the Q31 FIR filter structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_fir_q31( + const arm_fir_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Processing function for the fast Q31 FIR filter for Cortex-M3 and Cortex-M4. + * @param[in] S points to an instance of the Q31 FIR structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_fir_fast_q31( + const arm_fir_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q31 FIR filter. + * @param[in,out] S points to an instance of the Q31 FIR structure. + * @param[in] numTaps Number of filter coefficients in the filter. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] blockSize number of samples that are processed at a time. + */ + void arm_fir_init_q31( + arm_fir_instance_q31 * S, + uint16_t numTaps, + q31_t * pCoeffs, + q31_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the floating-point FIR filter. + * @param[in] S points to an instance of the floating-point FIR structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_fir_f32( + const arm_fir_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the floating-point FIR filter. + * @param[in,out] S points to an instance of the floating-point FIR filter structure. + * @param[in] numTaps Number of filter coefficients in the filter. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] blockSize number of samples that are processed at a time. + */ + void arm_fir_init_f32( + arm_fir_instance_f32 * S, + uint16_t numTaps, + float32_t * pCoeffs, + float32_t * pState, + uint32_t blockSize); + + + /** + * @brief Instance structure for the Q15 Biquad cascade filter. + */ + typedef struct + { + int8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + q15_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */ + q15_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */ + int8_t postShift; /**< Additional shift, in bits, applied to each output sample. */ + } arm_biquad_casd_df1_inst_q15; + + /** + * @brief Instance structure for the Q31 Biquad cascade filter. + */ + typedef struct + { + uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + q31_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */ + q31_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */ + uint8_t postShift; /**< Additional shift, in bits, applied to each output sample. */ + } arm_biquad_casd_df1_inst_q31; + + /** + * @brief Instance structure for the floating-point Biquad cascade filter. + */ + typedef struct + { + uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + float32_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */ + float32_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */ + } arm_biquad_casd_df1_inst_f32; + + + /** + * @brief Processing function for the Q15 Biquad cascade filter. + * @param[in] S points to an instance of the Q15 Biquad cascade structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_biquad_cascade_df1_q15( + const arm_biquad_casd_df1_inst_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q15 Biquad cascade filter. + * @param[in,out] S points to an instance of the Q15 Biquad cascade structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format + */ + void arm_biquad_cascade_df1_init_q15( + arm_biquad_casd_df1_inst_q15 * S, + uint8_t numStages, + q15_t * pCoeffs, + q15_t * pState, + int8_t postShift); + + + /** + * @brief Fast but less precise processing function for the Q15 Biquad cascade filter for Cortex-M3 and Cortex-M4. + * @param[in] S points to an instance of the Q15 Biquad cascade structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_biquad_cascade_df1_fast_q15( + const arm_biquad_casd_df1_inst_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q31 Biquad cascade filter + * @param[in] S points to an instance of the Q31 Biquad cascade structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_biquad_cascade_df1_q31( + const arm_biquad_casd_df1_inst_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Fast but less precise processing function for the Q31 Biquad cascade filter for Cortex-M3 and Cortex-M4. + * @param[in] S points to an instance of the Q31 Biquad cascade structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_biquad_cascade_df1_fast_q31( + const arm_biquad_casd_df1_inst_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q31 Biquad cascade filter. + * @param[in,out] S points to an instance of the Q31 Biquad cascade structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format + */ + void arm_biquad_cascade_df1_init_q31( + arm_biquad_casd_df1_inst_q31 * S, + uint8_t numStages, + q31_t * pCoeffs, + q31_t * pState, + int8_t postShift); + + + /** + * @brief Processing function for the floating-point Biquad cascade filter. + * @param[in] S points to an instance of the floating-point Biquad cascade structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_biquad_cascade_df1_f32( + const arm_biquad_casd_df1_inst_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the floating-point Biquad cascade filter. + * @param[in,out] S points to an instance of the floating-point Biquad cascade structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + */ + void arm_biquad_cascade_df1_init_f32( + arm_biquad_casd_df1_inst_f32 * S, + uint8_t numStages, + float32_t * pCoeffs, + float32_t * pState); + + + /** + * @brief Instance structure for the floating-point matrix structure. + */ + typedef struct + { + uint16_t numRows; /**< number of rows of the matrix. */ + uint16_t numCols; /**< number of columns of the matrix. */ + float32_t *pData; /**< points to the data of the matrix. */ + } arm_matrix_instance_f32; + + + /** + * @brief Instance structure for the floating-point matrix structure. + */ + typedef struct + { + uint16_t numRows; /**< number of rows of the matrix. */ + uint16_t numCols; /**< number of columns of the matrix. */ + float64_t *pData; /**< points to the data of the matrix. */ + } arm_matrix_instance_f64; + + /** + * @brief Instance structure for the Q15 matrix structure. + */ + typedef struct + { + uint16_t numRows; /**< number of rows of the matrix. */ + uint16_t numCols; /**< number of columns of the matrix. */ + q15_t *pData; /**< points to the data of the matrix. */ + } arm_matrix_instance_q15; + + /** + * @brief Instance structure for the Q31 matrix structure. + */ + typedef struct + { + uint16_t numRows; /**< number of rows of the matrix. */ + uint16_t numCols; /**< number of columns of the matrix. */ + q31_t *pData; /**< points to the data of the matrix. */ + } arm_matrix_instance_q31; + + + /** + * @brief Floating-point matrix addition. + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_add_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst); + + + /** + * @brief Q15 matrix addition. + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_add_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst); + + + /** + * @brief Q31 matrix addition. + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_add_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst); + + + /** + * @brief Floating-point, complex, matrix multiplication. + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_cmplx_mult_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst); + + + /** + * @brief Q15, complex, matrix multiplication. + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_cmplx_mult_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst, + q15_t * pScratch); + + + /** + * @brief Q31, complex, matrix multiplication. + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_cmplx_mult_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst); + + + /** + * @brief Floating-point matrix transpose. + * @param[in] pSrc points to the input matrix + * @param[out] pDst points to the output matrix + * @return The function returns either ARM_MATH_SIZE_MISMATCH + * or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_trans_f32( + const arm_matrix_instance_f32 * pSrc, + arm_matrix_instance_f32 * pDst); + + + /** + * @brief Q15 matrix transpose. + * @param[in] pSrc points to the input matrix + * @param[out] pDst points to the output matrix + * @return The function returns either ARM_MATH_SIZE_MISMATCH + * or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_trans_q15( + const arm_matrix_instance_q15 * pSrc, + arm_matrix_instance_q15 * pDst); + + + /** + * @brief Q31 matrix transpose. + * @param[in] pSrc points to the input matrix + * @param[out] pDst points to the output matrix + * @return The function returns either ARM_MATH_SIZE_MISMATCH + * or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_trans_q31( + const arm_matrix_instance_q31 * pSrc, + arm_matrix_instance_q31 * pDst); + + + /** + * @brief Floating-point matrix multiplication + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_mult_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst); + + + /** + * @brief Q15 matrix multiplication + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @param[in] pState points to the array for storing intermediate results + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_mult_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst, + q15_t * pState); + + + /** + * @brief Q15 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4 + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @param[in] pState points to the array for storing intermediate results + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_mult_fast_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst, + q15_t * pState); + + + /** + * @brief Q31 matrix multiplication + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_mult_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst); + + + /** + * @brief Q31 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4 + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_mult_fast_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst); + + + /** + * @brief Floating-point matrix subtraction + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_sub_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst); + + + /** + * @brief Q15 matrix subtraction + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_sub_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst); + + + /** + * @brief Q31 matrix subtraction + * @param[in] pSrcA points to the first input matrix structure + * @param[in] pSrcB points to the second input matrix structure + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_sub_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst); + + + /** + * @brief Floating-point matrix scaling. + * @param[in] pSrc points to the input matrix + * @param[in] scale scale factor + * @param[out] pDst points to the output matrix + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_scale_f32( + const arm_matrix_instance_f32 * pSrc, + float32_t scale, + arm_matrix_instance_f32 * pDst); + + + /** + * @brief Q15 matrix scaling. + * @param[in] pSrc points to input matrix + * @param[in] scaleFract fractional portion of the scale factor + * @param[in] shift number of bits to shift the result by + * @param[out] pDst points to output matrix + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_scale_q15( + const arm_matrix_instance_q15 * pSrc, + q15_t scaleFract, + int32_t shift, + arm_matrix_instance_q15 * pDst); + + + /** + * @brief Q31 matrix scaling. + * @param[in] pSrc points to input matrix + * @param[in] scaleFract fractional portion of the scale factor + * @param[in] shift number of bits to shift the result by + * @param[out] pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + arm_status arm_mat_scale_q31( + const arm_matrix_instance_q31 * pSrc, + q31_t scaleFract, + int32_t shift, + arm_matrix_instance_q31 * pDst); + + + /** + * @brief Q31 matrix initialization. + * @param[in,out] S points to an instance of the floating-point matrix structure. + * @param[in] nRows number of rows in the matrix. + * @param[in] nColumns number of columns in the matrix. + * @param[in] pData points to the matrix data array. + */ + void arm_mat_init_q31( + arm_matrix_instance_q31 * S, + uint16_t nRows, + uint16_t nColumns, + q31_t * pData); + + + /** + * @brief Q15 matrix initialization. + * @param[in,out] S points to an instance of the floating-point matrix structure. + * @param[in] nRows number of rows in the matrix. + * @param[in] nColumns number of columns in the matrix. + * @param[in] pData points to the matrix data array. + */ + void arm_mat_init_q15( + arm_matrix_instance_q15 * S, + uint16_t nRows, + uint16_t nColumns, + q15_t * pData); + + + /** + * @brief Floating-point matrix initialization. + * @param[in,out] S points to an instance of the floating-point matrix structure. + * @param[in] nRows number of rows in the matrix. + * @param[in] nColumns number of columns in the matrix. + * @param[in] pData points to the matrix data array. + */ + void arm_mat_init_f32( + arm_matrix_instance_f32 * S, + uint16_t nRows, + uint16_t nColumns, + float32_t * pData); + + + + /** + * @brief Instance structure for the Q15 PID Control. + */ + typedef struct + { + q15_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */ +#ifdef ARM_MATH_CM0_FAMILY + q15_t A1; + q15_t A2; +#else + q31_t A1; /**< The derived gain A1 = -Kp - 2Kd | Kd.*/ +#endif + q15_t state[3]; /**< The state array of length 3. */ + q15_t Kp; /**< The proportional gain. */ + q15_t Ki; /**< The integral gain. */ + q15_t Kd; /**< The derivative gain. */ + } arm_pid_instance_q15; + + /** + * @brief Instance structure for the Q31 PID Control. + */ + typedef struct + { + q31_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */ + q31_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */ + q31_t A2; /**< The derived gain, A2 = Kd . */ + q31_t state[3]; /**< The state array of length 3. */ + q31_t Kp; /**< The proportional gain. */ + q31_t Ki; /**< The integral gain. */ + q31_t Kd; /**< The derivative gain. */ + } arm_pid_instance_q31; + + /** + * @brief Instance structure for the floating-point PID Control. + */ + typedef struct + { + float32_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */ + float32_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */ + float32_t A2; /**< The derived gain, A2 = Kd . */ + float32_t state[3]; /**< The state array of length 3. */ + float32_t Kp; /**< The proportional gain. */ + float32_t Ki; /**< The integral gain. */ + float32_t Kd; /**< The derivative gain. */ + } arm_pid_instance_f32; + + + + /** + * @brief Initialization function for the floating-point PID Control. + * @param[in,out] S points to an instance of the PID structure. + * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state. + */ + void arm_pid_init_f32( + arm_pid_instance_f32 * S, + int32_t resetStateFlag); + + + /** + * @brief Reset function for the floating-point PID Control. + * @param[in,out] S is an instance of the floating-point PID Control structure + */ + void arm_pid_reset_f32( + arm_pid_instance_f32 * S); + + + /** + * @brief Initialization function for the Q31 PID Control. + * @param[in,out] S points to an instance of the Q15 PID structure. + * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state. + */ + void arm_pid_init_q31( + arm_pid_instance_q31 * S, + int32_t resetStateFlag); + + + /** + * @brief Reset function for the Q31 PID Control. + * @param[in,out] S points to an instance of the Q31 PID Control structure + */ + + void arm_pid_reset_q31( + arm_pid_instance_q31 * S); + + + /** + * @brief Initialization function for the Q15 PID Control. + * @param[in,out] S points to an instance of the Q15 PID structure. + * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state. + */ + void arm_pid_init_q15( + arm_pid_instance_q15 * S, + int32_t resetStateFlag); + + + /** + * @brief Reset function for the Q15 PID Control. + * @param[in,out] S points to an instance of the q15 PID Control structure + */ + void arm_pid_reset_q15( + arm_pid_instance_q15 * S); + + + /** + * @brief Instance structure for the floating-point Linear Interpolate function. + */ + typedef struct + { + uint32_t nValues; /**< nValues */ + float32_t x1; /**< x1 */ + float32_t xSpacing; /**< xSpacing */ + float32_t *pYData; /**< pointer to the table of Y values */ + } arm_linear_interp_instance_f32; + + /** + * @brief Instance structure for the floating-point bilinear interpolation function. + */ + typedef struct + { + uint16_t numRows; /**< number of rows in the data table. */ + uint16_t numCols; /**< number of columns in the data table. */ + float32_t *pData; /**< points to the data table. */ + } arm_bilinear_interp_instance_f32; + + /** + * @brief Instance structure for the Q31 bilinear interpolation function. + */ + typedef struct + { + uint16_t numRows; /**< number of rows in the data table. */ + uint16_t numCols; /**< number of columns in the data table. */ + q31_t *pData; /**< points to the data table. */ + } arm_bilinear_interp_instance_q31; + + /** + * @brief Instance structure for the Q15 bilinear interpolation function. + */ + typedef struct + { + uint16_t numRows; /**< number of rows in the data table. */ + uint16_t numCols; /**< number of columns in the data table. */ + q15_t *pData; /**< points to the data table. */ + } arm_bilinear_interp_instance_q15; + + /** + * @brief Instance structure for the Q15 bilinear interpolation function. + */ + typedef struct + { + uint16_t numRows; /**< number of rows in the data table. */ + uint16_t numCols; /**< number of columns in the data table. */ + q7_t *pData; /**< points to the data table. */ + } arm_bilinear_interp_instance_q7; + + + /** + * @brief Q7 vector multiplication. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_mult_q7( + q7_t * pSrcA, + q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Q15 vector multiplication. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_mult_q15( + q15_t * pSrcA, + q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Q31 vector multiplication. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_mult_q31( + q31_t * pSrcA, + q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Floating-point vector multiplication. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_mult_f32( + float32_t * pSrcA, + float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Instance structure for the Q15 CFFT/CIFFT function. + */ + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + q15_t *pTwiddle; /**< points to the Sin twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + } arm_cfft_radix2_instance_q15; + +/* Deprecated */ + arm_status arm_cfft_radix2_init_q15( + arm_cfft_radix2_instance_q15 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + +/* Deprecated */ + void arm_cfft_radix2_q15( + const arm_cfft_radix2_instance_q15 * S, + q15_t * pSrc); + + + /** + * @brief Instance structure for the Q15 CFFT/CIFFT function. + */ + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + q15_t *pTwiddle; /**< points to the twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + } arm_cfft_radix4_instance_q15; + +/* Deprecated */ + arm_status arm_cfft_radix4_init_q15( + arm_cfft_radix4_instance_q15 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + +/* Deprecated */ + void arm_cfft_radix4_q15( + const arm_cfft_radix4_instance_q15 * S, + q15_t * pSrc); + + /** + * @brief Instance structure for the Radix-2 Q31 CFFT/CIFFT function. + */ + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + q31_t *pTwiddle; /**< points to the Twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + } arm_cfft_radix2_instance_q31; + +/* Deprecated */ + arm_status arm_cfft_radix2_init_q31( + arm_cfft_radix2_instance_q31 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + +/* Deprecated */ + void arm_cfft_radix2_q31( + const arm_cfft_radix2_instance_q31 * S, + q31_t * pSrc); + + /** + * @brief Instance structure for the Q31 CFFT/CIFFT function. + */ + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + q31_t *pTwiddle; /**< points to the twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + } arm_cfft_radix4_instance_q31; + +/* Deprecated */ + void arm_cfft_radix4_q31( + const arm_cfft_radix4_instance_q31 * S, + q31_t * pSrc); + +/* Deprecated */ + arm_status arm_cfft_radix4_init_q31( + arm_cfft_radix4_instance_q31 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + + /** + * @brief Instance structure for the floating-point CFFT/CIFFT function. + */ + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + float32_t *pTwiddle; /**< points to the Twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + float32_t onebyfftLen; /**< value of 1/fftLen. */ + } arm_cfft_radix2_instance_f32; + +/* Deprecated */ + arm_status arm_cfft_radix2_init_f32( + arm_cfft_radix2_instance_f32 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + +/* Deprecated */ + void arm_cfft_radix2_f32( + const arm_cfft_radix2_instance_f32 * S, + float32_t * pSrc); + + /** + * @brief Instance structure for the floating-point CFFT/CIFFT function. + */ + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + float32_t *pTwiddle; /**< points to the Twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + float32_t onebyfftLen; /**< value of 1/fftLen. */ + } arm_cfft_radix4_instance_f32; + +/* Deprecated */ + arm_status arm_cfft_radix4_init_f32( + arm_cfft_radix4_instance_f32 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + +/* Deprecated */ + void arm_cfft_radix4_f32( + const arm_cfft_radix4_instance_f32 * S, + float32_t * pSrc); + + /** + * @brief Instance structure for the fixed-point CFFT/CIFFT function. + */ + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + const q15_t *pTwiddle; /**< points to the Twiddle factor table. */ + const uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t bitRevLength; /**< bit reversal table length. */ + } arm_cfft_instance_q15; + +void arm_cfft_q15( + const arm_cfft_instance_q15 * S, + q15_t * p1, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + + /** + * @brief Instance structure for the fixed-point CFFT/CIFFT function. + */ + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + const q31_t *pTwiddle; /**< points to the Twiddle factor table. */ + const uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t bitRevLength; /**< bit reversal table length. */ + } arm_cfft_instance_q31; + +void arm_cfft_q31( + const arm_cfft_instance_q31 * S, + q31_t * p1, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + + /** + * @brief Instance structure for the floating-point CFFT/CIFFT function. + */ + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + const float32_t *pTwiddle; /**< points to the Twiddle factor table. */ + const uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t bitRevLength; /**< bit reversal table length. */ + } arm_cfft_instance_f32; + + void arm_cfft_f32( + const arm_cfft_instance_f32 * S, + float32_t * p1, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + + /** + * @brief Instance structure for the Q15 RFFT/RIFFT function. + */ + typedef struct + { + uint32_t fftLenReal; /**< length of the real FFT. */ + uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */ + uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */ + uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + q15_t *pTwiddleAReal; /**< points to the real twiddle factor table. */ + q15_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */ + const arm_cfft_instance_q15 *pCfft; /**< points to the complex FFT instance. */ + } arm_rfft_instance_q15; + + arm_status arm_rfft_init_q15( + arm_rfft_instance_q15 * S, + uint32_t fftLenReal, + uint32_t ifftFlagR, + uint32_t bitReverseFlag); + + void arm_rfft_q15( + const arm_rfft_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst); + + /** + * @brief Instance structure for the Q31 RFFT/RIFFT function. + */ + typedef struct + { + uint32_t fftLenReal; /**< length of the real FFT. */ + uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */ + uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */ + uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + q31_t *pTwiddleAReal; /**< points to the real twiddle factor table. */ + q31_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */ + const arm_cfft_instance_q31 *pCfft; /**< points to the complex FFT instance. */ + } arm_rfft_instance_q31; + + arm_status arm_rfft_init_q31( + arm_rfft_instance_q31 * S, + uint32_t fftLenReal, + uint32_t ifftFlagR, + uint32_t bitReverseFlag); + + void arm_rfft_q31( + const arm_rfft_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst); + + /** + * @brief Instance structure for the floating-point RFFT/RIFFT function. + */ + typedef struct + { + uint32_t fftLenReal; /**< length of the real FFT. */ + uint16_t fftLenBy2; /**< length of the complex FFT. */ + uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */ + uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */ + uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + float32_t *pTwiddleAReal; /**< points to the real twiddle factor table. */ + float32_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */ + arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */ + } arm_rfft_instance_f32; + + arm_status arm_rfft_init_f32( + arm_rfft_instance_f32 * S, + arm_cfft_radix4_instance_f32 * S_CFFT, + uint32_t fftLenReal, + uint32_t ifftFlagR, + uint32_t bitReverseFlag); + + void arm_rfft_f32( + const arm_rfft_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst); + + /** + * @brief Instance structure for the floating-point RFFT/RIFFT function. + */ +typedef struct + { + arm_cfft_instance_f32 Sint; /**< Internal CFFT structure. */ + uint16_t fftLenRFFT; /**< length of the real sequence */ + float32_t * pTwiddleRFFT; /**< Twiddle factors real stage */ + } arm_rfft_fast_instance_f32 ; + +arm_status arm_rfft_fast_init_f32 ( + arm_rfft_fast_instance_f32 * S, + uint16_t fftLen); + +void arm_rfft_fast_f32( + arm_rfft_fast_instance_f32 * S, + float32_t * p, float32_t * pOut, + uint8_t ifftFlag); + + /** + * @brief Instance structure for the floating-point DCT4/IDCT4 function. + */ + typedef struct + { + uint16_t N; /**< length of the DCT4. */ + uint16_t Nby2; /**< half of the length of the DCT4. */ + float32_t normalize; /**< normalizing factor. */ + float32_t *pTwiddle; /**< points to the twiddle factor table. */ + float32_t *pCosFactor; /**< points to the cosFactor table. */ + arm_rfft_instance_f32 *pRfft; /**< points to the real FFT instance. */ + arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */ + } arm_dct4_instance_f32; + + + /** + * @brief Initialization function for the floating-point DCT4/IDCT4. + * @param[in,out] S points to an instance of floating-point DCT4/IDCT4 structure. + * @param[in] S_RFFT points to an instance of floating-point RFFT/RIFFT structure. + * @param[in] S_CFFT points to an instance of floating-point CFFT/CIFFT structure. + * @param[in] N length of the DCT4. + * @param[in] Nby2 half of the length of the DCT4. + * @param[in] normalize normalizing factor. + * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if fftLenReal is not a supported transform length. + */ + arm_status arm_dct4_init_f32( + arm_dct4_instance_f32 * S, + arm_rfft_instance_f32 * S_RFFT, + arm_cfft_radix4_instance_f32 * S_CFFT, + uint16_t N, + uint16_t Nby2, + float32_t normalize); + + + /** + * @brief Processing function for the floating-point DCT4/IDCT4. + * @param[in] S points to an instance of the floating-point DCT4/IDCT4 structure. + * @param[in] pState points to state buffer. + * @param[in,out] pInlineBuffer points to the in-place input and output buffer. + */ + void arm_dct4_f32( + const arm_dct4_instance_f32 * S, + float32_t * pState, + float32_t * pInlineBuffer); + + + /** + * @brief Instance structure for the Q31 DCT4/IDCT4 function. + */ + typedef struct + { + uint16_t N; /**< length of the DCT4. */ + uint16_t Nby2; /**< half of the length of the DCT4. */ + q31_t normalize; /**< normalizing factor. */ + q31_t *pTwiddle; /**< points to the twiddle factor table. */ + q31_t *pCosFactor; /**< points to the cosFactor table. */ + arm_rfft_instance_q31 *pRfft; /**< points to the real FFT instance. */ + arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */ + } arm_dct4_instance_q31; + + + /** + * @brief Initialization function for the Q31 DCT4/IDCT4. + * @param[in,out] S points to an instance of Q31 DCT4/IDCT4 structure. + * @param[in] S_RFFT points to an instance of Q31 RFFT/RIFFT structure + * @param[in] S_CFFT points to an instance of Q31 CFFT/CIFFT structure + * @param[in] N length of the DCT4. + * @param[in] Nby2 half of the length of the DCT4. + * @param[in] normalize normalizing factor. + * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if N is not a supported transform length. + */ + arm_status arm_dct4_init_q31( + arm_dct4_instance_q31 * S, + arm_rfft_instance_q31 * S_RFFT, + arm_cfft_radix4_instance_q31 * S_CFFT, + uint16_t N, + uint16_t Nby2, + q31_t normalize); + + + /** + * @brief Processing function for the Q31 DCT4/IDCT4. + * @param[in] S points to an instance of the Q31 DCT4 structure. + * @param[in] pState points to state buffer. + * @param[in,out] pInlineBuffer points to the in-place input and output buffer. + */ + void arm_dct4_q31( + const arm_dct4_instance_q31 * S, + q31_t * pState, + q31_t * pInlineBuffer); + + + /** + * @brief Instance structure for the Q15 DCT4/IDCT4 function. + */ + typedef struct + { + uint16_t N; /**< length of the DCT4. */ + uint16_t Nby2; /**< half of the length of the DCT4. */ + q15_t normalize; /**< normalizing factor. */ + q15_t *pTwiddle; /**< points to the twiddle factor table. */ + q15_t *pCosFactor; /**< points to the cosFactor table. */ + arm_rfft_instance_q15 *pRfft; /**< points to the real FFT instance. */ + arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */ + } arm_dct4_instance_q15; + + + /** + * @brief Initialization function for the Q15 DCT4/IDCT4. + * @param[in,out] S points to an instance of Q15 DCT4/IDCT4 structure. + * @param[in] S_RFFT points to an instance of Q15 RFFT/RIFFT structure. + * @param[in] S_CFFT points to an instance of Q15 CFFT/CIFFT structure. + * @param[in] N length of the DCT4. + * @param[in] Nby2 half of the length of the DCT4. + * @param[in] normalize normalizing factor. + * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if N is not a supported transform length. + */ + arm_status arm_dct4_init_q15( + arm_dct4_instance_q15 * S, + arm_rfft_instance_q15 * S_RFFT, + arm_cfft_radix4_instance_q15 * S_CFFT, + uint16_t N, + uint16_t Nby2, + q15_t normalize); + + + /** + * @brief Processing function for the Q15 DCT4/IDCT4. + * @param[in] S points to an instance of the Q15 DCT4 structure. + * @param[in] pState points to state buffer. + * @param[in,out] pInlineBuffer points to the in-place input and output buffer. + */ + void arm_dct4_q15( + const arm_dct4_instance_q15 * S, + q15_t * pState, + q15_t * pInlineBuffer); + + + /** + * @brief Floating-point vector addition. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_add_f32( + float32_t * pSrcA, + float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Q7 vector addition. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_add_q7( + q7_t * pSrcA, + q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Q15 vector addition. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_add_q15( + q15_t * pSrcA, + q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Q31 vector addition. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_add_q31( + q31_t * pSrcA, + q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Floating-point vector subtraction. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_sub_f32( + float32_t * pSrcA, + float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Q7 vector subtraction. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_sub_q7( + q7_t * pSrcA, + q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Q15 vector subtraction. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_sub_q15( + q15_t * pSrcA, + q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Q31 vector subtraction. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in each vector + */ + void arm_sub_q31( + q31_t * pSrcA, + q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Multiplies a floating-point vector by a scalar. + * @param[in] pSrc points to the input vector + * @param[in] scale scale factor to be applied + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_scale_f32( + float32_t * pSrc, + float32_t scale, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Multiplies a Q7 vector by a scalar. + * @param[in] pSrc points to the input vector + * @param[in] scaleFract fractional portion of the scale value + * @param[in] shift number of bits to shift the result by + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_scale_q7( + q7_t * pSrc, + q7_t scaleFract, + int8_t shift, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Multiplies a Q15 vector by a scalar. + * @param[in] pSrc points to the input vector + * @param[in] scaleFract fractional portion of the scale value + * @param[in] shift number of bits to shift the result by + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_scale_q15( + q15_t * pSrc, + q15_t scaleFract, + int8_t shift, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Multiplies a Q31 vector by a scalar. + * @param[in] pSrc points to the input vector + * @param[in] scaleFract fractional portion of the scale value + * @param[in] shift number of bits to shift the result by + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_scale_q31( + q31_t * pSrc, + q31_t scaleFract, + int8_t shift, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Q7 vector absolute value. + * @param[in] pSrc points to the input buffer + * @param[out] pDst points to the output buffer + * @param[in] blockSize number of samples in each vector + */ + void arm_abs_q7( + q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Floating-point vector absolute value. + * @param[in] pSrc points to the input buffer + * @param[out] pDst points to the output buffer + * @param[in] blockSize number of samples in each vector + */ + void arm_abs_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Q15 vector absolute value. + * @param[in] pSrc points to the input buffer + * @param[out] pDst points to the output buffer + * @param[in] blockSize number of samples in each vector + */ + void arm_abs_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Q31 vector absolute value. + * @param[in] pSrc points to the input buffer + * @param[out] pDst points to the output buffer + * @param[in] blockSize number of samples in each vector + */ + void arm_abs_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Dot product of floating-point vectors. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[in] blockSize number of samples in each vector + * @param[out] result output result returned here + */ + void arm_dot_prod_f32( + float32_t * pSrcA, + float32_t * pSrcB, + uint32_t blockSize, + float32_t * result); + + + /** + * @brief Dot product of Q7 vectors. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[in] blockSize number of samples in each vector + * @param[out] result output result returned here + */ + void arm_dot_prod_q7( + q7_t * pSrcA, + q7_t * pSrcB, + uint32_t blockSize, + q31_t * result); + + + /** + * @brief Dot product of Q15 vectors. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[in] blockSize number of samples in each vector + * @param[out] result output result returned here + */ + void arm_dot_prod_q15( + q15_t * pSrcA, + q15_t * pSrcB, + uint32_t blockSize, + q63_t * result); + + + /** + * @brief Dot product of Q31 vectors. + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[in] blockSize number of samples in each vector + * @param[out] result output result returned here + */ + void arm_dot_prod_q31( + q31_t * pSrcA, + q31_t * pSrcB, + uint32_t blockSize, + q63_t * result); + + + /** + * @brief Shifts the elements of a Q7 vector a specified number of bits. + * @param[in] pSrc points to the input vector + * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_shift_q7( + q7_t * pSrc, + int8_t shiftBits, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Shifts the elements of a Q15 vector a specified number of bits. + * @param[in] pSrc points to the input vector + * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_shift_q15( + q15_t * pSrc, + int8_t shiftBits, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Shifts the elements of a Q31 vector a specified number of bits. + * @param[in] pSrc points to the input vector + * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_shift_q31( + q31_t * pSrc, + int8_t shiftBits, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Adds a constant offset to a floating-point vector. + * @param[in] pSrc points to the input vector + * @param[in] offset is the offset to be added + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_offset_f32( + float32_t * pSrc, + float32_t offset, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Adds a constant offset to a Q7 vector. + * @param[in] pSrc points to the input vector + * @param[in] offset is the offset to be added + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_offset_q7( + q7_t * pSrc, + q7_t offset, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Adds a constant offset to a Q15 vector. + * @param[in] pSrc points to the input vector + * @param[in] offset is the offset to be added + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_offset_q15( + q15_t * pSrc, + q15_t offset, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Adds a constant offset to a Q31 vector. + * @param[in] pSrc points to the input vector + * @param[in] offset is the offset to be added + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_offset_q31( + q31_t * pSrc, + q31_t offset, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Negates the elements of a floating-point vector. + * @param[in] pSrc points to the input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_negate_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Negates the elements of a Q7 vector. + * @param[in] pSrc points to the input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_negate_q7( + q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Negates the elements of a Q15 vector. + * @param[in] pSrc points to the input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_negate_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Negates the elements of a Q31 vector. + * @param[in] pSrc points to the input vector + * @param[out] pDst points to the output vector + * @param[in] blockSize number of samples in the vector + */ + void arm_negate_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Copies the elements of a floating-point vector. + * @param[in] pSrc input pointer + * @param[out] pDst output pointer + * @param[in] blockSize number of samples to process + */ + void arm_copy_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Copies the elements of a Q7 vector. + * @param[in] pSrc input pointer + * @param[out] pDst output pointer + * @param[in] blockSize number of samples to process + */ + void arm_copy_q7( + q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Copies the elements of a Q15 vector. + * @param[in] pSrc input pointer + * @param[out] pDst output pointer + * @param[in] blockSize number of samples to process + */ + void arm_copy_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Copies the elements of a Q31 vector. + * @param[in] pSrc input pointer + * @param[out] pDst output pointer + * @param[in] blockSize number of samples to process + */ + void arm_copy_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Fills a constant value into a floating-point vector. + * @param[in] value input value to be filled + * @param[out] pDst output pointer + * @param[in] blockSize number of samples to process + */ + void arm_fill_f32( + float32_t value, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Fills a constant value into a Q7 vector. + * @param[in] value input value to be filled + * @param[out] pDst output pointer + * @param[in] blockSize number of samples to process + */ + void arm_fill_q7( + q7_t value, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Fills a constant value into a Q15 vector. + * @param[in] value input value to be filled + * @param[out] pDst output pointer + * @param[in] blockSize number of samples to process + */ + void arm_fill_q15( + q15_t value, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Fills a constant value into a Q31 vector. + * @param[in] value input value to be filled + * @param[out] pDst output pointer + * @param[in] blockSize number of samples to process + */ + void arm_fill_q31( + q31_t value, + q31_t * pDst, + uint32_t blockSize); + + +/** + * @brief Convolution of floating-point sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the location where the output result is written. Length srcALen+srcBLen-1. + */ + void arm_conv_f32( + float32_t * pSrcA, + uint32_t srcALen, + float32_t * pSrcB, + uint32_t srcBLen, + float32_t * pDst); + + + /** + * @brief Convolution of Q15 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length srcALen+srcBLen-1. + * @param[in] pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] pScratch2 points to scratch buffer of size min(srcALen, srcBLen). + */ + void arm_conv_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + q15_t * pScratch1, + q15_t * pScratch2); + + +/** + * @brief Convolution of Q15 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the location where the output result is written. Length srcALen+srcBLen-1. + */ + void arm_conv_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst); + + + /** + * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length srcALen+srcBLen-1. + */ + void arm_conv_fast_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst); + + + /** + * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length srcALen+srcBLen-1. + * @param[in] pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] pScratch2 points to scratch buffer of size min(srcALen, srcBLen). + */ + void arm_conv_fast_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + q15_t * pScratch1, + q15_t * pScratch2); + + + /** + * @brief Convolution of Q31 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length srcALen+srcBLen-1. + */ + void arm_conv_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst); + + + /** + * @brief Convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length srcALen+srcBLen-1. + */ + void arm_conv_fast_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst); + + + /** + * @brief Convolution of Q7 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length srcALen+srcBLen-1. + * @param[in] pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen). + */ + void arm_conv_opt_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst, + q15_t * pScratch1, + q15_t * pScratch2); + + + /** + * @brief Convolution of Q7 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length srcALen+srcBLen-1. + */ + void arm_conv_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst); + + + /** + * @brief Partial convolution of floating-point sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + arm_status arm_conv_partial_f32( + float32_t * pSrcA, + uint32_t srcALen, + float32_t * pSrcB, + uint32_t srcBLen, + float32_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + + /** + * @brief Partial convolution of Q15 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @param[in] pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] pScratch2 points to scratch buffer of size min(srcALen, srcBLen). + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + arm_status arm_conv_partial_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + uint32_t firstIndex, + uint32_t numPoints, + q15_t * pScratch1, + q15_t * pScratch2); + + + /** + * @brief Partial convolution of Q15 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + arm_status arm_conv_partial_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + + /** + * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + arm_status arm_conv_partial_fast_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + + /** + * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @param[in] pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] pScratch2 points to scratch buffer of size min(srcALen, srcBLen). + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + arm_status arm_conv_partial_fast_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + uint32_t firstIndex, + uint32_t numPoints, + q15_t * pScratch1, + q15_t * pScratch2); + + + /** + * @brief Partial convolution of Q31 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + arm_status arm_conv_partial_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + + /** + * @brief Partial convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + arm_status arm_conv_partial_fast_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + + /** + * @brief Partial convolution of Q7 sequences + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @param[in] pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen). + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + arm_status arm_conv_partial_opt_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst, + uint32_t firstIndex, + uint32_t numPoints, + q15_t * pScratch1, + q15_t * pScratch2); + + +/** + * @brief Partial convolution of Q7 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + arm_status arm_conv_partial_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + + /** + * @brief Instance structure for the Q15 FIR decimator. + */ + typedef struct + { + uint8_t M; /**< decimation factor. */ + uint16_t numTaps; /**< number of coefficients in the filter. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + } arm_fir_decimate_instance_q15; + + /** + * @brief Instance structure for the Q31 FIR decimator. + */ + typedef struct + { + uint8_t M; /**< decimation factor. */ + uint16_t numTaps; /**< number of coefficients in the filter. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + } arm_fir_decimate_instance_q31; + + /** + * @brief Instance structure for the floating-point FIR decimator. + */ + typedef struct + { + uint8_t M; /**< decimation factor. */ + uint16_t numTaps; /**< number of coefficients in the filter. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + } arm_fir_decimate_instance_f32; + + + /** + * @brief Processing function for the floating-point FIR decimator. + * @param[in] S points to an instance of the floating-point FIR decimator structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_decimate_f32( + const arm_fir_decimate_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the floating-point FIR decimator. + * @param[in,out] S points to an instance of the floating-point FIR decimator structure. + * @param[in] numTaps number of coefficients in the filter. + * @param[in] M decimation factor. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * blockSize is not a multiple of M. + */ + arm_status arm_fir_decimate_init_f32( + arm_fir_decimate_instance_f32 * S, + uint16_t numTaps, + uint8_t M, + float32_t * pCoeffs, + float32_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q15 FIR decimator. + * @param[in] S points to an instance of the Q15 FIR decimator structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_decimate_q15( + const arm_fir_decimate_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q15 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4. + * @param[in] S points to an instance of the Q15 FIR decimator structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_decimate_fast_q15( + const arm_fir_decimate_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q15 FIR decimator. + * @param[in,out] S points to an instance of the Q15 FIR decimator structure. + * @param[in] numTaps number of coefficients in the filter. + * @param[in] M decimation factor. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * blockSize is not a multiple of M. + */ + arm_status arm_fir_decimate_init_q15( + arm_fir_decimate_instance_q15 * S, + uint16_t numTaps, + uint8_t M, + q15_t * pCoeffs, + q15_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q31 FIR decimator. + * @param[in] S points to an instance of the Q31 FIR decimator structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_decimate_q31( + const arm_fir_decimate_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Processing function for the Q31 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4. + * @param[in] S points to an instance of the Q31 FIR decimator structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_decimate_fast_q31( + arm_fir_decimate_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q31 FIR decimator. + * @param[in,out] S points to an instance of the Q31 FIR decimator structure. + * @param[in] numTaps number of coefficients in the filter. + * @param[in] M decimation factor. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * blockSize is not a multiple of M. + */ + arm_status arm_fir_decimate_init_q31( + arm_fir_decimate_instance_q31 * S, + uint16_t numTaps, + uint8_t M, + q31_t * pCoeffs, + q31_t * pState, + uint32_t blockSize); + + + /** + * @brief Instance structure for the Q15 FIR interpolator. + */ + typedef struct + { + uint8_t L; /**< upsample factor. */ + uint16_t phaseLength; /**< length of each polyphase filter component. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */ + q15_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */ + } arm_fir_interpolate_instance_q15; + + /** + * @brief Instance structure for the Q31 FIR interpolator. + */ + typedef struct + { + uint8_t L; /**< upsample factor. */ + uint16_t phaseLength; /**< length of each polyphase filter component. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */ + q31_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */ + } arm_fir_interpolate_instance_q31; + + /** + * @brief Instance structure for the floating-point FIR interpolator. + */ + typedef struct + { + uint8_t L; /**< upsample factor. */ + uint16_t phaseLength; /**< length of each polyphase filter component. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */ + float32_t *pState; /**< points to the state variable array. The array is of length phaseLength+numTaps-1. */ + } arm_fir_interpolate_instance_f32; + + + /** + * @brief Processing function for the Q15 FIR interpolator. + * @param[in] S points to an instance of the Q15 FIR interpolator structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_interpolate_q15( + const arm_fir_interpolate_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q15 FIR interpolator. + * @param[in,out] S points to an instance of the Q15 FIR interpolator structure. + * @param[in] L upsample factor. + * @param[in] numTaps number of filter coefficients in the filter. + * @param[in] pCoeffs points to the filter coefficient buffer. + * @param[in] pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * the filter length numTaps is not a multiple of the interpolation factor L. + */ + arm_status arm_fir_interpolate_init_q15( + arm_fir_interpolate_instance_q15 * S, + uint8_t L, + uint16_t numTaps, + q15_t * pCoeffs, + q15_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q31 FIR interpolator. + * @param[in] S points to an instance of the Q15 FIR interpolator structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_interpolate_q31( + const arm_fir_interpolate_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q31 FIR interpolator. + * @param[in,out] S points to an instance of the Q31 FIR interpolator structure. + * @param[in] L upsample factor. + * @param[in] numTaps number of filter coefficients in the filter. + * @param[in] pCoeffs points to the filter coefficient buffer. + * @param[in] pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * the filter length numTaps is not a multiple of the interpolation factor L. + */ + arm_status arm_fir_interpolate_init_q31( + arm_fir_interpolate_instance_q31 * S, + uint8_t L, + uint16_t numTaps, + q31_t * pCoeffs, + q31_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the floating-point FIR interpolator. + * @param[in] S points to an instance of the floating-point FIR interpolator structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_interpolate_f32( + const arm_fir_interpolate_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the floating-point FIR interpolator. + * @param[in,out] S points to an instance of the floating-point FIR interpolator structure. + * @param[in] L upsample factor. + * @param[in] numTaps number of filter coefficients in the filter. + * @param[in] pCoeffs points to the filter coefficient buffer. + * @param[in] pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * the filter length numTaps is not a multiple of the interpolation factor L. + */ + arm_status arm_fir_interpolate_init_f32( + arm_fir_interpolate_instance_f32 * S, + uint8_t L, + uint16_t numTaps, + float32_t * pCoeffs, + float32_t * pState, + uint32_t blockSize); + + + /** + * @brief Instance structure for the high precision Q31 Biquad cascade filter. + */ + typedef struct + { + uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + q63_t *pState; /**< points to the array of state coefficients. The array is of length 4*numStages. */ + q31_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */ + uint8_t postShift; /**< additional shift, in bits, applied to each output sample. */ + } arm_biquad_cas_df1_32x64_ins_q31; + + + /** + * @param[in] S points to an instance of the high precision Q31 Biquad cascade filter structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] blockSize number of samples to process. + */ + void arm_biquad_cas_df1_32x64_q31( + const arm_biquad_cas_df1_32x64_ins_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @param[in,out] S points to an instance of the high precision Q31 Biquad cascade filter structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] postShift shift to be applied to the output. Varies according to the coefficients format + */ + void arm_biquad_cas_df1_32x64_init_q31( + arm_biquad_cas_df1_32x64_ins_q31 * S, + uint8_t numStages, + q31_t * pCoeffs, + q63_t * pState, + uint8_t postShift); + + + /** + * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter. + */ + typedef struct + { + uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + float32_t *pState; /**< points to the array of state coefficients. The array is of length 2*numStages. */ + float32_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */ + } arm_biquad_cascade_df2T_instance_f32; + + /** + * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter. + */ + typedef struct + { + uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + float32_t *pState; /**< points to the array of state coefficients. The array is of length 4*numStages. */ + float32_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */ + } arm_biquad_cascade_stereo_df2T_instance_f32; + + /** + * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter. + */ + typedef struct + { + uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + float64_t *pState; /**< points to the array of state coefficients. The array is of length 2*numStages. */ + float64_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */ + } arm_biquad_cascade_df2T_instance_f64; + + + /** + * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter. + * @param[in] S points to an instance of the filter data structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] blockSize number of samples to process. + */ + void arm_biquad_cascade_df2T_f32( + const arm_biquad_cascade_df2T_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter. 2 channels + * @param[in] S points to an instance of the filter data structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] blockSize number of samples to process. + */ + void arm_biquad_cascade_stereo_df2T_f32( + const arm_biquad_cascade_stereo_df2T_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter. + * @param[in] S points to an instance of the filter data structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] blockSize number of samples to process. + */ + void arm_biquad_cascade_df2T_f64( + const arm_biquad_cascade_df2T_instance_f64 * S, + float64_t * pSrc, + float64_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the floating-point transposed direct form II Biquad cascade filter. + * @param[in,out] S points to an instance of the filter data structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + */ + void arm_biquad_cascade_df2T_init_f32( + arm_biquad_cascade_df2T_instance_f32 * S, + uint8_t numStages, + float32_t * pCoeffs, + float32_t * pState); + + + /** + * @brief Initialization function for the floating-point transposed direct form II Biquad cascade filter. + * @param[in,out] S points to an instance of the filter data structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + */ + void arm_biquad_cascade_stereo_df2T_init_f32( + arm_biquad_cascade_stereo_df2T_instance_f32 * S, + uint8_t numStages, + float32_t * pCoeffs, + float32_t * pState); + + + /** + * @brief Initialization function for the floating-point transposed direct form II Biquad cascade filter. + * @param[in,out] S points to an instance of the filter data structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] pCoeffs points to the filter coefficients. + * @param[in] pState points to the state buffer. + */ + void arm_biquad_cascade_df2T_init_f64( + arm_biquad_cascade_df2T_instance_f64 * S, + uint8_t numStages, + float64_t * pCoeffs, + float64_t * pState); + + + /** + * @brief Instance structure for the Q15 FIR lattice filter. + */ + typedef struct + { + uint16_t numStages; /**< number of filter stages. */ + q15_t *pState; /**< points to the state variable array. The array is of length numStages. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */ + } arm_fir_lattice_instance_q15; + + /** + * @brief Instance structure for the Q31 FIR lattice filter. + */ + typedef struct + { + uint16_t numStages; /**< number of filter stages. */ + q31_t *pState; /**< points to the state variable array. The array is of length numStages. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */ + } arm_fir_lattice_instance_q31; + + /** + * @brief Instance structure for the floating-point FIR lattice filter. + */ + typedef struct + { + uint16_t numStages; /**< number of filter stages. */ + float32_t *pState; /**< points to the state variable array. The array is of length numStages. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */ + } arm_fir_lattice_instance_f32; + + + /** + * @brief Initialization function for the Q15 FIR lattice filter. + * @param[in] S points to an instance of the Q15 FIR lattice structure. + * @param[in] numStages number of filter stages. + * @param[in] pCoeffs points to the coefficient buffer. The array is of length numStages. + * @param[in] pState points to the state buffer. The array is of length numStages. + */ + void arm_fir_lattice_init_q15( + arm_fir_lattice_instance_q15 * S, + uint16_t numStages, + q15_t * pCoeffs, + q15_t * pState); + + + /** + * @brief Processing function for the Q15 FIR lattice filter. + * @param[in] S points to an instance of the Q15 FIR lattice structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_fir_lattice_q15( + const arm_fir_lattice_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q31 FIR lattice filter. + * @param[in] S points to an instance of the Q31 FIR lattice structure. + * @param[in] numStages number of filter stages. + * @param[in] pCoeffs points to the coefficient buffer. The array is of length numStages. + * @param[in] pState points to the state buffer. The array is of length numStages. + */ + void arm_fir_lattice_init_q31( + arm_fir_lattice_instance_q31 * S, + uint16_t numStages, + q31_t * pCoeffs, + q31_t * pState); + + + /** + * @brief Processing function for the Q31 FIR lattice filter. + * @param[in] S points to an instance of the Q31 FIR lattice structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] blockSize number of samples to process. + */ + void arm_fir_lattice_q31( + const arm_fir_lattice_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + +/** + * @brief Initialization function for the floating-point FIR lattice filter. + * @param[in] S points to an instance of the floating-point FIR lattice structure. + * @param[in] numStages number of filter stages. + * @param[in] pCoeffs points to the coefficient buffer. The array is of length numStages. + * @param[in] pState points to the state buffer. The array is of length numStages. + */ + void arm_fir_lattice_init_f32( + arm_fir_lattice_instance_f32 * S, + uint16_t numStages, + float32_t * pCoeffs, + float32_t * pState); + + + /** + * @brief Processing function for the floating-point FIR lattice filter. + * @param[in] S points to an instance of the floating-point FIR lattice structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] blockSize number of samples to process. + */ + void arm_fir_lattice_f32( + const arm_fir_lattice_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Instance structure for the Q15 IIR lattice filter. + */ + typedef struct + { + uint16_t numStages; /**< number of stages in the filter. */ + q15_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */ + q15_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */ + q15_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */ + } arm_iir_lattice_instance_q15; + + /** + * @brief Instance structure for the Q31 IIR lattice filter. + */ + typedef struct + { + uint16_t numStages; /**< number of stages in the filter. */ + q31_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */ + q31_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */ + q31_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */ + } arm_iir_lattice_instance_q31; + + /** + * @brief Instance structure for the floating-point IIR lattice filter. + */ + typedef struct + { + uint16_t numStages; /**< number of stages in the filter. */ + float32_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */ + float32_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */ + float32_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */ + } arm_iir_lattice_instance_f32; + + + /** + * @brief Processing function for the floating-point IIR lattice filter. + * @param[in] S points to an instance of the floating-point IIR lattice structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_iir_lattice_f32( + const arm_iir_lattice_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the floating-point IIR lattice filter. + * @param[in] S points to an instance of the floating-point IIR lattice structure. + * @param[in] numStages number of stages in the filter. + * @param[in] pkCoeffs points to the reflection coefficient buffer. The array is of length numStages. + * @param[in] pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1. + * @param[in] pState points to the state buffer. The array is of length numStages+blockSize-1. + * @param[in] blockSize number of samples to process. + */ + void arm_iir_lattice_init_f32( + arm_iir_lattice_instance_f32 * S, + uint16_t numStages, + float32_t * pkCoeffs, + float32_t * pvCoeffs, + float32_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q31 IIR lattice filter. + * @param[in] S points to an instance of the Q31 IIR lattice structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_iir_lattice_q31( + const arm_iir_lattice_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q31 IIR lattice filter. + * @param[in] S points to an instance of the Q31 IIR lattice structure. + * @param[in] numStages number of stages in the filter. + * @param[in] pkCoeffs points to the reflection coefficient buffer. The array is of length numStages. + * @param[in] pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1. + * @param[in] pState points to the state buffer. The array is of length numStages+blockSize. + * @param[in] blockSize number of samples to process. + */ + void arm_iir_lattice_init_q31( + arm_iir_lattice_instance_q31 * S, + uint16_t numStages, + q31_t * pkCoeffs, + q31_t * pvCoeffs, + q31_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q15 IIR lattice filter. + * @param[in] S points to an instance of the Q15 IIR lattice structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + */ + void arm_iir_lattice_q15( + const arm_iir_lattice_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + +/** + * @brief Initialization function for the Q15 IIR lattice filter. + * @param[in] S points to an instance of the fixed-point Q15 IIR lattice structure. + * @param[in] numStages number of stages in the filter. + * @param[in] pkCoeffs points to reflection coefficient buffer. The array is of length numStages. + * @param[in] pvCoeffs points to ladder coefficient buffer. The array is of length numStages+1. + * @param[in] pState points to state buffer. The array is of length numStages+blockSize. + * @param[in] blockSize number of samples to process per call. + */ + void arm_iir_lattice_init_q15( + arm_iir_lattice_instance_q15 * S, + uint16_t numStages, + q15_t * pkCoeffs, + q15_t * pvCoeffs, + q15_t * pState, + uint32_t blockSize); + + + /** + * @brief Instance structure for the floating-point LMS filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + float32_t mu; /**< step size that controls filter coefficient updates. */ + } arm_lms_instance_f32; + + + /** + * @brief Processing function for floating-point LMS filter. + * @param[in] S points to an instance of the floating-point LMS filter structure. + * @param[in] pSrc points to the block of input data. + * @param[in] pRef points to the block of reference data. + * @param[out] pOut points to the block of output data. + * @param[out] pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + */ + void arm_lms_f32( + const arm_lms_instance_f32 * S, + float32_t * pSrc, + float32_t * pRef, + float32_t * pOut, + float32_t * pErr, + uint32_t blockSize); + + + /** + * @brief Initialization function for floating-point LMS filter. + * @param[in] S points to an instance of the floating-point LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] pCoeffs points to the coefficient buffer. + * @param[in] pState points to state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + */ + void arm_lms_init_f32( + arm_lms_instance_f32 * S, + uint16_t numTaps, + float32_t * pCoeffs, + float32_t * pState, + float32_t mu, + uint32_t blockSize); + + + /** + * @brief Instance structure for the Q15 LMS filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + q15_t mu; /**< step size that controls filter coefficient updates. */ + uint32_t postShift; /**< bit shift applied to coefficients. */ + } arm_lms_instance_q15; + + + /** + * @brief Initialization function for the Q15 LMS filter. + * @param[in] S points to an instance of the Q15 LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] pCoeffs points to the coefficient buffer. + * @param[in] pState points to the state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + * @param[in] postShift bit shift applied to coefficients. + */ + void arm_lms_init_q15( + arm_lms_instance_q15 * S, + uint16_t numTaps, + q15_t * pCoeffs, + q15_t * pState, + q15_t mu, + uint32_t blockSize, + uint32_t postShift); + + + /** + * @brief Processing function for Q15 LMS filter. + * @param[in] S points to an instance of the Q15 LMS filter structure. + * @param[in] pSrc points to the block of input data. + * @param[in] pRef points to the block of reference data. + * @param[out] pOut points to the block of output data. + * @param[out] pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + */ + void arm_lms_q15( + const arm_lms_instance_q15 * S, + q15_t * pSrc, + q15_t * pRef, + q15_t * pOut, + q15_t * pErr, + uint32_t blockSize); + + + /** + * @brief Instance structure for the Q31 LMS filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + q31_t mu; /**< step size that controls filter coefficient updates. */ + uint32_t postShift; /**< bit shift applied to coefficients. */ + } arm_lms_instance_q31; + + + /** + * @brief Processing function for Q31 LMS filter. + * @param[in] S points to an instance of the Q15 LMS filter structure. + * @param[in] pSrc points to the block of input data. + * @param[in] pRef points to the block of reference data. + * @param[out] pOut points to the block of output data. + * @param[out] pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + */ + void arm_lms_q31( + const arm_lms_instance_q31 * S, + q31_t * pSrc, + q31_t * pRef, + q31_t * pOut, + q31_t * pErr, + uint32_t blockSize); + + + /** + * @brief Initialization function for Q31 LMS filter. + * @param[in] S points to an instance of the Q31 LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] pCoeffs points to coefficient buffer. + * @param[in] pState points to state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + * @param[in] postShift bit shift applied to coefficients. + */ + void arm_lms_init_q31( + arm_lms_instance_q31 * S, + uint16_t numTaps, + q31_t * pCoeffs, + q31_t * pState, + q31_t mu, + uint32_t blockSize, + uint32_t postShift); + + + /** + * @brief Instance structure for the floating-point normalized LMS filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + float32_t mu; /**< step size that control filter coefficient updates. */ + float32_t energy; /**< saves previous frame energy. */ + float32_t x0; /**< saves previous input sample. */ + } arm_lms_norm_instance_f32; + + + /** + * @brief Processing function for floating-point normalized LMS filter. + * @param[in] S points to an instance of the floating-point normalized LMS filter structure. + * @param[in] pSrc points to the block of input data. + * @param[in] pRef points to the block of reference data. + * @param[out] pOut points to the block of output data. + * @param[out] pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + */ + void arm_lms_norm_f32( + arm_lms_norm_instance_f32 * S, + float32_t * pSrc, + float32_t * pRef, + float32_t * pOut, + float32_t * pErr, + uint32_t blockSize); + + + /** + * @brief Initialization function for floating-point normalized LMS filter. + * @param[in] S points to an instance of the floating-point LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] pCoeffs points to coefficient buffer. + * @param[in] pState points to state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + */ + void arm_lms_norm_init_f32( + arm_lms_norm_instance_f32 * S, + uint16_t numTaps, + float32_t * pCoeffs, + float32_t * pState, + float32_t mu, + uint32_t blockSize); + + + /** + * @brief Instance structure for the Q31 normalized LMS filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + q31_t mu; /**< step size that controls filter coefficient updates. */ + uint8_t postShift; /**< bit shift applied to coefficients. */ + q31_t *recipTable; /**< points to the reciprocal initial value table. */ + q31_t energy; /**< saves previous frame energy. */ + q31_t x0; /**< saves previous input sample. */ + } arm_lms_norm_instance_q31; + + + /** + * @brief Processing function for Q31 normalized LMS filter. + * @param[in] S points to an instance of the Q31 normalized LMS filter structure. + * @param[in] pSrc points to the block of input data. + * @param[in] pRef points to the block of reference data. + * @param[out] pOut points to the block of output data. + * @param[out] pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + */ + void arm_lms_norm_q31( + arm_lms_norm_instance_q31 * S, + q31_t * pSrc, + q31_t * pRef, + q31_t * pOut, + q31_t * pErr, + uint32_t blockSize); + + + /** + * @brief Initialization function for Q31 normalized LMS filter. + * @param[in] S points to an instance of the Q31 normalized LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] pCoeffs points to coefficient buffer. + * @param[in] pState points to state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + * @param[in] postShift bit shift applied to coefficients. + */ + void arm_lms_norm_init_q31( + arm_lms_norm_instance_q31 * S, + uint16_t numTaps, + q31_t * pCoeffs, + q31_t * pState, + q31_t mu, + uint32_t blockSize, + uint8_t postShift); + + + /** + * @brief Instance structure for the Q15 normalized LMS filter. + */ + typedef struct + { + uint16_t numTaps; /**< Number of coefficients in the filter. */ + q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + q15_t mu; /**< step size that controls filter coefficient updates. */ + uint8_t postShift; /**< bit shift applied to coefficients. */ + q15_t *recipTable; /**< Points to the reciprocal initial value table. */ + q15_t energy; /**< saves previous frame energy. */ + q15_t x0; /**< saves previous input sample. */ + } arm_lms_norm_instance_q15; + + + /** + * @brief Processing function for Q15 normalized LMS filter. + * @param[in] S points to an instance of the Q15 normalized LMS filter structure. + * @param[in] pSrc points to the block of input data. + * @param[in] pRef points to the block of reference data. + * @param[out] pOut points to the block of output data. + * @param[out] pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + */ + void arm_lms_norm_q15( + arm_lms_norm_instance_q15 * S, + q15_t * pSrc, + q15_t * pRef, + q15_t * pOut, + q15_t * pErr, + uint32_t blockSize); + + + /** + * @brief Initialization function for Q15 normalized LMS filter. + * @param[in] S points to an instance of the Q15 normalized LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] pCoeffs points to coefficient buffer. + * @param[in] pState points to state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + * @param[in] postShift bit shift applied to coefficients. + */ + void arm_lms_norm_init_q15( + arm_lms_norm_instance_q15 * S, + uint16_t numTaps, + q15_t * pCoeffs, + q15_t * pState, + q15_t mu, + uint32_t blockSize, + uint8_t postShift); + + + /** + * @brief Correlation of floating-point sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + */ + void arm_correlate_f32( + float32_t * pSrcA, + uint32_t srcALen, + float32_t * pSrcB, + uint32_t srcBLen, + float32_t * pDst); + + + /** + * @brief Correlation of Q15 sequences + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @param[in] pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + */ + void arm_correlate_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + q15_t * pScratch); + + + /** + * @brief Correlation of Q15 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + */ + + void arm_correlate_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst); + + + /** + * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + */ + + void arm_correlate_fast_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst); + + + /** + * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @param[in] pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + */ + void arm_correlate_fast_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + q15_t * pScratch); + + + /** + * @brief Correlation of Q31 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + */ + void arm_correlate_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst); + + + /** + * @brief Correlation of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + */ + void arm_correlate_fast_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst); + + + /** + * @brief Correlation of Q7 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @param[in] pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen). + */ + void arm_correlate_opt_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst, + q15_t * pScratch1, + q15_t * pScratch2); + + + /** + * @brief Correlation of Q7 sequences. + * @param[in] pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + */ + void arm_correlate_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst); + + + /** + * @brief Instance structure for the floating-point sparse FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ + float32_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ + int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ + } arm_fir_sparse_instance_f32; + + /** + * @brief Instance structure for the Q31 sparse FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ + q31_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ + int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ + } arm_fir_sparse_instance_q31; + + /** + * @brief Instance structure for the Q15 sparse FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ + q15_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ + int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ + } arm_fir_sparse_instance_q15; + + /** + * @brief Instance structure for the Q7 sparse FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ + q7_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ + q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ + int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ + } arm_fir_sparse_instance_q7; + + + /** + * @brief Processing function for the floating-point sparse FIR filter. + * @param[in] S points to an instance of the floating-point sparse FIR structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] pScratchIn points to a temporary buffer of size blockSize. + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_sparse_f32( + arm_fir_sparse_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + float32_t * pScratchIn, + uint32_t blockSize); + + + /** + * @brief Initialization function for the floating-point sparse FIR filter. + * @param[in,out] S points to an instance of the floating-point sparse FIR structure. + * @param[in] numTaps number of nonzero coefficients in the filter. + * @param[in] pCoeffs points to the array of filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] pTapDelay points to the array of offset times. + * @param[in] maxDelay maximum offset time supported. + * @param[in] blockSize number of samples that will be processed per block. + */ + void arm_fir_sparse_init_f32( + arm_fir_sparse_instance_f32 * S, + uint16_t numTaps, + float32_t * pCoeffs, + float32_t * pState, + int32_t * pTapDelay, + uint16_t maxDelay, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q31 sparse FIR filter. + * @param[in] S points to an instance of the Q31 sparse FIR structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] pScratchIn points to a temporary buffer of size blockSize. + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_sparse_q31( + arm_fir_sparse_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + q31_t * pScratchIn, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q31 sparse FIR filter. + * @param[in,out] S points to an instance of the Q31 sparse FIR structure. + * @param[in] numTaps number of nonzero coefficients in the filter. + * @param[in] pCoeffs points to the array of filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] pTapDelay points to the array of offset times. + * @param[in] maxDelay maximum offset time supported. + * @param[in] blockSize number of samples that will be processed per block. + */ + void arm_fir_sparse_init_q31( + arm_fir_sparse_instance_q31 * S, + uint16_t numTaps, + q31_t * pCoeffs, + q31_t * pState, + int32_t * pTapDelay, + uint16_t maxDelay, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q15 sparse FIR filter. + * @param[in] S points to an instance of the Q15 sparse FIR structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] pScratchIn points to a temporary buffer of size blockSize. + * @param[in] pScratchOut points to a temporary buffer of size blockSize. + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_sparse_q15( + arm_fir_sparse_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + q15_t * pScratchIn, + q31_t * pScratchOut, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q15 sparse FIR filter. + * @param[in,out] S points to an instance of the Q15 sparse FIR structure. + * @param[in] numTaps number of nonzero coefficients in the filter. + * @param[in] pCoeffs points to the array of filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] pTapDelay points to the array of offset times. + * @param[in] maxDelay maximum offset time supported. + * @param[in] blockSize number of samples that will be processed per block. + */ + void arm_fir_sparse_init_q15( + arm_fir_sparse_instance_q15 * S, + uint16_t numTaps, + q15_t * pCoeffs, + q15_t * pState, + int32_t * pTapDelay, + uint16_t maxDelay, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q7 sparse FIR filter. + * @param[in] S points to an instance of the Q7 sparse FIR structure. + * @param[in] pSrc points to the block of input data. + * @param[out] pDst points to the block of output data + * @param[in] pScratchIn points to a temporary buffer of size blockSize. + * @param[in] pScratchOut points to a temporary buffer of size blockSize. + * @param[in] blockSize number of input samples to process per call. + */ + void arm_fir_sparse_q7( + arm_fir_sparse_instance_q7 * S, + q7_t * pSrc, + q7_t * pDst, + q7_t * pScratchIn, + q31_t * pScratchOut, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q7 sparse FIR filter. + * @param[in,out] S points to an instance of the Q7 sparse FIR structure. + * @param[in] numTaps number of nonzero coefficients in the filter. + * @param[in] pCoeffs points to the array of filter coefficients. + * @param[in] pState points to the state buffer. + * @param[in] pTapDelay points to the array of offset times. + * @param[in] maxDelay maximum offset time supported. + * @param[in] blockSize number of samples that will be processed per block. + */ + void arm_fir_sparse_init_q7( + arm_fir_sparse_instance_q7 * S, + uint16_t numTaps, + q7_t * pCoeffs, + q7_t * pState, + int32_t * pTapDelay, + uint16_t maxDelay, + uint32_t blockSize); + + + /** + * @brief Floating-point sin_cos function. + * @param[in] theta input value in degrees + * @param[out] pSinVal points to the processed sine output. + * @param[out] pCosVal points to the processed cos output. + */ + void arm_sin_cos_f32( + float32_t theta, + float32_t * pSinVal, + float32_t * pCosVal); + + + /** + * @brief Q31 sin_cos function. + * @param[in] theta scaled input value in degrees + * @param[out] pSinVal points to the processed sine output. + * @param[out] pCosVal points to the processed cosine output. + */ + void arm_sin_cos_q31( + q31_t theta, + q31_t * pSinVal, + q31_t * pCosVal); + + + /** + * @brief Floating-point complex conjugate. + * @param[in] pSrc points to the input vector + * @param[out] pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + */ + void arm_cmplx_conj_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples); + + /** + * @brief Q31 complex conjugate. + * @param[in] pSrc points to the input vector + * @param[out] pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + */ + void arm_cmplx_conj_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples); + + + /** + * @brief Q15 complex conjugate. + * @param[in] pSrc points to the input vector + * @param[out] pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + */ + void arm_cmplx_conj_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples); + + + /** + * @brief Floating-point complex magnitude squared + * @param[in] pSrc points to the complex input vector + * @param[out] pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + */ + void arm_cmplx_mag_squared_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples); + + + /** + * @brief Q31 complex magnitude squared + * @param[in] pSrc points to the complex input vector + * @param[out] pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + */ + void arm_cmplx_mag_squared_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples); + + + /** + * @brief Q15 complex magnitude squared + * @param[in] pSrc points to the complex input vector + * @param[out] pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + */ + void arm_cmplx_mag_squared_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples); + + + /** + * @ingroup groupController + */ + + /** + * @defgroup PID PID Motor Control + * + * A Proportional Integral Derivative (PID) controller is a generic feedback control + * loop mechanism widely used in industrial control systems. + * A PID controller is the most commonly used type of feedback controller. + * + * This set of functions implements (PID) controllers + * for Q15, Q31, and floating-point data types. The functions operate on a single sample + * of data and each call to the function returns a single processed value. + * S points to an instance of the PID control data structure. in + * is the input sample value. The functions return the output value. + * + * \par Algorithm: + *
+   *    y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
+   *    A0 = Kp + Ki + Kd
+   *    A1 = (-Kp ) - (2 * Kd )
+   *    A2 = Kd  
+ * + * \par + * where \c Kp is proportional constant, \c Ki is Integral constant and \c Kd is Derivative constant + * + * \par + * \image html PID.gif "Proportional Integral Derivative Controller" + * + * \par + * The PID controller calculates an "error" value as the difference between + * the measured output and the reference input. + * The controller attempts to minimize the error by adjusting the process control inputs. + * The proportional value determines the reaction to the current error, + * the integral value determines the reaction based on the sum of recent errors, + * and the derivative value determines the reaction based on the rate at which the error has been changing. + * + * \par Instance Structure + * The Gains A0, A1, A2 and state variables for a PID controller are stored together in an instance data structure. + * A separate instance structure must be defined for each PID Controller. + * There are separate instance structure declarations for each of the 3 supported data types. + * + * \par Reset Functions + * There is also an associated reset function for each data type which clears the state array. + * + * \par Initialization Functions + * There is also an associated initialization function for each data type. + * The initialization function performs the following operations: + * - Initializes the Gains A0, A1, A2 from Kp,Ki, Kd gains. + * - Zeros out the values in the state buffer. + * + * \par + * Instance structure cannot be placed into a const data section and it is recommended to use the initialization function. + * + * \par Fixed-Point Behavior + * Care must be taken when using the fixed-point versions of the PID Controller functions. + * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered. + * Refer to the function specific documentation below for usage guidelines. + */ + + /** + * @addtogroup PID + * @{ + */ + + /** + * @brief Process function for the floating-point PID Control. + * @param[in,out] S is an instance of the floating-point PID Control structure + * @param[in] in input sample to process + * @return out processed output sample. + */ + static __INLINE float32_t arm_pid_f32( + arm_pid_instance_f32 * S, + float32_t in) + { + float32_t out; + + /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] */ + out = (S->A0 * in) + + (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]); + + /* Update state */ + S->state[1] = S->state[0]; + S->state[0] = in; + S->state[2] = out; + + /* return to application */ + return (out); + + } + + /** + * @brief Process function for the Q31 PID Control. + * @param[in,out] S points to an instance of the Q31 PID Control structure + * @param[in] in input sample to process + * @return out processed output sample. + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using an internal 64-bit accumulator. + * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit. + * Thus, if the accumulator result overflows it wraps around rather than clip. + * In order to avoid overflows completely the input signal must be scaled down by 2 bits as there are four additions. + * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format. + */ + static __INLINE q31_t arm_pid_q31( + arm_pid_instance_q31 * S, + q31_t in) + { + q63_t acc; + q31_t out; + + /* acc = A0 * x[n] */ + acc = (q63_t) S->A0 * in; + + /* acc += A1 * x[n-1] */ + acc += (q63_t) S->A1 * S->state[0]; + + /* acc += A2 * x[n-2] */ + acc += (q63_t) S->A2 * S->state[1]; + + /* convert output to 1.31 format to add y[n-1] */ + out = (q31_t) (acc >> 31u); + + /* out += y[n-1] */ + out += S->state[2]; + + /* Update state */ + S->state[1] = S->state[0]; + S->state[0] = in; + S->state[2] = out; + + /* return to application */ + return (out); + } + + + /** + * @brief Process function for the Q15 PID Control. + * @param[in,out] S points to an instance of the Q15 PID Control structure + * @param[in] in input sample to process + * @return out processed output sample. + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using a 64-bit internal accumulator. + * Both Gains and state variables are represented in 1.15 format and multiplications yield a 2.30 result. + * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format. + * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved. + * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits. + * Lastly, the accumulator is saturated to yield a result in 1.15 format. + */ + static __INLINE q15_t arm_pid_q15( + arm_pid_instance_q15 * S, + q15_t in) + { + q63_t acc; + q15_t out; + +#ifndef ARM_MATH_CM0_FAMILY + __SIMD32_TYPE *vstate; + + /* Implementation of PID controller */ + + /* acc = A0 * x[n] */ + acc = (q31_t) __SMUAD((uint32_t)S->A0, (uint32_t)in); + + /* acc += A1 * x[n-1] + A2 * x[n-2] */ + vstate = __SIMD32_CONST(S->state); + acc = (q63_t)__SMLALD((uint32_t)S->A1, (uint32_t)*vstate, (uint64_t)acc); +#else + /* acc = A0 * x[n] */ + acc = ((q31_t) S->A0) * in; + + /* acc += A1 * x[n-1] + A2 * x[n-2] */ + acc += (q31_t) S->A1 * S->state[0]; + acc += (q31_t) S->A2 * S->state[1]; +#endif + + /* acc += y[n-1] */ + acc += (q31_t) S->state[2] << 15; + + /* saturate the output */ + out = (q15_t) (__SSAT((acc >> 15), 16)); + + /* Update state */ + S->state[1] = S->state[0]; + S->state[0] = in; + S->state[2] = out; + + /* return to application */ + return (out); + } + + /** + * @} end of PID group + */ + + + /** + * @brief Floating-point matrix inverse. + * @param[in] src points to the instance of the input floating-point matrix structure. + * @param[out] dst points to the instance of the output floating-point matrix structure. + * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match. + * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR. + */ + arm_status arm_mat_inverse_f32( + const arm_matrix_instance_f32 * src, + arm_matrix_instance_f32 * dst); + + + /** + * @brief Floating-point matrix inverse. + * @param[in] src points to the instance of the input floating-point matrix structure. + * @param[out] dst points to the instance of the output floating-point matrix structure. + * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match. + * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR. + */ + arm_status arm_mat_inverse_f64( + const arm_matrix_instance_f64 * src, + arm_matrix_instance_f64 * dst); + + + + /** + * @ingroup groupController + */ + + /** + * @defgroup clarke Vector Clarke Transform + * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector. + * Generally the Clarke transform uses three-phase currents Ia, Ib and Ic to calculate currents + * in the two-phase orthogonal stator axis Ialpha and Ibeta. + * When Ialpha is superposed with Ia as shown in the figure below + * \image html clarke.gif Stator current space vector and its components in (a,b). + * and Ia + Ib + Ic = 0, in this condition Ialpha and Ibeta + * can be calculated using only Ia and Ib. + * + * The function operates on a single sample of data and each call to the function returns the processed output. + * The library provides separate functions for Q31 and floating-point data types. + * \par Algorithm + * \image html clarkeFormula.gif + * where Ia and Ib are the instantaneous stator phases and + * pIalpha and pIbeta are the two coordinates of time invariant vector. + * \par Fixed-Point Behavior + * Care must be taken when using the Q31 version of the Clarke transform. + * In particular, the overflow and saturation behavior of the accumulator used must be considered. + * Refer to the function specific documentation below for usage guidelines. + */ + + /** + * @addtogroup clarke + * @{ + */ + + /** + * + * @brief Floating-point Clarke transform + * @param[in] Ia input three-phase coordinate a + * @param[in] Ib input three-phase coordinate b + * @param[out] pIalpha points to output two-phase orthogonal vector axis alpha + * @param[out] pIbeta points to output two-phase orthogonal vector axis beta + */ + static __INLINE void arm_clarke_f32( + float32_t Ia, + float32_t Ib, + float32_t * pIalpha, + float32_t * pIbeta) + { + /* Calculate pIalpha using the equation, pIalpha = Ia */ + *pIalpha = Ia; + + /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */ + *pIbeta = ((float32_t) 0.57735026919 * Ia + (float32_t) 1.15470053838 * Ib); + } + + + /** + * @brief Clarke transform for Q31 version + * @param[in] Ia input three-phase coordinate a + * @param[in] Ib input three-phase coordinate b + * @param[out] pIalpha points to output two-phase orthogonal vector axis alpha + * @param[out] pIbeta points to output two-phase orthogonal vector axis beta + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using an internal 32-bit accumulator. + * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. + * There is saturation on the addition, hence there is no risk of overflow. + */ + static __INLINE void arm_clarke_q31( + q31_t Ia, + q31_t Ib, + q31_t * pIalpha, + q31_t * pIbeta) + { + q31_t product1, product2; /* Temporary variables used to store intermediate results */ + + /* Calculating pIalpha from Ia by equation pIalpha = Ia */ + *pIalpha = Ia; + + /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */ + product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30); + + /* Intermediate product is calculated by (2/sqrt(3) * Ib) */ + product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30); + + /* pIbeta is calculated by adding the intermediate products */ + *pIbeta = __QADD(product1, product2); + } + + /** + * @} end of clarke group + */ + + /** + * @brief Converts the elements of the Q7 vector to Q31 vector. + * @param[in] pSrc input pointer + * @param[out] pDst output pointer + * @param[in] blockSize number of samples to process + */ + void arm_q7_to_q31( + q7_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + + /** + * @ingroup groupController + */ + + /** + * @defgroup inv_clarke Vector Inverse Clarke Transform + * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases. + * + * The function operates on a single sample of data and each call to the function returns the processed output. + * The library provides separate functions for Q31 and floating-point data types. + * \par Algorithm + * \image html clarkeInvFormula.gif + * where pIa and pIb are the instantaneous stator phases and + * Ialpha and Ibeta are the two coordinates of time invariant vector. + * \par Fixed-Point Behavior + * Care must be taken when using the Q31 version of the Clarke transform. + * In particular, the overflow and saturation behavior of the accumulator used must be considered. + * Refer to the function specific documentation below for usage guidelines. + */ + + /** + * @addtogroup inv_clarke + * @{ + */ + + /** + * @brief Floating-point Inverse Clarke transform + * @param[in] Ialpha input two-phase orthogonal vector axis alpha + * @param[in] Ibeta input two-phase orthogonal vector axis beta + * @param[out] pIa points to output three-phase coordinate a + * @param[out] pIb points to output three-phase coordinate b + */ + static __INLINE void arm_inv_clarke_f32( + float32_t Ialpha, + float32_t Ibeta, + float32_t * pIa, + float32_t * pIb) + { + /* Calculating pIa from Ialpha by equation pIa = Ialpha */ + *pIa = Ialpha; + + /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */ + *pIb = -0.5f * Ialpha + 0.8660254039f * Ibeta; + } + + + /** + * @brief Inverse Clarke transform for Q31 version + * @param[in] Ialpha input two-phase orthogonal vector axis alpha + * @param[in] Ibeta input two-phase orthogonal vector axis beta + * @param[out] pIa points to output three-phase coordinate a + * @param[out] pIb points to output three-phase coordinate b + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using an internal 32-bit accumulator. + * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. + * There is saturation on the subtraction, hence there is no risk of overflow. + */ + static __INLINE void arm_inv_clarke_q31( + q31_t Ialpha, + q31_t Ibeta, + q31_t * pIa, + q31_t * pIb) + { + q31_t product1, product2; /* Temporary variables used to store intermediate results */ + + /* Calculating pIa from Ialpha by equation pIa = Ialpha */ + *pIa = Ialpha; + + /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */ + product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31); + + /* Intermediate product is calculated by (1/sqrt(3) * pIb) */ + product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31); + + /* pIb is calculated by subtracting the products */ + *pIb = __QSUB(product2, product1); + } + + /** + * @} end of inv_clarke group + */ + + /** + * @brief Converts the elements of the Q7 vector to Q15 vector. + * @param[in] pSrc input pointer + * @param[out] pDst output pointer + * @param[in] blockSize number of samples to process + */ + void arm_q7_to_q15( + q7_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + + /** + * @ingroup groupController + */ + + /** + * @defgroup park Vector Park Transform + * + * Forward Park transform converts the input two-coordinate vector to flux and torque components. + * The Park transform can be used to realize the transformation of the Ialpha and the Ibeta currents + * from the stationary to the moving reference frame and control the spatial relationship between + * the stator vector current and rotor flux vector. + * If we consider the d axis aligned with the rotor flux, the diagram below shows the + * current vector and the relationship from the two reference frames: + * \image html park.gif "Stator current space vector and its component in (a,b) and in the d,q rotating reference frame" + * + * The function operates on a single sample of data and each call to the function returns the processed output. + * The library provides separate functions for Q31 and floating-point data types. + * \par Algorithm + * \image html parkFormula.gif + * where Ialpha and Ibeta are the stator vector components, + * pId and pIq are rotor vector components and cosVal and sinVal are the + * cosine and sine values of theta (rotor flux position). + * \par Fixed-Point Behavior + * Care must be taken when using the Q31 version of the Park transform. + * In particular, the overflow and saturation behavior of the accumulator used must be considered. + * Refer to the function specific documentation below for usage guidelines. + */ + + /** + * @addtogroup park + * @{ + */ + + /** + * @brief Floating-point Park transform + * @param[in] Ialpha input two-phase vector coordinate alpha + * @param[in] Ibeta input two-phase vector coordinate beta + * @param[out] pId points to output rotor reference frame d + * @param[out] pIq points to output rotor reference frame q + * @param[in] sinVal sine value of rotation angle theta + * @param[in] cosVal cosine value of rotation angle theta + * + * The function implements the forward Park transform. + * + */ + static __INLINE void arm_park_f32( + float32_t Ialpha, + float32_t Ibeta, + float32_t * pId, + float32_t * pIq, + float32_t sinVal, + float32_t cosVal) + { + /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */ + *pId = Ialpha * cosVal + Ibeta * sinVal; + + /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */ + *pIq = -Ialpha * sinVal + Ibeta * cosVal; + } + + + /** + * @brief Park transform for Q31 version + * @param[in] Ialpha input two-phase vector coordinate alpha + * @param[in] Ibeta input two-phase vector coordinate beta + * @param[out] pId points to output rotor reference frame d + * @param[out] pIq points to output rotor reference frame q + * @param[in] sinVal sine value of rotation angle theta + * @param[in] cosVal cosine value of rotation angle theta + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using an internal 32-bit accumulator. + * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. + * There is saturation on the addition and subtraction, hence there is no risk of overflow. + */ + static __INLINE void arm_park_q31( + q31_t Ialpha, + q31_t Ibeta, + q31_t * pId, + q31_t * pIq, + q31_t sinVal, + q31_t cosVal) + { + q31_t product1, product2; /* Temporary variables used to store intermediate results */ + q31_t product3, product4; /* Temporary variables used to store intermediate results */ + + /* Intermediate product is calculated by (Ialpha * cosVal) */ + product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31); + + /* Intermediate product is calculated by (Ibeta * sinVal) */ + product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31); + + + /* Intermediate product is calculated by (Ialpha * sinVal) */ + product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31); + + /* Intermediate product is calculated by (Ibeta * cosVal) */ + product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31); + + /* Calculate pId by adding the two intermediate products 1 and 2 */ + *pId = __QADD(product1, product2); + + /* Calculate pIq by subtracting the two intermediate products 3 from 4 */ + *pIq = __QSUB(product4, product3); + } + + /** + * @} end of park group + */ + + /** + * @brief Converts the elements of the Q7 vector to floating-point vector. + * @param[in] pSrc is input pointer + * @param[out] pDst is output pointer + * @param[in] blockSize is the number of samples to process + */ + void arm_q7_to_float( + q7_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @ingroup groupController + */ + + /** + * @defgroup inv_park Vector Inverse Park transform + * Inverse Park transform converts the input flux and torque components to two-coordinate vector. + * + * The function operates on a single sample of data and each call to the function returns the processed output. + * The library provides separate functions for Q31 and floating-point data types. + * \par Algorithm + * \image html parkInvFormula.gif + * where pIalpha and pIbeta are the stator vector components, + * Id and Iq are rotor vector components and cosVal and sinVal are the + * cosine and sine values of theta (rotor flux position). + * \par Fixed-Point Behavior + * Care must be taken when using the Q31 version of the Park transform. + * In particular, the overflow and saturation behavior of the accumulator used must be considered. + * Refer to the function specific documentation below for usage guidelines. + */ + + /** + * @addtogroup inv_park + * @{ + */ + + /** + * @brief Floating-point Inverse Park transform + * @param[in] Id input coordinate of rotor reference frame d + * @param[in] Iq input coordinate of rotor reference frame q + * @param[out] pIalpha points to output two-phase orthogonal vector axis alpha + * @param[out] pIbeta points to output two-phase orthogonal vector axis beta + * @param[in] sinVal sine value of rotation angle theta + * @param[in] cosVal cosine value of rotation angle theta + */ + static __INLINE void arm_inv_park_f32( + float32_t Id, + float32_t Iq, + float32_t * pIalpha, + float32_t * pIbeta, + float32_t sinVal, + float32_t cosVal) + { + /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */ + *pIalpha = Id * cosVal - Iq * sinVal; + + /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */ + *pIbeta = Id * sinVal + Iq * cosVal; + } + + + /** + * @brief Inverse Park transform for Q31 version + * @param[in] Id input coordinate of rotor reference frame d + * @param[in] Iq input coordinate of rotor reference frame q + * @param[out] pIalpha points to output two-phase orthogonal vector axis alpha + * @param[out] pIbeta points to output two-phase orthogonal vector axis beta + * @param[in] sinVal sine value of rotation angle theta + * @param[in] cosVal cosine value of rotation angle theta + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using an internal 32-bit accumulator. + * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. + * There is saturation on the addition, hence there is no risk of overflow. + */ + static __INLINE void arm_inv_park_q31( + q31_t Id, + q31_t Iq, + q31_t * pIalpha, + q31_t * pIbeta, + q31_t sinVal, + q31_t cosVal) + { + q31_t product1, product2; /* Temporary variables used to store intermediate results */ + q31_t product3, product4; /* Temporary variables used to store intermediate results */ + + /* Intermediate product is calculated by (Id * cosVal) */ + product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31); + + /* Intermediate product is calculated by (Iq * sinVal) */ + product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31); + + + /* Intermediate product is calculated by (Id * sinVal) */ + product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31); + + /* Intermediate product is calculated by (Iq * cosVal) */ + product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31); + + /* Calculate pIalpha by using the two intermediate products 1 and 2 */ + *pIalpha = __QSUB(product1, product2); + + /* Calculate pIbeta by using the two intermediate products 3 and 4 */ + *pIbeta = __QADD(product4, product3); + } + + /** + * @} end of Inverse park group + */ + + + /** + * @brief Converts the elements of the Q31 vector to floating-point vector. + * @param[in] pSrc is input pointer + * @param[out] pDst is output pointer + * @param[in] blockSize is the number of samples to process + */ + void arm_q31_to_float( + q31_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + /** + * @ingroup groupInterpolation + */ + + /** + * @defgroup LinearInterpolate Linear Interpolation + * + * Linear interpolation is a method of curve fitting using linear polynomials. + * Linear interpolation works by effectively drawing a straight line between two neighboring samples and returning the appropriate point along that line + * + * \par + * \image html LinearInterp.gif "Linear interpolation" + * + * \par + * A Linear Interpolate function calculates an output value(y), for the input(x) + * using linear interpolation of the input values x0, x1( nearest input values) and the output values y0 and y1(nearest output values) + * + * \par Algorithm: + *
+   *       y = y0 + (x - x0) * ((y1 - y0)/(x1-x0))
+   *       where x0, x1 are nearest values of input x
+   *             y0, y1 are nearest values to output y
+   * 
+ * + * \par + * This set of functions implements Linear interpolation process + * for Q7, Q15, Q31, and floating-point data types. The functions operate on a single + * sample of data and each call to the function returns a single processed value. + * S points to an instance of the Linear Interpolate function data structure. + * x is the input sample value. The functions returns the output value. + * + * \par + * if x is outside of the table boundary, Linear interpolation returns first value of the table + * if x is below input range and returns last value of table if x is above range. + */ + + /** + * @addtogroup LinearInterpolate + * @{ + */ + + /** + * @brief Process function for the floating-point Linear Interpolation Function. + * @param[in,out] S is an instance of the floating-point Linear Interpolation structure + * @param[in] x input sample to process + * @return y processed output sample. + * + */ + static __INLINE float32_t arm_linear_interp_f32( + arm_linear_interp_instance_f32 * S, + float32_t x) + { + float32_t y; + float32_t x0, x1; /* Nearest input values */ + float32_t y0, y1; /* Nearest output values */ + float32_t xSpacing = S->xSpacing; /* spacing between input values */ + int32_t i; /* Index variable */ + float32_t *pYData = S->pYData; /* pointer to output table */ + + /* Calculation of index */ + i = (int32_t) ((x - S->x1) / xSpacing); + + if(i < 0) + { + /* Iniatilize output for below specified range as least output value of table */ + y = pYData[0]; + } + else if((uint32_t)i >= S->nValues) + { + /* Iniatilize output for above specified range as last output value of table */ + y = pYData[S->nValues - 1]; + } + else + { + /* Calculation of nearest input values */ + x0 = S->x1 + i * xSpacing; + x1 = S->x1 + (i + 1) * xSpacing; + + /* Read of nearest output values */ + y0 = pYData[i]; + y1 = pYData[i + 1]; + + /* Calculation of output */ + y = y0 + (x - x0) * ((y1 - y0) / (x1 - x0)); + + } + + /* returns output value */ + return (y); + } + + + /** + * + * @brief Process function for the Q31 Linear Interpolation Function. + * @param[in] pYData pointer to Q31 Linear Interpolation table + * @param[in] x input sample to process + * @param[in] nValues number of table values + * @return y processed output sample. + * + * \par + * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part. + * This function can support maximum of table size 2^12. + * + */ + static __INLINE q31_t arm_linear_interp_q31( + q31_t * pYData, + q31_t x, + uint32_t nValues) + { + q31_t y; /* output */ + q31_t y0, y1; /* Nearest output values */ + q31_t fract; /* fractional part */ + int32_t index; /* Index to read nearest output values */ + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + index = ((x & (q31_t)0xFFF00000) >> 20); + + if(index >= (int32_t)(nValues - 1)) + { + return (pYData[nValues - 1]); + } + else if(index < 0) + { + return (pYData[0]); + } + else + { + /* 20 bits for the fractional part */ + /* shift left by 11 to keep fract in 1.31 format */ + fract = (x & 0x000FFFFF) << 11; + + /* Read two nearest output values from the index in 1.31(q31) format */ + y0 = pYData[index]; + y1 = pYData[index + 1]; + + /* Calculation of y0 * (1-fract) and y is in 2.30 format */ + y = ((q31_t) ((q63_t) y0 * (0x7FFFFFFF - fract) >> 32)); + + /* Calculation of y0 * (1-fract) + y1 *fract and y is in 2.30 format */ + y += ((q31_t) (((q63_t) y1 * fract) >> 32)); + + /* Convert y to 1.31 format */ + return (y << 1u); + } + } + + + /** + * + * @brief Process function for the Q15 Linear Interpolation Function. + * @param[in] pYData pointer to Q15 Linear Interpolation table + * @param[in] x input sample to process + * @param[in] nValues number of table values + * @return y processed output sample. + * + * \par + * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part. + * This function can support maximum of table size 2^12. + * + */ + static __INLINE q15_t arm_linear_interp_q15( + q15_t * pYData, + q31_t x, + uint32_t nValues) + { + q63_t y; /* output */ + q15_t y0, y1; /* Nearest output values */ + q31_t fract; /* fractional part */ + int32_t index; /* Index to read nearest output values */ + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + index = ((x & (int32_t)0xFFF00000) >> 20); + + if(index >= (int32_t)(nValues - 1)) + { + return (pYData[nValues - 1]); + } + else if(index < 0) + { + return (pYData[0]); + } + else + { + /* 20 bits for the fractional part */ + /* fract is in 12.20 format */ + fract = (x & 0x000FFFFF); + + /* Read two nearest output values from the index */ + y0 = pYData[index]; + y1 = pYData[index + 1]; + + /* Calculation of y0 * (1-fract) and y is in 13.35 format */ + y = ((q63_t) y0 * (0xFFFFF - fract)); + + /* Calculation of (y0 * (1-fract) + y1 * fract) and y is in 13.35 format */ + y += ((q63_t) y1 * (fract)); + + /* convert y to 1.15 format */ + return (q15_t) (y >> 20); + } + } + + + /** + * + * @brief Process function for the Q7 Linear Interpolation Function. + * @param[in] pYData pointer to Q7 Linear Interpolation table + * @param[in] x input sample to process + * @param[in] nValues number of table values + * @return y processed output sample. + * + * \par + * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part. + * This function can support maximum of table size 2^12. + */ + static __INLINE q7_t arm_linear_interp_q7( + q7_t * pYData, + q31_t x, + uint32_t nValues) + { + q31_t y; /* output */ + q7_t y0, y1; /* Nearest output values */ + q31_t fract; /* fractional part */ + uint32_t index; /* Index to read nearest output values */ + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + if (x < 0) + { + return (pYData[0]); + } + index = (x >> 20) & 0xfff; + + if(index >= (nValues - 1)) + { + return (pYData[nValues - 1]); + } + else + { + /* 20 bits for the fractional part */ + /* fract is in 12.20 format */ + fract = (x & 0x000FFFFF); + + /* Read two nearest output values from the index and are in 1.7(q7) format */ + y0 = pYData[index]; + y1 = pYData[index + 1]; + + /* Calculation of y0 * (1-fract ) and y is in 13.27(q27) format */ + y = ((y0 * (0xFFFFF - fract))); + + /* Calculation of y1 * fract + y0 * (1-fract) and y is in 13.27(q27) format */ + y += (y1 * fract); + + /* convert y to 1.7(q7) format */ + return (q7_t) (y >> 20); + } + } + + /** + * @} end of LinearInterpolate group + */ + + /** + * @brief Fast approximation to the trigonometric sine function for floating-point data. + * @param[in] x input value in radians. + * @return sin(x). + */ + float32_t arm_sin_f32( + float32_t x); + + + /** + * @brief Fast approximation to the trigonometric sine function for Q31 data. + * @param[in] x Scaled input value in radians. + * @return sin(x). + */ + q31_t arm_sin_q31( + q31_t x); + + + /** + * @brief Fast approximation to the trigonometric sine function for Q15 data. + * @param[in] x Scaled input value in radians. + * @return sin(x). + */ + q15_t arm_sin_q15( + q15_t x); + + + /** + * @brief Fast approximation to the trigonometric cosine function for floating-point data. + * @param[in] x input value in radians. + * @return cos(x). + */ + float32_t arm_cos_f32( + float32_t x); + + + /** + * @brief Fast approximation to the trigonometric cosine function for Q31 data. + * @param[in] x Scaled input value in radians. + * @return cos(x). + */ + q31_t arm_cos_q31( + q31_t x); + + + /** + * @brief Fast approximation to the trigonometric cosine function for Q15 data. + * @param[in] x Scaled input value in radians. + * @return cos(x). + */ + q15_t arm_cos_q15( + q15_t x); + + + /** + * @ingroup groupFastMath + */ + + + /** + * @defgroup SQRT Square Root + * + * Computes the square root of a number. + * There are separate functions for Q15, Q31, and floating-point data types. + * The square root function is computed using the Newton-Raphson algorithm. + * This is an iterative algorithm of the form: + *
+   *      x1 = x0 - f(x0)/f'(x0)
+   * 
+ * where x1 is the current estimate, + * x0 is the previous estimate, and + * f'(x0) is the derivative of f() evaluated at x0. + * For the square root function, the algorithm reduces to: + *
+   *     x0 = in/2                         [initial guess]
+   *     x1 = 1/2 * ( x0 + in / x0)        [each iteration]
+   * 
+ */ + + + /** + * @addtogroup SQRT + * @{ + */ + + /** + * @brief Floating-point square root function. + * @param[in] in input value. + * @param[out] pOut square root of input value. + * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if + * in is negative value and returns zero output for negative values. + */ + static __INLINE arm_status arm_sqrt_f32( + float32_t in, + float32_t * pOut) + { + if(in >= 0.0f) + { + +#if (__FPU_USED == 1) && defined ( __CC_ARM ) + *pOut = __sqrtf(in); +#elif (__FPU_USED == 1) && (defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)) + *pOut = __builtin_sqrtf(in); +#elif (__FPU_USED == 1) && defined(__GNUC__) + *pOut = __builtin_sqrtf(in); +#elif (__FPU_USED == 1) && defined ( __ICCARM__ ) && (__VER__ >= 6040000) + __ASM("VSQRT.F32 %0,%1" : "=t"(*pOut) : "t"(in)); +#else + *pOut = sqrtf(in); +#endif + + return (ARM_MATH_SUCCESS); + } + else + { + *pOut = 0.0f; + return (ARM_MATH_ARGUMENT_ERROR); + } + } + + + /** + * @brief Q31 square root function. + * @param[in] in input value. The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF. + * @param[out] pOut square root of input value. + * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if + * in is negative value and returns zero output for negative values. + */ + arm_status arm_sqrt_q31( + q31_t in, + q31_t * pOut); + + + /** + * @brief Q15 square root function. + * @param[in] in input value. The range of the input value is [0 +1) or 0x0000 to 0x7FFF. + * @param[out] pOut square root of input value. + * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if + * in is negative value and returns zero output for negative values. + */ + arm_status arm_sqrt_q15( + q15_t in, + q15_t * pOut); + + /** + * @} end of SQRT group + */ + + + /** + * @brief floating-point Circular write function. + */ + static __INLINE void arm_circularWrite_f32( + int32_t * circBuffer, + int32_t L, + uint16_t * writeOffset, + int32_t bufferInc, + const int32_t * src, + int32_t srcInc, + uint32_t blockSize) + { + uint32_t i = 0u; + int32_t wOffset; + + /* Copy the value of Index pointer that points + * to the current location where the input samples to be copied */ + wOffset = *writeOffset; + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the input sample to the circular buffer */ + circBuffer[wOffset] = *src; + + /* Update the input pointer */ + src += srcInc; + + /* Circularly update wOffset. Watch out for positive and negative value */ + wOffset += bufferInc; + if(wOffset >= L) + wOffset -= L; + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *writeOffset = (uint16_t)wOffset; + } + + + + /** + * @brief floating-point Circular Read function. + */ + static __INLINE void arm_circularRead_f32( + int32_t * circBuffer, + int32_t L, + int32_t * readOffset, + int32_t bufferInc, + int32_t * dst, + int32_t * dst_base, + int32_t dst_length, + int32_t dstInc, + uint32_t blockSize) + { + uint32_t i = 0u; + int32_t rOffset, dst_end; + + /* Copy the value of Index pointer that points + * to the current location from where the input samples to be read */ + rOffset = *readOffset; + dst_end = (int32_t) (dst_base + dst_length); + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the sample from the circular buffer to the destination buffer */ + *dst = circBuffer[rOffset]; + + /* Update the input pointer */ + dst += dstInc; + + if(dst == (int32_t *) dst_end) + { + dst = dst_base; + } + + /* Circularly update rOffset. Watch out for positive and negative value */ + rOffset += bufferInc; + + if(rOffset >= L) + { + rOffset -= L; + } + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *readOffset = rOffset; + } + + + /** + * @brief Q15 Circular write function. + */ + static __INLINE void arm_circularWrite_q15( + q15_t * circBuffer, + int32_t L, + uint16_t * writeOffset, + int32_t bufferInc, + const q15_t * src, + int32_t srcInc, + uint32_t blockSize) + { + uint32_t i = 0u; + int32_t wOffset; + + /* Copy the value of Index pointer that points + * to the current location where the input samples to be copied */ + wOffset = *writeOffset; + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the input sample to the circular buffer */ + circBuffer[wOffset] = *src; + + /* Update the input pointer */ + src += srcInc; + + /* Circularly update wOffset. Watch out for positive and negative value */ + wOffset += bufferInc; + if(wOffset >= L) + wOffset -= L; + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *writeOffset = (uint16_t)wOffset; + } + + + /** + * @brief Q15 Circular Read function. + */ + static __INLINE void arm_circularRead_q15( + q15_t * circBuffer, + int32_t L, + int32_t * readOffset, + int32_t bufferInc, + q15_t * dst, + q15_t * dst_base, + int32_t dst_length, + int32_t dstInc, + uint32_t blockSize) + { + uint32_t i = 0; + int32_t rOffset, dst_end; + + /* Copy the value of Index pointer that points + * to the current location from where the input samples to be read */ + rOffset = *readOffset; + + dst_end = (int32_t) (dst_base + dst_length); + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the sample from the circular buffer to the destination buffer */ + *dst = circBuffer[rOffset]; + + /* Update the input pointer */ + dst += dstInc; + + if(dst == (q15_t *) dst_end) + { + dst = dst_base; + } + + /* Circularly update wOffset. Watch out for positive and negative value */ + rOffset += bufferInc; + + if(rOffset >= L) + { + rOffset -= L; + } + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *readOffset = rOffset; + } + + + /** + * @brief Q7 Circular write function. + */ + static __INLINE void arm_circularWrite_q7( + q7_t * circBuffer, + int32_t L, + uint16_t * writeOffset, + int32_t bufferInc, + const q7_t * src, + int32_t srcInc, + uint32_t blockSize) + { + uint32_t i = 0u; + int32_t wOffset; + + /* Copy the value of Index pointer that points + * to the current location where the input samples to be copied */ + wOffset = *writeOffset; + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the input sample to the circular buffer */ + circBuffer[wOffset] = *src; + + /* Update the input pointer */ + src += srcInc; + + /* Circularly update wOffset. Watch out for positive and negative value */ + wOffset += bufferInc; + if(wOffset >= L) + wOffset -= L; + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *writeOffset = (uint16_t)wOffset; + } + + + /** + * @brief Q7 Circular Read function. + */ + static __INLINE void arm_circularRead_q7( + q7_t * circBuffer, + int32_t L, + int32_t * readOffset, + int32_t bufferInc, + q7_t * dst, + q7_t * dst_base, + int32_t dst_length, + int32_t dstInc, + uint32_t blockSize) + { + uint32_t i = 0; + int32_t rOffset, dst_end; + + /* Copy the value of Index pointer that points + * to the current location from where the input samples to be read */ + rOffset = *readOffset; + + dst_end = (int32_t) (dst_base + dst_length); + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the sample from the circular buffer to the destination buffer */ + *dst = circBuffer[rOffset]; + + /* Update the input pointer */ + dst += dstInc; + + if(dst == (q7_t *) dst_end) + { + dst = dst_base; + } + + /* Circularly update rOffset. Watch out for positive and negative value */ + rOffset += bufferInc; + + if(rOffset >= L) + { + rOffset -= L; + } + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *readOffset = rOffset; + } + + + /** + * @brief Sum of the squares of the elements of a Q31 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_power_q31( + q31_t * pSrc, + uint32_t blockSize, + q63_t * pResult); + + + /** + * @brief Sum of the squares of the elements of a floating-point vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_power_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult); + + + /** + * @brief Sum of the squares of the elements of a Q15 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_power_q15( + q15_t * pSrc, + uint32_t blockSize, + q63_t * pResult); + + + /** + * @brief Sum of the squares of the elements of a Q7 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_power_q7( + q7_t * pSrc, + uint32_t blockSize, + q31_t * pResult); + + + /** + * @brief Mean value of a Q7 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_mean_q7( + q7_t * pSrc, + uint32_t blockSize, + q7_t * pResult); + + + /** + * @brief Mean value of a Q15 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_mean_q15( + q15_t * pSrc, + uint32_t blockSize, + q15_t * pResult); + + + /** + * @brief Mean value of a Q31 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_mean_q31( + q31_t * pSrc, + uint32_t blockSize, + q31_t * pResult); + + + /** + * @brief Mean value of a floating-point vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_mean_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult); + + + /** + * @brief Variance of the elements of a floating-point vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_var_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult); + + + /** + * @brief Variance of the elements of a Q31 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_var_q31( + q31_t * pSrc, + uint32_t blockSize, + q31_t * pResult); + + + /** + * @brief Variance of the elements of a Q15 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_var_q15( + q15_t * pSrc, + uint32_t blockSize, + q15_t * pResult); + + + /** + * @brief Root Mean Square of the elements of a floating-point vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_rms_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult); + + + /** + * @brief Root Mean Square of the elements of a Q31 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_rms_q31( + q31_t * pSrc, + uint32_t blockSize, + q31_t * pResult); + + + /** + * @brief Root Mean Square of the elements of a Q15 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_rms_q15( + q15_t * pSrc, + uint32_t blockSize, + q15_t * pResult); + + + /** + * @brief Standard deviation of the elements of a floating-point vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_std_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult); + + + /** + * @brief Standard deviation of the elements of a Q31 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_std_q31( + q31_t * pSrc, + uint32_t blockSize, + q31_t * pResult); + + + /** + * @brief Standard deviation of the elements of a Q15 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output value. + */ + void arm_std_q15( + q15_t * pSrc, + uint32_t blockSize, + q15_t * pResult); + + + /** + * @brief Floating-point complex magnitude + * @param[in] pSrc points to the complex input vector + * @param[out] pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + */ + void arm_cmplx_mag_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples); + + + /** + * @brief Q31 complex magnitude + * @param[in] pSrc points to the complex input vector + * @param[out] pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + */ + void arm_cmplx_mag_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples); + + + /** + * @brief Q15 complex magnitude + * @param[in] pSrc points to the complex input vector + * @param[out] pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + */ + void arm_cmplx_mag_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples); + + + /** + * @brief Q15 complex dot product + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[in] numSamples number of complex samples in each vector + * @param[out] realResult real part of the result returned here + * @param[out] imagResult imaginary part of the result returned here + */ + void arm_cmplx_dot_prod_q15( + q15_t * pSrcA, + q15_t * pSrcB, + uint32_t numSamples, + q31_t * realResult, + q31_t * imagResult); + + + /** + * @brief Q31 complex dot product + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[in] numSamples number of complex samples in each vector + * @param[out] realResult real part of the result returned here + * @param[out] imagResult imaginary part of the result returned here + */ + void arm_cmplx_dot_prod_q31( + q31_t * pSrcA, + q31_t * pSrcB, + uint32_t numSamples, + q63_t * realResult, + q63_t * imagResult); + + + /** + * @brief Floating-point complex dot product + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[in] numSamples number of complex samples in each vector + * @param[out] realResult real part of the result returned here + * @param[out] imagResult imaginary part of the result returned here + */ + void arm_cmplx_dot_prod_f32( + float32_t * pSrcA, + float32_t * pSrcB, + uint32_t numSamples, + float32_t * realResult, + float32_t * imagResult); + + + /** + * @brief Q15 complex-by-real multiplication + * @param[in] pSrcCmplx points to the complex input vector + * @param[in] pSrcReal points to the real input vector + * @param[out] pCmplxDst points to the complex output vector + * @param[in] numSamples number of samples in each vector + */ + void arm_cmplx_mult_real_q15( + q15_t * pSrcCmplx, + q15_t * pSrcReal, + q15_t * pCmplxDst, + uint32_t numSamples); + + + /** + * @brief Q31 complex-by-real multiplication + * @param[in] pSrcCmplx points to the complex input vector + * @param[in] pSrcReal points to the real input vector + * @param[out] pCmplxDst points to the complex output vector + * @param[in] numSamples number of samples in each vector + */ + void arm_cmplx_mult_real_q31( + q31_t * pSrcCmplx, + q31_t * pSrcReal, + q31_t * pCmplxDst, + uint32_t numSamples); + + + /** + * @brief Floating-point complex-by-real multiplication + * @param[in] pSrcCmplx points to the complex input vector + * @param[in] pSrcReal points to the real input vector + * @param[out] pCmplxDst points to the complex output vector + * @param[in] numSamples number of samples in each vector + */ + void arm_cmplx_mult_real_f32( + float32_t * pSrcCmplx, + float32_t * pSrcReal, + float32_t * pCmplxDst, + uint32_t numSamples); + + + /** + * @brief Minimum value of a Q7 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] result is output pointer + * @param[in] index is the array index of the minimum value in the input buffer. + */ + void arm_min_q7( + q7_t * pSrc, + uint32_t blockSize, + q7_t * result, + uint32_t * index); + + + /** + * @brief Minimum value of a Q15 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output pointer + * @param[in] pIndex is the array index of the minimum value in the input buffer. + */ + void arm_min_q15( + q15_t * pSrc, + uint32_t blockSize, + q15_t * pResult, + uint32_t * pIndex); + + + /** + * @brief Minimum value of a Q31 vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output pointer + * @param[out] pIndex is the array index of the minimum value in the input buffer. + */ + void arm_min_q31( + q31_t * pSrc, + uint32_t blockSize, + q31_t * pResult, + uint32_t * pIndex); + + + /** + * @brief Minimum value of a floating-point vector. + * @param[in] pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] pResult is output pointer + * @param[out] pIndex is the array index of the minimum value in the input buffer. + */ + void arm_min_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult, + uint32_t * pIndex); + + +/** + * @brief Maximum value of a Q7 vector. + * @param[in] pSrc points to the input buffer + * @param[in] blockSize length of the input vector + * @param[out] pResult maximum value returned here + * @param[out] pIndex index of maximum value returned here + */ + void arm_max_q7( + q7_t * pSrc, + uint32_t blockSize, + q7_t * pResult, + uint32_t * pIndex); + + +/** + * @brief Maximum value of a Q15 vector. + * @param[in] pSrc points to the input buffer + * @param[in] blockSize length of the input vector + * @param[out] pResult maximum value returned here + * @param[out] pIndex index of maximum value returned here + */ + void arm_max_q15( + q15_t * pSrc, + uint32_t blockSize, + q15_t * pResult, + uint32_t * pIndex); + + +/** + * @brief Maximum value of a Q31 vector. + * @param[in] pSrc points to the input buffer + * @param[in] blockSize length of the input vector + * @param[out] pResult maximum value returned here + * @param[out] pIndex index of maximum value returned here + */ + void arm_max_q31( + q31_t * pSrc, + uint32_t blockSize, + q31_t * pResult, + uint32_t * pIndex); + + +/** + * @brief Maximum value of a floating-point vector. + * @param[in] pSrc points to the input buffer + * @param[in] blockSize length of the input vector + * @param[out] pResult maximum value returned here + * @param[out] pIndex index of maximum value returned here + */ + void arm_max_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult, + uint32_t * pIndex); + + + /** + * @brief Q15 complex-by-complex multiplication + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + */ + void arm_cmplx_mult_cmplx_q15( + q15_t * pSrcA, + q15_t * pSrcB, + q15_t * pDst, + uint32_t numSamples); + + + /** + * @brief Q31 complex-by-complex multiplication + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + */ + void arm_cmplx_mult_cmplx_q31( + q31_t * pSrcA, + q31_t * pSrcB, + q31_t * pDst, + uint32_t numSamples); + + + /** + * @brief Floating-point complex-by-complex multiplication + * @param[in] pSrcA points to the first input vector + * @param[in] pSrcB points to the second input vector + * @param[out] pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + */ + void arm_cmplx_mult_cmplx_f32( + float32_t * pSrcA, + float32_t * pSrcB, + float32_t * pDst, + uint32_t numSamples); + + + /** + * @brief Converts the elements of the floating-point vector to Q31 vector. + * @param[in] pSrc points to the floating-point input vector + * @param[out] pDst points to the Q31 output vector + * @param[in] blockSize length of the input vector + */ + void arm_float_to_q31( + float32_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Converts the elements of the floating-point vector to Q15 vector. + * @param[in] pSrc points to the floating-point input vector + * @param[out] pDst points to the Q15 output vector + * @param[in] blockSize length of the input vector + */ + void arm_float_to_q15( + float32_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Converts the elements of the floating-point vector to Q7 vector. + * @param[in] pSrc points to the floating-point input vector + * @param[out] pDst points to the Q7 output vector + * @param[in] blockSize length of the input vector + */ + void arm_float_to_q7( + float32_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Converts the elements of the Q31 vector to Q15 vector. + * @param[in] pSrc is input pointer + * @param[out] pDst is output pointer + * @param[in] blockSize is the number of samples to process + */ + void arm_q31_to_q15( + q31_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Converts the elements of the Q31 vector to Q7 vector. + * @param[in] pSrc is input pointer + * @param[out] pDst is output pointer + * @param[in] blockSize is the number of samples to process + */ + void arm_q31_to_q7( + q31_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Converts the elements of the Q15 vector to floating-point vector. + * @param[in] pSrc is input pointer + * @param[out] pDst is output pointer + * @param[in] blockSize is the number of samples to process + */ + void arm_q15_to_float( + q15_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Converts the elements of the Q15 vector to Q31 vector. + * @param[in] pSrc is input pointer + * @param[out] pDst is output pointer + * @param[in] blockSize is the number of samples to process + */ + void arm_q15_to_q31( + q15_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Converts the elements of the Q15 vector to Q7 vector. + * @param[in] pSrc is input pointer + * @param[out] pDst is output pointer + * @param[in] blockSize is the number of samples to process + */ + void arm_q15_to_q7( + q15_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @ingroup groupInterpolation + */ + + /** + * @defgroup BilinearInterpolate Bilinear Interpolation + * + * Bilinear interpolation is an extension of linear interpolation applied to a two dimensional grid. + * The underlying function f(x, y) is sampled on a regular grid and the interpolation process + * determines values between the grid points. + * Bilinear interpolation is equivalent to two step linear interpolation, first in the x-dimension and then in the y-dimension. + * Bilinear interpolation is often used in image processing to rescale images. + * The CMSIS DSP library provides bilinear interpolation functions for Q7, Q15, Q31, and floating-point data types. + * + * Algorithm + * \par + * The instance structure used by the bilinear interpolation functions describes a two dimensional data table. + * For floating-point, the instance structure is defined as: + *
+   *   typedef struct
+   *   {
+   *     uint16_t numRows;
+   *     uint16_t numCols;
+   *     float32_t *pData;
+   * } arm_bilinear_interp_instance_f32;
+   * 
+ * + * \par + * where numRows specifies the number of rows in the table; + * numCols specifies the number of columns in the table; + * and pData points to an array of size numRows*numCols values. + * The data table pTable is organized in row order and the supplied data values fall on integer indexes. + * That is, table element (x,y) is located at pTable[x + y*numCols] where x and y are integers. + * + * \par + * Let (x, y) specify the desired interpolation point. Then define: + *
+   *     XF = floor(x)
+   *     YF = floor(y)
+   * 
+ * \par + * The interpolated output point is computed as: + *
+   *  f(x, y) = f(XF, YF) * (1-(x-XF)) * (1-(y-YF))
+   *           + f(XF+1, YF) * (x-XF)*(1-(y-YF))
+   *           + f(XF, YF+1) * (1-(x-XF))*(y-YF)
+   *           + f(XF+1, YF+1) * (x-XF)*(y-YF)
+   * 
+ * Note that the coordinates (x, y) contain integer and fractional components. + * The integer components specify which portion of the table to use while the + * fractional components control the interpolation processor. + * + * \par + * if (x,y) are outside of the table boundary, Bilinear interpolation returns zero output. + */ + + /** + * @addtogroup BilinearInterpolate + * @{ + */ + + + /** + * + * @brief Floating-point bilinear interpolation. + * @param[in,out] S points to an instance of the interpolation structure. + * @param[in] X interpolation coordinate. + * @param[in] Y interpolation coordinate. + * @return out interpolated value. + */ + static __INLINE float32_t arm_bilinear_interp_f32( + const arm_bilinear_interp_instance_f32 * S, + float32_t X, + float32_t Y) + { + float32_t out; + float32_t f00, f01, f10, f11; + float32_t *pData = S->pData; + int32_t xIndex, yIndex, index; + float32_t xdiff, ydiff; + float32_t b1, b2, b3, b4; + + xIndex = (int32_t) X; + yIndex = (int32_t) Y; + + /* Care taken for table outside boundary */ + /* Returns zero output when values are outside table boundary */ + if(xIndex < 0 || xIndex > (S->numRows - 1) || yIndex < 0 || yIndex > (S->numCols - 1)) + { + return (0); + } + + /* Calculation of index for two nearest points in X-direction */ + index = (xIndex - 1) + (yIndex - 1) * S->numCols; + + + /* Read two nearest points in X-direction */ + f00 = pData[index]; + f01 = pData[index + 1]; + + /* Calculation of index for two nearest points in Y-direction */ + index = (xIndex - 1) + (yIndex) * S->numCols; + + + /* Read two nearest points in Y-direction */ + f10 = pData[index]; + f11 = pData[index + 1]; + + /* Calculation of intermediate values */ + b1 = f00; + b2 = f01 - f00; + b3 = f10 - f00; + b4 = f00 - f01 - f10 + f11; + + /* Calculation of fractional part in X */ + xdiff = X - xIndex; + + /* Calculation of fractional part in Y */ + ydiff = Y - yIndex; + + /* Calculation of bi-linear interpolated output */ + out = b1 + b2 * xdiff + b3 * ydiff + b4 * xdiff * ydiff; + + /* return to application */ + return (out); + } + + + /** + * + * @brief Q31 bilinear interpolation. + * @param[in,out] S points to an instance of the interpolation structure. + * @param[in] X interpolation coordinate in 12.20 format. + * @param[in] Y interpolation coordinate in 12.20 format. + * @return out interpolated value. + */ + static __INLINE q31_t arm_bilinear_interp_q31( + arm_bilinear_interp_instance_q31 * S, + q31_t X, + q31_t Y) + { + q31_t out; /* Temporary output */ + q31_t acc = 0; /* output */ + q31_t xfract, yfract; /* X, Y fractional parts */ + q31_t x1, x2, y1, y2; /* Nearest output values */ + int32_t rI, cI; /* Row and column indices */ + q31_t *pYData = S->pData; /* pointer to output table values */ + uint32_t nCols = S->numCols; /* num of rows */ + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + rI = ((X & (q31_t)0xFFF00000) >> 20); + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + cI = ((Y & (q31_t)0xFFF00000) >> 20); + + /* Care taken for table outside boundary */ + /* Returns zero output when values are outside table boundary */ + if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1)) + { + return (0); + } + + /* 20 bits for the fractional part */ + /* shift left xfract by 11 to keep 1.31 format */ + xfract = (X & 0x000FFFFF) << 11u; + + /* Read two nearest output values from the index */ + x1 = pYData[(rI) + (int32_t)nCols * (cI) ]; + x2 = pYData[(rI) + (int32_t)nCols * (cI) + 1]; + + /* 20 bits for the fractional part */ + /* shift left yfract by 11 to keep 1.31 format */ + yfract = (Y & 0x000FFFFF) << 11u; + + /* Read two nearest output values from the index */ + y1 = pYData[(rI) + (int32_t)nCols * (cI + 1) ]; + y2 = pYData[(rI) + (int32_t)nCols * (cI + 1) + 1]; + + /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 3.29(q29) format */ + out = ((q31_t) (((q63_t) x1 * (0x7FFFFFFF - xfract)) >> 32)); + acc = ((q31_t) (((q63_t) out * (0x7FFFFFFF - yfract)) >> 32)); + + /* x2 * (xfract) * (1-yfract) in 3.29(q29) and adding to acc */ + out = ((q31_t) ((q63_t) x2 * (0x7FFFFFFF - yfract) >> 32)); + acc += ((q31_t) ((q63_t) out * (xfract) >> 32)); + + /* y1 * (1 - xfract) * (yfract) in 3.29(q29) and adding to acc */ + out = ((q31_t) ((q63_t) y1 * (0x7FFFFFFF - xfract) >> 32)); + acc += ((q31_t) ((q63_t) out * (yfract) >> 32)); + + /* y2 * (xfract) * (yfract) in 3.29(q29) and adding to acc */ + out = ((q31_t) ((q63_t) y2 * (xfract) >> 32)); + acc += ((q31_t) ((q63_t) out * (yfract) >> 32)); + + /* Convert acc to 1.31(q31) format */ + return ((q31_t)(acc << 2)); + } + + + /** + * @brief Q15 bilinear interpolation. + * @param[in,out] S points to an instance of the interpolation structure. + * @param[in] X interpolation coordinate in 12.20 format. + * @param[in] Y interpolation coordinate in 12.20 format. + * @return out interpolated value. + */ + static __INLINE q15_t arm_bilinear_interp_q15( + arm_bilinear_interp_instance_q15 * S, + q31_t X, + q31_t Y) + { + q63_t acc = 0; /* output */ + q31_t out; /* Temporary output */ + q15_t x1, x2, y1, y2; /* Nearest output values */ + q31_t xfract, yfract; /* X, Y fractional parts */ + int32_t rI, cI; /* Row and column indices */ + q15_t *pYData = S->pData; /* pointer to output table values */ + uint32_t nCols = S->numCols; /* num of rows */ + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + rI = ((X & (q31_t)0xFFF00000) >> 20); + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + cI = ((Y & (q31_t)0xFFF00000) >> 20); + + /* Care taken for table outside boundary */ + /* Returns zero output when values are outside table boundary */ + if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1)) + { + return (0); + } + + /* 20 bits for the fractional part */ + /* xfract should be in 12.20 format */ + xfract = (X & 0x000FFFFF); + + /* Read two nearest output values from the index */ + x1 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI) ]; + x2 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI) + 1]; + + /* 20 bits for the fractional part */ + /* yfract should be in 12.20 format */ + yfract = (Y & 0x000FFFFF); + + /* Read two nearest output values from the index */ + y1 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI + 1) ]; + y2 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI + 1) + 1]; + + /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 13.51 format */ + + /* x1 is in 1.15(q15), xfract in 12.20 format and out is in 13.35 format */ + /* convert 13.35 to 13.31 by right shifting and out is in 1.31 */ + out = (q31_t) (((q63_t) x1 * (0xFFFFF - xfract)) >> 4u); + acc = ((q63_t) out * (0xFFFFF - yfract)); + + /* x2 * (xfract) * (1-yfract) in 1.51 and adding to acc */ + out = (q31_t) (((q63_t) x2 * (0xFFFFF - yfract)) >> 4u); + acc += ((q63_t) out * (xfract)); + + /* y1 * (1 - xfract) * (yfract) in 1.51 and adding to acc */ + out = (q31_t) (((q63_t) y1 * (0xFFFFF - xfract)) >> 4u); + acc += ((q63_t) out * (yfract)); + + /* y2 * (xfract) * (yfract) in 1.51 and adding to acc */ + out = (q31_t) (((q63_t) y2 * (xfract)) >> 4u); + acc += ((q63_t) out * (yfract)); + + /* acc is in 13.51 format and down shift acc by 36 times */ + /* Convert out to 1.15 format */ + return ((q15_t)(acc >> 36)); + } + + + /** + * @brief Q7 bilinear interpolation. + * @param[in,out] S points to an instance of the interpolation structure. + * @param[in] X interpolation coordinate in 12.20 format. + * @param[in] Y interpolation coordinate in 12.20 format. + * @return out interpolated value. + */ + static __INLINE q7_t arm_bilinear_interp_q7( + arm_bilinear_interp_instance_q7 * S, + q31_t X, + q31_t Y) + { + q63_t acc = 0; /* output */ + q31_t out; /* Temporary output */ + q31_t xfract, yfract; /* X, Y fractional parts */ + q7_t x1, x2, y1, y2; /* Nearest output values */ + int32_t rI, cI; /* Row and column indices */ + q7_t *pYData = S->pData; /* pointer to output table values */ + uint32_t nCols = S->numCols; /* num of rows */ + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + rI = ((X & (q31_t)0xFFF00000) >> 20); + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + cI = ((Y & (q31_t)0xFFF00000) >> 20); + + /* Care taken for table outside boundary */ + /* Returns zero output when values are outside table boundary */ + if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1)) + { + return (0); + } + + /* 20 bits for the fractional part */ + /* xfract should be in 12.20 format */ + xfract = (X & (q31_t)0x000FFFFF); + + /* Read two nearest output values from the index */ + x1 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI) ]; + x2 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI) + 1]; + + /* 20 bits for the fractional part */ + /* yfract should be in 12.20 format */ + yfract = (Y & (q31_t)0x000FFFFF); + + /* Read two nearest output values from the index */ + y1 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI + 1) ]; + y2 = pYData[((uint32_t)rI) + nCols * ((uint32_t)cI + 1) + 1]; + + /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 16.47 format */ + out = ((x1 * (0xFFFFF - xfract))); + acc = (((q63_t) out * (0xFFFFF - yfract))); + + /* x2 * (xfract) * (1-yfract) in 2.22 and adding to acc */ + out = ((x2 * (0xFFFFF - yfract))); + acc += (((q63_t) out * (xfract))); + + /* y1 * (1 - xfract) * (yfract) in 2.22 and adding to acc */ + out = ((y1 * (0xFFFFF - xfract))); + acc += (((q63_t) out * (yfract))); + + /* y2 * (xfract) * (yfract) in 2.22 and adding to acc */ + out = ((y2 * (yfract))); + acc += (((q63_t) out * (xfract))); + + /* acc in 16.47 format and down shift by 40 to convert to 1.7 format */ + return ((q7_t)(acc >> 40)); + } + + /** + * @} end of BilinearInterpolate group + */ + + +/* SMMLAR */ +#define multAcc_32x32_keep32_R(a, x, y) \ + a = (q31_t) (((((q63_t) a) << 32) + ((q63_t) x * y) + 0x80000000LL ) >> 32) + +/* SMMLSR */ +#define multSub_32x32_keep32_R(a, x, y) \ + a = (q31_t) (((((q63_t) a) << 32) - ((q63_t) x * y) + 0x80000000LL ) >> 32) + +/* SMMULR */ +#define mult_32x32_keep32_R(a, x, y) \ + a = (q31_t) (((q63_t) x * y + 0x80000000LL ) >> 32) + +/* SMMLA */ +#define multAcc_32x32_keep32(a, x, y) \ + a += (q31_t) (((q63_t) x * y) >> 32) + +/* SMMLS */ +#define multSub_32x32_keep32(a, x, y) \ + a -= (q31_t) (((q63_t) x * y) >> 32) + +/* SMMUL */ +#define mult_32x32_keep32(a, x, y) \ + a = (q31_t) (((q63_t) x * y ) >> 32) + + +#if defined ( __CC_ARM ) + /* Enter low optimization region - place directly above function definition */ + #if defined( ARM_MATH_CM4 ) || defined( ARM_MATH_CM7) + #define LOW_OPTIMIZATION_ENTER \ + _Pragma ("push") \ + _Pragma ("O1") + #else + #define LOW_OPTIMIZATION_ENTER + #endif + + /* Exit low optimization region - place directly after end of function definition */ + #if defined( ARM_MATH_CM4 ) || defined( ARM_MATH_CM7) + #define LOW_OPTIMIZATION_EXIT \ + _Pragma ("pop") + #else + #define LOW_OPTIMIZATION_EXIT + #endif + + /* Enter low optimization region - place directly above function definition */ + #define IAR_ONLY_LOW_OPTIMIZATION_ENTER + + /* Exit low optimization region - place directly after end of function definition */ + #define IAR_ONLY_LOW_OPTIMIZATION_EXIT + +#elif defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) + #define LOW_OPTIMIZATION_ENTER + #define LOW_OPTIMIZATION_EXIT + #define IAR_ONLY_LOW_OPTIMIZATION_ENTER + #define IAR_ONLY_LOW_OPTIMIZATION_EXIT + +#elif defined(__GNUC__) + #define LOW_OPTIMIZATION_ENTER __attribute__(( optimize("-O1") )) + #define LOW_OPTIMIZATION_EXIT + #define IAR_ONLY_LOW_OPTIMIZATION_ENTER + #define IAR_ONLY_LOW_OPTIMIZATION_EXIT + +#elif defined(__ICCARM__) + /* Enter low optimization region - place directly above function definition */ + #if defined( ARM_MATH_CM4 ) || defined( ARM_MATH_CM7) + #define LOW_OPTIMIZATION_ENTER \ + _Pragma ("optimize=low") + #else + #define LOW_OPTIMIZATION_ENTER + #endif + + /* Exit low optimization region - place directly after end of function definition */ + #define LOW_OPTIMIZATION_EXIT + + /* Enter low optimization region - place directly above function definition */ + #if defined( ARM_MATH_CM4 ) || defined( ARM_MATH_CM7) + #define IAR_ONLY_LOW_OPTIMIZATION_ENTER \ + _Pragma ("optimize=low") + #else + #define IAR_ONLY_LOW_OPTIMIZATION_ENTER + #endif + + /* Exit low optimization region - place directly after end of function definition */ + #define IAR_ONLY_LOW_OPTIMIZATION_EXIT + +#elif defined(__CSMC__) + #define LOW_OPTIMIZATION_ENTER + #define LOW_OPTIMIZATION_EXIT + #define IAR_ONLY_LOW_OPTIMIZATION_ENTER + #define IAR_ONLY_LOW_OPTIMIZATION_EXIT + +#elif defined(__TASKING__) + #define LOW_OPTIMIZATION_ENTER + #define LOW_OPTIMIZATION_EXIT + #define IAR_ONLY_LOW_OPTIMIZATION_ENTER + #define IAR_ONLY_LOW_OPTIMIZATION_EXIT + +#endif + + +#ifdef __cplusplus +} +#endif + + +#if defined ( __GNUC__ ) +#pragma GCC diagnostic pop +#endif + +#endif /* _ARM_MATH_H */ + +/** + * + * End of file. + */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/include/qmath.h b/examples/OpenIMU300RI/INS/lib/Core/Math/include/qmath.h new file mode 100644 index 0000000..256e8e2 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/include/qmath.h @@ -0,0 +1,345 @@ +/* ********************************************************************** + * + * Fixed Point Math Library + * + * ********************************************************************** + * + * @file qmath.h + * + * Alex Forencich + * alex@alexelectronics.com + * + * Jordan Rhee + * rhee.jordan@gmail.com + * + * IEEE UCSD + * http://ieee.ucsd.edu + * + * **********************************************************************/ + +#ifndef _QMATH_H_ +#define _QMATH_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef INLINE +#ifdef _MSC_VER +#define INLINE __inline +#else +#define INLINE inline +#endif /* _MSC_VER */ +#endif /* INLINE */ + +#include + +/* + * Default fractional bits. This precision is used in the routines + * and macros without a leading underscore. + * For example, if you are mostly working with values that come from + * a 10-bit A/D converter, you may want to choose 21. This leaves 11 + * bits for the whole part, which will help avoid overflow in addition. + * On ARM, bit shifts require a single cycle, so all fracbits + * require the same amount of time to compute and there is no advantage + * to selecting fracbits that are a multiple of 8. + */ +#define FIXED_FRACBITS 24 + +#define FIXED_RESOLUTION (1 << FIXED_FRACBITS) +#define FIXED_INT_MASK (0xffffffffL << FIXED_FRACBITS) +#define FIXED_FRAC_MASK (~FIXED_INT_MASK) + +// square roots +#define FIXED_SQRT_ERR 1 << (FIXED_FRACBITS - 10) + +// fixedp2a +#define FIXED_DECIMALDIGITS 6 + +typedef long fixedp; // int32_t + +// conversions for arbitrary fracbits +#define _short2q(x, fb) ((fixedp)((x) << (fb))) +#define _int2q(x, fb) ((fixedp)((x) << (fb))) +#define _long2q(x, fb) ((fixedp)((x) << (fb))) +#define _float2q(x, fb) ((fixedp)((x) * (1 << (fb)))) +#define _double2q(x, fb) ((fixedp)((x) * (1 << (fb)))) + +// conversions for default fracbits +#define short2q(x) _short2q(x, FIXED_FRACBITS) +#define int2q(x) _int2q(x, FIXED_FRACBITS) +#define long2q(x) _long2q(x, FIXED_FRACBITS) +#define float2q(x) _float2q(x, FIXED_FRACBITS) +#define double2q(x) _double2q(x, FIXED_FRACBITS) + +// conversions for arbitrary fracbits +#define _q2short(x, fb) (( short)((x) >> (fb))) +#define _q2int(x, fb) ( (int)((x) >> (fb))) +#define _q2long(x, fb) ( (long)((x) >> (fb))) +#define _q2float(x, fb) (( float)(x) / (1 << (fb))) +#define _q2double(x, fb) ((double)(x) / (1 << (fb))) + +// conversions for default fracbits +#define q2short(x) _q2short(x, FIXED_FRACBITS) +#define q2int(x) _q2int(x, FIXED_FRACBITS) +#define q2long(x) _q2long(x, FIXED_FRACBITS) +#define q2float(x) _q2float(x, FIXED_FRACBITS) +#define q2double(x) _q2double(x, FIXED_FRACBITS) + +// evaluates to the whole (integer) part of x +#define qipart(x) q2long(x) + +// evaluates to the fractional part of x +#define qfpart(x) ((x) & FIXED_FRAC_MASK) + +/* + * Constants + */ +#define _QPI 3.1415926535897932384626433832795 +#define QPI double2q(_QPI) +#define _Q2PI 6.283185307179586476925286766559 +#define Q2PI double2q(_Q2PI) +#define _QPIO2 1.5707963267948966192313216916398 +#define QPIO2 double2q(_QPIO2) +#define _QPIO4 0.78539816339744830961566084581988 +#define QPIO4 double2q(_QPIO4) +#define _QLN_E 2.71828182845904523536 +#define QLN_E double2q(_QLN_E) +#define _QLN_10 2.30258509299404568402 +#define QLN_10 double2q(_QLN_10) +#define _Q1OLN_10 0.43429448190325182765 +#define Q1OLN_10 double2q(_Q1OLN_10) + +// Note: while the angles are specified in degrees (i.e. ninety), the representation is actually in +// radians (90 deg = pi/2 rad). In Q29 format: round( ( 90 * pi/180 ) * 2^29 ) +#define FORTY_FIVE_DEGREES_Q29 421657428 +#define NINETY_DEGREES_Q29 843314857 +#define ONE_HUNDRED_THIRTY_FIVE_DEGREES_Q29 1264972285 +#define ONE_HUNDRED_EIGHTY_DEGS_Q29 1686629713 + +#define ONE_HALF_Q30 536870912 + +#define ONE_Q27 134217728 +#define ONE_Q29 536870912 +#define ONE_Q30 1073741824 + +#define TWO_Q27 268435456 +#define TWO_Q28 536870912 +#define TWO_Q29 1073741824 + +#define TWO_POW_27 134217728 +#define TWO_POW_28 268435456 +#define TWO_POW_29 536870912 +#define TWO_POW_30 1073741824 + +#define ONE_OVER_TWO_POW_27 7.450580596923828e-09 +#define ONE_OVER_TWO_POW_28 3.725290298461914e-09 +#define ONE_OVER_TWO_POW_29 1.862645149230957e-09 +#define ONE_OVER_TWO_POW_30 9.313225746154785e-10 + +// Both operands in addition and subtraction must have the same fracbits. +// If you need to add or subtract fixed point numbers with different +// fracbits, then use q2q to convert each operand beforehand. +#define qadd(a, b) ((a) + (b)) +#define qsub(a, b) ((a) - (b)) + +#define q27ToDouble(x) ( ( (double)x ) * ONE_OVER_TWO_POW_27 ) +#define q29ToDouble(x) ( ( (double)x ) * ONE_OVER_TWO_POW_29 ) +#define q30ToDouble(x) ( ( (double)x ) * ONE_OVER_TWO_POW_30 ) + +#define q27ToFloat(x) ((real)((x)*ONE_OVER_TWO_POW_27)) +#define q29ToFloat(x) ((real)((x)*ONE_OVER_TWO_POW_29)) + +#define doubleToQ27(x) ( (int32_t)( x * (double)TWO_POW_27 ) ) +#define doubleToQ29(x) ( (int32_t)( x * (double)TWO_POW_29 ) ) +#define doubleToQ30(x) ( (int32_t)( x * (double)TWO_POW_30 ) ) + +#define floatToQ27(x) ((int32_t)(x*(double)TWO_POW_27)) + + +#define PI_q29 1686629713 +#define TWO_PI_q29 3373259426 + + +/** + * q2q - convert one fixed point type to another + * x - the fixed point number to convert + * xFb - source fracbits + * yFb - destination fracbits + */ +static INLINE fixedp q2q(fixedp x, int xFb, int yFb) +{ + if(xFb == yFb) { + return x; + } else if(xFb < yFb) { + return x << (yFb - xFb); + } else { + return x >> (xFb - yFb); + } +} + +/** + * Multiply two fixed point numbers with arbitrary fracbits + * x - left operand + * y - right operand + * xFb - number of fracbits for X + * yFb - number of fracbits for Y + * resFb - number of fracbits for the result + */ +#define _qmul(x, y, xFb, yFb, resFb) ((fixedp)(((long long)(x) * (long long)(y)) >> ((xFb) + (yFb) - (resFb)))) + +/** + * Fixed point multiply for default fracbits. + */ +#define qmul(x, y) _qmul(x, y, FIXED_FRACBITS, FIXED_FRACBITS, FIXED_FRACBITS) + +/** + * divide +* shift into 64 bits and divide, then truncate (corrected - jmotyka: may 13, 2014) + */ +// Formulation: Idea is to compute the floating point value and scale to output Q-value +// +// ( ( x / 2^n ) / ( y / 2^m ) ) * 2^N = ( ( x / 2^n ) * ( 2^m / y ) ) * 2^N +// = ( x / y ) * 2^( m-n+N ) +// = ( x / y ) << m + N - n +// = ( x / y ) / 2^( n-m-N ) +// = ( x / y ) >> n - m - N +//#define _qdiv(x, y, xFb, yFb, resFb) ((fixedp)((((long long)x) << ((xFb) + (yFb) - (resFb))) / y)) --> Original: incorrect bit-shift +#define _qdiv(x, y, xFb, yFb, resFb) ((fixedp)((((long long)x) << ((yFb) + (resFb) - (xFb))) / y)) + +/** + * Fixed point divide for default fracbbits. + */ +#define qdiv(x, y) _qdiv(x, y, FIXED_FRACBITS, FIXED_FRACBITS, FIXED_FRACBITS) + +/** + * Invert a number (x^-1) for arbitrary fracbits + */ +#define _qinv(x, xFb, resFb) ((fixedp)((((long long)1) << (xFb + resFb)) / x)) + +/** + * Invert a number with default fracbits. + */ +#define qinv(x) _qinv(x, FIXED_FRACBITS, FIXED_FRACBITS); + +/** + * Modulus. Modulus is only defined for two numbers of the same fracbits + */ +#define qmod(x, y) ((x) % (y)) + +/** + * Absolute value. Works for any fracbits. + */ +#define qabs(x) (((x) < 0) ? (-x) : (x)) + +/** + * Floor for arbitrary fracbits + */ +#define _qfloor(x, fb) ((x) & (0xffffffff << (fb))) + +/** + * Floor for default fracbits + */ +#define qfloor(x) _qfloor(x, FIXED_FRACBITS) + +/** + * ceil for arbitrary fracbits. + */ +static INLINE fixedp _qceil(fixedp x, int fb) +{ + // masks off fraction bits and adds one if there were some fractional bits + fixedp f = _qfloor(x, fb); + if (f != x) return qadd(f, _int2q(1, fb)); + return x; +} + +/** + * ceil for default fracbits + */ +#define qceil(x) _qceil(x, FIXED_FRACBITS) + + +/** + * square root for default fracbits + */ +fixedp qsqrt(fixedp p_Square); + +/** + * log (base e) for default fracbits + */ +fixedp qlog( fixedp p_Base ); + +/** + * log base 10 for default fracbits + */ +fixedp qlog10( fixedp p_Base ); + +/** + * exp (e to the x) for default fracbits + */ +fixedp qexp(fixedp p_Base); + +/** + * pow for default fracbits + */ +fixedp qpow( fixedp p_Base, fixedp p_Power ); + +/** + * sine for default fracbits + */ +fixedp qsin(fixedp theta); + +/** + * cosine for default fracbits + */ +fixedp qcos(fixedp theta); + +/** + * tangent for default fracbits + */ +fixedp qtan(fixedp theta); + +/** + * fixedp2a - converts a fixed point number with default fracbits to a string + */ +char *q2a(char *buf, fixedp n); + +// Trigonometric functions +int32_t sin_q30( int32_t angleRad_q29 ); +int32_t cos_q30( int32_t angleRad_q29 ); + +int32_t asin_q27( int32_t x_q27 ); +int32_t asin_q29( int32_t x_q30 ); + +// Inverse trigonometric functions +int32_t atan2_q27( int32_t y, int32_t x ); +int32_t atan2Old_q27( int32_t y_q27, int32_t x_q27 ); + +int32_t atan2_q29( int32_t y_qX, int32_t x_qX, uint8_t qVal ); + +//int32_t nabs_q27( int32_t signedInteger ); +int32_t nabs( int32_t signedInteger ); +int32_t nabs_32( int32_t signedInteger ); +int64_t nabs_64( int64_t signedInteger ); +int32_t sign( int32_t signedInteger ); + + + +int32_t qsqrt_q23( int32_t pSquare_q23 ); +int32_t qsqrt_q27( int32_t pSquare_q27 ); +int32_t qsqrt_q29( int32_t pSquare_q29 ); +int32_t qsqrt_q30( int32_t pSquare_q30 ); + +void VectorNormalize_q30( int32_t *vectorIn_q27, int32_t *vectorOut_q30 ); +int32_t VectorMag_q27( int32_t *vectorIn_q27 ); +void VectorCrossProduct_q27( int32_t *vect1_q27, int32_t *vect2_q27, int32_t *vectOut_q27 ); +int32_t VectorDotProduct_q27( int32_t *vect1_q27, int32_t *vect2_q27 ); + +void firstOrderLowPass_q27( int32_t *output_q27, int32_t *input_q27, int32_t *inputPast_q27, uint8_t shiftCoeff ); + +#ifdef __cplusplus +} // extern C +#endif + + +#endif /* _QMATH_H_ */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/include/xmath.h b/examples/OpenIMU300RI/INS/lib/Core/Math/include/xmath.h new file mode 100644 index 0000000..37db1f3 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/include/xmath.h @@ -0,0 +1,26 @@ +/* ********************************************************************** + * + * supplement header file to build math library + * + * **********************************************************************/ + +#ifndef _XMATH_H +#define _XMATH_H + +// These are in IQMath.h + +typedef float iq27; +typedef float iq30; +typedef float iq23; +typedef float iq29; + +#define IQ27(x) (x) + + + +#define MAXUINT32 4294967295 ///< max unsigned 32 bit int=> ((2^32)-1) +#define MAXUINT16 65535 ///< max unsigned 16 bit int=> ((2^16)-1) +#define MAXINT16 ( 32767) ///< max signed 16 bit int=> ((2^15)-1) +#define MININT16 (-32768) ///< max negative signed 16 bit int=> (-(2^15)) + +#endif /* XMATH_H_ */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/src/FastInvTrigFuncs.c b/examples/OpenIMU300RI/INS/lib/Core/Math/src/FastInvTrigFuncs.c new file mode 100644 index 0000000..417ea0f --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/src/FastInvTrigFuncs.c @@ -0,0 +1,272 @@ +// Inverse trig function approximations +// https://github.com/jwr/sfl-sensor-fusion-lib/blob/master/approximations.c + + +#include // For uint8_t, etc +#include "math.h" // For sqrtf + +#include "FastInvTrigFuncs.h" + +// function returns angle in range -90 to 90 deg +// maximum error is 9.84E-6 deg +float fatan_deg(float x) +{ + float fangledeg; // compute computed (deg) + int8_t ixisnegative; // argument x is negative + uint8_t ixexceeds1; // argument x is greater than 1.0 + uint8_t ixmapped; // argument in range tan(15 deg) to tan(45 deg)=1.0 + +#define TAN15DEG 0.26794919243F // tan(15 deg) = 2 - sqrt(3) +#define TAN30DEG 0.57735026919F // tan(30 deg) = 1/sqrt(3) + + // reset all flags + ixisnegative = ixexceeds1 = ixmapped = 0; + + // test for negative argument to allow use of tan(-x)=-tan(x) + if (x < 0.0F) + { + x = -x; + ixisnegative = 1; + } + + // test for argument above 1 to allow use of atan(x)=pi/2-atan(1/x) + if (x > 1.0F) + { + x = 1.0F / x; + ixexceeds1 = 1; + } + + // at this point, x is in the range 0 to 1 inclusive + // map argument onto range -tan(15 deg) to tan(15 deg) + // using tan(angle-30deg) = (tan(angle)-tan(30deg)) / (1 + tan(angle)tan(30deg)) + // tan(15deg) maps to tan(-15 deg) = -tan(15 deg) + // 1. maps to (sqrt(3) - 1) / (sqrt(3) + 1) = 2 - sqrt(3) = tan(15 deg) + if (x > TAN15DEG) + { + x = (x - TAN30DEG)/(1.0F + TAN30DEG * x); + ixmapped = 1; + } + + // call the atan estimator to obtain -15 deg <= angle <= 15 deg + fangledeg = fatan_15deg(x); + + // undo the distortions applied earlier to obtain -90 deg <= angle <= 90 deg + if (ixmapped) fangledeg += 30.0F; + if (ixexceeds1) fangledeg = 90.0F - fangledeg; + if (ixisnegative) fangledeg = -fangledeg; + + return (fangledeg); +} + + +// function returns angle in range -90 to 90 deg +// maximum error is 9.84E-6 deg +float fatan_rad(float x) +{ + float fanglerad; // compute computed (deg) + int8_t ixisnegative; // argument x is negative + uint8_t ixexceeds1; // argument x is greater than 1.0 + uint8_t ixmapped; // argument in range tan(15 deg) to tan(45 deg)=1.0 + +#define TAN15DEG 0.26794919243F // tan(15 deg) = 2 - sqrt(3) +#define TAN30DEG 0.57735026919F // tan(30 deg) = 1/sqrt(3) + + // reset all flags + ixisnegative = ixexceeds1 = ixmapped = 0; + + // test for negative argument to allow use of tan(-x)=-tan(x) + if (x < 0.0F) + { + x = -x; + ixisnegative = 1; + } + + // test for argument above 1 to allow use of atan(x)=pi/2-atan(1/x) + if (x > 1.0F) + { + x = 1.0F / x; + ixexceeds1 = 1; + } + + // at this point, x is in the range 0 to 1 inclusive + // map argument onto range -tan(15 deg) to tan(15 deg) + // using tan(angle-30deg) = (tan(angle)-tan(30deg)) / (1 + tan(angle)tan(30deg)) + // tan(15deg) maps to tan(-15 deg) = -tan(15 deg) + // 1. maps to (sqrt(3) - 1) / (sqrt(3) + 1) = 2 - sqrt(3) = tan(15 deg) + if (x > TAN15DEG) + { + x = (x - TAN30DEG)/(1.0F + TAN30DEG * x); + ixmapped = 1; + } + + // call the atan estimator to obtain -15 deg <= angle <= 15 deg + fanglerad = fatan_15deg_rad(x); + +#define PI_OVER_SIX 0.5235987755983F +#define PI_OVER_TWO 1.570796326794897F + + // undo the distortions applied earlier to obtain -90 deg <= angle <= 90 deg + if (ixmapped) { fanglerad += PI_OVER_SIX; } + if (ixexceeds1) { fanglerad = PI_OVER_TWO - fanglerad; } + if (ixisnegative) { fanglerad = -fanglerad; } + + return (fanglerad); +} + + +// function returns approximate atan2 angle in range -180 to 180 deg +// maximum error is 14.58E-6 deg +float fatan2_deg(float y, float x) +{ + // check for zero x to avoid division by zero + if (x == 0.0F) + { + // return 90 deg for positive y + if (y > 0.0F) return 90.0F; + // return -90 deg for negative y + if (y < 0.0F) return -90.0F; + // otherwise y= 0.0 and return 0 deg (invalid arguments) + return 0.0F; + } + + // from here onwards, x is guaranteed to be non-zero + // compute atan2 for quadrant 1 (0 to 90 deg) and quadrant 4 (-90 to 0 deg) + if (x > 0.0F) return (fatan_deg(y / x)); + // compute atan2 for quadrant 2 (90 to 180 deg) + if ((x < 0.0F) && (y > 0.0F)) return (180.0F + fatan_deg(y / x)); + // compute atan2 for quadrant 3 (-180 to -90 deg) + return (-180.0F + fatan_deg(y / x)); +} + + +// function returns approximate atan2 angle in range -180 to 180 deg +// maximum error is 14.58E-6 deg +float fatan2_rad(float y, float x) +{ +#define PI 3.141592653589793F + + // check for zero x to avoid division by zero + if (x == 0.0F) + { + // return 90 deg for positive y + if (y > 0.0F) return 90.0F; + // return -90 deg for negative y + if (y < 0.0F) return -90.0F; + // otherwise y= 0.0 and return 0 deg (invalid arguments) + return 0.0F; + } + + // from here onwards, x is guaranteed to be non-zero + // compute atan2 for quadrant 1 (0 to 90 deg) and quadrant 4 (-90 to 0 deg) + if (x > 0.0F) return (fatan_rad(y / x)); + // compute atan2 for quadrant 2 (90 to 180 deg) + if ((x < 0.0F) && (y > 0.0F)) return (PI + fatan_rad(y / x)); + // compute atan2 for quadrant 3 (-180 to -90 deg) + return (-PI + fatan_rad(y / x)); +} + + +// approximation to inverse tan function (deg) for x in range +// -tan(15 deg) to tan(15 deg) giving an output -15 deg <= angle <= 15 deg +// using modified Pade[3/2] approximation +float fatan_15deg(float x) +{ + float xSq; // x^2 + +#define PADE_A 96.644395816F // theoretical Pade[3/2] value is 5/3*180/PI=95.49296 +#define PADE_B 25.086941612F // theoretical Pade[3/2] value is 4/9*180/PI=25.46479 +#define PADE_C 1.6867633134F // theoretical Pade[3/2] value is 5/3=1.66667 + + // compute the approximation to the inverse tangent + // the function is anti-symmetric as required for positive and negative arguments + xSq = x * x; + return (x * (PADE_A + xSq * PADE_B) / (PADE_C + xSq)); +} + + +// approximation to inverse tan function (deg) for x in range +// -tan(15 deg) to tan(15 deg) giving an output +// -0.261799388 [rad] <= angle <= 0.261799388 [rad] using +// modified Pade[3/2] approximation +float fatan_15deg_rad(float x) +{ + float xSq; // x^2 + +#define PADE_Ar 1.6867629106F // theoretical Pade[3/2] value is 5/3 = 1.66667 +#define PADE_Br 0.4378497304F // theoretical Pade[3/2] value is 4/9 = 0.44444 +#define PADE_Cr 1.6867633134F // theoretical Pade[3/2] value is 5/3 = 1.66667 + + // compute the approximation to the inverse tangent + // the function is anti-symmetric as required for positive and negative arguments + xSq = x * x; + return (x * (PADE_Ar + xSq * PADE_Br) / (PADE_Cr + xSq)); +} + + +// function returns an approximation to angle(deg)=asin(x) for x in the range -1 <= x <= 1 +// and returns -90 <= angle <= 90 deg +// maximum error is 10.29E-6 deg +float fasin_deg(float x) +{ + // for robustness, check for invalid argument + if (x >= 1.0F) return 90.0F; + if (x <= -1.0F) return -90.0F; + + // call the atan which will return an angle in the correct range -90 to 90 deg + // this line cannot fail from division by zero or negative square root since |x| < 1 + return (fatan_deg(x / sqrtf(1.0F - x * x))); +} + + +// function returns an approximation to angle(deg)=asin(x) for x in the range -1 <= x <= 1 +// and returns -90 <= angle <= 90 deg +// maximum error is 10.29E-6 deg +float fasin_rad(float x) +{ +#define PI_OVER_TWO 1.570796326794897F + + // for robustness, check for invalid argument + if (x >= 1.0F) return PI_OVER_TWO; + if (x <= -1.0F) return -PI_OVER_TWO; + + // call the atan which will return an angle in the correct range -90 to 90 deg + // this line cannot fail from division by zero or negative square root since |x| < 1 + return (fatan_rad(x / sqrtf(1.0F - x * x))); +} + + +// function returns an approximation to angle(deg)=acos(x) for x in the range -1 <= x <= 1 +// and returns 0 <= angle <= 180 deg +// maximum error is 14.67E-6 deg +float facos_deg(float x) +{ + // for robustness, check for invalid arguments + if (x >= 1.0F) return 0.0F; + if (x <= -1.0F) return 180.0F; + + // call the atan which will return an angle in the incorrect range -90 to 90 deg + // these lines cannot fail from division by zero or negative square root + if (x == 0.0F) return 90.0F; + if (x > 0.0F) return fatan_deg((sqrtf(1.0F - x * x) / x)); + return 180.0F + fatan_deg((sqrtf(1.0F - x * x) / x)); +} + + +// function returns an approximation to angle(deg)=acos(x) for x in the range -1 <= x <= 1 +// and returns 0 <= angle <= 180 deg +// maximum error is 14.67E-6 deg +float facos_rad(float x) +{ +#define PI 3.141592653589793F +#define PI_OVER_TWO 1.570796326794897F + + // for robustness, check for invalid arguments + if (x >= 1.0F) return 0.0F; + if (x <= -1.0F) return PI; + + // call the atan which will return an angle in the incorrect range -90 to 90 deg + // these lines cannot fail from division by zero or negative square root + if (x == 0.0F) return PI_OVER_TWO; + if (x > 0.0F) return fatan_rad((sqrtf(1.0F - x * x) / x)); + return PI + fatan_rad((sqrtf(1.0F - x * x) / x)); +} diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/src/MatrixMath.c b/examples/OpenIMU300RI/INS/lib/Core/Math/src/MatrixMath.c new file mode 100644 index 0000000..c2ba6dc --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/src/MatrixMath.c @@ -0,0 +1,560 @@ +/** *************************************************************************** +* @file matrixMath.c real precision Linear algebra calculation functions +* @author +* @date September, 2008 +* @copyright (c) 2013, 2014 All Rights Reserved. +* @section LICENSE +* 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. +* @details +* NOTE these count on the matrices being defined per the inputs. +* They use pointer math instead of array indexing so you can't pass in a +* 7x3 matrix and attempt to calculate using the first two columns +*****************************************************************************/ + +#include "MatrixMath.h" + +/** **************************************************************************** +* @name: ATimesBTranspose A*B' +* @brief Compute A x BTranspose (Only the B matrix is provided to the function, +* the inverse is performed automatically). Tested June 25, 2014. +* TRACE: +* @param [in] A matrix +* @param [in] B matrix +* @param [in] A[rowsInA][] +* @param [in] A[][colsInA] +* @param [in] B[rowsInB] +* @param [out] C[][] +* @retval always 1 +******************************************************************************/ +uint8_t AxBTranspose( real *A, + real *B, + uint8_t rowsInA, + uint8_t colsInA, + uint8_t rowsInB, + real *C ) +{ + uint8_t rowNum, colNum, multIndex; + + // Compute A*transpose( B ) + for (rowNum = 0; rowNum < rowsInA; rowNum++) { + for (colNum = 0; colNum < rowsInB; colNum++) { + *(C + rowNum*rowsInB + colNum) = 0.0; + for (multIndex = 0; multIndex < colsInA; multIndex++) { + *(C + rowNum*rowsInB + colNum) = *(C + rowNum*rowsInB + colNum) + + *(A + rowNum*colsInA + multIndex) * *(B + colNum*colsInA + multIndex); + } + } + } + return 1; +} + + +/** **************************************************************************** +* @name: ATimesB A * B +* @brief Compute A * B Tested June 25, 2014. +* TRACE: +* @param [in] A matrix +* @param [in] B matrix +* @param [in] A[rowsInA][] +* @param [in] A[][colsInA] +* @param [in] B[rowsInB] +* @param [out] C[][] +* @retval always 1 +******************************************************************************/ +uint8_t AxB( real *A, + real *B, + uint8_t rowsInA, + uint8_t colsInA, + uint8_t colsInB, + real *C ) +{ + uint8_t rowNum, colNum, multIndex; + + // Compute A * B + for (rowNum = 0; rowNum < rowsInA; rowNum++) { + for (colNum = 0; colNum < colsInB; colNum++) { + *(C + rowNum*colsInB + colNum) = 0.0; + for (multIndex = 0; multIndex < colsInA; multIndex++) { + *(C + rowNum*colsInB + colNum) = *(C + rowNum*colsInB + colNum) + + *(A + rowNum*colsInA + multIndex) * *(B + colNum + colsInB*multIndex); + } + } + } + return 1; +} + + +/** **************************************************************************** +* @name: ATimesV A * V multiply a matrix by a vector +* @brief Compute A * V Tested June 25, 2014. Assumes the vector is the right +* size +* TRACE: +* @param [in] A matrix +* @param [in] V vector +* @param [in] A[rowsInA][] +* @param [in] A[][colsInA] +* @param [out] C[][] +* @retval always 1 +******************************************************************************/ +uint8_t AxV( real *A, + real *V, + uint8_t rowsInA, + uint8_t colsInA, + real *C ) +{ + uint8_t rowNum, multIndex; + + for (rowNum = 0; rowNum < rowsInA; rowNum++) { + *(C + rowNum) = 0.0; + for (multIndex = 0; multIndex < colsInA; multIndex++) { + *(C + rowNum) = *(C + rowNum) + *(A + rowNum*colsInA + multIndex) * *(V + multIndex); + } + } + + return 1; +} + + +/** **************************************************************************** +* @name: ATimesBTranspose V1 dot V2 +* @brief Compute A' * B Tested June 25, 2014. Assumes the vectors are the same +* size +* TRACE: +* @param [in] V1 vector +* @param [in] V2 vector +* @param [in] [size] +* @retval the calculated dot product +******************************************************************************/ +real DotProduct(real *V1, + real *V2, + uint8_t size) +{ + real VOut = 0.0; + uint8_t rowNum = 0; + + for (rowNum = 0; rowNum < size; rowNum++) { + VOut = VOut + *(V1 + rowNum) * *(V2 + rowNum); + } + + return VOut; +} + + +/** **************************************************************************** +* @name: APlusB A + B +* @brief Compute A + B Tested June 25, 2014. Assumes the matrices are the same +* size +* TRACE: +* @param [in] A matrix +* @param [in] B matrix +* @param [in] A[rowsInA][] +* @param [in] A[][colsInA] +* @retval always 1 +******************************************************************************/ +uint8_t APlusB( real *A, + real *B, + uint8_t rowsInA, + uint8_t colsInA, + real *C ) +{ + uint8_t rowNum = 0, colNum = 0; + + for (rowNum = 0; rowNum < rowsInA; rowNum++) { + for (colNum = 0; colNum < colsInA; colNum++) { + *(C + rowNum*colsInA + colNum) = *(A + rowNum*colsInA + colNum) + + *(B + rowNum*colsInA + colNum); + } + } + + return 1; +} + + +/** **************************************************************************** +* @name: APlusB A * c +* @brief Compute C = A * c where c is a scaler +* TRACE: +* @param [in] A matrix +* @param [in] c scaler +* @param [in] A[rowsInA][] +* @param [in] A[][colsInA] +* @param [out] C[][] result +* @retval always 1 +******************************************************************************/ +uint8_t AxScalar( real *A, + real c, + uint8_t rowsInA, + uint8_t colsInA, + real *C ) +{ + uint8_t rowNum = 0, colNum = 0; + + for (rowNum = 0; rowNum < rowsInA; rowNum++) { + for (colNum = 0; colNum < colsInA; colNum++) { + *(C + rowNum*colsInA + colNum) = c * *(A + rowNum*colsInA + colNum); + } + } + + return 1; +} + +/** **************************************************************************** +* @name: AMinusB A - B +* @brief Compute C = A - B Tested June 25, 2014. +* TRACE: +* @param [in] A matrix +* @param [in] B matrix +* @param [in] A[rowsInA][] +* @param [in] A[][colsInA] +* @param [out] C[][] result +* @retval always 1 +******************************************************************************/ +uint8_t AMinusB( real *A, + real *B, + uint8_t rowsInA, + uint8_t colsInA, + real *C ) +{ + uint8_t rowNum = 0, colNum = 0; + + for (rowNum = 0; rowNum < rowsInA; rowNum++) { + for (colNum = 0; colNum < colsInA; colNum++) { + *(C + rowNum*colsInA + colNum) = *(A + rowNum*colsInA + colNum) - + *(B + rowNum*colsInA + colNum); + } + } + return 1; +} + + +/** **************************************************************************** +* @name: matrixInverse_2x2 A +* @brief Compute A = 1/A Tested June 25, 2014. Assumes the matrix is invertible.) +* TRACE: +* @param [in] A matrix +* @param [out] A[][] result +* @retval always 1 +******************************************************************************/ +uint8_t matrixInverse_2x2( real *A, + real *AInverse ) +{ + real determinant = 1.0, multiplier = 1.0; + + determinant = *(A + 0) * *(A + 3) - *(A + 1) * *(A + 2); + multiplier = (real)1.0 / determinant; + + *(AInverse + 0) = multiplier * *(A + 3); + *(AInverse + 1) = -multiplier * *(A + 1); + *(AInverse + 2) = -multiplier * *(A + 2); + *(AInverse + 3) = multiplier * *(A + 0); + + return 1; +} + + +/** **************************************************************************** + * @name: EnsureSymmetricMatrix A + * @brief Compute A = 1/A Tested June 25, 2014. + * TRACE: + * @param [in/out] A matrix + * @param [in] A[rowsInA][] + * @param [in] A[][colsInA] + * @retval always 1 + **************************************************************************** **/ +void ForceMatrixSymmetry( real *A, uint8_t rowsInA, uint8_t colsInA ) +{ + uint8_t rowNum, colNum; + + for (rowNum = 0; rowNum < rowsInA; rowNum++) { + for (colNum = 0; colNum < rowNum; colNum++) { + *(A + colNum*colsInA + rowNum) = *(A + rowNum*colsInA + colNum); + } + } +} + +void ForceMatrixSymmetry_avg( real *A, uint8_t rowsInA, uint8_t colsInA ) +{ + uint8_t rowNum, colNum; + + for (rowNum = 0; rowNum < rowsInA; rowNum++) { + for (colNum = 0; colNum < rowNum; colNum++) { + *(A + rowNum*colsInA + colNum) = (real)0.5 * ( *(A + rowNum*colsInA + colNum) + + *(A + colNum*colsInA + rowNum) ); + *(A + colNum*colsInA + rowNum) = *(A + rowNum*colsInA + colNum); + } + } +} + +void LimitValuesAndForceMatrixSymmetry_avg(real* A, real limit, uint8_t rowsInA, uint8_t colsInA) +{ + uint8_t rowNum, colNum; + + for (rowNum = 0; rowNum < rowsInA; rowNum++) { + for (colNum = 0; colNum < rowNum; colNum++) { + *(A + rowNum*colsInA + colNum) = (real)0.5 * ( *(A + rowNum*colsInA + colNum) + + *(A + colNum*colsInA + rowNum) ); + + if (*(A + rowNum*colsInA + colNum) > limit) { + *(A + rowNum*colsInA + colNum) = limit; + } else if (*(A + rowNum*colsInA + colNum) < -limit) { + *(A + rowNum*colsInA + colNum) = -limit; + } + + *(A + colNum*colsInA + rowNum) = *(A + rowNum*colsInA + colNum); + } + } +} + +void LimitValuesAndForceMatrixSymmetry_noAvg(real* A, real limit, uint8_t rowsInA, uint8_t colsInA) +{ + uint8_t rowNum, colNum; + + for (rowNum = 0; rowNum < rowsInA; rowNum++) { + for (colNum = 0; colNum < rowNum; colNum++) { + if (*(A + rowNum*colsInA + colNum) > limit) { + *(A + rowNum*colsInA + colNum) = limit; + } else if (*(A + rowNum*colsInA + colNum) < -limit) { + *(A + rowNum*colsInA + colNum) = -limit; + } + + *(A + colNum*colsInA + rowNum) = *(A + rowNum*colsInA + colNum); + } + } +} + + +/** **************************************************************************** +* @name: matrixInverse_3x3 A +* @brief Compute A = 1/A Tested Sep 19, 2014 (Matlab). (Assumes the matrix is +* invertible.) +* DKH 09.19.14 ported 440 code with change to input to allow indexing to ease +* debuging +* TRACE: +* @param [in] A matrix input 3x3 real precision matrix +* @param [out] AInverse - 1/A +* @retval always 1 +******************************************************************************/ +uint8_t matrixInverse_3x3( real* A, + real* AInverse ) +{ + real temp[3]; + real detInv; + + temp[0] = *(A + 8) * *(A + 4) - *(A + 7) * *(A + 5); + temp[1] = -*(A + 8) * *(A + 1) + *(A + 7) * *(A + 2); + temp[2] = *(A + 5) * *(A + 1) - *(A + 4) * *(A + 0*3 + 2); + detInv = (real)1.0 / ( *(A + 0*3 + 0) * temp[0] + *(A + 1*3 + 0) * temp[1] + *(A + 2*3 + 0) * temp[2] ); + + *(AInverse + 0*3 + 0) = temp[0] * detInv; + *(AInverse + 0*3 + 1) = temp[1] * detInv; + *(AInverse + 0*3 + 2) = temp[2] * detInv; + + temp[0] = -*(A + 2*3 + 2) * *(A + 1*3 + 0) + *(A + 2*3 + 0) * *(A + 1*3 + 2); + temp[1] = *(A + 2*3 + 2) * *(A + 0*3 + 0) - *(A + 2*3 + 0) * *(A + 0*3 + 2); + temp[2] = -*(A + 1*3 + 2) * *(A + 0*3 + 0) + *(A + 1*3 + 0) * *(A + 0*3 + 2); + *(AInverse + 1*3 + 0) = temp[0] * detInv; + *(AInverse + 1*3 + 1) = temp[1] * detInv; + *(AInverse + 1*3 + 2) = temp[2] * detInv; + + temp[0] = *(A + 2*3 + 1) * *(A + 1*3 + 0) - *(A + 2*3 + 0) * *(A + 1*3 + 1); + temp[1] = -*(A + 2*3 + 1) * *(A + 0*3 + 0) + *(A + 2*3 + 0) * *(A + 0*3 + 1); + temp[2] = *(A + 1*3 + 1) * *(A + 0*3 + 0) - *(A + 1*3 + 0) * *(A + 0*3 + 1); + *(AInverse + 2*3 + 0) = temp[0] * detInv; + *(AInverse + 2*3 + 1) = temp[1] * detInv; + *(AInverse + 2*3 + 2) = temp[2] * detInv; + + return 1; +} + + +// previous function was incorrect +// rewritten and retested on May 22, 2016 +void LimitMatrixValues(real* A, real limit, uint8_t rowsInA, uint8_t colsInA) +{ + real tmp; + uint8_t rowNum, colNum; + + for (rowNum = 0; rowNum < rowsInA; rowNum++) { + for (colNum = 0; colNum < colsInA; colNum++) { + tmp = *(A + rowNum*colsInA + colNum); + + if (tmp > limit) { + *(A + rowNum*colsInA + colNum) = limit; + } else if (tmp < -limit) { + *(A + rowNum*colsInA + colNum) = -limit; + } + } + } +} + + +/* ***** +// #defines for the 4x4 matrix inverse (to make debugging easier) +#define A11 *(A+0*4+0) +#define A12 *(A+0*4+1) +#define A13 *(A+0*4+2) +#define A14 *(A+0*4+3) + +#define A21 *(A+1*4+0) +#define A22 *(A+1*4+1) +#define A23 *(A+1*4+2) +#define A24 *(A+1*4+3) + +#define A31 *(A+2*4+0) +#define A32 *(A+2*4+1) +#define A33 *(A+2*4+2) +#define A34 *(A+2*4+3) + +#define A41 *(A+3*4+0) +#define A42 *(A+3*4+1) +#define A43 *(A+3*4+2) +#define A44 *(A+3*4+3) +***** */ + +/** **************************************************************************** +* @name: matrixInverse_3x3 A +* @brief Compute A = 1/A Tested Sep 19, 2014 (Matlab). (Assumes the matrix is +* invertible.) +* DKH 09.19.14 ported 440 code with change to input to allow indexing to ease +* debuging +* TRACE: +* @param [in] A matrix input 3x3 real precision matrix +* @param [out] AInverse - 1/A +* @retval always 1 +******************************************************************************/ +/* ***** +int MatrixInverse_4x4(real* A, real* AInverse) +{ + real inv[4][4], det; + + real tmp[36]; + + tmp[35] = A21 * A32; + tmp[28] = A21 * A33; + tmp[27] = A21 * A34; + tmp[32] = A21 * A42; + tmp[25] = A21 * A43; + tmp[22] = A21 * A44; + + tmp[34] = A22 * A31; + tmp[17] = A22 * A33; + tmp[14] = A22 * A34; + tmp[33] = A22 * A41; + tmp[10] = A22 * A43; + tmp[9] = A22 * A44; + + tmp[29] = A23 * A31; + tmp[16] = A23 * A32; + tmp[13] = A23 * A34; + tmp[24] = A23 * A41; + tmp[11] = A23 * A42; + tmp[6] = A23 * A44; + + tmp[26] = A24 * A31; + tmp[15] = A24 * A32; + tmp[12] = A24 * A33; + tmp[23] = A24 * A41; + tmp[8] = A24 * A42; + tmp[7] = A24 * A43; + + tmp[30] = A31 * A42; + tmp[21] = A31 * A43; + tmp[18] = A31 * A44; + + tmp[31] = A32 * A41; + tmp[4] = A32 * A43; + tmp[3] = A32 * A44; + + tmp[20] = A33 * A41; + tmp[5] = A33 * A42; + tmp[0] = A33 * A44; + + tmp[19] = A34 * A41; + tmp[2] = A34 * A42; + tmp[1] = A34 * A43; + + real temp[18]; + temp[0] = tmp[0] - tmp[1]; + temp[1] = tmp[2] - tmp[3]; + temp[2] = tmp[4] - tmp[5]; + temp[3] = tmp[6] - tmp[7]; + temp[4] = tmp[8] - tmp[9]; + temp[5] = tmp[10] - tmp[11]; + temp[6] = tmp[12] - tmp[13]; + temp[7] = tmp[14] - tmp[15]; + temp[8] = tmp[16] - tmp[17]; + temp[9] = tmp[18] - tmp[19]; + temp[10] = tmp[20] - tmp[21]; + temp[11] = tmp[22] - tmp[23]; + temp[12] = tmp[24] - tmp[25]; + temp[13] = tmp[26] - tmp[27]; + temp[14] = tmp[28] - tmp[29]; + temp[15] = tmp[30] - tmp[31]; + temp[16] = tmp[32] - tmp[33]; + temp[17] = tmp[34] - tmp[35]; + + + // First Row + inv[0][0] = (A22 * temp[0] + A23 * temp[1] + A24 * temp[2]); + inv[0][1] = -(A12 * temp[0] + A13 * temp[1] + A14 * temp[2]); + inv[0][2] = (A12 * temp[3] + A13 * temp[4] + A14 * temp[5]); + inv[0][3] = (A12 * temp[6] + A13 * temp[7] + A14 * temp[8]); + + // Second Row + inv[1][0] = -(A21 * temp[0] - A23 * temp[9] - A24 * temp[10]); + inv[1][1] = (A11 * temp[0] - A13 * temp[9] - A14 * temp[10]); + inv[1][2] = -(A11 * temp[3] - A13 * temp[11] - A14 * temp[12]); + inv[1][3] = -(A11 * temp[6] - A13 * temp[13] - A14 * temp[14]); + + // Third Row + inv[2][0] = -(A21 * temp[1] + A22 * temp[9] - A24 * temp[15]); + inv[2][1] = (A11 * temp[1] + A12 * temp[9] - A14 * temp[15]); + inv[2][2] = -(A11 * temp[4] + A12 * temp[11] - A14 * temp[16]); + inv[2][3] = -(A11 * temp[7] + A12 * temp[13] - A14 * temp[17]); + + // Fourth row + inv[3][0] = -(A21 * temp[2] + A22 * temp[10] + A23 * temp[15]); + inv[3][1] = (A11 * temp[2] + A12 * temp[10] + A13 * temp[15]); + inv[3][2] = -(A11 * temp[5] + A12 * temp[12] + A13 * temp[16]); + inv[3][3] = -(A11 * temp[8] + A12 * temp[14] + A13 * temp[17]); + + // compute the determinant (if "too small", declare the solution invalid) <-- problem with scaling here + det = A11 * inv[0][0] + A12 * inv[1][0] + A13 * inv[2][0] + A14 * inv[3][0]; + + BOOL retVal; + if (det < 1e-8) { + retVal = false; + } else { + retVal = true; + } + + det = (real)1.0 / det; + + // + *(AInverse + 0 * 4 + 0) = inv[0][0] * det; + *(AInverse + 0 * 4 + 1) = inv[0][1] * det; + *(AInverse + 0 * 4 + 2) = inv[0][2] * det; + *(AInverse + 0 * 4 + 3) = inv[0][3] * det; + + *(AInverse + 1 * 4 + 0) = inv[1][0] * det; + *(AInverse + 1 * 4 + 1) = inv[1][1] * det; + *(AInverse + 1 * 4 + 2) = inv[1][2] * det; + *(AInverse + 1 * 4 + 3) = inv[1][3] * det; + + *(AInverse + 2 * 4 + 0) = inv[2][0] * det; + *(AInverse + 2 * 4 + 1) = inv[2][1] * det; + *(AInverse + 2 * 4 + 2) = inv[2][2] * det; + *(AInverse + 2 * 4 + 3) = inv[2][3] * det; + + *(AInverse + 3 * 4 + 0) = inv[3][0] * det; + *(AInverse + 3 * 4 + 1) = inv[3][1] * det; + *(AInverse + 3 * 4 + 2) = inv[3][2] * det; + *(AInverse + 3 * 4 + 3) = inv[3][3] * det; + + return retVal; +} +***** */ + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/src/QuaternionMath.c b/examples/OpenIMU300RI/INS/lib/Core/Math/src/QuaternionMath.c new file mode 100644 index 0000000..1609b73 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/src/QuaternionMath.c @@ -0,0 +1,257 @@ +/* + * File: QuaternionMath.cpp + * Author: joemotyka + * + * Created on May 7, 2016, 5:03 PM + */ + +#include "QuaternionMath.h" +#ifdef DISPLAY_DIAGNOSTIC_MSG +#include "TimingVars.h" +#endif + +#include "Indices.h" +#include + +#if FAST_MATH +#include "arm_math.h" +#include "FastInvTrigFuncs.h" +#endif + +BOOL EulerAnglesToQuaternion( real* EulerAngles, real* Quaternion ) +{ + real sinThetaXOver2, cosThetaXOver2; // roll + real sinThetaYOver2, cosThetaYOver2; // pitch + real sinThetaZOver2, cosThetaZOver2; // yaw + + // Divide the angle by two (the angles used in computing the quaternion + // are theta/2) + real ThetaXOver2 = (real)0.5 * EulerAngles[ROLL]; + real ThetaYOver2 = (real)0.5 * EulerAngles[PITCH]; + real ThetaZOver2 = (real)0.5 * EulerAngles[YAW]; + + // Precompute sin/cos values used in the expressions below +#if !FAST_MATH + sinThetaXOver2 = sin( ThetaXOver2 ); + cosThetaXOver2 = cos( ThetaXOver2 ); + sinThetaYOver2 = sin( ThetaYOver2 ); + cosThetaYOver2 = cos( ThetaYOver2 ); + sinThetaZOver2 = sin( ThetaZOver2 ); + cosThetaZOver2 = cos( ThetaZOver2 ); +#else + sinThetaXOver2 = arm_sin_f32( ThetaXOver2 ); + cosThetaXOver2 = arm_cos_f32( ThetaXOver2 ); + sinThetaYOver2 = arm_sin_f32( ThetaYOver2 ); + cosThetaYOver2 = arm_cos_f32( ThetaYOver2 ); + sinThetaZOver2 = arm_sin_f32( ThetaZOver2 ); + cosThetaZOver2 = arm_cos_f32( ThetaZOver2 ); +#endif + + // q0 = SIN( ThetaX/2 ) * SIN( ThetaY/2 ) * SIN( ThetaZ/2 ) + COS( ThetaX/2 ) * COS( ThetaY/2 ) * COS( ThetaZ/2 ) + Quaternion[Q0] = sinThetaXOver2 * sinThetaYOver2 * sinThetaZOver2 + + cosThetaXOver2 * cosThetaYOver2 * cosThetaZOver2; + + // q1 = SIN( ThetaX/2 ) * COS( ThetaY/2 ) * COS( ThetaZ/2 ) - COS( ThetaX/2 ) * SIN( ThetaY/2 ) * SIN( ThetaZ/2 ) + Quaternion[Q1] = sinThetaXOver2 * cosThetaYOver2 * cosThetaZOver2 - + cosThetaXOver2 * sinThetaYOver2 * sinThetaZOver2; + + // q2 = SIN( ThetaX/2 ) * COS( ThetaY/2 ) * SIN( ThetaZ/2 ) + COS( ThetaX/2 ) * SIN( ThetaY/2 ) * COS( ThetaZ/2 ) + Quaternion[Q2] = sinThetaXOver2 * cosThetaYOver2 * sinThetaZOver2 + + cosThetaXOver2 * sinThetaYOver2 * cosThetaZOver2; + + // q3 = COS( ThetaX/2 ) * COS( ThetaY/2 ) * SIN( ThetaZ/2 ) - SIN( ThetaX/2 ) * SIN( ThetaY/2 ) * COS( ThetaZ/2 ) + Quaternion[Q3] = cosThetaXOver2 * cosThetaYOver2 * sinThetaZOver2 - + sinThetaXOver2 * sinThetaYOver2 * cosThetaZOver2; + + QuatNormalize( Quaternion ); + + return 1; +} + +BOOL EulerAnglesToR321(real* EulerAngles, real* R321) +{ + real sinThetaX, cosThetaX; + real sinThetaY, cosThetaY; + real sinThetaZ, cosThetaZ; + + // Precompute sin/cos values used in the expressions below +#if !FAST_MATH + sinThetaX = sin(EulerAngles[ROLL]); + cosThetaX = cos(EulerAngles[ROLL]); + sinThetaY = sin(EulerAngles[PITCH]); + cosThetaY = cos(EulerAngles[PITCH]); + sinThetaZ = sin(EulerAngles[YAW]); + cosThetaZ = cos(EulerAngles[YAW]); +#else + sinThetaX = arm_sin_f32(EulerAngles[ROLL]); + cosThetaX = arm_cos_f32(EulerAngles[ROLL]); + sinThetaY = arm_sin_f32(EulerAngles[PITCH]); + cosThetaY = arm_cos_f32(EulerAngles[PITCH]); + sinThetaZ = arm_sin_f32(EulerAngles[YAW]); + cosThetaZ = arm_cos_f32(EulerAngles[YAW]); +#endif + + *(R321 + 3*X_AXIS+X_AXIS) = cosThetaZ*cosThetaY; + *(R321 + 3*X_AXIS+Y_AXIS) = cosThetaZ*sinThetaY*sinThetaX - sinThetaZ*cosThetaX; + *(R321 + 3*X_AXIS+Z_AXIS) = cosThetaZ*sinThetaY*cosThetaX + sinThetaZ*sinThetaX; + + *(R321 + 3*Y_AXIS+X_AXIS) = sinThetaZ*cosThetaY; + *(R321 + 3*Y_AXIS+Y_AXIS) = sinThetaZ*sinThetaY*sinThetaX + cosThetaZ*cosThetaX; + *(R321 + 3*Y_AXIS+Z_AXIS) = sinThetaZ*sinThetaY*cosThetaX - cosThetaZ*sinThetaX; + + *(R321 + 3*Z_AXIS+X_AXIS) = -sinThetaY; + *(R321 + 3*Z_AXIS+Y_AXIS) = cosThetaY*sinThetaX; + *(R321 + 3*Z_AXIS+Z_AXIS) = cosThetaY*cosThetaX; + + return 1; +} + + +BOOL QuatNormalize( real *Quat ) +{ + real QuatSquared[4]; + real QuatMag; + real temp; + + // Square the components of the quaternion (0 <= sSquared <= 1) + QuatSquared[Q0] = Quat[Q0] * Quat[Q0]; + QuatSquared[Q1] = Quat[Q1] * Quat[Q1]; + QuatSquared[Q2] = Quat[Q2] * Quat[Q2]; + QuatSquared[Q3] = Quat[Q3] * Quat[Q3]; + + // Find the RSS of the quaternion: sqrt( q1^2 + q2^2 + q3^2 + q4^2 ) + QuatMag = QuatSquared[Q0] + + QuatSquared[Q1] + + QuatSquared[Q2] + + QuatSquared[Q3]; + QuatMag = sqrtf( QuatMag ); + + // Normalize the quaternion + temp = 1 / QuatMag; + Quat[Q0] = Quat[Q0] * temp; + Quat[Q1] = Quat[Q1] * temp; + Quat[Q2] = Quat[Q2] * temp; + Quat[Q3] = Quat[Q3] * temp; + +#ifdef FORCE_Q0_POSITIVE + // Force Q0 to be positive + if (Quat[Q0] < 0.0) { +//#ifdef DISPLAY_DIAGNOSTIC_MSG +// TimingVars_DiagnosticMsg("QuatNormalize: flip sign on q"); +//#endif + + // Flip signs on all quaternion elements + Quat[Q0] = -Quat[Q0]; + Quat[Q1] = -Quat[Q1]; + Quat[Q2] = -Quat[Q2]; + Quat[Q3] = -Quat[Q3]; + } +#endif + + return 1; +} + + +BOOL QuaternionToEulerAngles( real* EulerAngles, + real* Quaternion ) +{ + real R[3][3]; + + real q0Sq, q0q1, q0q2, q0q3; + real q1Sq, q1q2, q1q3; + real q2Sq, q2q3; + real q3Sq; + + // Compute values used repeatedly in the function + q0Sq = Quaternion[Q0] * Quaternion[Q0]; + q0q1 = Quaternion[Q0] * Quaternion[Q1]; + q0q2 = Quaternion[Q0] * Quaternion[Q2]; + q0q3 = Quaternion[Q0] * Quaternion[Q3]; + + q1Sq = Quaternion[Q1] * Quaternion[Q1]; + q1q2 = Quaternion[Q1] * Quaternion[Q2]; + q1q3 = Quaternion[Q1] * Quaternion[Q3]; + + q2Sq = Quaternion[Q2] * Quaternion[Q2]; + q2q3 = Quaternion[Q2] * Quaternion[Q3]; + + q3Sq = Quaternion[Q3] * Quaternion[Q3]; + + // Form the direction cosine matrix (DCM) from q + R[0][0] = q0Sq + q1Sq - q2Sq - q3Sq; +// R[0][1] = 2.0 * (q1q2 - q0q3); +// R[0][2] = 2.0 * (q1q3 + q0q2); + + R[1][0] = (real)2.0 * (q1q2 + q0q3); +// R[1][1] = q0Sq - q1Sq + q2Sq - q3Sq; +// R[1][2] = 2.0 * (q2q3 - q0q1); + + R[2][0] = (real)2.0 * (q1q3 - q0q2); + R[2][1] = (real)2.0 * (q2q3 + q0q1); + R[2][2] = q0Sq - q1Sq - q2Sq + q3Sq; + + // Calculate the euler angles from the DCM + // prevent NaN due to numerical error, dxg, 20180802 + if (R[2][0] > 1.0) + { + R[2][0] = 1.0; + } + if (R[2][0] < -1.0) + { + R[2][0] = -1.0; + } +#if !FAST_MATH + EulerAngles[ROLL] = atan2( R[2][1], R[2][2] ); + EulerAngles[PITCH] = -asin( R[2][0] ); + EulerAngles[YAW] = atan2( R[1][0], R[0][0] ); +#else + EulerAngles[ROLL] = fatan2_rad( R[2][1], R[2][2] ); + EulerAngles[PITCH] = -fasin_rad( R[2][0] ); + EulerAngles[YAW] = fatan2_rad( R[1][0], R[0][0] ); +#endif + + // What do do in the case that pitch = 90 degrees??? Indeterminate roll and yaw... + return 1; +} + + +BOOL QuaternionToR321(real* Quaternion, real* R321) +{ + real q0Sq, q0q1, q0q2, q0q3; + real q1Sq, q1q2, q1q3; + real q2Sq, q2q3; + real q3Sq; + + // Compute values used repeatedly in the function + q0Sq = Quaternion[Q0] * Quaternion[Q0]; + q0q1 = Quaternion[Q0] * Quaternion[Q1]; + q0q2 = Quaternion[Q0] * Quaternion[Q2]; + q0q3 = Quaternion[Q0] * Quaternion[Q3]; + + q1Sq = Quaternion[Q1] * Quaternion[Q1]; + q1q2 = Quaternion[Q1] * Quaternion[Q2]; + q1q3 = Quaternion[Q1] * Quaternion[Q3]; + + q2Sq = Quaternion[Q2] * Quaternion[Q2]; + q2q3 = Quaternion[Q2] * Quaternion[Q3]; + + q3Sq = Quaternion[Q3] * Quaternion[Q3]; + + //Form the direction cosine matrix (DCM) from q + *(R321 + 0) = q0Sq + q1Sq - q2Sq - q3Sq; + *(R321 + 1) = (real)2.0 * (q1q2 - q0q3); + *(R321 + 2) = (real)2.0 * (q1q3 + q0q2); + + *(R321 + 3) = (real)2.0 * (q1q2 + q0q3); + *(R321 + 4) = q0Sq - q1Sq + q2Sq - q3Sq; + *(R321 + 5) = (real)2.0 * (q2q3 - q0q1); + + *(R321 + 6) = (real)2.0 * (q1q3 - q0q2); + *(R321 + 7) = (real)2.0 * (q2q3 + q0q1); + *(R321 + 8) = q0Sq - q1Sq - q2Sq + q3Sq; + + // + return 1; +} + + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/src/TransformationMath.c b/examples/OpenIMU300RI/INS/lib/Core/Math/src/TransformationMath.c new file mode 100644 index 0000000..9158f9b --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/src/TransformationMath.c @@ -0,0 +1,437 @@ +/* + * File: TransformationMath.cpp + * Author: joemotyka + * + * Created on May 8, 2016, 12:35 AM + */ +#include +#include +#include "TransformationMath.h" +#include "MatrixMath.h" +#include "VectorMath.h" +#include "Indices.h" + +#if FAST_MATH +#include "arm_math.h" +#include "FastInvTrigFuncs.h" +#endif // FAST_MATH + + /****************************************************************************** + * @brief Calculate magnetic field in a perpendicular frame. + * This perpendicular frame is generated by rotating the navigation frame about + * its Z axis for YAW degree. The perpendicular frame is an intermediate frame + * from the navigation frame to the body frame. Let v be a vector. v is described + * in the navigation frame, the perpendicular frame and the body frame by v_n, + * v_p and v_b, respectively. The following equation hold: + * v_b = Rx(roll) * Ry(pitch) * Rz(yaw) * v_n + * v_b = Rx(roll) * Ry(pitch) * v_p + * In this routine, the gravity vector in the body frame and the magnetic vector + * in the body frame are known, the magnetic vector in the perpendicular frame + * need to be calculated. + * TRACE: + * @param [in] MagFieldVector magnetic vector in the body frame. + * @param [in] unitGravityVector unit gravity vector in the body frame. + * note: accelerometer measurement = -gravity when there is no linear acceleration. + * If acceleromter measurement is [0; 0; -1], the unit gravity vector should + * be [0; 0; 1]. + * @param [out] nedMagFieldVector magnetic vector in the NED frame. + * @retval 1 if magnetometers are used, otherwise it returns a zero. + ******************************************************************************/ +static void _TransformMagFieldToPerpFrame( real* MagFieldVector, + real* nedMagFieldVector, + real* unitGravityVector ); + +#define CORRECT_IN_BODY_FRAME + +//============================================================================= +void UnitGravity(real *accel, real *unitGravityVector) +{ + real accelMag = (real)sqrt( accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2] ); + real invAccelMag = 1.0f / accelMag; + unitGravityVector[0] = -accel[0] * invAccelMag; + unitGravityVector[1] = -accel[1] * invAccelMag; + unitGravityVector[2] = -accel[2] * invAccelMag; +} + +void UnitGravityToEulerAngles(real *unitGravityVector, real* eulerAngles) +{ + eulerAngles[ROLL] = (real)( atan2( unitGravityVector[Y_AXIS], + unitGravityVector[Z_AXIS] ) ); + eulerAngles[PITCH] = (real)( -asin( unitGravityVector[X_AXIS] ) ); +} + +real UnitGravityAndMagToYaw(real *unitGravityVector, real *magFieldVector) +{ + real nedMagFieldVector[3] = {0.0}; + // Transform the magnetic field vector from the body-frame to the plane normal to the gravity vector + _TransformMagFieldToPerpFrame( magFieldVector, nedMagFieldVector, unitGravityVector ); + + // The negative of the angle the vector makes with the unrotated (psi = 0) + // frame is the yaw-angle of the initial frame. + return (real)( -atan2( nedMagFieldVector[Y_AXIS], nedMagFieldVector[X_AXIS] ) ); +} + +real RollPitchAndMagToYaw(real roll, real pitch, real *magFieldVector) +{ + real nedMagFieldVector[3] = {0.0}; + + real sinRoll, cosRoll; + real sinPitch, cosPitch; + real temp; + + sinRoll = (real)(sin( roll )); + cosRoll = (real)(cos( roll )); + sinPitch = (real)(sin( pitch )); + cosPitch = (real)(cos( pitch )); + + temp = sinRoll * magFieldVector[Y_AXIS] + cosRoll * magFieldVector[Z_AXIS]; + + nedMagFieldVector[X_AXIS] = cosPitch * magFieldVector[X_AXIS] + sinPitch * temp; + nedMagFieldVector[Y_AXIS] = cosRoll * magFieldVector[Y_AXIS] - sinRoll * magFieldVector[Z_AXIS]; + //nedMagFieldVector[Z_AXIS] = -sinPitch * magFieldVector[X_AXIS] + cosPitch * temp; + + return (real)( -atan2( nedMagFieldVector[Y_AXIS], nedMagFieldVector[X_AXIS] ) ); +} + +static void _TransformMagFieldToPerpFrame( real* magFieldVector, + real* nedMagFieldVector, + real* unitGravityVector ) +{ + real sinRoll, cosRoll; + real sinPitch, cosPitch; + real temp; + + sinPitch = -unitGravityVector[0]; + if ( sinPitch >= 1.0 ) // roll and yaw undefined, assume roll = 0 + { + nedMagFieldVector[0] = magFieldVector[2]; + nedMagFieldVector[1] = magFieldVector[1]; + nedMagFieldVector[2] = -magFieldVector[0]; + // set up some kind of flag to return + } + else if ( sinPitch <= -1.0 ) // roll and yaw undefined, assume roll = 0 + { + nedMagFieldVector[0] = -magFieldVector[2]; + nedMagFieldVector[1] = magFieldVector[1]; + nedMagFieldVector[2] = magFieldVector[0]; + // set up some kind of flag to return + } + else + { + cosPitch = sqrtf( 1.0f - sinPitch*sinPitch ); // pitch is (-90, 90), cos(pitch)>0 + sinRoll = unitGravityVector[1] / cosPitch; + cosRoll = unitGravityVector[2] / cosPitch; + temp = sinRoll * magFieldVector[1] + cosRoll * magFieldVector[2]; + nedMagFieldVector[0] = cosPitch * magFieldVector[0] + sinPitch * temp; + nedMagFieldVector[1] = cosRoll * magFieldVector[1] - sinRoll * magFieldVector[2]; + nedMagFieldVector[2] = -sinPitch * magFieldVector[2] + cosPitch * temp; + } + // return something to indicate pitch = 90 +} + +///***************************************************************************** +//* @name LLA_To_R_EinN returns the rotation matrix that converts from the Earth- +//* Centered, Earth-Fixed [m] to the North/East/Down-Frame [m] +//* +//* @details Pre calculated all non-changing constants and unfolded the matrices +//* @param [in] LLA - array with the Latitude, Longitude and Altitude [rad] +//* @param [out] R_EinN - rotation matrix from ECEF to NED +//* @retval always 1 +//******************************************************************************/ +BOOL LLA_To_R_EinN( double* llaRad, + real* R_EinN ) +{ + real sinLat, cosLat; + real sinLon, cosLon; + + sinLat = (real)sin(llaRad[LAT]); + cosLat = (real)cos(llaRad[LAT]); + sinLon = (real)sin(llaRad[LON]); + cosLon = (real)cos(llaRad[LON]); + + // First row + *(R_EinN + 0 * 3 + 0) = -sinLat * cosLon; + *(R_EinN + 0 * 3 + 1) = -sinLat * sinLon; + *(R_EinN + 0 * 3 + 2) = cosLat; + + // Second row + *(R_EinN + 1 * 3 + 0) = -sinLon; + *(R_EinN + 1 * 3 + 1) = cosLon; + *(R_EinN + 1 * 3 + 2) = 0.0; + + // Third row + *(R_EinN + 2 * 3 + 0) = -cosLat * cosLon; + *(R_EinN + 2 * 3 + 1) = -cosLat * sinLon; + *(R_EinN + 2 * 3 + 2) = -sinLat; + + return 1; +} + + +///** *************************************************************************** +//* @name LLA_To_R_NinE returns a rotation matrix North [m], East [m] down [m] to +//* Earth Centered Earth Fixed [m] coordinates +//* @details Pre calculated all non-changing constants and unfolded the matrices +//* @param [in] LLA - array with the Latitude, Longitude and Altitude [rad] +//* @param [out] InvRne - rotation matrix from NED to ECEF +//* @retval always 1 +//******************************************************************************/ +BOOL LLA_To_R_NinE( double* llaRad, + real* R_NinE ) +{ + real sinLat, cosLat; + real sinLon, cosLon; + + sinLat = (real)(sin((real)llaRad[LAT])); + cosLat = (real)(cos((real)llaRad[LAT])); + sinLon = (real)(sin((real)llaRad[LON])); + cosLon = (real)(cos((real)llaRad[LON])); + + *(R_NinE + 0 * 3 + 0) = -sinLat * cosLon; + *(R_NinE + 0 * 3 + 1) = -sinLon; + *(R_NinE + 0 * 3 + 2) = -cosLat * cosLon; + + *(R_NinE + 1 * 3 + 0) = -sinLat * sinLon; + *(R_NinE + 1 * 3 + 1) = cosLon; + *(R_NinE + 1 * 3 + 2) = -cosLat * sinLon; + + *(R_NinE + 2 * 3 + 0) = cosLat; + *(R_NinE + 2 * 3 + 1) = 0.0; + *(R_NinE + 2 * 3 + 2) = -sinLat; + + return 1; +} + + +///** *************************************************************************** +//* @name Calculate NED relative position of two ECEF positions. +//* @details Pre calculated all non-changing constants and unfolded the matrices +//* @param [in] rECEF_Init - start of frame position +//* @param [in] rECEF - current position in ECEF from LLA +//* @param [in] R_NinE - rotation matrix from NED to ECEF +//* @param [out] dr_N - output of the position in North East and Down coords +//* @retval always 1 +//******************************************************************************/ +BOOL ECEF_To_Base( double* rECEF_Init, + double* rECEF, + real* R_NinE, + real* dr_N) +{ + real dr_E[NUM_AXIS]; + + dr_E[X_AXIS] = (real)( rECEF[X_AXIS] - *(rECEF_Init + X_AXIS) ); + dr_E[Y_AXIS] = (real)( rECEF[Y_AXIS] - *(rECEF_Init + Y_AXIS) ); + dr_E[Z_AXIS] = (real)( rECEF[Z_AXIS] - *(rECEF_Init + Z_AXIS) ); + + /* Convert from delta-position in the ECEF-frame to the NED-frame (the transpose + * in the equations that followed is handled in the formulation) + * + * N E ( E N )T + * dr_N = R * dr_E = ( R ) * dr_E + * ( ) + */ + dr_N[X_AXIS] = *(R_NinE + X_AXIS * 3 + X_AXIS) * dr_E[X_AXIS] + + *(R_NinE + Y_AXIS * 3 + X_AXIS) * dr_E[Y_AXIS] + + *(R_NinE + Z_AXIS * 3 + X_AXIS) * dr_E[Z_AXIS]; + dr_N[Z_AXIS] = *(R_NinE + X_AXIS * 3 + Z_AXIS) * dr_E[X_AXIS] + + *(R_NinE + Y_AXIS * 3 + Z_AXIS) * dr_E[Y_AXIS] + + *(R_NinE + Z_AXIS * 3 + Z_AXIS) * dr_E[Z_AXIS]; + dr_N[Y_AXIS] = *(R_NinE + X_AXIS * 3 + Y_AXIS) * dr_E[X_AXIS] + + *(R_NinE + Y_AXIS * 3 + Y_AXIS) * dr_E[Y_AXIS] + + *(R_NinE + Z_AXIS * 3 + Y_AXIS) * dr_E[Z_AXIS]; + + return 1; +} + + +/** *************************************************************************** +* @name LLA2ECEF Lat [rad], Lon [rad] to Earth Centered Earth Fixed coordinates +* [m] +* @details pre calculated all non-changing constants +* @param [in] LLA - array [rad] with the Latitude, Lonigtude and altitude +* @param [out] ECEF - array cartresian coordinate [m] +* @retval always 1 +******************************************************************************/ +BOOL LLA_To_ECEF( double* lla_Rad, + double* ecef_m ) +{ + double N; + double cosLat = cos( lla_Rad[LAT] ); + double sinLat = sin( lla_Rad[LAT] ); + double cosLon = cos( lla_Rad[LON] ); + double sinLon = sin( lla_Rad[LON] ); + + N = E_MAJOR / sqrt(1.0 - (E_ECC_SQ * sinLat * sinLat)); // radius of Curvature [meters] + + double temp = (N + lla_Rad[ALT]) * cosLat; + ecef_m[X_AXIS] = temp * cosLon; + ecef_m[Y_AXIS] = temp * sinLon; + ecef_m[Z_AXIS] = ((E_MINOR_OVER_MAJOR_SQ * N) + lla_Rad[ALT]) * sinLat; + + return 1; +} + + +/** *************************************************************************** +* @name Base2ECEF Express tangent (NED) coords in ECEF coordinates +* @details adds delta postion fro start of frame in NED to the ECEF postion +* at the current time step and returns the results in meters ECEF. +* @param [in] NED - input tangent position [m] in North East and Down coords +* @param {in} BaseECEF - start of frame (gps) position [m] +* @param {in} invRne - inverse of the rotation matrix from ECEF to NED +* @param [out] ECEF - current frame position [m] +* @retval always 1 +******************************************************************************/ +BOOL PosNED_To_PosECEF( real* r_N, + double* rECEF_Init, //BaseECEF, + real* R_NinE, + double* rECEF) +{ + // ECEF = Base + delta + *(rECEF + 0) = *(rECEF_Init + 0) + (double)(*(R_NinE + 0 * 3 + 0) * r_N[X_AXIS] + + *(R_NinE + 0 * 3 + 1) * r_N[Y_AXIS] + + *(R_NinE + 0 * 3 + 2) * r_N[Z_AXIS] ); // X + *(rECEF + 1) = *(rECEF_Init + 1) + (double)(*(R_NinE + 1 * 3 + 0) * r_N[X_AXIS] + + *(R_NinE + 1 * 3 + 1) * r_N[Y_AXIS] + + *(R_NinE + 1 * 3 + 2) * r_N[Z_AXIS] ); // Y + *(rECEF + 2) = *(rECEF_Init + 2) + (double)(*(R_NinE + 2 * 3 + 0) * r_N[X_AXIS] + + *(R_NinE + 2 * 3 + 1) * r_N[Y_AXIS] + + *(R_NinE + 2 * 3 + 2) * r_N[Z_AXIS] ); // Z + + return 1; +} + + +/** *************************************************************************** +* @name ECEF2LLA Earth Centered Earth Fixed [m] to Lat [rad], Lon [rad] coordinates +* @details pre calculated all non-changing constants +* @param [out] LLA - array the Latitude, Lonigtude [deg] and Altitude [m] +* @param [in] ECEF - cartresian coordiante [m] +* @retval always 1 +******************************************************************************/ +BOOL ECEF_To_LLA(double* llaDeg, double* ecef_m) +{ + double P; + double theta; + double sinLat; + double sinTheta, cosTheta; + double Lat; + + P = sqrt( ecef_m[X_AXIS] * ecef_m[X_AXIS] + ecef_m[Y_AXIS] * ecef_m[Y_AXIS] ); + + // theta = atan( ( ecef_m[Z_AXIS] * E_MAJOR ) / ( P * E_MINOR ) ); // sqrt( ecef(2) * const ) + theta = atan2(ecef_m[Z_AXIS] * E_MAJOR_OVER_MINOR, P); + + sinTheta = sin(theta); + cosTheta = cos(theta); + + Lat = atan2((ecef_m[Z_AXIS] + EP_SQ * sinTheta * sinTheta * sinTheta), (P - E_ECC_SQxE_MAJOR * cosTheta * cosTheta * cosTheta)); + *(llaDeg + LAT) = Lat * R2D; + *(llaDeg + LON) = atan2(ecef_m[Y_AXIS], ecef_m[X_AXIS]) * R2D; // arctan(Y/X) + + sinLat = sin(Lat); + *(llaDeg + ALT) = P / cos(Lat) - E_MAJOR / sqrt(1.0 - E_ECC_SQ * sinLat * sinLat); // alt + + return 1; +} + + +/** *************************************************************************** +* @name ECEF2NED Earth Centered Earth Fixed [m] to North [m], East [m] down [m] +* coordinates +* @details Pre calculated all non-changing constants and unfolded the matricies +* @param [in] LLA - Latitude, Longitude and Altitude [rad] +* @param [in] VelECEF - Earth centered earth fixed [m/s] +* @param [out] VelNED - North East Down [m/s] +* @retval always 1 +******************************************************************************/ +BOOL VelECEF_To_VelNED( double* LLA, + real* VelECEF, + real* VelNED ) +{ + real cosLat, sinLat; + real cosLon, sinLon; + +#if !FAST_MATH + cosLat = (real)(cos( (real)*(LLA + LAT) )); + sinLat = (real)(sin( (real)*(LLA + LAT) )); + cosLon = (real)(cos( (real)*(LLA + LON) )); + sinLon = (real)(sin( (real)*(LLA + LON) )); +#else + cosLat = (real)(arm_cos_f32( (real)*(LLA + LAT) )); + sinLat = (real)(arm_sin_f32( (real)*(LLA + LAT) )); + cosLon = (real)(arm_cos_f32( (real)*(LLA + LON) )); + sinLon = (real)(arm_sin_f32( (real)*(LLA + LON) )); +#endif + + // North + *(VelNED+X_AXIS) = -sinLat * cosLon * *(VelECEF + X_AXIS) + + -sinLon * sinLat * *(VelECEF + Y_AXIS) + + cosLat * *(VelECEF + Z_AXIS); + // East + *(VelNED+Y_AXIS) = -sinLon * *(VelECEF + X_AXIS) + + cosLon * *(VelECEF + Y_AXIS); + // Down + *(VelNED+Z_AXIS) = -cosLat * cosLon * *(VelECEF + X_AXIS) + + -cosLat * sinLon * *(VelECEF + Y_AXIS) + + -sinLat * *(VelECEF + Z_AXIS); + + return 1; +} + +void printMtx(float *a, int m, int n) +{ + int i, j; + for (i = 0; i < m; i++) + { + for (j = 0; j < n - 1; j++) + { + printf("%.9g, ", a[i*n + j]); + } + printf("%.9g\n", a[i*n + j]); + } +} + +void printVec(float *v, int n) +{ + int i; + for (i = 0; i < n - 1; i++) + { + printf("%.9g, ", v[i]); + } + printf("%.9g\n", v[i]); +} + +real AngleErrDeg(real aErr) +{ + while (fabs(aErr) > 180.0) + { + if (aErr > 180.0) + { + aErr -= (real)360.0; + } + else if (aErr < -180.0) + { + aErr += (real)360.0; + } + } + + return aErr; +} + +real AngleErrRad(real aErr) +{ + while (fabs(aErr) > PI) + { + if (aErr > PI) + { + aErr -= (real)TWO_PI; + } + else if (aErr < -PI) + { + aErr += (real)TWO_PI; + } + } + + return aErr; +} diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/src/VectorMath.c b/examples/OpenIMU300RI/INS/lib/Core/Math/src/VectorMath.c new file mode 100644 index 0000000..e164ee3 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/src/VectorMath.c @@ -0,0 +1,83 @@ +/* + * File: VectorMath.cpp + * Author: joemotyka + * + * Created on May 7, 2016, 12:50 AM + */ + +#include "VectorMath.h" + +#include + +// Function definitions for reals +void VectorNormalize( real *vectorIn, real *vectorOut ) +{ + real vectorMag; + real multiplier; + + // sqrt( v1^2 + v2^2 + v3^2 ) + vectorMag = VectorMag( vectorIn ); + multiplier = (real)1.0 / vectorMag; + + vectorOut[0] = multiplier * vectorIn[0]; + vectorOut[1] = multiplier * vectorIn[1]; + vectorOut[2] = multiplier * vectorIn[2]; +} + + +// Components must be less than 1239850262 (Q27) ~ 9.23 (dec) in order to use this function without +// overflow. Else, need to represent the output in another q-format. +real VectorMag( real *vectorIn ) +{ + real temp[3]; + + // + temp[0] = vectorIn[0] * vectorIn[0]; + temp[1] = vectorIn[1] * vectorIn[1]; + temp[2] = vectorIn[2] * vectorIn[2]; + + // sqrt( v1^2 + v2^2 + v3^2 ) + return( (real)(sqrtf( temp[0] + temp[1] + temp[2] )) ); +} + + +void VectorCrossProduct( real *vect1, real *vect2, real *vectOut ) +{ + // | i j k | + // | v1[0] v1[1] v1[2] | = i*( v1[1]*v2[2] - v1[2]*v2[1] ) - j*( v1[0]*v2[2] - v1[2]*v2[0] ) + k*( v1[0]*v2[1] - v1[1]*v2[0] ) + // | v2[0] v2[1] v2[2] | + + vectOut[0] = vect1[1] * vect2[2] - vect1[2] * vect2[1]; // v1[1]*v2[2] - v1[2]*v2[1] + vectOut[1] = vect1[2] * vect2[0] - vect1[0] * vect2[2]; // v1[2]*v2[0] - v1[0]*v2[2] + vectOut[2] = vect1[0] * vect2[1] - vect1[1] * vect2[0]; // v1[0]*v2[1] - v1[1]*v2[0] +} + +real VectorDotProduct( real *vect1, real *vect2 ) +{ + real result; + + result = vect1[0] * vect2[0] + + vect1[1] * vect2[1] + + vect1[2] * vect2[2]; + + return( result ); +} + +real vecVar(real *a, real am, int n) +{ + int i; + real sum = 0.0; + for (i = 0; i < n; i++) + { + real d = a[i] - am; + sum += d * d; + } + return sum / n; +} + +void cross(real* a, real* b, real* axb) +{ + axb[0] = a[1] * b[2] - a[2] * b[1]; + axb[1] = a[2] * b[0] - a[0] * b[2]; + axb[2] = a[0] * b[1] - a[1] * b[0]; +} diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_common_tables.c b/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_common_tables.c new file mode 100644 index 0000000..79fe976 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_common_tables.c @@ -0,0 +1,27251 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2014 ARM Limited. All rights reserved. +* +* $Date: 19. March 2015 +* $Revision: V.1.4.5 +* +* Project: CMSIS DSP Library +* Title: arm_common_tables.c +* +* Description: This file has common tables like fft twiddle factors, Bitreverse, reciprocal etc which are used across different functions +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* -------------------------------------------------------------------- */ + + +#include "arm_math.h" +#include "arm_common_tables.h" + +/** + * @ingroup groupTransforms + */ + +/** + * @addtogroup CFFT_CIFFT Complex FFT Tables + * @{ + */ + +/** +* \par +* Pseudo code for Generation of Bit reversal Table is +* \par +*
for(l=1;l <= N/4;l++)    
+* {    
+*   for(i=0;i> 1;    
+*  } 
+* \par +* where N = 4096 logN2 = 12 +* \par +* N is the maximum FFT Size supported +*/ + +/* +* @brief Table for bit reversal process +*/ +const uint16_t armBitRevTable[1024] = { + 0x400, 0x200, 0x600, 0x100, 0x500, 0x300, 0x700, 0x80, 0x480, 0x280, + 0x680, 0x180, 0x580, 0x380, 0x780, 0x40, 0x440, 0x240, 0x640, 0x140, + 0x540, 0x340, 0x740, 0xc0, 0x4c0, 0x2c0, 0x6c0, 0x1c0, 0x5c0, 0x3c0, + 0x7c0, 0x20, 0x420, 0x220, 0x620, 0x120, 0x520, 0x320, 0x720, 0xa0, + 0x4a0, 0x2a0, 0x6a0, 0x1a0, 0x5a0, 0x3a0, 0x7a0, 0x60, 0x460, 0x260, + 0x660, 0x160, 0x560, 0x360, 0x760, 0xe0, 0x4e0, 0x2e0, 0x6e0, 0x1e0, + 0x5e0, 0x3e0, 0x7e0, 0x10, 0x410, 0x210, 0x610, 0x110, 0x510, 0x310, + 0x710, 0x90, 0x490, 0x290, 0x690, 0x190, 0x590, 0x390, 0x790, 0x50, + 0x450, 0x250, 0x650, 0x150, 0x550, 0x350, 0x750, 0xd0, 0x4d0, 0x2d0, + 0x6d0, 0x1d0, 0x5d0, 0x3d0, 0x7d0, 0x30, 0x430, 0x230, 0x630, 0x130, + 0x530, 0x330, 0x730, 0xb0, 0x4b0, 0x2b0, 0x6b0, 0x1b0, 0x5b0, 0x3b0, + 0x7b0, 0x70, 0x470, 0x270, 0x670, 0x170, 0x570, 0x370, 0x770, 0xf0, + 0x4f0, 0x2f0, 0x6f0, 0x1f0, 0x5f0, 0x3f0, 0x7f0, 0x8, 0x408, 0x208, + 0x608, 0x108, 0x508, 0x308, 0x708, 0x88, 0x488, 0x288, 0x688, 0x188, + 0x588, 0x388, 0x788, 0x48, 0x448, 0x248, 0x648, 0x148, 0x548, 0x348, + 0x748, 0xc8, 0x4c8, 0x2c8, 0x6c8, 0x1c8, 0x5c8, 0x3c8, 0x7c8, 0x28, + 0x428, 0x228, 0x628, 0x128, 0x528, 0x328, 0x728, 0xa8, 0x4a8, 0x2a8, + 0x6a8, 0x1a8, 0x5a8, 0x3a8, 0x7a8, 0x68, 0x468, 0x268, 0x668, 0x168, + 0x568, 0x368, 0x768, 0xe8, 0x4e8, 0x2e8, 0x6e8, 0x1e8, 0x5e8, 0x3e8, + 0x7e8, 0x18, 0x418, 0x218, 0x618, 0x118, 0x518, 0x318, 0x718, 0x98, + 0x498, 0x298, 0x698, 0x198, 0x598, 0x398, 0x798, 0x58, 0x458, 0x258, + 0x658, 0x158, 0x558, 0x358, 0x758, 0xd8, 0x4d8, 0x2d8, 0x6d8, 0x1d8, + 0x5d8, 0x3d8, 0x7d8, 0x38, 0x438, 0x238, 0x638, 0x138, 0x538, 0x338, + 0x738, 0xb8, 0x4b8, 0x2b8, 0x6b8, 0x1b8, 0x5b8, 0x3b8, 0x7b8, 0x78, + 0x478, 0x278, 0x678, 0x178, 0x578, 0x378, 0x778, 0xf8, 0x4f8, 0x2f8, + 0x6f8, 0x1f8, 0x5f8, 0x3f8, 0x7f8, 0x4, 0x404, 0x204, 0x604, 0x104, + 0x504, 0x304, 0x704, 0x84, 0x484, 0x284, 0x684, 0x184, 0x584, 0x384, + 0x784, 0x44, 0x444, 0x244, 0x644, 0x144, 0x544, 0x344, 0x744, 0xc4, + 0x4c4, 0x2c4, 0x6c4, 0x1c4, 0x5c4, 0x3c4, 0x7c4, 0x24, 0x424, 0x224, + 0x624, 0x124, 0x524, 0x324, 0x724, 0xa4, 0x4a4, 0x2a4, 0x6a4, 0x1a4, + 0x5a4, 0x3a4, 0x7a4, 0x64, 0x464, 0x264, 0x664, 0x164, 0x564, 0x364, + 0x764, 0xe4, 0x4e4, 0x2e4, 0x6e4, 0x1e4, 0x5e4, 0x3e4, 0x7e4, 0x14, + 0x414, 0x214, 0x614, 0x114, 0x514, 0x314, 0x714, 0x94, 0x494, 0x294, + 0x694, 0x194, 0x594, 0x394, 0x794, 0x54, 0x454, 0x254, 0x654, 0x154, + 0x554, 0x354, 0x754, 0xd4, 0x4d4, 0x2d4, 0x6d4, 0x1d4, 0x5d4, 0x3d4, + 0x7d4, 0x34, 0x434, 0x234, 0x634, 0x134, 0x534, 0x334, 0x734, 0xb4, + 0x4b4, 0x2b4, 0x6b4, 0x1b4, 0x5b4, 0x3b4, 0x7b4, 0x74, 0x474, 0x274, + 0x674, 0x174, 0x574, 0x374, 0x774, 0xf4, 0x4f4, 0x2f4, 0x6f4, 0x1f4, + 0x5f4, 0x3f4, 0x7f4, 0xc, 0x40c, 0x20c, 0x60c, 0x10c, 0x50c, 0x30c, + 0x70c, 0x8c, 0x48c, 0x28c, 0x68c, 0x18c, 0x58c, 0x38c, 0x78c, 0x4c, + 0x44c, 0x24c, 0x64c, 0x14c, 0x54c, 0x34c, 0x74c, 0xcc, 0x4cc, 0x2cc, + 0x6cc, 0x1cc, 0x5cc, 0x3cc, 0x7cc, 0x2c, 0x42c, 0x22c, 0x62c, 0x12c, + 0x52c, 0x32c, 0x72c, 0xac, 0x4ac, 0x2ac, 0x6ac, 0x1ac, 0x5ac, 0x3ac, + 0x7ac, 0x6c, 0x46c, 0x26c, 0x66c, 0x16c, 0x56c, 0x36c, 0x76c, 0xec, + 0x4ec, 0x2ec, 0x6ec, 0x1ec, 0x5ec, 0x3ec, 0x7ec, 0x1c, 0x41c, 0x21c, + 0x61c, 0x11c, 0x51c, 0x31c, 0x71c, 0x9c, 0x49c, 0x29c, 0x69c, 0x19c, + 0x59c, 0x39c, 0x79c, 0x5c, 0x45c, 0x25c, 0x65c, 0x15c, 0x55c, 0x35c, + 0x75c, 0xdc, 0x4dc, 0x2dc, 0x6dc, 0x1dc, 0x5dc, 0x3dc, 0x7dc, 0x3c, + 0x43c, 0x23c, 0x63c, 0x13c, 0x53c, 0x33c, 0x73c, 0xbc, 0x4bc, 0x2bc, + 0x6bc, 0x1bc, 0x5bc, 0x3bc, 0x7bc, 0x7c, 0x47c, 0x27c, 0x67c, 0x17c, + 0x57c, 0x37c, 0x77c, 0xfc, 0x4fc, 0x2fc, 0x6fc, 0x1fc, 0x5fc, 0x3fc, + 0x7fc, 0x2, 0x402, 0x202, 0x602, 0x102, 0x502, 0x302, 0x702, 0x82, + 0x482, 0x282, 0x682, 0x182, 0x582, 0x382, 0x782, 0x42, 0x442, 0x242, + 0x642, 0x142, 0x542, 0x342, 0x742, 0xc2, 0x4c2, 0x2c2, 0x6c2, 0x1c2, + 0x5c2, 0x3c2, 0x7c2, 0x22, 0x422, 0x222, 0x622, 0x122, 0x522, 0x322, + 0x722, 0xa2, 0x4a2, 0x2a2, 0x6a2, 0x1a2, 0x5a2, 0x3a2, 0x7a2, 0x62, + 0x462, 0x262, 0x662, 0x162, 0x562, 0x362, 0x762, 0xe2, 0x4e2, 0x2e2, + 0x6e2, 0x1e2, 0x5e2, 0x3e2, 0x7e2, 0x12, 0x412, 0x212, 0x612, 0x112, + 0x512, 0x312, 0x712, 0x92, 0x492, 0x292, 0x692, 0x192, 0x592, 0x392, + 0x792, 0x52, 0x452, 0x252, 0x652, 0x152, 0x552, 0x352, 0x752, 0xd2, + 0x4d2, 0x2d2, 0x6d2, 0x1d2, 0x5d2, 0x3d2, 0x7d2, 0x32, 0x432, 0x232, + 0x632, 0x132, 0x532, 0x332, 0x732, 0xb2, 0x4b2, 0x2b2, 0x6b2, 0x1b2, + 0x5b2, 0x3b2, 0x7b2, 0x72, 0x472, 0x272, 0x672, 0x172, 0x572, 0x372, + 0x772, 0xf2, 0x4f2, 0x2f2, 0x6f2, 0x1f2, 0x5f2, 0x3f2, 0x7f2, 0xa, + 0x40a, 0x20a, 0x60a, 0x10a, 0x50a, 0x30a, 0x70a, 0x8a, 0x48a, 0x28a, + 0x68a, 0x18a, 0x58a, 0x38a, 0x78a, 0x4a, 0x44a, 0x24a, 0x64a, 0x14a, + 0x54a, 0x34a, 0x74a, 0xca, 0x4ca, 0x2ca, 0x6ca, 0x1ca, 0x5ca, 0x3ca, + 0x7ca, 0x2a, 0x42a, 0x22a, 0x62a, 0x12a, 0x52a, 0x32a, 0x72a, 0xaa, + 0x4aa, 0x2aa, 0x6aa, 0x1aa, 0x5aa, 0x3aa, 0x7aa, 0x6a, 0x46a, 0x26a, + 0x66a, 0x16a, 0x56a, 0x36a, 0x76a, 0xea, 0x4ea, 0x2ea, 0x6ea, 0x1ea, + 0x5ea, 0x3ea, 0x7ea, 0x1a, 0x41a, 0x21a, 0x61a, 0x11a, 0x51a, 0x31a, + 0x71a, 0x9a, 0x49a, 0x29a, 0x69a, 0x19a, 0x59a, 0x39a, 0x79a, 0x5a, + 0x45a, 0x25a, 0x65a, 0x15a, 0x55a, 0x35a, 0x75a, 0xda, 0x4da, 0x2da, + 0x6da, 0x1da, 0x5da, 0x3da, 0x7da, 0x3a, 0x43a, 0x23a, 0x63a, 0x13a, + 0x53a, 0x33a, 0x73a, 0xba, 0x4ba, 0x2ba, 0x6ba, 0x1ba, 0x5ba, 0x3ba, + 0x7ba, 0x7a, 0x47a, 0x27a, 0x67a, 0x17a, 0x57a, 0x37a, 0x77a, 0xfa, + 0x4fa, 0x2fa, 0x6fa, 0x1fa, 0x5fa, 0x3fa, 0x7fa, 0x6, 0x406, 0x206, + 0x606, 0x106, 0x506, 0x306, 0x706, 0x86, 0x486, 0x286, 0x686, 0x186, + 0x586, 0x386, 0x786, 0x46, 0x446, 0x246, 0x646, 0x146, 0x546, 0x346, + 0x746, 0xc6, 0x4c6, 0x2c6, 0x6c6, 0x1c6, 0x5c6, 0x3c6, 0x7c6, 0x26, + 0x426, 0x226, 0x626, 0x126, 0x526, 0x326, 0x726, 0xa6, 0x4a6, 0x2a6, + 0x6a6, 0x1a6, 0x5a6, 0x3a6, 0x7a6, 0x66, 0x466, 0x266, 0x666, 0x166, + 0x566, 0x366, 0x766, 0xe6, 0x4e6, 0x2e6, 0x6e6, 0x1e6, 0x5e6, 0x3e6, + 0x7e6, 0x16, 0x416, 0x216, 0x616, 0x116, 0x516, 0x316, 0x716, 0x96, + 0x496, 0x296, 0x696, 0x196, 0x596, 0x396, 0x796, 0x56, 0x456, 0x256, + 0x656, 0x156, 0x556, 0x356, 0x756, 0xd6, 0x4d6, 0x2d6, 0x6d6, 0x1d6, + 0x5d6, 0x3d6, 0x7d6, 0x36, 0x436, 0x236, 0x636, 0x136, 0x536, 0x336, + 0x736, 0xb6, 0x4b6, 0x2b6, 0x6b6, 0x1b6, 0x5b6, 0x3b6, 0x7b6, 0x76, + 0x476, 0x276, 0x676, 0x176, 0x576, 0x376, 0x776, 0xf6, 0x4f6, 0x2f6, + 0x6f6, 0x1f6, 0x5f6, 0x3f6, 0x7f6, 0xe, 0x40e, 0x20e, 0x60e, 0x10e, + 0x50e, 0x30e, 0x70e, 0x8e, 0x48e, 0x28e, 0x68e, 0x18e, 0x58e, 0x38e, + 0x78e, 0x4e, 0x44e, 0x24e, 0x64e, 0x14e, 0x54e, 0x34e, 0x74e, 0xce, + 0x4ce, 0x2ce, 0x6ce, 0x1ce, 0x5ce, 0x3ce, 0x7ce, 0x2e, 0x42e, 0x22e, + 0x62e, 0x12e, 0x52e, 0x32e, 0x72e, 0xae, 0x4ae, 0x2ae, 0x6ae, 0x1ae, + 0x5ae, 0x3ae, 0x7ae, 0x6e, 0x46e, 0x26e, 0x66e, 0x16e, 0x56e, 0x36e, + 0x76e, 0xee, 0x4ee, 0x2ee, 0x6ee, 0x1ee, 0x5ee, 0x3ee, 0x7ee, 0x1e, + 0x41e, 0x21e, 0x61e, 0x11e, 0x51e, 0x31e, 0x71e, 0x9e, 0x49e, 0x29e, + 0x69e, 0x19e, 0x59e, 0x39e, 0x79e, 0x5e, 0x45e, 0x25e, 0x65e, 0x15e, + 0x55e, 0x35e, 0x75e, 0xde, 0x4de, 0x2de, 0x6de, 0x1de, 0x5de, 0x3de, + 0x7de, 0x3e, 0x43e, 0x23e, 0x63e, 0x13e, 0x53e, 0x33e, 0x73e, 0xbe, + 0x4be, 0x2be, 0x6be, 0x1be, 0x5be, 0x3be, 0x7be, 0x7e, 0x47e, 0x27e, + 0x67e, 0x17e, 0x57e, 0x37e, 0x77e, 0xfe, 0x4fe, 0x2fe, 0x6fe, 0x1fe, + 0x5fe, 0x3fe, 0x7fe, 0x1 +}; + + +/* +* @brief Floating-point Twiddle factors Table Generation +*/ + +/** +* \par +* Example code for Floating-point Twiddle factors Generation: +* \par +*
for(i = 0; i< N/; i++)    
+* {    
+*	twiddleCoef[2*i]= cos(i * 2*PI/(float)N);    
+*	twiddleCoef[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 16 and PI = 3.14159265358979 +* \par +* Cos and Sin values are in interleaved fashion +* +*/ +const float32_t twiddleCoef_16[32] = { + 1.000000000f, 0.000000000f, + 0.923879533f, 0.382683432f, + 0.707106781f, 0.707106781f, + 0.382683432f, 0.923879533f, + 0.000000000f, 1.000000000f, + -0.382683432f, 0.923879533f, + -0.707106781f, 0.707106781f, + -0.923879533f, 0.382683432f, + -1.000000000f, 0.000000000f, + -0.923879533f, -0.382683432f, + -0.707106781f, -0.707106781f, + -0.382683432f, -0.923879533f, + -0.000000000f, -1.000000000f, + 0.382683432f, -0.923879533f, + 0.707106781f, -0.707106781f, + 0.923879533f, -0.382683432f +}; + +/** +* \par +* Example code for Floating-point Twiddle factors Generation: +* \par +*
for(i = 0; i< N/; i++)    
+* {    
+*	twiddleCoef[2*i]= cos(i * 2*PI/(float)N);    
+*	twiddleCoef[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 32 and PI = 3.14159265358979 +* \par +* Cos and Sin values are in interleaved fashion +* +*/ +const float32_t twiddleCoef_32[64] = { + 1.000000000f, 0.000000000f, + 0.980785280f, 0.195090322f, + 0.923879533f, 0.382683432f, + 0.831469612f, 0.555570233f, + 0.707106781f, 0.707106781f, + 0.555570233f, 0.831469612f, + 0.382683432f, 0.923879533f, + 0.195090322f, 0.980785280f, + 0.000000000f, 1.000000000f, + -0.195090322f, 0.980785280f, + -0.382683432f, 0.923879533f, + -0.555570233f, 0.831469612f, + -0.707106781f, 0.707106781f, + -0.831469612f, 0.555570233f, + -0.923879533f, 0.382683432f, + -0.980785280f, 0.195090322f, + -1.000000000f, 0.000000000f, + -0.980785280f, -0.195090322f, + -0.923879533f, -0.382683432f, + -0.831469612f, -0.555570233f, + -0.707106781f, -0.707106781f, + -0.555570233f, -0.831469612f, + -0.382683432f, -0.923879533f, + -0.195090322f, -0.980785280f, + -0.000000000f, -1.000000000f, + 0.195090322f, -0.980785280f, + 0.382683432f, -0.923879533f, + 0.555570233f, -0.831469612f, + 0.707106781f, -0.707106781f, + 0.831469612f, -0.555570233f, + 0.923879533f, -0.382683432f, + 0.980785280f, -0.195090322f +}; + +/** +* \par +* Example code for Floating-point Twiddle factors Generation: +* \par +*
for(i = 0; i< N/; i++)    
+* {    
+*	twiddleCoef[2*i]= cos(i * 2*PI/(float)N);    
+*	twiddleCoef[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 64 and PI = 3.14159265358979 +* \par +* Cos and Sin values are in interleaved fashion +* +*/ +const float32_t twiddleCoef_64[128] = { + 1.000000000f, 0.000000000f, + 0.995184727f, 0.098017140f, + 0.980785280f, 0.195090322f, + 0.956940336f, 0.290284677f, + 0.923879533f, 0.382683432f, + 0.881921264f, 0.471396737f, + 0.831469612f, 0.555570233f, + 0.773010453f, 0.634393284f, + 0.707106781f, 0.707106781f, + 0.634393284f, 0.773010453f, + 0.555570233f, 0.831469612f, + 0.471396737f, 0.881921264f, + 0.382683432f, 0.923879533f, + 0.290284677f, 0.956940336f, + 0.195090322f, 0.980785280f, + 0.098017140f, 0.995184727f, + 0.000000000f, 1.000000000f, + -0.098017140f, 0.995184727f, + -0.195090322f, 0.980785280f, + -0.290284677f, 0.956940336f, + -0.382683432f, 0.923879533f, + -0.471396737f, 0.881921264f, + -0.555570233f, 0.831469612f, + -0.634393284f, 0.773010453f, + -0.707106781f, 0.707106781f, + -0.773010453f, 0.634393284f, + -0.831469612f, 0.555570233f, + -0.881921264f, 0.471396737f, + -0.923879533f, 0.382683432f, + -0.956940336f, 0.290284677f, + -0.980785280f, 0.195090322f, + -0.995184727f, 0.098017140f, + -1.000000000f, 0.000000000f, + -0.995184727f, -0.098017140f, + -0.980785280f, -0.195090322f, + -0.956940336f, -0.290284677f, + -0.923879533f, -0.382683432f, + -0.881921264f, -0.471396737f, + -0.831469612f, -0.555570233f, + -0.773010453f, -0.634393284f, + -0.707106781f, -0.707106781f, + -0.634393284f, -0.773010453f, + -0.555570233f, -0.831469612f, + -0.471396737f, -0.881921264f, + -0.382683432f, -0.923879533f, + -0.290284677f, -0.956940336f, + -0.195090322f, -0.980785280f, + -0.098017140f, -0.995184727f, + -0.000000000f, -1.000000000f, + 0.098017140f, -0.995184727f, + 0.195090322f, -0.980785280f, + 0.290284677f, -0.956940336f, + 0.382683432f, -0.923879533f, + 0.471396737f, -0.881921264f, + 0.555570233f, -0.831469612f, + 0.634393284f, -0.773010453f, + 0.707106781f, -0.707106781f, + 0.773010453f, -0.634393284f, + 0.831469612f, -0.555570233f, + 0.881921264f, -0.471396737f, + 0.923879533f, -0.382683432f, + 0.956940336f, -0.290284677f, + 0.980785280f, -0.195090322f, + 0.995184727f, -0.098017140f +}; + +/** +* \par +* Example code for Floating-point Twiddle factors Generation: +* \par +*
for(i = 0; i< N/; i++)    
+* {    
+*	twiddleCoef[2*i]= cos(i * 2*PI/(float)N);    
+*	twiddleCoef[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 128 and PI = 3.14159265358979 +* \par +* Cos and Sin values are in interleaved fashion +* +*/ + +const float32_t twiddleCoef_128[256] = { + 1.000000000f , 0.000000000f , + 0.998795456f , 0.049067674f , + 0.995184727f , 0.098017140f , + 0.989176510f , 0.146730474f , + 0.980785280f , 0.195090322f , + 0.970031253f , 0.242980180f , + 0.956940336f , 0.290284677f , + 0.941544065f , 0.336889853f , + 0.923879533f , 0.382683432f , + 0.903989293f , 0.427555093f , + 0.881921264f , 0.471396737f , + 0.857728610f , 0.514102744f , + 0.831469612f , 0.555570233f , + 0.803207531f , 0.595699304f , + 0.773010453f , 0.634393284f , + 0.740951125f , 0.671558955f , + 0.707106781f , 0.707106781f , + 0.671558955f , 0.740951125f , + 0.634393284f , 0.773010453f , + 0.595699304f , 0.803207531f , + 0.555570233f , 0.831469612f , + 0.514102744f , 0.857728610f , + 0.471396737f , 0.881921264f , + 0.427555093f , 0.903989293f , + 0.382683432f , 0.923879533f , + 0.336889853f , 0.941544065f , + 0.290284677f , 0.956940336f , + 0.242980180f , 0.970031253f , + 0.195090322f , 0.980785280f , + 0.146730474f , 0.989176510f , + 0.098017140f , 0.995184727f , + 0.049067674f , 0.998795456f , + 0.000000000f , 1.000000000f , + -0.049067674f , 0.998795456f , + -0.098017140f , 0.995184727f , + -0.146730474f , 0.989176510f , + -0.195090322f , 0.980785280f , + -0.242980180f , 0.970031253f , + -0.290284677f , 0.956940336f , + -0.336889853f , 0.941544065f , + -0.382683432f , 0.923879533f , + -0.427555093f , 0.903989293f , + -0.471396737f , 0.881921264f , + -0.514102744f , 0.857728610f , + -0.555570233f , 0.831469612f , + -0.595699304f , 0.803207531f , + -0.634393284f , 0.773010453f , + -0.671558955f , 0.740951125f , + -0.707106781f , 0.707106781f , + -0.740951125f , 0.671558955f , + -0.773010453f , 0.634393284f , + -0.803207531f , 0.595699304f , + -0.831469612f , 0.555570233f , + -0.857728610f , 0.514102744f , + -0.881921264f , 0.471396737f , + -0.903989293f , 0.427555093f , + -0.923879533f , 0.382683432f , + -0.941544065f , 0.336889853f , + -0.956940336f , 0.290284677f , + -0.970031253f , 0.242980180f , + -0.980785280f , 0.195090322f , + -0.989176510f , 0.146730474f , + -0.995184727f , 0.098017140f , + -0.998795456f , 0.049067674f , + -1.000000000f , 0.000000000f , + -0.998795456f , -0.049067674f , + -0.995184727f , -0.098017140f , + -0.989176510f , -0.146730474f , + -0.980785280f , -0.195090322f , + -0.970031253f , -0.242980180f , + -0.956940336f , -0.290284677f , + -0.941544065f , -0.336889853f , + -0.923879533f , -0.382683432f , + -0.903989293f , -0.427555093f , + -0.881921264f , -0.471396737f , + -0.857728610f , -0.514102744f , + -0.831469612f , -0.555570233f , + -0.803207531f , -0.595699304f , + -0.773010453f , -0.634393284f , + -0.740951125f , -0.671558955f , + -0.707106781f , -0.707106781f , + -0.671558955f , -0.740951125f , + -0.634393284f , -0.773010453f , + -0.595699304f , -0.803207531f , + -0.555570233f , -0.831469612f , + -0.514102744f , -0.857728610f , + -0.471396737f , -0.881921264f , + -0.427555093f , -0.903989293f , + -0.382683432f , -0.923879533f , + -0.336889853f , -0.941544065f , + -0.290284677f , -0.956940336f , + -0.242980180f , -0.970031253f , + -0.195090322f , -0.980785280f , + -0.146730474f , -0.989176510f , + -0.098017140f , -0.995184727f , + -0.049067674f , -0.998795456f , + -0.000000000f , -1.000000000f , + 0.049067674f , -0.998795456f , + 0.098017140f , -0.995184727f , + 0.146730474f , -0.989176510f , + 0.195090322f , -0.980785280f , + 0.242980180f , -0.970031253f , + 0.290284677f , -0.956940336f , + 0.336889853f , -0.941544065f , + 0.382683432f , -0.923879533f , + 0.427555093f , -0.903989293f , + 0.471396737f , -0.881921264f , + 0.514102744f , -0.857728610f , + 0.555570233f , -0.831469612f , + 0.595699304f , -0.803207531f , + 0.634393284f , -0.773010453f , + 0.671558955f , -0.740951125f , + 0.707106781f , -0.707106781f , + 0.740951125f , -0.671558955f , + 0.773010453f , -0.634393284f , + 0.803207531f , -0.595699304f , + 0.831469612f , -0.555570233f , + 0.857728610f , -0.514102744f , + 0.881921264f , -0.471396737f , + 0.903989293f , -0.427555093f , + 0.923879533f , -0.382683432f , + 0.941544065f , -0.336889853f , + 0.956940336f , -0.290284677f , + 0.970031253f , -0.242980180f , + 0.980785280f , -0.195090322f , + 0.989176510f , -0.146730474f , + 0.995184727f , -0.098017140f , + 0.998795456f , -0.049067674f +}; + +/** +* \par +* Example code for Floating-point Twiddle factors Generation: +* \par +*
for(i = 0; i< N/; i++)    
+* {    
+*	twiddleCoef[2*i]= cos(i * 2*PI/(float)N);    
+*	twiddleCoef[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 256 and PI = 3.14159265358979 +* \par +* Cos and Sin values are in interleaved fashion +* +*/ +const float32_t twiddleCoef_256[512] = { + 1.000000000f, 0.000000000f, + 0.999698819f, 0.024541229f, + 0.998795456f, 0.049067674f, + 0.997290457f, 0.073564564f, + 0.995184727f, 0.098017140f, + 0.992479535f, 0.122410675f, + 0.989176510f, 0.146730474f, + 0.985277642f, 0.170961889f, + 0.980785280f, 0.195090322f, + 0.975702130f, 0.219101240f, + 0.970031253f, 0.242980180f, + 0.963776066f, 0.266712757f, + 0.956940336f, 0.290284677f, + 0.949528181f, 0.313681740f, + 0.941544065f, 0.336889853f, + 0.932992799f, 0.359895037f, + 0.923879533f, 0.382683432f, + 0.914209756f, 0.405241314f, + 0.903989293f, 0.427555093f, + 0.893224301f, 0.449611330f, + 0.881921264f, 0.471396737f, + 0.870086991f, 0.492898192f, + 0.857728610f, 0.514102744f, + 0.844853565f, 0.534997620f, + 0.831469612f, 0.555570233f, + 0.817584813f, 0.575808191f, + 0.803207531f, 0.595699304f, + 0.788346428f, 0.615231591f, + 0.773010453f, 0.634393284f, + 0.757208847f, 0.653172843f, + 0.740951125f, 0.671558955f, + 0.724247083f, 0.689540545f, + 0.707106781f, 0.707106781f, + 0.689540545f, 0.724247083f, + 0.671558955f, 0.740951125f, + 0.653172843f, 0.757208847f, + 0.634393284f, 0.773010453f, + 0.615231591f, 0.788346428f, + 0.595699304f, 0.803207531f, + 0.575808191f, 0.817584813f, + 0.555570233f, 0.831469612f, + 0.534997620f, 0.844853565f, + 0.514102744f, 0.857728610f, + 0.492898192f, 0.870086991f, + 0.471396737f, 0.881921264f, + 0.449611330f, 0.893224301f, + 0.427555093f, 0.903989293f, + 0.405241314f, 0.914209756f, + 0.382683432f, 0.923879533f, + 0.359895037f, 0.932992799f, + 0.336889853f, 0.941544065f, + 0.313681740f, 0.949528181f, + 0.290284677f, 0.956940336f, + 0.266712757f, 0.963776066f, + 0.242980180f, 0.970031253f, + 0.219101240f, 0.975702130f, + 0.195090322f, 0.980785280f, + 0.170961889f, 0.985277642f, + 0.146730474f, 0.989176510f, + 0.122410675f, 0.992479535f, + 0.098017140f, 0.995184727f, + 0.073564564f, 0.997290457f, + 0.049067674f, 0.998795456f, + 0.024541229f, 0.999698819f, + 0.000000000f, 1.000000000f, + -0.024541229f, 0.999698819f, + -0.049067674f, 0.998795456f, + -0.073564564f, 0.997290457f, + -0.098017140f, 0.995184727f, + -0.122410675f, 0.992479535f, + -0.146730474f, 0.989176510f, + -0.170961889f, 0.985277642f, + -0.195090322f, 0.980785280f, + -0.219101240f, 0.975702130f, + -0.242980180f, 0.970031253f, + -0.266712757f, 0.963776066f, + -0.290284677f, 0.956940336f, + -0.313681740f, 0.949528181f, + -0.336889853f, 0.941544065f, + -0.359895037f, 0.932992799f, + -0.382683432f, 0.923879533f, + -0.405241314f, 0.914209756f, + -0.427555093f, 0.903989293f, + -0.449611330f, 0.893224301f, + -0.471396737f, 0.881921264f, + -0.492898192f, 0.870086991f, + -0.514102744f, 0.857728610f, + -0.534997620f, 0.844853565f, + -0.555570233f, 0.831469612f, + -0.575808191f, 0.817584813f, + -0.595699304f, 0.803207531f, + -0.615231591f, 0.788346428f, + -0.634393284f, 0.773010453f, + -0.653172843f, 0.757208847f, + -0.671558955f, 0.740951125f, + -0.689540545f, 0.724247083f, + -0.707106781f, 0.707106781f, + -0.724247083f, 0.689540545f, + -0.740951125f, 0.671558955f, + -0.757208847f, 0.653172843f, + -0.773010453f, 0.634393284f, + -0.788346428f, 0.615231591f, + -0.803207531f, 0.595699304f, + -0.817584813f, 0.575808191f, + -0.831469612f, 0.555570233f, + -0.844853565f, 0.534997620f, + -0.857728610f, 0.514102744f, + -0.870086991f, 0.492898192f, + -0.881921264f, 0.471396737f, + -0.893224301f, 0.449611330f, + -0.903989293f, 0.427555093f, + -0.914209756f, 0.405241314f, + -0.923879533f, 0.382683432f, + -0.932992799f, 0.359895037f, + -0.941544065f, 0.336889853f, + -0.949528181f, 0.313681740f, + -0.956940336f, 0.290284677f, + -0.963776066f, 0.266712757f, + -0.970031253f, 0.242980180f, + -0.975702130f, 0.219101240f, + -0.980785280f, 0.195090322f, + -0.985277642f, 0.170961889f, + -0.989176510f, 0.146730474f, + -0.992479535f, 0.122410675f, + -0.995184727f, 0.098017140f, + -0.997290457f, 0.073564564f, + -0.998795456f, 0.049067674f, + -0.999698819f, 0.024541229f, + -1.000000000f, 0.000000000f, + -0.999698819f, -0.024541229f, + -0.998795456f, -0.049067674f, + -0.997290457f, -0.073564564f, + -0.995184727f, -0.098017140f, + -0.992479535f, -0.122410675f, + -0.989176510f, -0.146730474f, + -0.985277642f, -0.170961889f, + -0.980785280f, -0.195090322f, + -0.975702130f, -0.219101240f, + -0.970031253f, -0.242980180f, + -0.963776066f, -0.266712757f, + -0.956940336f, -0.290284677f, + -0.949528181f, -0.313681740f, + -0.941544065f, -0.336889853f, + -0.932992799f, -0.359895037f, + -0.923879533f, -0.382683432f, + -0.914209756f, -0.405241314f, + -0.903989293f, -0.427555093f, + -0.893224301f, -0.449611330f, + -0.881921264f, -0.471396737f, + -0.870086991f, -0.492898192f, + -0.857728610f, -0.514102744f, + -0.844853565f, -0.534997620f, + -0.831469612f, -0.555570233f, + -0.817584813f, -0.575808191f, + -0.803207531f, -0.595699304f, + -0.788346428f, -0.615231591f, + -0.773010453f, -0.634393284f, + -0.757208847f, -0.653172843f, + -0.740951125f, -0.671558955f, + -0.724247083f, -0.689540545f, + -0.707106781f, -0.707106781f, + -0.689540545f, -0.724247083f, + -0.671558955f, -0.740951125f, + -0.653172843f, -0.757208847f, + -0.634393284f, -0.773010453f, + -0.615231591f, -0.788346428f, + -0.595699304f, -0.803207531f, + -0.575808191f, -0.817584813f, + -0.555570233f, -0.831469612f, + -0.534997620f, -0.844853565f, + -0.514102744f, -0.857728610f, + -0.492898192f, -0.870086991f, + -0.471396737f, -0.881921264f, + -0.449611330f, -0.893224301f, + -0.427555093f, -0.903989293f, + -0.405241314f, -0.914209756f, + -0.382683432f, -0.923879533f, + -0.359895037f, -0.932992799f, + -0.336889853f, -0.941544065f, + -0.313681740f, -0.949528181f, + -0.290284677f, -0.956940336f, + -0.266712757f, -0.963776066f, + -0.242980180f, -0.970031253f, + -0.219101240f, -0.975702130f, + -0.195090322f, -0.980785280f, + -0.170961889f, -0.985277642f, + -0.146730474f, -0.989176510f, + -0.122410675f, -0.992479535f, + -0.098017140f, -0.995184727f, + -0.073564564f, -0.997290457f, + -0.049067674f, -0.998795456f, + -0.024541229f, -0.999698819f, + -0.000000000f, -1.000000000f, + 0.024541229f, -0.999698819f, + 0.049067674f, -0.998795456f, + 0.073564564f, -0.997290457f, + 0.098017140f, -0.995184727f, + 0.122410675f, -0.992479535f, + 0.146730474f, -0.989176510f, + 0.170961889f, -0.985277642f, + 0.195090322f, -0.980785280f, + 0.219101240f, -0.975702130f, + 0.242980180f, -0.970031253f, + 0.266712757f, -0.963776066f, + 0.290284677f, -0.956940336f, + 0.313681740f, -0.949528181f, + 0.336889853f, -0.941544065f, + 0.359895037f, -0.932992799f, + 0.382683432f, -0.923879533f, + 0.405241314f, -0.914209756f, + 0.427555093f, -0.903989293f, + 0.449611330f, -0.893224301f, + 0.471396737f, -0.881921264f, + 0.492898192f, -0.870086991f, + 0.514102744f, -0.857728610f, + 0.534997620f, -0.844853565f, + 0.555570233f, -0.831469612f, + 0.575808191f, -0.817584813f, + 0.595699304f, -0.803207531f, + 0.615231591f, -0.788346428f, + 0.634393284f, -0.773010453f, + 0.653172843f, -0.757208847f, + 0.671558955f, -0.740951125f, + 0.689540545f, -0.724247083f, + 0.707106781f, -0.707106781f, + 0.724247083f, -0.689540545f, + 0.740951125f, -0.671558955f, + 0.757208847f, -0.653172843f, + 0.773010453f, -0.634393284f, + 0.788346428f, -0.615231591f, + 0.803207531f, -0.595699304f, + 0.817584813f, -0.575808191f, + 0.831469612f, -0.555570233f, + 0.844853565f, -0.534997620f, + 0.857728610f, -0.514102744f, + 0.870086991f, -0.492898192f, + 0.881921264f, -0.471396737f, + 0.893224301f, -0.449611330f, + 0.903989293f, -0.427555093f, + 0.914209756f, -0.405241314f, + 0.923879533f, -0.382683432f, + 0.932992799f, -0.359895037f, + 0.941544065f, -0.336889853f, + 0.949528181f, -0.313681740f, + 0.956940336f, -0.290284677f, + 0.963776066f, -0.266712757f, + 0.970031253f, -0.242980180f, + 0.975702130f, -0.219101240f, + 0.980785280f, -0.195090322f, + 0.985277642f, -0.170961889f, + 0.989176510f, -0.146730474f, + 0.992479535f, -0.122410675f, + 0.995184727f, -0.098017140f, + 0.997290457f, -0.073564564f, + 0.998795456f, -0.049067674f, + 0.999698819f, -0.024541229f +}; + +/** +* \par +* Example code for Floating-point Twiddle factors Generation: +* \par +*
for(i = 0; i< N/; i++)    
+* {    
+*	twiddleCoef[2*i]= cos(i * 2*PI/(float)N);    
+*	twiddleCoef[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 512 and PI = 3.14159265358979 +* \par +* Cos and Sin values are in interleaved fashion +* +*/ +const float32_t twiddleCoef_512[1024] = { + 1.000000000f, 0.000000000f, + 0.999924702f, 0.012271538f, + 0.999698819f, 0.024541229f, + 0.999322385f, 0.036807223f, + 0.998795456f, 0.049067674f, + 0.998118113f, 0.061320736f, + 0.997290457f, 0.073564564f, + 0.996312612f, 0.085797312f, + 0.995184727f, 0.098017140f, + 0.993906970f, 0.110222207f, + 0.992479535f, 0.122410675f, + 0.990902635f, 0.134580709f, + 0.989176510f, 0.146730474f, + 0.987301418f, 0.158858143f, + 0.985277642f, 0.170961889f, + 0.983105487f, 0.183039888f, + 0.980785280f, 0.195090322f, + 0.978317371f, 0.207111376f, + 0.975702130f, 0.219101240f, + 0.972939952f, 0.231058108f, + 0.970031253f, 0.242980180f, + 0.966976471f, 0.254865660f, + 0.963776066f, 0.266712757f, + 0.960430519f, 0.278519689f, + 0.956940336f, 0.290284677f, + 0.953306040f, 0.302005949f, + 0.949528181f, 0.313681740f, + 0.945607325f, 0.325310292f, + 0.941544065f, 0.336889853f, + 0.937339012f, 0.348418680f, + 0.932992799f, 0.359895037f, + 0.928506080f, 0.371317194f, + 0.923879533f, 0.382683432f, + 0.919113852f, 0.393992040f, + 0.914209756f, 0.405241314f, + 0.909167983f, 0.416429560f, + 0.903989293f, 0.427555093f, + 0.898674466f, 0.438616239f, + 0.893224301f, 0.449611330f, + 0.887639620f, 0.460538711f, + 0.881921264f, 0.471396737f, + 0.876070094f, 0.482183772f, + 0.870086991f, 0.492898192f, + 0.863972856f, 0.503538384f, + 0.857728610f, 0.514102744f, + 0.851355193f, 0.524589683f, + 0.844853565f, 0.534997620f, + 0.838224706f, 0.545324988f, + 0.831469612f, 0.555570233f, + 0.824589303f, 0.565731811f, + 0.817584813f, 0.575808191f, + 0.810457198f, 0.585797857f, + 0.803207531f, 0.595699304f, + 0.795836905f, 0.605511041f, + 0.788346428f, 0.615231591f, + 0.780737229f, 0.624859488f, + 0.773010453f, 0.634393284f, + 0.765167266f, 0.643831543f, + 0.757208847f, 0.653172843f, + 0.749136395f, 0.662415778f, + 0.740951125f, 0.671558955f, + 0.732654272f, 0.680600998f, + 0.724247083f, 0.689540545f, + 0.715730825f, 0.698376249f, + 0.707106781f, 0.707106781f, + 0.698376249f, 0.715730825f, + 0.689540545f, 0.724247083f, + 0.680600998f, 0.732654272f, + 0.671558955f, 0.740951125f, + 0.662415778f, 0.749136395f, + 0.653172843f, 0.757208847f, + 0.643831543f, 0.765167266f, + 0.634393284f, 0.773010453f, + 0.624859488f, 0.780737229f, + 0.615231591f, 0.788346428f, + 0.605511041f, 0.795836905f, + 0.595699304f, 0.803207531f, + 0.585797857f, 0.810457198f, + 0.575808191f, 0.817584813f, + 0.565731811f, 0.824589303f, + 0.555570233f, 0.831469612f, + 0.545324988f, 0.838224706f, + 0.534997620f, 0.844853565f, + 0.524589683f, 0.851355193f, + 0.514102744f, 0.857728610f, + 0.503538384f, 0.863972856f, + 0.492898192f, 0.870086991f, + 0.482183772f, 0.876070094f, + 0.471396737f, 0.881921264f, + 0.460538711f, 0.887639620f, + 0.449611330f, 0.893224301f, + 0.438616239f, 0.898674466f, + 0.427555093f, 0.903989293f, + 0.416429560f, 0.909167983f, + 0.405241314f, 0.914209756f, + 0.393992040f, 0.919113852f, + 0.382683432f, 0.923879533f, + 0.371317194f, 0.928506080f, + 0.359895037f, 0.932992799f, + 0.348418680f, 0.937339012f, + 0.336889853f, 0.941544065f, + 0.325310292f, 0.945607325f, + 0.313681740f, 0.949528181f, + 0.302005949f, 0.953306040f, + 0.290284677f, 0.956940336f, + 0.278519689f, 0.960430519f, + 0.266712757f, 0.963776066f, + 0.254865660f, 0.966976471f, + 0.242980180f, 0.970031253f, + 0.231058108f, 0.972939952f, + 0.219101240f, 0.975702130f, + 0.207111376f, 0.978317371f, + 0.195090322f, 0.980785280f, + 0.183039888f, 0.983105487f, + 0.170961889f, 0.985277642f, + 0.158858143f, 0.987301418f, + 0.146730474f, 0.989176510f, + 0.134580709f, 0.990902635f, + 0.122410675f, 0.992479535f, + 0.110222207f, 0.993906970f, + 0.098017140f, 0.995184727f, + 0.085797312f, 0.996312612f, + 0.073564564f, 0.997290457f, + 0.061320736f, 0.998118113f, + 0.049067674f, 0.998795456f, + 0.036807223f, 0.999322385f, + 0.024541229f, 0.999698819f, + 0.012271538f, 0.999924702f, + 0.000000000f, 1.000000000f, + -0.012271538f, 0.999924702f, + -0.024541229f, 0.999698819f, + -0.036807223f, 0.999322385f, + -0.049067674f, 0.998795456f, + -0.061320736f, 0.998118113f, + -0.073564564f, 0.997290457f, + -0.085797312f, 0.996312612f, + -0.098017140f, 0.995184727f, + -0.110222207f, 0.993906970f, + -0.122410675f, 0.992479535f, + -0.134580709f, 0.990902635f, + -0.146730474f, 0.989176510f, + -0.158858143f, 0.987301418f, + -0.170961889f, 0.985277642f, + -0.183039888f, 0.983105487f, + -0.195090322f, 0.980785280f, + -0.207111376f, 0.978317371f, + -0.219101240f, 0.975702130f, + -0.231058108f, 0.972939952f, + -0.242980180f, 0.970031253f, + -0.254865660f, 0.966976471f, + -0.266712757f, 0.963776066f, + -0.278519689f, 0.960430519f, + -0.290284677f, 0.956940336f, + -0.302005949f, 0.953306040f, + -0.313681740f, 0.949528181f, + -0.325310292f, 0.945607325f, + -0.336889853f, 0.941544065f, + -0.348418680f, 0.937339012f, + -0.359895037f, 0.932992799f, + -0.371317194f, 0.928506080f, + -0.382683432f, 0.923879533f, + -0.393992040f, 0.919113852f, + -0.405241314f, 0.914209756f, + -0.416429560f, 0.909167983f, + -0.427555093f, 0.903989293f, + -0.438616239f, 0.898674466f, + -0.449611330f, 0.893224301f, + -0.460538711f, 0.887639620f, + -0.471396737f, 0.881921264f, + -0.482183772f, 0.876070094f, + -0.492898192f, 0.870086991f, + -0.503538384f, 0.863972856f, + -0.514102744f, 0.857728610f, + -0.524589683f, 0.851355193f, + -0.534997620f, 0.844853565f, + -0.545324988f, 0.838224706f, + -0.555570233f, 0.831469612f, + -0.565731811f, 0.824589303f, + -0.575808191f, 0.817584813f, + -0.585797857f, 0.810457198f, + -0.595699304f, 0.803207531f, + -0.605511041f, 0.795836905f, + -0.615231591f, 0.788346428f, + -0.624859488f, 0.780737229f, + -0.634393284f, 0.773010453f, + -0.643831543f, 0.765167266f, + -0.653172843f, 0.757208847f, + -0.662415778f, 0.749136395f, + -0.671558955f, 0.740951125f, + -0.680600998f, 0.732654272f, + -0.689540545f, 0.724247083f, + -0.698376249f, 0.715730825f, + -0.707106781f, 0.707106781f, + -0.715730825f, 0.698376249f, + -0.724247083f, 0.689540545f, + -0.732654272f, 0.680600998f, + -0.740951125f, 0.671558955f, + -0.749136395f, 0.662415778f, + -0.757208847f, 0.653172843f, + -0.765167266f, 0.643831543f, + -0.773010453f, 0.634393284f, + -0.780737229f, 0.624859488f, + -0.788346428f, 0.615231591f, + -0.795836905f, 0.605511041f, + -0.803207531f, 0.595699304f, + -0.810457198f, 0.585797857f, + -0.817584813f, 0.575808191f, + -0.824589303f, 0.565731811f, + -0.831469612f, 0.555570233f, + -0.838224706f, 0.545324988f, + -0.844853565f, 0.534997620f, + -0.851355193f, 0.524589683f, + -0.857728610f, 0.514102744f, + -0.863972856f, 0.503538384f, + -0.870086991f, 0.492898192f, + -0.876070094f, 0.482183772f, + -0.881921264f, 0.471396737f, + -0.887639620f, 0.460538711f, + -0.893224301f, 0.449611330f, + -0.898674466f, 0.438616239f, + -0.903989293f, 0.427555093f, + -0.909167983f, 0.416429560f, + -0.914209756f, 0.405241314f, + -0.919113852f, 0.393992040f, + -0.923879533f, 0.382683432f, + -0.928506080f, 0.371317194f, + -0.932992799f, 0.359895037f, + -0.937339012f, 0.348418680f, + -0.941544065f, 0.336889853f, + -0.945607325f, 0.325310292f, + -0.949528181f, 0.313681740f, + -0.953306040f, 0.302005949f, + -0.956940336f, 0.290284677f, + -0.960430519f, 0.278519689f, + -0.963776066f, 0.266712757f, + -0.966976471f, 0.254865660f, + -0.970031253f, 0.242980180f, + -0.972939952f, 0.231058108f, + -0.975702130f, 0.219101240f, + -0.978317371f, 0.207111376f, + -0.980785280f, 0.195090322f, + -0.983105487f, 0.183039888f, + -0.985277642f, 0.170961889f, + -0.987301418f, 0.158858143f, + -0.989176510f, 0.146730474f, + -0.990902635f, 0.134580709f, + -0.992479535f, 0.122410675f, + -0.993906970f, 0.110222207f, + -0.995184727f, 0.098017140f, + -0.996312612f, 0.085797312f, + -0.997290457f, 0.073564564f, + -0.998118113f, 0.061320736f, + -0.998795456f, 0.049067674f, + -0.999322385f, 0.036807223f, + -0.999698819f, 0.024541229f, + -0.999924702f, 0.012271538f, + -1.000000000f, 0.000000000f, + -0.999924702f, -0.012271538f, + -0.999698819f, -0.024541229f, + -0.999322385f, -0.036807223f, + -0.998795456f, -0.049067674f, + -0.998118113f, -0.061320736f, + -0.997290457f, -0.073564564f, + -0.996312612f, -0.085797312f, + -0.995184727f, -0.098017140f, + -0.993906970f, -0.110222207f, + -0.992479535f, -0.122410675f, + -0.990902635f, -0.134580709f, + -0.989176510f, -0.146730474f, + -0.987301418f, -0.158858143f, + -0.985277642f, -0.170961889f, + -0.983105487f, -0.183039888f, + -0.980785280f, -0.195090322f, + -0.978317371f, -0.207111376f, + -0.975702130f, -0.219101240f, + -0.972939952f, -0.231058108f, + -0.970031253f, -0.242980180f, + -0.966976471f, -0.254865660f, + -0.963776066f, -0.266712757f, + -0.960430519f, -0.278519689f, + -0.956940336f, -0.290284677f, + -0.953306040f, -0.302005949f, + -0.949528181f, -0.313681740f, + -0.945607325f, -0.325310292f, + -0.941544065f, -0.336889853f, + -0.937339012f, -0.348418680f, + -0.932992799f, -0.359895037f, + -0.928506080f, -0.371317194f, + -0.923879533f, -0.382683432f, + -0.919113852f, -0.393992040f, + -0.914209756f, -0.405241314f, + -0.909167983f, -0.416429560f, + -0.903989293f, -0.427555093f, + -0.898674466f, -0.438616239f, + -0.893224301f, -0.449611330f, + -0.887639620f, -0.460538711f, + -0.881921264f, -0.471396737f, + -0.876070094f, -0.482183772f, + -0.870086991f, -0.492898192f, + -0.863972856f, -0.503538384f, + -0.857728610f, -0.514102744f, + -0.851355193f, -0.524589683f, + -0.844853565f, -0.534997620f, + -0.838224706f, -0.545324988f, + -0.831469612f, -0.555570233f, + -0.824589303f, -0.565731811f, + -0.817584813f, -0.575808191f, + -0.810457198f, -0.585797857f, + -0.803207531f, -0.595699304f, + -0.795836905f, -0.605511041f, + -0.788346428f, -0.615231591f, + -0.780737229f, -0.624859488f, + -0.773010453f, -0.634393284f, + -0.765167266f, -0.643831543f, + -0.757208847f, -0.653172843f, + -0.749136395f, -0.662415778f, + -0.740951125f, -0.671558955f, + -0.732654272f, -0.680600998f, + -0.724247083f, -0.689540545f, + -0.715730825f, -0.698376249f, + -0.707106781f, -0.707106781f, + -0.698376249f, -0.715730825f, + -0.689540545f, -0.724247083f, + -0.680600998f, -0.732654272f, + -0.671558955f, -0.740951125f, + -0.662415778f, -0.749136395f, + -0.653172843f, -0.757208847f, + -0.643831543f, -0.765167266f, + -0.634393284f, -0.773010453f, + -0.624859488f, -0.780737229f, + -0.615231591f, -0.788346428f, + -0.605511041f, -0.795836905f, + -0.595699304f, -0.803207531f, + -0.585797857f, -0.810457198f, + -0.575808191f, -0.817584813f, + -0.565731811f, -0.824589303f, + -0.555570233f, -0.831469612f, + -0.545324988f, -0.838224706f, + -0.534997620f, -0.844853565f, + -0.524589683f, -0.851355193f, + -0.514102744f, -0.857728610f, + -0.503538384f, -0.863972856f, + -0.492898192f, -0.870086991f, + -0.482183772f, -0.876070094f, + -0.471396737f, -0.881921264f, + -0.460538711f, -0.887639620f, + -0.449611330f, -0.893224301f, + -0.438616239f, -0.898674466f, + -0.427555093f, -0.903989293f, + -0.416429560f, -0.909167983f, + -0.405241314f, -0.914209756f, + -0.393992040f, -0.919113852f, + -0.382683432f, -0.923879533f, + -0.371317194f, -0.928506080f, + -0.359895037f, -0.932992799f, + -0.348418680f, -0.937339012f, + -0.336889853f, -0.941544065f, + -0.325310292f, -0.945607325f, + -0.313681740f, -0.949528181f, + -0.302005949f, -0.953306040f, + -0.290284677f, -0.956940336f, + -0.278519689f, -0.960430519f, + -0.266712757f, -0.963776066f, + -0.254865660f, -0.966976471f, + -0.242980180f, -0.970031253f, + -0.231058108f, -0.972939952f, + -0.219101240f, -0.975702130f, + -0.207111376f, -0.978317371f, + -0.195090322f, -0.980785280f, + -0.183039888f, -0.983105487f, + -0.170961889f, -0.985277642f, + -0.158858143f, -0.987301418f, + -0.146730474f, -0.989176510f, + -0.134580709f, -0.990902635f, + -0.122410675f, -0.992479535f, + -0.110222207f, -0.993906970f, + -0.098017140f, -0.995184727f, + -0.085797312f, -0.996312612f, + -0.073564564f, -0.997290457f, + -0.061320736f, -0.998118113f, + -0.049067674f, -0.998795456f, + -0.036807223f, -0.999322385f, + -0.024541229f, -0.999698819f, + -0.012271538f, -0.999924702f, + -0.000000000f, -1.000000000f, + 0.012271538f, -0.999924702f, + 0.024541229f, -0.999698819f, + 0.036807223f, -0.999322385f, + 0.049067674f, -0.998795456f, + 0.061320736f, -0.998118113f, + 0.073564564f, -0.997290457f, + 0.085797312f, -0.996312612f, + 0.098017140f, -0.995184727f, + 0.110222207f, -0.993906970f, + 0.122410675f, -0.992479535f, + 0.134580709f, -0.990902635f, + 0.146730474f, -0.989176510f, + 0.158858143f, -0.987301418f, + 0.170961889f, -0.985277642f, + 0.183039888f, -0.983105487f, + 0.195090322f, -0.980785280f, + 0.207111376f, -0.978317371f, + 0.219101240f, -0.975702130f, + 0.231058108f, -0.972939952f, + 0.242980180f, -0.970031253f, + 0.254865660f, -0.966976471f, + 0.266712757f, -0.963776066f, + 0.278519689f, -0.960430519f, + 0.290284677f, -0.956940336f, + 0.302005949f, -0.953306040f, + 0.313681740f, -0.949528181f, + 0.325310292f, -0.945607325f, + 0.336889853f, -0.941544065f, + 0.348418680f, -0.937339012f, + 0.359895037f, -0.932992799f, + 0.371317194f, -0.928506080f, + 0.382683432f, -0.923879533f, + 0.393992040f, -0.919113852f, + 0.405241314f, -0.914209756f, + 0.416429560f, -0.909167983f, + 0.427555093f, -0.903989293f, + 0.438616239f, -0.898674466f, + 0.449611330f, -0.893224301f, + 0.460538711f, -0.887639620f, + 0.471396737f, -0.881921264f, + 0.482183772f, -0.876070094f, + 0.492898192f, -0.870086991f, + 0.503538384f, -0.863972856f, + 0.514102744f, -0.857728610f, + 0.524589683f, -0.851355193f, + 0.534997620f, -0.844853565f, + 0.545324988f, -0.838224706f, + 0.555570233f, -0.831469612f, + 0.565731811f, -0.824589303f, + 0.575808191f, -0.817584813f, + 0.585797857f, -0.810457198f, + 0.595699304f, -0.803207531f, + 0.605511041f, -0.795836905f, + 0.615231591f, -0.788346428f, + 0.624859488f, -0.780737229f, + 0.634393284f, -0.773010453f, + 0.643831543f, -0.765167266f, + 0.653172843f, -0.757208847f, + 0.662415778f, -0.749136395f, + 0.671558955f, -0.740951125f, + 0.680600998f, -0.732654272f, + 0.689540545f, -0.724247083f, + 0.698376249f, -0.715730825f, + 0.707106781f, -0.707106781f, + 0.715730825f, -0.698376249f, + 0.724247083f, -0.689540545f, + 0.732654272f, -0.680600998f, + 0.740951125f, -0.671558955f, + 0.749136395f, -0.662415778f, + 0.757208847f, -0.653172843f, + 0.765167266f, -0.643831543f, + 0.773010453f, -0.634393284f, + 0.780737229f, -0.624859488f, + 0.788346428f, -0.615231591f, + 0.795836905f, -0.605511041f, + 0.803207531f, -0.595699304f, + 0.810457198f, -0.585797857f, + 0.817584813f, -0.575808191f, + 0.824589303f, -0.565731811f, + 0.831469612f, -0.555570233f, + 0.838224706f, -0.545324988f, + 0.844853565f, -0.534997620f, + 0.851355193f, -0.524589683f, + 0.857728610f, -0.514102744f, + 0.863972856f, -0.503538384f, + 0.870086991f, -0.492898192f, + 0.876070094f, -0.482183772f, + 0.881921264f, -0.471396737f, + 0.887639620f, -0.460538711f, + 0.893224301f, -0.449611330f, + 0.898674466f, -0.438616239f, + 0.903989293f, -0.427555093f, + 0.909167983f, -0.416429560f, + 0.914209756f, -0.405241314f, + 0.919113852f, -0.393992040f, + 0.923879533f, -0.382683432f, + 0.928506080f, -0.371317194f, + 0.932992799f, -0.359895037f, + 0.937339012f, -0.348418680f, + 0.941544065f, -0.336889853f, + 0.945607325f, -0.325310292f, + 0.949528181f, -0.313681740f, + 0.953306040f, -0.302005949f, + 0.956940336f, -0.290284677f, + 0.960430519f, -0.278519689f, + 0.963776066f, -0.266712757f, + 0.966976471f, -0.254865660f, + 0.970031253f, -0.242980180f, + 0.972939952f, -0.231058108f, + 0.975702130f, -0.219101240f, + 0.978317371f, -0.207111376f, + 0.980785280f, -0.195090322f, + 0.983105487f, -0.183039888f, + 0.985277642f, -0.170961889f, + 0.987301418f, -0.158858143f, + 0.989176510f, -0.146730474f, + 0.990902635f, -0.134580709f, + 0.992479535f, -0.122410675f, + 0.993906970f, -0.110222207f, + 0.995184727f, -0.098017140f, + 0.996312612f, -0.085797312f, + 0.997290457f, -0.073564564f, + 0.998118113f, -0.061320736f, + 0.998795456f, -0.049067674f, + 0.999322385f, -0.036807223f, + 0.999698819f, -0.024541229f, + 0.999924702f, -0.012271538f +}; +/** +* \par +* Example code for Floating-point Twiddle factors Generation: +* \par +*
for(i = 0; i< N/; i++)    
+* {    
+*	twiddleCoef[2*i]= cos(i * 2*PI/(float)N);    
+*	twiddleCoef[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 1024 and PI = 3.14159265358979 +* \par +* Cos and Sin values are in interleaved fashion +* +*/ +const float32_t twiddleCoef_1024[2048] = { +1.000000000f , 0.000000000f , +0.999981175f , 0.006135885f , +0.999924702f , 0.012271538f , +0.999830582f , 0.018406730f , +0.999698819f , 0.024541229f , +0.999529418f , 0.030674803f , +0.999322385f , 0.036807223f , +0.999077728f , 0.042938257f , +0.998795456f , 0.049067674f , +0.998475581f , 0.055195244f , +0.998118113f , 0.061320736f , +0.997723067f , 0.067443920f , +0.997290457f , 0.073564564f , +0.996820299f , 0.079682438f , +0.996312612f , 0.085797312f , +0.995767414f , 0.091908956f , +0.995184727f , 0.098017140f , +0.994564571f , 0.104121634f , +0.993906970f , 0.110222207f , +0.993211949f , 0.116318631f , +0.992479535f , 0.122410675f , +0.991709754f , 0.128498111f , +0.990902635f , 0.134580709f , +0.990058210f , 0.140658239f , +0.989176510f , 0.146730474f , +0.988257568f , 0.152797185f , +0.987301418f , 0.158858143f , +0.986308097f , 0.164913120f , +0.985277642f , 0.170961889f , +0.984210092f , 0.177004220f , +0.983105487f , 0.183039888f , +0.981963869f , 0.189068664f , +0.980785280f , 0.195090322f , +0.979569766f , 0.201104635f , +0.978317371f , 0.207111376f , +0.977028143f , 0.213110320f , +0.975702130f , 0.219101240f , +0.974339383f , 0.225083911f , +0.972939952f , 0.231058108f , +0.971503891f , 0.237023606f , +0.970031253f , 0.242980180f , +0.968522094f , 0.248927606f , +0.966976471f , 0.254865660f , +0.965394442f , 0.260794118f , +0.963776066f , 0.266712757f , +0.962121404f , 0.272621355f , +0.960430519f , 0.278519689f , +0.958703475f , 0.284407537f , +0.956940336f , 0.290284677f , +0.955141168f , 0.296150888f , +0.953306040f , 0.302005949f , +0.951435021f , 0.307849640f , +0.949528181f , 0.313681740f , +0.947585591f , 0.319502031f , +0.945607325f , 0.325310292f , +0.943593458f , 0.331106306f , +0.941544065f , 0.336889853f , +0.939459224f , 0.342660717f , +0.937339012f , 0.348418680f , +0.935183510f , 0.354163525f , +0.932992799f , 0.359895037f , +0.930766961f , 0.365612998f , +0.928506080f , 0.371317194f , +0.926210242f , 0.377007410f , +0.923879533f , 0.382683432f , +0.921514039f , 0.388345047f , +0.919113852f , 0.393992040f , +0.916679060f , 0.399624200f , +0.914209756f , 0.405241314f , +0.911706032f , 0.410843171f , +0.909167983f , 0.416429560f , +0.906595705f , 0.422000271f , +0.903989293f , 0.427555093f , +0.901348847f , 0.433093819f , +0.898674466f , 0.438616239f , +0.895966250f , 0.444122145f , +0.893224301f , 0.449611330f , +0.890448723f , 0.455083587f , +0.887639620f , 0.460538711f , +0.884797098f , 0.465976496f , +0.881921264f , 0.471396737f , +0.879012226f , 0.476799230f , +0.876070094f , 0.482183772f , +0.873094978f , 0.487550160f , +0.870086991f , 0.492898192f , +0.867046246f , 0.498227667f , +0.863972856f , 0.503538384f , +0.860866939f , 0.508830143f , +0.857728610f , 0.514102744f , +0.854557988f , 0.519355990f , +0.851355193f , 0.524589683f , +0.848120345f , 0.529803625f , +0.844853565f , 0.534997620f , +0.841554977f , 0.540171473f , +0.838224706f , 0.545324988f , +0.834862875f , 0.550457973f , +0.831469612f , 0.555570233f , +0.828045045f , 0.560661576f , +0.824589303f , 0.565731811f , +0.821102515f , 0.570780746f , +0.817584813f , 0.575808191f , +0.814036330f , 0.580813958f , +0.810457198f , 0.585797857f , +0.806847554f , 0.590759702f , +0.803207531f , 0.595699304f , +0.799537269f , 0.600616479f , +0.795836905f , 0.605511041f , +0.792106577f , 0.610382806f , +0.788346428f , 0.615231591f , +0.784556597f , 0.620057212f , +0.780737229f , 0.624859488f , +0.776888466f , 0.629638239f , +0.773010453f , 0.634393284f , +0.769103338f , 0.639124445f , +0.765167266f , 0.643831543f , +0.761202385f , 0.648514401f , +0.757208847f , 0.653172843f , +0.753186799f , 0.657806693f , +0.749136395f , 0.662415778f , +0.745057785f , 0.666999922f , +0.740951125f , 0.671558955f , +0.736816569f , 0.676092704f , +0.732654272f , 0.680600998f , +0.728464390f , 0.685083668f , +0.724247083f , 0.689540545f , +0.720002508f , 0.693971461f , +0.715730825f , 0.698376249f , +0.711432196f , 0.702754744f , +0.707106781f , 0.707106781f , +0.702754744f , 0.711432196f , +0.698376249f , 0.715730825f , +0.693971461f , 0.720002508f , +0.689540545f , 0.724247083f , +0.685083668f , 0.728464390f , +0.680600998f , 0.732654272f , +0.676092704f , 0.736816569f , +0.671558955f , 0.740951125f , +0.666999922f , 0.745057785f , +0.662415778f , 0.749136395f , +0.657806693f , 0.753186799f , +0.653172843f , 0.757208847f , +0.648514401f , 0.761202385f , +0.643831543f , 0.765167266f , +0.639124445f , 0.769103338f , +0.634393284f , 0.773010453f , +0.629638239f , 0.776888466f , +0.624859488f , 0.780737229f , +0.620057212f , 0.784556597f , +0.615231591f , 0.788346428f , +0.610382806f , 0.792106577f , +0.605511041f , 0.795836905f , +0.600616479f , 0.799537269f , +0.595699304f , 0.803207531f , +0.590759702f , 0.806847554f , +0.585797857f , 0.810457198f , +0.580813958f , 0.814036330f , +0.575808191f , 0.817584813f , +0.570780746f , 0.821102515f , +0.565731811f , 0.824589303f , +0.560661576f , 0.828045045f , +0.555570233f , 0.831469612f , +0.550457973f , 0.834862875f , +0.545324988f , 0.838224706f , +0.540171473f , 0.841554977f , +0.534997620f , 0.844853565f , +0.529803625f , 0.848120345f , +0.524589683f , 0.851355193f , +0.519355990f , 0.854557988f , +0.514102744f , 0.857728610f , +0.508830143f , 0.860866939f , +0.503538384f , 0.863972856f , +0.498227667f , 0.867046246f , +0.492898192f , 0.870086991f , +0.487550160f , 0.873094978f , +0.482183772f , 0.876070094f , +0.476799230f , 0.879012226f , +0.471396737f , 0.881921264f , +0.465976496f , 0.884797098f , +0.460538711f , 0.887639620f , +0.455083587f , 0.890448723f , +0.449611330f , 0.893224301f , +0.444122145f , 0.895966250f , +0.438616239f , 0.898674466f , +0.433093819f , 0.901348847f , +0.427555093f , 0.903989293f , +0.422000271f , 0.906595705f , +0.416429560f , 0.909167983f , +0.410843171f , 0.911706032f , +0.405241314f , 0.914209756f , +0.399624200f , 0.916679060f , +0.393992040f , 0.919113852f , +0.388345047f , 0.921514039f , +0.382683432f , 0.923879533f , +0.377007410f , 0.926210242f , +0.371317194f , 0.928506080f , +0.365612998f , 0.930766961f , +0.359895037f , 0.932992799f , +0.354163525f , 0.935183510f , +0.348418680f , 0.937339012f , +0.342660717f , 0.939459224f , +0.336889853f , 0.941544065f , +0.331106306f , 0.943593458f , +0.325310292f , 0.945607325f , +0.319502031f , 0.947585591f , +0.313681740f , 0.949528181f , +0.307849640f , 0.951435021f , +0.302005949f , 0.953306040f , +0.296150888f , 0.955141168f , +0.290284677f , 0.956940336f , +0.284407537f , 0.958703475f , +0.278519689f , 0.960430519f , +0.272621355f , 0.962121404f , +0.266712757f , 0.963776066f , +0.260794118f , 0.965394442f , +0.254865660f , 0.966976471f , +0.248927606f , 0.968522094f , +0.242980180f , 0.970031253f , +0.237023606f , 0.971503891f , +0.231058108f , 0.972939952f , +0.225083911f , 0.974339383f , +0.219101240f , 0.975702130f , +0.213110320f , 0.977028143f , +0.207111376f , 0.978317371f , +0.201104635f , 0.979569766f , +0.195090322f , 0.980785280f , +0.189068664f , 0.981963869f , +0.183039888f , 0.983105487f , +0.177004220f , 0.984210092f , +0.170961889f , 0.985277642f , +0.164913120f , 0.986308097f , +0.158858143f , 0.987301418f , +0.152797185f , 0.988257568f , +0.146730474f , 0.989176510f , +0.140658239f , 0.990058210f , +0.134580709f , 0.990902635f , +0.128498111f , 0.991709754f , +0.122410675f , 0.992479535f , +0.116318631f , 0.993211949f , +0.110222207f , 0.993906970f , +0.104121634f , 0.994564571f , +0.098017140f , 0.995184727f , +0.091908956f , 0.995767414f , +0.085797312f , 0.996312612f , +0.079682438f , 0.996820299f , +0.073564564f , 0.997290457f , +0.067443920f , 0.997723067f , +0.061320736f , 0.998118113f , +0.055195244f , 0.998475581f , +0.049067674f , 0.998795456f , +0.042938257f , 0.999077728f , +0.036807223f , 0.999322385f , +0.030674803f , 0.999529418f , +0.024541229f , 0.999698819f , +0.018406730f , 0.999830582f , +0.012271538f , 0.999924702f , +0.006135885f , 0.999981175f , +0.000000000f , 1.000000000f , +-0.006135885f , 0.999981175f , +-0.012271538f , 0.999924702f , +-0.018406730f , 0.999830582f , +-0.024541229f , 0.999698819f , +-0.030674803f , 0.999529418f , +-0.036807223f , 0.999322385f , +-0.042938257f , 0.999077728f , +-0.049067674f , 0.998795456f , +-0.055195244f , 0.998475581f , +-0.061320736f , 0.998118113f , +-0.067443920f , 0.997723067f , +-0.073564564f , 0.997290457f , +-0.079682438f , 0.996820299f , +-0.085797312f , 0.996312612f , +-0.091908956f , 0.995767414f , +-0.098017140f , 0.995184727f , +-0.104121634f , 0.994564571f , +-0.110222207f , 0.993906970f , +-0.116318631f , 0.993211949f , +-0.122410675f , 0.992479535f , +-0.128498111f , 0.991709754f , +-0.134580709f , 0.990902635f , +-0.140658239f , 0.990058210f , +-0.146730474f , 0.989176510f , +-0.152797185f , 0.988257568f , +-0.158858143f , 0.987301418f , +-0.164913120f , 0.986308097f , +-0.170961889f , 0.985277642f , +-0.177004220f , 0.984210092f , +-0.183039888f , 0.983105487f , +-0.189068664f , 0.981963869f , +-0.195090322f , 0.980785280f , +-0.201104635f , 0.979569766f , +-0.207111376f , 0.978317371f , +-0.213110320f , 0.977028143f , +-0.219101240f , 0.975702130f , +-0.225083911f , 0.974339383f , +-0.231058108f , 0.972939952f , +-0.237023606f , 0.971503891f , +-0.242980180f , 0.970031253f , +-0.248927606f , 0.968522094f , +-0.254865660f , 0.966976471f , +-0.260794118f , 0.965394442f , +-0.266712757f , 0.963776066f , +-0.272621355f , 0.962121404f , +-0.278519689f , 0.960430519f , +-0.284407537f , 0.958703475f , +-0.290284677f , 0.956940336f , +-0.296150888f , 0.955141168f , +-0.302005949f , 0.953306040f , +-0.307849640f , 0.951435021f , +-0.313681740f , 0.949528181f , +-0.319502031f , 0.947585591f , +-0.325310292f , 0.945607325f , +-0.331106306f , 0.943593458f , +-0.336889853f , 0.941544065f , +-0.342660717f , 0.939459224f , +-0.348418680f , 0.937339012f , +-0.354163525f , 0.935183510f , +-0.359895037f , 0.932992799f , +-0.365612998f , 0.930766961f , +-0.371317194f , 0.928506080f , +-0.377007410f , 0.926210242f , +-0.382683432f , 0.923879533f , +-0.388345047f , 0.921514039f , +-0.393992040f , 0.919113852f , +-0.399624200f , 0.916679060f , +-0.405241314f , 0.914209756f , +-0.410843171f , 0.911706032f , +-0.416429560f , 0.909167983f , +-0.422000271f , 0.906595705f , +-0.427555093f , 0.903989293f , +-0.433093819f , 0.901348847f , +-0.438616239f , 0.898674466f , +-0.444122145f , 0.895966250f , +-0.449611330f , 0.893224301f , +-0.455083587f , 0.890448723f , +-0.460538711f , 0.887639620f , +-0.465976496f , 0.884797098f , +-0.471396737f , 0.881921264f , +-0.476799230f , 0.879012226f , +-0.482183772f , 0.876070094f , +-0.487550160f , 0.873094978f , +-0.492898192f , 0.870086991f , +-0.498227667f , 0.867046246f , +-0.503538384f , 0.863972856f , +-0.508830143f , 0.860866939f , +-0.514102744f , 0.857728610f , +-0.519355990f , 0.854557988f , +-0.524589683f , 0.851355193f , +-0.529803625f , 0.848120345f , +-0.534997620f , 0.844853565f , +-0.540171473f , 0.841554977f , +-0.545324988f , 0.838224706f , +-0.550457973f , 0.834862875f , +-0.555570233f , 0.831469612f , +-0.560661576f , 0.828045045f , +-0.565731811f , 0.824589303f , +-0.570780746f , 0.821102515f , +-0.575808191f , 0.817584813f , +-0.580813958f , 0.814036330f , +-0.585797857f , 0.810457198f , +-0.590759702f , 0.806847554f , +-0.595699304f , 0.803207531f , +-0.600616479f , 0.799537269f , +-0.605511041f , 0.795836905f , +-0.610382806f , 0.792106577f , +-0.615231591f , 0.788346428f , +-0.620057212f , 0.784556597f , +-0.624859488f , 0.780737229f , +-0.629638239f , 0.776888466f , +-0.634393284f , 0.773010453f , +-0.639124445f , 0.769103338f , +-0.643831543f , 0.765167266f , +-0.648514401f , 0.761202385f , +-0.653172843f , 0.757208847f , +-0.657806693f , 0.753186799f , +-0.662415778f , 0.749136395f , +-0.666999922f , 0.745057785f , +-0.671558955f , 0.740951125f , +-0.676092704f , 0.736816569f , +-0.680600998f , 0.732654272f , +-0.685083668f , 0.728464390f , +-0.689540545f , 0.724247083f , +-0.693971461f , 0.720002508f , +-0.698376249f , 0.715730825f , +-0.702754744f , 0.711432196f , +-0.707106781f , 0.707106781f , +-0.711432196f , 0.702754744f , +-0.715730825f , 0.698376249f , +-0.720002508f , 0.693971461f , +-0.724247083f , 0.689540545f , +-0.728464390f , 0.685083668f , +-0.732654272f , 0.680600998f , +-0.736816569f , 0.676092704f , +-0.740951125f , 0.671558955f , +-0.745057785f , 0.666999922f , +-0.749136395f , 0.662415778f , +-0.753186799f , 0.657806693f , +-0.757208847f , 0.653172843f , +-0.761202385f , 0.648514401f , +-0.765167266f , 0.643831543f , +-0.769103338f , 0.639124445f , +-0.773010453f , 0.634393284f , +-0.776888466f , 0.629638239f , +-0.780737229f , 0.624859488f , +-0.784556597f , 0.620057212f , +-0.788346428f , 0.615231591f , +-0.792106577f , 0.610382806f , +-0.795836905f , 0.605511041f , +-0.799537269f , 0.600616479f , +-0.803207531f , 0.595699304f , +-0.806847554f , 0.590759702f , +-0.810457198f , 0.585797857f , +-0.814036330f , 0.580813958f , +-0.817584813f , 0.575808191f , +-0.821102515f , 0.570780746f , +-0.824589303f , 0.565731811f , +-0.828045045f , 0.560661576f , +-0.831469612f , 0.555570233f , +-0.834862875f , 0.550457973f , +-0.838224706f , 0.545324988f , +-0.841554977f , 0.540171473f , +-0.844853565f , 0.534997620f , +-0.848120345f , 0.529803625f , +-0.851355193f , 0.524589683f , +-0.854557988f , 0.519355990f , +-0.857728610f , 0.514102744f , +-0.860866939f , 0.508830143f , +-0.863972856f , 0.503538384f , +-0.867046246f , 0.498227667f , +-0.870086991f , 0.492898192f , +-0.873094978f , 0.487550160f , +-0.876070094f , 0.482183772f , +-0.879012226f , 0.476799230f , +-0.881921264f , 0.471396737f , +-0.884797098f , 0.465976496f , +-0.887639620f , 0.460538711f , +-0.890448723f , 0.455083587f , +-0.893224301f , 0.449611330f , +-0.895966250f , 0.444122145f , +-0.898674466f , 0.438616239f , +-0.901348847f , 0.433093819f , +-0.903989293f , 0.427555093f , +-0.906595705f , 0.422000271f , +-0.909167983f , 0.416429560f , +-0.911706032f , 0.410843171f , +-0.914209756f , 0.405241314f , +-0.916679060f , 0.399624200f , +-0.919113852f , 0.393992040f , +-0.921514039f , 0.388345047f , +-0.923879533f , 0.382683432f , +-0.926210242f , 0.377007410f , +-0.928506080f , 0.371317194f , +-0.930766961f , 0.365612998f , +-0.932992799f , 0.359895037f , +-0.935183510f , 0.354163525f , +-0.937339012f , 0.348418680f , +-0.939459224f , 0.342660717f , +-0.941544065f , 0.336889853f , +-0.943593458f , 0.331106306f , +-0.945607325f , 0.325310292f , +-0.947585591f , 0.319502031f , +-0.949528181f , 0.313681740f , +-0.951435021f , 0.307849640f , +-0.953306040f , 0.302005949f , +-0.955141168f , 0.296150888f , +-0.956940336f , 0.290284677f , +-0.958703475f , 0.284407537f , +-0.960430519f , 0.278519689f , +-0.962121404f , 0.272621355f , +-0.963776066f , 0.266712757f , +-0.965394442f , 0.260794118f , +-0.966976471f , 0.254865660f , +-0.968522094f , 0.248927606f , +-0.970031253f , 0.242980180f , +-0.971503891f , 0.237023606f , +-0.972939952f , 0.231058108f , +-0.974339383f , 0.225083911f , +-0.975702130f , 0.219101240f , +-0.977028143f , 0.213110320f , +-0.978317371f , 0.207111376f , +-0.979569766f , 0.201104635f , +-0.980785280f , 0.195090322f , +-0.981963869f , 0.189068664f , +-0.983105487f , 0.183039888f , +-0.984210092f , 0.177004220f , +-0.985277642f , 0.170961889f , +-0.986308097f , 0.164913120f , +-0.987301418f , 0.158858143f , +-0.988257568f , 0.152797185f , +-0.989176510f , 0.146730474f , +-0.990058210f , 0.140658239f , +-0.990902635f , 0.134580709f , +-0.991709754f , 0.128498111f , +-0.992479535f , 0.122410675f , +-0.993211949f , 0.116318631f , +-0.993906970f , 0.110222207f , +-0.994564571f , 0.104121634f , +-0.995184727f , 0.098017140f , +-0.995767414f , 0.091908956f , +-0.996312612f , 0.085797312f , +-0.996820299f , 0.079682438f , +-0.997290457f , 0.073564564f , +-0.997723067f , 0.067443920f , +-0.998118113f , 0.061320736f , +-0.998475581f , 0.055195244f , +-0.998795456f , 0.049067674f , +-0.999077728f , 0.042938257f , +-0.999322385f , 0.036807223f , +-0.999529418f , 0.030674803f , +-0.999698819f , 0.024541229f , +-0.999830582f , 0.018406730f , +-0.999924702f , 0.012271538f , +-0.999981175f , 0.006135885f , +-1.000000000f , 0.000000000f , +-0.999981175f , -0.006135885f , +-0.999924702f , -0.012271538f , +-0.999830582f , -0.018406730f , +-0.999698819f , -0.024541229f , +-0.999529418f , -0.030674803f , +-0.999322385f , -0.036807223f , +-0.999077728f , -0.042938257f , +-0.998795456f , -0.049067674f , +-0.998475581f , -0.055195244f , +-0.998118113f , -0.061320736f , +-0.997723067f , -0.067443920f , +-0.997290457f , -0.073564564f , +-0.996820299f , -0.079682438f , +-0.996312612f , -0.085797312f , +-0.995767414f , -0.091908956f , +-0.995184727f , -0.098017140f , +-0.994564571f , -0.104121634f , +-0.993906970f , -0.110222207f , +-0.993211949f , -0.116318631f , +-0.992479535f , -0.122410675f , +-0.991709754f , -0.128498111f , +-0.990902635f , -0.134580709f , +-0.990058210f , -0.140658239f , +-0.989176510f , -0.146730474f , +-0.988257568f , -0.152797185f , +-0.987301418f , -0.158858143f , +-0.986308097f , -0.164913120f , +-0.985277642f , -0.170961889f , +-0.984210092f , -0.177004220f , +-0.983105487f , -0.183039888f , +-0.981963869f , -0.189068664f , +-0.980785280f , -0.195090322f , +-0.979569766f , -0.201104635f , +-0.978317371f , -0.207111376f , +-0.977028143f , -0.213110320f , +-0.975702130f , -0.219101240f , +-0.974339383f , -0.225083911f , +-0.972939952f , -0.231058108f , +-0.971503891f , -0.237023606f , +-0.970031253f , -0.242980180f , +-0.968522094f , -0.248927606f , +-0.966976471f , -0.254865660f , +-0.965394442f , -0.260794118f , +-0.963776066f , -0.266712757f , +-0.962121404f , -0.272621355f , +-0.960430519f , -0.278519689f , +-0.958703475f , -0.284407537f , +-0.956940336f , -0.290284677f , +-0.955141168f , -0.296150888f , +-0.953306040f , -0.302005949f , +-0.951435021f , -0.307849640f , +-0.949528181f , -0.313681740f , +-0.947585591f , -0.319502031f , +-0.945607325f , -0.325310292f , +-0.943593458f , -0.331106306f , +-0.941544065f , -0.336889853f , +-0.939459224f , -0.342660717f , +-0.937339012f , -0.348418680f , +-0.935183510f , -0.354163525f , +-0.932992799f , -0.359895037f , +-0.930766961f , -0.365612998f , +-0.928506080f , -0.371317194f , +-0.926210242f , -0.377007410f , +-0.923879533f , -0.382683432f , +-0.921514039f , -0.388345047f , +-0.919113852f , -0.393992040f , +-0.916679060f , -0.399624200f , +-0.914209756f , -0.405241314f , +-0.911706032f , -0.410843171f , +-0.909167983f , -0.416429560f , +-0.906595705f , -0.422000271f , +-0.903989293f , -0.427555093f , +-0.901348847f , -0.433093819f , +-0.898674466f , -0.438616239f , +-0.895966250f , -0.444122145f , +-0.893224301f , -0.449611330f , +-0.890448723f , -0.455083587f , +-0.887639620f , -0.460538711f , +-0.884797098f , -0.465976496f , +-0.881921264f , -0.471396737f , +-0.879012226f , -0.476799230f , +-0.876070094f , -0.482183772f , +-0.873094978f , -0.487550160f , +-0.870086991f , -0.492898192f , +-0.867046246f , -0.498227667f , +-0.863972856f , -0.503538384f , +-0.860866939f , -0.508830143f , +-0.857728610f , -0.514102744f , +-0.854557988f , -0.519355990f , +-0.851355193f , -0.524589683f , +-0.848120345f , -0.529803625f , +-0.844853565f , -0.534997620f , +-0.841554977f , -0.540171473f , +-0.838224706f , -0.545324988f , +-0.834862875f , -0.550457973f , +-0.831469612f , -0.555570233f , +-0.828045045f , -0.560661576f , +-0.824589303f , -0.565731811f , +-0.821102515f , -0.570780746f , +-0.817584813f , -0.575808191f , +-0.814036330f , -0.580813958f , +-0.810457198f , -0.585797857f , +-0.806847554f , -0.590759702f , +-0.803207531f , -0.595699304f , +-0.799537269f , -0.600616479f , +-0.795836905f , -0.605511041f , +-0.792106577f , -0.610382806f , +-0.788346428f , -0.615231591f , +-0.784556597f , -0.620057212f , +-0.780737229f , -0.624859488f , +-0.776888466f , -0.629638239f , +-0.773010453f , -0.634393284f , +-0.769103338f , -0.639124445f , +-0.765167266f , -0.643831543f , +-0.761202385f , -0.648514401f , +-0.757208847f , -0.653172843f , +-0.753186799f , -0.657806693f , +-0.749136395f , -0.662415778f , +-0.745057785f , -0.666999922f , +-0.740951125f , -0.671558955f , +-0.736816569f , -0.676092704f , +-0.732654272f , -0.680600998f , +-0.728464390f , -0.685083668f , +-0.724247083f , -0.689540545f , +-0.720002508f , -0.693971461f , +-0.715730825f , -0.698376249f , +-0.711432196f , -0.702754744f , +-0.707106781f , -0.707106781f , +-0.702754744f , -0.711432196f , +-0.698376249f , -0.715730825f , +-0.693971461f , -0.720002508f , +-0.689540545f , -0.724247083f , +-0.685083668f , -0.728464390f , +-0.680600998f , -0.732654272f , +-0.676092704f , -0.736816569f , +-0.671558955f , -0.740951125f , +-0.666999922f , -0.745057785f , +-0.662415778f , -0.749136395f , +-0.657806693f , -0.753186799f , +-0.653172843f , -0.757208847f , +-0.648514401f , -0.761202385f , +-0.643831543f , -0.765167266f , +-0.639124445f , -0.769103338f , +-0.634393284f , -0.773010453f , +-0.629638239f , -0.776888466f , +-0.624859488f , -0.780737229f , +-0.620057212f , -0.784556597f , +-0.615231591f , -0.788346428f , +-0.610382806f , -0.792106577f , +-0.605511041f , -0.795836905f , +-0.600616479f , -0.799537269f , +-0.595699304f , -0.803207531f , +-0.590759702f , -0.806847554f , +-0.585797857f , -0.810457198f , +-0.580813958f , -0.814036330f , +-0.575808191f , -0.817584813f , +-0.570780746f , -0.821102515f , +-0.565731811f , -0.824589303f , +-0.560661576f , -0.828045045f , +-0.555570233f , -0.831469612f , +-0.550457973f , -0.834862875f , +-0.545324988f , -0.838224706f , +-0.540171473f , -0.841554977f , +-0.534997620f , -0.844853565f , +-0.529803625f , -0.848120345f , +-0.524589683f , -0.851355193f , +-0.519355990f , -0.854557988f , +-0.514102744f , -0.857728610f , +-0.508830143f , -0.860866939f , +-0.503538384f , -0.863972856f , +-0.498227667f , -0.867046246f , +-0.492898192f , -0.870086991f , +-0.487550160f , -0.873094978f , +-0.482183772f , -0.876070094f , +-0.476799230f , -0.879012226f , +-0.471396737f , -0.881921264f , +-0.465976496f , -0.884797098f , +-0.460538711f , -0.887639620f , +-0.455083587f , -0.890448723f , +-0.449611330f , -0.893224301f , +-0.444122145f , -0.895966250f , +-0.438616239f , -0.898674466f , +-0.433093819f , -0.901348847f , +-0.427555093f , -0.903989293f , +-0.422000271f , -0.906595705f , +-0.416429560f , -0.909167983f , +-0.410843171f , -0.911706032f , +-0.405241314f , -0.914209756f , +-0.399624200f , -0.916679060f , +-0.393992040f , -0.919113852f , +-0.388345047f , -0.921514039f , +-0.382683432f , -0.923879533f , +-0.377007410f , -0.926210242f , +-0.371317194f , -0.928506080f , +-0.365612998f , -0.930766961f , +-0.359895037f , -0.932992799f , +-0.354163525f , -0.935183510f , +-0.348418680f , -0.937339012f , +-0.342660717f , -0.939459224f , +-0.336889853f , -0.941544065f , +-0.331106306f , -0.943593458f , +-0.325310292f , -0.945607325f , +-0.319502031f , -0.947585591f , +-0.313681740f , -0.949528181f , +-0.307849640f , -0.951435021f , +-0.302005949f , -0.953306040f , +-0.296150888f , -0.955141168f , +-0.290284677f , -0.956940336f , +-0.284407537f , -0.958703475f , +-0.278519689f , -0.960430519f , +-0.272621355f , -0.962121404f , +-0.266712757f , -0.963776066f , +-0.260794118f , -0.965394442f , +-0.254865660f , -0.966976471f , +-0.248927606f , -0.968522094f , +-0.242980180f , -0.970031253f , +-0.237023606f , -0.971503891f , +-0.231058108f , -0.972939952f , +-0.225083911f , -0.974339383f , +-0.219101240f , -0.975702130f , +-0.213110320f , -0.977028143f , +-0.207111376f , -0.978317371f , +-0.201104635f , -0.979569766f , +-0.195090322f , -0.980785280f , +-0.189068664f , -0.981963869f , +-0.183039888f , -0.983105487f , +-0.177004220f , -0.984210092f , +-0.170961889f , -0.985277642f , +-0.164913120f , -0.986308097f , +-0.158858143f , -0.987301418f , +-0.152797185f , -0.988257568f , +-0.146730474f , -0.989176510f , +-0.140658239f , -0.990058210f , +-0.134580709f , -0.990902635f , +-0.128498111f , -0.991709754f , +-0.122410675f , -0.992479535f , +-0.116318631f , -0.993211949f , +-0.110222207f , -0.993906970f , +-0.104121634f , -0.994564571f , +-0.098017140f , -0.995184727f , +-0.091908956f , -0.995767414f , +-0.085797312f , -0.996312612f , +-0.079682438f , -0.996820299f , +-0.073564564f , -0.997290457f , +-0.067443920f , -0.997723067f , +-0.061320736f , -0.998118113f , +-0.055195244f , -0.998475581f , +-0.049067674f , -0.998795456f , +-0.042938257f , -0.999077728f , +-0.036807223f , -0.999322385f , +-0.030674803f , -0.999529418f , +-0.024541229f , -0.999698819f , +-0.018406730f , -0.999830582f , +-0.012271538f , -0.999924702f , +-0.006135885f , -0.999981175f , +-0.000000000f , -1.000000000f , +0.006135885f , -0.999981175f , +0.012271538f , -0.999924702f , +0.018406730f , -0.999830582f , +0.024541229f , -0.999698819f , +0.030674803f , -0.999529418f , +0.036807223f , -0.999322385f , +0.042938257f , -0.999077728f , +0.049067674f , -0.998795456f , +0.055195244f , -0.998475581f , +0.061320736f , -0.998118113f , +0.067443920f , -0.997723067f , +0.073564564f , -0.997290457f , +0.079682438f , -0.996820299f , +0.085797312f , -0.996312612f , +0.091908956f , -0.995767414f , +0.098017140f , -0.995184727f , +0.104121634f , -0.994564571f , +0.110222207f , -0.993906970f , +0.116318631f , -0.993211949f , +0.122410675f , -0.992479535f , +0.128498111f , -0.991709754f , +0.134580709f , -0.990902635f , +0.140658239f , -0.990058210f , +0.146730474f , -0.989176510f , +0.152797185f , -0.988257568f , +0.158858143f , -0.987301418f , +0.164913120f , -0.986308097f , +0.170961889f , -0.985277642f , +0.177004220f , -0.984210092f , +0.183039888f , -0.983105487f , +0.189068664f , -0.981963869f , +0.195090322f , -0.980785280f , +0.201104635f , -0.979569766f , +0.207111376f , -0.978317371f , +0.213110320f , -0.977028143f , +0.219101240f , -0.975702130f , +0.225083911f , -0.974339383f , +0.231058108f , -0.972939952f , +0.237023606f , -0.971503891f , +0.242980180f , -0.970031253f , +0.248927606f , -0.968522094f , +0.254865660f , -0.966976471f , +0.260794118f , -0.965394442f , +0.266712757f , -0.963776066f , +0.272621355f , -0.962121404f , +0.278519689f , -0.960430519f , +0.284407537f , -0.958703475f , +0.290284677f , -0.956940336f , +0.296150888f , -0.955141168f , +0.302005949f , -0.953306040f , +0.307849640f , -0.951435021f , +0.313681740f , -0.949528181f , +0.319502031f , -0.947585591f , +0.325310292f , -0.945607325f , +0.331106306f , -0.943593458f , +0.336889853f , -0.941544065f , +0.342660717f , -0.939459224f , +0.348418680f , -0.937339012f , +0.354163525f , -0.935183510f , +0.359895037f , -0.932992799f , +0.365612998f , -0.930766961f , +0.371317194f , -0.928506080f , +0.377007410f , -0.926210242f , +0.382683432f , -0.923879533f , +0.388345047f , -0.921514039f , +0.393992040f , -0.919113852f , +0.399624200f , -0.916679060f , +0.405241314f , -0.914209756f , +0.410843171f , -0.911706032f , +0.416429560f , -0.909167983f , +0.422000271f , -0.906595705f , +0.427555093f , -0.903989293f , +0.433093819f , -0.901348847f , +0.438616239f , -0.898674466f , +0.444122145f , -0.895966250f , +0.449611330f , -0.893224301f , +0.455083587f , -0.890448723f , +0.460538711f , -0.887639620f , +0.465976496f , -0.884797098f , +0.471396737f , -0.881921264f , +0.476799230f , -0.879012226f , +0.482183772f , -0.876070094f , +0.487550160f , -0.873094978f , +0.492898192f , -0.870086991f , +0.498227667f , -0.867046246f , +0.503538384f , -0.863972856f , +0.508830143f , -0.860866939f , +0.514102744f , -0.857728610f , +0.519355990f , -0.854557988f , +0.524589683f , -0.851355193f , +0.529803625f , -0.848120345f , +0.534997620f , -0.844853565f , +0.540171473f , -0.841554977f , +0.545324988f , -0.838224706f , +0.550457973f , -0.834862875f , +0.555570233f , -0.831469612f , +0.560661576f , -0.828045045f , +0.565731811f , -0.824589303f , +0.570780746f , -0.821102515f , +0.575808191f , -0.817584813f , +0.580813958f , -0.814036330f , +0.585797857f , -0.810457198f , +0.590759702f , -0.806847554f , +0.595699304f , -0.803207531f , +0.600616479f , -0.799537269f , +0.605511041f , -0.795836905f , +0.610382806f , -0.792106577f , +0.615231591f , -0.788346428f , +0.620057212f , -0.784556597f , +0.624859488f , -0.780737229f , +0.629638239f , -0.776888466f , +0.634393284f , -0.773010453f , +0.639124445f , -0.769103338f , +0.643831543f , -0.765167266f , +0.648514401f , -0.761202385f , +0.653172843f , -0.757208847f , +0.657806693f , -0.753186799f , +0.662415778f , -0.749136395f , +0.666999922f , -0.745057785f , +0.671558955f , -0.740951125f , +0.676092704f , -0.736816569f , +0.680600998f , -0.732654272f , +0.685083668f , -0.728464390f , +0.689540545f , -0.724247083f , +0.693971461f , -0.720002508f , +0.698376249f , -0.715730825f , +0.702754744f , -0.711432196f , +0.707106781f , -0.707106781f , +0.711432196f , -0.702754744f , +0.715730825f , -0.698376249f , +0.720002508f , -0.693971461f , +0.724247083f , -0.689540545f , +0.728464390f , -0.685083668f , +0.732654272f , -0.680600998f , +0.736816569f , -0.676092704f , +0.740951125f , -0.671558955f , +0.745057785f , -0.666999922f , +0.749136395f , -0.662415778f , +0.753186799f , -0.657806693f , +0.757208847f , -0.653172843f , +0.761202385f , -0.648514401f , +0.765167266f , -0.643831543f , +0.769103338f , -0.639124445f , +0.773010453f , -0.634393284f , +0.776888466f , -0.629638239f , +0.780737229f , -0.624859488f , +0.784556597f , -0.620057212f , +0.788346428f , -0.615231591f , +0.792106577f , -0.610382806f , +0.795836905f , -0.605511041f , +0.799537269f , -0.600616479f , +0.803207531f , -0.595699304f , +0.806847554f , -0.590759702f , +0.810457198f , -0.585797857f , +0.814036330f , -0.580813958f , +0.817584813f , -0.575808191f , +0.821102515f , -0.570780746f , +0.824589303f , -0.565731811f , +0.828045045f , -0.560661576f , +0.831469612f , -0.555570233f , +0.834862875f , -0.550457973f , +0.838224706f , -0.545324988f , +0.841554977f , -0.540171473f , +0.844853565f , -0.534997620f , +0.848120345f , -0.529803625f , +0.851355193f , -0.524589683f , +0.854557988f , -0.519355990f , +0.857728610f , -0.514102744f , +0.860866939f , -0.508830143f , +0.863972856f , -0.503538384f , +0.867046246f , -0.498227667f , +0.870086991f , -0.492898192f , +0.873094978f , -0.487550160f , +0.876070094f , -0.482183772f , +0.879012226f , -0.476799230f , +0.881921264f , -0.471396737f , +0.884797098f , -0.465976496f , +0.887639620f , -0.460538711f , +0.890448723f , -0.455083587f , +0.893224301f , -0.449611330f , +0.895966250f , -0.444122145f , +0.898674466f , -0.438616239f , +0.901348847f , -0.433093819f , +0.903989293f , -0.427555093f , +0.906595705f , -0.422000271f , +0.909167983f , -0.416429560f , +0.911706032f , -0.410843171f , +0.914209756f , -0.405241314f , +0.916679060f , -0.399624200f , +0.919113852f , -0.393992040f , +0.921514039f , -0.388345047f , +0.923879533f , -0.382683432f , +0.926210242f , -0.377007410f , +0.928506080f , -0.371317194f , +0.930766961f , -0.365612998f , +0.932992799f , -0.359895037f , +0.935183510f , -0.354163525f , +0.937339012f , -0.348418680f , +0.939459224f , -0.342660717f , +0.941544065f , -0.336889853f , +0.943593458f , -0.331106306f , +0.945607325f , -0.325310292f , +0.947585591f , -0.319502031f , +0.949528181f , -0.313681740f , +0.951435021f , -0.307849640f , +0.953306040f , -0.302005949f , +0.955141168f , -0.296150888f , +0.956940336f , -0.290284677f , +0.958703475f , -0.284407537f , +0.960430519f , -0.278519689f , +0.962121404f , -0.272621355f , +0.963776066f , -0.266712757f , +0.965394442f , -0.260794118f , +0.966976471f , -0.254865660f , +0.968522094f , -0.248927606f , +0.970031253f , -0.242980180f , +0.971503891f , -0.237023606f , +0.972939952f , -0.231058108f , +0.974339383f , -0.225083911f , +0.975702130f , -0.219101240f , +0.977028143f , -0.213110320f , +0.978317371f , -0.207111376f , +0.979569766f , -0.201104635f , +0.980785280f , -0.195090322f , +0.981963869f , -0.189068664f , +0.983105487f , -0.183039888f , +0.984210092f , -0.177004220f , +0.985277642f , -0.170961889f , +0.986308097f , -0.164913120f , +0.987301418f , -0.158858143f , +0.988257568f , -0.152797185f , +0.989176510f , -0.146730474f , +0.990058210f , -0.140658239f , +0.990902635f , -0.134580709f , +0.991709754f , -0.128498111f , +0.992479535f , -0.122410675f , +0.993211949f , -0.116318631f , +0.993906970f , -0.110222207f , +0.994564571f , -0.104121634f , +0.995184727f , -0.098017140f , +0.995767414f , -0.091908956f , +0.996312612f , -0.085797312f , +0.996820299f , -0.079682438f , +0.997290457f , -0.073564564f , +0.997723067f , -0.067443920f , +0.998118113f , -0.061320736f , +0.998475581f , -0.055195244f , +0.998795456f , -0.049067674f , +0.999077728f , -0.042938257f , +0.999322385f , -0.036807223f , +0.999529418f , -0.030674803f , +0.999698819f , -0.024541229f , +0.999830582f , -0.018406730f , +0.999924702f , -0.012271538f , +0.999981175f , -0.006135885f +}; + +/** +* \par +* Example code for Floating-point Twiddle factors Generation: +* \par +*
for(i = 0; i< N/; i++)    
+* {    
+*	twiddleCoef[2*i]= cos(i * 2*PI/(float)N);    
+*	twiddleCoef[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 2048 and PI = 3.14159265358979 +* \par +* Cos and Sin values are in interleaved fashion +* +*/ +const float32_t twiddleCoef_2048[4096] = { + 1.000000000f, 0.000000000f, + 0.999995294f, 0.003067957f, + 0.999981175f, 0.006135885f, + 0.999957645f, 0.009203755f, + 0.999924702f, 0.012271538f, + 0.999882347f, 0.015339206f, + 0.999830582f, 0.018406730f, + 0.999769405f, 0.021474080f, + 0.999698819f, 0.024541229f, + 0.999618822f, 0.027608146f, + 0.999529418f, 0.030674803f, + 0.999430605f, 0.033741172f, + 0.999322385f, 0.036807223f, + 0.999204759f, 0.039872928f, + 0.999077728f, 0.042938257f, + 0.998941293f, 0.046003182f, + 0.998795456f, 0.049067674f, + 0.998640218f, 0.052131705f, + 0.998475581f, 0.055195244f, + 0.998301545f, 0.058258265f, + 0.998118113f, 0.061320736f, + 0.997925286f, 0.064382631f, + 0.997723067f, 0.067443920f, + 0.997511456f, 0.070504573f, + 0.997290457f, 0.073564564f, + 0.997060070f, 0.076623861f, + 0.996820299f, 0.079682438f, + 0.996571146f, 0.082740265f, + 0.996312612f, 0.085797312f, + 0.996044701f, 0.088853553f, + 0.995767414f, 0.091908956f, + 0.995480755f, 0.094963495f, + 0.995184727f, 0.098017140f, + 0.994879331f, 0.101069863f, + 0.994564571f, 0.104121634f, + 0.994240449f, 0.107172425f, + 0.993906970f, 0.110222207f, + 0.993564136f, 0.113270952f, + 0.993211949f, 0.116318631f, + 0.992850414f, 0.119365215f, + 0.992479535f, 0.122410675f, + 0.992099313f, 0.125454983f, + 0.991709754f, 0.128498111f, + 0.991310860f, 0.131540029f, + 0.990902635f, 0.134580709f, + 0.990485084f, 0.137620122f, + 0.990058210f, 0.140658239f, + 0.989622017f, 0.143695033f, + 0.989176510f, 0.146730474f, + 0.988721692f, 0.149764535f, + 0.988257568f, 0.152797185f, + 0.987784142f, 0.155828398f, + 0.987301418f, 0.158858143f, + 0.986809402f, 0.161886394f, + 0.986308097f, 0.164913120f, + 0.985797509f, 0.167938295f, + 0.985277642f, 0.170961889f, + 0.984748502f, 0.173983873f, + 0.984210092f, 0.177004220f, + 0.983662419f, 0.180022901f, + 0.983105487f, 0.183039888f, + 0.982539302f, 0.186055152f, + 0.981963869f, 0.189068664f, + 0.981379193f, 0.192080397f, + 0.980785280f, 0.195090322f, + 0.980182136f, 0.198098411f, + 0.979569766f, 0.201104635f, + 0.978948175f, 0.204108966f, + 0.978317371f, 0.207111376f, + 0.977677358f, 0.210111837f, + 0.977028143f, 0.213110320f, + 0.976369731f, 0.216106797f, + 0.975702130f, 0.219101240f, + 0.975025345f, 0.222093621f, + 0.974339383f, 0.225083911f, + 0.973644250f, 0.228072083f, + 0.972939952f, 0.231058108f, + 0.972226497f, 0.234041959f, + 0.971503891f, 0.237023606f, + 0.970772141f, 0.240003022f, + 0.970031253f, 0.242980180f, + 0.969281235f, 0.245955050f, + 0.968522094f, 0.248927606f, + 0.967753837f, 0.251897818f, + 0.966976471f, 0.254865660f, + 0.966190003f, 0.257831102f, + 0.965394442f, 0.260794118f, + 0.964589793f, 0.263754679f, + 0.963776066f, 0.266712757f, + 0.962953267f, 0.269668326f, + 0.962121404f, 0.272621355f, + 0.961280486f, 0.275571819f, + 0.960430519f, 0.278519689f, + 0.959571513f, 0.281464938f, + 0.958703475f, 0.284407537f, + 0.957826413f, 0.287347460f, + 0.956940336f, 0.290284677f, + 0.956045251f, 0.293219163f, + 0.955141168f, 0.296150888f, + 0.954228095f, 0.299079826f, + 0.953306040f, 0.302005949f, + 0.952375013f, 0.304929230f, + 0.951435021f, 0.307849640f, + 0.950486074f, 0.310767153f, + 0.949528181f, 0.313681740f, + 0.948561350f, 0.316593376f, + 0.947585591f, 0.319502031f, + 0.946600913f, 0.322407679f, + 0.945607325f, 0.325310292f, + 0.944604837f, 0.328209844f, + 0.943593458f, 0.331106306f, + 0.942573198f, 0.333999651f, + 0.941544065f, 0.336889853f, + 0.940506071f, 0.339776884f, + 0.939459224f, 0.342660717f, + 0.938403534f, 0.345541325f, + 0.937339012f, 0.348418680f, + 0.936265667f, 0.351292756f, + 0.935183510f, 0.354163525f, + 0.934092550f, 0.357030961f, + 0.932992799f, 0.359895037f, + 0.931884266f, 0.362755724f, + 0.930766961f, 0.365612998f, + 0.929640896f, 0.368466830f, + 0.928506080f, 0.371317194f, + 0.927362526f, 0.374164063f, + 0.926210242f, 0.377007410f, + 0.925049241f, 0.379847209f, + 0.923879533f, 0.382683432f, + 0.922701128f, 0.385516054f, + 0.921514039f, 0.388345047f, + 0.920318277f, 0.391170384f, + 0.919113852f, 0.393992040f, + 0.917900776f, 0.396809987f, + 0.916679060f, 0.399624200f, + 0.915448716f, 0.402434651f, + 0.914209756f, 0.405241314f, + 0.912962190f, 0.408044163f, + 0.911706032f, 0.410843171f, + 0.910441292f, 0.413638312f, + 0.909167983f, 0.416429560f, + 0.907886116f, 0.419216888f, + 0.906595705f, 0.422000271f, + 0.905296759f, 0.424779681f, + 0.903989293f, 0.427555093f, + 0.902673318f, 0.430326481f, + 0.901348847f, 0.433093819f, + 0.900015892f, 0.435857080f, + 0.898674466f, 0.438616239f, + 0.897324581f, 0.441371269f, + 0.895966250f, 0.444122145f, + 0.894599486f, 0.446868840f, + 0.893224301f, 0.449611330f, + 0.891840709f, 0.452349587f, + 0.890448723f, 0.455083587f, + 0.889048356f, 0.457813304f, + 0.887639620f, 0.460538711f, + 0.886222530f, 0.463259784f, + 0.884797098f, 0.465976496f, + 0.883363339f, 0.468688822f, + 0.881921264f, 0.471396737f, + 0.880470889f, 0.474100215f, + 0.879012226f, 0.476799230f, + 0.877545290f, 0.479493758f, + 0.876070094f, 0.482183772f, + 0.874586652f, 0.484869248f, + 0.873094978f, 0.487550160f, + 0.871595087f, 0.490226483f, + 0.870086991f, 0.492898192f, + 0.868570706f, 0.495565262f, + 0.867046246f, 0.498227667f, + 0.865513624f, 0.500885383f, + 0.863972856f, 0.503538384f, + 0.862423956f, 0.506186645f, + 0.860866939f, 0.508830143f, + 0.859301818f, 0.511468850f, + 0.857728610f, 0.514102744f, + 0.856147328f, 0.516731799f, + 0.854557988f, 0.519355990f, + 0.852960605f, 0.521975293f, + 0.851355193f, 0.524589683f, + 0.849741768f, 0.527199135f, + 0.848120345f, 0.529803625f, + 0.846490939f, 0.532403128f, + 0.844853565f, 0.534997620f, + 0.843208240f, 0.537587076f, + 0.841554977f, 0.540171473f, + 0.839893794f, 0.542750785f, + 0.838224706f, 0.545324988f, + 0.836547727f, 0.547894059f, + 0.834862875f, 0.550457973f, + 0.833170165f, 0.553016706f, + 0.831469612f, 0.555570233f, + 0.829761234f, 0.558118531f, + 0.828045045f, 0.560661576f, + 0.826321063f, 0.563199344f, + 0.824589303f, 0.565731811f, + 0.822849781f, 0.568258953f, + 0.821102515f, 0.570780746f, + 0.819347520f, 0.573297167f, + 0.817584813f, 0.575808191f, + 0.815814411f, 0.578313796f, + 0.814036330f, 0.580813958f, + 0.812250587f, 0.583308653f, + 0.810457198f, 0.585797857f, + 0.808656182f, 0.588281548f, + 0.806847554f, 0.590759702f, + 0.805031331f, 0.593232295f, + 0.803207531f, 0.595699304f, + 0.801376172f, 0.598160707f, + 0.799537269f, 0.600616479f, + 0.797690841f, 0.603066599f, + 0.795836905f, 0.605511041f, + 0.793975478f, 0.607949785f, + 0.792106577f, 0.610382806f, + 0.790230221f, 0.612810082f, + 0.788346428f, 0.615231591f, + 0.786455214f, 0.617647308f, + 0.784556597f, 0.620057212f, + 0.782650596f, 0.622461279f, + 0.780737229f, 0.624859488f, + 0.778816512f, 0.627251815f, + 0.776888466f, 0.629638239f, + 0.774953107f, 0.632018736f, + 0.773010453f, 0.634393284f, + 0.771060524f, 0.636761861f, + 0.769103338f, 0.639124445f, + 0.767138912f, 0.641481013f, + 0.765167266f, 0.643831543f, + 0.763188417f, 0.646176013f, + 0.761202385f, 0.648514401f, + 0.759209189f, 0.650846685f, + 0.757208847f, 0.653172843f, + 0.755201377f, 0.655492853f, + 0.753186799f, 0.657806693f, + 0.751165132f, 0.660114342f, + 0.749136395f, 0.662415778f, + 0.747100606f, 0.664710978f, + 0.745057785f, 0.666999922f, + 0.743007952f, 0.669282588f, + 0.740951125f, 0.671558955f, + 0.738887324f, 0.673829000f, + 0.736816569f, 0.676092704f, + 0.734738878f, 0.678350043f, + 0.732654272f, 0.680600998f, + 0.730562769f, 0.682845546f, + 0.728464390f, 0.685083668f, + 0.726359155f, 0.687315341f, + 0.724247083f, 0.689540545f, + 0.722128194f, 0.691759258f, + 0.720002508f, 0.693971461f, + 0.717870045f, 0.696177131f, + 0.715730825f, 0.698376249f, + 0.713584869f, 0.700568794f, + 0.711432196f, 0.702754744f, + 0.709272826f, 0.704934080f, + 0.707106781f, 0.707106781f, + 0.704934080f, 0.709272826f, + 0.702754744f, 0.711432196f, + 0.700568794f, 0.713584869f, + 0.698376249f, 0.715730825f, + 0.696177131f, 0.717870045f, + 0.693971461f, 0.720002508f, + 0.691759258f, 0.722128194f, + 0.689540545f, 0.724247083f, + 0.687315341f, 0.726359155f, + 0.685083668f, 0.728464390f, + 0.682845546f, 0.730562769f, + 0.680600998f, 0.732654272f, + 0.678350043f, 0.734738878f, + 0.676092704f, 0.736816569f, + 0.673829000f, 0.738887324f, + 0.671558955f, 0.740951125f, + 0.669282588f, 0.743007952f, + 0.666999922f, 0.745057785f, + 0.664710978f, 0.747100606f, + 0.662415778f, 0.749136395f, + 0.660114342f, 0.751165132f, + 0.657806693f, 0.753186799f, + 0.655492853f, 0.755201377f, + 0.653172843f, 0.757208847f, + 0.650846685f, 0.759209189f, + 0.648514401f, 0.761202385f, + 0.646176013f, 0.763188417f, + 0.643831543f, 0.765167266f, + 0.641481013f, 0.767138912f, + 0.639124445f, 0.769103338f, + 0.636761861f, 0.771060524f, + 0.634393284f, 0.773010453f, + 0.632018736f, 0.774953107f, + 0.629638239f, 0.776888466f, + 0.627251815f, 0.778816512f, + 0.624859488f, 0.780737229f, + 0.622461279f, 0.782650596f, + 0.620057212f, 0.784556597f, + 0.617647308f, 0.786455214f, + 0.615231591f, 0.788346428f, + 0.612810082f, 0.790230221f, + 0.610382806f, 0.792106577f, + 0.607949785f, 0.793975478f, + 0.605511041f, 0.795836905f, + 0.603066599f, 0.797690841f, + 0.600616479f, 0.799537269f, + 0.598160707f, 0.801376172f, + 0.595699304f, 0.803207531f, + 0.593232295f, 0.805031331f, + 0.590759702f, 0.806847554f, + 0.588281548f, 0.808656182f, + 0.585797857f, 0.810457198f, + 0.583308653f, 0.812250587f, + 0.580813958f, 0.814036330f, + 0.578313796f, 0.815814411f, + 0.575808191f, 0.817584813f, + 0.573297167f, 0.819347520f, + 0.570780746f, 0.821102515f, + 0.568258953f, 0.822849781f, + 0.565731811f, 0.824589303f, + 0.563199344f, 0.826321063f, + 0.560661576f, 0.828045045f, + 0.558118531f, 0.829761234f, + 0.555570233f, 0.831469612f, + 0.553016706f, 0.833170165f, + 0.550457973f, 0.834862875f, + 0.547894059f, 0.836547727f, + 0.545324988f, 0.838224706f, + 0.542750785f, 0.839893794f, + 0.540171473f, 0.841554977f, + 0.537587076f, 0.843208240f, + 0.534997620f, 0.844853565f, + 0.532403128f, 0.846490939f, + 0.529803625f, 0.848120345f, + 0.527199135f, 0.849741768f, + 0.524589683f, 0.851355193f, + 0.521975293f, 0.852960605f, + 0.519355990f, 0.854557988f, + 0.516731799f, 0.856147328f, + 0.514102744f, 0.857728610f, + 0.511468850f, 0.859301818f, + 0.508830143f, 0.860866939f, + 0.506186645f, 0.862423956f, + 0.503538384f, 0.863972856f, + 0.500885383f, 0.865513624f, + 0.498227667f, 0.867046246f, + 0.495565262f, 0.868570706f, + 0.492898192f, 0.870086991f, + 0.490226483f, 0.871595087f, + 0.487550160f, 0.873094978f, + 0.484869248f, 0.874586652f, + 0.482183772f, 0.876070094f, + 0.479493758f, 0.877545290f, + 0.476799230f, 0.879012226f, + 0.474100215f, 0.880470889f, + 0.471396737f, 0.881921264f, + 0.468688822f, 0.883363339f, + 0.465976496f, 0.884797098f, + 0.463259784f, 0.886222530f, + 0.460538711f, 0.887639620f, + 0.457813304f, 0.889048356f, + 0.455083587f, 0.890448723f, + 0.452349587f, 0.891840709f, + 0.449611330f, 0.893224301f, + 0.446868840f, 0.894599486f, + 0.444122145f, 0.895966250f, + 0.441371269f, 0.897324581f, + 0.438616239f, 0.898674466f, + 0.435857080f, 0.900015892f, + 0.433093819f, 0.901348847f, + 0.430326481f, 0.902673318f, + 0.427555093f, 0.903989293f, + 0.424779681f, 0.905296759f, + 0.422000271f, 0.906595705f, + 0.419216888f, 0.907886116f, + 0.416429560f, 0.909167983f, + 0.413638312f, 0.910441292f, + 0.410843171f, 0.911706032f, + 0.408044163f, 0.912962190f, + 0.405241314f, 0.914209756f, + 0.402434651f, 0.915448716f, + 0.399624200f, 0.916679060f, + 0.396809987f, 0.917900776f, + 0.393992040f, 0.919113852f, + 0.391170384f, 0.920318277f, + 0.388345047f, 0.921514039f, + 0.385516054f, 0.922701128f, + 0.382683432f, 0.923879533f, + 0.379847209f, 0.925049241f, + 0.377007410f, 0.926210242f, + 0.374164063f, 0.927362526f, + 0.371317194f, 0.928506080f, + 0.368466830f, 0.929640896f, + 0.365612998f, 0.930766961f, + 0.362755724f, 0.931884266f, + 0.359895037f, 0.932992799f, + 0.357030961f, 0.934092550f, + 0.354163525f, 0.935183510f, + 0.351292756f, 0.936265667f, + 0.348418680f, 0.937339012f, + 0.345541325f, 0.938403534f, + 0.342660717f, 0.939459224f, + 0.339776884f, 0.940506071f, + 0.336889853f, 0.941544065f, + 0.333999651f, 0.942573198f, + 0.331106306f, 0.943593458f, + 0.328209844f, 0.944604837f, + 0.325310292f, 0.945607325f, + 0.322407679f, 0.946600913f, + 0.319502031f, 0.947585591f, + 0.316593376f, 0.948561350f, + 0.313681740f, 0.949528181f, + 0.310767153f, 0.950486074f, + 0.307849640f, 0.951435021f, + 0.304929230f, 0.952375013f, + 0.302005949f, 0.953306040f, + 0.299079826f, 0.954228095f, + 0.296150888f, 0.955141168f, + 0.293219163f, 0.956045251f, + 0.290284677f, 0.956940336f, + 0.287347460f, 0.957826413f, + 0.284407537f, 0.958703475f, + 0.281464938f, 0.959571513f, + 0.278519689f, 0.960430519f, + 0.275571819f, 0.961280486f, + 0.272621355f, 0.962121404f, + 0.269668326f, 0.962953267f, + 0.266712757f, 0.963776066f, + 0.263754679f, 0.964589793f, + 0.260794118f, 0.965394442f, + 0.257831102f, 0.966190003f, + 0.254865660f, 0.966976471f, + 0.251897818f, 0.967753837f, + 0.248927606f, 0.968522094f, + 0.245955050f, 0.969281235f, + 0.242980180f, 0.970031253f, + 0.240003022f, 0.970772141f, + 0.237023606f, 0.971503891f, + 0.234041959f, 0.972226497f, + 0.231058108f, 0.972939952f, + 0.228072083f, 0.973644250f, + 0.225083911f, 0.974339383f, + 0.222093621f, 0.975025345f, + 0.219101240f, 0.975702130f, + 0.216106797f, 0.976369731f, + 0.213110320f, 0.977028143f, + 0.210111837f, 0.977677358f, + 0.207111376f, 0.978317371f, + 0.204108966f, 0.978948175f, + 0.201104635f, 0.979569766f, + 0.198098411f, 0.980182136f, + 0.195090322f, 0.980785280f, + 0.192080397f, 0.981379193f, + 0.189068664f, 0.981963869f, + 0.186055152f, 0.982539302f, + 0.183039888f, 0.983105487f, + 0.180022901f, 0.983662419f, + 0.177004220f, 0.984210092f, + 0.173983873f, 0.984748502f, + 0.170961889f, 0.985277642f, + 0.167938295f, 0.985797509f, + 0.164913120f, 0.986308097f, + 0.161886394f, 0.986809402f, + 0.158858143f, 0.987301418f, + 0.155828398f, 0.987784142f, + 0.152797185f, 0.988257568f, + 0.149764535f, 0.988721692f, + 0.146730474f, 0.989176510f, + 0.143695033f, 0.989622017f, + 0.140658239f, 0.990058210f, + 0.137620122f, 0.990485084f, + 0.134580709f, 0.990902635f, + 0.131540029f, 0.991310860f, + 0.128498111f, 0.991709754f, + 0.125454983f, 0.992099313f, + 0.122410675f, 0.992479535f, + 0.119365215f, 0.992850414f, + 0.116318631f, 0.993211949f, + 0.113270952f, 0.993564136f, + 0.110222207f, 0.993906970f, + 0.107172425f, 0.994240449f, + 0.104121634f, 0.994564571f, + 0.101069863f, 0.994879331f, + 0.098017140f, 0.995184727f, + 0.094963495f, 0.995480755f, + 0.091908956f, 0.995767414f, + 0.088853553f, 0.996044701f, + 0.085797312f, 0.996312612f, + 0.082740265f, 0.996571146f, + 0.079682438f, 0.996820299f, + 0.076623861f, 0.997060070f, + 0.073564564f, 0.997290457f, + 0.070504573f, 0.997511456f, + 0.067443920f, 0.997723067f, + 0.064382631f, 0.997925286f, + 0.061320736f, 0.998118113f, + 0.058258265f, 0.998301545f, + 0.055195244f, 0.998475581f, + 0.052131705f, 0.998640218f, + 0.049067674f, 0.998795456f, + 0.046003182f, 0.998941293f, + 0.042938257f, 0.999077728f, + 0.039872928f, 0.999204759f, + 0.036807223f, 0.999322385f, + 0.033741172f, 0.999430605f, + 0.030674803f, 0.999529418f, + 0.027608146f, 0.999618822f, + 0.024541229f, 0.999698819f, + 0.021474080f, 0.999769405f, + 0.018406730f, 0.999830582f, + 0.015339206f, 0.999882347f, + 0.012271538f, 0.999924702f, + 0.009203755f, 0.999957645f, + 0.006135885f, 0.999981175f, + 0.003067957f, 0.999995294f, + 0.000000000f, 1.000000000f, + -0.003067957f, 0.999995294f, + -0.006135885f, 0.999981175f, + -0.009203755f, 0.999957645f, + -0.012271538f, 0.999924702f, + -0.015339206f, 0.999882347f, + -0.018406730f, 0.999830582f, + -0.021474080f, 0.999769405f, + -0.024541229f, 0.999698819f, + -0.027608146f, 0.999618822f, + -0.030674803f, 0.999529418f, + -0.033741172f, 0.999430605f, + -0.036807223f, 0.999322385f, + -0.039872928f, 0.999204759f, + -0.042938257f, 0.999077728f, + -0.046003182f, 0.998941293f, + -0.049067674f, 0.998795456f, + -0.052131705f, 0.998640218f, + -0.055195244f, 0.998475581f, + -0.058258265f, 0.998301545f, + -0.061320736f, 0.998118113f, + -0.064382631f, 0.997925286f, + -0.067443920f, 0.997723067f, + -0.070504573f, 0.997511456f, + -0.073564564f, 0.997290457f, + -0.076623861f, 0.997060070f, + -0.079682438f, 0.996820299f, + -0.082740265f, 0.996571146f, + -0.085797312f, 0.996312612f, + -0.088853553f, 0.996044701f, + -0.091908956f, 0.995767414f, + -0.094963495f, 0.995480755f, + -0.098017140f, 0.995184727f, + -0.101069863f, 0.994879331f, + -0.104121634f, 0.994564571f, + -0.107172425f, 0.994240449f, + -0.110222207f, 0.993906970f, + -0.113270952f, 0.993564136f, + -0.116318631f, 0.993211949f, + -0.119365215f, 0.992850414f, + -0.122410675f, 0.992479535f, + -0.125454983f, 0.992099313f, + -0.128498111f, 0.991709754f, + -0.131540029f, 0.991310860f, + -0.134580709f, 0.990902635f, + -0.137620122f, 0.990485084f, + -0.140658239f, 0.990058210f, + -0.143695033f, 0.989622017f, + -0.146730474f, 0.989176510f, + -0.149764535f, 0.988721692f, + -0.152797185f, 0.988257568f, + -0.155828398f, 0.987784142f, + -0.158858143f, 0.987301418f, + -0.161886394f, 0.986809402f, + -0.164913120f, 0.986308097f, + -0.167938295f, 0.985797509f, + -0.170961889f, 0.985277642f, + -0.173983873f, 0.984748502f, + -0.177004220f, 0.984210092f, + -0.180022901f, 0.983662419f, + -0.183039888f, 0.983105487f, + -0.186055152f, 0.982539302f, + -0.189068664f, 0.981963869f, + -0.192080397f, 0.981379193f, + -0.195090322f, 0.980785280f, + -0.198098411f, 0.980182136f, + -0.201104635f, 0.979569766f, + -0.204108966f, 0.978948175f, + -0.207111376f, 0.978317371f, + -0.210111837f, 0.977677358f, + -0.213110320f, 0.977028143f, + -0.216106797f, 0.976369731f, + -0.219101240f, 0.975702130f, + -0.222093621f, 0.975025345f, + -0.225083911f, 0.974339383f, + -0.228072083f, 0.973644250f, + -0.231058108f, 0.972939952f, + -0.234041959f, 0.972226497f, + -0.237023606f, 0.971503891f, + -0.240003022f, 0.970772141f, + -0.242980180f, 0.970031253f, + -0.245955050f, 0.969281235f, + -0.248927606f, 0.968522094f, + -0.251897818f, 0.967753837f, + -0.254865660f, 0.966976471f, + -0.257831102f, 0.966190003f, + -0.260794118f, 0.965394442f, + -0.263754679f, 0.964589793f, + -0.266712757f, 0.963776066f, + -0.269668326f, 0.962953267f, + -0.272621355f, 0.962121404f, + -0.275571819f, 0.961280486f, + -0.278519689f, 0.960430519f, + -0.281464938f, 0.959571513f, + -0.284407537f, 0.958703475f, + -0.287347460f, 0.957826413f, + -0.290284677f, 0.956940336f, + -0.293219163f, 0.956045251f, + -0.296150888f, 0.955141168f, + -0.299079826f, 0.954228095f, + -0.302005949f, 0.953306040f, + -0.304929230f, 0.952375013f, + -0.307849640f, 0.951435021f, + -0.310767153f, 0.950486074f, + -0.313681740f, 0.949528181f, + -0.316593376f, 0.948561350f, + -0.319502031f, 0.947585591f, + -0.322407679f, 0.946600913f, + -0.325310292f, 0.945607325f, + -0.328209844f, 0.944604837f, + -0.331106306f, 0.943593458f, + -0.333999651f, 0.942573198f, + -0.336889853f, 0.941544065f, + -0.339776884f, 0.940506071f, + -0.342660717f, 0.939459224f, + -0.345541325f, 0.938403534f, + -0.348418680f, 0.937339012f, + -0.351292756f, 0.936265667f, + -0.354163525f, 0.935183510f, + -0.357030961f, 0.934092550f, + -0.359895037f, 0.932992799f, + -0.362755724f, 0.931884266f, + -0.365612998f, 0.930766961f, + -0.368466830f, 0.929640896f, + -0.371317194f, 0.928506080f, + -0.374164063f, 0.927362526f, + -0.377007410f, 0.926210242f, + -0.379847209f, 0.925049241f, + -0.382683432f, 0.923879533f, + -0.385516054f, 0.922701128f, + -0.388345047f, 0.921514039f, + -0.391170384f, 0.920318277f, + -0.393992040f, 0.919113852f, + -0.396809987f, 0.917900776f, + -0.399624200f, 0.916679060f, + -0.402434651f, 0.915448716f, + -0.405241314f, 0.914209756f, + -0.408044163f, 0.912962190f, + -0.410843171f, 0.911706032f, + -0.413638312f, 0.910441292f, + -0.416429560f, 0.909167983f, + -0.419216888f, 0.907886116f, + -0.422000271f, 0.906595705f, + -0.424779681f, 0.905296759f, + -0.427555093f, 0.903989293f, + -0.430326481f, 0.902673318f, + -0.433093819f, 0.901348847f, + -0.435857080f, 0.900015892f, + -0.438616239f, 0.898674466f, + -0.441371269f, 0.897324581f, + -0.444122145f, 0.895966250f, + -0.446868840f, 0.894599486f, + -0.449611330f, 0.893224301f, + -0.452349587f, 0.891840709f, + -0.455083587f, 0.890448723f, + -0.457813304f, 0.889048356f, + -0.460538711f, 0.887639620f, + -0.463259784f, 0.886222530f, + -0.465976496f, 0.884797098f, + -0.468688822f, 0.883363339f, + -0.471396737f, 0.881921264f, + -0.474100215f, 0.880470889f, + -0.476799230f, 0.879012226f, + -0.479493758f, 0.877545290f, + -0.482183772f, 0.876070094f, + -0.484869248f, 0.874586652f, + -0.487550160f, 0.873094978f, + -0.490226483f, 0.871595087f, + -0.492898192f, 0.870086991f, + -0.495565262f, 0.868570706f, + -0.498227667f, 0.867046246f, + -0.500885383f, 0.865513624f, + -0.503538384f, 0.863972856f, + -0.506186645f, 0.862423956f, + -0.508830143f, 0.860866939f, + -0.511468850f, 0.859301818f, + -0.514102744f, 0.857728610f, + -0.516731799f, 0.856147328f, + -0.519355990f, 0.854557988f, + -0.521975293f, 0.852960605f, + -0.524589683f, 0.851355193f, + -0.527199135f, 0.849741768f, + -0.529803625f, 0.848120345f, + -0.532403128f, 0.846490939f, + -0.534997620f, 0.844853565f, + -0.537587076f, 0.843208240f, + -0.540171473f, 0.841554977f, + -0.542750785f, 0.839893794f, + -0.545324988f, 0.838224706f, + -0.547894059f, 0.836547727f, + -0.550457973f, 0.834862875f, + -0.553016706f, 0.833170165f, + -0.555570233f, 0.831469612f, + -0.558118531f, 0.829761234f, + -0.560661576f, 0.828045045f, + -0.563199344f, 0.826321063f, + -0.565731811f, 0.824589303f, + -0.568258953f, 0.822849781f, + -0.570780746f, 0.821102515f, + -0.573297167f, 0.819347520f, + -0.575808191f, 0.817584813f, + -0.578313796f, 0.815814411f, + -0.580813958f, 0.814036330f, + -0.583308653f, 0.812250587f, + -0.585797857f, 0.810457198f, + -0.588281548f, 0.808656182f, + -0.590759702f, 0.806847554f, + -0.593232295f, 0.805031331f, + -0.595699304f, 0.803207531f, + -0.598160707f, 0.801376172f, + -0.600616479f, 0.799537269f, + -0.603066599f, 0.797690841f, + -0.605511041f, 0.795836905f, + -0.607949785f, 0.793975478f, + -0.610382806f, 0.792106577f, + -0.612810082f, 0.790230221f, + -0.615231591f, 0.788346428f, + -0.617647308f, 0.786455214f, + -0.620057212f, 0.784556597f, + -0.622461279f, 0.782650596f, + -0.624859488f, 0.780737229f, + -0.627251815f, 0.778816512f, + -0.629638239f, 0.776888466f, + -0.632018736f, 0.774953107f, + -0.634393284f, 0.773010453f, + -0.636761861f, 0.771060524f, + -0.639124445f, 0.769103338f, + -0.641481013f, 0.767138912f, + -0.643831543f, 0.765167266f, + -0.646176013f, 0.763188417f, + -0.648514401f, 0.761202385f, + -0.650846685f, 0.759209189f, + -0.653172843f, 0.757208847f, + -0.655492853f, 0.755201377f, + -0.657806693f, 0.753186799f, + -0.660114342f, 0.751165132f, + -0.662415778f, 0.749136395f, + -0.664710978f, 0.747100606f, + -0.666999922f, 0.745057785f, + -0.669282588f, 0.743007952f, + -0.671558955f, 0.740951125f, + -0.673829000f, 0.738887324f, + -0.676092704f, 0.736816569f, + -0.678350043f, 0.734738878f, + -0.680600998f, 0.732654272f, + -0.682845546f, 0.730562769f, + -0.685083668f, 0.728464390f, + -0.687315341f, 0.726359155f, + -0.689540545f, 0.724247083f, + -0.691759258f, 0.722128194f, + -0.693971461f, 0.720002508f, + -0.696177131f, 0.717870045f, + -0.698376249f, 0.715730825f, + -0.700568794f, 0.713584869f, + -0.702754744f, 0.711432196f, + -0.704934080f, 0.709272826f, + -0.707106781f, 0.707106781f, + -0.709272826f, 0.704934080f, + -0.711432196f, 0.702754744f, + -0.713584869f, 0.700568794f, + -0.715730825f, 0.698376249f, + -0.717870045f, 0.696177131f, + -0.720002508f, 0.693971461f, + -0.722128194f, 0.691759258f, + -0.724247083f, 0.689540545f, + -0.726359155f, 0.687315341f, + -0.728464390f, 0.685083668f, + -0.730562769f, 0.682845546f, + -0.732654272f, 0.680600998f, + -0.734738878f, 0.678350043f, + -0.736816569f, 0.676092704f, + -0.738887324f, 0.673829000f, + -0.740951125f, 0.671558955f, + -0.743007952f, 0.669282588f, + -0.745057785f, 0.666999922f, + -0.747100606f, 0.664710978f, + -0.749136395f, 0.662415778f, + -0.751165132f, 0.660114342f, + -0.753186799f, 0.657806693f, + -0.755201377f, 0.655492853f, + -0.757208847f, 0.653172843f, + -0.759209189f, 0.650846685f, + -0.761202385f, 0.648514401f, + -0.763188417f, 0.646176013f, + -0.765167266f, 0.643831543f, + -0.767138912f, 0.641481013f, + -0.769103338f, 0.639124445f, + -0.771060524f, 0.636761861f, + -0.773010453f, 0.634393284f, + -0.774953107f, 0.632018736f, + -0.776888466f, 0.629638239f, + -0.778816512f, 0.627251815f, + -0.780737229f, 0.624859488f, + -0.782650596f, 0.622461279f, + -0.784556597f, 0.620057212f, + -0.786455214f, 0.617647308f, + -0.788346428f, 0.615231591f, + -0.790230221f, 0.612810082f, + -0.792106577f, 0.610382806f, + -0.793975478f, 0.607949785f, + -0.795836905f, 0.605511041f, + -0.797690841f, 0.603066599f, + -0.799537269f, 0.600616479f, + -0.801376172f, 0.598160707f, + -0.803207531f, 0.595699304f, + -0.805031331f, 0.593232295f, + -0.806847554f, 0.590759702f, + -0.808656182f, 0.588281548f, + -0.810457198f, 0.585797857f, + -0.812250587f, 0.583308653f, + -0.814036330f, 0.580813958f, + -0.815814411f, 0.578313796f, + -0.817584813f, 0.575808191f, + -0.819347520f, 0.573297167f, + -0.821102515f, 0.570780746f, + -0.822849781f, 0.568258953f, + -0.824589303f, 0.565731811f, + -0.826321063f, 0.563199344f, + -0.828045045f, 0.560661576f, + -0.829761234f, 0.558118531f, + -0.831469612f, 0.555570233f, + -0.833170165f, 0.553016706f, + -0.834862875f, 0.550457973f, + -0.836547727f, 0.547894059f, + -0.838224706f, 0.545324988f, + -0.839893794f, 0.542750785f, + -0.841554977f, 0.540171473f, + -0.843208240f, 0.537587076f, + -0.844853565f, 0.534997620f, + -0.846490939f, 0.532403128f, + -0.848120345f, 0.529803625f, + -0.849741768f, 0.527199135f, + -0.851355193f, 0.524589683f, + -0.852960605f, 0.521975293f, + -0.854557988f, 0.519355990f, + -0.856147328f, 0.516731799f, + -0.857728610f, 0.514102744f, + -0.859301818f, 0.511468850f, + -0.860866939f, 0.508830143f, + -0.862423956f, 0.506186645f, + -0.863972856f, 0.503538384f, + -0.865513624f, 0.500885383f, + -0.867046246f, 0.498227667f, + -0.868570706f, 0.495565262f, + -0.870086991f, 0.492898192f, + -0.871595087f, 0.490226483f, + -0.873094978f, 0.487550160f, + -0.874586652f, 0.484869248f, + -0.876070094f, 0.482183772f, + -0.877545290f, 0.479493758f, + -0.879012226f, 0.476799230f, + -0.880470889f, 0.474100215f, + -0.881921264f, 0.471396737f, + -0.883363339f, 0.468688822f, + -0.884797098f, 0.465976496f, + -0.886222530f, 0.463259784f, + -0.887639620f, 0.460538711f, + -0.889048356f, 0.457813304f, + -0.890448723f, 0.455083587f, + -0.891840709f, 0.452349587f, + -0.893224301f, 0.449611330f, + -0.894599486f, 0.446868840f, + -0.895966250f, 0.444122145f, + -0.897324581f, 0.441371269f, + -0.898674466f, 0.438616239f, + -0.900015892f, 0.435857080f, + -0.901348847f, 0.433093819f, + -0.902673318f, 0.430326481f, + -0.903989293f, 0.427555093f, + -0.905296759f, 0.424779681f, + -0.906595705f, 0.422000271f, + -0.907886116f, 0.419216888f, + -0.909167983f, 0.416429560f, + -0.910441292f, 0.413638312f, + -0.911706032f, 0.410843171f, + -0.912962190f, 0.408044163f, + -0.914209756f, 0.405241314f, + -0.915448716f, 0.402434651f, + -0.916679060f, 0.399624200f, + -0.917900776f, 0.396809987f, + -0.919113852f, 0.393992040f, + -0.920318277f, 0.391170384f, + -0.921514039f, 0.388345047f, + -0.922701128f, 0.385516054f, + -0.923879533f, 0.382683432f, + -0.925049241f, 0.379847209f, + -0.926210242f, 0.377007410f, + -0.927362526f, 0.374164063f, + -0.928506080f, 0.371317194f, + -0.929640896f, 0.368466830f, + -0.930766961f, 0.365612998f, + -0.931884266f, 0.362755724f, + -0.932992799f, 0.359895037f, + -0.934092550f, 0.357030961f, + -0.935183510f, 0.354163525f, + -0.936265667f, 0.351292756f, + -0.937339012f, 0.348418680f, + -0.938403534f, 0.345541325f, + -0.939459224f, 0.342660717f, + -0.940506071f, 0.339776884f, + -0.941544065f, 0.336889853f, + -0.942573198f, 0.333999651f, + -0.943593458f, 0.331106306f, + -0.944604837f, 0.328209844f, + -0.945607325f, 0.325310292f, + -0.946600913f, 0.322407679f, + -0.947585591f, 0.319502031f, + -0.948561350f, 0.316593376f, + -0.949528181f, 0.313681740f, + -0.950486074f, 0.310767153f, + -0.951435021f, 0.307849640f, + -0.952375013f, 0.304929230f, + -0.953306040f, 0.302005949f, + -0.954228095f, 0.299079826f, + -0.955141168f, 0.296150888f, + -0.956045251f, 0.293219163f, + -0.956940336f, 0.290284677f, + -0.957826413f, 0.287347460f, + -0.958703475f, 0.284407537f, + -0.959571513f, 0.281464938f, + -0.960430519f, 0.278519689f, + -0.961280486f, 0.275571819f, + -0.962121404f, 0.272621355f, + -0.962953267f, 0.269668326f, + -0.963776066f, 0.266712757f, + -0.964589793f, 0.263754679f, + -0.965394442f, 0.260794118f, + -0.966190003f, 0.257831102f, + -0.966976471f, 0.254865660f, + -0.967753837f, 0.251897818f, + -0.968522094f, 0.248927606f, + -0.969281235f, 0.245955050f, + -0.970031253f, 0.242980180f, + -0.970772141f, 0.240003022f, + -0.971503891f, 0.237023606f, + -0.972226497f, 0.234041959f, + -0.972939952f, 0.231058108f, + -0.973644250f, 0.228072083f, + -0.974339383f, 0.225083911f, + -0.975025345f, 0.222093621f, + -0.975702130f, 0.219101240f, + -0.976369731f, 0.216106797f, + -0.977028143f, 0.213110320f, + -0.977677358f, 0.210111837f, + -0.978317371f, 0.207111376f, + -0.978948175f, 0.204108966f, + -0.979569766f, 0.201104635f, + -0.980182136f, 0.198098411f, + -0.980785280f, 0.195090322f, + -0.981379193f, 0.192080397f, + -0.981963869f, 0.189068664f, + -0.982539302f, 0.186055152f, + -0.983105487f, 0.183039888f, + -0.983662419f, 0.180022901f, + -0.984210092f, 0.177004220f, + -0.984748502f, 0.173983873f, + -0.985277642f, 0.170961889f, + -0.985797509f, 0.167938295f, + -0.986308097f, 0.164913120f, + -0.986809402f, 0.161886394f, + -0.987301418f, 0.158858143f, + -0.987784142f, 0.155828398f, + -0.988257568f, 0.152797185f, + -0.988721692f, 0.149764535f, + -0.989176510f, 0.146730474f, + -0.989622017f, 0.143695033f, + -0.990058210f, 0.140658239f, + -0.990485084f, 0.137620122f, + -0.990902635f, 0.134580709f, + -0.991310860f, 0.131540029f, + -0.991709754f, 0.128498111f, + -0.992099313f, 0.125454983f, + -0.992479535f, 0.122410675f, + -0.992850414f, 0.119365215f, + -0.993211949f, 0.116318631f, + -0.993564136f, 0.113270952f, + -0.993906970f, 0.110222207f, + -0.994240449f, 0.107172425f, + -0.994564571f, 0.104121634f, + -0.994879331f, 0.101069863f, + -0.995184727f, 0.098017140f, + -0.995480755f, 0.094963495f, + -0.995767414f, 0.091908956f, + -0.996044701f, 0.088853553f, + -0.996312612f, 0.085797312f, + -0.996571146f, 0.082740265f, + -0.996820299f, 0.079682438f, + -0.997060070f, 0.076623861f, + -0.997290457f, 0.073564564f, + -0.997511456f, 0.070504573f, + -0.997723067f, 0.067443920f, + -0.997925286f, 0.064382631f, + -0.998118113f, 0.061320736f, + -0.998301545f, 0.058258265f, + -0.998475581f, 0.055195244f, + -0.998640218f, 0.052131705f, + -0.998795456f, 0.049067674f, + -0.998941293f, 0.046003182f, + -0.999077728f, 0.042938257f, + -0.999204759f, 0.039872928f, + -0.999322385f, 0.036807223f, + -0.999430605f, 0.033741172f, + -0.999529418f, 0.030674803f, + -0.999618822f, 0.027608146f, + -0.999698819f, 0.024541229f, + -0.999769405f, 0.021474080f, + -0.999830582f, 0.018406730f, + -0.999882347f, 0.015339206f, + -0.999924702f, 0.012271538f, + -0.999957645f, 0.009203755f, + -0.999981175f, 0.006135885f, + -0.999995294f, 0.003067957f, + -1.000000000f, 0.000000000f, + -0.999995294f, -0.003067957f, + -0.999981175f, -0.006135885f, + -0.999957645f, -0.009203755f, + -0.999924702f, -0.012271538f, + -0.999882347f, -0.015339206f, + -0.999830582f, -0.018406730f, + -0.999769405f, -0.021474080f, + -0.999698819f, -0.024541229f, + -0.999618822f, -0.027608146f, + -0.999529418f, -0.030674803f, + -0.999430605f, -0.033741172f, + -0.999322385f, -0.036807223f, + -0.999204759f, -0.039872928f, + -0.999077728f, -0.042938257f, + -0.998941293f, -0.046003182f, + -0.998795456f, -0.049067674f, + -0.998640218f, -0.052131705f, + -0.998475581f, -0.055195244f, + -0.998301545f, -0.058258265f, + -0.998118113f, -0.061320736f, + -0.997925286f, -0.064382631f, + -0.997723067f, -0.067443920f, + -0.997511456f, -0.070504573f, + -0.997290457f, -0.073564564f, + -0.997060070f, -0.076623861f, + -0.996820299f, -0.079682438f, + -0.996571146f, -0.082740265f, + -0.996312612f, -0.085797312f, + -0.996044701f, -0.088853553f, + -0.995767414f, -0.091908956f, + -0.995480755f, -0.094963495f, + -0.995184727f, -0.098017140f, + -0.994879331f, -0.101069863f, + -0.994564571f, -0.104121634f, + -0.994240449f, -0.107172425f, + -0.993906970f, -0.110222207f, + -0.993564136f, -0.113270952f, + -0.993211949f, -0.116318631f, + -0.992850414f, -0.119365215f, + -0.992479535f, -0.122410675f, + -0.992099313f, -0.125454983f, + -0.991709754f, -0.128498111f, + -0.991310860f, -0.131540029f, + -0.990902635f, -0.134580709f, + -0.990485084f, -0.137620122f, + -0.990058210f, -0.140658239f, + -0.989622017f, -0.143695033f, + -0.989176510f, -0.146730474f, + -0.988721692f, -0.149764535f, + -0.988257568f, -0.152797185f, + -0.987784142f, -0.155828398f, + -0.987301418f, -0.158858143f, + -0.986809402f, -0.161886394f, + -0.986308097f, -0.164913120f, + -0.985797509f, -0.167938295f, + -0.985277642f, -0.170961889f, + -0.984748502f, -0.173983873f, + -0.984210092f, -0.177004220f, + -0.983662419f, -0.180022901f, + -0.983105487f, -0.183039888f, + -0.982539302f, -0.186055152f, + -0.981963869f, -0.189068664f, + -0.981379193f, -0.192080397f, + -0.980785280f, -0.195090322f, + -0.980182136f, -0.198098411f, + -0.979569766f, -0.201104635f, + -0.978948175f, -0.204108966f, + -0.978317371f, -0.207111376f, + -0.977677358f, -0.210111837f, + -0.977028143f, -0.213110320f, + -0.976369731f, -0.216106797f, + -0.975702130f, -0.219101240f, + -0.975025345f, -0.222093621f, + -0.974339383f, -0.225083911f, + -0.973644250f, -0.228072083f, + -0.972939952f, -0.231058108f, + -0.972226497f, -0.234041959f, + -0.971503891f, -0.237023606f, + -0.970772141f, -0.240003022f, + -0.970031253f, -0.242980180f, + -0.969281235f, -0.245955050f, + -0.968522094f, -0.248927606f, + -0.967753837f, -0.251897818f, + -0.966976471f, -0.254865660f, + -0.966190003f, -0.257831102f, + -0.965394442f, -0.260794118f, + -0.964589793f, -0.263754679f, + -0.963776066f, -0.266712757f, + -0.962953267f, -0.269668326f, + -0.962121404f, -0.272621355f, + -0.961280486f, -0.275571819f, + -0.960430519f, -0.278519689f, + -0.959571513f, -0.281464938f, + -0.958703475f, -0.284407537f, + -0.957826413f, -0.287347460f, + -0.956940336f, -0.290284677f, + -0.956045251f, -0.293219163f, + -0.955141168f, -0.296150888f, + -0.954228095f, -0.299079826f, + -0.953306040f, -0.302005949f, + -0.952375013f, -0.304929230f, + -0.951435021f, -0.307849640f, + -0.950486074f, -0.310767153f, + -0.949528181f, -0.313681740f, + -0.948561350f, -0.316593376f, + -0.947585591f, -0.319502031f, + -0.946600913f, -0.322407679f, + -0.945607325f, -0.325310292f, + -0.944604837f, -0.328209844f, + -0.943593458f, -0.331106306f, + -0.942573198f, -0.333999651f, + -0.941544065f, -0.336889853f, + -0.940506071f, -0.339776884f, + -0.939459224f, -0.342660717f, + -0.938403534f, -0.345541325f, + -0.937339012f, -0.348418680f, + -0.936265667f, -0.351292756f, + -0.935183510f, -0.354163525f, + -0.934092550f, -0.357030961f, + -0.932992799f, -0.359895037f, + -0.931884266f, -0.362755724f, + -0.930766961f, -0.365612998f, + -0.929640896f, -0.368466830f, + -0.928506080f, -0.371317194f, + -0.927362526f, -0.374164063f, + -0.926210242f, -0.377007410f, + -0.925049241f, -0.379847209f, + -0.923879533f, -0.382683432f, + -0.922701128f, -0.385516054f, + -0.921514039f, -0.388345047f, + -0.920318277f, -0.391170384f, + -0.919113852f, -0.393992040f, + -0.917900776f, -0.396809987f, + -0.916679060f, -0.399624200f, + -0.915448716f, -0.402434651f, + -0.914209756f, -0.405241314f, + -0.912962190f, -0.408044163f, + -0.911706032f, -0.410843171f, + -0.910441292f, -0.413638312f, + -0.909167983f, -0.416429560f, + -0.907886116f, -0.419216888f, + -0.906595705f, -0.422000271f, + -0.905296759f, -0.424779681f, + -0.903989293f, -0.427555093f, + -0.902673318f, -0.430326481f, + -0.901348847f, -0.433093819f, + -0.900015892f, -0.435857080f, + -0.898674466f, -0.438616239f, + -0.897324581f, -0.441371269f, + -0.895966250f, -0.444122145f, + -0.894599486f, -0.446868840f, + -0.893224301f, -0.449611330f, + -0.891840709f, -0.452349587f, + -0.890448723f, -0.455083587f, + -0.889048356f, -0.457813304f, + -0.887639620f, -0.460538711f, + -0.886222530f, -0.463259784f, + -0.884797098f, -0.465976496f, + -0.883363339f, -0.468688822f, + -0.881921264f, -0.471396737f, + -0.880470889f, -0.474100215f, + -0.879012226f, -0.476799230f, + -0.877545290f, -0.479493758f, + -0.876070094f, -0.482183772f, + -0.874586652f, -0.484869248f, + -0.873094978f, -0.487550160f, + -0.871595087f, -0.490226483f, + -0.870086991f, -0.492898192f, + -0.868570706f, -0.495565262f, + -0.867046246f, -0.498227667f, + -0.865513624f, -0.500885383f, + -0.863972856f, -0.503538384f, + -0.862423956f, -0.506186645f, + -0.860866939f, -0.508830143f, + -0.859301818f, -0.511468850f, + -0.857728610f, -0.514102744f, + -0.856147328f, -0.516731799f, + -0.854557988f, -0.519355990f, + -0.852960605f, -0.521975293f, + -0.851355193f, -0.524589683f, + -0.849741768f, -0.527199135f, + -0.848120345f, -0.529803625f, + -0.846490939f, -0.532403128f, + -0.844853565f, -0.534997620f, + -0.843208240f, -0.537587076f, + -0.841554977f, -0.540171473f, + -0.839893794f, -0.542750785f, + -0.838224706f, -0.545324988f, + -0.836547727f, -0.547894059f, + -0.834862875f, -0.550457973f, + -0.833170165f, -0.553016706f, + -0.831469612f, -0.555570233f, + -0.829761234f, -0.558118531f, + -0.828045045f, -0.560661576f, + -0.826321063f, -0.563199344f, + -0.824589303f, -0.565731811f, + -0.822849781f, -0.568258953f, + -0.821102515f, -0.570780746f, + -0.819347520f, -0.573297167f, + -0.817584813f, -0.575808191f, + -0.815814411f, -0.578313796f, + -0.814036330f, -0.580813958f, + -0.812250587f, -0.583308653f, + -0.810457198f, -0.585797857f, + -0.808656182f, -0.588281548f, + -0.806847554f, -0.590759702f, + -0.805031331f, -0.593232295f, + -0.803207531f, -0.595699304f, + -0.801376172f, -0.598160707f, + -0.799537269f, -0.600616479f, + -0.797690841f, -0.603066599f, + -0.795836905f, -0.605511041f, + -0.793975478f, -0.607949785f, + -0.792106577f, -0.610382806f, + -0.790230221f, -0.612810082f, + -0.788346428f, -0.615231591f, + -0.786455214f, -0.617647308f, + -0.784556597f, -0.620057212f, + -0.782650596f, -0.622461279f, + -0.780737229f, -0.624859488f, + -0.778816512f, -0.627251815f, + -0.776888466f, -0.629638239f, + -0.774953107f, -0.632018736f, + -0.773010453f, -0.634393284f, + -0.771060524f, -0.636761861f, + -0.769103338f, -0.639124445f, + -0.767138912f, -0.641481013f, + -0.765167266f, -0.643831543f, + -0.763188417f, -0.646176013f, + -0.761202385f, -0.648514401f, + -0.759209189f, -0.650846685f, + -0.757208847f, -0.653172843f, + -0.755201377f, -0.655492853f, + -0.753186799f, -0.657806693f, + -0.751165132f, -0.660114342f, + -0.749136395f, -0.662415778f, + -0.747100606f, -0.664710978f, + -0.745057785f, -0.666999922f, + -0.743007952f, -0.669282588f, + -0.740951125f, -0.671558955f, + -0.738887324f, -0.673829000f, + -0.736816569f, -0.676092704f, + -0.734738878f, -0.678350043f, + -0.732654272f, -0.680600998f, + -0.730562769f, -0.682845546f, + -0.728464390f, -0.685083668f, + -0.726359155f, -0.687315341f, + -0.724247083f, -0.689540545f, + -0.722128194f, -0.691759258f, + -0.720002508f, -0.693971461f, + -0.717870045f, -0.696177131f, + -0.715730825f, -0.698376249f, + -0.713584869f, -0.700568794f, + -0.711432196f, -0.702754744f, + -0.709272826f, -0.704934080f, + -0.707106781f, -0.707106781f, + -0.704934080f, -0.709272826f, + -0.702754744f, -0.711432196f, + -0.700568794f, -0.713584869f, + -0.698376249f, -0.715730825f, + -0.696177131f, -0.717870045f, + -0.693971461f, -0.720002508f, + -0.691759258f, -0.722128194f, + -0.689540545f, -0.724247083f, + -0.687315341f, -0.726359155f, + -0.685083668f, -0.728464390f, + -0.682845546f, -0.730562769f, + -0.680600998f, -0.732654272f, + -0.678350043f, -0.734738878f, + -0.676092704f, -0.736816569f, + -0.673829000f, -0.738887324f, + -0.671558955f, -0.740951125f, + -0.669282588f, -0.743007952f, + -0.666999922f, -0.745057785f, + -0.664710978f, -0.747100606f, + -0.662415778f, -0.749136395f, + -0.660114342f, -0.751165132f, + -0.657806693f, -0.753186799f, + -0.655492853f, -0.755201377f, + -0.653172843f, -0.757208847f, + -0.650846685f, -0.759209189f, + -0.648514401f, -0.761202385f, + -0.646176013f, -0.763188417f, + -0.643831543f, -0.765167266f, + -0.641481013f, -0.767138912f, + -0.639124445f, -0.769103338f, + -0.636761861f, -0.771060524f, + -0.634393284f, -0.773010453f, + -0.632018736f, -0.774953107f, + -0.629638239f, -0.776888466f, + -0.627251815f, -0.778816512f, + -0.624859488f, -0.780737229f, + -0.622461279f, -0.782650596f, + -0.620057212f, -0.784556597f, + -0.617647308f, -0.786455214f, + -0.615231591f, -0.788346428f, + -0.612810082f, -0.790230221f, + -0.610382806f, -0.792106577f, + -0.607949785f, -0.793975478f, + -0.605511041f, -0.795836905f, + -0.603066599f, -0.797690841f, + -0.600616479f, -0.799537269f, + -0.598160707f, -0.801376172f, + -0.595699304f, -0.803207531f, + -0.593232295f, -0.805031331f, + -0.590759702f, -0.806847554f, + -0.588281548f, -0.808656182f, + -0.585797857f, -0.810457198f, + -0.583308653f, -0.812250587f, + -0.580813958f, -0.814036330f, + -0.578313796f, -0.815814411f, + -0.575808191f, -0.817584813f, + -0.573297167f, -0.819347520f, + -0.570780746f, -0.821102515f, + -0.568258953f, -0.822849781f, + -0.565731811f, -0.824589303f, + -0.563199344f, -0.826321063f, + -0.560661576f, -0.828045045f, + -0.558118531f, -0.829761234f, + -0.555570233f, -0.831469612f, + -0.553016706f, -0.833170165f, + -0.550457973f, -0.834862875f, + -0.547894059f, -0.836547727f, + -0.545324988f, -0.838224706f, + -0.542750785f, -0.839893794f, + -0.540171473f, -0.841554977f, + -0.537587076f, -0.843208240f, + -0.534997620f, -0.844853565f, + -0.532403128f, -0.846490939f, + -0.529803625f, -0.848120345f, + -0.527199135f, -0.849741768f, + -0.524589683f, -0.851355193f, + -0.521975293f, -0.852960605f, + -0.519355990f, -0.854557988f, + -0.516731799f, -0.856147328f, + -0.514102744f, -0.857728610f, + -0.511468850f, -0.859301818f, + -0.508830143f, -0.860866939f, + -0.506186645f, -0.862423956f, + -0.503538384f, -0.863972856f, + -0.500885383f, -0.865513624f, + -0.498227667f, -0.867046246f, + -0.495565262f, -0.868570706f, + -0.492898192f, -0.870086991f, + -0.490226483f, -0.871595087f, + -0.487550160f, -0.873094978f, + -0.484869248f, -0.874586652f, + -0.482183772f, -0.876070094f, + -0.479493758f, -0.877545290f, + -0.476799230f, -0.879012226f, + -0.474100215f, -0.880470889f, + -0.471396737f, -0.881921264f, + -0.468688822f, -0.883363339f, + -0.465976496f, -0.884797098f, + -0.463259784f, -0.886222530f, + -0.460538711f, -0.887639620f, + -0.457813304f, -0.889048356f, + -0.455083587f, -0.890448723f, + -0.452349587f, -0.891840709f, + -0.449611330f, -0.893224301f, + -0.446868840f, -0.894599486f, + -0.444122145f, -0.895966250f, + -0.441371269f, -0.897324581f, + -0.438616239f, -0.898674466f, + -0.435857080f, -0.900015892f, + -0.433093819f, -0.901348847f, + -0.430326481f, -0.902673318f, + -0.427555093f, -0.903989293f, + -0.424779681f, -0.905296759f, + -0.422000271f, -0.906595705f, + -0.419216888f, -0.907886116f, + -0.416429560f, -0.909167983f, + -0.413638312f, -0.910441292f, + -0.410843171f, -0.911706032f, + -0.408044163f, -0.912962190f, + -0.405241314f, -0.914209756f, + -0.402434651f, -0.915448716f, + -0.399624200f, -0.916679060f, + -0.396809987f, -0.917900776f, + -0.393992040f, -0.919113852f, + -0.391170384f, -0.920318277f, + -0.388345047f, -0.921514039f, + -0.385516054f, -0.922701128f, + -0.382683432f, -0.923879533f, + -0.379847209f, -0.925049241f, + -0.377007410f, -0.926210242f, + -0.374164063f, -0.927362526f, + -0.371317194f, -0.928506080f, + -0.368466830f, -0.929640896f, + -0.365612998f, -0.930766961f, + -0.362755724f, -0.931884266f, + -0.359895037f, -0.932992799f, + -0.357030961f, -0.934092550f, + -0.354163525f, -0.935183510f, + -0.351292756f, -0.936265667f, + -0.348418680f, -0.937339012f, + -0.345541325f, -0.938403534f, + -0.342660717f, -0.939459224f, + -0.339776884f, -0.940506071f, + -0.336889853f, -0.941544065f, + -0.333999651f, -0.942573198f, + -0.331106306f, -0.943593458f, + -0.328209844f, -0.944604837f, + -0.325310292f, -0.945607325f, + -0.322407679f, -0.946600913f, + -0.319502031f, -0.947585591f, + -0.316593376f, -0.948561350f, + -0.313681740f, -0.949528181f, + -0.310767153f, -0.950486074f, + -0.307849640f, -0.951435021f, + -0.304929230f, -0.952375013f, + -0.302005949f, -0.953306040f, + -0.299079826f, -0.954228095f, + -0.296150888f, -0.955141168f, + -0.293219163f, -0.956045251f, + -0.290284677f, -0.956940336f, + -0.287347460f, -0.957826413f, + -0.284407537f, -0.958703475f, + -0.281464938f, -0.959571513f, + -0.278519689f, -0.960430519f, + -0.275571819f, -0.961280486f, + -0.272621355f, -0.962121404f, + -0.269668326f, -0.962953267f, + -0.266712757f, -0.963776066f, + -0.263754679f, -0.964589793f, + -0.260794118f, -0.965394442f, + -0.257831102f, -0.966190003f, + -0.254865660f, -0.966976471f, + -0.251897818f, -0.967753837f, + -0.248927606f, -0.968522094f, + -0.245955050f, -0.969281235f, + -0.242980180f, -0.970031253f, + -0.240003022f, -0.970772141f, + -0.237023606f, -0.971503891f, + -0.234041959f, -0.972226497f, + -0.231058108f, -0.972939952f, + -0.228072083f, -0.973644250f, + -0.225083911f, -0.974339383f, + -0.222093621f, -0.975025345f, + -0.219101240f, -0.975702130f, + -0.216106797f, -0.976369731f, + -0.213110320f, -0.977028143f, + -0.210111837f, -0.977677358f, + -0.207111376f, -0.978317371f, + -0.204108966f, -0.978948175f, + -0.201104635f, -0.979569766f, + -0.198098411f, -0.980182136f, + -0.195090322f, -0.980785280f, + -0.192080397f, -0.981379193f, + -0.189068664f, -0.981963869f, + -0.186055152f, -0.982539302f, + -0.183039888f, -0.983105487f, + -0.180022901f, -0.983662419f, + -0.177004220f, -0.984210092f, + -0.173983873f, -0.984748502f, + -0.170961889f, -0.985277642f, + -0.167938295f, -0.985797509f, + -0.164913120f, -0.986308097f, + -0.161886394f, -0.986809402f, + -0.158858143f, -0.987301418f, + -0.155828398f, -0.987784142f, + -0.152797185f, -0.988257568f, + -0.149764535f, -0.988721692f, + -0.146730474f, -0.989176510f, + -0.143695033f, -0.989622017f, + -0.140658239f, -0.990058210f, + -0.137620122f, -0.990485084f, + -0.134580709f, -0.990902635f, + -0.131540029f, -0.991310860f, + -0.128498111f, -0.991709754f, + -0.125454983f, -0.992099313f, + -0.122410675f, -0.992479535f, + -0.119365215f, -0.992850414f, + -0.116318631f, -0.993211949f, + -0.113270952f, -0.993564136f, + -0.110222207f, -0.993906970f, + -0.107172425f, -0.994240449f, + -0.104121634f, -0.994564571f, + -0.101069863f, -0.994879331f, + -0.098017140f, -0.995184727f, + -0.094963495f, -0.995480755f, + -0.091908956f, -0.995767414f, + -0.088853553f, -0.996044701f, + -0.085797312f, -0.996312612f, + -0.082740265f, -0.996571146f, + -0.079682438f, -0.996820299f, + -0.076623861f, -0.997060070f, + -0.073564564f, -0.997290457f, + -0.070504573f, -0.997511456f, + -0.067443920f, -0.997723067f, + -0.064382631f, -0.997925286f, + -0.061320736f, -0.998118113f, + -0.058258265f, -0.998301545f, + -0.055195244f, -0.998475581f, + -0.052131705f, -0.998640218f, + -0.049067674f, -0.998795456f, + -0.046003182f, -0.998941293f, + -0.042938257f, -0.999077728f, + -0.039872928f, -0.999204759f, + -0.036807223f, -0.999322385f, + -0.033741172f, -0.999430605f, + -0.030674803f, -0.999529418f, + -0.027608146f, -0.999618822f, + -0.024541229f, -0.999698819f, + -0.021474080f, -0.999769405f, + -0.018406730f, -0.999830582f, + -0.015339206f, -0.999882347f, + -0.012271538f, -0.999924702f, + -0.009203755f, -0.999957645f, + -0.006135885f, -0.999981175f, + -0.003067957f, -0.999995294f, + -0.000000000f, -1.000000000f, + 0.003067957f, -0.999995294f, + 0.006135885f, -0.999981175f, + 0.009203755f, -0.999957645f, + 0.012271538f, -0.999924702f, + 0.015339206f, -0.999882347f, + 0.018406730f, -0.999830582f, + 0.021474080f, -0.999769405f, + 0.024541229f, -0.999698819f, + 0.027608146f, -0.999618822f, + 0.030674803f, -0.999529418f, + 0.033741172f, -0.999430605f, + 0.036807223f, -0.999322385f, + 0.039872928f, -0.999204759f, + 0.042938257f, -0.999077728f, + 0.046003182f, -0.998941293f, + 0.049067674f, -0.998795456f, + 0.052131705f, -0.998640218f, + 0.055195244f, -0.998475581f, + 0.058258265f, -0.998301545f, + 0.061320736f, -0.998118113f, + 0.064382631f, -0.997925286f, + 0.067443920f, -0.997723067f, + 0.070504573f, -0.997511456f, + 0.073564564f, -0.997290457f, + 0.076623861f, -0.997060070f, + 0.079682438f, -0.996820299f, + 0.082740265f, -0.996571146f, + 0.085797312f, -0.996312612f, + 0.088853553f, -0.996044701f, + 0.091908956f, -0.995767414f, + 0.094963495f, -0.995480755f, + 0.098017140f, -0.995184727f, + 0.101069863f, -0.994879331f, + 0.104121634f, -0.994564571f, + 0.107172425f, -0.994240449f, + 0.110222207f, -0.993906970f, + 0.113270952f, -0.993564136f, + 0.116318631f, -0.993211949f, + 0.119365215f, -0.992850414f, + 0.122410675f, -0.992479535f, + 0.125454983f, -0.992099313f, + 0.128498111f, -0.991709754f, + 0.131540029f, -0.991310860f, + 0.134580709f, -0.990902635f, + 0.137620122f, -0.990485084f, + 0.140658239f, -0.990058210f, + 0.143695033f, -0.989622017f, + 0.146730474f, -0.989176510f, + 0.149764535f, -0.988721692f, + 0.152797185f, -0.988257568f, + 0.155828398f, -0.987784142f, + 0.158858143f, -0.987301418f, + 0.161886394f, -0.986809402f, + 0.164913120f, -0.986308097f, + 0.167938295f, -0.985797509f, + 0.170961889f, -0.985277642f, + 0.173983873f, -0.984748502f, + 0.177004220f, -0.984210092f, + 0.180022901f, -0.983662419f, + 0.183039888f, -0.983105487f, + 0.186055152f, -0.982539302f, + 0.189068664f, -0.981963869f, + 0.192080397f, -0.981379193f, + 0.195090322f, -0.980785280f, + 0.198098411f, -0.980182136f, + 0.201104635f, -0.979569766f, + 0.204108966f, -0.978948175f, + 0.207111376f, -0.978317371f, + 0.210111837f, -0.977677358f, + 0.213110320f, -0.977028143f, + 0.216106797f, -0.976369731f, + 0.219101240f, -0.975702130f, + 0.222093621f, -0.975025345f, + 0.225083911f, -0.974339383f, + 0.228072083f, -0.973644250f, + 0.231058108f, -0.972939952f, + 0.234041959f, -0.972226497f, + 0.237023606f, -0.971503891f, + 0.240003022f, -0.970772141f, + 0.242980180f, -0.970031253f, + 0.245955050f, -0.969281235f, + 0.248927606f, -0.968522094f, + 0.251897818f, -0.967753837f, + 0.254865660f, -0.966976471f, + 0.257831102f, -0.966190003f, + 0.260794118f, -0.965394442f, + 0.263754679f, -0.964589793f, + 0.266712757f, -0.963776066f, + 0.269668326f, -0.962953267f, + 0.272621355f, -0.962121404f, + 0.275571819f, -0.961280486f, + 0.278519689f, -0.960430519f, + 0.281464938f, -0.959571513f, + 0.284407537f, -0.958703475f, + 0.287347460f, -0.957826413f, + 0.290284677f, -0.956940336f, + 0.293219163f, -0.956045251f, + 0.296150888f, -0.955141168f, + 0.299079826f, -0.954228095f, + 0.302005949f, -0.953306040f, + 0.304929230f, -0.952375013f, + 0.307849640f, -0.951435021f, + 0.310767153f, -0.950486074f, + 0.313681740f, -0.949528181f, + 0.316593376f, -0.948561350f, + 0.319502031f, -0.947585591f, + 0.322407679f, -0.946600913f, + 0.325310292f, -0.945607325f, + 0.328209844f, -0.944604837f, + 0.331106306f, -0.943593458f, + 0.333999651f, -0.942573198f, + 0.336889853f, -0.941544065f, + 0.339776884f, -0.940506071f, + 0.342660717f, -0.939459224f, + 0.345541325f, -0.938403534f, + 0.348418680f, -0.937339012f, + 0.351292756f, -0.936265667f, + 0.354163525f, -0.935183510f, + 0.357030961f, -0.934092550f, + 0.359895037f, -0.932992799f, + 0.362755724f, -0.931884266f, + 0.365612998f, -0.930766961f, + 0.368466830f, -0.929640896f, + 0.371317194f, -0.928506080f, + 0.374164063f, -0.927362526f, + 0.377007410f, -0.926210242f, + 0.379847209f, -0.925049241f, + 0.382683432f, -0.923879533f, + 0.385516054f, -0.922701128f, + 0.388345047f, -0.921514039f, + 0.391170384f, -0.920318277f, + 0.393992040f, -0.919113852f, + 0.396809987f, -0.917900776f, + 0.399624200f, -0.916679060f, + 0.402434651f, -0.915448716f, + 0.405241314f, -0.914209756f, + 0.408044163f, -0.912962190f, + 0.410843171f, -0.911706032f, + 0.413638312f, -0.910441292f, + 0.416429560f, -0.909167983f, + 0.419216888f, -0.907886116f, + 0.422000271f, -0.906595705f, + 0.424779681f, -0.905296759f, + 0.427555093f, -0.903989293f, + 0.430326481f, -0.902673318f, + 0.433093819f, -0.901348847f, + 0.435857080f, -0.900015892f, + 0.438616239f, -0.898674466f, + 0.441371269f, -0.897324581f, + 0.444122145f, -0.895966250f, + 0.446868840f, -0.894599486f, + 0.449611330f, -0.893224301f, + 0.452349587f, -0.891840709f, + 0.455083587f, -0.890448723f, + 0.457813304f, -0.889048356f, + 0.460538711f, -0.887639620f, + 0.463259784f, -0.886222530f, + 0.465976496f, -0.884797098f, + 0.468688822f, -0.883363339f, + 0.471396737f, -0.881921264f, + 0.474100215f, -0.880470889f, + 0.476799230f, -0.879012226f, + 0.479493758f, -0.877545290f, + 0.482183772f, -0.876070094f, + 0.484869248f, -0.874586652f, + 0.487550160f, -0.873094978f, + 0.490226483f, -0.871595087f, + 0.492898192f, -0.870086991f, + 0.495565262f, -0.868570706f, + 0.498227667f, -0.867046246f, + 0.500885383f, -0.865513624f, + 0.503538384f, -0.863972856f, + 0.506186645f, -0.862423956f, + 0.508830143f, -0.860866939f, + 0.511468850f, -0.859301818f, + 0.514102744f, -0.857728610f, + 0.516731799f, -0.856147328f, + 0.519355990f, -0.854557988f, + 0.521975293f, -0.852960605f, + 0.524589683f, -0.851355193f, + 0.527199135f, -0.849741768f, + 0.529803625f, -0.848120345f, + 0.532403128f, -0.846490939f, + 0.534997620f, -0.844853565f, + 0.537587076f, -0.843208240f, + 0.540171473f, -0.841554977f, + 0.542750785f, -0.839893794f, + 0.545324988f, -0.838224706f, + 0.547894059f, -0.836547727f, + 0.550457973f, -0.834862875f, + 0.553016706f, -0.833170165f, + 0.555570233f, -0.831469612f, + 0.558118531f, -0.829761234f, + 0.560661576f, -0.828045045f, + 0.563199344f, -0.826321063f, + 0.565731811f, -0.824589303f, + 0.568258953f, -0.822849781f, + 0.570780746f, -0.821102515f, + 0.573297167f, -0.819347520f, + 0.575808191f, -0.817584813f, + 0.578313796f, -0.815814411f, + 0.580813958f, -0.814036330f, + 0.583308653f, -0.812250587f, + 0.585797857f, -0.810457198f, + 0.588281548f, -0.808656182f, + 0.590759702f, -0.806847554f, + 0.593232295f, -0.805031331f, + 0.595699304f, -0.803207531f, + 0.598160707f, -0.801376172f, + 0.600616479f, -0.799537269f, + 0.603066599f, -0.797690841f, + 0.605511041f, -0.795836905f, + 0.607949785f, -0.793975478f, + 0.610382806f, -0.792106577f, + 0.612810082f, -0.790230221f, + 0.615231591f, -0.788346428f, + 0.617647308f, -0.786455214f, + 0.620057212f, -0.784556597f, + 0.622461279f, -0.782650596f, + 0.624859488f, -0.780737229f, + 0.627251815f, -0.778816512f, + 0.629638239f, -0.776888466f, + 0.632018736f, -0.774953107f, + 0.634393284f, -0.773010453f, + 0.636761861f, -0.771060524f, + 0.639124445f, -0.769103338f, + 0.641481013f, -0.767138912f, + 0.643831543f, -0.765167266f, + 0.646176013f, -0.763188417f, + 0.648514401f, -0.761202385f, + 0.650846685f, -0.759209189f, + 0.653172843f, -0.757208847f, + 0.655492853f, -0.755201377f, + 0.657806693f, -0.753186799f, + 0.660114342f, -0.751165132f, + 0.662415778f, -0.749136395f, + 0.664710978f, -0.747100606f, + 0.666999922f, -0.745057785f, + 0.669282588f, -0.743007952f, + 0.671558955f, -0.740951125f, + 0.673829000f, -0.738887324f, + 0.676092704f, -0.736816569f, + 0.678350043f, -0.734738878f, + 0.680600998f, -0.732654272f, + 0.682845546f, -0.730562769f, + 0.685083668f, -0.728464390f, + 0.687315341f, -0.726359155f, + 0.689540545f, -0.724247083f, + 0.691759258f, -0.722128194f, + 0.693971461f, -0.720002508f, + 0.696177131f, -0.717870045f, + 0.698376249f, -0.715730825f, + 0.700568794f, -0.713584869f, + 0.702754744f, -0.711432196f, + 0.704934080f, -0.709272826f, + 0.707106781f, -0.707106781f, + 0.709272826f, -0.704934080f, + 0.711432196f, -0.702754744f, + 0.713584869f, -0.700568794f, + 0.715730825f, -0.698376249f, + 0.717870045f, -0.696177131f, + 0.720002508f, -0.693971461f, + 0.722128194f, -0.691759258f, + 0.724247083f, -0.689540545f, + 0.726359155f, -0.687315341f, + 0.728464390f, -0.685083668f, + 0.730562769f, -0.682845546f, + 0.732654272f, -0.680600998f, + 0.734738878f, -0.678350043f, + 0.736816569f, -0.676092704f, + 0.738887324f, -0.673829000f, + 0.740951125f, -0.671558955f, + 0.743007952f, -0.669282588f, + 0.745057785f, -0.666999922f, + 0.747100606f, -0.664710978f, + 0.749136395f, -0.662415778f, + 0.751165132f, -0.660114342f, + 0.753186799f, -0.657806693f, + 0.755201377f, -0.655492853f, + 0.757208847f, -0.653172843f, + 0.759209189f, -0.650846685f, + 0.761202385f, -0.648514401f, + 0.763188417f, -0.646176013f, + 0.765167266f, -0.643831543f, + 0.767138912f, -0.641481013f, + 0.769103338f, -0.639124445f, + 0.771060524f, -0.636761861f, + 0.773010453f, -0.634393284f, + 0.774953107f, -0.632018736f, + 0.776888466f, -0.629638239f, + 0.778816512f, -0.627251815f, + 0.780737229f, -0.624859488f, + 0.782650596f, -0.622461279f, + 0.784556597f, -0.620057212f, + 0.786455214f, -0.617647308f, + 0.788346428f, -0.615231591f, + 0.790230221f, -0.612810082f, + 0.792106577f, -0.610382806f, + 0.793975478f, -0.607949785f, + 0.795836905f, -0.605511041f, + 0.797690841f, -0.603066599f, + 0.799537269f, -0.600616479f, + 0.801376172f, -0.598160707f, + 0.803207531f, -0.595699304f, + 0.805031331f, -0.593232295f, + 0.806847554f, -0.590759702f, + 0.808656182f, -0.588281548f, + 0.810457198f, -0.585797857f, + 0.812250587f, -0.583308653f, + 0.814036330f, -0.580813958f, + 0.815814411f, -0.578313796f, + 0.817584813f, -0.575808191f, + 0.819347520f, -0.573297167f, + 0.821102515f, -0.570780746f, + 0.822849781f, -0.568258953f, + 0.824589303f, -0.565731811f, + 0.826321063f, -0.563199344f, + 0.828045045f, -0.560661576f, + 0.829761234f, -0.558118531f, + 0.831469612f, -0.555570233f, + 0.833170165f, -0.553016706f, + 0.834862875f, -0.550457973f, + 0.836547727f, -0.547894059f, + 0.838224706f, -0.545324988f, + 0.839893794f, -0.542750785f, + 0.841554977f, -0.540171473f, + 0.843208240f, -0.537587076f, + 0.844853565f, -0.534997620f, + 0.846490939f, -0.532403128f, + 0.848120345f, -0.529803625f, + 0.849741768f, -0.527199135f, + 0.851355193f, -0.524589683f, + 0.852960605f, -0.521975293f, + 0.854557988f, -0.519355990f, + 0.856147328f, -0.516731799f, + 0.857728610f, -0.514102744f, + 0.859301818f, -0.511468850f, + 0.860866939f, -0.508830143f, + 0.862423956f, -0.506186645f, + 0.863972856f, -0.503538384f, + 0.865513624f, -0.500885383f, + 0.867046246f, -0.498227667f, + 0.868570706f, -0.495565262f, + 0.870086991f, -0.492898192f, + 0.871595087f, -0.490226483f, + 0.873094978f, -0.487550160f, + 0.874586652f, -0.484869248f, + 0.876070094f, -0.482183772f, + 0.877545290f, -0.479493758f, + 0.879012226f, -0.476799230f, + 0.880470889f, -0.474100215f, + 0.881921264f, -0.471396737f, + 0.883363339f, -0.468688822f, + 0.884797098f, -0.465976496f, + 0.886222530f, -0.463259784f, + 0.887639620f, -0.460538711f, + 0.889048356f, -0.457813304f, + 0.890448723f, -0.455083587f, + 0.891840709f, -0.452349587f, + 0.893224301f, -0.449611330f, + 0.894599486f, -0.446868840f, + 0.895966250f, -0.444122145f, + 0.897324581f, -0.441371269f, + 0.898674466f, -0.438616239f, + 0.900015892f, -0.435857080f, + 0.901348847f, -0.433093819f, + 0.902673318f, -0.430326481f, + 0.903989293f, -0.427555093f, + 0.905296759f, -0.424779681f, + 0.906595705f, -0.422000271f, + 0.907886116f, -0.419216888f, + 0.909167983f, -0.416429560f, + 0.910441292f, -0.413638312f, + 0.911706032f, -0.410843171f, + 0.912962190f, -0.408044163f, + 0.914209756f, -0.405241314f, + 0.915448716f, -0.402434651f, + 0.916679060f, -0.399624200f, + 0.917900776f, -0.396809987f, + 0.919113852f, -0.393992040f, + 0.920318277f, -0.391170384f, + 0.921514039f, -0.388345047f, + 0.922701128f, -0.385516054f, + 0.923879533f, -0.382683432f, + 0.925049241f, -0.379847209f, + 0.926210242f, -0.377007410f, + 0.927362526f, -0.374164063f, + 0.928506080f, -0.371317194f, + 0.929640896f, -0.368466830f, + 0.930766961f, -0.365612998f, + 0.931884266f, -0.362755724f, + 0.932992799f, -0.359895037f, + 0.934092550f, -0.357030961f, + 0.935183510f, -0.354163525f, + 0.936265667f, -0.351292756f, + 0.937339012f, -0.348418680f, + 0.938403534f, -0.345541325f, + 0.939459224f, -0.342660717f, + 0.940506071f, -0.339776884f, + 0.941544065f, -0.336889853f, + 0.942573198f, -0.333999651f, + 0.943593458f, -0.331106306f, + 0.944604837f, -0.328209844f, + 0.945607325f, -0.325310292f, + 0.946600913f, -0.322407679f, + 0.947585591f, -0.319502031f, + 0.948561350f, -0.316593376f, + 0.949528181f, -0.313681740f, + 0.950486074f, -0.310767153f, + 0.951435021f, -0.307849640f, + 0.952375013f, -0.304929230f, + 0.953306040f, -0.302005949f, + 0.954228095f, -0.299079826f, + 0.955141168f, -0.296150888f, + 0.956045251f, -0.293219163f, + 0.956940336f, -0.290284677f, + 0.957826413f, -0.287347460f, + 0.958703475f, -0.284407537f, + 0.959571513f, -0.281464938f, + 0.960430519f, -0.278519689f, + 0.961280486f, -0.275571819f, + 0.962121404f, -0.272621355f, + 0.962953267f, -0.269668326f, + 0.963776066f, -0.266712757f, + 0.964589793f, -0.263754679f, + 0.965394442f, -0.260794118f, + 0.966190003f, -0.257831102f, + 0.966976471f, -0.254865660f, + 0.967753837f, -0.251897818f, + 0.968522094f, -0.248927606f, + 0.969281235f, -0.245955050f, + 0.970031253f, -0.242980180f, + 0.970772141f, -0.240003022f, + 0.971503891f, -0.237023606f, + 0.972226497f, -0.234041959f, + 0.972939952f, -0.231058108f, + 0.973644250f, -0.228072083f, + 0.974339383f, -0.225083911f, + 0.975025345f, -0.222093621f, + 0.975702130f, -0.219101240f, + 0.976369731f, -0.216106797f, + 0.977028143f, -0.213110320f, + 0.977677358f, -0.210111837f, + 0.978317371f, -0.207111376f, + 0.978948175f, -0.204108966f, + 0.979569766f, -0.201104635f, + 0.980182136f, -0.198098411f, + 0.980785280f, -0.195090322f, + 0.981379193f, -0.192080397f, + 0.981963869f, -0.189068664f, + 0.982539302f, -0.186055152f, + 0.983105487f, -0.183039888f, + 0.983662419f, -0.180022901f, + 0.984210092f, -0.177004220f, + 0.984748502f, -0.173983873f, + 0.985277642f, -0.170961889f, + 0.985797509f, -0.167938295f, + 0.986308097f, -0.164913120f, + 0.986809402f, -0.161886394f, + 0.987301418f, -0.158858143f, + 0.987784142f, -0.155828398f, + 0.988257568f, -0.152797185f, + 0.988721692f, -0.149764535f, + 0.989176510f, -0.146730474f, + 0.989622017f, -0.143695033f, + 0.990058210f, -0.140658239f, + 0.990485084f, -0.137620122f, + 0.990902635f, -0.134580709f, + 0.991310860f, -0.131540029f, + 0.991709754f, -0.128498111f, + 0.992099313f, -0.125454983f, + 0.992479535f, -0.122410675f, + 0.992850414f, -0.119365215f, + 0.993211949f, -0.116318631f, + 0.993564136f, -0.113270952f, + 0.993906970f, -0.110222207f, + 0.994240449f, -0.107172425f, + 0.994564571f, -0.104121634f, + 0.994879331f, -0.101069863f, + 0.995184727f, -0.098017140f, + 0.995480755f, -0.094963495f, + 0.995767414f, -0.091908956f, + 0.996044701f, -0.088853553f, + 0.996312612f, -0.085797312f, + 0.996571146f, -0.082740265f, + 0.996820299f, -0.079682438f, + 0.997060070f, -0.076623861f, + 0.997290457f, -0.073564564f, + 0.997511456f, -0.070504573f, + 0.997723067f, -0.067443920f, + 0.997925286f, -0.064382631f, + 0.998118113f, -0.061320736f, + 0.998301545f, -0.058258265f, + 0.998475581f, -0.055195244f, + 0.998640218f, -0.052131705f, + 0.998795456f, -0.049067674f, + 0.998941293f, -0.046003182f, + 0.999077728f, -0.042938257f, + 0.999204759f, -0.039872928f, + 0.999322385f, -0.036807223f, + 0.999430605f, -0.033741172f, + 0.999529418f, -0.030674803f, + 0.999618822f, -0.027608146f, + 0.999698819f, -0.024541229f, + 0.999769405f, -0.021474080f, + 0.999830582f, -0.018406730f, + 0.999882347f, -0.015339206f, + 0.999924702f, -0.012271538f, + 0.999957645f, -0.009203755f, + 0.999981175f, -0.006135885f, + 0.999995294f, -0.003067957f +}; + +/** +* \par +* Example code for Floating-point Twiddle factors Generation: +* \par +*
for(i = 0; i< N/; i++)    
+* {    
+*	twiddleCoef[2*i]= cos(i * 2*PI/(float)N);    
+*	twiddleCoef[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 4096 and PI = 3.14159265358979 +* \par +* Cos and Sin values are in interleaved fashion +* +*/ +const float32_t twiddleCoef_4096[8192] = { + 1.000000000f, 0.000000000f, + 0.999998823f, 0.001533980f, + 0.999995294f, 0.003067957f, + 0.999989411f, 0.004601926f, + 0.999981175f, 0.006135885f, + 0.999970586f, 0.007669829f, + 0.999957645f, 0.009203755f, + 0.999942350f, 0.010737659f, + 0.999924702f, 0.012271538f, + 0.999904701f, 0.013805389f, + 0.999882347f, 0.015339206f, + 0.999857641f, 0.016872988f, + 0.999830582f, 0.018406730f, + 0.999801170f, 0.019940429f, + 0.999769405f, 0.021474080f, + 0.999735288f, 0.023007681f, + 0.999698819f, 0.024541229f, + 0.999659997f, 0.026074718f, + 0.999618822f, 0.027608146f, + 0.999575296f, 0.029141509f, + 0.999529418f, 0.030674803f, + 0.999481187f, 0.032208025f, + 0.999430605f, 0.033741172f, + 0.999377670f, 0.035274239f, + 0.999322385f, 0.036807223f, + 0.999264747f, 0.038340120f, + 0.999204759f, 0.039872928f, + 0.999142419f, 0.041405641f, + 0.999077728f, 0.042938257f, + 0.999010686f, 0.044470772f, + 0.998941293f, 0.046003182f, + 0.998869550f, 0.047535484f, + 0.998795456f, 0.049067674f, + 0.998719012f, 0.050599749f, + 0.998640218f, 0.052131705f, + 0.998559074f, 0.053663538f, + 0.998475581f, 0.055195244f, + 0.998389737f, 0.056726821f, + 0.998301545f, 0.058258265f, + 0.998211003f, 0.059789571f, + 0.998118113f, 0.061320736f, + 0.998022874f, 0.062851758f, + 0.997925286f, 0.064382631f, + 0.997825350f, 0.065913353f, + 0.997723067f, 0.067443920f, + 0.997618435f, 0.068974328f, + 0.997511456f, 0.070504573f, + 0.997402130f, 0.072034653f, + 0.997290457f, 0.073564564f, + 0.997176437f, 0.075094301f, + 0.997060070f, 0.076623861f, + 0.996941358f, 0.078153242f, + 0.996820299f, 0.079682438f, + 0.996696895f, 0.081211447f, + 0.996571146f, 0.082740265f, + 0.996443051f, 0.084268888f, + 0.996312612f, 0.085797312f, + 0.996179829f, 0.087325535f, + 0.996044701f, 0.088853553f, + 0.995907229f, 0.090381361f, + 0.995767414f, 0.091908956f, + 0.995625256f, 0.093436336f, + 0.995480755f, 0.094963495f, + 0.995333912f, 0.096490431f, + 0.995184727f, 0.098017140f, + 0.995033199f, 0.099543619f, + 0.994879331f, 0.101069863f, + 0.994723121f, 0.102595869f, + 0.994564571f, 0.104121634f, + 0.994403680f, 0.105647154f, + 0.994240449f, 0.107172425f, + 0.994074879f, 0.108697444f, + 0.993906970f, 0.110222207f, + 0.993736722f, 0.111746711f, + 0.993564136f, 0.113270952f, + 0.993389211f, 0.114794927f, + 0.993211949f, 0.116318631f, + 0.993032350f, 0.117842062f, + 0.992850414f, 0.119365215f, + 0.992666142f, 0.120888087f, + 0.992479535f, 0.122410675f, + 0.992290591f, 0.123932975f, + 0.992099313f, 0.125454983f, + 0.991905700f, 0.126976696f, + 0.991709754f, 0.128498111f, + 0.991511473f, 0.130019223f, + 0.991310860f, 0.131540029f, + 0.991107914f, 0.133060525f, + 0.990902635f, 0.134580709f, + 0.990695025f, 0.136100575f, + 0.990485084f, 0.137620122f, + 0.990272812f, 0.139139344f, + 0.990058210f, 0.140658239f, + 0.989841278f, 0.142176804f, + 0.989622017f, 0.143695033f, + 0.989400428f, 0.145212925f, + 0.989176510f, 0.146730474f, + 0.988950265f, 0.148247679f, + 0.988721692f, 0.149764535f, + 0.988490793f, 0.151281038f, + 0.988257568f, 0.152797185f, + 0.988022017f, 0.154312973f, + 0.987784142f, 0.155828398f, + 0.987543942f, 0.157343456f, + 0.987301418f, 0.158858143f, + 0.987056571f, 0.160372457f, + 0.986809402f, 0.161886394f, + 0.986559910f, 0.163399949f, + 0.986308097f, 0.164913120f, + 0.986053963f, 0.166425904f, + 0.985797509f, 0.167938295f, + 0.985538735f, 0.169450291f, + 0.985277642f, 0.170961889f, + 0.985014231f, 0.172473084f, + 0.984748502f, 0.173983873f, + 0.984480455f, 0.175494253f, + 0.984210092f, 0.177004220f, + 0.983937413f, 0.178513771f, + 0.983662419f, 0.180022901f, + 0.983385110f, 0.181531608f, + 0.983105487f, 0.183039888f, + 0.982823551f, 0.184547737f, + 0.982539302f, 0.186055152f, + 0.982252741f, 0.187562129f, + 0.981963869f, 0.189068664f, + 0.981672686f, 0.190574755f, + 0.981379193f, 0.192080397f, + 0.981083391f, 0.193585587f, + 0.980785280f, 0.195090322f, + 0.980484862f, 0.196594598f, + 0.980182136f, 0.198098411f, + 0.979877104f, 0.199601758f, + 0.979569766f, 0.201104635f, + 0.979260123f, 0.202607039f, + 0.978948175f, 0.204108966f, + 0.978633924f, 0.205610413f, + 0.978317371f, 0.207111376f, + 0.977998515f, 0.208611852f, + 0.977677358f, 0.210111837f, + 0.977353900f, 0.211611327f, + 0.977028143f, 0.213110320f, + 0.976700086f, 0.214608811f, + 0.976369731f, 0.216106797f, + 0.976037079f, 0.217604275f, + 0.975702130f, 0.219101240f, + 0.975364885f, 0.220597690f, + 0.975025345f, 0.222093621f, + 0.974683511f, 0.223589029f, + 0.974339383f, 0.225083911f, + 0.973992962f, 0.226578264f, + 0.973644250f, 0.228072083f, + 0.973293246f, 0.229565366f, + 0.972939952f, 0.231058108f, + 0.972584369f, 0.232550307f, + 0.972226497f, 0.234041959f, + 0.971866337f, 0.235533059f, + 0.971503891f, 0.237023606f, + 0.971139158f, 0.238513595f, + 0.970772141f, 0.240003022f, + 0.970402839f, 0.241491885f, + 0.970031253f, 0.242980180f, + 0.969657385f, 0.244467903f, + 0.969281235f, 0.245955050f, + 0.968902805f, 0.247441619f, + 0.968522094f, 0.248927606f, + 0.968139105f, 0.250413007f, + 0.967753837f, 0.251897818f, + 0.967366292f, 0.253382037f, + 0.966976471f, 0.254865660f, + 0.966584374f, 0.256348682f, + 0.966190003f, 0.257831102f, + 0.965793359f, 0.259312915f, + 0.965394442f, 0.260794118f, + 0.964993253f, 0.262274707f, + 0.964589793f, 0.263754679f, + 0.964184064f, 0.265234030f, + 0.963776066f, 0.266712757f, + 0.963365800f, 0.268190857f, + 0.962953267f, 0.269668326f, + 0.962538468f, 0.271145160f, + 0.962121404f, 0.272621355f, + 0.961702077f, 0.274096910f, + 0.961280486f, 0.275571819f, + 0.960856633f, 0.277046080f, + 0.960430519f, 0.278519689f, + 0.960002146f, 0.279992643f, + 0.959571513f, 0.281464938f, + 0.959138622f, 0.282936570f, + 0.958703475f, 0.284407537f, + 0.958266071f, 0.285877835f, + 0.957826413f, 0.287347460f, + 0.957384501f, 0.288816408f, + 0.956940336f, 0.290284677f, + 0.956493919f, 0.291752263f, + 0.956045251f, 0.293219163f, + 0.955594334f, 0.294685372f, + 0.955141168f, 0.296150888f, + 0.954685755f, 0.297615707f, + 0.954228095f, 0.299079826f, + 0.953768190f, 0.300543241f, + 0.953306040f, 0.302005949f, + 0.952841648f, 0.303467947f, + 0.952375013f, 0.304929230f, + 0.951906137f, 0.306389795f, + 0.951435021f, 0.307849640f, + 0.950961666f, 0.309308760f, + 0.950486074f, 0.310767153f, + 0.950008245f, 0.312224814f, + 0.949528181f, 0.313681740f, + 0.949045882f, 0.315137929f, + 0.948561350f, 0.316593376f, + 0.948074586f, 0.318048077f, + 0.947585591f, 0.319502031f, + 0.947094366f, 0.320955232f, + 0.946600913f, 0.322407679f, + 0.946105232f, 0.323859367f, + 0.945607325f, 0.325310292f, + 0.945107193f, 0.326760452f, + 0.944604837f, 0.328209844f, + 0.944100258f, 0.329658463f, + 0.943593458f, 0.331106306f, + 0.943084437f, 0.332553370f, + 0.942573198f, 0.333999651f, + 0.942059740f, 0.335445147f, + 0.941544065f, 0.336889853f, + 0.941026175f, 0.338333767f, + 0.940506071f, 0.339776884f, + 0.939983753f, 0.341219202f, + 0.939459224f, 0.342660717f, + 0.938932484f, 0.344101426f, + 0.938403534f, 0.345541325f, + 0.937872376f, 0.346980411f, + 0.937339012f, 0.348418680f, + 0.936803442f, 0.349856130f, + 0.936265667f, 0.351292756f, + 0.935725689f, 0.352728556f, + 0.935183510f, 0.354163525f, + 0.934639130f, 0.355597662f, + 0.934092550f, 0.357030961f, + 0.933543773f, 0.358463421f, + 0.932992799f, 0.359895037f, + 0.932439629f, 0.361325806f, + 0.931884266f, 0.362755724f, + 0.931326709f, 0.364184790f, + 0.930766961f, 0.365612998f, + 0.930205023f, 0.367040346f, + 0.929640896f, 0.368466830f, + 0.929074581f, 0.369892447f, + 0.928506080f, 0.371317194f, + 0.927935395f, 0.372741067f, + 0.927362526f, 0.374164063f, + 0.926787474f, 0.375586178f, + 0.926210242f, 0.377007410f, + 0.925630831f, 0.378427755f, + 0.925049241f, 0.379847209f, + 0.924465474f, 0.381265769f, + 0.923879533f, 0.382683432f, + 0.923291417f, 0.384100195f, + 0.922701128f, 0.385516054f, + 0.922108669f, 0.386931006f, + 0.921514039f, 0.388345047f, + 0.920917242f, 0.389758174f, + 0.920318277f, 0.391170384f, + 0.919717146f, 0.392581674f, + 0.919113852f, 0.393992040f, + 0.918508394f, 0.395401479f, + 0.917900776f, 0.396809987f, + 0.917290997f, 0.398217562f, + 0.916679060f, 0.399624200f, + 0.916064966f, 0.401029897f, + 0.915448716f, 0.402434651f, + 0.914830312f, 0.403838458f, + 0.914209756f, 0.405241314f, + 0.913587048f, 0.406643217f, + 0.912962190f, 0.408044163f, + 0.912335185f, 0.409444149f, + 0.911706032f, 0.410843171f, + 0.911074734f, 0.412241227f, + 0.910441292f, 0.413638312f, + 0.909805708f, 0.415034424f, + 0.909167983f, 0.416429560f, + 0.908528119f, 0.417823716f, + 0.907886116f, 0.419216888f, + 0.907241978f, 0.420609074f, + 0.906595705f, 0.422000271f, + 0.905947298f, 0.423390474f, + 0.905296759f, 0.424779681f, + 0.904644091f, 0.426167889f, + 0.903989293f, 0.427555093f, + 0.903332368f, 0.428941292f, + 0.902673318f, 0.430326481f, + 0.902012144f, 0.431710658f, + 0.901348847f, 0.433093819f, + 0.900683429f, 0.434475961f, + 0.900015892f, 0.435857080f, + 0.899346237f, 0.437237174f, + 0.898674466f, 0.438616239f, + 0.898000580f, 0.439994271f, + 0.897324581f, 0.441371269f, + 0.896646470f, 0.442747228f, + 0.895966250f, 0.444122145f, + 0.895283921f, 0.445496017f, + 0.894599486f, 0.446868840f, + 0.893912945f, 0.448240612f, + 0.893224301f, 0.449611330f, + 0.892533555f, 0.450980989f, + 0.891840709f, 0.452349587f, + 0.891145765f, 0.453717121f, + 0.890448723f, 0.455083587f, + 0.889749586f, 0.456448982f, + 0.889048356f, 0.457813304f, + 0.888345033f, 0.459176548f, + 0.887639620f, 0.460538711f, + 0.886932119f, 0.461899791f, + 0.886222530f, 0.463259784f, + 0.885510856f, 0.464618686f, + 0.884797098f, 0.465976496f, + 0.884081259f, 0.467333209f, + 0.883363339f, 0.468688822f, + 0.882643340f, 0.470043332f, + 0.881921264f, 0.471396737f, + 0.881197113f, 0.472749032f, + 0.880470889f, 0.474100215f, + 0.879742593f, 0.475450282f, + 0.879012226f, 0.476799230f, + 0.878279792f, 0.478147056f, + 0.877545290f, 0.479493758f, + 0.876808724f, 0.480839331f, + 0.876070094f, 0.482183772f, + 0.875329403f, 0.483527079f, + 0.874586652f, 0.484869248f, + 0.873841843f, 0.486210276f, + 0.873094978f, 0.487550160f, + 0.872346059f, 0.488888897f, + 0.871595087f, 0.490226483f, + 0.870842063f, 0.491562916f, + 0.870086991f, 0.492898192f, + 0.869329871f, 0.494232309f, + 0.868570706f, 0.495565262f, + 0.867809497f, 0.496897049f, + 0.867046246f, 0.498227667f, + 0.866280954f, 0.499557113f, + 0.865513624f, 0.500885383f, + 0.864744258f, 0.502212474f, + 0.863972856f, 0.503538384f, + 0.863199422f, 0.504863109f, + 0.862423956f, 0.506186645f, + 0.861646461f, 0.507508991f, + 0.860866939f, 0.508830143f, + 0.860085390f, 0.510150097f, + 0.859301818f, 0.511468850f, + 0.858516224f, 0.512786401f, + 0.857728610f, 0.514102744f, + 0.856938977f, 0.515417878f, + 0.856147328f, 0.516731799f, + 0.855353665f, 0.518044504f, + 0.854557988f, 0.519355990f, + 0.853760301f, 0.520666254f, + 0.852960605f, 0.521975293f, + 0.852158902f, 0.523283103f, + 0.851355193f, 0.524589683f, + 0.850549481f, 0.525895027f, + 0.849741768f, 0.527199135f, + 0.848932055f, 0.528502002f, + 0.848120345f, 0.529803625f, + 0.847306639f, 0.531104001f, + 0.846490939f, 0.532403128f, + 0.845673247f, 0.533701002f, + 0.844853565f, 0.534997620f, + 0.844031895f, 0.536292979f, + 0.843208240f, 0.537587076f, + 0.842382600f, 0.538879909f, + 0.841554977f, 0.540171473f, + 0.840725375f, 0.541461766f, + 0.839893794f, 0.542750785f, + 0.839060237f, 0.544038527f, + 0.838224706f, 0.545324988f, + 0.837387202f, 0.546610167f, + 0.836547727f, 0.547894059f, + 0.835706284f, 0.549176662f, + 0.834862875f, 0.550457973f, + 0.834017501f, 0.551737988f, + 0.833170165f, 0.553016706f, + 0.832320868f, 0.554294121f, + 0.831469612f, 0.555570233f, + 0.830616400f, 0.556845037f, + 0.829761234f, 0.558118531f, + 0.828904115f, 0.559390712f, + 0.828045045f, 0.560661576f, + 0.827184027f, 0.561931121f, + 0.826321063f, 0.563199344f, + 0.825456154f, 0.564466242f, + 0.824589303f, 0.565731811f, + 0.823720511f, 0.566996049f, + 0.822849781f, 0.568258953f, + 0.821977115f, 0.569520519f, + 0.821102515f, 0.570780746f, + 0.820225983f, 0.572039629f, + 0.819347520f, 0.573297167f, + 0.818467130f, 0.574553355f, + 0.817584813f, 0.575808191f, + 0.816700573f, 0.577061673f, + 0.815814411f, 0.578313796f, + 0.814926329f, 0.579564559f, + 0.814036330f, 0.580813958f, + 0.813144415f, 0.582061990f, + 0.812250587f, 0.583308653f, + 0.811354847f, 0.584553943f, + 0.810457198f, 0.585797857f, + 0.809557642f, 0.587040394f, + 0.808656182f, 0.588281548f, + 0.807752818f, 0.589521319f, + 0.806847554f, 0.590759702f, + 0.805940391f, 0.591996695f, + 0.805031331f, 0.593232295f, + 0.804120377f, 0.594466499f, + 0.803207531f, 0.595699304f, + 0.802292796f, 0.596930708f, + 0.801376172f, 0.598160707f, + 0.800457662f, 0.599389298f, + 0.799537269f, 0.600616479f, + 0.798614995f, 0.601842247f, + 0.797690841f, 0.603066599f, + 0.796764810f, 0.604289531f, + 0.795836905f, 0.605511041f, + 0.794907126f, 0.606731127f, + 0.793975478f, 0.607949785f, + 0.793041960f, 0.609167012f, + 0.792106577f, 0.610382806f, + 0.791169330f, 0.611597164f, + 0.790230221f, 0.612810082f, + 0.789289253f, 0.614021559f, + 0.788346428f, 0.615231591f, + 0.787401747f, 0.616440175f, + 0.786455214f, 0.617647308f, + 0.785506830f, 0.618852988f, + 0.784556597f, 0.620057212f, + 0.783604519f, 0.621259977f, + 0.782650596f, 0.622461279f, + 0.781694832f, 0.623661118f, + 0.780737229f, 0.624859488f, + 0.779777788f, 0.626056388f, + 0.778816512f, 0.627251815f, + 0.777853404f, 0.628445767f, + 0.776888466f, 0.629638239f, + 0.775921699f, 0.630829230f, + 0.774953107f, 0.632018736f, + 0.773982691f, 0.633206755f, + 0.773010453f, 0.634393284f, + 0.772036397f, 0.635578320f, + 0.771060524f, 0.636761861f, + 0.770082837f, 0.637943904f, + 0.769103338f, 0.639124445f, + 0.768122029f, 0.640303482f, + 0.767138912f, 0.641481013f, + 0.766153990f, 0.642657034f, + 0.765167266f, 0.643831543f, + 0.764178741f, 0.645004537f, + 0.763188417f, 0.646176013f, + 0.762196298f, 0.647345969f, + 0.761202385f, 0.648514401f, + 0.760206682f, 0.649681307f, + 0.759209189f, 0.650846685f, + 0.758209910f, 0.652010531f, + 0.757208847f, 0.653172843f, + 0.756206001f, 0.654333618f, + 0.755201377f, 0.655492853f, + 0.754194975f, 0.656650546f, + 0.753186799f, 0.657806693f, + 0.752176850f, 0.658961293f, + 0.751165132f, 0.660114342f, + 0.750151646f, 0.661265838f, + 0.749136395f, 0.662415778f, + 0.748119380f, 0.663564159f, + 0.747100606f, 0.664710978f, + 0.746080074f, 0.665856234f, + 0.745057785f, 0.666999922f, + 0.744033744f, 0.668142041f, + 0.743007952f, 0.669282588f, + 0.741980412f, 0.670421560f, + 0.740951125f, 0.671558955f, + 0.739920095f, 0.672694769f, + 0.738887324f, 0.673829000f, + 0.737852815f, 0.674961646f, + 0.736816569f, 0.676092704f, + 0.735778589f, 0.677222170f, + 0.734738878f, 0.678350043f, + 0.733697438f, 0.679476320f, + 0.732654272f, 0.680600998f, + 0.731609381f, 0.681724074f, + 0.730562769f, 0.682845546f, + 0.729514438f, 0.683965412f, + 0.728464390f, 0.685083668f, + 0.727412629f, 0.686200312f, + 0.726359155f, 0.687315341f, + 0.725303972f, 0.688428753f, + 0.724247083f, 0.689540545f, + 0.723188489f, 0.690650714f, + 0.722128194f, 0.691759258f, + 0.721066199f, 0.692866175f, + 0.720002508f, 0.693971461f, + 0.718937122f, 0.695075114f, + 0.717870045f, 0.696177131f, + 0.716801279f, 0.697277511f, + 0.715730825f, 0.698376249f, + 0.714658688f, 0.699473345f, + 0.713584869f, 0.700568794f, + 0.712509371f, 0.701662595f, + 0.711432196f, 0.702754744f, + 0.710353347f, 0.703845241f, + 0.709272826f, 0.704934080f, + 0.708190637f, 0.706021261f, + 0.707106781f, 0.707106781f, + 0.706021261f, 0.708190637f, + 0.704934080f, 0.709272826f, + 0.703845241f, 0.710353347f, + 0.702754744f, 0.711432196f, + 0.701662595f, 0.712509371f, + 0.700568794f, 0.713584869f, + 0.699473345f, 0.714658688f, + 0.698376249f, 0.715730825f, + 0.697277511f, 0.716801279f, + 0.696177131f, 0.717870045f, + 0.695075114f, 0.718937122f, + 0.693971461f, 0.720002508f, + 0.692866175f, 0.721066199f, + 0.691759258f, 0.722128194f, + 0.690650714f, 0.723188489f, + 0.689540545f, 0.724247083f, + 0.688428753f, 0.725303972f, + 0.687315341f, 0.726359155f, + 0.686200312f, 0.727412629f, + 0.685083668f, 0.728464390f, + 0.683965412f, 0.729514438f, + 0.682845546f, 0.730562769f, + 0.681724074f, 0.731609381f, + 0.680600998f, 0.732654272f, + 0.679476320f, 0.733697438f, + 0.678350043f, 0.734738878f, + 0.677222170f, 0.735778589f, + 0.676092704f, 0.736816569f, + 0.674961646f, 0.737852815f, + 0.673829000f, 0.738887324f, + 0.672694769f, 0.739920095f, + 0.671558955f, 0.740951125f, + 0.670421560f, 0.741980412f, + 0.669282588f, 0.743007952f, + 0.668142041f, 0.744033744f, + 0.666999922f, 0.745057785f, + 0.665856234f, 0.746080074f, + 0.664710978f, 0.747100606f, + 0.663564159f, 0.748119380f, + 0.662415778f, 0.749136395f, + 0.661265838f, 0.750151646f, + 0.660114342f, 0.751165132f, + 0.658961293f, 0.752176850f, + 0.657806693f, 0.753186799f, + 0.656650546f, 0.754194975f, + 0.655492853f, 0.755201377f, + 0.654333618f, 0.756206001f, + 0.653172843f, 0.757208847f, + 0.652010531f, 0.758209910f, + 0.650846685f, 0.759209189f, + 0.649681307f, 0.760206682f, + 0.648514401f, 0.761202385f, + 0.647345969f, 0.762196298f, + 0.646176013f, 0.763188417f, + 0.645004537f, 0.764178741f, + 0.643831543f, 0.765167266f, + 0.642657034f, 0.766153990f, + 0.641481013f, 0.767138912f, + 0.640303482f, 0.768122029f, + 0.639124445f, 0.769103338f, + 0.637943904f, 0.770082837f, + 0.636761861f, 0.771060524f, + 0.635578320f, 0.772036397f, + 0.634393284f, 0.773010453f, + 0.633206755f, 0.773982691f, + 0.632018736f, 0.774953107f, + 0.630829230f, 0.775921699f, + 0.629638239f, 0.776888466f, + 0.628445767f, 0.777853404f, + 0.627251815f, 0.778816512f, + 0.626056388f, 0.779777788f, + 0.624859488f, 0.780737229f, + 0.623661118f, 0.781694832f, + 0.622461279f, 0.782650596f, + 0.621259977f, 0.783604519f, + 0.620057212f, 0.784556597f, + 0.618852988f, 0.785506830f, + 0.617647308f, 0.786455214f, + 0.616440175f, 0.787401747f, + 0.615231591f, 0.788346428f, + 0.614021559f, 0.789289253f, + 0.612810082f, 0.790230221f, + 0.611597164f, 0.791169330f, + 0.610382806f, 0.792106577f, + 0.609167012f, 0.793041960f, + 0.607949785f, 0.793975478f, + 0.606731127f, 0.794907126f, + 0.605511041f, 0.795836905f, + 0.604289531f, 0.796764810f, + 0.603066599f, 0.797690841f, + 0.601842247f, 0.798614995f, + 0.600616479f, 0.799537269f, + 0.599389298f, 0.800457662f, + 0.598160707f, 0.801376172f, + 0.596930708f, 0.802292796f, + 0.595699304f, 0.803207531f, + 0.594466499f, 0.804120377f, + 0.593232295f, 0.805031331f, + 0.591996695f, 0.805940391f, + 0.590759702f, 0.806847554f, + 0.589521319f, 0.807752818f, + 0.588281548f, 0.808656182f, + 0.587040394f, 0.809557642f, + 0.585797857f, 0.810457198f, + 0.584553943f, 0.811354847f, + 0.583308653f, 0.812250587f, + 0.582061990f, 0.813144415f, + 0.580813958f, 0.814036330f, + 0.579564559f, 0.814926329f, + 0.578313796f, 0.815814411f, + 0.577061673f, 0.816700573f, + 0.575808191f, 0.817584813f, + 0.574553355f, 0.818467130f, + 0.573297167f, 0.819347520f, + 0.572039629f, 0.820225983f, + 0.570780746f, 0.821102515f, + 0.569520519f, 0.821977115f, + 0.568258953f, 0.822849781f, + 0.566996049f, 0.823720511f, + 0.565731811f, 0.824589303f, + 0.564466242f, 0.825456154f, + 0.563199344f, 0.826321063f, + 0.561931121f, 0.827184027f, + 0.560661576f, 0.828045045f, + 0.559390712f, 0.828904115f, + 0.558118531f, 0.829761234f, + 0.556845037f, 0.830616400f, + 0.555570233f, 0.831469612f, + 0.554294121f, 0.832320868f, + 0.553016706f, 0.833170165f, + 0.551737988f, 0.834017501f, + 0.550457973f, 0.834862875f, + 0.549176662f, 0.835706284f, + 0.547894059f, 0.836547727f, + 0.546610167f, 0.837387202f, + 0.545324988f, 0.838224706f, + 0.544038527f, 0.839060237f, + 0.542750785f, 0.839893794f, + 0.541461766f, 0.840725375f, + 0.540171473f, 0.841554977f, + 0.538879909f, 0.842382600f, + 0.537587076f, 0.843208240f, + 0.536292979f, 0.844031895f, + 0.534997620f, 0.844853565f, + 0.533701002f, 0.845673247f, + 0.532403128f, 0.846490939f, + 0.531104001f, 0.847306639f, + 0.529803625f, 0.848120345f, + 0.528502002f, 0.848932055f, + 0.527199135f, 0.849741768f, + 0.525895027f, 0.850549481f, + 0.524589683f, 0.851355193f, + 0.523283103f, 0.852158902f, + 0.521975293f, 0.852960605f, + 0.520666254f, 0.853760301f, + 0.519355990f, 0.854557988f, + 0.518044504f, 0.855353665f, + 0.516731799f, 0.856147328f, + 0.515417878f, 0.856938977f, + 0.514102744f, 0.857728610f, + 0.512786401f, 0.858516224f, + 0.511468850f, 0.859301818f, + 0.510150097f, 0.860085390f, + 0.508830143f, 0.860866939f, + 0.507508991f, 0.861646461f, + 0.506186645f, 0.862423956f, + 0.504863109f, 0.863199422f, + 0.503538384f, 0.863972856f, + 0.502212474f, 0.864744258f, + 0.500885383f, 0.865513624f, + 0.499557113f, 0.866280954f, + 0.498227667f, 0.867046246f, + 0.496897049f, 0.867809497f, + 0.495565262f, 0.868570706f, + 0.494232309f, 0.869329871f, + 0.492898192f, 0.870086991f, + 0.491562916f, 0.870842063f, + 0.490226483f, 0.871595087f, + 0.488888897f, 0.872346059f, + 0.487550160f, 0.873094978f, + 0.486210276f, 0.873841843f, + 0.484869248f, 0.874586652f, + 0.483527079f, 0.875329403f, + 0.482183772f, 0.876070094f, + 0.480839331f, 0.876808724f, + 0.479493758f, 0.877545290f, + 0.478147056f, 0.878279792f, + 0.476799230f, 0.879012226f, + 0.475450282f, 0.879742593f, + 0.474100215f, 0.880470889f, + 0.472749032f, 0.881197113f, + 0.471396737f, 0.881921264f, + 0.470043332f, 0.882643340f, + 0.468688822f, 0.883363339f, + 0.467333209f, 0.884081259f, + 0.465976496f, 0.884797098f, + 0.464618686f, 0.885510856f, + 0.463259784f, 0.886222530f, + 0.461899791f, 0.886932119f, + 0.460538711f, 0.887639620f, + 0.459176548f, 0.888345033f, + 0.457813304f, 0.889048356f, + 0.456448982f, 0.889749586f, + 0.455083587f, 0.890448723f, + 0.453717121f, 0.891145765f, + 0.452349587f, 0.891840709f, + 0.450980989f, 0.892533555f, + 0.449611330f, 0.893224301f, + 0.448240612f, 0.893912945f, + 0.446868840f, 0.894599486f, + 0.445496017f, 0.895283921f, + 0.444122145f, 0.895966250f, + 0.442747228f, 0.896646470f, + 0.441371269f, 0.897324581f, + 0.439994271f, 0.898000580f, + 0.438616239f, 0.898674466f, + 0.437237174f, 0.899346237f, + 0.435857080f, 0.900015892f, + 0.434475961f, 0.900683429f, + 0.433093819f, 0.901348847f, + 0.431710658f, 0.902012144f, + 0.430326481f, 0.902673318f, + 0.428941292f, 0.903332368f, + 0.427555093f, 0.903989293f, + 0.426167889f, 0.904644091f, + 0.424779681f, 0.905296759f, + 0.423390474f, 0.905947298f, + 0.422000271f, 0.906595705f, + 0.420609074f, 0.907241978f, + 0.419216888f, 0.907886116f, + 0.417823716f, 0.908528119f, + 0.416429560f, 0.909167983f, + 0.415034424f, 0.909805708f, + 0.413638312f, 0.910441292f, + 0.412241227f, 0.911074734f, + 0.410843171f, 0.911706032f, + 0.409444149f, 0.912335185f, + 0.408044163f, 0.912962190f, + 0.406643217f, 0.913587048f, + 0.405241314f, 0.914209756f, + 0.403838458f, 0.914830312f, + 0.402434651f, 0.915448716f, + 0.401029897f, 0.916064966f, + 0.399624200f, 0.916679060f, + 0.398217562f, 0.917290997f, + 0.396809987f, 0.917900776f, + 0.395401479f, 0.918508394f, + 0.393992040f, 0.919113852f, + 0.392581674f, 0.919717146f, + 0.391170384f, 0.920318277f, + 0.389758174f, 0.920917242f, + 0.388345047f, 0.921514039f, + 0.386931006f, 0.922108669f, + 0.385516054f, 0.922701128f, + 0.384100195f, 0.923291417f, + 0.382683432f, 0.923879533f, + 0.381265769f, 0.924465474f, + 0.379847209f, 0.925049241f, + 0.378427755f, 0.925630831f, + 0.377007410f, 0.926210242f, + 0.375586178f, 0.926787474f, + 0.374164063f, 0.927362526f, + 0.372741067f, 0.927935395f, + 0.371317194f, 0.928506080f, + 0.369892447f, 0.929074581f, + 0.368466830f, 0.929640896f, + 0.367040346f, 0.930205023f, + 0.365612998f, 0.930766961f, + 0.364184790f, 0.931326709f, + 0.362755724f, 0.931884266f, + 0.361325806f, 0.932439629f, + 0.359895037f, 0.932992799f, + 0.358463421f, 0.933543773f, + 0.357030961f, 0.934092550f, + 0.355597662f, 0.934639130f, + 0.354163525f, 0.935183510f, + 0.352728556f, 0.935725689f, + 0.351292756f, 0.936265667f, + 0.349856130f, 0.936803442f, + 0.348418680f, 0.937339012f, + 0.346980411f, 0.937872376f, + 0.345541325f, 0.938403534f, + 0.344101426f, 0.938932484f, + 0.342660717f, 0.939459224f, + 0.341219202f, 0.939983753f, + 0.339776884f, 0.940506071f, + 0.338333767f, 0.941026175f, + 0.336889853f, 0.941544065f, + 0.335445147f, 0.942059740f, + 0.333999651f, 0.942573198f, + 0.332553370f, 0.943084437f, + 0.331106306f, 0.943593458f, + 0.329658463f, 0.944100258f, + 0.328209844f, 0.944604837f, + 0.326760452f, 0.945107193f, + 0.325310292f, 0.945607325f, + 0.323859367f, 0.946105232f, + 0.322407679f, 0.946600913f, + 0.320955232f, 0.947094366f, + 0.319502031f, 0.947585591f, + 0.318048077f, 0.948074586f, + 0.316593376f, 0.948561350f, + 0.315137929f, 0.949045882f, + 0.313681740f, 0.949528181f, + 0.312224814f, 0.950008245f, + 0.310767153f, 0.950486074f, + 0.309308760f, 0.950961666f, + 0.307849640f, 0.951435021f, + 0.306389795f, 0.951906137f, + 0.304929230f, 0.952375013f, + 0.303467947f, 0.952841648f, + 0.302005949f, 0.953306040f, + 0.300543241f, 0.953768190f, + 0.299079826f, 0.954228095f, + 0.297615707f, 0.954685755f, + 0.296150888f, 0.955141168f, + 0.294685372f, 0.955594334f, + 0.293219163f, 0.956045251f, + 0.291752263f, 0.956493919f, + 0.290284677f, 0.956940336f, + 0.288816408f, 0.957384501f, + 0.287347460f, 0.957826413f, + 0.285877835f, 0.958266071f, + 0.284407537f, 0.958703475f, + 0.282936570f, 0.959138622f, + 0.281464938f, 0.959571513f, + 0.279992643f, 0.960002146f, + 0.278519689f, 0.960430519f, + 0.277046080f, 0.960856633f, + 0.275571819f, 0.961280486f, + 0.274096910f, 0.961702077f, + 0.272621355f, 0.962121404f, + 0.271145160f, 0.962538468f, + 0.269668326f, 0.962953267f, + 0.268190857f, 0.963365800f, + 0.266712757f, 0.963776066f, + 0.265234030f, 0.964184064f, + 0.263754679f, 0.964589793f, + 0.262274707f, 0.964993253f, + 0.260794118f, 0.965394442f, + 0.259312915f, 0.965793359f, + 0.257831102f, 0.966190003f, + 0.256348682f, 0.966584374f, + 0.254865660f, 0.966976471f, + 0.253382037f, 0.967366292f, + 0.251897818f, 0.967753837f, + 0.250413007f, 0.968139105f, + 0.248927606f, 0.968522094f, + 0.247441619f, 0.968902805f, + 0.245955050f, 0.969281235f, + 0.244467903f, 0.969657385f, + 0.242980180f, 0.970031253f, + 0.241491885f, 0.970402839f, + 0.240003022f, 0.970772141f, + 0.238513595f, 0.971139158f, + 0.237023606f, 0.971503891f, + 0.235533059f, 0.971866337f, + 0.234041959f, 0.972226497f, + 0.232550307f, 0.972584369f, + 0.231058108f, 0.972939952f, + 0.229565366f, 0.973293246f, + 0.228072083f, 0.973644250f, + 0.226578264f, 0.973992962f, + 0.225083911f, 0.974339383f, + 0.223589029f, 0.974683511f, + 0.222093621f, 0.975025345f, + 0.220597690f, 0.975364885f, + 0.219101240f, 0.975702130f, + 0.217604275f, 0.976037079f, + 0.216106797f, 0.976369731f, + 0.214608811f, 0.976700086f, + 0.213110320f, 0.977028143f, + 0.211611327f, 0.977353900f, + 0.210111837f, 0.977677358f, + 0.208611852f, 0.977998515f, + 0.207111376f, 0.978317371f, + 0.205610413f, 0.978633924f, + 0.204108966f, 0.978948175f, + 0.202607039f, 0.979260123f, + 0.201104635f, 0.979569766f, + 0.199601758f, 0.979877104f, + 0.198098411f, 0.980182136f, + 0.196594598f, 0.980484862f, + 0.195090322f, 0.980785280f, + 0.193585587f, 0.981083391f, + 0.192080397f, 0.981379193f, + 0.190574755f, 0.981672686f, + 0.189068664f, 0.981963869f, + 0.187562129f, 0.982252741f, + 0.186055152f, 0.982539302f, + 0.184547737f, 0.982823551f, + 0.183039888f, 0.983105487f, + 0.181531608f, 0.983385110f, + 0.180022901f, 0.983662419f, + 0.178513771f, 0.983937413f, + 0.177004220f, 0.984210092f, + 0.175494253f, 0.984480455f, + 0.173983873f, 0.984748502f, + 0.172473084f, 0.985014231f, + 0.170961889f, 0.985277642f, + 0.169450291f, 0.985538735f, + 0.167938295f, 0.985797509f, + 0.166425904f, 0.986053963f, + 0.164913120f, 0.986308097f, + 0.163399949f, 0.986559910f, + 0.161886394f, 0.986809402f, + 0.160372457f, 0.987056571f, + 0.158858143f, 0.987301418f, + 0.157343456f, 0.987543942f, + 0.155828398f, 0.987784142f, + 0.154312973f, 0.988022017f, + 0.152797185f, 0.988257568f, + 0.151281038f, 0.988490793f, + 0.149764535f, 0.988721692f, + 0.148247679f, 0.988950265f, + 0.146730474f, 0.989176510f, + 0.145212925f, 0.989400428f, + 0.143695033f, 0.989622017f, + 0.142176804f, 0.989841278f, + 0.140658239f, 0.990058210f, + 0.139139344f, 0.990272812f, + 0.137620122f, 0.990485084f, + 0.136100575f, 0.990695025f, + 0.134580709f, 0.990902635f, + 0.133060525f, 0.991107914f, + 0.131540029f, 0.991310860f, + 0.130019223f, 0.991511473f, + 0.128498111f, 0.991709754f, + 0.126976696f, 0.991905700f, + 0.125454983f, 0.992099313f, + 0.123932975f, 0.992290591f, + 0.122410675f, 0.992479535f, + 0.120888087f, 0.992666142f, + 0.119365215f, 0.992850414f, + 0.117842062f, 0.993032350f, + 0.116318631f, 0.993211949f, + 0.114794927f, 0.993389211f, + 0.113270952f, 0.993564136f, + 0.111746711f, 0.993736722f, + 0.110222207f, 0.993906970f, + 0.108697444f, 0.994074879f, + 0.107172425f, 0.994240449f, + 0.105647154f, 0.994403680f, + 0.104121634f, 0.994564571f, + 0.102595869f, 0.994723121f, + 0.101069863f, 0.994879331f, + 0.099543619f, 0.995033199f, + 0.098017140f, 0.995184727f, + 0.096490431f, 0.995333912f, + 0.094963495f, 0.995480755f, + 0.093436336f, 0.995625256f, + 0.091908956f, 0.995767414f, + 0.090381361f, 0.995907229f, + 0.088853553f, 0.996044701f, + 0.087325535f, 0.996179829f, + 0.085797312f, 0.996312612f, + 0.084268888f, 0.996443051f, + 0.082740265f, 0.996571146f, + 0.081211447f, 0.996696895f, + 0.079682438f, 0.996820299f, + 0.078153242f, 0.996941358f, + 0.076623861f, 0.997060070f, + 0.075094301f, 0.997176437f, + 0.073564564f, 0.997290457f, + 0.072034653f, 0.997402130f, + 0.070504573f, 0.997511456f, + 0.068974328f, 0.997618435f, + 0.067443920f, 0.997723067f, + 0.065913353f, 0.997825350f, + 0.064382631f, 0.997925286f, + 0.062851758f, 0.998022874f, + 0.061320736f, 0.998118113f, + 0.059789571f, 0.998211003f, + 0.058258265f, 0.998301545f, + 0.056726821f, 0.998389737f, + 0.055195244f, 0.998475581f, + 0.053663538f, 0.998559074f, + 0.052131705f, 0.998640218f, + 0.050599749f, 0.998719012f, + 0.049067674f, 0.998795456f, + 0.047535484f, 0.998869550f, + 0.046003182f, 0.998941293f, + 0.044470772f, 0.999010686f, + 0.042938257f, 0.999077728f, + 0.041405641f, 0.999142419f, + 0.039872928f, 0.999204759f, + 0.038340120f, 0.999264747f, + 0.036807223f, 0.999322385f, + 0.035274239f, 0.999377670f, + 0.033741172f, 0.999430605f, + 0.032208025f, 0.999481187f, + 0.030674803f, 0.999529418f, + 0.029141509f, 0.999575296f, + 0.027608146f, 0.999618822f, + 0.026074718f, 0.999659997f, + 0.024541229f, 0.999698819f, + 0.023007681f, 0.999735288f, + 0.021474080f, 0.999769405f, + 0.019940429f, 0.999801170f, + 0.018406730f, 0.999830582f, + 0.016872988f, 0.999857641f, + 0.015339206f, 0.999882347f, + 0.013805389f, 0.999904701f, + 0.012271538f, 0.999924702f, + 0.010737659f, 0.999942350f, + 0.009203755f, 0.999957645f, + 0.007669829f, 0.999970586f, + 0.006135885f, 0.999981175f, + 0.004601926f, 0.999989411f, + 0.003067957f, 0.999995294f, + 0.001533980f, 0.999998823f, + 0.000000000f, 1.000000000f, + -0.001533980f, 0.999998823f, + -0.003067957f, 0.999995294f, + -0.004601926f, 0.999989411f, + -0.006135885f, 0.999981175f, + -0.007669829f, 0.999970586f, + -0.009203755f, 0.999957645f, + -0.010737659f, 0.999942350f, + -0.012271538f, 0.999924702f, + -0.013805389f, 0.999904701f, + -0.015339206f, 0.999882347f, + -0.016872988f, 0.999857641f, + -0.018406730f, 0.999830582f, + -0.019940429f, 0.999801170f, + -0.021474080f, 0.999769405f, + -0.023007681f, 0.999735288f, + -0.024541229f, 0.999698819f, + -0.026074718f, 0.999659997f, + -0.027608146f, 0.999618822f, + -0.029141509f, 0.999575296f, + -0.030674803f, 0.999529418f, + -0.032208025f, 0.999481187f, + -0.033741172f, 0.999430605f, + -0.035274239f, 0.999377670f, + -0.036807223f, 0.999322385f, + -0.038340120f, 0.999264747f, + -0.039872928f, 0.999204759f, + -0.041405641f, 0.999142419f, + -0.042938257f, 0.999077728f, + -0.044470772f, 0.999010686f, + -0.046003182f, 0.998941293f, + -0.047535484f, 0.998869550f, + -0.049067674f, 0.998795456f, + -0.050599749f, 0.998719012f, + -0.052131705f, 0.998640218f, + -0.053663538f, 0.998559074f, + -0.055195244f, 0.998475581f, + -0.056726821f, 0.998389737f, + -0.058258265f, 0.998301545f, + -0.059789571f, 0.998211003f, + -0.061320736f, 0.998118113f, + -0.062851758f, 0.998022874f, + -0.064382631f, 0.997925286f, + -0.065913353f, 0.997825350f, + -0.067443920f, 0.997723067f, + -0.068974328f, 0.997618435f, + -0.070504573f, 0.997511456f, + -0.072034653f, 0.997402130f, + -0.073564564f, 0.997290457f, + -0.075094301f, 0.997176437f, + -0.076623861f, 0.997060070f, + -0.078153242f, 0.996941358f, + -0.079682438f, 0.996820299f, + -0.081211447f, 0.996696895f, + -0.082740265f, 0.996571146f, + -0.084268888f, 0.996443051f, + -0.085797312f, 0.996312612f, + -0.087325535f, 0.996179829f, + -0.088853553f, 0.996044701f, + -0.090381361f, 0.995907229f, + -0.091908956f, 0.995767414f, + -0.093436336f, 0.995625256f, + -0.094963495f, 0.995480755f, + -0.096490431f, 0.995333912f, + -0.098017140f, 0.995184727f, + -0.099543619f, 0.995033199f, + -0.101069863f, 0.994879331f, + -0.102595869f, 0.994723121f, + -0.104121634f, 0.994564571f, + -0.105647154f, 0.994403680f, + -0.107172425f, 0.994240449f, + -0.108697444f, 0.994074879f, + -0.110222207f, 0.993906970f, + -0.111746711f, 0.993736722f, + -0.113270952f, 0.993564136f, + -0.114794927f, 0.993389211f, + -0.116318631f, 0.993211949f, + -0.117842062f, 0.993032350f, + -0.119365215f, 0.992850414f, + -0.120888087f, 0.992666142f, + -0.122410675f, 0.992479535f, + -0.123932975f, 0.992290591f, + -0.125454983f, 0.992099313f, + -0.126976696f, 0.991905700f, + -0.128498111f, 0.991709754f, + -0.130019223f, 0.991511473f, + -0.131540029f, 0.991310860f, + -0.133060525f, 0.991107914f, + -0.134580709f, 0.990902635f, + -0.136100575f, 0.990695025f, + -0.137620122f, 0.990485084f, + -0.139139344f, 0.990272812f, + -0.140658239f, 0.990058210f, + -0.142176804f, 0.989841278f, + -0.143695033f, 0.989622017f, + -0.145212925f, 0.989400428f, + -0.146730474f, 0.989176510f, + -0.148247679f, 0.988950265f, + -0.149764535f, 0.988721692f, + -0.151281038f, 0.988490793f, + -0.152797185f, 0.988257568f, + -0.154312973f, 0.988022017f, + -0.155828398f, 0.987784142f, + -0.157343456f, 0.987543942f, + -0.158858143f, 0.987301418f, + -0.160372457f, 0.987056571f, + -0.161886394f, 0.986809402f, + -0.163399949f, 0.986559910f, + -0.164913120f, 0.986308097f, + -0.166425904f, 0.986053963f, + -0.167938295f, 0.985797509f, + -0.169450291f, 0.985538735f, + -0.170961889f, 0.985277642f, + -0.172473084f, 0.985014231f, + -0.173983873f, 0.984748502f, + -0.175494253f, 0.984480455f, + -0.177004220f, 0.984210092f, + -0.178513771f, 0.983937413f, + -0.180022901f, 0.983662419f, + -0.181531608f, 0.983385110f, + -0.183039888f, 0.983105487f, + -0.184547737f, 0.982823551f, + -0.186055152f, 0.982539302f, + -0.187562129f, 0.982252741f, + -0.189068664f, 0.981963869f, + -0.190574755f, 0.981672686f, + -0.192080397f, 0.981379193f, + -0.193585587f, 0.981083391f, + -0.195090322f, 0.980785280f, + -0.196594598f, 0.980484862f, + -0.198098411f, 0.980182136f, + -0.199601758f, 0.979877104f, + -0.201104635f, 0.979569766f, + -0.202607039f, 0.979260123f, + -0.204108966f, 0.978948175f, + -0.205610413f, 0.978633924f, + -0.207111376f, 0.978317371f, + -0.208611852f, 0.977998515f, + -0.210111837f, 0.977677358f, + -0.211611327f, 0.977353900f, + -0.213110320f, 0.977028143f, + -0.214608811f, 0.976700086f, + -0.216106797f, 0.976369731f, + -0.217604275f, 0.976037079f, + -0.219101240f, 0.975702130f, + -0.220597690f, 0.975364885f, + -0.222093621f, 0.975025345f, + -0.223589029f, 0.974683511f, + -0.225083911f, 0.974339383f, + -0.226578264f, 0.973992962f, + -0.228072083f, 0.973644250f, + -0.229565366f, 0.973293246f, + -0.231058108f, 0.972939952f, + -0.232550307f, 0.972584369f, + -0.234041959f, 0.972226497f, + -0.235533059f, 0.971866337f, + -0.237023606f, 0.971503891f, + -0.238513595f, 0.971139158f, + -0.240003022f, 0.970772141f, + -0.241491885f, 0.970402839f, + -0.242980180f, 0.970031253f, + -0.244467903f, 0.969657385f, + -0.245955050f, 0.969281235f, + -0.247441619f, 0.968902805f, + -0.248927606f, 0.968522094f, + -0.250413007f, 0.968139105f, + -0.251897818f, 0.967753837f, + -0.253382037f, 0.967366292f, + -0.254865660f, 0.966976471f, + -0.256348682f, 0.966584374f, + -0.257831102f, 0.966190003f, + -0.259312915f, 0.965793359f, + -0.260794118f, 0.965394442f, + -0.262274707f, 0.964993253f, + -0.263754679f, 0.964589793f, + -0.265234030f, 0.964184064f, + -0.266712757f, 0.963776066f, + -0.268190857f, 0.963365800f, + -0.269668326f, 0.962953267f, + -0.271145160f, 0.962538468f, + -0.272621355f, 0.962121404f, + -0.274096910f, 0.961702077f, + -0.275571819f, 0.961280486f, + -0.277046080f, 0.960856633f, + -0.278519689f, 0.960430519f, + -0.279992643f, 0.960002146f, + -0.281464938f, 0.959571513f, + -0.282936570f, 0.959138622f, + -0.284407537f, 0.958703475f, + -0.285877835f, 0.958266071f, + -0.287347460f, 0.957826413f, + -0.288816408f, 0.957384501f, + -0.290284677f, 0.956940336f, + -0.291752263f, 0.956493919f, + -0.293219163f, 0.956045251f, + -0.294685372f, 0.955594334f, + -0.296150888f, 0.955141168f, + -0.297615707f, 0.954685755f, + -0.299079826f, 0.954228095f, + -0.300543241f, 0.953768190f, + -0.302005949f, 0.953306040f, + -0.303467947f, 0.952841648f, + -0.304929230f, 0.952375013f, + -0.306389795f, 0.951906137f, + -0.307849640f, 0.951435021f, + -0.309308760f, 0.950961666f, + -0.310767153f, 0.950486074f, + -0.312224814f, 0.950008245f, + -0.313681740f, 0.949528181f, + -0.315137929f, 0.949045882f, + -0.316593376f, 0.948561350f, + -0.318048077f, 0.948074586f, + -0.319502031f, 0.947585591f, + -0.320955232f, 0.947094366f, + -0.322407679f, 0.946600913f, + -0.323859367f, 0.946105232f, + -0.325310292f, 0.945607325f, + -0.326760452f, 0.945107193f, + -0.328209844f, 0.944604837f, + -0.329658463f, 0.944100258f, + -0.331106306f, 0.943593458f, + -0.332553370f, 0.943084437f, + -0.333999651f, 0.942573198f, + -0.335445147f, 0.942059740f, + -0.336889853f, 0.941544065f, + -0.338333767f, 0.941026175f, + -0.339776884f, 0.940506071f, + -0.341219202f, 0.939983753f, + -0.342660717f, 0.939459224f, + -0.344101426f, 0.938932484f, + -0.345541325f, 0.938403534f, + -0.346980411f, 0.937872376f, + -0.348418680f, 0.937339012f, + -0.349856130f, 0.936803442f, + -0.351292756f, 0.936265667f, + -0.352728556f, 0.935725689f, + -0.354163525f, 0.935183510f, + -0.355597662f, 0.934639130f, + -0.357030961f, 0.934092550f, + -0.358463421f, 0.933543773f, + -0.359895037f, 0.932992799f, + -0.361325806f, 0.932439629f, + -0.362755724f, 0.931884266f, + -0.364184790f, 0.931326709f, + -0.365612998f, 0.930766961f, + -0.367040346f, 0.930205023f, + -0.368466830f, 0.929640896f, + -0.369892447f, 0.929074581f, + -0.371317194f, 0.928506080f, + -0.372741067f, 0.927935395f, + -0.374164063f, 0.927362526f, + -0.375586178f, 0.926787474f, + -0.377007410f, 0.926210242f, + -0.378427755f, 0.925630831f, + -0.379847209f, 0.925049241f, + -0.381265769f, 0.924465474f, + -0.382683432f, 0.923879533f, + -0.384100195f, 0.923291417f, + -0.385516054f, 0.922701128f, + -0.386931006f, 0.922108669f, + -0.388345047f, 0.921514039f, + -0.389758174f, 0.920917242f, + -0.391170384f, 0.920318277f, + -0.392581674f, 0.919717146f, + -0.393992040f, 0.919113852f, + -0.395401479f, 0.918508394f, + -0.396809987f, 0.917900776f, + -0.398217562f, 0.917290997f, + -0.399624200f, 0.916679060f, + -0.401029897f, 0.916064966f, + -0.402434651f, 0.915448716f, + -0.403838458f, 0.914830312f, + -0.405241314f, 0.914209756f, + -0.406643217f, 0.913587048f, + -0.408044163f, 0.912962190f, + -0.409444149f, 0.912335185f, + -0.410843171f, 0.911706032f, + -0.412241227f, 0.911074734f, + -0.413638312f, 0.910441292f, + -0.415034424f, 0.909805708f, + -0.416429560f, 0.909167983f, + -0.417823716f, 0.908528119f, + -0.419216888f, 0.907886116f, + -0.420609074f, 0.907241978f, + -0.422000271f, 0.906595705f, + -0.423390474f, 0.905947298f, + -0.424779681f, 0.905296759f, + -0.426167889f, 0.904644091f, + -0.427555093f, 0.903989293f, + -0.428941292f, 0.903332368f, + -0.430326481f, 0.902673318f, + -0.431710658f, 0.902012144f, + -0.433093819f, 0.901348847f, + -0.434475961f, 0.900683429f, + -0.435857080f, 0.900015892f, + -0.437237174f, 0.899346237f, + -0.438616239f, 0.898674466f, + -0.439994271f, 0.898000580f, + -0.441371269f, 0.897324581f, + -0.442747228f, 0.896646470f, + -0.444122145f, 0.895966250f, + -0.445496017f, 0.895283921f, + -0.446868840f, 0.894599486f, + -0.448240612f, 0.893912945f, + -0.449611330f, 0.893224301f, + -0.450980989f, 0.892533555f, + -0.452349587f, 0.891840709f, + -0.453717121f, 0.891145765f, + -0.455083587f, 0.890448723f, + -0.456448982f, 0.889749586f, + -0.457813304f, 0.889048356f, + -0.459176548f, 0.888345033f, + -0.460538711f, 0.887639620f, + -0.461899791f, 0.886932119f, + -0.463259784f, 0.886222530f, + -0.464618686f, 0.885510856f, + -0.465976496f, 0.884797098f, + -0.467333209f, 0.884081259f, + -0.468688822f, 0.883363339f, + -0.470043332f, 0.882643340f, + -0.471396737f, 0.881921264f, + -0.472749032f, 0.881197113f, + -0.474100215f, 0.880470889f, + -0.475450282f, 0.879742593f, + -0.476799230f, 0.879012226f, + -0.478147056f, 0.878279792f, + -0.479493758f, 0.877545290f, + -0.480839331f, 0.876808724f, + -0.482183772f, 0.876070094f, + -0.483527079f, 0.875329403f, + -0.484869248f, 0.874586652f, + -0.486210276f, 0.873841843f, + -0.487550160f, 0.873094978f, + -0.488888897f, 0.872346059f, + -0.490226483f, 0.871595087f, + -0.491562916f, 0.870842063f, + -0.492898192f, 0.870086991f, + -0.494232309f, 0.869329871f, + -0.495565262f, 0.868570706f, + -0.496897049f, 0.867809497f, + -0.498227667f, 0.867046246f, + -0.499557113f, 0.866280954f, + -0.500885383f, 0.865513624f, + -0.502212474f, 0.864744258f, + -0.503538384f, 0.863972856f, + -0.504863109f, 0.863199422f, + -0.506186645f, 0.862423956f, + -0.507508991f, 0.861646461f, + -0.508830143f, 0.860866939f, + -0.510150097f, 0.860085390f, + -0.511468850f, 0.859301818f, + -0.512786401f, 0.858516224f, + -0.514102744f, 0.857728610f, + -0.515417878f, 0.856938977f, + -0.516731799f, 0.856147328f, + -0.518044504f, 0.855353665f, + -0.519355990f, 0.854557988f, + -0.520666254f, 0.853760301f, + -0.521975293f, 0.852960605f, + -0.523283103f, 0.852158902f, + -0.524589683f, 0.851355193f, + -0.525895027f, 0.850549481f, + -0.527199135f, 0.849741768f, + -0.528502002f, 0.848932055f, + -0.529803625f, 0.848120345f, + -0.531104001f, 0.847306639f, + -0.532403128f, 0.846490939f, + -0.533701002f, 0.845673247f, + -0.534997620f, 0.844853565f, + -0.536292979f, 0.844031895f, + -0.537587076f, 0.843208240f, + -0.538879909f, 0.842382600f, + -0.540171473f, 0.841554977f, + -0.541461766f, 0.840725375f, + -0.542750785f, 0.839893794f, + -0.544038527f, 0.839060237f, + -0.545324988f, 0.838224706f, + -0.546610167f, 0.837387202f, + -0.547894059f, 0.836547727f, + -0.549176662f, 0.835706284f, + -0.550457973f, 0.834862875f, + -0.551737988f, 0.834017501f, + -0.553016706f, 0.833170165f, + -0.554294121f, 0.832320868f, + -0.555570233f, 0.831469612f, + -0.556845037f, 0.830616400f, + -0.558118531f, 0.829761234f, + -0.559390712f, 0.828904115f, + -0.560661576f, 0.828045045f, + -0.561931121f, 0.827184027f, + -0.563199344f, 0.826321063f, + -0.564466242f, 0.825456154f, + -0.565731811f, 0.824589303f, + -0.566996049f, 0.823720511f, + -0.568258953f, 0.822849781f, + -0.569520519f, 0.821977115f, + -0.570780746f, 0.821102515f, + -0.572039629f, 0.820225983f, + -0.573297167f, 0.819347520f, + -0.574553355f, 0.818467130f, + -0.575808191f, 0.817584813f, + -0.577061673f, 0.816700573f, + -0.578313796f, 0.815814411f, + -0.579564559f, 0.814926329f, + -0.580813958f, 0.814036330f, + -0.582061990f, 0.813144415f, + -0.583308653f, 0.812250587f, + -0.584553943f, 0.811354847f, + -0.585797857f, 0.810457198f, + -0.587040394f, 0.809557642f, + -0.588281548f, 0.808656182f, + -0.589521319f, 0.807752818f, + -0.590759702f, 0.806847554f, + -0.591996695f, 0.805940391f, + -0.593232295f, 0.805031331f, + -0.594466499f, 0.804120377f, + -0.595699304f, 0.803207531f, + -0.596930708f, 0.802292796f, + -0.598160707f, 0.801376172f, + -0.599389298f, 0.800457662f, + -0.600616479f, 0.799537269f, + -0.601842247f, 0.798614995f, + -0.603066599f, 0.797690841f, + -0.604289531f, 0.796764810f, + -0.605511041f, 0.795836905f, + -0.606731127f, 0.794907126f, + -0.607949785f, 0.793975478f, + -0.609167012f, 0.793041960f, + -0.610382806f, 0.792106577f, + -0.611597164f, 0.791169330f, + -0.612810082f, 0.790230221f, + -0.614021559f, 0.789289253f, + -0.615231591f, 0.788346428f, + -0.616440175f, 0.787401747f, + -0.617647308f, 0.786455214f, + -0.618852988f, 0.785506830f, + -0.620057212f, 0.784556597f, + -0.621259977f, 0.783604519f, + -0.622461279f, 0.782650596f, + -0.623661118f, 0.781694832f, + -0.624859488f, 0.780737229f, + -0.626056388f, 0.779777788f, + -0.627251815f, 0.778816512f, + -0.628445767f, 0.777853404f, + -0.629638239f, 0.776888466f, + -0.630829230f, 0.775921699f, + -0.632018736f, 0.774953107f, + -0.633206755f, 0.773982691f, + -0.634393284f, 0.773010453f, + -0.635578320f, 0.772036397f, + -0.636761861f, 0.771060524f, + -0.637943904f, 0.770082837f, + -0.639124445f, 0.769103338f, + -0.640303482f, 0.768122029f, + -0.641481013f, 0.767138912f, + -0.642657034f, 0.766153990f, + -0.643831543f, 0.765167266f, + -0.645004537f, 0.764178741f, + -0.646176013f, 0.763188417f, + -0.647345969f, 0.762196298f, + -0.648514401f, 0.761202385f, + -0.649681307f, 0.760206682f, + -0.650846685f, 0.759209189f, + -0.652010531f, 0.758209910f, + -0.653172843f, 0.757208847f, + -0.654333618f, 0.756206001f, + -0.655492853f, 0.755201377f, + -0.656650546f, 0.754194975f, + -0.657806693f, 0.753186799f, + -0.658961293f, 0.752176850f, + -0.660114342f, 0.751165132f, + -0.661265838f, 0.750151646f, + -0.662415778f, 0.749136395f, + -0.663564159f, 0.748119380f, + -0.664710978f, 0.747100606f, + -0.665856234f, 0.746080074f, + -0.666999922f, 0.745057785f, + -0.668142041f, 0.744033744f, + -0.669282588f, 0.743007952f, + -0.670421560f, 0.741980412f, + -0.671558955f, 0.740951125f, + -0.672694769f, 0.739920095f, + -0.673829000f, 0.738887324f, + -0.674961646f, 0.737852815f, + -0.676092704f, 0.736816569f, + -0.677222170f, 0.735778589f, + -0.678350043f, 0.734738878f, + -0.679476320f, 0.733697438f, + -0.680600998f, 0.732654272f, + -0.681724074f, 0.731609381f, + -0.682845546f, 0.730562769f, + -0.683965412f, 0.729514438f, + -0.685083668f, 0.728464390f, + -0.686200312f, 0.727412629f, + -0.687315341f, 0.726359155f, + -0.688428753f, 0.725303972f, + -0.689540545f, 0.724247083f, + -0.690650714f, 0.723188489f, + -0.691759258f, 0.722128194f, + -0.692866175f, 0.721066199f, + -0.693971461f, 0.720002508f, + -0.695075114f, 0.718937122f, + -0.696177131f, 0.717870045f, + -0.697277511f, 0.716801279f, + -0.698376249f, 0.715730825f, + -0.699473345f, 0.714658688f, + -0.700568794f, 0.713584869f, + -0.701662595f, 0.712509371f, + -0.702754744f, 0.711432196f, + -0.703845241f, 0.710353347f, + -0.704934080f, 0.709272826f, + -0.706021261f, 0.708190637f, + -0.707106781f, 0.707106781f, + -0.708190637f, 0.706021261f, + -0.709272826f, 0.704934080f, + -0.710353347f, 0.703845241f, + -0.711432196f, 0.702754744f, + -0.712509371f, 0.701662595f, + -0.713584869f, 0.700568794f, + -0.714658688f, 0.699473345f, + -0.715730825f, 0.698376249f, + -0.716801279f, 0.697277511f, + -0.717870045f, 0.696177131f, + -0.718937122f, 0.695075114f, + -0.720002508f, 0.693971461f, + -0.721066199f, 0.692866175f, + -0.722128194f, 0.691759258f, + -0.723188489f, 0.690650714f, + -0.724247083f, 0.689540545f, + -0.725303972f, 0.688428753f, + -0.726359155f, 0.687315341f, + -0.727412629f, 0.686200312f, + -0.728464390f, 0.685083668f, + -0.729514438f, 0.683965412f, + -0.730562769f, 0.682845546f, + -0.731609381f, 0.681724074f, + -0.732654272f, 0.680600998f, + -0.733697438f, 0.679476320f, + -0.734738878f, 0.678350043f, + -0.735778589f, 0.677222170f, + -0.736816569f, 0.676092704f, + -0.737852815f, 0.674961646f, + -0.738887324f, 0.673829000f, + -0.739920095f, 0.672694769f, + -0.740951125f, 0.671558955f, + -0.741980412f, 0.670421560f, + -0.743007952f, 0.669282588f, + -0.744033744f, 0.668142041f, + -0.745057785f, 0.666999922f, + -0.746080074f, 0.665856234f, + -0.747100606f, 0.664710978f, + -0.748119380f, 0.663564159f, + -0.749136395f, 0.662415778f, + -0.750151646f, 0.661265838f, + -0.751165132f, 0.660114342f, + -0.752176850f, 0.658961293f, + -0.753186799f, 0.657806693f, + -0.754194975f, 0.656650546f, + -0.755201377f, 0.655492853f, + -0.756206001f, 0.654333618f, + -0.757208847f, 0.653172843f, + -0.758209910f, 0.652010531f, + -0.759209189f, 0.650846685f, + -0.760206682f, 0.649681307f, + -0.761202385f, 0.648514401f, + -0.762196298f, 0.647345969f, + -0.763188417f, 0.646176013f, + -0.764178741f, 0.645004537f, + -0.765167266f, 0.643831543f, + -0.766153990f, 0.642657034f, + -0.767138912f, 0.641481013f, + -0.768122029f, 0.640303482f, + -0.769103338f, 0.639124445f, + -0.770082837f, 0.637943904f, + -0.771060524f, 0.636761861f, + -0.772036397f, 0.635578320f, + -0.773010453f, 0.634393284f, + -0.773982691f, 0.633206755f, + -0.774953107f, 0.632018736f, + -0.775921699f, 0.630829230f, + -0.776888466f, 0.629638239f, + -0.777853404f, 0.628445767f, + -0.778816512f, 0.627251815f, + -0.779777788f, 0.626056388f, + -0.780737229f, 0.624859488f, + -0.781694832f, 0.623661118f, + -0.782650596f, 0.622461279f, + -0.783604519f, 0.621259977f, + -0.784556597f, 0.620057212f, + -0.785506830f, 0.618852988f, + -0.786455214f, 0.617647308f, + -0.787401747f, 0.616440175f, + -0.788346428f, 0.615231591f, + -0.789289253f, 0.614021559f, + -0.790230221f, 0.612810082f, + -0.791169330f, 0.611597164f, + -0.792106577f, 0.610382806f, + -0.793041960f, 0.609167012f, + -0.793975478f, 0.607949785f, + -0.794907126f, 0.606731127f, + -0.795836905f, 0.605511041f, + -0.796764810f, 0.604289531f, + -0.797690841f, 0.603066599f, + -0.798614995f, 0.601842247f, + -0.799537269f, 0.600616479f, + -0.800457662f, 0.599389298f, + -0.801376172f, 0.598160707f, + -0.802292796f, 0.596930708f, + -0.803207531f, 0.595699304f, + -0.804120377f, 0.594466499f, + -0.805031331f, 0.593232295f, + -0.805940391f, 0.591996695f, + -0.806847554f, 0.590759702f, + -0.807752818f, 0.589521319f, + -0.808656182f, 0.588281548f, + -0.809557642f, 0.587040394f, + -0.810457198f, 0.585797857f, + -0.811354847f, 0.584553943f, + -0.812250587f, 0.583308653f, + -0.813144415f, 0.582061990f, + -0.814036330f, 0.580813958f, + -0.814926329f, 0.579564559f, + -0.815814411f, 0.578313796f, + -0.816700573f, 0.577061673f, + -0.817584813f, 0.575808191f, + -0.818467130f, 0.574553355f, + -0.819347520f, 0.573297167f, + -0.820225983f, 0.572039629f, + -0.821102515f, 0.570780746f, + -0.821977115f, 0.569520519f, + -0.822849781f, 0.568258953f, + -0.823720511f, 0.566996049f, + -0.824589303f, 0.565731811f, + -0.825456154f, 0.564466242f, + -0.826321063f, 0.563199344f, + -0.827184027f, 0.561931121f, + -0.828045045f, 0.560661576f, + -0.828904115f, 0.559390712f, + -0.829761234f, 0.558118531f, + -0.830616400f, 0.556845037f, + -0.831469612f, 0.555570233f, + -0.832320868f, 0.554294121f, + -0.833170165f, 0.553016706f, + -0.834017501f, 0.551737988f, + -0.834862875f, 0.550457973f, + -0.835706284f, 0.549176662f, + -0.836547727f, 0.547894059f, + -0.837387202f, 0.546610167f, + -0.838224706f, 0.545324988f, + -0.839060237f, 0.544038527f, + -0.839893794f, 0.542750785f, + -0.840725375f, 0.541461766f, + -0.841554977f, 0.540171473f, + -0.842382600f, 0.538879909f, + -0.843208240f, 0.537587076f, + -0.844031895f, 0.536292979f, + -0.844853565f, 0.534997620f, + -0.845673247f, 0.533701002f, + -0.846490939f, 0.532403128f, + -0.847306639f, 0.531104001f, + -0.848120345f, 0.529803625f, + -0.848932055f, 0.528502002f, + -0.849741768f, 0.527199135f, + -0.850549481f, 0.525895027f, + -0.851355193f, 0.524589683f, + -0.852158902f, 0.523283103f, + -0.852960605f, 0.521975293f, + -0.853760301f, 0.520666254f, + -0.854557988f, 0.519355990f, + -0.855353665f, 0.518044504f, + -0.856147328f, 0.516731799f, + -0.856938977f, 0.515417878f, + -0.857728610f, 0.514102744f, + -0.858516224f, 0.512786401f, + -0.859301818f, 0.511468850f, + -0.860085390f, 0.510150097f, + -0.860866939f, 0.508830143f, + -0.861646461f, 0.507508991f, + -0.862423956f, 0.506186645f, + -0.863199422f, 0.504863109f, + -0.863972856f, 0.503538384f, + -0.864744258f, 0.502212474f, + -0.865513624f, 0.500885383f, + -0.866280954f, 0.499557113f, + -0.867046246f, 0.498227667f, + -0.867809497f, 0.496897049f, + -0.868570706f, 0.495565262f, + -0.869329871f, 0.494232309f, + -0.870086991f, 0.492898192f, + -0.870842063f, 0.491562916f, + -0.871595087f, 0.490226483f, + -0.872346059f, 0.488888897f, + -0.873094978f, 0.487550160f, + -0.873841843f, 0.486210276f, + -0.874586652f, 0.484869248f, + -0.875329403f, 0.483527079f, + -0.876070094f, 0.482183772f, + -0.876808724f, 0.480839331f, + -0.877545290f, 0.479493758f, + -0.878279792f, 0.478147056f, + -0.879012226f, 0.476799230f, + -0.879742593f, 0.475450282f, + -0.880470889f, 0.474100215f, + -0.881197113f, 0.472749032f, + -0.881921264f, 0.471396737f, + -0.882643340f, 0.470043332f, + -0.883363339f, 0.468688822f, + -0.884081259f, 0.467333209f, + -0.884797098f, 0.465976496f, + -0.885510856f, 0.464618686f, + -0.886222530f, 0.463259784f, + -0.886932119f, 0.461899791f, + -0.887639620f, 0.460538711f, + -0.888345033f, 0.459176548f, + -0.889048356f, 0.457813304f, + -0.889749586f, 0.456448982f, + -0.890448723f, 0.455083587f, + -0.891145765f, 0.453717121f, + -0.891840709f, 0.452349587f, + -0.892533555f, 0.450980989f, + -0.893224301f, 0.449611330f, + -0.893912945f, 0.448240612f, + -0.894599486f, 0.446868840f, + -0.895283921f, 0.445496017f, + -0.895966250f, 0.444122145f, + -0.896646470f, 0.442747228f, + -0.897324581f, 0.441371269f, + -0.898000580f, 0.439994271f, + -0.898674466f, 0.438616239f, + -0.899346237f, 0.437237174f, + -0.900015892f, 0.435857080f, + -0.900683429f, 0.434475961f, + -0.901348847f, 0.433093819f, + -0.902012144f, 0.431710658f, + -0.902673318f, 0.430326481f, + -0.903332368f, 0.428941292f, + -0.903989293f, 0.427555093f, + -0.904644091f, 0.426167889f, + -0.905296759f, 0.424779681f, + -0.905947298f, 0.423390474f, + -0.906595705f, 0.422000271f, + -0.907241978f, 0.420609074f, + -0.907886116f, 0.419216888f, + -0.908528119f, 0.417823716f, + -0.909167983f, 0.416429560f, + -0.909805708f, 0.415034424f, + -0.910441292f, 0.413638312f, + -0.911074734f, 0.412241227f, + -0.911706032f, 0.410843171f, + -0.912335185f, 0.409444149f, + -0.912962190f, 0.408044163f, + -0.913587048f, 0.406643217f, + -0.914209756f, 0.405241314f, + -0.914830312f, 0.403838458f, + -0.915448716f, 0.402434651f, + -0.916064966f, 0.401029897f, + -0.916679060f, 0.399624200f, + -0.917290997f, 0.398217562f, + -0.917900776f, 0.396809987f, + -0.918508394f, 0.395401479f, + -0.919113852f, 0.393992040f, + -0.919717146f, 0.392581674f, + -0.920318277f, 0.391170384f, + -0.920917242f, 0.389758174f, + -0.921514039f, 0.388345047f, + -0.922108669f, 0.386931006f, + -0.922701128f, 0.385516054f, + -0.923291417f, 0.384100195f, + -0.923879533f, 0.382683432f, + -0.924465474f, 0.381265769f, + -0.925049241f, 0.379847209f, + -0.925630831f, 0.378427755f, + -0.926210242f, 0.377007410f, + -0.926787474f, 0.375586178f, + -0.927362526f, 0.374164063f, + -0.927935395f, 0.372741067f, + -0.928506080f, 0.371317194f, + -0.929074581f, 0.369892447f, + -0.929640896f, 0.368466830f, + -0.930205023f, 0.367040346f, + -0.930766961f, 0.365612998f, + -0.931326709f, 0.364184790f, + -0.931884266f, 0.362755724f, + -0.932439629f, 0.361325806f, + -0.932992799f, 0.359895037f, + -0.933543773f, 0.358463421f, + -0.934092550f, 0.357030961f, + -0.934639130f, 0.355597662f, + -0.935183510f, 0.354163525f, + -0.935725689f, 0.352728556f, + -0.936265667f, 0.351292756f, + -0.936803442f, 0.349856130f, + -0.937339012f, 0.348418680f, + -0.937872376f, 0.346980411f, + -0.938403534f, 0.345541325f, + -0.938932484f, 0.344101426f, + -0.939459224f, 0.342660717f, + -0.939983753f, 0.341219202f, + -0.940506071f, 0.339776884f, + -0.941026175f, 0.338333767f, + -0.941544065f, 0.336889853f, + -0.942059740f, 0.335445147f, + -0.942573198f, 0.333999651f, + -0.943084437f, 0.332553370f, + -0.943593458f, 0.331106306f, + -0.944100258f, 0.329658463f, + -0.944604837f, 0.328209844f, + -0.945107193f, 0.326760452f, + -0.945607325f, 0.325310292f, + -0.946105232f, 0.323859367f, + -0.946600913f, 0.322407679f, + -0.947094366f, 0.320955232f, + -0.947585591f, 0.319502031f, + -0.948074586f, 0.318048077f, + -0.948561350f, 0.316593376f, + -0.949045882f, 0.315137929f, + -0.949528181f, 0.313681740f, + -0.950008245f, 0.312224814f, + -0.950486074f, 0.310767153f, + -0.950961666f, 0.309308760f, + -0.951435021f, 0.307849640f, + -0.951906137f, 0.306389795f, + -0.952375013f, 0.304929230f, + -0.952841648f, 0.303467947f, + -0.953306040f, 0.302005949f, + -0.953768190f, 0.300543241f, + -0.954228095f, 0.299079826f, + -0.954685755f, 0.297615707f, + -0.955141168f, 0.296150888f, + -0.955594334f, 0.294685372f, + -0.956045251f, 0.293219163f, + -0.956493919f, 0.291752263f, + -0.956940336f, 0.290284677f, + -0.957384501f, 0.288816408f, + -0.957826413f, 0.287347460f, + -0.958266071f, 0.285877835f, + -0.958703475f, 0.284407537f, + -0.959138622f, 0.282936570f, + -0.959571513f, 0.281464938f, + -0.960002146f, 0.279992643f, + -0.960430519f, 0.278519689f, + -0.960856633f, 0.277046080f, + -0.961280486f, 0.275571819f, + -0.961702077f, 0.274096910f, + -0.962121404f, 0.272621355f, + -0.962538468f, 0.271145160f, + -0.962953267f, 0.269668326f, + -0.963365800f, 0.268190857f, + -0.963776066f, 0.266712757f, + -0.964184064f, 0.265234030f, + -0.964589793f, 0.263754679f, + -0.964993253f, 0.262274707f, + -0.965394442f, 0.260794118f, + -0.965793359f, 0.259312915f, + -0.966190003f, 0.257831102f, + -0.966584374f, 0.256348682f, + -0.966976471f, 0.254865660f, + -0.967366292f, 0.253382037f, + -0.967753837f, 0.251897818f, + -0.968139105f, 0.250413007f, + -0.968522094f, 0.248927606f, + -0.968902805f, 0.247441619f, + -0.969281235f, 0.245955050f, + -0.969657385f, 0.244467903f, + -0.970031253f, 0.242980180f, + -0.970402839f, 0.241491885f, + -0.970772141f, 0.240003022f, + -0.971139158f, 0.238513595f, + -0.971503891f, 0.237023606f, + -0.971866337f, 0.235533059f, + -0.972226497f, 0.234041959f, + -0.972584369f, 0.232550307f, + -0.972939952f, 0.231058108f, + -0.973293246f, 0.229565366f, + -0.973644250f, 0.228072083f, + -0.973992962f, 0.226578264f, + -0.974339383f, 0.225083911f, + -0.974683511f, 0.223589029f, + -0.975025345f, 0.222093621f, + -0.975364885f, 0.220597690f, + -0.975702130f, 0.219101240f, + -0.976037079f, 0.217604275f, + -0.976369731f, 0.216106797f, + -0.976700086f, 0.214608811f, + -0.977028143f, 0.213110320f, + -0.977353900f, 0.211611327f, + -0.977677358f, 0.210111837f, + -0.977998515f, 0.208611852f, + -0.978317371f, 0.207111376f, + -0.978633924f, 0.205610413f, + -0.978948175f, 0.204108966f, + -0.979260123f, 0.202607039f, + -0.979569766f, 0.201104635f, + -0.979877104f, 0.199601758f, + -0.980182136f, 0.198098411f, + -0.980484862f, 0.196594598f, + -0.980785280f, 0.195090322f, + -0.981083391f, 0.193585587f, + -0.981379193f, 0.192080397f, + -0.981672686f, 0.190574755f, + -0.981963869f, 0.189068664f, + -0.982252741f, 0.187562129f, + -0.982539302f, 0.186055152f, + -0.982823551f, 0.184547737f, + -0.983105487f, 0.183039888f, + -0.983385110f, 0.181531608f, + -0.983662419f, 0.180022901f, + -0.983937413f, 0.178513771f, + -0.984210092f, 0.177004220f, + -0.984480455f, 0.175494253f, + -0.984748502f, 0.173983873f, + -0.985014231f, 0.172473084f, + -0.985277642f, 0.170961889f, + -0.985538735f, 0.169450291f, + -0.985797509f, 0.167938295f, + -0.986053963f, 0.166425904f, + -0.986308097f, 0.164913120f, + -0.986559910f, 0.163399949f, + -0.986809402f, 0.161886394f, + -0.987056571f, 0.160372457f, + -0.987301418f, 0.158858143f, + -0.987543942f, 0.157343456f, + -0.987784142f, 0.155828398f, + -0.988022017f, 0.154312973f, + -0.988257568f, 0.152797185f, + -0.988490793f, 0.151281038f, + -0.988721692f, 0.149764535f, + -0.988950265f, 0.148247679f, + -0.989176510f, 0.146730474f, + -0.989400428f, 0.145212925f, + -0.989622017f, 0.143695033f, + -0.989841278f, 0.142176804f, + -0.990058210f, 0.140658239f, + -0.990272812f, 0.139139344f, + -0.990485084f, 0.137620122f, + -0.990695025f, 0.136100575f, + -0.990902635f, 0.134580709f, + -0.991107914f, 0.133060525f, + -0.991310860f, 0.131540029f, + -0.991511473f, 0.130019223f, + -0.991709754f, 0.128498111f, + -0.991905700f, 0.126976696f, + -0.992099313f, 0.125454983f, + -0.992290591f, 0.123932975f, + -0.992479535f, 0.122410675f, + -0.992666142f, 0.120888087f, + -0.992850414f, 0.119365215f, + -0.993032350f, 0.117842062f, + -0.993211949f, 0.116318631f, + -0.993389211f, 0.114794927f, + -0.993564136f, 0.113270952f, + -0.993736722f, 0.111746711f, + -0.993906970f, 0.110222207f, + -0.994074879f, 0.108697444f, + -0.994240449f, 0.107172425f, + -0.994403680f, 0.105647154f, + -0.994564571f, 0.104121634f, + -0.994723121f, 0.102595869f, + -0.994879331f, 0.101069863f, + -0.995033199f, 0.099543619f, + -0.995184727f, 0.098017140f, + -0.995333912f, 0.096490431f, + -0.995480755f, 0.094963495f, + -0.995625256f, 0.093436336f, + -0.995767414f, 0.091908956f, + -0.995907229f, 0.090381361f, + -0.996044701f, 0.088853553f, + -0.996179829f, 0.087325535f, + -0.996312612f, 0.085797312f, + -0.996443051f, 0.084268888f, + -0.996571146f, 0.082740265f, + -0.996696895f, 0.081211447f, + -0.996820299f, 0.079682438f, + -0.996941358f, 0.078153242f, + -0.997060070f, 0.076623861f, + -0.997176437f, 0.075094301f, + -0.997290457f, 0.073564564f, + -0.997402130f, 0.072034653f, + -0.997511456f, 0.070504573f, + -0.997618435f, 0.068974328f, + -0.997723067f, 0.067443920f, + -0.997825350f, 0.065913353f, + -0.997925286f, 0.064382631f, + -0.998022874f, 0.062851758f, + -0.998118113f, 0.061320736f, + -0.998211003f, 0.059789571f, + -0.998301545f, 0.058258265f, + -0.998389737f, 0.056726821f, + -0.998475581f, 0.055195244f, + -0.998559074f, 0.053663538f, + -0.998640218f, 0.052131705f, + -0.998719012f, 0.050599749f, + -0.998795456f, 0.049067674f, + -0.998869550f, 0.047535484f, + -0.998941293f, 0.046003182f, + -0.999010686f, 0.044470772f, + -0.999077728f, 0.042938257f, + -0.999142419f, 0.041405641f, + -0.999204759f, 0.039872928f, + -0.999264747f, 0.038340120f, + -0.999322385f, 0.036807223f, + -0.999377670f, 0.035274239f, + -0.999430605f, 0.033741172f, + -0.999481187f, 0.032208025f, + -0.999529418f, 0.030674803f, + -0.999575296f, 0.029141509f, + -0.999618822f, 0.027608146f, + -0.999659997f, 0.026074718f, + -0.999698819f, 0.024541229f, + -0.999735288f, 0.023007681f, + -0.999769405f, 0.021474080f, + -0.999801170f, 0.019940429f, + -0.999830582f, 0.018406730f, + -0.999857641f, 0.016872988f, + -0.999882347f, 0.015339206f, + -0.999904701f, 0.013805389f, + -0.999924702f, 0.012271538f, + -0.999942350f, 0.010737659f, + -0.999957645f, 0.009203755f, + -0.999970586f, 0.007669829f, + -0.999981175f, 0.006135885f, + -0.999989411f, 0.004601926f, + -0.999995294f, 0.003067957f, + -0.999998823f, 0.001533980f, + -1.000000000f, 0.000000000f, + -0.999998823f, -0.001533980f, + -0.999995294f, -0.003067957f, + -0.999989411f, -0.004601926f, + -0.999981175f, -0.006135885f, + -0.999970586f, -0.007669829f, + -0.999957645f, -0.009203755f, + -0.999942350f, -0.010737659f, + -0.999924702f, -0.012271538f, + -0.999904701f, -0.013805389f, + -0.999882347f, -0.015339206f, + -0.999857641f, -0.016872988f, + -0.999830582f, -0.018406730f, + -0.999801170f, -0.019940429f, + -0.999769405f, -0.021474080f, + -0.999735288f, -0.023007681f, + -0.999698819f, -0.024541229f, + -0.999659997f, -0.026074718f, + -0.999618822f, -0.027608146f, + -0.999575296f, -0.029141509f, + -0.999529418f, -0.030674803f, + -0.999481187f, -0.032208025f, + -0.999430605f, -0.033741172f, + -0.999377670f, -0.035274239f, + -0.999322385f, -0.036807223f, + -0.999264747f, -0.038340120f, + -0.999204759f, -0.039872928f, + -0.999142419f, -0.041405641f, + -0.999077728f, -0.042938257f, + -0.999010686f, -0.044470772f, + -0.998941293f, -0.046003182f, + -0.998869550f, -0.047535484f, + -0.998795456f, -0.049067674f, + -0.998719012f, -0.050599749f, + -0.998640218f, -0.052131705f, + -0.998559074f, -0.053663538f, + -0.998475581f, -0.055195244f, + -0.998389737f, -0.056726821f, + -0.998301545f, -0.058258265f, + -0.998211003f, -0.059789571f, + -0.998118113f, -0.061320736f, + -0.998022874f, -0.062851758f, + -0.997925286f, -0.064382631f, + -0.997825350f, -0.065913353f, + -0.997723067f, -0.067443920f, + -0.997618435f, -0.068974328f, + -0.997511456f, -0.070504573f, + -0.997402130f, -0.072034653f, + -0.997290457f, -0.073564564f, + -0.997176437f, -0.075094301f, + -0.997060070f, -0.076623861f, + -0.996941358f, -0.078153242f, + -0.996820299f, -0.079682438f, + -0.996696895f, -0.081211447f, + -0.996571146f, -0.082740265f, + -0.996443051f, -0.084268888f, + -0.996312612f, -0.085797312f, + -0.996179829f, -0.087325535f, + -0.996044701f, -0.088853553f, + -0.995907229f, -0.090381361f, + -0.995767414f, -0.091908956f, + -0.995625256f, -0.093436336f, + -0.995480755f, -0.094963495f, + -0.995333912f, -0.096490431f, + -0.995184727f, -0.098017140f, + -0.995033199f, -0.099543619f, + -0.994879331f, -0.101069863f, + -0.994723121f, -0.102595869f, + -0.994564571f, -0.104121634f, + -0.994403680f, -0.105647154f, + -0.994240449f, -0.107172425f, + -0.994074879f, -0.108697444f, + -0.993906970f, -0.110222207f, + -0.993736722f, -0.111746711f, + -0.993564136f, -0.113270952f, + -0.993389211f, -0.114794927f, + -0.993211949f, -0.116318631f, + -0.993032350f, -0.117842062f, + -0.992850414f, -0.119365215f, + -0.992666142f, -0.120888087f, + -0.992479535f, -0.122410675f, + -0.992290591f, -0.123932975f, + -0.992099313f, -0.125454983f, + -0.991905700f, -0.126976696f, + -0.991709754f, -0.128498111f, + -0.991511473f, -0.130019223f, + -0.991310860f, -0.131540029f, + -0.991107914f, -0.133060525f, + -0.990902635f, -0.134580709f, + -0.990695025f, -0.136100575f, + -0.990485084f, -0.137620122f, + -0.990272812f, -0.139139344f, + -0.990058210f, -0.140658239f, + -0.989841278f, -0.142176804f, + -0.989622017f, -0.143695033f, + -0.989400428f, -0.145212925f, + -0.989176510f, -0.146730474f, + -0.988950265f, -0.148247679f, + -0.988721692f, -0.149764535f, + -0.988490793f, -0.151281038f, + -0.988257568f, -0.152797185f, + -0.988022017f, -0.154312973f, + -0.987784142f, -0.155828398f, + -0.987543942f, -0.157343456f, + -0.987301418f, -0.158858143f, + -0.987056571f, -0.160372457f, + -0.986809402f, -0.161886394f, + -0.986559910f, -0.163399949f, + -0.986308097f, -0.164913120f, + -0.986053963f, -0.166425904f, + -0.985797509f, -0.167938295f, + -0.985538735f, -0.169450291f, + -0.985277642f, -0.170961889f, + -0.985014231f, -0.172473084f, + -0.984748502f, -0.173983873f, + -0.984480455f, -0.175494253f, + -0.984210092f, -0.177004220f, + -0.983937413f, -0.178513771f, + -0.983662419f, -0.180022901f, + -0.983385110f, -0.181531608f, + -0.983105487f, -0.183039888f, + -0.982823551f, -0.184547737f, + -0.982539302f, -0.186055152f, + -0.982252741f, -0.187562129f, + -0.981963869f, -0.189068664f, + -0.981672686f, -0.190574755f, + -0.981379193f, -0.192080397f, + -0.981083391f, -0.193585587f, + -0.980785280f, -0.195090322f, + -0.980484862f, -0.196594598f, + -0.980182136f, -0.198098411f, + -0.979877104f, -0.199601758f, + -0.979569766f, -0.201104635f, + -0.979260123f, -0.202607039f, + -0.978948175f, -0.204108966f, + -0.978633924f, -0.205610413f, + -0.978317371f, -0.207111376f, + -0.977998515f, -0.208611852f, + -0.977677358f, -0.210111837f, + -0.977353900f, -0.211611327f, + -0.977028143f, -0.213110320f, + -0.976700086f, -0.214608811f, + -0.976369731f, -0.216106797f, + -0.976037079f, -0.217604275f, + -0.975702130f, -0.219101240f, + -0.975364885f, -0.220597690f, + -0.975025345f, -0.222093621f, + -0.974683511f, -0.223589029f, + -0.974339383f, -0.225083911f, + -0.973992962f, -0.226578264f, + -0.973644250f, -0.228072083f, + -0.973293246f, -0.229565366f, + -0.972939952f, -0.231058108f, + -0.972584369f, -0.232550307f, + -0.972226497f, -0.234041959f, + -0.971866337f, -0.235533059f, + -0.971503891f, -0.237023606f, + -0.971139158f, -0.238513595f, + -0.970772141f, -0.240003022f, + -0.970402839f, -0.241491885f, + -0.970031253f, -0.242980180f, + -0.969657385f, -0.244467903f, + -0.969281235f, -0.245955050f, + -0.968902805f, -0.247441619f, + -0.968522094f, -0.248927606f, + -0.968139105f, -0.250413007f, + -0.967753837f, -0.251897818f, + -0.967366292f, -0.253382037f, + -0.966976471f, -0.254865660f, + -0.966584374f, -0.256348682f, + -0.966190003f, -0.257831102f, + -0.965793359f, -0.259312915f, + -0.965394442f, -0.260794118f, + -0.964993253f, -0.262274707f, + -0.964589793f, -0.263754679f, + -0.964184064f, -0.265234030f, + -0.963776066f, -0.266712757f, + -0.963365800f, -0.268190857f, + -0.962953267f, -0.269668326f, + -0.962538468f, -0.271145160f, + -0.962121404f, -0.272621355f, + -0.961702077f, -0.274096910f, + -0.961280486f, -0.275571819f, + -0.960856633f, -0.277046080f, + -0.960430519f, -0.278519689f, + -0.960002146f, -0.279992643f, + -0.959571513f, -0.281464938f, + -0.959138622f, -0.282936570f, + -0.958703475f, -0.284407537f, + -0.958266071f, -0.285877835f, + -0.957826413f, -0.287347460f, + -0.957384501f, -0.288816408f, + -0.956940336f, -0.290284677f, + -0.956493919f, -0.291752263f, + -0.956045251f, -0.293219163f, + -0.955594334f, -0.294685372f, + -0.955141168f, -0.296150888f, + -0.954685755f, -0.297615707f, + -0.954228095f, -0.299079826f, + -0.953768190f, -0.300543241f, + -0.953306040f, -0.302005949f, + -0.952841648f, -0.303467947f, + -0.952375013f, -0.304929230f, + -0.951906137f, -0.306389795f, + -0.951435021f, -0.307849640f, + -0.950961666f, -0.309308760f, + -0.950486074f, -0.310767153f, + -0.950008245f, -0.312224814f, + -0.949528181f, -0.313681740f, + -0.949045882f, -0.315137929f, + -0.948561350f, -0.316593376f, + -0.948074586f, -0.318048077f, + -0.947585591f, -0.319502031f, + -0.947094366f, -0.320955232f, + -0.946600913f, -0.322407679f, + -0.946105232f, -0.323859367f, + -0.945607325f, -0.325310292f, + -0.945107193f, -0.326760452f, + -0.944604837f, -0.328209844f, + -0.944100258f, -0.329658463f, + -0.943593458f, -0.331106306f, + -0.943084437f, -0.332553370f, + -0.942573198f, -0.333999651f, + -0.942059740f, -0.335445147f, + -0.941544065f, -0.336889853f, + -0.941026175f, -0.338333767f, + -0.940506071f, -0.339776884f, + -0.939983753f, -0.341219202f, + -0.939459224f, -0.342660717f, + -0.938932484f, -0.344101426f, + -0.938403534f, -0.345541325f, + -0.937872376f, -0.346980411f, + -0.937339012f, -0.348418680f, + -0.936803442f, -0.349856130f, + -0.936265667f, -0.351292756f, + -0.935725689f, -0.352728556f, + -0.935183510f, -0.354163525f, + -0.934639130f, -0.355597662f, + -0.934092550f, -0.357030961f, + -0.933543773f, -0.358463421f, + -0.932992799f, -0.359895037f, + -0.932439629f, -0.361325806f, + -0.931884266f, -0.362755724f, + -0.931326709f, -0.364184790f, + -0.930766961f, -0.365612998f, + -0.930205023f, -0.367040346f, + -0.929640896f, -0.368466830f, + -0.929074581f, -0.369892447f, + -0.928506080f, -0.371317194f, + -0.927935395f, -0.372741067f, + -0.927362526f, -0.374164063f, + -0.926787474f, -0.375586178f, + -0.926210242f, -0.377007410f, + -0.925630831f, -0.378427755f, + -0.925049241f, -0.379847209f, + -0.924465474f, -0.381265769f, + -0.923879533f, -0.382683432f, + -0.923291417f, -0.384100195f, + -0.922701128f, -0.385516054f, + -0.922108669f, -0.386931006f, + -0.921514039f, -0.388345047f, + -0.920917242f, -0.389758174f, + -0.920318277f, -0.391170384f, + -0.919717146f, -0.392581674f, + -0.919113852f, -0.393992040f, + -0.918508394f, -0.395401479f, + -0.917900776f, -0.396809987f, + -0.917290997f, -0.398217562f, + -0.916679060f, -0.399624200f, + -0.916064966f, -0.401029897f, + -0.915448716f, -0.402434651f, + -0.914830312f, -0.403838458f, + -0.914209756f, -0.405241314f, + -0.913587048f, -0.406643217f, + -0.912962190f, -0.408044163f, + -0.912335185f, -0.409444149f, + -0.911706032f, -0.410843171f, + -0.911074734f, -0.412241227f, + -0.910441292f, -0.413638312f, + -0.909805708f, -0.415034424f, + -0.909167983f, -0.416429560f, + -0.908528119f, -0.417823716f, + -0.907886116f, -0.419216888f, + -0.907241978f, -0.420609074f, + -0.906595705f, -0.422000271f, + -0.905947298f, -0.423390474f, + -0.905296759f, -0.424779681f, + -0.904644091f, -0.426167889f, + -0.903989293f, -0.427555093f, + -0.903332368f, -0.428941292f, + -0.902673318f, -0.430326481f, + -0.902012144f, -0.431710658f, + -0.901348847f, -0.433093819f, + -0.900683429f, -0.434475961f, + -0.900015892f, -0.435857080f, + -0.899346237f, -0.437237174f, + -0.898674466f, -0.438616239f, + -0.898000580f, -0.439994271f, + -0.897324581f, -0.441371269f, + -0.896646470f, -0.442747228f, + -0.895966250f, -0.444122145f, + -0.895283921f, -0.445496017f, + -0.894599486f, -0.446868840f, + -0.893912945f, -0.448240612f, + -0.893224301f, -0.449611330f, + -0.892533555f, -0.450980989f, + -0.891840709f, -0.452349587f, + -0.891145765f, -0.453717121f, + -0.890448723f, -0.455083587f, + -0.889749586f, -0.456448982f, + -0.889048356f, -0.457813304f, + -0.888345033f, -0.459176548f, + -0.887639620f, -0.460538711f, + -0.886932119f, -0.461899791f, + -0.886222530f, -0.463259784f, + -0.885510856f, -0.464618686f, + -0.884797098f, -0.465976496f, + -0.884081259f, -0.467333209f, + -0.883363339f, -0.468688822f, + -0.882643340f, -0.470043332f, + -0.881921264f, -0.471396737f, + -0.881197113f, -0.472749032f, + -0.880470889f, -0.474100215f, + -0.879742593f, -0.475450282f, + -0.879012226f, -0.476799230f, + -0.878279792f, -0.478147056f, + -0.877545290f, -0.479493758f, + -0.876808724f, -0.480839331f, + -0.876070094f, -0.482183772f, + -0.875329403f, -0.483527079f, + -0.874586652f, -0.484869248f, + -0.873841843f, -0.486210276f, + -0.873094978f, -0.487550160f, + -0.872346059f, -0.488888897f, + -0.871595087f, -0.490226483f, + -0.870842063f, -0.491562916f, + -0.870086991f, -0.492898192f, + -0.869329871f, -0.494232309f, + -0.868570706f, -0.495565262f, + -0.867809497f, -0.496897049f, + -0.867046246f, -0.498227667f, + -0.866280954f, -0.499557113f, + -0.865513624f, -0.500885383f, + -0.864744258f, -0.502212474f, + -0.863972856f, -0.503538384f, + -0.863199422f, -0.504863109f, + -0.862423956f, -0.506186645f, + -0.861646461f, -0.507508991f, + -0.860866939f, -0.508830143f, + -0.860085390f, -0.510150097f, + -0.859301818f, -0.511468850f, + -0.858516224f, -0.512786401f, + -0.857728610f, -0.514102744f, + -0.856938977f, -0.515417878f, + -0.856147328f, -0.516731799f, + -0.855353665f, -0.518044504f, + -0.854557988f, -0.519355990f, + -0.853760301f, -0.520666254f, + -0.852960605f, -0.521975293f, + -0.852158902f, -0.523283103f, + -0.851355193f, -0.524589683f, + -0.850549481f, -0.525895027f, + -0.849741768f, -0.527199135f, + -0.848932055f, -0.528502002f, + -0.848120345f, -0.529803625f, + -0.847306639f, -0.531104001f, + -0.846490939f, -0.532403128f, + -0.845673247f, -0.533701002f, + -0.844853565f, -0.534997620f, + -0.844031895f, -0.536292979f, + -0.843208240f, -0.537587076f, + -0.842382600f, -0.538879909f, + -0.841554977f, -0.540171473f, + -0.840725375f, -0.541461766f, + -0.839893794f, -0.542750785f, + -0.839060237f, -0.544038527f, + -0.838224706f, -0.545324988f, + -0.837387202f, -0.546610167f, + -0.836547727f, -0.547894059f, + -0.835706284f, -0.549176662f, + -0.834862875f, -0.550457973f, + -0.834017501f, -0.551737988f, + -0.833170165f, -0.553016706f, + -0.832320868f, -0.554294121f, + -0.831469612f, -0.555570233f, + -0.830616400f, -0.556845037f, + -0.829761234f, -0.558118531f, + -0.828904115f, -0.559390712f, + -0.828045045f, -0.560661576f, + -0.827184027f, -0.561931121f, + -0.826321063f, -0.563199344f, + -0.825456154f, -0.564466242f, + -0.824589303f, -0.565731811f, + -0.823720511f, -0.566996049f, + -0.822849781f, -0.568258953f, + -0.821977115f, -0.569520519f, + -0.821102515f, -0.570780746f, + -0.820225983f, -0.572039629f, + -0.819347520f, -0.573297167f, + -0.818467130f, -0.574553355f, + -0.817584813f, -0.575808191f, + -0.816700573f, -0.577061673f, + -0.815814411f, -0.578313796f, + -0.814926329f, -0.579564559f, + -0.814036330f, -0.580813958f, + -0.813144415f, -0.582061990f, + -0.812250587f, -0.583308653f, + -0.811354847f, -0.584553943f, + -0.810457198f, -0.585797857f, + -0.809557642f, -0.587040394f, + -0.808656182f, -0.588281548f, + -0.807752818f, -0.589521319f, + -0.806847554f, -0.590759702f, + -0.805940391f, -0.591996695f, + -0.805031331f, -0.593232295f, + -0.804120377f, -0.594466499f, + -0.803207531f, -0.595699304f, + -0.802292796f, -0.596930708f, + -0.801376172f, -0.598160707f, + -0.800457662f, -0.599389298f, + -0.799537269f, -0.600616479f, + -0.798614995f, -0.601842247f, + -0.797690841f, -0.603066599f, + -0.796764810f, -0.604289531f, + -0.795836905f, -0.605511041f, + -0.794907126f, -0.606731127f, + -0.793975478f, -0.607949785f, + -0.793041960f, -0.609167012f, + -0.792106577f, -0.610382806f, + -0.791169330f, -0.611597164f, + -0.790230221f, -0.612810082f, + -0.789289253f, -0.614021559f, + -0.788346428f, -0.615231591f, + -0.787401747f, -0.616440175f, + -0.786455214f, -0.617647308f, + -0.785506830f, -0.618852988f, + -0.784556597f, -0.620057212f, + -0.783604519f, -0.621259977f, + -0.782650596f, -0.622461279f, + -0.781694832f, -0.623661118f, + -0.780737229f, -0.624859488f, + -0.779777788f, -0.626056388f, + -0.778816512f, -0.627251815f, + -0.777853404f, -0.628445767f, + -0.776888466f, -0.629638239f, + -0.775921699f, -0.630829230f, + -0.774953107f, -0.632018736f, + -0.773982691f, -0.633206755f, + -0.773010453f, -0.634393284f, + -0.772036397f, -0.635578320f, + -0.771060524f, -0.636761861f, + -0.770082837f, -0.637943904f, + -0.769103338f, -0.639124445f, + -0.768122029f, -0.640303482f, + -0.767138912f, -0.641481013f, + -0.766153990f, -0.642657034f, + -0.765167266f, -0.643831543f, + -0.764178741f, -0.645004537f, + -0.763188417f, -0.646176013f, + -0.762196298f, -0.647345969f, + -0.761202385f, -0.648514401f, + -0.760206682f, -0.649681307f, + -0.759209189f, -0.650846685f, + -0.758209910f, -0.652010531f, + -0.757208847f, -0.653172843f, + -0.756206001f, -0.654333618f, + -0.755201377f, -0.655492853f, + -0.754194975f, -0.656650546f, + -0.753186799f, -0.657806693f, + -0.752176850f, -0.658961293f, + -0.751165132f, -0.660114342f, + -0.750151646f, -0.661265838f, + -0.749136395f, -0.662415778f, + -0.748119380f, -0.663564159f, + -0.747100606f, -0.664710978f, + -0.746080074f, -0.665856234f, + -0.745057785f, -0.666999922f, + -0.744033744f, -0.668142041f, + -0.743007952f, -0.669282588f, + -0.741980412f, -0.670421560f, + -0.740951125f, -0.671558955f, + -0.739920095f, -0.672694769f, + -0.738887324f, -0.673829000f, + -0.737852815f, -0.674961646f, + -0.736816569f, -0.676092704f, + -0.735778589f, -0.677222170f, + -0.734738878f, -0.678350043f, + -0.733697438f, -0.679476320f, + -0.732654272f, -0.680600998f, + -0.731609381f, -0.681724074f, + -0.730562769f, -0.682845546f, + -0.729514438f, -0.683965412f, + -0.728464390f, -0.685083668f, + -0.727412629f, -0.686200312f, + -0.726359155f, -0.687315341f, + -0.725303972f, -0.688428753f, + -0.724247083f, -0.689540545f, + -0.723188489f, -0.690650714f, + -0.722128194f, -0.691759258f, + -0.721066199f, -0.692866175f, + -0.720002508f, -0.693971461f, + -0.718937122f, -0.695075114f, + -0.717870045f, -0.696177131f, + -0.716801279f, -0.697277511f, + -0.715730825f, -0.698376249f, + -0.714658688f, -0.699473345f, + -0.713584869f, -0.700568794f, + -0.712509371f, -0.701662595f, + -0.711432196f, -0.702754744f, + -0.710353347f, -0.703845241f, + -0.709272826f, -0.704934080f, + -0.708190637f, -0.706021261f, + -0.707106781f, -0.707106781f, + -0.706021261f, -0.708190637f, + -0.704934080f, -0.709272826f, + -0.703845241f, -0.710353347f, + -0.702754744f, -0.711432196f, + -0.701662595f, -0.712509371f, + -0.700568794f, -0.713584869f, + -0.699473345f, -0.714658688f, + -0.698376249f, -0.715730825f, + -0.697277511f, -0.716801279f, + -0.696177131f, -0.717870045f, + -0.695075114f, -0.718937122f, + -0.693971461f, -0.720002508f, + -0.692866175f, -0.721066199f, + -0.691759258f, -0.722128194f, + -0.690650714f, -0.723188489f, + -0.689540545f, -0.724247083f, + -0.688428753f, -0.725303972f, + -0.687315341f, -0.726359155f, + -0.686200312f, -0.727412629f, + -0.685083668f, -0.728464390f, + -0.683965412f, -0.729514438f, + -0.682845546f, -0.730562769f, + -0.681724074f, -0.731609381f, + -0.680600998f, -0.732654272f, + -0.679476320f, -0.733697438f, + -0.678350043f, -0.734738878f, + -0.677222170f, -0.735778589f, + -0.676092704f, -0.736816569f, + -0.674961646f, -0.737852815f, + -0.673829000f, -0.738887324f, + -0.672694769f, -0.739920095f, + -0.671558955f, -0.740951125f, + -0.670421560f, -0.741980412f, + -0.669282588f, -0.743007952f, + -0.668142041f, -0.744033744f, + -0.666999922f, -0.745057785f, + -0.665856234f, -0.746080074f, + -0.664710978f, -0.747100606f, + -0.663564159f, -0.748119380f, + -0.662415778f, -0.749136395f, + -0.661265838f, -0.750151646f, + -0.660114342f, -0.751165132f, + -0.658961293f, -0.752176850f, + -0.657806693f, -0.753186799f, + -0.656650546f, -0.754194975f, + -0.655492853f, -0.755201377f, + -0.654333618f, -0.756206001f, + -0.653172843f, -0.757208847f, + -0.652010531f, -0.758209910f, + -0.650846685f, -0.759209189f, + -0.649681307f, -0.760206682f, + -0.648514401f, -0.761202385f, + -0.647345969f, -0.762196298f, + -0.646176013f, -0.763188417f, + -0.645004537f, -0.764178741f, + -0.643831543f, -0.765167266f, + -0.642657034f, -0.766153990f, + -0.641481013f, -0.767138912f, + -0.640303482f, -0.768122029f, + -0.639124445f, -0.769103338f, + -0.637943904f, -0.770082837f, + -0.636761861f, -0.771060524f, + -0.635578320f, -0.772036397f, + -0.634393284f, -0.773010453f, + -0.633206755f, -0.773982691f, + -0.632018736f, -0.774953107f, + -0.630829230f, -0.775921699f, + -0.629638239f, -0.776888466f, + -0.628445767f, -0.777853404f, + -0.627251815f, -0.778816512f, + -0.626056388f, -0.779777788f, + -0.624859488f, -0.780737229f, + -0.623661118f, -0.781694832f, + -0.622461279f, -0.782650596f, + -0.621259977f, -0.783604519f, + -0.620057212f, -0.784556597f, + -0.618852988f, -0.785506830f, + -0.617647308f, -0.786455214f, + -0.616440175f, -0.787401747f, + -0.615231591f, -0.788346428f, + -0.614021559f, -0.789289253f, + -0.612810082f, -0.790230221f, + -0.611597164f, -0.791169330f, + -0.610382806f, -0.792106577f, + -0.609167012f, -0.793041960f, + -0.607949785f, -0.793975478f, + -0.606731127f, -0.794907126f, + -0.605511041f, -0.795836905f, + -0.604289531f, -0.796764810f, + -0.603066599f, -0.797690841f, + -0.601842247f, -0.798614995f, + -0.600616479f, -0.799537269f, + -0.599389298f, -0.800457662f, + -0.598160707f, -0.801376172f, + -0.596930708f, -0.802292796f, + -0.595699304f, -0.803207531f, + -0.594466499f, -0.804120377f, + -0.593232295f, -0.805031331f, + -0.591996695f, -0.805940391f, + -0.590759702f, -0.806847554f, + -0.589521319f, -0.807752818f, + -0.588281548f, -0.808656182f, + -0.587040394f, -0.809557642f, + -0.585797857f, -0.810457198f, + -0.584553943f, -0.811354847f, + -0.583308653f, -0.812250587f, + -0.582061990f, -0.813144415f, + -0.580813958f, -0.814036330f, + -0.579564559f, -0.814926329f, + -0.578313796f, -0.815814411f, + -0.577061673f, -0.816700573f, + -0.575808191f, -0.817584813f, + -0.574553355f, -0.818467130f, + -0.573297167f, -0.819347520f, + -0.572039629f, -0.820225983f, + -0.570780746f, -0.821102515f, + -0.569520519f, -0.821977115f, + -0.568258953f, -0.822849781f, + -0.566996049f, -0.823720511f, + -0.565731811f, -0.824589303f, + -0.564466242f, -0.825456154f, + -0.563199344f, -0.826321063f, + -0.561931121f, -0.827184027f, + -0.560661576f, -0.828045045f, + -0.559390712f, -0.828904115f, + -0.558118531f, -0.829761234f, + -0.556845037f, -0.830616400f, + -0.555570233f, -0.831469612f, + -0.554294121f, -0.832320868f, + -0.553016706f, -0.833170165f, + -0.551737988f, -0.834017501f, + -0.550457973f, -0.834862875f, + -0.549176662f, -0.835706284f, + -0.547894059f, -0.836547727f, + -0.546610167f, -0.837387202f, + -0.545324988f, -0.838224706f, + -0.544038527f, -0.839060237f, + -0.542750785f, -0.839893794f, + -0.541461766f, -0.840725375f, + -0.540171473f, -0.841554977f, + -0.538879909f, -0.842382600f, + -0.537587076f, -0.843208240f, + -0.536292979f, -0.844031895f, + -0.534997620f, -0.844853565f, + -0.533701002f, -0.845673247f, + -0.532403128f, -0.846490939f, + -0.531104001f, -0.847306639f, + -0.529803625f, -0.848120345f, + -0.528502002f, -0.848932055f, + -0.527199135f, -0.849741768f, + -0.525895027f, -0.850549481f, + -0.524589683f, -0.851355193f, + -0.523283103f, -0.852158902f, + -0.521975293f, -0.852960605f, + -0.520666254f, -0.853760301f, + -0.519355990f, -0.854557988f, + -0.518044504f, -0.855353665f, + -0.516731799f, -0.856147328f, + -0.515417878f, -0.856938977f, + -0.514102744f, -0.857728610f, + -0.512786401f, -0.858516224f, + -0.511468850f, -0.859301818f, + -0.510150097f, -0.860085390f, + -0.508830143f, -0.860866939f, + -0.507508991f, -0.861646461f, + -0.506186645f, -0.862423956f, + -0.504863109f, -0.863199422f, + -0.503538384f, -0.863972856f, + -0.502212474f, -0.864744258f, + -0.500885383f, -0.865513624f, + -0.499557113f, -0.866280954f, + -0.498227667f, -0.867046246f, + -0.496897049f, -0.867809497f, + -0.495565262f, -0.868570706f, + -0.494232309f, -0.869329871f, + -0.492898192f, -0.870086991f, + -0.491562916f, -0.870842063f, + -0.490226483f, -0.871595087f, + -0.488888897f, -0.872346059f, + -0.487550160f, -0.873094978f, + -0.486210276f, -0.873841843f, + -0.484869248f, -0.874586652f, + -0.483527079f, -0.875329403f, + -0.482183772f, -0.876070094f, + -0.480839331f, -0.876808724f, + -0.479493758f, -0.877545290f, + -0.478147056f, -0.878279792f, + -0.476799230f, -0.879012226f, + -0.475450282f, -0.879742593f, + -0.474100215f, -0.880470889f, + -0.472749032f, -0.881197113f, + -0.471396737f, -0.881921264f, + -0.470043332f, -0.882643340f, + -0.468688822f, -0.883363339f, + -0.467333209f, -0.884081259f, + -0.465976496f, -0.884797098f, + -0.464618686f, -0.885510856f, + -0.463259784f, -0.886222530f, + -0.461899791f, -0.886932119f, + -0.460538711f, -0.887639620f, + -0.459176548f, -0.888345033f, + -0.457813304f, -0.889048356f, + -0.456448982f, -0.889749586f, + -0.455083587f, -0.890448723f, + -0.453717121f, -0.891145765f, + -0.452349587f, -0.891840709f, + -0.450980989f, -0.892533555f, + -0.449611330f, -0.893224301f, + -0.448240612f, -0.893912945f, + -0.446868840f, -0.894599486f, + -0.445496017f, -0.895283921f, + -0.444122145f, -0.895966250f, + -0.442747228f, -0.896646470f, + -0.441371269f, -0.897324581f, + -0.439994271f, -0.898000580f, + -0.438616239f, -0.898674466f, + -0.437237174f, -0.899346237f, + -0.435857080f, -0.900015892f, + -0.434475961f, -0.900683429f, + -0.433093819f, -0.901348847f, + -0.431710658f, -0.902012144f, + -0.430326481f, -0.902673318f, + -0.428941292f, -0.903332368f, + -0.427555093f, -0.903989293f, + -0.426167889f, -0.904644091f, + -0.424779681f, -0.905296759f, + -0.423390474f, -0.905947298f, + -0.422000271f, -0.906595705f, + -0.420609074f, -0.907241978f, + -0.419216888f, -0.907886116f, + -0.417823716f, -0.908528119f, + -0.416429560f, -0.909167983f, + -0.415034424f, -0.909805708f, + -0.413638312f, -0.910441292f, + -0.412241227f, -0.911074734f, + -0.410843171f, -0.911706032f, + -0.409444149f, -0.912335185f, + -0.408044163f, -0.912962190f, + -0.406643217f, -0.913587048f, + -0.405241314f, -0.914209756f, + -0.403838458f, -0.914830312f, + -0.402434651f, -0.915448716f, + -0.401029897f, -0.916064966f, + -0.399624200f, -0.916679060f, + -0.398217562f, -0.917290997f, + -0.396809987f, -0.917900776f, + -0.395401479f, -0.918508394f, + -0.393992040f, -0.919113852f, + -0.392581674f, -0.919717146f, + -0.391170384f, -0.920318277f, + -0.389758174f, -0.920917242f, + -0.388345047f, -0.921514039f, + -0.386931006f, -0.922108669f, + -0.385516054f, -0.922701128f, + -0.384100195f, -0.923291417f, + -0.382683432f, -0.923879533f, + -0.381265769f, -0.924465474f, + -0.379847209f, -0.925049241f, + -0.378427755f, -0.925630831f, + -0.377007410f, -0.926210242f, + -0.375586178f, -0.926787474f, + -0.374164063f, -0.927362526f, + -0.372741067f, -0.927935395f, + -0.371317194f, -0.928506080f, + -0.369892447f, -0.929074581f, + -0.368466830f, -0.929640896f, + -0.367040346f, -0.930205023f, + -0.365612998f, -0.930766961f, + -0.364184790f, -0.931326709f, + -0.362755724f, -0.931884266f, + -0.361325806f, -0.932439629f, + -0.359895037f, -0.932992799f, + -0.358463421f, -0.933543773f, + -0.357030961f, -0.934092550f, + -0.355597662f, -0.934639130f, + -0.354163525f, -0.935183510f, + -0.352728556f, -0.935725689f, + -0.351292756f, -0.936265667f, + -0.349856130f, -0.936803442f, + -0.348418680f, -0.937339012f, + -0.346980411f, -0.937872376f, + -0.345541325f, -0.938403534f, + -0.344101426f, -0.938932484f, + -0.342660717f, -0.939459224f, + -0.341219202f, -0.939983753f, + -0.339776884f, -0.940506071f, + -0.338333767f, -0.941026175f, + -0.336889853f, -0.941544065f, + -0.335445147f, -0.942059740f, + -0.333999651f, -0.942573198f, + -0.332553370f, -0.943084437f, + -0.331106306f, -0.943593458f, + -0.329658463f, -0.944100258f, + -0.328209844f, -0.944604837f, + -0.326760452f, -0.945107193f, + -0.325310292f, -0.945607325f, + -0.323859367f, -0.946105232f, + -0.322407679f, -0.946600913f, + -0.320955232f, -0.947094366f, + -0.319502031f, -0.947585591f, + -0.318048077f, -0.948074586f, + -0.316593376f, -0.948561350f, + -0.315137929f, -0.949045882f, + -0.313681740f, -0.949528181f, + -0.312224814f, -0.950008245f, + -0.310767153f, -0.950486074f, + -0.309308760f, -0.950961666f, + -0.307849640f, -0.951435021f, + -0.306389795f, -0.951906137f, + -0.304929230f, -0.952375013f, + -0.303467947f, -0.952841648f, + -0.302005949f, -0.953306040f, + -0.300543241f, -0.953768190f, + -0.299079826f, -0.954228095f, + -0.297615707f, -0.954685755f, + -0.296150888f, -0.955141168f, + -0.294685372f, -0.955594334f, + -0.293219163f, -0.956045251f, + -0.291752263f, -0.956493919f, + -0.290284677f, -0.956940336f, + -0.288816408f, -0.957384501f, + -0.287347460f, -0.957826413f, + -0.285877835f, -0.958266071f, + -0.284407537f, -0.958703475f, + -0.282936570f, -0.959138622f, + -0.281464938f, -0.959571513f, + -0.279992643f, -0.960002146f, + -0.278519689f, -0.960430519f, + -0.277046080f, -0.960856633f, + -0.275571819f, -0.961280486f, + -0.274096910f, -0.961702077f, + -0.272621355f, -0.962121404f, + -0.271145160f, -0.962538468f, + -0.269668326f, -0.962953267f, + -0.268190857f, -0.963365800f, + -0.266712757f, -0.963776066f, + -0.265234030f, -0.964184064f, + -0.263754679f, -0.964589793f, + -0.262274707f, -0.964993253f, + -0.260794118f, -0.965394442f, + -0.259312915f, -0.965793359f, + -0.257831102f, -0.966190003f, + -0.256348682f, -0.966584374f, + -0.254865660f, -0.966976471f, + -0.253382037f, -0.967366292f, + -0.251897818f, -0.967753837f, + -0.250413007f, -0.968139105f, + -0.248927606f, -0.968522094f, + -0.247441619f, -0.968902805f, + -0.245955050f, -0.969281235f, + -0.244467903f, -0.969657385f, + -0.242980180f, -0.970031253f, + -0.241491885f, -0.970402839f, + -0.240003022f, -0.970772141f, + -0.238513595f, -0.971139158f, + -0.237023606f, -0.971503891f, + -0.235533059f, -0.971866337f, + -0.234041959f, -0.972226497f, + -0.232550307f, -0.972584369f, + -0.231058108f, -0.972939952f, + -0.229565366f, -0.973293246f, + -0.228072083f, -0.973644250f, + -0.226578264f, -0.973992962f, + -0.225083911f, -0.974339383f, + -0.223589029f, -0.974683511f, + -0.222093621f, -0.975025345f, + -0.220597690f, -0.975364885f, + -0.219101240f, -0.975702130f, + -0.217604275f, -0.976037079f, + -0.216106797f, -0.976369731f, + -0.214608811f, -0.976700086f, + -0.213110320f, -0.977028143f, + -0.211611327f, -0.977353900f, + -0.210111837f, -0.977677358f, + -0.208611852f, -0.977998515f, + -0.207111376f, -0.978317371f, + -0.205610413f, -0.978633924f, + -0.204108966f, -0.978948175f, + -0.202607039f, -0.979260123f, + -0.201104635f, -0.979569766f, + -0.199601758f, -0.979877104f, + -0.198098411f, -0.980182136f, + -0.196594598f, -0.980484862f, + -0.195090322f, -0.980785280f, + -0.193585587f, -0.981083391f, + -0.192080397f, -0.981379193f, + -0.190574755f, -0.981672686f, + -0.189068664f, -0.981963869f, + -0.187562129f, -0.982252741f, + -0.186055152f, -0.982539302f, + -0.184547737f, -0.982823551f, + -0.183039888f, -0.983105487f, + -0.181531608f, -0.983385110f, + -0.180022901f, -0.983662419f, + -0.178513771f, -0.983937413f, + -0.177004220f, -0.984210092f, + -0.175494253f, -0.984480455f, + -0.173983873f, -0.984748502f, + -0.172473084f, -0.985014231f, + -0.170961889f, -0.985277642f, + -0.169450291f, -0.985538735f, + -0.167938295f, -0.985797509f, + -0.166425904f, -0.986053963f, + -0.164913120f, -0.986308097f, + -0.163399949f, -0.986559910f, + -0.161886394f, -0.986809402f, + -0.160372457f, -0.987056571f, + -0.158858143f, -0.987301418f, + -0.157343456f, -0.987543942f, + -0.155828398f, -0.987784142f, + -0.154312973f, -0.988022017f, + -0.152797185f, -0.988257568f, + -0.151281038f, -0.988490793f, + -0.149764535f, -0.988721692f, + -0.148247679f, -0.988950265f, + -0.146730474f, -0.989176510f, + -0.145212925f, -0.989400428f, + -0.143695033f, -0.989622017f, + -0.142176804f, -0.989841278f, + -0.140658239f, -0.990058210f, + -0.139139344f, -0.990272812f, + -0.137620122f, -0.990485084f, + -0.136100575f, -0.990695025f, + -0.134580709f, -0.990902635f, + -0.133060525f, -0.991107914f, + -0.131540029f, -0.991310860f, + -0.130019223f, -0.991511473f, + -0.128498111f, -0.991709754f, + -0.126976696f, -0.991905700f, + -0.125454983f, -0.992099313f, + -0.123932975f, -0.992290591f, + -0.122410675f, -0.992479535f, + -0.120888087f, -0.992666142f, + -0.119365215f, -0.992850414f, + -0.117842062f, -0.993032350f, + -0.116318631f, -0.993211949f, + -0.114794927f, -0.993389211f, + -0.113270952f, -0.993564136f, + -0.111746711f, -0.993736722f, + -0.110222207f, -0.993906970f, + -0.108697444f, -0.994074879f, + -0.107172425f, -0.994240449f, + -0.105647154f, -0.994403680f, + -0.104121634f, -0.994564571f, + -0.102595869f, -0.994723121f, + -0.101069863f, -0.994879331f, + -0.099543619f, -0.995033199f, + -0.098017140f, -0.995184727f, + -0.096490431f, -0.995333912f, + -0.094963495f, -0.995480755f, + -0.093436336f, -0.995625256f, + -0.091908956f, -0.995767414f, + -0.090381361f, -0.995907229f, + -0.088853553f, -0.996044701f, + -0.087325535f, -0.996179829f, + -0.085797312f, -0.996312612f, + -0.084268888f, -0.996443051f, + -0.082740265f, -0.996571146f, + -0.081211447f, -0.996696895f, + -0.079682438f, -0.996820299f, + -0.078153242f, -0.996941358f, + -0.076623861f, -0.997060070f, + -0.075094301f, -0.997176437f, + -0.073564564f, -0.997290457f, + -0.072034653f, -0.997402130f, + -0.070504573f, -0.997511456f, + -0.068974328f, -0.997618435f, + -0.067443920f, -0.997723067f, + -0.065913353f, -0.997825350f, + -0.064382631f, -0.997925286f, + -0.062851758f, -0.998022874f, + -0.061320736f, -0.998118113f, + -0.059789571f, -0.998211003f, + -0.058258265f, -0.998301545f, + -0.056726821f, -0.998389737f, + -0.055195244f, -0.998475581f, + -0.053663538f, -0.998559074f, + -0.052131705f, -0.998640218f, + -0.050599749f, -0.998719012f, + -0.049067674f, -0.998795456f, + -0.047535484f, -0.998869550f, + -0.046003182f, -0.998941293f, + -0.044470772f, -0.999010686f, + -0.042938257f, -0.999077728f, + -0.041405641f, -0.999142419f, + -0.039872928f, -0.999204759f, + -0.038340120f, -0.999264747f, + -0.036807223f, -0.999322385f, + -0.035274239f, -0.999377670f, + -0.033741172f, -0.999430605f, + -0.032208025f, -0.999481187f, + -0.030674803f, -0.999529418f, + -0.029141509f, -0.999575296f, + -0.027608146f, -0.999618822f, + -0.026074718f, -0.999659997f, + -0.024541229f, -0.999698819f, + -0.023007681f, -0.999735288f, + -0.021474080f, -0.999769405f, + -0.019940429f, -0.999801170f, + -0.018406730f, -0.999830582f, + -0.016872988f, -0.999857641f, + -0.015339206f, -0.999882347f, + -0.013805389f, -0.999904701f, + -0.012271538f, -0.999924702f, + -0.010737659f, -0.999942350f, + -0.009203755f, -0.999957645f, + -0.007669829f, -0.999970586f, + -0.006135885f, -0.999981175f, + -0.004601926f, -0.999989411f, + -0.003067957f, -0.999995294f, + -0.001533980f, -0.999998823f, + -0.000000000f, -1.000000000f, + 0.001533980f, -0.999998823f, + 0.003067957f, -0.999995294f, + 0.004601926f, -0.999989411f, + 0.006135885f, -0.999981175f, + 0.007669829f, -0.999970586f, + 0.009203755f, -0.999957645f, + 0.010737659f, -0.999942350f, + 0.012271538f, -0.999924702f, + 0.013805389f, -0.999904701f, + 0.015339206f, -0.999882347f, + 0.016872988f, -0.999857641f, + 0.018406730f, -0.999830582f, + 0.019940429f, -0.999801170f, + 0.021474080f, -0.999769405f, + 0.023007681f, -0.999735288f, + 0.024541229f, -0.999698819f, + 0.026074718f, -0.999659997f, + 0.027608146f, -0.999618822f, + 0.029141509f, -0.999575296f, + 0.030674803f, -0.999529418f, + 0.032208025f, -0.999481187f, + 0.033741172f, -0.999430605f, + 0.035274239f, -0.999377670f, + 0.036807223f, -0.999322385f, + 0.038340120f, -0.999264747f, + 0.039872928f, -0.999204759f, + 0.041405641f, -0.999142419f, + 0.042938257f, -0.999077728f, + 0.044470772f, -0.999010686f, + 0.046003182f, -0.998941293f, + 0.047535484f, -0.998869550f, + 0.049067674f, -0.998795456f, + 0.050599749f, -0.998719012f, + 0.052131705f, -0.998640218f, + 0.053663538f, -0.998559074f, + 0.055195244f, -0.998475581f, + 0.056726821f, -0.998389737f, + 0.058258265f, -0.998301545f, + 0.059789571f, -0.998211003f, + 0.061320736f, -0.998118113f, + 0.062851758f, -0.998022874f, + 0.064382631f, -0.997925286f, + 0.065913353f, -0.997825350f, + 0.067443920f, -0.997723067f, + 0.068974328f, -0.997618435f, + 0.070504573f, -0.997511456f, + 0.072034653f, -0.997402130f, + 0.073564564f, -0.997290457f, + 0.075094301f, -0.997176437f, + 0.076623861f, -0.997060070f, + 0.078153242f, -0.996941358f, + 0.079682438f, -0.996820299f, + 0.081211447f, -0.996696895f, + 0.082740265f, -0.996571146f, + 0.084268888f, -0.996443051f, + 0.085797312f, -0.996312612f, + 0.087325535f, -0.996179829f, + 0.088853553f, -0.996044701f, + 0.090381361f, -0.995907229f, + 0.091908956f, -0.995767414f, + 0.093436336f, -0.995625256f, + 0.094963495f, -0.995480755f, + 0.096490431f, -0.995333912f, + 0.098017140f, -0.995184727f, + 0.099543619f, -0.995033199f, + 0.101069863f, -0.994879331f, + 0.102595869f, -0.994723121f, + 0.104121634f, -0.994564571f, + 0.105647154f, -0.994403680f, + 0.107172425f, -0.994240449f, + 0.108697444f, -0.994074879f, + 0.110222207f, -0.993906970f, + 0.111746711f, -0.993736722f, + 0.113270952f, -0.993564136f, + 0.114794927f, -0.993389211f, + 0.116318631f, -0.993211949f, + 0.117842062f, -0.993032350f, + 0.119365215f, -0.992850414f, + 0.120888087f, -0.992666142f, + 0.122410675f, -0.992479535f, + 0.123932975f, -0.992290591f, + 0.125454983f, -0.992099313f, + 0.126976696f, -0.991905700f, + 0.128498111f, -0.991709754f, + 0.130019223f, -0.991511473f, + 0.131540029f, -0.991310860f, + 0.133060525f, -0.991107914f, + 0.134580709f, -0.990902635f, + 0.136100575f, -0.990695025f, + 0.137620122f, -0.990485084f, + 0.139139344f, -0.990272812f, + 0.140658239f, -0.990058210f, + 0.142176804f, -0.989841278f, + 0.143695033f, -0.989622017f, + 0.145212925f, -0.989400428f, + 0.146730474f, -0.989176510f, + 0.148247679f, -0.988950265f, + 0.149764535f, -0.988721692f, + 0.151281038f, -0.988490793f, + 0.152797185f, -0.988257568f, + 0.154312973f, -0.988022017f, + 0.155828398f, -0.987784142f, + 0.157343456f, -0.987543942f, + 0.158858143f, -0.987301418f, + 0.160372457f, -0.987056571f, + 0.161886394f, -0.986809402f, + 0.163399949f, -0.986559910f, + 0.164913120f, -0.986308097f, + 0.166425904f, -0.986053963f, + 0.167938295f, -0.985797509f, + 0.169450291f, -0.985538735f, + 0.170961889f, -0.985277642f, + 0.172473084f, -0.985014231f, + 0.173983873f, -0.984748502f, + 0.175494253f, -0.984480455f, + 0.177004220f, -0.984210092f, + 0.178513771f, -0.983937413f, + 0.180022901f, -0.983662419f, + 0.181531608f, -0.983385110f, + 0.183039888f, -0.983105487f, + 0.184547737f, -0.982823551f, + 0.186055152f, -0.982539302f, + 0.187562129f, -0.982252741f, + 0.189068664f, -0.981963869f, + 0.190574755f, -0.981672686f, + 0.192080397f, -0.981379193f, + 0.193585587f, -0.981083391f, + 0.195090322f, -0.980785280f, + 0.196594598f, -0.980484862f, + 0.198098411f, -0.980182136f, + 0.199601758f, -0.979877104f, + 0.201104635f, -0.979569766f, + 0.202607039f, -0.979260123f, + 0.204108966f, -0.978948175f, + 0.205610413f, -0.978633924f, + 0.207111376f, -0.978317371f, + 0.208611852f, -0.977998515f, + 0.210111837f, -0.977677358f, + 0.211611327f, -0.977353900f, + 0.213110320f, -0.977028143f, + 0.214608811f, -0.976700086f, + 0.216106797f, -0.976369731f, + 0.217604275f, -0.976037079f, + 0.219101240f, -0.975702130f, + 0.220597690f, -0.975364885f, + 0.222093621f, -0.975025345f, + 0.223589029f, -0.974683511f, + 0.225083911f, -0.974339383f, + 0.226578264f, -0.973992962f, + 0.228072083f, -0.973644250f, + 0.229565366f, -0.973293246f, + 0.231058108f, -0.972939952f, + 0.232550307f, -0.972584369f, + 0.234041959f, -0.972226497f, + 0.235533059f, -0.971866337f, + 0.237023606f, -0.971503891f, + 0.238513595f, -0.971139158f, + 0.240003022f, -0.970772141f, + 0.241491885f, -0.970402839f, + 0.242980180f, -0.970031253f, + 0.244467903f, -0.969657385f, + 0.245955050f, -0.969281235f, + 0.247441619f, -0.968902805f, + 0.248927606f, -0.968522094f, + 0.250413007f, -0.968139105f, + 0.251897818f, -0.967753837f, + 0.253382037f, -0.967366292f, + 0.254865660f, -0.966976471f, + 0.256348682f, -0.966584374f, + 0.257831102f, -0.966190003f, + 0.259312915f, -0.965793359f, + 0.260794118f, -0.965394442f, + 0.262274707f, -0.964993253f, + 0.263754679f, -0.964589793f, + 0.265234030f, -0.964184064f, + 0.266712757f, -0.963776066f, + 0.268190857f, -0.963365800f, + 0.269668326f, -0.962953267f, + 0.271145160f, -0.962538468f, + 0.272621355f, -0.962121404f, + 0.274096910f, -0.961702077f, + 0.275571819f, -0.961280486f, + 0.277046080f, -0.960856633f, + 0.278519689f, -0.960430519f, + 0.279992643f, -0.960002146f, + 0.281464938f, -0.959571513f, + 0.282936570f, -0.959138622f, + 0.284407537f, -0.958703475f, + 0.285877835f, -0.958266071f, + 0.287347460f, -0.957826413f, + 0.288816408f, -0.957384501f, + 0.290284677f, -0.956940336f, + 0.291752263f, -0.956493919f, + 0.293219163f, -0.956045251f, + 0.294685372f, -0.955594334f, + 0.296150888f, -0.955141168f, + 0.297615707f, -0.954685755f, + 0.299079826f, -0.954228095f, + 0.300543241f, -0.953768190f, + 0.302005949f, -0.953306040f, + 0.303467947f, -0.952841648f, + 0.304929230f, -0.952375013f, + 0.306389795f, -0.951906137f, + 0.307849640f, -0.951435021f, + 0.309308760f, -0.950961666f, + 0.310767153f, -0.950486074f, + 0.312224814f, -0.950008245f, + 0.313681740f, -0.949528181f, + 0.315137929f, -0.949045882f, + 0.316593376f, -0.948561350f, + 0.318048077f, -0.948074586f, + 0.319502031f, -0.947585591f, + 0.320955232f, -0.947094366f, + 0.322407679f, -0.946600913f, + 0.323859367f, -0.946105232f, + 0.325310292f, -0.945607325f, + 0.326760452f, -0.945107193f, + 0.328209844f, -0.944604837f, + 0.329658463f, -0.944100258f, + 0.331106306f, -0.943593458f, + 0.332553370f, -0.943084437f, + 0.333999651f, -0.942573198f, + 0.335445147f, -0.942059740f, + 0.336889853f, -0.941544065f, + 0.338333767f, -0.941026175f, + 0.339776884f, -0.940506071f, + 0.341219202f, -0.939983753f, + 0.342660717f, -0.939459224f, + 0.344101426f, -0.938932484f, + 0.345541325f, -0.938403534f, + 0.346980411f, -0.937872376f, + 0.348418680f, -0.937339012f, + 0.349856130f, -0.936803442f, + 0.351292756f, -0.936265667f, + 0.352728556f, -0.935725689f, + 0.354163525f, -0.935183510f, + 0.355597662f, -0.934639130f, + 0.357030961f, -0.934092550f, + 0.358463421f, -0.933543773f, + 0.359895037f, -0.932992799f, + 0.361325806f, -0.932439629f, + 0.362755724f, -0.931884266f, + 0.364184790f, -0.931326709f, + 0.365612998f, -0.930766961f, + 0.367040346f, -0.930205023f, + 0.368466830f, -0.929640896f, + 0.369892447f, -0.929074581f, + 0.371317194f, -0.928506080f, + 0.372741067f, -0.927935395f, + 0.374164063f, -0.927362526f, + 0.375586178f, -0.926787474f, + 0.377007410f, -0.926210242f, + 0.378427755f, -0.925630831f, + 0.379847209f, -0.925049241f, + 0.381265769f, -0.924465474f, + 0.382683432f, -0.923879533f, + 0.384100195f, -0.923291417f, + 0.385516054f, -0.922701128f, + 0.386931006f, -0.922108669f, + 0.388345047f, -0.921514039f, + 0.389758174f, -0.920917242f, + 0.391170384f, -0.920318277f, + 0.392581674f, -0.919717146f, + 0.393992040f, -0.919113852f, + 0.395401479f, -0.918508394f, + 0.396809987f, -0.917900776f, + 0.398217562f, -0.917290997f, + 0.399624200f, -0.916679060f, + 0.401029897f, -0.916064966f, + 0.402434651f, -0.915448716f, + 0.403838458f, -0.914830312f, + 0.405241314f, -0.914209756f, + 0.406643217f, -0.913587048f, + 0.408044163f, -0.912962190f, + 0.409444149f, -0.912335185f, + 0.410843171f, -0.911706032f, + 0.412241227f, -0.911074734f, + 0.413638312f, -0.910441292f, + 0.415034424f, -0.909805708f, + 0.416429560f, -0.909167983f, + 0.417823716f, -0.908528119f, + 0.419216888f, -0.907886116f, + 0.420609074f, -0.907241978f, + 0.422000271f, -0.906595705f, + 0.423390474f, -0.905947298f, + 0.424779681f, -0.905296759f, + 0.426167889f, -0.904644091f, + 0.427555093f, -0.903989293f, + 0.428941292f, -0.903332368f, + 0.430326481f, -0.902673318f, + 0.431710658f, -0.902012144f, + 0.433093819f, -0.901348847f, + 0.434475961f, -0.900683429f, + 0.435857080f, -0.900015892f, + 0.437237174f, -0.899346237f, + 0.438616239f, -0.898674466f, + 0.439994271f, -0.898000580f, + 0.441371269f, -0.897324581f, + 0.442747228f, -0.896646470f, + 0.444122145f, -0.895966250f, + 0.445496017f, -0.895283921f, + 0.446868840f, -0.894599486f, + 0.448240612f, -0.893912945f, + 0.449611330f, -0.893224301f, + 0.450980989f, -0.892533555f, + 0.452349587f, -0.891840709f, + 0.453717121f, -0.891145765f, + 0.455083587f, -0.890448723f, + 0.456448982f, -0.889749586f, + 0.457813304f, -0.889048356f, + 0.459176548f, -0.888345033f, + 0.460538711f, -0.887639620f, + 0.461899791f, -0.886932119f, + 0.463259784f, -0.886222530f, + 0.464618686f, -0.885510856f, + 0.465976496f, -0.884797098f, + 0.467333209f, -0.884081259f, + 0.468688822f, -0.883363339f, + 0.470043332f, -0.882643340f, + 0.471396737f, -0.881921264f, + 0.472749032f, -0.881197113f, + 0.474100215f, -0.880470889f, + 0.475450282f, -0.879742593f, + 0.476799230f, -0.879012226f, + 0.478147056f, -0.878279792f, + 0.479493758f, -0.877545290f, + 0.480839331f, -0.876808724f, + 0.482183772f, -0.876070094f, + 0.483527079f, -0.875329403f, + 0.484869248f, -0.874586652f, + 0.486210276f, -0.873841843f, + 0.487550160f, -0.873094978f, + 0.488888897f, -0.872346059f, + 0.490226483f, -0.871595087f, + 0.491562916f, -0.870842063f, + 0.492898192f, -0.870086991f, + 0.494232309f, -0.869329871f, + 0.495565262f, -0.868570706f, + 0.496897049f, -0.867809497f, + 0.498227667f, -0.867046246f, + 0.499557113f, -0.866280954f, + 0.500885383f, -0.865513624f, + 0.502212474f, -0.864744258f, + 0.503538384f, -0.863972856f, + 0.504863109f, -0.863199422f, + 0.506186645f, -0.862423956f, + 0.507508991f, -0.861646461f, + 0.508830143f, -0.860866939f, + 0.510150097f, -0.860085390f, + 0.511468850f, -0.859301818f, + 0.512786401f, -0.858516224f, + 0.514102744f, -0.857728610f, + 0.515417878f, -0.856938977f, + 0.516731799f, -0.856147328f, + 0.518044504f, -0.855353665f, + 0.519355990f, -0.854557988f, + 0.520666254f, -0.853760301f, + 0.521975293f, -0.852960605f, + 0.523283103f, -0.852158902f, + 0.524589683f, -0.851355193f, + 0.525895027f, -0.850549481f, + 0.527199135f, -0.849741768f, + 0.528502002f, -0.848932055f, + 0.529803625f, -0.848120345f, + 0.531104001f, -0.847306639f, + 0.532403128f, -0.846490939f, + 0.533701002f, -0.845673247f, + 0.534997620f, -0.844853565f, + 0.536292979f, -0.844031895f, + 0.537587076f, -0.843208240f, + 0.538879909f, -0.842382600f, + 0.540171473f, -0.841554977f, + 0.541461766f, -0.840725375f, + 0.542750785f, -0.839893794f, + 0.544038527f, -0.839060237f, + 0.545324988f, -0.838224706f, + 0.546610167f, -0.837387202f, + 0.547894059f, -0.836547727f, + 0.549176662f, -0.835706284f, + 0.550457973f, -0.834862875f, + 0.551737988f, -0.834017501f, + 0.553016706f, -0.833170165f, + 0.554294121f, -0.832320868f, + 0.555570233f, -0.831469612f, + 0.556845037f, -0.830616400f, + 0.558118531f, -0.829761234f, + 0.559390712f, -0.828904115f, + 0.560661576f, -0.828045045f, + 0.561931121f, -0.827184027f, + 0.563199344f, -0.826321063f, + 0.564466242f, -0.825456154f, + 0.565731811f, -0.824589303f, + 0.566996049f, -0.823720511f, + 0.568258953f, -0.822849781f, + 0.569520519f, -0.821977115f, + 0.570780746f, -0.821102515f, + 0.572039629f, -0.820225983f, + 0.573297167f, -0.819347520f, + 0.574553355f, -0.818467130f, + 0.575808191f, -0.817584813f, + 0.577061673f, -0.816700573f, + 0.578313796f, -0.815814411f, + 0.579564559f, -0.814926329f, + 0.580813958f, -0.814036330f, + 0.582061990f, -0.813144415f, + 0.583308653f, -0.812250587f, + 0.584553943f, -0.811354847f, + 0.585797857f, -0.810457198f, + 0.587040394f, -0.809557642f, + 0.588281548f, -0.808656182f, + 0.589521319f, -0.807752818f, + 0.590759702f, -0.806847554f, + 0.591996695f, -0.805940391f, + 0.593232295f, -0.805031331f, + 0.594466499f, -0.804120377f, + 0.595699304f, -0.803207531f, + 0.596930708f, -0.802292796f, + 0.598160707f, -0.801376172f, + 0.599389298f, -0.800457662f, + 0.600616479f, -0.799537269f, + 0.601842247f, -0.798614995f, + 0.603066599f, -0.797690841f, + 0.604289531f, -0.796764810f, + 0.605511041f, -0.795836905f, + 0.606731127f, -0.794907126f, + 0.607949785f, -0.793975478f, + 0.609167012f, -0.793041960f, + 0.610382806f, -0.792106577f, + 0.611597164f, -0.791169330f, + 0.612810082f, -0.790230221f, + 0.614021559f, -0.789289253f, + 0.615231591f, -0.788346428f, + 0.616440175f, -0.787401747f, + 0.617647308f, -0.786455214f, + 0.618852988f, -0.785506830f, + 0.620057212f, -0.784556597f, + 0.621259977f, -0.783604519f, + 0.622461279f, -0.782650596f, + 0.623661118f, -0.781694832f, + 0.624859488f, -0.780737229f, + 0.626056388f, -0.779777788f, + 0.627251815f, -0.778816512f, + 0.628445767f, -0.777853404f, + 0.629638239f, -0.776888466f, + 0.630829230f, -0.775921699f, + 0.632018736f, -0.774953107f, + 0.633206755f, -0.773982691f, + 0.634393284f, -0.773010453f, + 0.635578320f, -0.772036397f, + 0.636761861f, -0.771060524f, + 0.637943904f, -0.770082837f, + 0.639124445f, -0.769103338f, + 0.640303482f, -0.768122029f, + 0.641481013f, -0.767138912f, + 0.642657034f, -0.766153990f, + 0.643831543f, -0.765167266f, + 0.645004537f, -0.764178741f, + 0.646176013f, -0.763188417f, + 0.647345969f, -0.762196298f, + 0.648514401f, -0.761202385f, + 0.649681307f, -0.760206682f, + 0.650846685f, -0.759209189f, + 0.652010531f, -0.758209910f, + 0.653172843f, -0.757208847f, + 0.654333618f, -0.756206001f, + 0.655492853f, -0.755201377f, + 0.656650546f, -0.754194975f, + 0.657806693f, -0.753186799f, + 0.658961293f, -0.752176850f, + 0.660114342f, -0.751165132f, + 0.661265838f, -0.750151646f, + 0.662415778f, -0.749136395f, + 0.663564159f, -0.748119380f, + 0.664710978f, -0.747100606f, + 0.665856234f, -0.746080074f, + 0.666999922f, -0.745057785f, + 0.668142041f, -0.744033744f, + 0.669282588f, -0.743007952f, + 0.670421560f, -0.741980412f, + 0.671558955f, -0.740951125f, + 0.672694769f, -0.739920095f, + 0.673829000f, -0.738887324f, + 0.674961646f, -0.737852815f, + 0.676092704f, -0.736816569f, + 0.677222170f, -0.735778589f, + 0.678350043f, -0.734738878f, + 0.679476320f, -0.733697438f, + 0.680600998f, -0.732654272f, + 0.681724074f, -0.731609381f, + 0.682845546f, -0.730562769f, + 0.683965412f, -0.729514438f, + 0.685083668f, -0.728464390f, + 0.686200312f, -0.727412629f, + 0.687315341f, -0.726359155f, + 0.688428753f, -0.725303972f, + 0.689540545f, -0.724247083f, + 0.690650714f, -0.723188489f, + 0.691759258f, -0.722128194f, + 0.692866175f, -0.721066199f, + 0.693971461f, -0.720002508f, + 0.695075114f, -0.718937122f, + 0.696177131f, -0.717870045f, + 0.697277511f, -0.716801279f, + 0.698376249f, -0.715730825f, + 0.699473345f, -0.714658688f, + 0.700568794f, -0.713584869f, + 0.701662595f, -0.712509371f, + 0.702754744f, -0.711432196f, + 0.703845241f, -0.710353347f, + 0.704934080f, -0.709272826f, + 0.706021261f, -0.708190637f, + 0.707106781f, -0.707106781f, + 0.708190637f, -0.706021261f, + 0.709272826f, -0.704934080f, + 0.710353347f, -0.703845241f, + 0.711432196f, -0.702754744f, + 0.712509371f, -0.701662595f, + 0.713584869f, -0.700568794f, + 0.714658688f, -0.699473345f, + 0.715730825f, -0.698376249f, + 0.716801279f, -0.697277511f, + 0.717870045f, -0.696177131f, + 0.718937122f, -0.695075114f, + 0.720002508f, -0.693971461f, + 0.721066199f, -0.692866175f, + 0.722128194f, -0.691759258f, + 0.723188489f, -0.690650714f, + 0.724247083f, -0.689540545f, + 0.725303972f, -0.688428753f, + 0.726359155f, -0.687315341f, + 0.727412629f, -0.686200312f, + 0.728464390f, -0.685083668f, + 0.729514438f, -0.683965412f, + 0.730562769f, -0.682845546f, + 0.731609381f, -0.681724074f, + 0.732654272f, -0.680600998f, + 0.733697438f, -0.679476320f, + 0.734738878f, -0.678350043f, + 0.735778589f, -0.677222170f, + 0.736816569f, -0.676092704f, + 0.737852815f, -0.674961646f, + 0.738887324f, -0.673829000f, + 0.739920095f, -0.672694769f, + 0.740951125f, -0.671558955f, + 0.741980412f, -0.670421560f, + 0.743007952f, -0.669282588f, + 0.744033744f, -0.668142041f, + 0.745057785f, -0.666999922f, + 0.746080074f, -0.665856234f, + 0.747100606f, -0.664710978f, + 0.748119380f, -0.663564159f, + 0.749136395f, -0.662415778f, + 0.750151646f, -0.661265838f, + 0.751165132f, -0.660114342f, + 0.752176850f, -0.658961293f, + 0.753186799f, -0.657806693f, + 0.754194975f, -0.656650546f, + 0.755201377f, -0.655492853f, + 0.756206001f, -0.654333618f, + 0.757208847f, -0.653172843f, + 0.758209910f, -0.652010531f, + 0.759209189f, -0.650846685f, + 0.760206682f, -0.649681307f, + 0.761202385f, -0.648514401f, + 0.762196298f, -0.647345969f, + 0.763188417f, -0.646176013f, + 0.764178741f, -0.645004537f, + 0.765167266f, -0.643831543f, + 0.766153990f, -0.642657034f, + 0.767138912f, -0.641481013f, + 0.768122029f, -0.640303482f, + 0.769103338f, -0.639124445f, + 0.770082837f, -0.637943904f, + 0.771060524f, -0.636761861f, + 0.772036397f, -0.635578320f, + 0.773010453f, -0.634393284f, + 0.773982691f, -0.633206755f, + 0.774953107f, -0.632018736f, + 0.775921699f, -0.630829230f, + 0.776888466f, -0.629638239f, + 0.777853404f, -0.628445767f, + 0.778816512f, -0.627251815f, + 0.779777788f, -0.626056388f, + 0.780737229f, -0.624859488f, + 0.781694832f, -0.623661118f, + 0.782650596f, -0.622461279f, + 0.783604519f, -0.621259977f, + 0.784556597f, -0.620057212f, + 0.785506830f, -0.618852988f, + 0.786455214f, -0.617647308f, + 0.787401747f, -0.616440175f, + 0.788346428f, -0.615231591f, + 0.789289253f, -0.614021559f, + 0.790230221f, -0.612810082f, + 0.791169330f, -0.611597164f, + 0.792106577f, -0.610382806f, + 0.793041960f, -0.609167012f, + 0.793975478f, -0.607949785f, + 0.794907126f, -0.606731127f, + 0.795836905f, -0.605511041f, + 0.796764810f, -0.604289531f, + 0.797690841f, -0.603066599f, + 0.798614995f, -0.601842247f, + 0.799537269f, -0.600616479f, + 0.800457662f, -0.599389298f, + 0.801376172f, -0.598160707f, + 0.802292796f, -0.596930708f, + 0.803207531f, -0.595699304f, + 0.804120377f, -0.594466499f, + 0.805031331f, -0.593232295f, + 0.805940391f, -0.591996695f, + 0.806847554f, -0.590759702f, + 0.807752818f, -0.589521319f, + 0.808656182f, -0.588281548f, + 0.809557642f, -0.587040394f, + 0.810457198f, -0.585797857f, + 0.811354847f, -0.584553943f, + 0.812250587f, -0.583308653f, + 0.813144415f, -0.582061990f, + 0.814036330f, -0.580813958f, + 0.814926329f, -0.579564559f, + 0.815814411f, -0.578313796f, + 0.816700573f, -0.577061673f, + 0.817584813f, -0.575808191f, + 0.818467130f, -0.574553355f, + 0.819347520f, -0.573297167f, + 0.820225983f, -0.572039629f, + 0.821102515f, -0.570780746f, + 0.821977115f, -0.569520519f, + 0.822849781f, -0.568258953f, + 0.823720511f, -0.566996049f, + 0.824589303f, -0.565731811f, + 0.825456154f, -0.564466242f, + 0.826321063f, -0.563199344f, + 0.827184027f, -0.561931121f, + 0.828045045f, -0.560661576f, + 0.828904115f, -0.559390712f, + 0.829761234f, -0.558118531f, + 0.830616400f, -0.556845037f, + 0.831469612f, -0.555570233f, + 0.832320868f, -0.554294121f, + 0.833170165f, -0.553016706f, + 0.834017501f, -0.551737988f, + 0.834862875f, -0.550457973f, + 0.835706284f, -0.549176662f, + 0.836547727f, -0.547894059f, + 0.837387202f, -0.546610167f, + 0.838224706f, -0.545324988f, + 0.839060237f, -0.544038527f, + 0.839893794f, -0.542750785f, + 0.840725375f, -0.541461766f, + 0.841554977f, -0.540171473f, + 0.842382600f, -0.538879909f, + 0.843208240f, -0.537587076f, + 0.844031895f, -0.536292979f, + 0.844853565f, -0.534997620f, + 0.845673247f, -0.533701002f, + 0.846490939f, -0.532403128f, + 0.847306639f, -0.531104001f, + 0.848120345f, -0.529803625f, + 0.848932055f, -0.528502002f, + 0.849741768f, -0.527199135f, + 0.850549481f, -0.525895027f, + 0.851355193f, -0.524589683f, + 0.852158902f, -0.523283103f, + 0.852960605f, -0.521975293f, + 0.853760301f, -0.520666254f, + 0.854557988f, -0.519355990f, + 0.855353665f, -0.518044504f, + 0.856147328f, -0.516731799f, + 0.856938977f, -0.515417878f, + 0.857728610f, -0.514102744f, + 0.858516224f, -0.512786401f, + 0.859301818f, -0.511468850f, + 0.860085390f, -0.510150097f, + 0.860866939f, -0.508830143f, + 0.861646461f, -0.507508991f, + 0.862423956f, -0.506186645f, + 0.863199422f, -0.504863109f, + 0.863972856f, -0.503538384f, + 0.864744258f, -0.502212474f, + 0.865513624f, -0.500885383f, + 0.866280954f, -0.499557113f, + 0.867046246f, -0.498227667f, + 0.867809497f, -0.496897049f, + 0.868570706f, -0.495565262f, + 0.869329871f, -0.494232309f, + 0.870086991f, -0.492898192f, + 0.870842063f, -0.491562916f, + 0.871595087f, -0.490226483f, + 0.872346059f, -0.488888897f, + 0.873094978f, -0.487550160f, + 0.873841843f, -0.486210276f, + 0.874586652f, -0.484869248f, + 0.875329403f, -0.483527079f, + 0.876070094f, -0.482183772f, + 0.876808724f, -0.480839331f, + 0.877545290f, -0.479493758f, + 0.878279792f, -0.478147056f, + 0.879012226f, -0.476799230f, + 0.879742593f, -0.475450282f, + 0.880470889f, -0.474100215f, + 0.881197113f, -0.472749032f, + 0.881921264f, -0.471396737f, + 0.882643340f, -0.470043332f, + 0.883363339f, -0.468688822f, + 0.884081259f, -0.467333209f, + 0.884797098f, -0.465976496f, + 0.885510856f, -0.464618686f, + 0.886222530f, -0.463259784f, + 0.886932119f, -0.461899791f, + 0.887639620f, -0.460538711f, + 0.888345033f, -0.459176548f, + 0.889048356f, -0.457813304f, + 0.889749586f, -0.456448982f, + 0.890448723f, -0.455083587f, + 0.891145765f, -0.453717121f, + 0.891840709f, -0.452349587f, + 0.892533555f, -0.450980989f, + 0.893224301f, -0.449611330f, + 0.893912945f, -0.448240612f, + 0.894599486f, -0.446868840f, + 0.895283921f, -0.445496017f, + 0.895966250f, -0.444122145f, + 0.896646470f, -0.442747228f, + 0.897324581f, -0.441371269f, + 0.898000580f, -0.439994271f, + 0.898674466f, -0.438616239f, + 0.899346237f, -0.437237174f, + 0.900015892f, -0.435857080f, + 0.900683429f, -0.434475961f, + 0.901348847f, -0.433093819f, + 0.902012144f, -0.431710658f, + 0.902673318f, -0.430326481f, + 0.903332368f, -0.428941292f, + 0.903989293f, -0.427555093f, + 0.904644091f, -0.426167889f, + 0.905296759f, -0.424779681f, + 0.905947298f, -0.423390474f, + 0.906595705f, -0.422000271f, + 0.907241978f, -0.420609074f, + 0.907886116f, -0.419216888f, + 0.908528119f, -0.417823716f, + 0.909167983f, -0.416429560f, + 0.909805708f, -0.415034424f, + 0.910441292f, -0.413638312f, + 0.911074734f, -0.412241227f, + 0.911706032f, -0.410843171f, + 0.912335185f, -0.409444149f, + 0.912962190f, -0.408044163f, + 0.913587048f, -0.406643217f, + 0.914209756f, -0.405241314f, + 0.914830312f, -0.403838458f, + 0.915448716f, -0.402434651f, + 0.916064966f, -0.401029897f, + 0.916679060f, -0.399624200f, + 0.917290997f, -0.398217562f, + 0.917900776f, -0.396809987f, + 0.918508394f, -0.395401479f, + 0.919113852f, -0.393992040f, + 0.919717146f, -0.392581674f, + 0.920318277f, -0.391170384f, + 0.920917242f, -0.389758174f, + 0.921514039f, -0.388345047f, + 0.922108669f, -0.386931006f, + 0.922701128f, -0.385516054f, + 0.923291417f, -0.384100195f, + 0.923879533f, -0.382683432f, + 0.924465474f, -0.381265769f, + 0.925049241f, -0.379847209f, + 0.925630831f, -0.378427755f, + 0.926210242f, -0.377007410f, + 0.926787474f, -0.375586178f, + 0.927362526f, -0.374164063f, + 0.927935395f, -0.372741067f, + 0.928506080f, -0.371317194f, + 0.929074581f, -0.369892447f, + 0.929640896f, -0.368466830f, + 0.930205023f, -0.367040346f, + 0.930766961f, -0.365612998f, + 0.931326709f, -0.364184790f, + 0.931884266f, -0.362755724f, + 0.932439629f, -0.361325806f, + 0.932992799f, -0.359895037f, + 0.933543773f, -0.358463421f, + 0.934092550f, -0.357030961f, + 0.934639130f, -0.355597662f, + 0.935183510f, -0.354163525f, + 0.935725689f, -0.352728556f, + 0.936265667f, -0.351292756f, + 0.936803442f, -0.349856130f, + 0.937339012f, -0.348418680f, + 0.937872376f, -0.346980411f, + 0.938403534f, -0.345541325f, + 0.938932484f, -0.344101426f, + 0.939459224f, -0.342660717f, + 0.939983753f, -0.341219202f, + 0.940506071f, -0.339776884f, + 0.941026175f, -0.338333767f, + 0.941544065f, -0.336889853f, + 0.942059740f, -0.335445147f, + 0.942573198f, -0.333999651f, + 0.943084437f, -0.332553370f, + 0.943593458f, -0.331106306f, + 0.944100258f, -0.329658463f, + 0.944604837f, -0.328209844f, + 0.945107193f, -0.326760452f, + 0.945607325f, -0.325310292f, + 0.946105232f, -0.323859367f, + 0.946600913f, -0.322407679f, + 0.947094366f, -0.320955232f, + 0.947585591f, -0.319502031f, + 0.948074586f, -0.318048077f, + 0.948561350f, -0.316593376f, + 0.949045882f, -0.315137929f, + 0.949528181f, -0.313681740f, + 0.950008245f, -0.312224814f, + 0.950486074f, -0.310767153f, + 0.950961666f, -0.309308760f, + 0.951435021f, -0.307849640f, + 0.951906137f, -0.306389795f, + 0.952375013f, -0.304929230f, + 0.952841648f, -0.303467947f, + 0.953306040f, -0.302005949f, + 0.953768190f, -0.300543241f, + 0.954228095f, -0.299079826f, + 0.954685755f, -0.297615707f, + 0.955141168f, -0.296150888f, + 0.955594334f, -0.294685372f, + 0.956045251f, -0.293219163f, + 0.956493919f, -0.291752263f, + 0.956940336f, -0.290284677f, + 0.957384501f, -0.288816408f, + 0.957826413f, -0.287347460f, + 0.958266071f, -0.285877835f, + 0.958703475f, -0.284407537f, + 0.959138622f, -0.282936570f, + 0.959571513f, -0.281464938f, + 0.960002146f, -0.279992643f, + 0.960430519f, -0.278519689f, + 0.960856633f, -0.277046080f, + 0.961280486f, -0.275571819f, + 0.961702077f, -0.274096910f, + 0.962121404f, -0.272621355f, + 0.962538468f, -0.271145160f, + 0.962953267f, -0.269668326f, + 0.963365800f, -0.268190857f, + 0.963776066f, -0.266712757f, + 0.964184064f, -0.265234030f, + 0.964589793f, -0.263754679f, + 0.964993253f, -0.262274707f, + 0.965394442f, -0.260794118f, + 0.965793359f, -0.259312915f, + 0.966190003f, -0.257831102f, + 0.966584374f, -0.256348682f, + 0.966976471f, -0.254865660f, + 0.967366292f, -0.253382037f, + 0.967753837f, -0.251897818f, + 0.968139105f, -0.250413007f, + 0.968522094f, -0.248927606f, + 0.968902805f, -0.247441619f, + 0.969281235f, -0.245955050f, + 0.969657385f, -0.244467903f, + 0.970031253f, -0.242980180f, + 0.970402839f, -0.241491885f, + 0.970772141f, -0.240003022f, + 0.971139158f, -0.238513595f, + 0.971503891f, -0.237023606f, + 0.971866337f, -0.235533059f, + 0.972226497f, -0.234041959f, + 0.972584369f, -0.232550307f, + 0.972939952f, -0.231058108f, + 0.973293246f, -0.229565366f, + 0.973644250f, -0.228072083f, + 0.973992962f, -0.226578264f, + 0.974339383f, -0.225083911f, + 0.974683511f, -0.223589029f, + 0.975025345f, -0.222093621f, + 0.975364885f, -0.220597690f, + 0.975702130f, -0.219101240f, + 0.976037079f, -0.217604275f, + 0.976369731f, -0.216106797f, + 0.976700086f, -0.214608811f, + 0.977028143f, -0.213110320f, + 0.977353900f, -0.211611327f, + 0.977677358f, -0.210111837f, + 0.977998515f, -0.208611852f, + 0.978317371f, -0.207111376f, + 0.978633924f, -0.205610413f, + 0.978948175f, -0.204108966f, + 0.979260123f, -0.202607039f, + 0.979569766f, -0.201104635f, + 0.979877104f, -0.199601758f, + 0.980182136f, -0.198098411f, + 0.980484862f, -0.196594598f, + 0.980785280f, -0.195090322f, + 0.981083391f, -0.193585587f, + 0.981379193f, -0.192080397f, + 0.981672686f, -0.190574755f, + 0.981963869f, -0.189068664f, + 0.982252741f, -0.187562129f, + 0.982539302f, -0.186055152f, + 0.982823551f, -0.184547737f, + 0.983105487f, -0.183039888f, + 0.983385110f, -0.181531608f, + 0.983662419f, -0.180022901f, + 0.983937413f, -0.178513771f, + 0.984210092f, -0.177004220f, + 0.984480455f, -0.175494253f, + 0.984748502f, -0.173983873f, + 0.985014231f, -0.172473084f, + 0.985277642f, -0.170961889f, + 0.985538735f, -0.169450291f, + 0.985797509f, -0.167938295f, + 0.986053963f, -0.166425904f, + 0.986308097f, -0.164913120f, + 0.986559910f, -0.163399949f, + 0.986809402f, -0.161886394f, + 0.987056571f, -0.160372457f, + 0.987301418f, -0.158858143f, + 0.987543942f, -0.157343456f, + 0.987784142f, -0.155828398f, + 0.988022017f, -0.154312973f, + 0.988257568f, -0.152797185f, + 0.988490793f, -0.151281038f, + 0.988721692f, -0.149764535f, + 0.988950265f, -0.148247679f, + 0.989176510f, -0.146730474f, + 0.989400428f, -0.145212925f, + 0.989622017f, -0.143695033f, + 0.989841278f, -0.142176804f, + 0.990058210f, -0.140658239f, + 0.990272812f, -0.139139344f, + 0.990485084f, -0.137620122f, + 0.990695025f, -0.136100575f, + 0.990902635f, -0.134580709f, + 0.991107914f, -0.133060525f, + 0.991310860f, -0.131540029f, + 0.991511473f, -0.130019223f, + 0.991709754f, -0.128498111f, + 0.991905700f, -0.126976696f, + 0.992099313f, -0.125454983f, + 0.992290591f, -0.123932975f, + 0.992479535f, -0.122410675f, + 0.992666142f, -0.120888087f, + 0.992850414f, -0.119365215f, + 0.993032350f, -0.117842062f, + 0.993211949f, -0.116318631f, + 0.993389211f, -0.114794927f, + 0.993564136f, -0.113270952f, + 0.993736722f, -0.111746711f, + 0.993906970f, -0.110222207f, + 0.994074879f, -0.108697444f, + 0.994240449f, -0.107172425f, + 0.994403680f, -0.105647154f, + 0.994564571f, -0.104121634f, + 0.994723121f, -0.102595869f, + 0.994879331f, -0.101069863f, + 0.995033199f, -0.099543619f, + 0.995184727f, -0.098017140f, + 0.995333912f, -0.096490431f, + 0.995480755f, -0.094963495f, + 0.995625256f, -0.093436336f, + 0.995767414f, -0.091908956f, + 0.995907229f, -0.090381361f, + 0.996044701f, -0.088853553f, + 0.996179829f, -0.087325535f, + 0.996312612f, -0.085797312f, + 0.996443051f, -0.084268888f, + 0.996571146f, -0.082740265f, + 0.996696895f, -0.081211447f, + 0.996820299f, -0.079682438f, + 0.996941358f, -0.078153242f, + 0.997060070f, -0.076623861f, + 0.997176437f, -0.075094301f, + 0.997290457f, -0.073564564f, + 0.997402130f, -0.072034653f, + 0.997511456f, -0.070504573f, + 0.997618435f, -0.068974328f, + 0.997723067f, -0.067443920f, + 0.997825350f, -0.065913353f, + 0.997925286f, -0.064382631f, + 0.998022874f, -0.062851758f, + 0.998118113f, -0.061320736f, + 0.998211003f, -0.059789571f, + 0.998301545f, -0.058258265f, + 0.998389737f, -0.056726821f, + 0.998475581f, -0.055195244f, + 0.998559074f, -0.053663538f, + 0.998640218f, -0.052131705f, + 0.998719012f, -0.050599749f, + 0.998795456f, -0.049067674f, + 0.998869550f, -0.047535484f, + 0.998941293f, -0.046003182f, + 0.999010686f, -0.044470772f, + 0.999077728f, -0.042938257f, + 0.999142419f, -0.041405641f, + 0.999204759f, -0.039872928f, + 0.999264747f, -0.038340120f, + 0.999322385f, -0.036807223f, + 0.999377670f, -0.035274239f, + 0.999430605f, -0.033741172f, + 0.999481187f, -0.032208025f, + 0.999529418f, -0.030674803f, + 0.999575296f, -0.029141509f, + 0.999618822f, -0.027608146f, + 0.999659997f, -0.026074718f, + 0.999698819f, -0.024541229f, + 0.999735288f, -0.023007681f, + 0.999769405f, -0.021474080f, + 0.999801170f, -0.019940429f, + 0.999830582f, -0.018406730f, + 0.999857641f, -0.016872988f, + 0.999882347f, -0.015339206f, + 0.999904701f, -0.013805389f, + 0.999924702f, -0.012271538f, + 0.999942350f, -0.010737659f, + 0.999957645f, -0.009203755f, + 0.999970586f, -0.007669829f, + 0.999981175f, -0.006135885f, + 0.999989411f, -0.004601926f, + 0.999995294f, -0.003067957f, + 0.999998823f, -0.001533980f +}; + +/* +* @brief Q31 Twiddle factors Table +*/ + + +/** +* \par +* Example code for Q31 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefQ31[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefQ31[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 16 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to Q31(Fixed point 1.31): +* round(twiddleCoefQ31(i) * pow(2, 31)) +* +*/ +const q31_t twiddleCoef_16_q31[24] = { + 0x7FFFFFFF, 0x00000000, + 0x7641AF3C, 0x30FBC54D, + 0x5A82799A, 0x5A82799A, + 0x30FBC54D, 0x7641AF3C, + 0x00000000, 0x7FFFFFFF, + 0xCF043AB2, 0x7641AF3C, + 0xA57D8666, 0x5A82799A, + 0x89BE50C3, 0x30FBC54D, + 0x80000000, 0x00000000, + 0x89BE50C3, 0xCF043AB2, + 0xA57D8666, 0xA57D8666, + 0xCF043AB2, 0x89BE50C3 +}; + +/** +* \par +* Example code for Q31 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefQ31[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefQ31[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 32 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to Q31(Fixed point 1.31): +* round(twiddleCoefQ31(i) * pow(2, 31)) +* +*/ +const q31_t twiddleCoef_32_q31[48] = { + 0x7FFFFFFF, 0x00000000, + 0x7D8A5F3F, 0x18F8B83C, + 0x7641AF3C, 0x30FBC54D, + 0x6A6D98A4, 0x471CECE6, + 0x5A82799A, 0x5A82799A, + 0x471CECE6, 0x6A6D98A4, + 0x30FBC54D, 0x7641AF3C, + 0x18F8B83C, 0x7D8A5F3F, + 0x00000000, 0x7FFFFFFF, + 0xE70747C3, 0x7D8A5F3F, + 0xCF043AB2, 0x7641AF3C, + 0xB8E31319, 0x6A6D98A4, + 0xA57D8666, 0x5A82799A, + 0x9592675B, 0x471CECE6, + 0x89BE50C3, 0x30FBC54D, + 0x8275A0C0, 0x18F8B83C, + 0x80000000, 0x00000000, + 0x8275A0C0, 0xE70747C3, + 0x89BE50C3, 0xCF043AB2, + 0x9592675B, 0xB8E31319, + 0xA57D8666, 0xA57D8666, + 0xB8E31319, 0x9592675B, + 0xCF043AB2, 0x89BE50C3, + 0xE70747C3, 0x8275A0C0 +}; + +/** +* \par +* Example code for Q31 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefQ31[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefQ31[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 64 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to Q31(Fixed point 1.31): +* round(twiddleCoefQ31(i) * pow(2, 31)) +* +*/ +const q31_t twiddleCoef_64_q31[96] = { + 0x7FFFFFFF, 0x00000000, + 0x7F62368F, 0x0C8BD35E, + 0x7D8A5F3F, 0x18F8B83C, + 0x7A7D055B, 0x25280C5D, + 0x7641AF3C, 0x30FBC54D, + 0x70E2CBC6, 0x3C56BA70, + 0x6A6D98A4, 0x471CECE6, + 0x62F201AC, 0x5133CC94, + 0x5A82799A, 0x5A82799A, + 0x5133CC94, 0x62F201AC, + 0x471CECE6, 0x6A6D98A4, + 0x3C56BA70, 0x70E2CBC6, + 0x30FBC54D, 0x7641AF3C, + 0x25280C5D, 0x7A7D055B, + 0x18F8B83C, 0x7D8A5F3F, + 0x0C8BD35E, 0x7F62368F, + 0x00000000, 0x7FFFFFFF, + 0xF3742CA1, 0x7F62368F, + 0xE70747C3, 0x7D8A5F3F, + 0xDAD7F3A2, 0x7A7D055B, + 0xCF043AB2, 0x7641AF3C, + 0xC3A9458F, 0x70E2CBC6, + 0xB8E31319, 0x6A6D98A4, + 0xAECC336B, 0x62F201AC, + 0xA57D8666, 0x5A82799A, + 0x9D0DFE53, 0x5133CC94, + 0x9592675B, 0x471CECE6, + 0x8F1D343A, 0x3C56BA70, + 0x89BE50C3, 0x30FBC54D, + 0x8582FAA4, 0x25280C5D, + 0x8275A0C0, 0x18F8B83C, + 0x809DC970, 0x0C8BD35E, + 0x80000000, 0x00000000, + 0x809DC970, 0xF3742CA1, + 0x8275A0C0, 0xE70747C3, + 0x8582FAA4, 0xDAD7F3A2, + 0x89BE50C3, 0xCF043AB2, + 0x8F1D343A, 0xC3A9458F, + 0x9592675B, 0xB8E31319, + 0x9D0DFE53, 0xAECC336B, + 0xA57D8666, 0xA57D8666, + 0xAECC336B, 0x9D0DFE53, + 0xB8E31319, 0x9592675B, + 0xC3A9458F, 0x8F1D343A, + 0xCF043AB2, 0x89BE50C3, + 0xDAD7F3A2, 0x8582FAA4, + 0xE70747C3, 0x8275A0C0, + 0xF3742CA1, 0x809DC970 +}; + +/** +* \par +* Example code for Q31 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefQ31[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefQ31[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 128 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to Q31(Fixed point 1.31): +* round(twiddleCoefQ31(i) * pow(2, 31)) +* +*/ +const q31_t twiddleCoef_128_q31[192] = { + 0x7FFFFFFF, 0x00000000, + 0x7FD8878D, 0x0647D97C, + 0x7F62368F, 0x0C8BD35E, + 0x7E9D55FC, 0x12C8106E, + 0x7D8A5F3F, 0x18F8B83C, + 0x7C29FBEE, 0x1F19F97B, + 0x7A7D055B, 0x25280C5D, + 0x78848413, 0x2B1F34EB, + 0x7641AF3C, 0x30FBC54D, + 0x73B5EBD0, 0x36BA2013, + 0x70E2CBC6, 0x3C56BA70, + 0x6DCA0D14, 0x41CE1E64, + 0x6A6D98A4, 0x471CECE6, + 0x66CF811F, 0x4C3FDFF3, + 0x62F201AC, 0x5133CC94, + 0x5ED77C89, 0x55F5A4D2, + 0x5A82799A, 0x5A82799A, + 0x55F5A4D2, 0x5ED77C89, + 0x5133CC94, 0x62F201AC, + 0x4C3FDFF3, 0x66CF811F, + 0x471CECE6, 0x6A6D98A4, + 0x41CE1E64, 0x6DCA0D14, + 0x3C56BA70, 0x70E2CBC6, + 0x36BA2013, 0x73B5EBD0, + 0x30FBC54D, 0x7641AF3C, + 0x2B1F34EB, 0x78848413, + 0x25280C5D, 0x7A7D055B, + 0x1F19F97B, 0x7C29FBEE, + 0x18F8B83C, 0x7D8A5F3F, + 0x12C8106E, 0x7E9D55FC, + 0x0C8BD35E, 0x7F62368F, + 0x0647D97C, 0x7FD8878D, + 0x00000000, 0x7FFFFFFF, + 0xF9B82683, 0x7FD8878D, + 0xF3742CA1, 0x7F62368F, + 0xED37EF91, 0x7E9D55FC, + 0xE70747C3, 0x7D8A5F3F, + 0xE0E60684, 0x7C29FBEE, + 0xDAD7F3A2, 0x7A7D055B, + 0xD4E0CB14, 0x78848413, + 0xCF043AB2, 0x7641AF3C, + 0xC945DFEC, 0x73B5EBD0, + 0xC3A9458F, 0x70E2CBC6, + 0xBE31E19B, 0x6DCA0D14, + 0xB8E31319, 0x6A6D98A4, + 0xB3C0200C, 0x66CF811F, + 0xAECC336B, 0x62F201AC, + 0xAA0A5B2D, 0x5ED77C89, + 0xA57D8666, 0x5A82799A, + 0xA1288376, 0x55F5A4D2, + 0x9D0DFE53, 0x5133CC94, + 0x99307EE0, 0x4C3FDFF3, + 0x9592675B, 0x471CECE6, + 0x9235F2EB, 0x41CE1E64, + 0x8F1D343A, 0x3C56BA70, + 0x8C4A142F, 0x36BA2013, + 0x89BE50C3, 0x30FBC54D, + 0x877B7BEC, 0x2B1F34EB, + 0x8582FAA4, 0x25280C5D, + 0x83D60411, 0x1F19F97B, + 0x8275A0C0, 0x18F8B83C, + 0x8162AA03, 0x12C8106E, + 0x809DC970, 0x0C8BD35E, + 0x80277872, 0x0647D97C, + 0x80000000, 0x00000000, + 0x80277872, 0xF9B82683, + 0x809DC970, 0xF3742CA1, + 0x8162AA03, 0xED37EF91, + 0x8275A0C0, 0xE70747C3, + 0x83D60411, 0xE0E60684, + 0x8582FAA4, 0xDAD7F3A2, + 0x877B7BEC, 0xD4E0CB14, + 0x89BE50C3, 0xCF043AB2, + 0x8C4A142F, 0xC945DFEC, + 0x8F1D343A, 0xC3A9458F, + 0x9235F2EB, 0xBE31E19B, + 0x9592675B, 0xB8E31319, + 0x99307EE0, 0xB3C0200C, + 0x9D0DFE53, 0xAECC336B, + 0xA1288376, 0xAA0A5B2D, + 0xA57D8666, 0xA57D8666, + 0xAA0A5B2D, 0xA1288376, + 0xAECC336B, 0x9D0DFE53, + 0xB3C0200C, 0x99307EE0, + 0xB8E31319, 0x9592675B, + 0xBE31E19B, 0x9235F2EB, + 0xC3A9458F, 0x8F1D343A, + 0xC945DFEC, 0x8C4A142F, + 0xCF043AB2, 0x89BE50C3, + 0xD4E0CB14, 0x877B7BEC, + 0xDAD7F3A2, 0x8582FAA4, + 0xE0E60684, 0x83D60411, + 0xE70747C3, 0x8275A0C0, + 0xED37EF91, 0x8162AA03, + 0xF3742CA1, 0x809DC970, + 0xF9B82683, 0x80277872 +}; + +/** +* \par +* Example code for Q31 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefQ31[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefQ31[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 256 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to Q31(Fixed point 1.31): +* round(twiddleCoefQ31(i) * pow(2, 31)) +* +*/ +const q31_t twiddleCoef_256_q31[384] = { + 0x7FFFFFFF, 0x00000000, + 0x7FF62182, 0x03242ABF, + 0x7FD8878D, 0x0647D97C, + 0x7FA736B4, 0x096A9049, + 0x7F62368F, 0x0C8BD35E, + 0x7F0991C3, 0x0FAB272B, + 0x7E9D55FC, 0x12C8106E, + 0x7E1D93E9, 0x15E21444, + 0x7D8A5F3F, 0x18F8B83C, + 0x7CE3CEB1, 0x1C0B826A, + 0x7C29FBEE, 0x1F19F97B, + 0x7B5D039D, 0x2223A4C5, + 0x7A7D055B, 0x25280C5D, + 0x798A23B1, 0x2826B928, + 0x78848413, 0x2B1F34EB, + 0x776C4EDB, 0x2E110A62, + 0x7641AF3C, 0x30FBC54D, + 0x7504D345, 0x33DEF287, + 0x73B5EBD0, 0x36BA2013, + 0x72552C84, 0x398CDD32, + 0x70E2CBC6, 0x3C56BA70, + 0x6F5F02B1, 0x3F1749B7, + 0x6DCA0D14, 0x41CE1E64, + 0x6C242960, 0x447ACD50, + 0x6A6D98A4, 0x471CECE6, + 0x68A69E81, 0x49B41533, + 0x66CF811F, 0x4C3FDFF3, + 0x64E88926, 0x4EBFE8A4, + 0x62F201AC, 0x5133CC94, + 0x60EC3830, 0x539B2AEF, + 0x5ED77C89, 0x55F5A4D2, + 0x5CB420DF, 0x5842DD54, + 0x5A82799A, 0x5A82799A, + 0x5842DD54, 0x5CB420DF, + 0x55F5A4D2, 0x5ED77C89, + 0x539B2AEF, 0x60EC3830, + 0x5133CC94, 0x62F201AC, + 0x4EBFE8A4, 0x64E88926, + 0x4C3FDFF3, 0x66CF811F, + 0x49B41533, 0x68A69E81, + 0x471CECE6, 0x6A6D98A4, + 0x447ACD50, 0x6C242960, + 0x41CE1E64, 0x6DCA0D14, + 0x3F1749B7, 0x6F5F02B1, + 0x3C56BA70, 0x70E2CBC6, + 0x398CDD32, 0x72552C84, + 0x36BA2013, 0x73B5EBD0, + 0x33DEF287, 0x7504D345, + 0x30FBC54D, 0x7641AF3C, + 0x2E110A62, 0x776C4EDB, + 0x2B1F34EB, 0x78848413, + 0x2826B928, 0x798A23B1, + 0x25280C5D, 0x7A7D055B, + 0x2223A4C5, 0x7B5D039D, + 0x1F19F97B, 0x7C29FBEE, + 0x1C0B826A, 0x7CE3CEB1, + 0x18F8B83C, 0x7D8A5F3F, + 0x15E21444, 0x7E1D93E9, + 0x12C8106E, 0x7E9D55FC, + 0x0FAB272B, 0x7F0991C3, + 0x0C8BD35E, 0x7F62368F, + 0x096A9049, 0x7FA736B4, + 0x0647D97C, 0x7FD8878D, + 0x03242ABF, 0x7FF62182, + 0x00000000, 0x7FFFFFFF, + 0xFCDBD541, 0x7FF62182, + 0xF9B82683, 0x7FD8878D, + 0xF6956FB6, 0x7FA736B4, + 0xF3742CA1, 0x7F62368F, + 0xF054D8D4, 0x7F0991C3, + 0xED37EF91, 0x7E9D55FC, + 0xEA1DEBBB, 0x7E1D93E9, + 0xE70747C3, 0x7D8A5F3F, + 0xE3F47D95, 0x7CE3CEB1, + 0xE0E60684, 0x7C29FBEE, + 0xDDDC5B3A, 0x7B5D039D, + 0xDAD7F3A2, 0x7A7D055B, + 0xD7D946D7, 0x798A23B1, + 0xD4E0CB14, 0x78848413, + 0xD1EEF59E, 0x776C4EDB, + 0xCF043AB2, 0x7641AF3C, + 0xCC210D78, 0x7504D345, + 0xC945DFEC, 0x73B5EBD0, + 0xC67322CD, 0x72552C84, + 0xC3A9458F, 0x70E2CBC6, + 0xC0E8B648, 0x6F5F02B1, + 0xBE31E19B, 0x6DCA0D14, + 0xBB8532AF, 0x6C242960, + 0xB8E31319, 0x6A6D98A4, + 0xB64BEACC, 0x68A69E81, + 0xB3C0200C, 0x66CF811F, + 0xB140175B, 0x64E88926, + 0xAECC336B, 0x62F201AC, + 0xAC64D510, 0x60EC3830, + 0xAA0A5B2D, 0x5ED77C89, + 0xA7BD22AB, 0x5CB420DF, + 0xA57D8666, 0x5A82799A, + 0xA34BDF20, 0x5842DD54, + 0xA1288376, 0x55F5A4D2, + 0x9F13C7D0, 0x539B2AEF, + 0x9D0DFE53, 0x5133CC94, + 0x9B1776D9, 0x4EBFE8A4, + 0x99307EE0, 0x4C3FDFF3, + 0x9759617E, 0x49B41533, + 0x9592675B, 0x471CECE6, + 0x93DBD69F, 0x447ACD50, + 0x9235F2EB, 0x41CE1E64, + 0x90A0FD4E, 0x3F1749B7, + 0x8F1D343A, 0x3C56BA70, + 0x8DAAD37B, 0x398CDD32, + 0x8C4A142F, 0x36BA2013, + 0x8AFB2CBA, 0x33DEF287, + 0x89BE50C3, 0x30FBC54D, + 0x8893B124, 0x2E110A62, + 0x877B7BEC, 0x2B1F34EB, + 0x8675DC4E, 0x2826B928, + 0x8582FAA4, 0x25280C5D, + 0x84A2FC62, 0x2223A4C5, + 0x83D60411, 0x1F19F97B, + 0x831C314E, 0x1C0B826A, + 0x8275A0C0, 0x18F8B83C, + 0x81E26C16, 0x15E21444, + 0x8162AA03, 0x12C8106E, + 0x80F66E3C, 0x0FAB272B, + 0x809DC970, 0x0C8BD35E, + 0x8058C94C, 0x096A9049, + 0x80277872, 0x0647D97C, + 0x8009DE7D, 0x03242ABF, + 0x80000000, 0x00000000, + 0x8009DE7D, 0xFCDBD541, + 0x80277872, 0xF9B82683, + 0x8058C94C, 0xF6956FB6, + 0x809DC970, 0xF3742CA1, + 0x80F66E3C, 0xF054D8D4, + 0x8162AA03, 0xED37EF91, + 0x81E26C16, 0xEA1DEBBB, + 0x8275A0C0, 0xE70747C3, + 0x831C314E, 0xE3F47D95, + 0x83D60411, 0xE0E60684, + 0x84A2FC62, 0xDDDC5B3A, + 0x8582FAA4, 0xDAD7F3A2, + 0x8675DC4E, 0xD7D946D7, + 0x877B7BEC, 0xD4E0CB14, + 0x8893B124, 0xD1EEF59E, + 0x89BE50C3, 0xCF043AB2, + 0x8AFB2CBA, 0xCC210D78, + 0x8C4A142F, 0xC945DFEC, + 0x8DAAD37B, 0xC67322CD, + 0x8F1D343A, 0xC3A9458F, + 0x90A0FD4E, 0xC0E8B648, + 0x9235F2EB, 0xBE31E19B, + 0x93DBD69F, 0xBB8532AF, + 0x9592675B, 0xB8E31319, + 0x9759617E, 0xB64BEACC, + 0x99307EE0, 0xB3C0200C, + 0x9B1776D9, 0xB140175B, + 0x9D0DFE53, 0xAECC336B, + 0x9F13C7D0, 0xAC64D510, + 0xA1288376, 0xAA0A5B2D, + 0xA34BDF20, 0xA7BD22AB, + 0xA57D8666, 0xA57D8666, + 0xA7BD22AB, 0xA34BDF20, + 0xAA0A5B2D, 0xA1288376, + 0xAC64D510, 0x9F13C7D0, + 0xAECC336B, 0x9D0DFE53, + 0xB140175B, 0x9B1776D9, + 0xB3C0200C, 0x99307EE0, + 0xB64BEACC, 0x9759617E, + 0xB8E31319, 0x9592675B, + 0xBB8532AF, 0x93DBD69F, + 0xBE31E19B, 0x9235F2EB, + 0xC0E8B648, 0x90A0FD4E, + 0xC3A9458F, 0x8F1D343A, + 0xC67322CD, 0x8DAAD37B, + 0xC945DFEC, 0x8C4A142F, + 0xCC210D78, 0x8AFB2CBA, + 0xCF043AB2, 0x89BE50C3, + 0xD1EEF59E, 0x8893B124, + 0xD4E0CB14, 0x877B7BEC, + 0xD7D946D7, 0x8675DC4E, + 0xDAD7F3A2, 0x8582FAA4, + 0xDDDC5B3A, 0x84A2FC62, + 0xE0E60684, 0x83D60411, + 0xE3F47D95, 0x831C314E, + 0xE70747C3, 0x8275A0C0, + 0xEA1DEBBB, 0x81E26C16, + 0xED37EF91, 0x8162AA03, + 0xF054D8D4, 0x80F66E3C, + 0xF3742CA1, 0x809DC970, + 0xF6956FB6, 0x8058C94C, + 0xF9B82683, 0x80277872, + 0xFCDBD541, 0x8009DE7D +}; + +/** +* \par +* Example code for Q31 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefQ31[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefQ31[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 512 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to Q31(Fixed point 1.31): +* round(twiddleCoefQ31(i) * pow(2, 31)) +* +*/ +const q31_t twiddleCoef_512_q31[768] = { + 0x7FFFFFFF, 0x00000000, + 0x7FFD885A, 0x01921D1F, + 0x7FF62182, 0x03242ABF, + 0x7FE9CBC0, 0x04B6195D, + 0x7FD8878D, 0x0647D97C, + 0x7FC25596, 0x07D95B9E, + 0x7FA736B4, 0x096A9049, + 0x7F872BF3, 0x0AFB6805, + 0x7F62368F, 0x0C8BD35E, + 0x7F3857F5, 0x0E1BC2E3, + 0x7F0991C3, 0x0FAB272B, + 0x7ED5E5C6, 0x1139F0CE, + 0x7E9D55FC, 0x12C8106E, + 0x7E5FE493, 0x145576B1, + 0x7E1D93E9, 0x15E21444, + 0x7DD6668E, 0x176DD9DE, + 0x7D8A5F3F, 0x18F8B83C, + 0x7D3980EC, 0x1A82A025, + 0x7CE3CEB1, 0x1C0B826A, + 0x7C894BDD, 0x1D934FE5, + 0x7C29FBEE, 0x1F19F97B, + 0x7BC5E28F, 0x209F701C, + 0x7B5D039D, 0x2223A4C5, + 0x7AEF6323, 0x23A6887E, + 0x7A7D055B, 0x25280C5D, + 0x7A05EEAD, 0x26A82185, + 0x798A23B1, 0x2826B928, + 0x7909A92C, 0x29A3C484, + 0x78848413, 0x2B1F34EB, + 0x77FAB988, 0x2C98FBBA, + 0x776C4EDB, 0x2E110A62, + 0x76D94988, 0x2F875262, + 0x7641AF3C, 0x30FBC54D, + 0x75A585CF, 0x326E54C7, + 0x7504D345, 0x33DEF287, + 0x745F9DD1, 0x354D9056, + 0x73B5EBD0, 0x36BA2013, + 0x7307C3D0, 0x382493B0, + 0x72552C84, 0x398CDD32, + 0x719E2CD2, 0x3AF2EEB7, + 0x70E2CBC6, 0x3C56BA70, + 0x70231099, 0x3DB832A5, + 0x6F5F02B1, 0x3F1749B7, + 0x6E96A99C, 0x4073F21D, + 0x6DCA0D14, 0x41CE1E64, + 0x6CF934FB, 0x4325C135, + 0x6C242960, 0x447ACD50, + 0x6B4AF278, 0x45CD358F, + 0x6A6D98A4, 0x471CECE6, + 0x698C246C, 0x4869E664, + 0x68A69E81, 0x49B41533, + 0x67BD0FBC, 0x4AFB6C97, + 0x66CF811F, 0x4C3FDFF3, + 0x65DDFBD3, 0x4D8162C4, + 0x64E88926, 0x4EBFE8A4, + 0x63EF328F, 0x4FFB654D, + 0x62F201AC, 0x5133CC94, + 0x61F1003E, 0x5269126E, + 0x60EC3830, 0x539B2AEF, + 0x5FE3B38D, 0x54CA0A4A, + 0x5ED77C89, 0x55F5A4D2, + 0x5DC79D7C, 0x571DEEF9, + 0x5CB420DF, 0x5842DD54, + 0x5B9D1153, 0x59646497, + 0x5A82799A, 0x5A82799A, + 0x59646497, 0x5B9D1153, + 0x5842DD54, 0x5CB420DF, + 0x571DEEF9, 0x5DC79D7C, + 0x55F5A4D2, 0x5ED77C89, + 0x54CA0A4A, 0x5FE3B38D, + 0x539B2AEF, 0x60EC3830, + 0x5269126E, 0x61F1003E, + 0x5133CC94, 0x62F201AC, + 0x4FFB654D, 0x63EF328F, + 0x4EBFE8A4, 0x64E88926, + 0x4D8162C4, 0x65DDFBD3, + 0x4C3FDFF3, 0x66CF811F, + 0x4AFB6C97, 0x67BD0FBC, + 0x49B41533, 0x68A69E81, + 0x4869E664, 0x698C246C, + 0x471CECE6, 0x6A6D98A4, + 0x45CD358F, 0x6B4AF278, + 0x447ACD50, 0x6C242960, + 0x4325C135, 0x6CF934FB, + 0x41CE1E64, 0x6DCA0D14, + 0x4073F21D, 0x6E96A99C, + 0x3F1749B7, 0x6F5F02B1, + 0x3DB832A5, 0x70231099, + 0x3C56BA70, 0x70E2CBC6, + 0x3AF2EEB7, 0x719E2CD2, + 0x398CDD32, 0x72552C84, + 0x382493B0, 0x7307C3D0, + 0x36BA2013, 0x73B5EBD0, + 0x354D9056, 0x745F9DD1, + 0x33DEF287, 0x7504D345, + 0x326E54C7, 0x75A585CF, + 0x30FBC54D, 0x7641AF3C, + 0x2F875262, 0x76D94988, + 0x2E110A62, 0x776C4EDB, + 0x2C98FBBA, 0x77FAB988, + 0x2B1F34EB, 0x78848413, + 0x29A3C484, 0x7909A92C, + 0x2826B928, 0x798A23B1, + 0x26A82185, 0x7A05EEAD, + 0x25280C5D, 0x7A7D055B, + 0x23A6887E, 0x7AEF6323, + 0x2223A4C5, 0x7B5D039D, + 0x209F701C, 0x7BC5E28F, + 0x1F19F97B, 0x7C29FBEE, + 0x1D934FE5, 0x7C894BDD, + 0x1C0B826A, 0x7CE3CEB1, + 0x1A82A025, 0x7D3980EC, + 0x18F8B83C, 0x7D8A5F3F, + 0x176DD9DE, 0x7DD6668E, + 0x15E21444, 0x7E1D93E9, + 0x145576B1, 0x7E5FE493, + 0x12C8106E, 0x7E9D55FC, + 0x1139F0CE, 0x7ED5E5C6, + 0x0FAB272B, 0x7F0991C3, + 0x0E1BC2E3, 0x7F3857F5, + 0x0C8BD35E, 0x7F62368F, + 0x0AFB6805, 0x7F872BF3, + 0x096A9049, 0x7FA736B4, + 0x07D95B9E, 0x7FC25596, + 0x0647D97C, 0x7FD8878D, + 0x04B6195D, 0x7FE9CBC0, + 0x03242ABF, 0x7FF62182, + 0x01921D1F, 0x7FFD885A, + 0x00000000, 0x7FFFFFFF, + 0xFE6DE2E0, 0x7FFD885A, + 0xFCDBD541, 0x7FF62182, + 0xFB49E6A2, 0x7FE9CBC0, + 0xF9B82683, 0x7FD8878D, + 0xF826A461, 0x7FC25596, + 0xF6956FB6, 0x7FA736B4, + 0xF50497FA, 0x7F872BF3, + 0xF3742CA1, 0x7F62368F, + 0xF1E43D1C, 0x7F3857F5, + 0xF054D8D4, 0x7F0991C3, + 0xEEC60F31, 0x7ED5E5C6, + 0xED37EF91, 0x7E9D55FC, + 0xEBAA894E, 0x7E5FE493, + 0xEA1DEBBB, 0x7E1D93E9, + 0xE8922621, 0x7DD6668E, + 0xE70747C3, 0x7D8A5F3F, + 0xE57D5FDA, 0x7D3980EC, + 0xE3F47D95, 0x7CE3CEB1, + 0xE26CB01A, 0x7C894BDD, + 0xE0E60684, 0x7C29FBEE, + 0xDF608FE3, 0x7BC5E28F, + 0xDDDC5B3A, 0x7B5D039D, + 0xDC597781, 0x7AEF6323, + 0xDAD7F3A2, 0x7A7D055B, + 0xD957DE7A, 0x7A05EEAD, + 0xD7D946D7, 0x798A23B1, + 0xD65C3B7B, 0x7909A92C, + 0xD4E0CB14, 0x78848413, + 0xD3670445, 0x77FAB988, + 0xD1EEF59E, 0x776C4EDB, + 0xD078AD9D, 0x76D94988, + 0xCF043AB2, 0x7641AF3C, + 0xCD91AB38, 0x75A585CF, + 0xCC210D78, 0x7504D345, + 0xCAB26FA9, 0x745F9DD1, + 0xC945DFEC, 0x73B5EBD0, + 0xC7DB6C50, 0x7307C3D0, + 0xC67322CD, 0x72552C84, + 0xC50D1148, 0x719E2CD2, + 0xC3A9458F, 0x70E2CBC6, + 0xC247CD5A, 0x70231099, + 0xC0E8B648, 0x6F5F02B1, + 0xBF8C0DE2, 0x6E96A99C, + 0xBE31E19B, 0x6DCA0D14, + 0xBCDA3ECA, 0x6CF934FB, + 0xBB8532AF, 0x6C242960, + 0xBA32CA70, 0x6B4AF278, + 0xB8E31319, 0x6A6D98A4, + 0xB796199B, 0x698C246C, + 0xB64BEACC, 0x68A69E81, + 0xB5049368, 0x67BD0FBC, + 0xB3C0200C, 0x66CF811F, + 0xB27E9D3B, 0x65DDFBD3, + 0xB140175B, 0x64E88926, + 0xB0049AB2, 0x63EF328F, + 0xAECC336B, 0x62F201AC, + 0xAD96ED91, 0x61F1003E, + 0xAC64D510, 0x60EC3830, + 0xAB35F5B5, 0x5FE3B38D, + 0xAA0A5B2D, 0x5ED77C89, + 0xA8E21106, 0x5DC79D7C, + 0xA7BD22AB, 0x5CB420DF, + 0xA69B9B68, 0x5B9D1153, + 0xA57D8666, 0x5A82799A, + 0xA462EEAC, 0x59646497, + 0xA34BDF20, 0x5842DD54, + 0xA2386283, 0x571DEEF9, + 0xA1288376, 0x55F5A4D2, + 0xA01C4C72, 0x54CA0A4A, + 0x9F13C7D0, 0x539B2AEF, + 0x9E0EFFC1, 0x5269126E, + 0x9D0DFE53, 0x5133CC94, + 0x9C10CD70, 0x4FFB654D, + 0x9B1776D9, 0x4EBFE8A4, + 0x9A22042C, 0x4D8162C4, + 0x99307EE0, 0x4C3FDFF3, + 0x9842F043, 0x4AFB6C97, + 0x9759617E, 0x49B41533, + 0x9673DB94, 0x4869E664, + 0x9592675B, 0x471CECE6, + 0x94B50D87, 0x45CD358F, + 0x93DBD69F, 0x447ACD50, + 0x9306CB04, 0x4325C135, + 0x9235F2EB, 0x41CE1E64, + 0x91695663, 0x4073F21D, + 0x90A0FD4E, 0x3F1749B7, + 0x8FDCEF66, 0x3DB832A5, + 0x8F1D343A, 0x3C56BA70, + 0x8E61D32D, 0x3AF2EEB7, + 0x8DAAD37B, 0x398CDD32, + 0x8CF83C30, 0x382493B0, + 0x8C4A142F, 0x36BA2013, + 0x8BA0622F, 0x354D9056, + 0x8AFB2CBA, 0x33DEF287, + 0x8A5A7A30, 0x326E54C7, + 0x89BE50C3, 0x30FBC54D, + 0x8926B677, 0x2F875262, + 0x8893B124, 0x2E110A62, + 0x88054677, 0x2C98FBBA, + 0x877B7BEC, 0x2B1F34EB, + 0x86F656D3, 0x29A3C484, + 0x8675DC4E, 0x2826B928, + 0x85FA1152, 0x26A82185, + 0x8582FAA4, 0x25280C5D, + 0x85109CDC, 0x23A6887E, + 0x84A2FC62, 0x2223A4C5, + 0x843A1D70, 0x209F701C, + 0x83D60411, 0x1F19F97B, + 0x8376B422, 0x1D934FE5, + 0x831C314E, 0x1C0B826A, + 0x82C67F13, 0x1A82A025, + 0x8275A0C0, 0x18F8B83C, + 0x82299971, 0x176DD9DE, + 0x81E26C16, 0x15E21444, + 0x81A01B6C, 0x145576B1, + 0x8162AA03, 0x12C8106E, + 0x812A1A39, 0x1139F0CE, + 0x80F66E3C, 0x0FAB272B, + 0x80C7A80A, 0x0E1BC2E3, + 0x809DC970, 0x0C8BD35E, + 0x8078D40D, 0x0AFB6805, + 0x8058C94C, 0x096A9049, + 0x803DAA69, 0x07D95B9E, + 0x80277872, 0x0647D97C, + 0x80163440, 0x04B6195D, + 0x8009DE7D, 0x03242ABF, + 0x800277A5, 0x01921D1F, + 0x80000000, 0x00000000, + 0x800277A5, 0xFE6DE2E0, + 0x8009DE7D, 0xFCDBD541, + 0x80163440, 0xFB49E6A2, + 0x80277872, 0xF9B82683, + 0x803DAA69, 0xF826A461, + 0x8058C94C, 0xF6956FB6, + 0x8078D40D, 0xF50497FA, + 0x809DC970, 0xF3742CA1, + 0x80C7A80A, 0xF1E43D1C, + 0x80F66E3C, 0xF054D8D4, + 0x812A1A39, 0xEEC60F31, + 0x8162AA03, 0xED37EF91, + 0x81A01B6C, 0xEBAA894E, + 0x81E26C16, 0xEA1DEBBB, + 0x82299971, 0xE8922621, + 0x8275A0C0, 0xE70747C3, + 0x82C67F13, 0xE57D5FDA, + 0x831C314E, 0xE3F47D95, + 0x8376B422, 0xE26CB01A, + 0x83D60411, 0xE0E60684, + 0x843A1D70, 0xDF608FE3, + 0x84A2FC62, 0xDDDC5B3A, + 0x85109CDC, 0xDC597781, + 0x8582FAA4, 0xDAD7F3A2, + 0x85FA1152, 0xD957DE7A, + 0x8675DC4E, 0xD7D946D7, + 0x86F656D3, 0xD65C3B7B, + 0x877B7BEC, 0xD4E0CB14, + 0x88054677, 0xD3670445, + 0x8893B124, 0xD1EEF59E, + 0x8926B677, 0xD078AD9D, + 0x89BE50C3, 0xCF043AB2, + 0x8A5A7A30, 0xCD91AB38, + 0x8AFB2CBA, 0xCC210D78, + 0x8BA0622F, 0xCAB26FA9, + 0x8C4A142F, 0xC945DFEC, + 0x8CF83C30, 0xC7DB6C50, + 0x8DAAD37B, 0xC67322CD, + 0x8E61D32D, 0xC50D1148, + 0x8F1D343A, 0xC3A9458F, + 0x8FDCEF66, 0xC247CD5A, + 0x90A0FD4E, 0xC0E8B648, + 0x91695663, 0xBF8C0DE2, + 0x9235F2EB, 0xBE31E19B, + 0x9306CB04, 0xBCDA3ECA, + 0x93DBD69F, 0xBB8532AF, + 0x94B50D87, 0xBA32CA70, + 0x9592675B, 0xB8E31319, + 0x9673DB94, 0xB796199B, + 0x9759617E, 0xB64BEACC, + 0x9842F043, 0xB5049368, + 0x99307EE0, 0xB3C0200C, + 0x9A22042C, 0xB27E9D3B, + 0x9B1776D9, 0xB140175B, + 0x9C10CD70, 0xB0049AB2, + 0x9D0DFE53, 0xAECC336B, + 0x9E0EFFC1, 0xAD96ED91, + 0x9F13C7D0, 0xAC64D510, + 0xA01C4C72, 0xAB35F5B5, + 0xA1288376, 0xAA0A5B2D, + 0xA2386283, 0xA8E21106, + 0xA34BDF20, 0xA7BD22AB, + 0xA462EEAC, 0xA69B9B68, + 0xA57D8666, 0xA57D8666, + 0xA69B9B68, 0xA462EEAC, + 0xA7BD22AB, 0xA34BDF20, + 0xA8E21106, 0xA2386283, + 0xAA0A5B2D, 0xA1288376, + 0xAB35F5B5, 0xA01C4C72, + 0xAC64D510, 0x9F13C7D0, + 0xAD96ED91, 0x9E0EFFC1, + 0xAECC336B, 0x9D0DFE53, + 0xB0049AB2, 0x9C10CD70, + 0xB140175B, 0x9B1776D9, + 0xB27E9D3B, 0x9A22042C, + 0xB3C0200C, 0x99307EE0, + 0xB5049368, 0x9842F043, + 0xB64BEACC, 0x9759617E, + 0xB796199B, 0x9673DB94, + 0xB8E31319, 0x9592675B, + 0xBA32CA70, 0x94B50D87, + 0xBB8532AF, 0x93DBD69F, + 0xBCDA3ECA, 0x9306CB04, + 0xBE31E19B, 0x9235F2EB, + 0xBF8C0DE2, 0x91695663, + 0xC0E8B648, 0x90A0FD4E, + 0xC247CD5A, 0x8FDCEF66, + 0xC3A9458F, 0x8F1D343A, + 0xC50D1148, 0x8E61D32D, + 0xC67322CD, 0x8DAAD37B, + 0xC7DB6C50, 0x8CF83C30, + 0xC945DFEC, 0x8C4A142F, + 0xCAB26FA9, 0x8BA0622F, + 0xCC210D78, 0x8AFB2CBA, + 0xCD91AB38, 0x8A5A7A30, + 0xCF043AB2, 0x89BE50C3, + 0xD078AD9D, 0x8926B677, + 0xD1EEF59E, 0x8893B124, + 0xD3670445, 0x88054677, + 0xD4E0CB14, 0x877B7BEC, + 0xD65C3B7B, 0x86F656D3, + 0xD7D946D7, 0x8675DC4E, + 0xD957DE7A, 0x85FA1152, + 0xDAD7F3A2, 0x8582FAA4, + 0xDC597781, 0x85109CDC, + 0xDDDC5B3A, 0x84A2FC62, + 0xDF608FE3, 0x843A1D70, + 0xE0E60684, 0x83D60411, + 0xE26CB01A, 0x8376B422, + 0xE3F47D95, 0x831C314E, + 0xE57D5FDA, 0x82C67F13, + 0xE70747C3, 0x8275A0C0, + 0xE8922621, 0x82299971, + 0xEA1DEBBB, 0x81E26C16, + 0xEBAA894E, 0x81A01B6C, + 0xED37EF91, 0x8162AA03, + 0xEEC60F31, 0x812A1A39, + 0xF054D8D4, 0x80F66E3C, + 0xF1E43D1C, 0x80C7A80A, + 0xF3742CA1, 0x809DC970, + 0xF50497FA, 0x8078D40D, + 0xF6956FB6, 0x8058C94C, + 0xF826A461, 0x803DAA69, + 0xF9B82683, 0x80277872, + 0xFB49E6A2, 0x80163440, + 0xFCDBD541, 0x8009DE7D, + 0xFE6DE2E0, 0x800277A5 +}; + +/** +* \par +* Example code for Q31 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefQ31[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefQ31[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 1024 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to Q31(Fixed point 1.31): +* round(twiddleCoefQ31(i) * pow(2, 31)) +* +*/ +const q31_t twiddleCoef_1024_q31[1536] = { + 0x7FFFFFFF, 0x00000000, + 0x7FFF6216, 0x00C90F88, + 0x7FFD885A, 0x01921D1F, + 0x7FFA72D1, 0x025B26D7, + 0x7FF62182, 0x03242ABF, + 0x7FF09477, 0x03ED26E6, + 0x7FE9CBC0, 0x04B6195D, + 0x7FE1C76B, 0x057F0034, + 0x7FD8878D, 0x0647D97C, + 0x7FCE0C3E, 0x0710A344, + 0x7FC25596, 0x07D95B9E, + 0x7FB563B2, 0x08A2009A, + 0x7FA736B4, 0x096A9049, + 0x7F97CEBC, 0x0A3308BC, + 0x7F872BF3, 0x0AFB6805, + 0x7F754E7F, 0x0BC3AC35, + 0x7F62368F, 0x0C8BD35E, + 0x7F4DE450, 0x0D53DB92, + 0x7F3857F5, 0x0E1BC2E3, + 0x7F2191B4, 0x0EE38765, + 0x7F0991C3, 0x0FAB272B, + 0x7EF0585F, 0x1072A047, + 0x7ED5E5C6, 0x1139F0CE, + 0x7EBA3A39, 0x120116D4, + 0x7E9D55FC, 0x12C8106E, + 0x7E7F3956, 0x138EDBB0, + 0x7E5FE493, 0x145576B1, + 0x7E3F57FE, 0x151BDF85, + 0x7E1D93E9, 0x15E21444, + 0x7DFA98A7, 0x16A81305, + 0x7DD6668E, 0x176DD9DE, + 0x7DB0FDF7, 0x183366E8, + 0x7D8A5F3F, 0x18F8B83C, + 0x7D628AC5, 0x19BDCBF2, + 0x7D3980EC, 0x1A82A025, + 0x7D0F4218, 0x1B4732EF, + 0x7CE3CEB1, 0x1C0B826A, + 0x7CB72724, 0x1CCF8CB3, + 0x7C894BDD, 0x1D934FE5, + 0x7C5A3D4F, 0x1E56CA1E, + 0x7C29FBEE, 0x1F19F97B, + 0x7BF88830, 0x1FDCDC1A, + 0x7BC5E28F, 0x209F701C, + 0x7B920B89, 0x2161B39F, + 0x7B5D039D, 0x2223A4C5, + 0x7B26CB4F, 0x22E541AE, + 0x7AEF6323, 0x23A6887E, + 0x7AB6CBA3, 0x24677757, + 0x7A7D055B, 0x25280C5D, + 0x7A4210D8, 0x25E845B5, + 0x7A05EEAD, 0x26A82185, + 0x79C89F6D, 0x27679DF4, + 0x798A23B1, 0x2826B928, + 0x794A7C11, 0x28E5714A, + 0x7909A92C, 0x29A3C484, + 0x78C7ABA1, 0x2A61B101, + 0x78848413, 0x2B1F34EB, + 0x78403328, 0x2BDC4E6F, + 0x77FAB988, 0x2C98FBBA, + 0x77B417DF, 0x2D553AFB, + 0x776C4EDB, 0x2E110A62, + 0x77235F2D, 0x2ECC681E, + 0x76D94988, 0x2F875262, + 0x768E0EA5, 0x3041C760, + 0x7641AF3C, 0x30FBC54D, + 0x75F42C0A, 0x31B54A5D, + 0x75A585CF, 0x326E54C7, + 0x7555BD4B, 0x3326E2C2, + 0x7504D345, 0x33DEF287, + 0x74B2C883, 0x3496824F, + 0x745F9DD1, 0x354D9056, + 0x740B53FA, 0x36041AD9, + 0x73B5EBD0, 0x36BA2013, + 0x735F6626, 0x376F9E46, + 0x7307C3D0, 0x382493B0, + 0x72AF05A6, 0x38D8FE93, + 0x72552C84, 0x398CDD32, + 0x71FA3948, 0x3A402DD1, + 0x719E2CD2, 0x3AF2EEB7, + 0x71410804, 0x3BA51E29, + 0x70E2CBC6, 0x3C56BA70, + 0x708378FE, 0x3D07C1D5, + 0x70231099, 0x3DB832A5, + 0x6FC19385, 0x3E680B2C, + 0x6F5F02B1, 0x3F1749B7, + 0x6EFB5F12, 0x3FC5EC97, + 0x6E96A99C, 0x4073F21D, + 0x6E30E349, 0x4121589A, + 0x6DCA0D14, 0x41CE1E64, + 0x6D6227FA, 0x427A41D0, + 0x6CF934FB, 0x4325C135, + 0x6C8F351C, 0x43D09AEC, + 0x6C242960, 0x447ACD50, + 0x6BB812D0, 0x452456BC, + 0x6B4AF278, 0x45CD358F, + 0x6ADCC964, 0x46756827, + 0x6A6D98A4, 0x471CECE6, + 0x69FD614A, 0x47C3C22E, + 0x698C246C, 0x4869E664, + 0x6919E320, 0x490F57EE, + 0x68A69E81, 0x49B41533, + 0x683257AA, 0x4A581C9D, + 0x67BD0FBC, 0x4AFB6C97, + 0x6746C7D7, 0x4B9E038F, + 0x66CF811F, 0x4C3FDFF3, + 0x66573CBB, 0x4CE10034, + 0x65DDFBD3, 0x4D8162C4, + 0x6563BF92, 0x4E210617, + 0x64E88926, 0x4EBFE8A4, + 0x646C59BF, 0x4F5E08E3, + 0x63EF328F, 0x4FFB654D, + 0x637114CC, 0x5097FC5E, + 0x62F201AC, 0x5133CC94, + 0x6271FA69, 0x51CED46E, + 0x61F1003E, 0x5269126E, + 0x616F146B, 0x53028517, + 0x60EC3830, 0x539B2AEF, + 0x60686CCE, 0x5433027D, + 0x5FE3B38D, 0x54CA0A4A, + 0x5F5E0DB3, 0x556040E2, + 0x5ED77C89, 0x55F5A4D2, + 0x5E50015D, 0x568A34A9, + 0x5DC79D7C, 0x571DEEF9, + 0x5D3E5236, 0x57B0D256, + 0x5CB420DF, 0x5842DD54, + 0x5C290ACC, 0x58D40E8C, + 0x5B9D1153, 0x59646497, + 0x5B1035CF, 0x59F3DE12, + 0x5A82799A, 0x5A82799A, + 0x59F3DE12, 0x5B1035CF, + 0x59646497, 0x5B9D1153, + 0x58D40E8C, 0x5C290ACC, + 0x5842DD54, 0x5CB420DF, + 0x57B0D256, 0x5D3E5236, + 0x571DEEF9, 0x5DC79D7C, + 0x568A34A9, 0x5E50015D, + 0x55F5A4D2, 0x5ED77C89, + 0x556040E2, 0x5F5E0DB3, + 0x54CA0A4A, 0x5FE3B38D, + 0x5433027D, 0x60686CCE, + 0x539B2AEF, 0x60EC3830, + 0x53028517, 0x616F146B, + 0x5269126E, 0x61F1003E, + 0x51CED46E, 0x6271FA69, + 0x5133CC94, 0x62F201AC, + 0x5097FC5E, 0x637114CC, + 0x4FFB654D, 0x63EF328F, + 0x4F5E08E3, 0x646C59BF, + 0x4EBFE8A4, 0x64E88926, + 0x4E210617, 0x6563BF92, + 0x4D8162C4, 0x65DDFBD3, + 0x4CE10034, 0x66573CBB, + 0x4C3FDFF3, 0x66CF811F, + 0x4B9E038F, 0x6746C7D7, + 0x4AFB6C97, 0x67BD0FBC, + 0x4A581C9D, 0x683257AA, + 0x49B41533, 0x68A69E81, + 0x490F57EE, 0x6919E320, + 0x4869E664, 0x698C246C, + 0x47C3C22E, 0x69FD614A, + 0x471CECE6, 0x6A6D98A4, + 0x46756827, 0x6ADCC964, + 0x45CD358F, 0x6B4AF278, + 0x452456BC, 0x6BB812D0, + 0x447ACD50, 0x6C242960, + 0x43D09AEC, 0x6C8F351C, + 0x4325C135, 0x6CF934FB, + 0x427A41D0, 0x6D6227FA, + 0x41CE1E64, 0x6DCA0D14, + 0x4121589A, 0x6E30E349, + 0x4073F21D, 0x6E96A99C, + 0x3FC5EC97, 0x6EFB5F12, + 0x3F1749B7, 0x6F5F02B1, + 0x3E680B2C, 0x6FC19385, + 0x3DB832A5, 0x70231099, + 0x3D07C1D5, 0x708378FE, + 0x3C56BA70, 0x70E2CBC6, + 0x3BA51E29, 0x71410804, + 0x3AF2EEB7, 0x719E2CD2, + 0x3A402DD1, 0x71FA3948, + 0x398CDD32, 0x72552C84, + 0x38D8FE93, 0x72AF05A6, + 0x382493B0, 0x7307C3D0, + 0x376F9E46, 0x735F6626, + 0x36BA2013, 0x73B5EBD0, + 0x36041AD9, 0x740B53FA, + 0x354D9056, 0x745F9DD1, + 0x3496824F, 0x74B2C883, + 0x33DEF287, 0x7504D345, + 0x3326E2C2, 0x7555BD4B, + 0x326E54C7, 0x75A585CF, + 0x31B54A5D, 0x75F42C0A, + 0x30FBC54D, 0x7641AF3C, + 0x3041C760, 0x768E0EA5, + 0x2F875262, 0x76D94988, + 0x2ECC681E, 0x77235F2D, + 0x2E110A62, 0x776C4EDB, + 0x2D553AFB, 0x77B417DF, + 0x2C98FBBA, 0x77FAB988, + 0x2BDC4E6F, 0x78403328, + 0x2B1F34EB, 0x78848413, + 0x2A61B101, 0x78C7ABA1, + 0x29A3C484, 0x7909A92C, + 0x28E5714A, 0x794A7C11, + 0x2826B928, 0x798A23B1, + 0x27679DF4, 0x79C89F6D, + 0x26A82185, 0x7A05EEAD, + 0x25E845B5, 0x7A4210D8, + 0x25280C5D, 0x7A7D055B, + 0x24677757, 0x7AB6CBA3, + 0x23A6887E, 0x7AEF6323, + 0x22E541AE, 0x7B26CB4F, + 0x2223A4C5, 0x7B5D039D, + 0x2161B39F, 0x7B920B89, + 0x209F701C, 0x7BC5E28F, + 0x1FDCDC1A, 0x7BF88830, + 0x1F19F97B, 0x7C29FBEE, + 0x1E56CA1E, 0x7C5A3D4F, + 0x1D934FE5, 0x7C894BDD, + 0x1CCF8CB3, 0x7CB72724, + 0x1C0B826A, 0x7CE3CEB1, + 0x1B4732EF, 0x7D0F4218, + 0x1A82A025, 0x7D3980EC, + 0x19BDCBF2, 0x7D628AC5, + 0x18F8B83C, 0x7D8A5F3F, + 0x183366E8, 0x7DB0FDF7, + 0x176DD9DE, 0x7DD6668E, + 0x16A81305, 0x7DFA98A7, + 0x15E21444, 0x7E1D93E9, + 0x151BDF85, 0x7E3F57FE, + 0x145576B1, 0x7E5FE493, + 0x138EDBB0, 0x7E7F3956, + 0x12C8106E, 0x7E9D55FC, + 0x120116D4, 0x7EBA3A39, + 0x1139F0CE, 0x7ED5E5C6, + 0x1072A047, 0x7EF0585F, + 0x0FAB272B, 0x7F0991C3, + 0x0EE38765, 0x7F2191B4, + 0x0E1BC2E3, 0x7F3857F5, + 0x0D53DB92, 0x7F4DE450, + 0x0C8BD35E, 0x7F62368F, + 0x0BC3AC35, 0x7F754E7F, + 0x0AFB6805, 0x7F872BF3, + 0x0A3308BC, 0x7F97CEBC, + 0x096A9049, 0x7FA736B4, + 0x08A2009A, 0x7FB563B2, + 0x07D95B9E, 0x7FC25596, + 0x0710A344, 0x7FCE0C3E, + 0x0647D97C, 0x7FD8878D, + 0x057F0034, 0x7FE1C76B, + 0x04B6195D, 0x7FE9CBC0, + 0x03ED26E6, 0x7FF09477, + 0x03242ABF, 0x7FF62182, + 0x025B26D7, 0x7FFA72D1, + 0x01921D1F, 0x7FFD885A, + 0x00C90F88, 0x7FFF6216, + 0x00000000, 0x7FFFFFFF, + 0xFF36F078, 0x7FFF6216, + 0xFE6DE2E0, 0x7FFD885A, + 0xFDA4D928, 0x7FFA72D1, + 0xFCDBD541, 0x7FF62182, + 0xFC12D919, 0x7FF09477, + 0xFB49E6A2, 0x7FE9CBC0, + 0xFA80FFCB, 0x7FE1C76B, + 0xF9B82683, 0x7FD8878D, + 0xF8EF5CBB, 0x7FCE0C3E, + 0xF826A461, 0x7FC25596, + 0xF75DFF65, 0x7FB563B2, + 0xF6956FB6, 0x7FA736B4, + 0xF5CCF743, 0x7F97CEBC, + 0xF50497FA, 0x7F872BF3, + 0xF43C53CA, 0x7F754E7F, + 0xF3742CA1, 0x7F62368F, + 0xF2AC246D, 0x7F4DE450, + 0xF1E43D1C, 0x7F3857F5, + 0xF11C789A, 0x7F2191B4, + 0xF054D8D4, 0x7F0991C3, + 0xEF8D5FB8, 0x7EF0585F, + 0xEEC60F31, 0x7ED5E5C6, + 0xEDFEE92B, 0x7EBA3A39, + 0xED37EF91, 0x7E9D55FC, + 0xEC71244F, 0x7E7F3956, + 0xEBAA894E, 0x7E5FE493, + 0xEAE4207A, 0x7E3F57FE, + 0xEA1DEBBB, 0x7E1D93E9, + 0xE957ECFB, 0x7DFA98A7, + 0xE8922621, 0x7DD6668E, + 0xE7CC9917, 0x7DB0FDF7, + 0xE70747C3, 0x7D8A5F3F, + 0xE642340D, 0x7D628AC5, + 0xE57D5FDA, 0x7D3980EC, + 0xE4B8CD10, 0x7D0F4218, + 0xE3F47D95, 0x7CE3CEB1, + 0xE330734C, 0x7CB72724, + 0xE26CB01A, 0x7C894BDD, + 0xE1A935E1, 0x7C5A3D4F, + 0xE0E60684, 0x7C29FBEE, + 0xE02323E5, 0x7BF88830, + 0xDF608FE3, 0x7BC5E28F, + 0xDE9E4C60, 0x7B920B89, + 0xDDDC5B3A, 0x7B5D039D, + 0xDD1ABE51, 0x7B26CB4F, + 0xDC597781, 0x7AEF6323, + 0xDB9888A8, 0x7AB6CBA3, + 0xDAD7F3A2, 0x7A7D055B, + 0xDA17BA4A, 0x7A4210D8, + 0xD957DE7A, 0x7A05EEAD, + 0xD898620C, 0x79C89F6D, + 0xD7D946D7, 0x798A23B1, + 0xD71A8EB5, 0x794A7C11, + 0xD65C3B7B, 0x7909A92C, + 0xD59E4EFE, 0x78C7ABA1, + 0xD4E0CB14, 0x78848413, + 0xD423B190, 0x78403328, + 0xD3670445, 0x77FAB988, + 0xD2AAC504, 0x77B417DF, + 0xD1EEF59E, 0x776C4EDB, + 0xD13397E1, 0x77235F2D, + 0xD078AD9D, 0x76D94988, + 0xCFBE389F, 0x768E0EA5, + 0xCF043AB2, 0x7641AF3C, + 0xCE4AB5A2, 0x75F42C0A, + 0xCD91AB38, 0x75A585CF, + 0xCCD91D3D, 0x7555BD4B, + 0xCC210D78, 0x7504D345, + 0xCB697DB0, 0x74B2C883, + 0xCAB26FA9, 0x745F9DD1, + 0xC9FBE527, 0x740B53FA, + 0xC945DFEC, 0x73B5EBD0, + 0xC89061BA, 0x735F6626, + 0xC7DB6C50, 0x7307C3D0, + 0xC727016C, 0x72AF05A6, + 0xC67322CD, 0x72552C84, + 0xC5BFD22E, 0x71FA3948, + 0xC50D1148, 0x719E2CD2, + 0xC45AE1D7, 0x71410804, + 0xC3A9458F, 0x70E2CBC6, + 0xC2F83E2A, 0x708378FE, + 0xC247CD5A, 0x70231099, + 0xC197F4D3, 0x6FC19385, + 0xC0E8B648, 0x6F5F02B1, + 0xC03A1368, 0x6EFB5F12, + 0xBF8C0DE2, 0x6E96A99C, + 0xBEDEA765, 0x6E30E349, + 0xBE31E19B, 0x6DCA0D14, + 0xBD85BE2F, 0x6D6227FA, + 0xBCDA3ECA, 0x6CF934FB, + 0xBC2F6513, 0x6C8F351C, + 0xBB8532AF, 0x6C242960, + 0xBADBA943, 0x6BB812D0, + 0xBA32CA70, 0x6B4AF278, + 0xB98A97D8, 0x6ADCC964, + 0xB8E31319, 0x6A6D98A4, + 0xB83C3DD1, 0x69FD614A, + 0xB796199B, 0x698C246C, + 0xB6F0A811, 0x6919E320, + 0xB64BEACC, 0x68A69E81, + 0xB5A7E362, 0x683257AA, + 0xB5049368, 0x67BD0FBC, + 0xB461FC70, 0x6746C7D7, + 0xB3C0200C, 0x66CF811F, + 0xB31EFFCB, 0x66573CBB, + 0xB27E9D3B, 0x65DDFBD3, + 0xB1DEF9E8, 0x6563BF92, + 0xB140175B, 0x64E88926, + 0xB0A1F71C, 0x646C59BF, + 0xB0049AB2, 0x63EF328F, + 0xAF6803A1, 0x637114CC, + 0xAECC336B, 0x62F201AC, + 0xAE312B91, 0x6271FA69, + 0xAD96ED91, 0x61F1003E, + 0xACFD7AE8, 0x616F146B, + 0xAC64D510, 0x60EC3830, + 0xABCCFD82, 0x60686CCE, + 0xAB35F5B5, 0x5FE3B38D, + 0xAA9FBF1D, 0x5F5E0DB3, + 0xAA0A5B2D, 0x5ED77C89, + 0xA975CB56, 0x5E50015D, + 0xA8E21106, 0x5DC79D7C, + 0xA84F2DA9, 0x5D3E5236, + 0xA7BD22AB, 0x5CB420DF, + 0xA72BF173, 0x5C290ACC, + 0xA69B9B68, 0x5B9D1153, + 0xA60C21ED, 0x5B1035CF, + 0xA57D8666, 0x5A82799A, + 0xA4EFCA31, 0x59F3DE12, + 0xA462EEAC, 0x59646497, + 0xA3D6F533, 0x58D40E8C, + 0xA34BDF20, 0x5842DD54, + 0xA2C1ADC9, 0x57B0D256, + 0xA2386283, 0x571DEEF9, + 0xA1AFFEA2, 0x568A34A9, + 0xA1288376, 0x55F5A4D2, + 0xA0A1F24C, 0x556040E2, + 0xA01C4C72, 0x54CA0A4A, + 0x9F979331, 0x5433027D, + 0x9F13C7D0, 0x539B2AEF, + 0x9E90EB94, 0x53028517, + 0x9E0EFFC1, 0x5269126E, + 0x9D8E0596, 0x51CED46E, + 0x9D0DFE53, 0x5133CC94, + 0x9C8EEB33, 0x5097FC5E, + 0x9C10CD70, 0x4FFB654D, + 0x9B93A640, 0x4F5E08E3, + 0x9B1776D9, 0x4EBFE8A4, + 0x9A9C406D, 0x4E210617, + 0x9A22042C, 0x4D8162C4, + 0x99A8C344, 0x4CE10034, + 0x99307EE0, 0x4C3FDFF3, + 0x98B93828, 0x4B9E038F, + 0x9842F043, 0x4AFB6C97, + 0x97CDA855, 0x4A581C9D, + 0x9759617E, 0x49B41533, + 0x96E61CDF, 0x490F57EE, + 0x9673DB94, 0x4869E664, + 0x96029EB5, 0x47C3C22E, + 0x9592675B, 0x471CECE6, + 0x9523369B, 0x46756827, + 0x94B50D87, 0x45CD358F, + 0x9447ED2F, 0x452456BC, + 0x93DBD69F, 0x447ACD50, + 0x9370CAE4, 0x43D09AEC, + 0x9306CB04, 0x4325C135, + 0x929DD805, 0x427A41D0, + 0x9235F2EB, 0x41CE1E64, + 0x91CF1CB6, 0x4121589A, + 0x91695663, 0x4073F21D, + 0x9104A0ED, 0x3FC5EC97, + 0x90A0FD4E, 0x3F1749B7, + 0x903E6C7A, 0x3E680B2C, + 0x8FDCEF66, 0x3DB832A5, + 0x8F7C8701, 0x3D07C1D5, + 0x8F1D343A, 0x3C56BA70, + 0x8EBEF7FB, 0x3BA51E29, + 0x8E61D32D, 0x3AF2EEB7, + 0x8E05C6B7, 0x3A402DD1, + 0x8DAAD37B, 0x398CDD32, + 0x8D50FA59, 0x38D8FE93, + 0x8CF83C30, 0x382493B0, + 0x8CA099D9, 0x376F9E46, + 0x8C4A142F, 0x36BA2013, + 0x8BF4AC05, 0x36041AD9, + 0x8BA0622F, 0x354D9056, + 0x8B4D377C, 0x3496824F, + 0x8AFB2CBA, 0x33DEF287, + 0x8AAA42B4, 0x3326E2C2, + 0x8A5A7A30, 0x326E54C7, + 0x8A0BD3F5, 0x31B54A5D, + 0x89BE50C3, 0x30FBC54D, + 0x8971F15A, 0x3041C760, + 0x8926B677, 0x2F875262, + 0x88DCA0D3, 0x2ECC681E, + 0x8893B124, 0x2E110A62, + 0x884BE820, 0x2D553AFB, + 0x88054677, 0x2C98FBBA, + 0x87BFCCD7, 0x2BDC4E6F, + 0x877B7BEC, 0x2B1F34EB, + 0x8738545E, 0x2A61B101, + 0x86F656D3, 0x29A3C484, + 0x86B583EE, 0x28E5714A, + 0x8675DC4E, 0x2826B928, + 0x86376092, 0x27679DF4, + 0x85FA1152, 0x26A82185, + 0x85BDEF27, 0x25E845B5, + 0x8582FAA4, 0x25280C5D, + 0x8549345C, 0x24677757, + 0x85109CDC, 0x23A6887E, + 0x84D934B0, 0x22E541AE, + 0x84A2FC62, 0x2223A4C5, + 0x846DF476, 0x2161B39F, + 0x843A1D70, 0x209F701C, + 0x840777CF, 0x1FDCDC1A, + 0x83D60411, 0x1F19F97B, + 0x83A5C2B0, 0x1E56CA1E, + 0x8376B422, 0x1D934FE5, + 0x8348D8DB, 0x1CCF8CB3, + 0x831C314E, 0x1C0B826A, + 0x82F0BDE8, 0x1B4732EF, + 0x82C67F13, 0x1A82A025, + 0x829D753A, 0x19BDCBF2, + 0x8275A0C0, 0x18F8B83C, + 0x824F0208, 0x183366E8, + 0x82299971, 0x176DD9DE, + 0x82056758, 0x16A81305, + 0x81E26C16, 0x15E21444, + 0x81C0A801, 0x151BDF85, + 0x81A01B6C, 0x145576B1, + 0x8180C6A9, 0x138EDBB0, + 0x8162AA03, 0x12C8106E, + 0x8145C5C6, 0x120116D4, + 0x812A1A39, 0x1139F0CE, + 0x810FA7A0, 0x1072A047, + 0x80F66E3C, 0x0FAB272B, + 0x80DE6E4C, 0x0EE38765, + 0x80C7A80A, 0x0E1BC2E3, + 0x80B21BAF, 0x0D53DB92, + 0x809DC970, 0x0C8BD35E, + 0x808AB180, 0x0BC3AC35, + 0x8078D40D, 0x0AFB6805, + 0x80683143, 0x0A3308BC, + 0x8058C94C, 0x096A9049, + 0x804A9C4D, 0x08A2009A, + 0x803DAA69, 0x07D95B9E, + 0x8031F3C1, 0x0710A344, + 0x80277872, 0x0647D97C, + 0x801E3894, 0x057F0034, + 0x80163440, 0x04B6195D, + 0x800F6B88, 0x03ED26E6, + 0x8009DE7D, 0x03242ABF, + 0x80058D2E, 0x025B26D7, + 0x800277A5, 0x01921D1F, + 0x80009DE9, 0x00C90F88, + 0x80000000, 0x00000000, + 0x80009DE9, 0xFF36F078, + 0x800277A5, 0xFE6DE2E0, + 0x80058D2E, 0xFDA4D928, + 0x8009DE7D, 0xFCDBD541, + 0x800F6B88, 0xFC12D919, + 0x80163440, 0xFB49E6A2, + 0x801E3894, 0xFA80FFCB, + 0x80277872, 0xF9B82683, + 0x8031F3C1, 0xF8EF5CBB, + 0x803DAA69, 0xF826A461, + 0x804A9C4D, 0xF75DFF65, + 0x8058C94C, 0xF6956FB6, + 0x80683143, 0xF5CCF743, + 0x8078D40D, 0xF50497FA, + 0x808AB180, 0xF43C53CA, + 0x809DC970, 0xF3742CA1, + 0x80B21BAF, 0xF2AC246D, + 0x80C7A80A, 0xF1E43D1C, + 0x80DE6E4C, 0xF11C789A, + 0x80F66E3C, 0xF054D8D4, + 0x810FA7A0, 0xEF8D5FB8, + 0x812A1A39, 0xEEC60F31, + 0x8145C5C6, 0xEDFEE92B, + 0x8162AA03, 0xED37EF91, + 0x8180C6A9, 0xEC71244F, + 0x81A01B6C, 0xEBAA894E, + 0x81C0A801, 0xEAE4207A, + 0x81E26C16, 0xEA1DEBBB, + 0x82056758, 0xE957ECFB, + 0x82299971, 0xE8922621, + 0x824F0208, 0xE7CC9917, + 0x8275A0C0, 0xE70747C3, + 0x829D753A, 0xE642340D, + 0x82C67F13, 0xE57D5FDA, + 0x82F0BDE8, 0xE4B8CD10, + 0x831C314E, 0xE3F47D95, + 0x8348D8DB, 0xE330734C, + 0x8376B422, 0xE26CB01A, + 0x83A5C2B0, 0xE1A935E1, + 0x83D60411, 0xE0E60684, + 0x840777CF, 0xE02323E5, + 0x843A1D70, 0xDF608FE3, + 0x846DF476, 0xDE9E4C60, + 0x84A2FC62, 0xDDDC5B3A, + 0x84D934B0, 0xDD1ABE51, + 0x85109CDC, 0xDC597781, + 0x8549345C, 0xDB9888A8, + 0x8582FAA4, 0xDAD7F3A2, + 0x85BDEF27, 0xDA17BA4A, + 0x85FA1152, 0xD957DE7A, + 0x86376092, 0xD898620C, + 0x8675DC4E, 0xD7D946D7, + 0x86B583EE, 0xD71A8EB5, + 0x86F656D3, 0xD65C3B7B, + 0x8738545E, 0xD59E4EFE, + 0x877B7BEC, 0xD4E0CB14, + 0x87BFCCD7, 0xD423B190, + 0x88054677, 0xD3670445, + 0x884BE820, 0xD2AAC504, + 0x8893B124, 0xD1EEF59E, + 0x88DCA0D3, 0xD13397E1, + 0x8926B677, 0xD078AD9D, + 0x8971F15A, 0xCFBE389F, + 0x89BE50C3, 0xCF043AB2, + 0x8A0BD3F5, 0xCE4AB5A2, + 0x8A5A7A30, 0xCD91AB38, + 0x8AAA42B4, 0xCCD91D3D, + 0x8AFB2CBA, 0xCC210D78, + 0x8B4D377C, 0xCB697DB0, + 0x8BA0622F, 0xCAB26FA9, + 0x8BF4AC05, 0xC9FBE527, + 0x8C4A142F, 0xC945DFEC, + 0x8CA099D9, 0xC89061BA, + 0x8CF83C30, 0xC7DB6C50, + 0x8D50FA59, 0xC727016C, + 0x8DAAD37B, 0xC67322CD, + 0x8E05C6B7, 0xC5BFD22E, + 0x8E61D32D, 0xC50D1148, + 0x8EBEF7FB, 0xC45AE1D7, + 0x8F1D343A, 0xC3A9458F, + 0x8F7C8701, 0xC2F83E2A, + 0x8FDCEF66, 0xC247CD5A, + 0x903E6C7A, 0xC197F4D3, + 0x90A0FD4E, 0xC0E8B648, + 0x9104A0ED, 0xC03A1368, + 0x91695663, 0xBF8C0DE2, + 0x91CF1CB6, 0xBEDEA765, + 0x9235F2EB, 0xBE31E19B, + 0x929DD805, 0xBD85BE2F, + 0x9306CB04, 0xBCDA3ECA, + 0x9370CAE4, 0xBC2F6513, + 0x93DBD69F, 0xBB8532AF, + 0x9447ED2F, 0xBADBA943, + 0x94B50D87, 0xBA32CA70, + 0x9523369B, 0xB98A97D8, + 0x9592675B, 0xB8E31319, + 0x96029EB5, 0xB83C3DD1, + 0x9673DB94, 0xB796199B, + 0x96E61CDF, 0xB6F0A811, + 0x9759617E, 0xB64BEACC, + 0x97CDA855, 0xB5A7E362, + 0x9842F043, 0xB5049368, + 0x98B93828, 0xB461FC70, + 0x99307EE0, 0xB3C0200C, + 0x99A8C344, 0xB31EFFCB, + 0x9A22042C, 0xB27E9D3B, + 0x9A9C406D, 0xB1DEF9E8, + 0x9B1776D9, 0xB140175B, + 0x9B93A640, 0xB0A1F71C, + 0x9C10CD70, 0xB0049AB2, + 0x9C8EEB33, 0xAF6803A1, + 0x9D0DFE53, 0xAECC336B, + 0x9D8E0596, 0xAE312B91, + 0x9E0EFFC1, 0xAD96ED91, + 0x9E90EB94, 0xACFD7AE8, + 0x9F13C7D0, 0xAC64D510, + 0x9F979331, 0xABCCFD82, + 0xA01C4C72, 0xAB35F5B5, + 0xA0A1F24C, 0xAA9FBF1D, + 0xA1288376, 0xAA0A5B2D, + 0xA1AFFEA2, 0xA975CB56, + 0xA2386283, 0xA8E21106, + 0xA2C1ADC9, 0xA84F2DA9, + 0xA34BDF20, 0xA7BD22AB, + 0xA3D6F533, 0xA72BF173, + 0xA462EEAC, 0xA69B9B68, + 0xA4EFCA31, 0xA60C21ED, + 0xA57D8666, 0xA57D8666, + 0xA60C21ED, 0xA4EFCA31, + 0xA69B9B68, 0xA462EEAC, + 0xA72BF173, 0xA3D6F533, + 0xA7BD22AB, 0xA34BDF20, + 0xA84F2DA9, 0xA2C1ADC9, + 0xA8E21106, 0xA2386283, + 0xA975CB56, 0xA1AFFEA2, + 0xAA0A5B2D, 0xA1288376, + 0xAA9FBF1D, 0xA0A1F24C, + 0xAB35F5B5, 0xA01C4C72, + 0xABCCFD82, 0x9F979331, + 0xAC64D510, 0x9F13C7D0, + 0xACFD7AE8, 0x9E90EB94, + 0xAD96ED91, 0x9E0EFFC1, + 0xAE312B91, 0x9D8E0596, + 0xAECC336B, 0x9D0DFE53, + 0xAF6803A1, 0x9C8EEB33, + 0xB0049AB2, 0x9C10CD70, + 0xB0A1F71C, 0x9B93A640, + 0xB140175B, 0x9B1776D9, + 0xB1DEF9E8, 0x9A9C406D, + 0xB27E9D3B, 0x9A22042C, + 0xB31EFFCB, 0x99A8C344, + 0xB3C0200C, 0x99307EE0, + 0xB461FC70, 0x98B93828, + 0xB5049368, 0x9842F043, + 0xB5A7E362, 0x97CDA855, + 0xB64BEACC, 0x9759617E, + 0xB6F0A811, 0x96E61CDF, + 0xB796199B, 0x9673DB94, + 0xB83C3DD1, 0x96029EB5, + 0xB8E31319, 0x9592675B, + 0xB98A97D8, 0x9523369B, + 0xBA32CA70, 0x94B50D87, + 0xBADBA943, 0x9447ED2F, + 0xBB8532AF, 0x93DBD69F, + 0xBC2F6513, 0x9370CAE4, + 0xBCDA3ECA, 0x9306CB04, + 0xBD85BE2F, 0x929DD805, + 0xBE31E19B, 0x9235F2EB, + 0xBEDEA765, 0x91CF1CB6, + 0xBF8C0DE2, 0x91695663, + 0xC03A1368, 0x9104A0ED, + 0xC0E8B648, 0x90A0FD4E, + 0xC197F4D3, 0x903E6C7A, + 0xC247CD5A, 0x8FDCEF66, + 0xC2F83E2A, 0x8F7C8701, + 0xC3A9458F, 0x8F1D343A, + 0xC45AE1D7, 0x8EBEF7FB, + 0xC50D1148, 0x8E61D32D, + 0xC5BFD22E, 0x8E05C6B7, + 0xC67322CD, 0x8DAAD37B, + 0xC727016C, 0x8D50FA59, + 0xC7DB6C50, 0x8CF83C30, + 0xC89061BA, 0x8CA099D9, + 0xC945DFEC, 0x8C4A142F, + 0xC9FBE527, 0x8BF4AC05, + 0xCAB26FA9, 0x8BA0622F, + 0xCB697DB0, 0x8B4D377C, + 0xCC210D78, 0x8AFB2CBA, + 0xCCD91D3D, 0x8AAA42B4, + 0xCD91AB38, 0x8A5A7A30, + 0xCE4AB5A2, 0x8A0BD3F5, + 0xCF043AB2, 0x89BE50C3, + 0xCFBE389F, 0x8971F15A, + 0xD078AD9D, 0x8926B677, + 0xD13397E1, 0x88DCA0D3, + 0xD1EEF59E, 0x8893B124, + 0xD2AAC504, 0x884BE820, + 0xD3670445, 0x88054677, + 0xD423B190, 0x87BFCCD7, + 0xD4E0CB14, 0x877B7BEC, + 0xD59E4EFE, 0x8738545E, + 0xD65C3B7B, 0x86F656D3, + 0xD71A8EB5, 0x86B583EE, + 0xD7D946D7, 0x8675DC4E, + 0xD898620C, 0x86376092, + 0xD957DE7A, 0x85FA1152, + 0xDA17BA4A, 0x85BDEF27, + 0xDAD7F3A2, 0x8582FAA4, + 0xDB9888A8, 0x8549345C, + 0xDC597781, 0x85109CDC, + 0xDD1ABE51, 0x84D934B0, + 0xDDDC5B3A, 0x84A2FC62, + 0xDE9E4C60, 0x846DF476, + 0xDF608FE3, 0x843A1D70, + 0xE02323E5, 0x840777CF, + 0xE0E60684, 0x83D60411, + 0xE1A935E1, 0x83A5C2B0, + 0xE26CB01A, 0x8376B422, + 0xE330734C, 0x8348D8DB, + 0xE3F47D95, 0x831C314E, + 0xE4B8CD10, 0x82F0BDE8, + 0xE57D5FDA, 0x82C67F13, + 0xE642340D, 0x829D753A, + 0xE70747C3, 0x8275A0C0, + 0xE7CC9917, 0x824F0208, + 0xE8922621, 0x82299971, + 0xE957ECFB, 0x82056758, + 0xEA1DEBBB, 0x81E26C16, + 0xEAE4207A, 0x81C0A801, + 0xEBAA894E, 0x81A01B6C, + 0xEC71244F, 0x8180C6A9, + 0xED37EF91, 0x8162AA03, + 0xEDFEE92B, 0x8145C5C6, + 0xEEC60F31, 0x812A1A39, + 0xEF8D5FB8, 0x810FA7A0, + 0xF054D8D4, 0x80F66E3C, + 0xF11C789A, 0x80DE6E4C, + 0xF1E43D1C, 0x80C7A80A, + 0xF2AC246D, 0x80B21BAF, + 0xF3742CA1, 0x809DC970, + 0xF43C53CA, 0x808AB180, + 0xF50497FA, 0x8078D40D, + 0xF5CCF743, 0x80683143, + 0xF6956FB6, 0x8058C94C, + 0xF75DFF65, 0x804A9C4D, + 0xF826A461, 0x803DAA69, + 0xF8EF5CBB, 0x8031F3C1, + 0xF9B82683, 0x80277872, + 0xFA80FFCB, 0x801E3894, + 0xFB49E6A2, 0x80163440, + 0xFC12D919, 0x800F6B88, + 0xFCDBD541, 0x8009DE7D, + 0xFDA4D928, 0x80058D2E, + 0xFE6DE2E0, 0x800277A5, + 0xFF36F078, 0x80009DE9 +}; + +/** +* \par +* Example code for Q31 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefQ31[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefQ31[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 2048 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to Q31(Fixed point 1.31): +* round(twiddleCoefQ31(i) * pow(2, 31)) +* +*/ +const q31_t twiddleCoef_2048_q31[3072] = { + 0x7FFFFFFF, 0x00000000, + 0x7FFFD885, 0x006487E3, + 0x7FFF6216, 0x00C90F88, + 0x7FFE9CB2, 0x012D96B0, + 0x7FFD885A, 0x01921D1F, + 0x7FFC250F, 0x01F6A296, + 0x7FFA72D1, 0x025B26D7, + 0x7FF871A1, 0x02BFA9A4, + 0x7FF62182, 0x03242ABF, + 0x7FF38273, 0x0388A9E9, + 0x7FF09477, 0x03ED26E6, + 0x7FED5790, 0x0451A176, + 0x7FE9CBC0, 0x04B6195D, + 0x7FE5F108, 0x051A8E5C, + 0x7FE1C76B, 0x057F0034, + 0x7FDD4EEC, 0x05E36EA9, + 0x7FD8878D, 0x0647D97C, + 0x7FD37152, 0x06AC406F, + 0x7FCE0C3E, 0x0710A344, + 0x7FC85853, 0x077501BE, + 0x7FC25596, 0x07D95B9E, + 0x7FBC040A, 0x083DB0A7, + 0x7FB563B2, 0x08A2009A, + 0x7FAE7494, 0x09064B3A, + 0x7FA736B4, 0x096A9049, + 0x7F9FAA15, 0x09CECF89, + 0x7F97CEBC, 0x0A3308BC, + 0x7F8FA4AF, 0x0A973BA5, + 0x7F872BF3, 0x0AFB6805, + 0x7F7E648B, 0x0B5F8D9F, + 0x7F754E7F, 0x0BC3AC35, + 0x7F6BE9D4, 0x0C27C389, + 0x7F62368F, 0x0C8BD35E, + 0x7F5834B6, 0x0CEFDB75, + 0x7F4DE450, 0x0D53DB92, + 0x7F434563, 0x0DB7D376, + 0x7F3857F5, 0x0E1BC2E3, + 0x7F2D1C0E, 0x0E7FA99D, + 0x7F2191B4, 0x0EE38765, + 0x7F15B8EE, 0x0F475BFE, + 0x7F0991C3, 0x0FAB272B, + 0x7EFD1C3C, 0x100EE8AD, + 0x7EF0585F, 0x1072A047, + 0x7EE34635, 0x10D64DBC, + 0x7ED5E5C6, 0x1139F0CE, + 0x7EC8371A, 0x119D8940, + 0x7EBA3A39, 0x120116D4, + 0x7EABEF2C, 0x1264994E, + 0x7E9D55FC, 0x12C8106E, + 0x7E8E6EB1, 0x132B7BF9, + 0x7E7F3956, 0x138EDBB0, + 0x7E6FB5F3, 0x13F22F57, + 0x7E5FE493, 0x145576B1, + 0x7E4FC53E, 0x14B8B17F, + 0x7E3F57FE, 0x151BDF85, + 0x7E2E9CDF, 0x157F0086, + 0x7E1D93E9, 0x15E21444, + 0x7E0C3D29, 0x16451A83, + 0x7DFA98A7, 0x16A81305, + 0x7DE8A670, 0x170AFD8D, + 0x7DD6668E, 0x176DD9DE, + 0x7DC3D90D, 0x17D0A7BB, + 0x7DB0FDF7, 0x183366E8, + 0x7D9DD55A, 0x18961727, + 0x7D8A5F3F, 0x18F8B83C, + 0x7D769BB5, 0x195B49E9, + 0x7D628AC5, 0x19BDCBF2, + 0x7D4E2C7E, 0x1A203E1B, + 0x7D3980EC, 0x1A82A025, + 0x7D24881A, 0x1AE4F1D6, + 0x7D0F4218, 0x1B4732EF, + 0x7CF9AEF0, 0x1BA96334, + 0x7CE3CEB1, 0x1C0B826A, + 0x7CCDA168, 0x1C6D9053, + 0x7CB72724, 0x1CCF8CB3, + 0x7CA05FF1, 0x1D31774D, + 0x7C894BDD, 0x1D934FE5, + 0x7C71EAF8, 0x1DF5163F, + 0x7C5A3D4F, 0x1E56CA1E, + 0x7C4242F2, 0x1EB86B46, + 0x7C29FBEE, 0x1F19F97B, + 0x7C116853, 0x1F7B7480, + 0x7BF88830, 0x1FDCDC1A, + 0x7BDF5B94, 0x203E300D, + 0x7BC5E28F, 0x209F701C, + 0x7BAC1D31, 0x21009C0B, + 0x7B920B89, 0x2161B39F, + 0x7B77ADA8, 0x21C2B69C, + 0x7B5D039D, 0x2223A4C5, + 0x7B420D7A, 0x22847DDF, + 0x7B26CB4F, 0x22E541AE, + 0x7B0B3D2C, 0x2345EFF7, + 0x7AEF6323, 0x23A6887E, + 0x7AD33D45, 0x24070B07, + 0x7AB6CBA3, 0x24677757, + 0x7A9A0E4F, 0x24C7CD32, + 0x7A7D055B, 0x25280C5D, + 0x7A5FB0D8, 0x2588349D, + 0x7A4210D8, 0x25E845B5, + 0x7A24256E, 0x26483F6C, + 0x7A05EEAD, 0x26A82185, + 0x79E76CA6, 0x2707EBC6, + 0x79C89F6D, 0x27679DF4, + 0x79A98715, 0x27C737D2, + 0x798A23B1, 0x2826B928, + 0x796A7554, 0x288621B9, + 0x794A7C11, 0x28E5714A, + 0x792A37FE, 0x2944A7A2, + 0x7909A92C, 0x29A3C484, + 0x78E8CFB1, 0x2A02C7B8, + 0x78C7ABA1, 0x2A61B101, + 0x78A63D10, 0x2AC08025, + 0x78848413, 0x2B1F34EB, + 0x786280BF, 0x2B7DCF17, + 0x78403328, 0x2BDC4E6F, + 0x781D9B64, 0x2C3AB2B9, + 0x77FAB988, 0x2C98FBBA, + 0x77D78DAA, 0x2CF72939, + 0x77B417DF, 0x2D553AFB, + 0x7790583D, 0x2DB330C7, + 0x776C4EDB, 0x2E110A62, + 0x7747FBCE, 0x2E6EC792, + 0x77235F2D, 0x2ECC681E, + 0x76FE790E, 0x2F29EBCC, + 0x76D94988, 0x2F875262, + 0x76B3D0B3, 0x2FE49BA6, + 0x768E0EA5, 0x3041C760, + 0x76680376, 0x309ED555, + 0x7641AF3C, 0x30FBC54D, + 0x761B1211, 0x3158970D, + 0x75F42C0A, 0x31B54A5D, + 0x75CCFD42, 0x3211DF03, + 0x75A585CF, 0x326E54C7, + 0x757DC5CA, 0x32CAAB6F, + 0x7555BD4B, 0x3326E2C2, + 0x752D6C6C, 0x3382FA88, + 0x7504D345, 0x33DEF287, + 0x74DBF1EF, 0x343ACA87, + 0x74B2C883, 0x3496824F, + 0x7489571B, 0x34F219A7, + 0x745F9DD1, 0x354D9056, + 0x74359CBD, 0x35A8E624, + 0x740B53FA, 0x36041AD9, + 0x73E0C3A3, 0x365F2E3B, + 0x73B5EBD0, 0x36BA2013, + 0x738ACC9E, 0x3714F02A, + 0x735F6626, 0x376F9E46, + 0x7333B883, 0x37CA2A30, + 0x7307C3D0, 0x382493B0, + 0x72DB8828, 0x387EDA8E, + 0x72AF05A6, 0x38D8FE93, + 0x72823C66, 0x3932FF87, + 0x72552C84, 0x398CDD32, + 0x7227D61C, 0x39E6975D, + 0x71FA3948, 0x3A402DD1, + 0x71CC5626, 0x3A99A057, + 0x719E2CD2, 0x3AF2EEB7, + 0x716FBD68, 0x3B4C18BA, + 0x71410804, 0x3BA51E29, + 0x71120CC5, 0x3BFDFECD, + 0x70E2CBC6, 0x3C56BA70, + 0x70B34524, 0x3CAF50DA, + 0x708378FE, 0x3D07C1D5, + 0x70536771, 0x3D600D2B, + 0x70231099, 0x3DB832A5, + 0x6FF27496, 0x3E10320D, + 0x6FC19385, 0x3E680B2C, + 0x6F906D84, 0x3EBFBDCC, + 0x6F5F02B1, 0x3F1749B7, + 0x6F2D532C, 0x3F6EAEB8, + 0x6EFB5F12, 0x3FC5EC97, + 0x6EC92682, 0x401D0320, + 0x6E96A99C, 0x4073F21D, + 0x6E63E87F, 0x40CAB957, + 0x6E30E349, 0x4121589A, + 0x6DFD9A1B, 0x4177CFB0, + 0x6DCA0D14, 0x41CE1E64, + 0x6D963C54, 0x42244480, + 0x6D6227FA, 0x427A41D0, + 0x6D2DD027, 0x42D0161E, + 0x6CF934FB, 0x4325C135, + 0x6CC45697, 0x437B42E1, + 0x6C8F351C, 0x43D09AEC, + 0x6C59D0A9, 0x4425C923, + 0x6C242960, 0x447ACD50, + 0x6BEE3F62, 0x44CFA73F, + 0x6BB812D0, 0x452456BC, + 0x6B81A3CD, 0x4578DB93, + 0x6B4AF278, 0x45CD358F, + 0x6B13FEF5, 0x4621647C, + 0x6ADCC964, 0x46756827, + 0x6AA551E8, 0x46C9405C, + 0x6A6D98A4, 0x471CECE6, + 0x6A359DB9, 0x47706D93, + 0x69FD614A, 0x47C3C22E, + 0x69C4E37A, 0x4816EA85, + 0x698C246C, 0x4869E664, + 0x69532442, 0x48BCB598, + 0x6919E320, 0x490F57EE, + 0x68E06129, 0x4961CD32, + 0x68A69E81, 0x49B41533, + 0x686C9B4B, 0x4A062FBD, + 0x683257AA, 0x4A581C9D, + 0x67F7D3C4, 0x4AA9DBA1, + 0x67BD0FBC, 0x4AFB6C97, + 0x67820BB6, 0x4B4CCF4D, + 0x6746C7D7, 0x4B9E038F, + 0x670B4443, 0x4BEF092D, + 0x66CF811F, 0x4C3FDFF3, + 0x66937E90, 0x4C9087B1, + 0x66573CBB, 0x4CE10034, + 0x661ABBC5, 0x4D31494B, + 0x65DDFBD3, 0x4D8162C4, + 0x65A0FD0B, 0x4DD14C6E, + 0x6563BF92, 0x4E210617, + 0x6526438E, 0x4E708F8F, + 0x64E88926, 0x4EBFE8A4, + 0x64AA907F, 0x4F0F1126, + 0x646C59BF, 0x4F5E08E3, + 0x642DE50D, 0x4FACCFAB, + 0x63EF328F, 0x4FFB654D, + 0x63B0426D, 0x5049C999, + 0x637114CC, 0x5097FC5E, + 0x6331A9D4, 0x50E5FD6C, + 0x62F201AC, 0x5133CC94, + 0x62B21C7B, 0x518169A4, + 0x6271FA69, 0x51CED46E, + 0x62319B9D, 0x521C0CC1, + 0x61F1003E, 0x5269126E, + 0x61B02876, 0x52B5E545, + 0x616F146B, 0x53028517, + 0x612DC446, 0x534EF1B5, + 0x60EC3830, 0x539B2AEF, + 0x60AA704F, 0x53E73097, + 0x60686CCE, 0x5433027D, + 0x60262DD5, 0x547EA073, + 0x5FE3B38D, 0x54CA0A4A, + 0x5FA0FE1E, 0x55153FD4, + 0x5F5E0DB3, 0x556040E2, + 0x5F1AE273, 0x55AB0D46, + 0x5ED77C89, 0x55F5A4D2, + 0x5E93DC1F, 0x56400757, + 0x5E50015D, 0x568A34A9, + 0x5E0BEC6E, 0x56D42C99, + 0x5DC79D7C, 0x571DEEF9, + 0x5D8314B0, 0x57677B9D, + 0x5D3E5236, 0x57B0D256, + 0x5CF95638, 0x57F9F2F7, + 0x5CB420DF, 0x5842DD54, + 0x5C6EB258, 0x588B913F, + 0x5C290ACC, 0x58D40E8C, + 0x5BE32A67, 0x591C550E, + 0x5B9D1153, 0x59646497, + 0x5B56BFBD, 0x59AC3CFD, + 0x5B1035CF, 0x59F3DE12, + 0x5AC973B4, 0x5A3B47AA, + 0x5A82799A, 0x5A82799A, + 0x5A3B47AA, 0x5AC973B4, + 0x59F3DE12, 0x5B1035CF, + 0x59AC3CFD, 0x5B56BFBD, + 0x59646497, 0x5B9D1153, + 0x591C550E, 0x5BE32A67, + 0x58D40E8C, 0x5C290ACC, + 0x588B913F, 0x5C6EB258, + 0x5842DD54, 0x5CB420DF, + 0x57F9F2F7, 0x5CF95638, + 0x57B0D256, 0x5D3E5236, + 0x57677B9D, 0x5D8314B0, + 0x571DEEF9, 0x5DC79D7C, + 0x56D42C99, 0x5E0BEC6E, + 0x568A34A9, 0x5E50015D, + 0x56400757, 0x5E93DC1F, + 0x55F5A4D2, 0x5ED77C89, + 0x55AB0D46, 0x5F1AE273, + 0x556040E2, 0x5F5E0DB3, + 0x55153FD4, 0x5FA0FE1E, + 0x54CA0A4A, 0x5FE3B38D, + 0x547EA073, 0x60262DD5, + 0x5433027D, 0x60686CCE, + 0x53E73097, 0x60AA704F, + 0x539B2AEF, 0x60EC3830, + 0x534EF1B5, 0x612DC446, + 0x53028517, 0x616F146B, + 0x52B5E545, 0x61B02876, + 0x5269126E, 0x61F1003E, + 0x521C0CC1, 0x62319B9D, + 0x51CED46E, 0x6271FA69, + 0x518169A4, 0x62B21C7B, + 0x5133CC94, 0x62F201AC, + 0x50E5FD6C, 0x6331A9D4, + 0x5097FC5E, 0x637114CC, + 0x5049C999, 0x63B0426D, + 0x4FFB654D, 0x63EF328F, + 0x4FACCFAB, 0x642DE50D, + 0x4F5E08E3, 0x646C59BF, + 0x4F0F1126, 0x64AA907F, + 0x4EBFE8A4, 0x64E88926, + 0x4E708F8F, 0x6526438E, + 0x4E210617, 0x6563BF92, + 0x4DD14C6E, 0x65A0FD0B, + 0x4D8162C4, 0x65DDFBD3, + 0x4D31494B, 0x661ABBC5, + 0x4CE10034, 0x66573CBB, + 0x4C9087B1, 0x66937E90, + 0x4C3FDFF3, 0x66CF811F, + 0x4BEF092D, 0x670B4443, + 0x4B9E038F, 0x6746C7D7, + 0x4B4CCF4D, 0x67820BB6, + 0x4AFB6C97, 0x67BD0FBC, + 0x4AA9DBA1, 0x67F7D3C4, + 0x4A581C9D, 0x683257AA, + 0x4A062FBD, 0x686C9B4B, + 0x49B41533, 0x68A69E81, + 0x4961CD32, 0x68E06129, + 0x490F57EE, 0x6919E320, + 0x48BCB598, 0x69532442, + 0x4869E664, 0x698C246C, + 0x4816EA85, 0x69C4E37A, + 0x47C3C22E, 0x69FD614A, + 0x47706D93, 0x6A359DB9, + 0x471CECE6, 0x6A6D98A4, + 0x46C9405C, 0x6AA551E8, + 0x46756827, 0x6ADCC964, + 0x4621647C, 0x6B13FEF5, + 0x45CD358F, 0x6B4AF278, + 0x4578DB93, 0x6B81A3CD, + 0x452456BC, 0x6BB812D0, + 0x44CFA73F, 0x6BEE3F62, + 0x447ACD50, 0x6C242960, + 0x4425C923, 0x6C59D0A9, + 0x43D09AEC, 0x6C8F351C, + 0x437B42E1, 0x6CC45697, + 0x4325C135, 0x6CF934FB, + 0x42D0161E, 0x6D2DD027, + 0x427A41D0, 0x6D6227FA, + 0x42244480, 0x6D963C54, + 0x41CE1E64, 0x6DCA0D14, + 0x4177CFB0, 0x6DFD9A1B, + 0x4121589A, 0x6E30E349, + 0x40CAB957, 0x6E63E87F, + 0x4073F21D, 0x6E96A99C, + 0x401D0320, 0x6EC92682, + 0x3FC5EC97, 0x6EFB5F12, + 0x3F6EAEB8, 0x6F2D532C, + 0x3F1749B7, 0x6F5F02B1, + 0x3EBFBDCC, 0x6F906D84, + 0x3E680B2C, 0x6FC19385, + 0x3E10320D, 0x6FF27496, + 0x3DB832A5, 0x70231099, + 0x3D600D2B, 0x70536771, + 0x3D07C1D5, 0x708378FE, + 0x3CAF50DA, 0x70B34524, + 0x3C56BA70, 0x70E2CBC6, + 0x3BFDFECD, 0x71120CC5, + 0x3BA51E29, 0x71410804, + 0x3B4C18BA, 0x716FBD68, + 0x3AF2EEB7, 0x719E2CD2, + 0x3A99A057, 0x71CC5626, + 0x3A402DD1, 0x71FA3948, + 0x39E6975D, 0x7227D61C, + 0x398CDD32, 0x72552C84, + 0x3932FF87, 0x72823C66, + 0x38D8FE93, 0x72AF05A6, + 0x387EDA8E, 0x72DB8828, + 0x382493B0, 0x7307C3D0, + 0x37CA2A30, 0x7333B883, + 0x376F9E46, 0x735F6626, + 0x3714F02A, 0x738ACC9E, + 0x36BA2013, 0x73B5EBD0, + 0x365F2E3B, 0x73E0C3A3, + 0x36041AD9, 0x740B53FA, + 0x35A8E624, 0x74359CBD, + 0x354D9056, 0x745F9DD1, + 0x34F219A7, 0x7489571B, + 0x3496824F, 0x74B2C883, + 0x343ACA87, 0x74DBF1EF, + 0x33DEF287, 0x7504D345, + 0x3382FA88, 0x752D6C6C, + 0x3326E2C2, 0x7555BD4B, + 0x32CAAB6F, 0x757DC5CA, + 0x326E54C7, 0x75A585CF, + 0x3211DF03, 0x75CCFD42, + 0x31B54A5D, 0x75F42C0A, + 0x3158970D, 0x761B1211, + 0x30FBC54D, 0x7641AF3C, + 0x309ED555, 0x76680376, + 0x3041C760, 0x768E0EA5, + 0x2FE49BA6, 0x76B3D0B3, + 0x2F875262, 0x76D94988, + 0x2F29EBCC, 0x76FE790E, + 0x2ECC681E, 0x77235F2D, + 0x2E6EC792, 0x7747FBCE, + 0x2E110A62, 0x776C4EDB, + 0x2DB330C7, 0x7790583D, + 0x2D553AFB, 0x77B417DF, + 0x2CF72939, 0x77D78DAA, + 0x2C98FBBA, 0x77FAB988, + 0x2C3AB2B9, 0x781D9B64, + 0x2BDC4E6F, 0x78403328, + 0x2B7DCF17, 0x786280BF, + 0x2B1F34EB, 0x78848413, + 0x2AC08025, 0x78A63D10, + 0x2A61B101, 0x78C7ABA1, + 0x2A02C7B8, 0x78E8CFB1, + 0x29A3C484, 0x7909A92C, + 0x2944A7A2, 0x792A37FE, + 0x28E5714A, 0x794A7C11, + 0x288621B9, 0x796A7554, + 0x2826B928, 0x798A23B1, + 0x27C737D2, 0x79A98715, + 0x27679DF4, 0x79C89F6D, + 0x2707EBC6, 0x79E76CA6, + 0x26A82185, 0x7A05EEAD, + 0x26483F6C, 0x7A24256E, + 0x25E845B5, 0x7A4210D8, + 0x2588349D, 0x7A5FB0D8, + 0x25280C5D, 0x7A7D055B, + 0x24C7CD32, 0x7A9A0E4F, + 0x24677757, 0x7AB6CBA3, + 0x24070B07, 0x7AD33D45, + 0x23A6887E, 0x7AEF6323, + 0x2345EFF7, 0x7B0B3D2C, + 0x22E541AE, 0x7B26CB4F, + 0x22847DDF, 0x7B420D7A, + 0x2223A4C5, 0x7B5D039D, + 0x21C2B69C, 0x7B77ADA8, + 0x2161B39F, 0x7B920B89, + 0x21009C0B, 0x7BAC1D31, + 0x209F701C, 0x7BC5E28F, + 0x203E300D, 0x7BDF5B94, + 0x1FDCDC1A, 0x7BF88830, + 0x1F7B7480, 0x7C116853, + 0x1F19F97B, 0x7C29FBEE, + 0x1EB86B46, 0x7C4242F2, + 0x1E56CA1E, 0x7C5A3D4F, + 0x1DF5163F, 0x7C71EAF8, + 0x1D934FE5, 0x7C894BDD, + 0x1D31774D, 0x7CA05FF1, + 0x1CCF8CB3, 0x7CB72724, + 0x1C6D9053, 0x7CCDA168, + 0x1C0B826A, 0x7CE3CEB1, + 0x1BA96334, 0x7CF9AEF0, + 0x1B4732EF, 0x7D0F4218, + 0x1AE4F1D6, 0x7D24881A, + 0x1A82A025, 0x7D3980EC, + 0x1A203E1B, 0x7D4E2C7E, + 0x19BDCBF2, 0x7D628AC5, + 0x195B49E9, 0x7D769BB5, + 0x18F8B83C, 0x7D8A5F3F, + 0x18961727, 0x7D9DD55A, + 0x183366E8, 0x7DB0FDF7, + 0x17D0A7BB, 0x7DC3D90D, + 0x176DD9DE, 0x7DD6668E, + 0x170AFD8D, 0x7DE8A670, + 0x16A81305, 0x7DFA98A7, + 0x16451A83, 0x7E0C3D29, + 0x15E21444, 0x7E1D93E9, + 0x157F0086, 0x7E2E9CDF, + 0x151BDF85, 0x7E3F57FE, + 0x14B8B17F, 0x7E4FC53E, + 0x145576B1, 0x7E5FE493, + 0x13F22F57, 0x7E6FB5F3, + 0x138EDBB0, 0x7E7F3956, + 0x132B7BF9, 0x7E8E6EB1, + 0x12C8106E, 0x7E9D55FC, + 0x1264994E, 0x7EABEF2C, + 0x120116D4, 0x7EBA3A39, + 0x119D8940, 0x7EC8371A, + 0x1139F0CE, 0x7ED5E5C6, + 0x10D64DBC, 0x7EE34635, + 0x1072A047, 0x7EF0585F, + 0x100EE8AD, 0x7EFD1C3C, + 0x0FAB272B, 0x7F0991C3, + 0x0F475BFE, 0x7F15B8EE, + 0x0EE38765, 0x7F2191B4, + 0x0E7FA99D, 0x7F2D1C0E, + 0x0E1BC2E3, 0x7F3857F5, + 0x0DB7D376, 0x7F434563, + 0x0D53DB92, 0x7F4DE450, + 0x0CEFDB75, 0x7F5834B6, + 0x0C8BD35E, 0x7F62368F, + 0x0C27C389, 0x7F6BE9D4, + 0x0BC3AC35, 0x7F754E7F, + 0x0B5F8D9F, 0x7F7E648B, + 0x0AFB6805, 0x7F872BF3, + 0x0A973BA5, 0x7F8FA4AF, + 0x0A3308BC, 0x7F97CEBC, + 0x09CECF89, 0x7F9FAA15, + 0x096A9049, 0x7FA736B4, + 0x09064B3A, 0x7FAE7494, + 0x08A2009A, 0x7FB563B2, + 0x083DB0A7, 0x7FBC040A, + 0x07D95B9E, 0x7FC25596, + 0x077501BE, 0x7FC85853, + 0x0710A344, 0x7FCE0C3E, + 0x06AC406F, 0x7FD37152, + 0x0647D97C, 0x7FD8878D, + 0x05E36EA9, 0x7FDD4EEC, + 0x057F0034, 0x7FE1C76B, + 0x051A8E5C, 0x7FE5F108, + 0x04B6195D, 0x7FE9CBC0, + 0x0451A176, 0x7FED5790, + 0x03ED26E6, 0x7FF09477, + 0x0388A9E9, 0x7FF38273, + 0x03242ABF, 0x7FF62182, + 0x02BFA9A4, 0x7FF871A1, + 0x025B26D7, 0x7FFA72D1, + 0x01F6A296, 0x7FFC250F, + 0x01921D1F, 0x7FFD885A, + 0x012D96B0, 0x7FFE9CB2, + 0x00C90F88, 0x7FFF6216, + 0x006487E3, 0x7FFFD885, + 0x00000000, 0x7FFFFFFF, + 0xFF9B781D, 0x7FFFD885, + 0xFF36F078, 0x7FFF6216, + 0xFED2694F, 0x7FFE9CB2, + 0xFE6DE2E0, 0x7FFD885A, + 0xFE095D69, 0x7FFC250F, + 0xFDA4D928, 0x7FFA72D1, + 0xFD40565B, 0x7FF871A1, + 0xFCDBD541, 0x7FF62182, + 0xFC775616, 0x7FF38273, + 0xFC12D919, 0x7FF09477, + 0xFBAE5E89, 0x7FED5790, + 0xFB49E6A2, 0x7FE9CBC0, + 0xFAE571A4, 0x7FE5F108, + 0xFA80FFCB, 0x7FE1C76B, + 0xFA1C9156, 0x7FDD4EEC, + 0xF9B82683, 0x7FD8878D, + 0xF953BF90, 0x7FD37152, + 0xF8EF5CBB, 0x7FCE0C3E, + 0xF88AFE41, 0x7FC85853, + 0xF826A461, 0x7FC25596, + 0xF7C24F58, 0x7FBC040A, + 0xF75DFF65, 0x7FB563B2, + 0xF6F9B4C5, 0x7FAE7494, + 0xF6956FB6, 0x7FA736B4, + 0xF6313076, 0x7F9FAA15, + 0xF5CCF743, 0x7F97CEBC, + 0xF568C45A, 0x7F8FA4AF, + 0xF50497FA, 0x7F872BF3, + 0xF4A07260, 0x7F7E648B, + 0xF43C53CA, 0x7F754E7F, + 0xF3D83C76, 0x7F6BE9D4, + 0xF3742CA1, 0x7F62368F, + 0xF310248A, 0x7F5834B6, + 0xF2AC246D, 0x7F4DE450, + 0xF2482C89, 0x7F434563, + 0xF1E43D1C, 0x7F3857F5, + 0xF1805662, 0x7F2D1C0E, + 0xF11C789A, 0x7F2191B4, + 0xF0B8A401, 0x7F15B8EE, + 0xF054D8D4, 0x7F0991C3, + 0xEFF11752, 0x7EFD1C3C, + 0xEF8D5FB8, 0x7EF0585F, + 0xEF29B243, 0x7EE34635, + 0xEEC60F31, 0x7ED5E5C6, + 0xEE6276BF, 0x7EC8371A, + 0xEDFEE92B, 0x7EBA3A39, + 0xED9B66B2, 0x7EABEF2C, + 0xED37EF91, 0x7E9D55FC, + 0xECD48406, 0x7E8E6EB1, + 0xEC71244F, 0x7E7F3956, + 0xEC0DD0A8, 0x7E6FB5F3, + 0xEBAA894E, 0x7E5FE493, + 0xEB474E80, 0x7E4FC53E, + 0xEAE4207A, 0x7E3F57FE, + 0xEA80FF79, 0x7E2E9CDF, + 0xEA1DEBBB, 0x7E1D93E9, + 0xE9BAE57C, 0x7E0C3D29, + 0xE957ECFB, 0x7DFA98A7, + 0xE8F50273, 0x7DE8A670, + 0xE8922621, 0x7DD6668E, + 0xE82F5844, 0x7DC3D90D, + 0xE7CC9917, 0x7DB0FDF7, + 0xE769E8D8, 0x7D9DD55A, + 0xE70747C3, 0x7D8A5F3F, + 0xE6A4B616, 0x7D769BB5, + 0xE642340D, 0x7D628AC5, + 0xE5DFC1E4, 0x7D4E2C7E, + 0xE57D5FDA, 0x7D3980EC, + 0xE51B0E2A, 0x7D24881A, + 0xE4B8CD10, 0x7D0F4218, + 0xE4569CCB, 0x7CF9AEF0, + 0xE3F47D95, 0x7CE3CEB1, + 0xE3926FAC, 0x7CCDA168, + 0xE330734C, 0x7CB72724, + 0xE2CE88B2, 0x7CA05FF1, + 0xE26CB01A, 0x7C894BDD, + 0xE20AE9C1, 0x7C71EAF8, + 0xE1A935E1, 0x7C5A3D4F, + 0xE14794B9, 0x7C4242F2, + 0xE0E60684, 0x7C29FBEE, + 0xE0848B7F, 0x7C116853, + 0xE02323E5, 0x7BF88830, + 0xDFC1CFF2, 0x7BDF5B94, + 0xDF608FE3, 0x7BC5E28F, + 0xDEFF63F4, 0x7BAC1D31, + 0xDE9E4C60, 0x7B920B89, + 0xDE3D4963, 0x7B77ADA8, + 0xDDDC5B3A, 0x7B5D039D, + 0xDD7B8220, 0x7B420D7A, + 0xDD1ABE51, 0x7B26CB4F, + 0xDCBA1008, 0x7B0B3D2C, + 0xDC597781, 0x7AEF6323, + 0xDBF8F4F8, 0x7AD33D45, + 0xDB9888A8, 0x7AB6CBA3, + 0xDB3832CD, 0x7A9A0E4F, + 0xDAD7F3A2, 0x7A7D055B, + 0xDA77CB62, 0x7A5FB0D8, + 0xDA17BA4A, 0x7A4210D8, + 0xD9B7C093, 0x7A24256E, + 0xD957DE7A, 0x7A05EEAD, + 0xD8F81439, 0x79E76CA6, + 0xD898620C, 0x79C89F6D, + 0xD838C82D, 0x79A98715, + 0xD7D946D7, 0x798A23B1, + 0xD779DE46, 0x796A7554, + 0xD71A8EB5, 0x794A7C11, + 0xD6BB585D, 0x792A37FE, + 0xD65C3B7B, 0x7909A92C, + 0xD5FD3847, 0x78E8CFB1, + 0xD59E4EFE, 0x78C7ABA1, + 0xD53F7FDA, 0x78A63D10, + 0xD4E0CB14, 0x78848413, + 0xD48230E8, 0x786280BF, + 0xD423B190, 0x78403328, + 0xD3C54D46, 0x781D9B64, + 0xD3670445, 0x77FAB988, + 0xD308D6C6, 0x77D78DAA, + 0xD2AAC504, 0x77B417DF, + 0xD24CCF38, 0x7790583D, + 0xD1EEF59E, 0x776C4EDB, + 0xD191386D, 0x7747FBCE, + 0xD13397E1, 0x77235F2D, + 0xD0D61433, 0x76FE790E, + 0xD078AD9D, 0x76D94988, + 0xD01B6459, 0x76B3D0B3, + 0xCFBE389F, 0x768E0EA5, + 0xCF612AAA, 0x76680376, + 0xCF043AB2, 0x7641AF3C, + 0xCEA768F2, 0x761B1211, + 0xCE4AB5A2, 0x75F42C0A, + 0xCDEE20FC, 0x75CCFD42, + 0xCD91AB38, 0x75A585CF, + 0xCD355490, 0x757DC5CA, + 0xCCD91D3D, 0x7555BD4B, + 0xCC7D0577, 0x752D6C6C, + 0xCC210D78, 0x7504D345, + 0xCBC53578, 0x74DBF1EF, + 0xCB697DB0, 0x74B2C883, + 0xCB0DE658, 0x7489571B, + 0xCAB26FA9, 0x745F9DD1, + 0xCA5719DB, 0x74359CBD, + 0xC9FBE527, 0x740B53FA, + 0xC9A0D1C4, 0x73E0C3A3, + 0xC945DFEC, 0x73B5EBD0, + 0xC8EB0FD6, 0x738ACC9E, + 0xC89061BA, 0x735F6626, + 0xC835D5D0, 0x7333B883, + 0xC7DB6C50, 0x7307C3D0, + 0xC7812571, 0x72DB8828, + 0xC727016C, 0x72AF05A6, + 0xC6CD0079, 0x72823C66, + 0xC67322CD, 0x72552C84, + 0xC61968A2, 0x7227D61C, + 0xC5BFD22E, 0x71FA3948, + 0xC5665FA8, 0x71CC5626, + 0xC50D1148, 0x719E2CD2, + 0xC4B3E746, 0x716FBD68, + 0xC45AE1D7, 0x71410804, + 0xC4020132, 0x71120CC5, + 0xC3A9458F, 0x70E2CBC6, + 0xC350AF25, 0x70B34524, + 0xC2F83E2A, 0x708378FE, + 0xC29FF2D4, 0x70536771, + 0xC247CD5A, 0x70231099, + 0xC1EFCDF2, 0x6FF27496, + 0xC197F4D3, 0x6FC19385, + 0xC1404233, 0x6F906D84, + 0xC0E8B648, 0x6F5F02B1, + 0xC0915147, 0x6F2D532C, + 0xC03A1368, 0x6EFB5F12, + 0xBFE2FCDF, 0x6EC92682, + 0xBF8C0DE2, 0x6E96A99C, + 0xBF3546A8, 0x6E63E87F, + 0xBEDEA765, 0x6E30E349, + 0xBE88304F, 0x6DFD9A1B, + 0xBE31E19B, 0x6DCA0D14, + 0xBDDBBB7F, 0x6D963C54, + 0xBD85BE2F, 0x6D6227FA, + 0xBD2FE9E1, 0x6D2DD027, + 0xBCDA3ECA, 0x6CF934FB, + 0xBC84BD1E, 0x6CC45697, + 0xBC2F6513, 0x6C8F351C, + 0xBBDA36DC, 0x6C59D0A9, + 0xBB8532AF, 0x6C242960, + 0xBB3058C0, 0x6BEE3F62, + 0xBADBA943, 0x6BB812D0, + 0xBA87246C, 0x6B81A3CD, + 0xBA32CA70, 0x6B4AF278, + 0xB9DE9B83, 0x6B13FEF5, + 0xB98A97D8, 0x6ADCC964, + 0xB936BFA3, 0x6AA551E8, + 0xB8E31319, 0x6A6D98A4, + 0xB88F926C, 0x6A359DB9, + 0xB83C3DD1, 0x69FD614A, + 0xB7E9157A, 0x69C4E37A, + 0xB796199B, 0x698C246C, + 0xB7434A67, 0x69532442, + 0xB6F0A811, 0x6919E320, + 0xB69E32CD, 0x68E06129, + 0xB64BEACC, 0x68A69E81, + 0xB5F9D042, 0x686C9B4B, + 0xB5A7E362, 0x683257AA, + 0xB556245E, 0x67F7D3C4, + 0xB5049368, 0x67BD0FBC, + 0xB4B330B2, 0x67820BB6, + 0xB461FC70, 0x6746C7D7, + 0xB410F6D2, 0x670B4443, + 0xB3C0200C, 0x66CF811F, + 0xB36F784E, 0x66937E90, + 0xB31EFFCB, 0x66573CBB, + 0xB2CEB6B5, 0x661ABBC5, + 0xB27E9D3B, 0x65DDFBD3, + 0xB22EB392, 0x65A0FD0B, + 0xB1DEF9E8, 0x6563BF92, + 0xB18F7070, 0x6526438E, + 0xB140175B, 0x64E88926, + 0xB0F0EEDA, 0x64AA907F, + 0xB0A1F71C, 0x646C59BF, + 0xB0533055, 0x642DE50D, + 0xB0049AB2, 0x63EF328F, + 0xAFB63667, 0x63B0426D, + 0xAF6803A1, 0x637114CC, + 0xAF1A0293, 0x6331A9D4, + 0xAECC336B, 0x62F201AC, + 0xAE7E965B, 0x62B21C7B, + 0xAE312B91, 0x6271FA69, + 0xADE3F33E, 0x62319B9D, + 0xAD96ED91, 0x61F1003E, + 0xAD4A1ABA, 0x61B02876, + 0xACFD7AE8, 0x616F146B, + 0xACB10E4A, 0x612DC446, + 0xAC64D510, 0x60EC3830, + 0xAC18CF68, 0x60AA704F, + 0xABCCFD82, 0x60686CCE, + 0xAB815F8C, 0x60262DD5, + 0xAB35F5B5, 0x5FE3B38D, + 0xAAEAC02B, 0x5FA0FE1E, + 0xAA9FBF1D, 0x5F5E0DB3, + 0xAA54F2B9, 0x5F1AE273, + 0xAA0A5B2D, 0x5ED77C89, + 0xA9BFF8A8, 0x5E93DC1F, + 0xA975CB56, 0x5E50015D, + 0xA92BD366, 0x5E0BEC6E, + 0xA8E21106, 0x5DC79D7C, + 0xA8988463, 0x5D8314B0, + 0xA84F2DA9, 0x5D3E5236, + 0xA8060D08, 0x5CF95638, + 0xA7BD22AB, 0x5CB420DF, + 0xA7746EC0, 0x5C6EB258, + 0xA72BF173, 0x5C290ACC, + 0xA6E3AAF2, 0x5BE32A67, + 0xA69B9B68, 0x5B9D1153, + 0xA653C302, 0x5B56BFBD, + 0xA60C21ED, 0x5B1035CF, + 0xA5C4B855, 0x5AC973B4, + 0xA57D8666, 0x5A82799A, + 0xA5368C4B, 0x5A3B47AA, + 0xA4EFCA31, 0x59F3DE12, + 0xA4A94042, 0x59AC3CFD, + 0xA462EEAC, 0x59646497, + 0xA41CD598, 0x591C550E, + 0xA3D6F533, 0x58D40E8C, + 0xA3914DA7, 0x588B913F, + 0xA34BDF20, 0x5842DD54, + 0xA306A9C7, 0x57F9F2F7, + 0xA2C1ADC9, 0x57B0D256, + 0xA27CEB4F, 0x57677B9D, + 0xA2386283, 0x571DEEF9, + 0xA1F41391, 0x56D42C99, + 0xA1AFFEA2, 0x568A34A9, + 0xA16C23E1, 0x56400757, + 0xA1288376, 0x55F5A4D2, + 0xA0E51D8C, 0x55AB0D46, + 0xA0A1F24C, 0x556040E2, + 0xA05F01E1, 0x55153FD4, + 0xA01C4C72, 0x54CA0A4A, + 0x9FD9D22A, 0x547EA073, + 0x9F979331, 0x5433027D, + 0x9F558FB0, 0x53E73097, + 0x9F13C7D0, 0x539B2AEF, + 0x9ED23BB9, 0x534EF1B5, + 0x9E90EB94, 0x53028517, + 0x9E4FD789, 0x52B5E545, + 0x9E0EFFC1, 0x5269126E, + 0x9DCE6462, 0x521C0CC1, + 0x9D8E0596, 0x51CED46E, + 0x9D4DE384, 0x518169A4, + 0x9D0DFE53, 0x5133CC94, + 0x9CCE562B, 0x50E5FD6C, + 0x9C8EEB33, 0x5097FC5E, + 0x9C4FBD92, 0x5049C999, + 0x9C10CD70, 0x4FFB654D, + 0x9BD21AF2, 0x4FACCFAB, + 0x9B93A640, 0x4F5E08E3, + 0x9B556F80, 0x4F0F1126, + 0x9B1776D9, 0x4EBFE8A4, + 0x9AD9BC71, 0x4E708F8F, + 0x9A9C406D, 0x4E210617, + 0x9A5F02F5, 0x4DD14C6E, + 0x9A22042C, 0x4D8162C4, + 0x99E5443A, 0x4D31494B, + 0x99A8C344, 0x4CE10034, + 0x996C816F, 0x4C9087B1, + 0x99307EE0, 0x4C3FDFF3, + 0x98F4BBBC, 0x4BEF092D, + 0x98B93828, 0x4B9E038F, + 0x987DF449, 0x4B4CCF4D, + 0x9842F043, 0x4AFB6C97, + 0x98082C3B, 0x4AA9DBA1, + 0x97CDA855, 0x4A581C9D, + 0x979364B5, 0x4A062FBD, + 0x9759617E, 0x49B41533, + 0x971F9ED6, 0x4961CD32, + 0x96E61CDF, 0x490F57EE, + 0x96ACDBBD, 0x48BCB598, + 0x9673DB94, 0x4869E664, + 0x963B1C85, 0x4816EA85, + 0x96029EB5, 0x47C3C22E, + 0x95CA6246, 0x47706D93, + 0x9592675B, 0x471CECE6, + 0x955AAE17, 0x46C9405C, + 0x9523369B, 0x46756827, + 0x94EC010B, 0x4621647C, + 0x94B50D87, 0x45CD358F, + 0x947E5C32, 0x4578DB93, + 0x9447ED2F, 0x452456BC, + 0x9411C09D, 0x44CFA73F, + 0x93DBD69F, 0x447ACD50, + 0x93A62F56, 0x4425C923, + 0x9370CAE4, 0x43D09AEC, + 0x933BA968, 0x437B42E1, + 0x9306CB04, 0x4325C135, + 0x92D22FD8, 0x42D0161E, + 0x929DD805, 0x427A41D0, + 0x9269C3AC, 0x42244480, + 0x9235F2EB, 0x41CE1E64, + 0x920265E4, 0x4177CFB0, + 0x91CF1CB6, 0x4121589A, + 0x919C1780, 0x40CAB957, + 0x91695663, 0x4073F21D, + 0x9136D97D, 0x401D0320, + 0x9104A0ED, 0x3FC5EC97, + 0x90D2ACD3, 0x3F6EAEB8, + 0x90A0FD4E, 0x3F1749B7, + 0x906F927B, 0x3EBFBDCC, + 0x903E6C7A, 0x3E680B2C, + 0x900D8B69, 0x3E10320D, + 0x8FDCEF66, 0x3DB832A5, + 0x8FAC988E, 0x3D600D2B, + 0x8F7C8701, 0x3D07C1D5, + 0x8F4CBADB, 0x3CAF50DA, + 0x8F1D343A, 0x3C56BA70, + 0x8EEDF33B, 0x3BFDFECD, + 0x8EBEF7FB, 0x3BA51E29, + 0x8E904298, 0x3B4C18BA, + 0x8E61D32D, 0x3AF2EEB7, + 0x8E33A9D9, 0x3A99A057, + 0x8E05C6B7, 0x3A402DD1, + 0x8DD829E4, 0x39E6975D, + 0x8DAAD37B, 0x398CDD32, + 0x8D7DC399, 0x3932FF87, + 0x8D50FA59, 0x38D8FE93, + 0x8D2477D8, 0x387EDA8E, + 0x8CF83C30, 0x382493B0, + 0x8CCC477D, 0x37CA2A30, + 0x8CA099D9, 0x376F9E46, + 0x8C753361, 0x3714F02A, + 0x8C4A142F, 0x36BA2013, + 0x8C1F3C5C, 0x365F2E3B, + 0x8BF4AC05, 0x36041AD9, + 0x8BCA6342, 0x35A8E624, + 0x8BA0622F, 0x354D9056, + 0x8B76A8E4, 0x34F219A7, + 0x8B4D377C, 0x3496824F, + 0x8B240E10, 0x343ACA87, + 0x8AFB2CBA, 0x33DEF287, + 0x8AD29393, 0x3382FA88, + 0x8AAA42B4, 0x3326E2C2, + 0x8A823A35, 0x32CAAB6F, + 0x8A5A7A30, 0x326E54C7, + 0x8A3302BD, 0x3211DF03, + 0x8A0BD3F5, 0x31B54A5D, + 0x89E4EDEE, 0x3158970D, + 0x89BE50C3, 0x30FBC54D, + 0x8997FC89, 0x309ED555, + 0x8971F15A, 0x3041C760, + 0x894C2F4C, 0x2FE49BA6, + 0x8926B677, 0x2F875262, + 0x890186F1, 0x2F29EBCC, + 0x88DCA0D3, 0x2ECC681E, + 0x88B80431, 0x2E6EC792, + 0x8893B124, 0x2E110A62, + 0x886FA7C2, 0x2DB330C7, + 0x884BE820, 0x2D553AFB, + 0x88287255, 0x2CF72939, + 0x88054677, 0x2C98FBBA, + 0x87E2649B, 0x2C3AB2B9, + 0x87BFCCD7, 0x2BDC4E6F, + 0x879D7F40, 0x2B7DCF17, + 0x877B7BEC, 0x2B1F34EB, + 0x8759C2EF, 0x2AC08025, + 0x8738545E, 0x2A61B101, + 0x8717304E, 0x2A02C7B8, + 0x86F656D3, 0x29A3C484, + 0x86D5C802, 0x2944A7A2, + 0x86B583EE, 0x28E5714A, + 0x86958AAB, 0x288621B9, + 0x8675DC4E, 0x2826B928, + 0x865678EA, 0x27C737D2, + 0x86376092, 0x27679DF4, + 0x86189359, 0x2707EBC6, + 0x85FA1152, 0x26A82185, + 0x85DBDA91, 0x26483F6C, + 0x85BDEF27, 0x25E845B5, + 0x85A04F28, 0x2588349D, + 0x8582FAA4, 0x25280C5D, + 0x8565F1B0, 0x24C7CD32, + 0x8549345C, 0x24677757, + 0x852CC2BA, 0x24070B07, + 0x85109CDC, 0x23A6887E, + 0x84F4C2D3, 0x2345EFF7, + 0x84D934B0, 0x22E541AE, + 0x84BDF285, 0x22847DDF, + 0x84A2FC62, 0x2223A4C5, + 0x84885257, 0x21C2B69C, + 0x846DF476, 0x2161B39F, + 0x8453E2CE, 0x21009C0B, + 0x843A1D70, 0x209F701C, + 0x8420A46B, 0x203E300D, + 0x840777CF, 0x1FDCDC1A, + 0x83EE97AC, 0x1F7B7480, + 0x83D60411, 0x1F19F97B, + 0x83BDBD0D, 0x1EB86B46, + 0x83A5C2B0, 0x1E56CA1E, + 0x838E1507, 0x1DF5163F, + 0x8376B422, 0x1D934FE5, + 0x835FA00E, 0x1D31774D, + 0x8348D8DB, 0x1CCF8CB3, + 0x83325E97, 0x1C6D9053, + 0x831C314E, 0x1C0B826A, + 0x8306510F, 0x1BA96334, + 0x82F0BDE8, 0x1B4732EF, + 0x82DB77E5, 0x1AE4F1D6, + 0x82C67F13, 0x1A82A025, + 0x82B1D381, 0x1A203E1B, + 0x829D753A, 0x19BDCBF2, + 0x8289644A, 0x195B49E9, + 0x8275A0C0, 0x18F8B83C, + 0x82622AA5, 0x18961727, + 0x824F0208, 0x183366E8, + 0x823C26F2, 0x17D0A7BB, + 0x82299971, 0x176DD9DE, + 0x8217598F, 0x170AFD8D, + 0x82056758, 0x16A81305, + 0x81F3C2D7, 0x16451A83, + 0x81E26C16, 0x15E21444, + 0x81D16320, 0x157F0086, + 0x81C0A801, 0x151BDF85, + 0x81B03AC1, 0x14B8B17F, + 0x81A01B6C, 0x145576B1, + 0x81904A0C, 0x13F22F57, + 0x8180C6A9, 0x138EDBB0, + 0x8171914E, 0x132B7BF9, + 0x8162AA03, 0x12C8106E, + 0x815410D3, 0x1264994E, + 0x8145C5C6, 0x120116D4, + 0x8137C8E6, 0x119D8940, + 0x812A1A39, 0x1139F0CE, + 0x811CB9CA, 0x10D64DBC, + 0x810FA7A0, 0x1072A047, + 0x8102E3C3, 0x100EE8AD, + 0x80F66E3C, 0x0FAB272B, + 0x80EA4712, 0x0F475BFE, + 0x80DE6E4C, 0x0EE38765, + 0x80D2E3F1, 0x0E7FA99D, + 0x80C7A80A, 0x0E1BC2E3, + 0x80BCBA9C, 0x0DB7D376, + 0x80B21BAF, 0x0D53DB92, + 0x80A7CB49, 0x0CEFDB75, + 0x809DC970, 0x0C8BD35E, + 0x8094162B, 0x0C27C389, + 0x808AB180, 0x0BC3AC35, + 0x80819B74, 0x0B5F8D9F, + 0x8078D40D, 0x0AFB6805, + 0x80705B50, 0x0A973BA5, + 0x80683143, 0x0A3308BC, + 0x806055EA, 0x09CECF89, + 0x8058C94C, 0x096A9049, + 0x80518B6B, 0x09064B3A, + 0x804A9C4D, 0x08A2009A, + 0x8043FBF6, 0x083DB0A7, + 0x803DAA69, 0x07D95B9E, + 0x8037A7AC, 0x077501BE, + 0x8031F3C1, 0x0710A344, + 0x802C8EAD, 0x06AC406F, + 0x80277872, 0x0647D97C, + 0x8022B113, 0x05E36EA9, + 0x801E3894, 0x057F0034, + 0x801A0EF7, 0x051A8E5C, + 0x80163440, 0x04B6195D, + 0x8012A86F, 0x0451A176, + 0x800F6B88, 0x03ED26E6, + 0x800C7D8C, 0x0388A9E9, + 0x8009DE7D, 0x03242ABF, + 0x80078E5E, 0x02BFA9A4, + 0x80058D2E, 0x025B26D7, + 0x8003DAF0, 0x01F6A296, + 0x800277A5, 0x01921D1F, + 0x8001634D, 0x012D96B0, + 0x80009DE9, 0x00C90F88, + 0x8000277A, 0x006487E3, + 0x80000000, 0x00000000, + 0x8000277A, 0xFF9B781D, + 0x80009DE9, 0xFF36F078, + 0x8001634D, 0xFED2694F, + 0x800277A5, 0xFE6DE2E0, + 0x8003DAF0, 0xFE095D69, + 0x80058D2E, 0xFDA4D928, + 0x80078E5E, 0xFD40565B, + 0x8009DE7D, 0xFCDBD541, + 0x800C7D8C, 0xFC775616, + 0x800F6B88, 0xFC12D919, + 0x8012A86F, 0xFBAE5E89, + 0x80163440, 0xFB49E6A2, + 0x801A0EF7, 0xFAE571A4, + 0x801E3894, 0xFA80FFCB, + 0x8022B113, 0xFA1C9156, + 0x80277872, 0xF9B82683, + 0x802C8EAD, 0xF953BF90, + 0x8031F3C1, 0xF8EF5CBB, + 0x8037A7AC, 0xF88AFE41, + 0x803DAA69, 0xF826A461, + 0x8043FBF6, 0xF7C24F58, + 0x804A9C4D, 0xF75DFF65, + 0x80518B6B, 0xF6F9B4C5, + 0x8058C94C, 0xF6956FB6, + 0x806055EA, 0xF6313076, + 0x80683143, 0xF5CCF743, + 0x80705B50, 0xF568C45A, + 0x8078D40D, 0xF50497FA, + 0x80819B74, 0xF4A07260, + 0x808AB180, 0xF43C53CA, + 0x8094162B, 0xF3D83C76, + 0x809DC970, 0xF3742CA1, + 0x80A7CB49, 0xF310248A, + 0x80B21BAF, 0xF2AC246D, + 0x80BCBA9C, 0xF2482C89, + 0x80C7A80A, 0xF1E43D1C, + 0x80D2E3F1, 0xF1805662, + 0x80DE6E4C, 0xF11C789A, + 0x80EA4712, 0xF0B8A401, + 0x80F66E3C, 0xF054D8D4, + 0x8102E3C3, 0xEFF11752, + 0x810FA7A0, 0xEF8D5FB8, + 0x811CB9CA, 0xEF29B243, + 0x812A1A39, 0xEEC60F31, + 0x8137C8E6, 0xEE6276BF, + 0x8145C5C6, 0xEDFEE92B, + 0x815410D3, 0xED9B66B2, + 0x8162AA03, 0xED37EF91, + 0x8171914E, 0xECD48406, + 0x8180C6A9, 0xEC71244F, + 0x81904A0C, 0xEC0DD0A8, + 0x81A01B6C, 0xEBAA894E, + 0x81B03AC1, 0xEB474E80, + 0x81C0A801, 0xEAE4207A, + 0x81D16320, 0xEA80FF79, + 0x81E26C16, 0xEA1DEBBB, + 0x81F3C2D7, 0xE9BAE57C, + 0x82056758, 0xE957ECFB, + 0x8217598F, 0xE8F50273, + 0x82299971, 0xE8922621, + 0x823C26F2, 0xE82F5844, + 0x824F0208, 0xE7CC9917, + 0x82622AA5, 0xE769E8D8, + 0x8275A0C0, 0xE70747C3, + 0x8289644A, 0xE6A4B616, + 0x829D753A, 0xE642340D, + 0x82B1D381, 0xE5DFC1E4, + 0x82C67F13, 0xE57D5FDA, + 0x82DB77E5, 0xE51B0E2A, + 0x82F0BDE8, 0xE4B8CD10, + 0x8306510F, 0xE4569CCB, + 0x831C314E, 0xE3F47D95, + 0x83325E97, 0xE3926FAC, + 0x8348D8DB, 0xE330734C, + 0x835FA00E, 0xE2CE88B2, + 0x8376B422, 0xE26CB01A, + 0x838E1507, 0xE20AE9C1, + 0x83A5C2B0, 0xE1A935E1, + 0x83BDBD0D, 0xE14794B9, + 0x83D60411, 0xE0E60684, + 0x83EE97AC, 0xE0848B7F, + 0x840777CF, 0xE02323E5, + 0x8420A46B, 0xDFC1CFF2, + 0x843A1D70, 0xDF608FE3, + 0x8453E2CE, 0xDEFF63F4, + 0x846DF476, 0xDE9E4C60, + 0x84885257, 0xDE3D4963, + 0x84A2FC62, 0xDDDC5B3A, + 0x84BDF285, 0xDD7B8220, + 0x84D934B0, 0xDD1ABE51, + 0x84F4C2D3, 0xDCBA1008, + 0x85109CDC, 0xDC597781, + 0x852CC2BA, 0xDBF8F4F8, + 0x8549345C, 0xDB9888A8, + 0x8565F1B0, 0xDB3832CD, + 0x8582FAA4, 0xDAD7F3A2, + 0x85A04F28, 0xDA77CB62, + 0x85BDEF27, 0xDA17BA4A, + 0x85DBDA91, 0xD9B7C093, + 0x85FA1152, 0xD957DE7A, + 0x86189359, 0xD8F81439, + 0x86376092, 0xD898620C, + 0x865678EA, 0xD838C82D, + 0x8675DC4E, 0xD7D946D7, + 0x86958AAB, 0xD779DE46, + 0x86B583EE, 0xD71A8EB5, + 0x86D5C802, 0xD6BB585D, + 0x86F656D3, 0xD65C3B7B, + 0x8717304E, 0xD5FD3847, + 0x8738545E, 0xD59E4EFE, + 0x8759C2EF, 0xD53F7FDA, + 0x877B7BEC, 0xD4E0CB14, + 0x879D7F40, 0xD48230E8, + 0x87BFCCD7, 0xD423B190, + 0x87E2649B, 0xD3C54D46, + 0x88054677, 0xD3670445, + 0x88287255, 0xD308D6C6, + 0x884BE820, 0xD2AAC504, + 0x886FA7C2, 0xD24CCF38, + 0x8893B124, 0xD1EEF59E, + 0x88B80431, 0xD191386D, + 0x88DCA0D3, 0xD13397E1, + 0x890186F1, 0xD0D61433, + 0x8926B677, 0xD078AD9D, + 0x894C2F4C, 0xD01B6459, + 0x8971F15A, 0xCFBE389F, + 0x8997FC89, 0xCF612AAA, + 0x89BE50C3, 0xCF043AB2, + 0x89E4EDEE, 0xCEA768F2, + 0x8A0BD3F5, 0xCE4AB5A2, + 0x8A3302BD, 0xCDEE20FC, + 0x8A5A7A30, 0xCD91AB38, + 0x8A823A35, 0xCD355490, + 0x8AAA42B4, 0xCCD91D3D, + 0x8AD29393, 0xCC7D0577, + 0x8AFB2CBA, 0xCC210D78, + 0x8B240E10, 0xCBC53578, + 0x8B4D377C, 0xCB697DB0, + 0x8B76A8E4, 0xCB0DE658, + 0x8BA0622F, 0xCAB26FA9, + 0x8BCA6342, 0xCA5719DB, + 0x8BF4AC05, 0xC9FBE527, + 0x8C1F3C5C, 0xC9A0D1C4, + 0x8C4A142F, 0xC945DFEC, + 0x8C753361, 0xC8EB0FD6, + 0x8CA099D9, 0xC89061BA, + 0x8CCC477D, 0xC835D5D0, + 0x8CF83C30, 0xC7DB6C50, + 0x8D2477D8, 0xC7812571, + 0x8D50FA59, 0xC727016C, + 0x8D7DC399, 0xC6CD0079, + 0x8DAAD37B, 0xC67322CD, + 0x8DD829E4, 0xC61968A2, + 0x8E05C6B7, 0xC5BFD22E, + 0x8E33A9D9, 0xC5665FA8, + 0x8E61D32D, 0xC50D1148, + 0x8E904298, 0xC4B3E746, + 0x8EBEF7FB, 0xC45AE1D7, + 0x8EEDF33B, 0xC4020132, + 0x8F1D343A, 0xC3A9458F, + 0x8F4CBADB, 0xC350AF25, + 0x8F7C8701, 0xC2F83E2A, + 0x8FAC988E, 0xC29FF2D4, + 0x8FDCEF66, 0xC247CD5A, + 0x900D8B69, 0xC1EFCDF2, + 0x903E6C7A, 0xC197F4D3, + 0x906F927B, 0xC1404233, + 0x90A0FD4E, 0xC0E8B648, + 0x90D2ACD3, 0xC0915147, + 0x9104A0ED, 0xC03A1368, + 0x9136D97D, 0xBFE2FCDF, + 0x91695663, 0xBF8C0DE2, + 0x919C1780, 0xBF3546A8, + 0x91CF1CB6, 0xBEDEA765, + 0x920265E4, 0xBE88304F, + 0x9235F2EB, 0xBE31E19B, + 0x9269C3AC, 0xBDDBBB7F, + 0x929DD805, 0xBD85BE2F, + 0x92D22FD8, 0xBD2FE9E1, + 0x9306CB04, 0xBCDA3ECA, + 0x933BA968, 0xBC84BD1E, + 0x9370CAE4, 0xBC2F6513, + 0x93A62F56, 0xBBDA36DC, + 0x93DBD69F, 0xBB8532AF, + 0x9411C09D, 0xBB3058C0, + 0x9447ED2F, 0xBADBA943, + 0x947E5C32, 0xBA87246C, + 0x94B50D87, 0xBA32CA70, + 0x94EC010B, 0xB9DE9B83, + 0x9523369B, 0xB98A97D8, + 0x955AAE17, 0xB936BFA3, + 0x9592675B, 0xB8E31319, + 0x95CA6246, 0xB88F926C, + 0x96029EB5, 0xB83C3DD1, + 0x963B1C85, 0xB7E9157A, + 0x9673DB94, 0xB796199B, + 0x96ACDBBD, 0xB7434A67, + 0x96E61CDF, 0xB6F0A811, + 0x971F9ED6, 0xB69E32CD, + 0x9759617E, 0xB64BEACC, + 0x979364B5, 0xB5F9D042, + 0x97CDA855, 0xB5A7E362, + 0x98082C3B, 0xB556245E, + 0x9842F043, 0xB5049368, + 0x987DF449, 0xB4B330B2, + 0x98B93828, 0xB461FC70, + 0x98F4BBBC, 0xB410F6D2, + 0x99307EE0, 0xB3C0200C, + 0x996C816F, 0xB36F784E, + 0x99A8C344, 0xB31EFFCB, + 0x99E5443A, 0xB2CEB6B5, + 0x9A22042C, 0xB27E9D3B, + 0x9A5F02F5, 0xB22EB392, + 0x9A9C406D, 0xB1DEF9E8, + 0x9AD9BC71, 0xB18F7070, + 0x9B1776D9, 0xB140175B, + 0x9B556F80, 0xB0F0EEDA, + 0x9B93A640, 0xB0A1F71C, + 0x9BD21AF2, 0xB0533055, + 0x9C10CD70, 0xB0049AB2, + 0x9C4FBD92, 0xAFB63667, + 0x9C8EEB33, 0xAF6803A1, + 0x9CCE562B, 0xAF1A0293, + 0x9D0DFE53, 0xAECC336B, + 0x9D4DE384, 0xAE7E965B, + 0x9D8E0596, 0xAE312B91, + 0x9DCE6462, 0xADE3F33E, + 0x9E0EFFC1, 0xAD96ED91, + 0x9E4FD789, 0xAD4A1ABA, + 0x9E90EB94, 0xACFD7AE8, + 0x9ED23BB9, 0xACB10E4A, + 0x9F13C7D0, 0xAC64D510, + 0x9F558FB0, 0xAC18CF68, + 0x9F979331, 0xABCCFD82, + 0x9FD9D22A, 0xAB815F8C, + 0xA01C4C72, 0xAB35F5B5, + 0xA05F01E1, 0xAAEAC02B, + 0xA0A1F24C, 0xAA9FBF1D, + 0xA0E51D8C, 0xAA54F2B9, + 0xA1288376, 0xAA0A5B2D, + 0xA16C23E1, 0xA9BFF8A8, + 0xA1AFFEA2, 0xA975CB56, + 0xA1F41391, 0xA92BD366, + 0xA2386283, 0xA8E21106, + 0xA27CEB4F, 0xA8988463, + 0xA2C1ADC9, 0xA84F2DA9, + 0xA306A9C7, 0xA8060D08, + 0xA34BDF20, 0xA7BD22AB, + 0xA3914DA7, 0xA7746EC0, + 0xA3D6F533, 0xA72BF173, + 0xA41CD598, 0xA6E3AAF2, + 0xA462EEAC, 0xA69B9B68, + 0xA4A94042, 0xA653C302, + 0xA4EFCA31, 0xA60C21ED, + 0xA5368C4B, 0xA5C4B855, + 0xA57D8666, 0xA57D8666, + 0xA5C4B855, 0xA5368C4B, + 0xA60C21ED, 0xA4EFCA31, + 0xA653C302, 0xA4A94042, + 0xA69B9B68, 0xA462EEAC, + 0xA6E3AAF2, 0xA41CD598, + 0xA72BF173, 0xA3D6F533, + 0xA7746EC0, 0xA3914DA7, + 0xA7BD22AB, 0xA34BDF20, + 0xA8060D08, 0xA306A9C7, + 0xA84F2DA9, 0xA2C1ADC9, + 0xA8988463, 0xA27CEB4F, + 0xA8E21106, 0xA2386283, + 0xA92BD366, 0xA1F41391, + 0xA975CB56, 0xA1AFFEA2, + 0xA9BFF8A8, 0xA16C23E1, + 0xAA0A5B2D, 0xA1288376, + 0xAA54F2B9, 0xA0E51D8C, + 0xAA9FBF1D, 0xA0A1F24C, + 0xAAEAC02B, 0xA05F01E1, + 0xAB35F5B5, 0xA01C4C72, + 0xAB815F8C, 0x9FD9D22A, + 0xABCCFD82, 0x9F979331, + 0xAC18CF68, 0x9F558FB0, + 0xAC64D510, 0x9F13C7D0, + 0xACB10E4A, 0x9ED23BB9, + 0xACFD7AE8, 0x9E90EB94, + 0xAD4A1ABA, 0x9E4FD789, + 0xAD96ED91, 0x9E0EFFC1, + 0xADE3F33E, 0x9DCE6462, + 0xAE312B91, 0x9D8E0596, + 0xAE7E965B, 0x9D4DE384, + 0xAECC336B, 0x9D0DFE53, + 0xAF1A0293, 0x9CCE562B, + 0xAF6803A1, 0x9C8EEB33, + 0xAFB63667, 0x9C4FBD92, + 0xB0049AB2, 0x9C10CD70, + 0xB0533055, 0x9BD21AF2, + 0xB0A1F71C, 0x9B93A640, + 0xB0F0EEDA, 0x9B556F80, + 0xB140175B, 0x9B1776D9, + 0xB18F7070, 0x9AD9BC71, + 0xB1DEF9E8, 0x9A9C406D, + 0xB22EB392, 0x9A5F02F5, + 0xB27E9D3B, 0x9A22042C, + 0xB2CEB6B5, 0x99E5443A, + 0xB31EFFCB, 0x99A8C344, + 0xB36F784E, 0x996C816F, + 0xB3C0200C, 0x99307EE0, + 0xB410F6D2, 0x98F4BBBC, + 0xB461FC70, 0x98B93828, + 0xB4B330B2, 0x987DF449, + 0xB5049368, 0x9842F043, + 0xB556245E, 0x98082C3B, + 0xB5A7E362, 0x97CDA855, + 0xB5F9D042, 0x979364B5, + 0xB64BEACC, 0x9759617E, + 0xB69E32CD, 0x971F9ED6, + 0xB6F0A811, 0x96E61CDF, + 0xB7434A67, 0x96ACDBBD, + 0xB796199B, 0x9673DB94, + 0xB7E9157A, 0x963B1C85, + 0xB83C3DD1, 0x96029EB5, + 0xB88F926C, 0x95CA6246, + 0xB8E31319, 0x9592675B, + 0xB936BFA3, 0x955AAE17, + 0xB98A97D8, 0x9523369B, + 0xB9DE9B83, 0x94EC010B, + 0xBA32CA70, 0x94B50D87, + 0xBA87246C, 0x947E5C32, + 0xBADBA943, 0x9447ED2F, + 0xBB3058C0, 0x9411C09D, + 0xBB8532AF, 0x93DBD69F, + 0xBBDA36DC, 0x93A62F56, + 0xBC2F6513, 0x9370CAE4, + 0xBC84BD1E, 0x933BA968, + 0xBCDA3ECA, 0x9306CB04, + 0xBD2FE9E1, 0x92D22FD8, + 0xBD85BE2F, 0x929DD805, + 0xBDDBBB7F, 0x9269C3AC, + 0xBE31E19B, 0x9235F2EB, + 0xBE88304F, 0x920265E4, + 0xBEDEA765, 0x91CF1CB6, + 0xBF3546A8, 0x919C1780, + 0xBF8C0DE2, 0x91695663, + 0xBFE2FCDF, 0x9136D97D, + 0xC03A1368, 0x9104A0ED, + 0xC0915147, 0x90D2ACD3, + 0xC0E8B648, 0x90A0FD4E, + 0xC1404233, 0x906F927B, + 0xC197F4D3, 0x903E6C7A, + 0xC1EFCDF2, 0x900D8B69, + 0xC247CD5A, 0x8FDCEF66, + 0xC29FF2D4, 0x8FAC988E, + 0xC2F83E2A, 0x8F7C8701, + 0xC350AF25, 0x8F4CBADB, + 0xC3A9458F, 0x8F1D343A, + 0xC4020132, 0x8EEDF33B, + 0xC45AE1D7, 0x8EBEF7FB, + 0xC4B3E746, 0x8E904298, + 0xC50D1148, 0x8E61D32D, + 0xC5665FA8, 0x8E33A9D9, + 0xC5BFD22E, 0x8E05C6B7, + 0xC61968A2, 0x8DD829E4, + 0xC67322CD, 0x8DAAD37B, + 0xC6CD0079, 0x8D7DC399, + 0xC727016C, 0x8D50FA59, + 0xC7812571, 0x8D2477D8, + 0xC7DB6C50, 0x8CF83C30, + 0xC835D5D0, 0x8CCC477D, + 0xC89061BA, 0x8CA099D9, + 0xC8EB0FD6, 0x8C753361, + 0xC945DFEC, 0x8C4A142F, + 0xC9A0D1C4, 0x8C1F3C5C, + 0xC9FBE527, 0x8BF4AC05, + 0xCA5719DB, 0x8BCA6342, + 0xCAB26FA9, 0x8BA0622F, + 0xCB0DE658, 0x8B76A8E4, + 0xCB697DB0, 0x8B4D377C, + 0xCBC53578, 0x8B240E10, + 0xCC210D78, 0x8AFB2CBA, + 0xCC7D0577, 0x8AD29393, + 0xCCD91D3D, 0x8AAA42B4, + 0xCD355490, 0x8A823A35, + 0xCD91AB38, 0x8A5A7A30, + 0xCDEE20FC, 0x8A3302BD, + 0xCE4AB5A2, 0x8A0BD3F5, + 0xCEA768F2, 0x89E4EDEE, + 0xCF043AB2, 0x89BE50C3, + 0xCF612AAA, 0x8997FC89, + 0xCFBE389F, 0x8971F15A, + 0xD01B6459, 0x894C2F4C, + 0xD078AD9D, 0x8926B677, + 0xD0D61433, 0x890186F1, + 0xD13397E1, 0x88DCA0D3, + 0xD191386D, 0x88B80431, + 0xD1EEF59E, 0x8893B124, + 0xD24CCF38, 0x886FA7C2, + 0xD2AAC504, 0x884BE820, + 0xD308D6C6, 0x88287255, + 0xD3670445, 0x88054677, + 0xD3C54D46, 0x87E2649B, + 0xD423B190, 0x87BFCCD7, + 0xD48230E8, 0x879D7F40, + 0xD4E0CB14, 0x877B7BEC, + 0xD53F7FDA, 0x8759C2EF, + 0xD59E4EFE, 0x8738545E, + 0xD5FD3847, 0x8717304E, + 0xD65C3B7B, 0x86F656D3, + 0xD6BB585D, 0x86D5C802, + 0xD71A8EB5, 0x86B583EE, + 0xD779DE46, 0x86958AAB, + 0xD7D946D7, 0x8675DC4E, + 0xD838C82D, 0x865678EA, + 0xD898620C, 0x86376092, + 0xD8F81439, 0x86189359, + 0xD957DE7A, 0x85FA1152, + 0xD9B7C093, 0x85DBDA91, + 0xDA17BA4A, 0x85BDEF27, + 0xDA77CB62, 0x85A04F28, + 0xDAD7F3A2, 0x8582FAA4, + 0xDB3832CD, 0x8565F1B0, + 0xDB9888A8, 0x8549345C, + 0xDBF8F4F8, 0x852CC2BA, + 0xDC597781, 0x85109CDC, + 0xDCBA1008, 0x84F4C2D3, + 0xDD1ABE51, 0x84D934B0, + 0xDD7B8220, 0x84BDF285, + 0xDDDC5B3A, 0x84A2FC62, + 0xDE3D4963, 0x84885257, + 0xDE9E4C60, 0x846DF476, + 0xDEFF63F4, 0x8453E2CE, + 0xDF608FE3, 0x843A1D70, + 0xDFC1CFF2, 0x8420A46B, + 0xE02323E5, 0x840777CF, + 0xE0848B7F, 0x83EE97AC, + 0xE0E60684, 0x83D60411, + 0xE14794B9, 0x83BDBD0D, + 0xE1A935E1, 0x83A5C2B0, + 0xE20AE9C1, 0x838E1507, + 0xE26CB01A, 0x8376B422, + 0xE2CE88B2, 0x835FA00E, + 0xE330734C, 0x8348D8DB, + 0xE3926FAC, 0x83325E97, + 0xE3F47D95, 0x831C314E, + 0xE4569CCB, 0x8306510F, + 0xE4B8CD10, 0x82F0BDE8, + 0xE51B0E2A, 0x82DB77E5, + 0xE57D5FDA, 0x82C67F13, + 0xE5DFC1E4, 0x82B1D381, + 0xE642340D, 0x829D753A, + 0xE6A4B616, 0x8289644A, + 0xE70747C3, 0x8275A0C0, + 0xE769E8D8, 0x82622AA5, + 0xE7CC9917, 0x824F0208, + 0xE82F5844, 0x823C26F2, + 0xE8922621, 0x82299971, + 0xE8F50273, 0x8217598F, + 0xE957ECFB, 0x82056758, + 0xE9BAE57C, 0x81F3C2D7, + 0xEA1DEBBB, 0x81E26C16, + 0xEA80FF79, 0x81D16320, + 0xEAE4207A, 0x81C0A801, + 0xEB474E80, 0x81B03AC1, + 0xEBAA894E, 0x81A01B6C, + 0xEC0DD0A8, 0x81904A0C, + 0xEC71244F, 0x8180C6A9, + 0xECD48406, 0x8171914E, + 0xED37EF91, 0x8162AA03, + 0xED9B66B2, 0x815410D3, + 0xEDFEE92B, 0x8145C5C6, + 0xEE6276BF, 0x8137C8E6, + 0xEEC60F31, 0x812A1A39, + 0xEF29B243, 0x811CB9CA, + 0xEF8D5FB8, 0x810FA7A0, + 0xEFF11752, 0x8102E3C3, + 0xF054D8D4, 0x80F66E3C, + 0xF0B8A401, 0x80EA4712, + 0xF11C789A, 0x80DE6E4C, + 0xF1805662, 0x80D2E3F1, + 0xF1E43D1C, 0x80C7A80A, + 0xF2482C89, 0x80BCBA9C, + 0xF2AC246D, 0x80B21BAF, + 0xF310248A, 0x80A7CB49, + 0xF3742CA1, 0x809DC970, + 0xF3D83C76, 0x8094162B, + 0xF43C53CA, 0x808AB180, + 0xF4A07260, 0x80819B74, + 0xF50497FA, 0x8078D40D, + 0xF568C45A, 0x80705B50, + 0xF5CCF743, 0x80683143, + 0xF6313076, 0x806055EA, + 0xF6956FB6, 0x8058C94C, + 0xF6F9B4C5, 0x80518B6B, + 0xF75DFF65, 0x804A9C4D, + 0xF7C24F58, 0x8043FBF6, + 0xF826A461, 0x803DAA69, + 0xF88AFE41, 0x8037A7AC, + 0xF8EF5CBB, 0x8031F3C1, + 0xF953BF90, 0x802C8EAD, + 0xF9B82683, 0x80277872, + 0xFA1C9156, 0x8022B113, + 0xFA80FFCB, 0x801E3894, + 0xFAE571A4, 0x801A0EF7, + 0xFB49E6A2, 0x80163440, + 0xFBAE5E89, 0x8012A86F, + 0xFC12D919, 0x800F6B88, + 0xFC775616, 0x800C7D8C, + 0xFCDBD541, 0x8009DE7D, + 0xFD40565B, 0x80078E5E, + 0xFDA4D928, 0x80058D2E, + 0xFE095D69, 0x8003DAF0, + 0xFE6DE2E0, 0x800277A5, + 0xFED2694F, 0x8001634D, + 0xFF36F078, 0x80009DE9, + 0xFF9B781D, 0x8000277A +}; + +/** +* \par +* Example code for Q31 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefQ31[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefQ31[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 4096 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to Q31(Fixed point 1.31): +* round(twiddleCoefQ31(i) * pow(2, 31)) +* +*/ +const q31_t twiddleCoef_4096_q31[6144] = +{ + 0x7FFFFFFF, 0x00000000, + 0x7FFFF621, 0x003243F5, + 0x7FFFD885, 0x006487E3, + 0x7FFFA72C, 0x0096CBC1, + 0x7FFF6216, 0x00C90F88, + 0x7FFF0942, 0x00FB532F, + 0x7FFE9CB2, 0x012D96B0, + 0x7FFE1C64, 0x015FDA03, + 0x7FFD885A, 0x01921D1F, + 0x7FFCE093, 0x01C45FFE, + 0x7FFC250F, 0x01F6A296, + 0x7FFB55CE, 0x0228E4E1, + 0x7FFA72D1, 0x025B26D7, + 0x7FF97C17, 0x028D6870, + 0x7FF871A1, 0x02BFA9A4, + 0x7FF7536F, 0x02F1EA6B, + 0x7FF62182, 0x03242ABF, + 0x7FF4DBD8, 0x03566A96, + 0x7FF38273, 0x0388A9E9, + 0x7FF21553, 0x03BAE8B1, + 0x7FF09477, 0x03ED26E6, + 0x7FEEFFE1, 0x041F647F, + 0x7FED5790, 0x0451A176, + 0x7FEB9B85, 0x0483DDC3, + 0x7FE9CBC0, 0x04B6195D, + 0x7FE7E840, 0x04E8543D, + 0x7FE5F108, 0x051A8E5C, + 0x7FE3E616, 0x054CC7B0, + 0x7FE1C76B, 0x057F0034, + 0x7FDF9508, 0x05B137DF, + 0x7FDD4EEC, 0x05E36EA9, + 0x7FDAF518, 0x0615A48A, + 0x7FD8878D, 0x0647D97C, + 0x7FD6064B, 0x067A0D75, + 0x7FD37152, 0x06AC406F, + 0x7FD0C8A3, 0x06DE7261, + 0x7FCE0C3E, 0x0710A344, + 0x7FCB3C23, 0x0742D310, + 0x7FC85853, 0x077501BE, + 0x7FC560CF, 0x07A72F45, + 0x7FC25596, 0x07D95B9E, + 0x7FBF36A9, 0x080B86C1, + 0x7FBC040A, 0x083DB0A7, + 0x7FB8BDB7, 0x086FD947, + 0x7FB563B2, 0x08A2009A, + 0x7FB1F5FC, 0x08D42698, + 0x7FAE7494, 0x09064B3A, + 0x7FAADF7C, 0x09386E77, + 0x7FA736B4, 0x096A9049, + 0x7FA37A3C, 0x099CB0A7, + 0x7F9FAA15, 0x09CECF89, + 0x7F9BC63F, 0x0A00ECE8, + 0x7F97CEBC, 0x0A3308BC, + 0x7F93C38C, 0x0A6522FE, + 0x7F8FA4AF, 0x0A973BA5, + 0x7F8B7226, 0x0AC952AA, + 0x7F872BF3, 0x0AFB6805, + 0x7F82D214, 0x0B2D7BAE, + 0x7F7E648B, 0x0B5F8D9F, + 0x7F79E35A, 0x0B919DCE, + 0x7F754E7F, 0x0BC3AC35, + 0x7F70A5FD, 0x0BF5B8CB, + 0x7F6BE9D4, 0x0C27C389, + 0x7F671A04, 0x0C59CC67, + 0x7F62368F, 0x0C8BD35E, + 0x7F5D3F75, 0x0CBDD865, + 0x7F5834B6, 0x0CEFDB75, + 0x7F531654, 0x0D21DC87, + 0x7F4DE450, 0x0D53DB92, + 0x7F489EAA, 0x0D85D88F, + 0x7F434563, 0x0DB7D376, + 0x7F3DD87C, 0x0DE9CC3F, + 0x7F3857F5, 0x0E1BC2E3, + 0x7F32C3D0, 0x0E4DB75B, + 0x7F2D1C0E, 0x0E7FA99D, + 0x7F2760AF, 0x0EB199A3, + 0x7F2191B4, 0x0EE38765, + 0x7F1BAF1E, 0x0F1572DC, + 0x7F15B8EE, 0x0F475BFE, + 0x7F0FAF24, 0x0F7942C6, + 0x7F0991C3, 0x0FAB272B, + 0x7F0360CB, 0x0FDD0925, + 0x7EFD1C3C, 0x100EE8AD, + 0x7EF6C418, 0x1040C5BB, + 0x7EF0585F, 0x1072A047, + 0x7EE9D913, 0x10A4784A, + 0x7EE34635, 0x10D64DBC, + 0x7EDC9FC6, 0x11082096, + 0x7ED5E5C6, 0x1139F0CE, + 0x7ECF1837, 0x116BBE5F, + 0x7EC8371A, 0x119D8940, + 0x7EC1426F, 0x11CF516A, + 0x7EBA3A39, 0x120116D4, + 0x7EB31E77, 0x1232D978, + 0x7EABEF2C, 0x1264994E, + 0x7EA4AC58, 0x1296564D, + 0x7E9D55FC, 0x12C8106E, + 0x7E95EC19, 0x12F9C7AA, + 0x7E8E6EB1, 0x132B7BF9, + 0x7E86DDC5, 0x135D2D53, + 0x7E7F3956, 0x138EDBB0, + 0x7E778165, 0x13C0870A, + 0x7E6FB5F3, 0x13F22F57, + 0x7E67D702, 0x1423D492, + 0x7E5FE493, 0x145576B1, + 0x7E57DEA6, 0x148715AD, + 0x7E4FC53E, 0x14B8B17F, + 0x7E47985B, 0x14EA4A1F, + 0x7E3F57FE, 0x151BDF85, + 0x7E37042A, 0x154D71AA, + 0x7E2E9CDF, 0x157F0086, + 0x7E26221E, 0x15B08C11, + 0x7E1D93E9, 0x15E21444, + 0x7E14F242, 0x16139917, + 0x7E0C3D29, 0x16451A83, + 0x7E03749F, 0x1676987F, + 0x7DFA98A7, 0x16A81305, + 0x7DF1A942, 0x16D98A0C, + 0x7DE8A670, 0x170AFD8D, + 0x7DDF9034, 0x173C6D80, + 0x7DD6668E, 0x176DD9DE, + 0x7DCD2981, 0x179F429F, + 0x7DC3D90D, 0x17D0A7BB, + 0x7DBA7534, 0x1802092C, + 0x7DB0FDF7, 0x183366E8, + 0x7DA77359, 0x1864C0E9, + 0x7D9DD55A, 0x18961727, + 0x7D9423FB, 0x18C7699B, + 0x7D8A5F3F, 0x18F8B83C, + 0x7D808727, 0x192A0303, + 0x7D769BB5, 0x195B49E9, + 0x7D6C9CE9, 0x198C8CE6, + 0x7D628AC5, 0x19BDCBF2, + 0x7D58654C, 0x19EF0706, + 0x7D4E2C7E, 0x1A203E1B, + 0x7D43E05E, 0x1A517127, + 0x7D3980EC, 0x1A82A025, + 0x7D2F0E2A, 0x1AB3CB0C, + 0x7D24881A, 0x1AE4F1D6, + 0x7D19EEBE, 0x1B161479, + 0x7D0F4218, 0x1B4732EF, + 0x7D048228, 0x1B784D30, + 0x7CF9AEF0, 0x1BA96334, + 0x7CEEC873, 0x1BDA74F5, + 0x7CE3CEB1, 0x1C0B826A, + 0x7CD8C1AD, 0x1C3C8B8C, + 0x7CCDA168, 0x1C6D9053, + 0x7CC26DE5, 0x1C9E90B8, + 0x7CB72724, 0x1CCF8CB3, + 0x7CABCD27, 0x1D00843C, + 0x7CA05FF1, 0x1D31774D, + 0x7C94DF82, 0x1D6265DD, + 0x7C894BDD, 0x1D934FE5, + 0x7C7DA504, 0x1DC4355D, + 0x7C71EAF8, 0x1DF5163F, + 0x7C661DBB, 0x1E25F281, + 0x7C5A3D4F, 0x1E56CA1E, + 0x7C4E49B6, 0x1E879D0C, + 0x7C4242F2, 0x1EB86B46, + 0x7C362904, 0x1EE934C2, + 0x7C29FBEE, 0x1F19F97B, + 0x7C1DBBB2, 0x1F4AB967, + 0x7C116853, 0x1F7B7480, + 0x7C0501D1, 0x1FAC2ABF, + 0x7BF88830, 0x1FDCDC1A, + 0x7BEBFB70, 0x200D888C, + 0x7BDF5B94, 0x203E300D, + 0x7BD2A89E, 0x206ED295, + 0x7BC5E28F, 0x209F701C, + 0x7BB9096A, 0x20D0089B, + 0x7BAC1D31, 0x21009C0B, + 0x7B9F1DE5, 0x21312A65, + 0x7B920B89, 0x2161B39F, + 0x7B84E61E, 0x219237B4, + 0x7B77ADA8, 0x21C2B69C, + 0x7B6A6227, 0x21F3304E, + 0x7B5D039D, 0x2223A4C5, + 0x7B4F920E, 0x225413F8, + 0x7B420D7A, 0x22847DDF, + 0x7B3475E4, 0x22B4E274, + 0x7B26CB4F, 0x22E541AE, + 0x7B190DBB, 0x23159B87, + 0x7B0B3D2C, 0x2345EFF7, + 0x7AFD59A3, 0x23763EF7, + 0x7AEF6323, 0x23A6887E, + 0x7AE159AE, 0x23D6CC86, + 0x7AD33D45, 0x24070B07, + 0x7AC50DEB, 0x243743FA, + 0x7AB6CBA3, 0x24677757, + 0x7AA8766E, 0x2497A517, + 0x7A9A0E4F, 0x24C7CD32, + 0x7A8B9348, 0x24F7EFA1, + 0x7A7D055B, 0x25280C5D, + 0x7A6E648A, 0x2558235E, + 0x7A5FB0D8, 0x2588349D, + 0x7A50EA46, 0x25B84012, + 0x7A4210D8, 0x25E845B5, + 0x7A33248F, 0x26184581, + 0x7A24256E, 0x26483F6C, + 0x7A151377, 0x26783370, + 0x7A05EEAD, 0x26A82185, + 0x79F6B711, 0x26D809A5, + 0x79E76CA6, 0x2707EBC6, + 0x79D80F6F, 0x2737C7E3, + 0x79C89F6D, 0x27679DF4, + 0x79B91CA4, 0x27976DF1, + 0x79A98715, 0x27C737D2, + 0x7999DEC3, 0x27F6FB92, + 0x798A23B1, 0x2826B928, + 0x797A55E0, 0x2856708C, + 0x796A7554, 0x288621B9, + 0x795A820E, 0x28B5CCA5, + 0x794A7C11, 0x28E5714A, + 0x793A6360, 0x29150FA1, + 0x792A37FE, 0x2944A7A2, + 0x7919F9EB, 0x29743945, + 0x7909A92C, 0x29A3C484, + 0x78F945C3, 0x29D34958, + 0x78E8CFB1, 0x2A02C7B8, + 0x78D846FB, 0x2A323F9D, + 0x78C7ABA1, 0x2A61B101, + 0x78B6FDA8, 0x2A911BDB, + 0x78A63D10, 0x2AC08025, + 0x789569DE, 0x2AEFDDD8, + 0x78848413, 0x2B1F34EB, + 0x78738BB3, 0x2B4E8558, + 0x786280BF, 0x2B7DCF17, + 0x7851633B, 0x2BAD1221, + 0x78403328, 0x2BDC4E6F, + 0x782EF08B, 0x2C0B83F9, + 0x781D9B64, 0x2C3AB2B9, + 0x780C33B8, 0x2C69DAA6, + 0x77FAB988, 0x2C98FBBA, + 0x77E92CD8, 0x2CC815ED, + 0x77D78DAA, 0x2CF72939, + 0x77C5DC01, 0x2D263595, + 0x77B417DF, 0x2D553AFB, + 0x77A24148, 0x2D843963, + 0x7790583D, 0x2DB330C7, + 0x777E5CC3, 0x2DE2211E, + 0x776C4EDB, 0x2E110A62, + 0x775A2E88, 0x2E3FEC8B, + 0x7747FBCE, 0x2E6EC792, + 0x7735B6AE, 0x2E9D9B70, + 0x77235F2D, 0x2ECC681E, + 0x7710F54B, 0x2EFB2D94, + 0x76FE790E, 0x2F29EBCC, + 0x76EBEA77, 0x2F58A2BD, + 0x76D94988, 0x2F875262, + 0x76C69646, 0x2FB5FAB2, + 0x76B3D0B3, 0x2FE49BA6, + 0x76A0F8D2, 0x30133538, + 0x768E0EA5, 0x3041C760, + 0x767B1230, 0x30705217, + 0x76680376, 0x309ED555, + 0x7654E279, 0x30CD5114, + 0x7641AF3C, 0x30FBC54D, + 0x762E69C3, 0x312A31F8, + 0x761B1211, 0x3158970D, + 0x7607A827, 0x3186F487, + 0x75F42C0A, 0x31B54A5D, + 0x75E09DBD, 0x31E39889, + 0x75CCFD42, 0x3211DF03, + 0x75B94A9C, 0x32401DC5, + 0x75A585CF, 0x326E54C7, + 0x7591AEDD, 0x329C8402, + 0x757DC5CA, 0x32CAAB6F, + 0x7569CA98, 0x32F8CB07, + 0x7555BD4B, 0x3326E2C2, + 0x75419DE6, 0x3354F29A, + 0x752D6C6C, 0x3382FA88, + 0x751928E0, 0x33B0FA84, + 0x7504D345, 0x33DEF287, + 0x74F06B9E, 0x340CE28A, + 0x74DBF1EF, 0x343ACA87, + 0x74C7663A, 0x3468AA76, + 0x74B2C883, 0x3496824F, + 0x749E18CD, 0x34C4520D, + 0x7489571B, 0x34F219A7, + 0x74748371, 0x351FD917, + 0x745F9DD1, 0x354D9056, + 0x744AA63E, 0x357B3F5D, + 0x74359CBD, 0x35A8E624, + 0x74208150, 0x35D684A5, + 0x740B53FA, 0x36041AD9, + 0x73F614C0, 0x3631A8B7, + 0x73E0C3A3, 0x365F2E3B, + 0x73CB60A7, 0x368CAB5C, + 0x73B5EBD0, 0x36BA2013, + 0x73A06522, 0x36E78C5A, + 0x738ACC9E, 0x3714F02A, + 0x73752249, 0x37424B7A, + 0x735F6626, 0x376F9E46, + 0x73499838, 0x379CE884, + 0x7333B883, 0x37CA2A30, + 0x731DC709, 0x37F76340, + 0x7307C3D0, 0x382493B0, + 0x72F1AED8, 0x3851BB76, + 0x72DB8828, 0x387EDA8E, + 0x72C54FC0, 0x38ABF0EF, + 0x72AF05A6, 0x38D8FE93, + 0x7298A9DC, 0x39060372, + 0x72823C66, 0x3932FF87, + 0x726BBD48, 0x395FF2C9, + 0x72552C84, 0x398CDD32, + 0x723E8A1F, 0x39B9BEBB, + 0x7227D61C, 0x39E6975D, + 0x7211107D, 0x3A136712, + 0x71FA3948, 0x3A402DD1, + 0x71E3507F, 0x3A6CEB95, + 0x71CC5626, 0x3A99A057, + 0x71B54A40, 0x3AC64C0F, + 0x719E2CD2, 0x3AF2EEB7, + 0x7186FDDE, 0x3B1F8847, + 0x716FBD68, 0x3B4C18BA, + 0x71586B73, 0x3B78A007, + 0x71410804, 0x3BA51E29, + 0x7129931E, 0x3BD19317, + 0x71120CC5, 0x3BFDFECD, + 0x70FA74FB, 0x3C2A6142, + 0x70E2CBC6, 0x3C56BA70, + 0x70CB1127, 0x3C830A4F, + 0x70B34524, 0x3CAF50DA, + 0x709B67C0, 0x3CDB8E09, + 0x708378FE, 0x3D07C1D5, + 0x706B78E3, 0x3D33EC39, + 0x70536771, 0x3D600D2B, + 0x703B44AC, 0x3D8C24A7, + 0x70231099, 0x3DB832A5, + 0x700ACB3B, 0x3DE4371F, + 0x6FF27496, 0x3E10320D, + 0x6FDA0CAD, 0x3E3C2369, + 0x6FC19385, 0x3E680B2C, + 0x6FA90920, 0x3E93E94F, + 0x6F906D84, 0x3EBFBDCC, + 0x6F77C0B3, 0x3EEB889C, + 0x6F5F02B1, 0x3F1749B7, + 0x6F463383, 0x3F430118, + 0x6F2D532C, 0x3F6EAEB8, + 0x6F1461AF, 0x3F9A528F, + 0x6EFB5F12, 0x3FC5EC97, + 0x6EE24B57, 0x3FF17CCA, + 0x6EC92682, 0x401D0320, + 0x6EAFF098, 0x40487F93, + 0x6E96A99C, 0x4073F21D, + 0x6E7D5193, 0x409F5AB6, + 0x6E63E87F, 0x40CAB957, + 0x6E4A6E65, 0x40F60DFB, + 0x6E30E349, 0x4121589A, + 0x6E17472F, 0x414C992E, + 0x6DFD9A1B, 0x4177CFB0, + 0x6DE3DC11, 0x41A2FC1A, + 0x6DCA0D14, 0x41CE1E64, + 0x6DB02D29, 0x41F93688, + 0x6D963C54, 0x42244480, + 0x6D7C3A98, 0x424F4845, + 0x6D6227FA, 0x427A41D0, + 0x6D48047E, 0x42A5311A, + 0x6D2DD027, 0x42D0161E, + 0x6D138AFA, 0x42FAF0D4, + 0x6CF934FB, 0x4325C135, + 0x6CDECE2E, 0x4350873C, + 0x6CC45697, 0x437B42E1, + 0x6CA9CE3A, 0x43A5F41E, + 0x6C8F351C, 0x43D09AEC, + 0x6C748B3F, 0x43FB3745, + 0x6C59D0A9, 0x4425C923, + 0x6C3F055D, 0x4450507E, + 0x6C242960, 0x447ACD50, + 0x6C093CB6, 0x44A53F93, + 0x6BEE3F62, 0x44CFA73F, + 0x6BD3316A, 0x44FA044F, + 0x6BB812D0, 0x452456BC, + 0x6B9CE39B, 0x454E9E80, + 0x6B81A3CD, 0x4578DB93, + 0x6B66536A, 0x45A30DF0, + 0x6B4AF278, 0x45CD358F, + 0x6B2F80FA, 0x45F7526B, + 0x6B13FEF5, 0x4621647C, + 0x6AF86C6C, 0x464B6BBD, + 0x6ADCC964, 0x46756827, + 0x6AC115E1, 0x469F59B4, + 0x6AA551E8, 0x46C9405C, + 0x6A897D7D, 0x46F31C1A, + 0x6A6D98A4, 0x471CECE6, + 0x6A51A361, 0x4746B2BC, + 0x6A359DB9, 0x47706D93, + 0x6A1987B0, 0x479A1D66, + 0x69FD614A, 0x47C3C22E, + 0x69E12A8C, 0x47ED5BE6, + 0x69C4E37A, 0x4816EA85, + 0x69A88C18, 0x48406E07, + 0x698C246C, 0x4869E664, + 0x696FAC78, 0x48935397, + 0x69532442, 0x48BCB598, + 0x69368BCE, 0x48E60C62, + 0x6919E320, 0x490F57EE, + 0x68FD2A3D, 0x49389836, + 0x68E06129, 0x4961CD32, + 0x68C387E9, 0x498AF6DE, + 0x68A69E81, 0x49B41533, + 0x6889A4F5, 0x49DD282A, + 0x686C9B4B, 0x4A062FBD, + 0x684F8186, 0x4A2F2BE5, + 0x683257AA, 0x4A581C9D, + 0x68151DBE, 0x4A8101DE, + 0x67F7D3C4, 0x4AA9DBA1, + 0x67DA79C2, 0x4AD2A9E1, + 0x67BD0FBC, 0x4AFB6C97, + 0x679F95B7, 0x4B2423BD, + 0x67820BB6, 0x4B4CCF4D, + 0x676471C0, 0x4B756F3F, + 0x6746C7D7, 0x4B9E038F, + 0x67290E02, 0x4BC68C36, + 0x670B4443, 0x4BEF092D, + 0x66ED6AA1, 0x4C177A6E, + 0x66CF811F, 0x4C3FDFF3, + 0x66B187C3, 0x4C6839B6, + 0x66937E90, 0x4C9087B1, + 0x6675658C, 0x4CB8C9DD, + 0x66573CBB, 0x4CE10034, + 0x66390422, 0x4D092AB0, + 0x661ABBC5, 0x4D31494B, + 0x65FC63A9, 0x4D595BFE, + 0x65DDFBD3, 0x4D8162C4, + 0x65BF8447, 0x4DA95D96, + 0x65A0FD0B, 0x4DD14C6E, + 0x65826622, 0x4DF92F45, + 0x6563BF92, 0x4E210617, + 0x6545095F, 0x4E48D0DC, + 0x6526438E, 0x4E708F8F, + 0x65076E24, 0x4E984229, + 0x64E88926, 0x4EBFE8A4, + 0x64C99498, 0x4EE782FA, + 0x64AA907F, 0x4F0F1126, + 0x648B7CDF, 0x4F369320, + 0x646C59BF, 0x4F5E08E3, + 0x644D2722, 0x4F857268, + 0x642DE50D, 0x4FACCFAB, + 0x640E9385, 0x4FD420A3, + 0x63EF328F, 0x4FFB654D, + 0x63CFC230, 0x50229DA0, + 0x63B0426D, 0x5049C999, + 0x6390B34A, 0x5070E92F, + 0x637114CC, 0x5097FC5E, + 0x635166F8, 0x50BF031F, + 0x6331A9D4, 0x50E5FD6C, + 0x6311DD63, 0x510CEB40, + 0x62F201AC, 0x5133CC94, + 0x62D216B2, 0x515AA162, + 0x62B21C7B, 0x518169A4, + 0x6292130C, 0x51A82555, + 0x6271FA69, 0x51CED46E, + 0x6251D297, 0x51F576E9, + 0x62319B9D, 0x521C0CC1, + 0x6211557D, 0x524295EF, + 0x61F1003E, 0x5269126E, + 0x61D09BE5, 0x528F8237, + 0x61B02876, 0x52B5E545, + 0x618FA5F6, 0x52DC3B92, + 0x616F146B, 0x53028517, + 0x614E73D9, 0x5328C1D0, + 0x612DC446, 0x534EF1B5, + 0x610D05B7, 0x537514C1, + 0x60EC3830, 0x539B2AEF, + 0x60CB5BB6, 0x53C13438, + 0x60AA704F, 0x53E73097, + 0x60897600, 0x540D2005, + 0x60686CCE, 0x5433027D, + 0x604754BE, 0x5458D7F9, + 0x60262DD5, 0x547EA073, + 0x6004F818, 0x54A45BE5, + 0x5FE3B38D, 0x54CA0A4A, + 0x5FC26038, 0x54EFAB9C, + 0x5FA0FE1E, 0x55153FD4, + 0x5F7F8D46, 0x553AC6ED, + 0x5F5E0DB3, 0x556040E2, + 0x5F3C7F6B, 0x5585ADAC, + 0x5F1AE273, 0x55AB0D46, + 0x5EF936D1, 0x55D05FAA, + 0x5ED77C89, 0x55F5A4D2, + 0x5EB5B3A1, 0x561ADCB8, + 0x5E93DC1F, 0x56400757, + 0x5E71F606, 0x566524AA, + 0x5E50015D, 0x568A34A9, + 0x5E2DFE28, 0x56AF3750, + 0x5E0BEC6E, 0x56D42C99, + 0x5DE9CC32, 0x56F9147E, + 0x5DC79D7C, 0x571DEEF9, + 0x5DA5604E, 0x5742BC05, + 0x5D8314B0, 0x57677B9D, + 0x5D60BAA6, 0x578C2DB9, + 0x5D3E5236, 0x57B0D256, + 0x5D1BDB65, 0x57D5696C, + 0x5CF95638, 0x57F9F2F7, + 0x5CD6C2B4, 0x581E6EF1, + 0x5CB420DF, 0x5842DD54, + 0x5C9170BF, 0x58673E1B, + 0x5C6EB258, 0x588B913F, + 0x5C4BE5B0, 0x58AFD6BC, + 0x5C290ACC, 0x58D40E8C, + 0x5C0621B2, 0x58F838A9, + 0x5BE32A67, 0x591C550E, + 0x5BC024F0, 0x594063B4, + 0x5B9D1153, 0x59646497, + 0x5B79EF96, 0x598857B1, + 0x5B56BFBD, 0x59AC3CFD, + 0x5B3381CE, 0x59D01474, + 0x5B1035CF, 0x59F3DE12, + 0x5AECDBC4, 0x5A1799D0, + 0x5AC973B4, 0x5A3B47AA, + 0x5AA5FDA4, 0x5A5EE79A, + 0x5A82799A, 0x5A82799A, + 0x5A5EE79A, 0x5AA5FDA4, + 0x5A3B47AA, 0x5AC973B4, + 0x5A1799D0, 0x5AECDBC4, + 0x59F3DE12, 0x5B1035CF, + 0x59D01474, 0x5B3381CE, + 0x59AC3CFD, 0x5B56BFBD, + 0x598857B1, 0x5B79EF96, + 0x59646497, 0x5B9D1153, + 0x594063B4, 0x5BC024F0, + 0x591C550E, 0x5BE32A67, + 0x58F838A9, 0x5C0621B2, + 0x58D40E8C, 0x5C290ACC, + 0x58AFD6BC, 0x5C4BE5B0, + 0x588B913F, 0x5C6EB258, + 0x58673E1B, 0x5C9170BF, + 0x5842DD54, 0x5CB420DF, + 0x581E6EF1, 0x5CD6C2B4, + 0x57F9F2F7, 0x5CF95638, + 0x57D5696C, 0x5D1BDB65, + 0x57B0D256, 0x5D3E5236, + 0x578C2DB9, 0x5D60BAA6, + 0x57677B9D, 0x5D8314B0, + 0x5742BC05, 0x5DA5604E, + 0x571DEEF9, 0x5DC79D7C, + 0x56F9147E, 0x5DE9CC32, + 0x56D42C99, 0x5E0BEC6E, + 0x56AF3750, 0x5E2DFE28, + 0x568A34A9, 0x5E50015D, + 0x566524AA, 0x5E71F606, + 0x56400757, 0x5E93DC1F, + 0x561ADCB8, 0x5EB5B3A1, + 0x55F5A4D2, 0x5ED77C89, + 0x55D05FAA, 0x5EF936D1, + 0x55AB0D46, 0x5F1AE273, + 0x5585ADAC, 0x5F3C7F6B, + 0x556040E2, 0x5F5E0DB3, + 0x553AC6ED, 0x5F7F8D46, + 0x55153FD4, 0x5FA0FE1E, + 0x54EFAB9C, 0x5FC26038, + 0x54CA0A4A, 0x5FE3B38D, + 0x54A45BE5, 0x6004F818, + 0x547EA073, 0x60262DD5, + 0x5458D7F9, 0x604754BE, + 0x5433027D, 0x60686CCE, + 0x540D2005, 0x60897600, + 0x53E73097, 0x60AA704F, + 0x53C13438, 0x60CB5BB6, + 0x539B2AEF, 0x60EC3830, + 0x537514C1, 0x610D05B7, + 0x534EF1B5, 0x612DC446, + 0x5328C1D0, 0x614E73D9, + 0x53028517, 0x616F146B, + 0x52DC3B92, 0x618FA5F6, + 0x52B5E545, 0x61B02876, + 0x528F8237, 0x61D09BE5, + 0x5269126E, 0x61F1003E, + 0x524295EF, 0x6211557D, + 0x521C0CC1, 0x62319B9D, + 0x51F576E9, 0x6251D297, + 0x51CED46E, 0x6271FA69, + 0x51A82555, 0x6292130C, + 0x518169A4, 0x62B21C7B, + 0x515AA162, 0x62D216B2, + 0x5133CC94, 0x62F201AC, + 0x510CEB40, 0x6311DD63, + 0x50E5FD6C, 0x6331A9D4, + 0x50BF031F, 0x635166F8, + 0x5097FC5E, 0x637114CC, + 0x5070E92F, 0x6390B34A, + 0x5049C999, 0x63B0426D, + 0x50229DA0, 0x63CFC230, + 0x4FFB654D, 0x63EF328F, + 0x4FD420A3, 0x640E9385, + 0x4FACCFAB, 0x642DE50D, + 0x4F857268, 0x644D2722, + 0x4F5E08E3, 0x646C59BF, + 0x4F369320, 0x648B7CDF, + 0x4F0F1126, 0x64AA907F, + 0x4EE782FA, 0x64C99498, + 0x4EBFE8A4, 0x64E88926, + 0x4E984229, 0x65076E24, + 0x4E708F8F, 0x6526438E, + 0x4E48D0DC, 0x6545095F, + 0x4E210617, 0x6563BF92, + 0x4DF92F45, 0x65826622, + 0x4DD14C6E, 0x65A0FD0B, + 0x4DA95D96, 0x65BF8447, + 0x4D8162C4, 0x65DDFBD3, + 0x4D595BFE, 0x65FC63A9, + 0x4D31494B, 0x661ABBC5, + 0x4D092AB0, 0x66390422, + 0x4CE10034, 0x66573CBB, + 0x4CB8C9DD, 0x6675658C, + 0x4C9087B1, 0x66937E90, + 0x4C6839B6, 0x66B187C3, + 0x4C3FDFF3, 0x66CF811F, + 0x4C177A6E, 0x66ED6AA1, + 0x4BEF092D, 0x670B4443, + 0x4BC68C36, 0x67290E02, + 0x4B9E038F, 0x6746C7D7, + 0x4B756F3F, 0x676471C0, + 0x4B4CCF4D, 0x67820BB6, + 0x4B2423BD, 0x679F95B7, + 0x4AFB6C97, 0x67BD0FBC, + 0x4AD2A9E1, 0x67DA79C2, + 0x4AA9DBA1, 0x67F7D3C4, + 0x4A8101DE, 0x68151DBE, + 0x4A581C9D, 0x683257AA, + 0x4A2F2BE5, 0x684F8186, + 0x4A062FBD, 0x686C9B4B, + 0x49DD282A, 0x6889A4F5, + 0x49B41533, 0x68A69E81, + 0x498AF6DE, 0x68C387E9, + 0x4961CD32, 0x68E06129, + 0x49389836, 0x68FD2A3D, + 0x490F57EE, 0x6919E320, + 0x48E60C62, 0x69368BCE, + 0x48BCB598, 0x69532442, + 0x48935397, 0x696FAC78, + 0x4869E664, 0x698C246C, + 0x48406E07, 0x69A88C18, + 0x4816EA85, 0x69C4E37A, + 0x47ED5BE6, 0x69E12A8C, + 0x47C3C22E, 0x69FD614A, + 0x479A1D66, 0x6A1987B0, + 0x47706D93, 0x6A359DB9, + 0x4746B2BC, 0x6A51A361, + 0x471CECE6, 0x6A6D98A4, + 0x46F31C1A, 0x6A897D7D, + 0x46C9405C, 0x6AA551E8, + 0x469F59B4, 0x6AC115E1, + 0x46756827, 0x6ADCC964, + 0x464B6BBD, 0x6AF86C6C, + 0x4621647C, 0x6B13FEF5, + 0x45F7526B, 0x6B2F80FA, + 0x45CD358F, 0x6B4AF278, + 0x45A30DF0, 0x6B66536A, + 0x4578DB93, 0x6B81A3CD, + 0x454E9E80, 0x6B9CE39B, + 0x452456BC, 0x6BB812D0, + 0x44FA044F, 0x6BD3316A, + 0x44CFA73F, 0x6BEE3F62, + 0x44A53F93, 0x6C093CB6, + 0x447ACD50, 0x6C242960, + 0x4450507E, 0x6C3F055D, + 0x4425C923, 0x6C59D0A9, + 0x43FB3745, 0x6C748B3F, + 0x43D09AEC, 0x6C8F351C, + 0x43A5F41E, 0x6CA9CE3A, + 0x437B42E1, 0x6CC45697, + 0x4350873C, 0x6CDECE2E, + 0x4325C135, 0x6CF934FB, + 0x42FAF0D4, 0x6D138AFA, + 0x42D0161E, 0x6D2DD027, + 0x42A5311A, 0x6D48047E, + 0x427A41D0, 0x6D6227FA, + 0x424F4845, 0x6D7C3A98, + 0x42244480, 0x6D963C54, + 0x41F93688, 0x6DB02D29, + 0x41CE1E64, 0x6DCA0D14, + 0x41A2FC1A, 0x6DE3DC11, + 0x4177CFB0, 0x6DFD9A1B, + 0x414C992E, 0x6E17472F, + 0x4121589A, 0x6E30E349, + 0x40F60DFB, 0x6E4A6E65, + 0x40CAB957, 0x6E63E87F, + 0x409F5AB6, 0x6E7D5193, + 0x4073F21D, 0x6E96A99C, + 0x40487F93, 0x6EAFF098, + 0x401D0320, 0x6EC92682, + 0x3FF17CCA, 0x6EE24B57, + 0x3FC5EC97, 0x6EFB5F12, + 0x3F9A528F, 0x6F1461AF, + 0x3F6EAEB8, 0x6F2D532C, + 0x3F430118, 0x6F463383, + 0x3F1749B7, 0x6F5F02B1, + 0x3EEB889C, 0x6F77C0B3, + 0x3EBFBDCC, 0x6F906D84, + 0x3E93E94F, 0x6FA90920, + 0x3E680B2C, 0x6FC19385, + 0x3E3C2369, 0x6FDA0CAD, + 0x3E10320D, 0x6FF27496, + 0x3DE4371F, 0x700ACB3B, + 0x3DB832A5, 0x70231099, + 0x3D8C24A7, 0x703B44AC, + 0x3D600D2B, 0x70536771, + 0x3D33EC39, 0x706B78E3, + 0x3D07C1D5, 0x708378FE, + 0x3CDB8E09, 0x709B67C0, + 0x3CAF50DA, 0x70B34524, + 0x3C830A4F, 0x70CB1127, + 0x3C56BA70, 0x70E2CBC6, + 0x3C2A6142, 0x70FA74FB, + 0x3BFDFECD, 0x71120CC5, + 0x3BD19317, 0x7129931E, + 0x3BA51E29, 0x71410804, + 0x3B78A007, 0x71586B73, + 0x3B4C18BA, 0x716FBD68, + 0x3B1F8847, 0x7186FDDE, + 0x3AF2EEB7, 0x719E2CD2, + 0x3AC64C0F, 0x71B54A40, + 0x3A99A057, 0x71CC5626, + 0x3A6CEB95, 0x71E3507F, + 0x3A402DD1, 0x71FA3948, + 0x3A136712, 0x7211107D, + 0x39E6975D, 0x7227D61C, + 0x39B9BEBB, 0x723E8A1F, + 0x398CDD32, 0x72552C84, + 0x395FF2C9, 0x726BBD48, + 0x3932FF87, 0x72823C66, + 0x39060372, 0x7298A9DC, + 0x38D8FE93, 0x72AF05A6, + 0x38ABF0EF, 0x72C54FC0, + 0x387EDA8E, 0x72DB8828, + 0x3851BB76, 0x72F1AED8, + 0x382493B0, 0x7307C3D0, + 0x37F76340, 0x731DC709, + 0x37CA2A30, 0x7333B883, + 0x379CE884, 0x73499838, + 0x376F9E46, 0x735F6626, + 0x37424B7A, 0x73752249, + 0x3714F02A, 0x738ACC9E, + 0x36E78C5A, 0x73A06522, + 0x36BA2013, 0x73B5EBD0, + 0x368CAB5C, 0x73CB60A7, + 0x365F2E3B, 0x73E0C3A3, + 0x3631A8B7, 0x73F614C0, + 0x36041AD9, 0x740B53FA, + 0x35D684A5, 0x74208150, + 0x35A8E624, 0x74359CBD, + 0x357B3F5D, 0x744AA63E, + 0x354D9056, 0x745F9DD1, + 0x351FD917, 0x74748371, + 0x34F219A7, 0x7489571B, + 0x34C4520D, 0x749E18CD, + 0x3496824F, 0x74B2C883, + 0x3468AA76, 0x74C7663A, + 0x343ACA87, 0x74DBF1EF, + 0x340CE28A, 0x74F06B9E, + 0x33DEF287, 0x7504D345, + 0x33B0FA84, 0x751928E0, + 0x3382FA88, 0x752D6C6C, + 0x3354F29A, 0x75419DE6, + 0x3326E2C2, 0x7555BD4B, + 0x32F8CB07, 0x7569CA98, + 0x32CAAB6F, 0x757DC5CA, + 0x329C8402, 0x7591AEDD, + 0x326E54C7, 0x75A585CF, + 0x32401DC5, 0x75B94A9C, + 0x3211DF03, 0x75CCFD42, + 0x31E39889, 0x75E09DBD, + 0x31B54A5D, 0x75F42C0A, + 0x3186F487, 0x7607A827, + 0x3158970D, 0x761B1211, + 0x312A31F8, 0x762E69C3, + 0x30FBC54D, 0x7641AF3C, + 0x30CD5114, 0x7654E279, + 0x309ED555, 0x76680376, + 0x30705217, 0x767B1230, + 0x3041C760, 0x768E0EA5, + 0x30133538, 0x76A0F8D2, + 0x2FE49BA6, 0x76B3D0B3, + 0x2FB5FAB2, 0x76C69646, + 0x2F875262, 0x76D94988, + 0x2F58A2BD, 0x76EBEA77, + 0x2F29EBCC, 0x76FE790E, + 0x2EFB2D94, 0x7710F54B, + 0x2ECC681E, 0x77235F2D, + 0x2E9D9B70, 0x7735B6AE, + 0x2E6EC792, 0x7747FBCE, + 0x2E3FEC8B, 0x775A2E88, + 0x2E110A62, 0x776C4EDB, + 0x2DE2211E, 0x777E5CC3, + 0x2DB330C7, 0x7790583D, + 0x2D843963, 0x77A24148, + 0x2D553AFB, 0x77B417DF, + 0x2D263595, 0x77C5DC01, + 0x2CF72939, 0x77D78DAA, + 0x2CC815ED, 0x77E92CD8, + 0x2C98FBBA, 0x77FAB988, + 0x2C69DAA6, 0x780C33B8, + 0x2C3AB2B9, 0x781D9B64, + 0x2C0B83F9, 0x782EF08B, + 0x2BDC4E6F, 0x78403328, + 0x2BAD1221, 0x7851633B, + 0x2B7DCF17, 0x786280BF, + 0x2B4E8558, 0x78738BB3, + 0x2B1F34EB, 0x78848413, + 0x2AEFDDD8, 0x789569DE, + 0x2AC08025, 0x78A63D10, + 0x2A911BDB, 0x78B6FDA8, + 0x2A61B101, 0x78C7ABA1, + 0x2A323F9D, 0x78D846FB, + 0x2A02C7B8, 0x78E8CFB1, + 0x29D34958, 0x78F945C3, + 0x29A3C484, 0x7909A92C, + 0x29743945, 0x7919F9EB, + 0x2944A7A2, 0x792A37FE, + 0x29150FA1, 0x793A6360, + 0x28E5714A, 0x794A7C11, + 0x28B5CCA5, 0x795A820E, + 0x288621B9, 0x796A7554, + 0x2856708C, 0x797A55E0, + 0x2826B928, 0x798A23B1, + 0x27F6FB92, 0x7999DEC3, + 0x27C737D2, 0x79A98715, + 0x27976DF1, 0x79B91CA4, + 0x27679DF4, 0x79C89F6D, + 0x2737C7E3, 0x79D80F6F, + 0x2707EBC6, 0x79E76CA6, + 0x26D809A5, 0x79F6B711, + 0x26A82185, 0x7A05EEAD, + 0x26783370, 0x7A151377, + 0x26483F6C, 0x7A24256E, + 0x26184581, 0x7A33248F, + 0x25E845B5, 0x7A4210D8, + 0x25B84012, 0x7A50EA46, + 0x2588349D, 0x7A5FB0D8, + 0x2558235E, 0x7A6E648A, + 0x25280C5D, 0x7A7D055B, + 0x24F7EFA1, 0x7A8B9348, + 0x24C7CD32, 0x7A9A0E4F, + 0x2497A517, 0x7AA8766E, + 0x24677757, 0x7AB6CBA3, + 0x243743FA, 0x7AC50DEB, + 0x24070B07, 0x7AD33D45, + 0x23D6CC86, 0x7AE159AE, + 0x23A6887E, 0x7AEF6323, + 0x23763EF7, 0x7AFD59A3, + 0x2345EFF7, 0x7B0B3D2C, + 0x23159B87, 0x7B190DBB, + 0x22E541AE, 0x7B26CB4F, + 0x22B4E274, 0x7B3475E4, + 0x22847DDF, 0x7B420D7A, + 0x225413F8, 0x7B4F920E, + 0x2223A4C5, 0x7B5D039D, + 0x21F3304E, 0x7B6A6227, + 0x21C2B69C, 0x7B77ADA8, + 0x219237B4, 0x7B84E61E, + 0x2161B39F, 0x7B920B89, + 0x21312A65, 0x7B9F1DE5, + 0x21009C0B, 0x7BAC1D31, + 0x20D0089B, 0x7BB9096A, + 0x209F701C, 0x7BC5E28F, + 0x206ED295, 0x7BD2A89E, + 0x203E300D, 0x7BDF5B94, + 0x200D888C, 0x7BEBFB70, + 0x1FDCDC1A, 0x7BF88830, + 0x1FAC2ABF, 0x7C0501D1, + 0x1F7B7480, 0x7C116853, + 0x1F4AB967, 0x7C1DBBB2, + 0x1F19F97B, 0x7C29FBEE, + 0x1EE934C2, 0x7C362904, + 0x1EB86B46, 0x7C4242F2, + 0x1E879D0C, 0x7C4E49B6, + 0x1E56CA1E, 0x7C5A3D4F, + 0x1E25F281, 0x7C661DBB, + 0x1DF5163F, 0x7C71EAF8, + 0x1DC4355D, 0x7C7DA504, + 0x1D934FE5, 0x7C894BDD, + 0x1D6265DD, 0x7C94DF82, + 0x1D31774D, 0x7CA05FF1, + 0x1D00843C, 0x7CABCD27, + 0x1CCF8CB3, 0x7CB72724, + 0x1C9E90B8, 0x7CC26DE5, + 0x1C6D9053, 0x7CCDA168, + 0x1C3C8B8C, 0x7CD8C1AD, + 0x1C0B826A, 0x7CE3CEB1, + 0x1BDA74F5, 0x7CEEC873, + 0x1BA96334, 0x7CF9AEF0, + 0x1B784D30, 0x7D048228, + 0x1B4732EF, 0x7D0F4218, + 0x1B161479, 0x7D19EEBE, + 0x1AE4F1D6, 0x7D24881A, + 0x1AB3CB0C, 0x7D2F0E2A, + 0x1A82A025, 0x7D3980EC, + 0x1A517127, 0x7D43E05E, + 0x1A203E1B, 0x7D4E2C7E, + 0x19EF0706, 0x7D58654C, + 0x19BDCBF2, 0x7D628AC5, + 0x198C8CE6, 0x7D6C9CE9, + 0x195B49E9, 0x7D769BB5, + 0x192A0303, 0x7D808727, + 0x18F8B83C, 0x7D8A5F3F, + 0x18C7699B, 0x7D9423FB, + 0x18961727, 0x7D9DD55A, + 0x1864C0E9, 0x7DA77359, + 0x183366E8, 0x7DB0FDF7, + 0x1802092C, 0x7DBA7534, + 0x17D0A7BB, 0x7DC3D90D, + 0x179F429F, 0x7DCD2981, + 0x176DD9DE, 0x7DD6668E, + 0x173C6D80, 0x7DDF9034, + 0x170AFD8D, 0x7DE8A670, + 0x16D98A0C, 0x7DF1A942, + 0x16A81305, 0x7DFA98A7, + 0x1676987F, 0x7E03749F, + 0x16451A83, 0x7E0C3D29, + 0x16139917, 0x7E14F242, + 0x15E21444, 0x7E1D93E9, + 0x15B08C11, 0x7E26221E, + 0x157F0086, 0x7E2E9CDF, + 0x154D71AA, 0x7E37042A, + 0x151BDF85, 0x7E3F57FE, + 0x14EA4A1F, 0x7E47985B, + 0x14B8B17F, 0x7E4FC53E, + 0x148715AD, 0x7E57DEA6, + 0x145576B1, 0x7E5FE493, + 0x1423D492, 0x7E67D702, + 0x13F22F57, 0x7E6FB5F3, + 0x13C0870A, 0x7E778165, + 0x138EDBB0, 0x7E7F3956, + 0x135D2D53, 0x7E86DDC5, + 0x132B7BF9, 0x7E8E6EB1, + 0x12F9C7AA, 0x7E95EC19, + 0x12C8106E, 0x7E9D55FC, + 0x1296564D, 0x7EA4AC58, + 0x1264994E, 0x7EABEF2C, + 0x1232D978, 0x7EB31E77, + 0x120116D4, 0x7EBA3A39, + 0x11CF516A, 0x7EC1426F, + 0x119D8940, 0x7EC8371A, + 0x116BBE5F, 0x7ECF1837, + 0x1139F0CE, 0x7ED5E5C6, + 0x11082096, 0x7EDC9FC6, + 0x10D64DBC, 0x7EE34635, + 0x10A4784A, 0x7EE9D913, + 0x1072A047, 0x7EF0585F, + 0x1040C5BB, 0x7EF6C418, + 0x100EE8AD, 0x7EFD1C3C, + 0x0FDD0925, 0x7F0360CB, + 0x0FAB272B, 0x7F0991C3, + 0x0F7942C6, 0x7F0FAF24, + 0x0F475BFE, 0x7F15B8EE, + 0x0F1572DC, 0x7F1BAF1E, + 0x0EE38765, 0x7F2191B4, + 0x0EB199A3, 0x7F2760AF, + 0x0E7FA99D, 0x7F2D1C0E, + 0x0E4DB75B, 0x7F32C3D0, + 0x0E1BC2E3, 0x7F3857F5, + 0x0DE9CC3F, 0x7F3DD87C, + 0x0DB7D376, 0x7F434563, + 0x0D85D88F, 0x7F489EAA, + 0x0D53DB92, 0x7F4DE450, + 0x0D21DC87, 0x7F531654, + 0x0CEFDB75, 0x7F5834B6, + 0x0CBDD865, 0x7F5D3F75, + 0x0C8BD35E, 0x7F62368F, + 0x0C59CC67, 0x7F671A04, + 0x0C27C389, 0x7F6BE9D4, + 0x0BF5B8CB, 0x7F70A5FD, + 0x0BC3AC35, 0x7F754E7F, + 0x0B919DCE, 0x7F79E35A, + 0x0B5F8D9F, 0x7F7E648B, + 0x0B2D7BAE, 0x7F82D214, + 0x0AFB6805, 0x7F872BF3, + 0x0AC952AA, 0x7F8B7226, + 0x0A973BA5, 0x7F8FA4AF, + 0x0A6522FE, 0x7F93C38C, + 0x0A3308BC, 0x7F97CEBC, + 0x0A00ECE8, 0x7F9BC63F, + 0x09CECF89, 0x7F9FAA15, + 0x099CB0A7, 0x7FA37A3C, + 0x096A9049, 0x7FA736B4, + 0x09386E77, 0x7FAADF7C, + 0x09064B3A, 0x7FAE7494, + 0x08D42698, 0x7FB1F5FC, + 0x08A2009A, 0x7FB563B2, + 0x086FD947, 0x7FB8BDB7, + 0x083DB0A7, 0x7FBC040A, + 0x080B86C1, 0x7FBF36A9, + 0x07D95B9E, 0x7FC25596, + 0x07A72F45, 0x7FC560CF, + 0x077501BE, 0x7FC85853, + 0x0742D310, 0x7FCB3C23, + 0x0710A344, 0x7FCE0C3E, + 0x06DE7261, 0x7FD0C8A3, + 0x06AC406F, 0x7FD37152, + 0x067A0D75, 0x7FD6064B, + 0x0647D97C, 0x7FD8878D, + 0x0615A48A, 0x7FDAF518, + 0x05E36EA9, 0x7FDD4EEC, + 0x05B137DF, 0x7FDF9508, + 0x057F0034, 0x7FE1C76B, + 0x054CC7B0, 0x7FE3E616, + 0x051A8E5C, 0x7FE5F108, + 0x04E8543D, 0x7FE7E840, + 0x04B6195D, 0x7FE9CBC0, + 0x0483DDC3, 0x7FEB9B85, + 0x0451A176, 0x7FED5790, + 0x041F647F, 0x7FEEFFE1, + 0x03ED26E6, 0x7FF09477, + 0x03BAE8B1, 0x7FF21553, + 0x0388A9E9, 0x7FF38273, + 0x03566A96, 0x7FF4DBD8, + 0x03242ABF, 0x7FF62182, + 0x02F1EA6B, 0x7FF7536F, + 0x02BFA9A4, 0x7FF871A1, + 0x028D6870, 0x7FF97C17, + 0x025B26D7, 0x7FFA72D1, + 0x0228E4E1, 0x7FFB55CE, + 0x01F6A296, 0x7FFC250F, + 0x01C45FFE, 0x7FFCE093, + 0x01921D1F, 0x7FFD885A, + 0x015FDA03, 0x7FFE1C64, + 0x012D96B0, 0x7FFE9CB2, + 0x00FB532F, 0x7FFF0942, + 0x00C90F88, 0x7FFF6216, + 0x0096CBC1, 0x7FFFA72C, + 0x006487E3, 0x7FFFD885, + 0x003243F5, 0x7FFFF621, + 0x00000000, 0x7FFFFFFF, + 0xFFCDBC0A, 0x7FFFF621, + 0xFF9B781D, 0x7FFFD885, + 0xFF69343E, 0x7FFFA72C, + 0xFF36F078, 0x7FFF6216, + 0xFF04ACD0, 0x7FFF0942, + 0xFED2694F, 0x7FFE9CB2, + 0xFEA025FC, 0x7FFE1C64, + 0xFE6DE2E0, 0x7FFD885A, + 0xFE3BA001, 0x7FFCE093, + 0xFE095D69, 0x7FFC250F, + 0xFDD71B1E, 0x7FFB55CE, + 0xFDA4D928, 0x7FFA72D1, + 0xFD72978F, 0x7FF97C17, + 0xFD40565B, 0x7FF871A1, + 0xFD0E1594, 0x7FF7536F, + 0xFCDBD541, 0x7FF62182, + 0xFCA99569, 0x7FF4DBD8, + 0xFC775616, 0x7FF38273, + 0xFC45174E, 0x7FF21553, + 0xFC12D919, 0x7FF09477, + 0xFBE09B80, 0x7FEEFFE1, + 0xFBAE5E89, 0x7FED5790, + 0xFB7C223C, 0x7FEB9B85, + 0xFB49E6A2, 0x7FE9CBC0, + 0xFB17ABC2, 0x7FE7E840, + 0xFAE571A4, 0x7FE5F108, + 0xFAB3384F, 0x7FE3E616, + 0xFA80FFCB, 0x7FE1C76B, + 0xFA4EC820, 0x7FDF9508, + 0xFA1C9156, 0x7FDD4EEC, + 0xF9EA5B75, 0x7FDAF518, + 0xF9B82683, 0x7FD8878D, + 0xF985F28A, 0x7FD6064B, + 0xF953BF90, 0x7FD37152, + 0xF9218D9E, 0x7FD0C8A3, + 0xF8EF5CBB, 0x7FCE0C3E, + 0xF8BD2CEF, 0x7FCB3C23, + 0xF88AFE41, 0x7FC85853, + 0xF858D0BA, 0x7FC560CF, + 0xF826A461, 0x7FC25596, + 0xF7F4793E, 0x7FBF36A9, + 0xF7C24F58, 0x7FBC040A, + 0xF79026B8, 0x7FB8BDB7, + 0xF75DFF65, 0x7FB563B2, + 0xF72BD967, 0x7FB1F5FC, + 0xF6F9B4C5, 0x7FAE7494, + 0xF6C79188, 0x7FAADF7C, + 0xF6956FB6, 0x7FA736B4, + 0xF6634F58, 0x7FA37A3C, + 0xF6313076, 0x7F9FAA15, + 0xF5FF1317, 0x7F9BC63F, + 0xF5CCF743, 0x7F97CEBC, + 0xF59ADD01, 0x7F93C38C, + 0xF568C45A, 0x7F8FA4AF, + 0xF536AD55, 0x7F8B7226, + 0xF50497FA, 0x7F872BF3, + 0xF4D28451, 0x7F82D214, + 0xF4A07260, 0x7F7E648B, + 0xF46E6231, 0x7F79E35A, + 0xF43C53CA, 0x7F754E7F, + 0xF40A4734, 0x7F70A5FD, + 0xF3D83C76, 0x7F6BE9D4, + 0xF3A63398, 0x7F671A04, + 0xF3742CA1, 0x7F62368F, + 0xF342279A, 0x7F5D3F75, + 0xF310248A, 0x7F5834B6, + 0xF2DE2378, 0x7F531654, + 0xF2AC246D, 0x7F4DE450, + 0xF27A2770, 0x7F489EAA, + 0xF2482C89, 0x7F434563, + 0xF21633C0, 0x7F3DD87C, + 0xF1E43D1C, 0x7F3857F5, + 0xF1B248A5, 0x7F32C3D0, + 0xF1805662, 0x7F2D1C0E, + 0xF14E665C, 0x7F2760AF, + 0xF11C789A, 0x7F2191B4, + 0xF0EA8D23, 0x7F1BAF1E, + 0xF0B8A401, 0x7F15B8EE, + 0xF086BD39, 0x7F0FAF24, + 0xF054D8D4, 0x7F0991C3, + 0xF022F6DA, 0x7F0360CB, + 0xEFF11752, 0x7EFD1C3C, + 0xEFBF3A44, 0x7EF6C418, + 0xEF8D5FB8, 0x7EF0585F, + 0xEF5B87B5, 0x7EE9D913, + 0xEF29B243, 0x7EE34635, + 0xEEF7DF6A, 0x7EDC9FC6, + 0xEEC60F31, 0x7ED5E5C6, + 0xEE9441A0, 0x7ECF1837, + 0xEE6276BF, 0x7EC8371A, + 0xEE30AE95, 0x7EC1426F, + 0xEDFEE92B, 0x7EBA3A39, + 0xEDCD2687, 0x7EB31E77, + 0xED9B66B2, 0x7EABEF2C, + 0xED69A9B2, 0x7EA4AC58, + 0xED37EF91, 0x7E9D55FC, + 0xED063855, 0x7E95EC19, + 0xECD48406, 0x7E8E6EB1, + 0xECA2D2AC, 0x7E86DDC5, + 0xEC71244F, 0x7E7F3956, + 0xEC3F78F5, 0x7E778165, + 0xEC0DD0A8, 0x7E6FB5F3, + 0xEBDC2B6D, 0x7E67D702, + 0xEBAA894E, 0x7E5FE493, + 0xEB78EA52, 0x7E57DEA6, + 0xEB474E80, 0x7E4FC53E, + 0xEB15B5E0, 0x7E47985B, + 0xEAE4207A, 0x7E3F57FE, + 0xEAB28E55, 0x7E37042A, + 0xEA80FF79, 0x7E2E9CDF, + 0xEA4F73EE, 0x7E26221E, + 0xEA1DEBBB, 0x7E1D93E9, + 0xE9EC66E8, 0x7E14F242, + 0xE9BAE57C, 0x7E0C3D29, + 0xE9896780, 0x7E03749F, + 0xE957ECFB, 0x7DFA98A7, + 0xE92675F4, 0x7DF1A942, + 0xE8F50273, 0x7DE8A670, + 0xE8C3927F, 0x7DDF9034, + 0xE8922621, 0x7DD6668E, + 0xE860BD60, 0x7DCD2981, + 0xE82F5844, 0x7DC3D90D, + 0xE7FDF6D3, 0x7DBA7534, + 0xE7CC9917, 0x7DB0FDF7, + 0xE79B3F16, 0x7DA77359, + 0xE769E8D8, 0x7D9DD55A, + 0xE7389664, 0x7D9423FB, + 0xE70747C3, 0x7D8A5F3F, + 0xE6D5FCFC, 0x7D808727, + 0xE6A4B616, 0x7D769BB5, + 0xE6737319, 0x7D6C9CE9, + 0xE642340D, 0x7D628AC5, + 0xE610F8F9, 0x7D58654C, + 0xE5DFC1E4, 0x7D4E2C7E, + 0xE5AE8ED8, 0x7D43E05E, + 0xE57D5FDA, 0x7D3980EC, + 0xE54C34F3, 0x7D2F0E2A, + 0xE51B0E2A, 0x7D24881A, + 0xE4E9EB86, 0x7D19EEBE, + 0xE4B8CD10, 0x7D0F4218, + 0xE487B2CF, 0x7D048228, + 0xE4569CCB, 0x7CF9AEF0, + 0xE4258B0A, 0x7CEEC873, + 0xE3F47D95, 0x7CE3CEB1, + 0xE3C37473, 0x7CD8C1AD, + 0xE3926FAC, 0x7CCDA168, + 0xE3616F47, 0x7CC26DE5, + 0xE330734C, 0x7CB72724, + 0xE2FF7BC3, 0x7CABCD27, + 0xE2CE88B2, 0x7CA05FF1, + 0xE29D9A22, 0x7C94DF82, + 0xE26CB01A, 0x7C894BDD, + 0xE23BCAA2, 0x7C7DA504, + 0xE20AE9C1, 0x7C71EAF8, + 0xE1DA0D7E, 0x7C661DBB, + 0xE1A935E1, 0x7C5A3D4F, + 0xE17862F3, 0x7C4E49B6, + 0xE14794B9, 0x7C4242F2, + 0xE116CB3D, 0x7C362904, + 0xE0E60684, 0x7C29FBEE, + 0xE0B54698, 0x7C1DBBB2, + 0xE0848B7F, 0x7C116853, + 0xE053D541, 0x7C0501D1, + 0xE02323E5, 0x7BF88830, + 0xDFF27773, 0x7BEBFB70, + 0xDFC1CFF2, 0x7BDF5B94, + 0xDF912D6A, 0x7BD2A89E, + 0xDF608FE3, 0x7BC5E28F, + 0xDF2FF764, 0x7BB9096A, + 0xDEFF63F4, 0x7BAC1D31, + 0xDECED59B, 0x7B9F1DE5, + 0xDE9E4C60, 0x7B920B89, + 0xDE6DC84B, 0x7B84E61E, + 0xDE3D4963, 0x7B77ADA8, + 0xDE0CCFB1, 0x7B6A6227, + 0xDDDC5B3A, 0x7B5D039D, + 0xDDABEC07, 0x7B4F920E, + 0xDD7B8220, 0x7B420D7A, + 0xDD4B1D8B, 0x7B3475E4, + 0xDD1ABE51, 0x7B26CB4F, + 0xDCEA6478, 0x7B190DBB, + 0xDCBA1008, 0x7B0B3D2C, + 0xDC89C108, 0x7AFD59A3, + 0xDC597781, 0x7AEF6323, + 0xDC293379, 0x7AE159AE, + 0xDBF8F4F8, 0x7AD33D45, + 0xDBC8BC05, 0x7AC50DEB, + 0xDB9888A8, 0x7AB6CBA3, + 0xDB685AE8, 0x7AA8766E, + 0xDB3832CD, 0x7A9A0E4F, + 0xDB08105E, 0x7A8B9348, + 0xDAD7F3A2, 0x7A7D055B, + 0xDAA7DCA1, 0x7A6E648A, + 0xDA77CB62, 0x7A5FB0D8, + 0xDA47BFED, 0x7A50EA46, + 0xDA17BA4A, 0x7A4210D8, + 0xD9E7BA7E, 0x7A33248F, + 0xD9B7C093, 0x7A24256E, + 0xD987CC8F, 0x7A151377, + 0xD957DE7A, 0x7A05EEAD, + 0xD927F65B, 0x79F6B711, + 0xD8F81439, 0x79E76CA6, + 0xD8C8381C, 0x79D80F6F, + 0xD898620C, 0x79C89F6D, + 0xD868920F, 0x79B91CA4, + 0xD838C82D, 0x79A98715, + 0xD809046D, 0x7999DEC3, + 0xD7D946D7, 0x798A23B1, + 0xD7A98F73, 0x797A55E0, + 0xD779DE46, 0x796A7554, + 0xD74A335A, 0x795A820E, + 0xD71A8EB5, 0x794A7C11, + 0xD6EAF05E, 0x793A6360, + 0xD6BB585D, 0x792A37FE, + 0xD68BC6BA, 0x7919F9EB, + 0xD65C3B7B, 0x7909A92C, + 0xD62CB6A7, 0x78F945C3, + 0xD5FD3847, 0x78E8CFB1, + 0xD5CDC062, 0x78D846FB, + 0xD59E4EFE, 0x78C7ABA1, + 0xD56EE424, 0x78B6FDA8, + 0xD53F7FDA, 0x78A63D10, + 0xD5102227, 0x789569DE, + 0xD4E0CB14, 0x78848413, + 0xD4B17AA7, 0x78738BB3, + 0xD48230E8, 0x786280BF, + 0xD452EDDE, 0x7851633B, + 0xD423B190, 0x78403328, + 0xD3F47C06, 0x782EF08B, + 0xD3C54D46, 0x781D9B64, + 0xD3962559, 0x780C33B8, + 0xD3670445, 0x77FAB988, + 0xD337EA12, 0x77E92CD8, + 0xD308D6C6, 0x77D78DAA, + 0xD2D9CA6A, 0x77C5DC01, + 0xD2AAC504, 0x77B417DF, + 0xD27BC69C, 0x77A24148, + 0xD24CCF38, 0x7790583D, + 0xD21DDEE1, 0x777E5CC3, + 0xD1EEF59E, 0x776C4EDB, + 0xD1C01374, 0x775A2E88, + 0xD191386D, 0x7747FBCE, + 0xD162648F, 0x7735B6AE, + 0xD13397E1, 0x77235F2D, + 0xD104D26B, 0x7710F54B, + 0xD0D61433, 0x76FE790E, + 0xD0A75D42, 0x76EBEA77, + 0xD078AD9D, 0x76D94988, + 0xD04A054D, 0x76C69646, + 0xD01B6459, 0x76B3D0B3, + 0xCFECCAC7, 0x76A0F8D2, + 0xCFBE389F, 0x768E0EA5, + 0xCF8FADE8, 0x767B1230, + 0xCF612AAA, 0x76680376, + 0xCF32AEEB, 0x7654E279, + 0xCF043AB2, 0x7641AF3C, + 0xCED5CE08, 0x762E69C3, + 0xCEA768F2, 0x761B1211, + 0xCE790B78, 0x7607A827, + 0xCE4AB5A2, 0x75F42C0A, + 0xCE1C6776, 0x75E09DBD, + 0xCDEE20FC, 0x75CCFD42, + 0xCDBFE23A, 0x75B94A9C, + 0xCD91AB38, 0x75A585CF, + 0xCD637BFD, 0x7591AEDD, + 0xCD355490, 0x757DC5CA, + 0xCD0734F8, 0x7569CA98, + 0xCCD91D3D, 0x7555BD4B, + 0xCCAB0D65, 0x75419DE6, + 0xCC7D0577, 0x752D6C6C, + 0xCC4F057B, 0x751928E0, + 0xCC210D78, 0x7504D345, + 0xCBF31D75, 0x74F06B9E, + 0xCBC53578, 0x74DBF1EF, + 0xCB975589, 0x74C7663A, + 0xCB697DB0, 0x74B2C883, + 0xCB3BADF2, 0x749E18CD, + 0xCB0DE658, 0x7489571B, + 0xCAE026E8, 0x74748371, + 0xCAB26FA9, 0x745F9DD1, + 0xCA84C0A2, 0x744AA63E, + 0xCA5719DB, 0x74359CBD, + 0xCA297B5A, 0x74208150, + 0xC9FBE527, 0x740B53FA, + 0xC9CE5748, 0x73F614C0, + 0xC9A0D1C4, 0x73E0C3A3, + 0xC97354A3, 0x73CB60A7, + 0xC945DFEC, 0x73B5EBD0, + 0xC91873A5, 0x73A06522, + 0xC8EB0FD6, 0x738ACC9E, + 0xC8BDB485, 0x73752249, + 0xC89061BA, 0x735F6626, + 0xC863177B, 0x73499838, + 0xC835D5D0, 0x7333B883, + 0xC8089CBF, 0x731DC709, + 0xC7DB6C50, 0x7307C3D0, + 0xC7AE4489, 0x72F1AED8, + 0xC7812571, 0x72DB8828, + 0xC7540F10, 0x72C54FC0, + 0xC727016C, 0x72AF05A6, + 0xC6F9FC8D, 0x7298A9DC, + 0xC6CD0079, 0x72823C66, + 0xC6A00D36, 0x726BBD48, + 0xC67322CD, 0x72552C84, + 0xC6464144, 0x723E8A1F, + 0xC61968A2, 0x7227D61C, + 0xC5EC98ED, 0x7211107D, + 0xC5BFD22E, 0x71FA3948, + 0xC593146A, 0x71E3507F, + 0xC5665FA8, 0x71CC5626, + 0xC539B3F0, 0x71B54A40, + 0xC50D1148, 0x719E2CD2, + 0xC4E077B8, 0x7186FDDE, + 0xC4B3E746, 0x716FBD68, + 0xC4875FF8, 0x71586B73, + 0xC45AE1D7, 0x71410804, + 0xC42E6CE8, 0x7129931E, + 0xC4020132, 0x71120CC5, + 0xC3D59EBD, 0x70FA74FB, + 0xC3A9458F, 0x70E2CBC6, + 0xC37CF5B0, 0x70CB1127, + 0xC350AF25, 0x70B34524, + 0xC32471F6, 0x709B67C0, + 0xC2F83E2A, 0x708378FE, + 0xC2CC13C7, 0x706B78E3, + 0xC29FF2D4, 0x70536771, + 0xC273DB58, 0x703B44AC, + 0xC247CD5A, 0x70231099, + 0xC21BC8E0, 0x700ACB3B, + 0xC1EFCDF2, 0x6FF27496, + 0xC1C3DC96, 0x6FDA0CAD, + 0xC197F4D3, 0x6FC19385, + 0xC16C16B0, 0x6FA90920, + 0xC1404233, 0x6F906D84, + 0xC1147763, 0x6F77C0B3, + 0xC0E8B648, 0x6F5F02B1, + 0xC0BCFEE7, 0x6F463383, + 0xC0915147, 0x6F2D532C, + 0xC065AD70, 0x6F1461AF, + 0xC03A1368, 0x6EFB5F12, + 0xC00E8335, 0x6EE24B57, + 0xBFE2FCDF, 0x6EC92682, + 0xBFB7806C, 0x6EAFF098, + 0xBF8C0DE2, 0x6E96A99C, + 0xBF60A54A, 0x6E7D5193, + 0xBF3546A8, 0x6E63E87F, + 0xBF09F204, 0x6E4A6E65, + 0xBEDEA765, 0x6E30E349, + 0xBEB366D1, 0x6E17472F, + 0xBE88304F, 0x6DFD9A1B, + 0xBE5D03E5, 0x6DE3DC11, + 0xBE31E19B, 0x6DCA0D14, + 0xBE06C977, 0x6DB02D29, + 0xBDDBBB7F, 0x6D963C54, + 0xBDB0B7BA, 0x6D7C3A98, + 0xBD85BE2F, 0x6D6227FA, + 0xBD5ACEE5, 0x6D48047E, + 0xBD2FE9E1, 0x6D2DD027, + 0xBD050F2C, 0x6D138AFA, + 0xBCDA3ECA, 0x6CF934FB, + 0xBCAF78C3, 0x6CDECE2E, + 0xBC84BD1E, 0x6CC45697, + 0xBC5A0BE1, 0x6CA9CE3A, + 0xBC2F6513, 0x6C8F351C, + 0xBC04C8BA, 0x6C748B3F, + 0xBBDA36DC, 0x6C59D0A9, + 0xBBAFAF81, 0x6C3F055D, + 0xBB8532AF, 0x6C242960, + 0xBB5AC06C, 0x6C093CB6, + 0xBB3058C0, 0x6BEE3F62, + 0xBB05FBB0, 0x6BD3316A, + 0xBADBA943, 0x6BB812D0, + 0xBAB1617F, 0x6B9CE39B, + 0xBA87246C, 0x6B81A3CD, + 0xBA5CF210, 0x6B66536A, + 0xBA32CA70, 0x6B4AF278, + 0xBA08AD94, 0x6B2F80FA, + 0xB9DE9B83, 0x6B13FEF5, + 0xB9B49442, 0x6AF86C6C, + 0xB98A97D8, 0x6ADCC964, + 0xB960A64B, 0x6AC115E1, + 0xB936BFA3, 0x6AA551E8, + 0xB90CE3E6, 0x6A897D7D, + 0xB8E31319, 0x6A6D98A4, + 0xB8B94D44, 0x6A51A361, + 0xB88F926C, 0x6A359DB9, + 0xB865E299, 0x6A1987B0, + 0xB83C3DD1, 0x69FD614A, + 0xB812A419, 0x69E12A8C, + 0xB7E9157A, 0x69C4E37A, + 0xB7BF91F8, 0x69A88C18, + 0xB796199B, 0x698C246C, + 0xB76CAC68, 0x696FAC78, + 0xB7434A67, 0x69532442, + 0xB719F39D, 0x69368BCE, + 0xB6F0A811, 0x6919E320, + 0xB6C767CA, 0x68FD2A3D, + 0xB69E32CD, 0x68E06129, + 0xB6750921, 0x68C387E9, + 0xB64BEACC, 0x68A69E81, + 0xB622D7D5, 0x6889A4F5, + 0xB5F9D042, 0x686C9B4B, + 0xB5D0D41A, 0x684F8186, + 0xB5A7E362, 0x683257AA, + 0xB57EFE21, 0x68151DBE, + 0xB556245E, 0x67F7D3C4, + 0xB52D561E, 0x67DA79C2, + 0xB5049368, 0x67BD0FBC, + 0xB4DBDC42, 0x679F95B7, + 0xB4B330B2, 0x67820BB6, + 0xB48A90C0, 0x676471C0, + 0xB461FC70, 0x6746C7D7, + 0xB43973C9, 0x67290E02, + 0xB410F6D2, 0x670B4443, + 0xB3E88591, 0x66ED6AA1, + 0xB3C0200C, 0x66CF811F, + 0xB397C649, 0x66B187C3, + 0xB36F784E, 0x66937E90, + 0xB3473622, 0x6675658C, + 0xB31EFFCB, 0x66573CBB, + 0xB2F6D54F, 0x66390422, + 0xB2CEB6B5, 0x661ABBC5, + 0xB2A6A401, 0x65FC63A9, + 0xB27E9D3B, 0x65DDFBD3, + 0xB256A26A, 0x65BF8447, + 0xB22EB392, 0x65A0FD0B, + 0xB206D0BA, 0x65826622, + 0xB1DEF9E8, 0x6563BF92, + 0xB1B72F23, 0x6545095F, + 0xB18F7070, 0x6526438E, + 0xB167BDD6, 0x65076E24, + 0xB140175B, 0x64E88926, + 0xB1187D05, 0x64C99498, + 0xB0F0EEDA, 0x64AA907F, + 0xB0C96CDF, 0x648B7CDF, + 0xB0A1F71C, 0x646C59BF, + 0xB07A8D97, 0x644D2722, + 0xB0533055, 0x642DE50D, + 0xB02BDF5C, 0x640E9385, + 0xB0049AB2, 0x63EF328F, + 0xAFDD625F, 0x63CFC230, + 0xAFB63667, 0x63B0426D, + 0xAF8F16D0, 0x6390B34A, + 0xAF6803A1, 0x637114CC, + 0xAF40FCE0, 0x635166F8, + 0xAF1A0293, 0x6331A9D4, + 0xAEF314BF, 0x6311DD63, + 0xAECC336B, 0x62F201AC, + 0xAEA55E9D, 0x62D216B2, + 0xAE7E965B, 0x62B21C7B, + 0xAE57DAAA, 0x6292130C, + 0xAE312B91, 0x6271FA69, + 0xAE0A8916, 0x6251D297, + 0xADE3F33E, 0x62319B9D, + 0xADBD6A10, 0x6211557D, + 0xAD96ED91, 0x61F1003E, + 0xAD707DC8, 0x61D09BE5, + 0xAD4A1ABA, 0x61B02876, + 0xAD23C46D, 0x618FA5F6, + 0xACFD7AE8, 0x616F146B, + 0xACD73E30, 0x614E73D9, + 0xACB10E4A, 0x612DC446, + 0xAC8AEB3E, 0x610D05B7, + 0xAC64D510, 0x60EC3830, + 0xAC3ECBC7, 0x60CB5BB6, + 0xAC18CF68, 0x60AA704F, + 0xABF2DFFA, 0x60897600, + 0xABCCFD82, 0x60686CCE, + 0xABA72806, 0x604754BE, + 0xAB815F8C, 0x60262DD5, + 0xAB5BA41A, 0x6004F818, + 0xAB35F5B5, 0x5FE3B38D, + 0xAB105464, 0x5FC26038, + 0xAAEAC02B, 0x5FA0FE1E, + 0xAAC53912, 0x5F7F8D46, + 0xAA9FBF1D, 0x5F5E0DB3, + 0xAA7A5253, 0x5F3C7F6B, + 0xAA54F2B9, 0x5F1AE273, + 0xAA2FA055, 0x5EF936D1, + 0xAA0A5B2D, 0x5ED77C89, + 0xA9E52347, 0x5EB5B3A1, + 0xA9BFF8A8, 0x5E93DC1F, + 0xA99ADB56, 0x5E71F606, + 0xA975CB56, 0x5E50015D, + 0xA950C8AF, 0x5E2DFE28, + 0xA92BD366, 0x5E0BEC6E, + 0xA906EB81, 0x5DE9CC32, + 0xA8E21106, 0x5DC79D7C, + 0xA8BD43FA, 0x5DA5604E, + 0xA8988463, 0x5D8314B0, + 0xA873D246, 0x5D60BAA6, + 0xA84F2DA9, 0x5D3E5236, + 0xA82A9693, 0x5D1BDB65, + 0xA8060D08, 0x5CF95638, + 0xA7E1910E, 0x5CD6C2B4, + 0xA7BD22AB, 0x5CB420DF, + 0xA798C1E4, 0x5C9170BF, + 0xA7746EC0, 0x5C6EB258, + 0xA7502943, 0x5C4BE5B0, + 0xA72BF173, 0x5C290ACC, + 0xA707C756, 0x5C0621B2, + 0xA6E3AAF2, 0x5BE32A67, + 0xA6BF9C4B, 0x5BC024F0, + 0xA69B9B68, 0x5B9D1153, + 0xA677A84E, 0x5B79EF96, + 0xA653C302, 0x5B56BFBD, + 0xA62FEB8B, 0x5B3381CE, + 0xA60C21ED, 0x5B1035CF, + 0xA5E8662F, 0x5AECDBC4, + 0xA5C4B855, 0x5AC973B4, + 0xA5A11865, 0x5AA5FDA4, + 0xA57D8666, 0x5A82799A, + 0xA55A025B, 0x5A5EE79A, + 0xA5368C4B, 0x5A3B47AA, + 0xA513243B, 0x5A1799D0, + 0xA4EFCA31, 0x59F3DE12, + 0xA4CC7E31, 0x59D01474, + 0xA4A94042, 0x59AC3CFD, + 0xA4861069, 0x598857B1, + 0xA462EEAC, 0x59646497, + 0xA43FDB0F, 0x594063B4, + 0xA41CD598, 0x591C550E, + 0xA3F9DE4D, 0x58F838A9, + 0xA3D6F533, 0x58D40E8C, + 0xA3B41A4F, 0x58AFD6BC, + 0xA3914DA7, 0x588B913F, + 0xA36E8F40, 0x58673E1B, + 0xA34BDF20, 0x5842DD54, + 0xA3293D4B, 0x581E6EF1, + 0xA306A9C7, 0x57F9F2F7, + 0xA2E4249A, 0x57D5696C, + 0xA2C1ADC9, 0x57B0D256, + 0xA29F4559, 0x578C2DB9, + 0xA27CEB4F, 0x57677B9D, + 0xA25A9FB1, 0x5742BC05, + 0xA2386283, 0x571DEEF9, + 0xA21633CD, 0x56F9147E, + 0xA1F41391, 0x56D42C99, + 0xA1D201D7, 0x56AF3750, + 0xA1AFFEA2, 0x568A34A9, + 0xA18E09F9, 0x566524AA, + 0xA16C23E1, 0x56400757, + 0xA14A4C5E, 0x561ADCB8, + 0xA1288376, 0x55F5A4D2, + 0xA106C92E, 0x55D05FAA, + 0xA0E51D8C, 0x55AB0D46, + 0xA0C38094, 0x5585ADAC, + 0xA0A1F24C, 0x556040E2, + 0xA08072BA, 0x553AC6ED, + 0xA05F01E1, 0x55153FD4, + 0xA03D9FC7, 0x54EFAB9C, + 0xA01C4C72, 0x54CA0A4A, + 0x9FFB07E7, 0x54A45BE5, + 0x9FD9D22A, 0x547EA073, + 0x9FB8AB41, 0x5458D7F9, + 0x9F979331, 0x5433027D, + 0x9F7689FF, 0x540D2005, + 0x9F558FB0, 0x53E73097, + 0x9F34A449, 0x53C13438, + 0x9F13C7D0, 0x539B2AEF, + 0x9EF2FA48, 0x537514C1, + 0x9ED23BB9, 0x534EF1B5, + 0x9EB18C26, 0x5328C1D0, + 0x9E90EB94, 0x53028517, + 0x9E705A09, 0x52DC3B92, + 0x9E4FD789, 0x52B5E545, + 0x9E2F641A, 0x528F8237, + 0x9E0EFFC1, 0x5269126E, + 0x9DEEAA82, 0x524295EF, + 0x9DCE6462, 0x521C0CC1, + 0x9DAE2D68, 0x51F576E9, + 0x9D8E0596, 0x51CED46E, + 0x9D6DECF4, 0x51A82555, + 0x9D4DE384, 0x518169A4, + 0x9D2DE94D, 0x515AA162, + 0x9D0DFE53, 0x5133CC94, + 0x9CEE229C, 0x510CEB40, + 0x9CCE562B, 0x50E5FD6C, + 0x9CAE9907, 0x50BF031F, + 0x9C8EEB33, 0x5097FC5E, + 0x9C6F4CB5, 0x5070E92F, + 0x9C4FBD92, 0x5049C999, + 0x9C303DCF, 0x50229DA0, + 0x9C10CD70, 0x4FFB654D, + 0x9BF16C7A, 0x4FD420A3, + 0x9BD21AF2, 0x4FACCFAB, + 0x9BB2D8DD, 0x4F857268, + 0x9B93A640, 0x4F5E08E3, + 0x9B748320, 0x4F369320, + 0x9B556F80, 0x4F0F1126, + 0x9B366B67, 0x4EE782FA, + 0x9B1776D9, 0x4EBFE8A4, + 0x9AF891DB, 0x4E984229, + 0x9AD9BC71, 0x4E708F8F, + 0x9ABAF6A0, 0x4E48D0DC, + 0x9A9C406D, 0x4E210617, + 0x9A7D99DD, 0x4DF92F45, + 0x9A5F02F5, 0x4DD14C6E, + 0x9A407BB8, 0x4DA95D96, + 0x9A22042C, 0x4D8162C4, + 0x9A039C56, 0x4D595BFE, + 0x99E5443A, 0x4D31494B, + 0x99C6FBDE, 0x4D092AB0, + 0x99A8C344, 0x4CE10034, + 0x998A9A73, 0x4CB8C9DD, + 0x996C816F, 0x4C9087B1, + 0x994E783C, 0x4C6839B6, + 0x99307EE0, 0x4C3FDFF3, + 0x9912955E, 0x4C177A6E, + 0x98F4BBBC, 0x4BEF092D, + 0x98D6F1FE, 0x4BC68C36, + 0x98B93828, 0x4B9E038F, + 0x989B8E3F, 0x4B756F3F, + 0x987DF449, 0x4B4CCF4D, + 0x98606A48, 0x4B2423BD, + 0x9842F043, 0x4AFB6C97, + 0x9825863D, 0x4AD2A9E1, + 0x98082C3B, 0x4AA9DBA1, + 0x97EAE241, 0x4A8101DE, + 0x97CDA855, 0x4A581C9D, + 0x97B07E7A, 0x4A2F2BE5, + 0x979364B5, 0x4A062FBD, + 0x97765B0A, 0x49DD282A, + 0x9759617E, 0x49B41533, + 0x973C7816, 0x498AF6DE, + 0x971F9ED6, 0x4961CD32, + 0x9702D5C2, 0x49389836, + 0x96E61CDF, 0x490F57EE, + 0x96C97431, 0x48E60C62, + 0x96ACDBBD, 0x48BCB598, + 0x96905387, 0x48935397, + 0x9673DB94, 0x4869E664, + 0x965773E7, 0x48406E07, + 0x963B1C85, 0x4816EA85, + 0x961ED573, 0x47ED5BE6, + 0x96029EB5, 0x47C3C22E, + 0x95E6784F, 0x479A1D66, + 0x95CA6246, 0x47706D93, + 0x95AE5C9E, 0x4746B2BC, + 0x9592675B, 0x471CECE6, + 0x95768282, 0x46F31C1A, + 0x955AAE17, 0x46C9405C, + 0x953EEA1E, 0x469F59B4, + 0x9523369B, 0x46756827, + 0x95079393, 0x464B6BBD, + 0x94EC010B, 0x4621647C, + 0x94D07F05, 0x45F7526B, + 0x94B50D87, 0x45CD358F, + 0x9499AC95, 0x45A30DF0, + 0x947E5C32, 0x4578DB93, + 0x94631C64, 0x454E9E80, + 0x9447ED2F, 0x452456BC, + 0x942CCE95, 0x44FA044F, + 0x9411C09D, 0x44CFA73F, + 0x93F6C34A, 0x44A53F93, + 0x93DBD69F, 0x447ACD50, + 0x93C0FAA2, 0x4450507E, + 0x93A62F56, 0x4425C923, + 0x938B74C0, 0x43FB3745, + 0x9370CAE4, 0x43D09AEC, + 0x935631C5, 0x43A5F41E, + 0x933BA968, 0x437B42E1, + 0x932131D1, 0x4350873C, + 0x9306CB04, 0x4325C135, + 0x92EC7505, 0x42FAF0D4, + 0x92D22FD8, 0x42D0161E, + 0x92B7FB82, 0x42A5311A, + 0x929DD805, 0x427A41D0, + 0x9283C567, 0x424F4845, + 0x9269C3AC, 0x42244480, + 0x924FD2D6, 0x41F93688, + 0x9235F2EB, 0x41CE1E64, + 0x921C23EE, 0x41A2FC1A, + 0x920265E4, 0x4177CFB0, + 0x91E8B8D0, 0x414C992E, + 0x91CF1CB6, 0x4121589A, + 0x91B5919A, 0x40F60DFB, + 0x919C1780, 0x40CAB957, + 0x9182AE6C, 0x409F5AB6, + 0x91695663, 0x4073F21D, + 0x91500F67, 0x40487F93, + 0x9136D97D, 0x401D0320, + 0x911DB4A8, 0x3FF17CCA, + 0x9104A0ED, 0x3FC5EC97, + 0x90EB9E50, 0x3F9A528F, + 0x90D2ACD3, 0x3F6EAEB8, + 0x90B9CC7C, 0x3F430118, + 0x90A0FD4E, 0x3F1749B7, + 0x90883F4C, 0x3EEB889C, + 0x906F927B, 0x3EBFBDCC, + 0x9056F6DF, 0x3E93E94F, + 0x903E6C7A, 0x3E680B2C, + 0x9025F352, 0x3E3C2369, + 0x900D8B69, 0x3E10320D, + 0x8FF534C4, 0x3DE4371F, + 0x8FDCEF66, 0x3DB832A5, + 0x8FC4BB53, 0x3D8C24A7, + 0x8FAC988E, 0x3D600D2B, + 0x8F94871D, 0x3D33EC39, + 0x8F7C8701, 0x3D07C1D5, + 0x8F64983F, 0x3CDB8E09, + 0x8F4CBADB, 0x3CAF50DA, + 0x8F34EED8, 0x3C830A4F, + 0x8F1D343A, 0x3C56BA70, + 0x8F058B04, 0x3C2A6142, + 0x8EEDF33B, 0x3BFDFECD, + 0x8ED66CE1, 0x3BD19317, + 0x8EBEF7FB, 0x3BA51E29, + 0x8EA7948C, 0x3B78A007, + 0x8E904298, 0x3B4C18BA, + 0x8E790222, 0x3B1F8847, + 0x8E61D32D, 0x3AF2EEB7, + 0x8E4AB5BF, 0x3AC64C0F, + 0x8E33A9D9, 0x3A99A057, + 0x8E1CAF80, 0x3A6CEB95, + 0x8E05C6B7, 0x3A402DD1, + 0x8DEEEF82, 0x3A136712, + 0x8DD829E4, 0x39E6975D, + 0x8DC175E0, 0x39B9BEBB, + 0x8DAAD37B, 0x398CDD32, + 0x8D9442B7, 0x395FF2C9, + 0x8D7DC399, 0x3932FF87, + 0x8D675623, 0x39060372, + 0x8D50FA59, 0x38D8FE93, + 0x8D3AB03F, 0x38ABF0EF, + 0x8D2477D8, 0x387EDA8E, + 0x8D0E5127, 0x3851BB76, + 0x8CF83C30, 0x382493B0, + 0x8CE238F6, 0x37F76340, + 0x8CCC477D, 0x37CA2A30, + 0x8CB667C7, 0x379CE884, + 0x8CA099D9, 0x376F9E46, + 0x8C8ADDB6, 0x37424B7A, + 0x8C753361, 0x3714F02A, + 0x8C5F9ADD, 0x36E78C5A, + 0x8C4A142F, 0x36BA2013, + 0x8C349F58, 0x368CAB5C, + 0x8C1F3C5C, 0x365F2E3B, + 0x8C09EB40, 0x3631A8B7, + 0x8BF4AC05, 0x36041AD9, + 0x8BDF7EAF, 0x35D684A5, + 0x8BCA6342, 0x35A8E624, + 0x8BB559C1, 0x357B3F5D, + 0x8BA0622F, 0x354D9056, + 0x8B8B7C8F, 0x351FD917, + 0x8B76A8E4, 0x34F219A7, + 0x8B61E732, 0x34C4520D, + 0x8B4D377C, 0x3496824F, + 0x8B3899C5, 0x3468AA76, + 0x8B240E10, 0x343ACA87, + 0x8B0F9461, 0x340CE28A, + 0x8AFB2CBA, 0x33DEF287, + 0x8AE6D71F, 0x33B0FA84, + 0x8AD29393, 0x3382FA88, + 0x8ABE6219, 0x3354F29A, + 0x8AAA42B4, 0x3326E2C2, + 0x8A963567, 0x32F8CB07, + 0x8A823A35, 0x32CAAB6F, + 0x8A6E5122, 0x329C8402, + 0x8A5A7A30, 0x326E54C7, + 0x8A46B563, 0x32401DC5, + 0x8A3302BD, 0x3211DF03, + 0x8A1F6242, 0x31E39889, + 0x8A0BD3F5, 0x31B54A5D, + 0x89F857D8, 0x3186F487, + 0x89E4EDEE, 0x3158970D, + 0x89D1963C, 0x312A31F8, + 0x89BE50C3, 0x30FBC54D, + 0x89AB1D86, 0x30CD5114, + 0x8997FC89, 0x309ED555, + 0x8984EDCF, 0x30705217, + 0x8971F15A, 0x3041C760, + 0x895F072D, 0x30133538, + 0x894C2F4C, 0x2FE49BA6, + 0x893969B9, 0x2FB5FAB2, + 0x8926B677, 0x2F875262, + 0x89141589, 0x2F58A2BD, + 0x890186F1, 0x2F29EBCC, + 0x88EF0AB4, 0x2EFB2D94, + 0x88DCA0D3, 0x2ECC681E, + 0x88CA4951, 0x2E9D9B70, + 0x88B80431, 0x2E6EC792, + 0x88A5D177, 0x2E3FEC8B, + 0x8893B124, 0x2E110A62, + 0x8881A33C, 0x2DE2211E, + 0x886FA7C2, 0x2DB330C7, + 0x885DBEB7, 0x2D843963, + 0x884BE820, 0x2D553AFB, + 0x883A23FE, 0x2D263595, + 0x88287255, 0x2CF72939, + 0x8816D327, 0x2CC815ED, + 0x88054677, 0x2C98FBBA, + 0x87F3CC47, 0x2C69DAA6, + 0x87E2649B, 0x2C3AB2B9, + 0x87D10F75, 0x2C0B83F9, + 0x87BFCCD7, 0x2BDC4E6F, + 0x87AE9CC5, 0x2BAD1221, + 0x879D7F40, 0x2B7DCF17, + 0x878C744C, 0x2B4E8558, + 0x877B7BEC, 0x2B1F34EB, + 0x876A9621, 0x2AEFDDD8, + 0x8759C2EF, 0x2AC08025, + 0x87490257, 0x2A911BDB, + 0x8738545E, 0x2A61B101, + 0x8727B904, 0x2A323F9D, + 0x8717304E, 0x2A02C7B8, + 0x8706BA3C, 0x29D34958, + 0x86F656D3, 0x29A3C484, + 0x86E60614, 0x29743945, + 0x86D5C802, 0x2944A7A2, + 0x86C59C9F, 0x29150FA1, + 0x86B583EE, 0x28E5714A, + 0x86A57DF1, 0x28B5CCA5, + 0x86958AAB, 0x288621B9, + 0x8685AA1F, 0x2856708C, + 0x8675DC4E, 0x2826B928, + 0x8666213C, 0x27F6FB92, + 0x865678EA, 0x27C737D2, + 0x8646E35B, 0x27976DF1, + 0x86376092, 0x27679DF4, + 0x8627F090, 0x2737C7E3, + 0x86189359, 0x2707EBC6, + 0x860948EE, 0x26D809A5, + 0x85FA1152, 0x26A82185, + 0x85EAEC88, 0x26783370, + 0x85DBDA91, 0x26483F6C, + 0x85CCDB70, 0x26184581, + 0x85BDEF27, 0x25E845B5, + 0x85AF15B9, 0x25B84012, + 0x85A04F28, 0x2588349D, + 0x85919B75, 0x2558235E, + 0x8582FAA4, 0x25280C5D, + 0x85746CB7, 0x24F7EFA1, + 0x8565F1B0, 0x24C7CD32, + 0x85578991, 0x2497A517, + 0x8549345C, 0x24677757, + 0x853AF214, 0x243743FA, + 0x852CC2BA, 0x24070B07, + 0x851EA652, 0x23D6CC86, + 0x85109CDC, 0x23A6887E, + 0x8502A65C, 0x23763EF7, + 0x84F4C2D3, 0x2345EFF7, + 0x84E6F244, 0x23159B87, + 0x84D934B0, 0x22E541AE, + 0x84CB8A1B, 0x22B4E274, + 0x84BDF285, 0x22847DDF, + 0x84B06DF1, 0x225413F8, + 0x84A2FC62, 0x2223A4C5, + 0x84959DD9, 0x21F3304E, + 0x84885257, 0x21C2B69C, + 0x847B19E1, 0x219237B4, + 0x846DF476, 0x2161B39F, + 0x8460E21A, 0x21312A65, + 0x8453E2CE, 0x21009C0B, + 0x8446F695, 0x20D0089B, + 0x843A1D70, 0x209F701C, + 0x842D5761, 0x206ED295, + 0x8420A46B, 0x203E300D, + 0x8414048F, 0x200D888C, + 0x840777CF, 0x1FDCDC1A, + 0x83FAFE2E, 0x1FAC2ABF, + 0x83EE97AC, 0x1F7B7480, + 0x83E2444D, 0x1F4AB967, + 0x83D60411, 0x1F19F97B, + 0x83C9D6FB, 0x1EE934C2, + 0x83BDBD0D, 0x1EB86B46, + 0x83B1B649, 0x1E879D0C, + 0x83A5C2B0, 0x1E56CA1E, + 0x8399E244, 0x1E25F281, + 0x838E1507, 0x1DF5163F, + 0x83825AFB, 0x1DC4355D, + 0x8376B422, 0x1D934FE5, + 0x836B207D, 0x1D6265DD, + 0x835FA00E, 0x1D31774D, + 0x835432D8, 0x1D00843C, + 0x8348D8DB, 0x1CCF8CB3, + 0x833D921A, 0x1C9E90B8, + 0x83325E97, 0x1C6D9053, + 0x83273E52, 0x1C3C8B8C, + 0x831C314E, 0x1C0B826A, + 0x8311378C, 0x1BDA74F5, + 0x8306510F, 0x1BA96334, + 0x82FB7DD8, 0x1B784D30, + 0x82F0BDE8, 0x1B4732EF, + 0x82E61141, 0x1B161479, + 0x82DB77E5, 0x1AE4F1D6, + 0x82D0F1D5, 0x1AB3CB0C, + 0x82C67F13, 0x1A82A025, + 0x82BC1FA1, 0x1A517127, + 0x82B1D381, 0x1A203E1B, + 0x82A79AB3, 0x19EF0706, + 0x829D753A, 0x19BDCBF2, + 0x82936316, 0x198C8CE6, + 0x8289644A, 0x195B49E9, + 0x827F78D8, 0x192A0303, + 0x8275A0C0, 0x18F8B83C, + 0x826BDC04, 0x18C7699B, + 0x82622AA5, 0x18961727, + 0x82588CA6, 0x1864C0E9, + 0x824F0208, 0x183366E8, + 0x82458ACB, 0x1802092C, + 0x823C26F2, 0x17D0A7BB, + 0x8232D67E, 0x179F429F, + 0x82299971, 0x176DD9DE, + 0x82206FCB, 0x173C6D80, + 0x8217598F, 0x170AFD8D, + 0x820E56BE, 0x16D98A0C, + 0x82056758, 0x16A81305, + 0x81FC8B60, 0x1676987F, + 0x81F3C2D7, 0x16451A83, + 0x81EB0DBD, 0x16139917, + 0x81E26C16, 0x15E21444, + 0x81D9DDE1, 0x15B08C11, + 0x81D16320, 0x157F0086, + 0x81C8FBD5, 0x154D71AA, + 0x81C0A801, 0x151BDF85, + 0x81B867A4, 0x14EA4A1F, + 0x81B03AC1, 0x14B8B17F, + 0x81A82159, 0x148715AD, + 0x81A01B6C, 0x145576B1, + 0x819828FD, 0x1423D492, + 0x81904A0C, 0x13F22F57, + 0x81887E9A, 0x13C0870A, + 0x8180C6A9, 0x138EDBB0, + 0x8179223A, 0x135D2D53, + 0x8171914E, 0x132B7BF9, + 0x816A13E6, 0x12F9C7AA, + 0x8162AA03, 0x12C8106E, + 0x815B53A8, 0x1296564D, + 0x815410D3, 0x1264994E, + 0x814CE188, 0x1232D978, + 0x8145C5C6, 0x120116D4, + 0x813EBD90, 0x11CF516A, + 0x8137C8E6, 0x119D8940, + 0x8130E7C8, 0x116BBE5F, + 0x812A1A39, 0x1139F0CE, + 0x81236039, 0x11082096, + 0x811CB9CA, 0x10D64DBC, + 0x811626EC, 0x10A4784A, + 0x810FA7A0, 0x1072A047, + 0x81093BE8, 0x1040C5BB, + 0x8102E3C3, 0x100EE8AD, + 0x80FC9F35, 0x0FDD0925, + 0x80F66E3C, 0x0FAB272B, + 0x80F050DB, 0x0F7942C6, + 0x80EA4712, 0x0F475BFE, + 0x80E450E2, 0x0F1572DC, + 0x80DE6E4C, 0x0EE38765, + 0x80D89F51, 0x0EB199A3, + 0x80D2E3F1, 0x0E7FA99D, + 0x80CD3C2F, 0x0E4DB75B, + 0x80C7A80A, 0x0E1BC2E3, + 0x80C22783, 0x0DE9CC3F, + 0x80BCBA9C, 0x0DB7D376, + 0x80B76155, 0x0D85D88F, + 0x80B21BAF, 0x0D53DB92, + 0x80ACE9AB, 0x0D21DC87, + 0x80A7CB49, 0x0CEFDB75, + 0x80A2C08B, 0x0CBDD865, + 0x809DC970, 0x0C8BD35E, + 0x8098E5FB, 0x0C59CC67, + 0x8094162B, 0x0C27C389, + 0x808F5A02, 0x0BF5B8CB, + 0x808AB180, 0x0BC3AC35, + 0x80861CA5, 0x0B919DCE, + 0x80819B74, 0x0B5F8D9F, + 0x807D2DEB, 0x0B2D7BAE, + 0x8078D40D, 0x0AFB6805, + 0x80748DD9, 0x0AC952AA, + 0x80705B50, 0x0A973BA5, + 0x806C3C73, 0x0A6522FE, + 0x80683143, 0x0A3308BC, + 0x806439C0, 0x0A00ECE8, + 0x806055EA, 0x09CECF89, + 0x805C85C3, 0x099CB0A7, + 0x8058C94C, 0x096A9049, + 0x80552083, 0x09386E77, + 0x80518B6B, 0x09064B3A, + 0x804E0A03, 0x08D42698, + 0x804A9C4D, 0x08A2009A, + 0x80474248, 0x086FD947, + 0x8043FBF6, 0x083DB0A7, + 0x8040C956, 0x080B86C1, + 0x803DAA69, 0x07D95B9E, + 0x803A9F31, 0x07A72F45, + 0x8037A7AC, 0x077501BE, + 0x8034C3DC, 0x0742D310, + 0x8031F3C1, 0x0710A344, + 0x802F375C, 0x06DE7261, + 0x802C8EAD, 0x06AC406F, + 0x8029F9B4, 0x067A0D75, + 0x80277872, 0x0647D97C, + 0x80250AE7, 0x0615A48A, + 0x8022B113, 0x05E36EA9, + 0x80206AF8, 0x05B137DF, + 0x801E3894, 0x057F0034, + 0x801C19E9, 0x054CC7B0, + 0x801A0EF7, 0x051A8E5C, + 0x801817BF, 0x04E8543D, + 0x80163440, 0x04B6195D, + 0x8014647A, 0x0483DDC3, + 0x8012A86F, 0x0451A176, + 0x8011001E, 0x041F647F, + 0x800F6B88, 0x03ED26E6, + 0x800DEAAC, 0x03BAE8B1, + 0x800C7D8C, 0x0388A9E9, + 0x800B2427, 0x03566A96, + 0x8009DE7D, 0x03242ABF, + 0x8008AC90, 0x02F1EA6B, + 0x80078E5E, 0x02BFA9A4, + 0x800683E8, 0x028D6870, + 0x80058D2E, 0x025B26D7, + 0x8004AA31, 0x0228E4E1, + 0x8003DAF0, 0x01F6A296, + 0x80031F6C, 0x01C45FFE, + 0x800277A5, 0x01921D1F, + 0x8001E39B, 0x015FDA03, + 0x8001634D, 0x012D96B0, + 0x8000F6BD, 0x00FB532F, + 0x80009DE9, 0x00C90F88, + 0x800058D3, 0x0096CBC1, + 0x8000277A, 0x006487E3, + 0x800009DE, 0x003243F5, + 0x80000000, 0x00000000, + 0x800009DE, 0xFFCDBC0A, + 0x8000277A, 0xFF9B781D, + 0x800058D3, 0xFF69343E, + 0x80009DE9, 0xFF36F078, + 0x8000F6BD, 0xFF04ACD0, + 0x8001634D, 0xFED2694F, + 0x8001E39B, 0xFEA025FC, + 0x800277A5, 0xFE6DE2E0, + 0x80031F6C, 0xFE3BA001, + 0x8003DAF0, 0xFE095D69, + 0x8004AA31, 0xFDD71B1E, + 0x80058D2E, 0xFDA4D928, + 0x800683E8, 0xFD72978F, + 0x80078E5E, 0xFD40565B, + 0x8008AC90, 0xFD0E1594, + 0x8009DE7D, 0xFCDBD541, + 0x800B2427, 0xFCA99569, + 0x800C7D8C, 0xFC775616, + 0x800DEAAC, 0xFC45174E, + 0x800F6B88, 0xFC12D919, + 0x8011001E, 0xFBE09B80, + 0x8012A86F, 0xFBAE5E89, + 0x8014647A, 0xFB7C223C, + 0x80163440, 0xFB49E6A2, + 0x801817BF, 0xFB17ABC2, + 0x801A0EF7, 0xFAE571A4, + 0x801C19E9, 0xFAB3384F, + 0x801E3894, 0xFA80FFCB, + 0x80206AF8, 0xFA4EC820, + 0x8022B113, 0xFA1C9156, + 0x80250AE7, 0xF9EA5B75, + 0x80277872, 0xF9B82683, + 0x8029F9B4, 0xF985F28A, + 0x802C8EAD, 0xF953BF90, + 0x802F375C, 0xF9218D9E, + 0x8031F3C1, 0xF8EF5CBB, + 0x8034C3DC, 0xF8BD2CEF, + 0x8037A7AC, 0xF88AFE41, + 0x803A9F31, 0xF858D0BA, + 0x803DAA69, 0xF826A461, + 0x8040C956, 0xF7F4793E, + 0x8043FBF6, 0xF7C24F58, + 0x80474248, 0xF79026B8, + 0x804A9C4D, 0xF75DFF65, + 0x804E0A03, 0xF72BD967, + 0x80518B6B, 0xF6F9B4C5, + 0x80552083, 0xF6C79188, + 0x8058C94C, 0xF6956FB6, + 0x805C85C3, 0xF6634F58, + 0x806055EA, 0xF6313076, + 0x806439C0, 0xF5FF1317, + 0x80683143, 0xF5CCF743, + 0x806C3C73, 0xF59ADD01, + 0x80705B50, 0xF568C45A, + 0x80748DD9, 0xF536AD55, + 0x8078D40D, 0xF50497FA, + 0x807D2DEB, 0xF4D28451, + 0x80819B74, 0xF4A07260, + 0x80861CA5, 0xF46E6231, + 0x808AB180, 0xF43C53CA, + 0x808F5A02, 0xF40A4734, + 0x8094162B, 0xF3D83C76, + 0x8098E5FB, 0xF3A63398, + 0x809DC970, 0xF3742CA1, + 0x80A2C08B, 0xF342279A, + 0x80A7CB49, 0xF310248A, + 0x80ACE9AB, 0xF2DE2378, + 0x80B21BAF, 0xF2AC246D, + 0x80B76155, 0xF27A2770, + 0x80BCBA9C, 0xF2482C89, + 0x80C22783, 0xF21633C0, + 0x80C7A80A, 0xF1E43D1C, + 0x80CD3C2F, 0xF1B248A5, + 0x80D2E3F1, 0xF1805662, + 0x80D89F51, 0xF14E665C, + 0x80DE6E4C, 0xF11C789A, + 0x80E450E2, 0xF0EA8D23, + 0x80EA4712, 0xF0B8A401, + 0x80F050DB, 0xF086BD39, + 0x80F66E3C, 0xF054D8D4, + 0x80FC9F35, 0xF022F6DA, + 0x8102E3C3, 0xEFF11752, + 0x81093BE8, 0xEFBF3A44, + 0x810FA7A0, 0xEF8D5FB8, + 0x811626EC, 0xEF5B87B5, + 0x811CB9CA, 0xEF29B243, + 0x81236039, 0xEEF7DF6A, + 0x812A1A39, 0xEEC60F31, + 0x8130E7C8, 0xEE9441A0, + 0x8137C8E6, 0xEE6276BF, + 0x813EBD90, 0xEE30AE95, + 0x8145C5C6, 0xEDFEE92B, + 0x814CE188, 0xEDCD2687, + 0x815410D3, 0xED9B66B2, + 0x815B53A8, 0xED69A9B2, + 0x8162AA03, 0xED37EF91, + 0x816A13E6, 0xED063855, + 0x8171914E, 0xECD48406, + 0x8179223A, 0xECA2D2AC, + 0x8180C6A9, 0xEC71244F, + 0x81887E9A, 0xEC3F78F5, + 0x81904A0C, 0xEC0DD0A8, + 0x819828FD, 0xEBDC2B6D, + 0x81A01B6C, 0xEBAA894E, + 0x81A82159, 0xEB78EA52, + 0x81B03AC1, 0xEB474E80, + 0x81B867A4, 0xEB15B5E0, + 0x81C0A801, 0xEAE4207A, + 0x81C8FBD5, 0xEAB28E55, + 0x81D16320, 0xEA80FF79, + 0x81D9DDE1, 0xEA4F73EE, + 0x81E26C16, 0xEA1DEBBB, + 0x81EB0DBD, 0xE9EC66E8, + 0x81F3C2D7, 0xE9BAE57C, + 0x81FC8B60, 0xE9896780, + 0x82056758, 0xE957ECFB, + 0x820E56BE, 0xE92675F4, + 0x8217598F, 0xE8F50273, + 0x82206FCB, 0xE8C3927F, + 0x82299971, 0xE8922621, + 0x8232D67E, 0xE860BD60, + 0x823C26F2, 0xE82F5844, + 0x82458ACB, 0xE7FDF6D3, + 0x824F0208, 0xE7CC9917, + 0x82588CA6, 0xE79B3F16, + 0x82622AA5, 0xE769E8D8, + 0x826BDC04, 0xE7389664, + 0x8275A0C0, 0xE70747C3, + 0x827F78D8, 0xE6D5FCFC, + 0x8289644A, 0xE6A4B616, + 0x82936316, 0xE6737319, + 0x829D753A, 0xE642340D, + 0x82A79AB3, 0xE610F8F9, + 0x82B1D381, 0xE5DFC1E4, + 0x82BC1FA1, 0xE5AE8ED8, + 0x82C67F13, 0xE57D5FDA, + 0x82D0F1D5, 0xE54C34F3, + 0x82DB77E5, 0xE51B0E2A, + 0x82E61141, 0xE4E9EB86, + 0x82F0BDE8, 0xE4B8CD10, + 0x82FB7DD8, 0xE487B2CF, + 0x8306510F, 0xE4569CCB, + 0x8311378C, 0xE4258B0A, + 0x831C314E, 0xE3F47D95, + 0x83273E52, 0xE3C37473, + 0x83325E97, 0xE3926FAC, + 0x833D921A, 0xE3616F47, + 0x8348D8DB, 0xE330734C, + 0x835432D8, 0xE2FF7BC3, + 0x835FA00E, 0xE2CE88B2, + 0x836B207D, 0xE29D9A22, + 0x8376B422, 0xE26CB01A, + 0x83825AFB, 0xE23BCAA2, + 0x838E1507, 0xE20AE9C1, + 0x8399E244, 0xE1DA0D7E, + 0x83A5C2B0, 0xE1A935E1, + 0x83B1B649, 0xE17862F3, + 0x83BDBD0D, 0xE14794B9, + 0x83C9D6FB, 0xE116CB3D, + 0x83D60411, 0xE0E60684, + 0x83E2444D, 0xE0B54698, + 0x83EE97AC, 0xE0848B7F, + 0x83FAFE2E, 0xE053D541, + 0x840777CF, 0xE02323E5, + 0x8414048F, 0xDFF27773, + 0x8420A46B, 0xDFC1CFF2, + 0x842D5761, 0xDF912D6A, + 0x843A1D70, 0xDF608FE3, + 0x8446F695, 0xDF2FF764, + 0x8453E2CE, 0xDEFF63F4, + 0x8460E21A, 0xDECED59B, + 0x846DF476, 0xDE9E4C60, + 0x847B19E1, 0xDE6DC84B, + 0x84885257, 0xDE3D4963, + 0x84959DD9, 0xDE0CCFB1, + 0x84A2FC62, 0xDDDC5B3A, + 0x84B06DF1, 0xDDABEC07, + 0x84BDF285, 0xDD7B8220, + 0x84CB8A1B, 0xDD4B1D8B, + 0x84D934B0, 0xDD1ABE51, + 0x84E6F244, 0xDCEA6478, + 0x84F4C2D3, 0xDCBA1008, + 0x8502A65C, 0xDC89C108, + 0x85109CDC, 0xDC597781, + 0x851EA652, 0xDC293379, + 0x852CC2BA, 0xDBF8F4F8, + 0x853AF214, 0xDBC8BC05, + 0x8549345C, 0xDB9888A8, + 0x85578991, 0xDB685AE8, + 0x8565F1B0, 0xDB3832CD, + 0x85746CB7, 0xDB08105E, + 0x8582FAA4, 0xDAD7F3A2, + 0x85919B75, 0xDAA7DCA1, + 0x85A04F28, 0xDA77CB62, + 0x85AF15B9, 0xDA47BFED, + 0x85BDEF27, 0xDA17BA4A, + 0x85CCDB70, 0xD9E7BA7E, + 0x85DBDA91, 0xD9B7C093, + 0x85EAEC88, 0xD987CC8F, + 0x85FA1152, 0xD957DE7A, + 0x860948EE, 0xD927F65B, + 0x86189359, 0xD8F81439, + 0x8627F090, 0xD8C8381C, + 0x86376092, 0xD898620C, + 0x8646E35B, 0xD868920F, + 0x865678EA, 0xD838C82D, + 0x8666213C, 0xD809046D, + 0x8675DC4E, 0xD7D946D7, + 0x8685AA1F, 0xD7A98F73, + 0x86958AAB, 0xD779DE46, + 0x86A57DF1, 0xD74A335A, + 0x86B583EE, 0xD71A8EB5, + 0x86C59C9F, 0xD6EAF05E, + 0x86D5C802, 0xD6BB585D, + 0x86E60614, 0xD68BC6BA, + 0x86F656D3, 0xD65C3B7B, + 0x8706BA3C, 0xD62CB6A7, + 0x8717304E, 0xD5FD3847, + 0x8727B904, 0xD5CDC062, + 0x8738545E, 0xD59E4EFE, + 0x87490257, 0xD56EE424, + 0x8759C2EF, 0xD53F7FDA, + 0x876A9621, 0xD5102227, + 0x877B7BEC, 0xD4E0CB14, + 0x878C744C, 0xD4B17AA7, + 0x879D7F40, 0xD48230E8, + 0x87AE9CC5, 0xD452EDDE, + 0x87BFCCD7, 0xD423B190, + 0x87D10F75, 0xD3F47C06, + 0x87E2649B, 0xD3C54D46, + 0x87F3CC47, 0xD3962559, + 0x88054677, 0xD3670445, + 0x8816D327, 0xD337EA12, + 0x88287255, 0xD308D6C6, + 0x883A23FE, 0xD2D9CA6A, + 0x884BE820, 0xD2AAC504, + 0x885DBEB7, 0xD27BC69C, + 0x886FA7C2, 0xD24CCF38, + 0x8881A33C, 0xD21DDEE1, + 0x8893B124, 0xD1EEF59E, + 0x88A5D177, 0xD1C01374, + 0x88B80431, 0xD191386D, + 0x88CA4951, 0xD162648F, + 0x88DCA0D3, 0xD13397E1, + 0x88EF0AB4, 0xD104D26B, + 0x890186F1, 0xD0D61433, + 0x89141589, 0xD0A75D42, + 0x8926B677, 0xD078AD9D, + 0x893969B9, 0xD04A054D, + 0x894C2F4C, 0xD01B6459, + 0x895F072D, 0xCFECCAC7, + 0x8971F15A, 0xCFBE389F, + 0x8984EDCF, 0xCF8FADE8, + 0x8997FC89, 0xCF612AAA, + 0x89AB1D86, 0xCF32AEEB, + 0x89BE50C3, 0xCF043AB2, + 0x89D1963C, 0xCED5CE08, + 0x89E4EDEE, 0xCEA768F2, + 0x89F857D8, 0xCE790B78, + 0x8A0BD3F5, 0xCE4AB5A2, + 0x8A1F6242, 0xCE1C6776, + 0x8A3302BD, 0xCDEE20FC, + 0x8A46B563, 0xCDBFE23A, + 0x8A5A7A30, 0xCD91AB38, + 0x8A6E5122, 0xCD637BFD, + 0x8A823A35, 0xCD355490, + 0x8A963567, 0xCD0734F8, + 0x8AAA42B4, 0xCCD91D3D, + 0x8ABE6219, 0xCCAB0D65, + 0x8AD29393, 0xCC7D0577, + 0x8AE6D71F, 0xCC4F057B, + 0x8AFB2CBA, 0xCC210D78, + 0x8B0F9461, 0xCBF31D75, + 0x8B240E10, 0xCBC53578, + 0x8B3899C5, 0xCB975589, + 0x8B4D377C, 0xCB697DB0, + 0x8B61E732, 0xCB3BADF2, + 0x8B76A8E4, 0xCB0DE658, + 0x8B8B7C8F, 0xCAE026E8, + 0x8BA0622F, 0xCAB26FA9, + 0x8BB559C1, 0xCA84C0A2, + 0x8BCA6342, 0xCA5719DB, + 0x8BDF7EAF, 0xCA297B5A, + 0x8BF4AC05, 0xC9FBE527, + 0x8C09EB40, 0xC9CE5748, + 0x8C1F3C5C, 0xC9A0D1C4, + 0x8C349F58, 0xC97354A3, + 0x8C4A142F, 0xC945DFEC, + 0x8C5F9ADD, 0xC91873A5, + 0x8C753361, 0xC8EB0FD6, + 0x8C8ADDB6, 0xC8BDB485, + 0x8CA099D9, 0xC89061BA, + 0x8CB667C7, 0xC863177B, + 0x8CCC477D, 0xC835D5D0, + 0x8CE238F6, 0xC8089CBF, + 0x8CF83C30, 0xC7DB6C50, + 0x8D0E5127, 0xC7AE4489, + 0x8D2477D8, 0xC7812571, + 0x8D3AB03F, 0xC7540F10, + 0x8D50FA59, 0xC727016C, + 0x8D675623, 0xC6F9FC8D, + 0x8D7DC399, 0xC6CD0079, + 0x8D9442B7, 0xC6A00D36, + 0x8DAAD37B, 0xC67322CD, + 0x8DC175E0, 0xC6464144, + 0x8DD829E4, 0xC61968A2, + 0x8DEEEF82, 0xC5EC98ED, + 0x8E05C6B7, 0xC5BFD22E, + 0x8E1CAF80, 0xC593146A, + 0x8E33A9D9, 0xC5665FA8, + 0x8E4AB5BF, 0xC539B3F0, + 0x8E61D32D, 0xC50D1148, + 0x8E790222, 0xC4E077B8, + 0x8E904298, 0xC4B3E746, + 0x8EA7948C, 0xC4875FF8, + 0x8EBEF7FB, 0xC45AE1D7, + 0x8ED66CE1, 0xC42E6CE8, + 0x8EEDF33B, 0xC4020132, + 0x8F058B04, 0xC3D59EBD, + 0x8F1D343A, 0xC3A9458F, + 0x8F34EED8, 0xC37CF5B0, + 0x8F4CBADB, 0xC350AF25, + 0x8F64983F, 0xC32471F6, + 0x8F7C8701, 0xC2F83E2A, + 0x8F94871D, 0xC2CC13C7, + 0x8FAC988E, 0xC29FF2D4, + 0x8FC4BB53, 0xC273DB58, + 0x8FDCEF66, 0xC247CD5A, + 0x8FF534C4, 0xC21BC8E0, + 0x900D8B69, 0xC1EFCDF2, + 0x9025F352, 0xC1C3DC96, + 0x903E6C7A, 0xC197F4D3, + 0x9056F6DF, 0xC16C16B0, + 0x906F927B, 0xC1404233, + 0x90883F4C, 0xC1147763, + 0x90A0FD4E, 0xC0E8B648, + 0x90B9CC7C, 0xC0BCFEE7, + 0x90D2ACD3, 0xC0915147, + 0x90EB9E50, 0xC065AD70, + 0x9104A0ED, 0xC03A1368, + 0x911DB4A8, 0xC00E8335, + 0x9136D97D, 0xBFE2FCDF, + 0x91500F67, 0xBFB7806C, + 0x91695663, 0xBF8C0DE2, + 0x9182AE6C, 0xBF60A54A, + 0x919C1780, 0xBF3546A8, + 0x91B5919A, 0xBF09F204, + 0x91CF1CB6, 0xBEDEA765, + 0x91E8B8D0, 0xBEB366D1, + 0x920265E4, 0xBE88304F, + 0x921C23EE, 0xBE5D03E5, + 0x9235F2EB, 0xBE31E19B, + 0x924FD2D6, 0xBE06C977, + 0x9269C3AC, 0xBDDBBB7F, + 0x9283C567, 0xBDB0B7BA, + 0x929DD805, 0xBD85BE2F, + 0x92B7FB82, 0xBD5ACEE5, + 0x92D22FD8, 0xBD2FE9E1, + 0x92EC7505, 0xBD050F2C, + 0x9306CB04, 0xBCDA3ECA, + 0x932131D1, 0xBCAF78C3, + 0x933BA968, 0xBC84BD1E, + 0x935631C5, 0xBC5A0BE1, + 0x9370CAE4, 0xBC2F6513, + 0x938B74C0, 0xBC04C8BA, + 0x93A62F56, 0xBBDA36DC, + 0x93C0FAA2, 0xBBAFAF81, + 0x93DBD69F, 0xBB8532AF, + 0x93F6C34A, 0xBB5AC06C, + 0x9411C09D, 0xBB3058C0, + 0x942CCE95, 0xBB05FBB0, + 0x9447ED2F, 0xBADBA943, + 0x94631C64, 0xBAB1617F, + 0x947E5C32, 0xBA87246C, + 0x9499AC95, 0xBA5CF210, + 0x94B50D87, 0xBA32CA70, + 0x94D07F05, 0xBA08AD94, + 0x94EC010B, 0xB9DE9B83, + 0x95079393, 0xB9B49442, + 0x9523369B, 0xB98A97D8, + 0x953EEA1E, 0xB960A64B, + 0x955AAE17, 0xB936BFA3, + 0x95768282, 0xB90CE3E6, + 0x9592675B, 0xB8E31319, + 0x95AE5C9E, 0xB8B94D44, + 0x95CA6246, 0xB88F926C, + 0x95E6784F, 0xB865E299, + 0x96029EB5, 0xB83C3DD1, + 0x961ED573, 0xB812A419, + 0x963B1C85, 0xB7E9157A, + 0x965773E7, 0xB7BF91F8, + 0x9673DB94, 0xB796199B, + 0x96905387, 0xB76CAC68, + 0x96ACDBBD, 0xB7434A67, + 0x96C97431, 0xB719F39D, + 0x96E61CDF, 0xB6F0A811, + 0x9702D5C2, 0xB6C767CA, + 0x971F9ED6, 0xB69E32CD, + 0x973C7816, 0xB6750921, + 0x9759617E, 0xB64BEACC, + 0x97765B0A, 0xB622D7D5, + 0x979364B5, 0xB5F9D042, + 0x97B07E7A, 0xB5D0D41A, + 0x97CDA855, 0xB5A7E362, + 0x97EAE241, 0xB57EFE21, + 0x98082C3B, 0xB556245E, + 0x9825863D, 0xB52D561E, + 0x9842F043, 0xB5049368, + 0x98606A48, 0xB4DBDC42, + 0x987DF449, 0xB4B330B2, + 0x989B8E3F, 0xB48A90C0, + 0x98B93828, 0xB461FC70, + 0x98D6F1FE, 0xB43973C9, + 0x98F4BBBC, 0xB410F6D2, + 0x9912955E, 0xB3E88591, + 0x99307EE0, 0xB3C0200C, + 0x994E783C, 0xB397C649, + 0x996C816F, 0xB36F784E, + 0x998A9A73, 0xB3473622, + 0x99A8C344, 0xB31EFFCB, + 0x99C6FBDE, 0xB2F6D54F, + 0x99E5443A, 0xB2CEB6B5, + 0x9A039C56, 0xB2A6A401, + 0x9A22042C, 0xB27E9D3B, + 0x9A407BB8, 0xB256A26A, + 0x9A5F02F5, 0xB22EB392, + 0x9A7D99DD, 0xB206D0BA, + 0x9A9C406D, 0xB1DEF9E8, + 0x9ABAF6A0, 0xB1B72F23, + 0x9AD9BC71, 0xB18F7070, + 0x9AF891DB, 0xB167BDD6, + 0x9B1776D9, 0xB140175B, + 0x9B366B67, 0xB1187D05, + 0x9B556F80, 0xB0F0EEDA, + 0x9B748320, 0xB0C96CDF, + 0x9B93A640, 0xB0A1F71C, + 0x9BB2D8DD, 0xB07A8D97, + 0x9BD21AF2, 0xB0533055, + 0x9BF16C7A, 0xB02BDF5C, + 0x9C10CD70, 0xB0049AB2, + 0x9C303DCF, 0xAFDD625F, + 0x9C4FBD92, 0xAFB63667, + 0x9C6F4CB5, 0xAF8F16D0, + 0x9C8EEB33, 0xAF6803A1, + 0x9CAE9907, 0xAF40FCE0, + 0x9CCE562B, 0xAF1A0293, + 0x9CEE229C, 0xAEF314BF, + 0x9D0DFE53, 0xAECC336B, + 0x9D2DE94D, 0xAEA55E9D, + 0x9D4DE384, 0xAE7E965B, + 0x9D6DECF4, 0xAE57DAAA, + 0x9D8E0596, 0xAE312B91, + 0x9DAE2D68, 0xAE0A8916, + 0x9DCE6462, 0xADE3F33E, + 0x9DEEAA82, 0xADBD6A10, + 0x9E0EFFC1, 0xAD96ED91, + 0x9E2F641A, 0xAD707DC8, + 0x9E4FD789, 0xAD4A1ABA, + 0x9E705A09, 0xAD23C46D, + 0x9E90EB94, 0xACFD7AE8, + 0x9EB18C26, 0xACD73E30, + 0x9ED23BB9, 0xACB10E4A, + 0x9EF2FA48, 0xAC8AEB3E, + 0x9F13C7D0, 0xAC64D510, + 0x9F34A449, 0xAC3ECBC7, + 0x9F558FB0, 0xAC18CF68, + 0x9F7689FF, 0xABF2DFFA, + 0x9F979331, 0xABCCFD82, + 0x9FB8AB41, 0xABA72806, + 0x9FD9D22A, 0xAB815F8C, + 0x9FFB07E7, 0xAB5BA41A, + 0xA01C4C72, 0xAB35F5B5, + 0xA03D9FC7, 0xAB105464, + 0xA05F01E1, 0xAAEAC02B, + 0xA08072BA, 0xAAC53912, + 0xA0A1F24C, 0xAA9FBF1D, + 0xA0C38094, 0xAA7A5253, + 0xA0E51D8C, 0xAA54F2B9, + 0xA106C92E, 0xAA2FA055, + 0xA1288376, 0xAA0A5B2D, + 0xA14A4C5E, 0xA9E52347, + 0xA16C23E1, 0xA9BFF8A8, + 0xA18E09F9, 0xA99ADB56, + 0xA1AFFEA2, 0xA975CB56, + 0xA1D201D7, 0xA950C8AF, + 0xA1F41391, 0xA92BD366, + 0xA21633CD, 0xA906EB81, + 0xA2386283, 0xA8E21106, + 0xA25A9FB1, 0xA8BD43FA, + 0xA27CEB4F, 0xA8988463, + 0xA29F4559, 0xA873D246, + 0xA2C1ADC9, 0xA84F2DA9, + 0xA2E4249A, 0xA82A9693, + 0xA306A9C7, 0xA8060D08, + 0xA3293D4B, 0xA7E1910E, + 0xA34BDF20, 0xA7BD22AB, + 0xA36E8F40, 0xA798C1E4, + 0xA3914DA7, 0xA7746EC0, + 0xA3B41A4F, 0xA7502943, + 0xA3D6F533, 0xA72BF173, + 0xA3F9DE4D, 0xA707C756, + 0xA41CD598, 0xA6E3AAF2, + 0xA43FDB0F, 0xA6BF9C4B, + 0xA462EEAC, 0xA69B9B68, + 0xA4861069, 0xA677A84E, + 0xA4A94042, 0xA653C302, + 0xA4CC7E31, 0xA62FEB8B, + 0xA4EFCA31, 0xA60C21ED, + 0xA513243B, 0xA5E8662F, + 0xA5368C4B, 0xA5C4B855, + 0xA55A025B, 0xA5A11865, + 0xA57D8666, 0xA57D8666, + 0xA5A11865, 0xA55A025B, + 0xA5C4B855, 0xA5368C4B, + 0xA5E8662F, 0xA513243B, + 0xA60C21ED, 0xA4EFCA31, + 0xA62FEB8B, 0xA4CC7E31, + 0xA653C302, 0xA4A94042, + 0xA677A84E, 0xA4861069, + 0xA69B9B68, 0xA462EEAC, + 0xA6BF9C4B, 0xA43FDB0F, + 0xA6E3AAF2, 0xA41CD598, + 0xA707C756, 0xA3F9DE4D, + 0xA72BF173, 0xA3D6F533, + 0xA7502943, 0xA3B41A4F, + 0xA7746EC0, 0xA3914DA7, + 0xA798C1E4, 0xA36E8F40, + 0xA7BD22AB, 0xA34BDF20, + 0xA7E1910E, 0xA3293D4B, + 0xA8060D08, 0xA306A9C7, + 0xA82A9693, 0xA2E4249A, + 0xA84F2DA9, 0xA2C1ADC9, + 0xA873D246, 0xA29F4559, + 0xA8988463, 0xA27CEB4F, + 0xA8BD43FA, 0xA25A9FB1, + 0xA8E21106, 0xA2386283, + 0xA906EB81, 0xA21633CD, + 0xA92BD366, 0xA1F41391, + 0xA950C8AF, 0xA1D201D7, + 0xA975CB56, 0xA1AFFEA2, + 0xA99ADB56, 0xA18E09F9, + 0xA9BFF8A8, 0xA16C23E1, + 0xA9E52347, 0xA14A4C5E, + 0xAA0A5B2D, 0xA1288376, + 0xAA2FA055, 0xA106C92E, + 0xAA54F2B9, 0xA0E51D8C, + 0xAA7A5253, 0xA0C38094, + 0xAA9FBF1D, 0xA0A1F24C, + 0xAAC53912, 0xA08072BA, + 0xAAEAC02B, 0xA05F01E1, + 0xAB105464, 0xA03D9FC7, + 0xAB35F5B5, 0xA01C4C72, + 0xAB5BA41A, 0x9FFB07E7, + 0xAB815F8C, 0x9FD9D22A, + 0xABA72806, 0x9FB8AB41, + 0xABCCFD82, 0x9F979331, + 0xABF2DFFA, 0x9F7689FF, + 0xAC18CF68, 0x9F558FB0, + 0xAC3ECBC7, 0x9F34A449, + 0xAC64D510, 0x9F13C7D0, + 0xAC8AEB3E, 0x9EF2FA48, + 0xACB10E4A, 0x9ED23BB9, + 0xACD73E30, 0x9EB18C26, + 0xACFD7AE8, 0x9E90EB94, + 0xAD23C46D, 0x9E705A09, + 0xAD4A1ABA, 0x9E4FD789, + 0xAD707DC8, 0x9E2F641A, + 0xAD96ED91, 0x9E0EFFC1, + 0xADBD6A10, 0x9DEEAA82, + 0xADE3F33E, 0x9DCE6462, + 0xAE0A8916, 0x9DAE2D68, + 0xAE312B91, 0x9D8E0596, + 0xAE57DAAA, 0x9D6DECF4, + 0xAE7E965B, 0x9D4DE384, + 0xAEA55E9D, 0x9D2DE94D, + 0xAECC336B, 0x9D0DFE53, + 0xAEF314BF, 0x9CEE229C, + 0xAF1A0293, 0x9CCE562B, + 0xAF40FCE0, 0x9CAE9907, + 0xAF6803A1, 0x9C8EEB33, + 0xAF8F16D0, 0x9C6F4CB5, + 0xAFB63667, 0x9C4FBD92, + 0xAFDD625F, 0x9C303DCF, + 0xB0049AB2, 0x9C10CD70, + 0xB02BDF5C, 0x9BF16C7A, + 0xB0533055, 0x9BD21AF2, + 0xB07A8D97, 0x9BB2D8DD, + 0xB0A1F71C, 0x9B93A640, + 0xB0C96CDF, 0x9B748320, + 0xB0F0EEDA, 0x9B556F80, + 0xB1187D05, 0x9B366B67, + 0xB140175B, 0x9B1776D9, + 0xB167BDD6, 0x9AF891DB, + 0xB18F7070, 0x9AD9BC71, + 0xB1B72F23, 0x9ABAF6A0, + 0xB1DEF9E8, 0x9A9C406D, + 0xB206D0BA, 0x9A7D99DD, + 0xB22EB392, 0x9A5F02F5, + 0xB256A26A, 0x9A407BB8, + 0xB27E9D3B, 0x9A22042C, + 0xB2A6A401, 0x9A039C56, + 0xB2CEB6B5, 0x99E5443A, + 0xB2F6D54F, 0x99C6FBDE, + 0xB31EFFCB, 0x99A8C344, + 0xB3473622, 0x998A9A73, + 0xB36F784E, 0x996C816F, + 0xB397C649, 0x994E783C, + 0xB3C0200C, 0x99307EE0, + 0xB3E88591, 0x9912955E, + 0xB410F6D2, 0x98F4BBBC, + 0xB43973C9, 0x98D6F1FE, + 0xB461FC70, 0x98B93828, + 0xB48A90C0, 0x989B8E3F, + 0xB4B330B2, 0x987DF449, + 0xB4DBDC42, 0x98606A48, + 0xB5049368, 0x9842F043, + 0xB52D561E, 0x9825863D, + 0xB556245E, 0x98082C3B, + 0xB57EFE21, 0x97EAE241, + 0xB5A7E362, 0x97CDA855, + 0xB5D0D41A, 0x97B07E7A, + 0xB5F9D042, 0x979364B5, + 0xB622D7D5, 0x97765B0A, + 0xB64BEACC, 0x9759617E, + 0xB6750921, 0x973C7816, + 0xB69E32CD, 0x971F9ED6, + 0xB6C767CA, 0x9702D5C2, + 0xB6F0A811, 0x96E61CDF, + 0xB719F39D, 0x96C97431, + 0xB7434A67, 0x96ACDBBD, + 0xB76CAC68, 0x96905387, + 0xB796199B, 0x9673DB94, + 0xB7BF91F8, 0x965773E7, + 0xB7E9157A, 0x963B1C85, + 0xB812A419, 0x961ED573, + 0xB83C3DD1, 0x96029EB5, + 0xB865E299, 0x95E6784F, + 0xB88F926C, 0x95CA6246, + 0xB8B94D44, 0x95AE5C9E, + 0xB8E31319, 0x9592675B, + 0xB90CE3E6, 0x95768282, + 0xB936BFA3, 0x955AAE17, + 0xB960A64B, 0x953EEA1E, + 0xB98A97D8, 0x9523369B, + 0xB9B49442, 0x95079393, + 0xB9DE9B83, 0x94EC010B, + 0xBA08AD94, 0x94D07F05, + 0xBA32CA70, 0x94B50D87, + 0xBA5CF210, 0x9499AC95, + 0xBA87246C, 0x947E5C32, + 0xBAB1617F, 0x94631C64, + 0xBADBA943, 0x9447ED2F, + 0xBB05FBB0, 0x942CCE95, + 0xBB3058C0, 0x9411C09D, + 0xBB5AC06C, 0x93F6C34A, + 0xBB8532AF, 0x93DBD69F, + 0xBBAFAF81, 0x93C0FAA2, + 0xBBDA36DC, 0x93A62F56, + 0xBC04C8BA, 0x938B74C0, + 0xBC2F6513, 0x9370CAE4, + 0xBC5A0BE1, 0x935631C5, + 0xBC84BD1E, 0x933BA968, + 0xBCAF78C3, 0x932131D1, + 0xBCDA3ECA, 0x9306CB04, + 0xBD050F2C, 0x92EC7505, + 0xBD2FE9E1, 0x92D22FD8, + 0xBD5ACEE5, 0x92B7FB82, + 0xBD85BE2F, 0x929DD805, + 0xBDB0B7BA, 0x9283C567, + 0xBDDBBB7F, 0x9269C3AC, + 0xBE06C977, 0x924FD2D6, + 0xBE31E19B, 0x9235F2EB, + 0xBE5D03E5, 0x921C23EE, + 0xBE88304F, 0x920265E4, + 0xBEB366D1, 0x91E8B8D0, + 0xBEDEA765, 0x91CF1CB6, + 0xBF09F204, 0x91B5919A, + 0xBF3546A8, 0x919C1780, + 0xBF60A54A, 0x9182AE6C, + 0xBF8C0DE2, 0x91695663, + 0xBFB7806C, 0x91500F67, + 0xBFE2FCDF, 0x9136D97D, + 0xC00E8335, 0x911DB4A8, + 0xC03A1368, 0x9104A0ED, + 0xC065AD70, 0x90EB9E50, + 0xC0915147, 0x90D2ACD3, + 0xC0BCFEE7, 0x90B9CC7C, + 0xC0E8B648, 0x90A0FD4E, + 0xC1147763, 0x90883F4C, + 0xC1404233, 0x906F927B, + 0xC16C16B0, 0x9056F6DF, + 0xC197F4D3, 0x903E6C7A, + 0xC1C3DC96, 0x9025F352, + 0xC1EFCDF2, 0x900D8B69, + 0xC21BC8E0, 0x8FF534C4, + 0xC247CD5A, 0x8FDCEF66, + 0xC273DB58, 0x8FC4BB53, + 0xC29FF2D4, 0x8FAC988E, + 0xC2CC13C7, 0x8F94871D, + 0xC2F83E2A, 0x8F7C8701, + 0xC32471F6, 0x8F64983F, + 0xC350AF25, 0x8F4CBADB, + 0xC37CF5B0, 0x8F34EED8, + 0xC3A9458F, 0x8F1D343A, + 0xC3D59EBD, 0x8F058B04, + 0xC4020132, 0x8EEDF33B, + 0xC42E6CE8, 0x8ED66CE1, + 0xC45AE1D7, 0x8EBEF7FB, + 0xC4875FF8, 0x8EA7948C, + 0xC4B3E746, 0x8E904298, + 0xC4E077B8, 0x8E790222, + 0xC50D1148, 0x8E61D32D, + 0xC539B3F0, 0x8E4AB5BF, + 0xC5665FA8, 0x8E33A9D9, + 0xC593146A, 0x8E1CAF80, + 0xC5BFD22E, 0x8E05C6B7, + 0xC5EC98ED, 0x8DEEEF82, + 0xC61968A2, 0x8DD829E4, + 0xC6464144, 0x8DC175E0, + 0xC67322CD, 0x8DAAD37B, + 0xC6A00D36, 0x8D9442B7, + 0xC6CD0079, 0x8D7DC399, + 0xC6F9FC8D, 0x8D675623, + 0xC727016C, 0x8D50FA59, + 0xC7540F10, 0x8D3AB03F, + 0xC7812571, 0x8D2477D8, + 0xC7AE4489, 0x8D0E5127, + 0xC7DB6C50, 0x8CF83C30, + 0xC8089CBF, 0x8CE238F6, + 0xC835D5D0, 0x8CCC477D, + 0xC863177B, 0x8CB667C7, + 0xC89061BA, 0x8CA099D9, + 0xC8BDB485, 0x8C8ADDB6, + 0xC8EB0FD6, 0x8C753361, + 0xC91873A5, 0x8C5F9ADD, + 0xC945DFEC, 0x8C4A142F, + 0xC97354A3, 0x8C349F58, + 0xC9A0D1C4, 0x8C1F3C5C, + 0xC9CE5748, 0x8C09EB40, + 0xC9FBE527, 0x8BF4AC05, + 0xCA297B5A, 0x8BDF7EAF, + 0xCA5719DB, 0x8BCA6342, + 0xCA84C0A2, 0x8BB559C1, + 0xCAB26FA9, 0x8BA0622F, + 0xCAE026E8, 0x8B8B7C8F, + 0xCB0DE658, 0x8B76A8E4, + 0xCB3BADF2, 0x8B61E732, + 0xCB697DB0, 0x8B4D377C, + 0xCB975589, 0x8B3899C5, + 0xCBC53578, 0x8B240E10, + 0xCBF31D75, 0x8B0F9461, + 0xCC210D78, 0x8AFB2CBA, + 0xCC4F057B, 0x8AE6D71F, + 0xCC7D0577, 0x8AD29393, + 0xCCAB0D65, 0x8ABE6219, + 0xCCD91D3D, 0x8AAA42B4, + 0xCD0734F8, 0x8A963567, + 0xCD355490, 0x8A823A35, + 0xCD637BFD, 0x8A6E5122, + 0xCD91AB38, 0x8A5A7A30, + 0xCDBFE23A, 0x8A46B563, + 0xCDEE20FC, 0x8A3302BD, + 0xCE1C6776, 0x8A1F6242, + 0xCE4AB5A2, 0x8A0BD3F5, + 0xCE790B78, 0x89F857D8, + 0xCEA768F2, 0x89E4EDEE, + 0xCED5CE08, 0x89D1963C, + 0xCF043AB2, 0x89BE50C3, + 0xCF32AEEB, 0x89AB1D86, + 0xCF612AAA, 0x8997FC89, + 0xCF8FADE8, 0x8984EDCF, + 0xCFBE389F, 0x8971F15A, + 0xCFECCAC7, 0x895F072D, + 0xD01B6459, 0x894C2F4C, + 0xD04A054D, 0x893969B9, + 0xD078AD9D, 0x8926B677, + 0xD0A75D42, 0x89141589, + 0xD0D61433, 0x890186F1, + 0xD104D26B, 0x88EF0AB4, + 0xD13397E1, 0x88DCA0D3, + 0xD162648F, 0x88CA4951, + 0xD191386D, 0x88B80431, + 0xD1C01374, 0x88A5D177, + 0xD1EEF59E, 0x8893B124, + 0xD21DDEE1, 0x8881A33C, + 0xD24CCF38, 0x886FA7C2, + 0xD27BC69C, 0x885DBEB7, + 0xD2AAC504, 0x884BE820, + 0xD2D9CA6A, 0x883A23FE, + 0xD308D6C6, 0x88287255, + 0xD337EA12, 0x8816D327, + 0xD3670445, 0x88054677, + 0xD3962559, 0x87F3CC47, + 0xD3C54D46, 0x87E2649B, + 0xD3F47C06, 0x87D10F75, + 0xD423B190, 0x87BFCCD7, + 0xD452EDDE, 0x87AE9CC5, + 0xD48230E8, 0x879D7F40, + 0xD4B17AA7, 0x878C744C, + 0xD4E0CB14, 0x877B7BEC, + 0xD5102227, 0x876A9621, + 0xD53F7FDA, 0x8759C2EF, + 0xD56EE424, 0x87490257, + 0xD59E4EFE, 0x8738545E, + 0xD5CDC062, 0x8727B904, + 0xD5FD3847, 0x8717304E, + 0xD62CB6A7, 0x8706BA3C, + 0xD65C3B7B, 0x86F656D3, + 0xD68BC6BA, 0x86E60614, + 0xD6BB585D, 0x86D5C802, + 0xD6EAF05E, 0x86C59C9F, + 0xD71A8EB5, 0x86B583EE, + 0xD74A335A, 0x86A57DF1, + 0xD779DE46, 0x86958AAB, + 0xD7A98F73, 0x8685AA1F, + 0xD7D946D7, 0x8675DC4E, + 0xD809046D, 0x8666213C, + 0xD838C82D, 0x865678EA, + 0xD868920F, 0x8646E35B, + 0xD898620C, 0x86376092, + 0xD8C8381C, 0x8627F090, + 0xD8F81439, 0x86189359, + 0xD927F65B, 0x860948EE, + 0xD957DE7A, 0x85FA1152, + 0xD987CC8F, 0x85EAEC88, + 0xD9B7C093, 0x85DBDA91, + 0xD9E7BA7E, 0x85CCDB70, + 0xDA17BA4A, 0x85BDEF27, + 0xDA47BFED, 0x85AF15B9, + 0xDA77CB62, 0x85A04F28, + 0xDAA7DCA1, 0x85919B75, + 0xDAD7F3A2, 0x8582FAA4, + 0xDB08105E, 0x85746CB7, + 0xDB3832CD, 0x8565F1B0, + 0xDB685AE8, 0x85578991, + 0xDB9888A8, 0x8549345C, + 0xDBC8BC05, 0x853AF214, + 0xDBF8F4F8, 0x852CC2BA, + 0xDC293379, 0x851EA652, + 0xDC597781, 0x85109CDC, + 0xDC89C108, 0x8502A65C, + 0xDCBA1008, 0x84F4C2D3, + 0xDCEA6478, 0x84E6F244, + 0xDD1ABE51, 0x84D934B0, + 0xDD4B1D8B, 0x84CB8A1B, + 0xDD7B8220, 0x84BDF285, + 0xDDABEC07, 0x84B06DF1, + 0xDDDC5B3A, 0x84A2FC62, + 0xDE0CCFB1, 0x84959DD9, + 0xDE3D4963, 0x84885257, + 0xDE6DC84B, 0x847B19E1, + 0xDE9E4C60, 0x846DF476, + 0xDECED59B, 0x8460E21A, + 0xDEFF63F4, 0x8453E2CE, + 0xDF2FF764, 0x8446F695, + 0xDF608FE3, 0x843A1D70, + 0xDF912D6A, 0x842D5761, + 0xDFC1CFF2, 0x8420A46B, + 0xDFF27773, 0x8414048F, + 0xE02323E5, 0x840777CF, + 0xE053D541, 0x83FAFE2E, + 0xE0848B7F, 0x83EE97AC, + 0xE0B54698, 0x83E2444D, + 0xE0E60684, 0x83D60411, + 0xE116CB3D, 0x83C9D6FB, + 0xE14794B9, 0x83BDBD0D, + 0xE17862F3, 0x83B1B649, + 0xE1A935E1, 0x83A5C2B0, + 0xE1DA0D7E, 0x8399E244, + 0xE20AE9C1, 0x838E1507, + 0xE23BCAA2, 0x83825AFB, + 0xE26CB01A, 0x8376B422, + 0xE29D9A22, 0x836B207D, + 0xE2CE88B2, 0x835FA00E, + 0xE2FF7BC3, 0x835432D8, + 0xE330734C, 0x8348D8DB, + 0xE3616F47, 0x833D921A, + 0xE3926FAC, 0x83325E97, + 0xE3C37473, 0x83273E52, + 0xE3F47D95, 0x831C314E, + 0xE4258B0A, 0x8311378C, + 0xE4569CCB, 0x8306510F, + 0xE487B2CF, 0x82FB7DD8, + 0xE4B8CD10, 0x82F0BDE8, + 0xE4E9EB86, 0x82E61141, + 0xE51B0E2A, 0x82DB77E5, + 0xE54C34F3, 0x82D0F1D5, + 0xE57D5FDA, 0x82C67F13, + 0xE5AE8ED8, 0x82BC1FA1, + 0xE5DFC1E4, 0x82B1D381, + 0xE610F8F9, 0x82A79AB3, + 0xE642340D, 0x829D753A, + 0xE6737319, 0x82936316, + 0xE6A4B616, 0x8289644A, + 0xE6D5FCFC, 0x827F78D8, + 0xE70747C3, 0x8275A0C0, + 0xE7389664, 0x826BDC04, + 0xE769E8D8, 0x82622AA5, + 0xE79B3F16, 0x82588CA6, + 0xE7CC9917, 0x824F0208, + 0xE7FDF6D3, 0x82458ACB, + 0xE82F5844, 0x823C26F2, + 0xE860BD60, 0x8232D67E, + 0xE8922621, 0x82299971, + 0xE8C3927F, 0x82206FCB, + 0xE8F50273, 0x8217598F, + 0xE92675F4, 0x820E56BE, + 0xE957ECFB, 0x82056758, + 0xE9896780, 0x81FC8B60, + 0xE9BAE57C, 0x81F3C2D7, + 0xE9EC66E8, 0x81EB0DBD, + 0xEA1DEBBB, 0x81E26C16, + 0xEA4F73EE, 0x81D9DDE1, + 0xEA80FF79, 0x81D16320, + 0xEAB28E55, 0x81C8FBD5, + 0xEAE4207A, 0x81C0A801, + 0xEB15B5E0, 0x81B867A4, + 0xEB474E80, 0x81B03AC1, + 0xEB78EA52, 0x81A82159, + 0xEBAA894E, 0x81A01B6C, + 0xEBDC2B6D, 0x819828FD, + 0xEC0DD0A8, 0x81904A0C, + 0xEC3F78F5, 0x81887E9A, + 0xEC71244F, 0x8180C6A9, + 0xECA2D2AC, 0x8179223A, + 0xECD48406, 0x8171914E, + 0xED063855, 0x816A13E6, + 0xED37EF91, 0x8162AA03, + 0xED69A9B2, 0x815B53A8, + 0xED9B66B2, 0x815410D3, + 0xEDCD2687, 0x814CE188, + 0xEDFEE92B, 0x8145C5C6, + 0xEE30AE95, 0x813EBD90, + 0xEE6276BF, 0x8137C8E6, + 0xEE9441A0, 0x8130E7C8, + 0xEEC60F31, 0x812A1A39, + 0xEEF7DF6A, 0x81236039, + 0xEF29B243, 0x811CB9CA, + 0xEF5B87B5, 0x811626EC, + 0xEF8D5FB8, 0x810FA7A0, + 0xEFBF3A44, 0x81093BE8, + 0xEFF11752, 0x8102E3C3, + 0xF022F6DA, 0x80FC9F35, + 0xF054D8D4, 0x80F66E3C, + 0xF086BD39, 0x80F050DB, + 0xF0B8A401, 0x80EA4712, + 0xF0EA8D23, 0x80E450E2, + 0xF11C789A, 0x80DE6E4C, + 0xF14E665C, 0x80D89F51, + 0xF1805662, 0x80D2E3F1, + 0xF1B248A5, 0x80CD3C2F, + 0xF1E43D1C, 0x80C7A80A, + 0xF21633C0, 0x80C22783, + 0xF2482C89, 0x80BCBA9C, + 0xF27A2770, 0x80B76155, + 0xF2AC246D, 0x80B21BAF, + 0xF2DE2378, 0x80ACE9AB, + 0xF310248A, 0x80A7CB49, + 0xF342279A, 0x80A2C08B, + 0xF3742CA1, 0x809DC970, + 0xF3A63398, 0x8098E5FB, + 0xF3D83C76, 0x8094162B, + 0xF40A4734, 0x808F5A02, + 0xF43C53CA, 0x808AB180, + 0xF46E6231, 0x80861CA5, + 0xF4A07260, 0x80819B74, + 0xF4D28451, 0x807D2DEB, + 0xF50497FA, 0x8078D40D, + 0xF536AD55, 0x80748DD9, + 0xF568C45A, 0x80705B50, + 0xF59ADD01, 0x806C3C73, + 0xF5CCF743, 0x80683143, + 0xF5FF1317, 0x806439C0, + 0xF6313076, 0x806055EA, + 0xF6634F58, 0x805C85C3, + 0xF6956FB6, 0x8058C94C, + 0xF6C79188, 0x80552083, + 0xF6F9B4C5, 0x80518B6B, + 0xF72BD967, 0x804E0A03, + 0xF75DFF65, 0x804A9C4D, + 0xF79026B8, 0x80474248, + 0xF7C24F58, 0x8043FBF6, + 0xF7F4793E, 0x8040C956, + 0xF826A461, 0x803DAA69, + 0xF858D0BA, 0x803A9F31, + 0xF88AFE41, 0x8037A7AC, + 0xF8BD2CEF, 0x8034C3DC, + 0xF8EF5CBB, 0x8031F3C1, + 0xF9218D9E, 0x802F375C, + 0xF953BF90, 0x802C8EAD, + 0xF985F28A, 0x8029F9B4, + 0xF9B82683, 0x80277872, + 0xF9EA5B75, 0x80250AE7, + 0xFA1C9156, 0x8022B113, + 0xFA4EC820, 0x80206AF8, + 0xFA80FFCB, 0x801E3894, + 0xFAB3384F, 0x801C19E9, + 0xFAE571A4, 0x801A0EF7, + 0xFB17ABC2, 0x801817BF, + 0xFB49E6A2, 0x80163440, + 0xFB7C223C, 0x8014647A, + 0xFBAE5E89, 0x8012A86F, + 0xFBE09B80, 0x8011001E, + 0xFC12D919, 0x800F6B88, + 0xFC45174E, 0x800DEAAC, + 0xFC775616, 0x800C7D8C, + 0xFCA99569, 0x800B2427, + 0xFCDBD541, 0x8009DE7D, + 0xFD0E1594, 0x8008AC90, + 0xFD40565B, 0x80078E5E, + 0xFD72978F, 0x800683E8, + 0xFDA4D928, 0x80058D2E, + 0xFDD71B1E, 0x8004AA31, + 0xFE095D69, 0x8003DAF0, + 0xFE3BA001, 0x80031F6C, + 0xFE6DE2E0, 0x800277A5, + 0xFEA025FC, 0x8001E39B, + 0xFED2694F, 0x8001634D, + 0xFF04ACD0, 0x8000F6BD, + 0xFF36F078, 0x80009DE9, + 0xFF69343E, 0x800058D3, + 0xFF9B781D, 0x8000277A, + 0xFFCDBC0A, 0x800009DE +}; + + + +/* +* @brief q15 Twiddle factors Table +*/ + + +/** +* \par +* Example code for q15 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefq15[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefq15[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 16 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to q15(Fixed point 1.15): +* round(twiddleCoefq15(i) * pow(2, 15)) +* +*/ +const q15_t twiddleCoef_16_q15[24] = { + 0x7FFF, 0x0000, + 0x7641, 0x30FB, + 0x5A82, 0x5A82, + 0x30FB, 0x7641, + 0x0000, 0x7FFF, + 0xCF04, 0x7641, + 0xA57D, 0x5A82, + 0x89BE, 0x30FB, + 0x8000, 0x0000, + 0x89BE, 0xCF04, + 0xA57D, 0xA57D, + 0xCF04, 0x89BE +}; + +/** +* \par +* Example code for q15 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefq15[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefq15[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 32 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to q15(Fixed point 1.15): +* round(twiddleCoefq15(i) * pow(2, 15)) +* +*/ +const q15_t twiddleCoef_32_q15[48] = { + 0x7FFF, 0x0000, + 0x7D8A, 0x18F8, + 0x7641, 0x30FB, + 0x6A6D, 0x471C, + 0x5A82, 0x5A82, + 0x471C, 0x6A6D, + 0x30FB, 0x7641, + 0x18F8, 0x7D8A, + 0x0000, 0x7FFF, + 0xE707, 0x7D8A, + 0xCF04, 0x7641, + 0xB8E3, 0x6A6D, + 0xA57D, 0x5A82, + 0x9592, 0x471C, + 0x89BE, 0x30FB, + 0x8275, 0x18F8, + 0x8000, 0x0000, + 0x8275, 0xE707, + 0x89BE, 0xCF04, + 0x9592, 0xB8E3, + 0xA57D, 0xA57D, + 0xB8E3, 0x9592, + 0xCF04, 0x89BE, + 0xE707, 0x8275 +}; + +/** +* \par +* Example code for q15 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefq15[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefq15[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 64 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to q15(Fixed point 1.15): +* round(twiddleCoefq15(i) * pow(2, 15)) +* +*/ +const q15_t twiddleCoef_64_q15[96] = { + 0x7FFF, 0x0000, + 0x7F62, 0x0C8B, + 0x7D8A, 0x18F8, + 0x7A7D, 0x2528, + 0x7641, 0x30FB, + 0x70E2, 0x3C56, + 0x6A6D, 0x471C, + 0x62F2, 0x5133, + 0x5A82, 0x5A82, + 0x5133, 0x62F2, + 0x471C, 0x6A6D, + 0x3C56, 0x70E2, + 0x30FB, 0x7641, + 0x2528, 0x7A7D, + 0x18F8, 0x7D8A, + 0x0C8B, 0x7F62, + 0x0000, 0x7FFF, + 0xF374, 0x7F62, + 0xE707, 0x7D8A, + 0xDAD7, 0x7A7D, + 0xCF04, 0x7641, + 0xC3A9, 0x70E2, + 0xB8E3, 0x6A6D, + 0xAECC, 0x62F2, + 0xA57D, 0x5A82, + 0x9D0D, 0x5133, + 0x9592, 0x471C, + 0x8F1D, 0x3C56, + 0x89BE, 0x30FB, + 0x8582, 0x2528, + 0x8275, 0x18F8, + 0x809D, 0x0C8B, + 0x8000, 0x0000, + 0x809D, 0xF374, + 0x8275, 0xE707, + 0x8582, 0xDAD7, + 0x89BE, 0xCF04, + 0x8F1D, 0xC3A9, + 0x9592, 0xB8E3, + 0x9D0D, 0xAECC, + 0xA57D, 0xA57D, + 0xAECC, 0x9D0D, + 0xB8E3, 0x9592, + 0xC3A9, 0x8F1D, + 0xCF04, 0x89BE, + 0xDAD7, 0x8582, + 0xE707, 0x8275, + 0xF374, 0x809D +}; + +/** +* \par +* Example code for q15 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefq15[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefq15[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 128 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to q15(Fixed point 1.15): +* round(twiddleCoefq15(i) * pow(2, 15)) +* +*/ +const q15_t twiddleCoef_128_q15[192] = { + 0x7FFF, 0x0000, + 0x7FD8, 0x0647, + 0x7F62, 0x0C8B, + 0x7E9D, 0x12C8, + 0x7D8A, 0x18F8, + 0x7C29, 0x1F19, + 0x7A7D, 0x2528, + 0x7884, 0x2B1F, + 0x7641, 0x30FB, + 0x73B5, 0x36BA, + 0x70E2, 0x3C56, + 0x6DCA, 0x41CE, + 0x6A6D, 0x471C, + 0x66CF, 0x4C3F, + 0x62F2, 0x5133, + 0x5ED7, 0x55F5, + 0x5A82, 0x5A82, + 0x55F5, 0x5ED7, + 0x5133, 0x62F2, + 0x4C3F, 0x66CF, + 0x471C, 0x6A6D, + 0x41CE, 0x6DCA, + 0x3C56, 0x70E2, + 0x36BA, 0x73B5, + 0x30FB, 0x7641, + 0x2B1F, 0x7884, + 0x2528, 0x7A7D, + 0x1F19, 0x7C29, + 0x18F8, 0x7D8A, + 0x12C8, 0x7E9D, + 0x0C8B, 0x7F62, + 0x0647, 0x7FD8, + 0x0000, 0x7FFF, + 0xF9B8, 0x7FD8, + 0xF374, 0x7F62, + 0xED37, 0x7E9D, + 0xE707, 0x7D8A, + 0xE0E6, 0x7C29, + 0xDAD7, 0x7A7D, + 0xD4E0, 0x7884, + 0xCF04, 0x7641, + 0xC945, 0x73B5, + 0xC3A9, 0x70E2, + 0xBE31, 0x6DCA, + 0xB8E3, 0x6A6D, + 0xB3C0, 0x66CF, + 0xAECC, 0x62F2, + 0xAA0A, 0x5ED7, + 0xA57D, 0x5A82, + 0xA128, 0x55F5, + 0x9D0D, 0x5133, + 0x9930, 0x4C3F, + 0x9592, 0x471C, + 0x9235, 0x41CE, + 0x8F1D, 0x3C56, + 0x8C4A, 0x36BA, + 0x89BE, 0x30FB, + 0x877B, 0x2B1F, + 0x8582, 0x2528, + 0x83D6, 0x1F19, + 0x8275, 0x18F8, + 0x8162, 0x12C8, + 0x809D, 0x0C8B, + 0x8027, 0x0647, + 0x8000, 0x0000, + 0x8027, 0xF9B8, + 0x809D, 0xF374, + 0x8162, 0xED37, + 0x8275, 0xE707, + 0x83D6, 0xE0E6, + 0x8582, 0xDAD7, + 0x877B, 0xD4E0, + 0x89BE, 0xCF04, + 0x8C4A, 0xC945, + 0x8F1D, 0xC3A9, + 0x9235, 0xBE31, + 0x9592, 0xB8E3, + 0x9930, 0xB3C0, + 0x9D0D, 0xAECC, + 0xA128, 0xAA0A, + 0xA57D, 0xA57D, + 0xAA0A, 0xA128, + 0xAECC, 0x9D0D, + 0xB3C0, 0x9930, + 0xB8E3, 0x9592, + 0xBE31, 0x9235, + 0xC3A9, 0x8F1D, + 0xC945, 0x8C4A, + 0xCF04, 0x89BE, + 0xD4E0, 0x877B, + 0xDAD7, 0x8582, + 0xE0E6, 0x83D6, + 0xE707, 0x8275, + 0xED37, 0x8162, + 0xF374, 0x809D, + 0xF9B8, 0x8027 +}; + +/** +* \par +* Example code for q15 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefq15[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefq15[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 256 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to q15(Fixed point 1.15): +* round(twiddleCoefq15(i) * pow(2, 15)) +* +*/ +const q15_t twiddleCoef_256_q15[384] = { + 0x7FFF, 0x0000, + 0x7FF6, 0x0324, + 0x7FD8, 0x0647, + 0x7FA7, 0x096A, + 0x7F62, 0x0C8B, + 0x7F09, 0x0FAB, + 0x7E9D, 0x12C8, + 0x7E1D, 0x15E2, + 0x7D8A, 0x18F8, + 0x7CE3, 0x1C0B, + 0x7C29, 0x1F19, + 0x7B5D, 0x2223, + 0x7A7D, 0x2528, + 0x798A, 0x2826, + 0x7884, 0x2B1F, + 0x776C, 0x2E11, + 0x7641, 0x30FB, + 0x7504, 0x33DE, + 0x73B5, 0x36BA, + 0x7255, 0x398C, + 0x70E2, 0x3C56, + 0x6F5F, 0x3F17, + 0x6DCA, 0x41CE, + 0x6C24, 0x447A, + 0x6A6D, 0x471C, + 0x68A6, 0x49B4, + 0x66CF, 0x4C3F, + 0x64E8, 0x4EBF, + 0x62F2, 0x5133, + 0x60EC, 0x539B, + 0x5ED7, 0x55F5, + 0x5CB4, 0x5842, + 0x5A82, 0x5A82, + 0x5842, 0x5CB4, + 0x55F5, 0x5ED7, + 0x539B, 0x60EC, + 0x5133, 0x62F2, + 0x4EBF, 0x64E8, + 0x4C3F, 0x66CF, + 0x49B4, 0x68A6, + 0x471C, 0x6A6D, + 0x447A, 0x6C24, + 0x41CE, 0x6DCA, + 0x3F17, 0x6F5F, + 0x3C56, 0x70E2, + 0x398C, 0x7255, + 0x36BA, 0x73B5, + 0x33DE, 0x7504, + 0x30FB, 0x7641, + 0x2E11, 0x776C, + 0x2B1F, 0x7884, + 0x2826, 0x798A, + 0x2528, 0x7A7D, + 0x2223, 0x7B5D, + 0x1F19, 0x7C29, + 0x1C0B, 0x7CE3, + 0x18F8, 0x7D8A, + 0x15E2, 0x7E1D, + 0x12C8, 0x7E9D, + 0x0FAB, 0x7F09, + 0x0C8B, 0x7F62, + 0x096A, 0x7FA7, + 0x0647, 0x7FD8, + 0x0324, 0x7FF6, + 0x0000, 0x7FFF, + 0xFCDB, 0x7FF6, + 0xF9B8, 0x7FD8, + 0xF695, 0x7FA7, + 0xF374, 0x7F62, + 0xF054, 0x7F09, + 0xED37, 0x7E9D, + 0xEA1D, 0x7E1D, + 0xE707, 0x7D8A, + 0xE3F4, 0x7CE3, + 0xE0E6, 0x7C29, + 0xDDDC, 0x7B5D, + 0xDAD7, 0x7A7D, + 0xD7D9, 0x798A, + 0xD4E0, 0x7884, + 0xD1EE, 0x776C, + 0xCF04, 0x7641, + 0xCC21, 0x7504, + 0xC945, 0x73B5, + 0xC673, 0x7255, + 0xC3A9, 0x70E2, + 0xC0E8, 0x6F5F, + 0xBE31, 0x6DCA, + 0xBB85, 0x6C24, + 0xB8E3, 0x6A6D, + 0xB64B, 0x68A6, + 0xB3C0, 0x66CF, + 0xB140, 0x64E8, + 0xAECC, 0x62F2, + 0xAC64, 0x60EC, + 0xAA0A, 0x5ED7, + 0xA7BD, 0x5CB4, + 0xA57D, 0x5A82, + 0xA34B, 0x5842, + 0xA128, 0x55F5, + 0x9F13, 0x539B, + 0x9D0D, 0x5133, + 0x9B17, 0x4EBF, + 0x9930, 0x4C3F, + 0x9759, 0x49B4, + 0x9592, 0x471C, + 0x93DB, 0x447A, + 0x9235, 0x41CE, + 0x90A0, 0x3F17, + 0x8F1D, 0x3C56, + 0x8DAA, 0x398C, + 0x8C4A, 0x36BA, + 0x8AFB, 0x33DE, + 0x89BE, 0x30FB, + 0x8893, 0x2E11, + 0x877B, 0x2B1F, + 0x8675, 0x2826, + 0x8582, 0x2528, + 0x84A2, 0x2223, + 0x83D6, 0x1F19, + 0x831C, 0x1C0B, + 0x8275, 0x18F8, + 0x81E2, 0x15E2, + 0x8162, 0x12C8, + 0x80F6, 0x0FAB, + 0x809D, 0x0C8B, + 0x8058, 0x096A, + 0x8027, 0x0647, + 0x8009, 0x0324, + 0x8000, 0x0000, + 0x8009, 0xFCDB, + 0x8027, 0xF9B8, + 0x8058, 0xF695, + 0x809D, 0xF374, + 0x80F6, 0xF054, + 0x8162, 0xED37, + 0x81E2, 0xEA1D, + 0x8275, 0xE707, + 0x831C, 0xE3F4, + 0x83D6, 0xE0E6, + 0x84A2, 0xDDDC, + 0x8582, 0xDAD7, + 0x8675, 0xD7D9, + 0x877B, 0xD4E0, + 0x8893, 0xD1EE, + 0x89BE, 0xCF04, + 0x8AFB, 0xCC21, + 0x8C4A, 0xC945, + 0x8DAA, 0xC673, + 0x8F1D, 0xC3A9, + 0x90A0, 0xC0E8, + 0x9235, 0xBE31, + 0x93DB, 0xBB85, + 0x9592, 0xB8E3, + 0x9759, 0xB64B, + 0x9930, 0xB3C0, + 0x9B17, 0xB140, + 0x9D0D, 0xAECC, + 0x9F13, 0xAC64, + 0xA128, 0xAA0A, + 0xA34B, 0xA7BD, + 0xA57D, 0xA57D, + 0xA7BD, 0xA34B, + 0xAA0A, 0xA128, + 0xAC64, 0x9F13, + 0xAECC, 0x9D0D, + 0xB140, 0x9B17, + 0xB3C0, 0x9930, + 0xB64B, 0x9759, + 0xB8E3, 0x9592, + 0xBB85, 0x93DB, + 0xBE31, 0x9235, + 0xC0E8, 0x90A0, + 0xC3A9, 0x8F1D, + 0xC673, 0x8DAA, + 0xC945, 0x8C4A, + 0xCC21, 0x8AFB, + 0xCF04, 0x89BE, + 0xD1EE, 0x8893, + 0xD4E0, 0x877B, + 0xD7D9, 0x8675, + 0xDAD7, 0x8582, + 0xDDDC, 0x84A2, + 0xE0E6, 0x83D6, + 0xE3F4, 0x831C, + 0xE707, 0x8275, + 0xEA1D, 0x81E2, + 0xED37, 0x8162, + 0xF054, 0x80F6, + 0xF374, 0x809D, + 0xF695, 0x8058, + 0xF9B8, 0x8027, + 0xFCDB, 0x8009 +}; + +/** +* \par +* Example code for q15 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefq15[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefq15[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 512 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to q15(Fixed point 1.15): +* round(twiddleCoefq15(i) * pow(2, 15)) +* +*/ +const q15_t twiddleCoef_512_q15[768] = { + 0x7FFF, 0x0000, + 0x7FFD, 0x0192, + 0x7FF6, 0x0324, + 0x7FE9, 0x04B6, + 0x7FD8, 0x0647, + 0x7FC2, 0x07D9, + 0x7FA7, 0x096A, + 0x7F87, 0x0AFB, + 0x7F62, 0x0C8B, + 0x7F38, 0x0E1B, + 0x7F09, 0x0FAB, + 0x7ED5, 0x1139, + 0x7E9D, 0x12C8, + 0x7E5F, 0x1455, + 0x7E1D, 0x15E2, + 0x7DD6, 0x176D, + 0x7D8A, 0x18F8, + 0x7D39, 0x1A82, + 0x7CE3, 0x1C0B, + 0x7C89, 0x1D93, + 0x7C29, 0x1F19, + 0x7BC5, 0x209F, + 0x7B5D, 0x2223, + 0x7AEF, 0x23A6, + 0x7A7D, 0x2528, + 0x7A05, 0x26A8, + 0x798A, 0x2826, + 0x7909, 0x29A3, + 0x7884, 0x2B1F, + 0x77FA, 0x2C98, + 0x776C, 0x2E11, + 0x76D9, 0x2F87, + 0x7641, 0x30FB, + 0x75A5, 0x326E, + 0x7504, 0x33DE, + 0x745F, 0x354D, + 0x73B5, 0x36BA, + 0x7307, 0x3824, + 0x7255, 0x398C, + 0x719E, 0x3AF2, + 0x70E2, 0x3C56, + 0x7023, 0x3DB8, + 0x6F5F, 0x3F17, + 0x6E96, 0x4073, + 0x6DCA, 0x41CE, + 0x6CF9, 0x4325, + 0x6C24, 0x447A, + 0x6B4A, 0x45CD, + 0x6A6D, 0x471C, + 0x698C, 0x4869, + 0x68A6, 0x49B4, + 0x67BD, 0x4AFB, + 0x66CF, 0x4C3F, + 0x65DD, 0x4D81, + 0x64E8, 0x4EBF, + 0x63EF, 0x4FFB, + 0x62F2, 0x5133, + 0x61F1, 0x5269, + 0x60EC, 0x539B, + 0x5FE3, 0x54CA, + 0x5ED7, 0x55F5, + 0x5DC7, 0x571D, + 0x5CB4, 0x5842, + 0x5B9D, 0x5964, + 0x5A82, 0x5A82, + 0x5964, 0x5B9D, + 0x5842, 0x5CB4, + 0x571D, 0x5DC7, + 0x55F5, 0x5ED7, + 0x54CA, 0x5FE3, + 0x539B, 0x60EC, + 0x5269, 0x61F1, + 0x5133, 0x62F2, + 0x4FFB, 0x63EF, + 0x4EBF, 0x64E8, + 0x4D81, 0x65DD, + 0x4C3F, 0x66CF, + 0x4AFB, 0x67BD, + 0x49B4, 0x68A6, + 0x4869, 0x698C, + 0x471C, 0x6A6D, + 0x45CD, 0x6B4A, + 0x447A, 0x6C24, + 0x4325, 0x6CF9, + 0x41CE, 0x6DCA, + 0x4073, 0x6E96, + 0x3F17, 0x6F5F, + 0x3DB8, 0x7023, + 0x3C56, 0x70E2, + 0x3AF2, 0x719E, + 0x398C, 0x7255, + 0x3824, 0x7307, + 0x36BA, 0x73B5, + 0x354D, 0x745F, + 0x33DE, 0x7504, + 0x326E, 0x75A5, + 0x30FB, 0x7641, + 0x2F87, 0x76D9, + 0x2E11, 0x776C, + 0x2C98, 0x77FA, + 0x2B1F, 0x7884, + 0x29A3, 0x7909, + 0x2826, 0x798A, + 0x26A8, 0x7A05, + 0x2528, 0x7A7D, + 0x23A6, 0x7AEF, + 0x2223, 0x7B5D, + 0x209F, 0x7BC5, + 0x1F19, 0x7C29, + 0x1D93, 0x7C89, + 0x1C0B, 0x7CE3, + 0x1A82, 0x7D39, + 0x18F8, 0x7D8A, + 0x176D, 0x7DD6, + 0x15E2, 0x7E1D, + 0x1455, 0x7E5F, + 0x12C8, 0x7E9D, + 0x1139, 0x7ED5, + 0x0FAB, 0x7F09, + 0x0E1B, 0x7F38, + 0x0C8B, 0x7F62, + 0x0AFB, 0x7F87, + 0x096A, 0x7FA7, + 0x07D9, 0x7FC2, + 0x0647, 0x7FD8, + 0x04B6, 0x7FE9, + 0x0324, 0x7FF6, + 0x0192, 0x7FFD, + 0x0000, 0x7FFF, + 0xFE6D, 0x7FFD, + 0xFCDB, 0x7FF6, + 0xFB49, 0x7FE9, + 0xF9B8, 0x7FD8, + 0xF826, 0x7FC2, + 0xF695, 0x7FA7, + 0xF504, 0x7F87, + 0xF374, 0x7F62, + 0xF1E4, 0x7F38, + 0xF054, 0x7F09, + 0xEEC6, 0x7ED5, + 0xED37, 0x7E9D, + 0xEBAA, 0x7E5F, + 0xEA1D, 0x7E1D, + 0xE892, 0x7DD6, + 0xE707, 0x7D8A, + 0xE57D, 0x7D39, + 0xE3F4, 0x7CE3, + 0xE26C, 0x7C89, + 0xE0E6, 0x7C29, + 0xDF60, 0x7BC5, + 0xDDDC, 0x7B5D, + 0xDC59, 0x7AEF, + 0xDAD7, 0x7A7D, + 0xD957, 0x7A05, + 0xD7D9, 0x798A, + 0xD65C, 0x7909, + 0xD4E0, 0x7884, + 0xD367, 0x77FA, + 0xD1EE, 0x776C, + 0xD078, 0x76D9, + 0xCF04, 0x7641, + 0xCD91, 0x75A5, + 0xCC21, 0x7504, + 0xCAB2, 0x745F, + 0xC945, 0x73B5, + 0xC7DB, 0x7307, + 0xC673, 0x7255, + 0xC50D, 0x719E, + 0xC3A9, 0x70E2, + 0xC247, 0x7023, + 0xC0E8, 0x6F5F, + 0xBF8C, 0x6E96, + 0xBE31, 0x6DCA, + 0xBCDA, 0x6CF9, + 0xBB85, 0x6C24, + 0xBA32, 0x6B4A, + 0xB8E3, 0x6A6D, + 0xB796, 0x698C, + 0xB64B, 0x68A6, + 0xB504, 0x67BD, + 0xB3C0, 0x66CF, + 0xB27E, 0x65DD, + 0xB140, 0x64E8, + 0xB004, 0x63EF, + 0xAECC, 0x62F2, + 0xAD96, 0x61F1, + 0xAC64, 0x60EC, + 0xAB35, 0x5FE3, + 0xAA0A, 0x5ED7, + 0xA8E2, 0x5DC7, + 0xA7BD, 0x5CB4, + 0xA69B, 0x5B9D, + 0xA57D, 0x5A82, + 0xA462, 0x5964, + 0xA34B, 0x5842, + 0xA238, 0x571D, + 0xA128, 0x55F5, + 0xA01C, 0x54CA, + 0x9F13, 0x539B, + 0x9E0E, 0x5269, + 0x9D0D, 0x5133, + 0x9C10, 0x4FFB, + 0x9B17, 0x4EBF, + 0x9A22, 0x4D81, + 0x9930, 0x4C3F, + 0x9842, 0x4AFB, + 0x9759, 0x49B4, + 0x9673, 0x4869, + 0x9592, 0x471C, + 0x94B5, 0x45CD, + 0x93DB, 0x447A, + 0x9306, 0x4325, + 0x9235, 0x41CE, + 0x9169, 0x4073, + 0x90A0, 0x3F17, + 0x8FDC, 0x3DB8, + 0x8F1D, 0x3C56, + 0x8E61, 0x3AF2, + 0x8DAA, 0x398C, + 0x8CF8, 0x3824, + 0x8C4A, 0x36BA, + 0x8BA0, 0x354D, + 0x8AFB, 0x33DE, + 0x8A5A, 0x326E, + 0x89BE, 0x30FB, + 0x8926, 0x2F87, + 0x8893, 0x2E11, + 0x8805, 0x2C98, + 0x877B, 0x2B1F, + 0x86F6, 0x29A3, + 0x8675, 0x2826, + 0x85FA, 0x26A8, + 0x8582, 0x2528, + 0x8510, 0x23A6, + 0x84A2, 0x2223, + 0x843A, 0x209F, + 0x83D6, 0x1F19, + 0x8376, 0x1D93, + 0x831C, 0x1C0B, + 0x82C6, 0x1A82, + 0x8275, 0x18F8, + 0x8229, 0x176D, + 0x81E2, 0x15E2, + 0x81A0, 0x1455, + 0x8162, 0x12C8, + 0x812A, 0x1139, + 0x80F6, 0x0FAB, + 0x80C7, 0x0E1B, + 0x809D, 0x0C8B, + 0x8078, 0x0AFB, + 0x8058, 0x096A, + 0x803D, 0x07D9, + 0x8027, 0x0647, + 0x8016, 0x04B6, + 0x8009, 0x0324, + 0x8002, 0x0192, + 0x8000, 0x0000, + 0x8002, 0xFE6D, + 0x8009, 0xFCDB, + 0x8016, 0xFB49, + 0x8027, 0xF9B8, + 0x803D, 0xF826, + 0x8058, 0xF695, + 0x8078, 0xF504, + 0x809D, 0xF374, + 0x80C7, 0xF1E4, + 0x80F6, 0xF054, + 0x812A, 0xEEC6, + 0x8162, 0xED37, + 0x81A0, 0xEBAA, + 0x81E2, 0xEA1D, + 0x8229, 0xE892, + 0x8275, 0xE707, + 0x82C6, 0xE57D, + 0x831C, 0xE3F4, + 0x8376, 0xE26C, + 0x83D6, 0xE0E6, + 0x843A, 0xDF60, + 0x84A2, 0xDDDC, + 0x8510, 0xDC59, + 0x8582, 0xDAD7, + 0x85FA, 0xD957, + 0x8675, 0xD7D9, + 0x86F6, 0xD65C, + 0x877B, 0xD4E0, + 0x8805, 0xD367, + 0x8893, 0xD1EE, + 0x8926, 0xD078, + 0x89BE, 0xCF04, + 0x8A5A, 0xCD91, + 0x8AFB, 0xCC21, + 0x8BA0, 0xCAB2, + 0x8C4A, 0xC945, + 0x8CF8, 0xC7DB, + 0x8DAA, 0xC673, + 0x8E61, 0xC50D, + 0x8F1D, 0xC3A9, + 0x8FDC, 0xC247, + 0x90A0, 0xC0E8, + 0x9169, 0xBF8C, + 0x9235, 0xBE31, + 0x9306, 0xBCDA, + 0x93DB, 0xBB85, + 0x94B5, 0xBA32, + 0x9592, 0xB8E3, + 0x9673, 0xB796, + 0x9759, 0xB64B, + 0x9842, 0xB504, + 0x9930, 0xB3C0, + 0x9A22, 0xB27E, + 0x9B17, 0xB140, + 0x9C10, 0xB004, + 0x9D0D, 0xAECC, + 0x9E0E, 0xAD96, + 0x9F13, 0xAC64, + 0xA01C, 0xAB35, + 0xA128, 0xAA0A, + 0xA238, 0xA8E2, + 0xA34B, 0xA7BD, + 0xA462, 0xA69B, + 0xA57D, 0xA57D, + 0xA69B, 0xA462, + 0xA7BD, 0xA34B, + 0xA8E2, 0xA238, + 0xAA0A, 0xA128, + 0xAB35, 0xA01C, + 0xAC64, 0x9F13, + 0xAD96, 0x9E0E, + 0xAECC, 0x9D0D, + 0xB004, 0x9C10, + 0xB140, 0x9B17, + 0xB27E, 0x9A22, + 0xB3C0, 0x9930, + 0xB504, 0x9842, + 0xB64B, 0x9759, + 0xB796, 0x9673, + 0xB8E3, 0x9592, + 0xBA32, 0x94B5, + 0xBB85, 0x93DB, + 0xBCDA, 0x9306, + 0xBE31, 0x9235, + 0xBF8C, 0x9169, + 0xC0E8, 0x90A0, + 0xC247, 0x8FDC, + 0xC3A9, 0x8F1D, + 0xC50D, 0x8E61, + 0xC673, 0x8DAA, + 0xC7DB, 0x8CF8, + 0xC945, 0x8C4A, + 0xCAB2, 0x8BA0, + 0xCC21, 0x8AFB, + 0xCD91, 0x8A5A, + 0xCF04, 0x89BE, + 0xD078, 0x8926, + 0xD1EE, 0x8893, + 0xD367, 0x8805, + 0xD4E0, 0x877B, + 0xD65C, 0x86F6, + 0xD7D9, 0x8675, + 0xD957, 0x85FA, + 0xDAD7, 0x8582, + 0xDC59, 0x8510, + 0xDDDC, 0x84A2, + 0xDF60, 0x843A, + 0xE0E6, 0x83D6, + 0xE26C, 0x8376, + 0xE3F4, 0x831C, + 0xE57D, 0x82C6, + 0xE707, 0x8275, + 0xE892, 0x8229, + 0xEA1D, 0x81E2, + 0xEBAA, 0x81A0, + 0xED37, 0x8162, + 0xEEC6, 0x812A, + 0xF054, 0x80F6, + 0xF1E4, 0x80C7, + 0xF374, 0x809D, + 0xF504, 0x8078, + 0xF695, 0x8058, + 0xF826, 0x803D, + 0xF9B8, 0x8027, + 0xFB49, 0x8016, + 0xFCDB, 0x8009, + 0xFE6D, 0x8002 +}; + +/** +* \par +* Example code for q15 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefq15[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefq15[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 1024 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to q15(Fixed point 1.15): +* round(twiddleCoefq15(i) * pow(2, 15)) +* +*/ +const q15_t twiddleCoef_1024_q15[1536] = { + 0x7FFF, 0x0000, + 0x7FFF, 0x00C9, + 0x7FFD, 0x0192, + 0x7FFA, 0x025B, + 0x7FF6, 0x0324, + 0x7FF0, 0x03ED, + 0x7FE9, 0x04B6, + 0x7FE1, 0x057F, + 0x7FD8, 0x0647, + 0x7FCE, 0x0710, + 0x7FC2, 0x07D9, + 0x7FB5, 0x08A2, + 0x7FA7, 0x096A, + 0x7F97, 0x0A33, + 0x7F87, 0x0AFB, + 0x7F75, 0x0BC3, + 0x7F62, 0x0C8B, + 0x7F4D, 0x0D53, + 0x7F38, 0x0E1B, + 0x7F21, 0x0EE3, + 0x7F09, 0x0FAB, + 0x7EF0, 0x1072, + 0x7ED5, 0x1139, + 0x7EBA, 0x1201, + 0x7E9D, 0x12C8, + 0x7E7F, 0x138E, + 0x7E5F, 0x1455, + 0x7E3F, 0x151B, + 0x7E1D, 0x15E2, + 0x7DFA, 0x16A8, + 0x7DD6, 0x176D, + 0x7DB0, 0x1833, + 0x7D8A, 0x18F8, + 0x7D62, 0x19BD, + 0x7D39, 0x1A82, + 0x7D0F, 0x1B47, + 0x7CE3, 0x1C0B, + 0x7CB7, 0x1CCF, + 0x7C89, 0x1D93, + 0x7C5A, 0x1E56, + 0x7C29, 0x1F19, + 0x7BF8, 0x1FDC, + 0x7BC5, 0x209F, + 0x7B92, 0x2161, + 0x7B5D, 0x2223, + 0x7B26, 0x22E5, + 0x7AEF, 0x23A6, + 0x7AB6, 0x2467, + 0x7A7D, 0x2528, + 0x7A42, 0x25E8, + 0x7A05, 0x26A8, + 0x79C8, 0x2767, + 0x798A, 0x2826, + 0x794A, 0x28E5, + 0x7909, 0x29A3, + 0x78C7, 0x2A61, + 0x7884, 0x2B1F, + 0x7840, 0x2BDC, + 0x77FA, 0x2C98, + 0x77B4, 0x2D55, + 0x776C, 0x2E11, + 0x7723, 0x2ECC, + 0x76D9, 0x2F87, + 0x768E, 0x3041, + 0x7641, 0x30FB, + 0x75F4, 0x31B5, + 0x75A5, 0x326E, + 0x7555, 0x3326, + 0x7504, 0x33DE, + 0x74B2, 0x3496, + 0x745F, 0x354D, + 0x740B, 0x3604, + 0x73B5, 0x36BA, + 0x735F, 0x376F, + 0x7307, 0x3824, + 0x72AF, 0x38D8, + 0x7255, 0x398C, + 0x71FA, 0x3A40, + 0x719E, 0x3AF2, + 0x7141, 0x3BA5, + 0x70E2, 0x3C56, + 0x7083, 0x3D07, + 0x7023, 0x3DB8, + 0x6FC1, 0x3E68, + 0x6F5F, 0x3F17, + 0x6EFB, 0x3FC5, + 0x6E96, 0x4073, + 0x6E30, 0x4121, + 0x6DCA, 0x41CE, + 0x6D62, 0x427A, + 0x6CF9, 0x4325, + 0x6C8F, 0x43D0, + 0x6C24, 0x447A, + 0x6BB8, 0x4524, + 0x6B4A, 0x45CD, + 0x6ADC, 0x4675, + 0x6A6D, 0x471C, + 0x69FD, 0x47C3, + 0x698C, 0x4869, + 0x6919, 0x490F, + 0x68A6, 0x49B4, + 0x6832, 0x4A58, + 0x67BD, 0x4AFB, + 0x6746, 0x4B9E, + 0x66CF, 0x4C3F, + 0x6657, 0x4CE1, + 0x65DD, 0x4D81, + 0x6563, 0x4E21, + 0x64E8, 0x4EBF, + 0x646C, 0x4F5E, + 0x63EF, 0x4FFB, + 0x6371, 0x5097, + 0x62F2, 0x5133, + 0x6271, 0x51CE, + 0x61F1, 0x5269, + 0x616F, 0x5302, + 0x60EC, 0x539B, + 0x6068, 0x5433, + 0x5FE3, 0x54CA, + 0x5F5E, 0x5560, + 0x5ED7, 0x55F5, + 0x5E50, 0x568A, + 0x5DC7, 0x571D, + 0x5D3E, 0x57B0, + 0x5CB4, 0x5842, + 0x5C29, 0x58D4, + 0x5B9D, 0x5964, + 0x5B10, 0x59F3, + 0x5A82, 0x5A82, + 0x59F3, 0x5B10, + 0x5964, 0x5B9D, + 0x58D4, 0x5C29, + 0x5842, 0x5CB4, + 0x57B0, 0x5D3E, + 0x571D, 0x5DC7, + 0x568A, 0x5E50, + 0x55F5, 0x5ED7, + 0x5560, 0x5F5E, + 0x54CA, 0x5FE3, + 0x5433, 0x6068, + 0x539B, 0x60EC, + 0x5302, 0x616F, + 0x5269, 0x61F1, + 0x51CE, 0x6271, + 0x5133, 0x62F2, + 0x5097, 0x6371, + 0x4FFB, 0x63EF, + 0x4F5E, 0x646C, + 0x4EBF, 0x64E8, + 0x4E21, 0x6563, + 0x4D81, 0x65DD, + 0x4CE1, 0x6657, + 0x4C3F, 0x66CF, + 0x4B9E, 0x6746, + 0x4AFB, 0x67BD, + 0x4A58, 0x6832, + 0x49B4, 0x68A6, + 0x490F, 0x6919, + 0x4869, 0x698C, + 0x47C3, 0x69FD, + 0x471C, 0x6A6D, + 0x4675, 0x6ADC, + 0x45CD, 0x6B4A, + 0x4524, 0x6BB8, + 0x447A, 0x6C24, + 0x43D0, 0x6C8F, + 0x4325, 0x6CF9, + 0x427A, 0x6D62, + 0x41CE, 0x6DCA, + 0x4121, 0x6E30, + 0x4073, 0x6E96, + 0x3FC5, 0x6EFB, + 0x3F17, 0x6F5F, + 0x3E68, 0x6FC1, + 0x3DB8, 0x7023, + 0x3D07, 0x7083, + 0x3C56, 0x70E2, + 0x3BA5, 0x7141, + 0x3AF2, 0x719E, + 0x3A40, 0x71FA, + 0x398C, 0x7255, + 0x38D8, 0x72AF, + 0x3824, 0x7307, + 0x376F, 0x735F, + 0x36BA, 0x73B5, + 0x3604, 0x740B, + 0x354D, 0x745F, + 0x3496, 0x74B2, + 0x33DE, 0x7504, + 0x3326, 0x7555, + 0x326E, 0x75A5, + 0x31B5, 0x75F4, + 0x30FB, 0x7641, + 0x3041, 0x768E, + 0x2F87, 0x76D9, + 0x2ECC, 0x7723, + 0x2E11, 0x776C, + 0x2D55, 0x77B4, + 0x2C98, 0x77FA, + 0x2BDC, 0x7840, + 0x2B1F, 0x7884, + 0x2A61, 0x78C7, + 0x29A3, 0x7909, + 0x28E5, 0x794A, + 0x2826, 0x798A, + 0x2767, 0x79C8, + 0x26A8, 0x7A05, + 0x25E8, 0x7A42, + 0x2528, 0x7A7D, + 0x2467, 0x7AB6, + 0x23A6, 0x7AEF, + 0x22E5, 0x7B26, + 0x2223, 0x7B5D, + 0x2161, 0x7B92, + 0x209F, 0x7BC5, + 0x1FDC, 0x7BF8, + 0x1F19, 0x7C29, + 0x1E56, 0x7C5A, + 0x1D93, 0x7C89, + 0x1CCF, 0x7CB7, + 0x1C0B, 0x7CE3, + 0x1B47, 0x7D0F, + 0x1A82, 0x7D39, + 0x19BD, 0x7D62, + 0x18F8, 0x7D8A, + 0x1833, 0x7DB0, + 0x176D, 0x7DD6, + 0x16A8, 0x7DFA, + 0x15E2, 0x7E1D, + 0x151B, 0x7E3F, + 0x1455, 0x7E5F, + 0x138E, 0x7E7F, + 0x12C8, 0x7E9D, + 0x1201, 0x7EBA, + 0x1139, 0x7ED5, + 0x1072, 0x7EF0, + 0x0FAB, 0x7F09, + 0x0EE3, 0x7F21, + 0x0E1B, 0x7F38, + 0x0D53, 0x7F4D, + 0x0C8B, 0x7F62, + 0x0BC3, 0x7F75, + 0x0AFB, 0x7F87, + 0x0A33, 0x7F97, + 0x096A, 0x7FA7, + 0x08A2, 0x7FB5, + 0x07D9, 0x7FC2, + 0x0710, 0x7FCE, + 0x0647, 0x7FD8, + 0x057F, 0x7FE1, + 0x04B6, 0x7FE9, + 0x03ED, 0x7FF0, + 0x0324, 0x7FF6, + 0x025B, 0x7FFA, + 0x0192, 0x7FFD, + 0x00C9, 0x7FFF, + 0x0000, 0x7FFF, + 0xFF36, 0x7FFF, + 0xFE6D, 0x7FFD, + 0xFDA4, 0x7FFA, + 0xFCDB, 0x7FF6, + 0xFC12, 0x7FF0, + 0xFB49, 0x7FE9, + 0xFA80, 0x7FE1, + 0xF9B8, 0x7FD8, + 0xF8EF, 0x7FCE, + 0xF826, 0x7FC2, + 0xF75D, 0x7FB5, + 0xF695, 0x7FA7, + 0xF5CC, 0x7F97, + 0xF504, 0x7F87, + 0xF43C, 0x7F75, + 0xF374, 0x7F62, + 0xF2AC, 0x7F4D, + 0xF1E4, 0x7F38, + 0xF11C, 0x7F21, + 0xF054, 0x7F09, + 0xEF8D, 0x7EF0, + 0xEEC6, 0x7ED5, + 0xEDFE, 0x7EBA, + 0xED37, 0x7E9D, + 0xEC71, 0x7E7F, + 0xEBAA, 0x7E5F, + 0xEAE4, 0x7E3F, + 0xEA1D, 0x7E1D, + 0xE957, 0x7DFA, + 0xE892, 0x7DD6, + 0xE7CC, 0x7DB0, + 0xE707, 0x7D8A, + 0xE642, 0x7D62, + 0xE57D, 0x7D39, + 0xE4B8, 0x7D0F, + 0xE3F4, 0x7CE3, + 0xE330, 0x7CB7, + 0xE26C, 0x7C89, + 0xE1A9, 0x7C5A, + 0xE0E6, 0x7C29, + 0xE023, 0x7BF8, + 0xDF60, 0x7BC5, + 0xDE9E, 0x7B92, + 0xDDDC, 0x7B5D, + 0xDD1A, 0x7B26, + 0xDC59, 0x7AEF, + 0xDB98, 0x7AB6, + 0xDAD7, 0x7A7D, + 0xDA17, 0x7A42, + 0xD957, 0x7A05, + 0xD898, 0x79C8, + 0xD7D9, 0x798A, + 0xD71A, 0x794A, + 0xD65C, 0x7909, + 0xD59E, 0x78C7, + 0xD4E0, 0x7884, + 0xD423, 0x7840, + 0xD367, 0x77FA, + 0xD2AA, 0x77B4, + 0xD1EE, 0x776C, + 0xD133, 0x7723, + 0xD078, 0x76D9, + 0xCFBE, 0x768E, + 0xCF04, 0x7641, + 0xCE4A, 0x75F4, + 0xCD91, 0x75A5, + 0xCCD9, 0x7555, + 0xCC21, 0x7504, + 0xCB69, 0x74B2, + 0xCAB2, 0x745F, + 0xC9FB, 0x740B, + 0xC945, 0x73B5, + 0xC890, 0x735F, + 0xC7DB, 0x7307, + 0xC727, 0x72AF, + 0xC673, 0x7255, + 0xC5BF, 0x71FA, + 0xC50D, 0x719E, + 0xC45A, 0x7141, + 0xC3A9, 0x70E2, + 0xC2F8, 0x7083, + 0xC247, 0x7023, + 0xC197, 0x6FC1, + 0xC0E8, 0x6F5F, + 0xC03A, 0x6EFB, + 0xBF8C, 0x6E96, + 0xBEDE, 0x6E30, + 0xBE31, 0x6DCA, + 0xBD85, 0x6D62, + 0xBCDA, 0x6CF9, + 0xBC2F, 0x6C8F, + 0xBB85, 0x6C24, + 0xBADB, 0x6BB8, + 0xBA32, 0x6B4A, + 0xB98A, 0x6ADC, + 0xB8E3, 0x6A6D, + 0xB83C, 0x69FD, + 0xB796, 0x698C, + 0xB6F0, 0x6919, + 0xB64B, 0x68A6, + 0xB5A7, 0x6832, + 0xB504, 0x67BD, + 0xB461, 0x6746, + 0xB3C0, 0x66CF, + 0xB31E, 0x6657, + 0xB27E, 0x65DD, + 0xB1DE, 0x6563, + 0xB140, 0x64E8, + 0xB0A1, 0x646C, + 0xB004, 0x63EF, + 0xAF68, 0x6371, + 0xAECC, 0x62F2, + 0xAE31, 0x6271, + 0xAD96, 0x61F1, + 0xACFD, 0x616F, + 0xAC64, 0x60EC, + 0xABCC, 0x6068, + 0xAB35, 0x5FE3, + 0xAA9F, 0x5F5E, + 0xAA0A, 0x5ED7, + 0xA975, 0x5E50, + 0xA8E2, 0x5DC7, + 0xA84F, 0x5D3E, + 0xA7BD, 0x5CB4, + 0xA72B, 0x5C29, + 0xA69B, 0x5B9D, + 0xA60C, 0x5B10, + 0xA57D, 0x5A82, + 0xA4EF, 0x59F3, + 0xA462, 0x5964, + 0xA3D6, 0x58D4, + 0xA34B, 0x5842, + 0xA2C1, 0x57B0, + 0xA238, 0x571D, + 0xA1AF, 0x568A, + 0xA128, 0x55F5, + 0xA0A1, 0x5560, + 0xA01C, 0x54CA, + 0x9F97, 0x5433, + 0x9F13, 0x539B, + 0x9E90, 0x5302, + 0x9E0E, 0x5269, + 0x9D8E, 0x51CE, + 0x9D0D, 0x5133, + 0x9C8E, 0x5097, + 0x9C10, 0x4FFB, + 0x9B93, 0x4F5E, + 0x9B17, 0x4EBF, + 0x9A9C, 0x4E21, + 0x9A22, 0x4D81, + 0x99A8, 0x4CE1, + 0x9930, 0x4C3F, + 0x98B9, 0x4B9E, + 0x9842, 0x4AFB, + 0x97CD, 0x4A58, + 0x9759, 0x49B4, + 0x96E6, 0x490F, + 0x9673, 0x4869, + 0x9602, 0x47C3, + 0x9592, 0x471C, + 0x9523, 0x4675, + 0x94B5, 0x45CD, + 0x9447, 0x4524, + 0x93DB, 0x447A, + 0x9370, 0x43D0, + 0x9306, 0x4325, + 0x929D, 0x427A, + 0x9235, 0x41CE, + 0x91CF, 0x4121, + 0x9169, 0x4073, + 0x9104, 0x3FC5, + 0x90A0, 0x3F17, + 0x903E, 0x3E68, + 0x8FDC, 0x3DB8, + 0x8F7C, 0x3D07, + 0x8F1D, 0x3C56, + 0x8EBE, 0x3BA5, + 0x8E61, 0x3AF2, + 0x8E05, 0x3A40, + 0x8DAA, 0x398C, + 0x8D50, 0x38D8, + 0x8CF8, 0x3824, + 0x8CA0, 0x376F, + 0x8C4A, 0x36BA, + 0x8BF4, 0x3604, + 0x8BA0, 0x354D, + 0x8B4D, 0x3496, + 0x8AFB, 0x33DE, + 0x8AAA, 0x3326, + 0x8A5A, 0x326E, + 0x8A0B, 0x31B5, + 0x89BE, 0x30FB, + 0x8971, 0x3041, + 0x8926, 0x2F87, + 0x88DC, 0x2ECC, + 0x8893, 0x2E11, + 0x884B, 0x2D55, + 0x8805, 0x2C98, + 0x87BF, 0x2BDC, + 0x877B, 0x2B1F, + 0x8738, 0x2A61, + 0x86F6, 0x29A3, + 0x86B5, 0x28E5, + 0x8675, 0x2826, + 0x8637, 0x2767, + 0x85FA, 0x26A8, + 0x85BD, 0x25E8, + 0x8582, 0x2528, + 0x8549, 0x2467, + 0x8510, 0x23A6, + 0x84D9, 0x22E5, + 0x84A2, 0x2223, + 0x846D, 0x2161, + 0x843A, 0x209F, + 0x8407, 0x1FDC, + 0x83D6, 0x1F19, + 0x83A5, 0x1E56, + 0x8376, 0x1D93, + 0x8348, 0x1CCF, + 0x831C, 0x1C0B, + 0x82F0, 0x1B47, + 0x82C6, 0x1A82, + 0x829D, 0x19BD, + 0x8275, 0x18F8, + 0x824F, 0x1833, + 0x8229, 0x176D, + 0x8205, 0x16A8, + 0x81E2, 0x15E2, + 0x81C0, 0x151B, + 0x81A0, 0x1455, + 0x8180, 0x138E, + 0x8162, 0x12C8, + 0x8145, 0x1201, + 0x812A, 0x1139, + 0x810F, 0x1072, + 0x80F6, 0x0FAB, + 0x80DE, 0x0EE3, + 0x80C7, 0x0E1B, + 0x80B2, 0x0D53, + 0x809D, 0x0C8B, + 0x808A, 0x0BC3, + 0x8078, 0x0AFB, + 0x8068, 0x0A33, + 0x8058, 0x096A, + 0x804A, 0x08A2, + 0x803D, 0x07D9, + 0x8031, 0x0710, + 0x8027, 0x0647, + 0x801E, 0x057F, + 0x8016, 0x04B6, + 0x800F, 0x03ED, + 0x8009, 0x0324, + 0x8005, 0x025B, + 0x8002, 0x0192, + 0x8000, 0x00C9, + 0x8000, 0x0000, + 0x8000, 0xFF36, + 0x8002, 0xFE6D, + 0x8005, 0xFDA4, + 0x8009, 0xFCDB, + 0x800F, 0xFC12, + 0x8016, 0xFB49, + 0x801E, 0xFA80, + 0x8027, 0xF9B8, + 0x8031, 0xF8EF, + 0x803D, 0xF826, + 0x804A, 0xF75D, + 0x8058, 0xF695, + 0x8068, 0xF5CC, + 0x8078, 0xF504, + 0x808A, 0xF43C, + 0x809D, 0xF374, + 0x80B2, 0xF2AC, + 0x80C7, 0xF1E4, + 0x80DE, 0xF11C, + 0x80F6, 0xF054, + 0x810F, 0xEF8D, + 0x812A, 0xEEC6, + 0x8145, 0xEDFE, + 0x8162, 0xED37, + 0x8180, 0xEC71, + 0x81A0, 0xEBAA, + 0x81C0, 0xEAE4, + 0x81E2, 0xEA1D, + 0x8205, 0xE957, + 0x8229, 0xE892, + 0x824F, 0xE7CC, + 0x8275, 0xE707, + 0x829D, 0xE642, + 0x82C6, 0xE57D, + 0x82F0, 0xE4B8, + 0x831C, 0xE3F4, + 0x8348, 0xE330, + 0x8376, 0xE26C, + 0x83A5, 0xE1A9, + 0x83D6, 0xE0E6, + 0x8407, 0xE023, + 0x843A, 0xDF60, + 0x846D, 0xDE9E, + 0x84A2, 0xDDDC, + 0x84D9, 0xDD1A, + 0x8510, 0xDC59, + 0x8549, 0xDB98, + 0x8582, 0xDAD7, + 0x85BD, 0xDA17, + 0x85FA, 0xD957, + 0x8637, 0xD898, + 0x8675, 0xD7D9, + 0x86B5, 0xD71A, + 0x86F6, 0xD65C, + 0x8738, 0xD59E, + 0x877B, 0xD4E0, + 0x87BF, 0xD423, + 0x8805, 0xD367, + 0x884B, 0xD2AA, + 0x8893, 0xD1EE, + 0x88DC, 0xD133, + 0x8926, 0xD078, + 0x8971, 0xCFBE, + 0x89BE, 0xCF04, + 0x8A0B, 0xCE4A, + 0x8A5A, 0xCD91, + 0x8AAA, 0xCCD9, + 0x8AFB, 0xCC21, + 0x8B4D, 0xCB69, + 0x8BA0, 0xCAB2, + 0x8BF4, 0xC9FB, + 0x8C4A, 0xC945, + 0x8CA0, 0xC890, + 0x8CF8, 0xC7DB, + 0x8D50, 0xC727, + 0x8DAA, 0xC673, + 0x8E05, 0xC5BF, + 0x8E61, 0xC50D, + 0x8EBE, 0xC45A, + 0x8F1D, 0xC3A9, + 0x8F7C, 0xC2F8, + 0x8FDC, 0xC247, + 0x903E, 0xC197, + 0x90A0, 0xC0E8, + 0x9104, 0xC03A, + 0x9169, 0xBF8C, + 0x91CF, 0xBEDE, + 0x9235, 0xBE31, + 0x929D, 0xBD85, + 0x9306, 0xBCDA, + 0x9370, 0xBC2F, + 0x93DB, 0xBB85, + 0x9447, 0xBADB, + 0x94B5, 0xBA32, + 0x9523, 0xB98A, + 0x9592, 0xB8E3, + 0x9602, 0xB83C, + 0x9673, 0xB796, + 0x96E6, 0xB6F0, + 0x9759, 0xB64B, + 0x97CD, 0xB5A7, + 0x9842, 0xB504, + 0x98B9, 0xB461, + 0x9930, 0xB3C0, + 0x99A8, 0xB31E, + 0x9A22, 0xB27E, + 0x9A9C, 0xB1DE, + 0x9B17, 0xB140, + 0x9B93, 0xB0A1, + 0x9C10, 0xB004, + 0x9C8E, 0xAF68, + 0x9D0D, 0xAECC, + 0x9D8E, 0xAE31, + 0x9E0E, 0xAD96, + 0x9E90, 0xACFD, + 0x9F13, 0xAC64, + 0x9F97, 0xABCC, + 0xA01C, 0xAB35, + 0xA0A1, 0xAA9F, + 0xA128, 0xAA0A, + 0xA1AF, 0xA975, + 0xA238, 0xA8E2, + 0xA2C1, 0xA84F, + 0xA34B, 0xA7BD, + 0xA3D6, 0xA72B, + 0xA462, 0xA69B, + 0xA4EF, 0xA60C, + 0xA57D, 0xA57D, + 0xA60C, 0xA4EF, + 0xA69B, 0xA462, + 0xA72B, 0xA3D6, + 0xA7BD, 0xA34B, + 0xA84F, 0xA2C1, + 0xA8E2, 0xA238, + 0xA975, 0xA1AF, + 0xAA0A, 0xA128, + 0xAA9F, 0xA0A1, + 0xAB35, 0xA01C, + 0xABCC, 0x9F97, + 0xAC64, 0x9F13, + 0xACFD, 0x9E90, + 0xAD96, 0x9E0E, + 0xAE31, 0x9D8E, + 0xAECC, 0x9D0D, + 0xAF68, 0x9C8E, + 0xB004, 0x9C10, + 0xB0A1, 0x9B93, + 0xB140, 0x9B17, + 0xB1DE, 0x9A9C, + 0xB27E, 0x9A22, + 0xB31E, 0x99A8, + 0xB3C0, 0x9930, + 0xB461, 0x98B9, + 0xB504, 0x9842, + 0xB5A7, 0x97CD, + 0xB64B, 0x9759, + 0xB6F0, 0x96E6, + 0xB796, 0x9673, + 0xB83C, 0x9602, + 0xB8E3, 0x9592, + 0xB98A, 0x9523, + 0xBA32, 0x94B5, + 0xBADB, 0x9447, + 0xBB85, 0x93DB, + 0xBC2F, 0x9370, + 0xBCDA, 0x9306, + 0xBD85, 0x929D, + 0xBE31, 0x9235, + 0xBEDE, 0x91CF, + 0xBF8C, 0x9169, + 0xC03A, 0x9104, + 0xC0E8, 0x90A0, + 0xC197, 0x903E, + 0xC247, 0x8FDC, + 0xC2F8, 0x8F7C, + 0xC3A9, 0x8F1D, + 0xC45A, 0x8EBE, + 0xC50D, 0x8E61, + 0xC5BF, 0x8E05, + 0xC673, 0x8DAA, + 0xC727, 0x8D50, + 0xC7DB, 0x8CF8, + 0xC890, 0x8CA0, + 0xC945, 0x8C4A, + 0xC9FB, 0x8BF4, + 0xCAB2, 0x8BA0, + 0xCB69, 0x8B4D, + 0xCC21, 0x8AFB, + 0xCCD9, 0x8AAA, + 0xCD91, 0x8A5A, + 0xCE4A, 0x8A0B, + 0xCF04, 0x89BE, + 0xCFBE, 0x8971, + 0xD078, 0x8926, + 0xD133, 0x88DC, + 0xD1EE, 0x8893, + 0xD2AA, 0x884B, + 0xD367, 0x8805, + 0xD423, 0x87BF, + 0xD4E0, 0x877B, + 0xD59E, 0x8738, + 0xD65C, 0x86F6, + 0xD71A, 0x86B5, + 0xD7D9, 0x8675, + 0xD898, 0x8637, + 0xD957, 0x85FA, + 0xDA17, 0x85BD, + 0xDAD7, 0x8582, + 0xDB98, 0x8549, + 0xDC59, 0x8510, + 0xDD1A, 0x84D9, + 0xDDDC, 0x84A2, + 0xDE9E, 0x846D, + 0xDF60, 0x843A, + 0xE023, 0x8407, + 0xE0E6, 0x83D6, + 0xE1A9, 0x83A5, + 0xE26C, 0x8376, + 0xE330, 0x8348, + 0xE3F4, 0x831C, + 0xE4B8, 0x82F0, + 0xE57D, 0x82C6, + 0xE642, 0x829D, + 0xE707, 0x8275, + 0xE7CC, 0x824F, + 0xE892, 0x8229, + 0xE957, 0x8205, + 0xEA1D, 0x81E2, + 0xEAE4, 0x81C0, + 0xEBAA, 0x81A0, + 0xEC71, 0x8180, + 0xED37, 0x8162, + 0xEDFE, 0x8145, + 0xEEC6, 0x812A, + 0xEF8D, 0x810F, + 0xF054, 0x80F6, + 0xF11C, 0x80DE, + 0xF1E4, 0x80C7, + 0xF2AC, 0x80B2, + 0xF374, 0x809D, + 0xF43C, 0x808A, + 0xF504, 0x8078, + 0xF5CC, 0x8068, + 0xF695, 0x8058, + 0xF75D, 0x804A, + 0xF826, 0x803D, + 0xF8EF, 0x8031, + 0xF9B8, 0x8027, + 0xFA80, 0x801E, + 0xFB49, 0x8016, + 0xFC12, 0x800F, + 0xFCDB, 0x8009, + 0xFDA4, 0x8005, + 0xFE6D, 0x8002, + 0xFF36, 0x8000 +}; + +/** +* \par +* Example code for q15 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefq15[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefq15[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 2048 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to q15(Fixed point 1.15): +* round(twiddleCoefq15(i) * pow(2, 15)) +* +*/ +const q15_t twiddleCoef_2048_q15[3072] = { + 0x7FFF, 0x0000, + 0x7FFF, 0x0064, + 0x7FFF, 0x00C9, + 0x7FFE, 0x012D, + 0x7FFD, 0x0192, + 0x7FFC, 0x01F6, + 0x7FFA, 0x025B, + 0x7FF8, 0x02BF, + 0x7FF6, 0x0324, + 0x7FF3, 0x0388, + 0x7FF0, 0x03ED, + 0x7FED, 0x0451, + 0x7FE9, 0x04B6, + 0x7FE5, 0x051A, + 0x7FE1, 0x057F, + 0x7FDD, 0x05E3, + 0x7FD8, 0x0647, + 0x7FD3, 0x06AC, + 0x7FCE, 0x0710, + 0x7FC8, 0x0775, + 0x7FC2, 0x07D9, + 0x7FBC, 0x083D, + 0x7FB5, 0x08A2, + 0x7FAE, 0x0906, + 0x7FA7, 0x096A, + 0x7F9F, 0x09CE, + 0x7F97, 0x0A33, + 0x7F8F, 0x0A97, + 0x7F87, 0x0AFB, + 0x7F7E, 0x0B5F, + 0x7F75, 0x0BC3, + 0x7F6B, 0x0C27, + 0x7F62, 0x0C8B, + 0x7F58, 0x0CEF, + 0x7F4D, 0x0D53, + 0x7F43, 0x0DB7, + 0x7F38, 0x0E1B, + 0x7F2D, 0x0E7F, + 0x7F21, 0x0EE3, + 0x7F15, 0x0F47, + 0x7F09, 0x0FAB, + 0x7EFD, 0x100E, + 0x7EF0, 0x1072, + 0x7EE3, 0x10D6, + 0x7ED5, 0x1139, + 0x7EC8, 0x119D, + 0x7EBA, 0x1201, + 0x7EAB, 0x1264, + 0x7E9D, 0x12C8, + 0x7E8E, 0x132B, + 0x7E7F, 0x138E, + 0x7E6F, 0x13F2, + 0x7E5F, 0x1455, + 0x7E4F, 0x14B8, + 0x7E3F, 0x151B, + 0x7E2E, 0x157F, + 0x7E1D, 0x15E2, + 0x7E0C, 0x1645, + 0x7DFA, 0x16A8, + 0x7DE8, 0x170A, + 0x7DD6, 0x176D, + 0x7DC3, 0x17D0, + 0x7DB0, 0x1833, + 0x7D9D, 0x1896, + 0x7D8A, 0x18F8, + 0x7D76, 0x195B, + 0x7D62, 0x19BD, + 0x7D4E, 0x1A20, + 0x7D39, 0x1A82, + 0x7D24, 0x1AE4, + 0x7D0F, 0x1B47, + 0x7CF9, 0x1BA9, + 0x7CE3, 0x1C0B, + 0x7CCD, 0x1C6D, + 0x7CB7, 0x1CCF, + 0x7CA0, 0x1D31, + 0x7C89, 0x1D93, + 0x7C71, 0x1DF5, + 0x7C5A, 0x1E56, + 0x7C42, 0x1EB8, + 0x7C29, 0x1F19, + 0x7C11, 0x1F7B, + 0x7BF8, 0x1FDC, + 0x7BDF, 0x203E, + 0x7BC5, 0x209F, + 0x7BAC, 0x2100, + 0x7B92, 0x2161, + 0x7B77, 0x21C2, + 0x7B5D, 0x2223, + 0x7B42, 0x2284, + 0x7B26, 0x22E5, + 0x7B0B, 0x2345, + 0x7AEF, 0x23A6, + 0x7AD3, 0x2407, + 0x7AB6, 0x2467, + 0x7A9A, 0x24C7, + 0x7A7D, 0x2528, + 0x7A5F, 0x2588, + 0x7A42, 0x25E8, + 0x7A24, 0x2648, + 0x7A05, 0x26A8, + 0x79E7, 0x2707, + 0x79C8, 0x2767, + 0x79A9, 0x27C7, + 0x798A, 0x2826, + 0x796A, 0x2886, + 0x794A, 0x28E5, + 0x792A, 0x2944, + 0x7909, 0x29A3, + 0x78E8, 0x2A02, + 0x78C7, 0x2A61, + 0x78A6, 0x2AC0, + 0x7884, 0x2B1F, + 0x7862, 0x2B7D, + 0x7840, 0x2BDC, + 0x781D, 0x2C3A, + 0x77FA, 0x2C98, + 0x77D7, 0x2CF7, + 0x77B4, 0x2D55, + 0x7790, 0x2DB3, + 0x776C, 0x2E11, + 0x7747, 0x2E6E, + 0x7723, 0x2ECC, + 0x76FE, 0x2F29, + 0x76D9, 0x2F87, + 0x76B3, 0x2FE4, + 0x768E, 0x3041, + 0x7668, 0x309E, + 0x7641, 0x30FB, + 0x761B, 0x3158, + 0x75F4, 0x31B5, + 0x75CC, 0x3211, + 0x75A5, 0x326E, + 0x757D, 0x32CA, + 0x7555, 0x3326, + 0x752D, 0x3382, + 0x7504, 0x33DE, + 0x74DB, 0x343A, + 0x74B2, 0x3496, + 0x7489, 0x34F2, + 0x745F, 0x354D, + 0x7435, 0x35A8, + 0x740B, 0x3604, + 0x73E0, 0x365F, + 0x73B5, 0x36BA, + 0x738A, 0x3714, + 0x735F, 0x376F, + 0x7333, 0x37CA, + 0x7307, 0x3824, + 0x72DB, 0x387E, + 0x72AF, 0x38D8, + 0x7282, 0x3932, + 0x7255, 0x398C, + 0x7227, 0x39E6, + 0x71FA, 0x3A40, + 0x71CC, 0x3A99, + 0x719E, 0x3AF2, + 0x716F, 0x3B4C, + 0x7141, 0x3BA5, + 0x7112, 0x3BFD, + 0x70E2, 0x3C56, + 0x70B3, 0x3CAF, + 0x7083, 0x3D07, + 0x7053, 0x3D60, + 0x7023, 0x3DB8, + 0x6FF2, 0x3E10, + 0x6FC1, 0x3E68, + 0x6F90, 0x3EBF, + 0x6F5F, 0x3F17, + 0x6F2D, 0x3F6E, + 0x6EFB, 0x3FC5, + 0x6EC9, 0x401D, + 0x6E96, 0x4073, + 0x6E63, 0x40CA, + 0x6E30, 0x4121, + 0x6DFD, 0x4177, + 0x6DCA, 0x41CE, + 0x6D96, 0x4224, + 0x6D62, 0x427A, + 0x6D2D, 0x42D0, + 0x6CF9, 0x4325, + 0x6CC4, 0x437B, + 0x6C8F, 0x43D0, + 0x6C59, 0x4425, + 0x6C24, 0x447A, + 0x6BEE, 0x44CF, + 0x6BB8, 0x4524, + 0x6B81, 0x4578, + 0x6B4A, 0x45CD, + 0x6B13, 0x4621, + 0x6ADC, 0x4675, + 0x6AA5, 0x46C9, + 0x6A6D, 0x471C, + 0x6A35, 0x4770, + 0x69FD, 0x47C3, + 0x69C4, 0x4816, + 0x698C, 0x4869, + 0x6953, 0x48BC, + 0x6919, 0x490F, + 0x68E0, 0x4961, + 0x68A6, 0x49B4, + 0x686C, 0x4A06, + 0x6832, 0x4A58, + 0x67F7, 0x4AA9, + 0x67BD, 0x4AFB, + 0x6782, 0x4B4C, + 0x6746, 0x4B9E, + 0x670B, 0x4BEF, + 0x66CF, 0x4C3F, + 0x6693, 0x4C90, + 0x6657, 0x4CE1, + 0x661A, 0x4D31, + 0x65DD, 0x4D81, + 0x65A0, 0x4DD1, + 0x6563, 0x4E21, + 0x6526, 0x4E70, + 0x64E8, 0x4EBF, + 0x64AA, 0x4F0F, + 0x646C, 0x4F5E, + 0x642D, 0x4FAC, + 0x63EF, 0x4FFB, + 0x63B0, 0x5049, + 0x6371, 0x5097, + 0x6331, 0x50E5, + 0x62F2, 0x5133, + 0x62B2, 0x5181, + 0x6271, 0x51CE, + 0x6231, 0x521C, + 0x61F1, 0x5269, + 0x61B0, 0x52B5, + 0x616F, 0x5302, + 0x612D, 0x534E, + 0x60EC, 0x539B, + 0x60AA, 0x53E7, + 0x6068, 0x5433, + 0x6026, 0x547E, + 0x5FE3, 0x54CA, + 0x5FA0, 0x5515, + 0x5F5E, 0x5560, + 0x5F1A, 0x55AB, + 0x5ED7, 0x55F5, + 0x5E93, 0x5640, + 0x5E50, 0x568A, + 0x5E0B, 0x56D4, + 0x5DC7, 0x571D, + 0x5D83, 0x5767, + 0x5D3E, 0x57B0, + 0x5CF9, 0x57F9, + 0x5CB4, 0x5842, + 0x5C6E, 0x588B, + 0x5C29, 0x58D4, + 0x5BE3, 0x591C, + 0x5B9D, 0x5964, + 0x5B56, 0x59AC, + 0x5B10, 0x59F3, + 0x5AC9, 0x5A3B, + 0x5A82, 0x5A82, + 0x5A3B, 0x5AC9, + 0x59F3, 0x5B10, + 0x59AC, 0x5B56, + 0x5964, 0x5B9D, + 0x591C, 0x5BE3, + 0x58D4, 0x5C29, + 0x588B, 0x5C6E, + 0x5842, 0x5CB4, + 0x57F9, 0x5CF9, + 0x57B0, 0x5D3E, + 0x5767, 0x5D83, + 0x571D, 0x5DC7, + 0x56D4, 0x5E0B, + 0x568A, 0x5E50, + 0x5640, 0x5E93, + 0x55F5, 0x5ED7, + 0x55AB, 0x5F1A, + 0x5560, 0x5F5E, + 0x5515, 0x5FA0, + 0x54CA, 0x5FE3, + 0x547E, 0x6026, + 0x5433, 0x6068, + 0x53E7, 0x60AA, + 0x539B, 0x60EC, + 0x534E, 0x612D, + 0x5302, 0x616F, + 0x52B5, 0x61B0, + 0x5269, 0x61F1, + 0x521C, 0x6231, + 0x51CE, 0x6271, + 0x5181, 0x62B2, + 0x5133, 0x62F2, + 0x50E5, 0x6331, + 0x5097, 0x6371, + 0x5049, 0x63B0, + 0x4FFB, 0x63EF, + 0x4FAC, 0x642D, + 0x4F5E, 0x646C, + 0x4F0F, 0x64AA, + 0x4EBF, 0x64E8, + 0x4E70, 0x6526, + 0x4E21, 0x6563, + 0x4DD1, 0x65A0, + 0x4D81, 0x65DD, + 0x4D31, 0x661A, + 0x4CE1, 0x6657, + 0x4C90, 0x6693, + 0x4C3F, 0x66CF, + 0x4BEF, 0x670B, + 0x4B9E, 0x6746, + 0x4B4C, 0x6782, + 0x4AFB, 0x67BD, + 0x4AA9, 0x67F7, + 0x4A58, 0x6832, + 0x4A06, 0x686C, + 0x49B4, 0x68A6, + 0x4961, 0x68E0, + 0x490F, 0x6919, + 0x48BC, 0x6953, + 0x4869, 0x698C, + 0x4816, 0x69C4, + 0x47C3, 0x69FD, + 0x4770, 0x6A35, + 0x471C, 0x6A6D, + 0x46C9, 0x6AA5, + 0x4675, 0x6ADC, + 0x4621, 0x6B13, + 0x45CD, 0x6B4A, + 0x4578, 0x6B81, + 0x4524, 0x6BB8, + 0x44CF, 0x6BEE, + 0x447A, 0x6C24, + 0x4425, 0x6C59, + 0x43D0, 0x6C8F, + 0x437B, 0x6CC4, + 0x4325, 0x6CF9, + 0x42D0, 0x6D2D, + 0x427A, 0x6D62, + 0x4224, 0x6D96, + 0x41CE, 0x6DCA, + 0x4177, 0x6DFD, + 0x4121, 0x6E30, + 0x40CA, 0x6E63, + 0x4073, 0x6E96, + 0x401D, 0x6EC9, + 0x3FC5, 0x6EFB, + 0x3F6E, 0x6F2D, + 0x3F17, 0x6F5F, + 0x3EBF, 0x6F90, + 0x3E68, 0x6FC1, + 0x3E10, 0x6FF2, + 0x3DB8, 0x7023, + 0x3D60, 0x7053, + 0x3D07, 0x7083, + 0x3CAF, 0x70B3, + 0x3C56, 0x70E2, + 0x3BFD, 0x7112, + 0x3BA5, 0x7141, + 0x3B4C, 0x716F, + 0x3AF2, 0x719E, + 0x3A99, 0x71CC, + 0x3A40, 0x71FA, + 0x39E6, 0x7227, + 0x398C, 0x7255, + 0x3932, 0x7282, + 0x38D8, 0x72AF, + 0x387E, 0x72DB, + 0x3824, 0x7307, + 0x37CA, 0x7333, + 0x376F, 0x735F, + 0x3714, 0x738A, + 0x36BA, 0x73B5, + 0x365F, 0x73E0, + 0x3604, 0x740B, + 0x35A8, 0x7435, + 0x354D, 0x745F, + 0x34F2, 0x7489, + 0x3496, 0x74B2, + 0x343A, 0x74DB, + 0x33DE, 0x7504, + 0x3382, 0x752D, + 0x3326, 0x7555, + 0x32CA, 0x757D, + 0x326E, 0x75A5, + 0x3211, 0x75CC, + 0x31B5, 0x75F4, + 0x3158, 0x761B, + 0x30FB, 0x7641, + 0x309E, 0x7668, + 0x3041, 0x768E, + 0x2FE4, 0x76B3, + 0x2F87, 0x76D9, + 0x2F29, 0x76FE, + 0x2ECC, 0x7723, + 0x2E6E, 0x7747, + 0x2E11, 0x776C, + 0x2DB3, 0x7790, + 0x2D55, 0x77B4, + 0x2CF7, 0x77D7, + 0x2C98, 0x77FA, + 0x2C3A, 0x781D, + 0x2BDC, 0x7840, + 0x2B7D, 0x7862, + 0x2B1F, 0x7884, + 0x2AC0, 0x78A6, + 0x2A61, 0x78C7, + 0x2A02, 0x78E8, + 0x29A3, 0x7909, + 0x2944, 0x792A, + 0x28E5, 0x794A, + 0x2886, 0x796A, + 0x2826, 0x798A, + 0x27C7, 0x79A9, + 0x2767, 0x79C8, + 0x2707, 0x79E7, + 0x26A8, 0x7A05, + 0x2648, 0x7A24, + 0x25E8, 0x7A42, + 0x2588, 0x7A5F, + 0x2528, 0x7A7D, + 0x24C7, 0x7A9A, + 0x2467, 0x7AB6, + 0x2407, 0x7AD3, + 0x23A6, 0x7AEF, + 0x2345, 0x7B0B, + 0x22E5, 0x7B26, + 0x2284, 0x7B42, + 0x2223, 0x7B5D, + 0x21C2, 0x7B77, + 0x2161, 0x7B92, + 0x2100, 0x7BAC, + 0x209F, 0x7BC5, + 0x203E, 0x7BDF, + 0x1FDC, 0x7BF8, + 0x1F7B, 0x7C11, + 0x1F19, 0x7C29, + 0x1EB8, 0x7C42, + 0x1E56, 0x7C5A, + 0x1DF5, 0x7C71, + 0x1D93, 0x7C89, + 0x1D31, 0x7CA0, + 0x1CCF, 0x7CB7, + 0x1C6D, 0x7CCD, + 0x1C0B, 0x7CE3, + 0x1BA9, 0x7CF9, + 0x1B47, 0x7D0F, + 0x1AE4, 0x7D24, + 0x1A82, 0x7D39, + 0x1A20, 0x7D4E, + 0x19BD, 0x7D62, + 0x195B, 0x7D76, + 0x18F8, 0x7D8A, + 0x1896, 0x7D9D, + 0x1833, 0x7DB0, + 0x17D0, 0x7DC3, + 0x176D, 0x7DD6, + 0x170A, 0x7DE8, + 0x16A8, 0x7DFA, + 0x1645, 0x7E0C, + 0x15E2, 0x7E1D, + 0x157F, 0x7E2E, + 0x151B, 0x7E3F, + 0x14B8, 0x7E4F, + 0x1455, 0x7E5F, + 0x13F2, 0x7E6F, + 0x138E, 0x7E7F, + 0x132B, 0x7E8E, + 0x12C8, 0x7E9D, + 0x1264, 0x7EAB, + 0x1201, 0x7EBA, + 0x119D, 0x7EC8, + 0x1139, 0x7ED5, + 0x10D6, 0x7EE3, + 0x1072, 0x7EF0, + 0x100E, 0x7EFD, + 0x0FAB, 0x7F09, + 0x0F47, 0x7F15, + 0x0EE3, 0x7F21, + 0x0E7F, 0x7F2D, + 0x0E1B, 0x7F38, + 0x0DB7, 0x7F43, + 0x0D53, 0x7F4D, + 0x0CEF, 0x7F58, + 0x0C8B, 0x7F62, + 0x0C27, 0x7F6B, + 0x0BC3, 0x7F75, + 0x0B5F, 0x7F7E, + 0x0AFB, 0x7F87, + 0x0A97, 0x7F8F, + 0x0A33, 0x7F97, + 0x09CE, 0x7F9F, + 0x096A, 0x7FA7, + 0x0906, 0x7FAE, + 0x08A2, 0x7FB5, + 0x083D, 0x7FBC, + 0x07D9, 0x7FC2, + 0x0775, 0x7FC8, + 0x0710, 0x7FCE, + 0x06AC, 0x7FD3, + 0x0647, 0x7FD8, + 0x05E3, 0x7FDD, + 0x057F, 0x7FE1, + 0x051A, 0x7FE5, + 0x04B6, 0x7FE9, + 0x0451, 0x7FED, + 0x03ED, 0x7FF0, + 0x0388, 0x7FF3, + 0x0324, 0x7FF6, + 0x02BF, 0x7FF8, + 0x025B, 0x7FFA, + 0x01F6, 0x7FFC, + 0x0192, 0x7FFD, + 0x012D, 0x7FFE, + 0x00C9, 0x7FFF, + 0x0064, 0x7FFF, + 0x0000, 0x7FFF, + 0xFF9B, 0x7FFF, + 0xFF36, 0x7FFF, + 0xFED2, 0x7FFE, + 0xFE6D, 0x7FFD, + 0xFE09, 0x7FFC, + 0xFDA4, 0x7FFA, + 0xFD40, 0x7FF8, + 0xFCDB, 0x7FF6, + 0xFC77, 0x7FF3, + 0xFC12, 0x7FF0, + 0xFBAE, 0x7FED, + 0xFB49, 0x7FE9, + 0xFAE5, 0x7FE5, + 0xFA80, 0x7FE1, + 0xFA1C, 0x7FDD, + 0xF9B8, 0x7FD8, + 0xF953, 0x7FD3, + 0xF8EF, 0x7FCE, + 0xF88A, 0x7FC8, + 0xF826, 0x7FC2, + 0xF7C2, 0x7FBC, + 0xF75D, 0x7FB5, + 0xF6F9, 0x7FAE, + 0xF695, 0x7FA7, + 0xF631, 0x7F9F, + 0xF5CC, 0x7F97, + 0xF568, 0x7F8F, + 0xF504, 0x7F87, + 0xF4A0, 0x7F7E, + 0xF43C, 0x7F75, + 0xF3D8, 0x7F6B, + 0xF374, 0x7F62, + 0xF310, 0x7F58, + 0xF2AC, 0x7F4D, + 0xF248, 0x7F43, + 0xF1E4, 0x7F38, + 0xF180, 0x7F2D, + 0xF11C, 0x7F21, + 0xF0B8, 0x7F15, + 0xF054, 0x7F09, + 0xEFF1, 0x7EFD, + 0xEF8D, 0x7EF0, + 0xEF29, 0x7EE3, + 0xEEC6, 0x7ED5, + 0xEE62, 0x7EC8, + 0xEDFE, 0x7EBA, + 0xED9B, 0x7EAB, + 0xED37, 0x7E9D, + 0xECD4, 0x7E8E, + 0xEC71, 0x7E7F, + 0xEC0D, 0x7E6F, + 0xEBAA, 0x7E5F, + 0xEB47, 0x7E4F, + 0xEAE4, 0x7E3F, + 0xEA80, 0x7E2E, + 0xEA1D, 0x7E1D, + 0xE9BA, 0x7E0C, + 0xE957, 0x7DFA, + 0xE8F5, 0x7DE8, + 0xE892, 0x7DD6, + 0xE82F, 0x7DC3, + 0xE7CC, 0x7DB0, + 0xE769, 0x7D9D, + 0xE707, 0x7D8A, + 0xE6A4, 0x7D76, + 0xE642, 0x7D62, + 0xE5DF, 0x7D4E, + 0xE57D, 0x7D39, + 0xE51B, 0x7D24, + 0xE4B8, 0x7D0F, + 0xE456, 0x7CF9, + 0xE3F4, 0x7CE3, + 0xE392, 0x7CCD, + 0xE330, 0x7CB7, + 0xE2CE, 0x7CA0, + 0xE26C, 0x7C89, + 0xE20A, 0x7C71, + 0xE1A9, 0x7C5A, + 0xE147, 0x7C42, + 0xE0E6, 0x7C29, + 0xE084, 0x7C11, + 0xE023, 0x7BF8, + 0xDFC1, 0x7BDF, + 0xDF60, 0x7BC5, + 0xDEFF, 0x7BAC, + 0xDE9E, 0x7B92, + 0xDE3D, 0x7B77, + 0xDDDC, 0x7B5D, + 0xDD7B, 0x7B42, + 0xDD1A, 0x7B26, + 0xDCBA, 0x7B0B, + 0xDC59, 0x7AEF, + 0xDBF8, 0x7AD3, + 0xDB98, 0x7AB6, + 0xDB38, 0x7A9A, + 0xDAD7, 0x7A7D, + 0xDA77, 0x7A5F, + 0xDA17, 0x7A42, + 0xD9B7, 0x7A24, + 0xD957, 0x7A05, + 0xD8F8, 0x79E7, + 0xD898, 0x79C8, + 0xD838, 0x79A9, + 0xD7D9, 0x798A, + 0xD779, 0x796A, + 0xD71A, 0x794A, + 0xD6BB, 0x792A, + 0xD65C, 0x7909, + 0xD5FD, 0x78E8, + 0xD59E, 0x78C7, + 0xD53F, 0x78A6, + 0xD4E0, 0x7884, + 0xD482, 0x7862, + 0xD423, 0x7840, + 0xD3C5, 0x781D, + 0xD367, 0x77FA, + 0xD308, 0x77D7, + 0xD2AA, 0x77B4, + 0xD24C, 0x7790, + 0xD1EE, 0x776C, + 0xD191, 0x7747, + 0xD133, 0x7723, + 0xD0D6, 0x76FE, + 0xD078, 0x76D9, + 0xD01B, 0x76B3, + 0xCFBE, 0x768E, + 0xCF61, 0x7668, + 0xCF04, 0x7641, + 0xCEA7, 0x761B, + 0xCE4A, 0x75F4, + 0xCDEE, 0x75CC, + 0xCD91, 0x75A5, + 0xCD35, 0x757D, + 0xCCD9, 0x7555, + 0xCC7D, 0x752D, + 0xCC21, 0x7504, + 0xCBC5, 0x74DB, + 0xCB69, 0x74B2, + 0xCB0D, 0x7489, + 0xCAB2, 0x745F, + 0xCA57, 0x7435, + 0xC9FB, 0x740B, + 0xC9A0, 0x73E0, + 0xC945, 0x73B5, + 0xC8EB, 0x738A, + 0xC890, 0x735F, + 0xC835, 0x7333, + 0xC7DB, 0x7307, + 0xC781, 0x72DB, + 0xC727, 0x72AF, + 0xC6CD, 0x7282, + 0xC673, 0x7255, + 0xC619, 0x7227, + 0xC5BF, 0x71FA, + 0xC566, 0x71CC, + 0xC50D, 0x719E, + 0xC4B3, 0x716F, + 0xC45A, 0x7141, + 0xC402, 0x7112, + 0xC3A9, 0x70E2, + 0xC350, 0x70B3, + 0xC2F8, 0x7083, + 0xC29F, 0x7053, + 0xC247, 0x7023, + 0xC1EF, 0x6FF2, + 0xC197, 0x6FC1, + 0xC140, 0x6F90, + 0xC0E8, 0x6F5F, + 0xC091, 0x6F2D, + 0xC03A, 0x6EFB, + 0xBFE2, 0x6EC9, + 0xBF8C, 0x6E96, + 0xBF35, 0x6E63, + 0xBEDE, 0x6E30, + 0xBE88, 0x6DFD, + 0xBE31, 0x6DCA, + 0xBDDB, 0x6D96, + 0xBD85, 0x6D62, + 0xBD2F, 0x6D2D, + 0xBCDA, 0x6CF9, + 0xBC84, 0x6CC4, + 0xBC2F, 0x6C8F, + 0xBBDA, 0x6C59, + 0xBB85, 0x6C24, + 0xBB30, 0x6BEE, + 0xBADB, 0x6BB8, + 0xBA87, 0x6B81, + 0xBA32, 0x6B4A, + 0xB9DE, 0x6B13, + 0xB98A, 0x6ADC, + 0xB936, 0x6AA5, + 0xB8E3, 0x6A6D, + 0xB88F, 0x6A35, + 0xB83C, 0x69FD, + 0xB7E9, 0x69C4, + 0xB796, 0x698C, + 0xB743, 0x6953, + 0xB6F0, 0x6919, + 0xB69E, 0x68E0, + 0xB64B, 0x68A6, + 0xB5F9, 0x686C, + 0xB5A7, 0x6832, + 0xB556, 0x67F7, + 0xB504, 0x67BD, + 0xB4B3, 0x6782, + 0xB461, 0x6746, + 0xB410, 0x670B, + 0xB3C0, 0x66CF, + 0xB36F, 0x6693, + 0xB31E, 0x6657, + 0xB2CE, 0x661A, + 0xB27E, 0x65DD, + 0xB22E, 0x65A0, + 0xB1DE, 0x6563, + 0xB18F, 0x6526, + 0xB140, 0x64E8, + 0xB0F0, 0x64AA, + 0xB0A1, 0x646C, + 0xB053, 0x642D, + 0xB004, 0x63EF, + 0xAFB6, 0x63B0, + 0xAF68, 0x6371, + 0xAF1A, 0x6331, + 0xAECC, 0x62F2, + 0xAE7E, 0x62B2, + 0xAE31, 0x6271, + 0xADE3, 0x6231, + 0xAD96, 0x61F1, + 0xAD4A, 0x61B0, + 0xACFD, 0x616F, + 0xACB1, 0x612D, + 0xAC64, 0x60EC, + 0xAC18, 0x60AA, + 0xABCC, 0x6068, + 0xAB81, 0x6026, + 0xAB35, 0x5FE3, + 0xAAEA, 0x5FA0, + 0xAA9F, 0x5F5E, + 0xAA54, 0x5F1A, + 0xAA0A, 0x5ED7, + 0xA9BF, 0x5E93, + 0xA975, 0x5E50, + 0xA92B, 0x5E0B, + 0xA8E2, 0x5DC7, + 0xA898, 0x5D83, + 0xA84F, 0x5D3E, + 0xA806, 0x5CF9, + 0xA7BD, 0x5CB4, + 0xA774, 0x5C6E, + 0xA72B, 0x5C29, + 0xA6E3, 0x5BE3, + 0xA69B, 0x5B9D, + 0xA653, 0x5B56, + 0xA60C, 0x5B10, + 0xA5C4, 0x5AC9, + 0xA57D, 0x5A82, + 0xA536, 0x5A3B, + 0xA4EF, 0x59F3, + 0xA4A9, 0x59AC, + 0xA462, 0x5964, + 0xA41C, 0x591C, + 0xA3D6, 0x58D4, + 0xA391, 0x588B, + 0xA34B, 0x5842, + 0xA306, 0x57F9, + 0xA2C1, 0x57B0, + 0xA27C, 0x5767, + 0xA238, 0x571D, + 0xA1F4, 0x56D4, + 0xA1AF, 0x568A, + 0xA16C, 0x5640, + 0xA128, 0x55F5, + 0xA0E5, 0x55AB, + 0xA0A1, 0x5560, + 0xA05F, 0x5515, + 0xA01C, 0x54CA, + 0x9FD9, 0x547E, + 0x9F97, 0x5433, + 0x9F55, 0x53E7, + 0x9F13, 0x539B, + 0x9ED2, 0x534E, + 0x9E90, 0x5302, + 0x9E4F, 0x52B5, + 0x9E0E, 0x5269, + 0x9DCE, 0x521C, + 0x9D8E, 0x51CE, + 0x9D4D, 0x5181, + 0x9D0D, 0x5133, + 0x9CCE, 0x50E5, + 0x9C8E, 0x5097, + 0x9C4F, 0x5049, + 0x9C10, 0x4FFB, + 0x9BD2, 0x4FAC, + 0x9B93, 0x4F5E, + 0x9B55, 0x4F0F, + 0x9B17, 0x4EBF, + 0x9AD9, 0x4E70, + 0x9A9C, 0x4E21, + 0x9A5F, 0x4DD1, + 0x9A22, 0x4D81, + 0x99E5, 0x4D31, + 0x99A8, 0x4CE1, + 0x996C, 0x4C90, + 0x9930, 0x4C3F, + 0x98F4, 0x4BEF, + 0x98B9, 0x4B9E, + 0x987D, 0x4B4C, + 0x9842, 0x4AFB, + 0x9808, 0x4AA9, + 0x97CD, 0x4A58, + 0x9793, 0x4A06, + 0x9759, 0x49B4, + 0x971F, 0x4961, + 0x96E6, 0x490F, + 0x96AC, 0x48BC, + 0x9673, 0x4869, + 0x963B, 0x4816, + 0x9602, 0x47C3, + 0x95CA, 0x4770, + 0x9592, 0x471C, + 0x955A, 0x46C9, + 0x9523, 0x4675, + 0x94EC, 0x4621, + 0x94B5, 0x45CD, + 0x947E, 0x4578, + 0x9447, 0x4524, + 0x9411, 0x44CF, + 0x93DB, 0x447A, + 0x93A6, 0x4425, + 0x9370, 0x43D0, + 0x933B, 0x437B, + 0x9306, 0x4325, + 0x92D2, 0x42D0, + 0x929D, 0x427A, + 0x9269, 0x4224, + 0x9235, 0x41CE, + 0x9202, 0x4177, + 0x91CF, 0x4121, + 0x919C, 0x40CA, + 0x9169, 0x4073, + 0x9136, 0x401D, + 0x9104, 0x3FC5, + 0x90D2, 0x3F6E, + 0x90A0, 0x3F17, + 0x906F, 0x3EBF, + 0x903E, 0x3E68, + 0x900D, 0x3E10, + 0x8FDC, 0x3DB8, + 0x8FAC, 0x3D60, + 0x8F7C, 0x3D07, + 0x8F4C, 0x3CAF, + 0x8F1D, 0x3C56, + 0x8EED, 0x3BFD, + 0x8EBE, 0x3BA5, + 0x8E90, 0x3B4C, + 0x8E61, 0x3AF2, + 0x8E33, 0x3A99, + 0x8E05, 0x3A40, + 0x8DD8, 0x39E6, + 0x8DAA, 0x398C, + 0x8D7D, 0x3932, + 0x8D50, 0x38D8, + 0x8D24, 0x387E, + 0x8CF8, 0x3824, + 0x8CCC, 0x37CA, + 0x8CA0, 0x376F, + 0x8C75, 0x3714, + 0x8C4A, 0x36BA, + 0x8C1F, 0x365F, + 0x8BF4, 0x3604, + 0x8BCA, 0x35A8, + 0x8BA0, 0x354D, + 0x8B76, 0x34F2, + 0x8B4D, 0x3496, + 0x8B24, 0x343A, + 0x8AFB, 0x33DE, + 0x8AD2, 0x3382, + 0x8AAA, 0x3326, + 0x8A82, 0x32CA, + 0x8A5A, 0x326E, + 0x8A33, 0x3211, + 0x8A0B, 0x31B5, + 0x89E4, 0x3158, + 0x89BE, 0x30FB, + 0x8997, 0x309E, + 0x8971, 0x3041, + 0x894C, 0x2FE4, + 0x8926, 0x2F87, + 0x8901, 0x2F29, + 0x88DC, 0x2ECC, + 0x88B8, 0x2E6E, + 0x8893, 0x2E11, + 0x886F, 0x2DB3, + 0x884B, 0x2D55, + 0x8828, 0x2CF7, + 0x8805, 0x2C98, + 0x87E2, 0x2C3A, + 0x87BF, 0x2BDC, + 0x879D, 0x2B7D, + 0x877B, 0x2B1F, + 0x8759, 0x2AC0, + 0x8738, 0x2A61, + 0x8717, 0x2A02, + 0x86F6, 0x29A3, + 0x86D5, 0x2944, + 0x86B5, 0x28E5, + 0x8695, 0x2886, + 0x8675, 0x2826, + 0x8656, 0x27C7, + 0x8637, 0x2767, + 0x8618, 0x2707, + 0x85FA, 0x26A8, + 0x85DB, 0x2648, + 0x85BD, 0x25E8, + 0x85A0, 0x2588, + 0x8582, 0x2528, + 0x8565, 0x24C7, + 0x8549, 0x2467, + 0x852C, 0x2407, + 0x8510, 0x23A6, + 0x84F4, 0x2345, + 0x84D9, 0x22E5, + 0x84BD, 0x2284, + 0x84A2, 0x2223, + 0x8488, 0x21C2, + 0x846D, 0x2161, + 0x8453, 0x2100, + 0x843A, 0x209F, + 0x8420, 0x203E, + 0x8407, 0x1FDC, + 0x83EE, 0x1F7B, + 0x83D6, 0x1F19, + 0x83BD, 0x1EB8, + 0x83A5, 0x1E56, + 0x838E, 0x1DF5, + 0x8376, 0x1D93, + 0x835F, 0x1D31, + 0x8348, 0x1CCF, + 0x8332, 0x1C6D, + 0x831C, 0x1C0B, + 0x8306, 0x1BA9, + 0x82F0, 0x1B47, + 0x82DB, 0x1AE4, + 0x82C6, 0x1A82, + 0x82B1, 0x1A20, + 0x829D, 0x19BD, + 0x8289, 0x195B, + 0x8275, 0x18F8, + 0x8262, 0x1896, + 0x824F, 0x1833, + 0x823C, 0x17D0, + 0x8229, 0x176D, + 0x8217, 0x170A, + 0x8205, 0x16A8, + 0x81F3, 0x1645, + 0x81E2, 0x15E2, + 0x81D1, 0x157F, + 0x81C0, 0x151B, + 0x81B0, 0x14B8, + 0x81A0, 0x1455, + 0x8190, 0x13F2, + 0x8180, 0x138E, + 0x8171, 0x132B, + 0x8162, 0x12C8, + 0x8154, 0x1264, + 0x8145, 0x1201, + 0x8137, 0x119D, + 0x812A, 0x1139, + 0x811C, 0x10D6, + 0x810F, 0x1072, + 0x8102, 0x100E, + 0x80F6, 0x0FAB, + 0x80EA, 0x0F47, + 0x80DE, 0x0EE3, + 0x80D2, 0x0E7F, + 0x80C7, 0x0E1B, + 0x80BC, 0x0DB7, + 0x80B2, 0x0D53, + 0x80A7, 0x0CEF, + 0x809D, 0x0C8B, + 0x8094, 0x0C27, + 0x808A, 0x0BC3, + 0x8081, 0x0B5F, + 0x8078, 0x0AFB, + 0x8070, 0x0A97, + 0x8068, 0x0A33, + 0x8060, 0x09CE, + 0x8058, 0x096A, + 0x8051, 0x0906, + 0x804A, 0x08A2, + 0x8043, 0x083D, + 0x803D, 0x07D9, + 0x8037, 0x0775, + 0x8031, 0x0710, + 0x802C, 0x06AC, + 0x8027, 0x0647, + 0x8022, 0x05E3, + 0x801E, 0x057F, + 0x801A, 0x051A, + 0x8016, 0x04B6, + 0x8012, 0x0451, + 0x800F, 0x03ED, + 0x800C, 0x0388, + 0x8009, 0x0324, + 0x8007, 0x02BF, + 0x8005, 0x025B, + 0x8003, 0x01F6, + 0x8002, 0x0192, + 0x8001, 0x012D, + 0x8000, 0x00C9, + 0x8000, 0x0064, + 0x8000, 0x0000, + 0x8000, 0xFF9B, + 0x8000, 0xFF36, + 0x8001, 0xFED2, + 0x8002, 0xFE6D, + 0x8003, 0xFE09, + 0x8005, 0xFDA4, + 0x8007, 0xFD40, + 0x8009, 0xFCDB, + 0x800C, 0xFC77, + 0x800F, 0xFC12, + 0x8012, 0xFBAE, + 0x8016, 0xFB49, + 0x801A, 0xFAE5, + 0x801E, 0xFA80, + 0x8022, 0xFA1C, + 0x8027, 0xF9B8, + 0x802C, 0xF953, + 0x8031, 0xF8EF, + 0x8037, 0xF88A, + 0x803D, 0xF826, + 0x8043, 0xF7C2, + 0x804A, 0xF75D, + 0x8051, 0xF6F9, + 0x8058, 0xF695, + 0x8060, 0xF631, + 0x8068, 0xF5CC, + 0x8070, 0xF568, + 0x8078, 0xF504, + 0x8081, 0xF4A0, + 0x808A, 0xF43C, + 0x8094, 0xF3D8, + 0x809D, 0xF374, + 0x80A7, 0xF310, + 0x80B2, 0xF2AC, + 0x80BC, 0xF248, + 0x80C7, 0xF1E4, + 0x80D2, 0xF180, + 0x80DE, 0xF11C, + 0x80EA, 0xF0B8, + 0x80F6, 0xF054, + 0x8102, 0xEFF1, + 0x810F, 0xEF8D, + 0x811C, 0xEF29, + 0x812A, 0xEEC6, + 0x8137, 0xEE62, + 0x8145, 0xEDFE, + 0x8154, 0xED9B, + 0x8162, 0xED37, + 0x8171, 0xECD4, + 0x8180, 0xEC71, + 0x8190, 0xEC0D, + 0x81A0, 0xEBAA, + 0x81B0, 0xEB47, + 0x81C0, 0xEAE4, + 0x81D1, 0xEA80, + 0x81E2, 0xEA1D, + 0x81F3, 0xE9BA, + 0x8205, 0xE957, + 0x8217, 0xE8F5, + 0x8229, 0xE892, + 0x823C, 0xE82F, + 0x824F, 0xE7CC, + 0x8262, 0xE769, + 0x8275, 0xE707, + 0x8289, 0xE6A4, + 0x829D, 0xE642, + 0x82B1, 0xE5DF, + 0x82C6, 0xE57D, + 0x82DB, 0xE51B, + 0x82F0, 0xE4B8, + 0x8306, 0xE456, + 0x831C, 0xE3F4, + 0x8332, 0xE392, + 0x8348, 0xE330, + 0x835F, 0xE2CE, + 0x8376, 0xE26C, + 0x838E, 0xE20A, + 0x83A5, 0xE1A9, + 0x83BD, 0xE147, + 0x83D6, 0xE0E6, + 0x83EE, 0xE084, + 0x8407, 0xE023, + 0x8420, 0xDFC1, + 0x843A, 0xDF60, + 0x8453, 0xDEFF, + 0x846D, 0xDE9E, + 0x8488, 0xDE3D, + 0x84A2, 0xDDDC, + 0x84BD, 0xDD7B, + 0x84D9, 0xDD1A, + 0x84F4, 0xDCBA, + 0x8510, 0xDC59, + 0x852C, 0xDBF8, + 0x8549, 0xDB98, + 0x8565, 0xDB38, + 0x8582, 0xDAD7, + 0x85A0, 0xDA77, + 0x85BD, 0xDA17, + 0x85DB, 0xD9B7, + 0x85FA, 0xD957, + 0x8618, 0xD8F8, + 0x8637, 0xD898, + 0x8656, 0xD838, + 0x8675, 0xD7D9, + 0x8695, 0xD779, + 0x86B5, 0xD71A, + 0x86D5, 0xD6BB, + 0x86F6, 0xD65C, + 0x8717, 0xD5FD, + 0x8738, 0xD59E, + 0x8759, 0xD53F, + 0x877B, 0xD4E0, + 0x879D, 0xD482, + 0x87BF, 0xD423, + 0x87E2, 0xD3C5, + 0x8805, 0xD367, + 0x8828, 0xD308, + 0x884B, 0xD2AA, + 0x886F, 0xD24C, + 0x8893, 0xD1EE, + 0x88B8, 0xD191, + 0x88DC, 0xD133, + 0x8901, 0xD0D6, + 0x8926, 0xD078, + 0x894C, 0xD01B, + 0x8971, 0xCFBE, + 0x8997, 0xCF61, + 0x89BE, 0xCF04, + 0x89E4, 0xCEA7, + 0x8A0B, 0xCE4A, + 0x8A33, 0xCDEE, + 0x8A5A, 0xCD91, + 0x8A82, 0xCD35, + 0x8AAA, 0xCCD9, + 0x8AD2, 0xCC7D, + 0x8AFB, 0xCC21, + 0x8B24, 0xCBC5, + 0x8B4D, 0xCB69, + 0x8B76, 0xCB0D, + 0x8BA0, 0xCAB2, + 0x8BCA, 0xCA57, + 0x8BF4, 0xC9FB, + 0x8C1F, 0xC9A0, + 0x8C4A, 0xC945, + 0x8C75, 0xC8EB, + 0x8CA0, 0xC890, + 0x8CCC, 0xC835, + 0x8CF8, 0xC7DB, + 0x8D24, 0xC781, + 0x8D50, 0xC727, + 0x8D7D, 0xC6CD, + 0x8DAA, 0xC673, + 0x8DD8, 0xC619, + 0x8E05, 0xC5BF, + 0x8E33, 0xC566, + 0x8E61, 0xC50D, + 0x8E90, 0xC4B3, + 0x8EBE, 0xC45A, + 0x8EED, 0xC402, + 0x8F1D, 0xC3A9, + 0x8F4C, 0xC350, + 0x8F7C, 0xC2F8, + 0x8FAC, 0xC29F, + 0x8FDC, 0xC247, + 0x900D, 0xC1EF, + 0x903E, 0xC197, + 0x906F, 0xC140, + 0x90A0, 0xC0E8, + 0x90D2, 0xC091, + 0x9104, 0xC03A, + 0x9136, 0xBFE2, + 0x9169, 0xBF8C, + 0x919C, 0xBF35, + 0x91CF, 0xBEDE, + 0x9202, 0xBE88, + 0x9235, 0xBE31, + 0x9269, 0xBDDB, + 0x929D, 0xBD85, + 0x92D2, 0xBD2F, + 0x9306, 0xBCDA, + 0x933B, 0xBC84, + 0x9370, 0xBC2F, + 0x93A6, 0xBBDA, + 0x93DB, 0xBB85, + 0x9411, 0xBB30, + 0x9447, 0xBADB, + 0x947E, 0xBA87, + 0x94B5, 0xBA32, + 0x94EC, 0xB9DE, + 0x9523, 0xB98A, + 0x955A, 0xB936, + 0x9592, 0xB8E3, + 0x95CA, 0xB88F, + 0x9602, 0xB83C, + 0x963B, 0xB7E9, + 0x9673, 0xB796, + 0x96AC, 0xB743, + 0x96E6, 0xB6F0, + 0x971F, 0xB69E, + 0x9759, 0xB64B, + 0x9793, 0xB5F9, + 0x97CD, 0xB5A7, + 0x9808, 0xB556, + 0x9842, 0xB504, + 0x987D, 0xB4B3, + 0x98B9, 0xB461, + 0x98F4, 0xB410, + 0x9930, 0xB3C0, + 0x996C, 0xB36F, + 0x99A8, 0xB31E, + 0x99E5, 0xB2CE, + 0x9A22, 0xB27E, + 0x9A5F, 0xB22E, + 0x9A9C, 0xB1DE, + 0x9AD9, 0xB18F, + 0x9B17, 0xB140, + 0x9B55, 0xB0F0, + 0x9B93, 0xB0A1, + 0x9BD2, 0xB053, + 0x9C10, 0xB004, + 0x9C4F, 0xAFB6, + 0x9C8E, 0xAF68, + 0x9CCE, 0xAF1A, + 0x9D0D, 0xAECC, + 0x9D4D, 0xAE7E, + 0x9D8E, 0xAE31, + 0x9DCE, 0xADE3, + 0x9E0E, 0xAD96, + 0x9E4F, 0xAD4A, + 0x9E90, 0xACFD, + 0x9ED2, 0xACB1, + 0x9F13, 0xAC64, + 0x9F55, 0xAC18, + 0x9F97, 0xABCC, + 0x9FD9, 0xAB81, + 0xA01C, 0xAB35, + 0xA05F, 0xAAEA, + 0xA0A1, 0xAA9F, + 0xA0E5, 0xAA54, + 0xA128, 0xAA0A, + 0xA16C, 0xA9BF, + 0xA1AF, 0xA975, + 0xA1F4, 0xA92B, + 0xA238, 0xA8E2, + 0xA27C, 0xA898, + 0xA2C1, 0xA84F, + 0xA306, 0xA806, + 0xA34B, 0xA7BD, + 0xA391, 0xA774, + 0xA3D6, 0xA72B, + 0xA41C, 0xA6E3, + 0xA462, 0xA69B, + 0xA4A9, 0xA653, + 0xA4EF, 0xA60C, + 0xA536, 0xA5C4, + 0xA57D, 0xA57D, + 0xA5C4, 0xA536, + 0xA60C, 0xA4EF, + 0xA653, 0xA4A9, + 0xA69B, 0xA462, + 0xA6E3, 0xA41C, + 0xA72B, 0xA3D6, + 0xA774, 0xA391, + 0xA7BD, 0xA34B, + 0xA806, 0xA306, + 0xA84F, 0xA2C1, + 0xA898, 0xA27C, + 0xA8E2, 0xA238, + 0xA92B, 0xA1F4, + 0xA975, 0xA1AF, + 0xA9BF, 0xA16C, + 0xAA0A, 0xA128, + 0xAA54, 0xA0E5, + 0xAA9F, 0xA0A1, + 0xAAEA, 0xA05F, + 0xAB35, 0xA01C, + 0xAB81, 0x9FD9, + 0xABCC, 0x9F97, + 0xAC18, 0x9F55, + 0xAC64, 0x9F13, + 0xACB1, 0x9ED2, + 0xACFD, 0x9E90, + 0xAD4A, 0x9E4F, + 0xAD96, 0x9E0E, + 0xADE3, 0x9DCE, + 0xAE31, 0x9D8E, + 0xAE7E, 0x9D4D, + 0xAECC, 0x9D0D, + 0xAF1A, 0x9CCE, + 0xAF68, 0x9C8E, + 0xAFB6, 0x9C4F, + 0xB004, 0x9C10, + 0xB053, 0x9BD2, + 0xB0A1, 0x9B93, + 0xB0F0, 0x9B55, + 0xB140, 0x9B17, + 0xB18F, 0x9AD9, + 0xB1DE, 0x9A9C, + 0xB22E, 0x9A5F, + 0xB27E, 0x9A22, + 0xB2CE, 0x99E5, + 0xB31E, 0x99A8, + 0xB36F, 0x996C, + 0xB3C0, 0x9930, + 0xB410, 0x98F4, + 0xB461, 0x98B9, + 0xB4B3, 0x987D, + 0xB504, 0x9842, + 0xB556, 0x9808, + 0xB5A7, 0x97CD, + 0xB5F9, 0x9793, + 0xB64B, 0x9759, + 0xB69E, 0x971F, + 0xB6F0, 0x96E6, + 0xB743, 0x96AC, + 0xB796, 0x9673, + 0xB7E9, 0x963B, + 0xB83C, 0x9602, + 0xB88F, 0x95CA, + 0xB8E3, 0x9592, + 0xB936, 0x955A, + 0xB98A, 0x9523, + 0xB9DE, 0x94EC, + 0xBA32, 0x94B5, + 0xBA87, 0x947E, + 0xBADB, 0x9447, + 0xBB30, 0x9411, + 0xBB85, 0x93DB, + 0xBBDA, 0x93A6, + 0xBC2F, 0x9370, + 0xBC84, 0x933B, + 0xBCDA, 0x9306, + 0xBD2F, 0x92D2, + 0xBD85, 0x929D, + 0xBDDB, 0x9269, + 0xBE31, 0x9235, + 0xBE88, 0x9202, + 0xBEDE, 0x91CF, + 0xBF35, 0x919C, + 0xBF8C, 0x9169, + 0xBFE2, 0x9136, + 0xC03A, 0x9104, + 0xC091, 0x90D2, + 0xC0E8, 0x90A0, + 0xC140, 0x906F, + 0xC197, 0x903E, + 0xC1EF, 0x900D, + 0xC247, 0x8FDC, + 0xC29F, 0x8FAC, + 0xC2F8, 0x8F7C, + 0xC350, 0x8F4C, + 0xC3A9, 0x8F1D, + 0xC402, 0x8EED, + 0xC45A, 0x8EBE, + 0xC4B3, 0x8E90, + 0xC50D, 0x8E61, + 0xC566, 0x8E33, + 0xC5BF, 0x8E05, + 0xC619, 0x8DD8, + 0xC673, 0x8DAA, + 0xC6CD, 0x8D7D, + 0xC727, 0x8D50, + 0xC781, 0x8D24, + 0xC7DB, 0x8CF8, + 0xC835, 0x8CCC, + 0xC890, 0x8CA0, + 0xC8EB, 0x8C75, + 0xC945, 0x8C4A, + 0xC9A0, 0x8C1F, + 0xC9FB, 0x8BF4, + 0xCA57, 0x8BCA, + 0xCAB2, 0x8BA0, + 0xCB0D, 0x8B76, + 0xCB69, 0x8B4D, + 0xCBC5, 0x8B24, + 0xCC21, 0x8AFB, + 0xCC7D, 0x8AD2, + 0xCCD9, 0x8AAA, + 0xCD35, 0x8A82, + 0xCD91, 0x8A5A, + 0xCDEE, 0x8A33, + 0xCE4A, 0x8A0B, + 0xCEA7, 0x89E4, + 0xCF04, 0x89BE, + 0xCF61, 0x8997, + 0xCFBE, 0x8971, + 0xD01B, 0x894C, + 0xD078, 0x8926, + 0xD0D6, 0x8901, + 0xD133, 0x88DC, + 0xD191, 0x88B8, + 0xD1EE, 0x8893, + 0xD24C, 0x886F, + 0xD2AA, 0x884B, + 0xD308, 0x8828, + 0xD367, 0x8805, + 0xD3C5, 0x87E2, + 0xD423, 0x87BF, + 0xD482, 0x879D, + 0xD4E0, 0x877B, + 0xD53F, 0x8759, + 0xD59E, 0x8738, + 0xD5FD, 0x8717, + 0xD65C, 0x86F6, + 0xD6BB, 0x86D5, + 0xD71A, 0x86B5, + 0xD779, 0x8695, + 0xD7D9, 0x8675, + 0xD838, 0x8656, + 0xD898, 0x8637, + 0xD8F8, 0x8618, + 0xD957, 0x85FA, + 0xD9B7, 0x85DB, + 0xDA17, 0x85BD, + 0xDA77, 0x85A0, + 0xDAD7, 0x8582, + 0xDB38, 0x8565, + 0xDB98, 0x8549, + 0xDBF8, 0x852C, + 0xDC59, 0x8510, + 0xDCBA, 0x84F4, + 0xDD1A, 0x84D9, + 0xDD7B, 0x84BD, + 0xDDDC, 0x84A2, + 0xDE3D, 0x8488, + 0xDE9E, 0x846D, + 0xDEFF, 0x8453, + 0xDF60, 0x843A, + 0xDFC1, 0x8420, + 0xE023, 0x8407, + 0xE084, 0x83EE, + 0xE0E6, 0x83D6, + 0xE147, 0x83BD, + 0xE1A9, 0x83A5, + 0xE20A, 0x838E, + 0xE26C, 0x8376, + 0xE2CE, 0x835F, + 0xE330, 0x8348, + 0xE392, 0x8332, + 0xE3F4, 0x831C, + 0xE456, 0x8306, + 0xE4B8, 0x82F0, + 0xE51B, 0x82DB, + 0xE57D, 0x82C6, + 0xE5DF, 0x82B1, + 0xE642, 0x829D, + 0xE6A4, 0x8289, + 0xE707, 0x8275, + 0xE769, 0x8262, + 0xE7CC, 0x824F, + 0xE82F, 0x823C, + 0xE892, 0x8229, + 0xE8F5, 0x8217, + 0xE957, 0x8205, + 0xE9BA, 0x81F3, + 0xEA1D, 0x81E2, + 0xEA80, 0x81D1, + 0xEAE4, 0x81C0, + 0xEB47, 0x81B0, + 0xEBAA, 0x81A0, + 0xEC0D, 0x8190, + 0xEC71, 0x8180, + 0xECD4, 0x8171, + 0xED37, 0x8162, + 0xED9B, 0x8154, + 0xEDFE, 0x8145, + 0xEE62, 0x8137, + 0xEEC6, 0x812A, + 0xEF29, 0x811C, + 0xEF8D, 0x810F, + 0xEFF1, 0x8102, + 0xF054, 0x80F6, + 0xF0B8, 0x80EA, + 0xF11C, 0x80DE, + 0xF180, 0x80D2, + 0xF1E4, 0x80C7, + 0xF248, 0x80BC, + 0xF2AC, 0x80B2, + 0xF310, 0x80A7, + 0xF374, 0x809D, + 0xF3D8, 0x8094, + 0xF43C, 0x808A, + 0xF4A0, 0x8081, + 0xF504, 0x8078, + 0xF568, 0x8070, + 0xF5CC, 0x8068, + 0xF631, 0x8060, + 0xF695, 0x8058, + 0xF6F9, 0x8051, + 0xF75D, 0x804A, + 0xF7C2, 0x8043, + 0xF826, 0x803D, + 0xF88A, 0x8037, + 0xF8EF, 0x8031, + 0xF953, 0x802C, + 0xF9B8, 0x8027, + 0xFA1C, 0x8022, + 0xFA80, 0x801E, + 0xFAE5, 0x801A, + 0xFB49, 0x8016, + 0xFBAE, 0x8012, + 0xFC12, 0x800F, + 0xFC77, 0x800C, + 0xFCDB, 0x8009, + 0xFD40, 0x8007, + 0xFDA4, 0x8005, + 0xFE09, 0x8003, + 0xFE6D, 0x8002, + 0xFED2, 0x8001, + 0xFF36, 0x8000, + 0xFF9B, 0x8000 +}; + +/** +* \par +* Example code for q15 Twiddle factors Generation:: +* \par +*
for(i = 0; i< 3N/4; i++)    
+* {    
+*    twiddleCoefq15[2*i]= cos(i * 2*PI/(float)N);    
+*    twiddleCoefq15[2*i+1]= sin(i * 2*PI/(float)N);    
+* } 
+* \par +* where N = 4096 and PI = 3.14159265358979 +* \par +* Cos and Sin values are interleaved fashion +* \par +* Convert Floating point to q15(Fixed point 1.15): +* round(twiddleCoefq15(i) * pow(2, 15)) +* +*/ +const q15_t twiddleCoef_4096_q15[6144] = +{ + 0x7FFF, 0x0000, + 0x7FFF, 0x0032, + 0x7FFF, 0x0064, + 0x7FFF, 0x0096, + 0x7FFF, 0x00C9, + 0x7FFF, 0x00FB, + 0x7FFE, 0x012D, + 0x7FFE, 0x015F, + 0x7FFD, 0x0192, + 0x7FFC, 0x01C4, + 0x7FFC, 0x01F6, + 0x7FFB, 0x0228, + 0x7FFA, 0x025B, + 0x7FF9, 0x028D, + 0x7FF8, 0x02BF, + 0x7FF7, 0x02F1, + 0x7FF6, 0x0324, + 0x7FF4, 0x0356, + 0x7FF3, 0x0388, + 0x7FF2, 0x03BA, + 0x7FF0, 0x03ED, + 0x7FEE, 0x041F, + 0x7FED, 0x0451, + 0x7FEB, 0x0483, + 0x7FE9, 0x04B6, + 0x7FE7, 0x04E8, + 0x7FE5, 0x051A, + 0x7FE3, 0x054C, + 0x7FE1, 0x057F, + 0x7FDF, 0x05B1, + 0x7FDD, 0x05E3, + 0x7FDA, 0x0615, + 0x7FD8, 0x0647, + 0x7FD6, 0x067A, + 0x7FD3, 0x06AC, + 0x7FD0, 0x06DE, + 0x7FCE, 0x0710, + 0x7FCB, 0x0742, + 0x7FC8, 0x0775, + 0x7FC5, 0x07A7, + 0x7FC2, 0x07D9, + 0x7FBF, 0x080B, + 0x7FBC, 0x083D, + 0x7FB8, 0x086F, + 0x7FB5, 0x08A2, + 0x7FB1, 0x08D4, + 0x7FAE, 0x0906, + 0x7FAA, 0x0938, + 0x7FA7, 0x096A, + 0x7FA3, 0x099C, + 0x7F9F, 0x09CE, + 0x7F9B, 0x0A00, + 0x7F97, 0x0A33, + 0x7F93, 0x0A65, + 0x7F8F, 0x0A97, + 0x7F8B, 0x0AC9, + 0x7F87, 0x0AFB, + 0x7F82, 0x0B2D, + 0x7F7E, 0x0B5F, + 0x7F79, 0x0B91, + 0x7F75, 0x0BC3, + 0x7F70, 0x0BF5, + 0x7F6B, 0x0C27, + 0x7F67, 0x0C59, + 0x7F62, 0x0C8B, + 0x7F5D, 0x0CBD, + 0x7F58, 0x0CEF, + 0x7F53, 0x0D21, + 0x7F4D, 0x0D53, + 0x7F48, 0x0D85, + 0x7F43, 0x0DB7, + 0x7F3D, 0x0DE9, + 0x7F38, 0x0E1B, + 0x7F32, 0x0E4D, + 0x7F2D, 0x0E7F, + 0x7F27, 0x0EB1, + 0x7F21, 0x0EE3, + 0x7F1B, 0x0F15, + 0x7F15, 0x0F47, + 0x7F0F, 0x0F79, + 0x7F09, 0x0FAB, + 0x7F03, 0x0FDD, + 0x7EFD, 0x100E, + 0x7EF6, 0x1040, + 0x7EF0, 0x1072, + 0x7EE9, 0x10A4, + 0x7EE3, 0x10D6, + 0x7EDC, 0x1108, + 0x7ED5, 0x1139, + 0x7ECF, 0x116B, + 0x7EC8, 0x119D, + 0x7EC1, 0x11CF, + 0x7EBA, 0x1201, + 0x7EB3, 0x1232, + 0x7EAB, 0x1264, + 0x7EA4, 0x1296, + 0x7E9D, 0x12C8, + 0x7E95, 0x12F9, + 0x7E8E, 0x132B, + 0x7E86, 0x135D, + 0x7E7F, 0x138E, + 0x7E77, 0x13C0, + 0x7E6F, 0x13F2, + 0x7E67, 0x1423, + 0x7E5F, 0x1455, + 0x7E57, 0x1487, + 0x7E4F, 0x14B8, + 0x7E47, 0x14EA, + 0x7E3F, 0x151B, + 0x7E37, 0x154D, + 0x7E2E, 0x157F, + 0x7E26, 0x15B0, + 0x7E1D, 0x15E2, + 0x7E14, 0x1613, + 0x7E0C, 0x1645, + 0x7E03, 0x1676, + 0x7DFA, 0x16A8, + 0x7DF1, 0x16D9, + 0x7DE8, 0x170A, + 0x7DDF, 0x173C, + 0x7DD6, 0x176D, + 0x7DCD, 0x179F, + 0x7DC3, 0x17D0, + 0x7DBA, 0x1802, + 0x7DB0, 0x1833, + 0x7DA7, 0x1864, + 0x7D9D, 0x1896, + 0x7D94, 0x18C7, + 0x7D8A, 0x18F8, + 0x7D80, 0x192A, + 0x7D76, 0x195B, + 0x7D6C, 0x198C, + 0x7D62, 0x19BD, + 0x7D58, 0x19EF, + 0x7D4E, 0x1A20, + 0x7D43, 0x1A51, + 0x7D39, 0x1A82, + 0x7D2F, 0x1AB3, + 0x7D24, 0x1AE4, + 0x7D19, 0x1B16, + 0x7D0F, 0x1B47, + 0x7D04, 0x1B78, + 0x7CF9, 0x1BA9, + 0x7CEE, 0x1BDA, + 0x7CE3, 0x1C0B, + 0x7CD8, 0x1C3C, + 0x7CCD, 0x1C6D, + 0x7CC2, 0x1C9E, + 0x7CB7, 0x1CCF, + 0x7CAB, 0x1D00, + 0x7CA0, 0x1D31, + 0x7C94, 0x1D62, + 0x7C89, 0x1D93, + 0x7C7D, 0x1DC4, + 0x7C71, 0x1DF5, + 0x7C66, 0x1E25, + 0x7C5A, 0x1E56, + 0x7C4E, 0x1E87, + 0x7C42, 0x1EB8, + 0x7C36, 0x1EE9, + 0x7C29, 0x1F19, + 0x7C1D, 0x1F4A, + 0x7C11, 0x1F7B, + 0x7C05, 0x1FAC, + 0x7BF8, 0x1FDC, + 0x7BEB, 0x200D, + 0x7BDF, 0x203E, + 0x7BD2, 0x206E, + 0x7BC5, 0x209F, + 0x7BB9, 0x20D0, + 0x7BAC, 0x2100, + 0x7B9F, 0x2131, + 0x7B92, 0x2161, + 0x7B84, 0x2192, + 0x7B77, 0x21C2, + 0x7B6A, 0x21F3, + 0x7B5D, 0x2223, + 0x7B4F, 0x2254, + 0x7B42, 0x2284, + 0x7B34, 0x22B4, + 0x7B26, 0x22E5, + 0x7B19, 0x2315, + 0x7B0B, 0x2345, + 0x7AFD, 0x2376, + 0x7AEF, 0x23A6, + 0x7AE1, 0x23D6, + 0x7AD3, 0x2407, + 0x7AC5, 0x2437, + 0x7AB6, 0x2467, + 0x7AA8, 0x2497, + 0x7A9A, 0x24C7, + 0x7A8B, 0x24F7, + 0x7A7D, 0x2528, + 0x7A6E, 0x2558, + 0x7A5F, 0x2588, + 0x7A50, 0x25B8, + 0x7A42, 0x25E8, + 0x7A33, 0x2618, + 0x7A24, 0x2648, + 0x7A15, 0x2678, + 0x7A05, 0x26A8, + 0x79F6, 0x26D8, + 0x79E7, 0x2707, + 0x79D8, 0x2737, + 0x79C8, 0x2767, + 0x79B9, 0x2797, + 0x79A9, 0x27C7, + 0x7999, 0x27F6, + 0x798A, 0x2826, + 0x797A, 0x2856, + 0x796A, 0x2886, + 0x795A, 0x28B5, + 0x794A, 0x28E5, + 0x793A, 0x2915, + 0x792A, 0x2944, + 0x7919, 0x2974, + 0x7909, 0x29A3, + 0x78F9, 0x29D3, + 0x78E8, 0x2A02, + 0x78D8, 0x2A32, + 0x78C7, 0x2A61, + 0x78B6, 0x2A91, + 0x78A6, 0x2AC0, + 0x7895, 0x2AEF, + 0x7884, 0x2B1F, + 0x7873, 0x2B4E, + 0x7862, 0x2B7D, + 0x7851, 0x2BAD, + 0x7840, 0x2BDC, + 0x782E, 0x2C0B, + 0x781D, 0x2C3A, + 0x780C, 0x2C69, + 0x77FA, 0x2C98, + 0x77E9, 0x2CC8, + 0x77D7, 0x2CF7, + 0x77C5, 0x2D26, + 0x77B4, 0x2D55, + 0x77A2, 0x2D84, + 0x7790, 0x2DB3, + 0x777E, 0x2DE2, + 0x776C, 0x2E11, + 0x775A, 0x2E3F, + 0x7747, 0x2E6E, + 0x7735, 0x2E9D, + 0x7723, 0x2ECC, + 0x7710, 0x2EFB, + 0x76FE, 0x2F29, + 0x76EB, 0x2F58, + 0x76D9, 0x2F87, + 0x76C6, 0x2FB5, + 0x76B3, 0x2FE4, + 0x76A0, 0x3013, + 0x768E, 0x3041, + 0x767B, 0x3070, + 0x7668, 0x309E, + 0x7654, 0x30CD, + 0x7641, 0x30FB, + 0x762E, 0x312A, + 0x761B, 0x3158, + 0x7607, 0x3186, + 0x75F4, 0x31B5, + 0x75E0, 0x31E3, + 0x75CC, 0x3211, + 0x75B9, 0x3240, + 0x75A5, 0x326E, + 0x7591, 0x329C, + 0x757D, 0x32CA, + 0x7569, 0x32F8, + 0x7555, 0x3326, + 0x7541, 0x3354, + 0x752D, 0x3382, + 0x7519, 0x33B0, + 0x7504, 0x33DE, + 0x74F0, 0x340C, + 0x74DB, 0x343A, + 0x74C7, 0x3468, + 0x74B2, 0x3496, + 0x749E, 0x34C4, + 0x7489, 0x34F2, + 0x7474, 0x351F, + 0x745F, 0x354D, + 0x744A, 0x357B, + 0x7435, 0x35A8, + 0x7420, 0x35D6, + 0x740B, 0x3604, + 0x73F6, 0x3631, + 0x73E0, 0x365F, + 0x73CB, 0x368C, + 0x73B5, 0x36BA, + 0x73A0, 0x36E7, + 0x738A, 0x3714, + 0x7375, 0x3742, + 0x735F, 0x376F, + 0x7349, 0x379C, + 0x7333, 0x37CA, + 0x731D, 0x37F7, + 0x7307, 0x3824, + 0x72F1, 0x3851, + 0x72DB, 0x387E, + 0x72C5, 0x38AB, + 0x72AF, 0x38D8, + 0x7298, 0x3906, + 0x7282, 0x3932, + 0x726B, 0x395F, + 0x7255, 0x398C, + 0x723E, 0x39B9, + 0x7227, 0x39E6, + 0x7211, 0x3A13, + 0x71FA, 0x3A40, + 0x71E3, 0x3A6C, + 0x71CC, 0x3A99, + 0x71B5, 0x3AC6, + 0x719E, 0x3AF2, + 0x7186, 0x3B1F, + 0x716F, 0x3B4C, + 0x7158, 0x3B78, + 0x7141, 0x3BA5, + 0x7129, 0x3BD1, + 0x7112, 0x3BFD, + 0x70FA, 0x3C2A, + 0x70E2, 0x3C56, + 0x70CB, 0x3C83, + 0x70B3, 0x3CAF, + 0x709B, 0x3CDB, + 0x7083, 0x3D07, + 0x706B, 0x3D33, + 0x7053, 0x3D60, + 0x703B, 0x3D8C, + 0x7023, 0x3DB8, + 0x700A, 0x3DE4, + 0x6FF2, 0x3E10, + 0x6FDA, 0x3E3C, + 0x6FC1, 0x3E68, + 0x6FA9, 0x3E93, + 0x6F90, 0x3EBF, + 0x6F77, 0x3EEB, + 0x6F5F, 0x3F17, + 0x6F46, 0x3F43, + 0x6F2D, 0x3F6E, + 0x6F14, 0x3F9A, + 0x6EFB, 0x3FC5, + 0x6EE2, 0x3FF1, + 0x6EC9, 0x401D, + 0x6EAF, 0x4048, + 0x6E96, 0x4073, + 0x6E7D, 0x409F, + 0x6E63, 0x40CA, + 0x6E4A, 0x40F6, + 0x6E30, 0x4121, + 0x6E17, 0x414C, + 0x6DFD, 0x4177, + 0x6DE3, 0x41A2, + 0x6DCA, 0x41CE, + 0x6DB0, 0x41F9, + 0x6D96, 0x4224, + 0x6D7C, 0x424F, + 0x6D62, 0x427A, + 0x6D48, 0x42A5, + 0x6D2D, 0x42D0, + 0x6D13, 0x42FA, + 0x6CF9, 0x4325, + 0x6CDE, 0x4350, + 0x6CC4, 0x437B, + 0x6CA9, 0x43A5, + 0x6C8F, 0x43D0, + 0x6C74, 0x43FB, + 0x6C59, 0x4425, + 0x6C3F, 0x4450, + 0x6C24, 0x447A, + 0x6C09, 0x44A5, + 0x6BEE, 0x44CF, + 0x6BD3, 0x44FA, + 0x6BB8, 0x4524, + 0x6B9C, 0x454E, + 0x6B81, 0x4578, + 0x6B66, 0x45A3, + 0x6B4A, 0x45CD, + 0x6B2F, 0x45F7, + 0x6B13, 0x4621, + 0x6AF8, 0x464B, + 0x6ADC, 0x4675, + 0x6AC1, 0x469F, + 0x6AA5, 0x46C9, + 0x6A89, 0x46F3, + 0x6A6D, 0x471C, + 0x6A51, 0x4746, + 0x6A35, 0x4770, + 0x6A19, 0x479A, + 0x69FD, 0x47C3, + 0x69E1, 0x47ED, + 0x69C4, 0x4816, + 0x69A8, 0x4840, + 0x698C, 0x4869, + 0x696F, 0x4893, + 0x6953, 0x48BC, + 0x6936, 0x48E6, + 0x6919, 0x490F, + 0x68FD, 0x4938, + 0x68E0, 0x4961, + 0x68C3, 0x498A, + 0x68A6, 0x49B4, + 0x6889, 0x49DD, + 0x686C, 0x4A06, + 0x684F, 0x4A2F, + 0x6832, 0x4A58, + 0x6815, 0x4A81, + 0x67F7, 0x4AA9, + 0x67DA, 0x4AD2, + 0x67BD, 0x4AFB, + 0x679F, 0x4B24, + 0x6782, 0x4B4C, + 0x6764, 0x4B75, + 0x6746, 0x4B9E, + 0x6729, 0x4BC6, + 0x670B, 0x4BEF, + 0x66ED, 0x4C17, + 0x66CF, 0x4C3F, + 0x66B1, 0x4C68, + 0x6693, 0x4C90, + 0x6675, 0x4CB8, + 0x6657, 0x4CE1, + 0x6639, 0x4D09, + 0x661A, 0x4D31, + 0x65FC, 0x4D59, + 0x65DD, 0x4D81, + 0x65BF, 0x4DA9, + 0x65A0, 0x4DD1, + 0x6582, 0x4DF9, + 0x6563, 0x4E21, + 0x6545, 0x4E48, + 0x6526, 0x4E70, + 0x6507, 0x4E98, + 0x64E8, 0x4EBF, + 0x64C9, 0x4EE7, + 0x64AA, 0x4F0F, + 0x648B, 0x4F36, + 0x646C, 0x4F5E, + 0x644D, 0x4F85, + 0x642D, 0x4FAC, + 0x640E, 0x4FD4, + 0x63EF, 0x4FFB, + 0x63CF, 0x5022, + 0x63B0, 0x5049, + 0x6390, 0x5070, + 0x6371, 0x5097, + 0x6351, 0x50BF, + 0x6331, 0x50E5, + 0x6311, 0x510C, + 0x62F2, 0x5133, + 0x62D2, 0x515A, + 0x62B2, 0x5181, + 0x6292, 0x51A8, + 0x6271, 0x51CE, + 0x6251, 0x51F5, + 0x6231, 0x521C, + 0x6211, 0x5242, + 0x61F1, 0x5269, + 0x61D0, 0x528F, + 0x61B0, 0x52B5, + 0x618F, 0x52DC, + 0x616F, 0x5302, + 0x614E, 0x5328, + 0x612D, 0x534E, + 0x610D, 0x5375, + 0x60EC, 0x539B, + 0x60CB, 0x53C1, + 0x60AA, 0x53E7, + 0x6089, 0x540D, + 0x6068, 0x5433, + 0x6047, 0x5458, + 0x6026, 0x547E, + 0x6004, 0x54A4, + 0x5FE3, 0x54CA, + 0x5FC2, 0x54EF, + 0x5FA0, 0x5515, + 0x5F7F, 0x553A, + 0x5F5E, 0x5560, + 0x5F3C, 0x5585, + 0x5F1A, 0x55AB, + 0x5EF9, 0x55D0, + 0x5ED7, 0x55F5, + 0x5EB5, 0x561A, + 0x5E93, 0x5640, + 0x5E71, 0x5665, + 0x5E50, 0x568A, + 0x5E2D, 0x56AF, + 0x5E0B, 0x56D4, + 0x5DE9, 0x56F9, + 0x5DC7, 0x571D, + 0x5DA5, 0x5742, + 0x5D83, 0x5767, + 0x5D60, 0x578C, + 0x5D3E, 0x57B0, + 0x5D1B, 0x57D5, + 0x5CF9, 0x57F9, + 0x5CD6, 0x581E, + 0x5CB4, 0x5842, + 0x5C91, 0x5867, + 0x5C6E, 0x588B, + 0x5C4B, 0x58AF, + 0x5C29, 0x58D4, + 0x5C06, 0x58F8, + 0x5BE3, 0x591C, + 0x5BC0, 0x5940, + 0x5B9D, 0x5964, + 0x5B79, 0x5988, + 0x5B56, 0x59AC, + 0x5B33, 0x59D0, + 0x5B10, 0x59F3, + 0x5AEC, 0x5A17, + 0x5AC9, 0x5A3B, + 0x5AA5, 0x5A5E, + 0x5A82, 0x5A82, + 0x5A5E, 0x5AA5, + 0x5A3B, 0x5AC9, + 0x5A17, 0x5AEC, + 0x59F3, 0x5B10, + 0x59D0, 0x5B33, + 0x59AC, 0x5B56, + 0x5988, 0x5B79, + 0x5964, 0x5B9D, + 0x5940, 0x5BC0, + 0x591C, 0x5BE3, + 0x58F8, 0x5C06, + 0x58D4, 0x5C29, + 0x58AF, 0x5C4B, + 0x588B, 0x5C6E, + 0x5867, 0x5C91, + 0x5842, 0x5CB4, + 0x581E, 0x5CD6, + 0x57F9, 0x5CF9, + 0x57D5, 0x5D1B, + 0x57B0, 0x5D3E, + 0x578C, 0x5D60, + 0x5767, 0x5D83, + 0x5742, 0x5DA5, + 0x571D, 0x5DC7, + 0x56F9, 0x5DE9, + 0x56D4, 0x5E0B, + 0x56AF, 0x5E2D, + 0x568A, 0x5E50, + 0x5665, 0x5E71, + 0x5640, 0x5E93, + 0x561A, 0x5EB5, + 0x55F5, 0x5ED7, + 0x55D0, 0x5EF9, + 0x55AB, 0x5F1A, + 0x5585, 0x5F3C, + 0x5560, 0x5F5E, + 0x553A, 0x5F7F, + 0x5515, 0x5FA0, + 0x54EF, 0x5FC2, + 0x54CA, 0x5FE3, + 0x54A4, 0x6004, + 0x547E, 0x6026, + 0x5458, 0x6047, + 0x5433, 0x6068, + 0x540D, 0x6089, + 0x53E7, 0x60AA, + 0x53C1, 0x60CB, + 0x539B, 0x60EC, + 0x5375, 0x610D, + 0x534E, 0x612D, + 0x5328, 0x614E, + 0x5302, 0x616F, + 0x52DC, 0x618F, + 0x52B5, 0x61B0, + 0x528F, 0x61D0, + 0x5269, 0x61F1, + 0x5242, 0x6211, + 0x521C, 0x6231, + 0x51F5, 0x6251, + 0x51CE, 0x6271, + 0x51A8, 0x6292, + 0x5181, 0x62B2, + 0x515A, 0x62D2, + 0x5133, 0x62F2, + 0x510C, 0x6311, + 0x50E5, 0x6331, + 0x50BF, 0x6351, + 0x5097, 0x6371, + 0x5070, 0x6390, + 0x5049, 0x63B0, + 0x5022, 0x63CF, + 0x4FFB, 0x63EF, + 0x4FD4, 0x640E, + 0x4FAC, 0x642D, + 0x4F85, 0x644D, + 0x4F5E, 0x646C, + 0x4F36, 0x648B, + 0x4F0F, 0x64AA, + 0x4EE7, 0x64C9, + 0x4EBF, 0x64E8, + 0x4E98, 0x6507, + 0x4E70, 0x6526, + 0x4E48, 0x6545, + 0x4E21, 0x6563, + 0x4DF9, 0x6582, + 0x4DD1, 0x65A0, + 0x4DA9, 0x65BF, + 0x4D81, 0x65DD, + 0x4D59, 0x65FC, + 0x4D31, 0x661A, + 0x4D09, 0x6639, + 0x4CE1, 0x6657, + 0x4CB8, 0x6675, + 0x4C90, 0x6693, + 0x4C68, 0x66B1, + 0x4C3F, 0x66CF, + 0x4C17, 0x66ED, + 0x4BEF, 0x670B, + 0x4BC6, 0x6729, + 0x4B9E, 0x6746, + 0x4B75, 0x6764, + 0x4B4C, 0x6782, + 0x4B24, 0x679F, + 0x4AFB, 0x67BD, + 0x4AD2, 0x67DA, + 0x4AA9, 0x67F7, + 0x4A81, 0x6815, + 0x4A58, 0x6832, + 0x4A2F, 0x684F, + 0x4A06, 0x686C, + 0x49DD, 0x6889, + 0x49B4, 0x68A6, + 0x498A, 0x68C3, + 0x4961, 0x68E0, + 0x4938, 0x68FD, + 0x490F, 0x6919, + 0x48E6, 0x6936, + 0x48BC, 0x6953, + 0x4893, 0x696F, + 0x4869, 0x698C, + 0x4840, 0x69A8, + 0x4816, 0x69C4, + 0x47ED, 0x69E1, + 0x47C3, 0x69FD, + 0x479A, 0x6A19, + 0x4770, 0x6A35, + 0x4746, 0x6A51, + 0x471C, 0x6A6D, + 0x46F3, 0x6A89, + 0x46C9, 0x6AA5, + 0x469F, 0x6AC1, + 0x4675, 0x6ADC, + 0x464B, 0x6AF8, + 0x4621, 0x6B13, + 0x45F7, 0x6B2F, + 0x45CD, 0x6B4A, + 0x45A3, 0x6B66, + 0x4578, 0x6B81, + 0x454E, 0x6B9C, + 0x4524, 0x6BB8, + 0x44FA, 0x6BD3, + 0x44CF, 0x6BEE, + 0x44A5, 0x6C09, + 0x447A, 0x6C24, + 0x4450, 0x6C3F, + 0x4425, 0x6C59, + 0x43FB, 0x6C74, + 0x43D0, 0x6C8F, + 0x43A5, 0x6CA9, + 0x437B, 0x6CC4, + 0x4350, 0x6CDE, + 0x4325, 0x6CF9, + 0x42FA, 0x6D13, + 0x42D0, 0x6D2D, + 0x42A5, 0x6D48, + 0x427A, 0x6D62, + 0x424F, 0x6D7C, + 0x4224, 0x6D96, + 0x41F9, 0x6DB0, + 0x41CE, 0x6DCA, + 0x41A2, 0x6DE3, + 0x4177, 0x6DFD, + 0x414C, 0x6E17, + 0x4121, 0x6E30, + 0x40F6, 0x6E4A, + 0x40CA, 0x6E63, + 0x409F, 0x6E7D, + 0x4073, 0x6E96, + 0x4048, 0x6EAF, + 0x401D, 0x6EC9, + 0x3FF1, 0x6EE2, + 0x3FC5, 0x6EFB, + 0x3F9A, 0x6F14, + 0x3F6E, 0x6F2D, + 0x3F43, 0x6F46, + 0x3F17, 0x6F5F, + 0x3EEB, 0x6F77, + 0x3EBF, 0x6F90, + 0x3E93, 0x6FA9, + 0x3E68, 0x6FC1, + 0x3E3C, 0x6FDA, + 0x3E10, 0x6FF2, + 0x3DE4, 0x700A, + 0x3DB8, 0x7023, + 0x3D8C, 0x703B, + 0x3D60, 0x7053, + 0x3D33, 0x706B, + 0x3D07, 0x7083, + 0x3CDB, 0x709B, + 0x3CAF, 0x70B3, + 0x3C83, 0x70CB, + 0x3C56, 0x70E2, + 0x3C2A, 0x70FA, + 0x3BFD, 0x7112, + 0x3BD1, 0x7129, + 0x3BA5, 0x7141, + 0x3B78, 0x7158, + 0x3B4C, 0x716F, + 0x3B1F, 0x7186, + 0x3AF2, 0x719E, + 0x3AC6, 0x71B5, + 0x3A99, 0x71CC, + 0x3A6C, 0x71E3, + 0x3A40, 0x71FA, + 0x3A13, 0x7211, + 0x39E6, 0x7227, + 0x39B9, 0x723E, + 0x398C, 0x7255, + 0x395F, 0x726B, + 0x3932, 0x7282, + 0x3906, 0x7298, + 0x38D8, 0x72AF, + 0x38AB, 0x72C5, + 0x387E, 0x72DB, + 0x3851, 0x72F1, + 0x3824, 0x7307, + 0x37F7, 0x731D, + 0x37CA, 0x7333, + 0x379C, 0x7349, + 0x376F, 0x735F, + 0x3742, 0x7375, + 0x3714, 0x738A, + 0x36E7, 0x73A0, + 0x36BA, 0x73B5, + 0x368C, 0x73CB, + 0x365F, 0x73E0, + 0x3631, 0x73F6, + 0x3604, 0x740B, + 0x35D6, 0x7420, + 0x35A8, 0x7435, + 0x357B, 0x744A, + 0x354D, 0x745F, + 0x351F, 0x7474, + 0x34F2, 0x7489, + 0x34C4, 0x749E, + 0x3496, 0x74B2, + 0x3468, 0x74C7, + 0x343A, 0x74DB, + 0x340C, 0x74F0, + 0x33DE, 0x7504, + 0x33B0, 0x7519, + 0x3382, 0x752D, + 0x3354, 0x7541, + 0x3326, 0x7555, + 0x32F8, 0x7569, + 0x32CA, 0x757D, + 0x329C, 0x7591, + 0x326E, 0x75A5, + 0x3240, 0x75B9, + 0x3211, 0x75CC, + 0x31E3, 0x75E0, + 0x31B5, 0x75F4, + 0x3186, 0x7607, + 0x3158, 0x761B, + 0x312A, 0x762E, + 0x30FB, 0x7641, + 0x30CD, 0x7654, + 0x309E, 0x7668, + 0x3070, 0x767B, + 0x3041, 0x768E, + 0x3013, 0x76A0, + 0x2FE4, 0x76B3, + 0x2FB5, 0x76C6, + 0x2F87, 0x76D9, + 0x2F58, 0x76EB, + 0x2F29, 0x76FE, + 0x2EFB, 0x7710, + 0x2ECC, 0x7723, + 0x2E9D, 0x7735, + 0x2E6E, 0x7747, + 0x2E3F, 0x775A, + 0x2E11, 0x776C, + 0x2DE2, 0x777E, + 0x2DB3, 0x7790, + 0x2D84, 0x77A2, + 0x2D55, 0x77B4, + 0x2D26, 0x77C5, + 0x2CF7, 0x77D7, + 0x2CC8, 0x77E9, + 0x2C98, 0x77FA, + 0x2C69, 0x780C, + 0x2C3A, 0x781D, + 0x2C0B, 0x782E, + 0x2BDC, 0x7840, + 0x2BAD, 0x7851, + 0x2B7D, 0x7862, + 0x2B4E, 0x7873, + 0x2B1F, 0x7884, + 0x2AEF, 0x7895, + 0x2AC0, 0x78A6, + 0x2A91, 0x78B6, + 0x2A61, 0x78C7, + 0x2A32, 0x78D8, + 0x2A02, 0x78E8, + 0x29D3, 0x78F9, + 0x29A3, 0x7909, + 0x2974, 0x7919, + 0x2944, 0x792A, + 0x2915, 0x793A, + 0x28E5, 0x794A, + 0x28B5, 0x795A, + 0x2886, 0x796A, + 0x2856, 0x797A, + 0x2826, 0x798A, + 0x27F6, 0x7999, + 0x27C7, 0x79A9, + 0x2797, 0x79B9, + 0x2767, 0x79C8, + 0x2737, 0x79D8, + 0x2707, 0x79E7, + 0x26D8, 0x79F6, + 0x26A8, 0x7A05, + 0x2678, 0x7A15, + 0x2648, 0x7A24, + 0x2618, 0x7A33, + 0x25E8, 0x7A42, + 0x25B8, 0x7A50, + 0x2588, 0x7A5F, + 0x2558, 0x7A6E, + 0x2528, 0x7A7D, + 0x24F7, 0x7A8B, + 0x24C7, 0x7A9A, + 0x2497, 0x7AA8, + 0x2467, 0x7AB6, + 0x2437, 0x7AC5, + 0x2407, 0x7AD3, + 0x23D6, 0x7AE1, + 0x23A6, 0x7AEF, + 0x2376, 0x7AFD, + 0x2345, 0x7B0B, + 0x2315, 0x7B19, + 0x22E5, 0x7B26, + 0x22B4, 0x7B34, + 0x2284, 0x7B42, + 0x2254, 0x7B4F, + 0x2223, 0x7B5D, + 0x21F3, 0x7B6A, + 0x21C2, 0x7B77, + 0x2192, 0x7B84, + 0x2161, 0x7B92, + 0x2131, 0x7B9F, + 0x2100, 0x7BAC, + 0x20D0, 0x7BB9, + 0x209F, 0x7BC5, + 0x206E, 0x7BD2, + 0x203E, 0x7BDF, + 0x200D, 0x7BEB, + 0x1FDC, 0x7BF8, + 0x1FAC, 0x7C05, + 0x1F7B, 0x7C11, + 0x1F4A, 0x7C1D, + 0x1F19, 0x7C29, + 0x1EE9, 0x7C36, + 0x1EB8, 0x7C42, + 0x1E87, 0x7C4E, + 0x1E56, 0x7C5A, + 0x1E25, 0x7C66, + 0x1DF5, 0x7C71, + 0x1DC4, 0x7C7D, + 0x1D93, 0x7C89, + 0x1D62, 0x7C94, + 0x1D31, 0x7CA0, + 0x1D00, 0x7CAB, + 0x1CCF, 0x7CB7, + 0x1C9E, 0x7CC2, + 0x1C6D, 0x7CCD, + 0x1C3C, 0x7CD8, + 0x1C0B, 0x7CE3, + 0x1BDA, 0x7CEE, + 0x1BA9, 0x7CF9, + 0x1B78, 0x7D04, + 0x1B47, 0x7D0F, + 0x1B16, 0x7D19, + 0x1AE4, 0x7D24, + 0x1AB3, 0x7D2F, + 0x1A82, 0x7D39, + 0x1A51, 0x7D43, + 0x1A20, 0x7D4E, + 0x19EF, 0x7D58, + 0x19BD, 0x7D62, + 0x198C, 0x7D6C, + 0x195B, 0x7D76, + 0x192A, 0x7D80, + 0x18F8, 0x7D8A, + 0x18C7, 0x7D94, + 0x1896, 0x7D9D, + 0x1864, 0x7DA7, + 0x1833, 0x7DB0, + 0x1802, 0x7DBA, + 0x17D0, 0x7DC3, + 0x179F, 0x7DCD, + 0x176D, 0x7DD6, + 0x173C, 0x7DDF, + 0x170A, 0x7DE8, + 0x16D9, 0x7DF1, + 0x16A8, 0x7DFA, + 0x1676, 0x7E03, + 0x1645, 0x7E0C, + 0x1613, 0x7E14, + 0x15E2, 0x7E1D, + 0x15B0, 0x7E26, + 0x157F, 0x7E2E, + 0x154D, 0x7E37, + 0x151B, 0x7E3F, + 0x14EA, 0x7E47, + 0x14B8, 0x7E4F, + 0x1487, 0x7E57, + 0x1455, 0x7E5F, + 0x1423, 0x7E67, + 0x13F2, 0x7E6F, + 0x13C0, 0x7E77, + 0x138E, 0x7E7F, + 0x135D, 0x7E86, + 0x132B, 0x7E8E, + 0x12F9, 0x7E95, + 0x12C8, 0x7E9D, + 0x1296, 0x7EA4, + 0x1264, 0x7EAB, + 0x1232, 0x7EB3, + 0x1201, 0x7EBA, + 0x11CF, 0x7EC1, + 0x119D, 0x7EC8, + 0x116B, 0x7ECF, + 0x1139, 0x7ED5, + 0x1108, 0x7EDC, + 0x10D6, 0x7EE3, + 0x10A4, 0x7EE9, + 0x1072, 0x7EF0, + 0x1040, 0x7EF6, + 0x100E, 0x7EFD, + 0x0FDD, 0x7F03, + 0x0FAB, 0x7F09, + 0x0F79, 0x7F0F, + 0x0F47, 0x7F15, + 0x0F15, 0x7F1B, + 0x0EE3, 0x7F21, + 0x0EB1, 0x7F27, + 0x0E7F, 0x7F2D, + 0x0E4D, 0x7F32, + 0x0E1B, 0x7F38, + 0x0DE9, 0x7F3D, + 0x0DB7, 0x7F43, + 0x0D85, 0x7F48, + 0x0D53, 0x7F4D, + 0x0D21, 0x7F53, + 0x0CEF, 0x7F58, + 0x0CBD, 0x7F5D, + 0x0C8B, 0x7F62, + 0x0C59, 0x7F67, + 0x0C27, 0x7F6B, + 0x0BF5, 0x7F70, + 0x0BC3, 0x7F75, + 0x0B91, 0x7F79, + 0x0B5F, 0x7F7E, + 0x0B2D, 0x7F82, + 0x0AFB, 0x7F87, + 0x0AC9, 0x7F8B, + 0x0A97, 0x7F8F, + 0x0A65, 0x7F93, + 0x0A33, 0x7F97, + 0x0A00, 0x7F9B, + 0x09CE, 0x7F9F, + 0x099C, 0x7FA3, + 0x096A, 0x7FA7, + 0x0938, 0x7FAA, + 0x0906, 0x7FAE, + 0x08D4, 0x7FB1, + 0x08A2, 0x7FB5, + 0x086F, 0x7FB8, + 0x083D, 0x7FBC, + 0x080B, 0x7FBF, + 0x07D9, 0x7FC2, + 0x07A7, 0x7FC5, + 0x0775, 0x7FC8, + 0x0742, 0x7FCB, + 0x0710, 0x7FCE, + 0x06DE, 0x7FD0, + 0x06AC, 0x7FD3, + 0x067A, 0x7FD6, + 0x0647, 0x7FD8, + 0x0615, 0x7FDA, + 0x05E3, 0x7FDD, + 0x05B1, 0x7FDF, + 0x057F, 0x7FE1, + 0x054C, 0x7FE3, + 0x051A, 0x7FE5, + 0x04E8, 0x7FE7, + 0x04B6, 0x7FE9, + 0x0483, 0x7FEB, + 0x0451, 0x7FED, + 0x041F, 0x7FEE, + 0x03ED, 0x7FF0, + 0x03BA, 0x7FF2, + 0x0388, 0x7FF3, + 0x0356, 0x7FF4, + 0x0324, 0x7FF6, + 0x02F1, 0x7FF7, + 0x02BF, 0x7FF8, + 0x028D, 0x7FF9, + 0x025B, 0x7FFA, + 0x0228, 0x7FFB, + 0x01F6, 0x7FFC, + 0x01C4, 0x7FFC, + 0x0192, 0x7FFD, + 0x015F, 0x7FFE, + 0x012D, 0x7FFE, + 0x00FB, 0x7FFF, + 0x00C9, 0x7FFF, + 0x0096, 0x7FFF, + 0x0064, 0x7FFF, + 0x0032, 0x7FFF, + 0x0000, 0x7FFF, + 0xFFCD, 0x7FFF, + 0xFF9B, 0x7FFF, + 0xFF69, 0x7FFF, + 0xFF36, 0x7FFF, + 0xFF04, 0x7FFF, + 0xFED2, 0x7FFE, + 0xFEA0, 0x7FFE, + 0xFE6D, 0x7FFD, + 0xFE3B, 0x7FFC, + 0xFE09, 0x7FFC, + 0xFDD7, 0x7FFB, + 0xFDA4, 0x7FFA, + 0xFD72, 0x7FF9, + 0xFD40, 0x7FF8, + 0xFD0E, 0x7FF7, + 0xFCDB, 0x7FF6, + 0xFCA9, 0x7FF4, + 0xFC77, 0x7FF3, + 0xFC45, 0x7FF2, + 0xFC12, 0x7FF0, + 0xFBE0, 0x7FEE, + 0xFBAE, 0x7FED, + 0xFB7C, 0x7FEB, + 0xFB49, 0x7FE9, + 0xFB17, 0x7FE7, + 0xFAE5, 0x7FE5, + 0xFAB3, 0x7FE3, + 0xFA80, 0x7FE1, + 0xFA4E, 0x7FDF, + 0xFA1C, 0x7FDD, + 0xF9EA, 0x7FDA, + 0xF9B8, 0x7FD8, + 0xF985, 0x7FD6, + 0xF953, 0x7FD3, + 0xF921, 0x7FD0, + 0xF8EF, 0x7FCE, + 0xF8BD, 0x7FCB, + 0xF88A, 0x7FC8, + 0xF858, 0x7FC5, + 0xF826, 0x7FC2, + 0xF7F4, 0x7FBF, + 0xF7C2, 0x7FBC, + 0xF790, 0x7FB8, + 0xF75D, 0x7FB5, + 0xF72B, 0x7FB1, + 0xF6F9, 0x7FAE, + 0xF6C7, 0x7FAA, + 0xF695, 0x7FA7, + 0xF663, 0x7FA3, + 0xF631, 0x7F9F, + 0xF5FF, 0x7F9B, + 0xF5CC, 0x7F97, + 0xF59A, 0x7F93, + 0xF568, 0x7F8F, + 0xF536, 0x7F8B, + 0xF504, 0x7F87, + 0xF4D2, 0x7F82, + 0xF4A0, 0x7F7E, + 0xF46E, 0x7F79, + 0xF43C, 0x7F75, + 0xF40A, 0x7F70, + 0xF3D8, 0x7F6B, + 0xF3A6, 0x7F67, + 0xF374, 0x7F62, + 0xF342, 0x7F5D, + 0xF310, 0x7F58, + 0xF2DE, 0x7F53, + 0xF2AC, 0x7F4D, + 0xF27A, 0x7F48, + 0xF248, 0x7F43, + 0xF216, 0x7F3D, + 0xF1E4, 0x7F38, + 0xF1B2, 0x7F32, + 0xF180, 0x7F2D, + 0xF14E, 0x7F27, + 0xF11C, 0x7F21, + 0xF0EA, 0x7F1B, + 0xF0B8, 0x7F15, + 0xF086, 0x7F0F, + 0xF054, 0x7F09, + 0xF022, 0x7F03, + 0xEFF1, 0x7EFD, + 0xEFBF, 0x7EF6, + 0xEF8D, 0x7EF0, + 0xEF5B, 0x7EE9, + 0xEF29, 0x7EE3, + 0xEEF7, 0x7EDC, + 0xEEC6, 0x7ED5, + 0xEE94, 0x7ECF, + 0xEE62, 0x7EC8, + 0xEE30, 0x7EC1, + 0xEDFE, 0x7EBA, + 0xEDCD, 0x7EB3, + 0xED9B, 0x7EAB, + 0xED69, 0x7EA4, + 0xED37, 0x7E9D, + 0xED06, 0x7E95, + 0xECD4, 0x7E8E, + 0xECA2, 0x7E86, + 0xEC71, 0x7E7F, + 0xEC3F, 0x7E77, + 0xEC0D, 0x7E6F, + 0xEBDC, 0x7E67, + 0xEBAA, 0x7E5F, + 0xEB78, 0x7E57, + 0xEB47, 0x7E4F, + 0xEB15, 0x7E47, + 0xEAE4, 0x7E3F, + 0xEAB2, 0x7E37, + 0xEA80, 0x7E2E, + 0xEA4F, 0x7E26, + 0xEA1D, 0x7E1D, + 0xE9EC, 0x7E14, + 0xE9BA, 0x7E0C, + 0xE989, 0x7E03, + 0xE957, 0x7DFA, + 0xE926, 0x7DF1, + 0xE8F5, 0x7DE8, + 0xE8C3, 0x7DDF, + 0xE892, 0x7DD6, + 0xE860, 0x7DCD, + 0xE82F, 0x7DC3, + 0xE7FD, 0x7DBA, + 0xE7CC, 0x7DB0, + 0xE79B, 0x7DA7, + 0xE769, 0x7D9D, + 0xE738, 0x7D94, + 0xE707, 0x7D8A, + 0xE6D5, 0x7D80, + 0xE6A4, 0x7D76, + 0xE673, 0x7D6C, + 0xE642, 0x7D62, + 0xE610, 0x7D58, + 0xE5DF, 0x7D4E, + 0xE5AE, 0x7D43, + 0xE57D, 0x7D39, + 0xE54C, 0x7D2F, + 0xE51B, 0x7D24, + 0xE4E9, 0x7D19, + 0xE4B8, 0x7D0F, + 0xE487, 0x7D04, + 0xE456, 0x7CF9, + 0xE425, 0x7CEE, + 0xE3F4, 0x7CE3, + 0xE3C3, 0x7CD8, + 0xE392, 0x7CCD, + 0xE361, 0x7CC2, + 0xE330, 0x7CB7, + 0xE2FF, 0x7CAB, + 0xE2CE, 0x7CA0, + 0xE29D, 0x7C94, + 0xE26C, 0x7C89, + 0xE23B, 0x7C7D, + 0xE20A, 0x7C71, + 0xE1DA, 0x7C66, + 0xE1A9, 0x7C5A, + 0xE178, 0x7C4E, + 0xE147, 0x7C42, + 0xE116, 0x7C36, + 0xE0E6, 0x7C29, + 0xE0B5, 0x7C1D, + 0xE084, 0x7C11, + 0xE053, 0x7C05, + 0xE023, 0x7BF8, + 0xDFF2, 0x7BEB, + 0xDFC1, 0x7BDF, + 0xDF91, 0x7BD2, + 0xDF60, 0x7BC5, + 0xDF2F, 0x7BB9, + 0xDEFF, 0x7BAC, + 0xDECE, 0x7B9F, + 0xDE9E, 0x7B92, + 0xDE6D, 0x7B84, + 0xDE3D, 0x7B77, + 0xDE0C, 0x7B6A, + 0xDDDC, 0x7B5D, + 0xDDAB, 0x7B4F, + 0xDD7B, 0x7B42, + 0xDD4B, 0x7B34, + 0xDD1A, 0x7B26, + 0xDCEA, 0x7B19, + 0xDCBA, 0x7B0B, + 0xDC89, 0x7AFD, + 0xDC59, 0x7AEF, + 0xDC29, 0x7AE1, + 0xDBF8, 0x7AD3, + 0xDBC8, 0x7AC5, + 0xDB98, 0x7AB6, + 0xDB68, 0x7AA8, + 0xDB38, 0x7A9A, + 0xDB08, 0x7A8B, + 0xDAD7, 0x7A7D, + 0xDAA7, 0x7A6E, + 0xDA77, 0x7A5F, + 0xDA47, 0x7A50, + 0xDA17, 0x7A42, + 0xD9E7, 0x7A33, + 0xD9B7, 0x7A24, + 0xD987, 0x7A15, + 0xD957, 0x7A05, + 0xD927, 0x79F6, + 0xD8F8, 0x79E7, + 0xD8C8, 0x79D8, + 0xD898, 0x79C8, + 0xD868, 0x79B9, + 0xD838, 0x79A9, + 0xD809, 0x7999, + 0xD7D9, 0x798A, + 0xD7A9, 0x797A, + 0xD779, 0x796A, + 0xD74A, 0x795A, + 0xD71A, 0x794A, + 0xD6EA, 0x793A, + 0xD6BB, 0x792A, + 0xD68B, 0x7919, + 0xD65C, 0x7909, + 0xD62C, 0x78F9, + 0xD5FD, 0x78E8, + 0xD5CD, 0x78D8, + 0xD59E, 0x78C7, + 0xD56E, 0x78B6, + 0xD53F, 0x78A6, + 0xD510, 0x7895, + 0xD4E0, 0x7884, + 0xD4B1, 0x7873, + 0xD482, 0x7862, + 0xD452, 0x7851, + 0xD423, 0x7840, + 0xD3F4, 0x782E, + 0xD3C5, 0x781D, + 0xD396, 0x780C, + 0xD367, 0x77FA, + 0xD337, 0x77E9, + 0xD308, 0x77D7, + 0xD2D9, 0x77C5, + 0xD2AA, 0x77B4, + 0xD27B, 0x77A2, + 0xD24C, 0x7790, + 0xD21D, 0x777E, + 0xD1EE, 0x776C, + 0xD1C0, 0x775A, + 0xD191, 0x7747, + 0xD162, 0x7735, + 0xD133, 0x7723, + 0xD104, 0x7710, + 0xD0D6, 0x76FE, + 0xD0A7, 0x76EB, + 0xD078, 0x76D9, + 0xD04A, 0x76C6, + 0xD01B, 0x76B3, + 0xCFEC, 0x76A0, + 0xCFBE, 0x768E, + 0xCF8F, 0x767B, + 0xCF61, 0x7668, + 0xCF32, 0x7654, + 0xCF04, 0x7641, + 0xCED5, 0x762E, + 0xCEA7, 0x761B, + 0xCE79, 0x7607, + 0xCE4A, 0x75F4, + 0xCE1C, 0x75E0, + 0xCDEE, 0x75CC, + 0xCDBF, 0x75B9, + 0xCD91, 0x75A5, + 0xCD63, 0x7591, + 0xCD35, 0x757D, + 0xCD07, 0x7569, + 0xCCD9, 0x7555, + 0xCCAB, 0x7541, + 0xCC7D, 0x752D, + 0xCC4F, 0x7519, + 0xCC21, 0x7504, + 0xCBF3, 0x74F0, + 0xCBC5, 0x74DB, + 0xCB97, 0x74C7, + 0xCB69, 0x74B2, + 0xCB3B, 0x749E, + 0xCB0D, 0x7489, + 0xCAE0, 0x7474, + 0xCAB2, 0x745F, + 0xCA84, 0x744A, + 0xCA57, 0x7435, + 0xCA29, 0x7420, + 0xC9FB, 0x740B, + 0xC9CE, 0x73F6, + 0xC9A0, 0x73E0, + 0xC973, 0x73CB, + 0xC945, 0x73B5, + 0xC918, 0x73A0, + 0xC8EB, 0x738A, + 0xC8BD, 0x7375, + 0xC890, 0x735F, + 0xC863, 0x7349, + 0xC835, 0x7333, + 0xC808, 0x731D, + 0xC7DB, 0x7307, + 0xC7AE, 0x72F1, + 0xC781, 0x72DB, + 0xC754, 0x72C5, + 0xC727, 0x72AF, + 0xC6F9, 0x7298, + 0xC6CD, 0x7282, + 0xC6A0, 0x726B, + 0xC673, 0x7255, + 0xC646, 0x723E, + 0xC619, 0x7227, + 0xC5EC, 0x7211, + 0xC5BF, 0x71FA, + 0xC593, 0x71E3, + 0xC566, 0x71CC, + 0xC539, 0x71B5, + 0xC50D, 0x719E, + 0xC4E0, 0x7186, + 0xC4B3, 0x716F, + 0xC487, 0x7158, + 0xC45A, 0x7141, + 0xC42E, 0x7129, + 0xC402, 0x7112, + 0xC3D5, 0x70FA, + 0xC3A9, 0x70E2, + 0xC37C, 0x70CB, + 0xC350, 0x70B3, + 0xC324, 0x709B, + 0xC2F8, 0x7083, + 0xC2CC, 0x706B, + 0xC29F, 0x7053, + 0xC273, 0x703B, + 0xC247, 0x7023, + 0xC21B, 0x700A, + 0xC1EF, 0x6FF2, + 0xC1C3, 0x6FDA, + 0xC197, 0x6FC1, + 0xC16C, 0x6FA9, + 0xC140, 0x6F90, + 0xC114, 0x6F77, + 0xC0E8, 0x6F5F, + 0xC0BC, 0x6F46, + 0xC091, 0x6F2D, + 0xC065, 0x6F14, + 0xC03A, 0x6EFB, + 0xC00E, 0x6EE2, + 0xBFE2, 0x6EC9, + 0xBFB7, 0x6EAF, + 0xBF8C, 0x6E96, + 0xBF60, 0x6E7D, + 0xBF35, 0x6E63, + 0xBF09, 0x6E4A, + 0xBEDE, 0x6E30, + 0xBEB3, 0x6E17, + 0xBE88, 0x6DFD, + 0xBE5D, 0x6DE3, + 0xBE31, 0x6DCA, + 0xBE06, 0x6DB0, + 0xBDDB, 0x6D96, + 0xBDB0, 0x6D7C, + 0xBD85, 0x6D62, + 0xBD5A, 0x6D48, + 0xBD2F, 0x6D2D, + 0xBD05, 0x6D13, + 0xBCDA, 0x6CF9, + 0xBCAF, 0x6CDE, + 0xBC84, 0x6CC4, + 0xBC5A, 0x6CA9, + 0xBC2F, 0x6C8F, + 0xBC04, 0x6C74, + 0xBBDA, 0x6C59, + 0xBBAF, 0x6C3F, + 0xBB85, 0x6C24, + 0xBB5A, 0x6C09, + 0xBB30, 0x6BEE, + 0xBB05, 0x6BD3, + 0xBADB, 0x6BB8, + 0xBAB1, 0x6B9C, + 0xBA87, 0x6B81, + 0xBA5C, 0x6B66, + 0xBA32, 0x6B4A, + 0xBA08, 0x6B2F, + 0xB9DE, 0x6B13, + 0xB9B4, 0x6AF8, + 0xB98A, 0x6ADC, + 0xB960, 0x6AC1, + 0xB936, 0x6AA5, + 0xB90C, 0x6A89, + 0xB8E3, 0x6A6D, + 0xB8B9, 0x6A51, + 0xB88F, 0x6A35, + 0xB865, 0x6A19, + 0xB83C, 0x69FD, + 0xB812, 0x69E1, + 0xB7E9, 0x69C4, + 0xB7BF, 0x69A8, + 0xB796, 0x698C, + 0xB76C, 0x696F, + 0xB743, 0x6953, + 0xB719, 0x6936, + 0xB6F0, 0x6919, + 0xB6C7, 0x68FD, + 0xB69E, 0x68E0, + 0xB675, 0x68C3, + 0xB64B, 0x68A6, + 0xB622, 0x6889, + 0xB5F9, 0x686C, + 0xB5D0, 0x684F, + 0xB5A7, 0x6832, + 0xB57E, 0x6815, + 0xB556, 0x67F7, + 0xB52D, 0x67DA, + 0xB504, 0x67BD, + 0xB4DB, 0x679F, + 0xB4B3, 0x6782, + 0xB48A, 0x6764, + 0xB461, 0x6746, + 0xB439, 0x6729, + 0xB410, 0x670B, + 0xB3E8, 0x66ED, + 0xB3C0, 0x66CF, + 0xB397, 0x66B1, + 0xB36F, 0x6693, + 0xB347, 0x6675, + 0xB31E, 0x6657, + 0xB2F6, 0x6639, + 0xB2CE, 0x661A, + 0xB2A6, 0x65FC, + 0xB27E, 0x65DD, + 0xB256, 0x65BF, + 0xB22E, 0x65A0, + 0xB206, 0x6582, + 0xB1DE, 0x6563, + 0xB1B7, 0x6545, + 0xB18F, 0x6526, + 0xB167, 0x6507, + 0xB140, 0x64E8, + 0xB118, 0x64C9, + 0xB0F0, 0x64AA, + 0xB0C9, 0x648B, + 0xB0A1, 0x646C, + 0xB07A, 0x644D, + 0xB053, 0x642D, + 0xB02B, 0x640E, + 0xB004, 0x63EF, + 0xAFDD, 0x63CF, + 0xAFB6, 0x63B0, + 0xAF8F, 0x6390, + 0xAF68, 0x6371, + 0xAF40, 0x6351, + 0xAF1A, 0x6331, + 0xAEF3, 0x6311, + 0xAECC, 0x62F2, + 0xAEA5, 0x62D2, + 0xAE7E, 0x62B2, + 0xAE57, 0x6292, + 0xAE31, 0x6271, + 0xAE0A, 0x6251, + 0xADE3, 0x6231, + 0xADBD, 0x6211, + 0xAD96, 0x61F1, + 0xAD70, 0x61D0, + 0xAD4A, 0x61B0, + 0xAD23, 0x618F, + 0xACFD, 0x616F, + 0xACD7, 0x614E, + 0xACB1, 0x612D, + 0xAC8A, 0x610D, + 0xAC64, 0x60EC, + 0xAC3E, 0x60CB, + 0xAC18, 0x60AA, + 0xABF2, 0x6089, + 0xABCC, 0x6068, + 0xABA7, 0x6047, + 0xAB81, 0x6026, + 0xAB5B, 0x6004, + 0xAB35, 0x5FE3, + 0xAB10, 0x5FC2, + 0xAAEA, 0x5FA0, + 0xAAC5, 0x5F7F, + 0xAA9F, 0x5F5E, + 0xAA7A, 0x5F3C, + 0xAA54, 0x5F1A, + 0xAA2F, 0x5EF9, + 0xAA0A, 0x5ED7, + 0xA9E5, 0x5EB5, + 0xA9BF, 0x5E93, + 0xA99A, 0x5E71, + 0xA975, 0x5E50, + 0xA950, 0x5E2D, + 0xA92B, 0x5E0B, + 0xA906, 0x5DE9, + 0xA8E2, 0x5DC7, + 0xA8BD, 0x5DA5, + 0xA898, 0x5D83, + 0xA873, 0x5D60, + 0xA84F, 0x5D3E, + 0xA82A, 0x5D1B, + 0xA806, 0x5CF9, + 0xA7E1, 0x5CD6, + 0xA7BD, 0x5CB4, + 0xA798, 0x5C91, + 0xA774, 0x5C6E, + 0xA750, 0x5C4B, + 0xA72B, 0x5C29, + 0xA707, 0x5C06, + 0xA6E3, 0x5BE3, + 0xA6BF, 0x5BC0, + 0xA69B, 0x5B9D, + 0xA677, 0x5B79, + 0xA653, 0x5B56, + 0xA62F, 0x5B33, + 0xA60C, 0x5B10, + 0xA5E8, 0x5AEC, + 0xA5C4, 0x5AC9, + 0xA5A1, 0x5AA5, + 0xA57D, 0x5A82, + 0xA55A, 0x5A5E, + 0xA536, 0x5A3B, + 0xA513, 0x5A17, + 0xA4EF, 0x59F3, + 0xA4CC, 0x59D0, + 0xA4A9, 0x59AC, + 0xA486, 0x5988, + 0xA462, 0x5964, + 0xA43F, 0x5940, + 0xA41C, 0x591C, + 0xA3F9, 0x58F8, + 0xA3D6, 0x58D4, + 0xA3B4, 0x58AF, + 0xA391, 0x588B, + 0xA36E, 0x5867, + 0xA34B, 0x5842, + 0xA329, 0x581E, + 0xA306, 0x57F9, + 0xA2E4, 0x57D5, + 0xA2C1, 0x57B0, + 0xA29F, 0x578C, + 0xA27C, 0x5767, + 0xA25A, 0x5742, + 0xA238, 0x571D, + 0xA216, 0x56F9, + 0xA1F4, 0x56D4, + 0xA1D2, 0x56AF, + 0xA1AF, 0x568A, + 0xA18E, 0x5665, + 0xA16C, 0x5640, + 0xA14A, 0x561A, + 0xA128, 0x55F5, + 0xA106, 0x55D0, + 0xA0E5, 0x55AB, + 0xA0C3, 0x5585, + 0xA0A1, 0x5560, + 0xA080, 0x553A, + 0xA05F, 0x5515, + 0xA03D, 0x54EF, + 0xA01C, 0x54CA, + 0x9FFB, 0x54A4, + 0x9FD9, 0x547E, + 0x9FB8, 0x5458, + 0x9F97, 0x5433, + 0x9F76, 0x540D, + 0x9F55, 0x53E7, + 0x9F34, 0x53C1, + 0x9F13, 0x539B, + 0x9EF2, 0x5375, + 0x9ED2, 0x534E, + 0x9EB1, 0x5328, + 0x9E90, 0x5302, + 0x9E70, 0x52DC, + 0x9E4F, 0x52B5, + 0x9E2F, 0x528F, + 0x9E0E, 0x5269, + 0x9DEE, 0x5242, + 0x9DCE, 0x521C, + 0x9DAE, 0x51F5, + 0x9D8E, 0x51CE, + 0x9D6D, 0x51A8, + 0x9D4D, 0x5181, + 0x9D2D, 0x515A, + 0x9D0D, 0x5133, + 0x9CEE, 0x510C, + 0x9CCE, 0x50E5, + 0x9CAE, 0x50BF, + 0x9C8E, 0x5097, + 0x9C6F, 0x5070, + 0x9C4F, 0x5049, + 0x9C30, 0x5022, + 0x9C10, 0x4FFB, + 0x9BF1, 0x4FD4, + 0x9BD2, 0x4FAC, + 0x9BB2, 0x4F85, + 0x9B93, 0x4F5E, + 0x9B74, 0x4F36, + 0x9B55, 0x4F0F, + 0x9B36, 0x4EE7, + 0x9B17, 0x4EBF, + 0x9AF8, 0x4E98, + 0x9AD9, 0x4E70, + 0x9ABA, 0x4E48, + 0x9A9C, 0x4E21, + 0x9A7D, 0x4DF9, + 0x9A5F, 0x4DD1, + 0x9A40, 0x4DA9, + 0x9A22, 0x4D81, + 0x9A03, 0x4D59, + 0x99E5, 0x4D31, + 0x99C6, 0x4D09, + 0x99A8, 0x4CE1, + 0x998A, 0x4CB8, + 0x996C, 0x4C90, + 0x994E, 0x4C68, + 0x9930, 0x4C3F, + 0x9912, 0x4C17, + 0x98F4, 0x4BEF, + 0x98D6, 0x4BC6, + 0x98B9, 0x4B9E, + 0x989B, 0x4B75, + 0x987D, 0x4B4C, + 0x9860, 0x4B24, + 0x9842, 0x4AFB, + 0x9825, 0x4AD2, + 0x9808, 0x4AA9, + 0x97EA, 0x4A81, + 0x97CD, 0x4A58, + 0x97B0, 0x4A2F, + 0x9793, 0x4A06, + 0x9776, 0x49DD, + 0x9759, 0x49B4, + 0x973C, 0x498A, + 0x971F, 0x4961, + 0x9702, 0x4938, + 0x96E6, 0x490F, + 0x96C9, 0x48E6, + 0x96AC, 0x48BC, + 0x9690, 0x4893, + 0x9673, 0x4869, + 0x9657, 0x4840, + 0x963B, 0x4816, + 0x961E, 0x47ED, + 0x9602, 0x47C3, + 0x95E6, 0x479A, + 0x95CA, 0x4770, + 0x95AE, 0x4746, + 0x9592, 0x471C, + 0x9576, 0x46F3, + 0x955A, 0x46C9, + 0x953E, 0x469F, + 0x9523, 0x4675, + 0x9507, 0x464B, + 0x94EC, 0x4621, + 0x94D0, 0x45F7, + 0x94B5, 0x45CD, + 0x9499, 0x45A3, + 0x947E, 0x4578, + 0x9463, 0x454E, + 0x9447, 0x4524, + 0x942C, 0x44FA, + 0x9411, 0x44CF, + 0x93F6, 0x44A5, + 0x93DB, 0x447A, + 0x93C0, 0x4450, + 0x93A6, 0x4425, + 0x938B, 0x43FB, + 0x9370, 0x43D0, + 0x9356, 0x43A5, + 0x933B, 0x437B, + 0x9321, 0x4350, + 0x9306, 0x4325, + 0x92EC, 0x42FA, + 0x92D2, 0x42D0, + 0x92B7, 0x42A5, + 0x929D, 0x427A, + 0x9283, 0x424F, + 0x9269, 0x4224, + 0x924F, 0x41F9, + 0x9235, 0x41CE, + 0x921C, 0x41A2, + 0x9202, 0x4177, + 0x91E8, 0x414C, + 0x91CF, 0x4121, + 0x91B5, 0x40F6, + 0x919C, 0x40CA, + 0x9182, 0x409F, + 0x9169, 0x4073, + 0x9150, 0x4048, + 0x9136, 0x401D, + 0x911D, 0x3FF1, + 0x9104, 0x3FC5, + 0x90EB, 0x3F9A, + 0x90D2, 0x3F6E, + 0x90B9, 0x3F43, + 0x90A0, 0x3F17, + 0x9088, 0x3EEB, + 0x906F, 0x3EBF, + 0x9056, 0x3E93, + 0x903E, 0x3E68, + 0x9025, 0x3E3C, + 0x900D, 0x3E10, + 0x8FF5, 0x3DE4, + 0x8FDC, 0x3DB8, + 0x8FC4, 0x3D8C, + 0x8FAC, 0x3D60, + 0x8F94, 0x3D33, + 0x8F7C, 0x3D07, + 0x8F64, 0x3CDB, + 0x8F4C, 0x3CAF, + 0x8F34, 0x3C83, + 0x8F1D, 0x3C56, + 0x8F05, 0x3C2A, + 0x8EED, 0x3BFD, + 0x8ED6, 0x3BD1, + 0x8EBE, 0x3BA5, + 0x8EA7, 0x3B78, + 0x8E90, 0x3B4C, + 0x8E79, 0x3B1F, + 0x8E61, 0x3AF2, + 0x8E4A, 0x3AC6, + 0x8E33, 0x3A99, + 0x8E1C, 0x3A6C, + 0x8E05, 0x3A40, + 0x8DEE, 0x3A13, + 0x8DD8, 0x39E6, + 0x8DC1, 0x39B9, + 0x8DAA, 0x398C, + 0x8D94, 0x395F, + 0x8D7D, 0x3932, + 0x8D67, 0x3906, + 0x8D50, 0x38D8, + 0x8D3A, 0x38AB, + 0x8D24, 0x387E, + 0x8D0E, 0x3851, + 0x8CF8, 0x3824, + 0x8CE2, 0x37F7, + 0x8CCC, 0x37CA, + 0x8CB6, 0x379C, + 0x8CA0, 0x376F, + 0x8C8A, 0x3742, + 0x8C75, 0x3714, + 0x8C5F, 0x36E7, + 0x8C4A, 0x36BA, + 0x8C34, 0x368C, + 0x8C1F, 0x365F, + 0x8C09, 0x3631, + 0x8BF4, 0x3604, + 0x8BDF, 0x35D6, + 0x8BCA, 0x35A8, + 0x8BB5, 0x357B, + 0x8BA0, 0x354D, + 0x8B8B, 0x351F, + 0x8B76, 0x34F2, + 0x8B61, 0x34C4, + 0x8B4D, 0x3496, + 0x8B38, 0x3468, + 0x8B24, 0x343A, + 0x8B0F, 0x340C, + 0x8AFB, 0x33DE, + 0x8AE6, 0x33B0, + 0x8AD2, 0x3382, + 0x8ABE, 0x3354, + 0x8AAA, 0x3326, + 0x8A96, 0x32F8, + 0x8A82, 0x32CA, + 0x8A6E, 0x329C, + 0x8A5A, 0x326E, + 0x8A46, 0x3240, + 0x8A33, 0x3211, + 0x8A1F, 0x31E3, + 0x8A0B, 0x31B5, + 0x89F8, 0x3186, + 0x89E4, 0x3158, + 0x89D1, 0x312A, + 0x89BE, 0x30FB, + 0x89AB, 0x30CD, + 0x8997, 0x309E, + 0x8984, 0x3070, + 0x8971, 0x3041, + 0x895F, 0x3013, + 0x894C, 0x2FE4, + 0x8939, 0x2FB5, + 0x8926, 0x2F87, + 0x8914, 0x2F58, + 0x8901, 0x2F29, + 0x88EF, 0x2EFB, + 0x88DC, 0x2ECC, + 0x88CA, 0x2E9D, + 0x88B8, 0x2E6E, + 0x88A5, 0x2E3F, + 0x8893, 0x2E11, + 0x8881, 0x2DE2, + 0x886F, 0x2DB3, + 0x885D, 0x2D84, + 0x884B, 0x2D55, + 0x883A, 0x2D26, + 0x8828, 0x2CF7, + 0x8816, 0x2CC8, + 0x8805, 0x2C98, + 0x87F3, 0x2C69, + 0x87E2, 0x2C3A, + 0x87D1, 0x2C0B, + 0x87BF, 0x2BDC, + 0x87AE, 0x2BAD, + 0x879D, 0x2B7D, + 0x878C, 0x2B4E, + 0x877B, 0x2B1F, + 0x876A, 0x2AEF, + 0x8759, 0x2AC0, + 0x8749, 0x2A91, + 0x8738, 0x2A61, + 0x8727, 0x2A32, + 0x8717, 0x2A02, + 0x8706, 0x29D3, + 0x86F6, 0x29A3, + 0x86E6, 0x2974, + 0x86D5, 0x2944, + 0x86C5, 0x2915, + 0x86B5, 0x28E5, + 0x86A5, 0x28B5, + 0x8695, 0x2886, + 0x8685, 0x2856, + 0x8675, 0x2826, + 0x8666, 0x27F6, + 0x8656, 0x27C7, + 0x8646, 0x2797, + 0x8637, 0x2767, + 0x8627, 0x2737, + 0x8618, 0x2707, + 0x8609, 0x26D8, + 0x85FA, 0x26A8, + 0x85EA, 0x2678, + 0x85DB, 0x2648, + 0x85CC, 0x2618, + 0x85BD, 0x25E8, + 0x85AF, 0x25B8, + 0x85A0, 0x2588, + 0x8591, 0x2558, + 0x8582, 0x2528, + 0x8574, 0x24F7, + 0x8565, 0x24C7, + 0x8557, 0x2497, + 0x8549, 0x2467, + 0x853A, 0x2437, + 0x852C, 0x2407, + 0x851E, 0x23D6, + 0x8510, 0x23A6, + 0x8502, 0x2376, + 0x84F4, 0x2345, + 0x84E6, 0x2315, + 0x84D9, 0x22E5, + 0x84CB, 0x22B4, + 0x84BD, 0x2284, + 0x84B0, 0x2254, + 0x84A2, 0x2223, + 0x8495, 0x21F3, + 0x8488, 0x21C2, + 0x847B, 0x2192, + 0x846D, 0x2161, + 0x8460, 0x2131, + 0x8453, 0x2100, + 0x8446, 0x20D0, + 0x843A, 0x209F, + 0x842D, 0x206E, + 0x8420, 0x203E, + 0x8414, 0x200D, + 0x8407, 0x1FDC, + 0x83FA, 0x1FAC, + 0x83EE, 0x1F7B, + 0x83E2, 0x1F4A, + 0x83D6, 0x1F19, + 0x83C9, 0x1EE9, + 0x83BD, 0x1EB8, + 0x83B1, 0x1E87, + 0x83A5, 0x1E56, + 0x8399, 0x1E25, + 0x838E, 0x1DF5, + 0x8382, 0x1DC4, + 0x8376, 0x1D93, + 0x836B, 0x1D62, + 0x835F, 0x1D31, + 0x8354, 0x1D00, + 0x8348, 0x1CCF, + 0x833D, 0x1C9E, + 0x8332, 0x1C6D, + 0x8327, 0x1C3C, + 0x831C, 0x1C0B, + 0x8311, 0x1BDA, + 0x8306, 0x1BA9, + 0x82FB, 0x1B78, + 0x82F0, 0x1B47, + 0x82E6, 0x1B16, + 0x82DB, 0x1AE4, + 0x82D0, 0x1AB3, + 0x82C6, 0x1A82, + 0x82BC, 0x1A51, + 0x82B1, 0x1A20, + 0x82A7, 0x19EF, + 0x829D, 0x19BD, + 0x8293, 0x198C, + 0x8289, 0x195B, + 0x827F, 0x192A, + 0x8275, 0x18F8, + 0x826B, 0x18C7, + 0x8262, 0x1896, + 0x8258, 0x1864, + 0x824F, 0x1833, + 0x8245, 0x1802, + 0x823C, 0x17D0, + 0x8232, 0x179F, + 0x8229, 0x176D, + 0x8220, 0x173C, + 0x8217, 0x170A, + 0x820E, 0x16D9, + 0x8205, 0x16A8, + 0x81FC, 0x1676, + 0x81F3, 0x1645, + 0x81EB, 0x1613, + 0x81E2, 0x15E2, + 0x81D9, 0x15B0, + 0x81D1, 0x157F, + 0x81C8, 0x154D, + 0x81C0, 0x151B, + 0x81B8, 0x14EA, + 0x81B0, 0x14B8, + 0x81A8, 0x1487, + 0x81A0, 0x1455, + 0x8198, 0x1423, + 0x8190, 0x13F2, + 0x8188, 0x13C0, + 0x8180, 0x138E, + 0x8179, 0x135D, + 0x8171, 0x132B, + 0x816A, 0x12F9, + 0x8162, 0x12C8, + 0x815B, 0x1296, + 0x8154, 0x1264, + 0x814C, 0x1232, + 0x8145, 0x1201, + 0x813E, 0x11CF, + 0x8137, 0x119D, + 0x8130, 0x116B, + 0x812A, 0x1139, + 0x8123, 0x1108, + 0x811C, 0x10D6, + 0x8116, 0x10A4, + 0x810F, 0x1072, + 0x8109, 0x1040, + 0x8102, 0x100E, + 0x80FC, 0x0FDD, + 0x80F6, 0x0FAB, + 0x80F0, 0x0F79, + 0x80EA, 0x0F47, + 0x80E4, 0x0F15, + 0x80DE, 0x0EE3, + 0x80D8, 0x0EB1, + 0x80D2, 0x0E7F, + 0x80CD, 0x0E4D, + 0x80C7, 0x0E1B, + 0x80C2, 0x0DE9, + 0x80BC, 0x0DB7, + 0x80B7, 0x0D85, + 0x80B2, 0x0D53, + 0x80AC, 0x0D21, + 0x80A7, 0x0CEF, + 0x80A2, 0x0CBD, + 0x809D, 0x0C8B, + 0x8098, 0x0C59, + 0x8094, 0x0C27, + 0x808F, 0x0BF5, + 0x808A, 0x0BC3, + 0x8086, 0x0B91, + 0x8081, 0x0B5F, + 0x807D, 0x0B2D, + 0x8078, 0x0AFB, + 0x8074, 0x0AC9, + 0x8070, 0x0A97, + 0x806C, 0x0A65, + 0x8068, 0x0A33, + 0x8064, 0x0A00, + 0x8060, 0x09CE, + 0x805C, 0x099C, + 0x8058, 0x096A, + 0x8055, 0x0938, + 0x8051, 0x0906, + 0x804E, 0x08D4, + 0x804A, 0x08A2, + 0x8047, 0x086F, + 0x8043, 0x083D, + 0x8040, 0x080B, + 0x803D, 0x07D9, + 0x803A, 0x07A7, + 0x8037, 0x0775, + 0x8034, 0x0742, + 0x8031, 0x0710, + 0x802F, 0x06DE, + 0x802C, 0x06AC, + 0x8029, 0x067A, + 0x8027, 0x0647, + 0x8025, 0x0615, + 0x8022, 0x05E3, + 0x8020, 0x05B1, + 0x801E, 0x057F, + 0x801C, 0x054C, + 0x801A, 0x051A, + 0x8018, 0x04E8, + 0x8016, 0x04B6, + 0x8014, 0x0483, + 0x8012, 0x0451, + 0x8011, 0x041F, + 0x800F, 0x03ED, + 0x800D, 0x03BA, + 0x800C, 0x0388, + 0x800B, 0x0356, + 0x8009, 0x0324, + 0x8008, 0x02F1, + 0x8007, 0x02BF, + 0x8006, 0x028D, + 0x8005, 0x025B, + 0x8004, 0x0228, + 0x8003, 0x01F6, + 0x8003, 0x01C4, + 0x8002, 0x0192, + 0x8001, 0x015F, + 0x8001, 0x012D, + 0x8000, 0x00FB, + 0x8000, 0x00C9, + 0x8000, 0x0096, + 0x8000, 0x0064, + 0x8000, 0x0032, + 0x8000, 0x0000, + 0x8000, 0xFFCD, + 0x8000, 0xFF9B, + 0x8000, 0xFF69, + 0x8000, 0xFF36, + 0x8000, 0xFF04, + 0x8001, 0xFED2, + 0x8001, 0xFEA0, + 0x8002, 0xFE6D, + 0x8003, 0xFE3B, + 0x8003, 0xFE09, + 0x8004, 0xFDD7, + 0x8005, 0xFDA4, + 0x8006, 0xFD72, + 0x8007, 0xFD40, + 0x8008, 0xFD0E, + 0x8009, 0xFCDB, + 0x800B, 0xFCA9, + 0x800C, 0xFC77, + 0x800D, 0xFC45, + 0x800F, 0xFC12, + 0x8011, 0xFBE0, + 0x8012, 0xFBAE, + 0x8014, 0xFB7C, + 0x8016, 0xFB49, + 0x8018, 0xFB17, + 0x801A, 0xFAE5, + 0x801C, 0xFAB3, + 0x801E, 0xFA80, + 0x8020, 0xFA4E, + 0x8022, 0xFA1C, + 0x8025, 0xF9EA, + 0x8027, 0xF9B8, + 0x8029, 0xF985, + 0x802C, 0xF953, + 0x802F, 0xF921, + 0x8031, 0xF8EF, + 0x8034, 0xF8BD, + 0x8037, 0xF88A, + 0x803A, 0xF858, + 0x803D, 0xF826, + 0x8040, 0xF7F4, + 0x8043, 0xF7C2, + 0x8047, 0xF790, + 0x804A, 0xF75D, + 0x804E, 0xF72B, + 0x8051, 0xF6F9, + 0x8055, 0xF6C7, + 0x8058, 0xF695, + 0x805C, 0xF663, + 0x8060, 0xF631, + 0x8064, 0xF5FF, + 0x8068, 0xF5CC, + 0x806C, 0xF59A, + 0x8070, 0xF568, + 0x8074, 0xF536, + 0x8078, 0xF504, + 0x807D, 0xF4D2, + 0x8081, 0xF4A0, + 0x8086, 0xF46E, + 0x808A, 0xF43C, + 0x808F, 0xF40A, + 0x8094, 0xF3D8, + 0x8098, 0xF3A6, + 0x809D, 0xF374, + 0x80A2, 0xF342, + 0x80A7, 0xF310, + 0x80AC, 0xF2DE, + 0x80B2, 0xF2AC, + 0x80B7, 0xF27A, + 0x80BC, 0xF248, + 0x80C2, 0xF216, + 0x80C7, 0xF1E4, + 0x80CD, 0xF1B2, + 0x80D2, 0xF180, + 0x80D8, 0xF14E, + 0x80DE, 0xF11C, + 0x80E4, 0xF0EA, + 0x80EA, 0xF0B8, + 0x80F0, 0xF086, + 0x80F6, 0xF054, + 0x80FC, 0xF022, + 0x8102, 0xEFF1, + 0x8109, 0xEFBF, + 0x810F, 0xEF8D, + 0x8116, 0xEF5B, + 0x811C, 0xEF29, + 0x8123, 0xEEF7, + 0x812A, 0xEEC6, + 0x8130, 0xEE94, + 0x8137, 0xEE62, + 0x813E, 0xEE30, + 0x8145, 0xEDFE, + 0x814C, 0xEDCD, + 0x8154, 0xED9B, + 0x815B, 0xED69, + 0x8162, 0xED37, + 0x816A, 0xED06, + 0x8171, 0xECD4, + 0x8179, 0xECA2, + 0x8180, 0xEC71, + 0x8188, 0xEC3F, + 0x8190, 0xEC0D, + 0x8198, 0xEBDC, + 0x81A0, 0xEBAA, + 0x81A8, 0xEB78, + 0x81B0, 0xEB47, + 0x81B8, 0xEB15, + 0x81C0, 0xEAE4, + 0x81C8, 0xEAB2, + 0x81D1, 0xEA80, + 0x81D9, 0xEA4F, + 0x81E2, 0xEA1D, + 0x81EB, 0xE9EC, + 0x81F3, 0xE9BA, + 0x81FC, 0xE989, + 0x8205, 0xE957, + 0x820E, 0xE926, + 0x8217, 0xE8F5, + 0x8220, 0xE8C3, + 0x8229, 0xE892, + 0x8232, 0xE860, + 0x823C, 0xE82F, + 0x8245, 0xE7FD, + 0x824F, 0xE7CC, + 0x8258, 0xE79B, + 0x8262, 0xE769, + 0x826B, 0xE738, + 0x8275, 0xE707, + 0x827F, 0xE6D5, + 0x8289, 0xE6A4, + 0x8293, 0xE673, + 0x829D, 0xE642, + 0x82A7, 0xE610, + 0x82B1, 0xE5DF, + 0x82BC, 0xE5AE, + 0x82C6, 0xE57D, + 0x82D0, 0xE54C, + 0x82DB, 0xE51B, + 0x82E6, 0xE4E9, + 0x82F0, 0xE4B8, + 0x82FB, 0xE487, + 0x8306, 0xE456, + 0x8311, 0xE425, + 0x831C, 0xE3F4, + 0x8327, 0xE3C3, + 0x8332, 0xE392, + 0x833D, 0xE361, + 0x8348, 0xE330, + 0x8354, 0xE2FF, + 0x835F, 0xE2CE, + 0x836B, 0xE29D, + 0x8376, 0xE26C, + 0x8382, 0xE23B, + 0x838E, 0xE20A, + 0x8399, 0xE1DA, + 0x83A5, 0xE1A9, + 0x83B1, 0xE178, + 0x83BD, 0xE147, + 0x83C9, 0xE116, + 0x83D6, 0xE0E6, + 0x83E2, 0xE0B5, + 0x83EE, 0xE084, + 0x83FA, 0xE053, + 0x8407, 0xE023, + 0x8414, 0xDFF2, + 0x8420, 0xDFC1, + 0x842D, 0xDF91, + 0x843A, 0xDF60, + 0x8446, 0xDF2F, + 0x8453, 0xDEFF, + 0x8460, 0xDECE, + 0x846D, 0xDE9E, + 0x847B, 0xDE6D, + 0x8488, 0xDE3D, + 0x8495, 0xDE0C, + 0x84A2, 0xDDDC, + 0x84B0, 0xDDAB, + 0x84BD, 0xDD7B, + 0x84CB, 0xDD4B, + 0x84D9, 0xDD1A, + 0x84E6, 0xDCEA, + 0x84F4, 0xDCBA, + 0x8502, 0xDC89, + 0x8510, 0xDC59, + 0x851E, 0xDC29, + 0x852C, 0xDBF8, + 0x853A, 0xDBC8, + 0x8549, 0xDB98, + 0x8557, 0xDB68, + 0x8565, 0xDB38, + 0x8574, 0xDB08, + 0x8582, 0xDAD7, + 0x8591, 0xDAA7, + 0x85A0, 0xDA77, + 0x85AF, 0xDA47, + 0x85BD, 0xDA17, + 0x85CC, 0xD9E7, + 0x85DB, 0xD9B7, + 0x85EA, 0xD987, + 0x85FA, 0xD957, + 0x8609, 0xD927, + 0x8618, 0xD8F8, + 0x8627, 0xD8C8, + 0x8637, 0xD898, + 0x8646, 0xD868, + 0x8656, 0xD838, + 0x8666, 0xD809, + 0x8675, 0xD7D9, + 0x8685, 0xD7A9, + 0x8695, 0xD779, + 0x86A5, 0xD74A, + 0x86B5, 0xD71A, + 0x86C5, 0xD6EA, + 0x86D5, 0xD6BB, + 0x86E6, 0xD68B, + 0x86F6, 0xD65C, + 0x8706, 0xD62C, + 0x8717, 0xD5FD, + 0x8727, 0xD5CD, + 0x8738, 0xD59E, + 0x8749, 0xD56E, + 0x8759, 0xD53F, + 0x876A, 0xD510, + 0x877B, 0xD4E0, + 0x878C, 0xD4B1, + 0x879D, 0xD482, + 0x87AE, 0xD452, + 0x87BF, 0xD423, + 0x87D1, 0xD3F4, + 0x87E2, 0xD3C5, + 0x87F3, 0xD396, + 0x8805, 0xD367, + 0x8816, 0xD337, + 0x8828, 0xD308, + 0x883A, 0xD2D9, + 0x884B, 0xD2AA, + 0x885D, 0xD27B, + 0x886F, 0xD24C, + 0x8881, 0xD21D, + 0x8893, 0xD1EE, + 0x88A5, 0xD1C0, + 0x88B8, 0xD191, + 0x88CA, 0xD162, + 0x88DC, 0xD133, + 0x88EF, 0xD104, + 0x8901, 0xD0D6, + 0x8914, 0xD0A7, + 0x8926, 0xD078, + 0x8939, 0xD04A, + 0x894C, 0xD01B, + 0x895F, 0xCFEC, + 0x8971, 0xCFBE, + 0x8984, 0xCF8F, + 0x8997, 0xCF61, + 0x89AB, 0xCF32, + 0x89BE, 0xCF04, + 0x89D1, 0xCED5, + 0x89E4, 0xCEA7, + 0x89F8, 0xCE79, + 0x8A0B, 0xCE4A, + 0x8A1F, 0xCE1C, + 0x8A33, 0xCDEE, + 0x8A46, 0xCDBF, + 0x8A5A, 0xCD91, + 0x8A6E, 0xCD63, + 0x8A82, 0xCD35, + 0x8A96, 0xCD07, + 0x8AAA, 0xCCD9, + 0x8ABE, 0xCCAB, + 0x8AD2, 0xCC7D, + 0x8AE6, 0xCC4F, + 0x8AFB, 0xCC21, + 0x8B0F, 0xCBF3, + 0x8B24, 0xCBC5, + 0x8B38, 0xCB97, + 0x8B4D, 0xCB69, + 0x8B61, 0xCB3B, + 0x8B76, 0xCB0D, + 0x8B8B, 0xCAE0, + 0x8BA0, 0xCAB2, + 0x8BB5, 0xCA84, + 0x8BCA, 0xCA57, + 0x8BDF, 0xCA29, + 0x8BF4, 0xC9FB, + 0x8C09, 0xC9CE, + 0x8C1F, 0xC9A0, + 0x8C34, 0xC973, + 0x8C4A, 0xC945, + 0x8C5F, 0xC918, + 0x8C75, 0xC8EB, + 0x8C8A, 0xC8BD, + 0x8CA0, 0xC890, + 0x8CB6, 0xC863, + 0x8CCC, 0xC835, + 0x8CE2, 0xC808, + 0x8CF8, 0xC7DB, + 0x8D0E, 0xC7AE, + 0x8D24, 0xC781, + 0x8D3A, 0xC754, + 0x8D50, 0xC727, + 0x8D67, 0xC6F9, + 0x8D7D, 0xC6CD, + 0x8D94, 0xC6A0, + 0x8DAA, 0xC673, + 0x8DC1, 0xC646, + 0x8DD8, 0xC619, + 0x8DEE, 0xC5EC, + 0x8E05, 0xC5BF, + 0x8E1C, 0xC593, + 0x8E33, 0xC566, + 0x8E4A, 0xC539, + 0x8E61, 0xC50D, + 0x8E79, 0xC4E0, + 0x8E90, 0xC4B3, + 0x8EA7, 0xC487, + 0x8EBE, 0xC45A, + 0x8ED6, 0xC42E, + 0x8EED, 0xC402, + 0x8F05, 0xC3D5, + 0x8F1D, 0xC3A9, + 0x8F34, 0xC37C, + 0x8F4C, 0xC350, + 0x8F64, 0xC324, + 0x8F7C, 0xC2F8, + 0x8F94, 0xC2CC, + 0x8FAC, 0xC29F, + 0x8FC4, 0xC273, + 0x8FDC, 0xC247, + 0x8FF5, 0xC21B, + 0x900D, 0xC1EF, + 0x9025, 0xC1C3, + 0x903E, 0xC197, + 0x9056, 0xC16C, + 0x906F, 0xC140, + 0x9088, 0xC114, + 0x90A0, 0xC0E8, + 0x90B9, 0xC0BC, + 0x90D2, 0xC091, + 0x90EB, 0xC065, + 0x9104, 0xC03A, + 0x911D, 0xC00E, + 0x9136, 0xBFE2, + 0x9150, 0xBFB7, + 0x9169, 0xBF8C, + 0x9182, 0xBF60, + 0x919C, 0xBF35, + 0x91B5, 0xBF09, + 0x91CF, 0xBEDE, + 0x91E8, 0xBEB3, + 0x9202, 0xBE88, + 0x921C, 0xBE5D, + 0x9235, 0xBE31, + 0x924F, 0xBE06, + 0x9269, 0xBDDB, + 0x9283, 0xBDB0, + 0x929D, 0xBD85, + 0x92B7, 0xBD5A, + 0x92D2, 0xBD2F, + 0x92EC, 0xBD05, + 0x9306, 0xBCDA, + 0x9321, 0xBCAF, + 0x933B, 0xBC84, + 0x9356, 0xBC5A, + 0x9370, 0xBC2F, + 0x938B, 0xBC04, + 0x93A6, 0xBBDA, + 0x93C0, 0xBBAF, + 0x93DB, 0xBB85, + 0x93F6, 0xBB5A, + 0x9411, 0xBB30, + 0x942C, 0xBB05, + 0x9447, 0xBADB, + 0x9463, 0xBAB1, + 0x947E, 0xBA87, + 0x9499, 0xBA5C, + 0x94B5, 0xBA32, + 0x94D0, 0xBA08, + 0x94EC, 0xB9DE, + 0x9507, 0xB9B4, + 0x9523, 0xB98A, + 0x953E, 0xB960, + 0x955A, 0xB936, + 0x9576, 0xB90C, + 0x9592, 0xB8E3, + 0x95AE, 0xB8B9, + 0x95CA, 0xB88F, + 0x95E6, 0xB865, + 0x9602, 0xB83C, + 0x961E, 0xB812, + 0x963B, 0xB7E9, + 0x9657, 0xB7BF, + 0x9673, 0xB796, + 0x9690, 0xB76C, + 0x96AC, 0xB743, + 0x96C9, 0xB719, + 0x96E6, 0xB6F0, + 0x9702, 0xB6C7, + 0x971F, 0xB69E, + 0x973C, 0xB675, + 0x9759, 0xB64B, + 0x9776, 0xB622, + 0x9793, 0xB5F9, + 0x97B0, 0xB5D0, + 0x97CD, 0xB5A7, + 0x97EA, 0xB57E, + 0x9808, 0xB556, + 0x9825, 0xB52D, + 0x9842, 0xB504, + 0x9860, 0xB4DB, + 0x987D, 0xB4B3, + 0x989B, 0xB48A, + 0x98B9, 0xB461, + 0x98D6, 0xB439, + 0x98F4, 0xB410, + 0x9912, 0xB3E8, + 0x9930, 0xB3C0, + 0x994E, 0xB397, + 0x996C, 0xB36F, + 0x998A, 0xB347, + 0x99A8, 0xB31E, + 0x99C6, 0xB2F6, + 0x99E5, 0xB2CE, + 0x9A03, 0xB2A6, + 0x9A22, 0xB27E, + 0x9A40, 0xB256, + 0x9A5F, 0xB22E, + 0x9A7D, 0xB206, + 0x9A9C, 0xB1DE, + 0x9ABA, 0xB1B7, + 0x9AD9, 0xB18F, + 0x9AF8, 0xB167, + 0x9B17, 0xB140, + 0x9B36, 0xB118, + 0x9B55, 0xB0F0, + 0x9B74, 0xB0C9, + 0x9B93, 0xB0A1, + 0x9BB2, 0xB07A, + 0x9BD2, 0xB053, + 0x9BF1, 0xB02B, + 0x9C10, 0xB004, + 0x9C30, 0xAFDD, + 0x9C4F, 0xAFB6, + 0x9C6F, 0xAF8F, + 0x9C8E, 0xAF68, + 0x9CAE, 0xAF40, + 0x9CCE, 0xAF1A, + 0x9CEE, 0xAEF3, + 0x9D0D, 0xAECC, + 0x9D2D, 0xAEA5, + 0x9D4D, 0xAE7E, + 0x9D6D, 0xAE57, + 0x9D8E, 0xAE31, + 0x9DAE, 0xAE0A, + 0x9DCE, 0xADE3, + 0x9DEE, 0xADBD, + 0x9E0E, 0xAD96, + 0x9E2F, 0xAD70, + 0x9E4F, 0xAD4A, + 0x9E70, 0xAD23, + 0x9E90, 0xACFD, + 0x9EB1, 0xACD7, + 0x9ED2, 0xACB1, + 0x9EF2, 0xAC8A, + 0x9F13, 0xAC64, + 0x9F34, 0xAC3E, + 0x9F55, 0xAC18, + 0x9F76, 0xABF2, + 0x9F97, 0xABCC, + 0x9FB8, 0xABA7, + 0x9FD9, 0xAB81, + 0x9FFB, 0xAB5B, + 0xA01C, 0xAB35, + 0xA03D, 0xAB10, + 0xA05F, 0xAAEA, + 0xA080, 0xAAC5, + 0xA0A1, 0xAA9F, + 0xA0C3, 0xAA7A, + 0xA0E5, 0xAA54, + 0xA106, 0xAA2F, + 0xA128, 0xAA0A, + 0xA14A, 0xA9E5, + 0xA16C, 0xA9BF, + 0xA18E, 0xA99A, + 0xA1AF, 0xA975, + 0xA1D2, 0xA950, + 0xA1F4, 0xA92B, + 0xA216, 0xA906, + 0xA238, 0xA8E2, + 0xA25A, 0xA8BD, + 0xA27C, 0xA898, + 0xA29F, 0xA873, + 0xA2C1, 0xA84F, + 0xA2E4, 0xA82A, + 0xA306, 0xA806, + 0xA329, 0xA7E1, + 0xA34B, 0xA7BD, + 0xA36E, 0xA798, + 0xA391, 0xA774, + 0xA3B4, 0xA750, + 0xA3D6, 0xA72B, + 0xA3F9, 0xA707, + 0xA41C, 0xA6E3, + 0xA43F, 0xA6BF, + 0xA462, 0xA69B, + 0xA486, 0xA677, + 0xA4A9, 0xA653, + 0xA4CC, 0xA62F, + 0xA4EF, 0xA60C, + 0xA513, 0xA5E8, + 0xA536, 0xA5C4, + 0xA55A, 0xA5A1, + 0xA57D, 0xA57D, + 0xA5A1, 0xA55A, + 0xA5C4, 0xA536, + 0xA5E8, 0xA513, + 0xA60C, 0xA4EF, + 0xA62F, 0xA4CC, + 0xA653, 0xA4A9, + 0xA677, 0xA486, + 0xA69B, 0xA462, + 0xA6BF, 0xA43F, + 0xA6E3, 0xA41C, + 0xA707, 0xA3F9, + 0xA72B, 0xA3D6, + 0xA750, 0xA3B4, + 0xA774, 0xA391, + 0xA798, 0xA36E, + 0xA7BD, 0xA34B, + 0xA7E1, 0xA329, + 0xA806, 0xA306, + 0xA82A, 0xA2E4, + 0xA84F, 0xA2C1, + 0xA873, 0xA29F, + 0xA898, 0xA27C, + 0xA8BD, 0xA25A, + 0xA8E2, 0xA238, + 0xA906, 0xA216, + 0xA92B, 0xA1F4, + 0xA950, 0xA1D2, + 0xA975, 0xA1AF, + 0xA99A, 0xA18E, + 0xA9BF, 0xA16C, + 0xA9E5, 0xA14A, + 0xAA0A, 0xA128, + 0xAA2F, 0xA106, + 0xAA54, 0xA0E5, + 0xAA7A, 0xA0C3, + 0xAA9F, 0xA0A1, + 0xAAC5, 0xA080, + 0xAAEA, 0xA05F, + 0xAB10, 0xA03D, + 0xAB35, 0xA01C, + 0xAB5B, 0x9FFB, + 0xAB81, 0x9FD9, + 0xABA7, 0x9FB8, + 0xABCC, 0x9F97, + 0xABF2, 0x9F76, + 0xAC18, 0x9F55, + 0xAC3E, 0x9F34, + 0xAC64, 0x9F13, + 0xAC8A, 0x9EF2, + 0xACB1, 0x9ED2, + 0xACD7, 0x9EB1, + 0xACFD, 0x9E90, + 0xAD23, 0x9E70, + 0xAD4A, 0x9E4F, + 0xAD70, 0x9E2F, + 0xAD96, 0x9E0E, + 0xADBD, 0x9DEE, + 0xADE3, 0x9DCE, + 0xAE0A, 0x9DAE, + 0xAE31, 0x9D8E, + 0xAE57, 0x9D6D, + 0xAE7E, 0x9D4D, + 0xAEA5, 0x9D2D, + 0xAECC, 0x9D0D, + 0xAEF3, 0x9CEE, + 0xAF1A, 0x9CCE, + 0xAF40, 0x9CAE, + 0xAF68, 0x9C8E, + 0xAF8F, 0x9C6F, + 0xAFB6, 0x9C4F, + 0xAFDD, 0x9C30, + 0xB004, 0x9C10, + 0xB02B, 0x9BF1, + 0xB053, 0x9BD2, + 0xB07A, 0x9BB2, + 0xB0A1, 0x9B93, + 0xB0C9, 0x9B74, + 0xB0F0, 0x9B55, + 0xB118, 0x9B36, + 0xB140, 0x9B17, + 0xB167, 0x9AF8, + 0xB18F, 0x9AD9, + 0xB1B7, 0x9ABA, + 0xB1DE, 0x9A9C, + 0xB206, 0x9A7D, + 0xB22E, 0x9A5F, + 0xB256, 0x9A40, + 0xB27E, 0x9A22, + 0xB2A6, 0x9A03, + 0xB2CE, 0x99E5, + 0xB2F6, 0x99C6, + 0xB31E, 0x99A8, + 0xB347, 0x998A, + 0xB36F, 0x996C, + 0xB397, 0x994E, + 0xB3C0, 0x9930, + 0xB3E8, 0x9912, + 0xB410, 0x98F4, + 0xB439, 0x98D6, + 0xB461, 0x98B9, + 0xB48A, 0x989B, + 0xB4B3, 0x987D, + 0xB4DB, 0x9860, + 0xB504, 0x9842, + 0xB52D, 0x9825, + 0xB556, 0x9808, + 0xB57E, 0x97EA, + 0xB5A7, 0x97CD, + 0xB5D0, 0x97B0, + 0xB5F9, 0x9793, + 0xB622, 0x9776, + 0xB64B, 0x9759, + 0xB675, 0x973C, + 0xB69E, 0x971F, + 0xB6C7, 0x9702, + 0xB6F0, 0x96E6, + 0xB719, 0x96C9, + 0xB743, 0x96AC, + 0xB76C, 0x9690, + 0xB796, 0x9673, + 0xB7BF, 0x9657, + 0xB7E9, 0x963B, + 0xB812, 0x961E, + 0xB83C, 0x9602, + 0xB865, 0x95E6, + 0xB88F, 0x95CA, + 0xB8B9, 0x95AE, + 0xB8E3, 0x9592, + 0xB90C, 0x9576, + 0xB936, 0x955A, + 0xB960, 0x953E, + 0xB98A, 0x9523, + 0xB9B4, 0x9507, + 0xB9DE, 0x94EC, + 0xBA08, 0x94D0, + 0xBA32, 0x94B5, + 0xBA5C, 0x9499, + 0xBA87, 0x947E, + 0xBAB1, 0x9463, + 0xBADB, 0x9447, + 0xBB05, 0x942C, + 0xBB30, 0x9411, + 0xBB5A, 0x93F6, + 0xBB85, 0x93DB, + 0xBBAF, 0x93C0, + 0xBBDA, 0x93A6, + 0xBC04, 0x938B, + 0xBC2F, 0x9370, + 0xBC5A, 0x9356, + 0xBC84, 0x933B, + 0xBCAF, 0x9321, + 0xBCDA, 0x9306, + 0xBD05, 0x92EC, + 0xBD2F, 0x92D2, + 0xBD5A, 0x92B7, + 0xBD85, 0x929D, + 0xBDB0, 0x9283, + 0xBDDB, 0x9269, + 0xBE06, 0x924F, + 0xBE31, 0x9235, + 0xBE5D, 0x921C, + 0xBE88, 0x9202, + 0xBEB3, 0x91E8, + 0xBEDE, 0x91CF, + 0xBF09, 0x91B5, + 0xBF35, 0x919C, + 0xBF60, 0x9182, + 0xBF8C, 0x9169, + 0xBFB7, 0x9150, + 0xBFE2, 0x9136, + 0xC00E, 0x911D, + 0xC03A, 0x9104, + 0xC065, 0x90EB, + 0xC091, 0x90D2, + 0xC0BC, 0x90B9, + 0xC0E8, 0x90A0, + 0xC114, 0x9088, + 0xC140, 0x906F, + 0xC16C, 0x9056, + 0xC197, 0x903E, + 0xC1C3, 0x9025, + 0xC1EF, 0x900D, + 0xC21B, 0x8FF5, + 0xC247, 0x8FDC, + 0xC273, 0x8FC4, + 0xC29F, 0x8FAC, + 0xC2CC, 0x8F94, + 0xC2F8, 0x8F7C, + 0xC324, 0x8F64, + 0xC350, 0x8F4C, + 0xC37C, 0x8F34, + 0xC3A9, 0x8F1D, + 0xC3D5, 0x8F05, + 0xC402, 0x8EED, + 0xC42E, 0x8ED6, + 0xC45A, 0x8EBE, + 0xC487, 0x8EA7, + 0xC4B3, 0x8E90, + 0xC4E0, 0x8E79, + 0xC50D, 0x8E61, + 0xC539, 0x8E4A, + 0xC566, 0x8E33, + 0xC593, 0x8E1C, + 0xC5BF, 0x8E05, + 0xC5EC, 0x8DEE, + 0xC619, 0x8DD8, + 0xC646, 0x8DC1, + 0xC673, 0x8DAA, + 0xC6A0, 0x8D94, + 0xC6CD, 0x8D7D, + 0xC6F9, 0x8D67, + 0xC727, 0x8D50, + 0xC754, 0x8D3A, + 0xC781, 0x8D24, + 0xC7AE, 0x8D0E, + 0xC7DB, 0x8CF8, + 0xC808, 0x8CE2, + 0xC835, 0x8CCC, + 0xC863, 0x8CB6, + 0xC890, 0x8CA0, + 0xC8BD, 0x8C8A, + 0xC8EB, 0x8C75, + 0xC918, 0x8C5F, + 0xC945, 0x8C4A, + 0xC973, 0x8C34, + 0xC9A0, 0x8C1F, + 0xC9CE, 0x8C09, + 0xC9FB, 0x8BF4, + 0xCA29, 0x8BDF, + 0xCA57, 0x8BCA, + 0xCA84, 0x8BB5, + 0xCAB2, 0x8BA0, + 0xCAE0, 0x8B8B, + 0xCB0D, 0x8B76, + 0xCB3B, 0x8B61, + 0xCB69, 0x8B4D, + 0xCB97, 0x8B38, + 0xCBC5, 0x8B24, + 0xCBF3, 0x8B0F, + 0xCC21, 0x8AFB, + 0xCC4F, 0x8AE6, + 0xCC7D, 0x8AD2, + 0xCCAB, 0x8ABE, + 0xCCD9, 0x8AAA, + 0xCD07, 0x8A96, + 0xCD35, 0x8A82, + 0xCD63, 0x8A6E, + 0xCD91, 0x8A5A, + 0xCDBF, 0x8A46, + 0xCDEE, 0x8A33, + 0xCE1C, 0x8A1F, + 0xCE4A, 0x8A0B, + 0xCE79, 0x89F8, + 0xCEA7, 0x89E4, + 0xCED5, 0x89D1, + 0xCF04, 0x89BE, + 0xCF32, 0x89AB, + 0xCF61, 0x8997, + 0xCF8F, 0x8984, + 0xCFBE, 0x8971, + 0xCFEC, 0x895F, + 0xD01B, 0x894C, + 0xD04A, 0x8939, + 0xD078, 0x8926, + 0xD0A7, 0x8914, + 0xD0D6, 0x8901, + 0xD104, 0x88EF, + 0xD133, 0x88DC, + 0xD162, 0x88CA, + 0xD191, 0x88B8, + 0xD1C0, 0x88A5, + 0xD1EE, 0x8893, + 0xD21D, 0x8881, + 0xD24C, 0x886F, + 0xD27B, 0x885D, + 0xD2AA, 0x884B, + 0xD2D9, 0x883A, + 0xD308, 0x8828, + 0xD337, 0x8816, + 0xD367, 0x8805, + 0xD396, 0x87F3, + 0xD3C5, 0x87E2, + 0xD3F4, 0x87D1, + 0xD423, 0x87BF, + 0xD452, 0x87AE, + 0xD482, 0x879D, + 0xD4B1, 0x878C, + 0xD4E0, 0x877B, + 0xD510, 0x876A, + 0xD53F, 0x8759, + 0xD56E, 0x8749, + 0xD59E, 0x8738, + 0xD5CD, 0x8727, + 0xD5FD, 0x8717, + 0xD62C, 0x8706, + 0xD65C, 0x86F6, + 0xD68B, 0x86E6, + 0xD6BB, 0x86D5, + 0xD6EA, 0x86C5, + 0xD71A, 0x86B5, + 0xD74A, 0x86A5, + 0xD779, 0x8695, + 0xD7A9, 0x8685, + 0xD7D9, 0x8675, + 0xD809, 0x8666, + 0xD838, 0x8656, + 0xD868, 0x8646, + 0xD898, 0x8637, + 0xD8C8, 0x8627, + 0xD8F8, 0x8618, + 0xD927, 0x8609, + 0xD957, 0x85FA, + 0xD987, 0x85EA, + 0xD9B7, 0x85DB, + 0xD9E7, 0x85CC, + 0xDA17, 0x85BD, + 0xDA47, 0x85AF, + 0xDA77, 0x85A0, + 0xDAA7, 0x8591, + 0xDAD7, 0x8582, + 0xDB08, 0x8574, + 0xDB38, 0x8565, + 0xDB68, 0x8557, + 0xDB98, 0x8549, + 0xDBC8, 0x853A, + 0xDBF8, 0x852C, + 0xDC29, 0x851E, + 0xDC59, 0x8510, + 0xDC89, 0x8502, + 0xDCBA, 0x84F4, + 0xDCEA, 0x84E6, + 0xDD1A, 0x84D9, + 0xDD4B, 0x84CB, + 0xDD7B, 0x84BD, + 0xDDAB, 0x84B0, + 0xDDDC, 0x84A2, + 0xDE0C, 0x8495, + 0xDE3D, 0x8488, + 0xDE6D, 0x847B, + 0xDE9E, 0x846D, + 0xDECE, 0x8460, + 0xDEFF, 0x8453, + 0xDF2F, 0x8446, + 0xDF60, 0x843A, + 0xDF91, 0x842D, + 0xDFC1, 0x8420, + 0xDFF2, 0x8414, + 0xE023, 0x8407, + 0xE053, 0x83FA, + 0xE084, 0x83EE, + 0xE0B5, 0x83E2, + 0xE0E6, 0x83D6, + 0xE116, 0x83C9, + 0xE147, 0x83BD, + 0xE178, 0x83B1, + 0xE1A9, 0x83A5, + 0xE1DA, 0x8399, + 0xE20A, 0x838E, + 0xE23B, 0x8382, + 0xE26C, 0x8376, + 0xE29D, 0x836B, + 0xE2CE, 0x835F, + 0xE2FF, 0x8354, + 0xE330, 0x8348, + 0xE361, 0x833D, + 0xE392, 0x8332, + 0xE3C3, 0x8327, + 0xE3F4, 0x831C, + 0xE425, 0x8311, + 0xE456, 0x8306, + 0xE487, 0x82FB, + 0xE4B8, 0x82F0, + 0xE4E9, 0x82E6, + 0xE51B, 0x82DB, + 0xE54C, 0x82D0, + 0xE57D, 0x82C6, + 0xE5AE, 0x82BC, + 0xE5DF, 0x82B1, + 0xE610, 0x82A7, + 0xE642, 0x829D, + 0xE673, 0x8293, + 0xE6A4, 0x8289, + 0xE6D5, 0x827F, + 0xE707, 0x8275, + 0xE738, 0x826B, + 0xE769, 0x8262, + 0xE79B, 0x8258, + 0xE7CC, 0x824F, + 0xE7FD, 0x8245, + 0xE82F, 0x823C, + 0xE860, 0x8232, + 0xE892, 0x8229, + 0xE8C3, 0x8220, + 0xE8F5, 0x8217, + 0xE926, 0x820E, + 0xE957, 0x8205, + 0xE989, 0x81FC, + 0xE9BA, 0x81F3, + 0xE9EC, 0x81EB, + 0xEA1D, 0x81E2, + 0xEA4F, 0x81D9, + 0xEA80, 0x81D1, + 0xEAB2, 0x81C8, + 0xEAE4, 0x81C0, + 0xEB15, 0x81B8, + 0xEB47, 0x81B0, + 0xEB78, 0x81A8, + 0xEBAA, 0x81A0, + 0xEBDC, 0x8198, + 0xEC0D, 0x8190, + 0xEC3F, 0x8188, + 0xEC71, 0x8180, + 0xECA2, 0x8179, + 0xECD4, 0x8171, + 0xED06, 0x816A, + 0xED37, 0x8162, + 0xED69, 0x815B, + 0xED9B, 0x8154, + 0xEDCD, 0x814C, + 0xEDFE, 0x8145, + 0xEE30, 0x813E, + 0xEE62, 0x8137, + 0xEE94, 0x8130, + 0xEEC6, 0x812A, + 0xEEF7, 0x8123, + 0xEF29, 0x811C, + 0xEF5B, 0x8116, + 0xEF8D, 0x810F, + 0xEFBF, 0x8109, + 0xEFF1, 0x8102, + 0xF022, 0x80FC, + 0xF054, 0x80F6, + 0xF086, 0x80F0, + 0xF0B8, 0x80EA, + 0xF0EA, 0x80E4, + 0xF11C, 0x80DE, + 0xF14E, 0x80D8, + 0xF180, 0x80D2, + 0xF1B2, 0x80CD, + 0xF1E4, 0x80C7, + 0xF216, 0x80C2, + 0xF248, 0x80BC, + 0xF27A, 0x80B7, + 0xF2AC, 0x80B2, + 0xF2DE, 0x80AC, + 0xF310, 0x80A7, + 0xF342, 0x80A2, + 0xF374, 0x809D, + 0xF3A6, 0x8098, + 0xF3D8, 0x8094, + 0xF40A, 0x808F, + 0xF43C, 0x808A, + 0xF46E, 0x8086, + 0xF4A0, 0x8081, + 0xF4D2, 0x807D, + 0xF504, 0x8078, + 0xF536, 0x8074, + 0xF568, 0x8070, + 0xF59A, 0x806C, + 0xF5CC, 0x8068, + 0xF5FF, 0x8064, + 0xF631, 0x8060, + 0xF663, 0x805C, + 0xF695, 0x8058, + 0xF6C7, 0x8055, + 0xF6F9, 0x8051, + 0xF72B, 0x804E, + 0xF75D, 0x804A, + 0xF790, 0x8047, + 0xF7C2, 0x8043, + 0xF7F4, 0x8040, + 0xF826, 0x803D, + 0xF858, 0x803A, + 0xF88A, 0x8037, + 0xF8BD, 0x8034, + 0xF8EF, 0x8031, + 0xF921, 0x802F, + 0xF953, 0x802C, + 0xF985, 0x8029, + 0xF9B8, 0x8027, + 0xF9EA, 0x8025, + 0xFA1C, 0x8022, + 0xFA4E, 0x8020, + 0xFA80, 0x801E, + 0xFAB3, 0x801C, + 0xFAE5, 0x801A, + 0xFB17, 0x8018, + 0xFB49, 0x8016, + 0xFB7C, 0x8014, + 0xFBAE, 0x8012, + 0xFBE0, 0x8011, + 0xFC12, 0x800F, + 0xFC45, 0x800D, + 0xFC77, 0x800C, + 0xFCA9, 0x800B, + 0xFCDB, 0x8009, + 0xFD0E, 0x8008, + 0xFD40, 0x8007, + 0xFD72, 0x8006, + 0xFDA4, 0x8005, + 0xFDD7, 0x8004, + 0xFE09, 0x8003, + 0xFE3B, 0x8003, + 0xFE6D, 0x8002, + 0xFEA0, 0x8001, + 0xFED2, 0x8001, + 0xFF04, 0x8000, + 0xFF36, 0x8000, + 0xFF69, 0x8000, + 0xFF9B, 0x8000, + 0xFFCD, 0x8000 +}; + + +/** +* @} end of CFFT_CIFFT group +*/ + +/* +* @brief Q15 table for reciprocal +*/ +const q15_t ALIGN4 armRecipTableQ15[64] = { + 0x7F03, 0x7D13, 0x7B31, 0x795E, 0x7798, 0x75E0, + 0x7434, 0x7294, 0x70FF, 0x6F76, 0x6DF6, 0x6C82, + 0x6B16, 0x69B5, 0x685C, 0x670C, 0x65C4, 0x6484, + 0x634C, 0x621C, 0x60F3, 0x5FD0, 0x5EB5, 0x5DA0, + 0x5C91, 0x5B88, 0x5A85, 0x5988, 0x5890, 0x579E, + 0x56B0, 0x55C8, 0x54E4, 0x5405, 0x532B, 0x5255, + 0x5183, 0x50B6, 0x4FEC, 0x4F26, 0x4E64, 0x4DA6, + 0x4CEC, 0x4C34, 0x4B81, 0x4AD0, 0x4A23, 0x4978, + 0x48D1, 0x482D, 0x478C, 0x46ED, 0x4651, 0x45B8, + 0x4521, 0x448D, 0x43FC, 0x436C, 0x42DF, 0x4255, + 0x41CC, 0x4146, 0x40C2, 0x4040 +}; + +/* +* @brief Q31 table for reciprocal +*/ +const q31_t armRecipTableQ31[64] = { + 0x7F03F03F, 0x7D137420, 0x7B31E739, 0x795E9F94, 0x7798FD29, 0x75E06928, + 0x7434554D, 0x72943B4B, 0x70FF9C40, 0x6F760031, 0x6DF6F593, 0x6C8210E3, + 0x6B16EC3A, 0x69B526F6, 0x685C655F, 0x670C505D, 0x65C4952D, 0x6484E519, + 0x634CF53E, 0x621C7E4F, 0x60F33C61, 0x5FD0EEB3, 0x5EB55785, 0x5DA03BEB, + 0x5C9163A1, 0x5B8898E6, 0x5A85A85A, 0x598860DF, 0x58909373, 0x579E1318, + 0x56B0B4B8, 0x55C84F0B, 0x54E4BA80, 0x5405D124, 0x532B6E8F, 0x52556FD0, + 0x5183B35A, 0x50B618F3, 0x4FEC81A2, 0x4F26CFA2, 0x4E64E64E, 0x4DA6AA1D, + 0x4CEC008B, 0x4C34D010, 0x4B810016, 0x4AD078EF, 0x4A2323C4, 0x4978EA96, + 0x48D1B827, 0x482D77FE, 0x478C1657, 0x46ED801D, 0x4651A2E5, 0x45B86CE2, + 0x4521CCE1, 0x448DB244, 0x43FC0CFA, 0x436CCD78, 0x42DFE4B4, 0x42554426, + 0x41CCDDB6, 0x4146A3C6, 0x40C28923, 0x40408102 +}; + +const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH] = +{ + //8x2, size 20 + 8,64, 24,72, 16,64, 40,80, 32,64, 56,88, 48,72, 88,104, 72,96, 104,112 +}; + +const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH] = +{ + //8x4, size 48 + 8,64, 16,128, 24,192, 32,64, 40,72, 48,136, 56,200, 64,128, 72,80, 88,208, + 80,144, 96,192, 104,208, 112,152, 120,216, 136,192, 144,160, 168,208, + 152,224, 176,208, 184,232, 216,240, 200,224, 232,240 +}; + +const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH] = +{ + //radix 8, size 56 + 8,64, 16,128, 24,192, 32,256, 40,320, 48,384, 56,448, 80,136, 88,200, + 96,264, 104,328, 112,392, 120,456, 152,208, 160,272, 168,336, 176,400, + 184,464, 224,280, 232,344, 240,408, 248,472, 296,352, 304,416, 312,480, + 368,424, 376,488, 440,496 +}; + +const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH] = +{ + //8x2, size 208 + 8,512, 16,64, 24,576, 32,128, 40,640, 48,192, 56,704, 64,256, 72,768, + 80,320, 88,832, 96,384, 104,896, 112,448, 120,960, 128,512, 136,520, + 144,768, 152,584, 160,520, 168,648, 176,200, 184,712, 192,264, 200,776, + 208,328, 216,840, 224,392, 232,904, 240,456, 248,968, 264,528, 272,320, + 280,592, 288,768, 296,656, 304,328, 312,720, 328,784, 344,848, 352,400, + 360,912, 368,464, 376,976, 384,576, 392,536, 400,832, 408,600, 416,584, + 424,664, 432,840, 440,728, 448,592, 456,792, 464,848, 472,856, 480,600, + 488,920, 496,856, 504,984, 520,544, 528,576, 536,608, 552,672, 560,608, + 568,736, 576,768, 584,800, 592,832, 600,864, 608,800, 616,928, 624,864, + 632,992, 648,672, 656,896, 664,928, 688,904, 696,744, 704,896, 712,808, + 720,912, 728,872, 736,928, 744,936, 752,920, 760,1000, 776,800, 784,832, + 792,864, 808,904, 816,864, 824,920, 840,864, 856,880, 872,944, 888,1008, + 904,928, 912,960, 920,992, 944,968, 952,1000, 968,992, 984,1008 +}; + +const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH] = +{ + //8x4, size 440 + 8,512, 16,1024, 24,1536, 32,64, 40,576, 48,1088, 56,1600, 64,128, 72,640, + 80,1152, 88,1664, 96,192, 104,704, 112,1216, 120,1728, 128,256, 136,768, + 144,1280, 152,1792, 160,320, 168,832, 176,1344, 184,1856, 192,384, + 200,896, 208,1408, 216,1920, 224,448, 232,960, 240,1472, 248,1984, + 256,512, 264,520, 272,1032, 280,1544, 288,640, 296,584, 304,1096, 312,1608, + 320,768, 328,648, 336,1160, 344,1672, 352,896, 360,712, 368,1224, 376,1736, + 384,520, 392,776, 400,1288, 408,1800, 416,648, 424,840, 432,1352, 440,1864, + 448,776, 456,904, 464,1416, 472,1928, 480,904, 488,968, 496,1480, 504,1992, + 520,528, 512,1024, 528,1040, 536,1552, 544,1152, 552,592, 560,1104, + 568,1616, 576,1280, 584,656, 592,1168, 600,1680, 608,1408, 616,720, + 624,1232, 632,1744, 640,1032, 648,784, 656,1296, 664,1808, 672,1160, + 680,848, 688,1360, 696,1872, 704,1288, 712,912, 720,1424, 728,1936, + 736,1416, 744,976, 752,1488, 760,2000, 768,1536, 776,1552, 784,1048, + 792,1560, 800,1664, 808,1680, 816,1112, 824,1624, 832,1792, 840,1808, + 848,1176, 856,1688, 864,1920, 872,1936, 880,1240, 888,1752, 896,1544, + 904,1560, 912,1304, 920,1816, 928,1672, 936,1688, 944,1368, 952,1880, + 960,1800, 968,1816, 976,1432, 984,1944, 992,1928, 1000,1944, 1008,1496, + 1016,2008, 1032,1152, 1040,1056, 1048,1568, 1064,1408, 1072,1120, + 1080,1632, 1088,1536, 1096,1160, 1104,1184, 1112,1696, 1120,1552, + 1128,1416, 1136,1248, 1144,1760, 1160,1664, 1168,1312, 1176,1824, + 1184,1544, 1192,1920, 1200,1376, 1208,1888, 1216,1568, 1224,1672, + 1232,1440, 1240,1952, 1248,1560, 1256,1928, 1264,1504, 1272,2016, + 1288,1312, 1296,1408, 1304,1576, 1320,1424, 1328,1416, 1336,1640, + 1344,1792, 1352,1824, 1360,1920, 1368,1704, 1376,1800, 1384,1432, + 1392,1928, 1400,1768, 1416,1680, 1432,1832, 1440,1576, 1448,1936, + 1456,1832, 1464,1896, 1472,1808, 1480,1688, 1488,1936, 1496,1960, + 1504,1816, 1512,1944, 1520,1944, 1528,2024, 1560,1584, 1592,1648, + 1600,1792, 1608,1920, 1616,1800, 1624,1712, 1632,1808, 1640,1936, + 1648,1816, 1656,1776, 1672,1696, 1688,1840, 1704,1952, 1712,1928, + 1720,1904, 1728,1824, 1736,1952, 1744,1832, 1752,1968, 1760,1840, + 1768,1960, 1776,1944, 1784,2032, 1864,1872, 1848,1944, 1872,1888, + 1880,1904, 1888,1984, 1896,2000, 1912,2032, 1904,2016, 1976,2032, + 1960,1968, 2008,2032, 1992,2016, 2024,2032 +}; + +const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH] = +{ + //radix 8, size 448 + 8,512, 16,1024, 24,1536, 32,2048, 40,2560, 48,3072, 56,3584, 72,576, + 80,1088, 88,1600, 96,2112, 104,2624, 112,3136, 120,3648, 136,640, 144,1152, + 152,1664, 160,2176, 168,2688, 176,3200, 184,3712, 200,704, 208,1216, + 216,1728, 224,2240, 232,2752, 240,3264, 248,3776, 264,768, 272,1280, + 280,1792, 288,2304, 296,2816, 304,3328, 312,3840, 328,832, 336,1344, + 344,1856, 352,2368, 360,2880, 368,3392, 376,3904, 392,896, 400,1408, + 408,1920, 416,2432, 424,2944, 432,3456, 440,3968, 456,960, 464,1472, + 472,1984, 480,2496, 488,3008, 496,3520, 504,4032, 528,1032, 536,1544, + 544,2056, 552,2568, 560,3080, 568,3592, 592,1096, 600,1608, 608,2120, + 616,2632, 624,3144, 632,3656, 656,1160, 664,1672, 672,2184, 680,2696, + 688,3208, 696,3720, 720,1224, 728,1736, 736,2248, 744,2760, 752,3272, + 760,3784, 784,1288, 792,1800, 800,2312, 808,2824, 816,3336, 824,3848, + 848,1352, 856,1864, 864,2376, 872,2888, 880,3400, 888,3912, 912,1416, + 920,1928, 928,2440, 936,2952, 944,3464, 952,3976, 976,1480, 984,1992, + 992,2504, 1000,3016, 1008,3528, 1016,4040, 1048,1552, 1056,2064, 1064,2576, + 1072,3088, 1080,3600, 1112,1616, 1120,2128, 1128,2640, 1136,3152, + 1144,3664, 1176,1680, 1184,2192, 1192,2704, 1200,3216, 1208,3728, + 1240,1744, 1248,2256, 1256,2768, 1264,3280, 1272,3792, 1304,1808, + 1312,2320, 1320,2832, 1328,3344, 1336,3856, 1368,1872, 1376,2384, + 1384,2896, 1392,3408, 1400,3920, 1432,1936, 1440,2448, 1448,2960, + 1456,3472, 1464,3984, 1496,2000, 1504,2512, 1512,3024, 1520,3536, + 1528,4048, 1568,2072, 1576,2584, 1584,3096, 1592,3608, 1632,2136, + 1640,2648, 1648,3160, 1656,3672, 1696,2200, 1704,2712, 1712,3224, + 1720,3736, 1760,2264, 1768,2776, 1776,3288, 1784,3800, 1824,2328, + 1832,2840, 1840,3352, 1848,3864, 1888,2392, 1896,2904, 1904,3416, + 1912,3928, 1952,2456, 1960,2968, 1968,3480, 1976,3992, 2016,2520, + 2024,3032, 2032,3544, 2040,4056, 2088,2592, 2096,3104, 2104,3616, + 2152,2656, 2160,3168, 2168,3680, 2216,2720, 2224,3232, 2232,3744, + 2280,2784, 2288,3296, 2296,3808, 2344,2848, 2352,3360, 2360,3872, + 2408,2912, 2416,3424, 2424,3936, 2472,2976, 2480,3488, 2488,4000, + 2536,3040, 2544,3552, 2552,4064, 2608,3112, 2616,3624, 2672,3176, + 2680,3688, 2736,3240, 2744,3752, 2800,3304, 2808,3816, 2864,3368, + 2872,3880, 2928,3432, 2936,3944, 2992,3496, 3000,4008, 3056,3560, + 3064,4072, 3128,3632, 3192,3696, 3256,3760, 3320,3824, 3384,3888, + 3448,3952, 3512,4016, 3576,4080 +}; + +const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH] = +{ + //8x2, size 1800 + 8,4096, 16,512, 24,4608, 32,1024, 40,5120, 48,1536, 56,5632, 64,2048, + 72,6144, 80,2560, 88,6656, 96,3072, 104,7168, 112,3584, 120,7680, 128,2048, + 136,4160, 144,576, 152,4672, 160,1088, 168,5184, 176,1600, 184,5696, + 192,2112, 200,6208, 208,2624, 216,6720, 224,3136, 232,7232, 240,3648, + 248,7744, 256,2048, 264,4224, 272,640, 280,4736, 288,1152, 296,5248, + 304,1664, 312,5760, 320,2176, 328,6272, 336,2688, 344,6784, 352,3200, + 360,7296, 368,3712, 376,7808, 384,2112, 392,4288, 400,704, 408,4800, + 416,1216, 424,5312, 432,1728, 440,5824, 448,2240, 456,6336, 464,2752, + 472,6848, 480,3264, 488,7360, 496,3776, 504,7872, 512,2048, 520,4352, + 528,768, 536,4864, 544,1280, 552,5376, 560,1792, 568,5888, 576,2304, + 584,6400, 592,2816, 600,6912, 608,3328, 616,7424, 624,3840, 632,7936, + 640,2176, 648,4416, 656,832, 664,4928, 672,1344, 680,5440, 688,1856, + 696,5952, 704,2368, 712,6464, 720,2880, 728,6976, 736,3392, 744,7488, + 752,3904, 760,8000, 768,2112, 776,4480, 784,896, 792,4992, 800,1408, + 808,5504, 816,1920, 824,6016, 832,2432, 840,6528, 848,2944, 856,7040, + 864,3456, 872,7552, 880,3968, 888,8064, 896,2240, 904,4544, 912,960, + 920,5056, 928,1472, 936,5568, 944,1984, 952,6080, 960,2496, 968,6592, + 976,3008, 984,7104, 992,3520, 1000,7616, 1008,4032, 1016,8128, 1024,4096, + 1032,4104, 1040,4352, 1048,4616, 1056,4104, 1064,5128, 1072,1544, + 1080,5640, 1088,2056, 1096,6152, 1104,2568, 1112,6664, 1120,3080, + 1128,7176, 1136,3592, 1144,7688, 1152,6144, 1160,4168, 1168,6400, + 1176,4680, 1184,6152, 1192,5192, 1200,1608, 1208,5704, 1216,2120, + 1224,6216, 1232,2632, 1240,6728, 1248,3144, 1256,7240, 1264,3656, + 1272,7752, 1280,4160, 1288,4232, 1296,4416, 1304,4744, 1312,4168, + 1320,5256, 1328,1672, 1336,5768, 1344,2184, 1352,6280, 1360,2696, + 1368,6792, 1376,3208, 1384,7304, 1392,3720, 1400,7816, 1408,6208, + 1416,4296, 1424,6464, 1432,4808, 1440,6216, 1448,5320, 1456,1736, + 1464,5832, 1472,2248, 1480,6344, 1488,2760, 1496,6856, 1504,3272, + 1512,7368, 1520,3784, 1528,7880, 1536,4224, 1544,4360, 1552,4480, + 1560,4872, 1568,4232, 1576,5384, 1584,1800, 1592,5896, 1600,2312, + 1608,6408, 1616,2824, 1624,6920, 1632,3336, 1640,7432, 1648,3848, + 1656,7944, 1664,6272, 1672,4424, 1680,6528, 1688,4936, 1696,6280, + 1704,5448, 1712,1864, 1720,5960, 1728,2376, 1736,6472, 1744,2888, + 1752,6984, 1760,3400, 1768,7496, 1776,3912, 1784,8008, 1792,4288, + 1800,4488, 1808,4544, 1816,5000, 1824,4296, 1832,5512, 1840,1928, + 1848,6024, 1856,2440, 1864,6536, 1872,2952, 1880,7048, 1888,3464, + 1896,7560, 1904,3976, 1912,8072, 1920,6336, 1928,4552, 1936,6592, + 1944,5064, 1952,6344, 1960,5576, 1968,1992, 1976,6088, 1984,2504, + 1992,6600, 2000,3016, 2008,7112, 2016,3528, 2024,7624, 2032,4040, + 2040,8136, 2056,4112, 2064,2112, 2072,4624, 2080,4352, 2088,5136, + 2096,4480, 2104,5648, 2120,6160, 2128,2576, 2136,6672, 2144,3088, + 2152,7184, 2160,3600, 2168,7696, 2176,2560, 2184,4176, 2192,2816, + 2200,4688, 2208,2568, 2216,5200, 2224,2824, 2232,5712, 2240,2576, + 2248,6224, 2256,2640, 2264,6736, 2272,3152, 2280,7248, 2288,3664, + 2296,7760, 2312,4240, 2320,2432, 2328,4752, 2336,6400, 2344,5264, + 2352,6528, 2360,5776, 2368,2816, 2376,6288, 2384,2704, 2392,6800, + 2400,3216, 2408,7312, 2416,3728, 2424,7824, 2432,2624, 2440,4304, + 2448,2880, 2456,4816, 2464,2632, 2472,5328, 2480,2888, 2488,5840, + 2496,2640, 2504,6352, 2512,2768, 2520,6864, 2528,3280, 2536,7376, + 2544,3792, 2552,7888, 2568,4368, 2584,4880, 2592,4416, 2600,5392, + 2608,4544, 2616,5904, 2632,6416, 2640,2832, 2648,6928, 2656,3344, + 2664,7440, 2672,3856, 2680,7952, 2696,4432, 2704,2944, 2712,4944, + 2720,4432, 2728,5456, 2736,2952, 2744,5968, 2752,2944, 2760,6480, + 2768,2896, 2776,6992, 2784,3408, 2792,7504, 2800,3920, 2808,8016, + 2824,4496, 2840,5008, 2848,6464, 2856,5520, 2864,6592, 2872,6032, + 2888,6544, 2896,2960, 2904,7056, 2912,3472, 2920,7568, 2928,3984, + 2936,8080, 2952,4560, 2960,3008, 2968,5072, 2976,6480, 2984,5584, + 2992,3016, 3000,6096, 3016,6608, 3032,7120, 3040,3536, 3048,7632, + 3056,4048, 3064,8144, 3072,4608, 3080,4120, 3088,4864, 3096,4632, + 3104,4616, 3112,5144, 3120,4872, 3128,5656, 3136,4624, 3144,6168, + 3152,4880, 3160,6680, 3168,4632, 3176,7192, 3184,3608, 3192,7704, + 3200,6656, 3208,4184, 3216,6912, 3224,4696, 3232,6664, 3240,5208, + 3248,6920, 3256,5720, 3264,6672, 3272,6232, 3280,6928, 3288,6744, + 3296,6680, 3304,7256, 3312,3672, 3320,7768, 3328,4672, 3336,4248, + 3344,4928, 3352,4760, 3360,4680, 3368,5272, 3376,4936, 3384,5784, + 3392,4688, 3400,6296, 3408,4944, 3416,6808, 3424,4696, 3432,7320, + 3440,3736, 3448,7832, 3456,6720, 3464,4312, 3472,6976, 3480,4824, + 3488,6728, 3496,5336, 3504,6984, 3512,5848, 3520,6736, 3528,6360, + 3536,6992, 3544,6872, 3552,6744, 3560,7384, 3568,3800, 3576,7896, + 3584,4736, 3592,4376, 3600,4992, 3608,4888, 3616,4744, 3624,5400, + 3632,5000, 3640,5912, 3648,4752, 3656,6424, 3664,5008, 3672,6936, + 3680,4760, 3688,7448, 3696,3864, 3704,7960, 3712,6784, 3720,4440, + 3728,7040, 3736,4952, 3744,6792, 3752,5464, 3760,7048, 3768,5976, + 3776,6800, 3784,6488, 3792,7056, 3800,7000, 3808,6808, 3816,7512, + 3824,3928, 3832,8024, 3840,4800, 3848,4504, 3856,5056, 3864,5016, + 3872,4808, 3880,5528, 3888,5064, 3896,6040, 3904,4816, 3912,6552, + 3920,5072, 3928,7064, 3936,4824, 3944,7576, 3952,3992, 3960,8088, + 3968,6848, 3976,4568, 3984,7104, 3992,5080, 4000,6856, 4008,5592, + 4016,7112, 4024,6104, 4032,6864, 4040,6616, 4048,7120, 4056,7128, + 4064,6872, 4072,7640, 4080,7128, 4088,8152, 4104,4128, 4112,4160, + 4120,4640, 4136,5152, 4144,4232, 4152,5664, 4160,4352, 4168,6176, + 4176,4416, 4184,6688, 4192,4616, 4200,7200, 4208,4744, 4216,7712, + 4224,4608, 4232,4616, 4240,4672, 4248,4704, 4256,4640, 4264,5216, + 4272,4704, 4280,5728, 4288,4864, 4296,6240, 4304,4928, 4312,6752, + 4320,4632, 4328,7264, 4336,4760, 4344,7776, 4360,4640, 4368,4416, + 4376,4768, 4384,6152, 4392,5280, 4400,6280, 4408,5792, 4424,6304, + 4440,6816, 4448,6664, 4456,7328, 4464,6792, 4472,7840, 4480,4624, + 4488,4632, 4496,4688, 4504,4832, 4512,6168, 4520,5344, 4528,6296, + 4536,5856, 4544,4880, 4552,6368, 4560,4944, 4568,6880, 4576,6680, + 4584,7392, 4592,6808, 4600,7904, 4608,6144, 4616,6152, 4624,6208, + 4632,4896, 4640,6176, 4648,5408, 4656,6240, 4664,5920, 4672,6400, + 4680,6432, 4688,6464, 4696,6944, 4704,6432, 4712,7456, 4720,4808, + 4728,7968, 4736,6656, 4744,6664, 4752,6720, 4760,4960, 4768,6688, + 4776,5472, 4784,6752, 4792,5984, 4800,6912, 4808,6496, 4816,6976, + 4824,7008, 4832,6944, 4840,7520, 4848,7008, 4856,8032, 4864,6160, + 4872,6168, 4880,6224, 4888,5024, 4896,6216, 4904,5536, 4912,6344, + 4920,6048, 4928,6416, 4936,6560, 4944,6480, 4952,7072, 4960,6728, + 4968,7584, 4976,6856, 4984,8096, 4992,6672, 5000,6680, 5008,6736, + 5016,5088, 5024,6232, 5032,5600, 5040,6360, 5048,6112, 5056,6928, + 5064,6624, 5072,6992, 5080,7136, 5088,6744, 5096,7648, 5104,6872, + 5112,8160, 5128,5152, 5136,5376, 5144,5408, 5168,5384, 5176,5672, + 5184,5376, 5192,6184, 5200,5392, 5208,6696, 5216,5408, 5224,7208, + 5232,5400, 5240,7720, 5248,7168, 5256,7200, 5264,7424, 5272,7456, + 5280,7176, 5288,7208, 5296,7432, 5304,5736, 5312,7184, 5320,6248, + 5328,7440, 5336,6760, 5344,7192, 5352,7272, 5360,7448, 5368,7784, + 5384,5408, 5392,5440, 5400,5472, 5408,6184, 5416,7208, 5424,5448, + 5432,5800, 5448,6312, 5464,6824, 5472,6696, 5480,7336, 5488,6824, + 5496,7848, 5504,7232, 5512,7264, 5520,7488, 5528,7520, 5536,7240, + 5544,7272, 5552,7496, 5560,5864, 5568,7248, 5576,6376, 5584,7504, + 5592,6888, 5600,7256, 5608,7400, 5616,7512, 5624,7912, 5632,7168, + 5640,7176, 5648,7232, 5656,7240, 5664,7200, 5672,7208, 5680,7264, + 5688,5928, 5696,7424, 5704,6440, 5712,7488, 5720,6952, 5728,7456, + 5736,7464, 5744,7520, 5752,7976, 5760,7296, 5768,7328, 5776,7552, + 5784,7584, 5792,7304, 5800,7336, 5808,7560, 5816,5992, 5824,7312, + 5832,6504, 5840,7568, 5848,7016, 5856,7320, 5864,7528, 5872,7576, + 5880,8040, 5888,7184, 5896,7192, 5904,7248, 5912,7256, 5920,6248, + 5928,7272, 5936,6376, 5944,6056, 5952,7440, 5960,6568, 5968,7504, + 5976,7080, 5984,6760, 5992,7592, 6000,6888, 6008,8104, 6016,7360, + 6024,7392, 6032,7616, 6040,7648, 6048,7368, 6056,7400, 6064,7624, + 6072,6120, 6080,7376, 6088,6632, 6096,7632, 6104,7144, 6112,7384, + 6120,7656, 6128,7640, 6136,8168, 6168,6240, 6192,6216, 6200,7264, + 6232,6704, 6248,7216, 6256,6680, 6264,7728, 6272,6656, 6280,6664, + 6288,6912, 6296,6496, 6304,6688, 6312,6696, 6320,6944, 6328,7520, + 6336,6672, 6344,6680, 6352,6928, 6360,6768, 6368,6704, 6376,7280, + 6384,6744, 6392,7792, 6408,6432, 6424,6752, 6440,7432, 6448,6536, + 6456,7560, 6472,6944, 6488,6832, 6496,6920, 6504,7344, 6512,7048, + 6520,7856, 6528,6720, 6536,6728, 6544,6976, 6552,7008, 6560,6752, + 6568,7448, 6576,7008, 6584,7576, 6592,6736, 6600,6744, 6608,6992, + 6616,6896, 6624,6936, 6632,7408, 6640,7064, 6648,7920, 6712,7280, + 6744,6960, 6760,7472, 6768,6936, 6776,7984, 6800,6848, 6808,6856, + 6832,6880, 6840,6888, 6848,7040, 6856,7048, 6864,7104, 6872,7024, + 6880,7072, 6888,7536, 6896,7136, 6904,8048, 6952,7496, 6968,7624, + 6984,7008, 7000,7088, 7016,7600, 7024,7112, 7032,8112, 7056,7104, + 7064,7112, 7080,7512, 7088,7136, 7096,7640, 7128,7152, 7144,7664, + 7160,8176, 7176,7200, 7192,7216, 7224,7272, 7240,7264, 7256,7280, + 7288,7736, 7296,7680, 7304,7712, 7312,7936, 7320,7968, 7328,7688, + 7336,7720, 7344,7944, 7352,7976, 7360,7696, 7368,7728, 7376,7952, + 7384,7984, 7392,7704, 7400,7736, 7408,7960, 7416,7800, 7432,7456, + 7448,7472, 7480,7592, 7496,7520, 7512,7536, 7528,7976, 7544,7864, + 7552,7744, 7560,7776, 7568,8000, 7576,8032, 7584,7752, 7592,7784, + 7600,8008, 7608,8040, 7616,7760, 7624,7792, 7632,8016, 7640,8048, + 7648,7768, 7656,7800, 7664,8024, 7672,7928, 7688,7712, 7704,7728, + 7752,7776, 7768,7792, 7800,7992, 7816,7840, 7824,8064, 7832,8096, + 7856,8072, 7864,8104, 7872,8064, 7880,8072, 7888,8080, 7896,8112, + 7904,8096, 7912,8104, 7920,8088, 7928,8056, 7944,7968, 7960,7984, + 8008,8032, 8024,8048, 8056,8120, 8072,8096, 8080,8128, 8088,8160, + 8112,8136, 8120,8168, 8136,8160, 8152,8176 +}; + +const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH] = +{ + //8x2, size 3808 + 8,4096, 16,8192, 24,12288, 32,512, 40,4608, 48,8704, 56,12800, 64,1024, + 72,5120, 80,9216, 88,13312, 96,1536, 104,5632, 112,9728, 120,13824, + 128,2048, 136,6144, 144,10240, 152,14336, 160,2560, 168,6656, 176,10752, + 184,14848, 192,3072, 200,7168, 208,11264, 216,15360, 224,3584, 232,7680, + 240,11776, 248,15872, 256,1024, 264,4160, 272,8256, 280,12352, 288,576, + 296,4672, 304,8768, 312,12864, 320,1088, 328,5184, 336,9280, 344,13376, + 352,1600, 360,5696, 368,9792, 376,13888, 384,2112, 392,6208, 400,10304, + 408,14400, 416,2624, 424,6720, 432,10816, 440,14912, 448,3136, 456,7232, + 464,11328, 472,15424, 480,3648, 488,7744, 496,11840, 504,15936, 512,2048, + 520,4224, 528,8320, 536,12416, 544,640, 552,4736, 560,8832, 568,12928, + 576,1152, 584,5248, 592,9344, 600,13440, 608,1664, 616,5760, 624,9856, + 632,13952, 640,2176, 648,6272, 656,10368, 664,14464, 672,2688, 680,6784, + 688,10880, 696,14976, 704,3200, 712,7296, 720,11392, 728,15488, 736,3712, + 744,7808, 752,11904, 760,16000, 768,3072, 776,4288, 784,8384, 792,12480, + 800,3200, 808,4800, 816,8896, 824,12992, 832,1216, 840,5312, 848,9408, + 856,13504, 864,1728, 872,5824, 880,9920, 888,14016, 896,2240, 904,6336, + 912,10432, 920,14528, 928,2752, 936,6848, 944,10944, 952,15040, 960,3264, + 968,7360, 976,11456, 984,15552, 992,3776, 1000,7872, 1008,11968, 1016,16064, + 1032,4352, 1040,8448, 1048,12544, 1056,3072, 1064,4864, 1072,8960, + 1080,13056, 1088,1280, 1096,5376, 1104,9472, 1112,13568, 1120,1792, + 1128,5888, 1136,9984, 1144,14080, 1152,2304, 1160,6400, 1168,10496, + 1176,14592, 1184,2816, 1192,6912, 1200,11008, 1208,15104, 1216,3328, + 1224,7424, 1232,11520, 1240,15616, 1248,3840, 1256,7936, 1264,12032, + 1272,16128, 1288,4416, 1296,8512, 1304,12608, 1312,3328, 1320,4928, + 1328,9024, 1336,13120, 1352,5440, 1360,9536, 1368,13632, 1376,1856, + 1384,5952, 1392,10048, 1400,14144, 1408,2368, 1416,6464, 1424,10560, + 1432,14656, 1440,2880, 1448,6976, 1456,11072, 1464,15168, 1472,3392, + 1480,7488, 1488,11584, 1496,15680, 1504,3904, 1512,8000, 1520,12096, + 1528,16192, 1536,2112, 1544,4480, 1552,8576, 1560,12672, 1568,2240, + 1576,4992, 1584,9088, 1592,13184, 1600,2368, 1608,5504, 1616,9600, + 1624,13696, 1632,1920, 1640,6016, 1648,10112, 1656,14208, 1664,2432, + 1672,6528, 1680,10624, 1688,14720, 1696,2944, 1704,7040, 1712,11136, + 1720,15232, 1728,3456, 1736,7552, 1744,11648, 1752,15744, 1760,3968, + 1768,8064, 1776,12160, 1784,16256, 1792,3136, 1800,4544, 1808,8640, + 1816,12736, 1824,3264, 1832,5056, 1840,9152, 1848,13248, 1856,3392, + 1864,5568, 1872,9664, 1880,13760, 1888,1984, 1896,6080, 1904,10176, + 1912,14272, 1920,2496, 1928,6592, 1936,10688, 1944,14784, 1952,3008, + 1960,7104, 1968,11200, 1976,15296, 1984,3520, 1992,7616, 2000,11712, + 2008,15808, 2016,4032, 2024,8128, 2032,12224, 2040,16320, 2048,4096, + 2056,4104, 2064,8200, 2072,12296, 2080,4224, 2088,4616, 2096,8712, + 2104,12808, 2112,4352, 2120,5128, 2128,9224, 2136,13320, 2144,4480, + 2152,5640, 2160,9736, 2168,13832, 2176,4104, 2184,6152, 2192,10248, + 2200,14344, 2208,2568, 2216,6664, 2224,10760, 2232,14856, 2240,3080, + 2248,7176, 2256,11272, 2264,15368, 2272,3592, 2280,7688, 2288,11784, + 2296,15880, 2304,5120, 2312,4168, 2320,8264, 2328,12360, 2336,5248, + 2344,4680, 2352,8776, 2360,12872, 2368,5376, 2376,5192, 2384,9288, + 2392,13384, 2400,5504, 2408,5704, 2416,9800, 2424,13896, 2432,5128, + 2440,6216, 2448,10312, 2456,14408, 2464,2632, 2472,6728, 2480,10824, + 2488,14920, 2496,3144, 2504,7240, 2512,11336, 2520,15432, 2528,3656, + 2536,7752, 2544,11848, 2552,15944, 2560,6144, 2568,4232, 2576,8328, + 2584,12424, 2592,6272, 2600,4744, 2608,8840, 2616,12936, 2624,6400, + 2632,5256, 2640,9352, 2648,13448, 2656,6528, 2664,5768, 2672,9864, + 2680,13960, 2688,6152, 2696,6280, 2704,10376, 2712,14472, 2720,6280, + 2728,6792, 2736,10888, 2744,14984, 2752,3208, 2760,7304, 2768,11400, + 2776,15496, 2784,3720, 2792,7816, 2800,11912, 2808,16008, 2816,7168, + 2824,4296, 2832,8392, 2840,12488, 2848,7296, 2856,4808, 2864,8904, + 2872,13000, 2880,7424, 2888,5320, 2896,9416, 2904,13512, 2912,7552, + 2920,5832, 2928,9928, 2936,14024, 2944,7176, 2952,6344, 2960,10440, + 2968,14536, 2976,7304, 2984,6856, 2992,10952, 3000,15048, 3008,3272, + 3016,7368, 3024,11464, 3032,15560, 3040,3784, 3048,7880, 3056,11976, + 3064,16072, 3072,4160, 3080,4360, 3088,8456, 3096,12552, 3104,4288, + 3112,4872, 3120,8968, 3128,13064, 3136,4416, 3144,5384, 3152,9480, + 3160,13576, 3168,4544, 3176,5896, 3184,9992, 3192,14088, 3200,4168, + 3208,6408, 3216,10504, 3224,14600, 3232,4296, 3240,6920, 3248,11016, + 3256,15112, 3264,3336, 3272,7432, 3280,11528, 3288,15624, 3296,3848, + 3304,7944, 3312,12040, 3320,16136, 3328,5184, 3336,4424, 3344,8520, + 3352,12616, 3360,5312, 3368,4936, 3376,9032, 3384,13128, 3392,5440, + 3400,5448, 3408,9544, 3416,13640, 3424,5568, 3432,5960, 3440,10056, + 3448,14152, 3456,5192, 3464,6472, 3472,10568, 3480,14664, 3488,5320, + 3496,6984, 3504,11080, 3512,15176, 3520,5448, 3528,7496, 3536,11592, + 3544,15688, 3552,3912, 3560,8008, 3568,12104, 3576,16200, 3584,6208, + 3592,4488, 3600,8584, 3608,12680, 3616,6336, 3624,5000, 3632,9096, + 3640,13192, 3648,6464, 3656,5512, 3664,9608, 3672,13704, 3680,6592, + 3688,6024, 3696,10120, 3704,14216, 3712,6216, 3720,6536, 3728,10632, + 3736,14728, 3744,6344, 3752,7048, 3760,11144, 3768,15240, 3776,6472, + 3784,7560, 3792,11656, 3800,15752, 3808,3976, 3816,8072, 3824,12168, + 3832,16264, 3840,7232, 3848,4552, 3856,8648, 3864,12744, 3872,7360, + 3880,5064, 3888,9160, 3896,13256, 3904,7488, 3912,5576, 3920,9672, + 3928,13768, 3936,7616, 3944,6088, 3952,10184, 3960,14280, 3968,7240, + 3976,6600, 3984,10696, 3992,14792, 4000,7368, 4008,7112, 4016,11208, + 4024,15304, 4032,7496, 4040,7624, 4048,11720, 4056,15816, 4064,7624, + 4072,8136, 4080,12232, 4088,16328, 4096,8192, 4104,4112, 4112,8208, + 4120,12304, 4128,8320, 4136,4624, 4144,8720, 4152,12816, 4160,8448, + 4168,5136, 4176,9232, 4184,13328, 4192,8576, 4200,5648, 4208,9744, + 4216,13840, 4224,8200, 4232,6160, 4240,10256, 4248,14352, 4256,8328, + 4264,6672, 4272,10768, 4280,14864, 4288,8456, 4296,7184, 4304,11280, + 4312,15376, 4320,8584, 4328,7696, 4336,11792, 4344,15888, 4352,9216, + 4360,9232, 4368,8272, 4376,12368, 4384,9344, 4392,4688, 4400,8784, + 4408,12880, 4416,9472, 4424,5200, 4432,9296, 4440,13392, 4448,9600, + 4456,5712, 4464,9808, 4472,13904, 4480,9224, 4488,6224, 4496,10320, + 4504,14416, 4512,9352, 4520,6736, 4528,10832, 4536,14928, 4544,9480, + 4552,7248, 4560,11344, 4568,15440, 4576,9608, 4584,7760, 4592,11856, + 4600,15952, 4608,10240, 4616,10256, 4624,8336, 4632,12432, 4640,10368, + 4648,4752, 4656,8848, 4664,12944, 4672,10496, 4680,5264, 4688,9360, + 4696,13456, 4704,10624, 4712,5776, 4720,9872, 4728,13968, 4736,10248, + 4744,6288, 4752,10384, 4760,14480, 4768,10376, 4776,6800, 4784,10896, + 4792,14992, 4800,10504, 4808,7312, 4816,11408, 4824,15504, 4832,10632, + 4840,7824, 4848,11920, 4856,16016, 4864,11264, 4872,11280, 4880,8400, + 4888,12496, 4896,11392, 4904,11408, 4912,8912, 4920,13008, 4928,11520, + 4936,5328, 4944,9424, 4952,13520, 4960,11648, 4968,5840, 4976,9936, + 4984,14032, 4992,11272, 5000,6352, 5008,10448, 5016,14544, 5024,11400, + 5032,6864, 5040,10960, 5048,15056, 5056,11528, 5064,7376, 5072,11472, + 5080,15568, 5088,11656, 5096,7888, 5104,11984, 5112,16080, 5120,8256, + 5128,8272, 5136,8464, 5144,12560, 5152,8384, 5160,8400, 5168,8976, + 5176,13072, 5184,8512, 5192,5392, 5200,9488, 5208,13584, 5216,8640, + 5224,5904, 5232,10000, 5240,14096, 5248,8264, 5256,6416, 5264,10512, + 5272,14608, 5280,8392, 5288,6928, 5296,11024, 5304,15120, 5312,8520, + 5320,7440, 5328,11536, 5336,15632, 5344,8648, 5352,7952, 5360,12048, + 5368,16144, 5376,9280, 5384,9296, 5392,8528, 5400,12624, 5408,9408, + 5416,9424, 5424,9040, 5432,13136, 5440,9536, 5448,5456, 5456,9552, + 5464,13648, 5472,9664, 5480,5968, 5488,10064, 5496,14160, 5504,9288, + 5512,6480, 5520,10576, 5528,14672, 5536,9416, 5544,6992, 5552,11088, + 5560,15184, 5568,9544, 5576,7504, 5584,11600, 5592,15696, 5600,9672, + 5608,8016, 5616,12112, 5624,16208, 5632,10304, 5640,10320, 5648,8592, + 5656,12688, 5664,10432, 5672,10448, 5680,9104, 5688,13200, 5696,10560, + 5704,10576, 5712,9616, 5720,13712, 5728,10688, 5736,6032, 5744,10128, + 5752,14224, 5760,10312, 5768,6544, 5776,10640, 5784,14736, 5792,10440, + 5800,7056, 5808,11152, 5816,15248, 5824,10568, 5832,7568, 5840,11664, + 5848,15760, 5856,10696, 5864,8080, 5872,12176, 5880,16272, 5888,11328, + 5896,11344, 5904,8656, 5912,12752, 5920,11456, 5928,11472, 5936,9168, + 5944,13264, 5952,11584, 5960,11600, 5968,9680, 5976,13776, 5984,11712, + 5992,6096, 6000,10192, 6008,14288, 6016,11336, 6024,6608, 6032,10704, + 6040,14800, 6048,11464, 6056,7120, 6064,11216, 6072,15312, 6080,11592, + 6088,7632, 6096,11728, 6104,15824, 6112,11720, 6120,8144, 6128,12240, + 6136,16336, 6144,12288, 6152,12304, 6160,8216, 6168,12312, 6176,12416, + 6184,12432, 6192,8728, 6200,12824, 6208,12544, 6216,12560, 6224,9240, + 6232,13336, 6240,12672, 6248,12688, 6256,9752, 6264,13848, 6272,12296, + 6280,12312, 6288,10264, 6296,14360, 6304,12424, 6312,6680, 6320,10776, + 6328,14872, 6336,12552, 6344,7192, 6352,11288, 6360,15384, 6368,12680, + 6376,7704, 6384,11800, 6392,15896, 6400,13312, 6408,13328, 6416,8280, + 6424,12376, 6432,13440, 6440,13456, 6448,8792, 6456,12888, 6464,13568, + 6472,13584, 6480,9304, 6488,13400, 6496,13696, 6504,13712, 6512,9816, + 6520,13912, 6528,13320, 6536,13336, 6544,10328, 6552,14424, 6560,13448, + 6568,6744, 6576,10840, 6584,14936, 6592,13576, 6600,7256, 6608,11352, + 6616,15448, 6624,13704, 6632,7768, 6640,11864, 6648,15960, 6656,14336, + 6664,14352, 6672,8344, 6680,12440, 6688,14464, 6696,14480, 6704,8856, + 6712,12952, 6720,14592, 6728,14608, 6736,9368, 6744,13464, 6752,14720, + 6760,14736, 6768,9880, 6776,13976, 6784,14344, 6792,14360, 6800,10392, + 6808,14488, 6816,14472, 6824,14488, 6832,10904, 6840,15000, 6848,14600, + 6856,7320, 6864,11416, 6872,15512, 6880,14728, 6888,7832, 6896,11928, + 6904,16024, 6912,15360, 6920,15376, 6928,8408, 6936,12504, 6944,15488, + 6952,15504, 6960,8920, 6968,13016, 6976,15616, 6984,15632, 6992,9432, + 7000,13528, 7008,15744, 7016,15760, 7024,9944, 7032,14040, 7040,15368, + 7048,15384, 7056,10456, 7064,14552, 7072,15496, 7080,15512, 7088,10968, + 7096,15064, 7104,15624, 7112,7384, 7120,11480, 7128,15576, 7136,15752, + 7144,7896, 7152,11992, 7160,16088, 7168,12352, 7176,12368, 7184,8472, + 7192,12568, 7200,12480, 7208,12496, 7216,8984, 7224,13080, 7232,12608, + 7240,12624, 7248,9496, 7256,13592, 7264,12736, 7272,12752, 7280,10008, + 7288,14104, 7296,12360, 7304,12376, 7312,10520, 7320,14616, 7328,12488, + 7336,12504, 7344,11032, 7352,15128, 7360,12616, 7368,7448, 7376,11544, + 7384,15640, 7392,12744, 7400,7960, 7408,12056, 7416,16152, 7424,13376, + 7432,13392, 7440,8536, 7448,12632, 7456,13504, 7464,13520, 7472,9048, + 7480,13144, 7488,13632, 7496,13648, 7504,9560, 7512,13656, 7520,13760, + 7528,13776, 7536,10072, 7544,14168, 7552,13384, 7560,13400, 7568,10584, + 7576,14680, 7584,13512, 7592,13528, 7600,11096, 7608,15192, 7616,13640, + 7624,13656, 7632,11608, 7640,15704, 7648,13768, 7656,8024, 7664,12120, + 7672,16216, 7680,14400, 7688,14416, 7696,8600, 7704,12696, 7712,14528, + 7720,14544, 7728,9112, 7736,13208, 7744,14656, 7752,14672, 7760,9624, + 7768,13720, 7776,14784, 7784,14800, 7792,10136, 7800,14232, 7808,14408, + 7816,14424, 7824,10648, 7832,14744, 7840,14536, 7848,14552, 7856,11160, + 7864,15256, 7872,14664, 7880,14680, 7888,11672, 7896,15768, 7904,14792, + 7912,8088, 7920,12184, 7928,16280, 7936,15424, 7944,15440, 7952,8664, + 7960,12760, 7968,15552, 7976,15568, 7984,9176, 7992,13272, 8000,15680, + 8008,15696, 8016,9688, 8024,13784, 8032,15808, 8040,15824, 8048,10200, + 8056,14296, 8064,15432, 8072,15448, 8080,10712, 8088,14808, 8096,15560, + 8104,15576, 8112,11224, 8120,15320, 8128,15688, 8136,15704, 8144,11736, + 8152,15832, 8160,15816, 8168,15832, 8176,12248, 8184,16344, 8200,8320, + 8208,8224, 8216,12320, 8232,10368, 8240,8736, 8248,12832, 8256,8448, + 8264,8384, 8272,9248, 8280,13344, 8288,9232, 8296,10432, 8304,9760, + 8312,13856, 8328,12416, 8336,10272, 8344,14368, 8352,12296, 8360,14464, + 8368,10784, 8376,14880, 8384,8456, 8392,12480, 8400,11296, 8408,15392, + 8416,12552, 8424,14528, 8432,11808, 8440,15904, 8448,9216, 8456,8576, + 8464,9232, 8472,12384, 8480,9248, 8488,10624, 8496,8800, 8504,12896, + 8512,9472, 8520,8640, 8528,9312, 8536,13408, 8544,9296, 8552,10688, + 8560,9824, 8568,13920, 8576,9224, 8584,12672, 8592,10336, 8600,14432, + 8608,13320, 8616,14720, 8624,10848, 8632,14944, 8640,9480, 8648,12736, + 8656,11360, 8664,15456, 8672,13576, 8680,14784, 8688,11872, 8696,15968, + 8704,12288, 8712,12416, 8720,12296, 8728,12448, 8736,12304, 8744,10376, + 8752,8864, 8760,12960, 8768,12352, 8776,12480, 8784,9376, 8792,13472, + 8800,12368, 8808,10440, 8816,9888, 8824,13984, 8832,12320, 8840,12424, + 8848,10400, 8856,14496, 8864,12312, 8872,14472, 8880,10912, 8888,15008, + 8896,12384, 8904,12488, 8912,11424, 8920,15520, 8928,12568, 8936,14536, + 8944,11936, 8952,16032, 8960,12544, 8968,12672, 8976,12552, 8984,12512, + 8992,12560, 9000,10632, 9008,12568, 9016,13024, 9024,12608, 9032,12736, + 9040,9440, 9048,13536, 9056,12624, 9064,10696, 9072,9952, 9080,14048, + 9088,9240, 9096,12680, 9104,10464, 9112,14560, 9120,13336, 9128,14728, + 9136,10976, 9144,15072, 9152,9496, 9160,12744, 9168,11488, 9176,15584, + 9184,13592, 9192,14792, 9200,12000, 9208,16096, 9224,9344, 9232,9248, + 9240,12576, 9256,11392, 9264,12560, 9272,13088, 9280,9472, 9288,9408, + 9296,9504, 9304,13600, 9312,9488, 9320,11456, 9328,10016, 9336,14112, + 9352,13440, 9360,10528, 9368,14624, 9376,12360, 9384,15488, 9392,11040, + 9400,15136, 9408,9480, 9416,13504, 9424,11552, 9432,15648, 9440,12616, + 9448,15552, 9456,12064, 9464,16160, 9480,9600, 9488,9504, 9496,12640, + 9512,11648, 9520,12624, 9528,13152, 9544,9664, 9552,9568, 9560,13664, + 9576,11712, 9584,10080, 9592,14176, 9608,13696, 9616,10592, 9624,14688, + 9632,13384, 9640,15744, 9648,11104, 9656,15200, 9672,13760, 9680,11616, + 9688,15712, 9696,13640, 9704,15808, 9712,12128, 9720,16224, 9728,13312, + 9736,13440, 9744,13320, 9752,12704, 9760,13328, 9768,11400, 9776,13336, + 9784,13216, 9792,13376, 9800,13504, 9808,13384, 9816,13728, 9824,13392, + 9832,11464, 9840,10144, 9848,14240, 9856,13344, 9864,13448, 9872,10656, + 9880,14752, 9888,12376, 9896,15496, 9904,11168, 9912,15264, 9920,13408, + 9928,13512, 9936,11680, 9944,15776, 9952,12632, 9960,15560, 9968,12192, + 9976,16288, 9984,13568, 9992,13696, 10000,13576, 10008,12768, 10016,13584, + 10024,11656, 10032,13592, 10040,13280, 10048,13632, 10056,13760, + 10064,13640, 10072,13792, 10080,13648, 10088,11720, 10096,10208, + 10104,14304, 10112,13600, 10120,13704, 10128,10720, 10136,14816, + 10144,13400, 10152,15752, 10160,11232, 10168,15328, 10176,13664, + 10184,13768, 10192,11744, 10200,15840, 10208,13656, 10216,15816, + 10224,12256, 10232,16352, 10248,10272, 10256,10368, 10264,12328, + 10280,10384, 10288,10376, 10296,12840, 10304,11264, 10312,11296, + 10320,11392, 10328,13352, 10336,11272, 10344,10448, 10352,11400, + 10360,13864, 10376,12432, 10392,14376, 10400,12328, 10408,14480, + 10416,10792, 10424,14888, 10432,11280, 10440,12496, 10448,11304, + 10456,15400, 10464,11288, 10472,14544, 10480,11816, 10488,15912, + 10496,11264, 10504,11272, 10512,11280, 10520,12392, 10528,11296, + 10536,10640, 10544,12496, 10552,12904, 10560,11328, 10568,11360, + 10576,11456, 10584,13416, 10592,11336, 10600,10704, 10608,11464, + 10616,13928, 10624,11392, 10632,12688, 10640,11304, 10648,14440, + 10656,13352, 10664,14736, 10672,10856, 10680,14952, 10688,11344, + 10696,12752, 10704,11368, 10712,15464, 10720,11352, 10728,14800, + 10736,11880, 10744,15976, 10752,14336, 10760,14368, 10768,14464, + 10776,12456, 10784,14344, 10792,14376, 10800,14472, 10808,12968, + 10816,15360, 10824,15392, 10832,15488, 10840,13480, 10848,15368, + 10856,15400, 10864,15496, 10872,13992, 10880,14352, 10888,12440, + 10896,14480, 10904,14504, 10912,14360, 10920,14488, 10928,14488, + 10936,15016, 10944,15376, 10952,12504, 10960,11432, 10968,15528, + 10976,15384, 10984,14552, 10992,11944, 11000,16040, 11008,14400, + 11016,14432, 11024,14528, 11032,12520, 11040,14408, 11048,14440, + 11056,14536, 11064,13032, 11072,15424, 11080,15456, 11088,15552, + 11096,13544, 11104,15432, 11112,15464, 11120,15560, 11128,14056, + 11136,14416, 11144,12696, 11152,14544, 11160,14568, 11168,14424, + 11176,14744, 11184,14552, 11192,15080, 11200,15440, 11208,12760, + 11216,11496, 11224,15592, 11232,15448, 11240,14808, 11248,12008, + 11256,16104, 11272,11296, 11280,11392, 11288,12584, 11304,11408, + 11312,12688, 11320,13096, 11328,11520, 11336,11552, 11344,11648, + 11352,13608, 11360,11528, 11368,11472, 11376,11656, 11384,14120, + 11400,13456, 11416,14632, 11424,12392, 11432,15504, 11440,14440, + 11448,15144, 11456,11536, 11464,13520, 11472,11560, 11480,15656, + 11488,11544, 11496,15568, 11504,12072, 11512,16168, 11528,11552, + 11536,11648, 11544,12648, 11560,11664, 11568,12752, 11576,13160, + 11592,11616, 11600,11712, 11608,13672, 11624,11728, 11632,11720, + 11640,14184, 11656,13712, 11672,14696, 11680,13416, 11688,15760, + 11696,15464, 11704,15208, 11720,13776, 11736,15720, 11744,13672, + 11752,15824, 11760,12136, 11768,16232, 11776,14592, 11784,14624, + 11792,14720, 11800,12712, 11808,14600, 11816,14632, 11824,14728, + 11832,13224, 11840,15616, 11848,15648, 11856,15744, 11864,13736, + 11872,15624, 11880,15656, 11888,15752, 11896,14248, 11904,14608, + 11912,13464, 11920,14736, 11928,14760, 11936,14616, 11944,15512, + 11952,14744, 11960,15272, 11968,15632, 11976,13528, 11984,15760, + 11992,15784, 12000,15640, 12008,15576, 12016,12200, 12024,16296, + 12032,14656, 12040,14688, 12048,14784, 12056,12776, 12064,14664, + 12072,14696, 12080,14792, 12088,13288, 12096,15680, 12104,15712, + 12112,15808, 12120,13800, 12128,15688, 12136,15720, 12144,15816, + 12152,14312, 12160,14672, 12168,13720, 12176,14800, 12184,14824, + 12192,14680, 12200,15768, 12208,14808, 12216,15336, 12224,15696, + 12232,13784, 12240,15824, 12248,15848, 12256,15704, 12264,15832, + 12272,15832, 12280,16360, 12312,12336, 12344,12848, 12352,12544, + 12360,12552, 12368,12560, 12376,13360, 12384,12576, 12392,12584, + 12400,13336, 12408,13872, 12424,12448, 12440,14384, 12456,14496, + 12464,14472, 12472,14896, 12480,12672, 12488,12512, 12496,12688, + 12504,15408, 12512,12680, 12520,14560, 12528,14728, 12536,15920, + 12544,13312, 12552,13320, 12560,13328, 12568,13336, 12576,13344, + 12584,13352, 12592,13360, 12600,12912, 12608,13568, 12616,13576, + 12624,13584, 12632,13424, 12640,13600, 12648,13608, 12656,13400, + 12664,13936, 12672,13440, 12680,12704, 12688,13456, 12696,14448, + 12704,13448, 12712,14752, 12720,15496, 12728,14960, 12736,13696, + 12744,12768, 12752,13712, 12760,15472, 12768,13704, 12776,14816, + 12784,15752, 12792,15984, 12800,14336, 12808,14464, 12816,14344, + 12824,14472, 12832,14352, 12840,14480, 12848,14360, 12856,12976, + 12864,14400, 12872,14528, 12880,14408, 12888,13488, 12896,14416, + 12904,14544, 12912,14424, 12920,14000, 12928,14368, 12936,14496, + 12944,14376, 12952,14512, 12960,14384, 12968,14504, 12976,14488, + 12984,15024, 12992,14432, 13000,14560, 13008,14440, 13016,15536, + 13024,14448, 13032,14568, 13040,14744, 13048,16048, 13056,14592, + 13064,14720, 13072,14600, 13080,14728, 13088,14608, 13096,14736, + 13104,14616, 13112,14744, 13120,14656, 13128,14784, 13136,14664, + 13144,13552, 13152,14672, 13160,14800, 13168,14680, 13176,14064, + 13184,14624, 13192,14752, 13200,14632, 13208,14576, 13216,13464, + 13224,14760, 13232,15512, 13240,15088, 13248,14688, 13256,14816, + 13264,14696, 13272,15600, 13280,13720, 13288,14824, 13296,15768, + 13304,16112, 13336,13360, 13368,14616, 13376,13568, 13384,13576, + 13392,13584, 13400,13616, 13408,13600, 13416,13608, 13424,13592, + 13432,14128, 13448,13472, 13464,14640, 13480,15520, 13488,14536, + 13496,15152, 13504,13696, 13512,13536, 13520,13712, 13528,15664, + 13536,13704, 13544,15584, 13552,14792, 13560,16176, 13592,13616, + 13624,14680, 13656,13680, 13688,14192, 13704,13728, 13720,14704, + 13736,15776, 13744,15560, 13752,15216, 13768,13792, 13784,15728, + 13800,15840, 13808,15816, 13816,16240, 13824,15360, 13832,15488, + 13840,15368, 13848,15496, 13856,15376, 13864,15504, 13872,15384, + 13880,15512, 13888,15424, 13896,15552, 13904,15432, 13912,15560, + 13920,15440, 13928,15568, 13936,15448, 13944,14256, 13952,15392, + 13960,15520, 13968,15400, 13976,14768, 13984,15408, 13992,15528, + 14000,14552, 14008,15280, 14016,15456, 14024,15584, 14032,15464, + 14040,15792, 14048,15472, 14056,15592, 14064,14808, 14072,16304, + 14080,15616, 14088,15744, 14096,15624, 14104,15752, 14112,15632, + 14120,15760, 14128,15640, 14136,15768, 14144,15680, 14152,15808, + 14160,15688, 14168,15816, 14176,15696, 14184,15824, 14192,15704, + 14200,14320, 14208,15648, 14216,15776, 14224,15656, 14232,14832, + 14240,15664, 14248,15784, 14256,15576, 14264,15344, 14272,15712, + 14280,15840, 14288,15720, 14296,15856, 14304,15728, 14312,15848, + 14320,15832, 14328,16368, 14392,14488, 14400,14592, 14408,14600, + 14416,14608, 14424,14616, 14432,14624, 14440,14632, 14448,14640, + 14456,15512, 14504,14512, 14520,14904, 14528,14720, 14536,14728, + 14544,14736, 14552,15416, 14560,14752, 14568,14576, 14584,15928, + 14576,14760, 14592,15360, 14600,15368, 14608,15376, 14616,15384, + 14624,15392, 14632,15400, 14640,15408, 14648,15416, 14656,15616, + 14664,15624, 14672,15632, 14680,15640, 14688,15648, 14696,15656, + 14704,15664, 14712,15576, 14720,15488, 14728,15496, 14736,15504, + 14744,15512, 14752,15520, 14760,14768, 14776,14968, 14768,15528, + 14784,15744, 14792,15752, 14800,15760, 14808,15480, 14816,15776, + 14824,14832, 14840,15992, 14832,15784, 14856,14864, 14864,14880, + 14872,14896, 14880,14976, 14888,14992, 14896,15008, 14904,15024, + 14912,15104, 14920,15120, 14928,15136, 14936,15152, 14944,15232, + 14952,15248, 14960,15264, 14968,15280, 14984,15008, 15000,15024, + 15016,15024, 15040,15112, 15048,15128, 15056,15144, 15064,15544, + 15072,15240, 15080,15256, 15088,15272, 15096,16056, 15104,15872, + 15112,15888, 15120,15904, 15128,15920, 15136,16000, 15144,16016, + 15152,16032, 15160,16048, 15168,16128, 15176,16144, 15184,16160, + 15192,16176, 15200,16256, 15208,16272, 15216,16288, 15224,16304, + 15232,15880, 15240,15896, 15248,15912, 15256,15928, 15264,16008, + 15272,16024, 15280,16040, 15288,16056, 15296,16136, 15304,16152, + 15312,16168, 15320,15608, 15328,16264, 15336,16280, 15344,16296, + 15352,16120, 15416,15512, 15424,15616, 15432,15624, 15440,15632, + 15448,15640, 15456,15648, 15464,15656, 15472,15664, 15480,15768, + 15528,15536, 15544,16048, 15552,15744, 15560,15752, 15568,15760, + 15576,15672, 15584,15776, 15592,15600, 15600,15784, 15608,16184, + 15672,15768, 15736,15832, 15784,15792, 15800,16304, 15848,15856, + 15880,16000, 15864,16248, 15888,16000, 15896,16008, 15904,16000, + 15912,16016, 15920,16008, 15928,16024, 15936,16128, 15944,16160, + 15952,16256, 15960,16288, 15968,16136, 15976,16168, 15984,16264, + 15992,16296, 16008,16032, 16024,16040, 16064,16144, 16040,16048, + 16072,16176, 16080,16272, 16088,16304, 16096,16152, 16104,16184, + 16112,16280, 16136,16256, 16120,16312, 16144,16256, 16152,16264, + 16160,16256, 16168,16272, 16176,16264, 16184,16280, 16200,16208, + 16208,16224, 16216,16240, 16224,16320, 16232,16336, 16240,16352, + 16248,16368, 16264,16288, 16280,16296, 16296,16304, 16344,16368, + 16328,16352, 16360,16368 +}; + +const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH] = +{ + //radix 8, size 4032 + 8,4096, 16,8192, 24,12288, 32,16384, 40,20480, 48,24576, 56,28672, 64,512, + 72,4608, 80,8704, 88,12800, 96,16896, 104,20992, 112,25088, 120,29184, + 128,1024, 136,5120, 144,9216, 152,13312, 160,17408, 168,21504, 176,25600, + 184,29696, 192,1536, 200,5632, 208,9728, 216,13824, 224,17920, 232,22016, + 240,26112, 248,30208, 256,2048, 264,6144, 272,10240, 280,14336, 288,18432, + 296,22528, 304,26624, 312,30720, 320,2560, 328,6656, 336,10752, 344,14848, + 352,18944, 360,23040, 368,27136, 376,31232, 384,3072, 392,7168, 400,11264, + 408,15360, 416,19456, 424,23552, 432,27648, 440,31744, 448,3584, 456,7680, + 464,11776, 472,15872, 480,19968, 488,24064, 496,28160, 504,32256, 520,4160, + 528,8256, 536,12352, 544,16448, 552,20544, 560,24640, 568,28736, 584,4672, + 592,8768, 600,12864, 608,16960, 616,21056, 624,25152, 632,29248, 640,1088, + 648,5184, 656,9280, 664,13376, 672,17472, 680,21568, 688,25664, 696,29760, + 704,1600, 712,5696, 720,9792, 728,13888, 736,17984, 744,22080, 752,26176, + 760,30272, 768,2112, 776,6208, 784,10304, 792,14400, 800,18496, 808,22592, + 816,26688, 824,30784, 832,2624, 840,6720, 848,10816, 856,14912, 864,19008, + 872,23104, 880,27200, 888,31296, 896,3136, 904,7232, 912,11328, 920,15424, + 928,19520, 936,23616, 944,27712, 952,31808, 960,3648, 968,7744, 976,11840, + 984,15936, 992,20032, 1000,24128, 1008,28224, 1016,32320, 1032,4224, + 1040,8320, 1048,12416, 1056,16512, 1064,20608, 1072,24704, 1080,28800, + 1096,4736, 1104,8832, 1112,12928, 1120,17024, 1128,21120, 1136,25216, + 1144,29312, 1160,5248, 1168,9344, 1176,13440, 1184,17536, 1192,21632, + 1200,25728, 1208,29824, 1216,1664, 1224,5760, 1232,9856, 1240,13952, + 1248,18048, 1256,22144, 1264,26240, 1272,30336, 1280,2176, 1288,6272, + 1296,10368, 1304,14464, 1312,18560, 1320,22656, 1328,26752, 1336,30848, + 1344,2688, 1352,6784, 1360,10880, 1368,14976, 1376,19072, 1384,23168, + 1392,27264, 1400,31360, 1408,3200, 1416,7296, 1424,11392, 1432,15488, + 1440,19584, 1448,23680, 1456,27776, 1464,31872, 1472,3712, 1480,7808, + 1488,11904, 1496,16000, 1504,20096, 1512,24192, 1520,28288, 1528,32384, + 1544,4288, 1552,8384, 1560,12480, 1568,16576, 1576,20672, 1584,24768, + 1592,28864, 1608,4800, 1616,8896, 1624,12992, 1632,17088, 1640,21184, + 1648,25280, 1656,29376, 1672,5312, 1680,9408, 1688,13504, 1696,17600, + 1704,21696, 1712,25792, 1720,29888, 1736,5824, 1744,9920, 1752,14016, + 1760,18112, 1768,22208, 1776,26304, 1784,30400, 1792,2240, 1800,6336, + 1808,10432, 1816,14528, 1824,18624, 1832,22720, 1840,26816, 1848,30912, + 1856,2752, 1864,6848, 1872,10944, 1880,15040, 1888,19136, 1896,23232, + 1904,27328, 1912,31424, 1920,3264, 1928,7360, 1936,11456, 1944,15552, + 1952,19648, 1960,23744, 1968,27840, 1976,31936, 1984,3776, 1992,7872, + 2000,11968, 2008,16064, 2016,20160, 2024,24256, 2032,28352, 2040,32448, + 2056,4352, 2064,8448, 2072,12544, 2080,16640, 2088,20736, 2096,24832, + 2104,28928, 2120,4864, 2128,8960, 2136,13056, 2144,17152, 2152,21248, + 2160,25344, 2168,29440, 2184,5376, 2192,9472, 2200,13568, 2208,17664, + 2216,21760, 2224,25856, 2232,29952, 2248,5888, 2256,9984, 2264,14080, + 2272,18176, 2280,22272, 2288,26368, 2296,30464, 2312,6400, 2320,10496, + 2328,14592, 2336,18688, 2344,22784, 2352,26880, 2360,30976, 2368,2816, + 2376,6912, 2384,11008, 2392,15104, 2400,19200, 2408,23296, 2416,27392, + 2424,31488, 2432,3328, 2440,7424, 2448,11520, 2456,15616, 2464,19712, + 2472,23808, 2480,27904, 2488,32000, 2496,3840, 2504,7936, 2512,12032, + 2520,16128, 2528,20224, 2536,24320, 2544,28416, 2552,32512, 2568,4416, + 2576,8512, 2584,12608, 2592,16704, 2600,20800, 2608,24896, 2616,28992, + 2632,4928, 2640,9024, 2648,13120, 2656,17216, 2664,21312, 2672,25408, + 2680,29504, 2696,5440, 2704,9536, 2712,13632, 2720,17728, 2728,21824, + 2736,25920, 2744,30016, 2760,5952, 2768,10048, 2776,14144, 2784,18240, + 2792,22336, 2800,26432, 2808,30528, 2824,6464, 2832,10560, 2840,14656, + 2848,18752, 2856,22848, 2864,26944, 2872,31040, 2888,6976, 2896,11072, + 2904,15168, 2912,19264, 2920,23360, 2928,27456, 2936,31552, 2944,3392, + 2952,7488, 2960,11584, 2968,15680, 2976,19776, 2984,23872, 2992,27968, + 3000,32064, 3008,3904, 3016,8000, 3024,12096, 3032,16192, 3040,20288, + 3048,24384, 3056,28480, 3064,32576, 3080,4480, 3088,8576, 3096,12672, + 3104,16768, 3112,20864, 3120,24960, 3128,29056, 3144,4992, 3152,9088, + 3160,13184, 3168,17280, 3176,21376, 3184,25472, 3192,29568, 3208,5504, + 3216,9600, 3224,13696, 3232,17792, 3240,21888, 3248,25984, 3256,30080, + 3272,6016, 3280,10112, 3288,14208, 3296,18304, 3304,22400, 3312,26496, + 3320,30592, 3336,6528, 3344,10624, 3352,14720, 3360,18816, 3368,22912, + 3376,27008, 3384,31104, 3400,7040, 3408,11136, 3416,15232, 3424,19328, + 3432,23424, 3440,27520, 3448,31616, 3464,7552, 3472,11648, 3480,15744, + 3488,19840, 3496,23936, 3504,28032, 3512,32128, 3520,3968, 3528,8064, + 3536,12160, 3544,16256, 3552,20352, 3560,24448, 3568,28544, 3576,32640, + 3592,4544, 3600,8640, 3608,12736, 3616,16832, 3624,20928, 3632,25024, + 3640,29120, 3656,5056, 3664,9152, 3672,13248, 3680,17344, 3688,21440, + 3696,25536, 3704,29632, 3720,5568, 3728,9664, 3736,13760, 3744,17856, + 3752,21952, 3760,26048, 3768,30144, 3784,6080, 3792,10176, 3800,14272, + 3808,18368, 3816,22464, 3824,26560, 3832,30656, 3848,6592, 3856,10688, + 3864,14784, 3872,18880, 3880,22976, 3888,27072, 3896,31168, 3912,7104, + 3920,11200, 3928,15296, 3936,19392, 3944,23488, 3952,27584, 3960,31680, + 3976,7616, 3984,11712, 3992,15808, 4000,19904, 4008,24000, 4016,28096, + 4024,32192, 4040,8128, 4048,12224, 4056,16320, 4064,20416, 4072,24512, + 4080,28608, 4088,32704, 4112,8200, 4120,12296, 4128,16392, 4136,20488, + 4144,24584, 4152,28680, 4168,4616, 4176,8712, 4184,12808, 4192,16904, + 4200,21000, 4208,25096, 4216,29192, 4232,5128, 4240,9224, 4248,13320, + 4256,17416, 4264,21512, 4272,25608, 4280,29704, 4296,5640, 4304,9736, + 4312,13832, 4320,17928, 4328,22024, 4336,26120, 4344,30216, 4360,6152, + 4368,10248, 4376,14344, 4384,18440, 4392,22536, 4400,26632, 4408,30728, + 4424,6664, 4432,10760, 4440,14856, 4448,18952, 4456,23048, 4464,27144, + 4472,31240, 4488,7176, 4496,11272, 4504,15368, 4512,19464, 4520,23560, + 4528,27656, 4536,31752, 4552,7688, 4560,11784, 4568,15880, 4576,19976, + 4584,24072, 4592,28168, 4600,32264, 4624,8264, 4632,12360, 4640,16456, + 4648,20552, 4656,24648, 4664,28744, 4688,8776, 4696,12872, 4704,16968, + 4712,21064, 4720,25160, 4728,29256, 4744,5192, 4752,9288, 4760,13384, + 4768,17480, 4776,21576, 4784,25672, 4792,29768, 4808,5704, 4816,9800, + 4824,13896, 4832,17992, 4840,22088, 4848,26184, 4856,30280, 4872,6216, + 4880,10312, 4888,14408, 4896,18504, 4904,22600, 4912,26696, 4920,30792, + 4936,6728, 4944,10824, 4952,14920, 4960,19016, 4968,23112, 4976,27208, + 4984,31304, 5000,7240, 5008,11336, 5016,15432, 5024,19528, 5032,23624, + 5040,27720, 5048,31816, 5064,7752, 5072,11848, 5080,15944, 5088,20040, + 5096,24136, 5104,28232, 5112,32328, 5136,8328, 5144,12424, 5152,16520, + 5160,20616, 5168,24712, 5176,28808, 5200,8840, 5208,12936, 5216,17032, + 5224,21128, 5232,25224, 5240,29320, 5264,9352, 5272,13448, 5280,17544, + 5288,21640, 5296,25736, 5304,29832, 5320,5768, 5328,9864, 5336,13960, + 5344,18056, 5352,22152, 5360,26248, 5368,30344, 5384,6280, 5392,10376, + 5400,14472, 5408,18568, 5416,22664, 5424,26760, 5432,30856, 5448,6792, + 5456,10888, 5464,14984, 5472,19080, 5480,23176, 5488,27272, 5496,31368, + 5512,7304, 5520,11400, 5528,15496, 5536,19592, 5544,23688, 5552,27784, + 5560,31880, 5576,7816, 5584,11912, 5592,16008, 5600,20104, 5608,24200, + 5616,28296, 5624,32392, 5648,8392, 5656,12488, 5664,16584, 5672,20680, + 5680,24776, 5688,28872, 5712,8904, 5720,13000, 5728,17096, 5736,21192, + 5744,25288, 5752,29384, 5776,9416, 5784,13512, 5792,17608, 5800,21704, + 5808,25800, 5816,29896, 5840,9928, 5848,14024, 5856,18120, 5864,22216, + 5872,26312, 5880,30408, 5896,6344, 5904,10440, 5912,14536, 5920,18632, + 5928,22728, 5936,26824, 5944,30920, 5960,6856, 5968,10952, 5976,15048, + 5984,19144, 5992,23240, 6000,27336, 6008,31432, 6024,7368, 6032,11464, + 6040,15560, 6048,19656, 6056,23752, 6064,27848, 6072,31944, 6088,7880, + 6096,11976, 6104,16072, 6112,20168, 6120,24264, 6128,28360, 6136,32456, + 6160,8456, 6168,12552, 6176,16648, 6184,20744, 6192,24840, 6200,28936, + 6224,8968, 6232,13064, 6240,17160, 6248,21256, 6256,25352, 6264,29448, + 6288,9480, 6296,13576, 6304,17672, 6312,21768, 6320,25864, 6328,29960, + 6352,9992, 6360,14088, 6368,18184, 6376,22280, 6384,26376, 6392,30472, + 6416,10504, 6424,14600, 6432,18696, 6440,22792, 6448,26888, 6456,30984, + 6472,6920, 6480,11016, 6488,15112, 6496,19208, 6504,23304, 6512,27400, + 6520,31496, 6536,7432, 6544,11528, 6552,15624, 6560,19720, 6568,23816, + 6576,27912, 6584,32008, 6600,7944, 6608,12040, 6616,16136, 6624,20232, + 6632,24328, 6640,28424, 6648,32520, 6672,8520, 6680,12616, 6688,16712, + 6696,20808, 6704,24904, 6712,29000, 6736,9032, 6744,13128, 6752,17224, + 6760,21320, 6768,25416, 6776,29512, 6800,9544, 6808,13640, 6816,17736, + 6824,21832, 6832,25928, 6840,30024, 6864,10056, 6872,14152, 6880,18248, + 6888,22344, 6896,26440, 6904,30536, 6928,10568, 6936,14664, 6944,18760, + 6952,22856, 6960,26952, 6968,31048, 6992,11080, 7000,15176, 7008,19272, + 7016,23368, 7024,27464, 7032,31560, 7048,7496, 7056,11592, 7064,15688, + 7072,19784, 7080,23880, 7088,27976, 7096,32072, 7112,8008, 7120,12104, + 7128,16200, 7136,20296, 7144,24392, 7152,28488, 7160,32584, 7184,8584, + 7192,12680, 7200,16776, 7208,20872, 7216,24968, 7224,29064, 7248,9096, + 7256,13192, 7264,17288, 7272,21384, 7280,25480, 7288,29576, 7312,9608, + 7320,13704, 7328,17800, 7336,21896, 7344,25992, 7352,30088, 7376,10120, + 7384,14216, 7392,18312, 7400,22408, 7408,26504, 7416,30600, 7440,10632, + 7448,14728, 7456,18824, 7464,22920, 7472,27016, 7480,31112, 7504,11144, + 7512,15240, 7520,19336, 7528,23432, 7536,27528, 7544,31624, 7568,11656, + 7576,15752, 7584,19848, 7592,23944, 7600,28040, 7608,32136, 7624,8072, + 7632,12168, 7640,16264, 7648,20360, 7656,24456, 7664,28552, 7672,32648, + 7696,8648, 7704,12744, 7712,16840, 7720,20936, 7728,25032, 7736,29128, + 7760,9160, 7768,13256, 7776,17352, 7784,21448, 7792,25544, 7800,29640, + 7824,9672, 7832,13768, 7840,17864, 7848,21960, 7856,26056, 7864,30152, + 7888,10184, 7896,14280, 7904,18376, 7912,22472, 7920,26568, 7928,30664, + 7952,10696, 7960,14792, 7968,18888, 7976,22984, 7984,27080, 7992,31176, + 8016,11208, 8024,15304, 8032,19400, 8040,23496, 8048,27592, 8056,31688, + 8080,11720, 8088,15816, 8096,19912, 8104,24008, 8112,28104, 8120,32200, + 8144,12232, 8152,16328, 8160,20424, 8168,24520, 8176,28616, 8184,32712, + 8216,12304, 8224,16400, 8232,20496, 8240,24592, 8248,28688, 8272,8720, + 8280,12816, 8288,16912, 8296,21008, 8304,25104, 8312,29200, 8336,9232, + 8344,13328, 8352,17424, 8360,21520, 8368,25616, 8376,29712, 8400,9744, + 8408,13840, 8416,17936, 8424,22032, 8432,26128, 8440,30224, 8464,10256, + 8472,14352, 8480,18448, 8488,22544, 8496,26640, 8504,30736, 8528,10768, + 8536,14864, 8544,18960, 8552,23056, 8560,27152, 8568,31248, 8592,11280, + 8600,15376, 8608,19472, 8616,23568, 8624,27664, 8632,31760, 8656,11792, + 8664,15888, 8672,19984, 8680,24080, 8688,28176, 8696,32272, 8728,12368, + 8736,16464, 8744,20560, 8752,24656, 8760,28752, 8792,12880, 8800,16976, + 8808,21072, 8816,25168, 8824,29264, 8848,9296, 8856,13392, 8864,17488, + 8872,21584, 8880,25680, 8888,29776, 8912,9808, 8920,13904, 8928,18000, + 8936,22096, 8944,26192, 8952,30288, 8976,10320, 8984,14416, 8992,18512, + 9000,22608, 9008,26704, 9016,30800, 9040,10832, 9048,14928, 9056,19024, + 9064,23120, 9072,27216, 9080,31312, 9104,11344, 9112,15440, 9120,19536, + 9128,23632, 9136,27728, 9144,31824, 9168,11856, 9176,15952, 9184,20048, + 9192,24144, 9200,28240, 9208,32336, 9240,12432, 9248,16528, 9256,20624, + 9264,24720, 9272,28816, 9304,12944, 9312,17040, 9320,21136, 9328,25232, + 9336,29328, 9368,13456, 9376,17552, 9384,21648, 9392,25744, 9400,29840, + 9424,9872, 9432,13968, 9440,18064, 9448,22160, 9456,26256, 9464,30352, + 9488,10384, 9496,14480, 9504,18576, 9512,22672, 9520,26768, 9528,30864, + 9552,10896, 9560,14992, 9568,19088, 9576,23184, 9584,27280, 9592,31376, + 9616,11408, 9624,15504, 9632,19600, 9640,23696, 9648,27792, 9656,31888, + 9680,11920, 9688,16016, 9696,20112, 9704,24208, 9712,28304, 9720,32400, + 9752,12496, 9760,16592, 9768,20688, 9776,24784, 9784,28880, 9816,13008, + 9824,17104, 9832,21200, 9840,25296, 9848,29392, 9880,13520, 9888,17616, + 9896,21712, 9904,25808, 9912,29904, 9944,14032, 9952,18128, 9960,22224, + 9968,26320, 9976,30416, 10000,10448, 10008,14544, 10016,18640, 10024,22736, + 10032,26832, 10040,30928, 10064,10960, 10072,15056, 10080,19152, + 10088,23248, 10096,27344, 10104,31440, 10128,11472, 10136,15568, + 10144,19664, 10152,23760, 10160,27856, 10168,31952, 10192,11984, + 10200,16080, 10208,20176, 10216,24272, 10224,28368, 10232,32464, + 10264,12560, 10272,16656, 10280,20752, 10288,24848, 10296,28944, + 10328,13072, 10336,17168, 10344,21264, 10352,25360, 10360,29456, + 10392,13584, 10400,17680, 10408,21776, 10416,25872, 10424,29968, + 10456,14096, 10464,18192, 10472,22288, 10480,26384, 10488,30480, + 10520,14608, 10528,18704, 10536,22800, 10544,26896, 10552,30992, + 10576,11024, 10584,15120, 10592,19216, 10600,23312, 10608,27408, + 10616,31504, 10640,11536, 10648,15632, 10656,19728, 10664,23824, + 10672,27920, 10680,32016, 10704,12048, 10712,16144, 10720,20240, + 10728,24336, 10736,28432, 10744,32528, 10776,12624, 10784,16720, + 10792,20816, 10800,24912, 10808,29008, 10840,13136, 10848,17232, + 10856,21328, 10864,25424, 10872,29520, 10904,13648, 10912,17744, + 10920,21840, 10928,25936, 10936,30032, 10968,14160, 10976,18256, + 10984,22352, 10992,26448, 11000,30544, 11032,14672, 11040,18768, + 11048,22864, 11056,26960, 11064,31056, 11096,15184, 11104,19280, + 11112,23376, 11120,27472, 11128,31568, 11152,11600, 11160,15696, + 11168,19792, 11176,23888, 11184,27984, 11192,32080, 11216,12112, + 11224,16208, 11232,20304, 11240,24400, 11248,28496, 11256,32592, + 11288,12688, 11296,16784, 11304,20880, 11312,24976, 11320,29072, + 11352,13200, 11360,17296, 11368,21392, 11376,25488, 11384,29584, + 11416,13712, 11424,17808, 11432,21904, 11440,26000, 11448,30096, + 11480,14224, 11488,18320, 11496,22416, 11504,26512, 11512,30608, + 11544,14736, 11552,18832, 11560,22928, 11568,27024, 11576,31120, + 11608,15248, 11616,19344, 11624,23440, 11632,27536, 11640,31632, + 11672,15760, 11680,19856, 11688,23952, 11696,28048, 11704,32144, + 11728,12176, 11736,16272, 11744,20368, 11752,24464, 11760,28560, + 11768,32656, 11800,12752, 11808,16848, 11816,20944, 11824,25040, + 11832,29136, 11864,13264, 11872,17360, 11880,21456, 11888,25552, + 11896,29648, 11928,13776, 11936,17872, 11944,21968, 11952,26064, + 11960,30160, 11992,14288, 12000,18384, 12008,22480, 12016,26576, + 12024,30672, 12056,14800, 12064,18896, 12072,22992, 12080,27088, + 12088,31184, 12120,15312, 12128,19408, 12136,23504, 12144,27600, + 12152,31696, 12184,15824, 12192,19920, 12200,24016, 12208,28112, + 12216,32208, 12248,16336, 12256,20432, 12264,24528, 12272,28624, + 12280,32720, 12320,16408, 12328,20504, 12336,24600, 12344,28696, + 12376,12824, 12384,16920, 12392,21016, 12400,25112, 12408,29208, + 12440,13336, 12448,17432, 12456,21528, 12464,25624, 12472,29720, + 12504,13848, 12512,17944, 12520,22040, 12528,26136, 12536,30232, + 12568,14360, 12576,18456, 12584,22552, 12592,26648, 12600,30744, + 12632,14872, 12640,18968, 12648,23064, 12656,27160, 12664,31256, + 12696,15384, 12704,19480, 12712,23576, 12720,27672, 12728,31768, + 12760,15896, 12768,19992, 12776,24088, 12784,28184, 12792,32280, + 12832,16472, 12840,20568, 12848,24664, 12856,28760, 12896,16984, + 12904,21080, 12912,25176, 12920,29272, 12952,13400, 12960,17496, + 12968,21592, 12976,25688, 12984,29784, 13016,13912, 13024,18008, + 13032,22104, 13040,26200, 13048,30296, 13080,14424, 13088,18520, + 13096,22616, 13104,26712, 13112,30808, 13144,14936, 13152,19032, + 13160,23128, 13168,27224, 13176,31320, 13208,15448, 13216,19544, + 13224,23640, 13232,27736, 13240,31832, 13272,15960, 13280,20056, + 13288,24152, 13296,28248, 13304,32344, 13344,16536, 13352,20632, + 13360,24728, 13368,28824, 13408,17048, 13416,21144, 13424,25240, + 13432,29336, 13472,17560, 13480,21656, 13488,25752, 13496,29848, + 13528,13976, 13536,18072, 13544,22168, 13552,26264, 13560,30360, + 13592,14488, 13600,18584, 13608,22680, 13616,26776, 13624,30872, + 13656,15000, 13664,19096, 13672,23192, 13680,27288, 13688,31384, + 13720,15512, 13728,19608, 13736,23704, 13744,27800, 13752,31896, + 13784,16024, 13792,20120, 13800,24216, 13808,28312, 13816,32408, + 13856,16600, 13864,20696, 13872,24792, 13880,28888, 13920,17112, + 13928,21208, 13936,25304, 13944,29400, 13984,17624, 13992,21720, + 14000,25816, 14008,29912, 14048,18136, 14056,22232, 14064,26328, + 14072,30424, 14104,14552, 14112,18648, 14120,22744, 14128,26840, + 14136,30936, 14168,15064, 14176,19160, 14184,23256, 14192,27352, + 14200,31448, 14232,15576, 14240,19672, 14248,23768, 14256,27864, + 14264,31960, 14296,16088, 14304,20184, 14312,24280, 14320,28376, + 14328,32472, 14368,16664, 14376,20760, 14384,24856, 14392,28952, + 14432,17176, 14440,21272, 14448,25368, 14456,29464, 14496,17688, + 14504,21784, 14512,25880, 14520,29976, 14560,18200, 14568,22296, + 14576,26392, 14584,30488, 14624,18712, 14632,22808, 14640,26904, + 14648,31000, 14680,15128, 14688,19224, 14696,23320, 14704,27416, + 14712,31512, 14744,15640, 14752,19736, 14760,23832, 14768,27928, + 14776,32024, 14808,16152, 14816,20248, 14824,24344, 14832,28440, + 14840,32536, 14880,16728, 14888,20824, 14896,24920, 14904,29016, + 14944,17240, 14952,21336, 14960,25432, 14968,29528, 15008,17752, + 15016,21848, 15024,25944, 15032,30040, 15072,18264, 15080,22360, + 15088,26456, 15096,30552, 15136,18776, 15144,22872, 15152,26968, + 15160,31064, 15200,19288, 15208,23384, 15216,27480, 15224,31576, + 15256,15704, 15264,19800, 15272,23896, 15280,27992, 15288,32088, + 15320,16216, 15328,20312, 15336,24408, 15344,28504, 15352,32600, + 15392,16792, 15400,20888, 15408,24984, 15416,29080, 15456,17304, + 15464,21400, 15472,25496, 15480,29592, 15520,17816, 15528,21912, + 15536,26008, 15544,30104, 15584,18328, 15592,22424, 15600,26520, + 15608,30616, 15648,18840, 15656,22936, 15664,27032, 15672,31128, + 15712,19352, 15720,23448, 15728,27544, 15736,31640, 15776,19864, + 15784,23960, 15792,28056, 15800,32152, 15832,16280, 15840,20376, + 15848,24472, 15856,28568, 15864,32664, 15904,16856, 15912,20952, + 15920,25048, 15928,29144, 15968,17368, 15976,21464, 15984,25560, + 15992,29656, 16032,17880, 16040,21976, 16048,26072, 16056,30168, + 16096,18392, 16104,22488, 16112,26584, 16120,30680, 16160,18904, + 16168,23000, 16176,27096, 16184,31192, 16224,19416, 16232,23512, + 16240,27608, 16248,31704, 16288,19928, 16296,24024, 16304,28120, + 16312,32216, 16352,20440, 16360,24536, 16368,28632, 16376,32728, + 16424,20512, 16432,24608, 16440,28704, 16480,16928, 16488,21024, + 16496,25120, 16504,29216, 16544,17440, 16552,21536, 16560,25632, + 16568,29728, 16608,17952, 16616,22048, 16624,26144, 16632,30240, + 16672,18464, 16680,22560, 16688,26656, 16696,30752, 16736,18976, + 16744,23072, 16752,27168, 16760,31264, 16800,19488, 16808,23584, + 16816,27680, 16824,31776, 16864,20000, 16872,24096, 16880,28192, + 16888,32288, 16936,20576, 16944,24672, 16952,28768, 17000,21088, + 17008,25184, 17016,29280, 17056,17504, 17064,21600, 17072,25696, + 17080,29792, 17120,18016, 17128,22112, 17136,26208, 17144,30304, + 17184,18528, 17192,22624, 17200,26720, 17208,30816, 17248,19040, + 17256,23136, 17264,27232, 17272,31328, 17312,19552, 17320,23648, + 17328,27744, 17336,31840, 17376,20064, 17384,24160, 17392,28256, + 17400,32352, 17448,20640, 17456,24736, 17464,28832, 17512,21152, + 17520,25248, 17528,29344, 17576,21664, 17584,25760, 17592,29856, + 17632,18080, 17640,22176, 17648,26272, 17656,30368, 17696,18592, + 17704,22688, 17712,26784, 17720,30880, 17760,19104, 17768,23200, + 17776,27296, 17784,31392, 17824,19616, 17832,23712, 17840,27808, + 17848,31904, 17888,20128, 17896,24224, 17904,28320, 17912,32416, + 17960,20704, 17968,24800, 17976,28896, 18024,21216, 18032,25312, + 18040,29408, 18088,21728, 18096,25824, 18104,29920, 18152,22240, + 18160,26336, 18168,30432, 18208,18656, 18216,22752, 18224,26848, + 18232,30944, 18272,19168, 18280,23264, 18288,27360, 18296,31456, + 18336,19680, 18344,23776, 18352,27872, 18360,31968, 18400,20192, + 18408,24288, 18416,28384, 18424,32480, 18472,20768, 18480,24864, + 18488,28960, 18536,21280, 18544,25376, 18552,29472, 18600,21792, + 18608,25888, 18616,29984, 18664,22304, 18672,26400, 18680,30496, + 18728,22816, 18736,26912, 18744,31008, 18784,19232, 18792,23328, + 18800,27424, 18808,31520, 18848,19744, 18856,23840, 18864,27936, + 18872,32032, 18912,20256, 18920,24352, 18928,28448, 18936,32544, + 18984,20832, 18992,24928, 19000,29024, 19048,21344, 19056,25440, + 19064,29536, 19112,21856, 19120,25952, 19128,30048, 19176,22368, + 19184,26464, 19192,30560, 19240,22880, 19248,26976, 19256,31072, + 19304,23392, 19312,27488, 19320,31584, 19360,19808, 19368,23904, + 19376,28000, 19384,32096, 19424,20320, 19432,24416, 19440,28512, + 19448,32608, 19496,20896, 19504,24992, 19512,29088, 19560,21408, + 19568,25504, 19576,29600, 19624,21920, 19632,26016, 19640,30112, + 19688,22432, 19696,26528, 19704,30624, 19752,22944, 19760,27040, + 19768,31136, 19816,23456, 19824,27552, 19832,31648, 19880,23968, + 19888,28064, 19896,32160, 19936,20384, 19944,24480, 19952,28576, + 19960,32672, 20008,20960, 20016,25056, 20024,29152, 20072,21472, + 20080,25568, 20088,29664, 20136,21984, 20144,26080, 20152,30176, + 20200,22496, 20208,26592, 20216,30688, 20264,23008, 20272,27104, + 20280,31200, 20328,23520, 20336,27616, 20344,31712, 20392,24032, + 20400,28128, 20408,32224, 20456,24544, 20464,28640, 20472,32736, + 20528,24616, 20536,28712, 20584,21032, 20592,25128, 20600,29224, + 20648,21544, 20656,25640, 20664,29736, 20712,22056, 20720,26152, + 20728,30248, 20776,22568, 20784,26664, 20792,30760, 20840,23080, + 20848,27176, 20856,31272, 20904,23592, 20912,27688, 20920,31784, + 20968,24104, 20976,28200, 20984,32296, 21040,24680, 21048,28776, + 21104,25192, 21112,29288, 21160,21608, 21168,25704, 21176,29800, + 21224,22120, 21232,26216, 21240,30312, 21288,22632, 21296,26728, + 21304,30824, 21352,23144, 21360,27240, 21368,31336, 21416,23656, + 21424,27752, 21432,31848, 21480,24168, 21488,28264, 21496,32360, + 21552,24744, 21560,28840, 21616,25256, 21624,29352, 21680,25768, + 21688,29864, 21736,22184, 21744,26280, 21752,30376, 21800,22696, + 21808,26792, 21816,30888, 21864,23208, 21872,27304, 21880,31400, + 21928,23720, 21936,27816, 21944,31912, 21992,24232, 22000,28328, + 22008,32424, 22064,24808, 22072,28904, 22128,25320, 22136,29416, + 22192,25832, 22200,29928, 22256,26344, 22264,30440, 22312,22760, + 22320,26856, 22328,30952, 22376,23272, 22384,27368, 22392,31464, + 22440,23784, 22448,27880, 22456,31976, 22504,24296, 22512,28392, + 22520,32488, 22576,24872, 22584,28968, 22640,25384, 22648,29480, + 22704,25896, 22712,29992, 22768,26408, 22776,30504, 22832,26920, + 22840,31016, 22888,23336, 22896,27432, 22904,31528, 22952,23848, + 22960,27944, 22968,32040, 23016,24360, 23024,28456, 23032,32552, + 23088,24936, 23096,29032, 23152,25448, 23160,29544, 23216,25960, + 23224,30056, 23280,26472, 23288,30568, 23344,26984, 23352,31080, + 23408,27496, 23416,31592, 23464,23912, 23472,28008, 23480,32104, + 23528,24424, 23536,28520, 23544,32616, 23600,25000, 23608,29096, + 23664,25512, 23672,29608, 23728,26024, 23736,30120, 23792,26536, + 23800,30632, 23856,27048, 23864,31144, 23920,27560, 23928,31656, + 23984,28072, 23992,32168, 24040,24488, 24048,28584, 24056,32680, + 24112,25064, 24120,29160, 24176,25576, 24184,29672, 24240,26088, + 24248,30184, 24304,26600, 24312,30696, 24368,27112, 24376,31208, + 24432,27624, 24440,31720, 24496,28136, 24504,32232, 24560,28648, + 24568,32744, 24632,28720, 24688,25136, 24696,29232, 24752,25648, + 24760,29744, 24816,26160, 24824,30256, 24880,26672, 24888,30768, + 24944,27184, 24952,31280, 25008,27696, 25016,31792, 25072,28208, + 25080,32304, 25144,28784, 25208,29296, 25264,25712, 25272,29808, + 25328,26224, 25336,30320, 25392,26736, 25400,30832, 25456,27248, + 25464,31344, 25520,27760, 25528,31856, 25584,28272, 25592,32368, + 25656,28848, 25720,29360, 25784,29872, 25840,26288, 25848,30384, + 25904,26800, 25912,30896, 25968,27312, 25976,31408, 26032,27824, + 26040,31920, 26096,28336, 26104,32432, 26168,28912, 26232,29424, + 26296,29936, 26360,30448, 26416,26864, 26424,30960, 26480,27376, + 26488,31472, 26544,27888, 26552,31984, 26608,28400, 26616,32496, + 26680,28976, 26744,29488, 26808,30000, 26872,30512, 26936,31024, + 26992,27440, 27000,31536, 27056,27952, 27064,32048, 27120,28464, + 27128,32560, 27192,29040, 27256,29552, 27320,30064, 27384,30576, + 27448,31088, 27512,31600, 27568,28016, 27576,32112, 27632,28528, + 27640,32624, 27704,29104, 27768,29616, 27832,30128, 27896,30640, + 27960,31152, 28024,31664, 28088,32176, 28144,28592, 28152,32688, + 28216,29168, 28280,29680, 28344,30192, 28408,30704, 28472,31216, + 28536,31728, 28600,32240, 28664,32752, 28792,29240, 28856,29752, + 28920,30264, 28984,30776, 29048,31288, 29112,31800, 29176,32312, + 29368,29816, 29432,30328, 29496,30840, 29560,31352, 29624,31864, + 29688,32376, 29944,30392, 30008,30904, 30072,31416, 30136,31928, + 30200,32440, 30520,30968, 30584,31480, 30648,31992, 30712,32504, + 31096,31544, 31160,32056, 31224,32568, 31672,32120, 31736,32632, + 32248,32696 +}; + + +const uint16_t armBitRevIndexTable_fixed_16[ARMBITREVINDEXTABLE_FIXED___16_TABLE_LENGTH] = +{ + //radix 4, size 12 + 8,64, 16,32, 24,96, 40,80, 56,112, 88,104 +}; + +const uint16_t armBitRevIndexTable_fixed_32[ARMBITREVINDEXTABLE_FIXED___32_TABLE_LENGTH] = +{ + //4x2, size 24 + 8,128, 16,64, 24,192, 40,160, 48,96, 56,224, 72,144, + 88,208, 104,176, 120,240, 152,200, 184,232 +}; + +const uint16_t armBitRevIndexTable_fixed_64[ARMBITREVINDEXTABLE_FIXED___64_TABLE_LENGTH] = +{ + //radix 4, size 56 + 8,256, 16,128, 24,384, 32,64, 40,320, 48,192, 56,448, 72,288, 80,160, 88,416, 104,352, + 112,224, 120,480, 136,272, 152,400, 168,336, 176,208, 184,464, 200,304, 216,432, + 232,368, 248,496, 280,392, 296,328, 312,456, 344,424, 376,488, 440,472 +}; + +const uint16_t armBitRevIndexTable_fixed_128[ARMBITREVINDEXTABLE_FIXED__128_TABLE_LENGTH] = +{ + //4x2, size 112 + 8,512, 16,256, 24,768, 32,128, 40,640, 48,384, 56,896, 72,576, 80,320, 88,832, 96,192, + 104,704, 112,448, 120,960, 136,544, 144,288, 152,800, 168,672, 176,416, 184,928, 200,608, + 208,352, 216,864, 232,736, 240,480, 248,992, 264,528, 280,784, 296,656, 304,400, 312,912, + 328,592, 344,848, 360,720, 368,464, 376,976, 392,560, 408,816, 424,688, 440,944, 456,624, + 472,880, 488,752, 504,1008, 536,776, 552,648, 568,904, 600,840, 616,712, 632,968, + 664,808, 696,936, 728,872, 760,1000, 824,920, 888,984 +}; + +const uint16_t armBitRevIndexTable_fixed_256[ARMBITREVINDEXTABLE_FIXED__256_TABLE_LENGTH] = +{ + //radix 4, size 240 + 8,1024, 16,512, 24,1536, 32,256, 40,1280, 48,768, 56,1792, 64,128, 72,1152, 80,640, + 88,1664, 96,384, 104,1408, 112,896, 120,1920, 136,1088, 144,576, 152,1600, 160,320, + 168,1344, 176,832, 184,1856, 200,1216, 208,704, 216,1728, 224,448, 232,1472, 240,960, + 248,1984, 264,1056, 272,544, 280,1568, 296,1312, 304,800, 312,1824, 328,1184, 336,672, + 344,1696, 352,416, 360,1440, 368,928, 376,1952, 392,1120, 400,608, 408,1632, 424,1376, + 432,864, 440,1888, 456,1248, 464,736, 472,1760, 488,1504, 496,992, 504,2016, 520,1040, + 536,1552, 552,1296, 560,784, 568,1808, 584,1168, 592,656, 600,1680, 616,1424, 624,912, + 632,1936, 648,1104, 664,1616, 680,1360, 688,848, 696,1872, 712,1232, 728,1744, 744,1488, + 752,976, 760,2000, 776,1072, 792,1584, 808,1328, 824,1840, 840,1200, 856,1712, 872,1456, + 880,944, 888,1968, 904,1136, 920,1648, 936,1392, 952,1904, 968,1264, 984,1776, 1000,1520, + 1016,2032, 1048,1544, 1064,1288, 1080,1800, 1096,1160, 1112,1672, 1128,1416, 1144,1928, + 1176,1608, 1192,1352, 1208,1864, 1240,1736, 1256,1480, 1272,1992, 1304,1576, 1336,1832, + 1368,1704, 1384,1448, 1400,1960, 1432,1640, 1464,1896, 1496,1768, 1528,2024, 1592,1816, + 1624,1688, 1656,1944, 1720,1880, 1784,2008, 1912,1976 +}; + +const uint16_t armBitRevIndexTable_fixed_512[ARMBITREVINDEXTABLE_FIXED__512_TABLE_LENGTH] = +{ + //4x2, size 480 + 8,2048, 16,1024, 24,3072, 32,512, 40,2560, 48,1536, 56,3584, 64,256, 72,2304, 80,1280, + 88,3328, 96,768, 104,2816, 112,1792, 120,3840, 136,2176, 144,1152, 152,3200, 160,640, + 168,2688, 176,1664, 184,3712, 192,384, 200,2432, 208,1408, 216,3456, 224,896, 232,2944, + 240,1920, 248,3968, 264,2112, 272,1088, 280,3136, 288,576, 296,2624, 304,1600, 312,3648, + 328,2368, 336,1344, 344,3392, 352,832, 360,2880, 368,1856, 376,3904, 392,2240, 400,1216, + 408,3264, 416,704, 424,2752, 432,1728, 440,3776, 456,2496, 464,1472, 472,3520, 480,960, + 488,3008, 496,1984, 504,4032, 520,2080, 528,1056, 536,3104, 552,2592, 560,1568, 568,3616, + 584,2336, 592,1312, 600,3360, 608,800, 616,2848, 624,1824, 632,3872, 648,2208, 656,1184, + 664,3232, 680,2720, 688,1696, 696,3744, 712,2464, 720,1440, 728,3488, 736,928, 744,2976, + 752,1952, 760,4000, 776,2144, 784,1120, 792,3168, 808,2656, 816,1632, 824,3680, 840,2400, + 848,1376, 856,3424, 872,2912, 880,1888, 888,3936, 904,2272, 912,1248, 920,3296, 936,2784, + 944,1760, 952,3808, 968,2528, 976,1504, 984,3552, 1000,3040, 1008,2016, 1016,4064, + 1032,2064, 1048,3088, 1064,2576, 1072,1552, 1080,3600, 1096,2320, 1104,1296, 1112,3344, + 1128,2832, 1136,1808, 1144,3856, 1160,2192, 1176,3216, 1192,2704, 1200,1680, 1208,3728, + 1224,2448, 1232,1424, 1240,3472, 1256,2960, 1264,1936, 1272,3984, 1288,2128, 1304,3152, + 1320,2640, 1328,1616, 1336,3664, 1352,2384, 1368,3408, 1384,2896, 1392,1872, 1400,3920, + 1416,2256, 1432,3280, 1448,2768, 1456,1744, 1464,3792, 1480,2512, 1496,3536, 1512,3024, + 1520,2000, 1528,4048, 1544,2096, 1560,3120, 1576,2608, 1592,3632, 1608,2352, 1624,3376, + 1640,2864, 1648,1840, 1656,3888, 1672,2224, 1688,3248, 1704,2736, 1720,3760, 1736,2480, + 1752,3504, 1768,2992, 1776,1968, 1784,4016, 1800,2160, 1816,3184, 1832,2672, 1848,3696, + 1864,2416, 1880,3440, 1896,2928, 1912,3952, 1928,2288, 1944,3312, 1960,2800, 1976,3824, + 1992,2544, 2008,3568, 2024,3056, 2040,4080, 2072,3080, 2088,2568, 2104,3592, 2120,2312, + 2136,3336, 2152,2824, 2168,3848, 2200,3208, 2216,2696, 2232,3720, 2248,2440, 2264,3464, + 2280,2952, 2296,3976, 2328,3144, 2344,2632, 2360,3656, 2392,3400, 2408,2888, 2424,3912, + 2456,3272, 2472,2760, 2488,3784, 2520,3528, 2536,3016, 2552,4040, 2584,3112, 2616,3624, + 2648,3368, 2664,2856, 2680,3880, 2712,3240, 2744,3752, 2776,3496, 2792,2984, 2808,4008, + 2840,3176, 2872,3688, 2904,3432, 2936,3944, 2968,3304, 3000,3816, 3032,3560, 3064,4072, + 3128,3608, 3160,3352, 3192,3864, 3256,3736, 3288,3480, 3320,3992, 3384,3672, 3448,3928, + 3512,3800, 3576,4056, 3704,3896, 3832,4024 +}; + +const uint16_t armBitRevIndexTable_fixed_1024[ARMBITREVINDEXTABLE_FIXED_1024_TABLE_LENGTH] = +{ + //radix 4, size 992 + 8,4096, 16,2048, 24,6144, 32,1024, 40,5120, 48,3072, 56,7168, 64,512, 72,4608, + 80,2560, 88,6656, 96,1536, 104,5632, 112,3584, 120,7680, 128,256, 136,4352, + 144,2304, 152,6400, 160,1280, 168,5376, 176,3328, 184,7424, 192,768, 200,4864, + 208,2816, 216,6912, 224,1792, 232,5888, 240,3840, 248,7936, 264,4224, 272,2176, + 280,6272, 288,1152, 296,5248, 304,3200, 312,7296, 320,640, 328,4736, 336,2688, + 344,6784, 352,1664, 360,5760, 368,3712, 376,7808, 392,4480, 400,2432, 408,6528, + 416,1408, 424,5504, 432,3456, 440,7552, 448,896, 456,4992, 464,2944, 472,7040, + 480,1920, 488,6016, 496,3968, 504,8064, 520,4160, 528,2112, 536,6208, 544,1088, + 552,5184, 560,3136, 568,7232, 584,4672, 592,2624, 600,6720, 608,1600, 616,5696, + 624,3648, 632,7744, 648,4416, 656,2368, 664,6464, 672,1344, 680,5440, 688,3392, + 696,7488, 704,832, 712,4928, 720,2880, 728,6976, 736,1856, 744,5952, 752,3904, + 760,8000, 776,4288, 784,2240, 792,6336, 800,1216, 808,5312, 816,3264, 824,7360, + 840,4800, 848,2752, 856,6848, 864,1728, 872,5824, 880,3776, 888,7872, 904,4544, + 912,2496, 920,6592, 928,1472, 936,5568, 944,3520, 952,7616, 968,5056, 976,3008, + 984,7104, 992,1984, 1000,6080, 1008,4032, 1016,8128, 1032,4128, 1040,2080, + 1048,6176, 1064,5152, 1072,3104, 1080,7200, 1096,4640, 1104,2592, 1112,6688, + 1120,1568, 1128,5664, 1136,3616, 1144,7712, 1160,4384, 1168,2336, 1176,6432, + 1184,1312, 1192,5408, 1200,3360, 1208,7456, 1224,4896, 1232,2848, 1240,6944, + 1248,1824, 1256,5920, 1264,3872, 1272,7968, 1288,4256, 1296,2208, 1304,6304, + 1320,5280, 1328,3232, 1336,7328, 1352,4768, 1360,2720, 1368,6816, 1376,1696, + 1384,5792, 1392,3744, 1400,7840, 1416,4512, 1424,2464, 1432,6560, 1448,5536, + 1456,3488, 1464,7584, 1480,5024, 1488,2976, 1496,7072, 1504,1952, 1512,6048, + 1520,4000, 1528,8096, 1544,4192, 1552,2144, 1560,6240, 1576,5216, 1584,3168, + 1592,7264, 1608,4704, 1616,2656, 1624,6752, 1640,5728, 1648,3680, 1656,7776, + 1672,4448, 1680,2400, 1688,6496, 1704,5472, 1712,3424, 1720,7520, 1736,4960, + 1744,2912, 1752,7008, 1760,1888, 1768,5984, 1776,3936, 1784,8032, 1800,4320, + 1808,2272, 1816,6368, 1832,5344, 1840,3296, 1848,7392, 1864,4832, 1872,2784, + 1880,6880, 1896,5856, 1904,3808, 1912,7904, 1928,4576, 1936,2528, 1944,6624, + 1960,5600, 1968,3552, 1976,7648, 1992,5088, 2000,3040, 2008,7136, 2024,6112, + 2032,4064, 2040,8160, 2056,4112, 2072,6160, 2088,5136, 2096,3088, 2104,7184, + 2120,4624, 2128,2576, 2136,6672, 2152,5648, 2160,3600, 2168,7696, 2184,4368, + 2192,2320, 2200,6416, 2216,5392, 2224,3344, 2232,7440, 2248,4880, 2256,2832, + 2264,6928, 2280,5904, 2288,3856, 2296,7952, 2312,4240, 2328,6288, 2344,5264, + 2352,3216, 2360,7312, 2376,4752, 2384,2704, 2392,6800, 2408,5776, 2416,3728, + 2424,7824, 2440,4496, 2456,6544, 2472,5520, 2480,3472, 2488,7568, 2504,5008, + 2512,2960, 2520,7056, 2536,6032, 2544,3984, 2552,8080, 2568,4176, 2584,6224, + 2600,5200, 2608,3152, 2616,7248, 2632,4688, 2648,6736, 2664,5712, 2672,3664, + 2680,7760, 2696,4432, 2712,6480, 2728,5456, 2736,3408, 2744,7504, 2760,4944, + 2768,2896, 2776,6992, 2792,5968, 2800,3920, 2808,8016, 2824,4304, 2840,6352, + 2856,5328, 2864,3280, 2872,7376, 2888,4816, 2904,6864, 2920,5840, 2928,3792, + 2936,7888, 2952,4560, 2968,6608, 2984,5584, 2992,3536, 3000,7632, 3016,5072, + 3032,7120, 3048,6096, 3056,4048, 3064,8144, 3080,4144, 3096,6192, 3112,5168, + 3128,7216, 3144,4656, 3160,6704, 3176,5680, 3184,3632, 3192,7728, 3208,4400, + 3224,6448, 3240,5424, 3248,3376, 3256,7472, 3272,4912, 3288,6960, 3304,5936, + 3312,3888, 3320,7984, 3336,4272, 3352,6320, 3368,5296, 3384,7344, 3400,4784, + 3416,6832, 3432,5808, 3440,3760, 3448,7856, 3464,4528, 3480,6576, 3496,5552, + 3512,7600, 3528,5040, 3544,7088, 3560,6064, 3568,4016, 3576,8112, 3592,4208, + 3608,6256, 3624,5232, 3640,7280, 3656,4720, 3672,6768, 3688,5744, 3704,7792, + 3720,4464, 3736,6512, 3752,5488, 3768,7536, 3784,4976, 3800,7024, 3816,6000, + 3824,3952, 3832,8048, 3848,4336, 3864,6384, 3880,5360, 3896,7408, 3912,4848, + 3928,6896, 3944,5872, 3960,7920, 3976,4592, 3992,6640, 4008,5616, 4024,7664, + 4040,5104, 4056,7152, 4072,6128, 4088,8176, 4120,6152, 4136,5128, 4152,7176, + 4168,4616, 4184,6664, 4200,5640, 4216,7688, 4232,4360, 4248,6408, 4264,5384, + 4280,7432, 4296,4872, 4312,6920, 4328,5896, 4344,7944, 4376,6280, 4392,5256, + 4408,7304, 4424,4744, 4440,6792, 4456,5768, 4472,7816, 4504,6536, 4520,5512, + 4536,7560, 4552,5000, 4568,7048, 4584,6024, 4600,8072, 4632,6216, 4648,5192, + 4664,7240, 4696,6728, 4712,5704, 4728,7752, 4760,6472, 4776,5448, 4792,7496, + 4808,4936, 4824,6984, 4840,5960, 4856,8008, 4888,6344, 4904,5320, 4920,7368, + 4952,6856, 4968,5832, 4984,7880, 5016,6600, 5032,5576, 5048,7624, 5080,7112, + 5096,6088, 5112,8136, 5144,6184, 5176,7208, 5208,6696, 5224,5672, 5240,7720, + 5272,6440, 5288,5416, 5304,7464, 5336,6952, 5352,5928, 5368,7976, 5400,6312, + 5432,7336, 5464,6824, 5480,5800, 5496,7848, 5528,6568, 5560,7592, 5592,7080, + 5608,6056, 5624,8104, 5656,6248, 5688,7272, 5720,6760, 5752,7784, 5784,6504, + 5816,7528, 5848,7016, 5864,5992, 5880,8040, 5912,6376, 5944,7400, 5976,6888, + 6008,7912, 6040,6632, 6072,7656, 6104,7144, 6136,8168, 6200,7192, 6232,6680, + 6264,7704, 6296,6424, 6328,7448, 6360,6936, 6392,7960, 6456,7320, 6488,6808, + 6520,7832, 6584,7576, 6616,7064, 6648,8088, 6712,7256, 6776,7768, 6840,7512, + 6872,7000, 6904,8024, 6968,7384, 7032,7896, 7096,7640, 7160,8152, 7288,7736, + 7352,7480, 7416,7992, 7544,7864, 7672,8120, 7928,8056 +}; + +const uint16_t armBitRevIndexTable_fixed_2048[ARMBITREVINDEXTABLE_FIXED_2048_TABLE_LENGTH] = +{ + //4x2, size 1984 + 8,8192, 16,4096, 24,12288, 32,2048, 40,10240, 48,6144, 56,14336, 64,1024, + 72,9216, 80,5120, 88,13312, 96,3072, 104,11264, 112,7168, 120,15360, 128,512, + 136,8704, 144,4608, 152,12800, 160,2560, 168,10752, 176,6656, 184,14848, + 192,1536, 200,9728, 208,5632, 216,13824, 224,3584, 232,11776, 240,7680, + 248,15872, 264,8448, 272,4352, 280,12544, 288,2304, 296,10496, 304,6400, + 312,14592, 320,1280, 328,9472, 336,5376, 344,13568, 352,3328, 360,11520, + 368,7424, 376,15616, 384,768, 392,8960, 400,4864, 408,13056, 416,2816, + 424,11008, 432,6912, 440,15104, 448,1792, 456,9984, 464,5888, 472,14080, + 480,3840, 488,12032, 496,7936, 504,16128, 520,8320, 528,4224, 536,12416, + 544,2176, 552,10368, 560,6272, 568,14464, 576,1152, 584,9344, 592,5248, + 600,13440, 608,3200, 616,11392, 624,7296, 632,15488, 648,8832, 656,4736, + 664,12928, 672,2688, 680,10880, 688,6784, 696,14976, 704,1664, 712,9856, + 720,5760, 728,13952, 736,3712, 744,11904, 752,7808, 760,16000, 776,8576, + 784,4480, 792,12672, 800,2432, 808,10624, 816,6528, 824,14720, 832,1408, + 840,9600, 848,5504, 856,13696, 864,3456, 872,11648, 880,7552, 888,15744, + 904,9088, 912,4992, 920,13184, 928,2944, 936,11136, 944,7040, 952,15232, + 960,1920, 968,10112, 976,6016, 984,14208, 992,3968, 1000,12160, 1008,8064, + 1016,16256, 1032,8256, 1040,4160, 1048,12352, 1056,2112, 1064,10304, 1072,6208, + 1080,14400, 1096,9280, 1104,5184, 1112,13376, 1120,3136, 1128,11328, 1136,7232, + 1144,15424, 1160,8768, 1168,4672, 1176,12864, 1184,2624, 1192,10816, 1200,6720, + 1208,14912, 1216,1600, 1224,9792, 1232,5696, 1240,13888, 1248,3648, 1256,11840, + 1264,7744, 1272,15936, 1288,8512, 1296,4416, 1304,12608, 1312,2368, 1320,10560, + 1328,6464, 1336,14656, 1352,9536, 1360,5440, 1368,13632, 1376,3392, 1384,11584, + 1392,7488, 1400,15680, 1416,9024, 1424,4928, 1432,13120, 1440,2880, 1448,11072, + 1456,6976, 1464,15168, 1472,1856, 1480,10048, 1488,5952, 1496,14144, 1504,3904, + 1512,12096, 1520,8000, 1528,16192, 1544,8384, 1552,4288, 1560,12480, 1568,2240, + 1576,10432, 1584,6336, 1592,14528, 1608,9408, 1616,5312, 1624,13504, 1632,3264, + 1640,11456, 1648,7360, 1656,15552, 1672,8896, 1680,4800, 1688,12992, 1696,2752, + 1704,10944, 1712,6848, 1720,15040, 1736,9920, 1744,5824, 1752,14016, 1760,3776, + 1768,11968, 1776,7872, 1784,16064, 1800,8640, 1808,4544, 1816,12736, 1824,2496, + 1832,10688, 1840,6592, 1848,14784, 1864,9664, 1872,5568, 1880,13760, 1888,3520, + 1896,11712, 1904,7616, 1912,15808, 1928,9152, 1936,5056, 1944,13248, 1952,3008, + 1960,11200, 1968,7104, 1976,15296, 1992,10176, 2000,6080, 2008,14272, 2016,4032, + 2024,12224, 2032,8128, 2040,16320, 2056,8224, 2064,4128, 2072,12320, 2088,10272, + 2096,6176, 2104,14368, 2120,9248, 2128,5152, 2136,13344, 2144,3104, 2152,11296, + 2160,7200, 2168,15392, 2184,8736, 2192,4640, 2200,12832, 2208,2592, 2216,10784, + 2224,6688, 2232,14880, 2248,9760, 2256,5664, 2264,13856, 2272,3616, 2280,11808, + 2288,7712, 2296,15904, 2312,8480, 2320,4384, 2328,12576, 2344,10528, 2352,6432, + 2360,14624, 2376,9504, 2384,5408, 2392,13600, 2400,3360, 2408,11552, 2416,7456, + 2424,15648, 2440,8992, 2448,4896, 2456,13088, 2464,2848, 2472,11040, 2480,6944, + 2488,15136, 2504,10016, 2512,5920, 2520,14112, 2528,3872, 2536,12064, 2544,7968, + 2552,16160, 2568,8352, 2576,4256, 2584,12448, 2600,10400, 2608,6304, 2616,14496, + 2632,9376, 2640,5280, 2648,13472, 2656,3232, 2664,11424, 2672,7328, 2680,15520, + 2696,8864, 2704,4768, 2712,12960, 2728,10912, 2736,6816, 2744,15008, 2760,9888, + 2768,5792, 2776,13984, 2784,3744, 2792,11936, 2800,7840, 2808,16032, 2824,8608, + 2832,4512, 2840,12704, 2856,10656, 2864,6560, 2872,14752, 2888,9632, 2896,5536, + 2904,13728, 2912,3488, 2920,11680, 2928,7584, 2936,15776, 2952,9120, 2960,5024, + 2968,13216, 2984,11168, 2992,7072, 3000,15264, 3016,10144, 3024,6048, + 3032,14240, 3040,4000, 3048,12192, 3056,8096, 3064,16288, 3080,8288, 3088,4192, + 3096,12384, 3112,10336, 3120,6240, 3128,14432, 3144,9312, 3152,5216, 3160,13408, + 3176,11360, 3184,7264, 3192,15456, 3208,8800, 3216,4704, 3224,12896, 3240,10848, + 3248,6752, 3256,14944, 3272,9824, 3280,5728, 3288,13920, 3296,3680, 3304,11872, + 3312,7776, 3320,15968, 3336,8544, 3344,4448, 3352,12640, 3368,10592, 3376,6496, + 3384,14688, 3400,9568, 3408,5472, 3416,13664, 3432,11616, 3440,7520, 3448,15712, + 3464,9056, 3472,4960, 3480,13152, 3496,11104, 3504,7008, 3512,15200, 3528,10080, + 3536,5984, 3544,14176, 3552,3936, 3560,12128, 3568,8032, 3576,16224, 3592,8416, + 3600,4320, 3608,12512, 3624,10464, 3632,6368, 3640,14560, 3656,9440, 3664,5344, + 3672,13536, 3688,11488, 3696,7392, 3704,15584, 3720,8928, 3728,4832, 3736,13024, + 3752,10976, 3760,6880, 3768,15072, 3784,9952, 3792,5856, 3800,14048, 3816,12000, + 3824,7904, 3832,16096, 3848,8672, 3856,4576, 3864,12768, 3880,10720, 3888,6624, + 3896,14816, 3912,9696, 3920,5600, 3928,13792, 3944,11744, 3952,7648, 3960,15840, + 3976,9184, 3984,5088, 3992,13280, 4008,11232, 4016,7136, 4024,15328, 4040,10208, + 4048,6112, 4056,14304, 4072,12256, 4080,8160, 4088,16352, 4104,8208, 4120,12304, + 4136,10256, 4144,6160, 4152,14352, 4168,9232, 4176,5136, 4184,13328, 4200,11280, + 4208,7184, 4216,15376, 4232,8720, 4240,4624, 4248,12816, 4264,10768, 4272,6672, + 4280,14864, 4296,9744, 4304,5648, 4312,13840, 4328,11792, 4336,7696, 4344,15888, + 4360,8464, 4376,12560, 4392,10512, 4400,6416, 4408,14608, 4424,9488, 4432,5392, + 4440,13584, 4456,11536, 4464,7440, 4472,15632, 4488,8976, 4496,4880, 4504,13072, + 4520,11024, 4528,6928, 4536,15120, 4552,10000, 4560,5904, 4568,14096, + 4584,12048, 4592,7952, 4600,16144, 4616,8336, 4632,12432, 4648,10384, 4656,6288, + 4664,14480, 4680,9360, 4688,5264, 4696,13456, 4712,11408, 4720,7312, 4728,15504, + 4744,8848, 4760,12944, 4776,10896, 4784,6800, 4792,14992, 4808,9872, 4816,5776, + 4824,13968, 4840,11920, 4848,7824, 4856,16016, 4872,8592, 4888,12688, + 4904,10640, 4912,6544, 4920,14736, 4936,9616, 4944,5520, 4952,13712, 4968,11664, + 4976,7568, 4984,15760, 5000,9104, 5016,13200, 5032,11152, 5040,7056, 5048,15248, + 5064,10128, 5072,6032, 5080,14224, 5096,12176, 5104,8080, 5112,16272, 5128,8272, + 5144,12368, 5160,10320, 5168,6224, 5176,14416, 5192,9296, 5208,13392, + 5224,11344, 5232,7248, 5240,15440, 5256,8784, 5272,12880, 5288,10832, 5296,6736, + 5304,14928, 5320,9808, 5328,5712, 5336,13904, 5352,11856, 5360,7760, 5368,15952, + 5384,8528, 5400,12624, 5416,10576, 5424,6480, 5432,14672, 5448,9552, 5464,13648, + 5480,11600, 5488,7504, 5496,15696, 5512,9040, 5528,13136, 5544,11088, 5552,6992, + 5560,15184, 5576,10064, 5584,5968, 5592,14160, 5608,12112, 5616,8016, + 5624,16208, 5640,8400, 5656,12496, 5672,10448, 5680,6352, 5688,14544, 5704,9424, + 5720,13520, 5736,11472, 5744,7376, 5752,15568, 5768,8912, 5784,13008, + 5800,10960, 5808,6864, 5816,15056, 5832,9936, 5848,14032, 5864,11984, 5872,7888, + 5880,16080, 5896,8656, 5912,12752, 5928,10704, 5936,6608, 5944,14800, 5960,9680, + 5976,13776, 5992,11728, 6000,7632, 6008,15824, 6024,9168, 6040,13264, + 6056,11216, 6064,7120, 6072,15312, 6088,10192, 6104,14288, 6120,12240, + 6128,8144, 6136,16336, 6152,8240, 6168,12336, 6184,10288, 6200,14384, 6216,9264, + 6232,13360, 6248,11312, 6256,7216, 6264,15408, 6280,8752, 6296,12848, + 6312,10800, 6320,6704, 6328,14896, 6344,9776, 6360,13872, 6376,11824, 6384,7728, + 6392,15920, 6408,8496, 6424,12592, 6440,10544, 6456,14640, 6472,9520, + 6488,13616, 6504,11568, 6512,7472, 6520,15664, 6536,9008, 6552,13104, + 6568,11056, 6576,6960, 6584,15152, 6600,10032, 6616,14128, 6632,12080, + 6640,7984, 6648,16176, 6664,8368, 6680,12464, 6696,10416, 6712,14512, 6728,9392, + 6744,13488, 6760,11440, 6768,7344, 6776,15536, 6792,8880, 6808,12976, + 6824,10928, 6840,15024, 6856,9904, 6872,14000, 6888,11952, 6896,7856, + 6904,16048, 6920,8624, 6936,12720, 6952,10672, 6968,14768, 6984,9648, + 7000,13744, 7016,11696, 7024,7600, 7032,15792, 7048,9136, 7064,13232, + 7080,11184, 7096,15280, 7112,10160, 7128,14256, 7144,12208, 7152,8112, + 7160,16304, 7176,8304, 7192,12400, 7208,10352, 7224,14448, 7240,9328, + 7256,13424, 7272,11376, 7288,15472, 7304,8816, 7320,12912, 7336,10864, + 7352,14960, 7368,9840, 7384,13936, 7400,11888, 7408,7792, 7416,15984, 7432,8560, + 7448,12656, 7464,10608, 7480,14704, 7496,9584, 7512,13680, 7528,11632, + 7544,15728, 7560,9072, 7576,13168, 7592,11120, 7608,15216, 7624,10096, + 7640,14192, 7656,12144, 7664,8048, 7672,16240, 7688,8432, 7704,12528, + 7720,10480, 7736,14576, 7752,9456, 7768,13552, 7784,11504, 7800,15600, + 7816,8944, 7832,13040, 7848,10992, 7864,15088, 7880,9968, 7896,14064, + 7912,12016, 7928,16112, 7944,8688, 7960,12784, 7976,10736, 7992,14832, + 8008,9712, 8024,13808, 8040,11760, 8056,15856, 8072,9200, 8088,13296, + 8104,11248, 8120,15344, 8136,10224, 8152,14320, 8168,12272, 8184,16368, + 8216,12296, 8232,10248, 8248,14344, 8264,9224, 8280,13320, 8296,11272, + 8312,15368, 8328,8712, 8344,12808, 8360,10760, 8376,14856, 8392,9736, + 8408,13832, 8424,11784, 8440,15880, 8472,12552, 8488,10504, 8504,14600, + 8520,9480, 8536,13576, 8552,11528, 8568,15624, 8584,8968, 8600,13064, + 8616,11016, 8632,15112, 8648,9992, 8664,14088, 8680,12040, 8696,16136, + 8728,12424, 8744,10376, 8760,14472, 8776,9352, 8792,13448, 8808,11400, + 8824,15496, 8856,12936, 8872,10888, 8888,14984, 8904,9864, 8920,13960, + 8936,11912, 8952,16008, 8984,12680, 9000,10632, 9016,14728, 9032,9608, + 9048,13704, 9064,11656, 9080,15752, 9112,13192, 9128,11144, 9144,15240, + 9160,10120, 9176,14216, 9192,12168, 9208,16264, 9240,12360, 9256,10312, + 9272,14408, 9304,13384, 9320,11336, 9336,15432, 9368,12872, 9384,10824, + 9400,14920, 9416,9800, 9432,13896, 9448,11848, 9464,15944, 9496,12616, + 9512,10568, 9528,14664, 9560,13640, 9576,11592, 9592,15688, 9624,13128, + 9640,11080, 9656,15176, 9672,10056, 9688,14152, 9704,12104, 9720,16200, + 9752,12488, 9768,10440, 9784,14536, 9816,13512, 9832,11464, 9848,15560, + 9880,13000, 9896,10952, 9912,15048, 9944,14024, 9960,11976, 9976,16072, + 10008,12744, 10024,10696, 10040,14792, 10072,13768, 10088,11720, 10104,15816, + 10136,13256, 10152,11208, 10168,15304, 10200,14280, 10216,12232, 10232,16328, + 10264,12328, 10296,14376, 10328,13352, 10344,11304, 10360,15400, 10392,12840, + 10408,10792, 10424,14888, 10456,13864, 10472,11816, 10488,15912, 10520,12584, + 10552,14632, 10584,13608, 10600,11560, 10616,15656, 10648,13096, 10664,11048, + 10680,15144, 10712,14120, 10728,12072, 10744,16168, 10776,12456, 10808,14504, + 10840,13480, 10856,11432, 10872,15528, 10904,12968, 10936,15016, 10968,13992, + 10984,11944, 11000,16040, 11032,12712, 11064,14760, 11096,13736, 11112,11688, + 11128,15784, 11160,13224, 11192,15272, 11224,14248, 11240,12200, 11256,16296, + 11288,12392, 11320,14440, 11352,13416, 11384,15464, 11416,12904, 11448,14952, + 11480,13928, 11496,11880, 11512,15976, 11544,12648, 11576,14696, 11608,13672, + 11640,15720, 11672,13160, 11704,15208, 11736,14184, 11752,12136, 11768,16232, + 11800,12520, 11832,14568, 11864,13544, 11896,15592, 11928,13032, 11960,15080, + 11992,14056, 12024,16104, 12056,12776, 12088,14824, 12120,13800, 12152,15848, + 12184,13288, 12216,15336, 12248,14312, 12280,16360, 12344,14360, 12376,13336, + 12408,15384, 12440,12824, 12472,14872, 12504,13848, 12536,15896, 12600,14616, + 12632,13592, 12664,15640, 12696,13080, 12728,15128, 12760,14104, 12792,16152, + 12856,14488, 12888,13464, 12920,15512, 12984,15000, 13016,13976, 13048,16024, + 13112,14744, 13144,13720, 13176,15768, 13240,15256, 13272,14232, 13304,16280, + 13368,14424, 13432,15448, 13496,14936, 13528,13912, 13560,15960, 13624,14680, + 13688,15704, 13752,15192, 13784,14168, 13816,16216, 13880,14552, 13944,15576, + 14008,15064, 14072,16088, 14136,14808, 14200,15832, 14264,15320, 14328,16344, + 14456,15416, 14520,14904, 14584,15928, 14712,15672, 14776,15160, 14840,16184, + 14968,15544, 15096,16056, 15224,15800, 15352,16312, 15608,15992, 15864,16248 +}; + +const uint16_t armBitRevIndexTable_fixed_4096[ARMBITREVINDEXTABLE_FIXED_4096_TABLE_LENGTH] = +{ + //radix 4, size 4032 + 8,16384, 16,8192, 24,24576, 32,4096, 40,20480, 48,12288, 56,28672, 64,2048, + 72,18432, 80,10240, 88,26624, 96,6144, 104,22528, 112,14336, 120,30720, + 128,1024, 136,17408, 144,9216, 152,25600, 160,5120, 168,21504, 176,13312, + 184,29696, 192,3072, 200,19456, 208,11264, 216,27648, 224,7168, 232,23552, + 240,15360, 248,31744, 256,512, 264,16896, 272,8704, 280,25088, 288,4608, + 296,20992, 304,12800, 312,29184, 320,2560, 328,18944, 336,10752, 344,27136, + 352,6656, 360,23040, 368,14848, 376,31232, 384,1536, 392,17920, 400,9728, + 408,26112, 416,5632, 424,22016, 432,13824, 440,30208, 448,3584, 456,19968, + 464,11776, 472,28160, 480,7680, 488,24064, 496,15872, 504,32256, 520,16640, + 528,8448, 536,24832, 544,4352, 552,20736, 560,12544, 568,28928, 576,2304, + 584,18688, 592,10496, 600,26880, 608,6400, 616,22784, 624,14592, 632,30976, + 640,1280, 648,17664, 656,9472, 664,25856, 672,5376, 680,21760, 688,13568, + 696,29952, 704,3328, 712,19712, 720,11520, 728,27904, 736,7424, 744,23808, + 752,15616, 760,32000, 776,17152, 784,8960, 792,25344, 800,4864, 808,21248, + 816,13056, 824,29440, 832,2816, 840,19200, 848,11008, 856,27392, 864,6912, + 872,23296, 880,15104, 888,31488, 896,1792, 904,18176, 912,9984, 920,26368, + 928,5888, 936,22272, 944,14080, 952,30464, 960,3840, 968,20224, 976,12032, + 984,28416, 992,7936, 1000,24320, 1008,16128, 1016,32512, 1032,16512, 1040,8320, + 1048,24704, 1056,4224, 1064,20608, 1072,12416, 1080,28800, 1088,2176, + 1096,18560, 1104,10368, 1112,26752, 1120,6272, 1128,22656, 1136,14464, + 1144,30848, 1160,17536, 1168,9344, 1176,25728, 1184,5248, 1192,21632, + 1200,13440, 1208,29824, 1216,3200, 1224,19584, 1232,11392, 1240,27776, + 1248,7296, 1256,23680, 1264,15488, 1272,31872, 1288,17024, 1296,8832, + 1304,25216, 1312,4736, 1320,21120, 1328,12928, 1336,29312, 1344,2688, + 1352,19072, 1360,10880, 1368,27264, 1376,6784, 1384,23168, 1392,14976, + 1400,31360, 1408,1664, 1416,18048, 1424,9856, 1432,26240, 1440,5760, 1448,22144, + 1456,13952, 1464,30336, 1472,3712, 1480,20096, 1488,11904, 1496,28288, + 1504,7808, 1512,24192, 1520,16000, 1528,32384, 1544,16768, 1552,8576, + 1560,24960, 1568,4480, 1576,20864, 1584,12672, 1592,29056, 1600,2432, + 1608,18816, 1616,10624, 1624,27008, 1632,6528, 1640,22912, 1648,14720, + 1656,31104, 1672,17792, 1680,9600, 1688,25984, 1696,5504, 1704,21888, + 1712,13696, 1720,30080, 1728,3456, 1736,19840, 1744,11648, 1752,28032, + 1760,7552, 1768,23936, 1776,15744, 1784,32128, 1800,17280, 1808,9088, + 1816,25472, 1824,4992, 1832,21376, 1840,13184, 1848,29568, 1856,2944, + 1864,19328, 1872,11136, 1880,27520, 1888,7040, 1896,23424, 1904,15232, + 1912,31616, 1928,18304, 1936,10112, 1944,26496, 1952,6016, 1960,22400, + 1968,14208, 1976,30592, 1984,3968, 1992,20352, 2000,12160, 2008,28544, + 2016,8064, 2024,24448, 2032,16256, 2040,32640, 2056,16448, 2064,8256, + 2072,24640, 2080,4160, 2088,20544, 2096,12352, 2104,28736, 2120,18496, + 2128,10304, 2136,26688, 2144,6208, 2152,22592, 2160,14400, 2168,30784, + 2184,17472, 2192,9280, 2200,25664, 2208,5184, 2216,21568, 2224,13376, + 2232,29760, 2240,3136, 2248,19520, 2256,11328, 2264,27712, 2272,7232, + 2280,23616, 2288,15424, 2296,31808, 2312,16960, 2320,8768, 2328,25152, + 2336,4672, 2344,21056, 2352,12864, 2360,29248, 2368,2624, 2376,19008, + 2384,10816, 2392,27200, 2400,6720, 2408,23104, 2416,14912, 2424,31296, + 2440,17984, 2448,9792, 2456,26176, 2464,5696, 2472,22080, 2480,13888, + 2488,30272, 2496,3648, 2504,20032, 2512,11840, 2520,28224, 2528,7744, + 2536,24128, 2544,15936, 2552,32320, 2568,16704, 2576,8512, 2584,24896, + 2592,4416, 2600,20800, 2608,12608, 2616,28992, 2632,18752, 2640,10560, + 2648,26944, 2656,6464, 2664,22848, 2672,14656, 2680,31040, 2696,17728, + 2704,9536, 2712,25920, 2720,5440, 2728,21824, 2736,13632, 2744,30016, 2752,3392, + 2760,19776, 2768,11584, 2776,27968, 2784,7488, 2792,23872, 2800,15680, + 2808,32064, 2824,17216, 2832,9024, 2840,25408, 2848,4928, 2856,21312, + 2864,13120, 2872,29504, 2888,19264, 2896,11072, 2904,27456, 2912,6976, + 2920,23360, 2928,15168, 2936,31552, 2952,18240, 2960,10048, 2968,26432, + 2976,5952, 2984,22336, 2992,14144, 3000,30528, 3008,3904, 3016,20288, + 3024,12096, 3032,28480, 3040,8000, 3048,24384, 3056,16192, 3064,32576, + 3080,16576, 3088,8384, 3096,24768, 3104,4288, 3112,20672, 3120,12480, + 3128,28864, 3144,18624, 3152,10432, 3160,26816, 3168,6336, 3176,22720, + 3184,14528, 3192,30912, 3208,17600, 3216,9408, 3224,25792, 3232,5312, + 3240,21696, 3248,13504, 3256,29888, 3272,19648, 3280,11456, 3288,27840, + 3296,7360, 3304,23744, 3312,15552, 3320,31936, 3336,17088, 3344,8896, + 3352,25280, 3360,4800, 3368,21184, 3376,12992, 3384,29376, 3400,19136, + 3408,10944, 3416,27328, 3424,6848, 3432,23232, 3440,15040, 3448,31424, + 3464,18112, 3472,9920, 3480,26304, 3488,5824, 3496,22208, 3504,14016, + 3512,30400, 3520,3776, 3528,20160, 3536,11968, 3544,28352, 3552,7872, + 3560,24256, 3568,16064, 3576,32448, 3592,16832, 3600,8640, 3608,25024, + 3616,4544, 3624,20928, 3632,12736, 3640,29120, 3656,18880, 3664,10688, + 3672,27072, 3680,6592, 3688,22976, 3696,14784, 3704,31168, 3720,17856, + 3728,9664, 3736,26048, 3744,5568, 3752,21952, 3760,13760, 3768,30144, + 3784,19904, 3792,11712, 3800,28096, 3808,7616, 3816,24000, 3824,15808, + 3832,32192, 3848,17344, 3856,9152, 3864,25536, 3872,5056, 3880,21440, + 3888,13248, 3896,29632, 3912,19392, 3920,11200, 3928,27584, 3936,7104, + 3944,23488, 3952,15296, 3960,31680, 3976,18368, 3984,10176, 3992,26560, + 4000,6080, 4008,22464, 4016,14272, 4024,30656, 4040,20416, 4048,12224, + 4056,28608, 4064,8128, 4072,24512, 4080,16320, 4088,32704, 4104,16416, + 4112,8224, 4120,24608, 4136,20512, 4144,12320, 4152,28704, 4168,18464, + 4176,10272, 4184,26656, 4192,6176, 4200,22560, 4208,14368, 4216,30752, + 4232,17440, 4240,9248, 4248,25632, 4256,5152, 4264,21536, 4272,13344, + 4280,29728, 4296,19488, 4304,11296, 4312,27680, 4320,7200, 4328,23584, + 4336,15392, 4344,31776, 4360,16928, 4368,8736, 4376,25120, 4384,4640, + 4392,21024, 4400,12832, 4408,29216, 4424,18976, 4432,10784, 4440,27168, + 4448,6688, 4456,23072, 4464,14880, 4472,31264, 4488,17952, 4496,9760, + 4504,26144, 4512,5664, 4520,22048, 4528,13856, 4536,30240, 4552,20000, + 4560,11808, 4568,28192, 4576,7712, 4584,24096, 4592,15904, 4600,32288, + 4616,16672, 4624,8480, 4632,24864, 4648,20768, 4656,12576, 4664,28960, + 4680,18720, 4688,10528, 4696,26912, 4704,6432, 4712,22816, 4720,14624, + 4728,31008, 4744,17696, 4752,9504, 4760,25888, 4768,5408, 4776,21792, + 4784,13600, 4792,29984, 4808,19744, 4816,11552, 4824,27936, 4832,7456, + 4840,23840, 4848,15648, 4856,32032, 4872,17184, 4880,8992, 4888,25376, + 4904,21280, 4912,13088, 4920,29472, 4936,19232, 4944,11040, 4952,27424, + 4960,6944, 4968,23328, 4976,15136, 4984,31520, 5000,18208, 5008,10016, + 5016,26400, 5024,5920, 5032,22304, 5040,14112, 5048,30496, 5064,20256, + 5072,12064, 5080,28448, 5088,7968, 5096,24352, 5104,16160, 5112,32544, + 5128,16544, 5136,8352, 5144,24736, 5160,20640, 5168,12448, 5176,28832, + 5192,18592, 5200,10400, 5208,26784, 5216,6304, 5224,22688, 5232,14496, + 5240,30880, 5256,17568, 5264,9376, 5272,25760, 5288,21664, 5296,13472, + 5304,29856, 5320,19616, 5328,11424, 5336,27808, 5344,7328, 5352,23712, + 5360,15520, 5368,31904, 5384,17056, 5392,8864, 5400,25248, 5416,21152, + 5424,12960, 5432,29344, 5448,19104, 5456,10912, 5464,27296, 5472,6816, + 5480,23200, 5488,15008, 5496,31392, 5512,18080, 5520,9888, 5528,26272, + 5536,5792, 5544,22176, 5552,13984, 5560,30368, 5576,20128, 5584,11936, + 5592,28320, 5600,7840, 5608,24224, 5616,16032, 5624,32416, 5640,16800, + 5648,8608, 5656,24992, 5672,20896, 5680,12704, 5688,29088, 5704,18848, + 5712,10656, 5720,27040, 5728,6560, 5736,22944, 5744,14752, 5752,31136, + 5768,17824, 5776,9632, 5784,26016, 5800,21920, 5808,13728, 5816,30112, + 5832,19872, 5840,11680, 5848,28064, 5856,7584, 5864,23968, 5872,15776, + 5880,32160, 5896,17312, 5904,9120, 5912,25504, 5928,21408, 5936,13216, + 5944,29600, 5960,19360, 5968,11168, 5976,27552, 5984,7072, 5992,23456, + 6000,15264, 6008,31648, 6024,18336, 6032,10144, 6040,26528, 6056,22432, + 6064,14240, 6072,30624, 6088,20384, 6096,12192, 6104,28576, 6112,8096, + 6120,24480, 6128,16288, 6136,32672, 6152,16480, 6160,8288, 6168,24672, + 6184,20576, 6192,12384, 6200,28768, 6216,18528, 6224,10336, 6232,26720, + 6248,22624, 6256,14432, 6264,30816, 6280,17504, 6288,9312, 6296,25696, + 6312,21600, 6320,13408, 6328,29792, 6344,19552, 6352,11360, 6360,27744, + 6368,7264, 6376,23648, 6384,15456, 6392,31840, 6408,16992, 6416,8800, + 6424,25184, 6440,21088, 6448,12896, 6456,29280, 6472,19040, 6480,10848, + 6488,27232, 6496,6752, 6504,23136, 6512,14944, 6520,31328, 6536,18016, + 6544,9824, 6552,26208, 6568,22112, 6576,13920, 6584,30304, 6600,20064, + 6608,11872, 6616,28256, 6624,7776, 6632,24160, 6640,15968, 6648,32352, + 6664,16736, 6672,8544, 6680,24928, 6696,20832, 6704,12640, 6712,29024, + 6728,18784, 6736,10592, 6744,26976, 6760,22880, 6768,14688, 6776,31072, + 6792,17760, 6800,9568, 6808,25952, 6824,21856, 6832,13664, 6840,30048, + 6856,19808, 6864,11616, 6872,28000, 6880,7520, 6888,23904, 6896,15712, + 6904,32096, 6920,17248, 6928,9056, 6936,25440, 6952,21344, 6960,13152, + 6968,29536, 6984,19296, 6992,11104, 7000,27488, 7016,23392, 7024,15200, + 7032,31584, 7048,18272, 7056,10080, 7064,26464, 7080,22368, 7088,14176, + 7096,30560, 7112,20320, 7120,12128, 7128,28512, 7136,8032, 7144,24416, + 7152,16224, 7160,32608, 7176,16608, 7184,8416, 7192,24800, 7208,20704, + 7216,12512, 7224,28896, 7240,18656, 7248,10464, 7256,26848, 7272,22752, + 7280,14560, 7288,30944, 7304,17632, 7312,9440, 7320,25824, 7336,21728, + 7344,13536, 7352,29920, 7368,19680, 7376,11488, 7384,27872, 7400,23776, + 7408,15584, 7416,31968, 7432,17120, 7440,8928, 7448,25312, 7464,21216, + 7472,13024, 7480,29408, 7496,19168, 7504,10976, 7512,27360, 7528,23264, + 7536,15072, 7544,31456, 7560,18144, 7568,9952, 7576,26336, 7592,22240, + 7600,14048, 7608,30432, 7624,20192, 7632,12000, 7640,28384, 7648,7904, + 7656,24288, 7664,16096, 7672,32480, 7688,16864, 7696,8672, 7704,25056, + 7720,20960, 7728,12768, 7736,29152, 7752,18912, 7760,10720, 7768,27104, + 7784,23008, 7792,14816, 7800,31200, 7816,17888, 7824,9696, 7832,26080, + 7848,21984, 7856,13792, 7864,30176, 7880,19936, 7888,11744, 7896,28128, + 7912,24032, 7920,15840, 7928,32224, 7944,17376, 7952,9184, 7960,25568, + 7976,21472, 7984,13280, 7992,29664, 8008,19424, 8016,11232, 8024,27616, + 8040,23520, 8048,15328, 8056,31712, 8072,18400, 8080,10208, 8088,26592, + 8104,22496, 8112,14304, 8120,30688, 8136,20448, 8144,12256, 8152,28640, + 8168,24544, 8176,16352, 8184,32736, 8200,16400, 8216,24592, 8232,20496, + 8240,12304, 8248,28688, 8264,18448, 8272,10256, 8280,26640, 8296,22544, + 8304,14352, 8312,30736, 8328,17424, 8336,9232, 8344,25616, 8360,21520, + 8368,13328, 8376,29712, 8392,19472, 8400,11280, 8408,27664, 8424,23568, + 8432,15376, 8440,31760, 8456,16912, 8464,8720, 8472,25104, 8488,21008, + 8496,12816, 8504,29200, 8520,18960, 8528,10768, 8536,27152, 8552,23056, + 8560,14864, 8568,31248, 8584,17936, 8592,9744, 8600,26128, 8616,22032, + 8624,13840, 8632,30224, 8648,19984, 8656,11792, 8664,28176, 8680,24080, + 8688,15888, 8696,32272, 8712,16656, 8728,24848, 8744,20752, 8752,12560, + 8760,28944, 8776,18704, 8784,10512, 8792,26896, 8808,22800, 8816,14608, + 8824,30992, 8840,17680, 8848,9488, 8856,25872, 8872,21776, 8880,13584, + 8888,29968, 8904,19728, 8912,11536, 8920,27920, 8936,23824, 8944,15632, + 8952,32016, 8968,17168, 8984,25360, 9000,21264, 9008,13072, 9016,29456, + 9032,19216, 9040,11024, 9048,27408, 9064,23312, 9072,15120, 9080,31504, + 9096,18192, 9104,10000, 9112,26384, 9128,22288, 9136,14096, 9144,30480, + 9160,20240, 9168,12048, 9176,28432, 9192,24336, 9200,16144, 9208,32528, + 9224,16528, 9240,24720, 9256,20624, 9264,12432, 9272,28816, 9288,18576, + 9296,10384, 9304,26768, 9320,22672, 9328,14480, 9336,30864, 9352,17552, + 9368,25744, 9384,21648, 9392,13456, 9400,29840, 9416,19600, 9424,11408, + 9432,27792, 9448,23696, 9456,15504, 9464,31888, 9480,17040, 9496,25232, + 9512,21136, 9520,12944, 9528,29328, 9544,19088, 9552,10896, 9560,27280, + 9576,23184, 9584,14992, 9592,31376, 9608,18064, 9616,9872, 9624,26256, + 9640,22160, 9648,13968, 9656,30352, 9672,20112, 9680,11920, 9688,28304, + 9704,24208, 9712,16016, 9720,32400, 9736,16784, 9752,24976, 9768,20880, + 9776,12688, 9784,29072, 9800,18832, 9808,10640, 9816,27024, 9832,22928, + 9840,14736, 9848,31120, 9864,17808, 9880,26000, 9896,21904, 9904,13712, + 9912,30096, 9928,19856, 9936,11664, 9944,28048, 9960,23952, 9968,15760, + 9976,32144, 9992,17296, 10008,25488, 10024,21392, 10032,13200, 10040,29584, + 10056,19344, 10064,11152, 10072,27536, 10088,23440, 10096,15248, 10104,31632, + 10120,18320, 10136,26512, 10152,22416, 10160,14224, 10168,30608, 10184,20368, + 10192,12176, 10200,28560, 10216,24464, 10224,16272, 10232,32656, 10248,16464, + 10264,24656, 10280,20560, 10288,12368, 10296,28752, 10312,18512, 10328,26704, + 10344,22608, 10352,14416, 10360,30800, 10376,17488, 10392,25680, 10408,21584, + 10416,13392, 10424,29776, 10440,19536, 10448,11344, 10456,27728, 10472,23632, + 10480,15440, 10488,31824, 10504,16976, 10520,25168, 10536,21072, 10544,12880, + 10552,29264, 10568,19024, 10576,10832, 10584,27216, 10600,23120, 10608,14928, + 10616,31312, 10632,18000, 10648,26192, 10664,22096, 10672,13904, 10680,30288, + 10696,20048, 10704,11856, 10712,28240, 10728,24144, 10736,15952, 10744,32336, + 10760,16720, 10776,24912, 10792,20816, 10800,12624, 10808,29008, 10824,18768, + 10840,26960, 10856,22864, 10864,14672, 10872,31056, 10888,17744, 10904,25936, + 10920,21840, 10928,13648, 10936,30032, 10952,19792, 10960,11600, 10968,27984, + 10984,23888, 10992,15696, 11000,32080, 11016,17232, 11032,25424, 11048,21328, + 11056,13136, 11064,29520, 11080,19280, 11096,27472, 11112,23376, 11120,15184, + 11128,31568, 11144,18256, 11160,26448, 11176,22352, 11184,14160, 11192,30544, + 11208,20304, 11216,12112, 11224,28496, 11240,24400, 11248,16208, 11256,32592, + 11272,16592, 11288,24784, 11304,20688, 11312,12496, 11320,28880, 11336,18640, + 11352,26832, 11368,22736, 11376,14544, 11384,30928, 11400,17616, 11416,25808, + 11432,21712, 11440,13520, 11448,29904, 11464,19664, 11480,27856, 11496,23760, + 11504,15568, 11512,31952, 11528,17104, 11544,25296, 11560,21200, 11568,13008, + 11576,29392, 11592,19152, 11608,27344, 11624,23248, 11632,15056, 11640,31440, + 11656,18128, 11672,26320, 11688,22224, 11696,14032, 11704,30416, 11720,20176, + 11728,11984, 11736,28368, 11752,24272, 11760,16080, 11768,32464, 11784,16848, + 11800,25040, 11816,20944, 11824,12752, 11832,29136, 11848,18896, 11864,27088, + 11880,22992, 11888,14800, 11896,31184, 11912,17872, 11928,26064, 11944,21968, + 11952,13776, 11960,30160, 11976,19920, 11992,28112, 12008,24016, 12016,15824, + 12024,32208, 12040,17360, 12056,25552, 12072,21456, 12080,13264, 12088,29648, + 12104,19408, 12120,27600, 12136,23504, 12144,15312, 12152,31696, 12168,18384, + 12184,26576, 12200,22480, 12208,14288, 12216,30672, 12232,20432, 12248,28624, + 12264,24528, 12272,16336, 12280,32720, 12296,16432, 12312,24624, 12328,20528, + 12344,28720, 12360,18480, 12376,26672, 12392,22576, 12400,14384, 12408,30768, + 12424,17456, 12440,25648, 12456,21552, 12464,13360, 12472,29744, 12488,19504, + 12504,27696, 12520,23600, 12528,15408, 12536,31792, 12552,16944, 12568,25136, + 12584,21040, 12592,12848, 12600,29232, 12616,18992, 12632,27184, 12648,23088, + 12656,14896, 12664,31280, 12680,17968, 12696,26160, 12712,22064, 12720,13872, + 12728,30256, 12744,20016, 12760,28208, 12776,24112, 12784,15920, 12792,32304, + 12808,16688, 12824,24880, 12840,20784, 12856,28976, 12872,18736, 12888,26928, + 12904,22832, 12912,14640, 12920,31024, 12936,17712, 12952,25904, 12968,21808, + 12976,13616, 12984,30000, 13000,19760, 13016,27952, 13032,23856, 13040,15664, + 13048,32048, 13064,17200, 13080,25392, 13096,21296, 13112,29488, 13128,19248, + 13144,27440, 13160,23344, 13168,15152, 13176,31536, 13192,18224, 13208,26416, + 13224,22320, 13232,14128, 13240,30512, 13256,20272, 13272,28464, 13288,24368, + 13296,16176, 13304,32560, 13320,16560, 13336,24752, 13352,20656, 13368,28848, + 13384,18608, 13400,26800, 13416,22704, 13424,14512, 13432,30896, 13448,17584, + 13464,25776, 13480,21680, 13496,29872, 13512,19632, 13528,27824, 13544,23728, + 13552,15536, 13560,31920, 13576,17072, 13592,25264, 13608,21168, 13624,29360, + 13640,19120, 13656,27312, 13672,23216, 13680,15024, 13688,31408, 13704,18096, + 13720,26288, 13736,22192, 13744,14000, 13752,30384, 13768,20144, 13784,28336, + 13800,24240, 13808,16048, 13816,32432, 13832,16816, 13848,25008, 13864,20912, + 13880,29104, 13896,18864, 13912,27056, 13928,22960, 13936,14768, 13944,31152, + 13960,17840, 13976,26032, 13992,21936, 14008,30128, 14024,19888, 14040,28080, + 14056,23984, 14064,15792, 14072,32176, 14088,17328, 14104,25520, 14120,21424, + 14136,29616, 14152,19376, 14168,27568, 14184,23472, 14192,15280, 14200,31664, + 14216,18352, 14232,26544, 14248,22448, 14264,30640, 14280,20400, 14296,28592, + 14312,24496, 14320,16304, 14328,32688, 14344,16496, 14360,24688, 14376,20592, + 14392,28784, 14408,18544, 14424,26736, 14440,22640, 14456,30832, 14472,17520, + 14488,25712, 14504,21616, 14520,29808, 14536,19568, 14552,27760, 14568,23664, + 14576,15472, 14584,31856, 14600,17008, 14616,25200, 14632,21104, 14648,29296, + 14664,19056, 14680,27248, 14696,23152, 14704,14960, 14712,31344, 14728,18032, + 14744,26224, 14760,22128, 14776,30320, 14792,20080, 14808,28272, 14824,24176, + 14832,15984, 14840,32368, 14856,16752, 14872,24944, 14888,20848, 14904,29040, + 14920,18800, 14936,26992, 14952,22896, 14968,31088, 14984,17776, 15000,25968, + 15016,21872, 15032,30064, 15048,19824, 15064,28016, 15080,23920, 15088,15728, + 15096,32112, 15112,17264, 15128,25456, 15144,21360, 15160,29552, 15176,19312, + 15192,27504, 15208,23408, 15224,31600, 15240,18288, 15256,26480, 15272,22384, + 15288,30576, 15304,20336, 15320,28528, 15336,24432, 15344,16240, 15352,32624, + 15368,16624, 15384,24816, 15400,20720, 15416,28912, 15432,18672, 15448,26864, + 15464,22768, 15480,30960, 15496,17648, 15512,25840, 15528,21744, 15544,29936, + 15560,19696, 15576,27888, 15592,23792, 15608,31984, 15624,17136, 15640,25328, + 15656,21232, 15672,29424, 15688,19184, 15704,27376, 15720,23280, 15736,31472, + 15752,18160, 15768,26352, 15784,22256, 15800,30448, 15816,20208, 15832,28400, + 15848,24304, 15856,16112, 15864,32496, 15880,16880, 15896,25072, 15912,20976, + 15928,29168, 15944,18928, 15960,27120, 15976,23024, 15992,31216, 16008,17904, + 16024,26096, 16040,22000, 16056,30192, 16072,19952, 16088,28144, 16104,24048, + 16120,32240, 16136,17392, 16152,25584, 16168,21488, 16184,29680, 16200,19440, + 16216,27632, 16232,23536, 16248,31728, 16264,18416, 16280,26608, 16296,22512, + 16312,30704, 16328,20464, 16344,28656, 16360,24560, 16376,32752, 16408,24584, + 16424,20488, 16440,28680, 16456,18440, 16472,26632, 16488,22536, 16504,30728, + 16520,17416, 16536,25608, 16552,21512, 16568,29704, 16584,19464, 16600,27656, + 16616,23560, 16632,31752, 16648,16904, 16664,25096, 16680,21000, 16696,29192, + 16712,18952, 16728,27144, 16744,23048, 16760,31240, 16776,17928, 16792,26120, + 16808,22024, 16824,30216, 16840,19976, 16856,28168, 16872,24072, 16888,32264, + 16920,24840, 16936,20744, 16952,28936, 16968,18696, 16984,26888, 17000,22792, + 17016,30984, 17032,17672, 17048,25864, 17064,21768, 17080,29960, 17096,19720, + 17112,27912, 17128,23816, 17144,32008, 17176,25352, 17192,21256, 17208,29448, + 17224,19208, 17240,27400, 17256,23304, 17272,31496, 17288,18184, 17304,26376, + 17320,22280, 17336,30472, 17352,20232, 17368,28424, 17384,24328, 17400,32520, + 17432,24712, 17448,20616, 17464,28808, 17480,18568, 17496,26760, 17512,22664, + 17528,30856, 17560,25736, 17576,21640, 17592,29832, 17608,19592, 17624,27784, + 17640,23688, 17656,31880, 17688,25224, 17704,21128, 17720,29320, 17736,19080, + 17752,27272, 17768,23176, 17784,31368, 17800,18056, 17816,26248, 17832,22152, + 17848,30344, 17864,20104, 17880,28296, 17896,24200, 17912,32392, 17944,24968, + 17960,20872, 17976,29064, 17992,18824, 18008,27016, 18024,22920, 18040,31112, + 18072,25992, 18088,21896, 18104,30088, 18120,19848, 18136,28040, 18152,23944, + 18168,32136, 18200,25480, 18216,21384, 18232,29576, 18248,19336, 18264,27528, + 18280,23432, 18296,31624, 18328,26504, 18344,22408, 18360,30600, 18376,20360, + 18392,28552, 18408,24456, 18424,32648, 18456,24648, 18472,20552, 18488,28744, + 18520,26696, 18536,22600, 18552,30792, 18584,25672, 18600,21576, 18616,29768, + 18632,19528, 18648,27720, 18664,23624, 18680,31816, 18712,25160, 18728,21064, + 18744,29256, 18760,19016, 18776,27208, 18792,23112, 18808,31304, 18840,26184, + 18856,22088, 18872,30280, 18888,20040, 18904,28232, 18920,24136, 18936,32328, + 18968,24904, 18984,20808, 19000,29000, 19032,26952, 19048,22856, 19064,31048, + 19096,25928, 19112,21832, 19128,30024, 19144,19784, 19160,27976, 19176,23880, + 19192,32072, 19224,25416, 19240,21320, 19256,29512, 19288,27464, 19304,23368, + 19320,31560, 19352,26440, 19368,22344, 19384,30536, 19400,20296, 19416,28488, + 19432,24392, 19448,32584, 19480,24776, 19496,20680, 19512,28872, 19544,26824, + 19560,22728, 19576,30920, 19608,25800, 19624,21704, 19640,29896, 19672,27848, + 19688,23752, 19704,31944, 19736,25288, 19752,21192, 19768,29384, 19800,27336, + 19816,23240, 19832,31432, 19864,26312, 19880,22216, 19896,30408, 19912,20168, + 19928,28360, 19944,24264, 19960,32456, 19992,25032, 20008,20936, 20024,29128, + 20056,27080, 20072,22984, 20088,31176, 20120,26056, 20136,21960, 20152,30152, + 20184,28104, 20200,24008, 20216,32200, 20248,25544, 20264,21448, 20280,29640, + 20312,27592, 20328,23496, 20344,31688, 20376,26568, 20392,22472, 20408,30664, + 20440,28616, 20456,24520, 20472,32712, 20504,24616, 20536,28712, 20568,26664, + 20584,22568, 20600,30760, 20632,25640, 20648,21544, 20664,29736, 20696,27688, + 20712,23592, 20728,31784, 20760,25128, 20776,21032, 20792,29224, 20824,27176, + 20840,23080, 20856,31272, 20888,26152, 20904,22056, 20920,30248, 20952,28200, + 20968,24104, 20984,32296, 21016,24872, 21048,28968, 21080,26920, 21096,22824, + 21112,31016, 21144,25896, 21160,21800, 21176,29992, 21208,27944, 21224,23848, + 21240,32040, 21272,25384, 21304,29480, 21336,27432, 21352,23336, 21368,31528, + 21400,26408, 21416,22312, 21432,30504, 21464,28456, 21480,24360, 21496,32552, + 21528,24744, 21560,28840, 21592,26792, 21608,22696, 21624,30888, 21656,25768, + 21688,29864, 21720,27816, 21736,23720, 21752,31912, 21784,25256, 21816,29352, + 21848,27304, 21864,23208, 21880,31400, 21912,26280, 21928,22184, 21944,30376, + 21976,28328, 21992,24232, 22008,32424, 22040,25000, 22072,29096, 22104,27048, + 22120,22952, 22136,31144, 22168,26024, 22200,30120, 22232,28072, 22248,23976, + 22264,32168, 22296,25512, 22328,29608, 22360,27560, 22376,23464, 22392,31656, + 22424,26536, 22456,30632, 22488,28584, 22504,24488, 22520,32680, 22552,24680, + 22584,28776, 22616,26728, 22648,30824, 22680,25704, 22712,29800, 22744,27752, + 22760,23656, 22776,31848, 22808,25192, 22840,29288, 22872,27240, 22888,23144, + 22904,31336, 22936,26216, 22968,30312, 23000,28264, 23016,24168, 23032,32360, + 23064,24936, 23096,29032, 23128,26984, 23160,31080, 23192,25960, 23224,30056, + 23256,28008, 23272,23912, 23288,32104, 23320,25448, 23352,29544, 23384,27496, + 23416,31592, 23448,26472, 23480,30568, 23512,28520, 23528,24424, 23544,32616, + 23576,24808, 23608,28904, 23640,26856, 23672,30952, 23704,25832, 23736,29928, + 23768,27880, 23800,31976, 23832,25320, 23864,29416, 23896,27368, 23928,31464, + 23960,26344, 23992,30440, 24024,28392, 24040,24296, 24056,32488, 24088,25064, + 24120,29160, 24152,27112, 24184,31208, 24216,26088, 24248,30184, 24280,28136, + 24312,32232, 24344,25576, 24376,29672, 24408,27624, 24440,31720, 24472,26600, + 24504,30696, 24536,28648, 24568,32744, 24632,28696, 24664,26648, 24696,30744, + 24728,25624, 24760,29720, 24792,27672, 24824,31768, 24856,25112, 24888,29208, + 24920,27160, 24952,31256, 24984,26136, 25016,30232, 25048,28184, 25080,32280, + 25144,28952, 25176,26904, 25208,31000, 25240,25880, 25272,29976, 25304,27928, + 25336,32024, 25400,29464, 25432,27416, 25464,31512, 25496,26392, 25528,30488, + 25560,28440, 25592,32536, 25656,28824, 25688,26776, 25720,30872, 25784,29848, + 25816,27800, 25848,31896, 25912,29336, 25944,27288, 25976,31384, 26008,26264, + 26040,30360, 26072,28312, 26104,32408, 26168,29080, 26200,27032, 26232,31128, + 26296,30104, 26328,28056, 26360,32152, 26424,29592, 26456,27544, 26488,31640, + 26552,30616, 26584,28568, 26616,32664, 26680,28760, 26744,30808, 26808,29784, + 26840,27736, 26872,31832, 26936,29272, 26968,27224, 27000,31320, 27064,30296, + 27096,28248, 27128,32344, 27192,29016, 27256,31064, 27320,30040, 27352,27992, + 27384,32088, 27448,29528, 27512,31576, 27576,30552, 27608,28504, 27640,32600, + 27704,28888, 27768,30936, 27832,29912, 27896,31960, 27960,29400, 28024,31448, + 28088,30424, 28120,28376, 28152,32472, 28216,29144, 28280,31192, 28344,30168, + 28408,32216, 28472,29656, 28536,31704, 28600,30680, 28664,32728, 28792,30776, + 28856,29752, 28920,31800, 28984,29240, 29048,31288, 29112,30264, 29176,32312, + 29304,31032, 29368,30008, 29432,32056, 29560,31544, 29624,30520, 29688,32568, + 29816,30904, 29944,31928, 30072,31416, 30136,30392, 30200,32440, 30328,31160, + 30456,32184, 30584,31672, 30712,32696, 30968,31864, 31096,31352, 31224,32376, + 31480,32120, 31736,32632, 32248,32504 +}; + +/** +* \par +* Example code for Floating-point RFFT Twiddle factors Generation: +* \par +*
TW = exp(2*pi*i*[0:L/2-1]/L - pi/2*i).' 
+* \par +* Real and Imag values are in interleaved fashion +*/ +const float32_t twiddleCoef_rfft_32[32] = { +0.0f , 1.0f , +0.195090322f , 0.98078528f , +0.382683432f , 0.923879533f , +0.555570233f , 0.831469612f , +0.707106781f , 0.707106781f , +0.831469612f , 0.555570233f , +0.923879533f , 0.382683432f , +0.98078528f , 0.195090322f , +1.0f , 0.0f , +0.98078528f , -0.195090322f , +0.923879533f , -0.382683432f , +0.831469612f , -0.555570233f , +0.707106781f , -0.707106781f , +0.555570233f , -0.831469612f , +0.382683432f , -0.923879533f , +0.195090322f , -0.98078528f +}; + +const float32_t twiddleCoef_rfft_64[64] = { +0.0f, 1.0f, +0.098017140329561f, 0.995184726672197f, +0.195090322016128f, 0.98078528040323f, +0.290284677254462f, 0.956940335732209f, +0.38268343236509f, 0.923879532511287f, +0.471396736825998f, 0.881921264348355f, +0.555570233019602f, 0.831469612302545f, +0.634393284163645f, 0.773010453362737f, +0.707106781186547f, 0.707106781186548f, +0.773010453362737f, 0.634393284163645f, +0.831469612302545f, 0.555570233019602f, +0.881921264348355f, 0.471396736825998f, +0.923879532511287f, 0.38268343236509f, +0.956940335732209f, 0.290284677254462f, +0.98078528040323f, 0.195090322016128f, +0.995184726672197f, 0.098017140329561f, +1.0f, 0.0f, +0.995184726672197f, -0.098017140329561f, +0.98078528040323f, -0.195090322016128f, +0.956940335732209f, -0.290284677254462f, +0.923879532511287f, -0.38268343236509f, +0.881921264348355f, -0.471396736825998f, +0.831469612302545f, -0.555570233019602f, +0.773010453362737f, -0.634393284163645f, +0.707106781186548f, -0.707106781186547f, +0.634393284163645f, -0.773010453362737f, +0.555570233019602f, -0.831469612302545f, +0.471396736825998f, -0.881921264348355f, +0.38268343236509f, -0.923879532511287f, +0.290284677254462f, -0.956940335732209f, +0.195090322016129f, -0.98078528040323f, +0.098017140329561f, -0.995184726672197f +}; + +const float32_t twiddleCoef_rfft_128[128] = { + 0.000000000f, 1.000000000f, + 0.049067674f, 0.998795456f, + 0.098017140f, 0.995184727f, + 0.146730474f, 0.989176510f, + 0.195090322f, 0.980785280f, + 0.242980180f, 0.970031253f, + 0.290284677f, 0.956940336f, + 0.336889853f, 0.941544065f, + 0.382683432f, 0.923879533f, + 0.427555093f, 0.903989293f, + 0.471396737f, 0.881921264f, + 0.514102744f, 0.857728610f, + 0.555570233f, 0.831469612f, + 0.595699304f, 0.803207531f, + 0.634393284f, 0.773010453f, + 0.671558955f, 0.740951125f, + 0.707106781f, 0.707106781f, + 0.740951125f, 0.671558955f, + 0.773010453f, 0.634393284f, + 0.803207531f, 0.595699304f, + 0.831469612f, 0.555570233f, + 0.857728610f, 0.514102744f, + 0.881921264f, 0.471396737f, + 0.903989293f, 0.427555093f, + 0.923879533f, 0.382683432f, + 0.941544065f, 0.336889853f, + 0.956940336f, 0.290284677f, + 0.970031253f, 0.242980180f, + 0.980785280f, 0.195090322f, + 0.989176510f, 0.146730474f, + 0.995184727f, 0.098017140f, + 0.998795456f, 0.049067674f, + 1.000000000f, 0.000000000f, + 0.998795456f, -0.049067674f, + 0.995184727f, -0.098017140f, + 0.989176510f, -0.146730474f, + 0.980785280f, -0.195090322f, + 0.970031253f, -0.242980180f, + 0.956940336f, -0.290284677f, + 0.941544065f, -0.336889853f, + 0.923879533f, -0.382683432f, + 0.903989293f, -0.427555093f, + 0.881921264f, -0.471396737f, + 0.857728610f, -0.514102744f, + 0.831469612f, -0.555570233f, + 0.803207531f, -0.595699304f, + 0.773010453f, -0.634393284f, + 0.740951125f, -0.671558955f, + 0.707106781f, -0.707106781f, + 0.671558955f, -0.740951125f, + 0.634393284f, -0.773010453f, + 0.595699304f, -0.803207531f, + 0.555570233f, -0.831469612f, + 0.514102744f, -0.857728610f, + 0.471396737f, -0.881921264f, + 0.427555093f, -0.903989293f, + 0.382683432f, -0.923879533f, + 0.336889853f, -0.941544065f, + 0.290284677f, -0.956940336f, + 0.242980180f, -0.970031253f, + 0.195090322f, -0.980785280f, + 0.146730474f, -0.989176510f, + 0.098017140f, -0.995184727f, + 0.049067674f, -0.998795456f +}; + +const float32_t twiddleCoef_rfft_256[256] = { + 0.000000000f, 1.000000000f, + 0.024541229f, 0.999698819f, + 0.049067674f, 0.998795456f, + 0.073564564f, 0.997290457f, + 0.098017140f, 0.995184727f, + 0.122410675f, 0.992479535f, + 0.146730474f, 0.989176510f, + 0.170961889f, 0.985277642f, + 0.195090322f, 0.980785280f, + 0.219101240f, 0.975702130f, + 0.242980180f, 0.970031253f, + 0.266712757f, 0.963776066f, + 0.290284677f, 0.956940336f, + 0.313681740f, 0.949528181f, + 0.336889853f, 0.941544065f, + 0.359895037f, 0.932992799f, + 0.382683432f, 0.923879533f, + 0.405241314f, 0.914209756f, + 0.427555093f, 0.903989293f, + 0.449611330f, 0.893224301f, + 0.471396737f, 0.881921264f, + 0.492898192f, 0.870086991f, + 0.514102744f, 0.857728610f, + 0.534997620f, 0.844853565f, + 0.555570233f, 0.831469612f, + 0.575808191f, 0.817584813f, + 0.595699304f, 0.803207531f, + 0.615231591f, 0.788346428f, + 0.634393284f, 0.773010453f, + 0.653172843f, 0.757208847f, + 0.671558955f, 0.740951125f, + 0.689540545f, 0.724247083f, + 0.707106781f, 0.707106781f, + 0.724247083f, 0.689540545f, + 0.740951125f, 0.671558955f, + 0.757208847f, 0.653172843f, + 0.773010453f, 0.634393284f, + 0.788346428f, 0.615231591f, + 0.803207531f, 0.595699304f, + 0.817584813f, 0.575808191f, + 0.831469612f, 0.555570233f, + 0.844853565f, 0.534997620f, + 0.857728610f, 0.514102744f, + 0.870086991f, 0.492898192f, + 0.881921264f, 0.471396737f, + 0.893224301f, 0.449611330f, + 0.903989293f, 0.427555093f, + 0.914209756f, 0.405241314f, + 0.923879533f, 0.382683432f, + 0.932992799f, 0.359895037f, + 0.941544065f, 0.336889853f, + 0.949528181f, 0.313681740f, + 0.956940336f, 0.290284677f, + 0.963776066f, 0.266712757f, + 0.970031253f, 0.242980180f, + 0.975702130f, 0.219101240f, + 0.980785280f, 0.195090322f, + 0.985277642f, 0.170961889f, + 0.989176510f, 0.146730474f, + 0.992479535f, 0.122410675f, + 0.995184727f, 0.098017140f, + 0.997290457f, 0.073564564f, + 0.998795456f, 0.049067674f, + 0.999698819f, 0.024541229f, + 1.000000000f, 0.000000000f, + 0.999698819f, -0.024541229f, + 0.998795456f, -0.049067674f, + 0.997290457f, -0.073564564f, + 0.995184727f, -0.098017140f, + 0.992479535f, -0.122410675f, + 0.989176510f, -0.146730474f, + 0.985277642f, -0.170961889f, + 0.980785280f, -0.195090322f, + 0.975702130f, -0.219101240f, + 0.970031253f, -0.242980180f, + 0.963776066f, -0.266712757f, + 0.956940336f, -0.290284677f, + 0.949528181f, -0.313681740f, + 0.941544065f, -0.336889853f, + 0.932992799f, -0.359895037f, + 0.923879533f, -0.382683432f, + 0.914209756f, -0.405241314f, + 0.903989293f, -0.427555093f, + 0.893224301f, -0.449611330f, + 0.881921264f, -0.471396737f, + 0.870086991f, -0.492898192f, + 0.857728610f, -0.514102744f, + 0.844853565f, -0.534997620f, + 0.831469612f, -0.555570233f, + 0.817584813f, -0.575808191f, + 0.803207531f, -0.595699304f, + 0.788346428f, -0.615231591f, + 0.773010453f, -0.634393284f, + 0.757208847f, -0.653172843f, + 0.740951125f, -0.671558955f, + 0.724247083f, -0.689540545f, + 0.707106781f, -0.707106781f, + 0.689540545f, -0.724247083f, + 0.671558955f, -0.740951125f, + 0.653172843f, -0.757208847f, + 0.634393284f, -0.773010453f, + 0.615231591f, -0.788346428f, + 0.595699304f, -0.803207531f, + 0.575808191f, -0.817584813f, + 0.555570233f, -0.831469612f, + 0.534997620f, -0.844853565f, + 0.514102744f, -0.857728610f, + 0.492898192f, -0.870086991f, + 0.471396737f, -0.881921264f, + 0.449611330f, -0.893224301f, + 0.427555093f, -0.903989293f, + 0.405241314f, -0.914209756f, + 0.382683432f, -0.923879533f, + 0.359895037f, -0.932992799f, + 0.336889853f, -0.941544065f, + 0.313681740f, -0.949528181f, + 0.290284677f, -0.956940336f, + 0.266712757f, -0.963776066f, + 0.242980180f, -0.970031253f, + 0.219101240f, -0.975702130f, + 0.195090322f, -0.980785280f, + 0.170961889f, -0.985277642f, + 0.146730474f, -0.989176510f, + 0.122410675f, -0.992479535f, + 0.098017140f, -0.995184727f, + 0.073564564f, -0.997290457f, + 0.049067674f, -0.998795456f, + 0.024541229f, -0.999698819f +}; + +const float32_t twiddleCoef_rfft_512[512] = { + 0.000000000f, 1.000000000f, + 0.012271538f, 0.999924702f, + 0.024541229f, 0.999698819f, + 0.036807223f, 0.999322385f, + 0.049067674f, 0.998795456f, + 0.061320736f, 0.998118113f, + 0.073564564f, 0.997290457f, + 0.085797312f, 0.996312612f, + 0.098017140f, 0.995184727f, + 0.110222207f, 0.993906970f, + 0.122410675f, 0.992479535f, + 0.134580709f, 0.990902635f, + 0.146730474f, 0.989176510f, + 0.158858143f, 0.987301418f, + 0.170961889f, 0.985277642f, + 0.183039888f, 0.983105487f, + 0.195090322f, 0.980785280f, + 0.207111376f, 0.978317371f, + 0.219101240f, 0.975702130f, + 0.231058108f, 0.972939952f, + 0.242980180f, 0.970031253f, + 0.254865660f, 0.966976471f, + 0.266712757f, 0.963776066f, + 0.278519689f, 0.960430519f, + 0.290284677f, 0.956940336f, + 0.302005949f, 0.953306040f, + 0.313681740f, 0.949528181f, + 0.325310292f, 0.945607325f, + 0.336889853f, 0.941544065f, + 0.348418680f, 0.937339012f, + 0.359895037f, 0.932992799f, + 0.371317194f, 0.928506080f, + 0.382683432f, 0.923879533f, + 0.393992040f, 0.919113852f, + 0.405241314f, 0.914209756f, + 0.416429560f, 0.909167983f, + 0.427555093f, 0.903989293f, + 0.438616239f, 0.898674466f, + 0.449611330f, 0.893224301f, + 0.460538711f, 0.887639620f, + 0.471396737f, 0.881921264f, + 0.482183772f, 0.876070094f, + 0.492898192f, 0.870086991f, + 0.503538384f, 0.863972856f, + 0.514102744f, 0.857728610f, + 0.524589683f, 0.851355193f, + 0.534997620f, 0.844853565f, + 0.545324988f, 0.838224706f, + 0.555570233f, 0.831469612f, + 0.565731811f, 0.824589303f, + 0.575808191f, 0.817584813f, + 0.585797857f, 0.810457198f, + 0.595699304f, 0.803207531f, + 0.605511041f, 0.795836905f, + 0.615231591f, 0.788346428f, + 0.624859488f, 0.780737229f, + 0.634393284f, 0.773010453f, + 0.643831543f, 0.765167266f, + 0.653172843f, 0.757208847f, + 0.662415778f, 0.749136395f, + 0.671558955f, 0.740951125f, + 0.680600998f, 0.732654272f, + 0.689540545f, 0.724247083f, + 0.698376249f, 0.715730825f, + 0.707106781f, 0.707106781f, + 0.715730825f, 0.698376249f, + 0.724247083f, 0.689540545f, + 0.732654272f, 0.680600998f, + 0.740951125f, 0.671558955f, + 0.749136395f, 0.662415778f, + 0.757208847f, 0.653172843f, + 0.765167266f, 0.643831543f, + 0.773010453f, 0.634393284f, + 0.780737229f, 0.624859488f, + 0.788346428f, 0.615231591f, + 0.795836905f, 0.605511041f, + 0.803207531f, 0.595699304f, + 0.810457198f, 0.585797857f, + 0.817584813f, 0.575808191f, + 0.824589303f, 0.565731811f, + 0.831469612f, 0.555570233f, + 0.838224706f, 0.545324988f, + 0.844853565f, 0.534997620f, + 0.851355193f, 0.524589683f, + 0.857728610f, 0.514102744f, + 0.863972856f, 0.503538384f, + 0.870086991f, 0.492898192f, + 0.876070094f, 0.482183772f, + 0.881921264f, 0.471396737f, + 0.887639620f, 0.460538711f, + 0.893224301f, 0.449611330f, + 0.898674466f, 0.438616239f, + 0.903989293f, 0.427555093f, + 0.909167983f, 0.416429560f, + 0.914209756f, 0.405241314f, + 0.919113852f, 0.393992040f, + 0.923879533f, 0.382683432f, + 0.928506080f, 0.371317194f, + 0.932992799f, 0.359895037f, + 0.937339012f, 0.348418680f, + 0.941544065f, 0.336889853f, + 0.945607325f, 0.325310292f, + 0.949528181f, 0.313681740f, + 0.953306040f, 0.302005949f, + 0.956940336f, 0.290284677f, + 0.960430519f, 0.278519689f, + 0.963776066f, 0.266712757f, + 0.966976471f, 0.254865660f, + 0.970031253f, 0.242980180f, + 0.972939952f, 0.231058108f, + 0.975702130f, 0.219101240f, + 0.978317371f, 0.207111376f, + 0.980785280f, 0.195090322f, + 0.983105487f, 0.183039888f, + 0.985277642f, 0.170961889f, + 0.987301418f, 0.158858143f, + 0.989176510f, 0.146730474f, + 0.990902635f, 0.134580709f, + 0.992479535f, 0.122410675f, + 0.993906970f, 0.110222207f, + 0.995184727f, 0.098017140f, + 0.996312612f, 0.085797312f, + 0.997290457f, 0.073564564f, + 0.998118113f, 0.061320736f, + 0.998795456f, 0.049067674f, + 0.999322385f, 0.036807223f, + 0.999698819f, 0.024541229f, + 0.999924702f, 0.012271538f, + 1.000000000f, 0.000000000f, + 0.999924702f, -0.012271538f, + 0.999698819f, -0.024541229f, + 0.999322385f, -0.036807223f, + 0.998795456f, -0.049067674f, + 0.998118113f, -0.061320736f, + 0.997290457f, -0.073564564f, + 0.996312612f, -0.085797312f, + 0.995184727f, -0.098017140f, + 0.993906970f, -0.110222207f, + 0.992479535f, -0.122410675f, + 0.990902635f, -0.134580709f, + 0.989176510f, -0.146730474f, + 0.987301418f, -0.158858143f, + 0.985277642f, -0.170961889f, + 0.983105487f, -0.183039888f, + 0.980785280f, -0.195090322f, + 0.978317371f, -0.207111376f, + 0.975702130f, -0.219101240f, + 0.972939952f, -0.231058108f, + 0.970031253f, -0.242980180f, + 0.966976471f, -0.254865660f, + 0.963776066f, -0.266712757f, + 0.960430519f, -0.278519689f, + 0.956940336f, -0.290284677f, + 0.953306040f, -0.302005949f, + 0.949528181f, -0.313681740f, + 0.945607325f, -0.325310292f, + 0.941544065f, -0.336889853f, + 0.937339012f, -0.348418680f, + 0.932992799f, -0.359895037f, + 0.928506080f, -0.371317194f, + 0.923879533f, -0.382683432f, + 0.919113852f, -0.393992040f, + 0.914209756f, -0.405241314f, + 0.909167983f, -0.416429560f, + 0.903989293f, -0.427555093f, + 0.898674466f, -0.438616239f, + 0.893224301f, -0.449611330f, + 0.887639620f, -0.460538711f, + 0.881921264f, -0.471396737f, + 0.876070094f, -0.482183772f, + 0.870086991f, -0.492898192f, + 0.863972856f, -0.503538384f, + 0.857728610f, -0.514102744f, + 0.851355193f, -0.524589683f, + 0.844853565f, -0.534997620f, + 0.838224706f, -0.545324988f, + 0.831469612f, -0.555570233f, + 0.824589303f, -0.565731811f, + 0.817584813f, -0.575808191f, + 0.810457198f, -0.585797857f, + 0.803207531f, -0.595699304f, + 0.795836905f, -0.605511041f, + 0.788346428f, -0.615231591f, + 0.780737229f, -0.624859488f, + 0.773010453f, -0.634393284f, + 0.765167266f, -0.643831543f, + 0.757208847f, -0.653172843f, + 0.749136395f, -0.662415778f, + 0.740951125f, -0.671558955f, + 0.732654272f, -0.680600998f, + 0.724247083f, -0.689540545f, + 0.715730825f, -0.698376249f, + 0.707106781f, -0.707106781f, + 0.698376249f, -0.715730825f, + 0.689540545f, -0.724247083f, + 0.680600998f, -0.732654272f, + 0.671558955f, -0.740951125f, + 0.662415778f, -0.749136395f, + 0.653172843f, -0.757208847f, + 0.643831543f, -0.765167266f, + 0.634393284f, -0.773010453f, + 0.624859488f, -0.780737229f, + 0.615231591f, -0.788346428f, + 0.605511041f, -0.795836905f, + 0.595699304f, -0.803207531f, + 0.585797857f, -0.810457198f, + 0.575808191f, -0.817584813f, + 0.565731811f, -0.824589303f, + 0.555570233f, -0.831469612f, + 0.545324988f, -0.838224706f, + 0.534997620f, -0.844853565f, + 0.524589683f, -0.851355193f, + 0.514102744f, -0.857728610f, + 0.503538384f, -0.863972856f, + 0.492898192f, -0.870086991f, + 0.482183772f, -0.876070094f, + 0.471396737f, -0.881921264f, + 0.460538711f, -0.887639620f, + 0.449611330f, -0.893224301f, + 0.438616239f, -0.898674466f, + 0.427555093f, -0.903989293f, + 0.416429560f, -0.909167983f, + 0.405241314f, -0.914209756f, + 0.393992040f, -0.919113852f, + 0.382683432f, -0.923879533f, + 0.371317194f, -0.928506080f, + 0.359895037f, -0.932992799f, + 0.348418680f, -0.937339012f, + 0.336889853f, -0.941544065f, + 0.325310292f, -0.945607325f, + 0.313681740f, -0.949528181f, + 0.302005949f, -0.953306040f, + 0.290284677f, -0.956940336f, + 0.278519689f, -0.960430519f, + 0.266712757f, -0.963776066f, + 0.254865660f, -0.966976471f, + 0.242980180f, -0.970031253f, + 0.231058108f, -0.972939952f, + 0.219101240f, -0.975702130f, + 0.207111376f, -0.978317371f, + 0.195090322f, -0.980785280f, + 0.183039888f, -0.983105487f, + 0.170961889f, -0.985277642f, + 0.158858143f, -0.987301418f, + 0.146730474f, -0.989176510f, + 0.134580709f, -0.990902635f, + 0.122410675f, -0.992479535f, + 0.110222207f, -0.993906970f, + 0.098017140f, -0.995184727f, + 0.085797312f, -0.996312612f, + 0.073564564f, -0.997290457f, + 0.061320736f, -0.998118113f, + 0.049067674f, -0.998795456f, + 0.036807223f, -0.999322385f, + 0.024541229f, -0.999698819f, + 0.012271538f, -0.999924702f +}; + +const float32_t twiddleCoef_rfft_1024[1024] = { + 0.000000000f, 1.000000000f, + 0.006135885f, 0.999981175f, + 0.012271538f, 0.999924702f, + 0.018406730f, 0.999830582f, + 0.024541229f, 0.999698819f, + 0.030674803f, 0.999529418f, + 0.036807223f, 0.999322385f, + 0.042938257f, 0.999077728f, + 0.049067674f, 0.998795456f, + 0.055195244f, 0.998475581f, + 0.061320736f, 0.998118113f, + 0.067443920f, 0.997723067f, + 0.073564564f, 0.997290457f, + 0.079682438f, 0.996820299f, + 0.085797312f, 0.996312612f, + 0.091908956f, 0.995767414f, + 0.098017140f, 0.995184727f, + 0.104121634f, 0.994564571f, + 0.110222207f, 0.993906970f, + 0.116318631f, 0.993211949f, + 0.122410675f, 0.992479535f, + 0.128498111f, 0.991709754f, + 0.134580709f, 0.990902635f, + 0.140658239f, 0.990058210f, + 0.146730474f, 0.989176510f, + 0.152797185f, 0.988257568f, + 0.158858143f, 0.987301418f, + 0.164913120f, 0.986308097f, + 0.170961889f, 0.985277642f, + 0.177004220f, 0.984210092f, + 0.183039888f, 0.983105487f, + 0.189068664f, 0.981963869f, + 0.195090322f, 0.980785280f, + 0.201104635f, 0.979569766f, + 0.207111376f, 0.978317371f, + 0.213110320f, 0.977028143f, + 0.219101240f, 0.975702130f, + 0.225083911f, 0.974339383f, + 0.231058108f, 0.972939952f, + 0.237023606f, 0.971503891f, + 0.242980180f, 0.970031253f, + 0.248927606f, 0.968522094f, + 0.254865660f, 0.966976471f, + 0.260794118f, 0.965394442f, + 0.266712757f, 0.963776066f, + 0.272621355f, 0.962121404f, + 0.278519689f, 0.960430519f, + 0.284407537f, 0.958703475f, + 0.290284677f, 0.956940336f, + 0.296150888f, 0.955141168f, + 0.302005949f, 0.953306040f, + 0.307849640f, 0.951435021f, + 0.313681740f, 0.949528181f, + 0.319502031f, 0.947585591f, + 0.325310292f, 0.945607325f, + 0.331106306f, 0.943593458f, + 0.336889853f, 0.941544065f, + 0.342660717f, 0.939459224f, + 0.348418680f, 0.937339012f, + 0.354163525f, 0.935183510f, + 0.359895037f, 0.932992799f, + 0.365612998f, 0.930766961f, + 0.371317194f, 0.928506080f, + 0.377007410f, 0.926210242f, + 0.382683432f, 0.923879533f, + 0.388345047f, 0.921514039f, + 0.393992040f, 0.919113852f, + 0.399624200f, 0.916679060f, + 0.405241314f, 0.914209756f, + 0.410843171f, 0.911706032f, + 0.416429560f, 0.909167983f, + 0.422000271f, 0.906595705f, + 0.427555093f, 0.903989293f, + 0.433093819f, 0.901348847f, + 0.438616239f, 0.898674466f, + 0.444122145f, 0.895966250f, + 0.449611330f, 0.893224301f, + 0.455083587f, 0.890448723f, + 0.460538711f, 0.887639620f, + 0.465976496f, 0.884797098f, + 0.471396737f, 0.881921264f, + 0.476799230f, 0.879012226f, + 0.482183772f, 0.876070094f, + 0.487550160f, 0.873094978f, + 0.492898192f, 0.870086991f, + 0.498227667f, 0.867046246f, + 0.503538384f, 0.863972856f, + 0.508830143f, 0.860866939f, + 0.514102744f, 0.857728610f, + 0.519355990f, 0.854557988f, + 0.524589683f, 0.851355193f, + 0.529803625f, 0.848120345f, + 0.534997620f, 0.844853565f, + 0.540171473f, 0.841554977f, + 0.545324988f, 0.838224706f, + 0.550457973f, 0.834862875f, + 0.555570233f, 0.831469612f, + 0.560661576f, 0.828045045f, + 0.565731811f, 0.824589303f, + 0.570780746f, 0.821102515f, + 0.575808191f, 0.817584813f, + 0.580813958f, 0.814036330f, + 0.585797857f, 0.810457198f, + 0.590759702f, 0.806847554f, + 0.595699304f, 0.803207531f, + 0.600616479f, 0.799537269f, + 0.605511041f, 0.795836905f, + 0.610382806f, 0.792106577f, + 0.615231591f, 0.788346428f, + 0.620057212f, 0.784556597f, + 0.624859488f, 0.780737229f, + 0.629638239f, 0.776888466f, + 0.634393284f, 0.773010453f, + 0.639124445f, 0.769103338f, + 0.643831543f, 0.765167266f, + 0.648514401f, 0.761202385f, + 0.653172843f, 0.757208847f, + 0.657806693f, 0.753186799f, + 0.662415778f, 0.749136395f, + 0.666999922f, 0.745057785f, + 0.671558955f, 0.740951125f, + 0.676092704f, 0.736816569f, + 0.680600998f, 0.732654272f, + 0.685083668f, 0.728464390f, + 0.689540545f, 0.724247083f, + 0.693971461f, 0.720002508f, + 0.698376249f, 0.715730825f, + 0.702754744f, 0.711432196f, + 0.707106781f, 0.707106781f, + 0.711432196f, 0.702754744f, + 0.715730825f, 0.698376249f, + 0.720002508f, 0.693971461f, + 0.724247083f, 0.689540545f, + 0.728464390f, 0.685083668f, + 0.732654272f, 0.680600998f, + 0.736816569f, 0.676092704f, + 0.740951125f, 0.671558955f, + 0.745057785f, 0.666999922f, + 0.749136395f, 0.662415778f, + 0.753186799f, 0.657806693f, + 0.757208847f, 0.653172843f, + 0.761202385f, 0.648514401f, + 0.765167266f, 0.643831543f, + 0.769103338f, 0.639124445f, + 0.773010453f, 0.634393284f, + 0.776888466f, 0.629638239f, + 0.780737229f, 0.624859488f, + 0.784556597f, 0.620057212f, + 0.788346428f, 0.615231591f, + 0.792106577f, 0.610382806f, + 0.795836905f, 0.605511041f, + 0.799537269f, 0.600616479f, + 0.803207531f, 0.595699304f, + 0.806847554f, 0.590759702f, + 0.810457198f, 0.585797857f, + 0.814036330f, 0.580813958f, + 0.817584813f, 0.575808191f, + 0.821102515f, 0.570780746f, + 0.824589303f, 0.565731811f, + 0.828045045f, 0.560661576f, + 0.831469612f, 0.555570233f, + 0.834862875f, 0.550457973f, + 0.838224706f, 0.545324988f, + 0.841554977f, 0.540171473f, + 0.844853565f, 0.534997620f, + 0.848120345f, 0.529803625f, + 0.851355193f, 0.524589683f, + 0.854557988f, 0.519355990f, + 0.857728610f, 0.514102744f, + 0.860866939f, 0.508830143f, + 0.863972856f, 0.503538384f, + 0.867046246f, 0.498227667f, + 0.870086991f, 0.492898192f, + 0.873094978f, 0.487550160f, + 0.876070094f, 0.482183772f, + 0.879012226f, 0.476799230f, + 0.881921264f, 0.471396737f, + 0.884797098f, 0.465976496f, + 0.887639620f, 0.460538711f, + 0.890448723f, 0.455083587f, + 0.893224301f, 0.449611330f, + 0.895966250f, 0.444122145f, + 0.898674466f, 0.438616239f, + 0.901348847f, 0.433093819f, + 0.903989293f, 0.427555093f, + 0.906595705f, 0.422000271f, + 0.909167983f, 0.416429560f, + 0.911706032f, 0.410843171f, + 0.914209756f, 0.405241314f, + 0.916679060f, 0.399624200f, + 0.919113852f, 0.393992040f, + 0.921514039f, 0.388345047f, + 0.923879533f, 0.382683432f, + 0.926210242f, 0.377007410f, + 0.928506080f, 0.371317194f, + 0.930766961f, 0.365612998f, + 0.932992799f, 0.359895037f, + 0.935183510f, 0.354163525f, + 0.937339012f, 0.348418680f, + 0.939459224f, 0.342660717f, + 0.941544065f, 0.336889853f, + 0.943593458f, 0.331106306f, + 0.945607325f, 0.325310292f, + 0.947585591f, 0.319502031f, + 0.949528181f, 0.313681740f, + 0.951435021f, 0.307849640f, + 0.953306040f, 0.302005949f, + 0.955141168f, 0.296150888f, + 0.956940336f, 0.290284677f, + 0.958703475f, 0.284407537f, + 0.960430519f, 0.278519689f, + 0.962121404f, 0.272621355f, + 0.963776066f, 0.266712757f, + 0.965394442f, 0.260794118f, + 0.966976471f, 0.254865660f, + 0.968522094f, 0.248927606f, + 0.970031253f, 0.242980180f, + 0.971503891f, 0.237023606f, + 0.972939952f, 0.231058108f, + 0.974339383f, 0.225083911f, + 0.975702130f, 0.219101240f, + 0.977028143f, 0.213110320f, + 0.978317371f, 0.207111376f, + 0.979569766f, 0.201104635f, + 0.980785280f, 0.195090322f, + 0.981963869f, 0.189068664f, + 0.983105487f, 0.183039888f, + 0.984210092f, 0.177004220f, + 0.985277642f, 0.170961889f, + 0.986308097f, 0.164913120f, + 0.987301418f, 0.158858143f, + 0.988257568f, 0.152797185f, + 0.989176510f, 0.146730474f, + 0.990058210f, 0.140658239f, + 0.990902635f, 0.134580709f, + 0.991709754f, 0.128498111f, + 0.992479535f, 0.122410675f, + 0.993211949f, 0.116318631f, + 0.993906970f, 0.110222207f, + 0.994564571f, 0.104121634f, + 0.995184727f, 0.098017140f, + 0.995767414f, 0.091908956f, + 0.996312612f, 0.085797312f, + 0.996820299f, 0.079682438f, + 0.997290457f, 0.073564564f, + 0.997723067f, 0.067443920f, + 0.998118113f, 0.061320736f, + 0.998475581f, 0.055195244f, + 0.998795456f, 0.049067674f, + 0.999077728f, 0.042938257f, + 0.999322385f, 0.036807223f, + 0.999529418f, 0.030674803f, + 0.999698819f, 0.024541229f, + 0.999830582f, 0.018406730f, + 0.999924702f, 0.012271538f, + 0.999981175f, 0.006135885f, + 1.000000000f, 0.000000000f, + 0.999981175f, -0.006135885f, + 0.999924702f, -0.012271538f, + 0.999830582f, -0.018406730f, + 0.999698819f, -0.024541229f, + 0.999529418f, -0.030674803f, + 0.999322385f, -0.036807223f, + 0.999077728f, -0.042938257f, + 0.998795456f, -0.049067674f, + 0.998475581f, -0.055195244f, + 0.998118113f, -0.061320736f, + 0.997723067f, -0.067443920f, + 0.997290457f, -0.073564564f, + 0.996820299f, -0.079682438f, + 0.996312612f, -0.085797312f, + 0.995767414f, -0.091908956f, + 0.995184727f, -0.098017140f, + 0.994564571f, -0.104121634f, + 0.993906970f, -0.110222207f, + 0.993211949f, -0.116318631f, + 0.992479535f, -0.122410675f, + 0.991709754f, -0.128498111f, + 0.990902635f, -0.134580709f, + 0.990058210f, -0.140658239f, + 0.989176510f, -0.146730474f, + 0.988257568f, -0.152797185f, + 0.987301418f, -0.158858143f, + 0.986308097f, -0.164913120f, + 0.985277642f, -0.170961889f, + 0.984210092f, -0.177004220f, + 0.983105487f, -0.183039888f, + 0.981963869f, -0.189068664f, + 0.980785280f, -0.195090322f, + 0.979569766f, -0.201104635f, + 0.978317371f, -0.207111376f, + 0.977028143f, -0.213110320f, + 0.975702130f, -0.219101240f, + 0.974339383f, -0.225083911f, + 0.972939952f, -0.231058108f, + 0.971503891f, -0.237023606f, + 0.970031253f, -0.242980180f, + 0.968522094f, -0.248927606f, + 0.966976471f, -0.254865660f, + 0.965394442f, -0.260794118f, + 0.963776066f, -0.266712757f, + 0.962121404f, -0.272621355f, + 0.960430519f, -0.278519689f, + 0.958703475f, -0.284407537f, + 0.956940336f, -0.290284677f, + 0.955141168f, -0.296150888f, + 0.953306040f, -0.302005949f, + 0.951435021f, -0.307849640f, + 0.949528181f, -0.313681740f, + 0.947585591f, -0.319502031f, + 0.945607325f, -0.325310292f, + 0.943593458f, -0.331106306f, + 0.941544065f, -0.336889853f, + 0.939459224f, -0.342660717f, + 0.937339012f, -0.348418680f, + 0.935183510f, -0.354163525f, + 0.932992799f, -0.359895037f, + 0.930766961f, -0.365612998f, + 0.928506080f, -0.371317194f, + 0.926210242f, -0.377007410f, + 0.923879533f, -0.382683432f, + 0.921514039f, -0.388345047f, + 0.919113852f, -0.393992040f, + 0.916679060f, -0.399624200f, + 0.914209756f, -0.405241314f, + 0.911706032f, -0.410843171f, + 0.909167983f, -0.416429560f, + 0.906595705f, -0.422000271f, + 0.903989293f, -0.427555093f, + 0.901348847f, -0.433093819f, + 0.898674466f, -0.438616239f, + 0.895966250f, -0.444122145f, + 0.893224301f, -0.449611330f, + 0.890448723f, -0.455083587f, + 0.887639620f, -0.460538711f, + 0.884797098f, -0.465976496f, + 0.881921264f, -0.471396737f, + 0.879012226f, -0.476799230f, + 0.876070094f, -0.482183772f, + 0.873094978f, -0.487550160f, + 0.870086991f, -0.492898192f, + 0.867046246f, -0.498227667f, + 0.863972856f, -0.503538384f, + 0.860866939f, -0.508830143f, + 0.857728610f, -0.514102744f, + 0.854557988f, -0.519355990f, + 0.851355193f, -0.524589683f, + 0.848120345f, -0.529803625f, + 0.844853565f, -0.534997620f, + 0.841554977f, -0.540171473f, + 0.838224706f, -0.545324988f, + 0.834862875f, -0.550457973f, + 0.831469612f, -0.555570233f, + 0.828045045f, -0.560661576f, + 0.824589303f, -0.565731811f, + 0.821102515f, -0.570780746f, + 0.817584813f, -0.575808191f, + 0.814036330f, -0.580813958f, + 0.810457198f, -0.585797857f, + 0.806847554f, -0.590759702f, + 0.803207531f, -0.595699304f, + 0.799537269f, -0.600616479f, + 0.795836905f, -0.605511041f, + 0.792106577f, -0.610382806f, + 0.788346428f, -0.615231591f, + 0.784556597f, -0.620057212f, + 0.780737229f, -0.624859488f, + 0.776888466f, -0.629638239f, + 0.773010453f, -0.634393284f, + 0.769103338f, -0.639124445f, + 0.765167266f, -0.643831543f, + 0.761202385f, -0.648514401f, + 0.757208847f, -0.653172843f, + 0.753186799f, -0.657806693f, + 0.749136395f, -0.662415778f, + 0.745057785f, -0.666999922f, + 0.740951125f, -0.671558955f, + 0.736816569f, -0.676092704f, + 0.732654272f, -0.680600998f, + 0.728464390f, -0.685083668f, + 0.724247083f, -0.689540545f, + 0.720002508f, -0.693971461f, + 0.715730825f, -0.698376249f, + 0.711432196f, -0.702754744f, + 0.707106781f, -0.707106781f, + 0.702754744f, -0.711432196f, + 0.698376249f, -0.715730825f, + 0.693971461f, -0.720002508f, + 0.689540545f, -0.724247083f, + 0.685083668f, -0.728464390f, + 0.680600998f, -0.732654272f, + 0.676092704f, -0.736816569f, + 0.671558955f, -0.740951125f, + 0.666999922f, -0.745057785f, + 0.662415778f, -0.749136395f, + 0.657806693f, -0.753186799f, + 0.653172843f, -0.757208847f, + 0.648514401f, -0.761202385f, + 0.643831543f, -0.765167266f, + 0.639124445f, -0.769103338f, + 0.634393284f, -0.773010453f, + 0.629638239f, -0.776888466f, + 0.624859488f, -0.780737229f, + 0.620057212f, -0.784556597f, + 0.615231591f, -0.788346428f, + 0.610382806f, -0.792106577f, + 0.605511041f, -0.795836905f, + 0.600616479f, -0.799537269f, + 0.595699304f, -0.803207531f, + 0.590759702f, -0.806847554f, + 0.585797857f, -0.810457198f, + 0.580813958f, -0.814036330f, + 0.575808191f, -0.817584813f, + 0.570780746f, -0.821102515f, + 0.565731811f, -0.824589303f, + 0.560661576f, -0.828045045f, + 0.555570233f, -0.831469612f, + 0.550457973f, -0.834862875f, + 0.545324988f, -0.838224706f, + 0.540171473f, -0.841554977f, + 0.534997620f, -0.844853565f, + 0.529803625f, -0.848120345f, + 0.524589683f, -0.851355193f, + 0.519355990f, -0.854557988f, + 0.514102744f, -0.857728610f, + 0.508830143f, -0.860866939f, + 0.503538384f, -0.863972856f, + 0.498227667f, -0.867046246f, + 0.492898192f, -0.870086991f, + 0.487550160f, -0.873094978f, + 0.482183772f, -0.876070094f, + 0.476799230f, -0.879012226f, + 0.471396737f, -0.881921264f, + 0.465976496f, -0.884797098f, + 0.460538711f, -0.887639620f, + 0.455083587f, -0.890448723f, + 0.449611330f, -0.893224301f, + 0.444122145f, -0.895966250f, + 0.438616239f, -0.898674466f, + 0.433093819f, -0.901348847f, + 0.427555093f, -0.903989293f, + 0.422000271f, -0.906595705f, + 0.416429560f, -0.909167983f, + 0.410843171f, -0.911706032f, + 0.405241314f, -0.914209756f, + 0.399624200f, -0.916679060f, + 0.393992040f, -0.919113852f, + 0.388345047f, -0.921514039f, + 0.382683432f, -0.923879533f, + 0.377007410f, -0.926210242f, + 0.371317194f, -0.928506080f, + 0.365612998f, -0.930766961f, + 0.359895037f, -0.932992799f, + 0.354163525f, -0.935183510f, + 0.348418680f, -0.937339012f, + 0.342660717f, -0.939459224f, + 0.336889853f, -0.941544065f, + 0.331106306f, -0.943593458f, + 0.325310292f, -0.945607325f, + 0.319502031f, -0.947585591f, + 0.313681740f, -0.949528181f, + 0.307849640f, -0.951435021f, + 0.302005949f, -0.953306040f, + 0.296150888f, -0.955141168f, + 0.290284677f, -0.956940336f, + 0.284407537f, -0.958703475f, + 0.278519689f, -0.960430519f, + 0.272621355f, -0.962121404f, + 0.266712757f, -0.963776066f, + 0.260794118f, -0.965394442f, + 0.254865660f, -0.966976471f, + 0.248927606f, -0.968522094f, + 0.242980180f, -0.970031253f, + 0.237023606f, -0.971503891f, + 0.231058108f, -0.972939952f, + 0.225083911f, -0.974339383f, + 0.219101240f, -0.975702130f, + 0.213110320f, -0.977028143f, + 0.207111376f, -0.978317371f, + 0.201104635f, -0.979569766f, + 0.195090322f, -0.980785280f, + 0.189068664f, -0.981963869f, + 0.183039888f, -0.983105487f, + 0.177004220f, -0.984210092f, + 0.170961889f, -0.985277642f, + 0.164913120f, -0.986308097f, + 0.158858143f, -0.987301418f, + 0.152797185f, -0.988257568f, + 0.146730474f, -0.989176510f, + 0.140658239f, -0.990058210f, + 0.134580709f, -0.990902635f, + 0.128498111f, -0.991709754f, + 0.122410675f, -0.992479535f, + 0.116318631f, -0.993211949f, + 0.110222207f, -0.993906970f, + 0.104121634f, -0.994564571f, + 0.098017140f, -0.995184727f, + 0.091908956f, -0.995767414f, + 0.085797312f, -0.996312612f, + 0.079682438f, -0.996820299f, + 0.073564564f, -0.997290457f, + 0.067443920f, -0.997723067f, + 0.061320736f, -0.998118113f, + 0.055195244f, -0.998475581f, + 0.049067674f, -0.998795456f, + 0.042938257f, -0.999077728f, + 0.036807223f, -0.999322385f, + 0.030674803f, -0.999529418f, + 0.024541229f, -0.999698819f, + 0.018406730f, -0.999830582f, + 0.012271538f, -0.999924702f, + 0.006135885f, -0.999981175f +}; + +const float32_t twiddleCoef_rfft_2048[2048] = { + 0.000000000f, 1.000000000f, + 0.003067957f, 0.999995294f, + 0.006135885f, 0.999981175f, + 0.009203755f, 0.999957645f, + 0.012271538f, 0.999924702f, + 0.015339206f, 0.999882347f, + 0.018406730f, 0.999830582f, + 0.021474080f, 0.999769405f, + 0.024541229f, 0.999698819f, + 0.027608146f, 0.999618822f, + 0.030674803f, 0.999529418f, + 0.033741172f, 0.999430605f, + 0.036807223f, 0.999322385f, + 0.039872928f, 0.999204759f, + 0.042938257f, 0.999077728f, + 0.046003182f, 0.998941293f, + 0.049067674f, 0.998795456f, + 0.052131705f, 0.998640218f, + 0.055195244f, 0.998475581f, + 0.058258265f, 0.998301545f, + 0.061320736f, 0.998118113f, + 0.064382631f, 0.997925286f, + 0.067443920f, 0.997723067f, + 0.070504573f, 0.997511456f, + 0.073564564f, 0.997290457f, + 0.076623861f, 0.997060070f, + 0.079682438f, 0.996820299f, + 0.082740265f, 0.996571146f, + 0.085797312f, 0.996312612f, + 0.088853553f, 0.996044701f, + 0.091908956f, 0.995767414f, + 0.094963495f, 0.995480755f, + 0.098017140f, 0.995184727f, + 0.101069863f, 0.994879331f, + 0.104121634f, 0.994564571f, + 0.107172425f, 0.994240449f, + 0.110222207f, 0.993906970f, + 0.113270952f, 0.993564136f, + 0.116318631f, 0.993211949f, + 0.119365215f, 0.992850414f, + 0.122410675f, 0.992479535f, + 0.125454983f, 0.992099313f, + 0.128498111f, 0.991709754f, + 0.131540029f, 0.991310860f, + 0.134580709f, 0.990902635f, + 0.137620122f, 0.990485084f, + 0.140658239f, 0.990058210f, + 0.143695033f, 0.989622017f, + 0.146730474f, 0.989176510f, + 0.149764535f, 0.988721692f, + 0.152797185f, 0.988257568f, + 0.155828398f, 0.987784142f, + 0.158858143f, 0.987301418f, + 0.161886394f, 0.986809402f, + 0.164913120f, 0.986308097f, + 0.167938295f, 0.985797509f, + 0.170961889f, 0.985277642f, + 0.173983873f, 0.984748502f, + 0.177004220f, 0.984210092f, + 0.180022901f, 0.983662419f, + 0.183039888f, 0.983105487f, + 0.186055152f, 0.982539302f, + 0.189068664f, 0.981963869f, + 0.192080397f, 0.981379193f, + 0.195090322f, 0.980785280f, + 0.198098411f, 0.980182136f, + 0.201104635f, 0.979569766f, + 0.204108966f, 0.978948175f, + 0.207111376f, 0.978317371f, + 0.210111837f, 0.977677358f, + 0.213110320f, 0.977028143f, + 0.216106797f, 0.976369731f, + 0.219101240f, 0.975702130f, + 0.222093621f, 0.975025345f, + 0.225083911f, 0.974339383f, + 0.228072083f, 0.973644250f, + 0.231058108f, 0.972939952f, + 0.234041959f, 0.972226497f, + 0.237023606f, 0.971503891f, + 0.240003022f, 0.970772141f, + 0.242980180f, 0.970031253f, + 0.245955050f, 0.969281235f, + 0.248927606f, 0.968522094f, + 0.251897818f, 0.967753837f, + 0.254865660f, 0.966976471f, + 0.257831102f, 0.966190003f, + 0.260794118f, 0.965394442f, + 0.263754679f, 0.964589793f, + 0.266712757f, 0.963776066f, + 0.269668326f, 0.962953267f, + 0.272621355f, 0.962121404f, + 0.275571819f, 0.961280486f, + 0.278519689f, 0.960430519f, + 0.281464938f, 0.959571513f, + 0.284407537f, 0.958703475f, + 0.287347460f, 0.957826413f, + 0.290284677f, 0.956940336f, + 0.293219163f, 0.956045251f, + 0.296150888f, 0.955141168f, + 0.299079826f, 0.954228095f, + 0.302005949f, 0.953306040f, + 0.304929230f, 0.952375013f, + 0.307849640f, 0.951435021f, + 0.310767153f, 0.950486074f, + 0.313681740f, 0.949528181f, + 0.316593376f, 0.948561350f, + 0.319502031f, 0.947585591f, + 0.322407679f, 0.946600913f, + 0.325310292f, 0.945607325f, + 0.328209844f, 0.944604837f, + 0.331106306f, 0.943593458f, + 0.333999651f, 0.942573198f, + 0.336889853f, 0.941544065f, + 0.339776884f, 0.940506071f, + 0.342660717f, 0.939459224f, + 0.345541325f, 0.938403534f, + 0.348418680f, 0.937339012f, + 0.351292756f, 0.936265667f, + 0.354163525f, 0.935183510f, + 0.357030961f, 0.934092550f, + 0.359895037f, 0.932992799f, + 0.362755724f, 0.931884266f, + 0.365612998f, 0.930766961f, + 0.368466830f, 0.929640896f, + 0.371317194f, 0.928506080f, + 0.374164063f, 0.927362526f, + 0.377007410f, 0.926210242f, + 0.379847209f, 0.925049241f, + 0.382683432f, 0.923879533f, + 0.385516054f, 0.922701128f, + 0.388345047f, 0.921514039f, + 0.391170384f, 0.920318277f, + 0.393992040f, 0.919113852f, + 0.396809987f, 0.917900776f, + 0.399624200f, 0.916679060f, + 0.402434651f, 0.915448716f, + 0.405241314f, 0.914209756f, + 0.408044163f, 0.912962190f, + 0.410843171f, 0.911706032f, + 0.413638312f, 0.910441292f, + 0.416429560f, 0.909167983f, + 0.419216888f, 0.907886116f, + 0.422000271f, 0.906595705f, + 0.424779681f, 0.905296759f, + 0.427555093f, 0.903989293f, + 0.430326481f, 0.902673318f, + 0.433093819f, 0.901348847f, + 0.435857080f, 0.900015892f, + 0.438616239f, 0.898674466f, + 0.441371269f, 0.897324581f, + 0.444122145f, 0.895966250f, + 0.446868840f, 0.894599486f, + 0.449611330f, 0.893224301f, + 0.452349587f, 0.891840709f, + 0.455083587f, 0.890448723f, + 0.457813304f, 0.889048356f, + 0.460538711f, 0.887639620f, + 0.463259784f, 0.886222530f, + 0.465976496f, 0.884797098f, + 0.468688822f, 0.883363339f, + 0.471396737f, 0.881921264f, + 0.474100215f, 0.880470889f, + 0.476799230f, 0.879012226f, + 0.479493758f, 0.877545290f, + 0.482183772f, 0.876070094f, + 0.484869248f, 0.874586652f, + 0.487550160f, 0.873094978f, + 0.490226483f, 0.871595087f, + 0.492898192f, 0.870086991f, + 0.495565262f, 0.868570706f, + 0.498227667f, 0.867046246f, + 0.500885383f, 0.865513624f, + 0.503538384f, 0.863972856f, + 0.506186645f, 0.862423956f, + 0.508830143f, 0.860866939f, + 0.511468850f, 0.859301818f, + 0.514102744f, 0.857728610f, + 0.516731799f, 0.856147328f, + 0.519355990f, 0.854557988f, + 0.521975293f, 0.852960605f, + 0.524589683f, 0.851355193f, + 0.527199135f, 0.849741768f, + 0.529803625f, 0.848120345f, + 0.532403128f, 0.846490939f, + 0.534997620f, 0.844853565f, + 0.537587076f, 0.843208240f, + 0.540171473f, 0.841554977f, + 0.542750785f, 0.839893794f, + 0.545324988f, 0.838224706f, + 0.547894059f, 0.836547727f, + 0.550457973f, 0.834862875f, + 0.553016706f, 0.833170165f, + 0.555570233f, 0.831469612f, + 0.558118531f, 0.829761234f, + 0.560661576f, 0.828045045f, + 0.563199344f, 0.826321063f, + 0.565731811f, 0.824589303f, + 0.568258953f, 0.822849781f, + 0.570780746f, 0.821102515f, + 0.573297167f, 0.819347520f, + 0.575808191f, 0.817584813f, + 0.578313796f, 0.815814411f, + 0.580813958f, 0.814036330f, + 0.583308653f, 0.812250587f, + 0.585797857f, 0.810457198f, + 0.588281548f, 0.808656182f, + 0.590759702f, 0.806847554f, + 0.593232295f, 0.805031331f, + 0.595699304f, 0.803207531f, + 0.598160707f, 0.801376172f, + 0.600616479f, 0.799537269f, + 0.603066599f, 0.797690841f, + 0.605511041f, 0.795836905f, + 0.607949785f, 0.793975478f, + 0.610382806f, 0.792106577f, + 0.612810082f, 0.790230221f, + 0.615231591f, 0.788346428f, + 0.617647308f, 0.786455214f, + 0.620057212f, 0.784556597f, + 0.622461279f, 0.782650596f, + 0.624859488f, 0.780737229f, + 0.627251815f, 0.778816512f, + 0.629638239f, 0.776888466f, + 0.632018736f, 0.774953107f, + 0.634393284f, 0.773010453f, + 0.636761861f, 0.771060524f, + 0.639124445f, 0.769103338f, + 0.641481013f, 0.767138912f, + 0.643831543f, 0.765167266f, + 0.646176013f, 0.763188417f, + 0.648514401f, 0.761202385f, + 0.650846685f, 0.759209189f, + 0.653172843f, 0.757208847f, + 0.655492853f, 0.755201377f, + 0.657806693f, 0.753186799f, + 0.660114342f, 0.751165132f, + 0.662415778f, 0.749136395f, + 0.664710978f, 0.747100606f, + 0.666999922f, 0.745057785f, + 0.669282588f, 0.743007952f, + 0.671558955f, 0.740951125f, + 0.673829000f, 0.738887324f, + 0.676092704f, 0.736816569f, + 0.678350043f, 0.734738878f, + 0.680600998f, 0.732654272f, + 0.682845546f, 0.730562769f, + 0.685083668f, 0.728464390f, + 0.687315341f, 0.726359155f, + 0.689540545f, 0.724247083f, + 0.691759258f, 0.722128194f, + 0.693971461f, 0.720002508f, + 0.696177131f, 0.717870045f, + 0.698376249f, 0.715730825f, + 0.700568794f, 0.713584869f, + 0.702754744f, 0.711432196f, + 0.704934080f, 0.709272826f, + 0.707106781f, 0.707106781f, + 0.709272826f, 0.704934080f, + 0.711432196f, 0.702754744f, + 0.713584869f, 0.700568794f, + 0.715730825f, 0.698376249f, + 0.717870045f, 0.696177131f, + 0.720002508f, 0.693971461f, + 0.722128194f, 0.691759258f, + 0.724247083f, 0.689540545f, + 0.726359155f, 0.687315341f, + 0.728464390f, 0.685083668f, + 0.730562769f, 0.682845546f, + 0.732654272f, 0.680600998f, + 0.734738878f, 0.678350043f, + 0.736816569f, 0.676092704f, + 0.738887324f, 0.673829000f, + 0.740951125f, 0.671558955f, + 0.743007952f, 0.669282588f, + 0.745057785f, 0.666999922f, + 0.747100606f, 0.664710978f, + 0.749136395f, 0.662415778f, + 0.751165132f, 0.660114342f, + 0.753186799f, 0.657806693f, + 0.755201377f, 0.655492853f, + 0.757208847f, 0.653172843f, + 0.759209189f, 0.650846685f, + 0.761202385f, 0.648514401f, + 0.763188417f, 0.646176013f, + 0.765167266f, 0.643831543f, + 0.767138912f, 0.641481013f, + 0.769103338f, 0.639124445f, + 0.771060524f, 0.636761861f, + 0.773010453f, 0.634393284f, + 0.774953107f, 0.632018736f, + 0.776888466f, 0.629638239f, + 0.778816512f, 0.627251815f, + 0.780737229f, 0.624859488f, + 0.782650596f, 0.622461279f, + 0.784556597f, 0.620057212f, + 0.786455214f, 0.617647308f, + 0.788346428f, 0.615231591f, + 0.790230221f, 0.612810082f, + 0.792106577f, 0.610382806f, + 0.793975478f, 0.607949785f, + 0.795836905f, 0.605511041f, + 0.797690841f, 0.603066599f, + 0.799537269f, 0.600616479f, + 0.801376172f, 0.598160707f, + 0.803207531f, 0.595699304f, + 0.805031331f, 0.593232295f, + 0.806847554f, 0.590759702f, + 0.808656182f, 0.588281548f, + 0.810457198f, 0.585797857f, + 0.812250587f, 0.583308653f, + 0.814036330f, 0.580813958f, + 0.815814411f, 0.578313796f, + 0.817584813f, 0.575808191f, + 0.819347520f, 0.573297167f, + 0.821102515f, 0.570780746f, + 0.822849781f, 0.568258953f, + 0.824589303f, 0.565731811f, + 0.826321063f, 0.563199344f, + 0.828045045f, 0.560661576f, + 0.829761234f, 0.558118531f, + 0.831469612f, 0.555570233f, + 0.833170165f, 0.553016706f, + 0.834862875f, 0.550457973f, + 0.836547727f, 0.547894059f, + 0.838224706f, 0.545324988f, + 0.839893794f, 0.542750785f, + 0.841554977f, 0.540171473f, + 0.843208240f, 0.537587076f, + 0.844853565f, 0.534997620f, + 0.846490939f, 0.532403128f, + 0.848120345f, 0.529803625f, + 0.849741768f, 0.527199135f, + 0.851355193f, 0.524589683f, + 0.852960605f, 0.521975293f, + 0.854557988f, 0.519355990f, + 0.856147328f, 0.516731799f, + 0.857728610f, 0.514102744f, + 0.859301818f, 0.511468850f, + 0.860866939f, 0.508830143f, + 0.862423956f, 0.506186645f, + 0.863972856f, 0.503538384f, + 0.865513624f, 0.500885383f, + 0.867046246f, 0.498227667f, + 0.868570706f, 0.495565262f, + 0.870086991f, 0.492898192f, + 0.871595087f, 0.490226483f, + 0.873094978f, 0.487550160f, + 0.874586652f, 0.484869248f, + 0.876070094f, 0.482183772f, + 0.877545290f, 0.479493758f, + 0.879012226f, 0.476799230f, + 0.880470889f, 0.474100215f, + 0.881921264f, 0.471396737f, + 0.883363339f, 0.468688822f, + 0.884797098f, 0.465976496f, + 0.886222530f, 0.463259784f, + 0.887639620f, 0.460538711f, + 0.889048356f, 0.457813304f, + 0.890448723f, 0.455083587f, + 0.891840709f, 0.452349587f, + 0.893224301f, 0.449611330f, + 0.894599486f, 0.446868840f, + 0.895966250f, 0.444122145f, + 0.897324581f, 0.441371269f, + 0.898674466f, 0.438616239f, + 0.900015892f, 0.435857080f, + 0.901348847f, 0.433093819f, + 0.902673318f, 0.430326481f, + 0.903989293f, 0.427555093f, + 0.905296759f, 0.424779681f, + 0.906595705f, 0.422000271f, + 0.907886116f, 0.419216888f, + 0.909167983f, 0.416429560f, + 0.910441292f, 0.413638312f, + 0.911706032f, 0.410843171f, + 0.912962190f, 0.408044163f, + 0.914209756f, 0.405241314f, + 0.915448716f, 0.402434651f, + 0.916679060f, 0.399624200f, + 0.917900776f, 0.396809987f, + 0.919113852f, 0.393992040f, + 0.920318277f, 0.391170384f, + 0.921514039f, 0.388345047f, + 0.922701128f, 0.385516054f, + 0.923879533f, 0.382683432f, + 0.925049241f, 0.379847209f, + 0.926210242f, 0.377007410f, + 0.927362526f, 0.374164063f, + 0.928506080f, 0.371317194f, + 0.929640896f, 0.368466830f, + 0.930766961f, 0.365612998f, + 0.931884266f, 0.362755724f, + 0.932992799f, 0.359895037f, + 0.934092550f, 0.357030961f, + 0.935183510f, 0.354163525f, + 0.936265667f, 0.351292756f, + 0.937339012f, 0.348418680f, + 0.938403534f, 0.345541325f, + 0.939459224f, 0.342660717f, + 0.940506071f, 0.339776884f, + 0.941544065f, 0.336889853f, + 0.942573198f, 0.333999651f, + 0.943593458f, 0.331106306f, + 0.944604837f, 0.328209844f, + 0.945607325f, 0.325310292f, + 0.946600913f, 0.322407679f, + 0.947585591f, 0.319502031f, + 0.948561350f, 0.316593376f, + 0.949528181f, 0.313681740f, + 0.950486074f, 0.310767153f, + 0.951435021f, 0.307849640f, + 0.952375013f, 0.304929230f, + 0.953306040f, 0.302005949f, + 0.954228095f, 0.299079826f, + 0.955141168f, 0.296150888f, + 0.956045251f, 0.293219163f, + 0.956940336f, 0.290284677f, + 0.957826413f, 0.287347460f, + 0.958703475f, 0.284407537f, + 0.959571513f, 0.281464938f, + 0.960430519f, 0.278519689f, + 0.961280486f, 0.275571819f, + 0.962121404f, 0.272621355f, + 0.962953267f, 0.269668326f, + 0.963776066f, 0.266712757f, + 0.964589793f, 0.263754679f, + 0.965394442f, 0.260794118f, + 0.966190003f, 0.257831102f, + 0.966976471f, 0.254865660f, + 0.967753837f, 0.251897818f, + 0.968522094f, 0.248927606f, + 0.969281235f, 0.245955050f, + 0.970031253f, 0.242980180f, + 0.970772141f, 0.240003022f, + 0.971503891f, 0.237023606f, + 0.972226497f, 0.234041959f, + 0.972939952f, 0.231058108f, + 0.973644250f, 0.228072083f, + 0.974339383f, 0.225083911f, + 0.975025345f, 0.222093621f, + 0.975702130f, 0.219101240f, + 0.976369731f, 0.216106797f, + 0.977028143f, 0.213110320f, + 0.977677358f, 0.210111837f, + 0.978317371f, 0.207111376f, + 0.978948175f, 0.204108966f, + 0.979569766f, 0.201104635f, + 0.980182136f, 0.198098411f, + 0.980785280f, 0.195090322f, + 0.981379193f, 0.192080397f, + 0.981963869f, 0.189068664f, + 0.982539302f, 0.186055152f, + 0.983105487f, 0.183039888f, + 0.983662419f, 0.180022901f, + 0.984210092f, 0.177004220f, + 0.984748502f, 0.173983873f, + 0.985277642f, 0.170961889f, + 0.985797509f, 0.167938295f, + 0.986308097f, 0.164913120f, + 0.986809402f, 0.161886394f, + 0.987301418f, 0.158858143f, + 0.987784142f, 0.155828398f, + 0.988257568f, 0.152797185f, + 0.988721692f, 0.149764535f, + 0.989176510f, 0.146730474f, + 0.989622017f, 0.143695033f, + 0.990058210f, 0.140658239f, + 0.990485084f, 0.137620122f, + 0.990902635f, 0.134580709f, + 0.991310860f, 0.131540029f, + 0.991709754f, 0.128498111f, + 0.992099313f, 0.125454983f, + 0.992479535f, 0.122410675f, + 0.992850414f, 0.119365215f, + 0.993211949f, 0.116318631f, + 0.993564136f, 0.113270952f, + 0.993906970f, 0.110222207f, + 0.994240449f, 0.107172425f, + 0.994564571f, 0.104121634f, + 0.994879331f, 0.101069863f, + 0.995184727f, 0.098017140f, + 0.995480755f, 0.094963495f, + 0.995767414f, 0.091908956f, + 0.996044701f, 0.088853553f, + 0.996312612f, 0.085797312f, + 0.996571146f, 0.082740265f, + 0.996820299f, 0.079682438f, + 0.997060070f, 0.076623861f, + 0.997290457f, 0.073564564f, + 0.997511456f, 0.070504573f, + 0.997723067f, 0.067443920f, + 0.997925286f, 0.064382631f, + 0.998118113f, 0.061320736f, + 0.998301545f, 0.058258265f, + 0.998475581f, 0.055195244f, + 0.998640218f, 0.052131705f, + 0.998795456f, 0.049067674f, + 0.998941293f, 0.046003182f, + 0.999077728f, 0.042938257f, + 0.999204759f, 0.039872928f, + 0.999322385f, 0.036807223f, + 0.999430605f, 0.033741172f, + 0.999529418f, 0.030674803f, + 0.999618822f, 0.027608146f, + 0.999698819f, 0.024541229f, + 0.999769405f, 0.021474080f, + 0.999830582f, 0.018406730f, + 0.999882347f, 0.015339206f, + 0.999924702f, 0.012271538f, + 0.999957645f, 0.009203755f, + 0.999981175f, 0.006135885f, + 0.999995294f, 0.003067957f, + 1.000000000f, 0.000000000f, + 0.999995294f, -0.003067957f, + 0.999981175f, -0.006135885f, + 0.999957645f, -0.009203755f, + 0.999924702f, -0.012271538f, + 0.999882347f, -0.015339206f, + 0.999830582f, -0.018406730f, + 0.999769405f, -0.021474080f, + 0.999698819f, -0.024541229f, + 0.999618822f, -0.027608146f, + 0.999529418f, -0.030674803f, + 0.999430605f, -0.033741172f, + 0.999322385f, -0.036807223f, + 0.999204759f, -0.039872928f, + 0.999077728f, -0.042938257f, + 0.998941293f, -0.046003182f, + 0.998795456f, -0.049067674f, + 0.998640218f, -0.052131705f, + 0.998475581f, -0.055195244f, + 0.998301545f, -0.058258265f, + 0.998118113f, -0.061320736f, + 0.997925286f, -0.064382631f, + 0.997723067f, -0.067443920f, + 0.997511456f, -0.070504573f, + 0.997290457f, -0.073564564f, + 0.997060070f, -0.076623861f, + 0.996820299f, -0.079682438f, + 0.996571146f, -0.082740265f, + 0.996312612f, -0.085797312f, + 0.996044701f, -0.088853553f, + 0.995767414f, -0.091908956f, + 0.995480755f, -0.094963495f, + 0.995184727f, -0.098017140f, + 0.994879331f, -0.101069863f, + 0.994564571f, -0.104121634f, + 0.994240449f, -0.107172425f, + 0.993906970f, -0.110222207f, + 0.993564136f, -0.113270952f, + 0.993211949f, -0.116318631f, + 0.992850414f, -0.119365215f, + 0.992479535f, -0.122410675f, + 0.992099313f, -0.125454983f, + 0.991709754f, -0.128498111f, + 0.991310860f, -0.131540029f, + 0.990902635f, -0.134580709f, + 0.990485084f, -0.137620122f, + 0.990058210f, -0.140658239f, + 0.989622017f, -0.143695033f, + 0.989176510f, -0.146730474f, + 0.988721692f, -0.149764535f, + 0.988257568f, -0.152797185f, + 0.987784142f, -0.155828398f, + 0.987301418f, -0.158858143f, + 0.986809402f, -0.161886394f, + 0.986308097f, -0.164913120f, + 0.985797509f, -0.167938295f, + 0.985277642f, -0.170961889f, + 0.984748502f, -0.173983873f, + 0.984210092f, -0.177004220f, + 0.983662419f, -0.180022901f, + 0.983105487f, -0.183039888f, + 0.982539302f, -0.186055152f, + 0.981963869f, -0.189068664f, + 0.981379193f, -0.192080397f, + 0.980785280f, -0.195090322f, + 0.980182136f, -0.198098411f, + 0.979569766f, -0.201104635f, + 0.978948175f, -0.204108966f, + 0.978317371f, -0.207111376f, + 0.977677358f, -0.210111837f, + 0.977028143f, -0.213110320f, + 0.976369731f, -0.216106797f, + 0.975702130f, -0.219101240f, + 0.975025345f, -0.222093621f, + 0.974339383f, -0.225083911f, + 0.973644250f, -0.228072083f, + 0.972939952f, -0.231058108f, + 0.972226497f, -0.234041959f, + 0.971503891f, -0.237023606f, + 0.970772141f, -0.240003022f, + 0.970031253f, -0.242980180f, + 0.969281235f, -0.245955050f, + 0.968522094f, -0.248927606f, + 0.967753837f, -0.251897818f, + 0.966976471f, -0.254865660f, + 0.966190003f, -0.257831102f, + 0.965394442f, -0.260794118f, + 0.964589793f, -0.263754679f, + 0.963776066f, -0.266712757f, + 0.962953267f, -0.269668326f, + 0.962121404f, -0.272621355f, + 0.961280486f, -0.275571819f, + 0.960430519f, -0.278519689f, + 0.959571513f, -0.281464938f, + 0.958703475f, -0.284407537f, + 0.957826413f, -0.287347460f, + 0.956940336f, -0.290284677f, + 0.956045251f, -0.293219163f, + 0.955141168f, -0.296150888f, + 0.954228095f, -0.299079826f, + 0.953306040f, -0.302005949f, + 0.952375013f, -0.304929230f, + 0.951435021f, -0.307849640f, + 0.950486074f, -0.310767153f, + 0.949528181f, -0.313681740f, + 0.948561350f, -0.316593376f, + 0.947585591f, -0.319502031f, + 0.946600913f, -0.322407679f, + 0.945607325f, -0.325310292f, + 0.944604837f, -0.328209844f, + 0.943593458f, -0.331106306f, + 0.942573198f, -0.333999651f, + 0.941544065f, -0.336889853f, + 0.940506071f, -0.339776884f, + 0.939459224f, -0.342660717f, + 0.938403534f, -0.345541325f, + 0.937339012f, -0.348418680f, + 0.936265667f, -0.351292756f, + 0.935183510f, -0.354163525f, + 0.934092550f, -0.357030961f, + 0.932992799f, -0.359895037f, + 0.931884266f, -0.362755724f, + 0.930766961f, -0.365612998f, + 0.929640896f, -0.368466830f, + 0.928506080f, -0.371317194f, + 0.927362526f, -0.374164063f, + 0.926210242f, -0.377007410f, + 0.925049241f, -0.379847209f, + 0.923879533f, -0.382683432f, + 0.922701128f, -0.385516054f, + 0.921514039f, -0.388345047f, + 0.920318277f, -0.391170384f, + 0.919113852f, -0.393992040f, + 0.917900776f, -0.396809987f, + 0.916679060f, -0.399624200f, + 0.915448716f, -0.402434651f, + 0.914209756f, -0.405241314f, + 0.912962190f, -0.408044163f, + 0.911706032f, -0.410843171f, + 0.910441292f, -0.413638312f, + 0.909167983f, -0.416429560f, + 0.907886116f, -0.419216888f, + 0.906595705f, -0.422000271f, + 0.905296759f, -0.424779681f, + 0.903989293f, -0.427555093f, + 0.902673318f, -0.430326481f, + 0.901348847f, -0.433093819f, + 0.900015892f, -0.435857080f, + 0.898674466f, -0.438616239f, + 0.897324581f, -0.441371269f, + 0.895966250f, -0.444122145f, + 0.894599486f, -0.446868840f, + 0.893224301f, -0.449611330f, + 0.891840709f, -0.452349587f, + 0.890448723f, -0.455083587f, + 0.889048356f, -0.457813304f, + 0.887639620f, -0.460538711f, + 0.886222530f, -0.463259784f, + 0.884797098f, -0.465976496f, + 0.883363339f, -0.468688822f, + 0.881921264f, -0.471396737f, + 0.880470889f, -0.474100215f, + 0.879012226f, -0.476799230f, + 0.877545290f, -0.479493758f, + 0.876070094f, -0.482183772f, + 0.874586652f, -0.484869248f, + 0.873094978f, -0.487550160f, + 0.871595087f, -0.490226483f, + 0.870086991f, -0.492898192f, + 0.868570706f, -0.495565262f, + 0.867046246f, -0.498227667f, + 0.865513624f, -0.500885383f, + 0.863972856f, -0.503538384f, + 0.862423956f, -0.506186645f, + 0.860866939f, -0.508830143f, + 0.859301818f, -0.511468850f, + 0.857728610f, -0.514102744f, + 0.856147328f, -0.516731799f, + 0.854557988f, -0.519355990f, + 0.852960605f, -0.521975293f, + 0.851355193f, -0.524589683f, + 0.849741768f, -0.527199135f, + 0.848120345f, -0.529803625f, + 0.846490939f, -0.532403128f, + 0.844853565f, -0.534997620f, + 0.843208240f, -0.537587076f, + 0.841554977f, -0.540171473f, + 0.839893794f, -0.542750785f, + 0.838224706f, -0.545324988f, + 0.836547727f, -0.547894059f, + 0.834862875f, -0.550457973f, + 0.833170165f, -0.553016706f, + 0.831469612f, -0.555570233f, + 0.829761234f, -0.558118531f, + 0.828045045f, -0.560661576f, + 0.826321063f, -0.563199344f, + 0.824589303f, -0.565731811f, + 0.822849781f, -0.568258953f, + 0.821102515f, -0.570780746f, + 0.819347520f, -0.573297167f, + 0.817584813f, -0.575808191f, + 0.815814411f, -0.578313796f, + 0.814036330f, -0.580813958f, + 0.812250587f, -0.583308653f, + 0.810457198f, -0.585797857f, + 0.808656182f, -0.588281548f, + 0.806847554f, -0.590759702f, + 0.805031331f, -0.593232295f, + 0.803207531f, -0.595699304f, + 0.801376172f, -0.598160707f, + 0.799537269f, -0.600616479f, + 0.797690841f, -0.603066599f, + 0.795836905f, -0.605511041f, + 0.793975478f, -0.607949785f, + 0.792106577f, -0.610382806f, + 0.790230221f, -0.612810082f, + 0.788346428f, -0.615231591f, + 0.786455214f, -0.617647308f, + 0.784556597f, -0.620057212f, + 0.782650596f, -0.622461279f, + 0.780737229f, -0.624859488f, + 0.778816512f, -0.627251815f, + 0.776888466f, -0.629638239f, + 0.774953107f, -0.632018736f, + 0.773010453f, -0.634393284f, + 0.771060524f, -0.636761861f, + 0.769103338f, -0.639124445f, + 0.767138912f, -0.641481013f, + 0.765167266f, -0.643831543f, + 0.763188417f, -0.646176013f, + 0.761202385f, -0.648514401f, + 0.759209189f, -0.650846685f, + 0.757208847f, -0.653172843f, + 0.755201377f, -0.655492853f, + 0.753186799f, -0.657806693f, + 0.751165132f, -0.660114342f, + 0.749136395f, -0.662415778f, + 0.747100606f, -0.664710978f, + 0.745057785f, -0.666999922f, + 0.743007952f, -0.669282588f, + 0.740951125f, -0.671558955f, + 0.738887324f, -0.673829000f, + 0.736816569f, -0.676092704f, + 0.734738878f, -0.678350043f, + 0.732654272f, -0.680600998f, + 0.730562769f, -0.682845546f, + 0.728464390f, -0.685083668f, + 0.726359155f, -0.687315341f, + 0.724247083f, -0.689540545f, + 0.722128194f, -0.691759258f, + 0.720002508f, -0.693971461f, + 0.717870045f, -0.696177131f, + 0.715730825f, -0.698376249f, + 0.713584869f, -0.700568794f, + 0.711432196f, -0.702754744f, + 0.709272826f, -0.704934080f, + 0.707106781f, -0.707106781f, + 0.704934080f, -0.709272826f, + 0.702754744f, -0.711432196f, + 0.700568794f, -0.713584869f, + 0.698376249f, -0.715730825f, + 0.696177131f, -0.717870045f, + 0.693971461f, -0.720002508f, + 0.691759258f, -0.722128194f, + 0.689540545f, -0.724247083f, + 0.687315341f, -0.726359155f, + 0.685083668f, -0.728464390f, + 0.682845546f, -0.730562769f, + 0.680600998f, -0.732654272f, + 0.678350043f, -0.734738878f, + 0.676092704f, -0.736816569f, + 0.673829000f, -0.738887324f, + 0.671558955f, -0.740951125f, + 0.669282588f, -0.743007952f, + 0.666999922f, -0.745057785f, + 0.664710978f, -0.747100606f, + 0.662415778f, -0.749136395f, + 0.660114342f, -0.751165132f, + 0.657806693f, -0.753186799f, + 0.655492853f, -0.755201377f, + 0.653172843f, -0.757208847f, + 0.650846685f, -0.759209189f, + 0.648514401f, -0.761202385f, + 0.646176013f, -0.763188417f, + 0.643831543f, -0.765167266f, + 0.641481013f, -0.767138912f, + 0.639124445f, -0.769103338f, + 0.636761861f, -0.771060524f, + 0.634393284f, -0.773010453f, + 0.632018736f, -0.774953107f, + 0.629638239f, -0.776888466f, + 0.627251815f, -0.778816512f, + 0.624859488f, -0.780737229f, + 0.622461279f, -0.782650596f, + 0.620057212f, -0.784556597f, + 0.617647308f, -0.786455214f, + 0.615231591f, -0.788346428f, + 0.612810082f, -0.790230221f, + 0.610382806f, -0.792106577f, + 0.607949785f, -0.793975478f, + 0.605511041f, -0.795836905f, + 0.603066599f, -0.797690841f, + 0.600616479f, -0.799537269f, + 0.598160707f, -0.801376172f, + 0.595699304f, -0.803207531f, + 0.593232295f, -0.805031331f, + 0.590759702f, -0.806847554f, + 0.588281548f, -0.808656182f, + 0.585797857f, -0.810457198f, + 0.583308653f, -0.812250587f, + 0.580813958f, -0.814036330f, + 0.578313796f, -0.815814411f, + 0.575808191f, -0.817584813f, + 0.573297167f, -0.819347520f, + 0.570780746f, -0.821102515f, + 0.568258953f, -0.822849781f, + 0.565731811f, -0.824589303f, + 0.563199344f, -0.826321063f, + 0.560661576f, -0.828045045f, + 0.558118531f, -0.829761234f, + 0.555570233f, -0.831469612f, + 0.553016706f, -0.833170165f, + 0.550457973f, -0.834862875f, + 0.547894059f, -0.836547727f, + 0.545324988f, -0.838224706f, + 0.542750785f, -0.839893794f, + 0.540171473f, -0.841554977f, + 0.537587076f, -0.843208240f, + 0.534997620f, -0.844853565f, + 0.532403128f, -0.846490939f, + 0.529803625f, -0.848120345f, + 0.527199135f, -0.849741768f, + 0.524589683f, -0.851355193f, + 0.521975293f, -0.852960605f, + 0.519355990f, -0.854557988f, + 0.516731799f, -0.856147328f, + 0.514102744f, -0.857728610f, + 0.511468850f, -0.859301818f, + 0.508830143f, -0.860866939f, + 0.506186645f, -0.862423956f, + 0.503538384f, -0.863972856f, + 0.500885383f, -0.865513624f, + 0.498227667f, -0.867046246f, + 0.495565262f, -0.868570706f, + 0.492898192f, -0.870086991f, + 0.490226483f, -0.871595087f, + 0.487550160f, -0.873094978f, + 0.484869248f, -0.874586652f, + 0.482183772f, -0.876070094f, + 0.479493758f, -0.877545290f, + 0.476799230f, -0.879012226f, + 0.474100215f, -0.880470889f, + 0.471396737f, -0.881921264f, + 0.468688822f, -0.883363339f, + 0.465976496f, -0.884797098f, + 0.463259784f, -0.886222530f, + 0.460538711f, -0.887639620f, + 0.457813304f, -0.889048356f, + 0.455083587f, -0.890448723f, + 0.452349587f, -0.891840709f, + 0.449611330f, -0.893224301f, + 0.446868840f, -0.894599486f, + 0.444122145f, -0.895966250f, + 0.441371269f, -0.897324581f, + 0.438616239f, -0.898674466f, + 0.435857080f, -0.900015892f, + 0.433093819f, -0.901348847f, + 0.430326481f, -0.902673318f, + 0.427555093f, -0.903989293f, + 0.424779681f, -0.905296759f, + 0.422000271f, -0.906595705f, + 0.419216888f, -0.907886116f, + 0.416429560f, -0.909167983f, + 0.413638312f, -0.910441292f, + 0.410843171f, -0.911706032f, + 0.408044163f, -0.912962190f, + 0.405241314f, -0.914209756f, + 0.402434651f, -0.915448716f, + 0.399624200f, -0.916679060f, + 0.396809987f, -0.917900776f, + 0.393992040f, -0.919113852f, + 0.391170384f, -0.920318277f, + 0.388345047f, -0.921514039f, + 0.385516054f, -0.922701128f, + 0.382683432f, -0.923879533f, + 0.379847209f, -0.925049241f, + 0.377007410f, -0.926210242f, + 0.374164063f, -0.927362526f, + 0.371317194f, -0.928506080f, + 0.368466830f, -0.929640896f, + 0.365612998f, -0.930766961f, + 0.362755724f, -0.931884266f, + 0.359895037f, -0.932992799f, + 0.357030961f, -0.934092550f, + 0.354163525f, -0.935183510f, + 0.351292756f, -0.936265667f, + 0.348418680f, -0.937339012f, + 0.345541325f, -0.938403534f, + 0.342660717f, -0.939459224f, + 0.339776884f, -0.940506071f, + 0.336889853f, -0.941544065f, + 0.333999651f, -0.942573198f, + 0.331106306f, -0.943593458f, + 0.328209844f, -0.944604837f, + 0.325310292f, -0.945607325f, + 0.322407679f, -0.946600913f, + 0.319502031f, -0.947585591f, + 0.316593376f, -0.948561350f, + 0.313681740f, -0.949528181f, + 0.310767153f, -0.950486074f, + 0.307849640f, -0.951435021f, + 0.304929230f, -0.952375013f, + 0.302005949f, -0.953306040f, + 0.299079826f, -0.954228095f, + 0.296150888f, -0.955141168f, + 0.293219163f, -0.956045251f, + 0.290284677f, -0.956940336f, + 0.287347460f, -0.957826413f, + 0.284407537f, -0.958703475f, + 0.281464938f, -0.959571513f, + 0.278519689f, -0.960430519f, + 0.275571819f, -0.961280486f, + 0.272621355f, -0.962121404f, + 0.269668326f, -0.962953267f, + 0.266712757f, -0.963776066f, + 0.263754679f, -0.964589793f, + 0.260794118f, -0.965394442f, + 0.257831102f, -0.966190003f, + 0.254865660f, -0.966976471f, + 0.251897818f, -0.967753837f, + 0.248927606f, -0.968522094f, + 0.245955050f, -0.969281235f, + 0.242980180f, -0.970031253f, + 0.240003022f, -0.970772141f, + 0.237023606f, -0.971503891f, + 0.234041959f, -0.972226497f, + 0.231058108f, -0.972939952f, + 0.228072083f, -0.973644250f, + 0.225083911f, -0.974339383f, + 0.222093621f, -0.975025345f, + 0.219101240f, -0.975702130f, + 0.216106797f, -0.976369731f, + 0.213110320f, -0.977028143f, + 0.210111837f, -0.977677358f, + 0.207111376f, -0.978317371f, + 0.204108966f, -0.978948175f, + 0.201104635f, -0.979569766f, + 0.198098411f, -0.980182136f, + 0.195090322f, -0.980785280f, + 0.192080397f, -0.981379193f, + 0.189068664f, -0.981963869f, + 0.186055152f, -0.982539302f, + 0.183039888f, -0.983105487f, + 0.180022901f, -0.983662419f, + 0.177004220f, -0.984210092f, + 0.173983873f, -0.984748502f, + 0.170961889f, -0.985277642f, + 0.167938295f, -0.985797509f, + 0.164913120f, -0.986308097f, + 0.161886394f, -0.986809402f, + 0.158858143f, -0.987301418f, + 0.155828398f, -0.987784142f, + 0.152797185f, -0.988257568f, + 0.149764535f, -0.988721692f, + 0.146730474f, -0.989176510f, + 0.143695033f, -0.989622017f, + 0.140658239f, -0.990058210f, + 0.137620122f, -0.990485084f, + 0.134580709f, -0.990902635f, + 0.131540029f, -0.991310860f, + 0.128498111f, -0.991709754f, + 0.125454983f, -0.992099313f, + 0.122410675f, -0.992479535f, + 0.119365215f, -0.992850414f, + 0.116318631f, -0.993211949f, + 0.113270952f, -0.993564136f, + 0.110222207f, -0.993906970f, + 0.107172425f, -0.994240449f, + 0.104121634f, -0.994564571f, + 0.101069863f, -0.994879331f, + 0.098017140f, -0.995184727f, + 0.094963495f, -0.995480755f, + 0.091908956f, -0.995767414f, + 0.088853553f, -0.996044701f, + 0.085797312f, -0.996312612f, + 0.082740265f, -0.996571146f, + 0.079682438f, -0.996820299f, + 0.076623861f, -0.997060070f, + 0.073564564f, -0.997290457f, + 0.070504573f, -0.997511456f, + 0.067443920f, -0.997723067f, + 0.064382631f, -0.997925286f, + 0.061320736f, -0.998118113f, + 0.058258265f, -0.998301545f, + 0.055195244f, -0.998475581f, + 0.052131705f, -0.998640218f, + 0.049067674f, -0.998795456f, + 0.046003182f, -0.998941293f, + 0.042938257f, -0.999077728f, + 0.039872928f, -0.999204759f, + 0.036807223f, -0.999322385f, + 0.033741172f, -0.999430605f, + 0.030674803f, -0.999529418f, + 0.027608146f, -0.999618822f, + 0.024541229f, -0.999698819f, + 0.021474080f, -0.999769405f, + 0.018406730f, -0.999830582f, + 0.015339206f, -0.999882347f, + 0.012271538f, -0.999924702f, + 0.009203755f, -0.999957645f, + 0.006135885f, -0.999981175f, + 0.003067957f, -0.999995294f +}; + +const float32_t twiddleCoef_rfft_4096[4096] = { + 0.000000000f, 1.000000000f, + 0.001533980f, 0.999998823f, + 0.003067957f, 0.999995294f, + 0.004601926f, 0.999989411f, + 0.006135885f, 0.999981175f, + 0.007669829f, 0.999970586f, + 0.009203755f, 0.999957645f, + 0.010737659f, 0.999942350f, + 0.012271538f, 0.999924702f, + 0.013805389f, 0.999904701f, + 0.015339206f, 0.999882347f, + 0.016872988f, 0.999857641f, + 0.018406730f, 0.999830582f, + 0.019940429f, 0.999801170f, + 0.021474080f, 0.999769405f, + 0.023007681f, 0.999735288f, + 0.024541229f, 0.999698819f, + 0.026074718f, 0.999659997f, + 0.027608146f, 0.999618822f, + 0.029141509f, 0.999575296f, + 0.030674803f, 0.999529418f, + 0.032208025f, 0.999481187f, + 0.033741172f, 0.999430605f, + 0.035274239f, 0.999377670f, + 0.036807223f, 0.999322385f, + 0.038340120f, 0.999264747f, + 0.039872928f, 0.999204759f, + 0.041405641f, 0.999142419f, + 0.042938257f, 0.999077728f, + 0.044470772f, 0.999010686f, + 0.046003182f, 0.998941293f, + 0.047535484f, 0.998869550f, + 0.049067674f, 0.998795456f, + 0.050599749f, 0.998719012f, + 0.052131705f, 0.998640218f, + 0.053663538f, 0.998559074f, + 0.055195244f, 0.998475581f, + 0.056726821f, 0.998389737f, + 0.058258265f, 0.998301545f, + 0.059789571f, 0.998211003f, + 0.061320736f, 0.998118113f, + 0.062851758f, 0.998022874f, + 0.064382631f, 0.997925286f, + 0.065913353f, 0.997825350f, + 0.067443920f, 0.997723067f, + 0.068974328f, 0.997618435f, + 0.070504573f, 0.997511456f, + 0.072034653f, 0.997402130f, + 0.073564564f, 0.997290457f, + 0.075094301f, 0.997176437f, + 0.076623861f, 0.997060070f, + 0.078153242f, 0.996941358f, + 0.079682438f, 0.996820299f, + 0.081211447f, 0.996696895f, + 0.082740265f, 0.996571146f, + 0.084268888f, 0.996443051f, + 0.085797312f, 0.996312612f, + 0.087325535f, 0.996179829f, + 0.088853553f, 0.996044701f, + 0.090381361f, 0.995907229f, + 0.091908956f, 0.995767414f, + 0.093436336f, 0.995625256f, + 0.094963495f, 0.995480755f, + 0.096490431f, 0.995333912f, + 0.098017140f, 0.995184727f, + 0.099543619f, 0.995033199f, + 0.101069863f, 0.994879331f, + 0.102595869f, 0.994723121f, + 0.104121634f, 0.994564571f, + 0.105647154f, 0.994403680f, + 0.107172425f, 0.994240449f, + 0.108697444f, 0.994074879f, + 0.110222207f, 0.993906970f, + 0.111746711f, 0.993736722f, + 0.113270952f, 0.993564136f, + 0.114794927f, 0.993389211f, + 0.116318631f, 0.993211949f, + 0.117842062f, 0.993032350f, + 0.119365215f, 0.992850414f, + 0.120888087f, 0.992666142f, + 0.122410675f, 0.992479535f, + 0.123932975f, 0.992290591f, + 0.125454983f, 0.992099313f, + 0.126976696f, 0.991905700f, + 0.128498111f, 0.991709754f, + 0.130019223f, 0.991511473f, + 0.131540029f, 0.991310860f, + 0.133060525f, 0.991107914f, + 0.134580709f, 0.990902635f, + 0.136100575f, 0.990695025f, + 0.137620122f, 0.990485084f, + 0.139139344f, 0.990272812f, + 0.140658239f, 0.990058210f, + 0.142176804f, 0.989841278f, + 0.143695033f, 0.989622017f, + 0.145212925f, 0.989400428f, + 0.146730474f, 0.989176510f, + 0.148247679f, 0.988950265f, + 0.149764535f, 0.988721692f, + 0.151281038f, 0.988490793f, + 0.152797185f, 0.988257568f, + 0.154312973f, 0.988022017f, + 0.155828398f, 0.987784142f, + 0.157343456f, 0.987543942f, + 0.158858143f, 0.987301418f, + 0.160372457f, 0.987056571f, + 0.161886394f, 0.986809402f, + 0.163399949f, 0.986559910f, + 0.164913120f, 0.986308097f, + 0.166425904f, 0.986053963f, + 0.167938295f, 0.985797509f, + 0.169450291f, 0.985538735f, + 0.170961889f, 0.985277642f, + 0.172473084f, 0.985014231f, + 0.173983873f, 0.984748502f, + 0.175494253f, 0.984480455f, + 0.177004220f, 0.984210092f, + 0.178513771f, 0.983937413f, + 0.180022901f, 0.983662419f, + 0.181531608f, 0.983385110f, + 0.183039888f, 0.983105487f, + 0.184547737f, 0.982823551f, + 0.186055152f, 0.982539302f, + 0.187562129f, 0.982252741f, + 0.189068664f, 0.981963869f, + 0.190574755f, 0.981672686f, + 0.192080397f, 0.981379193f, + 0.193585587f, 0.981083391f, + 0.195090322f, 0.980785280f, + 0.196594598f, 0.980484862f, + 0.198098411f, 0.980182136f, + 0.199601758f, 0.979877104f, + 0.201104635f, 0.979569766f, + 0.202607039f, 0.979260123f, + 0.204108966f, 0.978948175f, + 0.205610413f, 0.978633924f, + 0.207111376f, 0.978317371f, + 0.208611852f, 0.977998515f, + 0.210111837f, 0.977677358f, + 0.211611327f, 0.977353900f, + 0.213110320f, 0.977028143f, + 0.214608811f, 0.976700086f, + 0.216106797f, 0.976369731f, + 0.217604275f, 0.976037079f, + 0.219101240f, 0.975702130f, + 0.220597690f, 0.975364885f, + 0.222093621f, 0.975025345f, + 0.223589029f, 0.974683511f, + 0.225083911f, 0.974339383f, + 0.226578264f, 0.973992962f, + 0.228072083f, 0.973644250f, + 0.229565366f, 0.973293246f, + 0.231058108f, 0.972939952f, + 0.232550307f, 0.972584369f, + 0.234041959f, 0.972226497f, + 0.235533059f, 0.971866337f, + 0.237023606f, 0.971503891f, + 0.238513595f, 0.971139158f, + 0.240003022f, 0.970772141f, + 0.241491885f, 0.970402839f, + 0.242980180f, 0.970031253f, + 0.244467903f, 0.969657385f, + 0.245955050f, 0.969281235f, + 0.247441619f, 0.968902805f, + 0.248927606f, 0.968522094f, + 0.250413007f, 0.968139105f, + 0.251897818f, 0.967753837f, + 0.253382037f, 0.967366292f, + 0.254865660f, 0.966976471f, + 0.256348682f, 0.966584374f, + 0.257831102f, 0.966190003f, + 0.259312915f, 0.965793359f, + 0.260794118f, 0.965394442f, + 0.262274707f, 0.964993253f, + 0.263754679f, 0.964589793f, + 0.265234030f, 0.964184064f, + 0.266712757f, 0.963776066f, + 0.268190857f, 0.963365800f, + 0.269668326f, 0.962953267f, + 0.271145160f, 0.962538468f, + 0.272621355f, 0.962121404f, + 0.274096910f, 0.961702077f, + 0.275571819f, 0.961280486f, + 0.277046080f, 0.960856633f, + 0.278519689f, 0.960430519f, + 0.279992643f, 0.960002146f, + 0.281464938f, 0.959571513f, + 0.282936570f, 0.959138622f, + 0.284407537f, 0.958703475f, + 0.285877835f, 0.958266071f, + 0.287347460f, 0.957826413f, + 0.288816408f, 0.957384501f, + 0.290284677f, 0.956940336f, + 0.291752263f, 0.956493919f, + 0.293219163f, 0.956045251f, + 0.294685372f, 0.955594334f, + 0.296150888f, 0.955141168f, + 0.297615707f, 0.954685755f, + 0.299079826f, 0.954228095f, + 0.300543241f, 0.953768190f, + 0.302005949f, 0.953306040f, + 0.303467947f, 0.952841648f, + 0.304929230f, 0.952375013f, + 0.306389795f, 0.951906137f, + 0.307849640f, 0.951435021f, + 0.309308760f, 0.950961666f, + 0.310767153f, 0.950486074f, + 0.312224814f, 0.950008245f, + 0.313681740f, 0.949528181f, + 0.315137929f, 0.949045882f, + 0.316593376f, 0.948561350f, + 0.318048077f, 0.948074586f, + 0.319502031f, 0.947585591f, + 0.320955232f, 0.947094366f, + 0.322407679f, 0.946600913f, + 0.323859367f, 0.946105232f, + 0.325310292f, 0.945607325f, + 0.326760452f, 0.945107193f, + 0.328209844f, 0.944604837f, + 0.329658463f, 0.944100258f, + 0.331106306f, 0.943593458f, + 0.332553370f, 0.943084437f, + 0.333999651f, 0.942573198f, + 0.335445147f, 0.942059740f, + 0.336889853f, 0.941544065f, + 0.338333767f, 0.941026175f, + 0.339776884f, 0.940506071f, + 0.341219202f, 0.939983753f, + 0.342660717f, 0.939459224f, + 0.344101426f, 0.938932484f, + 0.345541325f, 0.938403534f, + 0.346980411f, 0.937872376f, + 0.348418680f, 0.937339012f, + 0.349856130f, 0.936803442f, + 0.351292756f, 0.936265667f, + 0.352728556f, 0.935725689f, + 0.354163525f, 0.935183510f, + 0.355597662f, 0.934639130f, + 0.357030961f, 0.934092550f, + 0.358463421f, 0.933543773f, + 0.359895037f, 0.932992799f, + 0.361325806f, 0.932439629f, + 0.362755724f, 0.931884266f, + 0.364184790f, 0.931326709f, + 0.365612998f, 0.930766961f, + 0.367040346f, 0.930205023f, + 0.368466830f, 0.929640896f, + 0.369892447f, 0.929074581f, + 0.371317194f, 0.928506080f, + 0.372741067f, 0.927935395f, + 0.374164063f, 0.927362526f, + 0.375586178f, 0.926787474f, + 0.377007410f, 0.926210242f, + 0.378427755f, 0.925630831f, + 0.379847209f, 0.925049241f, + 0.381265769f, 0.924465474f, + 0.382683432f, 0.923879533f, + 0.384100195f, 0.923291417f, + 0.385516054f, 0.922701128f, + 0.386931006f, 0.922108669f, + 0.388345047f, 0.921514039f, + 0.389758174f, 0.920917242f, + 0.391170384f, 0.920318277f, + 0.392581674f, 0.919717146f, + 0.393992040f, 0.919113852f, + 0.395401479f, 0.918508394f, + 0.396809987f, 0.917900776f, + 0.398217562f, 0.917290997f, + 0.399624200f, 0.916679060f, + 0.401029897f, 0.916064966f, + 0.402434651f, 0.915448716f, + 0.403838458f, 0.914830312f, + 0.405241314f, 0.914209756f, + 0.406643217f, 0.913587048f, + 0.408044163f, 0.912962190f, + 0.409444149f, 0.912335185f, + 0.410843171f, 0.911706032f, + 0.412241227f, 0.911074734f, + 0.413638312f, 0.910441292f, + 0.415034424f, 0.909805708f, + 0.416429560f, 0.909167983f, + 0.417823716f, 0.908528119f, + 0.419216888f, 0.907886116f, + 0.420609074f, 0.907241978f, + 0.422000271f, 0.906595705f, + 0.423390474f, 0.905947298f, + 0.424779681f, 0.905296759f, + 0.426167889f, 0.904644091f, + 0.427555093f, 0.903989293f, + 0.428941292f, 0.903332368f, + 0.430326481f, 0.902673318f, + 0.431710658f, 0.902012144f, + 0.433093819f, 0.901348847f, + 0.434475961f, 0.900683429f, + 0.435857080f, 0.900015892f, + 0.437237174f, 0.899346237f, + 0.438616239f, 0.898674466f, + 0.439994271f, 0.898000580f, + 0.441371269f, 0.897324581f, + 0.442747228f, 0.896646470f, + 0.444122145f, 0.895966250f, + 0.445496017f, 0.895283921f, + 0.446868840f, 0.894599486f, + 0.448240612f, 0.893912945f, + 0.449611330f, 0.893224301f, + 0.450980989f, 0.892533555f, + 0.452349587f, 0.891840709f, + 0.453717121f, 0.891145765f, + 0.455083587f, 0.890448723f, + 0.456448982f, 0.889749586f, + 0.457813304f, 0.889048356f, + 0.459176548f, 0.888345033f, + 0.460538711f, 0.887639620f, + 0.461899791f, 0.886932119f, + 0.463259784f, 0.886222530f, + 0.464618686f, 0.885510856f, + 0.465976496f, 0.884797098f, + 0.467333209f, 0.884081259f, + 0.468688822f, 0.883363339f, + 0.470043332f, 0.882643340f, + 0.471396737f, 0.881921264f, + 0.472749032f, 0.881197113f, + 0.474100215f, 0.880470889f, + 0.475450282f, 0.879742593f, + 0.476799230f, 0.879012226f, + 0.478147056f, 0.878279792f, + 0.479493758f, 0.877545290f, + 0.480839331f, 0.876808724f, + 0.482183772f, 0.876070094f, + 0.483527079f, 0.875329403f, + 0.484869248f, 0.874586652f, + 0.486210276f, 0.873841843f, + 0.487550160f, 0.873094978f, + 0.488888897f, 0.872346059f, + 0.490226483f, 0.871595087f, + 0.491562916f, 0.870842063f, + 0.492898192f, 0.870086991f, + 0.494232309f, 0.869329871f, + 0.495565262f, 0.868570706f, + 0.496897049f, 0.867809497f, + 0.498227667f, 0.867046246f, + 0.499557113f, 0.866280954f, + 0.500885383f, 0.865513624f, + 0.502212474f, 0.864744258f, + 0.503538384f, 0.863972856f, + 0.504863109f, 0.863199422f, + 0.506186645f, 0.862423956f, + 0.507508991f, 0.861646461f, + 0.508830143f, 0.860866939f, + 0.510150097f, 0.860085390f, + 0.511468850f, 0.859301818f, + 0.512786401f, 0.858516224f, + 0.514102744f, 0.857728610f, + 0.515417878f, 0.856938977f, + 0.516731799f, 0.856147328f, + 0.518044504f, 0.855353665f, + 0.519355990f, 0.854557988f, + 0.520666254f, 0.853760301f, + 0.521975293f, 0.852960605f, + 0.523283103f, 0.852158902f, + 0.524589683f, 0.851355193f, + 0.525895027f, 0.850549481f, + 0.527199135f, 0.849741768f, + 0.528502002f, 0.848932055f, + 0.529803625f, 0.848120345f, + 0.531104001f, 0.847306639f, + 0.532403128f, 0.846490939f, + 0.533701002f, 0.845673247f, + 0.534997620f, 0.844853565f, + 0.536292979f, 0.844031895f, + 0.537587076f, 0.843208240f, + 0.538879909f, 0.842382600f, + 0.540171473f, 0.841554977f, + 0.541461766f, 0.840725375f, + 0.542750785f, 0.839893794f, + 0.544038527f, 0.839060237f, + 0.545324988f, 0.838224706f, + 0.546610167f, 0.837387202f, + 0.547894059f, 0.836547727f, + 0.549176662f, 0.835706284f, + 0.550457973f, 0.834862875f, + 0.551737988f, 0.834017501f, + 0.553016706f, 0.833170165f, + 0.554294121f, 0.832320868f, + 0.555570233f, 0.831469612f, + 0.556845037f, 0.830616400f, + 0.558118531f, 0.829761234f, + 0.559390712f, 0.828904115f, + 0.560661576f, 0.828045045f, + 0.561931121f, 0.827184027f, + 0.563199344f, 0.826321063f, + 0.564466242f, 0.825456154f, + 0.565731811f, 0.824589303f, + 0.566996049f, 0.823720511f, + 0.568258953f, 0.822849781f, + 0.569520519f, 0.821977115f, + 0.570780746f, 0.821102515f, + 0.572039629f, 0.820225983f, + 0.573297167f, 0.819347520f, + 0.574553355f, 0.818467130f, + 0.575808191f, 0.817584813f, + 0.577061673f, 0.816700573f, + 0.578313796f, 0.815814411f, + 0.579564559f, 0.814926329f, + 0.580813958f, 0.814036330f, + 0.582061990f, 0.813144415f, + 0.583308653f, 0.812250587f, + 0.584553943f, 0.811354847f, + 0.585797857f, 0.810457198f, + 0.587040394f, 0.809557642f, + 0.588281548f, 0.808656182f, + 0.589521319f, 0.807752818f, + 0.590759702f, 0.806847554f, + 0.591996695f, 0.805940391f, + 0.593232295f, 0.805031331f, + 0.594466499f, 0.804120377f, + 0.595699304f, 0.803207531f, + 0.596930708f, 0.802292796f, + 0.598160707f, 0.801376172f, + 0.599389298f, 0.800457662f, + 0.600616479f, 0.799537269f, + 0.601842247f, 0.798614995f, + 0.603066599f, 0.797690841f, + 0.604289531f, 0.796764810f, + 0.605511041f, 0.795836905f, + 0.606731127f, 0.794907126f, + 0.607949785f, 0.793975478f, + 0.609167012f, 0.793041960f, + 0.610382806f, 0.792106577f, + 0.611597164f, 0.791169330f, + 0.612810082f, 0.790230221f, + 0.614021559f, 0.789289253f, + 0.615231591f, 0.788346428f, + 0.616440175f, 0.787401747f, + 0.617647308f, 0.786455214f, + 0.618852988f, 0.785506830f, + 0.620057212f, 0.784556597f, + 0.621259977f, 0.783604519f, + 0.622461279f, 0.782650596f, + 0.623661118f, 0.781694832f, + 0.624859488f, 0.780737229f, + 0.626056388f, 0.779777788f, + 0.627251815f, 0.778816512f, + 0.628445767f, 0.777853404f, + 0.629638239f, 0.776888466f, + 0.630829230f, 0.775921699f, + 0.632018736f, 0.774953107f, + 0.633206755f, 0.773982691f, + 0.634393284f, 0.773010453f, + 0.635578320f, 0.772036397f, + 0.636761861f, 0.771060524f, + 0.637943904f, 0.770082837f, + 0.639124445f, 0.769103338f, + 0.640303482f, 0.768122029f, + 0.641481013f, 0.767138912f, + 0.642657034f, 0.766153990f, + 0.643831543f, 0.765167266f, + 0.645004537f, 0.764178741f, + 0.646176013f, 0.763188417f, + 0.647345969f, 0.762196298f, + 0.648514401f, 0.761202385f, + 0.649681307f, 0.760206682f, + 0.650846685f, 0.759209189f, + 0.652010531f, 0.758209910f, + 0.653172843f, 0.757208847f, + 0.654333618f, 0.756206001f, + 0.655492853f, 0.755201377f, + 0.656650546f, 0.754194975f, + 0.657806693f, 0.753186799f, + 0.658961293f, 0.752176850f, + 0.660114342f, 0.751165132f, + 0.661265838f, 0.750151646f, + 0.662415778f, 0.749136395f, + 0.663564159f, 0.748119380f, + 0.664710978f, 0.747100606f, + 0.665856234f, 0.746080074f, + 0.666999922f, 0.745057785f, + 0.668142041f, 0.744033744f, + 0.669282588f, 0.743007952f, + 0.670421560f, 0.741980412f, + 0.671558955f, 0.740951125f, + 0.672694769f, 0.739920095f, + 0.673829000f, 0.738887324f, + 0.674961646f, 0.737852815f, + 0.676092704f, 0.736816569f, + 0.677222170f, 0.735778589f, + 0.678350043f, 0.734738878f, + 0.679476320f, 0.733697438f, + 0.680600998f, 0.732654272f, + 0.681724074f, 0.731609381f, + 0.682845546f, 0.730562769f, + 0.683965412f, 0.729514438f, + 0.685083668f, 0.728464390f, + 0.686200312f, 0.727412629f, + 0.687315341f, 0.726359155f, + 0.688428753f, 0.725303972f, + 0.689540545f, 0.724247083f, + 0.690650714f, 0.723188489f, + 0.691759258f, 0.722128194f, + 0.692866175f, 0.721066199f, + 0.693971461f, 0.720002508f, + 0.695075114f, 0.718937122f, + 0.696177131f, 0.717870045f, + 0.697277511f, 0.716801279f, + 0.698376249f, 0.715730825f, + 0.699473345f, 0.714658688f, + 0.700568794f, 0.713584869f, + 0.701662595f, 0.712509371f, + 0.702754744f, 0.711432196f, + 0.703845241f, 0.710353347f, + 0.704934080f, 0.709272826f, + 0.706021261f, 0.708190637f, + 0.707106781f, 0.707106781f, + 0.708190637f, 0.706021261f, + 0.709272826f, 0.704934080f, + 0.710353347f, 0.703845241f, + 0.711432196f, 0.702754744f, + 0.712509371f, 0.701662595f, + 0.713584869f, 0.700568794f, + 0.714658688f, 0.699473345f, + 0.715730825f, 0.698376249f, + 0.716801279f, 0.697277511f, + 0.717870045f, 0.696177131f, + 0.718937122f, 0.695075114f, + 0.720002508f, 0.693971461f, + 0.721066199f, 0.692866175f, + 0.722128194f, 0.691759258f, + 0.723188489f, 0.690650714f, + 0.724247083f, 0.689540545f, + 0.725303972f, 0.688428753f, + 0.726359155f, 0.687315341f, + 0.727412629f, 0.686200312f, + 0.728464390f, 0.685083668f, + 0.729514438f, 0.683965412f, + 0.730562769f, 0.682845546f, + 0.731609381f, 0.681724074f, + 0.732654272f, 0.680600998f, + 0.733697438f, 0.679476320f, + 0.734738878f, 0.678350043f, + 0.735778589f, 0.677222170f, + 0.736816569f, 0.676092704f, + 0.737852815f, 0.674961646f, + 0.738887324f, 0.673829000f, + 0.739920095f, 0.672694769f, + 0.740951125f, 0.671558955f, + 0.741980412f, 0.670421560f, + 0.743007952f, 0.669282588f, + 0.744033744f, 0.668142041f, + 0.745057785f, 0.666999922f, + 0.746080074f, 0.665856234f, + 0.747100606f, 0.664710978f, + 0.748119380f, 0.663564159f, + 0.749136395f, 0.662415778f, + 0.750151646f, 0.661265838f, + 0.751165132f, 0.660114342f, + 0.752176850f, 0.658961293f, + 0.753186799f, 0.657806693f, + 0.754194975f, 0.656650546f, + 0.755201377f, 0.655492853f, + 0.756206001f, 0.654333618f, + 0.757208847f, 0.653172843f, + 0.758209910f, 0.652010531f, + 0.759209189f, 0.650846685f, + 0.760206682f, 0.649681307f, + 0.761202385f, 0.648514401f, + 0.762196298f, 0.647345969f, + 0.763188417f, 0.646176013f, + 0.764178741f, 0.645004537f, + 0.765167266f, 0.643831543f, + 0.766153990f, 0.642657034f, + 0.767138912f, 0.641481013f, + 0.768122029f, 0.640303482f, + 0.769103338f, 0.639124445f, + 0.770082837f, 0.637943904f, + 0.771060524f, 0.636761861f, + 0.772036397f, 0.635578320f, + 0.773010453f, 0.634393284f, + 0.773982691f, 0.633206755f, + 0.774953107f, 0.632018736f, + 0.775921699f, 0.630829230f, + 0.776888466f, 0.629638239f, + 0.777853404f, 0.628445767f, + 0.778816512f, 0.627251815f, + 0.779777788f, 0.626056388f, + 0.780737229f, 0.624859488f, + 0.781694832f, 0.623661118f, + 0.782650596f, 0.622461279f, + 0.783604519f, 0.621259977f, + 0.784556597f, 0.620057212f, + 0.785506830f, 0.618852988f, + 0.786455214f, 0.617647308f, + 0.787401747f, 0.616440175f, + 0.788346428f, 0.615231591f, + 0.789289253f, 0.614021559f, + 0.790230221f, 0.612810082f, + 0.791169330f, 0.611597164f, + 0.792106577f, 0.610382806f, + 0.793041960f, 0.609167012f, + 0.793975478f, 0.607949785f, + 0.794907126f, 0.606731127f, + 0.795836905f, 0.605511041f, + 0.796764810f, 0.604289531f, + 0.797690841f, 0.603066599f, + 0.798614995f, 0.601842247f, + 0.799537269f, 0.600616479f, + 0.800457662f, 0.599389298f, + 0.801376172f, 0.598160707f, + 0.802292796f, 0.596930708f, + 0.803207531f, 0.595699304f, + 0.804120377f, 0.594466499f, + 0.805031331f, 0.593232295f, + 0.805940391f, 0.591996695f, + 0.806847554f, 0.590759702f, + 0.807752818f, 0.589521319f, + 0.808656182f, 0.588281548f, + 0.809557642f, 0.587040394f, + 0.810457198f, 0.585797857f, + 0.811354847f, 0.584553943f, + 0.812250587f, 0.583308653f, + 0.813144415f, 0.582061990f, + 0.814036330f, 0.580813958f, + 0.814926329f, 0.579564559f, + 0.815814411f, 0.578313796f, + 0.816700573f, 0.577061673f, + 0.817584813f, 0.575808191f, + 0.818467130f, 0.574553355f, + 0.819347520f, 0.573297167f, + 0.820225983f, 0.572039629f, + 0.821102515f, 0.570780746f, + 0.821977115f, 0.569520519f, + 0.822849781f, 0.568258953f, + 0.823720511f, 0.566996049f, + 0.824589303f, 0.565731811f, + 0.825456154f, 0.564466242f, + 0.826321063f, 0.563199344f, + 0.827184027f, 0.561931121f, + 0.828045045f, 0.560661576f, + 0.828904115f, 0.559390712f, + 0.829761234f, 0.558118531f, + 0.830616400f, 0.556845037f, + 0.831469612f, 0.555570233f, + 0.832320868f, 0.554294121f, + 0.833170165f, 0.553016706f, + 0.834017501f, 0.551737988f, + 0.834862875f, 0.550457973f, + 0.835706284f, 0.549176662f, + 0.836547727f, 0.547894059f, + 0.837387202f, 0.546610167f, + 0.838224706f, 0.545324988f, + 0.839060237f, 0.544038527f, + 0.839893794f, 0.542750785f, + 0.840725375f, 0.541461766f, + 0.841554977f, 0.540171473f, + 0.842382600f, 0.538879909f, + 0.843208240f, 0.537587076f, + 0.844031895f, 0.536292979f, + 0.844853565f, 0.534997620f, + 0.845673247f, 0.533701002f, + 0.846490939f, 0.532403128f, + 0.847306639f, 0.531104001f, + 0.848120345f, 0.529803625f, + 0.848932055f, 0.528502002f, + 0.849741768f, 0.527199135f, + 0.850549481f, 0.525895027f, + 0.851355193f, 0.524589683f, + 0.852158902f, 0.523283103f, + 0.852960605f, 0.521975293f, + 0.853760301f, 0.520666254f, + 0.854557988f, 0.519355990f, + 0.855353665f, 0.518044504f, + 0.856147328f, 0.516731799f, + 0.856938977f, 0.515417878f, + 0.857728610f, 0.514102744f, + 0.858516224f, 0.512786401f, + 0.859301818f, 0.511468850f, + 0.860085390f, 0.510150097f, + 0.860866939f, 0.508830143f, + 0.861646461f, 0.507508991f, + 0.862423956f, 0.506186645f, + 0.863199422f, 0.504863109f, + 0.863972856f, 0.503538384f, + 0.864744258f, 0.502212474f, + 0.865513624f, 0.500885383f, + 0.866280954f, 0.499557113f, + 0.867046246f, 0.498227667f, + 0.867809497f, 0.496897049f, + 0.868570706f, 0.495565262f, + 0.869329871f, 0.494232309f, + 0.870086991f, 0.492898192f, + 0.870842063f, 0.491562916f, + 0.871595087f, 0.490226483f, + 0.872346059f, 0.488888897f, + 0.873094978f, 0.487550160f, + 0.873841843f, 0.486210276f, + 0.874586652f, 0.484869248f, + 0.875329403f, 0.483527079f, + 0.876070094f, 0.482183772f, + 0.876808724f, 0.480839331f, + 0.877545290f, 0.479493758f, + 0.878279792f, 0.478147056f, + 0.879012226f, 0.476799230f, + 0.879742593f, 0.475450282f, + 0.880470889f, 0.474100215f, + 0.881197113f, 0.472749032f, + 0.881921264f, 0.471396737f, + 0.882643340f, 0.470043332f, + 0.883363339f, 0.468688822f, + 0.884081259f, 0.467333209f, + 0.884797098f, 0.465976496f, + 0.885510856f, 0.464618686f, + 0.886222530f, 0.463259784f, + 0.886932119f, 0.461899791f, + 0.887639620f, 0.460538711f, + 0.888345033f, 0.459176548f, + 0.889048356f, 0.457813304f, + 0.889749586f, 0.456448982f, + 0.890448723f, 0.455083587f, + 0.891145765f, 0.453717121f, + 0.891840709f, 0.452349587f, + 0.892533555f, 0.450980989f, + 0.893224301f, 0.449611330f, + 0.893912945f, 0.448240612f, + 0.894599486f, 0.446868840f, + 0.895283921f, 0.445496017f, + 0.895966250f, 0.444122145f, + 0.896646470f, 0.442747228f, + 0.897324581f, 0.441371269f, + 0.898000580f, 0.439994271f, + 0.898674466f, 0.438616239f, + 0.899346237f, 0.437237174f, + 0.900015892f, 0.435857080f, + 0.900683429f, 0.434475961f, + 0.901348847f, 0.433093819f, + 0.902012144f, 0.431710658f, + 0.902673318f, 0.430326481f, + 0.903332368f, 0.428941292f, + 0.903989293f, 0.427555093f, + 0.904644091f, 0.426167889f, + 0.905296759f, 0.424779681f, + 0.905947298f, 0.423390474f, + 0.906595705f, 0.422000271f, + 0.907241978f, 0.420609074f, + 0.907886116f, 0.419216888f, + 0.908528119f, 0.417823716f, + 0.909167983f, 0.416429560f, + 0.909805708f, 0.415034424f, + 0.910441292f, 0.413638312f, + 0.911074734f, 0.412241227f, + 0.911706032f, 0.410843171f, + 0.912335185f, 0.409444149f, + 0.912962190f, 0.408044163f, + 0.913587048f, 0.406643217f, + 0.914209756f, 0.405241314f, + 0.914830312f, 0.403838458f, + 0.915448716f, 0.402434651f, + 0.916064966f, 0.401029897f, + 0.916679060f, 0.399624200f, + 0.917290997f, 0.398217562f, + 0.917900776f, 0.396809987f, + 0.918508394f, 0.395401479f, + 0.919113852f, 0.393992040f, + 0.919717146f, 0.392581674f, + 0.920318277f, 0.391170384f, + 0.920917242f, 0.389758174f, + 0.921514039f, 0.388345047f, + 0.922108669f, 0.386931006f, + 0.922701128f, 0.385516054f, + 0.923291417f, 0.384100195f, + 0.923879533f, 0.382683432f, + 0.924465474f, 0.381265769f, + 0.925049241f, 0.379847209f, + 0.925630831f, 0.378427755f, + 0.926210242f, 0.377007410f, + 0.926787474f, 0.375586178f, + 0.927362526f, 0.374164063f, + 0.927935395f, 0.372741067f, + 0.928506080f, 0.371317194f, + 0.929074581f, 0.369892447f, + 0.929640896f, 0.368466830f, + 0.930205023f, 0.367040346f, + 0.930766961f, 0.365612998f, + 0.931326709f, 0.364184790f, + 0.931884266f, 0.362755724f, + 0.932439629f, 0.361325806f, + 0.932992799f, 0.359895037f, + 0.933543773f, 0.358463421f, + 0.934092550f, 0.357030961f, + 0.934639130f, 0.355597662f, + 0.935183510f, 0.354163525f, + 0.935725689f, 0.352728556f, + 0.936265667f, 0.351292756f, + 0.936803442f, 0.349856130f, + 0.937339012f, 0.348418680f, + 0.937872376f, 0.346980411f, + 0.938403534f, 0.345541325f, + 0.938932484f, 0.344101426f, + 0.939459224f, 0.342660717f, + 0.939983753f, 0.341219202f, + 0.940506071f, 0.339776884f, + 0.941026175f, 0.338333767f, + 0.941544065f, 0.336889853f, + 0.942059740f, 0.335445147f, + 0.942573198f, 0.333999651f, + 0.943084437f, 0.332553370f, + 0.943593458f, 0.331106306f, + 0.944100258f, 0.329658463f, + 0.944604837f, 0.328209844f, + 0.945107193f, 0.326760452f, + 0.945607325f, 0.325310292f, + 0.946105232f, 0.323859367f, + 0.946600913f, 0.322407679f, + 0.947094366f, 0.320955232f, + 0.947585591f, 0.319502031f, + 0.948074586f, 0.318048077f, + 0.948561350f, 0.316593376f, + 0.949045882f, 0.315137929f, + 0.949528181f, 0.313681740f, + 0.950008245f, 0.312224814f, + 0.950486074f, 0.310767153f, + 0.950961666f, 0.309308760f, + 0.951435021f, 0.307849640f, + 0.951906137f, 0.306389795f, + 0.952375013f, 0.304929230f, + 0.952841648f, 0.303467947f, + 0.953306040f, 0.302005949f, + 0.953768190f, 0.300543241f, + 0.954228095f, 0.299079826f, + 0.954685755f, 0.297615707f, + 0.955141168f, 0.296150888f, + 0.955594334f, 0.294685372f, + 0.956045251f, 0.293219163f, + 0.956493919f, 0.291752263f, + 0.956940336f, 0.290284677f, + 0.957384501f, 0.288816408f, + 0.957826413f, 0.287347460f, + 0.958266071f, 0.285877835f, + 0.958703475f, 0.284407537f, + 0.959138622f, 0.282936570f, + 0.959571513f, 0.281464938f, + 0.960002146f, 0.279992643f, + 0.960430519f, 0.278519689f, + 0.960856633f, 0.277046080f, + 0.961280486f, 0.275571819f, + 0.961702077f, 0.274096910f, + 0.962121404f, 0.272621355f, + 0.962538468f, 0.271145160f, + 0.962953267f, 0.269668326f, + 0.963365800f, 0.268190857f, + 0.963776066f, 0.266712757f, + 0.964184064f, 0.265234030f, + 0.964589793f, 0.263754679f, + 0.964993253f, 0.262274707f, + 0.965394442f, 0.260794118f, + 0.965793359f, 0.259312915f, + 0.966190003f, 0.257831102f, + 0.966584374f, 0.256348682f, + 0.966976471f, 0.254865660f, + 0.967366292f, 0.253382037f, + 0.967753837f, 0.251897818f, + 0.968139105f, 0.250413007f, + 0.968522094f, 0.248927606f, + 0.968902805f, 0.247441619f, + 0.969281235f, 0.245955050f, + 0.969657385f, 0.244467903f, + 0.970031253f, 0.242980180f, + 0.970402839f, 0.241491885f, + 0.970772141f, 0.240003022f, + 0.971139158f, 0.238513595f, + 0.971503891f, 0.237023606f, + 0.971866337f, 0.235533059f, + 0.972226497f, 0.234041959f, + 0.972584369f, 0.232550307f, + 0.972939952f, 0.231058108f, + 0.973293246f, 0.229565366f, + 0.973644250f, 0.228072083f, + 0.973992962f, 0.226578264f, + 0.974339383f, 0.225083911f, + 0.974683511f, 0.223589029f, + 0.975025345f, 0.222093621f, + 0.975364885f, 0.220597690f, + 0.975702130f, 0.219101240f, + 0.976037079f, 0.217604275f, + 0.976369731f, 0.216106797f, + 0.976700086f, 0.214608811f, + 0.977028143f, 0.213110320f, + 0.977353900f, 0.211611327f, + 0.977677358f, 0.210111837f, + 0.977998515f, 0.208611852f, + 0.978317371f, 0.207111376f, + 0.978633924f, 0.205610413f, + 0.978948175f, 0.204108966f, + 0.979260123f, 0.202607039f, + 0.979569766f, 0.201104635f, + 0.979877104f, 0.199601758f, + 0.980182136f, 0.198098411f, + 0.980484862f, 0.196594598f, + 0.980785280f, 0.195090322f, + 0.981083391f, 0.193585587f, + 0.981379193f, 0.192080397f, + 0.981672686f, 0.190574755f, + 0.981963869f, 0.189068664f, + 0.982252741f, 0.187562129f, + 0.982539302f, 0.186055152f, + 0.982823551f, 0.184547737f, + 0.983105487f, 0.183039888f, + 0.983385110f, 0.181531608f, + 0.983662419f, 0.180022901f, + 0.983937413f, 0.178513771f, + 0.984210092f, 0.177004220f, + 0.984480455f, 0.175494253f, + 0.984748502f, 0.173983873f, + 0.985014231f, 0.172473084f, + 0.985277642f, 0.170961889f, + 0.985538735f, 0.169450291f, + 0.985797509f, 0.167938295f, + 0.986053963f, 0.166425904f, + 0.986308097f, 0.164913120f, + 0.986559910f, 0.163399949f, + 0.986809402f, 0.161886394f, + 0.987056571f, 0.160372457f, + 0.987301418f, 0.158858143f, + 0.987543942f, 0.157343456f, + 0.987784142f, 0.155828398f, + 0.988022017f, 0.154312973f, + 0.988257568f, 0.152797185f, + 0.988490793f, 0.151281038f, + 0.988721692f, 0.149764535f, + 0.988950265f, 0.148247679f, + 0.989176510f, 0.146730474f, + 0.989400428f, 0.145212925f, + 0.989622017f, 0.143695033f, + 0.989841278f, 0.142176804f, + 0.990058210f, 0.140658239f, + 0.990272812f, 0.139139344f, + 0.990485084f, 0.137620122f, + 0.990695025f, 0.136100575f, + 0.990902635f, 0.134580709f, + 0.991107914f, 0.133060525f, + 0.991310860f, 0.131540029f, + 0.991511473f, 0.130019223f, + 0.991709754f, 0.128498111f, + 0.991905700f, 0.126976696f, + 0.992099313f, 0.125454983f, + 0.992290591f, 0.123932975f, + 0.992479535f, 0.122410675f, + 0.992666142f, 0.120888087f, + 0.992850414f, 0.119365215f, + 0.993032350f, 0.117842062f, + 0.993211949f, 0.116318631f, + 0.993389211f, 0.114794927f, + 0.993564136f, 0.113270952f, + 0.993736722f, 0.111746711f, + 0.993906970f, 0.110222207f, + 0.994074879f, 0.108697444f, + 0.994240449f, 0.107172425f, + 0.994403680f, 0.105647154f, + 0.994564571f, 0.104121634f, + 0.994723121f, 0.102595869f, + 0.994879331f, 0.101069863f, + 0.995033199f, 0.099543619f, + 0.995184727f, 0.098017140f, + 0.995333912f, 0.096490431f, + 0.995480755f, 0.094963495f, + 0.995625256f, 0.093436336f, + 0.995767414f, 0.091908956f, + 0.995907229f, 0.090381361f, + 0.996044701f, 0.088853553f, + 0.996179829f, 0.087325535f, + 0.996312612f, 0.085797312f, + 0.996443051f, 0.084268888f, + 0.996571146f, 0.082740265f, + 0.996696895f, 0.081211447f, + 0.996820299f, 0.079682438f, + 0.996941358f, 0.078153242f, + 0.997060070f, 0.076623861f, + 0.997176437f, 0.075094301f, + 0.997290457f, 0.073564564f, + 0.997402130f, 0.072034653f, + 0.997511456f, 0.070504573f, + 0.997618435f, 0.068974328f, + 0.997723067f, 0.067443920f, + 0.997825350f, 0.065913353f, + 0.997925286f, 0.064382631f, + 0.998022874f, 0.062851758f, + 0.998118113f, 0.061320736f, + 0.998211003f, 0.059789571f, + 0.998301545f, 0.058258265f, + 0.998389737f, 0.056726821f, + 0.998475581f, 0.055195244f, + 0.998559074f, 0.053663538f, + 0.998640218f, 0.052131705f, + 0.998719012f, 0.050599749f, + 0.998795456f, 0.049067674f, + 0.998869550f, 0.047535484f, + 0.998941293f, 0.046003182f, + 0.999010686f, 0.044470772f, + 0.999077728f, 0.042938257f, + 0.999142419f, 0.041405641f, + 0.999204759f, 0.039872928f, + 0.999264747f, 0.038340120f, + 0.999322385f, 0.036807223f, + 0.999377670f, 0.035274239f, + 0.999430605f, 0.033741172f, + 0.999481187f, 0.032208025f, + 0.999529418f, 0.030674803f, + 0.999575296f, 0.029141509f, + 0.999618822f, 0.027608146f, + 0.999659997f, 0.026074718f, + 0.999698819f, 0.024541229f, + 0.999735288f, 0.023007681f, + 0.999769405f, 0.021474080f, + 0.999801170f, 0.019940429f, + 0.999830582f, 0.018406730f, + 0.999857641f, 0.016872988f, + 0.999882347f, 0.015339206f, + 0.999904701f, 0.013805389f, + 0.999924702f, 0.012271538f, + 0.999942350f, 0.010737659f, + 0.999957645f, 0.009203755f, + 0.999970586f, 0.007669829f, + 0.999981175f, 0.006135885f, + 0.999989411f, 0.004601926f, + 0.999995294f, 0.003067957f, + 0.999998823f, 0.001533980f, + 1.000000000f, 0.000000000f, + 0.999998823f, -0.001533980f, + 0.999995294f, -0.003067957f, + 0.999989411f, -0.004601926f, + 0.999981175f, -0.006135885f, + 0.999970586f, -0.007669829f, + 0.999957645f, -0.009203755f, + 0.999942350f, -0.010737659f, + 0.999924702f, -0.012271538f, + 0.999904701f, -0.013805389f, + 0.999882347f, -0.015339206f, + 0.999857641f, -0.016872988f, + 0.999830582f, -0.018406730f, + 0.999801170f, -0.019940429f, + 0.999769405f, -0.021474080f, + 0.999735288f, -0.023007681f, + 0.999698819f, -0.024541229f, + 0.999659997f, -0.026074718f, + 0.999618822f, -0.027608146f, + 0.999575296f, -0.029141509f, + 0.999529418f, -0.030674803f, + 0.999481187f, -0.032208025f, + 0.999430605f, -0.033741172f, + 0.999377670f, -0.035274239f, + 0.999322385f, -0.036807223f, + 0.999264747f, -0.038340120f, + 0.999204759f, -0.039872928f, + 0.999142419f, -0.041405641f, + 0.999077728f, -0.042938257f, + 0.999010686f, -0.044470772f, + 0.998941293f, -0.046003182f, + 0.998869550f, -0.047535484f, + 0.998795456f, -0.049067674f, + 0.998719012f, -0.050599749f, + 0.998640218f, -0.052131705f, + 0.998559074f, -0.053663538f, + 0.998475581f, -0.055195244f, + 0.998389737f, -0.056726821f, + 0.998301545f, -0.058258265f, + 0.998211003f, -0.059789571f, + 0.998118113f, -0.061320736f, + 0.998022874f, -0.062851758f, + 0.997925286f, -0.064382631f, + 0.997825350f, -0.065913353f, + 0.997723067f, -0.067443920f, + 0.997618435f, -0.068974328f, + 0.997511456f, -0.070504573f, + 0.997402130f, -0.072034653f, + 0.997290457f, -0.073564564f, + 0.997176437f, -0.075094301f, + 0.997060070f, -0.076623861f, + 0.996941358f, -0.078153242f, + 0.996820299f, -0.079682438f, + 0.996696895f, -0.081211447f, + 0.996571146f, -0.082740265f, + 0.996443051f, -0.084268888f, + 0.996312612f, -0.085797312f, + 0.996179829f, -0.087325535f, + 0.996044701f, -0.088853553f, + 0.995907229f, -0.090381361f, + 0.995767414f, -0.091908956f, + 0.995625256f, -0.093436336f, + 0.995480755f, -0.094963495f, + 0.995333912f, -0.096490431f, + 0.995184727f, -0.098017140f, + 0.995033199f, -0.099543619f, + 0.994879331f, -0.101069863f, + 0.994723121f, -0.102595869f, + 0.994564571f, -0.104121634f, + 0.994403680f, -0.105647154f, + 0.994240449f, -0.107172425f, + 0.994074879f, -0.108697444f, + 0.993906970f, -0.110222207f, + 0.993736722f, -0.111746711f, + 0.993564136f, -0.113270952f, + 0.993389211f, -0.114794927f, + 0.993211949f, -0.116318631f, + 0.993032350f, -0.117842062f, + 0.992850414f, -0.119365215f, + 0.992666142f, -0.120888087f, + 0.992479535f, -0.122410675f, + 0.992290591f, -0.123932975f, + 0.992099313f, -0.125454983f, + 0.991905700f, -0.126976696f, + 0.991709754f, -0.128498111f, + 0.991511473f, -0.130019223f, + 0.991310860f, -0.131540029f, + 0.991107914f, -0.133060525f, + 0.990902635f, -0.134580709f, + 0.990695025f, -0.136100575f, + 0.990485084f, -0.137620122f, + 0.990272812f, -0.139139344f, + 0.990058210f, -0.140658239f, + 0.989841278f, -0.142176804f, + 0.989622017f, -0.143695033f, + 0.989400428f, -0.145212925f, + 0.989176510f, -0.146730474f, + 0.988950265f, -0.148247679f, + 0.988721692f, -0.149764535f, + 0.988490793f, -0.151281038f, + 0.988257568f, -0.152797185f, + 0.988022017f, -0.154312973f, + 0.987784142f, -0.155828398f, + 0.987543942f, -0.157343456f, + 0.987301418f, -0.158858143f, + 0.987056571f, -0.160372457f, + 0.986809402f, -0.161886394f, + 0.986559910f, -0.163399949f, + 0.986308097f, -0.164913120f, + 0.986053963f, -0.166425904f, + 0.985797509f, -0.167938295f, + 0.985538735f, -0.169450291f, + 0.985277642f, -0.170961889f, + 0.985014231f, -0.172473084f, + 0.984748502f, -0.173983873f, + 0.984480455f, -0.175494253f, + 0.984210092f, -0.177004220f, + 0.983937413f, -0.178513771f, + 0.983662419f, -0.180022901f, + 0.983385110f, -0.181531608f, + 0.983105487f, -0.183039888f, + 0.982823551f, -0.184547737f, + 0.982539302f, -0.186055152f, + 0.982252741f, -0.187562129f, + 0.981963869f, -0.189068664f, + 0.981672686f, -0.190574755f, + 0.981379193f, -0.192080397f, + 0.981083391f, -0.193585587f, + 0.980785280f, -0.195090322f, + 0.980484862f, -0.196594598f, + 0.980182136f, -0.198098411f, + 0.979877104f, -0.199601758f, + 0.979569766f, -0.201104635f, + 0.979260123f, -0.202607039f, + 0.978948175f, -0.204108966f, + 0.978633924f, -0.205610413f, + 0.978317371f, -0.207111376f, + 0.977998515f, -0.208611852f, + 0.977677358f, -0.210111837f, + 0.977353900f, -0.211611327f, + 0.977028143f, -0.213110320f, + 0.976700086f, -0.214608811f, + 0.976369731f, -0.216106797f, + 0.976037079f, -0.217604275f, + 0.975702130f, -0.219101240f, + 0.975364885f, -0.220597690f, + 0.975025345f, -0.222093621f, + 0.974683511f, -0.223589029f, + 0.974339383f, -0.225083911f, + 0.973992962f, -0.226578264f, + 0.973644250f, -0.228072083f, + 0.973293246f, -0.229565366f, + 0.972939952f, -0.231058108f, + 0.972584369f, -0.232550307f, + 0.972226497f, -0.234041959f, + 0.971866337f, -0.235533059f, + 0.971503891f, -0.237023606f, + 0.971139158f, -0.238513595f, + 0.970772141f, -0.240003022f, + 0.970402839f, -0.241491885f, + 0.970031253f, -0.242980180f, + 0.969657385f, -0.244467903f, + 0.969281235f, -0.245955050f, + 0.968902805f, -0.247441619f, + 0.968522094f, -0.248927606f, + 0.968139105f, -0.250413007f, + 0.967753837f, -0.251897818f, + 0.967366292f, -0.253382037f, + 0.966976471f, -0.254865660f, + 0.966584374f, -0.256348682f, + 0.966190003f, -0.257831102f, + 0.965793359f, -0.259312915f, + 0.965394442f, -0.260794118f, + 0.964993253f, -0.262274707f, + 0.964589793f, -0.263754679f, + 0.964184064f, -0.265234030f, + 0.963776066f, -0.266712757f, + 0.963365800f, -0.268190857f, + 0.962953267f, -0.269668326f, + 0.962538468f, -0.271145160f, + 0.962121404f, -0.272621355f, + 0.961702077f, -0.274096910f, + 0.961280486f, -0.275571819f, + 0.960856633f, -0.277046080f, + 0.960430519f, -0.278519689f, + 0.960002146f, -0.279992643f, + 0.959571513f, -0.281464938f, + 0.959138622f, -0.282936570f, + 0.958703475f, -0.284407537f, + 0.958266071f, -0.285877835f, + 0.957826413f, -0.287347460f, + 0.957384501f, -0.288816408f, + 0.956940336f, -0.290284677f, + 0.956493919f, -0.291752263f, + 0.956045251f, -0.293219163f, + 0.955594334f, -0.294685372f, + 0.955141168f, -0.296150888f, + 0.954685755f, -0.297615707f, + 0.954228095f, -0.299079826f, + 0.953768190f, -0.300543241f, + 0.953306040f, -0.302005949f, + 0.952841648f, -0.303467947f, + 0.952375013f, -0.304929230f, + 0.951906137f, -0.306389795f, + 0.951435021f, -0.307849640f, + 0.950961666f, -0.309308760f, + 0.950486074f, -0.310767153f, + 0.950008245f, -0.312224814f, + 0.949528181f, -0.313681740f, + 0.949045882f, -0.315137929f, + 0.948561350f, -0.316593376f, + 0.948074586f, -0.318048077f, + 0.947585591f, -0.319502031f, + 0.947094366f, -0.320955232f, + 0.946600913f, -0.322407679f, + 0.946105232f, -0.323859367f, + 0.945607325f, -0.325310292f, + 0.945107193f, -0.326760452f, + 0.944604837f, -0.328209844f, + 0.944100258f, -0.329658463f, + 0.943593458f, -0.331106306f, + 0.943084437f, -0.332553370f, + 0.942573198f, -0.333999651f, + 0.942059740f, -0.335445147f, + 0.941544065f, -0.336889853f, + 0.941026175f, -0.338333767f, + 0.940506071f, -0.339776884f, + 0.939983753f, -0.341219202f, + 0.939459224f, -0.342660717f, + 0.938932484f, -0.344101426f, + 0.938403534f, -0.345541325f, + 0.937872376f, -0.346980411f, + 0.937339012f, -0.348418680f, + 0.936803442f, -0.349856130f, + 0.936265667f, -0.351292756f, + 0.935725689f, -0.352728556f, + 0.935183510f, -0.354163525f, + 0.934639130f, -0.355597662f, + 0.934092550f, -0.357030961f, + 0.933543773f, -0.358463421f, + 0.932992799f, -0.359895037f, + 0.932439629f, -0.361325806f, + 0.931884266f, -0.362755724f, + 0.931326709f, -0.364184790f, + 0.930766961f, -0.365612998f, + 0.930205023f, -0.367040346f, + 0.929640896f, -0.368466830f, + 0.929074581f, -0.369892447f, + 0.928506080f, -0.371317194f, + 0.927935395f, -0.372741067f, + 0.927362526f, -0.374164063f, + 0.926787474f, -0.375586178f, + 0.926210242f, -0.377007410f, + 0.925630831f, -0.378427755f, + 0.925049241f, -0.379847209f, + 0.924465474f, -0.381265769f, + 0.923879533f, -0.382683432f, + 0.923291417f, -0.384100195f, + 0.922701128f, -0.385516054f, + 0.922108669f, -0.386931006f, + 0.921514039f, -0.388345047f, + 0.920917242f, -0.389758174f, + 0.920318277f, -0.391170384f, + 0.919717146f, -0.392581674f, + 0.919113852f, -0.393992040f, + 0.918508394f, -0.395401479f, + 0.917900776f, -0.396809987f, + 0.917290997f, -0.398217562f, + 0.916679060f, -0.399624200f, + 0.916064966f, -0.401029897f, + 0.915448716f, -0.402434651f, + 0.914830312f, -0.403838458f, + 0.914209756f, -0.405241314f, + 0.913587048f, -0.406643217f, + 0.912962190f, -0.408044163f, + 0.912335185f, -0.409444149f, + 0.911706032f, -0.410843171f, + 0.911074734f, -0.412241227f, + 0.910441292f, -0.413638312f, + 0.909805708f, -0.415034424f, + 0.909167983f, -0.416429560f, + 0.908528119f, -0.417823716f, + 0.907886116f, -0.419216888f, + 0.907241978f, -0.420609074f, + 0.906595705f, -0.422000271f, + 0.905947298f, -0.423390474f, + 0.905296759f, -0.424779681f, + 0.904644091f, -0.426167889f, + 0.903989293f, -0.427555093f, + 0.903332368f, -0.428941292f, + 0.902673318f, -0.430326481f, + 0.902012144f, -0.431710658f, + 0.901348847f, -0.433093819f, + 0.900683429f, -0.434475961f, + 0.900015892f, -0.435857080f, + 0.899346237f, -0.437237174f, + 0.898674466f, -0.438616239f, + 0.898000580f, -0.439994271f, + 0.897324581f, -0.441371269f, + 0.896646470f, -0.442747228f, + 0.895966250f, -0.444122145f, + 0.895283921f, -0.445496017f, + 0.894599486f, -0.446868840f, + 0.893912945f, -0.448240612f, + 0.893224301f, -0.449611330f, + 0.892533555f, -0.450980989f, + 0.891840709f, -0.452349587f, + 0.891145765f, -0.453717121f, + 0.890448723f, -0.455083587f, + 0.889749586f, -0.456448982f, + 0.889048356f, -0.457813304f, + 0.888345033f, -0.459176548f, + 0.887639620f, -0.460538711f, + 0.886932119f, -0.461899791f, + 0.886222530f, -0.463259784f, + 0.885510856f, -0.464618686f, + 0.884797098f, -0.465976496f, + 0.884081259f, -0.467333209f, + 0.883363339f, -0.468688822f, + 0.882643340f, -0.470043332f, + 0.881921264f, -0.471396737f, + 0.881197113f, -0.472749032f, + 0.880470889f, -0.474100215f, + 0.879742593f, -0.475450282f, + 0.879012226f, -0.476799230f, + 0.878279792f, -0.478147056f, + 0.877545290f, -0.479493758f, + 0.876808724f, -0.480839331f, + 0.876070094f, -0.482183772f, + 0.875329403f, -0.483527079f, + 0.874586652f, -0.484869248f, + 0.873841843f, -0.486210276f, + 0.873094978f, -0.487550160f, + 0.872346059f, -0.488888897f, + 0.871595087f, -0.490226483f, + 0.870842063f, -0.491562916f, + 0.870086991f, -0.492898192f, + 0.869329871f, -0.494232309f, + 0.868570706f, -0.495565262f, + 0.867809497f, -0.496897049f, + 0.867046246f, -0.498227667f, + 0.866280954f, -0.499557113f, + 0.865513624f, -0.500885383f, + 0.864744258f, -0.502212474f, + 0.863972856f, -0.503538384f, + 0.863199422f, -0.504863109f, + 0.862423956f, -0.506186645f, + 0.861646461f, -0.507508991f, + 0.860866939f, -0.508830143f, + 0.860085390f, -0.510150097f, + 0.859301818f, -0.511468850f, + 0.858516224f, -0.512786401f, + 0.857728610f, -0.514102744f, + 0.856938977f, -0.515417878f, + 0.856147328f, -0.516731799f, + 0.855353665f, -0.518044504f, + 0.854557988f, -0.519355990f, + 0.853760301f, -0.520666254f, + 0.852960605f, -0.521975293f, + 0.852158902f, -0.523283103f, + 0.851355193f, -0.524589683f, + 0.850549481f, -0.525895027f, + 0.849741768f, -0.527199135f, + 0.848932055f, -0.528502002f, + 0.848120345f, -0.529803625f, + 0.847306639f, -0.531104001f, + 0.846490939f, -0.532403128f, + 0.845673247f, -0.533701002f, + 0.844853565f, -0.534997620f, + 0.844031895f, -0.536292979f, + 0.843208240f, -0.537587076f, + 0.842382600f, -0.538879909f, + 0.841554977f, -0.540171473f, + 0.840725375f, -0.541461766f, + 0.839893794f, -0.542750785f, + 0.839060237f, -0.544038527f, + 0.838224706f, -0.545324988f, + 0.837387202f, -0.546610167f, + 0.836547727f, -0.547894059f, + 0.835706284f, -0.549176662f, + 0.834862875f, -0.550457973f, + 0.834017501f, -0.551737988f, + 0.833170165f, -0.553016706f, + 0.832320868f, -0.554294121f, + 0.831469612f, -0.555570233f, + 0.830616400f, -0.556845037f, + 0.829761234f, -0.558118531f, + 0.828904115f, -0.559390712f, + 0.828045045f, -0.560661576f, + 0.827184027f, -0.561931121f, + 0.826321063f, -0.563199344f, + 0.825456154f, -0.564466242f, + 0.824589303f, -0.565731811f, + 0.823720511f, -0.566996049f, + 0.822849781f, -0.568258953f, + 0.821977115f, -0.569520519f, + 0.821102515f, -0.570780746f, + 0.820225983f, -0.572039629f, + 0.819347520f, -0.573297167f, + 0.818467130f, -0.574553355f, + 0.817584813f, -0.575808191f, + 0.816700573f, -0.577061673f, + 0.815814411f, -0.578313796f, + 0.814926329f, -0.579564559f, + 0.814036330f, -0.580813958f, + 0.813144415f, -0.582061990f, + 0.812250587f, -0.583308653f, + 0.811354847f, -0.584553943f, + 0.810457198f, -0.585797857f, + 0.809557642f, -0.587040394f, + 0.808656182f, -0.588281548f, + 0.807752818f, -0.589521319f, + 0.806847554f, -0.590759702f, + 0.805940391f, -0.591996695f, + 0.805031331f, -0.593232295f, + 0.804120377f, -0.594466499f, + 0.803207531f, -0.595699304f, + 0.802292796f, -0.596930708f, + 0.801376172f, -0.598160707f, + 0.800457662f, -0.599389298f, + 0.799537269f, -0.600616479f, + 0.798614995f, -0.601842247f, + 0.797690841f, -0.603066599f, + 0.796764810f, -0.604289531f, + 0.795836905f, -0.605511041f, + 0.794907126f, -0.606731127f, + 0.793975478f, -0.607949785f, + 0.793041960f, -0.609167012f, + 0.792106577f, -0.610382806f, + 0.791169330f, -0.611597164f, + 0.790230221f, -0.612810082f, + 0.789289253f, -0.614021559f, + 0.788346428f, -0.615231591f, + 0.787401747f, -0.616440175f, + 0.786455214f, -0.617647308f, + 0.785506830f, -0.618852988f, + 0.784556597f, -0.620057212f, + 0.783604519f, -0.621259977f, + 0.782650596f, -0.622461279f, + 0.781694832f, -0.623661118f, + 0.780737229f, -0.624859488f, + 0.779777788f, -0.626056388f, + 0.778816512f, -0.627251815f, + 0.777853404f, -0.628445767f, + 0.776888466f, -0.629638239f, + 0.775921699f, -0.630829230f, + 0.774953107f, -0.632018736f, + 0.773982691f, -0.633206755f, + 0.773010453f, -0.634393284f, + 0.772036397f, -0.635578320f, + 0.771060524f, -0.636761861f, + 0.770082837f, -0.637943904f, + 0.769103338f, -0.639124445f, + 0.768122029f, -0.640303482f, + 0.767138912f, -0.641481013f, + 0.766153990f, -0.642657034f, + 0.765167266f, -0.643831543f, + 0.764178741f, -0.645004537f, + 0.763188417f, -0.646176013f, + 0.762196298f, -0.647345969f, + 0.761202385f, -0.648514401f, + 0.760206682f, -0.649681307f, + 0.759209189f, -0.650846685f, + 0.758209910f, -0.652010531f, + 0.757208847f, -0.653172843f, + 0.756206001f, -0.654333618f, + 0.755201377f, -0.655492853f, + 0.754194975f, -0.656650546f, + 0.753186799f, -0.657806693f, + 0.752176850f, -0.658961293f, + 0.751165132f, -0.660114342f, + 0.750151646f, -0.661265838f, + 0.749136395f, -0.662415778f, + 0.748119380f, -0.663564159f, + 0.747100606f, -0.664710978f, + 0.746080074f, -0.665856234f, + 0.745057785f, -0.666999922f, + 0.744033744f, -0.668142041f, + 0.743007952f, -0.669282588f, + 0.741980412f, -0.670421560f, + 0.740951125f, -0.671558955f, + 0.739920095f, -0.672694769f, + 0.738887324f, -0.673829000f, + 0.737852815f, -0.674961646f, + 0.736816569f, -0.676092704f, + 0.735778589f, -0.677222170f, + 0.734738878f, -0.678350043f, + 0.733697438f, -0.679476320f, + 0.732654272f, -0.680600998f, + 0.731609381f, -0.681724074f, + 0.730562769f, -0.682845546f, + 0.729514438f, -0.683965412f, + 0.728464390f, -0.685083668f, + 0.727412629f, -0.686200312f, + 0.726359155f, -0.687315341f, + 0.725303972f, -0.688428753f, + 0.724247083f, -0.689540545f, + 0.723188489f, -0.690650714f, + 0.722128194f, -0.691759258f, + 0.721066199f, -0.692866175f, + 0.720002508f, -0.693971461f, + 0.718937122f, -0.695075114f, + 0.717870045f, -0.696177131f, + 0.716801279f, -0.697277511f, + 0.715730825f, -0.698376249f, + 0.714658688f, -0.699473345f, + 0.713584869f, -0.700568794f, + 0.712509371f, -0.701662595f, + 0.711432196f, -0.702754744f, + 0.710353347f, -0.703845241f, + 0.709272826f, -0.704934080f, + 0.708190637f, -0.706021261f, + 0.707106781f, -0.707106781f, + 0.706021261f, -0.708190637f, + 0.704934080f, -0.709272826f, + 0.703845241f, -0.710353347f, + 0.702754744f, -0.711432196f, + 0.701662595f, -0.712509371f, + 0.700568794f, -0.713584869f, + 0.699473345f, -0.714658688f, + 0.698376249f, -0.715730825f, + 0.697277511f, -0.716801279f, + 0.696177131f, -0.717870045f, + 0.695075114f, -0.718937122f, + 0.693971461f, -0.720002508f, + 0.692866175f, -0.721066199f, + 0.691759258f, -0.722128194f, + 0.690650714f, -0.723188489f, + 0.689540545f, -0.724247083f, + 0.688428753f, -0.725303972f, + 0.687315341f, -0.726359155f, + 0.686200312f, -0.727412629f, + 0.685083668f, -0.728464390f, + 0.683965412f, -0.729514438f, + 0.682845546f, -0.730562769f, + 0.681724074f, -0.731609381f, + 0.680600998f, -0.732654272f, + 0.679476320f, -0.733697438f, + 0.678350043f, -0.734738878f, + 0.677222170f, -0.735778589f, + 0.676092704f, -0.736816569f, + 0.674961646f, -0.737852815f, + 0.673829000f, -0.738887324f, + 0.672694769f, -0.739920095f, + 0.671558955f, -0.740951125f, + 0.670421560f, -0.741980412f, + 0.669282588f, -0.743007952f, + 0.668142041f, -0.744033744f, + 0.666999922f, -0.745057785f, + 0.665856234f, -0.746080074f, + 0.664710978f, -0.747100606f, + 0.663564159f, -0.748119380f, + 0.662415778f, -0.749136395f, + 0.661265838f, -0.750151646f, + 0.660114342f, -0.751165132f, + 0.658961293f, -0.752176850f, + 0.657806693f, -0.753186799f, + 0.656650546f, -0.754194975f, + 0.655492853f, -0.755201377f, + 0.654333618f, -0.756206001f, + 0.653172843f, -0.757208847f, + 0.652010531f, -0.758209910f, + 0.650846685f, -0.759209189f, + 0.649681307f, -0.760206682f, + 0.648514401f, -0.761202385f, + 0.647345969f, -0.762196298f, + 0.646176013f, -0.763188417f, + 0.645004537f, -0.764178741f, + 0.643831543f, -0.765167266f, + 0.642657034f, -0.766153990f, + 0.641481013f, -0.767138912f, + 0.640303482f, -0.768122029f, + 0.639124445f, -0.769103338f, + 0.637943904f, -0.770082837f, + 0.636761861f, -0.771060524f, + 0.635578320f, -0.772036397f, + 0.634393284f, -0.773010453f, + 0.633206755f, -0.773982691f, + 0.632018736f, -0.774953107f, + 0.630829230f, -0.775921699f, + 0.629638239f, -0.776888466f, + 0.628445767f, -0.777853404f, + 0.627251815f, -0.778816512f, + 0.626056388f, -0.779777788f, + 0.624859488f, -0.780737229f, + 0.623661118f, -0.781694832f, + 0.622461279f, -0.782650596f, + 0.621259977f, -0.783604519f, + 0.620057212f, -0.784556597f, + 0.618852988f, -0.785506830f, + 0.617647308f, -0.786455214f, + 0.616440175f, -0.787401747f, + 0.615231591f, -0.788346428f, + 0.614021559f, -0.789289253f, + 0.612810082f, -0.790230221f, + 0.611597164f, -0.791169330f, + 0.610382806f, -0.792106577f, + 0.609167012f, -0.793041960f, + 0.607949785f, -0.793975478f, + 0.606731127f, -0.794907126f, + 0.605511041f, -0.795836905f, + 0.604289531f, -0.796764810f, + 0.603066599f, -0.797690841f, + 0.601842247f, -0.798614995f, + 0.600616479f, -0.799537269f, + 0.599389298f, -0.800457662f, + 0.598160707f, -0.801376172f, + 0.596930708f, -0.802292796f, + 0.595699304f, -0.803207531f, + 0.594466499f, -0.804120377f, + 0.593232295f, -0.805031331f, + 0.591996695f, -0.805940391f, + 0.590759702f, -0.806847554f, + 0.589521319f, -0.807752818f, + 0.588281548f, -0.808656182f, + 0.587040394f, -0.809557642f, + 0.585797857f, -0.810457198f, + 0.584553943f, -0.811354847f, + 0.583308653f, -0.812250587f, + 0.582061990f, -0.813144415f, + 0.580813958f, -0.814036330f, + 0.579564559f, -0.814926329f, + 0.578313796f, -0.815814411f, + 0.577061673f, -0.816700573f, + 0.575808191f, -0.817584813f, + 0.574553355f, -0.818467130f, + 0.573297167f, -0.819347520f, + 0.572039629f, -0.820225983f, + 0.570780746f, -0.821102515f, + 0.569520519f, -0.821977115f, + 0.568258953f, -0.822849781f, + 0.566996049f, -0.823720511f, + 0.565731811f, -0.824589303f, + 0.564466242f, -0.825456154f, + 0.563199344f, -0.826321063f, + 0.561931121f, -0.827184027f, + 0.560661576f, -0.828045045f, + 0.559390712f, -0.828904115f, + 0.558118531f, -0.829761234f, + 0.556845037f, -0.830616400f, + 0.555570233f, -0.831469612f, + 0.554294121f, -0.832320868f, + 0.553016706f, -0.833170165f, + 0.551737988f, -0.834017501f, + 0.550457973f, -0.834862875f, + 0.549176662f, -0.835706284f, + 0.547894059f, -0.836547727f, + 0.546610167f, -0.837387202f, + 0.545324988f, -0.838224706f, + 0.544038527f, -0.839060237f, + 0.542750785f, -0.839893794f, + 0.541461766f, -0.840725375f, + 0.540171473f, -0.841554977f, + 0.538879909f, -0.842382600f, + 0.537587076f, -0.843208240f, + 0.536292979f, -0.844031895f, + 0.534997620f, -0.844853565f, + 0.533701002f, -0.845673247f, + 0.532403128f, -0.846490939f, + 0.531104001f, -0.847306639f, + 0.529803625f, -0.848120345f, + 0.528502002f, -0.848932055f, + 0.527199135f, -0.849741768f, + 0.525895027f, -0.850549481f, + 0.524589683f, -0.851355193f, + 0.523283103f, -0.852158902f, + 0.521975293f, -0.852960605f, + 0.520666254f, -0.853760301f, + 0.519355990f, -0.854557988f, + 0.518044504f, -0.855353665f, + 0.516731799f, -0.856147328f, + 0.515417878f, -0.856938977f, + 0.514102744f, -0.857728610f, + 0.512786401f, -0.858516224f, + 0.511468850f, -0.859301818f, + 0.510150097f, -0.860085390f, + 0.508830143f, -0.860866939f, + 0.507508991f, -0.861646461f, + 0.506186645f, -0.862423956f, + 0.504863109f, -0.863199422f, + 0.503538384f, -0.863972856f, + 0.502212474f, -0.864744258f, + 0.500885383f, -0.865513624f, + 0.499557113f, -0.866280954f, + 0.498227667f, -0.867046246f, + 0.496897049f, -0.867809497f, + 0.495565262f, -0.868570706f, + 0.494232309f, -0.869329871f, + 0.492898192f, -0.870086991f, + 0.491562916f, -0.870842063f, + 0.490226483f, -0.871595087f, + 0.488888897f, -0.872346059f, + 0.487550160f, -0.873094978f, + 0.486210276f, -0.873841843f, + 0.484869248f, -0.874586652f, + 0.483527079f, -0.875329403f, + 0.482183772f, -0.876070094f, + 0.480839331f, -0.876808724f, + 0.479493758f, -0.877545290f, + 0.478147056f, -0.878279792f, + 0.476799230f, -0.879012226f, + 0.475450282f, -0.879742593f, + 0.474100215f, -0.880470889f, + 0.472749032f, -0.881197113f, + 0.471396737f, -0.881921264f, + 0.470043332f, -0.882643340f, + 0.468688822f, -0.883363339f, + 0.467333209f, -0.884081259f, + 0.465976496f, -0.884797098f, + 0.464618686f, -0.885510856f, + 0.463259784f, -0.886222530f, + 0.461899791f, -0.886932119f, + 0.460538711f, -0.887639620f, + 0.459176548f, -0.888345033f, + 0.457813304f, -0.889048356f, + 0.456448982f, -0.889749586f, + 0.455083587f, -0.890448723f, + 0.453717121f, -0.891145765f, + 0.452349587f, -0.891840709f, + 0.450980989f, -0.892533555f, + 0.449611330f, -0.893224301f, + 0.448240612f, -0.893912945f, + 0.446868840f, -0.894599486f, + 0.445496017f, -0.895283921f, + 0.444122145f, -0.895966250f, + 0.442747228f, -0.896646470f, + 0.441371269f, -0.897324581f, + 0.439994271f, -0.898000580f, + 0.438616239f, -0.898674466f, + 0.437237174f, -0.899346237f, + 0.435857080f, -0.900015892f, + 0.434475961f, -0.900683429f, + 0.433093819f, -0.901348847f, + 0.431710658f, -0.902012144f, + 0.430326481f, -0.902673318f, + 0.428941292f, -0.903332368f, + 0.427555093f, -0.903989293f, + 0.426167889f, -0.904644091f, + 0.424779681f, -0.905296759f, + 0.423390474f, -0.905947298f, + 0.422000271f, -0.906595705f, + 0.420609074f, -0.907241978f, + 0.419216888f, -0.907886116f, + 0.417823716f, -0.908528119f, + 0.416429560f, -0.909167983f, + 0.415034424f, -0.909805708f, + 0.413638312f, -0.910441292f, + 0.412241227f, -0.911074734f, + 0.410843171f, -0.911706032f, + 0.409444149f, -0.912335185f, + 0.408044163f, -0.912962190f, + 0.406643217f, -0.913587048f, + 0.405241314f, -0.914209756f, + 0.403838458f, -0.914830312f, + 0.402434651f, -0.915448716f, + 0.401029897f, -0.916064966f, + 0.399624200f, -0.916679060f, + 0.398217562f, -0.917290997f, + 0.396809987f, -0.917900776f, + 0.395401479f, -0.918508394f, + 0.393992040f, -0.919113852f, + 0.392581674f, -0.919717146f, + 0.391170384f, -0.920318277f, + 0.389758174f, -0.920917242f, + 0.388345047f, -0.921514039f, + 0.386931006f, -0.922108669f, + 0.385516054f, -0.922701128f, + 0.384100195f, -0.923291417f, + 0.382683432f, -0.923879533f, + 0.381265769f, -0.924465474f, + 0.379847209f, -0.925049241f, + 0.378427755f, -0.925630831f, + 0.377007410f, -0.926210242f, + 0.375586178f, -0.926787474f, + 0.374164063f, -0.927362526f, + 0.372741067f, -0.927935395f, + 0.371317194f, -0.928506080f, + 0.369892447f, -0.929074581f, + 0.368466830f, -0.929640896f, + 0.367040346f, -0.930205023f, + 0.365612998f, -0.930766961f, + 0.364184790f, -0.931326709f, + 0.362755724f, -0.931884266f, + 0.361325806f, -0.932439629f, + 0.359895037f, -0.932992799f, + 0.358463421f, -0.933543773f, + 0.357030961f, -0.934092550f, + 0.355597662f, -0.934639130f, + 0.354163525f, -0.935183510f, + 0.352728556f, -0.935725689f, + 0.351292756f, -0.936265667f, + 0.349856130f, -0.936803442f, + 0.348418680f, -0.937339012f, + 0.346980411f, -0.937872376f, + 0.345541325f, -0.938403534f, + 0.344101426f, -0.938932484f, + 0.342660717f, -0.939459224f, + 0.341219202f, -0.939983753f, + 0.339776884f, -0.940506071f, + 0.338333767f, -0.941026175f, + 0.336889853f, -0.941544065f, + 0.335445147f, -0.942059740f, + 0.333999651f, -0.942573198f, + 0.332553370f, -0.943084437f, + 0.331106306f, -0.943593458f, + 0.329658463f, -0.944100258f, + 0.328209844f, -0.944604837f, + 0.326760452f, -0.945107193f, + 0.325310292f, -0.945607325f, + 0.323859367f, -0.946105232f, + 0.322407679f, -0.946600913f, + 0.320955232f, -0.947094366f, + 0.319502031f, -0.947585591f, + 0.318048077f, -0.948074586f, + 0.316593376f, -0.948561350f, + 0.315137929f, -0.949045882f, + 0.313681740f, -0.949528181f, + 0.312224814f, -0.950008245f, + 0.310767153f, -0.950486074f, + 0.309308760f, -0.950961666f, + 0.307849640f, -0.951435021f, + 0.306389795f, -0.951906137f, + 0.304929230f, -0.952375013f, + 0.303467947f, -0.952841648f, + 0.302005949f, -0.953306040f, + 0.300543241f, -0.953768190f, + 0.299079826f, -0.954228095f, + 0.297615707f, -0.954685755f, + 0.296150888f, -0.955141168f, + 0.294685372f, -0.955594334f, + 0.293219163f, -0.956045251f, + 0.291752263f, -0.956493919f, + 0.290284677f, -0.956940336f, + 0.288816408f, -0.957384501f, + 0.287347460f, -0.957826413f, + 0.285877835f, -0.958266071f, + 0.284407537f, -0.958703475f, + 0.282936570f, -0.959138622f, + 0.281464938f, -0.959571513f, + 0.279992643f, -0.960002146f, + 0.278519689f, -0.960430519f, + 0.277046080f, -0.960856633f, + 0.275571819f, -0.961280486f, + 0.274096910f, -0.961702077f, + 0.272621355f, -0.962121404f, + 0.271145160f, -0.962538468f, + 0.269668326f, -0.962953267f, + 0.268190857f, -0.963365800f, + 0.266712757f, -0.963776066f, + 0.265234030f, -0.964184064f, + 0.263754679f, -0.964589793f, + 0.262274707f, -0.964993253f, + 0.260794118f, -0.965394442f, + 0.259312915f, -0.965793359f, + 0.257831102f, -0.966190003f, + 0.256348682f, -0.966584374f, + 0.254865660f, -0.966976471f, + 0.253382037f, -0.967366292f, + 0.251897818f, -0.967753837f, + 0.250413007f, -0.968139105f, + 0.248927606f, -0.968522094f, + 0.247441619f, -0.968902805f, + 0.245955050f, -0.969281235f, + 0.244467903f, -0.969657385f, + 0.242980180f, -0.970031253f, + 0.241491885f, -0.970402839f, + 0.240003022f, -0.970772141f, + 0.238513595f, -0.971139158f, + 0.237023606f, -0.971503891f, + 0.235533059f, -0.971866337f, + 0.234041959f, -0.972226497f, + 0.232550307f, -0.972584369f, + 0.231058108f, -0.972939952f, + 0.229565366f, -0.973293246f, + 0.228072083f, -0.973644250f, + 0.226578264f, -0.973992962f, + 0.225083911f, -0.974339383f, + 0.223589029f, -0.974683511f, + 0.222093621f, -0.975025345f, + 0.220597690f, -0.975364885f, + 0.219101240f, -0.975702130f, + 0.217604275f, -0.976037079f, + 0.216106797f, -0.976369731f, + 0.214608811f, -0.976700086f, + 0.213110320f, -0.977028143f, + 0.211611327f, -0.977353900f, + 0.210111837f, -0.977677358f, + 0.208611852f, -0.977998515f, + 0.207111376f, -0.978317371f, + 0.205610413f, -0.978633924f, + 0.204108966f, -0.978948175f, + 0.202607039f, -0.979260123f, + 0.201104635f, -0.979569766f, + 0.199601758f, -0.979877104f, + 0.198098411f, -0.980182136f, + 0.196594598f, -0.980484862f, + 0.195090322f, -0.980785280f, + 0.193585587f, -0.981083391f, + 0.192080397f, -0.981379193f, + 0.190574755f, -0.981672686f, + 0.189068664f, -0.981963869f, + 0.187562129f, -0.982252741f, + 0.186055152f, -0.982539302f, + 0.184547737f, -0.982823551f, + 0.183039888f, -0.983105487f, + 0.181531608f, -0.983385110f, + 0.180022901f, -0.983662419f, + 0.178513771f, -0.983937413f, + 0.177004220f, -0.984210092f, + 0.175494253f, -0.984480455f, + 0.173983873f, -0.984748502f, + 0.172473084f, -0.985014231f, + 0.170961889f, -0.985277642f, + 0.169450291f, -0.985538735f, + 0.167938295f, -0.985797509f, + 0.166425904f, -0.986053963f, + 0.164913120f, -0.986308097f, + 0.163399949f, -0.986559910f, + 0.161886394f, -0.986809402f, + 0.160372457f, -0.987056571f, + 0.158858143f, -0.987301418f, + 0.157343456f, -0.987543942f, + 0.155828398f, -0.987784142f, + 0.154312973f, -0.988022017f, + 0.152797185f, -0.988257568f, + 0.151281038f, -0.988490793f, + 0.149764535f, -0.988721692f, + 0.148247679f, -0.988950265f, + 0.146730474f, -0.989176510f, + 0.145212925f, -0.989400428f, + 0.143695033f, -0.989622017f, + 0.142176804f, -0.989841278f, + 0.140658239f, -0.990058210f, + 0.139139344f, -0.990272812f, + 0.137620122f, -0.990485084f, + 0.136100575f, -0.990695025f, + 0.134580709f, -0.990902635f, + 0.133060525f, -0.991107914f, + 0.131540029f, -0.991310860f, + 0.130019223f, -0.991511473f, + 0.128498111f, -0.991709754f, + 0.126976696f, -0.991905700f, + 0.125454983f, -0.992099313f, + 0.123932975f, -0.992290591f, + 0.122410675f, -0.992479535f, + 0.120888087f, -0.992666142f, + 0.119365215f, -0.992850414f, + 0.117842062f, -0.993032350f, + 0.116318631f, -0.993211949f, + 0.114794927f, -0.993389211f, + 0.113270952f, -0.993564136f, + 0.111746711f, -0.993736722f, + 0.110222207f, -0.993906970f, + 0.108697444f, -0.994074879f, + 0.107172425f, -0.994240449f, + 0.105647154f, -0.994403680f, + 0.104121634f, -0.994564571f, + 0.102595869f, -0.994723121f, + 0.101069863f, -0.994879331f, + 0.099543619f, -0.995033199f, + 0.098017140f, -0.995184727f, + 0.096490431f, -0.995333912f, + 0.094963495f, -0.995480755f, + 0.093436336f, -0.995625256f, + 0.091908956f, -0.995767414f, + 0.090381361f, -0.995907229f, + 0.088853553f, -0.996044701f, + 0.087325535f, -0.996179829f, + 0.085797312f, -0.996312612f, + 0.084268888f, -0.996443051f, + 0.082740265f, -0.996571146f, + 0.081211447f, -0.996696895f, + 0.079682438f, -0.996820299f, + 0.078153242f, -0.996941358f, + 0.076623861f, -0.997060070f, + 0.075094301f, -0.997176437f, + 0.073564564f, -0.997290457f, + 0.072034653f, -0.997402130f, + 0.070504573f, -0.997511456f, + 0.068974328f, -0.997618435f, + 0.067443920f, -0.997723067f, + 0.065913353f, -0.997825350f, + 0.064382631f, -0.997925286f, + 0.062851758f, -0.998022874f, + 0.061320736f, -0.998118113f, + 0.059789571f, -0.998211003f, + 0.058258265f, -0.998301545f, + 0.056726821f, -0.998389737f, + 0.055195244f, -0.998475581f, + 0.053663538f, -0.998559074f, + 0.052131705f, -0.998640218f, + 0.050599749f, -0.998719012f, + 0.049067674f, -0.998795456f, + 0.047535484f, -0.998869550f, + 0.046003182f, -0.998941293f, + 0.044470772f, -0.999010686f, + 0.042938257f, -0.999077728f, + 0.041405641f, -0.999142419f, + 0.039872928f, -0.999204759f, + 0.038340120f, -0.999264747f, + 0.036807223f, -0.999322385f, + 0.035274239f, -0.999377670f, + 0.033741172f, -0.999430605f, + 0.032208025f, -0.999481187f, + 0.030674803f, -0.999529418f, + 0.029141509f, -0.999575296f, + 0.027608146f, -0.999618822f, + 0.026074718f, -0.999659997f, + 0.024541229f, -0.999698819f, + 0.023007681f, -0.999735288f, + 0.021474080f, -0.999769405f, + 0.019940429f, -0.999801170f, + 0.018406730f, -0.999830582f, + 0.016872988f, -0.999857641f, + 0.015339206f, -0.999882347f, + 0.013805389f, -0.999904701f, + 0.012271538f, -0.999924702f, + 0.010737659f, -0.999942350f, + 0.009203755f, -0.999957645f, + 0.007669829f, -0.999970586f, + 0.006135885f, -0.999981175f, + 0.004601926f, -0.999989411f, + 0.003067957f, -0.999995294f, + 0.001533980f, -0.999998823f +}; + + +/** + * \par + * Example code for the generation of the floating-point sine table: + *
+ * tableSize = 512;    
+ * for(n = 0; n < (tableSize + 1); n++)    
+ * {    
+ *	sinTable[n]=sin(2*pi*n/tableSize);    
+ * }
+ * \par + * where pi value is 3.14159265358979 + */ + +const float32_t sinTable_f32[FAST_MATH_TABLE_SIZE + 1] = { + 0.00000000f, 0.01227154f, 0.02454123f, 0.03680722f, 0.04906767f, 0.06132074f, + 0.07356456f, 0.08579731f, 0.09801714f, 0.11022221f, 0.12241068f, 0.13458071f, + 0.14673047f, 0.15885814f, 0.17096189f, 0.18303989f, 0.19509032f, 0.20711138f, + 0.21910124f, 0.23105811f, 0.24298018f, 0.25486566f, 0.26671276f, 0.27851969f, + 0.29028468f, 0.30200595f, 0.31368174f, 0.32531029f, 0.33688985f, 0.34841868f, + 0.35989504f, 0.37131719f, 0.38268343f, 0.39399204f, 0.40524131f, 0.41642956f, + 0.42755509f, 0.43861624f, 0.44961133f, 0.46053871f, 0.47139674f, 0.48218377f, + 0.49289819f, 0.50353838f, 0.51410274f, 0.52458968f, 0.53499762f, 0.54532499f, + 0.55557023f, 0.56573181f, 0.57580819f, 0.58579786f, 0.59569930f, 0.60551104f, + 0.61523159f, 0.62485949f, 0.63439328f, 0.64383154f, 0.65317284f, 0.66241578f, + 0.67155895f, 0.68060100f, 0.68954054f, 0.69837625f, 0.70710678f, 0.71573083f, + 0.72424708f, 0.73265427f, 0.74095113f, 0.74913639f, 0.75720885f, 0.76516727f, + 0.77301045f, 0.78073723f, 0.78834643f, 0.79583690f, 0.80320753f, 0.81045720f, + 0.81758481f, 0.82458930f, 0.83146961f, 0.83822471f, 0.84485357f, 0.85135519f, + 0.85772861f, 0.86397286f, 0.87008699f, 0.87607009f, 0.88192126f, 0.88763962f, + 0.89322430f, 0.89867447f, 0.90398929f, 0.90916798f, 0.91420976f, 0.91911385f, + 0.92387953f, 0.92850608f, 0.93299280f, 0.93733901f, 0.94154407f, 0.94560733f, + 0.94952818f, 0.95330604f, 0.95694034f, 0.96043052f, 0.96377607f, 0.96697647f, + 0.97003125f, 0.97293995f, 0.97570213f, 0.97831737f, 0.98078528f, 0.98310549f, + 0.98527764f, 0.98730142f, 0.98917651f, 0.99090264f, 0.99247953f, 0.99390697f, + 0.99518473f, 0.99631261f, 0.99729046f, 0.99811811f, 0.99879546f, 0.99932238f, + 0.99969882f, 0.99992470f, 1.00000000f, 0.99992470f, 0.99969882f, 0.99932238f, + 0.99879546f, 0.99811811f, 0.99729046f, 0.99631261f, 0.99518473f, 0.99390697f, + 0.99247953f, 0.99090264f, 0.98917651f, 0.98730142f, 0.98527764f, 0.98310549f, + 0.98078528f, 0.97831737f, 0.97570213f, 0.97293995f, 0.97003125f, 0.96697647f, + 0.96377607f, 0.96043052f, 0.95694034f, 0.95330604f, 0.94952818f, 0.94560733f, + 0.94154407f, 0.93733901f, 0.93299280f, 0.92850608f, 0.92387953f, 0.91911385f, + 0.91420976f, 0.90916798f, 0.90398929f, 0.89867447f, 0.89322430f, 0.88763962f, + 0.88192126f, 0.87607009f, 0.87008699f, 0.86397286f, 0.85772861f, 0.85135519f, + 0.84485357f, 0.83822471f, 0.83146961f, 0.82458930f, 0.81758481f, 0.81045720f, + 0.80320753f, 0.79583690f, 0.78834643f, 0.78073723f, 0.77301045f, 0.76516727f, + 0.75720885f, 0.74913639f, 0.74095113f, 0.73265427f, 0.72424708f, 0.71573083f, + 0.70710678f, 0.69837625f, 0.68954054f, 0.68060100f, 0.67155895f, 0.66241578f, + 0.65317284f, 0.64383154f, 0.63439328f, 0.62485949f, 0.61523159f, 0.60551104f, + 0.59569930f, 0.58579786f, 0.57580819f, 0.56573181f, 0.55557023f, 0.54532499f, + 0.53499762f, 0.52458968f, 0.51410274f, 0.50353838f, 0.49289819f, 0.48218377f, + 0.47139674f, 0.46053871f, 0.44961133f, 0.43861624f, 0.42755509f, 0.41642956f, + 0.40524131f, 0.39399204f, 0.38268343f, 0.37131719f, 0.35989504f, 0.34841868f, + 0.33688985f, 0.32531029f, 0.31368174f, 0.30200595f, 0.29028468f, 0.27851969f, + 0.26671276f, 0.25486566f, 0.24298018f, 0.23105811f, 0.21910124f, 0.20711138f, + 0.19509032f, 0.18303989f, 0.17096189f, 0.15885814f, 0.14673047f, 0.13458071f, + 0.12241068f, 0.11022221f, 0.09801714f, 0.08579731f, 0.07356456f, 0.06132074f, + 0.04906767f, 0.03680722f, 0.02454123f, 0.01227154f, 0.00000000f, -0.01227154f, + -0.02454123f, -0.03680722f, -0.04906767f, -0.06132074f, -0.07356456f, + -0.08579731f, -0.09801714f, -0.11022221f, -0.12241068f, -0.13458071f, + -0.14673047f, -0.15885814f, -0.17096189f, -0.18303989f, -0.19509032f, + -0.20711138f, -0.21910124f, -0.23105811f, -0.24298018f, -0.25486566f, + -0.26671276f, -0.27851969f, -0.29028468f, -0.30200595f, -0.31368174f, + -0.32531029f, -0.33688985f, -0.34841868f, -0.35989504f, -0.37131719f, + -0.38268343f, -0.39399204f, -0.40524131f, -0.41642956f, -0.42755509f, + -0.43861624f, -0.44961133f, -0.46053871f, -0.47139674f, -0.48218377f, + -0.49289819f, -0.50353838f, -0.51410274f, -0.52458968f, -0.53499762f, + -0.54532499f, -0.55557023f, -0.56573181f, -0.57580819f, -0.58579786f, + -0.59569930f, -0.60551104f, -0.61523159f, -0.62485949f, -0.63439328f, + -0.64383154f, -0.65317284f, -0.66241578f, -0.67155895f, -0.68060100f, + -0.68954054f, -0.69837625f, -0.70710678f, -0.71573083f, -0.72424708f, + -0.73265427f, -0.74095113f, -0.74913639f, -0.75720885f, -0.76516727f, + -0.77301045f, -0.78073723f, -0.78834643f, -0.79583690f, -0.80320753f, + -0.81045720f, -0.81758481f, -0.82458930f, -0.83146961f, -0.83822471f, + -0.84485357f, -0.85135519f, -0.85772861f, -0.86397286f, -0.87008699f, + -0.87607009f, -0.88192126f, -0.88763962f, -0.89322430f, -0.89867447f, + -0.90398929f, -0.90916798f, -0.91420976f, -0.91911385f, -0.92387953f, + -0.92850608f, -0.93299280f, -0.93733901f, -0.94154407f, -0.94560733f, + -0.94952818f, -0.95330604f, -0.95694034f, -0.96043052f, -0.96377607f, + -0.96697647f, -0.97003125f, -0.97293995f, -0.97570213f, -0.97831737f, + -0.98078528f, -0.98310549f, -0.98527764f, -0.98730142f, -0.98917651f, + -0.99090264f, -0.99247953f, -0.99390697f, -0.99518473f, -0.99631261f, + -0.99729046f, -0.99811811f, -0.99879546f, -0.99932238f, -0.99969882f, + -0.99992470f, -1.00000000f, -0.99992470f, -0.99969882f, -0.99932238f, + -0.99879546f, -0.99811811f, -0.99729046f, -0.99631261f, -0.99518473f, + -0.99390697f, -0.99247953f, -0.99090264f, -0.98917651f, -0.98730142f, + -0.98527764f, -0.98310549f, -0.98078528f, -0.97831737f, -0.97570213f, + -0.97293995f, -0.97003125f, -0.96697647f, -0.96377607f, -0.96043052f, + -0.95694034f, -0.95330604f, -0.94952818f, -0.94560733f, -0.94154407f, + -0.93733901f, -0.93299280f, -0.92850608f, -0.92387953f, -0.91911385f, + -0.91420976f, -0.90916798f, -0.90398929f, -0.89867447f, -0.89322430f, + -0.88763962f, -0.88192126f, -0.87607009f, -0.87008699f, -0.86397286f, + -0.85772861f, -0.85135519f, -0.84485357f, -0.83822471f, -0.83146961f, + -0.82458930f, -0.81758481f, -0.81045720f, -0.80320753f, -0.79583690f, + -0.78834643f, -0.78073723f, -0.77301045f, -0.76516727f, -0.75720885f, + -0.74913639f, -0.74095113f, -0.73265427f, -0.72424708f, -0.71573083f, + -0.70710678f, -0.69837625f, -0.68954054f, -0.68060100f, -0.67155895f, + -0.66241578f, -0.65317284f, -0.64383154f, -0.63439328f, -0.62485949f, + -0.61523159f, -0.60551104f, -0.59569930f, -0.58579786f, -0.57580819f, + -0.56573181f, -0.55557023f, -0.54532499f, -0.53499762f, -0.52458968f, + -0.51410274f, -0.50353838f, -0.49289819f, -0.48218377f, -0.47139674f, + -0.46053871f, -0.44961133f, -0.43861624f, -0.42755509f, -0.41642956f, + -0.40524131f, -0.39399204f, -0.38268343f, -0.37131719f, -0.35989504f, + -0.34841868f, -0.33688985f, -0.32531029f, -0.31368174f, -0.30200595f, + -0.29028468f, -0.27851969f, -0.26671276f, -0.25486566f, -0.24298018f, + -0.23105811f, -0.21910124f, -0.20711138f, -0.19509032f, -0.18303989f, + -0.17096189f, -0.15885814f, -0.14673047f, -0.13458071f, -0.12241068f, + -0.11022221f, -0.09801714f, -0.08579731f, -0.07356456f, -0.06132074f, + -0.04906767f, -0.03680722f, -0.02454123f, -0.01227154f, -0.00000000f +}; + +/** + * \par + * Table values are in Q31 (1.31 fixed-point format) and generation is done in + * three steps. First, generate sin values in floating point: + *
+ * tableSize = 512;      
+ * for(n = 0; n < (tableSize + 1); n++)    
+ * {    
+ *	sinTable[n]= sin(2*pi*n/tableSize);    
+ * } 
+ * where pi value is 3.14159265358979 + * \par + * Second, convert floating-point to Q31 (Fixed point): + * (sinTable[i] * pow(2, 31)) + * \par + * Finally, round to the nearest integer value: + * sinTable[i] += (sinTable[i] > 0 ? 0.5 :-0.5); + */ +const q31_t sinTable_q31[FAST_MATH_TABLE_SIZE + 1] = { + 0x00000000, 0x01921D20, 0x03242ABF, 0x04B6195D, 0x0647D97C, 0x07D95B9E, + 0x096A9049, 0x0AFB6805, 0x0C8BD35E, 0x0E1BC2E4, 0x0FAB272B, 0x1139F0CF, + 0x12C8106F, 0x145576B1, 0x15E21445, 0x176DD9DE, 0x18F8B83C, 0x1A82A026, + 0x1C0B826A, 0x1D934FE5, 0x1F19F97B, 0x209F701C, 0x2223A4C5, 0x23A6887F, + 0x25280C5E, 0x26A82186, 0x2826B928, 0x29A3C485, 0x2B1F34EB, 0x2C98FBBA, + 0x2E110A62, 0x2F875262, 0x30FBC54D, 0x326E54C7, 0x33DEF287, 0x354D9057, + 0x36BA2014, 0x382493B0, 0x398CDD32, 0x3AF2EEB7, 0x3C56BA70, 0x3DB832A6, + 0x3F1749B8, 0x4073F21D, 0x41CE1E65, 0x4325C135, 0x447ACD50, 0x45CD358F, + 0x471CECE7, 0x4869E665, 0x49B41533, 0x4AFB6C98, 0x4C3FDFF4, 0x4D8162C4, + 0x4EBFE8A5, 0x4FFB654D, 0x5133CC94, 0x5269126E, 0x539B2AF0, 0x54CA0A4B, + 0x55F5A4D2, 0x571DEEFA, 0x5842DD54, 0x59646498, 0x5A82799A, 0x5B9D1154, + 0x5CB420E0, 0x5DC79D7C, 0x5ED77C8A, 0x5FE3B38D, 0x60EC3830, 0x61F1003F, + 0x62F201AC, 0x63EF3290, 0x64E88926, 0x65DDFBD3, 0x66CF8120, 0x67BD0FBD, + 0x68A69E81, 0x698C246C, 0x6A6D98A4, 0x6B4AF279, 0x6C242960, 0x6CF934FC, + 0x6DCA0D14, 0x6E96A99D, 0x6F5F02B2, 0x7023109A, 0x70E2CBC6, 0x719E2CD2, + 0x72552C85, 0x7307C3D0, 0x73B5EBD1, 0x745F9DD1, 0x7504D345, 0x75A585CF, + 0x7641AF3D, 0x76D94989, 0x776C4EDB, 0x77FAB989, 0x78848414, 0x7909A92D, + 0x798A23B1, 0x7A05EEAD, 0x7A7D055B, 0x7AEF6323, 0x7B5D039E, 0x7BC5E290, + 0x7C29FBEE, 0x7C894BDE, 0x7CE3CEB2, 0x7D3980EC, 0x7D8A5F40, 0x7DD6668F, + 0x7E1D93EA, 0x7E5FE493, 0x7E9D55FC, 0x7ED5E5C6, 0x7F0991C4, 0x7F3857F6, + 0x7F62368F, 0x7F872BF3, 0x7FA736B4, 0x7FC25596, 0x7FD8878E, 0x7FE9CBC0, + 0x7FF62182, 0x7FFD885A, 0x7FFFFFFF, 0x7FFD885A, 0x7FF62182, 0x7FE9CBC0, + 0x7FD8878E, 0x7FC25596, 0x7FA736B4, 0x7F872BF3, 0x7F62368F, 0x7F3857F6, + 0x7F0991C4, 0x7ED5E5C6, 0x7E9D55FC, 0x7E5FE493, 0x7E1D93EA, 0x7DD6668F, + 0x7D8A5F40, 0x7D3980EC, 0x7CE3CEB2, 0x7C894BDE, 0x7C29FBEE, 0x7BC5E290, + 0x7B5D039E, 0x7AEF6323, 0x7A7D055B, 0x7A05EEAD, 0x798A23B1, 0x7909A92D, + 0x78848414, 0x77FAB989, 0x776C4EDB, 0x76D94989, 0x7641AF3D, 0x75A585CF, + 0x7504D345, 0x745F9DD1, 0x73B5EBD1, 0x7307C3D0, 0x72552C85, 0x719E2CD2, + 0x70E2CBC6, 0x7023109A, 0x6F5F02B2, 0x6E96A99D, 0x6DCA0D14, 0x6CF934FC, + 0x6C242960, 0x6B4AF279, 0x6A6D98A4, 0x698C246C, 0x68A69E81, 0x67BD0FBD, + 0x66CF8120, 0x65DDFBD3, 0x64E88926, 0x63EF3290, 0x62F201AC, 0x61F1003F, + 0x60EC3830, 0x5FE3B38D, 0x5ED77C8A, 0x5DC79D7C, 0x5CB420E0, 0x5B9D1154, + 0x5A82799A, 0x59646498, 0x5842DD54, 0x571DEEFA, 0x55F5A4D2, 0x54CA0A4B, + 0x539B2AF0, 0x5269126E, 0x5133CC94, 0x4FFB654D, 0x4EBFE8A5, 0x4D8162C4, + 0x4C3FDFF4, 0x4AFB6C98, 0x49B41533, 0x4869E665, 0x471CECE7, 0x45CD358F, + 0x447ACD50, 0x4325C135, 0x41CE1E65, 0x4073F21D, 0x3F1749B8, 0x3DB832A6, + 0x3C56BA70, 0x3AF2EEB7, 0x398CDD32, 0x382493B0, 0x36BA2014, 0x354D9057, + 0x33DEF287, 0x326E54C7, 0x30FBC54D, 0x2F875262, 0x2E110A62, 0x2C98FBBA, + 0x2B1F34EB, 0x29A3C485, 0x2826B928, 0x26A82186, 0x25280C5E, 0x23A6887F, + 0x2223A4C5, 0x209F701C, 0x1F19F97B, 0x1D934FE5, 0x1C0B826A, 0x1A82A026, + 0x18F8B83C, 0x176DD9DE, 0x15E21445, 0x145576B1, 0x12C8106F, 0x1139F0CF, + 0x0FAB272B, 0x0E1BC2E4, 0x0C8BD35E, 0x0AFB6805, 0x096A9049, 0x07D95B9E, + 0x0647D97C, 0x04B6195D, 0x03242ABF, 0x01921D20, 0x00000000, 0xFE6DE2E0, + 0xFCDBD541, 0xFB49E6A3, 0xF9B82684, 0xF826A462, 0xF6956FB7, 0xF50497FB, + 0xF3742CA2, 0xF1E43D1C, 0xF054D8D5, 0xEEC60F31, 0xED37EF91, 0xEBAA894F, + 0xEA1DEBBB, 0xE8922622, 0xE70747C4, 0xE57D5FDA, 0xE3F47D96, 0xE26CB01B, + 0xE0E60685, 0xDF608FE4, 0xDDDC5B3B, 0xDC597781, 0xDAD7F3A2, 0xD957DE7A, + 0xD7D946D8, 0xD65C3B7B, 0xD4E0CB15, 0xD3670446, 0xD1EEF59E, 0xD078AD9E, + 0xCF043AB3, 0xCD91AB39, 0xCC210D79, 0xCAB26FA9, 0xC945DFEC, 0xC7DB6C50, + 0xC67322CE, 0xC50D1149, 0xC3A94590, 0xC247CD5A, 0xC0E8B648, 0xBF8C0DE3, + 0xBE31E19B, 0xBCDA3ECB, 0xBB8532B0, 0xBA32CA71, 0xB8E31319, 0xB796199B, + 0xB64BEACD, 0xB5049368, 0xB3C0200C, 0xB27E9D3C, 0xB140175B, 0xB0049AB3, + 0xAECC336C, 0xAD96ED92, 0xAC64D510, 0xAB35F5B5, 0xAA0A5B2E, 0xA8E21106, + 0xA7BD22AC, 0xA69B9B68, 0xA57D8666, 0xA462EEAC, 0xA34BDF20, 0xA2386284, + 0xA1288376, 0xA01C4C73, 0x9F13C7D0, 0x9E0EFFC1, 0x9D0DFE54, 0x9C10CD70, + 0x9B1776DA, 0x9A22042D, 0x99307EE0, 0x9842F043, 0x9759617F, 0x9673DB94, + 0x9592675C, 0x94B50D87, 0x93DBD6A0, 0x9306CB04, 0x9235F2EC, 0x91695663, + 0x90A0FD4E, 0x8FDCEF66, 0x8F1D343A, 0x8E61D32E, 0x8DAAD37B, 0x8CF83C30, + 0x8C4A142F, 0x8BA0622F, 0x8AFB2CBB, 0x8A5A7A31, 0x89BE50C3, 0x8926B677, + 0x8893B125, 0x88054677, 0x877B7BEC, 0x86F656D3, 0x8675DC4F, 0x85FA1153, + 0x8582FAA5, 0x85109CDD, 0x84A2FC62, 0x843A1D70, 0x83D60412, 0x8376B422, + 0x831C314E, 0x82C67F14, 0x8275A0C0, 0x82299971, 0x81E26C16, 0x81A01B6D, + 0x8162AA04, 0x812A1A3A, 0x80F66E3C, 0x80C7A80A, 0x809DC971, 0x8078D40D, + 0x8058C94C, 0x803DAA6A, 0x80277872, 0x80163440, 0x8009DE7E, 0x800277A6, + 0x80000000, 0x800277A6, 0x8009DE7E, 0x80163440, 0x80277872, 0x803DAA6A, + 0x8058C94C, 0x8078D40D, 0x809DC971, 0x80C7A80A, 0x80F66E3C, 0x812A1A3A, + 0x8162AA04, 0x81A01B6D, 0x81E26C16, 0x82299971, 0x8275A0C0, 0x82C67F14, + 0x831C314E, 0x8376B422, 0x83D60412, 0x843A1D70, 0x84A2FC62, 0x85109CDD, + 0x8582FAA5, 0x85FA1153, 0x8675DC4F, 0x86F656D3, 0x877B7BEC, 0x88054677, + 0x8893B125, 0x8926B677, 0x89BE50C3, 0x8A5A7A31, 0x8AFB2CBB, 0x8BA0622F, + 0x8C4A142F, 0x8CF83C30, 0x8DAAD37B, 0x8E61D32E, 0x8F1D343A, 0x8FDCEF66, + 0x90A0FD4E, 0x91695663, 0x9235F2EC, 0x9306CB04, 0x93DBD6A0, 0x94B50D87, + 0x9592675C, 0x9673DB94, 0x9759617F, 0x9842F043, 0x99307EE0, 0x9A22042D, + 0x9B1776DA, 0x9C10CD70, 0x9D0DFE54, 0x9E0EFFC1, 0x9F13C7D0, 0xA01C4C73, + 0xA1288376, 0xA2386284, 0xA34BDF20, 0xA462EEAC, 0xA57D8666, 0xA69B9B68, + 0xA7BD22AC, 0xA8E21106, 0xAA0A5B2E, 0xAB35F5B5, 0xAC64D510, 0xAD96ED92, + 0xAECC336C, 0xB0049AB3, 0xB140175B, 0xB27E9D3C, 0xB3C0200C, 0xB5049368, + 0xB64BEACD, 0xB796199B, 0xB8E31319, 0xBA32CA71, 0xBB8532B0, 0xBCDA3ECB, + 0xBE31E19B, 0xBF8C0DE3, 0xC0E8B648, 0xC247CD5A, 0xC3A94590, 0xC50D1149, + 0xC67322CE, 0xC7DB6C50, 0xC945DFEC, 0xCAB26FA9, 0xCC210D79, 0xCD91AB39, + 0xCF043AB3, 0xD078AD9E, 0xD1EEF59E, 0xD3670446, 0xD4E0CB15, 0xD65C3B7B, + 0xD7D946D8, 0xD957DE7A, 0xDAD7F3A2, 0xDC597781, 0xDDDC5B3B, 0xDF608FE4, + 0xE0E60685, 0xE26CB01B, 0xE3F47D96, 0xE57D5FDA, 0xE70747C4, 0xE8922622, + 0xEA1DEBBB, 0xEBAA894F, 0xED37EF91, 0xEEC60F31, 0xF054D8D5, 0xF1E43D1C, + 0xF3742CA2, 0xF50497FB, 0xF6956FB7, 0xF826A462, 0xF9B82684, 0xFB49E6A3, + 0xFCDBD541, 0xFE6DE2E0, 0x00000000 +}; + +/** + * \par + * Table values are in Q15 (1.15 fixed-point format) and generation is done in + * three steps. First, generate sin values in floating point: + *
+ * tableSize = 512;      
+ * for(n = 0; n < (tableSize + 1); n++)    
+ * {    
+ *	sinTable[n]= sin(2*pi*n/tableSize);    
+ * } 
+ * where pi value is 3.14159265358979 + * \par + * Second, convert floating-point to Q15 (Fixed point): + * (sinTable[i] * pow(2, 15)) + * \par + * Finally, round to the nearest integer value: + * sinTable[i] += (sinTable[i] > 0 ? 0.5 :-0.5); + */ +const q15_t sinTable_q15[FAST_MATH_TABLE_SIZE + 1] = { + 0x0000, 0x0192, 0x0324, 0x04B6, 0x0648, 0x07D9, 0x096B, 0x0AFB, 0x0C8C, 0x0E1C, 0x0FAB, 0x113A, 0x12C8, + 0x1455, 0x15E2, 0x176E, 0x18F9, 0x1A83, 0x1C0C, 0x1D93, 0x1F1A, 0x209F, 0x2224, 0x23A7, 0x2528, 0x26A8, + 0x2827, 0x29A4, 0x2B1F, 0x2C99, 0x2E11, 0x2F87, 0x30FC, 0x326E, 0x33DF, 0x354E, 0x36BA, 0x3825, 0x398D, + 0x3AF3, 0x3C57, 0x3DB8, 0x3F17, 0x4074, 0x41CE, 0x4326, 0x447B, 0x45CD, 0x471D, 0x486A, 0x49B4, 0x4AFB, + 0x4C40, 0x4D81, 0x4EC0, 0x4FFB, 0x5134, 0x5269, 0x539B, 0x54CA, 0x55F6, 0x571E, 0x5843, 0x5964, 0x5A82, + 0x5B9D, 0x5CB4, 0x5DC8, 0x5ED7, 0x5FE4, 0x60EC, 0x61F1, 0x62F2, 0x63EF, 0x64E9, 0x65DE, 0x66D0, 0x67BD, + 0x68A7, 0x698C, 0x6A6E, 0x6B4B, 0x6C24, 0x6CF9, 0x6DCA, 0x6E97, 0x6F5F, 0x7023, 0x70E3, 0x719E, 0x7255, + 0x7308, 0x73B6, 0x7460, 0x7505, 0x75A6, 0x7642, 0x76D9, 0x776C, 0x77FB, 0x7885, 0x790A, 0x798A, 0x7A06, + 0x7A7D, 0x7AEF, 0x7B5D, 0x7BC6, 0x7C2A, 0x7C89, 0x7CE4, 0x7D3A, 0x7D8A, 0x7DD6, 0x7E1E, 0x7E60, 0x7E9D, + 0x7ED6, 0x7F0A, 0x7F38, 0x7F62, 0x7F87, 0x7FA7, 0x7FC2, 0x7FD9, 0x7FEA, 0x7FF6, 0x7FFE, 0x7FFF, 0x7FFE, + 0x7FF6, 0x7FEA, 0x7FD9, 0x7FC2, 0x7FA7, 0x7F87, 0x7F62, 0x7F38, 0x7F0A, 0x7ED6, 0x7E9D, 0x7E60, 0x7E1E, + 0x7DD6, 0x7D8A, 0x7D3A, 0x7CE4, 0x7C89, 0x7C2A, 0x7BC6, 0x7B5D, 0x7AEF, 0x7A7D, 0x7A06, 0x798A, 0x790A, + 0x7885, 0x77FB, 0x776C, 0x76D9, 0x7642, 0x75A6, 0x7505, 0x7460, 0x73B6, 0x7308, 0x7255, 0x719E, 0x70E3, + 0x7023, 0x6F5F, 0x6E97, 0x6DCA, 0x6CF9, 0x6C24, 0x6B4B, 0x6A6E, 0x698C, 0x68A7, 0x67BD, 0x66D0, 0x65DE, + 0x64E9, 0x63EF, 0x62F2, 0x61F1, 0x60EC, 0x5FE4, 0x5ED7, 0x5DC8, 0x5CB4, 0x5B9D, 0x5A82, 0x5964, 0x5843, + 0x571E, 0x55F6, 0x54CA, 0x539B, 0x5269, 0x5134, 0x4FFB, 0x4EC0, 0x4D81, 0x4C40, 0x4AFB, 0x49B4, 0x486A, + 0x471D, 0x45CD, 0x447B, 0x4326, 0x41CE, 0x4074, 0x3F17, 0x3DB8, 0x3C57, 0x3AF3, 0x398D, 0x3825, 0x36BA, + 0x354E, 0x33DF, 0x326E, 0x30FC, 0x2F87, 0x2E11, 0x2C99, 0x2B1F, 0x29A4, 0x2827, 0x26A8, 0x2528, 0x23A7, + 0x2224, 0x209F, 0x1F1A, 0x1D93, 0x1C0C, 0x1A83, 0x18F9, 0x176E, 0x15E2, 0x1455, 0x12C8, 0x113A, 0x0FAB, + 0x0E1C, 0x0C8C, 0x0AFB, 0x096B, 0x07D9, 0x0648, 0x04B6, 0x0324, 0x0192, 0x0000, 0xFE6E, 0xFCDC, 0xFB4A, + 0xF9B8, 0xF827, 0xF695, 0xF505, 0xF374, 0xF1E4, 0xF055, 0xEEC6, 0xED38, 0xEBAB, 0xEA1E, 0xE892, 0xE707, + 0xE57D, 0xE3F4, 0xE26D, 0xE0E6, 0xDF61, 0xDDDC, 0xDC59, 0xDAD8, 0xD958, 0xD7D9, 0xD65C, 0xD4E1, 0xD367, + 0xD1EF, 0xD079, 0xCF04, 0xCD92, 0xCC21, 0xCAB2, 0xC946, 0xC7DB, 0xC673, 0xC50D, 0xC3A9, 0xC248, 0xC0E9, + 0xBF8C, 0xBE32, 0xBCDA, 0xBB85, 0xBA33, 0xB8E3, 0xB796, 0xB64C, 0xB505, 0xB3C0, 0xB27F, 0xB140, 0xB005, + 0xAECC, 0xAD97, 0xAC65, 0xAB36, 0xAA0A, 0xA8E2, 0xA7BD, 0xA69C, 0xA57E, 0xA463, 0xA34C, 0xA238, 0xA129, + 0xA01C, 0x9F14, 0x9E0F, 0x9D0E, 0x9C11, 0x9B17, 0x9A22, 0x9930, 0x9843, 0x9759, 0x9674, 0x9592, 0x94B5, + 0x93DC, 0x9307, 0x9236, 0x9169, 0x90A1, 0x8FDD, 0x8F1D, 0x8E62, 0x8DAB, 0x8CF8, 0x8C4A, 0x8BA0, 0x8AFB, + 0x8A5A, 0x89BE, 0x8927, 0x8894, 0x8805, 0x877B, 0x86F6, 0x8676, 0x85FA, 0x8583, 0x8511, 0x84A3, 0x843A, + 0x83D6, 0x8377, 0x831C, 0x82C6, 0x8276, 0x822A, 0x81E2, 0x81A0, 0x8163, 0x812A, 0x80F6, 0x80C8, 0x809E, + 0x8079, 0x8059, 0x803E, 0x8027, 0x8016, 0x800A, 0x8002, 0x8000, 0x8002, 0x800A, 0x8016, 0x8027, 0x803E, + 0x8059, 0x8079, 0x809E, 0x80C8, 0x80F6, 0x812A, 0x8163, 0x81A0, 0x81E2, 0x822A, 0x8276, 0x82C6, 0x831C, + 0x8377, 0x83D6, 0x843A, 0x84A3, 0x8511, 0x8583, 0x85FA, 0x8676, 0x86F6, 0x877B, 0x8805, 0x8894, 0x8927, + 0x89BE, 0x8A5A, 0x8AFB, 0x8BA0, 0x8C4A, 0x8CF8, 0x8DAB, 0x8E62, 0x8F1D, 0x8FDD, 0x90A1, 0x9169, 0x9236, + 0x9307, 0x93DC, 0x94B5, 0x9592, 0x9674, 0x9759, 0x9843, 0x9930, 0x9A22, 0x9B17, 0x9C11, 0x9D0E, 0x9E0F, + 0x9F14, 0xA01C, 0xA129, 0xA238, 0xA34C, 0xA463, 0xA57E, 0xA69C, 0xA7BD, 0xA8E2, 0xAA0A, 0xAB36, 0xAC65, + 0xAD97, 0xAECC, 0xB005, 0xB140, 0xB27F, 0xB3C0, 0xB505, 0xB64C, 0xB796, 0xB8E3, 0xBA33, 0xBB85, 0xBCDA, + 0xBE32, 0xBF8C, 0xC0E9, 0xC248, 0xC3A9, 0xC50D, 0xC673, 0xC7DB, 0xC946, 0xCAB2, 0xCC21, 0xCD92, 0xCF04, + 0xD079, 0xD1EF, 0xD367, 0xD4E1, 0xD65C, 0xD7D9, 0xD958, 0xDAD8, 0xDC59, 0xDDDC, 0xDF61, 0xE0E6, 0xE26D, + 0xE3F4, 0xE57D, 0xE707, 0xE892, 0xEA1E, 0xEBAB, 0xED38, 0xEEC6, 0xF055, 0xF1E4, 0xF374, 0xF505, 0xF695, + 0xF827, 0xF9B8, 0xFB4A, 0xFCDC, 0xFE6E, 0x0000 +}; diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_const_structs.c b/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_const_structs.c new file mode 100644 index 0000000..f05c1c2 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_const_structs.c @@ -0,0 +1,156 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2014 ARM Limited. All rights reserved. +* +* $Date: 19. March 2015 +* $Revision: V.1.4.5 +* +* Project: CMSIS DSP Library +* Title: arm_const_structs.c +* +* Description: This file has constant structs that are initialized for +* user convenience. For example, some can be given as +* arguments to the arm_cfft_f32() function. +* +* Target Processor: Cortex-M4/Cortex-M3 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* -------------------------------------------------------------------- */ + +#include "arm_const_structs.h" + +//Floating-point structs + +const arm_cfft_instance_f32 arm_cfft_sR_f32_len16 = { + 16, twiddleCoef_16, armBitRevIndexTable16, ARMBITREVINDEXTABLE__16_TABLE_LENGTH +}; + +const arm_cfft_instance_f32 arm_cfft_sR_f32_len32 = { + 32, twiddleCoef_32, armBitRevIndexTable32, ARMBITREVINDEXTABLE__32_TABLE_LENGTH +}; + +const arm_cfft_instance_f32 arm_cfft_sR_f32_len64 = { + 64, twiddleCoef_64, armBitRevIndexTable64, ARMBITREVINDEXTABLE__64_TABLE_LENGTH +}; + +const arm_cfft_instance_f32 arm_cfft_sR_f32_len128 = { + 128, twiddleCoef_128, armBitRevIndexTable128, ARMBITREVINDEXTABLE_128_TABLE_LENGTH +}; + +const arm_cfft_instance_f32 arm_cfft_sR_f32_len256 = { + 256, twiddleCoef_256, armBitRevIndexTable256, ARMBITREVINDEXTABLE_256_TABLE_LENGTH +}; + +const arm_cfft_instance_f32 arm_cfft_sR_f32_len512 = { + 512, twiddleCoef_512, armBitRevIndexTable512, ARMBITREVINDEXTABLE_512_TABLE_LENGTH +}; + +const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024 = { + 1024, twiddleCoef_1024, armBitRevIndexTable1024, ARMBITREVINDEXTABLE1024_TABLE_LENGTH +}; + +const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048 = { + 2048, twiddleCoef_2048, armBitRevIndexTable2048, ARMBITREVINDEXTABLE2048_TABLE_LENGTH +}; + +const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096 = { + 4096, twiddleCoef_4096, armBitRevIndexTable4096, ARMBITREVINDEXTABLE4096_TABLE_LENGTH +}; + +//Fixed-point structs + +const arm_cfft_instance_q31 arm_cfft_sR_q31_len16 = { + 16, twiddleCoef_16_q31, armBitRevIndexTable_fixed_16, ARMBITREVINDEXTABLE_FIXED___16_TABLE_LENGTH +}; + +const arm_cfft_instance_q31 arm_cfft_sR_q31_len32 = { + 32, twiddleCoef_32_q31, armBitRevIndexTable_fixed_32, ARMBITREVINDEXTABLE_FIXED___32_TABLE_LENGTH +}; + +const arm_cfft_instance_q31 arm_cfft_sR_q31_len64 = { + 64, twiddleCoef_64_q31, armBitRevIndexTable_fixed_64, ARMBITREVINDEXTABLE_FIXED___64_TABLE_LENGTH +}; + +const arm_cfft_instance_q31 arm_cfft_sR_q31_len128 = { + 128, twiddleCoef_128_q31, armBitRevIndexTable_fixed_128, ARMBITREVINDEXTABLE_FIXED__128_TABLE_LENGTH +}; + +const arm_cfft_instance_q31 arm_cfft_sR_q31_len256 = { + 256, twiddleCoef_256_q31, armBitRevIndexTable_fixed_256, ARMBITREVINDEXTABLE_FIXED__256_TABLE_LENGTH +}; + +const arm_cfft_instance_q31 arm_cfft_sR_q31_len512 = { + 512, twiddleCoef_512_q31, armBitRevIndexTable_fixed_512, ARMBITREVINDEXTABLE_FIXED__512_TABLE_LENGTH +}; + +const arm_cfft_instance_q31 arm_cfft_sR_q31_len1024 = { + 1024, twiddleCoef_1024_q31, armBitRevIndexTable_fixed_1024, ARMBITREVINDEXTABLE_FIXED_1024_TABLE_LENGTH +}; + +const arm_cfft_instance_q31 arm_cfft_sR_q31_len2048 = { + 2048, twiddleCoef_2048_q31, armBitRevIndexTable_fixed_2048, ARMBITREVINDEXTABLE_FIXED_2048_TABLE_LENGTH +}; + +const arm_cfft_instance_q31 arm_cfft_sR_q31_len4096 = { + 4096, twiddleCoef_4096_q31, armBitRevIndexTable_fixed_4096, ARMBITREVINDEXTABLE_FIXED_4096_TABLE_LENGTH +}; + + +const arm_cfft_instance_q15 arm_cfft_sR_q15_len16 = { + 16, twiddleCoef_16_q15, armBitRevIndexTable_fixed_16, ARMBITREVINDEXTABLE_FIXED___16_TABLE_LENGTH +}; + +const arm_cfft_instance_q15 arm_cfft_sR_q15_len32 = { + 32, twiddleCoef_32_q15, armBitRevIndexTable_fixed_32, ARMBITREVINDEXTABLE_FIXED___32_TABLE_LENGTH +}; + +const arm_cfft_instance_q15 arm_cfft_sR_q15_len64 = { + 64, twiddleCoef_64_q15, armBitRevIndexTable_fixed_64, ARMBITREVINDEXTABLE_FIXED___64_TABLE_LENGTH +}; + +const arm_cfft_instance_q15 arm_cfft_sR_q15_len128 = { + 128, twiddleCoef_128_q15, armBitRevIndexTable_fixed_128, ARMBITREVINDEXTABLE_FIXED__128_TABLE_LENGTH +}; + +const arm_cfft_instance_q15 arm_cfft_sR_q15_len256 = { + 256, twiddleCoef_256_q15, armBitRevIndexTable_fixed_256, ARMBITREVINDEXTABLE_FIXED__256_TABLE_LENGTH +}; + +const arm_cfft_instance_q15 arm_cfft_sR_q15_len512 = { + 512, twiddleCoef_512_q15, armBitRevIndexTable_fixed_512, ARMBITREVINDEXTABLE_FIXED__512_TABLE_LENGTH +}; + +const arm_cfft_instance_q15 arm_cfft_sR_q15_len1024 = { + 1024, twiddleCoef_1024_q15, armBitRevIndexTable_fixed_1024, ARMBITREVINDEXTABLE_FIXED_1024_TABLE_LENGTH +}; + +const arm_cfft_instance_q15 arm_cfft_sR_q15_len2048 = { + 2048, twiddleCoef_2048_q15, armBitRevIndexTable_fixed_2048, ARMBITREVINDEXTABLE_FIXED_2048_TABLE_LENGTH +}; + +const arm_cfft_instance_q15 arm_cfft_sR_q15_len4096 = { + 4096, twiddleCoef_4096_q15, armBitRevIndexTable_fixed_4096, ARMBITREVINDEXTABLE_FIXED_4096_TABLE_LENGTH +}; diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_cos_f32.c b/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_cos_f32.c new file mode 100644 index 0000000..29e82e7 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_cos_f32.c @@ -0,0 +1,128 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2014 ARM Limited. All rights reserved. +* +* $Date: 21. September 2015 +* $Revision: V.1.4.5 a +* +* Project: CMSIS DSP Library +* Title: arm_cos_f32.c +* +* Description: Fast cosine calculation for floating-point values. +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* -------------------------------------------------------------------- */ + +#include "arm_math.h" +#include "arm_common_tables.h" + +/** + * @ingroup groupFastMath + */ + +/** + * @defgroup cos Cosine + * + * Computes the trigonometric cosine function using a combination of table lookup + * and linear interpolation. There are separate functions for + * Q15, Q31, and floating-point data types. + * The input to the floating-point version is in radians while the + * fixed-point Q15 and Q31 have a scaled input with the range + * [0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a + * value of 2*pi wraps around to 0. + * + * The implementation is based on table lookup using 256 values together with linear interpolation. + * The steps used are: + * -# Calculation of the nearest integer table index + * -# Compute the fractional portion (fract) of the table index. + * -# The final result equals (1.0f-fract)*a + fract*b; + * + * where + *
+ *    b=Table[index+0];
+ *    c=Table[index+1];
+ * 
+ */ + + /** + * @addtogroup cos + * @{ + */ + +/** + * @brief Fast approximation to the trigonometric cosine function for floating-point data. + * @param[in] x input value in radians. + * @return cos(x). + */ + +float32_t arm_cos_f32( + float32_t x) +{ + float32_t cosVal, fract, in; /* Temporary variables for input, output */ + uint16_t index; /* Index variable */ + float32_t a, b; /* Two nearest output values */ + int32_t n; + float32_t findex; + + /* input x is in radians */ + /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi, add 0.25 (pi/2) to read sine table */ + in = x * 0.159154943092f + 0.25f; + + /* Calculation of floor value of input */ + n = (int32_t) in; + + /* Make negative values towards -infinity */ + if(in < 0.0f) + { + n--; + } + + /* Map input value to [0 1] */ + in = in - (float32_t) n; + + /* Calculation of index of the table */ + findex = (float32_t) FAST_MATH_TABLE_SIZE * in; + index = ((uint16_t)findex) & 0x1ff; + + /* fractional value calculation */ + fract = findex - (float32_t) index; + + /* Read two nearest values of input value from the cos table */ + a = sinTable_f32[index]; + b = sinTable_f32[index+1]; + + /* Linear interpolation process */ + cosVal = (1.0f-fract)*a + fract*b; + + /* Return the output value */ + return (cosVal); +} + +/** + * @} end of cos group + */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_sin_f32.c b/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_sin_f32.c new file mode 100644 index 0000000..8655cf9 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/src/arm_sin_f32.c @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2014 ARM Limited. All rights reserved. +* +* $Date: 21. September 2015 +* $Revision: V.1.4.5 a +* +* Project: CMSIS DSP Library +* Title: arm_sin_f32.c +* +* Description: Fast sine calculation for floating-point values. +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* -------------------------------------------------------------------- */ + +#include "arm_math.h" +#include "arm_common_tables.h" +#include + +/** + * @ingroup groupFastMath + */ + +/** + * @defgroup sin Sine + * + * Computes the trigonometric sine function using a combination of table lookup + * and linear interpolation. There are separate functions for + * Q15, Q31, and floating-point data types. + * The input to the floating-point version is in radians while the + * fixed-point Q15 and Q31 have a scaled input with the range + * [0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a + * value of 2*pi wraps around to 0. + * + * The implementation is based on table lookup using 256 values together with linear interpolation. + * The steps used are: + * -# Calculation of the nearest integer table index + * -# Compute the fractional portion (fract) of the table index. + * -# The final result equals (1.0f-fract)*a + fract*b; + * + * where + *
+ *    b=Table[index+0];
+ *    c=Table[index+1];
+ * 
+ */ + +/** + * @addtogroup sin + * @{ + */ + +/** + * @brief Fast approximation to the trigonometric sine function for floating-point data. + * @param[in] x input value in radians. + * @return sin(x). + */ + +float32_t arm_sin_f32( + float32_t x) +{ + float32_t sinVal, fract, in; /* Temporary variables for input, output */ + uint16_t index; /* Index variable */ + float32_t a, b; /* Two nearest output values */ + int32_t n; + float32_t findex; + + /* input x is in radians */ + /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */ + in = x * 0.159154943092f; + + /* Calculation of floor value of input */ + n = (int32_t) in; + + /* Make negative values towards -infinity */ + if(x < 0.0f) + { + n--; + } + + /* Map input value to [0 1] */ + in = in - (float32_t) n; + + /* Calculation of index of the table */ + findex = (float32_t) FAST_MATH_TABLE_SIZE * in; + if (findex >= 512.0f) { + findex -= 512.0f; + } + + index = ((uint16_t)findex) & 0x1ff; + + /* fractional value calculation */ + fract = findex - (float32_t) index; + + /* Read two nearest values of input value from the sin table */ + a = sinTable_f32[index]; + b = sinTable_f32[index+1]; + + /* Linear interpolation process */ + sinVal = (1.0f-fract)*a + fract*b; + + /* Return the output value */ + return (sinVal); +} + +/** + * @} end of sin group + */ diff --git a/examples/OpenIMU300RI/INS/lib/Core/Math/src/qmath.c b/examples/OpenIMU300RI/INS/lib/Core/Math/src/qmath.c new file mode 100644 index 0000000..c483256 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/Math/src/qmath.c @@ -0,0 +1,1322 @@ +/** *************************************************************************** + * @file qmath.c Fixed Point Math Library + * @Author Alex Forencich alex@alexelectronics.com + * Jordan Rhee rhee.jordan@gmail.com + * @date September, 2008 + * @brief Copyright (c) 2013, 2014 All Rights Reserved. + * + * 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 IEEE UCSD + * http://ieee.ucsd.edu + *****************************************************************************/ +#include "qmath.h" +#include "math.h" + + +#ifdef DISPLAY_DIAGNOSTIC_MSG +#include "debug.h" +#endif + +#include "Indices.h" + +#define Q27 134217728 +#define Q54 18014398509481984 + +/** + * square root + */ +fixedp qsqrt( fixedp p_Square ) +{ + // fixedp = long + fixedp res; + fixedp delta; + fixedp maxError; + + if ( p_Square <= 0 ) + return 0; + + /* start at half */ + res = (p_Square >> 1); + + /* determine allowable error */ + maxError = qmul( p_Square, FIXED_SQRT_ERR ); + +//#define qmul(x, y) _qmul(x, y, FIXED_FRACBITS, FIXED_FRACBITS, FIXED_FRACBITS) + + do { + delta = ( qmul( res, res ) - p_Square ); + res -= qdiv( delta, ( res << 1 ) ); + } while ( delta > maxError || delta < -maxError ); + + return res; +} + + +// Range of input: +/- 16 +int32_t qsqrt_q27( int32_t pSquare_q27 ) +{ + // fixedp = long + int32_t res; + int32_t delta; + int32_t maxError; + + // Check for negative number and return 0 (error checking: return -1???) + if( pSquare_q27 <= 0 ) { + return 0; + } + + /* start at half */ + res = ( pSquare_q27 >> 1 ); + + /* determine allowable error */ + // this limits the error to be less than 1e-6 + maxError = 135; + + uint8_t loopCntr = 0; + do { + loopCntr++; + delta = ( _qmul( res, res, 27, 27, 27 ) - pSquare_q27 ); + res = res - _qdiv( delta, ( res << 1 ), 27, 27, 27 ); + if( loopCntr >= 250 ) { +// DEBUG_STRING( " SQRT27 Err " ); + break; + } + } while ( delta > maxError || delta < -maxError ); + + // Do one more iteration for the heck of it + delta = ( _qmul( res, res, 27, 27, 27 ) - pSquare_q27 ); + res = res - _qdiv( delta, ( res << 1 ), 27, 27, 27 ); + return res; +} + + +// Range of input: +/- 2 +int32_t qsqrt_q30( int32_t pSquare_q30 ) +{ + // fixedp = long + int32_t res; + int32_t delta; + int32_t maxError; + + // Check for negative number and return 0 (error checking: return -1???) + if( pSquare_q30 <= 0 ) { + return 0; + } + + /* start at one (2^30) */ + res = 536870912; //pSquare_q30 >> 1; + + /* determine allowable error */ + // this limits the error to be less than 1e-6 + //maxError = 135; + maxError = 1080; + + uint8_t loopCntr = 0; + do { + loopCntr++; + + delta = ( _qmul( res, res, 30, 30, 30 ) - pSquare_q30 ); + res = res - _qdiv( delta, ( res << 1 ), 30, 30, 30 ); + + if( loopCntr == 250 ) { + // DEBUG_ENDLINE(); + // DEBUG_INT( " QSQRT_Q30 stopped at iteration ", loopCntr ); + // DEBUG_LONGINT( " (function argument: ", pSquare_q30 ); + // DEBUG_STRING( ")" ); + // DEBUG_ENDLINE(); + break; + } + } while ( ( delta > maxError ) || ( delta < -maxError ) ); + +// for( int loopCntr = 0; loopCntr < 8; loopCntr++ ) { +// delta = ( _qmul( res, res, 30, 30, 30 ) - pSquare_q30 ); +// res = res - _qdiv( delta, ( res << 1 ), 30, 30, 30 ); +// } + + // Do one more iteration for the heck of it + delta = ( _qmul( res, res, 30, 30, 30 ) - pSquare_q30 ); + res = res - _qdiv( delta, ( res << 1 ), 30, 30, 30 ); + return res; +} + + +// Range of input: +/- 2 +int32_t qsqrt_q29( int32_t pSquare_q29 ) +{ + // fixedp = long + int32_t res; + int32_t delta; + int32_t maxError; + + // Check for negative number and return 0 (error checking: return -1???) + if( pSquare_q29 <= 0 ) { + return 0; + } + + /* start at one (2^30) */ + res = pSquare_q29 >> 1; + + /* determine allowable error */ + // this limits the error to be less than 1e-6 + //maxError = 135; + maxError = 1080; + + uint8_t loopCntr = 0; + do { + loopCntr++; + + delta = ( _qmul( res, res, 29, 29, 29 ) - pSquare_q29 ); + res = res - _qdiv( delta, ( res << 1 ), 29, 29, 29 ); + + if( loopCntr == 250 ) { +// DEBUG_ENDLINE(); +// DEBUG_INT( " QSQRT_Q29 stopped at iteration ", loopCntr ); +// DEBUG_LONGINT( " (function argument: ", pSquare_q29 ); +// DEBUG_STRING( ")" ); +// DEBUG_ENDLINE(); + break; + } + } while ( ( delta > maxError ) || ( delta < -maxError ) ); + +// for( int loopCntr = 0; loopCntr < 8; loopCntr++ ) { +// delta = ( _qmul( res, res, 30, 30, 30 ) - pSquare_q30 ); +// res = res - _qdiv( delta, ( res << 1 ), 30, 30, 30 ); +// } + + // Do one more iteration for the heck of it + delta = ( _qmul( res, res, 29, 29, 29 ) - pSquare_q29 ); + res = res - _qdiv( delta, ( res << 1 ), 29, 29, 29 ); + return res; +} + + +// +int32_t qsqrt_q23( int32_t pSquare_q23 ) +{ + // fixedp = long + int32_t res; + int32_t delta; + int32_t maxError; + + if( pSquare_q23 <= 0 ) + return 0; + + /* start at half */ + res = ( pSquare_q23 >> 1 ); + + /* determine allowable error */ + // this limits the error to be less than 1e-6 + maxError = 10; + + do { + delta = ( _qmul( res, res, 23, 23, 23 ) - pSquare_q23 ); + res = res - _qdiv( delta, ( res << 1 ), 23, 23, 23 ); + } while ( delta > maxError || delta < -maxError ); + + // Do one more iteration for the heck of it + delta = ( _qmul( res, res, 23, 23, 23 ) - pSquare_q23 ); + res = res - _qdiv( delta, ( res << 1 ), 23, 23, 23 ); + return res; +} + + + +/** + * log (base e) + */ +fixedp qlog( fixedp p_Base ) +{ + fixedp w = 0; + fixedp y = 0; + fixedp z = 0; + fixedp num = int2q(1); + fixedp dec = 0; + + if ( p_Base == int2q(1) ) + return 0; + + if ( p_Base == 0 ) + return 0xffffffff; + + for ( dec=0 ; qabs( p_Base ) >= int2q(2) ; dec += int2q(1) ) + p_Base = qdiv(p_Base, QLN_E); + + p_Base -= int2q(1); + z = p_Base; + y = p_Base; + w = int2q(1); + + while ( y != y + w ) + { + z = 0 - qmul( z , p_Base ); + num += int2q(1); + w = qdiv( z , num ); + y += w; + } + + return y + dec; +} + +/** + * log base 10 + */ +fixedp qlog10( fixedp p_Base ) +{ + // ln(p_Base)/ln(10) + // more accurately, ln(p_Base) * (1/ln(10)) + return qmul(qlog(p_Base), Q1OLN_10); +} + +/** + * exp (e to the x) + */ +fixedp qexp(fixedp p_Base) +{ + fixedp w; + fixedp y; + fixedp num; + + for ( w=int2q(1), y=int2q(1), num=int2q(1) ; y != y+w ; num += int2q(1) ) + { + w = qmul(w, qdiv(p_Base, num)); + y += w; + } + + return y; +} + +/** + * pow + */ +fixedp qpow( fixedp p_Base, + fixedp p_Power ) +{ + if ( p_Base < 0 && qmod(p_Power, int2q(2)) != 0 ) + return - qexp( qmul(p_Power, qlog( -p_Base )) ); + else + return qexp( qmul(p_Power, qlog(qabs( p_Base ))) ); +} + +/** + * sinx, internal sine function + */ +static fixedp sinx(fixedp x) +{ + fixedp xpwr; + long xftl; + fixedp xresult; + short positive; + int i; + + xresult = 0; + xpwr = x; + xftl = 1; + positive = -1; + + // Note: 12! largest for long + for (i = 1; i < 7; i+=2) + { + if ( positive ) + xresult += qdiv(xpwr, long2q(xftl)); + else + xresult -= qdiv(xpwr, long2q(xftl)); + + xpwr = qmul(x, xpwr); + xpwr = qmul(x, xpwr); + xftl *= i+1; + xftl *= i+2; + positive = !positive; + } + + return xresult; +} + +/** + * sine + */ +fixedp qsin(fixedp theta) +{ + fixedp xresult; + short bBottom = 0; + //static fixed xPI = XPI; + //static fixed x2PI = X2PI; + //static fixed xPIO2 = XPIO2; + + fixedp x = qmod(theta, Q2PI); + if ( x < 0 ) + x += Q2PI; + + if ( x > QPI ) + { + bBottom = -1; + x -= QPI; + } + + if ( x <= QPIO2 ) + xresult = sinx(x); + else + xresult = sinx(QPIO2-(x-QPIO2)); + + if ( bBottom ) + return -xresult; + + return xresult; +} + +/** + * cosine + */ +fixedp qcos(fixedp theta) +{ + return qsin(theta + QPIO2); +} + +/** + * tangent + */ +fixedp qtan(fixedp theta) +{ + return qdiv(qsin(theta), qcos(theta)); +} + +/** + * q2a - converts a fixed point number to a string + */ +char *q2a(char *buf, + fixedp n) +{ + long ipart = qipart(n); + long fpart = qfpart(n); + long intdigits = 0; + + int i = 0; + int j = 0; + int d = 0; + + int offset = 0; + + long v; + long t; + long st = 0; + + if (n != 0) + { + intdigits = qipart(qceil(qlog10(qabs(n)))); + } + + if (intdigits < 1) intdigits = 1; + + offset = intdigits - 1; + + if (n < 0) + { + buf[0] = '-'; + offset++; + n = -n; + ipart = -ipart; + fpart = -fpart; + } + + // integer portion + for (i = 0; i < intdigits; i++) + { + j = offset - i; + d = ipart % 10; + ipart = ipart / 10; + buf[j] = '0' + d; + } + + // decimal point + buf[offset + 1] = '.'; + + // fractional portion + v = 5; + t = FIXED_RESOLUTION >> 1; + + for (i = 0; i < FIXED_DECIMALDIGITS - 1; i++) + { + v = (v << 1) + (v << 3); + } + + for (i = 0; i < FIXED_FRACBITS; i++) + { + if (fpart & t) + { + st += v; + } + v = v >> 1; + t = t >> 1; + } + + offset += FIXED_DECIMALDIGITS + 1; + + for (i = 0; i < FIXED_DECIMALDIGITS; i++) + { + j = offset - i; + d = st % 10; + st = st / 10; + buf[j] = '0' + d; + } + + buf[offset + 1] = '\0'; // ending zero + + return buf; +} + + +// +int32_t nabs( int32_t signedInteger ) +{ + if( signedInteger >= 0 ) { + return signedInteger; + } else { + return -signedInteger; + } +} + + +// +int32_t nabs_32( int32_t signedInteger ) +{ + if( signedInteger >= 0 ) { + return signedInteger; + } else { + return -signedInteger; + } +} + +// +int64_t nabs_64( int64_t signedInteger ) +{ + if( signedInteger >= 0 ) { + return signedInteger; + } else { + return -signedInteger; + } +} + +// +int32_t sign( int32_t signedInteger ) +{ + if( signedInteger >= 0 ) { + return 1; + } else { + return -1; + } +} + + +// asin function based on sqrt and atan2 +// asin( x ) returns an angle (between -pi and +pi) for an input between -1 and +1 +// however, x is represented as a q number (i.e. q27). Tested. Result is within ~0.2 degrees +// (the precision of the atan2 function). FIXME: change atan2 to increase precision. +int32_t asin_q27( int32_t x_q27 ) +{ + int32_t tmp1_q27; + + tmp1_q27 = _qmul( x_q27, x_q27, 27, 27, 27 ); + tmp1_q27 = Q27 - tmp1_q27; // 1 - x^2 (x < 1) + tmp1_q27 = qsqrt_q27( tmp1_q27 ); + + return atan2_q27( x_q27, tmp1_q27 ); +} + + +// asin function based on sqrt and atan2 +// asin( x ) returns an angle (between -pi and +pi) for an input between -1 and +1 +// however, x is represented as a q number (i.e. q27). Tested. Result is within ~0.2 degrees +// (the precision of the atan2 function). FIXME: change atan2 to increase precision. +int32_t asin_q29( int32_t x_q30 ) +{ + int32_t tmp1_q30; + + tmp1_q30 = _qmul( x_q30, x_q30, 30, 30, 30 ); + tmp1_q30 = ONE_Q30 - tmp1_q30; // 1 - x^2 (x <= 1) +//DEBUG_STRING( " A" ); +// tmp1_q30 = qsqrt_q30( tmp1_q30 ); + tmp1_q30 = qsqrt_q27( tmp1_q30 >> 3) << 3; + + return atan2_q29( x_q30, tmp1_q30, 30 ); +} + +//asin(x) = atan2 (x, sqrt ((1.0 + x) * (1.0 - x))) +//acos(x) = atan2 (sqrt ((1.0 + x) * (1.0 - x)), x) + +//[ 1072959488, 345219611, 157684912, 42200635 ] + +typedef struct atan2_coeff { + int32_t a0_q27; + int32_t a1_q27; + int32_t a2_q27; + int32_t a3_q27; + + int32_t a0_q29; + int32_t a1_q29; + int32_t a2_q29; + int32_t a3_q29; + + int32_t a0_q30; + int32_t a1_q30; + int32_t a2_q30; + int32_t a3_q30; +} atan2_coeff; + + +// +// 32-bit fixed point (Q27) four-quadrant arctangent. Given a Cartesian vector (x, y), return the +// angle subtended by the vector and the positive x-axis. Both function input and output are in +// q27 format. Accuracy is greater than 5 mdeg (18 arcsec). +// +// The value returned falls in the follwing range: [ -pi, pi ) and is converted to a q27 data-type +// (floating-point value is multiplied by 2^27). +// +// * Because the magnitude of the input vector does not change the angle it +// * represents, the inputs can be in any signed 16-bit fixed-point format. +// * +// * @param y y-coordinate is int32_t, q27 format +// * @param x x-coordinate is int32_t, q27 format +// * @return angle in int32_t, q27 format between -( pi*2^27 ) and +( pi*2^27 ) +// +// atan( y, x ) returns an angle in the range [ -pi, +pi ). A Q29 representation will encompass +// the range of output values. The input must support a wider range of values +int32_t atan2_q27( int32_t y_q27, int32_t x_q27 ) +{ + static atan2_coeff coeff; + coeff.a0_q27 = 134119936; + coeff.a1_q27 = 43152451; + coeff.a2_q27 = 19710614; + coeff.a3_q27 = 5275079; + + coeff.a0_q29 = 536479744; + coeff.a1_q29 = 172609805; + coeff.a2_q29 = 78842456; + coeff.a3_q29 = 21100318; + + int32_t nabs_y = nabs( y_q27 ); + int32_t nabs_x = nabs( x_q27 ); + + int32_t angle; + + int32_t tmp1_q27, tmp2_q27, tmp3_q27; + int32_t xOverY_q27, yOverX_q27; + int32_t xOverYSquared_q27, yOverXSquared_q27; + + // If the absolute values of x and y are the same, then the angle is 45 degrees off the x-axis. + // Only need to determine the quadrant to find the angle. Additionally, check if both are + // zero. FIXME: Return zero but should also set a bit indicating this. + if( nabs_y == nabs_x ) { + if( y_q27 > 0 ) { + if( x_q27 > 0 ) { + // angle = round( ( 45* pi/180 ) * 2^27 ); + return 105414357; + } else { + // angle = round( ( 3*45* pi/180 ) * 2^27 ) + return 316243071; + } + } else if( y_q27 < 0 ) { + if( x_q27 > 0 ) { + // angle = round( ( -45* pi/180 ) * 2^27 ); + return -105414357; + } else { + // angle = round( ( -3*45* pi/180 ) * 2^27 ); + return -316243071; + } + } else { + // Both x and y are zero. Indeterminate angle (return 0 but set an error flag) + return 0; + } + } + + // The point is on the y-axis. Angle = +/-90 deg + if( nabs_x == 0 ) { + if( y_q27 > 0 ) { + //angle = round( ( 90 * pi/180 ) * 2^27 ); + return 210828714; + } else { + // angle = round( ( -90 * pi/180 ) * 2^27 ); + return -210828714; + } + } + + // The point is on the x-axis; angle = 0, 180 deg (but return -180 degrees to be consistent with + // range definition: [ -pi, +pi ) + if( nabs_y == 0 ) { + if( x_q27 > 0 ) { + // angle = round( ( 0 * pi/180 ) * 2^27 ); + return 0; + } else { + // angle = round( ( -180 * pi/180 ) * 2^27 ); + return -421657428; + } + } + + // Point is not on the x or y-axes nor is it on a +/-45 degree line. Calculate the angle + if( nabs_x < nabs_y ) { + // The point is above a line that is 45 degree above the x-axis (octants II, III, Vi, and VII) + xOverY_q27 = _qdiv( x_q27, y_q27, 27, 27, 27 ); // ( x >> 27 ) / y; + xOverYSquared_q27 = _qmul( xOverY_q27, xOverY_q27, 27, 27, 27 ); + + tmp1_q27 = coeff.a2_q27 - _qmul( coeff.a3_q27, xOverYSquared_q27, 27, 27, 27 ); + tmp2_q27 = coeff.a1_q27 - _qmul( tmp1_q27, xOverYSquared_q27, 27, 27, 27 ); + tmp3_q27 = coeff.a0_q27 - _qmul( tmp2_q27, xOverYSquared_q27, 27, 27, 27 ); + angle = _qmul( xOverY_q27, tmp3_q27, 27, 27, 27 ); + + // Adjust the angle depending upon the octant in which the point falls + if( y_q27 > 0 ) { + // angle = 90 - result + angle = 210828714 - angle; + } else { + angle = -210828714 - angle; + } + } else { + // The point is below a line that is 45 degree above the x-axis (octants I, IV, V, and VIII) + yOverX_q27 = _qdiv( y_q27, x_q27, 27, 27, 27 ); + yOverXSquared_q27 = _qmul( yOverX_q27, yOverX_q27, 27, 27, 27 ); + + tmp1_q27 = coeff.a2_q27 - _qmul( coeff.a3_q27, yOverXSquared_q27, 27, 27, 27 ); + tmp2_q27 = coeff.a1_q27 - _qmul( tmp1_q27, yOverXSquared_q27, 27, 27, 27 ); + tmp3_q27 = coeff.a0_q27 - _qmul( tmp2_q27, yOverXSquared_q27, 27, 27, 27 ); + angle = _qmul( yOverX_q27, tmp3_q27, 27, 27, 27 ); + + // Near theta = 180 degrees, the program needs to check the quadrant when computing the + // angle as the output must fall between -pi (-180 deg) and +pi (180 deg) + if( x_q27 < 0 ) { + if( y_q27 > 0 ) { + // In octant IV + angle = 421657428 + angle; + } else { + // In octant V + angle = -421657429 + angle; + } + } + } + + // Must be less than 2,147,483,648 = 2^15-1 + return (int32_t)angle; +} + +// atan( y, x ) returns an angle in the range [ -pi, +pi ). A Q29 representation will encompass +// the range of output values. The input must support a wider range of values +int32_t atan2_q29Out_q27In( int32_t y_q27, int32_t x_q27 ) +{ + static atan2_coeff coeff; + + // fixed-point floating-point + // value value + coeff.a0_q29 = 536479744; // a0 = 0.999271392822266 + coeff.a1_q29 = 172609805; // a1 = 0.321510815992951 + coeff.a2_q29 = 78842456; // a2 = 0.146855518221855 + coeff.a3_q29 = 21100318; // a3 = 0.039302404969931 + + coeff.a0_q30 = 1072959488; + coeff.a1_q30 = 345219611; + coeff.a2_q30 = 157684912; + coeff.a3_q30 = 42200635; + + int32_t nabs_y = nabs( y_q27 ); + int32_t nabs_x = nabs( x_q27 ); + + int32_t angle_q29; + + int32_t tmp1_q30, tmp2_q30, tmp3_q30; + int32_t xOverY_q30, yOverX_q30; + int32_t xOverYSquared_q30, yOverXSquared_q30; + + // If the absolute values of x and y are the same, then the angle is 45 degrees off the x-axis. + // Only need to determine the quadrant to find the angle. Additionally, check if both are + // zero. FIXME: Return zero but should also set a bit indicating this. + if( nabs_y == nabs_x ) { + if( y_q27 > 0 ) { + if( x_q27 > 0 ) { + return FORTY_FIVE_DEGREES_Q29; + } else { + return ONE_HUNDRED_THIRTY_FIVE_DEGREES_Q29; + } + } else if( y_q27 < 0 ) { + if( x_q27 > 0 ) { + return -FORTY_FIVE_DEGREES_Q29; + } else { + return -ONE_HUNDRED_THIRTY_FIVE_DEGREES_Q29; + } + } else { + // Both x and y are zero. Indeterminate angle (return 0 but set an error flag) + return 0; + } + } + + // The point is on the y-axis. Angle = +/-90 deg + if( nabs_x == 0 ) { + if( y_q27 > 0 ) { + return NINETY_DEGREES_Q29; + } else { + return -NINETY_DEGREES_Q29; + } + } + + // The point is on the x-axis; angle = 0, 180 deg (but return -180 degrees to be consistent with + // range definition: [ -pi, +pi ) + if( nabs_y == 0 ) { + if( x_q27 > 0 ) { + return 0; + } else { + return -ONE_HUNDRED_EIGHTY_DEGS_Q29; + } + } + + // Point is not on the x or y-axes nor is it on a +/-45 degree line. Calculate the angle. + if( nabs_x < nabs_y ) { + // The point is above a line that is 45 degree above the x-axis (octants II, III, VI, and VII) + // The slope is always less than one as, in this case, it is computed from the y-axis + xOverY_q30 = _qdiv( x_q27, y_q27, 27, 27, 30 ); + xOverYSquared_q30 = _qmul( xOverY_q30, xOverY_q30, 30, 30, 30 ); + + // Can use Q30 since the angle will be less than 45 degrees (pi/4) but convert it at the end + // so it can be combined with the offset in q_29 format. + tmp1_q30 = coeff.a2_q30 - _qmul( coeff.a3_q30, xOverYSquared_q30, 30, 30, 30 ); + tmp2_q30 = coeff.a1_q30 - _qmul( tmp1_q30, xOverYSquared_q30, 30, 30, 30 ); + tmp3_q30 = coeff.a0_q30 - _qmul( tmp2_q30, xOverYSquared_q30, 30, 30, 30 ); + angle_q29 = _qmul( xOverY_q30, tmp3_q30, 30, 30, 29 ); + + // Adjust the angle depending upon the octant in which the point falls + if( y_q27 > 0 ) { + // angle = 90 - result + angle_q29 = NINETY_DEGREES_Q29 - angle_q29; + } else { + angle_q29 = -NINETY_DEGREES_Q29 - angle_q29; + } + } else { + // The point is below a line that is 45 degree above the x-axis (octants I, IV, V, and VIII) + yOverX_q30 = _qdiv( y_q27, x_q27, 27, 27, 30 ); + yOverXSquared_q30 = _qmul( yOverX_q30, yOverX_q30, 30, 30, 30 ); + + // Can use Q30 since the angle will be less than 45 degrees (pi/4) but convert it at the end + // so it can be combined with the offset in q_29 format. + tmp1_q30 = coeff.a2_q30 - _qmul( coeff.a3_q30, yOverXSquared_q30, 30, 30, 30 ); + tmp2_q30 = coeff.a1_q30 - _qmul( tmp1_q30, yOverXSquared_q30, 30, 30, 30 ); + tmp3_q30 = coeff.a0_q30 - _qmul( tmp2_q30, yOverXSquared_q30, 30, 30, 30 ); + angle_q29 = _qmul( yOverX_q30, tmp3_q30, 30, 30, 29 ); + + // Near theta = 180 degrees, the program needs to check the quadrant when computing the + // angle as the output must fall between -pi (-180 deg) and +pi (180 deg) + if( x_q27 < 0 ) { + if( y_q27 > 0 ) { + // In octant IV + angle_q29 = ONE_HUNDRED_EIGHTY_DEGS_Q29 + angle_q29; + } else { + // In octant V + angle_q29 = -ONE_HUNDRED_EIGHTY_DEGS_Q29 + angle_q29; + } + } + } + + // Must be less than 2,147,483,648 = 2^15-1 + return (int32_t)angle_q29; +} + +// atan( y, x ) returns an angle in the range [ -pi, +pi ). A Q29 representation will encompass +// the range of output values. The input must support a wider range of values +int32_t atan2_q29Out_q30In( int32_t y_q30, + int32_t x_q30 ) +{ + static atan2_coeff coeff; + + // fixed-point floating-point + // value value + coeff.a0_q29 = 536479744; // a0 = 0.999271392822266 + coeff.a1_q29 = 172609805; // a1 = 0.321510815992951 + coeff.a2_q29 = 78842456; // a2 = 0.146855518221855 + coeff.a3_q29 = 21100318; // a3 = 0.039302404969931 + + coeff.a0_q30 = 1072959488; + coeff.a1_q30 = 345219611; + coeff.a2_q30 = 157684912; + coeff.a3_q30 = 42200635; + + int32_t nabs_y = nabs( y_q30 ); + int32_t nabs_x = nabs( x_q30 ); + + int32_t angle_q29; + + int32_t tmp1_q30, tmp2_q30, tmp3_q30; + int32_t xOverY_q30, yOverX_q30; + int32_t xOverYSquared_q30, yOverXSquared_q30; + + // If the absolute values of x and y are the same, then the angle is 45 degrees off the x-axis. + // Only need to determine the quadrant to find the angle. Additionally, check if both are + // zero. FIXME: Return zero but should also set a bit indicating this. + if( nabs_y == nabs_x ) { + if( y_q30 > 0 ) { + if( x_q30 > 0 ) { + return FORTY_FIVE_DEGREES_Q29; + } else { + return ONE_HUNDRED_THIRTY_FIVE_DEGREES_Q29; + } + } else if( y_q30 < 0 ) { + if( x_q30 > 0 ) { + return -FORTY_FIVE_DEGREES_Q29; + } else { + return -ONE_HUNDRED_THIRTY_FIVE_DEGREES_Q29; + } + } else { + // Both x and y are zero. Indeterminate angle (return 0 but set an error flag) + return 0; + } + } + + // The point is on the y-axis. Angle = +/-90 deg + if( nabs_x == 0 ) { + if( y_q30 > 0 ) { + return NINETY_DEGREES_Q29; + } else { + return -NINETY_DEGREES_Q29; + } + } + + // The point is on the x-axis; angle = 0, 180 deg (but return -180 degrees to be consistent with + // range definition: [ -pi, +pi ) + if( nabs_y == 0 ) { + if( x_q30 > 0 ) { + return 0; + } else { + return -ONE_HUNDRED_EIGHTY_DEGS_Q29; + } + } + + // Point is not on the x or y-axes nor is it on a +/-45 degree line. Calculate the angle. + if( nabs_x < nabs_y ) { + // The point is above a line that is 45 degree above the x-axis (octants II, III, VI, and VII) + // The slope is always less than one as, in this case, it is computed from the y-axis + xOverY_q30 = _qdiv( x_q30, y_q30, 30, 30, 30 ); + xOverYSquared_q30 = _qmul( xOverY_q30, xOverY_q30, 30, 30, 30 ); + + // Can use Q30 since the angle will be less than 45 degrees (pi/4) but convert it at the end + // so it can be combined with the offset in q_29 format. + tmp1_q30 = coeff.a2_q30 - _qmul( coeff.a3_q30, xOverYSquared_q30, 30, 30, 30 ); + tmp2_q30 = coeff.a1_q30 - _qmul( tmp1_q30, xOverYSquared_q30, 30, 30, 30 ); + tmp3_q30 = coeff.a0_q30 - _qmul( tmp2_q30, xOverYSquared_q30, 30, 30, 30 ); + angle_q29 = _qmul( xOverY_q30, tmp3_q30, 30, 30, 29 ); + + // Adjust the angle depending upon the octant in which the point falls + if( y_q30 > 0 ) { + // angle = 90 - result + angle_q29 = NINETY_DEGREES_Q29 - angle_q29; + } else { + angle_q29 = -NINETY_DEGREES_Q29 - angle_q29; + } + } else { + // The point is below a line that is 45 degree above the x-axis (octants I, IV, V, and VIII) + yOverX_q30 = _qdiv( y_q30, x_q30, 30, 30, 30 ); + yOverXSquared_q30 = _qmul( yOverX_q30, yOverX_q30, 30, 30, 30 ); + + // Can use Q30 since the angle will be less than 45 degrees (pi/4) but convert it at the end + // so it can be combined with the offset in q_29 format. + tmp1_q30 = coeff.a2_q30 - _qmul( coeff.a3_q30, yOverXSquared_q30, 30, 30, 30 ); + tmp2_q30 = coeff.a1_q30 - _qmul( tmp1_q30, yOverXSquared_q30, 30, 30, 30 ); + tmp3_q30 = coeff.a0_q30 - _qmul( tmp2_q30, yOverXSquared_q30, 30, 30, 30 ); + angle_q29 = _qmul( yOverX_q30, tmp3_q30, 30, 30, 29 ); + + // Near theta = 180 degrees, the program needs to check the quadrant when computing the + // angle as the output must fall between -pi (-180 deg) and +pi (180 deg) + if( x_q30 < 0 ) { + if( y_q30 > 0 ) { + // In octant IV + angle_q29 = ONE_HUNDRED_EIGHTY_DEGS_Q29 + angle_q29; + } else { + // In octant V + angle_q29 = -ONE_HUNDRED_EIGHTY_DEGS_Q29 + angle_q29; + } + } + } + return (int32_t)angle_q29; // Must be less than 2,147,483,648 = 2^15-1 +} + +// atan( y, x ) returns an angle in the range [ -pi, +pi ). A Q29 representation will encompass +// the range of output values. The input must support a wider range of values +int32_t atan2_q29( int32_t y_qX, + int32_t x_qX, + uint8_t qVal ) +{ + static atan2_coeff coeff; + + // Note: while the result of the calculation below is an angle represented in Q29 format, the + // intermediate calculation generates an angle between -45 and +45 degrees (pi/4 = 0.7854) + // radians. Therefore, all calulations prior to the final one can be performed using Q30 + // math. + // fixed-point floating-point + // value value + coeff.a0_q30 = 1072959488; + coeff.a1_q30 = 345219611; + coeff.a2_q30 = 157684912; + coeff.a3_q30 = 42200635; + + int32_t nabs_y = nabs( y_qX ); + int32_t nabs_x = nabs( x_qX ); + + int32_t angle_q29; + + int32_t tmp1_q30, tmp2_q30, tmp3_q30; + int32_t xOverY_q30, yOverX_q30; + int32_t xOverYSquared_q30, yOverXSquared_q30; + + // If the absolute values of x and y are the same, then the angle is 45 degrees off the x-axis. + // Only need to determine the quadrant to find the angle. Additionally, check if both are + // zero. FIXME: Return zero but should also set a bit indicating this. + if( nabs_y == nabs_x ) { + if( y_qX > 0 ) { + if( x_qX > 0 ) { + return FORTY_FIVE_DEGREES_Q29; + } else { + return ONE_HUNDRED_THIRTY_FIVE_DEGREES_Q29; + } + } else if( y_qX < 0 ) { + if( x_qX > 0 ) { + return -FORTY_FIVE_DEGREES_Q29; + } else { + return -ONE_HUNDRED_THIRTY_FIVE_DEGREES_Q29; + } + } else { + // Both x and y are zero. Indeterminate angle (return 0 but set an error flag) + return 0; + } + } + + // The point is on the y-axis. Angle = +/-90 deg + if( nabs_x == 0 ) { + if( y_qX > 0 ) { + return NINETY_DEGREES_Q29; + } else { + return -NINETY_DEGREES_Q29; + } + } + + // The point is on the x-axis; angle = 0, 180 deg (but return -180 degrees to be consistent with + // range definition: [ -pi, +pi ) + if( nabs_y == 0 ) { + if( x_qX > 0 ) { + return 0; + } else { + return -ONE_HUNDRED_EIGHTY_DEGS_Q29; + } + } + + // Point is not on the x or y-axes nor is it on a +/-45 degree line. Calculate the angle. + if( nabs_x < nabs_y ) { + // The point is above a line that is 45 degree above the x-axis (octants II, III, VI, and VII) + // The slope is always less than one as, in this case, it is computed from the y-axis + xOverY_q30 = _qdiv( x_qX, y_qX, qVal, qVal, 30 ); // slope < 1 (Q30) + xOverYSquared_q30 = _qmul( xOverY_q30, xOverY_q30, 30, 30, 30 ); // slope^2 < 1 (Q30) + + // Can use Q30 since the angle will be less than 45 degrees (pi/4) but convert it at the end + // so it can be combined with the offset in q_29 format. + tmp1_q30 = coeff.a2_q30 - _qmul( coeff.a3_q30, xOverYSquared_q30, 30, 30, 30 ); + tmp2_q30 = coeff.a1_q30 - _qmul( tmp1_q30, xOverYSquared_q30, 30, 30, 30 ); + tmp3_q30 = coeff.a0_q30 - _qmul( tmp2_q30, xOverYSquared_q30, 30, 30, 30 ); + angle_q29 = _qmul( xOverY_q30, tmp3_q30, 30, 30, 29 ); + + // Adjust the angle depending upon the octant in which the point falls + if( y_qX > 0 ) { + // angle = 90 - result + angle_q29 = NINETY_DEGREES_Q29 - angle_q29; + } else { + angle_q29 = -NINETY_DEGREES_Q29 - angle_q29; + } + } else { + // The point is below a line that is 45 degree above the x-axis (octants I, IV, V, and VIII) + yOverX_q30 = _qdiv( y_qX, x_qX, qVal, qVal, 30 ); + yOverXSquared_q30 = _qmul( yOverX_q30, yOverX_q30, 30, 30, 30 ); + + // Can use Q30 since the angle will be less than 45 degrees (pi/4) but convert it at the end + // so it can be combined with the offset in q_29 format. + tmp1_q30 = coeff.a2_q30 - _qmul( coeff.a3_q30, yOverXSquared_q30, 30, 30, 30 ); + tmp2_q30 = coeff.a1_q30 - _qmul( tmp1_q30, yOverXSquared_q30, 30, 30, 30 ); + tmp3_q30 = coeff.a0_q30 - _qmul( tmp2_q30, yOverXSquared_q30, 30, 30, 30 ); + angle_q29 = _qmul( yOverX_q30, tmp3_q30, 30, 30, 29 ); + + // Near theta = 180 degrees check the quadrant + // the output must fall between -pi (-180 deg) and +pi (180 deg) + if( x_qX < 0 ) { + if( y_qX > 0 ) { + // In octant IV + angle_q29 = ONE_HUNDRED_EIGHTY_DEGS_Q29 + angle_q29; + } else { + // In octant V + angle_q29 = -ONE_HUNDRED_EIGHTY_DEGS_Q29 + angle_q29; + } + } + } + + // Must be less than 2,147,483,648 = 2^15-1 + return (int32_t)angle_q29; +} + + +// Create an atan function +typedef struct sin_coeff { + int32_t a0_q29; + int32_t a1_q29; + int32_t a2_q29; + int32_t a3_q29; + int32_t a4_q29; + int32_t a5_q29; +} sin_coeff; + + +#define PI_OVER_TWO_Q29 843314857 +#define PI_Q29 1686629713 + +//#define ONE_Q30 1073741824 + +// Input can range from -pi to +pi. Must not be larger than 4.0, so angles larger than 229 degrees +// can't be used. +// Input: angle in radians +// Checked and verified on May 15, 2014 +int32_t sin_q30( int32_t angleRad_q29 ) +{ + static sin_coeff coeff; + coeff.a0_q29 = 536870912; + coeff.a1_q29 = 89478485; + coeff.a2_q29 = 4473924; + coeff.a3_q29 = 106522; + coeff.a4_q29 = 1479; + coeff.a5_q29 = 13; + + // find the absolute value of the angle and, if greater than pi/2 (843314857), reduce the value + int32_t ang_q29 = nabs( angleRad_q29 ); + if( ang_q29 > PI_OVER_TWO_Q29 ) { + // constrain the value to be less than 90 degrees + ang_q29 = PI_Q29 - ang_q29; + } + + // check for zero or pi + if( angleRad_q29 == 0 || ang_q29 == PI_Q29 ) { + return 0; + } + + int32_t angleSign = sign( angleRad_q29 ); + + // Check for +/- 90 degrees = pi/2 radians = 1073741824 rad_q30 + if( ang_q29 == PI_OVER_TWO_Q29 ) { + return( angleSign*ONE_Q30 ); + } + + int32_t tmp1_q29, tmp2_q29, tmp3_q29, tmp4_q29, tmp5_q29, tmp6_q30; + int32_t angleSquared_q29 = _qmul( ang_q29, ang_q29, 29, 29, 29 ); + + // Maclaurin series for sine + // sin( x ) = x*( 1 - x^2*( 1/3! - x^2*( 1/5! - x^2*( 1/7! - x^2*( 1/9! - x^2*( 1/11! - � ) ) ) ) ) + tmp1_q29 = coeff.a4_q29 - _qmul( angleSquared_q29, coeff.a5_q29, 29, 29, 29 ); + tmp2_q29 = coeff.a3_q29 - _qmul( angleSquared_q29, tmp1_q29, 29, 29, 29 ); + tmp3_q29 = coeff.a2_q29 - _qmul( angleSquared_q29, tmp2_q29, 29, 29, 29 ); + tmp4_q29 = coeff.a1_q29 - _qmul( angleSquared_q29, tmp3_q29, 29, 29, 29 ); + tmp5_q29 = coeff.a0_q29 - _qmul( angleSquared_q29, tmp4_q29, 29, 29, 29 ); + tmp6_q30 = _qmul( ang_q29, tmp5_q29, 29, 29, 30 ); + + return( angleSign * tmp6_q30 ); +} + + +// Input can range from -pi to +pi. Input value cannot be larger than 4.0, so +// angles larger than 229 degrees won't produce correct results. +// Input: angle in radians +// Checked and verified on May 15, 2014 +int32_t cos_q30( int32_t angleRad_q29 ) +{ + // Use the identity to calc cosine from sin: + // cos( x ) = sin( x + 90 ), where x is in degrees. + if( angleRad_q29 >= PI_OVER_TWO_Q29 ) { + // Note: the angle (x+90) cannot be larger than 229 degrees and remain in q29 format (the + // value is represented in radians). Therefore, if the angle is greater than 90 + // degrees, subtract 270 (+90-360) from the angle to prevent the intermediate value + // from going beyond 229 degrees. + angleRad_q29 = angleRad_q29 - PI_OVER_TWO_Q29; // -90 + angleRad_q29 = angleRad_q29 - PI_OVER_TWO_Q29; // -180 + angleRad_q29 = angleRad_q29 - PI_OVER_TWO_Q29; // -270 + // Equivalent to (int32_t)( (int64_t)angleRad_q29 - 2529944570 ); + } else { + // x is less than 90 degrees: cos( x ) = sin( x + 90 ) + angleRad_q29 = angleRad_q29 + PI_OVER_TWO_Q29; + } + return sin_q30( angleRad_q29 ); +} + + +void VectorNormalize_q30( int32_t *vectorIn_q27, + int32_t *vectorOut_q30 ) +{ + int32_t vectorMag_q27; + + // sqrt( v1^2 + v2^2 + v3^2 ) + vectorMag_q27 = VectorMag_q27( vectorIn_q27 ); + + vectorOut_q30[0] = _qdiv( vectorIn_q27[0], vectorMag_q27, 27, 27, 30 ); + vectorOut_q30[1] = _qdiv( vectorIn_q27[1], vectorMag_q27, 27, 27, 30 ); + vectorOut_q30[2] = _qdiv( vectorIn_q27[2], vectorMag_q27, 27, 27, 30 ); +} + + +// Components must be less than 1239850262 (Q27) ~ 9.23 (dec) in order to use this function without +// overflow. Else, need to represent the output in another q-format. +int32_t VectorMag_q27( int32_t *vectorIn_q27 ) +{ + int32_t temp_q27[3]; + + // + temp_q27[0] = _qmul( vectorIn_q27[0], vectorIn_q27[0], 27, 27, 27 ); + temp_q27[1] = _qmul( vectorIn_q27[1], vectorIn_q27[1], 27, 27, 27 ); + temp_q27[2] = _qmul( vectorIn_q27[2], vectorIn_q27[2], 27, 27, 27 ); + + // sqrt( v1^2 + v2^2 + v3^2 ) + return( qsqrt_q27( temp_q27[0] + temp_q27[1] + temp_q27[2] ) ); +} + + +void VectorCrossProduct_q27( int32_t *vect1_q27, + int32_t *vect2_q27, + int32_t *vectOut_q27 ) +{ + // | i j k | + // | v1[0] v1[1] v1[2] | = i*( v1[1]*v2[2] - v1[2]*v2[1] ) - j*( v1[0]*v2[2] - v1[2]*v2[0] ) + k*( v1[0]*v2[1] - v1[1]*v2[0] ) + // | v2[0] v2[1] v2[2] | + + vectOut_q27[0] = _qmul( vect1_q27[1], vect2_q27[2], 27, 27, 27 ) - + _qmul( vect1_q27[2], vect2_q27[1], 27, 27, 27 ); // v1[1]*v2[2] - v1[2]*v2[1] + vectOut_q27[1] = _qmul( vect1_q27[2], vect2_q27[0], 27, 27, 27 ) - + _qmul( vect1_q27[0], vect2_q27[2], 27, 27, 27 ); // v1[2]*v2[0] - v1[0]*v2[2] + vectOut_q27[2] = _qmul( vect1_q27[0], vect2_q27[1], 27, 27, 27 ) - + _qmul( vect1_q27[1], vect2_q27[0], 27, 27, 27 ); // v1[0]*v2[1] - v1[1]*v2[0] +} + +int32_t VectorDotProduct_q27( int32_t *vect1_q27, int32_t *vect2_q27 ) +{ + int32_t result_q27; + + result_q27 = _qmul( vect1_q27[0], vect2_q27[0], 27, 27, 27 ) + + _qmul( vect1_q27[1], vect2_q27[1], 27, 27, 27 ) + + _qmul( vect1_q27[2], vect2_q27[2], 27, 27, 27 ); + + return result_q27; +} + + +// +// 32-bit fixed point (Q27) four-quadrant arctangent. Given a Cartesian vector (x, y), return the +// angle subtended by the vector and the positive x-axis. Both function input and output are in +// q27 format. +// +// The value returned falls in the follwing range: [ -pi, pi ) and is converted to a q27 data-type +// (floating-point value is multiplied by 2^27). +// +// * Because the magnitude of the input vector does not change the angle it +// * represents, the inputs can be in any signed 16-bit fixed-point format. +// * +// * @param y y-coordinate is int32_t, q27 format +// * @param x x-coordinate is int32_t, q27 format +// * @return angle in int32_t, q27 format between -( pi*2^27 ) and +( pi*2^27 ) +// +int32_t atan2Old_q27( int32_t y, + int32_t x ) +{ + int32_t nabs_y = nabs( y ); + int32_t nabs_x = nabs( x ); + + int32_t angle; + int32_t innerTerm_q27; + + // If the absolute values of x and y are the same, then the angle is 45 degrees off the x-axis. + // Only need to determine the quadrant to find the angle. + if( nabs_y == nabs_x ) { + if( y > 0 ) { + if( x > 0 ) { + return 105414357; // angle = round( ( 45* pi/180 ) * 2^27 ); + } else { + return 316243071; // angle = round( ( 3*45* pi/180 ) * 2^27 ) + } + } else if( y < 0 ) { + if( x > 0 ) + { + return -105414357; // angle = round( ( -45* pi/180 ) * 2^27 ); + } else { + return -316243071; // angle = round( ( -3*45* pi/180 ) * 2^27 ); + } + } else { + // Both x and y are zero. Indeterminate angle (return 0 but set error flag) + return 0; + } + } + + // The point is on the y-axis. Angle = +/-90 deg + if( nabs_x == 0 ) { + if( y > 0 ) { + return 210828714; // angle = round( ( 90 * pi/180 ) * 2^27 ); + } else { + return -210828714; // angle = round( ( -90 * pi/180 ) * 2^27 ); + } + } + + // The point is on the x-axis; angle = 0, 180 deg + if( nabs_y == 0 ) { + if( x > 0 ) { + return 0; // angle = round( ( 0 * pi/180 ) * 2^27 ); + } else { + return -421657428; // angle = round( ( -180 * pi/180 ) * 2^27 ); + } + } + + // Point is not on the x or y-axis nor is it on a +/-45 degree line. Calculate the angle + if( nabs_x < nabs_y ) { + // The point is above a line that is 45 degree above the x-axis (octants II and III) + int32_t xOverY_q27 = _qdiv( x, y, 27, 27, 27 ); // ( x >> 27 ) / y; + int32_t absXOverY_q27 = nabs( xOverY_q27 ); + + // ( pi/4 + 0.273 ) = round( ( pi/4 + 0.273 ) * 2^27 ) = 142055797 + // 0.273 = round( 0.273 * 2^27 ) = 36641440 + innerTerm_q27 = 142055797 - _qmul( 36641440, absXOverY_q27, 27, 27, 27 ); + if( y > 0 ) { + // angle = 90 - result + angle = 210828714 - _qmul( xOverY_q27, innerTerm_q27, 27, 27, 27 ); + } else { + angle = -210828714 - _qmul( xOverY_q27, innerTerm_q27, 27, 27, 27 ); + } + } else { + // The point is below a line that is 45 degree above the x-axis (octants I and IV) + int32_t yOverX_q27 = _qdiv( y, x, 27, 27, 27 ); //fix( y * 2^27 / x ); + int32_t absYOverX_q27 = nabs( yOverX_q27 ); + + // ( pi/4 + 0.273 ) = round( ( pi/4 + 0.273 ) * 2^27 ) = 142055797 + // 0.273 = round( 0.273 * 2^27 ) = 36641440 + innerTerm_q27 = 142055797 - _qmul( 36641440, absYOverX_q27, 27, 27, 27 ); + if( x > 0 ) { + // In octant I or VIII + angle = _qmul( yOverX_q27, innerTerm_q27, 27, 27, 27 ); + } else { + // Near theta = 180 degrees, the program needs to check the quadrant when computing the + // angle as the output must fall between -pi (-180 deg) and +pi (180 deg) + if( y > 0 ) { + // In octant IV or V + angle = 421657428 + _qmul( yOverX_q27, innerTerm_q27, 27, 27, 27 ); + } else { + angle = -421657429 + _qmul( yOverX_q27, innerTerm_q27, 27, 27, 27 ); + } + } + } + return (int32_t)angle; // Must be less than 2,147,483,648 = 2^15-1 +} + + +// This is a replacement for 'lowPass2Pole' found in the 440 code. Note, the implementation in the +// 440 SW is not a second-order filter but is an implementation of a first-order filter. +void firstOrderLowPass_q27( int32_t *output_q27, + int32_t *input_q27, + int32_t *inputPast_q27, + uint8_t shiftCoeff ) +{ + *output_q27 = *output_q27 + + _qmul( ( *input_q27 - *output_q27 ), ONE_Q30 >> shiftCoeff, 27, 30, 27 ) + + _qmul( ( *inputPast_q27 - *output_q27 ), ONE_Q30 >> shiftCoeff, 27, 30, 27 ); + + *inputPast_q27 = *input_q27; +} + diff --git a/examples/OpenIMU300RI/INS/lib/Core/Odo/odoAPI.h b/examples/OpenIMU300RI/INS/lib/Core/Odo/odoAPI.h new file mode 100644 index 0000000..458560b --- /dev/null +++ b/examples/OpenIMU300RI/INS/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/OpenIMU300RI/INS/lib/Core/UARTComm/CommonMessages.c b/examples/OpenIMU300RI/INS/lib/Core/UARTComm/CommonMessages.c new file mode 100644 index 0000000..09cbf0b --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/UARTComm/CommonMessages.c @@ -0,0 +1,362 @@ +/** *************************************************************************** + * @file UARTMessages.c + * + * 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. + * + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#include +#include +#include + +#include "algorithmAPI.h" +#include "platformAPI.h" +#include "sensorsAPI.h" +#include "appVersion.h" + +#include "CommonMessages.h" +#include "algorithm.h" + +#include "EKF_Algorithm.h" + + +/****************************************************************************** + * @name FillPingPacketPayload - API call ro prepare user output packet + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL Fill_PingPacketPayload(uint8_t *payload, uint8_t *payloadLen) +{ + int len; + uint8_t *model = (uint8_t*)unitVersionString(); + uint8_t *rev = (uint8_t*)platformBuildInfo(); + unsigned int serialNum = unitSerialNumber(); + len = snprintf((char*)payload, 250, "%s %s SN:%u", model, rev, serialNum ); + *payloadLen = len; + return TRUE; +} + +/****************************************************************************** + * @name FillVersionPacketPayload - API call ro prepare user output packet + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL Fill_VersionPacketPayload(uint8_t *payload, uint8_t *payloadLen) +{ + int len = snprintf((char*)payload, 250, "%s", APP_VERSION_STRING ); + *payloadLen = len; + return TRUE; +} + +/****************************************************************************** + * @name FillTestPacketPayload - API call ro prepare user output packet + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL Fill_zTPacketPayload(uint8_t *payload, uint8_t *payloadLen) +{ + static uint32_t _testVal = 0; + test_payload_t* pld = (test_payload_t*)payload; + pld->counter = _testVal++; + *payloadLen = sizeof(test_payload_t); + return TRUE; +} + +/****************************************************************************** + * @name FillTestPacketPayloadt - API call ro prepare user output packet + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL Fill_z1PacketPayload(uint8_t *payload, uint8_t *payloadLen) +{ + uint64_t tstamp; + double accels[NUM_AXIS]; + double mags[NUM_AXIS]; + double rates[NUM_AXIS]; + + data1_payload_t *pld = (data1_payload_t *)payload; + +// tstamp = platformGetDacqTimeStamp(); // time stamp of last sensor sample in microseconds from system start + tstamp = platformGetCurrTimeStamp(); // current time stamp in microseconds from system start +// tstamp /= 1000; // convert to miliseconds +// timer = getSystemTime(); // OS timer value (tick defined in FreeRTOSConfig.h) + pld->timer = tstamp; + GetAccelData_mPerSecSq(accels); + GetRateData_degPerSec(rates); + GetMagData_G(mags); + + for (int i = 0; i < NUM_AXIS; i++){ + pld->accel_mpss[i] = (float)accels[i]; + pld->rate_dps[i] = (float)rates[i]; + pld->mag_G[i] = (float)mags[i]; + } + + *payloadLen = sizeof(data1_payload_t); + return TRUE; +} + + +/****************************************************************************** + * @name FillTestPacketPayloadt - API call ro prepare user output packet + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL Fill_a1PacketPayload(uint8_t *payload, uint8_t *payloadLen) +{ + angle1_payload_t* pld = (angle1_payload_t*)payload; + + // Variables used to hold the EKF values + real EulerAngles[NUM_AXIS]; + real CorrRates_B[NUM_AXIS]; + double accels[NUM_AXIS]; + // Diagnostic flags + uint8_t OperMode, LinAccelSwitch, TurnSwitch; + + EKF_GetAttitude_EA(EulerAngles); + EKF_GetCorrectedAngRates(CorrRates_B); + GetAccelData_mPerSecSq(accels); + EKF_GetOperationalMode(&OperMode); + EKF_GetOperationalSwitches(&LinAccelSwitch, &TurnSwitch); + + pld->itow = getAlgorithmITOW(); + pld->dblItow = 1.0e-3 * pld->itow; + pld->roll = (float)EulerAngles[ROLL]; + pld->pitch = (float)EulerAngles[PITCH]; + pld->ekfOpMode = OperMode; + pld->accelLinSwitch = LinAccelSwitch; + pld->turnSwitch = TurnSwitch; + + for(int i = 0; i < NUM_AXIS; i++){ + pld->corrRates[i] = (float)CorrRates_B[i]; + pld->accels[i] = (float)accels[i]; + } + + *payloadLen = sizeof(angle1_payload_t); + + return TRUE; +} + +/****************************************************************************** + * @name FillTestPacketPayloadt - API call ro prepare user output packet + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL Fill_a2PacketPayload(uint8_t *payload, uint8_t *payloadLen) +{ + angle2_payload_t* pld = (angle2_payload_t*)payload; + + // Variables used to hold the EKF values + real EulerAngles[NUM_AXIS]; + real CorrRates_B[NUM_AXIS]; + double accels[NUM_AXIS]; + + EKF_GetAttitude_EA(EulerAngles); + EKF_GetCorrectedAngRates(CorrRates_B); + GetAccelData_mPerSecSq(accels); + + pld->itow = getAlgorithmITOW(); + pld->dblItow = 1.0e-3 * pld->itow; + pld->roll = (float)EulerAngles[ROLL]; + pld->pitch = (float)EulerAngles[PITCH]; + pld->yaw = (float)EulerAngles[YAW]; + + for(int i = 0; i < NUM_AXIS; i++){ + pld->corrRates[i] = (float)CorrRates_B[i]; + pld->accels[i] = (float)accels[i]; + } + + *payloadLen = sizeof(angle2_payload_t); + + return TRUE; +} + +/****************************************************************************** + * @name Fills1PacketPayloadt - API call ro prepare user output packet + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL Fill_s1PacketPayload(uint8_t *payload, uint8_t *payloadLen) +{ + double accels[NUM_AXIS]; + double mags[NUM_AXIS]; + double rates[NUM_AXIS]; + double temp; + scaled1_payload_t *pld = (scaled1_payload_t *)payload; + *payloadLen = sizeof(scaled1_payload_t); + + GetAccelData_mPerSecSq(accels); + GetRateData_degPerSec(rates); + GetMagData_G(mags); + GetBoardTempData(&temp); + + pld->tstmp = platformGetIMUCounter(); + pld->dbTstmp = (double)pld->tstmp * 1.0e-3; // seconds + + for (int i = 0; i < NUM_AXIS; i++){ + pld->accel_g[i] = (float)accels[i]; + pld->rate_dps[i] = (float)rates[i]; + pld->mag_G[i] = (float)mags[i]; + } + + pld->temp_C = (float)temp; + + return TRUE; +} + +/****************************************************************************** + * @name Fille1PacketPayloadt - API call ro prepare user output packet - EKF data 1 + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL Fill_e1PacketPayload(uint8_t *payload, uint8_t *payloadLen) +{ + // Variables used to hold the EKF values + uint8_t opMode, linAccelSw, turnSw; + double data[NUM_AXIS]; + real EulerAngles[NUM_AXIS]; + + ekf1_payload_t *pld = (ekf1_payload_t *)payload; + + *payloadLen = sizeof(ekf1_payload_t); + pld->tstmp = platformGetIMUCounter(); + pld->dbTstmp = platformGetSolutionTstampAsDouble() * 0.000001; // seconds + + EKF_GetAttitude_EA(EulerAngles); + pld->roll = (float)EulerAngles[ROLL]; + pld->pitch = (float)EulerAngles[PITCH]; + pld->yaw = (float)EulerAngles[YAW]; + + GetAccelData_g(data); + for(int i = 0; i < NUM_AXIS; i++){ + pld->accels_g[i] = (float)data[i]; + } + + GetRateData_degPerSec(data); + for(int i = 0; i < NUM_AXIS; i++){ + pld->rates_dps[i] = data[i]; + } + + GetMagData_G(data); + for(int i = 0; i < NUM_AXIS; i++){ + pld->mags[i] = data[i]; + } + + float rateBias[NUM_AXIS]; + EKF_GetEstimatedAngRateBias(rateBias); + for(int i = 0; i < NUM_AXIS; i++){ + pld->rateBias[i] = (float)rateBias[i]; + } + + EKF_GetOperationalMode(&opMode); + EKF_GetOperationalSwitches(&linAccelSw, &turnSw); + pld->opMode = opMode; + pld->accelLinSwitch = linAccelSw; + pld->turnSwitch = turnSw; + return TRUE; +} + +/****************************************************************************** + * @name Fille2PacketPayloadt - API call ro prepare user output packet + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL Fill_e2PacketPayload(uint8_t *payload, uint8_t *payloadLen) +{ + float fData[3]; + real rData[3]; + double dData[3]; + + ekf2_payload_t *pld = (ekf2_payload_t *)payload; + + *payloadLen = sizeof(ekf2_payload_t); + pld->tstmp = platformGetIMUCounter(); + pld->dbTstmp = platformGetSolutionTstampAsDouble() * 0.000001; // seconds + + EKF_GetAttitude_EA(rData); + pld->roll = (float)rData[ROLL]; + pld->pitch = (float)rData[PITCH]; + pld->yaw = (float)rData[YAW]; + + GetAccelData_g(dData); + for(int i = 0; i < NUM_AXIS; i++){ + pld->accels_g[i] = (float)dData[i]; + } + + EKF_GetEstimatedAccelBias(fData); + for(int i = 0; i < NUM_AXIS; i++){ + pld->accelBias[i] = fData[i]; + } + + GetRateData_degPerSec(dData); + for(int i = 0; i < NUM_AXIS; i++){ + pld->rates_dps[i] = (float)dData[i]; + } + + EKF_GetEstimatedAngRateBias(fData); + for(int i = 0; i < NUM_AXIS; i++){ + pld->rateBias[i] = fData[i]; + } + + + EKF_GetEstimatedVelocity(fData); + for(int i = 0; i < NUM_AXIS; i++){ + pld->velocity[i] = fData[i]; + } + + GetMagData_G(dData); + for(int i = 0; i < NUM_AXIS; i++){ + pld->mags[i] = (float)dData[i]; + } + + EKF_GetEstimatedLLA(dData); + for(int i = 0; i < NUM_AXIS; i++){ + pld->pos[i] = dData[i]; + } + + uint8_t opMode, linAccelSw, turnSw; + EKF_GetOperationalMode(&opMode); + EKF_GetOperationalSwitches(&linAccelSw, &turnSw); + + pld->opMode = opMode; + pld->accelLinSwitch = linAccelSw; + pld->turnSwitch = turnSw; + + return TRUE; +} + diff --git a/examples/OpenIMU300RI/INS/lib/Core/UARTComm/CommonMessages.h b/examples/OpenIMU300RI/INS/lib/Core/UARTComm/CommonMessages.h new file mode 100644 index 0000000..0f373f0 --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/UARTComm/CommonMessages.h @@ -0,0 +1,126 @@ +/** *************************************************************************** + * @file UARTMessages.h + * + * 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. + * + ******************************************************************************/ +/******************************************************************************* +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 _UART_MESSAGES_H +#define _UART_MESSAGES_H +#include + +#pragma pack(1) + +// payload structure of standard IMU data message +typedef struct { + uint32_t timer; + float accel_mpss[3]; + float rate_dps[3]; + float mag_G[3]; +}data1_payload_t; + +// payload structure of standard test message +typedef struct { + uint32_t counter; +}test_payload_t; + +// payload structure of standard unit attitude message +typedef struct { + uint32_t itow; + double dblItow; + float roll; + float pitch; + float corrRates[3]; + float accels[3]; + uint8_t ekfOpMode; + uint8_t accelLinSwitch; + uint8_t turnSwitch; +}angle1_payload_t; + +// payload structure of alternative IMU data message +typedef struct { + uint32_t tstmp; + double dbTstmp; + float accel_g[3]; + float rate_dps[3]; + float mag_G[3]; + float temp_C; +}scaled1_payload_t; + +// payload structure of standard EKF message +typedef struct { + uint32_t tstmp; + double dbTstmp; + float roll; + float pitch; + float yaw; + float accels_g[3]; + float rates_dps[3]; + float rateBias[3]; + float mags[3]; + uint8_t opMode; + uint8_t accelLinSwitch; + uint8_t turnSwitch; +}ekf1_payload_t; + +// payload structure of enchanced EKF message +typedef struct { + uint32_t tstmp; + double dbTstmp; + float roll; + float pitch; + float yaw; + float accels_g[3]; + float accelBias[3]; + float rates_dps[3]; + float rateBias[3]; + float velocity[3]; + float mags[3]; + double pos[3]; + uint8_t opMode; + uint8_t accelLinSwitch; + uint8_t turnSwitch; +}ekf2_payload_t; + +// payload structure of standard unit attitude message +typedef struct { + uint32_t itow; + double dblItow; + float roll; + float pitch; + float yaw; + float corrRates[3]; + float accels[3]; +}angle2_payload_t; + + +#pragma pack() + +BOOL Fill_PingPacketPayload(uint8_t *payload, uint8_t *payloadLen); +BOOL Fill_VersionPacketPayload(uint8_t *payload, uint8_t *payloadLen); +BOOL Fill_zTPacketPayload(uint8_t *payload, uint8_t *payloadLen); +BOOL Fill_z1PacketPayload(uint8_t *payload, uint8_t *payloadLen); +BOOL Fill_a1PacketPayload(uint8_t *payload, uint8_t *payloadLen); +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); + +#endif diff --git a/examples/OpenIMU300RI/INS/lib/Core/library.json b/examples/OpenIMU300RI/INS/lib/Core/library.json new file mode 100644 index 0000000..f5babce --- /dev/null +++ b/examples/OpenIMU300RI/INS/lib/Core/library.json @@ -0,0 +1,24 @@ +{ + "name": "Core", + "version": "1.0.4", + "description": "Algorithm and GPS libraries", + "build": { + "flags": [ + "-I Algorithm", + "-I Algorithm/include", + "-I Buffer", + "-I Buffer/include", + "-I Common", + "-I Common/include", + "-I GPS", + "-I GPS/include", + "-I Odo", + "-I Odo/include", + "-I Math", + "-I Math/include", + "-I SPIComm", + "-I UARTComm", + "-I ." + ] + } + } diff --git a/examples/OpenIMU300RI/IMU/lib/readme.txt b/examples/OpenIMU300RI/INS/lib/readme.txt similarity index 100% rename from examples/OpenIMU300RI/IMU/lib/readme.txt rename to examples/OpenIMU300RI/INS/lib/readme.txt diff --git a/examples/OpenIMU300RI/INS/openimu.json b/examples/OpenIMU300RI/INS/openimu.json new file mode 100644 index 0000000..109c95b --- /dev/null +++ b/examples/OpenIMU300RI/INS/openimu.json @@ -0,0 +1,730 @@ +{ + "appVersion": "OpenIMU300ZI INS 1.1.0", + "description": "9-axis OpenIMU with triaxial rate, acceleration, and magnetic measurement", + "userConfiguration": [ + { "paramId": 0,"category": "General", "paramType": "disabled", "type": "uint64", "name": "Data CRC" }, + { "paramId": 1,"category": "General", "paramType": "disabled", "type": "uint64", "name": "Data Size" }, + { "paramId": 2, "category": "General","paramType": "select", "type": "int64", "name": "Baud Rate", "argument": "baud_rate","value_range":[-1,1], "value_accuracy":6, "options": [115200, 57600, 230400, 38400,0] }, + { "paramId": 3, "category": "General","paramType": "select", "type": "char8", "name": "Packet Type", "argument": "type", "value_range":[-1,1],"value_accuracy":6, "options": ["z1", "zT", "s1", "e1", "e2"] }, + { "paramId": 4, "category": "General","paramType": "select", "type": "int64", "name": "Packet Rate", "argument": "rate", "value_range":[-1,1],"value_accuracy":6, "options": [200, 100, 50, 20, 10, 5, 2, 0] }, + { "paramId": 5, "category": "General","paramType": "select", "type": "int64", "name": "Accel LPF", "argument": "xl_lpf", "value_range":[-1,1],"value_accuracy":6,"options": [50, 40, 25, 20, 10, 5, 2] }, + { "paramId": 6, "category": "General","paramType": "select", "type": "int64", "name": "Rate LPF", "argument": "rate_lpf", "value_range":[-1,1],"value_accuracy":6,"options": [50, 40, 25, 20, 10, 5, 2] }, + { "paramId": 7, "category": "General","paramType": "select", "type": "char8", "name": "Orientation", "argument": "orien", "value_range":[-1,1],"value_accuracy":6,"options": ["+X+Y+Z"] } + ], + "userMessages": { + "inputPackets": [{ + "name": "pG", + "description": "Get device serial number & factory ID", + "inputPayload": {}, + "responsePayload": { + "type": "string", + "name": "Device ID and SN" + } + }, + { + "name": "gV", + "description": "Get user app version", + "inputPayload": {}, + "responsePayload": { + "type": "string", + "name": "User Version" + } + }, + { + "name": "gA", + "description": "Get All Configuration Parameters", + "inputPayload": {}, + "responsePayload": { + "type": "userConfiguration", + "name": "Full Current Configuration" + } + }, + { + "name": "gP", + "description": "Get a Configuration Parameter", + "inputPayload": { + "type": "paramId", + "name": "Request Parameter Id" + }, + "responsePayload": { + "type": "userParameter", + "name": "User Parameter" + } + }, + { + "name": "uP", + "description": "Update Configuration Parameter", + "inputPayload": { + "type": "userParameter", + "name": "Parameter to be Updated" + }, + "responsePayload": { + "type": "paramId", + "name": "ID of the Updated Parameter" + } + } + ], + "outputPackets": [{ + "name": "z1", + "description": "Scaled 9-Axis IMU", + "payload": [{ + "type": "uint32", + "name": "time", + "unit": "s" + }, + { + "type": "float", + "name": "xAccel", + "unit": "m/s/sG" + }, + { + "type": "float", + "name": "yAccel", + "unit": "m/s/sG" + }, + { + "type": "float", + "name": "zAccel", + "unit": "m/s/sG" + }, + { + "type": "float", + "name": "xRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "yRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "zRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "xMag", + "unit": "Gauss" + }, + { + "type": "float", + "name": "yMag", + "unit": "Gauss" + }, + { + "type": "float", + "name": "zMag", + "unit": "Gauss" + } + ], + "graphs": [{ + "name": "Acceleration", + "units": "m/s/s", + "xAxis": "Time (s)", + "yAxes": ["xAccel", "yAccel", "zAccel"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 80 + }, + { + "name": "Angular Rate", + "units": "deg/s", + "xAxis": "Time (s)", + "yAxes": ["xRate", "yRate", "zRate"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 400 + } + ] + }, + { + "name": "e1", + "description": "VG/AHRS Output Message", + "payload": [{ + "type": "uint32", + "name": "timeCntr", + "unit": "msec" + }, + { + "type": "double", + "name": "time", + "unit": "s" + }, + { + "type": "float", + "name": "roll", + "unit": "rad" + }, + { + "type": "float", + "name": "pitch", + "unit": "rad" + }, + { + "type": "float", + "name": "heading", + "unit": "rad" + }, + { + "type": "float", + "name": "xAccel", + "unit": "g" + }, + { + "type": "float", + "name": "yAccel", + "unit": "g" + }, + { + "type": "float", + "name": "zAccel", + "unit": "g" + }, + { + "type": "float", + "name": "xRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "yRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "zRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "xRateBias", + "unit": "deg/s" + }, + { + "type": "float", + "name": "yRateBias", + "unit": "deg/s" + }, + { + "type": "float", + "name": "zRateBias", + "unit": "deg/s" + }, + { + "type": "float", + "name": "xMag", + "unit": "G" + }, + { + "type": "float", + "name": "yMag", + "unit": "G" + }, + { + "type": "float", + "name": "zMag", + "unit": "G" + }, + { + "type": "uint8", + "name": "opMode", + "unit": "unitless" + }, + { + "type": "uint8", + "name": "linAccSw", + "unit": "unitless" + }, + { + "type": "uint8", + "name": "turnSw", + "unit": "unitless" + } + ], + "graphs": [ + { + "name": "Euler Angles (Roll and Pitch)", + "units": "deg", + "xAxis": "Time (s)", + "yAxes": ["roll", "pitch"], + "colors": ["#FF0000", "#00FF00"], + "yMax": 90 + }, + { + "name": "Euler Angles (Heading)", + "units": "deg", + "xAxis": "Time (s)", + "yAxes": ["heading"], + "colors": ["#FF0000"], + "yMax": 190 + }, + { + "name": "Acceleration", + "units": "g", + "xAxis": "Time (s)", + "yAxes": ["xAccel", "yAccel", "zAccel"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 5 + }, + { + "name": "Angular-Rate", + "units": "deg/s", + "xAxis": "Time (s)", + "yAxes": ["xRate", "yRate", "zRate"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 200 + }, + { + "name": "Angular-Rate Bias", + "units": "deg/s", + "xAxis": "Time (s)", + "yAxes": ["xRateBias", "yRateBias", "zRateBias"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 200 + }, + { + "name": "Magnetic-Field", + "units": "G", + "xAxis": "Time (s)", + "yAxes": ["xMag", "yMag", "zMag"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 20 + }, + { + "name": "Operational Flags", + "units": "unitless", + "xAxis": "Time (s)", + "yAxes": ["opMode", "linAccSw", "turnSw"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 5 + } + ] + }, + { + "name": "e2", + "description": "INS Output Message", + "payload": [{ + "type": "uint32", + "name": "timeCntr", + "unit": "msec" + }, + { + "type": "double", + "name": "time", + "unit": "s" + }, + { + "type": "float", + "name": "roll", + "unit": "rad" + }, + { + "type": "float", + "name": "pitch", + "unit": "rad" + }, + { + "type": "float", + "name": "heading", + "unit": "rad" + }, + { + "type": "float", + "name": "xAccel", + "unit": "g" + }, + { + "type": "float", + "name": "yAccel", + "unit": "g" + }, + { + "type": "float", + "name": "zAccel", + "unit": "g" + }, + { + "type": "float", + "name": "xAccelBias", + "unit": "g" + }, + { + "type": "float", + "name": "yAccelBias", + "unit": "g" + }, + { + "type": "float", + "name": "zAccelBias", + "unit": "g" + }, + { + "type": "float", + "name": "xRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "yRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "zRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "xRateBias", + "unit": "deg/s" + }, + { + "type": "float", + "name": "yRateBias", + "unit": "deg/s" + }, + { + "type": "float", + "name": "zRateBias", + "unit": "deg/s" + }, + { + "type": "float", + "name": "velNorth", + "unit": "m/s" + }, + { + "type": "float", + "name": "velEast", + "unit": "m/s" + }, + { + "type": "float", + "name": "velDown", + "unit": "m/s" + }, + { + "type": "float", + "name": "xMag", + "unit": "G" + }, + { + "type": "float", + "name": "yMag", + "unit": "G" + }, + { + "type": "float", + "name": "zMag", + "unit": "G" + }, + { + "type": "double", + "name": "lat", + "unit": "deg" + }, + { + "type": "double", + "name": "lon", + "unit": "deg" + }, + { + "type": "double", + "name": "alt", + "unit": "m" + }, + { + "type": "uint8", + "name": "opMode", + "unit": "unitless" + }, + { + "type": "uint8", + "name": "linAccSw", + "unit": "unitless" + }, + { + "type": "uint8", + "name": "turnSw", + "unit": "unitless" + } + ], + "graphs": [ + { + "name": "Euler Angles (Roll and Pitch)", + "units": "deg", + "xAxis": "Time (s)", + "yAxes": ["roll", "pitch"], + "colors": ["#FF0000", "#00FF00"], + "yMax": 90 + }, + { + "name": "Euler Angles (Heading)", + "units": "deg", + "xAxis": "Time (s)", + "yAxes": ["heading"], + "colors": ["#FF0000"], + "yMax": 190 + }, + { + "name": "Acceleration", + "units": "g", + "xAxis": "Time (s)", + "yAxes": ["xAccel", "yAccel", "zAccel"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 8 + }, + { + "name": "Angular-Rate", + "units": "deg/s", + "xAxis": "Time (s)", + "yAxes": ["xRate", "yRate", "zRate"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 400 + }, + { + "name": "Angular-Rate Bias", + "units": "deg/s", + "xAxis": "Time (s)", + "yAxes": ["xRateBias", "yRateBias", "zRateBias"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 400 + }, + { + "name": "Acceleration Bias", + "units": "g", + "xAxis": "Time (s)", + "yAxes": ["xAccelBias", "yAccelBias", "zAccelBias"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 8 + }, + { + "name": "Magnetic-Field", + "units": "G", + "xAxis": "Time (s)", + "yAxes": ["xMag", "yMag", "zMag"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 8 + }, + { + "name": "Latitude/Longitude", + "units": "deg", + "xAxis": "Time (s)", + "yAxes": ["lat", "lon"], + "colors": ["#FF0000", "#00FF00"], + "yMax": 200 + }, + { + "name": "Altitude", + "units": "m", + "xAxis": "Time (s)", + "yAxes": ["alt"], + "colors": ["#FF0000"], + "yMax": 100 + }, + { + "name": "NED Velocity", + "units": "m/s", + "xAxis": "Time (s)", + "yAxes": ["velNorth", "velEast", "velDown"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 100 + }, + { + "name": "Operational Flags", + "units": "unitless", + "xAxis": "Time (s)", + "yAxes": ["opMode", "linAccSw", "turnSw"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 5 + } + ] + }, + { + "name": "s1", + "description": "IMU Scaled-Sensor Output Message", + "payload": [{ + "type": "uint32", + "name": "timeCntr", + "unit": "msec" + }, + { + "type": "double", + "name": "time", + "unit": "s" + }, + { + "type": "float", + "name": "xAccel", + "unit": "m/s/sg" + }, + { + "type": "float", + "name": "yAccel", + "unit": "m/s/s" + }, + { + "type": "float", + "name": "zAccel", + "unit": "m/s/s" + }, + { + "type": "float", + "name": "xRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "yRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "zRate", + "unit": "deg/s" + }, + { + "type": "float", + "name": "xMag", + "unit": "G" + }, + { + "type": "float", + "name": "yMag", + "unit": "G" + }, + { + "type": "float", + "name": "zMag", + "unit": "G" + }, + { + "type": "float", + "name": "temp", + "unit": "degC" + } + ], + "graphs": [{ + "name": "Acceleration", + "units": "m/s/s", + "xAxis": "Time (s)", + "yAxes": ["xAccel", "yAccel", "zAccel"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 5 + }, + { + "name": "Angular-Rate", + "units": "deg/s", + "xAxis": "Time (s)", + "yAxes": ["xRate", "yRate", "zRate"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 200 + }, + { + "name": "Magnetic-Field", + "units": "G", + "xAxis": "Time (s)", + "yAxes": ["xMag", "yMag", "zMag"], + "colors": ["#FF0000", "#00FF00", "#0000FF"], + "yMax": 5 + }, + { + "name": "Board-Temperature", + "units": "degC", + "xAxis": "Time (s)", + "yAxes": ["temp"], + "colors": ["#FF0000"], + "yMax": 100 + } + ] + } + ] + }, + "bootloaderMessages": [{ + "name": "JI", + "description": "Jump to Bootloader", + "inputPayload": {}, + "responsePayload": { + "type": "ack", + "response": "Acknowledgement" + } + }, + { + "name": "JA", + "description": "Jump to App", + "inputPayload": {}, + "responsePayload": { + "type": "none", + "response": "Empty" + } + }, + { + "name": "WA", + "description": "Write App Block", + "inputPayload": { + "type": "block", + "name": "4 byte block address followed by up to 240 bytes data" + }, + "responsePayload": { + "type": "ack", + "response": "Acknowledgement" + } + } + ], + "CLICommands": [{ + "name": "help", + "description": "CLI help menu", + "function": "help_handler" + }, + { + "name": "exit", + "description": "exit CLI", + "function": "" + }, + { + "name": "run", + "description": "Operations defined by users", + "function": "run_handler" + }, + { + "name": "save", + "description": "Save the configuration into EEPROM", + "function": "save_handler" + }, + { + "name": "connect", + "description": "Find OpenIMU device", + "function": "connect_handler" + }, + { + "name": "upgrade", + "description": "Upgrade firmware", + "function": "upgrade_handler" + }, + { + "name": "record", + "description": "Record output data of OpenIMU on local machine", + "function": "record_handler" + }, + { + "name": "stop", + "description": "stop recording outputs on local machine", + "function": "stop_handler" + }, + { + "name": "server_start", + "description": "start server thread and must use exit command to quit", + "function": "server_start_handler" + }, + { + "name": "get", + "description": "Read the current configuration and output data", + "function": "get_handler" + }, + { + "name": "set", + "description": "Write parameters to OpenIMU", + "function": "set_handler" + } + ] +} + diff --git a/examples/OpenIMU300RI/INS/platformio.ini b/examples/OpenIMU300RI/INS/platformio.ini new file mode 100644 index 0000000..9e3284a --- /dev/null +++ b/examples/OpenIMU300RI/INS/platformio.ini @@ -0,0 +1,45 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; http://docs.platformio.org/page/projectconf.html +[platformio] +description = A Kalman filter based algorithm that uses rate-sensors to propagate attitude (roll, pitch, and heading angles) forward in time and accelerometers and magnetometers as references, to correct for bias in the rate-sensor signal. Sensors data messages sent over CAN using J1939 protocol. + +[env:OpenIMU300RI] +platform = aceinna_imu +lib_archive = false +board = OpenIMU300 +;lib_deps = ../../openIMU300-lib\openIMU300-lib +lib_deps = OpenIMU300-base-library@1.1.6 +build_flags = + -D GPS + -D GPS_OVER_CAN + -D SAE_J1939 + -D CAN_BUS_COMM + -D DETECT_USER_SERIAL_CMD + -D __FPU_PRESENT + -D ARM_MATH_CM4 + -I include + -I src/user + -I src + -I lib/CAN/include + -I lib/CAN +; -L ldscripts + -Og +; -Wno-comment +; -Wl,-Map,imu380.map +; -Wl,-Tstm32f40x.ld + -mthumb -mcpu=cortex-m4 -mfloat-abi=softfp -mfpu=fpv4-sp-d16 + +;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/OpenIMU300RI/VG_AHRS/lib/Core/Common/src/dummy.c b/examples/OpenIMU300RI/INS/src/appVersion.h similarity index 77% rename from examples/OpenIMU300RI/VG_AHRS/lib/Core/Common/src/dummy.c rename to examples/OpenIMU300RI/INS/src/appVersion.h index 395b50f..94fac93 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Common/src/dummy.c +++ b/examples/OpenIMU300RI/INS/src/appVersion.h @@ -1,11 +1,12 @@ -/** *************************************************************************** - * @file spi_dummy.c +/***************************************************************************** + * @file appVersion.h * * 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 Version definition based on UCB serial protocol. ******************************************************************************/ /******************************************************************************* Copyright 2018 ACEINNA, INC @@ -22,21 +23,10 @@ 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 SPI_BUS_COMM +#ifndef _IMU_APP_VERSION_H +#define _IMU_APP_VERSION_H -#include -#include +#define APP_VERSION_STRING "INS_J1939 1.0.3" -// Dummy placeholder function -void SPI_ProcessCommand(uint8_t cmd) -{ - -} - -// Dummy placeholder function -void SPI_ProcessData(uint8_t* in) -{ - -} #endif diff --git a/examples/OpenIMU300RI/INS/src/main.c b/examples/OpenIMU300RI/INS/src/main.c new file mode 100644 index 0000000..f1a5a8f --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/main.c @@ -0,0 +1,201 @@ +/** *************************************************************************** + * @file main.c + * + * 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. + * + * main is the center of the system universe, it all starts here. And never ends. + * entry point for system (pins, clocks, interrupts), data and task initialization. + * contains the main processing loop. - this is a standard implementation + * which has mainly os functionality in the main loop + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ +#define __MAIN + +#include + +#include "boardAPI.h" +#include "magAPI.h" +#include "platformAPI.h" +#include "userAPI.h" +#include "debug.h" + +#include "taskDataAcquisition.h" +#include "taskCanCommunicationJ1939.h" + +#include "osapi.h" +#include "osresources.h" +#include "canAPI.h" + +char buildInfo[] = {__DATE__"," __TIME__}; + +/** *************************************************************************** + * @name getBuildInfo - provides the pointer to the build date and time string + * @brief + * + * @param [in] N/A + * @param [out] pointer to build info string + * @retval N/A + ******************************************************************************/ +char *getBuildInfo() +{ + return buildInfo; +} + +/** *************************************************************************** + * @name initDebugInterface Debug serial interface initialization + * @brief Initializes debug serial console messaging support + * + * @param [in] N/A + * @param [out] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +void DebugInterfaceInit(void) +{ + char status[100]; + + BOOL res = platformAssignPortTypeToSerialChannel(DEBUG_SERIAL_PORT, UART_CHANNEL_0); // assign debug output to user serial channel + + if(!res){ + // some error + return; + } +#ifdef GPS_OVER_CAN + res = platformAssignPortTypeToSerialChannel(USER_SERIAL_PORT, UART_CHANNEL_0); // assign debug output to user serial channel + if(!res){ + // some error + return; + } +#endif + + // Initialize the debug serial port + InitDebugSerialCommunication(38400); + + BoardGetResetStatus(status, sizeof(status)); + +} + +void CreateTasks(void) +{ + osThreadId iD; + + /// Create RTOS tasks: + // dacq task + osThreadDef(DACQ_TASK, TaskDataAcquisition, osPriorityAboveNormal, 0, 2048); + iD = osThreadCreate(osThread(DACQ_TASK), NULL); + if(iD == NULL){ + while(1); + } + + //user communication task + osThreadDef(CAN_COMM_TASK, TaskCANCommunicationJ1939, osPriorityNormal, 0, 2048); + iD = osThreadCreate(osThread(CAN_COMM_TASK), NULL); + if(iD == NULL){ + while(1); + } + +#ifdef GPS + // GPS task + osThreadDef(GPS_TASK, TaskGps, osPriorityBelowNormal, 0, 1024); + iD = osThreadCreate(osThread(GPS_TASK), NULL); + if(iD == NULL){ + while(1); + } + + // WMM task + osThreadDef(WMM_TASK, TaskWorldMagneticModel, osPriorityLow, 0, 512); + iD = osThreadCreate(osThread(WMM_TASK), NULL); + if(iD == NULL){ + while(1); + } + +#endif + + + gyroReadySem = osSemaphoreCreate(osSemaphore(GYRO_READY_SEM), 1); + accelReadySem = osSemaphoreCreate(osSemaphore(ACCEL_READY_SEM), 1); + magReadySem = osSemaphoreCreate(osSemaphore(MAG_READY_SEM), 1); + tempReadySem = osSemaphoreCreate(osSemaphore(TEMP_READY_SEM), 1); + canDataSem = osSemaphoreCreate(osSemaphore(CAN_DATA_SEM), 1); + dataAcqSem = osSemaphoreCreate(osSemaphore(DATA_ACQ_SEM), 1); + +} + +/** *************************************************************************** + * @name main() The DMU380 firmware initialization and entry point + * @brief creates tasks for data collection SPI1 and user communications SPI3 + * or USART serial stream (Nav-View) debug serial console messaging + * + * @param [in] N/A + * @param [out] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +int main(void) +{ +#ifdef GPS + BOOL res; +#endif + + // Initialize processor and board-related signals + BoardInit(); + + platformSetUnitCommunicationType(CAN_BUS); + + #ifdef GPS + platformUnassignSerialChannels(); + + #ifndef GPS_OVER_CAN + // user serial port will be used for GPS + res = platformAssignPortTypeToSerialChannel(GPS_SERIAL_PORT, UART_CHANNEL_0); + while(!res){}; // check if valid + #else + DebugInterfaceInit(); + #endif +#else + DebugInterfaceInit(); +#endif + + // Apply factory configuration + platformInitConfigureUnit(); + + // Apply user-chosen configuration + userInitConfigureUnit(); + + // Initialize OS and create required tasks + CreateTasks(); + + // Start Running the tasks... + // Start scheduler + osKernelStart(); + + // We should never get here as control is now taken by the scheduler + for (;;); + +} + + +void vApplicationStackOverflowHook() +{ + while(1); +} +void vApplicationMallocFailedHook() +{ + while(1); +} + diff --git a/examples/OpenIMU300RI/INS/src/taskDataAcquisition.c b/examples/OpenIMU300RI/INS/src/taskDataAcquisition.c new file mode 100644 index 0000000..5db5a57 --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/taskDataAcquisition.c @@ -0,0 +1,155 @@ +/***************************************************************************** + * @file taskDataAcquisition.c + * + * 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. + * + * sensor data acquisition task runs at 100Hz, gets the data for each sensor + * and applies available calibration + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#include "algorithmAPI.h" +#include "bitAPI.h" +#include "boardAPI.h" +#include "magAPI.h" +#include "platformAPI.h" +#include "userAPI.h" + +#include "taskDataAcquisition.h" + +#include "osapi.h" +#include "osresources.h" + +/** *************************************************************************** + * @name TaskDataAcquisition() CALLBACK main loop + * @brief Get the sensor data at the specified frequency (based on the + * configuration of the accelerometer rate-sensor). Process and provide + * information to the user via the UART or SPI. + * @param N/A + * @retval N/A + ******************************************************************************/ +void TaskDataAcquisition(void const *argument) +{ + // + int res; + uint16_t dacqRate; + +#pragma GCC diagnostic ignored "-Wunused-but-set-variable" + BOOL overRange = FALSE; //uncomment this line if overrange processing required +#pragma GCC diagnostic warning "-Wunused-but-set-variable" + + // This routine sets up the timer. Can use the structure below this point. + TaskDataAcquisition_Init(); + // Initialize user algorithm parameters, if needed + initUserDataProcessingEngine(); + // set the mag align scale factors + InitMagAlignParams(); + // Start sensors data acquisition + DataAquisitionStart(); + + // Set the sampling frequency of data acquisition task + dacqRate = DACQ_200_HZ; + + //************** Add user initialization here, if needed **************** + + // Data is provided by the primary sensors (accelerometer and rate-sensor) + // as fast as possible. Data is obtained from secondary sensors at a + // lower rate. The rate-sensor data read is synced to TIM5 or an + // external signal. When the read is commanded and in the data buffer, + // The data-event flag is set and the wait is bypassed. Data is obtained + // from the buffer, calibrated, filtered and provided to the user for + // subsequent processing. + + while( 1 ) + { + // ***************************************************************** + // NOTE: This task loop runs at 200 Hz + // ***************************************************************** + // Handle Timing vard, watchdog and BIT + PrepareToNewDacqTickAndProcessUartMessages(); + // Wait for next tick + // Upon timeout of TIM5(orTIM2 or user sync), let the process continue + + res = osSemaphoreWait(dataAcqSem, 1000); + if(res != osOK){ + // Wait timeout expired. Something wrong wit the dacq system + // Process timeout here + } + + // Get calibrated sensor data: + // Inside next 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) + // + // Refer to UserConfiguration.c for implementation and to the enumerator structure + // 'eFilterType' in file filter.h for available selections. + // + // Low pass filtering is followed by application of the unit calibration parameters + // - scaling, temperature compensation, bias removal, and misalignment. + // + // Results are placed in the structure of type double. The pointer to this + // structure is 'pScaledSensors'. Ordering of sensors data in this structure + // defined by 'rawSensor_e' enumerator in the file indices.h + GetSensorsData(); + + // Check if sensors data over range + // If overRange is TRUE - limit values are put into sensors data based + // on chosen sensors sensitivity + overRange = handleOverRange(); + + // ***************************************************************** + // At this point sensors data is in next units + // Acceleration - g's + // Rates - rad/s + // Magnetometer - Gauss + // Temperature - degrees C + //****************************************************************** + + // BIT status. May have inadvertently changed this during an update. + updateBITandStatus(); + + // ********************** Algorithm ************************ + // Due to CPU performance related to algorithm math operations, GPS related + // algorithms are run at 100 Hz or less (large number of calculations + // involved). Incorporate timing logic inside algorithm function, if + // desired. + // + // For the initial simplicity use pScaledSensors array as algorithm input + // and output. Use 'is100Hz' variable to determine the rate of this task + // loop. + inertialAndPositionDataProcessing(dacqRate); + + // Uncomment next line if there is intention of using S0 or S1 xbow + // packets for continuous data output + //***************************************************************** + // applyNewScaledSensorsData(); + //***************************************************************** + + 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) + MagAlign(); // only does this on align + } + + } +} diff --git a/examples/OpenIMU300RI/INS/src/taskDataAcquisition.h b/examples/OpenIMU300RI/INS/src/taskDataAcquisition.h new file mode 100644 index 0000000..dd8dfd8 --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/taskDataAcquisition.h @@ -0,0 +1,42 @@ +/** *************************************************************************** + * @file taskDataAcquisition.h + * + * 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. + * + * sensor data acquisition task runs at 100Hz, gets the data for each sensor + * and applies available calibration + ******************************************************************************/ +/******************************************************************************* +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 _TASK_DATA_ACQUISITION_H_ +#define _TASK_DATA_ACQUISITION_H_ + +#include "stdint.h" +#include "GlobalConstants.h" +extern void TaskDataAcquisition(void const *argument); +extern void PrepareToNewDacqTick(); +extern void PrepareToNewDacqTickAndProcessUartMessages(); +extern void TaskDataAcquisition_Init(void); +extern void GetSensorsData(void); +extern void EnterMainAlgLoop(void); +extern void DataAquisitionStart(void); +extern BOOL isOneHundredHertzFlag(void); + +#endif diff --git a/examples/OpenIMU300RI/INS/src/user/UserAlgorithm.c b/examples/OpenIMU300RI/INS/src/user/UserAlgorithm.c new file mode 100644 index 0000000..7dc6ec0 --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/user/UserAlgorithm.c @@ -0,0 +1,278 @@ +/** *************************************************************************** + * @file UserAlgorithm.c + * + * 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. + * + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#include + +#include "algorithmAPI.h" +#include "gpsAPI.h" +#include "platformAPI.h" +#include "userAPI.h" +#include "osapi.h" + +#include "Indices.h" +#include "GlobalConstants.h" + +#include "algorithm.h" +#include "EKF_Algorithm.h" +#include "BitStatus.h" +#include "UserConfiguration.h" +#include "bsp.h" +#include "debug.h" + +#include "MagAlign.h" +#include "sae_j1939.h" + +#define DEBUG_FREQ_0HZ 0 +#define DEBUG_FREQ_1HZ 1 +#define DEBUG_FREQ_2HZ 2 +#define DEBUG_FREQ_5HZ 5 + +// +static void _Algorithm(uint16_t dacqRate, uint8_t algoType); +static void _InitAlgo(uint8_t algoType); + +#ifndef INS_OFFLINE +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); +#endif // !INS_OFFLINE + +// Initialize GPS algorithm variables +void InitUserAlgorithm() +{ + // Initialize built-in algorithm structure + InitializeAlgorithmStruct(FREQ_100_HZ); + + setLeverArm( (real)gUserConfiguration.leverArmBx, + (real)gUserConfiguration.leverArmBy, + (real)gUserConfiguration.leverArmBz ); + setPointOfInterest( (real)gUserConfiguration.pointOfInterestBx, + (real)gUserConfiguration.pointOfInterestBy, + (real)gUserConfiguration.pointOfInterestBz ); + +} + +// Initialization variable +int initAlgo = 1; +odoDataStruct_t gOdo; + +void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, gpsDataStruct_t *gps, uint16_t dacqRate) +{ + + // 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, &gOdo, FALSE); + + // Call the desired algorithm based on the EKF with different + // calling rates and different settings. + _Algorithm(dacqRate, algoType); + + // Fill the output data structure with the EKF states and other + // desired information + OSDisableHook(); + EKF_SetOutputStruct(); + OSEnableHook(); + +#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) +{ + // 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); + gAlgorithm.callingFreq = FREQ_100_HZ; // redundant; set above + break; + default: + // Nothing to do + break; + } + } +} + + + +// +static void _Algorithm(uint16_t dacqRate, uint8_t algoType) +{ + // Aceinna VG/AHRS/INS algorithm + EKF_Algorithm(); +} + +#ifndef INS_OFFLINE +// +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) +{ + // 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); + + 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 + } + } +} +#endif // !INS_OFFLINE diff --git a/examples/OpenIMU300RI/INS/src/user/UserAlgorithm.h b/examples/OpenIMU300RI/INS/src/user/UserAlgorithm.h new file mode 100644 index 0000000..1c8c3d6 --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/user/UserAlgorithm.h @@ -0,0 +1,13 @@ +/* + * File: UserAlgorithm.h + * Author: joemotyka + * + * Created on June 28, 2018, 12:23 AM + */ + +#ifndef _USER_ALGORITHM_H_ +#define _USER_ALGORITHM_H_ + + +#endif /* _USER_ALGORITHM_H_ */ + diff --git a/examples/OpenIMU300RI/INS/src/user/UserConfiguration.c b/examples/OpenIMU300RI/INS/src/user/UserConfiguration.c new file mode 100644 index 0000000..613a67f --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/user/UserConfiguration.c @@ -0,0 +1,401 @@ +/** *************************************************************************** + * @file UserConfiguration.c + * + * 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. + * + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#include "string.h" + +#include "algorithmAPI.h" +#include "gpsAPI.h" +#include "magAPI.h" +#include "platformAPI.h" +#include "userAPI.h" + +#include "UserConfiguration.h" +#include "eepromAPI.h" +#include "Indices.h" +#include "sae_j1939.h" + +EcuConfigurationStruct *gEcuConfigPtr; + + +// Default user configuration structure +// Applied to unit upon reception of "zR" command +// Do Not remove - just add extra parameters if needed +// Change default settings if desired +const UserConfigurationStruct gDefaultUserConfig = { + .dataCRC = 0, + .dataSize = sizeof(UserConfigurationStruct), + .ecuAddress = 128, + .ecuBaudRate = _ECU_250K, + .ecuPacketRate = 10, // 10Hz + .ecuFilterFreqAccel = 25, + .ecuFilterFreqRate = 25, + + .ecuPacketType = ( ACEINNA_SAE_J1939_PACKET_SLOPE_SENSOR | + ACEINNA_SAE_J1939_PACKET_ANGULAR_RATE | + ACEINNA_SAE_J1939_PACKET_ACCELERATION | + ACEINNA_SAE_J1939_PACKET_RAPID_COURSE_UPDATE | + ACEINNA_SAE_J1939_PACKET_ATTITUDE | + ACEINNA_SAE_J1939_PACKET_RAPID_POSITION_UPDATE + ), + +// .ecuPacketType = 0, + .ecuOrientation = 0, // +X +Y +Z + .canTermResistorEnabled = 0, + .canBaudRateDetectEnabled = 0, + .userBehavior = + USER_BEHAVIOR_RUN_ALGORITHM_MASK | // algorithm enabled + USER_BEHAVIOR_USE_GNSS_OVER_CAN | // gnss input data coming over CAN bus + USER_BEHAVIOR_USE_MAGS_MASK | // if usage of mags desired + USER_BEHAVIOR_DUMMY_MASK, + .algResetPs = 0, + .saveCfgPs = 0, + .packetTypePs = 0, + .packetRatePs = 0, + .filterPs = 0, + .orientationPs = 0, + .userBehvPs = 0, + .statusPs = 0, +//*********************************************** +// INS - related parameters +//*********************************************** + .gpsBaudRate = 9600, + .gpsProtocol = UBLOX_BINARY, + .hardIron_X = 0.0, + .hardIron_Y = 0.0, + .softIron_Ratio = 1.0, + .softIron_Angle = 0.0, + .leverArmBx = 0.0, + .leverArmBy = 0.0, + .leverArmBz = 0.0, + .pointOfInterestBx = 0.0, + .pointOfInterestBy = 0.0, + .pointOfInterestBz = 0.0, + // add default parameter values here, if desired +}; + +UserConfigurationStruct gUserConfiguration; +UserConfigurationStruct gTmpUserConfiguration; + +uint8_t UserDataBuffer[4096]; +volatile char *info; +BOOL configValid = FALSE; + +void setUserMagAlignParams(magAlignUserParams_t *params) +{ + gUserConfiguration.hardIron_X = params->hardIron_X; + gUserConfiguration.hardIron_Y = params->hardIron_Y; + gUserConfiguration.softIron_Ratio = params->softIron_Ratio; + gUserConfiguration.softIron_Angle = params->softIron_Angle; +} + +void getUserMagAlignParams(magAlignUserParams_t *params) +{ + params->hardIron_X = gUserConfiguration.hardIron_X; + params->hardIron_Y = gUserConfiguration.hardIron_Y; + params->softIron_Ratio = gUserConfiguration.softIron_Ratio; + params->softIron_Angle = gUserConfiguration.softIron_Angle; +} + + +void userInitConfigureUnit() +{ + int size = sizeof(gUserConfiguration); // total size in bytes + + // sanity check for maximum size of user config structure; + if(size >= 0x4000){ + while(1); + } + + if(EEPROM_IsAppStartedFirstTime()) { + // comment next line if want to keep previously stored in EEPROM parameters + // after rebuilding and/or reloading new application + LoadDefaultUserConfig(TRUE); + } + + // Validate checksum of user configuration structure + configValid = EEPROM_ValidateUserConfig(&size); + + 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{ + LoadDefaultUserConfig(FALSE); + } + + // assign new actual size + gUserConfiguration.dataSize = sizeof(UserConfigurationStruct); + memset(&gEcuConfig, 0, sizeof(gEcuConfig)); + + ApplyEcuSettings(&gEcuConfig); + + if(EEPROM_IsUserApplicationActive()) + { + ApplySystemParameters(&gEcuConfig); + } + +#ifndef GPS_OVER_CAN + SetGpsBaudRate((int) gUserConfiguration.gpsBaudRate, TRUE); + SetGpsProtocol((int) gUserConfiguration.gpsProtocol, TRUE); +#endif + + + info = getBuildInfo(); +} + + +/** *************************************************************************** + * @name SaveUserConfig - saving of user configuration structure un the + * predefined flash sector + * @brief + * + * @param [in] N/A + * @retval error (0), no error (1) + ******************************************************************************/ +BOOL SaveUserConfig(void) +{ + int size; + BOOL status; + + size = sizeof(UserConfigurationStruct); + status = EEPROM_SaveUserConfig((uint8_t *)&gUserConfiguration, size); + + if(status){ + return TRUE; + } + + return FALSE; + +} + + +BOOL LoadDefaultUserConfig(BOOL fSave) +{ + BOOL valid = TRUE; + // Load default user configuration + memcpy((void*)&gUserConfiguration, (void*)&gDefaultUserConfig, sizeof(UserConfigurationStruct)); + + if(!fSave){ + return TRUE; + } + + if(!SaveUserConfig()){ + valid = FALSE; + } + return valid; +} + + +/** *************************************************************************** + * @name user_config_save() save current configuration to EEPROM while getting + * config save request +* @brief write global parameters of configuration in RAM to EEPROM + * + * @param [in] + * @retval 1 successfuk or 0 failure + ******************************************************************************/ +BOOL UpdateEcuConfig(EcuConfigurationStruct *gEcuConfigPtr, BOOL fSave) +{ + // address, baud rate, new PS, orientation and lpf + gUserConfiguration.ecuAddress = (uint16_t)gEcuConfigPtr->address; + gUserConfiguration.ecuBaudRate = (uint16_t)gEcuConfigPtr->baudRate; + + gUserConfiguration.algResetPs = gEcuConfigPtr->alg_reset_ps; + gUserConfiguration.saveCfgPs = gEcuConfigPtr->save_cfg_ps; + gUserConfiguration.statusPs = gEcuConfigPtr->status_ps; + gUserConfiguration.packetRatePs = gEcuConfigPtr->packet_rate_ps; + gUserConfiguration.packetTypePs = gEcuConfigPtr->packet_type_ps; + gUserConfiguration.filterPs = gEcuConfigPtr->digital_filter_ps; + gUserConfiguration.orientationPs = gEcuConfigPtr->orientation_ps; + gUserConfiguration.userBehvPs = gEcuConfigPtr->user_behavior_ps; + gUserConfiguration.magAlignPs = gEcuConfigPtr->mag_align_ps; + + if(fSave){ + return SaveUserConfig(); + } + + return TRUE; +} + +BOOL SaveEcuConfig(EcuConfigurationStruct *gEcuConfigPtr) +{ + return UpdateEcuConfig(gEcuConfigPtr, TRUE); +} + +void SaveEcuAddress(uint16_t address) +{ + gUserConfiguration.ecuAddress = address; + SaveUserConfig(); +} + + +/** *************************************************************************** + * @name is_valid_config_command() check incoming SET packets are valid or invalid + * @brief a general API of checking supported SET commands + * + * @param [in] ident, identifier message + * + * @retval ACEINNA_J1939_INVALID_IDENTIFIER, ACEINNA_J1939_CONFIG or ACEINNA_J1939_IGNORE + ******************************************************************************/ +ACEINNA_J1939_PACKET_TYPE is_valid_config_command(SAE_J1939_IDENTIFIER_FIELD *ident) +{ + uint8_t pf_val, ps_val; + + if (ident == NULL) + return ACEINNA_J1939_INVALID_IDENTIFIER; + + pf_val = ident->pdu_format; + ps_val = ident->pdu_specific; + + if ( + (pf_val == SAE_J1939_PDU_FORMAT_GLOBAL) && + ( + ((ps_val >= SAE_J1939_GROUP_EXTENSION_ALGORITHM_RESET) && (ps_val <= SAE_J1939_GROUP_EXTENSION_ACCELERATION_PARAM)) || + (ps_val == SAE_J1939_GROUP_EXTENSION_BANK0) || + (ps_val == SAE_J1939_GROUP_EXTENSION_BANK1) || + (ps_val == gEcuConfig.alg_reset_ps) || + (ps_val == gEcuConfig.status_ps) || + (ps_val == gEcuConfig.packet_rate_ps) || + (ps_val == gEcuConfig.packet_type_ps) || + (ps_val == gEcuConfig.digital_filter_ps) || + (ps_val == gEcuConfig.mag_align_ps) || + (ps_val == gEcuConfig.orientation_ps)) + ) { + return ACEINNA_J1939_CONFIG; + } + + return ACEINNA_J1939_IGNORE; +} + +uint8_t GetEcuAddress() +{ + return gUserConfiguration.ecuAddress; +} + +int GetEcuBaudRate() +{ + return gUserConfiguration.ecuBaudRate; +} + +void SetEcuBaudRate(_ECU_BAUD_RATE rate ) +{ + gUserConfiguration.ecuBaudRate = rate; +} + +void SetEcuPacketType(uint16_t type) +{ + gUserConfiguration.ecuPacketType = type; +} + +void SetEcuPacketRate(uint16_t rate) +{ + gUserConfiguration.ecuPacketRate = rate; +} + +void SetEcuFilterFreq(EcuConfigurationStruct *pEcuConfig) +{ + platformSelectLPFilter(RATE_SENSOR, pEcuConfig->rate_cut_off, TRUE); + platformSelectLPFilter(ACCEL_SENSOR, pEcuConfig->accel_cut_off, TRUE); + gUserConfiguration.ecuFilterFreqAccel = pEcuConfig->accel_cut_off; + gUserConfiguration.ecuFilterFreqRate = pEcuConfig->rate_cut_off; +} + +void SetEcuOrientation(uint16_t orien_bits) +{ + if(platformApplyOrientation(orien_bits)){ + gUserConfiguration.ecuOrientation = orien_bits; + } +} + + +void ApplyEcuSettings(void *pConfig) +{ + EcuConfigurationStruct *pEcuConfig = (EcuConfigurationStruct *)pConfig; + + // Add/Remove/Verify ECU-specific parameters here + pEcuConfig->address = gUserConfiguration.ecuAddress; + pEcuConfig->baudRate = gUserConfiguration.ecuBaudRate; + pEcuConfig->packet_rate = gUserConfiguration.ecuPacketRate; + pEcuConfig->accel_cut_off = gUserConfiguration.ecuFilterFreqAccel; + pEcuConfig->rate_cut_off = gUserConfiguration.ecuFilterFreqRate; + pEcuConfig->packet_type = gUserConfiguration.ecuPacketType; + pEcuConfig->orien_bits = gUserConfiguration.ecuOrientation; + + // Add/Remove/Verify payload specific codes here + // If Payload-specific code here is zero - it will be substituted by predefined code + // in function initialize_j1939_config(uint16_t baudRate, uint8_t address) + + pEcuConfig->alg_reset_ps = gUserConfiguration.algResetPs; + pEcuConfig->save_cfg_ps = gUserConfiguration.saveCfgPs; + pEcuConfig->packet_type_ps = gUserConfiguration.packetTypePs; + pEcuConfig->packet_rate_ps = gUserConfiguration.packetRatePs; + pEcuConfig->digital_filter_ps = gUserConfiguration.filterPs; + pEcuConfig->orientation_ps = gUserConfiguration.orientationPs; + pEcuConfig->user_behavior_ps = gUserConfiguration.userBehvPs; + pEcuConfig->status_ps = gUserConfiguration.statusPs; + pEcuConfig->mag_align_ps = gUserConfiguration.magAlignPs; + +} + + +BOOL CanTermResistorEnabled() +{ + return gUserConfiguration.canTermResistorEnabled != 0; +} + +BOOL CanBaudRateDetectionEnabled() +{ + return gUserConfiguration.canBaudRateDetectEnabled != 0; +} + +BOOL UseAlgorithm() +{ + return (gUserConfiguration.userBehavior & USER_BEHAVIOR_RUN_ALGORITHM_MASK) != 0? TRUE : FALSE; +} + +BOOL UseMags() +{ + return (gUserConfiguration.userBehavior & USER_BEHAVIOR_USE_MAGS_MASK) != 0? TRUE : FALSE; +} + +BOOL GnssInputDataOverCan() +{ + return (gUserConfiguration.userBehavior & USER_BEHAVIOR_USE_GNSS_OVER_CAN) != 0? TRUE : FALSE; +} + + +void ApplySystemParameters(void *pConfig) +{ + EcuConfigurationStruct *pEcuConfig = (EcuConfigurationStruct *)pConfig; + + platformSelectLPFilter(RATE_SENSOR, pEcuConfig->rate_cut_off, TRUE); + platformSelectLPFilter(ACCEL_SENSOR, pEcuConfig->accel_cut_off, TRUE); + platformApplyOrientation(pEcuConfig->orien_bits); + if(UseMags()){ + platformForceMagUsage(); + } + userInitConfigureUart(); +} + diff --git a/examples/OpenIMU300RI/INS/src/user/UserConfiguration.h b/examples/OpenIMU300RI/INS/src/user/UserConfiguration.h new file mode 100644 index 0000000..56506fb --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/user/UserConfiguration.h @@ -0,0 +1,209 @@ +/******************************************************************************* + * File: UserConfiguration.h + * Created on JAn 25, 2017 + ******************************************************************************/ +/******************************************************************************* +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 USER_CONFIGURATION_H +#define USER_CONFIGURATION_H + +#include + +#include "GlobalConstants.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 + +typedef struct { + uint64_t dataCRC; // CRC of user configuration structure CRC-16 + uint64_t dataSize; // Size of the user configuration structure + + uint16_t ecuAddress; // ecu address + uint16_t ecuBaudRate; // ecu baudrate - 250, 500, 1000 + uint16_t ecuPacketRate; // divider for 200Hz tick + uint16_t ecuFilterFreqAccel; // accels cutoff frequency for digital filter + uint16_t ecuFilterFreqRate; // rates cutoff frequency for digital filter + uint16_t ecuPacketType; // packet types being transmitted + uint16_t canTermResistorEnabled; // enable/disable CAN termination resistor + uint16_t canBaudRateDetectEnabled; // enable/disable CAN baud rate auto detection + uint16_t ecuOrientation; // unit orientation + //0 +Z +Y +X + //9 +Z -Y -X + // 35 +Z +X +Y + // 42 +Z -X +Y + // 65 -Z +Y -X + // 72 -Z -Y +X + // 98 -Z +X +Y + // 107 -Z -X -Y + // 133 +X +Y -Z + // 140 +X -Y +Z + // 146 +X +Z +Y + // 155 +X -Z -Y + // 196 -X +Y +Z + // 205 -X -Y -Z + // 211 -X +Z -Y + // 218 -X -Z +Y + // 273 +Y +Z -X + // 280 +Y -Z +X + // 292 +Y +X +Z + // 301 +Y -X -Z + // 336 -Y +Z +X + // 345 -Y -Z -X + // 357 -Y +X -Z + // 364 -Y -X +Z + uint16_t userBehavior; // user algorithm behaviour + + uint16_t statusPs; // ps value of status message + uint16_t algResetPs; // ps value of alg reset + uint16_t saveCfgPs; // ps value of config save + uint16_t packetRatePs; // ps value of packet type + uint16_t packetTypePs; // ps value of packet type + uint16_t filterPs; // ps value of lpf + uint16_t orientationPs; // ps value of orientation + uint16_t userBehvPs; // ps value of user behavior + uint16_t magAlignPs; // ps value of mag align command + int64_t gpsBaudRate; /// baudrate of GPS UART, bps. + /// valid options are: + /// 4800 + /// 9600 + /// 19200 + /// 38400 + /// 57600 + /// 115200 + /// 230400 + int64_t gpsProtocol; /// protocol of GPS receicer. + /// so far valid options are: + /// NMEA_TEXT + /// NOVATEL_BINARY + double hardIron_X; + double hardIron_Y; + double softIron_Ratio; + double softIron_Angle; + double leverArmBx; + double leverArmBy; + double leverArmBz; + double pointOfInterestBx; + double pointOfInterestBy; + double pointOfInterestBz; + +} UserConfigurationStruct; + +/// User defined configuration strucrture +///Please notice, that parameters are 64 bit to accomodate double types as well as longer string types + +typedef struct { + uint64_t dataCRC; /// CRC of user configuration structure CRC-16 + uint64_t dataSize; /// Size of the user configuration structure + + int64_t userUartBaudRate; /// baudrate of user UART, bps. + /// valid options are: + /// 4800 + /// 9600 + /// 19200 + /// 38400 + /// 57600 + /// 115200 + /// 230400 + /// 460800 + uint8_t userPacketType[8]; /// User packet to be continiously sent by unit + /// Packet types defined in structure UserOutPacketType + /// in file UserMessaging.h + + int64_t userPacketRate; /// Packet rate for continiously output packet, Hz. + /// Valid settings are: 0 ,2, 5, 10, 20, 25, 50, 100, 200 + + int64_t lpfAccelFilterFreq; /// built-in lpf filter cutoff frequency selection for accelerometers + int64_t lpfRateFilterFreq; /// built-in lpf filter cutoff frequency selection for rate sensors + /// Options are: + /// 0 - Filter turned off + /// 50 - Butterworth LPF 50HZ + /// 20 - Butterworth LPF 20HZ + /// 10 - Butterworth LPF 10HZ + /// 05 - Butterworth LPF 5HZ + /// 02 - Butterworth LPF 2HZ + /// 25 - Butterworth LPF 25HZ + /// 40 - Butterworth LPF 40HZ + + uint8_t orientation[8]; /// unit orientation in format 0x0000000000ddrrff + /// where dd - down axis, rr - right axis, ff - forward axis + /// next axis values a valid : + /// 'X' (0x58) -> plus X, 'Y' (0x59) -> plus Y, 'Z' (0x5a) -> plus Z + /// 'x' (0x78) -> minus X, 'y' (0x79) -> minus Y, 'z' (0x7a) ->minusZ + + //*************************************************************************************** + // here is the border between arbitrary parameters and platform configuration parameters + //*************************************************************************************** + + // place new arbitrary configuration parameters here + // parameter size should even to 8 bytes + // Add parameter offset in UserConfigParamOffset structure if validation or + // special processing required + +} UserConfigurationUartStruct; + +enum{ +//***************************************************************************************** +// These parateters are not saved into eeprom as of yet + USER_UART_CRC = 0, + USER_UART_CONFIG_SIZE , // 1 + USER_UART_BAUD_RATE , // 2 + USER_UART_PACKET_TYPE , // 3 + USER_UART_PACKET_RATE , // 4 + USER_UART_LPF_ACCEL_TYPE , // 5 prefered LPF filter type for accelerometer + USER_UART_LPF_RATE_TYPE , // 6 prefered LPF filter type for rate sensor + USER_UART_ORIENTATION , // 7 unit orientation + USER_UART_MAX_PARAM // 8 +}; + + +#define MAX_SYSTEM_PARAM USER_ORIENTATION + +extern int userPacketOut; + +#define USER_OK 0x00 +#define USER_NAK 0x80 +#define USER_INVALID 0x81 + +#define INVALID_PARAM -1 +#define INVALID_VALUE -2 +#define INVALID_PAYLOAD_SIZE -3 + +#define USER_BEHAVIOR_DUMMY_MASK 0x0000 // to conveniently select default behavior +#define USER_BEHAVIOR_RUN_ALGORITHM_MASK 0x0001 +#define USER_BEHAVIOR_USE_MAGS_MASK 0x0002 +#define USER_BEHAVIOR_USE_GNSS_OVER_CAN 0x0004 +// define additional algorithm flavours here + +extern UserConfigurationStruct gUserConfiguration; + +extern void InitializeUserAlgorithmParams(void); +extern BOOL validateUserConfigInEeprom(int *numParams); +extern uint32_t getUserParamFromEeprom(uint32_t offset); +extern BOOL saveUserConfigInEeprom(uint8_t *ptrToUserConfigStruct, int userConfigStructLen); +extern BOOL checkIfUserEEPROMErased(void); +extern BOOL loadUserConfigFromEeprom(uint8_t *ptrToUserConfigInRam, int *userConfigSize); +extern BOOL SaveUserConfig(void); +extern BOOL LoadDefaultUserConfig(BOOL fSave); +extern void ApplyEcuSettings(void* pEcuConfig); +extern void ApplySystemParameters(void* pEcuConfig); + + + +#endif /* USER_CONFIGURATION_H */ + + diff --git a/examples/OpenIMU300RI/INS/src/user/UserConfigurationUart.c b/examples/OpenIMU300RI/INS/src/user/UserConfigurationUart.c new file mode 100644 index 0000000..b564b3f --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/user/UserConfigurationUart.c @@ -0,0 +1,364 @@ +/** *************************************************************************** + * @file UserConfiguration.c + * + * 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. + * + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#include "string.h" +#include "magAPI.h" +#include "platformAPI.h" +#include "UserConfiguration.h" +#include "UserMessagingUART.h" +#include "Indices.h" +#include "eepromAPI.h" + +// Default user configuration structure +// Not save4d to EEPROM so far +// Do Not remove - just add extra parameters if needed +// Change default settings if desired +UserConfigurationUartStruct gUserUartConfig = { + .dataCRC = 0, + .dataSize = sizeof(UserConfigurationUartStruct), + .userUartBaudRate = 115200, // needs to be changed to 230400 for e1 and e2 packets at 100 Hz + .userPacketType = "z1", + .userPacketRate = 0, + .lpfAccelFilterFreq = 25, + .lpfRateFilterFreq = 25, + .orientation = "+X+Y+Z", + // add default parameter values here, if desired +}; + +uint8_t UserDataBuffer[4096]; +static volatile char *info; + + + +/** *************************************************************************** + * @name UpdateSystemParameter - updating of system configuration parameter based of user preferences + * @brief + * + * @param [in] number - parameter number in user configuration structure + * @param [in] data - value of the parameter in little endian format + * @retval error (1), no error (0) + ******************************************************************************/ +// NEEDS TO BE CHECKED +BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) +{ + BOOL result = TRUE; + uint64_t *ptr = (uint64_t *)&gUserUartConfig; + + if(number < USER_UART_CRC || number >= USER_UART_MAX_PARAM ){ + return FALSE; + } + + switch (number) { + case USER_UART_BAUD_RATE: + result = platformSetBaudRate((int)data, fApply); + break; + case USER_UART_PACKET_TYPE: + result = setUserPacketType((uint8_t*)&data, fApply); + break; + case USER_UART_PACKET_RATE: + result = platformSetPacketRate((int)data, fApply); + break; + case USER_UART_CRC: + case USER_UART_CONFIG_SIZE: + return TRUE; + + // case USER_XXX: add function calls here if parameter XXXX + // required be updated on the fly + // break; + default: + // by default result should be FALSE for system parameter + result = FALSE; + break; + } + + if(result == TRUE){ + ptr[number] = data; + } + + return result; +} + + +void userInitConfigureUart() +{ + + uint64_t *ptr = (uint64_t*)&gUserUartConfig; + + // apply parameters to the platform + for(int i = USER_UART_BAUD_RATE; i < USER_UART_MAX_PARAM; i++){ + UpdateSystemParameter(i, ptr[i], TRUE); + } + + info = getBuildInfo(); +} + + + +/** **************************************************************************** + * @name UpdateUserConfig + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL UpdateUserConfig(userConfigPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i; + BOOL offsetValid = TRUE; + BOOL lenValid = TRUE; + BOOL numValid = TRUE; + BOOL ret = TRUE; + int32_t result = 0; + + // Validate parameters numbers and quantity + if(pld->numParams >= USER_UART_MAX_PARAM){ + lenValid = FALSE; + result = INVALID_PAYLOAD_SIZE; + } + + + if((pld->numParams + pld->paramOffset) >= USER_UART_MAX_PARAM){ + numValid = FALSE; + result = INVALID_PARAM; + } + + if(offsetValid && numValid && lenValid){ + // Validate parameters values first + offset = pld->paramOffset; + for (i = 0; i < pld->numParams && ret; i++){ + ret = UpdateSystemParameter(offset, pld->parameters[i], FALSE); + offset++; + } + if(ret == TRUE){ + // Apply parameters values here + offset = pld->paramOffset; + for (i = 0; i < pld->numParams; i++){ + ret = UpdateSystemParameter(offset, pld->parameters[i], TRUE); + offset++; + } + } + } + + if(ret == FALSE){ + result = INVALID_VALUE; + } + + pld->numParams = result; + *payloadLen = 4; + + return TRUE; +} + + +/** **************************************************************************** + * @name UpdateUserParam + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL UpdateUserParam(userParamPayload* pld, uint8_t *payloadLen) +{ + BOOL offsetValid; + BOOL ret = TRUE; + int32_t result = 0; + + offsetValid = pld->paramNum <= USER_UART_MAX_PARAM; + + if(offsetValid){ + // Validate parameter first + ret = UpdateSystemParameter(pld->paramNum, pld->parameter, FALSE); + if(ret == TRUE){ + // Apply parameter if valid + ret = UpdateSystemParameter(pld->paramNum, pld->parameter, TRUE); + }else{ + result = INVALID_VALUE; + } + } else { + result = INVALID_PARAM; + } + + pld->paramNum = result; + *payloadLen = 4; + + return TRUE; +} + + +/** **************************************************************************** + * @name UpdateAllUserParams + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +/** **************************************************************************** + * @name UpdateUserConfig + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i; + BOOL numValid = TRUE; + BOOL ret = TRUE; + int32_t result = 0; + + int numParams = (*payloadLen)/8; + + if(numParams > USER_UART_MAX_PARAM){ + numValid = FALSE; + result = INVALID_PAYLOAD_SIZE; + } + + + if(numValid){ + // Validate parameters here + offset = 0; + for (i = 0; i < numParams && ret; i++){ + ret = UpdateSystemParameter(offset, pld->parameters[i], FALSE); + offset++; + } + if(ret == TRUE){ + // Apply parameters here + offset = 0; + for (i = 0; i < numParams; i++){ + UpdateSystemParameter(offset, pld->parameters[i], TRUE); + offset++; + } + } + } + + if(ret == FALSE){ + result = INVALID_VALUE; + } + + pld->parameters[0] = result; + *payloadLen = 4; // return error code + + return TRUE; + +} + + +/** **************************************************************************** + * @name GetUserConfig + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i; + BOOL lenValid = TRUE; + uint64_t *ptr = (uint64_t *)&gUserUartConfig; + + lenValid = (pld->numParams + pld->paramOffset) <= USER_UART_MAX_PARAM; + + if(lenValid){ + offset = pld->paramOffset; + for (i = 0; i < pld->numParams; i++, offset++) + { + pld->parameters[i] = ptr[offset]; + } + *payloadLen = (pld->numParams + 1) * 8; + } else { + *payloadLen = 4; + pld->numParams = INVALID_PARAM; + } + + return TRUE; + +} + + +/** **************************************************************************** + * @name GetUserParam + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset; + BOOL offsetValid; + uint64_t *ptr = (uint64_t *)&gUserUartConfig; + + offsetValid = pld->paramNum < USER_UART_MAX_PARAM; + + if(offsetValid){ + offset = pld->paramNum; + pld->parameter = ptr[offset]; + *payloadLen = 8 + 4; // parameter + number + } else { + *payloadLen = 4; // number + pld->paramNum = INVALID_PARAM; // invalid + } + + return TRUE; + +} + + +/** **************************************************************************** + * @name GetAllUserParams + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, numParams; + uint64_t *ptr = (uint64_t *)&gUserUartConfig; + + numParams = USER_UART_MAX_PARAM; + + offset = 0; + + for (i = 0; i < numParams; i++, offset++){ + pld->parameters[i] = ptr[offset]; + } + + *payloadLen = numParams* 8; + + return TRUE; +} + + diff --git a/examples/OpenIMU300RI/INS/src/user/UserMessagingCAN.c b/examples/OpenIMU300RI/INS/src/user/UserMessagingCAN.c new file mode 100644 index 0000000..82183dd --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/user/UserMessagingCAN.c @@ -0,0 +1,659 @@ +/** *************************************************************************** + * @file UserConfiguration.c + * + * 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. + * + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#include +#include +#include + +#include "algorithmAPI.h" +#include "magAPI.h" +#include "platformAPI.h" +#include "sensorsAPI.h" +#include "userAPI.h" +#include "UserMessagingCAN.h" +#include "UserConfiguration.h" +#include "canAPI.h" +#include "MagAlign.h" +#include "algorithm.h" +#include "BITStatus.h" +#include "sensorsAPI.h" +#include "sae_j1939.h" +#include "math.h" +#include "osapi.h" +#include "ins_params_j1939.h" +#include "driverGPS.h" + +// for EKFOutputDataStruct +#include "EKF_Algorithm.h" +EKF_OutputDataStruct *algo_res; +extern int initAlgo; +uint32_t forced_packets = 0, gpsFixTypeTmp; +j1939_input_nav_params_t canInputNavParams; + +/** *************************************************************************** + * @name process_request() an API of processing request message + * @brief decode identifier of incoming request message + * + * @param [in] desc, rx descriptor + * @retval N/A + ******************************************************************************/ +void ProcessRequest(void *dsc) +{ + struct sae_j1939_rx_desc *desc = (struct sae_j1939_rx_desc*)dsc; + SAE_J1939_IDENTIFIER_FIELD *ident; + uint8_t pf_val, req_pf_val, req_ps_val; + uint8_t *command; + + // check desc + if (desc == NULL) + return; + + + // check identifier + ident = &(desc->rx_identifier); + if (ident == NULL) + return; + + // check receiving buffer + if (desc->rx_buffer.RTR || !desc->rx_buffer.IDE || (desc->rx_buffer.DLC != 3)) + return; + + // check commands + command = desc->rx_buffer.Data; + if (command == NULL) + return; + + pf_val = ident->pdu_format; + req_pf_val = command[1]; + req_ps_val = command[2]; + + if (pf_val != SAE_J1939_PDU_FORMAT_REQUEST) + return; + + // dispatch the requests to the corresponding handlers + switch(req_pf_val) { + // software version request + case SAE_J1939_PDU_FORMAT_254: + { + if(req_ps_val == SAE_J1939_GROUP_EXTENSION_SOFTWARE_VERSION){ + aceinna_j1939_send_software_version(); + } + } + break; + // ecu id request + case SAE_J1939_PDU_FORMAT_ECU: + { + aceinna_j1939_send_ecu_id(); + } + break; + // data packet request + case SAE_J1939_PDU_FORMAT_DATA: + forced_packets = 0; + if(req_ps_val == SAE_J1939_GROUP_EXTENSION_SLOPE_SENSOR) + { + forced_packets |= ACEINNA_SAE_J1939_PACKET_SLOPE_SENSOR; + } + if(req_ps_val == SAE_J1939_GROUP_EXTENSION_ANGULAR_RATE) + { + forced_packets |= ACEINNA_SAE_J1939_PACKET_ANGULAR_RATE; + } + if(req_ps_val == SAE_J1939_GROUP_EXTENSION_ACCELERATION) + { + forced_packets |= ACEINNA_SAE_J1939_PACKET_ACCELERATION; + } + if(req_ps_val == SAE_J1939_GROUP_EXTENSION_MAG) + { + forced_packets |= ACEINNA_SAE_J1939_PACKET_MAG; + } + break; + + case SAE_J1939_PDU_FORMAT_GLOBAL: + // status request + if ((req_ps_val == gEcuConfigPtr->status_ps)) { + STATUS_TEST_PAYLOAD status_bits; + status_bits.masterStatus = (gBitStatus.BITStatus.all & 0x1E00) ? 1:0; + status_bits.hardwareStatus = gBitStatus.hwStatus.all? 1 : 0; + status_bits.softwareStatus = gBitStatus.swStatus.all? 1 : 0; + status_bits.sensorStatus = gBitStatus.sensorStatus.all? 1 : 0; + status_bits.unlocked1PPS = gBitStatus.hwStatus.bit.unlocked1PPS; + status_bits.unlockedInternalGPS = gBitStatus.hwStatus.bit.unlockedInternalGPS; + status_bits.noDGPS = gBitStatus.hwStatus.bit.unlockedInternalGPS; + status_bits.unlockedEEPROM = gBitStatus.hwStatus.bit.unlockedEEPROM; + status_bits.algorithmInit = gBitStatus.swStatus.bit.algorithmInit; + status_bits.highGain = gBitStatus.swStatus.bit.highGain; + status_bits.attitudeOnlyAlgorithm = gBitStatus.swStatus.bit.attitudeOnlyAlgorithm; + status_bits.turnSwitch = gBitStatus.swStatus.bit.turnSwitch; + status_bits.sensorOverRange = gBitStatus.sensorStatus.bit.overRange; + aceinna_j1939_send_status_packet(ACEINNA_SAE_J1939_BUILTIN_STATUS, (void *)&status_bits); + } + // packet rate request + else if ((req_ps_val == gEcuConfigPtr->packet_rate_ps)) { + aceinna_j1939_send_packet_rate(gEcuConfigPtr->packet_rate); + } + // packet type request + else if ((req_ps_val == gEcuConfigPtr->packet_type_ps)) { + aceinna_j1939_send_packet_type(gEcuConfigPtr->packet_type); + } + // filter settings request + else if ((req_ps_val == gEcuConfigPtr->digital_filter_ps)) { + aceinna_j1939_send_digital_filter(gEcuConfigPtr->accel_cut_off, gEcuConfigPtr->rate_cut_off); + } + // orientation settings request + else if ((req_ps_val == gEcuConfigPtr->orien_bits)) { + uint8_t bytes[2]; + bytes[0] = (gEcuConfigPtr->orien_bits >> 8) & 0xff; + bytes[1] = (gEcuConfigPtr->orien_bits) & 0xff; + aceinna_j1939_send_orientation(bytes); + } + + break; + // address claim request + case SAE_J1939_PDU_FORMAT_ADDRESS_CLAIM: + process_request_pg(desc); + break; + default: + break; + } + + return; +} + +void ProcessVehiclePosition(struct sae_j1939_rx_desc *desc) +{ + GPS_DATA *posData = (GPS_DATA*)desc->rx_buffer.Data; + canInputNavParams.latitude = (double)posData->latitude*0.000001 - 210; // degrees + canInputNavParams.longitude = (double)posData->longitude*0.000001 - 210; // degrees + canInputNavParams.status |= J1939_GPS_POSITION_DATA_UPDATED; + gCanGps.latitude = (double)posData->latitude*0.000001 - 210; // degrees + gCanGps.longitude = (double)posData->longitude*0.000001 - 210; // degrees +} + +void ProcessWheelSpeed(struct sae_j1939_rx_desc *desc) +{ + WHEEL_SPEED_DATA *speedData = (WHEEL_SPEED_DATA*)desc->rx_buffer.Data; + + canInputNavParams.frontAxleSpeed = (float)speedData->frontAxleSpeed * 0.00390625; + canInputNavParams.relSpeedFrontAxleLeftWheel = (float)speedData->relSpeedFrontAxleLeftWheel * 0.0625; + canInputNavParams.relSpeedFrontAxleRightWheel = (float)speedData->relSpeedFrontAxleRightWheel * 0.0625; + canInputNavParams.relSpeedRearAxle1LeftWheel = (float)speedData->relSpeedRearAxle1LeftWheel * 0.0625; + canInputNavParams.relSpeedRearAxle1RigthWheel = (float)speedData->relSpeedRearAxle1RigthWheel * 0.0625; + canInputNavParams.relSpeedRearAxle2LeftWheel = (float)speedData->relSpeedRearAxle2LeftWheel * 0.0625; + canInputNavParams.relSpeedRearAxle2RigthWheel = (float)speedData->relSpeedRearAxle2RigthWheel * 0.0625; + canInputNavParams.status |= J1939_VEH_SPEED_DATA_UPDATED; +} + +void ProcesVehicleDirection(struct sae_j1939_rx_desc *desc) +{ + DIR_SPEED_DATA* dirData = (DIR_SPEED_DATA*)desc->rx_buffer.Data; + + canInputNavParams.gpsBearing = (float)dirData->compassBearing * 0.0078125; // + canInputNavParams.navSpeed = (float)dirData->navSpeed * 0.00390625; // kph + canInputNavParams.pitch = (float)dirData->pitch * 0.0078125 - 200; // + canInputNavParams.altitude = (float)dirData->altitude * 0.125 - 2500; // + canInputNavParams.status |= J1939_GPS_ALTITUDE_DATA_UPDATED; +} + +void ProcessCustomGnssDopPacket(struct sae_j1939_rx_desc *desc) +{ + CUSTOM_GNSS_DOP_DATA* dopData = (CUSTOM_GNSS_DOP_DATA*)desc->rx_buffer.Data; + + gCanGps.GPSHorizAcc = (float)dopData->hAcc; // in meters + gCanGps.GPSVertAcc = (float)dopData->vAcc; // in meters + gCanGps.HDOP = ((float)dopData->pDOP * 0.01)*0.7; + gCanGps.trueCourse = (float)dopData->headMot * 0.01; +} + +void ProcessCustomGnssFixPacket(struct sae_j1939_rx_desc *desc) +{ + CUSTOM_GNSS_FIX_DATA* fixData = (CUSTOM_GNSS_FIX_DATA*)desc->rx_buffer.Data; + + gpsFixTypeTmp = (fixData->fixType == 3) || (fixData->fixType == 4) ; + gCanGps.altitude = (float)fixData->height * 1.0e-3; // in m + gCanGps.numSatellites = fixData->numSV; +} + +void ProcessCustomGnssTimePacket(struct sae_j1939_rx_desc *desc) +{ + CUSTOM_GNSS_TIME_DATA* dopData = (CUSTOM_GNSS_TIME_DATA*)desc->rx_buffer.Data; + + gCanGps.itow = dopData->iTOW; // in m + + if(gCanGps.itow%1000 != 0){ + gCanGps.itow+=1; // correction of numerical issue + } + gCanGps.rawGroundSpeed = (double)dopData->gSpeed * 1.0e-3; // m/s + gCanGps.vNed[0] = gCanGps.rawGroundSpeed * cos(D2R * gCanGps.trueCourse); + gCanGps.vNed[1] = gCanGps.rawGroundSpeed * sin(D2R * gCanGps.trueCourse); + + gCanGps.gpsFixType = gpsFixTypeTmp; + gpsFixTypeTmp = 0; + // update flag + gCanGps.gpsUpdate = 1; +} + + +/** *************************************************************************** + * @name ProcessDataPackets() an API of processing network data messages + * @brief decode identifier of incoming request message + * + * @param [in] desc, rx descriptor + * @retval N/A + ******************************************************************************/ +void ProcessDataPackets(void *dsc) +{ + struct sae_j1939_rx_desc *desc = (struct sae_j1939_rx_desc*)dsc; + SAE_J1939_IDENTIFIER_FIELD *ident; + uint8_t pf_val, ps_val; + + // check desc + if (desc == NULL) + return; + + + // check identifier + ident = (SAE_J1939_IDENTIFIER_FIELD*)&(desc->rx_identifier); + if (ident == NULL) + return; + + + pf_val = ident->pdu_format; + ps_val = ident->pdu_specific; + + if (pf_val == SAE_J1939_PDU_FORMAT_254){ + + // dispatch the requests to the corresponding handlers + switch(ps_val) { + // software version request + case SAE_J1939_PDU_SPECIFIC_243: // 65267 Vehicle Position 1 FEF3 + ProcessVehiclePosition(desc); + break; + case SAE_J1939_PDU_SPECIFIC_191: // 65215 Wheel Speed Information FEBF + ProcessWheelSpeed(desc); + break; + case SAE_J1939_PDU_SPECIFIC_232: // 65256 Vehicle Direction/Speed FEE8 + ProcesVehicleDirection(desc); + break; + default: + break; + } + } else if(pf_val == SAE_J1939_PDU_FORMAT_GLOBAL){ + switch(ps_val) { + // software version request + case SAE_J1939_PDU_SPECIFIC_110: // 65390 Vehicle Position 1 FEF3 + ProcessCustomGnssTimePacket(desc); + break; + case SAE_J1939_PDU_SPECIFIC_111: // 65391 Wheel Speed Information FEBF + ProcessCustomGnssFixPacket(desc); + break; + case SAE_J1939_PDU_SPECIFIC_112: // 65392 Vehicle Direction/Speed FEE8 + ProcessCustomGnssDopPacket(desc); + break; + } + } + + return; +} + + +/** *************************************************************************** + * @name send_tilt_sensor_data() an API of data packet generation + * @brief build up MTLT's data packets and send out + * + * @param [in] + * @retval N/A + ******************************************************************************/ +static int outputCounter = 0; + +void EnqeuePeriodicDataPackets(int latency, int sendPeriodicPackets) +{ + SLOPE_SENSOR_2 slope_data; + AUGULAR_RATE angle_data; + ACCELERATION_SENSOR accel_data; + MAGNETIC_SENSOR mag_data; + int packets_to_send = 0; + float rate[3]; + int i; + static uint8_t SID = 0; + + outputCounter++; + SID++; + + if (sendPeriodicPackets && gEcuConfig.packet_rate != 0){ + packets_to_send = gEcuConfigPtr->packet_type; + } + + packets_to_send |= forced_packets; + + // ss2 packets supported by 9DOF + if (packets_to_send & ACEINNA_SAE_J1939_PACKET_SLOPE_SENSOR) { + slope_data.pitch = (uint32_t)((gKalmanFilter.eulerAngles[PITCH] * 57.296 + 250.00) * 32768); + slope_data.roll = (uint32_t)((gKalmanFilter.eulerAngles[ROLL] * 57.296 + 250.00) * 32768); + slope_data.pitch_compensation = 0; + slope_data.pitch_merit = 3; + slope_data.roll_compensation = 0; + slope_data.roll_merit = 3; + slope_data.measure_latency = latency/5; + + aceinna_j1939_send_slope_sensor(&slope_data); + } + + // ss2 packets supported by 9DOF + if (packets_to_send & ACEINNA_SAE_J1939_PACKET_LATLONG) { + GPS_DATA gpsData; + +#ifdef GPS + GetGPSData(&gGPS); +#endif + + double lat = gGPS.latitude; // +- 90 degrees + double lon = gGPS.longitude; // +- 180 degrees + + gpsData.latitude = (int32_t)((lat + 210.0) * 10000000); + gpsData.longitude = (int32_t)((lon + 210.0) * 10000000); + + aceinna_j1939_send_GPS(&gpsData); + } + + + // acceleration packets + if (packets_to_send & ACEINNA_SAE_J1939_PACKET_ACCELERATION) { + double accelData[3]; + GetAccelData_mPerSecSq(accelData); + accel_data.acceleration_x = (uint16_t)(((accelData[0]) + 320.00) * 100); + accel_data.acceleration_y = (uint16_t)(((accelData[1]) + 320.00) * 100); + accel_data.acceleration_z = (uint16_t)(((accelData[2]) + 320.00) * 100); + + accel_data.lateral_merit = 3; + accel_data.longitudinal_merit = 3; + accel_data.vertical_merit = 3; + accel_data.transmit_rate = 2; + accel_data.rsvd = 0; + + aceinna_j1939_send_acceleration(&accel_data); + } + + if (packets_to_send & ACEINNA_SAE_J1939_PACKET_MAG) { + double magData[3]; + GetMagData_G(magData); + mag_data.mag_x = (uint16_t)(((magData[0]) + 8.00)/0.00025); + mag_data.mag_y = (uint16_t)(((magData[1]) + 8.00)/0.00025); + mag_data.mag_z = (uint16_t)(((magData[2]) + 8.00)/0.00025); + mag_data.unuzed = 0; + aceinna_j1939_send_mags(&mag_data); + } + + if (packets_to_send & ACEINNA_SAE_J1939_PACKET_ANGULAR_RATE) { + if(UseAlgorithm()) { + rate[0] = (float)(gKalmanFilter.correctedRate_B[0] * 57.296); + rate[1] = (float)(gKalmanFilter.correctedRate_B[1] * 57.296); + rate[2] = (float)(gKalmanFilter.correctedRate_B[2] * 57.296); + } + else { + double rateData[3]; + GetRateData_degPerSec(rateData); + rate[0] = (float)(rateData[0]); + rate[1] = (float)(rateData[1]); + rate[2] = (float)(rateData[2]); + } + + for (i = 0; i < 3; i++) + { + if (rate[i] < -250.00) + rate[i] = -250.00; + + if (rate[i] > 250.00) + rate[i] = 250.00; + } + angle_data.pitch_rate = (uint16_t)((rate[0] + 250.00) * 128); + angle_data.roll_rate = (uint16_t)((rate[1] + 250.00) * 128); + angle_data.yaw_rate = (uint16_t)((rate[2] + 250.00) * 128); + angle_data.pitch_merit = 3; + angle_data.roll_merit = 3; + angle_data.yaw_merit = 3; + angle_data.rsvd = 0; + angle_data.measurement_latency = latency/5; + + aceinna_j1939_send_angular_rate(&angle_data); + } + + if (packets_to_send & ACEINNA_SAE_J1939_PACKET_RAPID_POSITION_UPDATE) { + GPS_DATA data; + double lla[3]; + OSDisableHook(); + EKF_GetEstimatedLLA(lla); + OSEnableHook(); + data.latitude = (uint32_t)((lla[LAT] + 210) * 10000000.0); + data.longitude = (uint32_t)((lla[LON] + 210) * 10000000.0); + aceinna_j1939_send_position_rapid_update(&data); + } + + if (packets_to_send & ACEINNA_SAE_J1939_PACKET_RAPID_COURSE_UPDATE) { + COURSE_RAPID_UPDATE_DATA data; + real EstVelocity[3]; + real EulerAngles[3]; + + OSDisableHook(); + EKF_GetEstimatedVelocity(EstVelocity); + EKF_GetAttitude_EA_RAD(EulerAngles); + OSEnableHook(); + + real SOG = sqrt(pow(EstVelocity[X_AXIS],2) + pow(EstVelocity[Y_AXIS],2)) * 100; ; + + data.COG = (int16_t)((EulerAngles[YAW] + 3.141592653589) * 10000); + data.SOG = (int16_t)SOG; + data.SID = SID; + data.Rsvd = 0xffff; + data.Reserved = 0x3f; + + aceinna_j1939_send_course_rapid_update(&data); + } + + if (packets_to_send & ACEINNA_SAE_J1939_PACKET_ATTITUDE) { + ATTITUDE_DATA data; + real EulerAngles[3]; + + OSDisableHook(); + EKF_GetAttitude_EA_RAD(EulerAngles); + OSEnableHook(); + + data.SID = SID; + data.Pitch = (int16_t)((EulerAngles[PITCH] + 3.141592653589) * 10000); + data.Roll = (int16_t)((EulerAngles[ROLL] + 3.141592653589) * 10000); + data.Yaw = (int16_t)((EulerAngles[YAW] + 3.141592653589) * 10000); + data.Rsvd = 0xff; + aceinna_j1939_send_attitude(&data); + } + + + forced_packets = 0; + + return; +} + +/** *************************************************************************** + * @name ProcessEcuCommands +* @brief decode incoming SET packets and put the request into the corresponding + * handlers + * @param [in] command, SET request + * ps, pdu specific value + * addr, host address + * @retval 1 successfuk or 0 failure + ******************************************************************************/ +void ProcessEcuCommands(void * command, uint8_t ps, uint8_t addr) +{ + // SET request is NULL + if (command == NULL) + return; + + // Go to the correponding handlers + if (ps == gEcuConfigPtr->alg_reset_ps) + { + // Algotithm restart command + COMMAND_SET_PAYLOAD * pld = (COMMAND_SET_PAYLOAD *)command; + if (pld->request == ACEINNA_SAE_J1939_REQUEST && pld->dest_address == *(uint8_t *)gEcu->addr) + { + ECU_ADDRESS_ENTRY target; + initAlgo = 1; + target.address = addr; + target.category = _ECU_MASTER; + aceinna_j1939_send_algrst_cfgsave(&target, 1, 1); + } + } + else if ((ps == SAE_J1939_GROUP_EXTENSION_SAVE_CONFIGURATION) || + (ps == gEcuConfigPtr->save_cfg_ps)) + { + // Save configuration command + COMMAND_SET_PAYLOAD * pld = (COMMAND_SET_PAYLOAD *)command; + if (pld->request == ACEINNA_SAE_J1939_REQUEST && pld->dest_address == *(uint8_t *)gEcu->addr){ + ECU_ADDRESS_ENTRY target; + SaveEcuConfig(gEcuConfigPtr); + target.address = addr; + target.category = _ECU_MASTER; + aceinna_j1939_send_algrst_cfgsave(&target, 0, 1); + } + } + else if (ps == gEcuConfigPtr->packet_rate_ps) + { + // Update packet rate command + RATE_CONFIG_PAYLOAD * pld = (RATE_CONFIG_PAYLOAD *)command; + if (pld->dest_address == *(uint8_t *)gEcu->addr) { + gEcuConfigPtr->packet_rate = pld->odr; + gUserConfiguration.ecuPacketRate = pld->odr; + } + } + else if (ps == gEcuConfigPtr->packet_type_ps) + { + // Update set of transmitted packets + PACKET_TYPE_PAYLOAD * pld = (PACKET_TYPE_PAYLOAD *)command; + if(pld->dest_address == *(uint8_t *)gEcu->addr) { + gEcuConfigPtr->packet_type = pld->type1_bits.r | (pld->type2_bits.r << 8); + gUserConfiguration.ecuPacketType = gEcuConfigPtr->packet_type; + } + } + else if (ps == gEcuConfigPtr->digital_filter_ps) + { + // Update cutoff frequencies for digital filters + DIGITAL_FILTER_PAYLOAD * pld = (DIGITAL_FILTER_PAYLOAD *)command; + if (pld->dest_address == *(uint8_t *)gEcu->addr) { + gEcuConfigPtr->rate_cut_off = pld->rate_cutoff; + gEcuConfigPtr->accel_cut_off = pld->accel_cutoff; + gUserConfiguration.ecuFilterFreqAccel = pld->accel_cutoff; + gUserConfiguration.ecuFilterFreqRate = pld->rate_cutoff; + platformSelectLPFilter(RATE_SENSOR, gEcuConfigPtr->rate_cut_off, TRUE); + platformSelectLPFilter(ACCEL_SENSOR, gEcuConfigPtr->accel_cut_off, TRUE); + } + } + else if (ps == gEcuConfigPtr->orientation_ps) + { + // Update unit orientation + ORIENTATION_SETTING * pld = (ORIENTATION_SETTING *)command; + if (pld->dest_address == *(uint8_t *)gEcu->addr) { + gEcuConfigPtr->orien_bits = pld->orien_bits[0] << 8 | pld->orien_bits[1]; + if(platformApplyOrientation(gEcuConfigPtr->orien_bits)){ + gUserConfiguration.ecuOrientation = gEcuConfigPtr->orien_bits; + } + } + } + else if (ps == gEcuConfigPtr->mag_align_ps) + { + // Update unit orientation + MAG_ALIGN_PAYLOAD *pld = (MAG_ALIGN_PAYLOAD *)command; + if (pld->dest_address == *(uint8_t *)gEcu->addr) { + uint8_t len = 1; + uint8_t cmd = pld->cmd[0]; + uint8_t state; + real params[4]; + ProcessMagAlignCmds((magAlignCmdPayload*)&pld->cmd[0], &len); + state = GetMagAlignEstimatedParams(params); + aceinna_j1939_send_mag_align(cmd, state, params); + } + } + else if (ps == gEcuConfigPtr->user_behavior_ps) + { + // Update algotirtm behaviour + USER_BEHAVIOR_PAYLOAD * pld = (USER_BEHAVIOR_PAYLOAD *)command; + if (pld->dest_address == *(uint8_t *)gEcu->addr) { + gEcuConfigPtr->restart_on_overrange = pld->restart_on_overrange; + gEcuConfigPtr->dynamic_motion = pld->dynamic_motion; + } + } + else if (ps == SAE_J1939_GROUP_EXTENSION_BANK0) + { + // Reassign pdu-specific codes + BANK0_PS_PAYLOAD * pld = (BANK0_PS_PAYLOAD *)command; + if (pld->dest_address == *(uint8_t *)gEcu->addr) { + gEcuConfigPtr->alg_reset_ps = pld->alg_reset_ps; + gEcuConfigPtr->status_ps = pld->status_ps; + gEcuConfigPtr->mag_align_ps = pld->mag_align_ps; + UpdateEcuConfig(gEcuConfigPtr, FALSE); + } + } + else if (ps == SAE_J1939_GROUP_EXTENSION_BANK1) + { + // Reassign pdu-specific codes + BANK1_PS_PAYLOAD * pld = (BANK1_PS_PAYLOAD *)command; + if (pld->dest_address == *(uint8_t *)gEcu->addr) { + gEcuConfigPtr->packet_rate_ps = pld->packet_rate_ps; + gEcuConfigPtr->packet_type_ps = pld->packet_type_ps; + gEcuConfigPtr->digital_filter_ps = pld->digital_filter_ps; + gEcuConfigPtr->orientation_ps = pld->orientation_ps; + gEcuConfigPtr->user_behavior_ps = pld->user_behavior_ps; + UpdateEcuConfig(gEcuConfigPtr, FALSE); + } + } + + return; +} + + +// configure CAN controller for selective reception of CAN messages +/** *************************************************************************** + * @name ConfigureCANMessageFilters +* @brief configure CAN controller for selective reception of CAN messages +******************************************************************************/ +void ConfigureCANMessageFilters() +{ + // initialize filter for ECU ID + ConfigureCANMessageFilter(SAE_J1939_ECU_ID_BASE, SAE_J1939_ECU_FILTER_BASE_MASK); + // initialize filter for control messages + ConfigureCANMessageFilter(SAE_J1939_CONTROL1_ID_BASE, SAE_J1939_CONTROL1_FILTER_BASE_MASK); + // initialize filter for requests + ConfigureCANMessageFilter(SAE_J1939_REQUEST_ID_BASE, SAE_J1939_REQUEST_FILTER_BASE_MASK); + // initialize filter for address claim + ConfigureCANMessageFilter(SAE_J1939_ADDRESS_CLAIM_ID_BASE, SAE_J1939_ADDRESS_CLAIM_FILTER_BASE_MASK); + // initialize filter for Bank 1 and Bank 0 commands + ConfigureCANMessageFilter(ACEINNA_BANK_ID_BASE, ACEINNA_BANK_FILTER_BASE_MASK); + // initialize Filter for incoming data messages + ConfigureCANMessageFilter(VEHICLE_DATA_ID_BASE, VEHICLE_DATA_FILTER_BASE_MASK); + // initialize Filter for incoming data1 messages + ConfigureCANMessageFilter(VEHICLE_DATA2_ID_BASE, VEHICLE_DATA2_FILTER_BASE_MASK); +} + + +void GetGPSFromCAN(gpsDataStruct_t *data) +{ + +} + diff --git a/examples/OpenIMU300RI/INS/src/user/UserMessagingCAN.h b/examples/OpenIMU300RI/INS/src/user/UserMessagingCAN.h new file mode 100644 index 0000000..a4e6f40 --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/user/UserMessagingCAN.h @@ -0,0 +1,175 @@ +/******************************************************************************* + * File: UserConfiguration.h + * Created on Jan 25, 2017 + ******************************************************************************/ +/******************************************************************************* +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 USER_MESSAGING_H +#define USER_MESSAGING_H + +#include + +#include "GlobalConstants.h" + + +extern int userPacketOut; + + + +typedef struct { + uint16_t masterFail : 1; // 0 = normal, 1 = fatal error has occurred + uint16_t hardwareError : 1; // 0 = normal, 1 = internal hardware error + uint16_t reserved : 1; + uint16_t softwareError : 1; // 0 = normal, 1 = internal software error + uint16_t inpPower : 1; // 0 = normal, 1 = out of bounds + uint16_t inpCurrent : 1; // 0 = normal, 1 = out of bounds + uint16_t inpVoltage : 1; // 0 = normal, 1 = out of bounds + uint16_t fiveVolt : 1; // 0 = normal, 1 = out of bounds + uint16_t threeVolt : 1; // 0 = normal, 1 = out of bounds + uint16_t twoVolt : 1; // 0 = normal, 1 = out of bounds + uint16_t twoFiveref : 1; // 0 = normal, 1 = out of bounds + uint16_t sixVolt : 1; // 0 = normal, 1 = out of bounds + uint16_t grdRef : 1; // 0 = normal, 1 = out of bounds + uint16_t pcbTemp : 1; // 0 = normal, 1 = out of bounds + uint16_t reserved1 : 2; +} HARDWARE_TEST_PAYLOAD; + +typedef struct { + uint16_t softwareError : 1; // 0 = normal, 1 = internal software error + uint16_t algorithmerror : 1; // 0 = normal, 1 = error + uint16_t dataError : 1; // 0 = normal, 1 = error + uint16_t initialization : 1; // 0 = normal, 1 = error during algorithm initialization + uint16_t overRange : 1; // 0 = normal, 1 = fatal sensor over range + uint16_t missedNavigationStep : 1; // 0 = normal, 1 = deadline missed for navigation + uint16_t calibrationCRCError : 1; // 0 = normal, 1 = incorrect CRC on calibration EEPROM data + uint16_t reserved : 9; +} SOFTWARE_TEST_PAYLOAD; + +typedef struct { + uint16_t masterStatus : 1; // 0 = normal, 1 = hardware, sensor, CAN, or software alert + uint16_t hardwareStatus : 1; // 0 = normal, 1 = programmable alert + uint16_t softwareStatus : 1; // 0 = normal, 1 = programmable alert + uint16_t sensorStatus : 1; // 0 = normal, 1 = programmable alert + uint16_t unlocked1PPS : 1; // 0 = not asserted, 1 = asserted + uint16_t unlockedInternalGPS : 1; // 0 = not asserted, 1 = asserted + uint16_t noDGPS : 1; // 0 = DGPS lock, 1 = no DGPS + uint16_t unlockedEEPROM : 1; // 0 = locked, 1 = unlocked + uint16_t algorithmInit : 1; // 0 = normal, 1 = in initialization mode + uint16_t highGain : 1; // 0 = low gain mode, 1 = high gain mode + uint16_t attitudeOnlyAlgorithm : 1; // 0 = navigation state tracking, 1 = attitude only state tracking + uint16_t turnSwitch : 1; // 0 = off, 1 = yaw rate greater than turnSwitch threshold + uint16_t sensorOverRange : 1; // 0 = not asserted, 1 = asserted + uint16_t reserved : 3; +} STATUS_TEST_PAYLOAD; + + +// ODR set packet type +typedef struct { + uint8_t dest_address; // target's address + uint8_t odr; // odr values +} RATE_CONFIG_PAYLOAD; + +// data packet payload +typedef struct { + uint8_t dest_address; // targer address + union { + struct { + uint8_t slope_sensor : 1; // ss2 packet + uint8_t angular_rate : 1; // angular rate packet + uint8_t accelerator : 1; // acceleration packet + uint8_t reserved : 5; + } b; + + uint8_t r; + } type1_bits; + union { + struct { + uint8_t reserved : 8; + } b; + + uint8_t r; + } type2_bits; +} PACKET_TYPE_PAYLOAD; + + +// LPF set payload format +typedef struct { + uint8_t dest_address; // target's address + uint8_t rate_cutoff; // LPF of rate sensor + uint8_t accel_cutoff; // LPF of XL sensor +} DIGITAL_FILTER_PAYLOAD; + + +// orientation set payload format +typedef struct { + uint8_t dest_address; // target's address + uint8_t orien_bits[2]; // orientation setting +} ORIENTATION_SETTING; + +// orientation set payload format +typedef struct { + uint8_t dest_address; // target's address + uint8_t cmd[7]; // command +} MAG_ALIGN_PAYLOAD; + + +// user behavior set payload format +typedef struct { + uint8_t dest_address; // target's address + uint8_t restart_on_overrange; // restart or not while over range + uint8_t dynamic_motion; // dynamic motion +} USER_BEHAVIOR_PAYLOAD; + + +// Angle alarm set payload format +typedef struct { + uint8_t dest_address; // targert's address + uint8_t roll_upper; // upper limitation of roll + uint8_t roll_lower; // lower limitation of roll + uint8_t pitch_upper; // upper limitation of pitch + uint8_t pitch_lower; // lower limitation of pitch + uint8_t roll_hysteresis; // hysteresis of roll + uint8_t pitch_hyseresis; // hysteresis of pitch +} ANGLE_ALARM_PAYLOAD; + + +// Cone alarm set payload format +typedef struct { + uint8_t dest_address; // target's address + uint16_t alarm_selector; // alarm selector + uint16_t angle_limit; // angular limitation + uint16_t angle_hysteresis; // angular hysteresis +} CONE_ALARM_PAYLOAD; + + +// acceleration set payload format +typedef struct { + uint8_t dest_address; // target's address + uint16_t x_acceleration; // x-axis acceleration + uint16_t y_acceleration; // y-axis acceleration + uint16_t z_acceleration; // z-axis acceleration +} ACCELERATION_PARAM_PAYLOAD; + +extern void ProcessRequest(void *dsc); +extern void ProcessEcuCommands(void * command, uint8_t ps, uint8_t addr); +extern void PrepareJ1939DataPackets(void); +extern void EnqeuePeriodicDataPackets(int latency, int sendPeriodicPackets); +extern void ProcessDataPackets(void *dsc); + + + +#endif /* USER_CONFIGURATION_H */ diff --git a/examples/OpenIMU300RI/INS/src/user/UserMessagingUART.c b/examples/OpenIMU300RI/INS/src/user/UserMessagingUART.c new file mode 100644 index 0000000..ad7debd --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/user/UserMessagingUART.c @@ -0,0 +1,391 @@ +/** *************************************************************************** + * @file UserConfiguration.c + * + * 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. + * + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#include +#include +#include + +#include "algorithmAPI.h" +#include "platformAPI.h" +#include "sensorsAPI.h" +#include "userAPI.h" +#include "appVersion.h" + +#include "UserMessagingUART.h" +#include "UserConfiguration.h" + +#include "Indices.h" // For X_AXIS, etc +#include "CommonMessages.h" + +// Declare the IMU data structure +IMUDataStruct gIMU; + +// Version string +//char userVersionString[] = "IMU 1.0.0"; + + + + + +/// List of allowed packet codes +usr_packet_t userInputPackets[] = { + {USR_IN_NONE, {0,0}}, + {USR_IN_PING, "pG"}, + {USR_IN_UPDATE_CONFIG, "uC"}, + {USR_IN_UPDATE_PARAM, "uP"}, + {USR_IN_UPDATE_ALL, "uA"}, + {USR_IN_SAVE_CONFIG, "sC"}, + {USR_IN_GET_CONFIG, "gC"}, + {USR_IN_GET_PARAM, "gP"}, + {USR_IN_GET_ALL, "gA"}, + {USR_IN_GET_VERSION, "gV"}, + {USR_IN_RESET, "rS"}, +// place new input packet code here, before USR_IN_MAX + {USR_IN_MAX, {0xff, 0xff}}, // "" +}; + + +// packet codes here should be unique - +// should not overlap codes for input packets and system packets +// First byte of Packet code should have value >= 0x61 +usr_packet_t userOutputPackets[] = { +// Packet Type Packet Code + {USR_OUT_NONE, {0x00, 0x00}}, + {USR_OUT_TEST, "zT"}, + {USR_OUT_DATA1, "z1"}, + {USR_OUT_DATA2, "z2"}, +// place new type and code here + {USR_OUT_SCALED1, "s1"}, + {USR_OUT_EKF1, "e1"}, + {USR_OUT_EKF2, "e2"}, + {USR_OUT_MAX, {0xff, 0xff}}, // "" +}; + +volatile char *info; +static int _userPayloadLen = 0; +static int _outputPacketType = USR_OUT_MAX; +static int _inputPacketType = USR_IN_MAX; + + +int checkUserPacketType(uint16_t receivedCode) +{ + int res = UCB_ERROR_INVALID_TYPE; + usr_packet_t *packet = &userInputPackets[1]; + uint16_t code; + + // validate packet code here and memorise for further processing + while(packet->packetType != USR_IN_MAX){ + code = (packet->packetCode[0] << 8) | packet->packetCode[1]; + if(code == receivedCode){ + _inputPacketType = packet->packetType; + return UCB_USER_IN; + } + packet++; + } + + packet = &userOutputPackets[1]; + + // validate packet code here and memorize for further processing + while(packet->packetType != USR_OUT_MAX){ + code = (packet->packetCode[0] << 8) | packet->packetCode[1]; + if(code == receivedCode){ + _outputPacketType = packet->packetType; + return UCB_USER_OUT; + } + packet++; + } + + return res; +} + + +void userPacketTypeToBytes(uint8_t bytes[]) +{ + if(_inputPacketType && _inputPacketType < USR_IN_MAX){ + // response to request. Return same packet code + bytes[0] = userInputPackets[_inputPacketType].packetCode[0]; + bytes[1] = userInputPackets[_inputPacketType].packetCode[1]; + _inputPacketType = USR_IN_MAX; // wait for next input packet + return; + } + + if(_outputPacketType && _outputPacketType < USR_OUT_MAX){ + // continuous packet + bytes[0] = userOutputPackets[_outputPacketType].packetCode[0]; + bytes[1] = userOutputPackets[_outputPacketType].packetCode[1]; + }else { + bytes[0] = 0; + bytes[1] = 0; + } + +} + + +/** *************************************************************************** + * @name setUserPacketType - set user output packet type + * @brief + * @param [in] packet type + * @retval - TRUE if success, FALSE otherwise + ******************************************************************************/ +BOOL setUserPacketType(uint8_t *data, BOOL fApply) +{ + int type = -1; + uint16_t *code = (uint16_t*)data; + uint16_t tmp; + BOOL result = TRUE; + + usr_packet_t *packet = &userOutputPackets[1]; + for(int i = 0; i < USR_OUT_MAX; i++, packet++){ + if(*code == *((uint16_t*)packet->packetCode)){ + type = packet->packetType; + break; + } + } + + switch(type){ + case USR_OUT_TEST: // simple test packet to check communication + _outputPacketType = type; + _userPayloadLen = USR_OUT_TEST_PAYLOAD_LEN; + break; + case USR_OUT_DATA1: // packet with sensors data. Change at will + _outputPacketType = type; + _userPayloadLen = USR_OUT_DATA1_PAYLOAD_LEN; + break; + case USR_OUT_DATA2: // packet with arbitrary data + _outputPacketType = type; + _userPayloadLen = sizeof(data2_payload_t); + break; + case USR_OUT_SCALED1: // packet with arbitrary data + _outputPacketType = type; + _userPayloadLen = USR_OUT_SCALED1_PAYLOAD_LEN; + break; + case USR_OUT_EKF1: // packet with EKF algorithm data + _outputPacketType = type; + _userPayloadLen = USR_OUT_EKF1_PAYLOAD_LEN; + break; + case USR_OUT_EKF2: // packet with EKF algorithm data + _outputPacketType = type; + _userPayloadLen = USR_OUT_EKF2_PAYLOAD_LEN; + break; + default: + result = FALSE; + break; + } + + if(result == FALSE){ + return FALSE; + } + + tmp = (data[0] << 8) | data[1]; + + result = platformSetOutputPacketCode(tmp, fApply); + + return result; +} + + +/** *************************************************************************** + * @name getUserPayloadLength - get user payload length for sanity check + * @brief + * + * @retval - user payload length + ******************************************************************************/ +int getUserPayloadLength(void) +{ + // ATTENTION: return actual user payload length, if user packet used + return _userPayloadLen; +} + +/****************************************************************************** + * @name HandleUserInputPacket - API + * @brief general handler + * @param [out] packetPtr - filled in packet from the mapped physical port + * @retval N/A + ******************************************************************************/ +int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) +{ + BOOL valid = TRUE; + int ret = USER_PACKET_OK; +// userPacket *pkt = (userPacket *)ptrUcbPacket->payload; + + /// call appropriate function based on packet type + switch (_inputPacketType) { + case USR_IN_RESET: + Reset(); + break; + case USR_IN_PING: + { + uint8_t len; + Fill_PingPacketPayload(ptrUcbPacket->payload, &len); + ptrUcbPacket->payloadLength = len; + } + // leave all the same - it will be bounced back unchanged + break; + case USR_IN_GET_VERSION: + { + uint8_t len; + Fill_VersionPacketPayload(ptrUcbPacket->payload, &len); + ptrUcbPacket->payloadLength = len; + } + break; + case USR_IN_SAVE_CONFIG: + // payload length does not change + if(!SaveUserConfig()){ + valid = FALSE; + } + break; + case USR_IN_UPDATE_CONFIG: + UpdateUserConfig((userConfigPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + break; + case USR_IN_UPDATE_PARAM: + UpdateUserParam((userParamPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + break; + case USR_IN_UPDATE_ALL: + UpdateAllUserParams((allUserParamsPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + break; + case USR_IN_GET_CONFIG: + if(!GetUserConfig((userConfigPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength)){ + valid = FALSE; + } + break; + case USR_IN_GET_PARAM: + if(!GetUserParam((userParamPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength)){ + valid = FALSE; + } + break; + case USR_IN_GET_ALL: + if(!GetAllUserParams((allUserParamsPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength)){ + valid = FALSE; + } + break; + default: + /// default handler - unknown packet + valid = FALSE; + break; + } + + if(!valid){ + ptrUcbPacket->payloadLength = 0; + ret = USER_PACKET_ERROR; + } + + ptrUcbPacket->packetType = UCB_USER_OUT; // do not remove - done for proper packet routing + + return ret; +} + + +/****************************************************************************** + * @name HandleUserOutputPacket - API call ro prepare continuous user output packet + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) +{ + static uint32_t _testVal = 0; + static uint64_t ppsTstamp = 0; + BOOL ret = TRUE; + + switch (_outputPacketType) { + case USR_OUT_TEST: + { + uint32_t *testParam = (uint32_t*)(payload); + *payloadLen = USR_OUT_TEST_PAYLOAD_LEN; + *testParam = _testVal++; + } + break; + + case USR_OUT_DATA1: + { + uint8_t len; + Fill_z1PacketPayload(payload, &len); + *payloadLen = len; + } + break; + case USR_OUT_DATA2: + { + data2_payload_t *pld = (data2_payload_t *)payload; + pld->timer = platformGetCurrTimeStamp()/1000; // in miliseconds + pld->c = 'A'; + pld->s = 1234; + pld->i = -5; + if(platformGetPpsFlag(TRUE)){ + ppsTstamp = platformGetPpsTimeStamp(); + } +// pld->ll = platformGetDacqTimeStamp(); // in microseconds; + pld->ll = ppsTstamp; // time stamp of last PPS edge in microseconds from system start + pld->d = 1.23456789; + *payloadLen = sizeof(data2_payload_t); + } + break; + case USR_OUT_SCALED1: + { + uint8_t len; + Fill_s1PacketPayload(payload, &len); + *payloadLen = len; + } + break; + case USR_OUT_EKF1: + { + // Variables used to hold the EKF values + uint8_t len; + Fill_e1PacketPayload(payload, &len); + *payloadLen = len; + } + break; + + case USR_OUT_EKF2: + { + // Variables used to hold the EKF values + uint8_t len; + Fill_e2PacketPayload(payload, &len); + *payloadLen = len; + } + break; + // place additional user packet preparing calls here + // case USR_OUT_XXXX: + // *payloadLen = YYYY; // total user payload length, including user packet type + // payload[0] = ZZZZ; // user packet type + // prepare dada here + // break; + default: + { + *payloadLen = 0; + ret = FALSE; + } + break; /// unknown user packet, will send error in response + } + + return ret; +} + + +void WriteResultsIntoOutputStream(void *results) +{ +// implement specific data processing/saving here + +} diff --git a/examples/OpenIMU300RI/INS/src/user/UserMessagingUART.h b/examples/OpenIMU300RI/INS/src/user/UserMessagingUART.h new file mode 100644 index 0000000..e8615ef --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/user/UserMessagingUART.h @@ -0,0 +1,155 @@ +/******************************************************************************* + * File: UserConfiguration.h + * Created on Jan 25, 2017 + ******************************************************************************/ +/******************************************************************************* +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 _USER_MESSAGING_UART_H +#define _USER_MESSAGING_UART_H + +#include + +#include "GlobalConstants.h" +#include "ucb_packet_struct.h" + +#define USER_PACKET_OK 0 +#define UNKNOWN_USER_PACKET 1 +#define USER_PACKET_ERROR 2 + +// here is definition for packet rate divider +// considering that data acquisition task runs at 200 Hz +typedef enum { + PACKET_RATE_INVALID = -1, + PACKET_RATE_QUIET = 0, // quiet mode + PACKET_RATE_200HZ = 200, // packet rate 200 Hz + PACKET_RATE_100HZ = 100, // packet rate 100 Hz + PACKET_RATE_50HZ = 50, // packet rate 50 Hz + PACKET_RATE_25HZ = 25, // packet rate 25 Hz + PACKET_RATE_20HZ = 20, // packet rate 20 Hz + PACKET_RATE_10HZ = 10, // packet rate 10 Hz + 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; + + +// User Input packet payload has next structure: +// number offset +// of of first +// parameters parameter +// U2 U2 U4/I4/F +// XXXX YYYY [parameters] +// User input packet codes, change at will +typedef enum { + USR_IN_NONE = 0 , + USR_IN_PING , + USR_IN_UPDATE_CONFIG , + USR_IN_UPDATE_PARAM , + USR_IN_UPDATE_ALL , + USR_IN_SAVE_CONFIG , + USR_IN_GET_CONFIG , + USR_IN_GET_PARAM , + USR_IN_GET_ALL , + USR_IN_GET_VERSION , + USR_IN_RESET , + // add new packet type here, before USR_IN_MAX + USR_IN_MAX , +}UserInPacketType; + +// User output packet codes, change at will +typedef enum { + USR_OUT_NONE = 0, // 0 + USR_OUT_TEST, // 1 + USR_OUT_DATA1, // 2 + USR_OUT_DATA2, // 3 +// add new output packet type here, before USR_OUT_MAX + USR_OUT_SCALED1, // 4 + USR_OUT_EKF1, + USR_OUT_EKF2, + USR_OUT_MAX +} UserOutPacketType; + + +// total size of user packet structure should not exceed 255 bytes +#pragma pack(1) +typedef struct { + uint8_t packetPayload[252]; // maximum 252 bytes +} userPacket; +#define MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET 30 +#define FIRST_30_PARAMS 0xFFFFFFFF + +// example of user payload structure +typedef struct { + uint32_t numParams; // number of consecutive parameters to update (little endian) + uint32_t paramOffset; // parameter number in parameters structure (little endian) + uint64_t parameters[MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET]; // up to 30 64-bit parameters (little endian) +}userConfigPayload; + +#pragma pack(1) +// example of user payload structure +typedef struct { + uint32_t paramNum; // parameter number in parameters structure (little endian) + uint64_t parameter; // up to 30 64-bit parameters (little endian) +}userParamPayload; +#pragma pack() + +// example of user payload structure +typedef struct { + uint64_t parameters[MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET]; // up to 30 64-bit parameters (little endian) +}allUserParamsPayload; + +#pragma pack(1) +typedef struct { + uint32_t timer; + uint8_t c; + short s; + int i; + long long ll; + double d; +}data2_payload_t; + +#pragma pack() + + +#define USR_OUT_TEST_PAYLOAD_LEN (4) // test parameter (uint32_t) +#define USR_OUT_DATA1_PAYLOAD_LEN (4*10) // 1 integer +3accels (float LE) + 3gyros (float LE) + 3 mags (floatLE) +#define USR_OUT_SCALED1_PAYLOAD_LEN (52) // See UserMessaging.c for make-up of Scaled1 message +#define USR_OUT_EKF1_PAYLOAD_LEN (75) +#define USR_OUT_EKF2_PAYLOAD_LEN (123) + + +#define USER_OK 0x00 +#define USER_NAK 0x80 +#define USER_INVALID 0x81 + +extern int userPacketOut; + +extern int getUserPayloadLength(void); +extern int checkUserPacketType(uint16_t receivedCode); +extern void userPacketTypeToBytes(uint8_t bytes[]); +extern void WriteResultsIntoOutputStream(void *results); +BOOL setUserPacketType(uint8_t* type, BOOL fApply); + + +extern BOOL UpdateUserConfig(userConfigPayload* pld, uint8_t *payloadLen); +extern BOOL UpdateUserParam(userParamPayload* pld, uint8_t *payloadLen); +extern BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen); +extern BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen); +extern BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen); +extern BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen); + +#endif /* USER_CONFIGURATION_H */ diff --git a/examples/OpenIMU300RI/INS/src/user/dataProcessingAndPresentation.c b/examples/OpenIMU300RI/INS/src/user/dataProcessingAndPresentation.c new file mode 100644 index 0000000..d66f256 --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/user/dataProcessingAndPresentation.c @@ -0,0 +1,283 @@ +/***************************************************************************** + * @file dataProcessingAndPresentation.c + * + * 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. + * + * contains of data post-processing framework + ******************************************************************************/ +/******************************************************************************* +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. +*******************************************************************************/ + +#include "string.h" +#include "userAPI.h" +#include "sensorsAPI.h" +#include "gpsAPI.h" +#include "algorithmAPI.h" +#include "UserMessagingCAN.h" +#include "BITStatus.h" + +#include "Indices.h" // For X_AXIS and Z_AXIS +#include "debug.h" // For debug commands + +IMUDataStruct gIMU; +gpsDataStruct_t gGPS, gCanGps, *gpsDataPtr; +BOOL GNSS_From_Serial = 1; + +// Local-function prototypes +static void _IncrementIMUTimer(uint16_t dacqRate); +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); +static void _IMUDebugMessage(void); +static void _GPSDebugMessage(void); + +/* * + ****************** + * + Sensors data processing flow (from left to right) + +Raw *************** Filtered * ************ Calibrated **************** ********************* Data +Data * Built-in * Raw Data * * Data * User Filter s* * Algorithm * Output +****** LP Filter ************* Calibration ************* (if desired) ***** (Kalman Filter ******** + * * * * * * * or user Algorithm)* + ************* * ************** **************** ********************* + ^^^^^^^ + Can be selected during + initialization or turned + OFF +*///<--------------------- Cyclical processing at 100 or 200 Hz in Data Acquisition Task --------------> + + +// Next function is common for all platforms, but implementation of the methods inside is platform-dependent +// Call to this function made from DataAcquisitionTask during initialization phase +// All user algorithm and data structures should be initialized here, if used +void initUserDataProcessingEngine() +{ + InitUserAlgorithm(); // default implementation located in file user_algorithm.c +} + + +// Notes: +// 1) 'inertialAndPositionDataProcessing' is common for all platforms, but implementation +// of the methods inside is platform and user-dependent. +// 2) 'DataAcquisitionTask' calls this function after retrieving samples of current +// sensors data and applying corresponding calibration +// 3) 'dacqRate' is rate with which new set of sensors data arrives +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; + if(initAlgo) + { + if(GnssInputDataOverCan()){ + gpsDataPtr = &gCanGps; + }else{ + gpsDataPtr = &gGPS; + } + gpsDataPtr->gpsUpdate = FALSE; + + // 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; + } + algoCntr++; + if(algoCntr >= algoCntrLimit) + { + // Reset counter + algoCntr = 0; + + // Obtain accelerometer data [g] + GetAccelData_g(gIMU.accel_g); + + // Obtain rate-sensor data [rad/sec] + GetRateData_radPerSec(gIMU.rate_radPerSec); + GetRateData_degPerSec(gIMU.rate_degPerSec); + + // Obtain magnetometer data [G] + GetMagData_G(gIMU.mag_G); + + // Obtain board temperature data [degC] + GetBoardTempData(&gIMU.temp_C); + + // Obtain GPS data (lat/lon: deg, alt: meters, vel: m/s, ITOW: msec, ) +#ifdef GPS + if(!GnssInputDataOverCan()){ + GetGPSData(gpsDataPtr); + }else{ + // data should be in gCanGps if any + } +#endif + // Execute user algorithm (default implementation located in file user_algorithm.c) + RunUserNavAlgorithm(gIMU.accel_g, gIMU.rate_radPerSec, gIMU.mag_G, gpsDataPtr, dacqRate); + + if(gpsDataPtr->gpsUpdate){ + gpsDataPtr->gpsUpdate = 0; + } + + // Get algorithm status, and set the state in system status + AlgoStatus algoStatus; + GetAlgoStatus(&algoStatus); + gBitStatus.swStatus.all = algoStatus.all; + gBitStatus.swAlgBIT.bit.initialization = algoStatus.bit.algorithmInit; + } + + // Generate a debug message that provide sensor data in order to verify the + // Algorithm input data is as expected. + _GenerateDebugMessage(dacqRate, ZERO_HZ); + + +} + + +// +static void _IncrementIMUTimer(uint16_t dacqRate) +{ + // Initialize timer variables (used to control the output of the debug + // messages and the IMU timer value) + static int initFlag = 1; + if(initFlag) { + // Reset 'initFlag' so this section is not executed more than once. + initFlag = 0; + + // Set the IMU output delta-counter value + gIMU.dTimerCntr = (uint32_t)( 1000.0 / (float)(dacqRate) + 0.5 ); + } + + // Increment the timer-counter by the sampling period equivalent to the + // rate at which inertialAndPositionDataProcessing is called + gIMU.timerCntr = gIMU.timerCntr + gIMU.dTimerCntr; +} + + +// +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) +{ + // 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 loading of the GPS data into the GPS data structure + 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; + + // Reset 'new GPS data' flag (this should be done in UpdateFunctions + // to ensure the EKF can use the data) + //gGPS.updateFlag = 0; <-- This would make a difference as the input + // to the algorithm isn't set yet. + + // Create message here + static uint8_t msgType = 1; + switch( msgType ) + { + case 0: + // None + break; + + case 1: + // IMU data + _IMUDebugMessage(); + break; + + case 2: + // GPS data + _GPSDebugMessage(); + break; + } + } + } +} + + +static void _IMUDebugMessage(void) +{ + // IMU Data + DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); + DebugPrintFloat(", a: [ ", (float)gIMU.accel_g[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Z_AXIS], 3); + DebugPrintFloat(" ], w: [ ", (float)gIMU.rate_radPerSec[X_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Y_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Z_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ], m: [ ", (float)gIMU.mag_G[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Z_AXIS], 3); + DebugPrintString(" ]"); + DebugPrintEndline(); +} + + +static void _GPSDebugMessage(void) +{ +#if 1 + // GPS Data + DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); + DebugPrintFloat(", Lat: ", (float)gGPS.latitude, 8); + DebugPrintFloat(", Lon: ", (float)gGPS.longitude, 8); + DebugPrintFloat(", Alt: ", (float)gGPS.altitude, 5); + DebugPrintLongInt(", ITOW: ", gGPS.itow ); +#else + // + DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); + 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 +#endif + DebugPrintEndline(); +} diff --git a/examples/OpenIMU300RI/INS/src/user/userAPI.h b/examples/OpenIMU300RI/INS/src/user/userAPI.h new file mode 100644 index 0000000..7bd9004 --- /dev/null +++ b/examples/OpenIMU300RI/INS/src/user/userAPI.h @@ -0,0 +1,72 @@ +/** ****************************************************************************** + * @file userAPI.h API functions for Interfacing with user algorithm + * + * 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. + * + *****************************************************************************/ +/******************************************************************************* +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 _USER_API_H +#define _USER_API_H + +// Some common constants used in the user algorithm logic +#define ZERO_HZ 0 +#define ONE_HZ 1 +#define TWO_HZ 2 +#define FOUR_HZ 4 +#define FIVE_HZ 5 +#define TEN_HZ 10 +#define TWENTY_HZ 20 +#define TWENTY_FIVE_HZ 25 +#define FIFTY_HZ 50 + +#define NUM_AXIS 3 + +#include +#include "gpsAPI.h" + +void inertialAndPositionDataProcessing(uint16_t dacqRate); + +void *RunUserNavAlgorithm(double *accels, double *rates, double* mags, gpsDataStruct_t *gps, uint16_t dacqRate); +void WriteResultsIntoOutputStream(void *results) ; +void InitUserDataStructures(); +void InitUserFilters(); +void InitUserAlgorithm(); +void initUserDataProcessingEngine(); +void userInitConfigureUnit(); +void userInitConfigureUart(); +BOOL GnssInputDataOverCan(); + +// IMU data structure +typedef struct { + // Timer output counter + uint32_t timerCntr, dTimerCntr; + + // Algorithm states + double accel_g[3]; + double rate_radPerSec[3]; + double rate_degPerSec[3]; + double mag_G[3]; + double temp_C; +} IMUDataStruct; + +extern IMUDataStruct gIMU; + +#endif diff --git a/examples/OpenIMU300RI/VG_AHRS/board/OpenIMU300.json b/examples/OpenIMU300RI/VG_AHRS/board/OpenIMU300.json deleted file mode 100644 index cacb4ca..0000000 --- a/examples/OpenIMU300RI/VG_AHRS/board/OpenIMU300.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "build": { - "core": "stm32", - "cpu": "cortex-m4", - "extra_flags": [ - "-mcpu=cortex-m4", - "-mfloat-abi=softfp", - "-mfpu=fpv4-sp-d16", - "-DSTM32F4", - "-DSTM32F405RG", - "-DSTM32F405xx" - ], - "ldscript": "stm32f40x.ld", - "f_cpu": "120000000L", - "mcu": "stm32f405rg" - }, - "debug": { - "default_tools": ["stlink"], - "openocd_target": "stm32f4x", - "jlink_device": "STM32F405RG", - "svd_path": "STM32F40x.svd" - }, - "frameworks": [], - "name": "Aceinna OpenIMU 300", - "upload": { - "offset_address": "0x08010000", - "maximum_ram_size": 131072, - "maximum_size": 1048576, - "protocols": [ - "blackmagic", - "stlink", - "jlink" - ], - "protocol": "stlink" - }, - "url": "https://www.aceinna.com/inertial-systems/", - "vendor": "Aceinna" -} diff --git a/examples/OpenIMU300RI/VG_AHRS/include/FreeRTOSConfig.h b/examples/OpenIMU300RI/VG_AHRS/include/FreeRTOSConfig.h new file mode 100644 index 0000000..80ca6b9 --- /dev/null +++ b/examples/OpenIMU300RI/VG_AHRS/include/FreeRTOSConfig.h @@ -0,0 +1,180 @@ +/* + FreeRTOS V9.0.0 - Copyright (C) 2016 Real Time Engineers Ltd. + All rights reserved + + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. + + This file is part of the FreeRTOS distribution. + + FreeRTOS is free software; you can redistribute it and/or modify it under + the terms of the GNU General Public License (version 2) as published by the + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. + + *************************************************************************** + >>! NOTE: The modification to the GPL is included to allow you to !<< + >>! distribute a combined work that includes FreeRTOS without being !<< + >>! obliged to provide the source code for proprietary components !<< + >>! outside of the FreeRTOS kernel. !<< + *************************************************************************** + + FreeRTOS is distributed in the hope that it will be useful, but WITHOUT ANY + WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html + + *************************************************************************** + * * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * + * * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * + * * + *************************************************************************** + + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. + + http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and + mission critical applications that require provable dependability. + + 1 tab == 4 spaces! +*/ + + +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- + * Application specific definitions. + * + * These definitions should be adjusted for your particular hardware and + * application requirements. + * + * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE + * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. + * + * See http://www.freertos.org/a00110.html. + *----------------------------------------------------------*/ + +/* Ensure stdint is only used by the compiler, and not the assembler. */ +#if defined(__ICCARM__) || defined(__CC_ARM) || defined(__GNUC__) + #include + extern uint32_t SystemCoreClock; +#endif + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 0 +#define configUSE_TICK_HOOK 0 +#define configCPU_CLOCK_HZ (SystemCoreClock) +#define configTICK_RATE_HZ ((TickType_t)1000) +#define configMAX_PRIORITIES (7) +#define configMINIMAL_STACK_SIZE ((uint16_t)2048) +#define configTOTAL_HEAP_SIZE ((size_t)(38000)) +#define configMAX_TASK_NAME_LEN (30) +#define configUSE_TRACE_FACILITY 1 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 1 +#define configUSE_MUTEXES 1 +#define configQUEUE_REGISTRY_SIZE 8 +#define configCHECK_FOR_STACK_OVERFLOW 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configUSE_APPLICATION_TASK_TAG 0 +#define configUSE_COUNTING_SEMAPHORES 1 +#define configGENERATE_RUN_TIME_STATS 0 +#define configAPPLICATION_ALLOCATED_HEAP 0 + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) + +/* Software timer definitions. */ +#define configUSE_TIMERS 0 +#define configTIMER_TASK_PRIORITY (2) +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH (configMINIMAL_STACK_SIZE * 2) + +/* Set the following definitions to 1 to include the API function, or zero +to exclude the API function. */ +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 0 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 + +/* Cortex-M specific definitions. */ +#ifdef __NVIC_PRIO_BITS + /* __BVIC_PRIO_BITS will be specified when CMSIS is being used. */ + #define configPRIO_BITS __NVIC_PRIO_BITS +#else + #define configPRIO_BITS 4 /* 15 priority levels */ +#endif + +/* The lowest interrupt priority that can be used in a call to a "set priority" +function. */ +#define configLIBRARY_LOWEST_INTERRUPT_PRIORITY 0xf + +/* The highest interrupt priority that can be used by any interrupt service +routine that makes calls to interrupt safe FreeRTOS API functions. DO NOT CALL +INTERRUPT SAFE FREERTOS API FUNCTIONS FROM ANY INTERRUPT THAT HAS A HIGHER +PRIORITY THAN THIS! (higher priorities are lower numeric values. */ +#define configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY 1 + + +//#define configKERNEL_INTERRUPT_PRIORITY ( 0x01 << 4 ) /* Priority 15, or 255 as only the top four bits are implemented. This is the lowest priority. */ +//#define configMAX_SYSCALL_INTERRUPT_PRIORITY ( 0x01 << 4 ) /* Priority 5, or 80 as only the top four bits are implemented. */ + + +/* Interrupt priorities used by the kernel port layer itself. These are generic +to all Cortex-M ports, and do not rely on any particular library functions. */ +#define configKERNEL_INTERRUPT_PRIORITY ( configLIBRARY_LOWEST_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) ) +/* !!!! configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to zero !!!! +See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html. */ +#define configMAX_SYSCALL_INTERRUPT_PRIORITY ( configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) ) + +/* Normal assert() semantics without relying on the provision of an assert.h +header file. */ +#define configASSERT( x ) if( ( x ) == 0 ) { taskDISABLE_INTERRUPTS(); for( ;; ); } + +/* Definitions that map the FreeRTOS port interrupt handlers to their CMSIS + standard names. */ +#define vPortSVCHandler SVC_Handler +#define xPortPendSVHandler PendSV_Handler + +/* IMPORTANT: This define MUST be commented when used with STM32Cube firmware, + to prevent overwriting SysTick_Handler defined within STM32Cube HAL */ +//#define xPortSysTickHandler SysTick_Handler +#define osSystickHandler SysTick_Handler + +#endif /* FREERTOS_CONFIG_H */ + diff --git a/examples/OpenIMU300RI/VG_AHRS/include/osresources.h b/examples/OpenIMU300RI/VG_AHRS/include/osresources.h new file mode 100644 index 0000000..47ac800 --- /dev/null +++ b/examples/OpenIMU300RI/VG_AHRS/include/osresources.h @@ -0,0 +1,42 @@ +#ifndef _OS_RES_H +#define _OS_RES_H +#include "cmsis_os.h" +#include "portmacro.h" + + +#ifdef __MAIN + +osSemaphoreDef(GYRO_READY_SEM); +osSemaphoreDef(ACCEL_READY_SEM); +osSemaphoreDef(MAG_READY_SEM); +osSemaphoreDef(TEMP_READY_SEM); +osSemaphoreDef(NAV_DATA_READY_SEM); +osSemaphoreDef(CLI_READY_SEM); +osSemaphoreDef(DATA_ACQ_SEM); +osSemaphoreDef(CAN_DATA_SEM); +osSemaphoreDef(CLI_SEM); +osSemaphoreId gyroReadySem; +osSemaphoreId accelReadySem; +osSemaphoreId magReadySem; +osSemaphoreId tempReadySem; +osSemaphoreId navDataReadySem; +osSemaphoreId cliReadySem; +osSemaphoreId dataAcqSem; +osSemaphoreId canDataSem; +osSemaphoreId cliSem; + +#else + +extern osSemaphoreId gyroReadySem; +extern osSemaphoreId accelReadySem; +extern osSemaphoreId magReadySem; +extern osSemaphoreId tempReadySem; +extern osSemaphoreId navDataReadySem; +extern osSemaphoreId dataAcqSem; +extern osSemaphoreId canDataSem; +extern osSemaphoreId cliSem; + +#endif + + +#endif diff --git a/examples/OpenIMU300RI/VG_AHRS/ldscripts/stm32f40x.ld b/examples/OpenIMU300RI/VG_AHRS/ldscripts/stm32f40x.ld deleted file mode 100644 index 1c0c9f6..0000000 --- a/examples/OpenIMU300RI/VG_AHRS/ldscripts/stm32f40x.ld +++ /dev/null @@ -1,202 +0,0 @@ -/* -***************************************************************************** -** - -** File : LinkerScript.ld -** -** Abstract : Linker script for STM32F405RGx Device with -** 1024KByte FLASH, 128KByte RAM -** -** Set heap size, stack size and stack location according -** to application requirements. -** -** Set memory bank area and size if external memory is used. -** -** Target : STMicroelectronics STM32 -** -** -** Distribution: The file is distributed as is, without any warranty -** of any kind. -** -***************************************************************************** -** @attention -** -**

© COPYRIGHT(c) 2014 Ac6

-** -** Redistribution and use in source and binary forms, with or without modification, -** are permitted provided that the following conditions are met: -** 1. Redistributions of source code must retain the above copyright notice, -** this list of conditions and the following disclaimer. -** 2. Redistributions in binary form must reproduce the above copyright notice, -** this list of conditions and the following disclaimer in the documentation -** and/or other materials provided with the distribution. -** 3. Neither the name of Ac6 nor the names of its contributors -** may be used to endorse or promote products derived from this software -** without specific prior written permission. -** -** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -** -***************************************************************************** -*/ - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* Highest address of the user mode stack */ -_estack = 0x2001ff00; /* end of RAM */ -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0x2000; /* required amount of heap */ -_Min_Stack_Size = 0x2000; /* required amount of stack */ - -/* Specify the memory areas */ -MEMORY -{ -FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 448K -EEPROM (rw) : ORIGIN = 0x08008000, LENGTH = 16K -USER_EEPROM(rw) : ORIGIN = 0x0800C000, LENGTH = 16K -RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K -} - -/* Define output sections */ -SECTIONS -{ - /* The startup code goes first into FLASH */ - .isr_vector : - { - . = ALIGN(4); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - } >FLASH - - /* The program code and other data goes into FLASH */ - .text : - { - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) - - KEEP (*(.init)) - KEEP (*(.fini)) - - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH - - /* Constant data goes into FLASH */ - .rodata : - { - . = ALIGN(4); - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - . = ALIGN(4); - } >FLASH - - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH - .ARM : { - __exidx_start = .; - *(.ARM.exidx*) - __exidx_end = .; - } >FLASH - - .preinit_array : - { - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP (*(.preinit_array*)) - PROVIDE_HIDDEN (__preinit_array_end = .); - } >FLASH - .init_array : - { - PROVIDE_HIDDEN (__init_array_start = .); - KEEP (*(SORT(.init_array.*))) - KEEP (*(.init_array*)) - PROVIDE_HIDDEN (__init_array_end = .); - } >FLASH - .fini_array : - { - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP (*(SORT(.fini_array.*))) - KEEP (*(.fini_array*)) - PROVIDE_HIDDEN (__fini_array_end = .); - } >FLASH - - /* used by the startup to initialize data */ - _sidata = LOADADDR(.data); - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { - . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT> FLASH - - - /* Uninitialized data section */ - . = ALIGN(4); - .bss : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(.bss*) - *(COMMON) - - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM - - /* User_heap_stack section, used to check that there is enough RAM left */ - ._user_heap_stack : - { - . = ALIGN(8); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(8); - } >RAM - - .eeprom 0x08008000: - { - . = ALIGN(4); - KEEP(*(.xbowEeprom)) /* keep my variable even if not referenced */ - . = ALIGN(4); - _eeeprom = .; /* define a global symbols at end of code */ - } >EEPROM - - .usereeprom 0x0800C000: - { - . = ALIGN(4); - KEEP(*(.userEeprom)) /* keep my variable even if not referenced */ - . = ALIGN(4); - _eusereeprom = .; /* define a global symbols at end of code */ - } >USER_EEPROM - - /* Remove information from the standard libraries */ - /DISCARD/ : - { - libc.a ( * ) - libm.a ( * ) - libgcc.a ( * ) - } - - .ARM.attributes 0 : { *(.ARM.attributes) } -} diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/algorithmAPI.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/algorithmAPI.h index 80b664b..95f0185 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/algorithmAPI.h +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/algorithmAPI.h @@ -36,7 +36,7 @@ limitations under the License. * * @retval N/A *****************************************************************************/ -void InitializeAlgorithmStruct(uint16_t callingFreq); +void InitializeAlgorithmStruct(uint8_t callingFreq); /****************************************************************************** * @brief Get algorithm status. @@ -68,7 +68,23 @@ void enableGpsInAlgorithm(BOOL enable); BOOL gpsUsedInAlgorithm(); -void SetAlgorithmUseDgps(BOOL d); +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 ); diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/AlgorithmLimits.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/AlgorithmLimits.h index 0aa7590..6a1262c 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/AlgorithmLimits.h +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/AlgorithmLimits.h @@ -39,12 +39,13 @@ limitations under the License. #define LIMIT_BIAS_RATE_UPDATE_INS 5.0e-4 #define LIMIT_MIN_GPS_VELOCITY_HEADING 0.45 //0.45 m/s ~= 1.0 mph +#define RELIABLE_GPS_VELOCITY_HEADING 1.0 // velocity of 1.0m/s should provide reliable GNSS heading #define LIMIT_OBS_JACOBIAN_DENOM 1e-3; // The following times are compared against ITOW (units in [msec]) -//#define LIMIT_MAX_GPS_DROP_TIME 3 // 3 [sec] -#define LIMIT_MAX_GPS_DROP_TIME 30 // [sec] +#define LIMIT_MAX_GPS_DROP_TIME 300 // [sec] +#define LIMIT_RELIABLE_DR_TIME 10 // [sec] #define LIMIT_MAX_REST_TIME_BEFORE_DROP_TO_AHRS 60000 // 60000 [ msec ] = 60 [ sec ] #define LIMIT_MAX_REST_TIME_BEFORE_HEADING_INVALID 120000 // 120sec, heading drifts much slower than pos #define LIMIT_DECL_EXPIRATION_TIME 60000 // 60,000 [ counts ] = 10 [ min ] diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/EKF_Algorithm.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/EKF_Algorithm.h index f6bc5b0..6c0e946 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/EKF_Algorithm.h +++ b/examples/OpenIMU300RI/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" @@ -30,8 +31,6 @@ typedef struct { real rateBias_B[NUM_AXIS]; real accelBias_B[NUM_AXIS]; - real stateUpdate[NUMBER_OF_EKF_STATES]; - // Prediction variables: P = FxPxFTranspose + Q real F[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; real P[NUMBER_OF_EKF_STATES][NUMBER_OF_EKF_STATES]; @@ -42,27 +41,47 @@ typedef struct { * is symmetric, only 6 off-diagonal terms need stored. */ - real correctedRate_B[NUM_AXIS]; - real correctedAccel_B[NUM_AXIS]; - real aCorr_N[NUM_AXIS]; + real correctedRate_B[NUM_AXIS]; // [rad/s] + real correctedAccel_B[NUM_AXIS]; // [m/s/s] + real linearAccel_B[NUM_AXIS]; // [m/s/s], linear acceleration in body frame, used to detect drive position + + /* Algorithm results. Velocity states are directly used as results for output. + * The following two are calculated from state + */ + real eulerAngles[NUM_AXIS]; + double llaDeg[NUM_AXIS]; - real R_BinN[NUM_AXIS][NUM_AXIS]; // convert body to NED - real eulerAngles[NUM_AXIS], measuredEulerAngles[NUM_AXIS]; + // measurements + real R_BinN[3][3]; // convert body to NED + real Rn2e[3][3]; // Coordinate tranformation matrix from NED to ECEF + real measuredEulerAngles[3]; // Euler angles measurements + real rGPS_N[3]; // current IMU position w.r.t rGPS0_E in NED. + double rGPS0_E[3]; // Initial IMU ECEF position when first entering INS state. + double rGPS_E[3]; // current IMU ECEF position // Update variables: S = HxPxHTranspose + R - // DEBUG H is 3x16 or 9x16 (AHRS vs INS) real nu[9]; - real H[3][NUMBER_OF_EKF_STATES]; 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]; - double llaDeg[NUM_AXIS]; -// double Position_E[NUM_AXIS]; - + // 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; @@ -70,27 +89,37 @@ extern KalmanFilterStruct gKalmanFilter; /* Global Algorithm structure */ typedef struct { // Sensor readings in the body-frame (B) - real accel_B[NUM_AXIS]; // [g] - real angRate_B[NUM_AXIS]; // [rad/s] - real magField_B[NUM_AXIS]; // [G] + real accel_B[NUM_AXIS]; // [m/s/s] + real angRate_B[NUM_AXIS]; // [rad/s] + real magField_B[NUM_AXIS]; // [G] // GPS information - BOOL gpsValid; - BOOL gpsUpdate; - - double llaRad[3]; - double vNed[NUM_AXIS]; - - double trueCourse; - double rawGroundSpeed; - uint32_t itow; - - float GPSHorizAcc, GPSVertAcc; + double llaAnt[3]; // Antenna Lat, Lon, ellipsoid Altitude, [rad, rad, meter] + double vNedAnt[NUM_AXIS]; // Antenna NED velocity, [m/s, m/s, m/s] + double lla[3]; // IMU Lat, Lon, ellipsoid Altitude, [rad, rad, meter] + double vNed[3]; // IMU NED velocity, [m/s, m/s, m/s] + float geoidAboveEllipsoid; // [m] + real trueCourse; // Antenna heading, [deg] + real rawGroundSpeed; // IMU ground speed, calculated from vNed, [m/s] + float GPSHorizAcc; // [m] + float GPSVertAcc; // [m] float HDOP; + uint8_t gpsFixType; // Indicate if this GNSS measurement is valid + uint8_t numSatellites; /* Num of satellites in this GNSS measurement. + * 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 gEKFInputData; +extern EKF_InputDataStruct gEKFInput; /* Global Algorithm structure */ @@ -113,21 +142,13 @@ typedef struct { uint8_t opMode; uint8_t turnSwitchFlag; uint8_t linAccelSwitch; - uint8_t gpsMeasurementUpdate; } EKF_OutputDataStruct; -extern EKF_OutputDataStruct gEKFOutputData; +extern EKF_OutputDataStruct gEKFOutput; + -typedef struct -{ - BOOL bValid; // tell if stats are valid - BOOL accelErrLimit; // accelErr is set to max/min limit - real lpfAccel[3]; // low-pass filtered accel - real accelNorm; // magnitude of current accel - real accelMean[3]; // average of past n accel - real accelVar[3]; // variance of past n accel - real accelErr[3]; // estimated accel error -} AccelStatsStruct; +// Initialize Kalman filter parameters of the INS app +uint8_t InitINSFilter(void); void EKF_Algorithm(void); void enableFreeIntegration(BOOL enable); @@ -148,7 +169,9 @@ 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_SetOutputStruct(void); #endif /* _EKF_ALGORITHM_H_ */ diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/MotionStatus.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/MotionStatus.h new file mode 100644 index 0000000..785ac68 --- /dev/null +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/MotionStatus.h @@ -0,0 +1,90 @@ +/***************************************************************************** + * @file MotionStatus.h + * @brief Calculate sensor stats, and detect motion status using IMU/ODO/GNSS + * @author Dong Xiaoguang + * @version 1.0 + * @date 20190801 + *****************************************************************************/ + +#ifndef MOTION_STATUS_H_INCLUDED +#define MOTION_STATUS_H_INCLUDED + +#include +#include "GlobalConstants.h" +#include "algorithm.h" + + +typedef struct +{ + BOOL bValid; // tell if stats are valid + BOOL bStaticIMU; // Static period detected by IMU + BOOL accelErrLimit; // accelErr is set to max/min limit + real lpfAccel[3]; // [m/s/s], low-pass filtered accel + real accelNorm; // [m/s/s], magnitude of current accel + real accelMean[3]; // [m/s/s], average of past n accel samples + real accelVar[3]; // [m/s/s]^2, variance of past n accel samples + real accelErr[3]; // [m/s/s], estimated accel error + real lpfGyro[3]; // [rad/s], low-pass filtered gyro + real gyroMean[3]; // [rad/s], average of past n gyro samples + real gyroVar[3]; // [rad/s]^2, variance of past n gyro samples +} ImuStatsStruct; + +/****************************************************************************** + * @brief Calculate IMU data stats, and detect zero velocity. + * TRACE: + * @param [in] gyro [rad/s] + * @param [in] accel [m/s/s] + * @param [in] reset TRUE to reset this process + * @param [Out] imuStats results + * @retval None. +******************************************************************************/ +void MotionStatusImu(real *gyro, real *accel, ImuStatsStruct *imuStats, BOOL reset); + +/****************************************************************************** + * @brief Using gyro propagation to estimate accel error. + * g_dot = -cross(w, g), g is gravity and w is angular rate. + * TRACE: + * @param [in] accel Input accel, m/s/s. + * @param [in] w Input angular rate, rad/s. + * @param [in] dt Sampling interval, sec. + * @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 [out] gAccelStats A struct for results storage. + * @retval None. +******************************************************************************/ +void EstimateAccelError(real *accel, real *w, real dt, uint32_t staticDelay, ImuStatsStruct *gAccelStats); + +/****************************************************************************** + * @brief Detect motion according to the difference between measured accel magnitude and 1g. + * Set gAlgorithm.linAccelSwitch to be True if being static for a while. + * This result is no longer used in current algorithm. + * TRACE: + * @param [in] accelNorm Input accel magnitude, g. + * @param [in] iReset Reset the procedure. + * @retval Always true. +******************************************************************************/ +BOOL DetectMotionFromAccel(real accelNorm, int iReset); + +/****************************************************************************** + * @brief Detect zero velocity using GNSS speed. + * TRACE: + * @param [in] vNED NED velocity measured by GNSS [m/s] + * @param [in] gnssValid Indicate if GNSS measurement is valid. + * If valid, vNED will be used to detect zero velocity. + * If not, detection will be reset and FALSE is always + * returned. + * @retval TRUE if static, otherwise FALSE. +******************************************************************************/ +BOOL DetectStaticGnssVelocity(double *vNED, real threshold, BOOL gnssValid); + +/****************************************************************************** + * @brief Detect zero velocity using odometer data. + * TRACE: + * @param [in] odo velocity measured by odometer [m/s] + * @retval TRUE if static, otherwise FALSE. +******************************************************************************/ +BOOL DetectStaticOdo(real odo); + +#endif // MOTION_STATUS_H_INCLUDED + diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/SensorNoiseParameters.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/SensorNoiseParameters.h index 462161a..5b509d2 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/SensorNoiseParameters.h +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/SensorNoiseParameters.h @@ -10,13 +10,30 @@ #include "GlobalConstants.h" - // Add 25% uncertainty to the noise parameter -#define SENSOR_NOISE_RATE_STD_DEV (1.25 * 1e-1 * D2R) -#define SENSOR_NOISE_ACCEL_STD_DEV 1.25 * 7.0e-4 * GRAVITY -#define SENSOR_NOISE_MAG_STD_DEV 0.00 // todo tm20160603 - ?check this value ? - -#define R_VALS_GPS_TRACK 0.1 - +// IMU spec +#define RW_ODR 100.0 /* [Hz], The data sampling rate when calculate ARW and VRW. + * ARW = sigma * sqrt(dt) = sigma * sqrt(1/ODR) + */ +#define ARW_300ZA 8.73e-5 /* [rad/sqrt(s)], gyro angular random walk, sampled at 100Hz + * 0.3deg/sqrt(Hr) = 0.3 / 60 * D2R = 8.72664625997165e-05rad/sqrt(s) + */ +#define BIW_300ZA 2.91e-5 /* [rad/s], gyro bias instability + * 6.0deg/Hr = 6.0 / 3600 * D2R = 2.90888208665722e-05rad/s + */ +#define MAX_BW 8.73e-3 /* [rad/s], max possible gyro bias + * 0.5deg/s = 0.5 * D2R = 0.00872664625997165rad/s + */ +#define VRW_300ZA 1.0e-3 /* [m/s/sqrt(s)], accel velocity random walk, sampled at 100Hz + * 0.06m/s/sqrt(Hr) = 0.06 / 60 = 0.001m/s/sqrt(s) + */ +#define BIA_300ZA 10.0e-6 * GRAVITY /* [m/s/s], accel bias instability + * 10ug = 10.0e-6g * GRAVITY + */ +#define MAX_BA 3.0e-3 * GRAVITY /* [m/s/s], max possible accel bias + * 3mg = 3.0e-3g * GRAVITY + */ + +// GNSS spec #define R_VALS_GPS_POS_X 5.0 #define R_VALS_GPS_POS_Y 5.0 #define R_VALS_GPS_POS_Z 7.5 diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/TimingVars.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/TimingVars.h index 1799cbc..5f545f0 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/TimingVars.h +++ b/examples/OpenIMU300RI/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/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/algorithm.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/algorithm.h index 47e11f3..8b92c36 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/algorithm.h +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/include/algorithm.h @@ -47,6 +47,12 @@ limitations under the License. #define HIGH_GAIN_AHRS_DURATION 30.0 // 60.0 * SAMPLING_RATE #define LOW_GAIN_AHRS_DURATION 30.0 // 30.0 * SAMPLING_RATE +// Define heading initialization reliability +#define HEADING_UNINITIALIZED 0 +#define HEADING_MAG 1 +#define HEADING_GNSS_LOW 2 +#define HEADING_GNSS_HIGH 3 + typedef struct { uint32_t Stabilize_System; // SAMPLING_RATE * 0.36 uint32_t Initialize_Attitude; // SAMPLING_RATE * ( 1.0 - 0.36 ) @@ -61,13 +67,16 @@ typedef struct { } InnovationStruct; typedef struct { - int32_t Max_GPS_Drop_Time; // [msec] + int32_t maxGpsDropTime; // [msec] + int32_t maxReliableDRTime; /* [msec] When GPS outage duration exceeds this limit, + * the position and velocity will be reinitialized when GPS + * is available again. Otherwise, the fusion algorithm will + * gradually correct the position and velocity to the GPS. + */ int32_t Max_Rest_Time_Before_Drop_To_AHRS; // [msec] 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; @@ -76,14 +85,16 @@ typedef struct { } 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 @@ -95,15 +106,25 @@ union AlgoBehavior // Algorithm states struct ALGO_STATUS_BITS { - uint16_t algorithmInit : 1; // 0 algorithm initialization - uint16_t highGain : 1; // 1 high gain mode + uint16_t algorithmInit : 1; // 0 algorithm initialization + 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 turnSwitch : 1; // 3 turn switch + 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 @@ -114,57 +135,92 @@ typedef union ALGO_STATUS extern AlgoStatus gAlgoStatus; +typedef struct +{ + real arw; // [rad/sqrt(s)], gyro angle random walk + real sigmaW; // [rad/s], gyro noise std + real biW; // [rad/s], gyro bias instability + real maxBiasW; // [rad/s], max possible gyro bias + real vrw; // [m/s/sqrt(s)], accel velocity random walk + real sigmaA; // [m/s/s], accel noise std + real biA; // [m/s/s], accel bias instability + real maxBiasA; // [m/s/s], max possible accel bias +} IMU_SPEC; + +typedef struct +{ + real staticVarGyro; // [rad/s]^2 + real staticVarAccel; // [m/s/s]^2 + real maxGyroBias; // [rad/s] + real staticGnssVel; // [m/s] + real staticNoiseMultiplier[3]; /* Use IMU noise level and gyro output to detect static period. + * The nominal noise level and max gyro bias of an IMU is defined in + * SensorNoiseParameters.h. These parameters are determined by IMU + * output when static and are hightly related to ARW and VRW. + * When IMU is installed on a vehicle, its noise level when + * vehicle is static could be higher than the nominal noise + * level due to vibration. This setting is used to scale + * the nominal noise level and gyro bias for static detection. + * [scale_gyro_var, scale_accel_var, scale_gyro_bias] + */ +} STATIC_DETECT_SETTING; + + /* Global Algorithm structure */ typedef struct { - uint32_t itow; - uint32_t dITOW; + uint32_t itow; + uint32_t dITOW; + + // control the stage of operation for the algorithms + uint32_t stateTimer; + uint8_t state; // takes values from HARDWARE_STABILIZE to INIT_ATTITUDE to HG_AHRS - // control the stage of operation for the algorithms - uint32_t stateTimer; - uint8_t state; // takes values from HARDWARE_STABILIZE to INIT_ATTITUDE to HG_AHRS + uint8_t insFirstTime; + uint8_t headingIni; + uint8_t applyDeclFlag; - uint8_t insFirstTime; - uint8_t gnssHeadingFirstTime; - uint8_t applyDeclFlag; + int32_t timeOfLastSufficientGPSVelocity; + int32_t timeOfLastGoodGPSReading; - int32_t timeOfLastSufficientGPSVelocity; - int32_t timeOfLastGoodGPSReading; + real filteredYawRate; // Yaw-Rate (Turn-Switch) filter - real rGPS_N[3]; // current IMU position (lever-arm) w.r.t rGPS0_E in NED. - real R_NinE[3][3]; // convert NED to ECEF - double rGPS0_E[3]; // Initial antenna ECEF position when first entering INS state - double rGPS_E[3]; // current antenna ECEF position + /* The following variables are used to increase the Kalman filter gain when the + * acceleration is very close to one (i.e. the system is at rest) + */ + uint32_t linAccelSwitchCntr; + uint8_t linAccelSwitch; - real filteredYawRate; // Yaw-Rate (Turn-Switch) filter + uint8_t linAccelLPFType; + uint8_t useRawAccToDetectLinAccel; - /* The following variables are used to increase the Kalman filter gain when the - * acceleration is very close to one (i.e. the system is at rest) - */ - uint32_t linAccelSwitchCntr; - uint8_t linAccelSwitch; + uint8_t callingFreq; + real dt; + real dtOverTwo; + real dtSquared; + real sqrtDt; - uint8_t linAccelLPFType; - uint8_t useRawAccToDetectLinAccel; + volatile uint32_t timer; // timer since power up (ms) + volatile uint16_t counter; // inc. with every continuous mode output packet - uint8_t callingFreq; - real dt; - real dtOverTwo; - real dtSquared; - real sqrtDt; + union AlgoBehavior Behavior; + float turnSwitchThreshold; // 0, 0.4, 10 driving, 1 flying [deg/sec] 0x000d - volatile uint32_t timer; // timer since power up (ms) - volatile uint16_t counter; // inc. with every continuous mode output packet + real leverArmB[3]; // Antenna position w.r.t IMU in vehicle body frame + real pointOfInterestB[3]; // Point of interest position w.r.t IMU in vehicle body frame - union AlgoBehavior Behavior; - float turnSwitchThreshold; // 0, 0.4, 10 driving, 1 flying [deg/sec] 0x000d + BOOL velocityAlwaysAlongBodyX; // enable zero velocity update - real leverArmB[3]; // Antenna position w.r.t IMU in vehicle body frame - real pointOfInterestB[3]; // Point of interest position w.r.t IMU in vehicle body frame + IMU_SPEC imuSpec; // IMU specifications + STATIC_DETECT_SETTING staticDetectParam; // params used for static detection - DurationStruct Duration; - LimitStruct Limit; + DurationStruct Duration; + LimitStruct Limit; } 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/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/EKF_Algorithm.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/EKF_Algorithm.c index c3faadf..8e79f6b 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/EKF_Algorithm.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/EKF_Algorithm.c @@ -7,17 +7,19 @@ #include // std::abs #include // EXIT_FAILURE #include +#include // memset #include "GlobalConstants.h" // TRUE, FALSE, etc #include "platformAPI.h" #include "Indices.h" // IND #include "MagAlign.h" +#include "QuaternionMath.h" #include "algorithm.h" // gAlgorithm #include "AlgorithmLimits.h" #include "TransformationMath.h" #include "VectorMath.h" -#include "buffer.h" +#include "MotionStatus.h" #include "SelectState.h" #include "EKF_Algorithm.h" #include "PredictFunctions.h" @@ -31,88 +33,36 @@ #endif #endif -KalmanFilterStruct gKalmanFilter; -EKF_InputDataStruct gEKFInputData; -EKF_OutputDataStruct gEKFOutputData; -AccelStatsStruct gAccelStats; - -//============================================================================= -// Filter variables (Third-Order BWF w/ default 5 Hz Cutoff) -#define FILTER_ORDER 3 - -#define CURRENT 0 -#define PASTx1 1 -#define PASTx2 2 -#define PASTx3 3 -/* Replace this with a fuction that will compute the coefficients so the - * input is the cutoff frequency in Hertz - */ -#define NO_LPF 0 -#define TWO_HZ_LPF 1 -#define FIVE_HZ_LPF 2 -#define TEN_HZ_LPF 3 -#define TWENTY_HZ_LPF 4 -#define TWENTY_FIVE_HZ_LPF 5 -#define N_LPF 6 - -/****************************************************************************** - * @brief Get filter coefficients of a 3rd Butterworth low-pass filter. - * For now only a few specific cut-off frequencies are supported. - * TRACE: - * @param [in] lpfType Low-pass filter cut-off frequency. - * @param [in] callingFreq Sampling frequency, only 100Hz and 200Hz are supported. - * @param [out] b coefficients of the numerator of the filter. - * @param [out] a coefficients of the denominator of the filter. - * @retval None. -******************************************************************************/ -static void _PopulateFilterCoefficients( uint8_t lpfType, uint8_t callingFreq, real *b, real *a ); - -/****************************************************************************** - * @brief Process input data through a low-pass Butterworth filter. - * TRACE: - * @param [in] accel Input accel - * @param [in] lpfType Low-pass filter cut-off frequency. - * @param [in] callingFreq Sampling frequency, only 100Hz and 200Hz are supported. - * @param [out] filteredAccel Filter accel. - * @retval None. -******************************************************************************/ -static void _AccelLPF( real *accel, uint8_t lpfType, uint8_t callingFreq, real *filteredAccel ); - -#define DATA_NUM_FOR_STATS 20 // 20 samples can give a relative good estimate of var -/****************************************************************************** - * @brief Compute mean and var of the input data. - * A recursive mean calculation and a naive var calculation are implemented. A recursive var - * calculation was also tested but abandoned due to its inherent numerical stability. - * TRACE: - * @param [in] accel Input accel, g. - * @param [in] iReset Reset the procedure. - * @param [out] gAccelStats A struct for results storage. - * @retval None. -******************************************************************************/ -static void ComputeAccelStats( real *accel, AccelStatsStruct *gAccelStats, int iReset ); +KalmanFilterStruct gKalmanFilter; +EKF_InputDataStruct gEKFInput; +EKF_OutputDataStruct gEKFOutput; +ImuStatsStruct gImuStats; /****************************************************************************** - * @brief Using gyro propagation to estimate accel error. - * g_dot = -cross(w, g), g is gravity and w is angular rate. + * @brief Remove lever arm in position and velocity. + * GNSS measured position is the position of the antenna. For GNSS/INS integration, + * the position of IMU is needed. Before using the GNSS position and velocity, + * those shold be first converted to the IMU position and velocity by removing + * lever arm effects. Lever arm introduces an offset in position. This offset + * can be directly canceled by substracting lever arm. Combined with angular + * velocity, lever arm further introduces relative velocity. * TRACE: - * @param [in] accel Input accel, g. - * @param [in] w Input angular rate, rad/s. - * @param [in] dt Sampling interval, sec. - * @param [out] gAccelStats A struct for results storage. - * @retval None. + * @param [in/out] LLA [rad, rad, m], Lat, lon and alt of the antenna, and + * will be converted to LLA of the IMU. + * @param [in/out] vNed [m/s], NED velocity of the antenna, and will be + * converted to NED velocity of the IMU. + * @param [in] w [rad/s], angular velocity of the vehicle relative to + * ECEF in the body frame. + * @param [in] leverArmB [m], lever arm in the body frame. + * @param [out] rn2e Transformation matrix from NED to ECEF. + * @param [out] ecef [m], ECEF position without lever arm. + * @retval TRUE if heading initialized, FALSE if not. ******************************************************************************/ -static void EstimateAccelError( real *accel, real *w, real dt, AccelStatsStruct *gAccelStats); +static void RemoveLeverArm(double *lla, double *vNed, real *w, real *leverArmB, + real *rn2e, double *ecef); -/****************************************************************************** - * @brief Detect motion according to the difference between measured accel magnitude and 1g. - * Set gAlgorithm.linAccelSwitch to be True if being static for a while. - * This result is no longer used in current algorithm. - * TRACE: - * @param [in] accelNorm Input accel magnitude, g. - * @param [in] iReset Reset the procedure. - * @retval Always true. -******************************************************************************/ -static BOOL DetectMotionFromAccel(real accelNorm, int iReset); +static void HandlePps(); +static void SaveKfStateAtPps(); //============================================================================= @@ -131,39 +81,40 @@ void EKF_Algorithm(void) */ if ( gAlgorithm.state > STABILIZE_SYSTEM ) { - // Low-pass filter - _AccelLPF( gEKFInputData.accel_B, - gAlgorithm.linAccelLPFType, - gAlgorithm.callingFreq, - gAccelStats.lpfAccel ); - // Compute accel norm using raw accel data. The norm will be used to detect static periods. - gAccelStats.accelNorm = sqrtf(gEKFInputData.accel_B[X_AXIS] * gEKFInputData.accel_B[X_AXIS] + - gEKFInputData.accel_B[Y_AXIS] * gEKFInputData.accel_B[Y_AXIS] + - gEKFInputData.accel_B[Z_AXIS] * gEKFInputData.accel_B[Z_AXIS] ); - /* Compute accel mean/var. - * Compute this after STABILIZE_SYSTEM so there is enough data to fill the buffer - * Before buffer is full, results are not accurate and not used indeed. + /* Compute IMU mean/var, filter IMU data (optional), detect static. + * After STABILIZE_SYSTEM, each IMU sample is pushed into a buffer. + * Before the buffer is full, results are not accurate should not be used. */ - ComputeAccelStats(gAccelStats.lpfAccel, &gAccelStats, 0); + MotionStatusImu(gEKFInput.angRate_B, gEKFInput.accel_B, &gImuStats, FALSE); // estimate accel error if (gAlgorithm.useRawAccToDetectLinAccel) { - EstimateAccelError( gEKFInputData.accel_B, - gEKFInputData.angRate_B, + EstimateAccelError( gEKFInput.accel_B, + gEKFInput.angRate_B, gAlgorithm.dt, - &gAccelStats); + gAlgorithm.Limit.linAccelSwitchDelay, + &gImuStats); } else { - EstimateAccelError( gAccelStats.lpfAccel, - gEKFInputData.angRate_B, + EstimateAccelError( gImuStats.lpfAccel, + gEKFInput.angRate_B, gAlgorithm.dt, - &gAccelStats); + gAlgorithm.Limit.linAccelSwitchDelay, + &gImuStats); } // Detect if the unit is static or dynamic - DetectMotionFromAccel(gAccelStats.accelNorm, 0); + 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 @@ -173,7 +124,12 @@ void EKF_Algorithm(void) gAlgorithm.itow = gAlgorithm.itow + gAlgorithm.dITOW; // Perform EKF Prediction - EKF_PredictionStage(gAccelStats.lpfAccel); + 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. @@ -207,6 +163,7 @@ void EKF_Algorithm(void) // Perform EKF Update EKF_UpdateStage(); } + /* Save the past attitude quaternion before updating (for use in the * covariance estimation calculations). */ @@ -214,6 +171,41 @@ void EKF_Algorithm(void) gKalmanFilter.quaternion_Past[Q1] = gKalmanFilter.quaternion[Q1]; gKalmanFilter.quaternion_Past[Q2] = gKalmanFilter.quaternion[Q2]; gKalmanFilter.quaternion_Past[Q3] = gKalmanFilter.quaternion[Q3]; + + /* Generate the transformation matrix (R_BinN) based on the past value of + * the attitude quaternion (prior to prediction at the new time-step) + */ + QuaternionToR321(gKalmanFilter.quaternion, &gKalmanFilter.R_BinN[0][0]); + + /* Euler angels are not calcualted here because it is used as a propagation resutls + * to calculate system innovation. So, Euler angles are updated in the prediction + * stage. In theory, Euler angles should be updated after each measurement update. + * However, after EKF converges, it does not matter. + */ + + // Update LLA + if ((gAlgorithm.insFirstTime == FALSE)) + { + double r_E[NUM_AXIS]; + float pointOfInterestN[3]; + pointOfInterestN[0] = gKalmanFilter.R_BinN[0][0] * gAlgorithm.pointOfInterestB[0] + + gKalmanFilter.R_BinN[0][1] * gAlgorithm.pointOfInterestB[1] + + gKalmanFilter.R_BinN[0][2] * gAlgorithm.pointOfInterestB[2]; + pointOfInterestN[1] = gKalmanFilter.R_BinN[1][0] * gAlgorithm.pointOfInterestB[0] + + gKalmanFilter.R_BinN[1][1] * gAlgorithm.pointOfInterestB[1] + + gKalmanFilter.R_BinN[1][2] * gAlgorithm.pointOfInterestB[2]; + pointOfInterestN[2] = gKalmanFilter.R_BinN[2][0] * gAlgorithm.pointOfInterestB[0] + + gKalmanFilter.R_BinN[2][1] * gAlgorithm.pointOfInterestB[1] + + gKalmanFilter.R_BinN[2][2] * gAlgorithm.pointOfInterestB[2]; + pointOfInterestN[0] += gKalmanFilter.Position_N[0]; + pointOfInterestN[1] += gKalmanFilter.Position_N[1]; + pointOfInterestN[2] += gKalmanFilter.Position_N[2]; + PosNED_To_PosECEF(pointOfInterestN, &gKalmanFilter.rGPS0_E[0], &gKalmanFilter.Rn2e[0][0], &r_E[0]); + // 100 Hz generated once 1 Hz 100 Hz + // output variable (ned used for anything else); this is in [ deg, deg, m ] + ECEF_To_LLA(&gKalmanFilter.llaDeg[LAT], &r_E[X_AXIS]); + // 100 Hz 100 Hz + } } /* Select the algorithm state based upon the present state as well as @@ -239,14 +231,6 @@ void EKF_Algorithm(void) INS_To_AHRS_Transition_Test(); break; default: -#ifdef DISPLAY_DIAGNOSTIC_MSG -#ifdef INS_OFFLINE - // Shouldn't be able to make it here - TimingVars_DiagnosticMsg("Uh-oh! Invalid algorithm state in EKF_Algorithm.cpp"); - std::cout << "Press enter to finish ..."; - std::cin.get(); -#endif -#endif return; } @@ -254,46 +238,67 @@ void EKF_Algorithm(void) DynamicMotion(); } -void enableFreeIntegration(BOOL enable) -{ - gAlgorithm.Behavior.bit.freeIntegrate = enable; -} - - -BOOL freeIntegrationEnabled() +static void HandlePps() { - return 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 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() /* Extract the attitude (expressed as Euler-angles) of the body-frame (B) @@ -302,9 +307,9 @@ void enableGpsInAlgorithm(BOOL enable) void EKF_GetAttitude_EA(real *EulerAngles) { // Euler-angles in [deg] - EulerAngles[ROLL] = (real)gEKFOutputData.eulerAngs_BinN[ROLL]; - EulerAngles[PITCH] = (real)gEKFOutputData.eulerAngs_BinN[PITCH]; - EulerAngles[YAW] = (real)gEKFOutputData.eulerAngs_BinN[YAW]; + EulerAngles[ROLL] = (real)gEKFOutput.eulerAngs_BinN[ROLL]; + EulerAngles[PITCH] = (real)gEKFOutput.eulerAngs_BinN[PITCH]; + EulerAngles[YAW] = (real)gEKFOutput.eulerAngs_BinN[YAW]; } @@ -322,10 +327,10 @@ void EKF_GetAttitude_EA_RAD(real *EulerAngles) */ void EKF_GetAttitude_Q(real *Quaternions) { - Quaternions[Q0] = (real)gEKFOutputData.quaternion_BinN[Q0]; - Quaternions[Q1] = (real)gEKFOutputData.quaternion_BinN[Q1]; - Quaternions[Q2] = (real)gEKFOutputData.quaternion_BinN[Q2]; - Quaternions[Q3] = (real)gEKFOutputData.quaternion_BinN[Q3]; + Quaternions[Q0] = (real)gEKFOutput.quaternion_BinN[Q0]; + Quaternions[Q1] = (real)gEKFOutput.quaternion_BinN[Q1]; + Quaternions[Q2] = (real)gEKFOutput.quaternion_BinN[Q2]; + Quaternions[Q3] = (real)gEKFOutput.quaternion_BinN[Q3]; } @@ -335,9 +340,9 @@ void EKF_GetAttitude_Q(real *Quaternions) void EKF_GetCorrectedAngRates(real *CorrAngRates_B) { // Angular-rate in [deg/s] - CorrAngRates_B[X_AXIS] = (real)gEKFOutputData.corrAngRates_B[X_AXIS]; - CorrAngRates_B[Y_AXIS] = (real)gEKFOutputData.corrAngRates_B[Y_AXIS]; - CorrAngRates_B[Z_AXIS] = (real)gEKFOutputData.corrAngRates_B[Z_AXIS]; + CorrAngRates_B[X_AXIS] = (real)gEKFOutput.corrAngRates_B[X_AXIS]; + CorrAngRates_B[Y_AXIS] = (real)gEKFOutput.corrAngRates_B[Y_AXIS]; + CorrAngRates_B[Z_AXIS] = (real)gEKFOutput.corrAngRates_B[Z_AXIS]; } @@ -347,9 +352,9 @@ void EKF_GetCorrectedAngRates(real *CorrAngRates_B) void EKF_GetCorrectedAccels(real *CorrAccels_B) { // Acceleration in [m/s^2] - CorrAccels_B[X_AXIS] = (real)gEKFOutputData.corrAccel_B[X_AXIS]; - CorrAccels_B[Y_AXIS] = (real)gEKFOutputData.corrAccel_B[Y_AXIS]; - CorrAccels_B[Z_AXIS] = (real)gEKFOutputData.corrAccel_B[Z_AXIS]; + CorrAccels_B[X_AXIS] = (real)gEKFOutput.corrAccel_B[X_AXIS]; + CorrAccels_B[Y_AXIS] = (real)gEKFOutput.corrAccel_B[Y_AXIS]; + CorrAccels_B[Z_AXIS] = (real)gEKFOutput.corrAccel_B[Z_AXIS]; } @@ -359,9 +364,9 @@ void EKF_GetCorrectedAccels(real *CorrAccels_B) void EKF_GetEstimatedAngRateBias(real *AngRateBias_B) { // Angular-rate bias in [deg/sec] - AngRateBias_B[X_AXIS] = (real)gEKFOutputData.angRateBias_B[X_AXIS]; - AngRateBias_B[Y_AXIS] = (real)gEKFOutputData.angRateBias_B[Y_AXIS]; - AngRateBias_B[Z_AXIS] = (real)gEKFOutputData.angRateBias_B[Z_AXIS]; + AngRateBias_B[X_AXIS] = (real)gEKFOutput.angRateBias_B[X_AXIS]; + AngRateBias_B[Y_AXIS] = (real)gEKFOutput.angRateBias_B[Y_AXIS]; + AngRateBias_B[Z_AXIS] = (real)gEKFOutput.angRateBias_B[Z_AXIS]; } @@ -371,9 +376,9 @@ void EKF_GetEstimatedAngRateBias(real *AngRateBias_B) void EKF_GetEstimatedAccelBias(real *AccelBias_B) { // Acceleration-bias in [m/s^2] - AccelBias_B[X_AXIS] = (real)gEKFOutputData.accelBias_B[X_AXIS]; - AccelBias_B[Y_AXIS] = (real)gEKFOutputData.accelBias_B[Y_AXIS]; - AccelBias_B[Z_AXIS] = (real)gEKFOutputData.accelBias_B[Z_AXIS]; + AccelBias_B[X_AXIS] = (real)gEKFOutput.accelBias_B[X_AXIS]; + AccelBias_B[Y_AXIS] = (real)gEKFOutput.accelBias_B[Y_AXIS]; + AccelBias_B[Z_AXIS] = (real)gEKFOutput.accelBias_B[Z_AXIS]; } @@ -381,9 +386,9 @@ void EKF_GetEstimatedAccelBias(real *AccelBias_B) void EKF_GetEstimatedPosition(real *Position_N) { // Position in [m] - Position_N[X_AXIS] = (real)gEKFOutputData.position_N[X_AXIS]; - Position_N[Y_AXIS] = (real)gEKFOutputData.position_N[Y_AXIS]; - Position_N[Z_AXIS] = (real)gEKFOutputData.position_N[Z_AXIS]; + Position_N[X_AXIS] = (real)gEKFOutput.position_N[X_AXIS]; + Position_N[Y_AXIS] = (real)gEKFOutput.position_N[Y_AXIS]; + Position_N[Z_AXIS] = (real)gEKFOutput.position_N[Z_AXIS]; } @@ -391,9 +396,9 @@ void EKF_GetEstimatedPosition(real *Position_N) void EKF_GetEstimatedVelocity(real *Velocity_N) { // Velocity in [m/s] - Velocity_N[X_AXIS] = (real)gEKFOutputData.velocity_N[X_AXIS]; - Velocity_N[Y_AXIS] = (real)gEKFOutputData.velocity_N[Y_AXIS]; - Velocity_N[Z_AXIS] = (real)gEKFOutputData.velocity_N[Z_AXIS]; + Velocity_N[X_AXIS] = (real)gEKFOutput.velocity_N[X_AXIS]; + Velocity_N[Y_AXIS] = (real)gEKFOutput.velocity_N[Y_AXIS]; + Velocity_N[Z_AXIS] = (real)gEKFOutput.velocity_N[Z_AXIS]; } @@ -401,9 +406,9 @@ void EKF_GetEstimatedVelocity(real *Velocity_N) void EKF_GetEstimatedLLA(double *LLA) { // Velocity in [m/s] - LLA[X_AXIS] = (double)gEKFOutputData.llaDeg[X_AXIS]; - LLA[Y_AXIS] = (double)gEKFOutputData.llaDeg[Y_AXIS]; - LLA[Z_AXIS] = (double)gEKFOutputData.llaDeg[Z_AXIS]; + LLA[X_AXIS] = (double)gEKFOutput.llaDeg[X_AXIS]; + LLA[Y_AXIS] = (double)gEKFOutput.llaDeg[Y_AXIS]; + LLA[Z_AXIS] = (double)gEKFOutput.llaDeg[Z_AXIS]; } @@ -416,69 +421,123 @@ void EKF_GetEstimatedLLA(double *LLA) */ void EKF_GetOperationalMode(uint8_t *EKF_OperMode) { - *EKF_OperMode = gEKFOutputData.opMode; + *EKF_OperMode = gEKFOutput.opMode; } // Extract the linear-acceleration and turn-switch flags void EKF_GetOperationalSwitches(uint8_t *EKF_LinAccelSwitch, uint8_t *EKF_TurnSwitch) { - *EKF_LinAccelSwitch = gEKFOutputData.linAccelSwitch; - *EKF_TurnSwitch = gEKFOutputData.turnSwitchFlag; + *EKF_LinAccelSwitch = gEKFOutput.linAccelSwitch; + *EKF_TurnSwitch = gEKFOutput.turnSwitchFlag; } // 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 [g] - gEKFInputData.accel_B[X_AXIS] = (real)accels[X_AXIS]; - gEKFInputData.accel_B[Y_AXIS] = (real)accels[Y_AXIS]; - gEKFInputData.accel_B[Z_AXIS] = (real)accels[Z_AXIS]; + // Accelerometer signal is in [m/s/s] + gEKFInput.accel_B[X_AXIS] = (real)accels[X_AXIS] * GRAVITY; + gEKFInput.accel_B[Y_AXIS] = (real)accels[Y_AXIS] * GRAVITY; + gEKFInput.accel_B[Z_AXIS] = (real)accels[Z_AXIS] * GRAVITY; // Angular-rate signal is in [rad/s] - gEKFInputData.angRate_B[X_AXIS] = (real)rates[X_AXIS]; - gEKFInputData.angRate_B[Y_AXIS] = (real)rates[Y_AXIS]; - gEKFInputData.angRate_B[Z_AXIS] = (real)rates[Z_AXIS]; + gEKFInput.angRate_B[X_AXIS] = (real)rates[X_AXIS]; + gEKFInput.angRate_B[Y_AXIS] = (real)rates[Y_AXIS]; + gEKFInput.angRate_B[Z_AXIS] = (real)rates[Z_AXIS]; // Magnetometer signal is in [G] - gEKFInputData.magField_B[X_AXIS] = (real)mags[X_AXIS]; - gEKFInputData.magField_B[Y_AXIS] = (real)mags[Y_AXIS]; - gEKFInputData.magField_B[Z_AXIS] = (real)mags[Z_AXIS]; + gEKFInput.magField_B[X_AXIS] = (real)mags[X_AXIS]; + gEKFInput.magField_B[Y_AXIS] = (real)mags[Y_AXIS]; + gEKFInput.magField_B[Z_AXIS] = (real)mags[Z_AXIS]; real tmp[2]; - tmp[X_AXIS] = gEKFInputData.magField_B[X_AXIS] - gMagAlign.hardIronBias[X_AXIS]; - tmp[Y_AXIS] = gEKFInputData.magField_B[Y_AXIS] - gMagAlign.hardIronBias[Y_AXIS]; - gEKFInputData.magField_B[X_AXIS] = gMagAlign.SF[0] * tmp[X_AXIS] + gMagAlign.SF[1] * tmp[Y_AXIS]; - gEKFInputData.magField_B[Y_AXIS] = gMagAlign.SF[2] * tmp[X_AXIS] + gMagAlign.SF[3] * tmp[Y_AXIS]; + tmp[X_AXIS] = gEKFInput.magField_B[X_AXIS] - gMagAlign.hardIronBias[X_AXIS]; + tmp[Y_AXIS] = gEKFInput.magField_B[Y_AXIS] - gMagAlign.hardIronBias[Y_AXIS]; + gEKFInput.magField_B[X_AXIS] = gMagAlign.SF[0] * tmp[X_AXIS] + gMagAlign.SF[1] * tmp[Y_AXIS]; + gEKFInput.magField_B[Y_AXIS] = gMagAlign.SF[2] * tmp[X_AXIS] + gMagAlign.SF[3] * tmp[Y_AXIS]; // ----- Input from the GPS goes here ----- - // Validity data - gEKFInputData.gpsValid = (BOOL)gps->gpsValid; - gEKFInputData.gpsUpdate = gps->gpsUpdate; - - // Lat/Lon/Alt data - gEKFInputData.llaRad[LAT] = gps->latitude * D2R; - gEKFInputData.llaRad[LON] = gps->longitude * D2R; - gEKFInputData.llaRad[ALT] = gps->altitude; - - // Velocity data - gEKFInputData.vNed[X_AXIS] = gps->vNed[X_AXIS]; - gEKFInputData.vNed[Y_AXIS] = gps->vNed[Y_AXIS]; - gEKFInputData.vNed[Z_AXIS] = gps->vNed[Z_AXIS]; - - // Course and velocity data - gEKFInputData.rawGroundSpeed = gps->rawGroundSpeed; - gEKFInputData.trueCourse = gps->trueCourse; - - // ITOW data - gEKFInputData.itow = gps->itow; - - // Data quality measures - gEKFInputData.GPSHorizAcc = gps->GPSHorizAcc; - gEKFInputData.GPSVertAcc = gps->GPSVertAcc; - gEKFInputData.HDOP = gps->HDOP; + gEKFInput.gpsUpdate = gps->gpsUpdate; + + if (gEKFInput.gpsUpdate) + { + // Validity data + gEKFInput.gpsFixType = gps->gpsFixType; + + // num of satellites + gEKFInput.numSatellites = gps->numSatellites; + + // ITOW data + gEKFInput.itow = gps->itow; + + // Data quality measures + gEKFInput.GPSHorizAcc = gps->GPSHorizAcc; + gEKFInput.GPSVertAcc = gps->GPSVertAcc; + gEKFInput.HDOP = gps->HDOP; + + // Lat/Lon/Alt data + gEKFInput.llaAnt[LAT] = gps->latitude * D2R; + gEKFInput.llaAnt[LON] = gps->longitude * D2R; + gEKFInput.llaAnt[ALT] = gps->altitude; + gEKFInput.geoidAboveEllipsoid = gps->geoidAboveEllipsoid; + + // Velocity data + gEKFInput.vNedAnt[X_AXIS] = gps->vNed[X_AXIS]; + gEKFInput.vNedAnt[Y_AXIS] = gps->vNed[Y_AXIS]; + gEKFInput.vNedAnt[Z_AXIS] = gps->vNed[Z_AXIS]; + + // Course and velocity data + 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 + * from the body frame to the NED frame. Before heading initialized, lever arm cannot + * be correctly removed. After heading initialized, there would be position jump if + * initial heading is different from uninitlized one and the lever arm is large. + * After heading intialized, the position/velocity could also be reinitialized, and + * lever arm effects on the position/velocity are not corrected removed. + * LLA without lever arm is used to update Rn2e/ECEF postion, and calculate relative + * position in NED + */ + if (gEKFInput.gpsFixType) + { + gEKFInput.lla[LAT] = gEKFInput.llaAnt[LAT]; + gEKFInput.lla[LON] = gEKFInput.llaAnt[LON]; + gEKFInput.lla[ALT] = gEKFInput.llaAnt[ALT]; + gEKFInput.vNed[0] = gEKFInput.vNedAnt[0]; + gEKFInput.vNed[1] = gEKFInput.vNedAnt[1]; + gEKFInput.vNed[2] = gEKFInput.vNedAnt[2]; + /* remove lever arm. Indeed, corrected angular rate should be used. Considering angular + * bias is small, raw angular rate is used. + */ + RemoveLeverArm( gEKFInput.lla, + gEKFInput.vNed, + gEKFInput.angRate_B, + gAlgorithm.leverArmB, + &gKalmanFilter.Rn2e[0][0], + gKalmanFilter.rGPS_E); + + /* Calculate relative position in the NED frame. The initial position is rGPS0_E which. + * is determined when the algorithm first enters the INS mode (InitINSFilter). + */ + ECEF_To_Base( &gKalmanFilter.rGPS0_E[0], + &gKalmanFilter.rGPS_E[0], + &gKalmanFilter.Rn2e[0][0], + &gKalmanFilter.rGPS_N[0]); + } + } + + // odometer + gEKFInput.odoUpdate = odo->update; + gEKFInput.odoVelocity = odo->v; + + // 1PPS signal from GNSS receiver + gEKFInput.ppsDetected = ppsDetected; } @@ -488,563 +547,236 @@ void EKF_SetOutputStruct(void) // ------------------ States ------------------ // Position in [m] - gEKFOutputData.position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS]; - gEKFOutputData.position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS]; - gEKFOutputData.position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS]; + gEKFOutput.position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS]; + gEKFOutput.position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS]; + gEKFOutput.position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS]; // Velocity in [m/s] - gEKFOutputData.velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS]; - gEKFOutputData.velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS]; - gEKFOutputData.velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS]; + gEKFOutput.velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS]; + gEKFOutput.velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS]; + gEKFOutput.velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS]; // Position in [N/A] - gEKFOutputData.quaternion_BinN[Q0] = gKalmanFilter.quaternion[Q0]; - gEKFOutputData.quaternion_BinN[Q1] = gKalmanFilter.quaternion[Q1]; - gEKFOutputData.quaternion_BinN[Q2] = gKalmanFilter.quaternion[Q2]; - gEKFOutputData.quaternion_BinN[Q3] = gKalmanFilter.quaternion[Q3]; + gEKFOutput.quaternion_BinN[Q0] = gKalmanFilter.quaternion[Q0]; + gEKFOutput.quaternion_BinN[Q1] = gKalmanFilter.quaternion[Q1]; + gEKFOutput.quaternion_BinN[Q2] = gKalmanFilter.quaternion[Q2]; + gEKFOutput.quaternion_BinN[Q3] = gKalmanFilter.quaternion[Q3]; // Angular-rate bias in [deg/sec] - gEKFOutputData.angRateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] * RAD_TO_DEG; - gEKFOutputData.angRateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] * RAD_TO_DEG; - gEKFOutputData.angRateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] * RAD_TO_DEG; + gEKFOutput.angRateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] * RAD_TO_DEG; + gEKFOutput.angRateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] * RAD_TO_DEG; + gEKFOutput.angRateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] * RAD_TO_DEG; // Acceleration-bias in [m/s^2] - gEKFOutputData.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS]; - gEKFOutputData.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS]; - gEKFOutputData.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS]; + gEKFOutput.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS]; + gEKFOutput.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS]; + gEKFOutput.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS]; // ------------------ Derived variables ------------------ // Euler-angles in [deg] - gEKFOutputData.eulerAngs_BinN[ROLL] = gKalmanFilter.eulerAngles[ROLL] * RAD_TO_DEG; - gEKFOutputData.eulerAngs_BinN[PITCH] = gKalmanFilter.eulerAngles[PITCH] * RAD_TO_DEG; - gEKFOutputData.eulerAngs_BinN[YAW] = gKalmanFilter.eulerAngles[YAW] * RAD_TO_DEG; + gEKFOutput.eulerAngs_BinN[ROLL] = gKalmanFilter.eulerAngles[ROLL] * RAD_TO_DEG; + gEKFOutput.eulerAngs_BinN[PITCH] = gKalmanFilter.eulerAngles[PITCH] * RAD_TO_DEG; + gEKFOutput.eulerAngs_BinN[YAW] = gKalmanFilter.eulerAngles[YAW] * RAD_TO_DEG; // Angular-rate in [deg/s] - gEKFOutputData.corrAngRates_B[X_AXIS] = ( gEKFInputData.angRate_B[X_AXIS] - + gEKFOutput.corrAngRates_B[X_AXIS] = ( gEKFInput.angRate_B[X_AXIS] - gKalmanFilter.rateBias_B[X_AXIS] ) * RAD_TO_DEG; - gEKFOutputData.corrAngRates_B[Y_AXIS] = ( gEKFInputData.angRate_B[Y_AXIS] - + gEKFOutput.corrAngRates_B[Y_AXIS] = ( gEKFInput.angRate_B[Y_AXIS] - gKalmanFilter.rateBias_B[Y_AXIS] ) * RAD_TO_DEG; - gEKFOutputData.corrAngRates_B[Z_AXIS] = ( gEKFInputData.angRate_B[Z_AXIS] - + gEKFOutput.corrAngRates_B[Z_AXIS] = ( gEKFInput.angRate_B[Z_AXIS] - gKalmanFilter.rateBias_B[Z_AXIS] ) * RAD_TO_DEG; // Acceleration in [m/s^2] - gEKFOutputData.corrAccel_B[X_AXIS] = gEKFInputData.accel_B[X_AXIS] * GRAVITY - - gKalmanFilter.accelBias_B[X_AXIS]; - gEKFOutputData.corrAccel_B[Y_AXIS] = gEKFInputData.accel_B[Y_AXIS] * GRAVITY - - gKalmanFilter.accelBias_B[Y_AXIS]; - gEKFOutputData.corrAccel_B[Z_AXIS] = gEKFInputData.accel_B[Z_AXIS] * GRAVITY - - gKalmanFilter.accelBias_B[Z_AXIS]; + gEKFOutput.corrAccel_B[X_AXIS] = gEKFInput.accel_B[X_AXIS] - gKalmanFilter.accelBias_B[X_AXIS]; + gEKFOutput.corrAccel_B[Y_AXIS] = gEKFInput.accel_B[Y_AXIS] - gKalmanFilter.accelBias_B[Y_AXIS]; + gEKFOutput.corrAccel_B[Z_AXIS] = gEKFInput.accel_B[Z_AXIS] - gKalmanFilter.accelBias_B[Z_AXIS]; // ------------------ Algorithm flags ------------------ - gEKFOutputData.opMode = gAlgorithm.state; - gEKFOutputData.linAccelSwitch = gAlgorithm.linAccelSwitch; - gEKFOutputData.turnSwitchFlag = gAlgoStatus.bit.turnSwitch; - gEKFOutputData.gpsMeasurementUpdate = gAlgoStatus.bit.gpsUpdate; + gEKFOutput.opMode = gAlgorithm.state; + gEKFOutput.linAccelSwitch = gAlgorithm.linAccelSwitch; + gEKFOutput.turnSwitchFlag = gAlgoStatus.bit.turnSwitch; // ------------------ Latitude and Longitude Data ------------------ - gEKFOutputData.llaDeg[LAT] = gKalmanFilter.llaDeg[LAT]; - gEKFOutputData.llaDeg[LON] = gKalmanFilter.llaDeg[LON]; - gEKFOutputData.llaDeg[ALT] = gKalmanFilter.llaDeg[ALT]; + gEKFOutput.llaDeg[LAT] = gKalmanFilter.llaDeg[LAT]; + gEKFOutput.llaDeg[LON] = gKalmanFilter.llaDeg[LON]; + gEKFOutput.llaDeg[ALT] = gKalmanFilter.llaDeg[ALT]; } -static void EstimateAccelError( real *accel, real *w, real dt, AccelStatsStruct *gAccelStats) + +// +uint8_t InitINSFilter(void) { - static BOOL bIni = false; // indicate if the procedure is initialized - static real lastAccel[3]; // accel input of last step - static real lastGyro[3]; // gyro input of last step - static float lastEstimatedAccel[3]; // propagated accel of last step - static uint32_t counter = 0; // propagation counter - static uint32_t t[3]; - // initialize - if ( !bIni ) - { - bIni = true; - lastAccel[0] = accel[0]; - lastAccel[1] = accel[1]; - lastAccel[2] = accel[2]; - lastGyro[0] = w[0]; - lastGyro[1] = w[1]; - lastGyro[2] = w[2]; - t[0] = 0; - t[1] = 0; - t[2] = 0; - gAccelStats->accelErr[0] = 0.0; - gAccelStats->accelErr[1] = 0.0; - gAccelStats->accelErr[2] = 0.0; - return; - } + real tmp[7][7]; + int rowNum, colNum; + +#ifdef INS_OFFLINE + printf("reset INS filter.\n"); +#endif // INS_OFFLINE + + gAlgorithm.insFirstTime = FALSE; + + // Sync the algorithm and GPS ITOW + gAlgorithm.itow = gEKFInput.itow; - /* Using gyro to propagate accel and then to detect accel error can give valid result for a - * short period of time because the inhere long-term drift of integrating gyro data. - * So, after this period of time, a new accel input will be selected. - * Beside, this method cannot detect long-time smooth linear acceleration. In this case, we - * can only hope the linear acceleration is large enough to make an obvious diffeerence from - * the Earth gravity 1g. + /* We have a good GPS reading now - set this variable so we + * don't drop into INS right away */ - if ( counter==0 ) - { - lastEstimatedAccel[0] = lastAccel[0]; - lastEstimatedAccel[1] = lastAccel[1]; - lastEstimatedAccel[2] = lastAccel[2]; - } - counter++; - if ( counter == gAlgorithm.Limit.linAccelSwitchDelay ) - { - counter = 0; - } - - // propagate accel using gyro - // a(k) = a(k-1) -w x a(k-1)*dt - real ae[3]; - lastGyro[0] *= -dt; - lastGyro[1] *= -dt; - lastGyro[2] *= -dt; - cross(lastGyro, lastEstimatedAccel, ae); - ae[0] += lastEstimatedAccel[0]; - ae[1] += lastEstimatedAccel[1]; - ae[2] += lastEstimatedAccel[2]; - - // save this estimated accel - lastEstimatedAccel[0] = ae[0]; - lastEstimatedAccel[1] = ae[1]; - lastEstimatedAccel[2] = ae[2]; - - // err = a(k) - am - ae[0] -= accel[0]; - ae[1] -= accel[1]; - ae[2] -= accel[2]; - - /* If the difference between the propagted accel and the input accel exceeds some threshold, - * we assume there is linear acceleration and set .accelErr to be a large value (0.1g). - * If the difference has been within the threshold for a period of time, we start to decrease - * estimated accel error .accelErr. + gAlgorithm.timeOfLastGoodGPSReading = gEKFInput.itow; + + /* Upon the first entry into INS, save off the base position and reset the + * Kalman filter variables. */ - int j; - gAccelStats->accelErrLimit = false; - for(j=0; j<3; j++) - { - if ( fabs(ae[j]) > 1.0e-2 ) // linear accel detected - { - t[j] = 0; - gAccelStats->accelErr[j] = 0.1f; - } - else // no linear accel detected, start to decrease estimated accel error - { - if ( t[j] > gAlgorithm.Limit.linAccelSwitchDelay ) // decrase error - { - gAccelStats->accelErr[j] *= 0.9f; - gAccelStats->accelErr[j] += 0.1f * ae[j]; - } - else // keep previous error value - { - t[j]++; - // gAccelStats->accelErr[j]; - } - } - // limit error, not taking effect here since the max accelErr should be 0.1 - if ( gAccelStats->accelErr[j] > 0.5 ) - { - gAccelStats->accelErr[j] = 0.5; - gAccelStats->accelErrLimit = true; - } - if ( gAccelStats->accelErr[j] < -0.5 ) - { - gAccelStats->accelErr[j] = -0.5; - gAccelStats->accelErrLimit = true; - } - } - // record accel for next step - lastAccel[0] = accel[0]; - lastAccel[1] = accel[1]; - lastAccel[2] = accel[2]; - lastGyro[0] = w[0]; - lastGyro[1] = w[1]; - lastGyro[2] = w[2]; -} + // Save off the base ECEF location + gKalmanFilter.rGPS0_E[X_AXIS] = gKalmanFilter.rGPS_E[X_AXIS]; + gKalmanFilter.rGPS0_E[Y_AXIS] = gKalmanFilter.rGPS_E[Y_AXIS]; + gKalmanFilter.rGPS0_E[Z_AXIS] = gKalmanFilter.rGPS_E[Z_AXIS]; -static void ComputeAccelStats( real *accel, AccelStatsStruct *gAccelStats, int iReset ) -{ - static BOOL bIni = false; // indicate the routine is initialized or not - static real data[3][DATA_NUM_FOR_STATS]; // a section in memoty to store buffer data - static Buffer bf; // a ring buffer using the above memory section - - // reset the calculation of motion stats - if ( iReset ) - { - bIni = false; - } + // Reset the gps position (as position is relative to starting location) + gKalmanFilter.rGPS_N[X_AXIS] = 0.0; + gKalmanFilter.rGPS_N[Y_AXIS] = 0.0; + gKalmanFilter.rGPS_N[Z_AXIS] = 0.0; - // initialization - if ( !bIni ) - { - bIni = true; - // reset stats - gAccelStats->bValid = false; - gAccelStats->accelMean[0] = 0.0; - gAccelStats->accelMean[1] = 0.0; - gAccelStats->accelMean[2] = 0.0; - gAccelStats->accelVar[0] = 0.0; - gAccelStats->accelVar[1] = 0.0; - gAccelStats->accelVar[2] = 0.0; - // create/reset buffer - bfNew(&bf, &data[0][0], 3, DATA_NUM_FOR_STATS); - } + // Reset prediction values. Position_N is also IMU position. + gKalmanFilter.Position_N[X_AXIS] = (real)0.0; + gKalmanFilter.Position_N[Y_AXIS] = (real)0.0; + gKalmanFilter.Position_N[Z_AXIS] = (real)0.0; + + gKalmanFilter.Velocity_N[X_AXIS] = (real)gEKFInput.vNed[X_AXIS]; + gKalmanFilter.Velocity_N[Y_AXIS] = (real)gEKFInput.vNed[Y_AXIS]; + gKalmanFilter.Velocity_N[Z_AXIS] = (real)gEKFInput.vNed[Z_AXIS]; + + gKalmanFilter.accelBias_B[X_AXIS] = (real)0.0; + gKalmanFilter.accelBias_B[Y_AXIS] = (real)0.0; + gKalmanFilter.accelBias_B[Z_AXIS] = (real)0.0; - /* Compute mean/variance from input data. - * When the input data buffer is full, the stats can be assumed valid. + gKalmanFilter.linearAccel_B[X_AXIS] = (real)0.0; + + /* Extract the Quaternion and rate-bias values from the matrix before + * resetting */ - // save last stats for recursive calculation - real lastMean[3]; - real lastVar[3]; - lastMean[0] = gAccelStats->accelMean[0]; - lastMean[1] = gAccelStats->accelMean[1]; - lastMean[2] = gAccelStats->accelMean[2]; - lastVar[0] = gAccelStats->accelVar[0]; - lastVar[1] = gAccelStats->accelVar[1]; - lastVar[2] = gAccelStats->accelVar[2]; - if ( bf.full ) + // Save off the quaternion and rate-bias covariance values + for (rowNum = Q0; rowNum <= Q3 + Z_AXIS + 1; rowNum++) { - // Enough data collected, stats can be assumed valid - gAccelStats->bValid = true; - /* when buffer is full, the var and mean are computed from - * all data in the buffer. From now on, the var and mean - * should be computed by removing the oldest data and including - * the latest data. - */ - // Get the oldest data which will be removed by following bfPut - real oldest[3]; - bfGet(&bf, oldest, bf.num-1); - // put this accel into buffer - bfPut(&bf, accel); - // mean(n+1) = ( mean(n) * n - x(1) + x(n+1) ) / n - gAccelStats->accelMean[0] = lastMean[0] + ( accel[0] - oldest[0] ) / (real)(bf.num); - gAccelStats->accelMean[1] = lastMean[1] + ( accel[1] - oldest[1] ) / (real)(bf.num); - gAccelStats->accelMean[2] = lastMean[2] + ( accel[2] - oldest[2] ) / (real)(bf.num); - - // naive var calculation is adopted because recursive method is numerically instable - real tmpVar[3]; - tmpVar[0] = vecVar(&bf.d[0], gAccelStats->accelMean[0], bf.num); - tmpVar[1] = vecVar(&bf.d[bf.n], gAccelStats->accelMean[1], bf.num); - tmpVar[2] = vecVar(&bf.d[2*bf.n], gAccelStats->accelMean[2], bf.num); - // make var estimation smooth - real k = 0.99f; - int i; - for ( i=0; i<3; i++ ) + for (colNum = Q0; colNum <= Q3 + Z_AXIS + 1; colNum++) { - if ( tmpVar[i] >= gAccelStats->accelVar[i] ) - { - gAccelStats->accelVar[i] = tmpVar[i]; - } - else - { - gAccelStats->accelVar[i] = k*gAccelStats->accelVar[i] + (1.0f-k)*tmpVar[i]; - } + tmp[rowNum][colNum] = gKalmanFilter.P[rowNum + STATE_Q0][colNum + STATE_Q0]; } } - else - { - // put this accel into buffer - bfPut(&bf, accel); - /* Recursivly include new accel. The data num used to compute mean and - * var are increasing. - */ - // mean(n+1) = mean(n) *n / (n+1) + x(n+1) / (n+1) - gAccelStats->accelMean[0] = lastMean[0] + ( accel[0] - lastMean[0] ) / (real)(bf.num); - gAccelStats->accelMean[1] = lastMean[1] + ( accel[1] - lastMean[1] ) / (real)(bf.num); - gAccelStats->accelMean[2] = lastMean[2] + ( accel[2] - lastMean[2] ) / (real)(bf.num); - gAccelStats->accelVar[0] = lastVar[0] + lastMean[0]*lastMean[0] - - gAccelStats->accelMean[0] * gAccelStats->accelMean[0] + - ( accel[0]*accel[0] - lastVar[0] -lastMean[0]*lastMean[0] ) / (real)(bf.num); - gAccelStats->accelVar[1] = lastVar[1] + lastMean[1]*lastMean[1] - - gAccelStats->accelMean[1] * gAccelStats->accelMean[1] + - ( accel[1]*accel[1] - lastVar[1] -lastMean[1]*lastMean[1] ) / (real)(bf.num); - gAccelStats->accelVar[2] = lastVar[2] + lastMean[2]*lastMean[2] - - gAccelStats->accelMean[2] * gAccelStats->accelMean[2] + - ( accel[2]*accel[2] - lastVar[2] -lastMean[2]*lastMean[2] ) / (real)(bf.num); - } -} - -static BOOL DetectMotionFromAccel(real accelNorm, int iReset) -{ - if( iReset ) + // Reset P + memset(gKalmanFilter.P, 0, sizeof(gKalmanFilter.P)); + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) { - gAlgorithm.linAccelSwitch = false; - gAlgorithm.linAccelSwitchCntr = 0; + gKalmanFilter.P[rowNum][rowNum] = (real)INIT_P_INS; } - /* Check for times when the acceleration is 'close' to 1 [g]. When thisoccurs, - * increment a counter. When it exceeds a threshold (indicating that the system - * has been at rest for a given period) then decrease the R-values (in the - * update stage of the EKF), effectively increasing the Kalman gain. - */ - if (fabs( 1.0 - accelNorm ) < gAlgorithm.Limit.accelSwitch ) + + // Repopulate the P matrix with the quaternion and rate-bias values + for (rowNum = Q0; rowNum <= Q3 + Z_AXIS + 1; rowNum++) { - gAlgorithm.linAccelSwitchCntr++; - if ( gAlgorithm.linAccelSwitchCntr >= gAlgorithm.Limit.linAccelSwitchDelay ) + for (colNum = Q0; colNum <= Q3 + Z_AXIS + 1; colNum++) { - gAlgorithm.linAccelSwitch = TRUE; + gKalmanFilter.P[rowNum + STATE_Q0][colNum + STATE_Q0] = tmp[rowNum][colNum]; } - else - { - gAlgorithm.linAccelSwitch = FALSE; - } - } - else - { - gAlgorithm.linAccelSwitchCntr = 0; - gAlgorithm.linAccelSwitch = FALSE; } - return true; -} + /* Use the GPS-provided horizontal and vertical accuracy values to populate + * the covariance values. + */ + gKalmanFilter.P[STATE_RX][STATE_RX] = gEKFInput.GPSHorizAcc * gEKFInput.GPSHorizAcc; + gKalmanFilter.P[STATE_RY][STATE_RY] = gKalmanFilter.P[STATE_RX][STATE_RX]; + gKalmanFilter.P[STATE_RZ][STATE_RZ] = gEKFInput.GPSVertAcc * gEKFInput.GPSVertAcc; -/* Set the accelerometer filter coefficients, which are used to filter the - * accelerometer readings prior to determining the setting of the linear- - * acceleration switch and computing the roll and pitch from accelerometer - * readings. - */ -static void _PopulateFilterCoefficients( uint8_t lpfType, uint8_t callingFreq, real *b, real *a ) -{ - switch( lpfType ) + /* Scale the best velocity error by HDOP then multiply by the z-axis angular + * rate PLUS one (to prevent the number from being zero) so the velocity + * update during high-rate turns is reduced. + */ + float temp = (real)0.0625 * gEKFInput.HDOP; // 0.0625 = 0.05 / 0.8 + real absFilteredYawRate = (real)fabs(gAlgorithm.filteredYawRate); + if (absFilteredYawRate > TEN_DEGREES_IN_RAD) { - case NO_LPF: - b[0] = (real)(1.0); - b[1] = (real)(0.0); - b[2] = (real)(0.0); - b[3] = (real)(0.0); - - a[0] = (real)(0.0); - a[1] = (real)(0.0); - a[2] = (real)(0.0); - a[3] = (real)(0.0); - break; - case TWO_HZ_LPF: - if( callingFreq == FREQ_100_HZ) - { - b[0] = (real)(2.19606211225382e-4); - b[1] = (real)(6.58818633676145e-4); - b[2] = (real)(6.58818633676145e-4); - b[3] = (real)(2.19606211225382e-4); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-2.748835809214676); - a[2] = (real)( 2.528231219142559); - a[3] = (real)(-0.777638560238080); - } - else - { - b[0] = (real)(2.91464944656705e-5); - b[1] = (real)(8.74394833970116e-5); - b[2] = (real)(8.74394833970116e-5); - b[3] = (real)(2.91464944656705e-5); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-2.874356892677485); - a[2] = (real)( 2.756483195225695); - a[3] = (real)(-0.881893130592486); - } - break; - case FIVE_HZ_LPF: - if( callingFreq == FREQ_100_HZ) - { - b[0] = (real)( 0.002898194633721); - b[1] = (real)( 0.008694583901164); - b[2] = (real)( 0.008694583901164); - b[3] = (real)( 0.002898194633721); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-2.374094743709352); - a[2] = (real)( 1.929355669091215); - a[3] = (real)(-0.532075368312092); - } - else - { - b[0] = (real)( 0.000416546139076); - b[1] = (real)( 0.001249638417227); - b[2] = (real)( 0.001249638417227); - b[3] = (real)( 0.000416546139076); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-2.686157396548143); - a[2] = (real)( 2.419655110966473); - a[3] = (real)(-0.730165345305723); - } - break; - case TWENTY_HZ_LPF: - if (callingFreq == FREQ_100_HZ) - { - // [B,A] = butter(3,20/(100/2)) - b[0] = (real)( 0.098531160923927); - b[1] = (real)( 0.295593482771781); - b[2] = (real)( 0.295593482771781); - b[3] = (real)( 0.098531160923927); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-0.577240524806303); - a[2] = (real)( 0.421787048689562); - a[3] = (real)(-0.056297236491843); - } - else - { - // [B,A] = butter(3,20/(200/2)) - b[0] = (real)( 0.018098933007514); - b[1] = (real)( 0.054296799022543); - b[2] = (real)( 0.054296799022543); - b[3] = (real)( 0.018098933007514); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-1.760041880343169); - a[2] = (real)( 1.182893262037831); - a[3] = (real)(-0.278059917634546); - } - break; - case TWENTY_FIVE_HZ_LPF: - if (callingFreq == FREQ_100_HZ) - { - b[0] = (real)( 0.166666666666667); - b[1] = (real)( 0.500000000000000); - b[2] = (real)( 0.500000000000000); - b[3] = (real)( 0.166666666666667); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-0.000000000000000); - a[2] = (real)( 0.333333333333333); - a[3] = (real)(-0.000000000000000); - } - else - { - b[0] = (real)( 0.031689343849711); - b[1] = (real)( 0.095068031549133); - b[2] = (real)( 0.095068031549133); - b[3] = (real)( 0.031689343849711); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-1.459029062228061); - a[2] = (real)( 0.910369000290069); - a[3] = (real)(-0.197825187264319); - } - break; - case TEN_HZ_LPF: - default: - if( callingFreq == FREQ_100_HZ) - { - b[0] = (real)( 0.0180989330075144); - b[1] = (real)( 0.0542967990225433); - b[2] = (real)( 0.0542967990225433); - b[3] = (real)( 0.0180989330075144); - - a[0] = (real)( 1.0000000000000000); - a[1] = (real)(-1.7600418803431690); - a[2] = (real)( 1.1828932620378310); - a[3] = (real)(-0.2780599176345460); - } - else - { - b[0] = (real)( 0.002898194633721); - b[1] = (real)( 0.008694583901164); - b[2] = (real)( 0.008694583901164); - b[3] = (real)( 0.002898194633721); - - a[0] = (real)( 1.000000000000000); - a[1] = (real)(-2.374094743709352); - a[2] = (real)( 1.929355669091215); - a[3] = (real)(-0.532075368312092); - } - break; + temp *= (1.0f + absFilteredYawRate); } -} - -static void _AccelLPF( real *accel, uint8_t lpfType, uint8_t callingFreq, real *filteredAccel ) -{ - // Floating-point filter variables - static real accelFilt[FILTER_ORDER+1][NUM_AXIS]; - static real accelReading[FILTER_ORDER+1][NUM_AXIS]; + gKalmanFilter.P[STATE_VX][STATE_VX] = temp;// *((real)1.0 + fabs(gAlgorithm.filteredYawRate) * (real)RAD_TO_DEG); + gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_VX] * gKalmanFilter.P[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.P[STATE_VX][STATE_VX]; - // filter coefficients. y/x = b/a - static real b_AccelFilt[FILTER_ORDER+1]; - static real a_AccelFilt[FILTER_ORDER+1]; + // z-axis velocity isn't really a function of yaw-rate and hdop + //gKalmanFilter.R[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VZ][STATE_VZ] = (float)(0.1 * 0.1); - // Compute filter coefficients and initialize data buffer - static BOOL initAccelFilt = true; - if (initAccelFilt) - { - initAccelFilt = false; - - // Set the filter coefficients based on selected cutoff frequency and sampling rate - _PopulateFilterCoefficients( lpfType, callingFreq, b_AccelFilt, a_AccelFilt); - - // Initialize the filter variables (do not need to populate the 0th element - // as it is never used) - for( int i = PASTx1; i <= PASTx3; i++ ) - { - accelReading[i][X_AXIS] = accel[X_AXIS]; - accelReading[i][Y_AXIS] = accel[Y_AXIS]; - accelReading[i][Z_AXIS] = accel[Z_AXIS]; - - accelFilt[i][X_AXIS] = accel[X_AXIS]; - accelFilt[i][Y_AXIS] = accel[Y_AXIS]; - accelFilt[i][Z_AXIS] = accel[Z_AXIS]; - } - } + return 1; +} - /* Filter accelerometer readings (Note: a[0] = 1.0 and the filter coefficients are symmetric) - * y = filtered output; x = raw input; - */ - /* a[0]*y(k) + a[1]*y(k-1) + a[2]*y(k-2) + a[3]*y(k-3) = - * b[0]*x(k) + b[1]*x(k-1) + b[2]*x(k-2) + b[3]*x(k-3) = - * b[0]*( x(k) + x(k-3) ) + b[1]*( x(k-1) + x(k-2) ) +static void RemoveLeverArm(double *lla, double *vNed, real *w, real *leverArmB, real *rn2e, double *ecef) +{ + // Using position with lever arm to calculate rm and rn + double sinLat = sin(lla[LAT]); + double cosLat = cos(lla[LAT]); + double tmp = 1.0 - (E_ECC_SQ * sinLat * sinLat); + double sqrtTmp = sqrt(tmp); + double rn = E_MAJOR / sqrtTmp; // radius of Curvature [meters] + double rm = rn * (1.0 - E_ECC_SQ) / tmp; + // Remove lever arm from position + real leverArmN[3]; // lever arm in the NED frame + leverArmN[0] = gKalmanFilter.R_BinN[0][0] * leverArmB[0] + + gKalmanFilter.R_BinN[0][1] * leverArmB[1] + + gKalmanFilter.R_BinN[0][2] * leverArmB[2]; + leverArmN[1] = gKalmanFilter.R_BinN[1][0] * leverArmB[0] + + gKalmanFilter.R_BinN[1][1] * leverArmB[1] + + gKalmanFilter.R_BinN[1][2] * leverArmB[2]; + leverArmN[2] = gKalmanFilter.R_BinN[2][0] * leverArmB[0] + + gKalmanFilter.R_BinN[2][1] * leverArmB[1] + + gKalmanFilter.R_BinN[2][2] * leverArmB[2]; + lla[0] -= leverArmN[0] / rm; + lla[1] -= leverArmN[1] / rn / cosLat; + lla[2] += leverArmN[2]; /* Notice: lever arm is now in NED frame while altitude is + * in the opposite direction of the z axis of NED frame. + */ + + /* Remove lever arm effects from velocity + * v_gnss = v_imu + C_b2n * cross(wB, leverArmB) */ - accelFilt[CURRENT][X_AXIS] = b_AccelFilt[CURRENT] * accel[X_AXIS] + - b_AccelFilt[PASTx1] * ( accelReading[PASTx1][X_AXIS] + - accelReading[PASTx2][X_AXIS] ) + - b_AccelFilt[PASTx3] * accelReading[PASTx3][X_AXIS] - - a_AccelFilt[PASTx1] * accelFilt[PASTx1][X_AXIS] - - a_AccelFilt[PASTx2] * accelFilt[PASTx2][X_AXIS] - - a_AccelFilt[PASTx3] * accelFilt[PASTx3][X_AXIS]; - accelFilt[CURRENT][Y_AXIS] = b_AccelFilt[CURRENT] * accel[Y_AXIS] + - b_AccelFilt[PASTx1] * ( accelReading[PASTx1][Y_AXIS] + - accelReading[PASTx2][Y_AXIS] ) + - b_AccelFilt[PASTx3] * accelReading[PASTx3][Y_AXIS] - - a_AccelFilt[PASTx1] * accelFilt[PASTx1][Y_AXIS] - - a_AccelFilt[PASTx2] * accelFilt[PASTx2][Y_AXIS] - - a_AccelFilt[PASTx3] * accelFilt[PASTx3][Y_AXIS]; - accelFilt[CURRENT][Z_AXIS] = b_AccelFilt[CURRENT] * accel[Z_AXIS] + - b_AccelFilt[PASTx1] * ( accelReading[PASTx1][Z_AXIS] + - accelReading[PASTx2][Z_AXIS] ) + - b_AccelFilt[PASTx3] * accelReading[PASTx3][Z_AXIS] - - a_AccelFilt[PASTx1] * accelFilt[PASTx1][Z_AXIS] - - a_AccelFilt[PASTx2] * accelFilt[PASTx2][Z_AXIS] - - a_AccelFilt[PASTx3] * accelFilt[PASTx3][Z_AXIS]; - - // Update past readings - accelReading[PASTx3][X_AXIS] = accelReading[PASTx2][X_AXIS]; - accelReading[PASTx2][X_AXIS] = accelReading[PASTx1][X_AXIS]; - accelReading[PASTx1][X_AXIS] = accel[X_AXIS]; - - accelReading[PASTx3][Y_AXIS] = accelReading[PASTx2][Y_AXIS]; - accelReading[PASTx2][Y_AXIS] = accelReading[PASTx1][Y_AXIS]; - accelReading[PASTx1][Y_AXIS] = accel[Y_AXIS]; - - accelReading[PASTx3][Z_AXIS] = accelReading[PASTx2][Z_AXIS]; - accelReading[PASTx2][Z_AXIS] = accelReading[PASTx1][Z_AXIS]; - accelReading[PASTx1][Z_AXIS] = accel[Z_AXIS]; - - accelFilt[PASTx3][X_AXIS] = accelFilt[PASTx2][X_AXIS]; - accelFilt[PASTx2][X_AXIS] = accelFilt[PASTx1][X_AXIS]; - accelFilt[PASTx1][X_AXIS] = accelFilt[CURRENT][X_AXIS]; - - accelFilt[PASTx3][Y_AXIS] = accelFilt[PASTx2][Y_AXIS]; - accelFilt[PASTx2][Y_AXIS] = accelFilt[PASTx1][Y_AXIS]; - accelFilt[PASTx1][Y_AXIS] = accelFilt[CURRENT][Y_AXIS]; - - accelFilt[PASTx3][Z_AXIS] = accelFilt[PASTx2][Z_AXIS]; - accelFilt[PASTx2][Z_AXIS] = accelFilt[PASTx1][Z_AXIS]; - accelFilt[PASTx1][Z_AXIS] = accelFilt[CURRENT][Z_AXIS]; - - // Output filtered accel - filteredAccel[X_AXIS] = accelFilt[CURRENT][X_AXIS]; - filteredAccel[Y_AXIS] = accelFilt[CURRENT][Y_AXIS]; - filteredAccel[Z_AXIS] = accelFilt[CURRENT][Z_AXIS]; + cross(w, leverArmB, leverArmN); // use leverArmN to temporatily hold w x leverArmB in body frame + vNed[0] -= gKalmanFilter.R_BinN[0][0] * leverArmN[0] + + gKalmanFilter.R_BinN[0][1] * leverArmN[1] + + gKalmanFilter.R_BinN[0][2] * leverArmN[2]; + vNed[1] -= gKalmanFilter.R_BinN[1][0] * leverArmN[0] + + gKalmanFilter.R_BinN[1][1] * leverArmN[1] + + gKalmanFilter.R_BinN[1][2] * leverArmN[2]; + vNed[2] -= gKalmanFilter.R_BinN[2][0] * leverArmN[0] + + gKalmanFilter.R_BinN[2][1] * leverArmN[1] + + gKalmanFilter.R_BinN[2][2] * leverArmN[2]; + + // calcualte transfromation matrix from NED to ECEF + sinLat = sin(lla[LAT]); // recalculate with LLA without lever arm + cosLat = cos(lla[LAT]); + double sinLon = sin(lla[LON]); + double cosLon = cos(lla[LON]); + + real sinLat_r = (real)sinLat; + real cosLat_r = (real)cosLat; + real sinLon_r = (real)sinLon; + real cosLon_r = (real)cosLon; + + // Form the transformation matrix from NED to ECEF + // First row + *(rn2e + 0 * 3 + 0) = -sinLat_r * cosLon_r; + *(rn2e + 0 * 3 + 1) = -sinLon_r; + *(rn2e + 0 * 3 + 2) = -cosLat_r * cosLon_r; + // Second row + *(rn2e + 1 * 3 + 0) = -sinLat_r * sinLon_r; + *(rn2e + 1 * 3 + 1) = cosLon_r; + *(rn2e + 1 * 3 + 2) = -cosLat_r * sinLon_r; + // Third row + *(rn2e + 2 * 3 + 0) = cosLat_r; + *(rn2e + 2 * 3 + 1) = 0.0; + *(rn2e + 2 * 3 + 2) = -sinLat_r; + + // calculate ECEF position + tmp = (rn + lla[ALT]) * cosLat; + ecef[X_AXIS] = tmp * cosLon; + ecef[Y_AXIS] = tmp * sinLon; + ecef[Z_AXIS] = ((E_MINOR_OVER_MAJOR_SQ * (rn)) + lla[ALT]) * sinLat; } diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/MotionStatus.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/MotionStatus.c new file mode 100644 index 0000000..220f0cd --- /dev/null +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/MotionStatus.c @@ -0,0 +1,642 @@ +/***************************************************************************** + * @file MotionStatus.c + * @brief Calculate sensor stats, and detect motion status using IMU/ODO/GNSS + * @author Dong Xiaoguang + * @version 1.0 + * @date 20190801 + *****************************************************************************/ +#include +#include "MotionStatus.h" +#include "buffer.h" +#include "VectorMath.h" + + +//============================================================================= +// Filter variables (Third-Order BWF w/ default 5 Hz Cutoff) +#define FILTER_ORDER 3 + +#define CURRENT 0 +#define PASTx1 1 +#define PASTx2 2 +#define PASTx3 3 +/* Replace this with a fuction that will compute the coefficients so the + * input is the cutoff frequency in Hertz + */ +#define NO_LPF 0 +#define TWO_HZ_LPF 1 +#define FIVE_HZ_LPF 2 +#define TEN_HZ_LPF 3 +#define TWENTY_HZ_LPF 4 +#define TWENTY_FIVE_HZ_LPF 5 +#define N_LPF 6 + +#define SAMPLES_FOR_STATS 20 /* 20 samples can give a relative good estimate of var + * This value should not be below FILTER_ORDER. + */ + + /****************************************************************************** + * @brief Get filter coefficients of a 3rd Butterworth low-pass filter. + * For now only a few specific cut-off frequencies are supported. + * TRACE: + * @param [in] lpfType Low-pass filter cut-off frequency. + * @param [in] callingFreq Sampling frequency, only 100Hz and 200Hz are supported. + * @param [out] b coefficients of the numerator of the filter. + * @param [out] a coefficients of the denominator of the filter. + * @retval None. + ******************************************************************************/ +static void _PopulateFilterCoefficients(uint8_t lpfType, uint8_t callingFreq, real *b, real *a); + +/****************************************************************************** + * @brief Process input data through a low-pass Butterworth filter. + * TRACE: + * @param [in] in Input data + * @param [in] bfIn Input data buffer + * @param [in] bfOut Output data buffer + * @param [in] b Numerator coef of the filter + * @param [in] a Denominator coef of the filter + * + * @param [out] filtered Filtered IMU data. + * @retval None. +******************************************************************************/ +static void LowPassFilter(real *in, Buffer *bfIn, Buffer *bfOut, real *b, real *a, real *filtered); + +/****************************************************************************** + * @brief Compute mean and var of the input data. + * Calculate mena and var of the latest n samples. + * TRACE: + * @param [in] bf The buffer to hold the latest n samples. + * @param [in] latest The latest sample. + * @param [in/out] mean Mean of samples already in buffer is used as input, and + * mean of latest samples (remove oldest and include latest) + * is returned as output. + * @param [in/out] var Var of samples already in buffer is used as input, and + * var of latest samples (remove oldest and include latest) + * is returned as output. + * @retval None. +******************************************************************************/ +static void ComputeStats(Buffer *bf, real *latest, real *mean, real *var); + +/****************************************************************************** + * @brief Detect zero velocity using IMU data. + * TRACE: + * @param [in] gyroVar variance of gyro [rad/s]^2 + * @param [in] gyroMean mean of gyro [rad/s] + * @param [in] accelVar variance of accel [m/s/s]^2 + * @param [in] threshold threshold to detect zero velocity + * @retval TRUE if static, otherwise FALSE. +******************************************************************************/ +BOOL DetectStaticIMU(real *gyroVar, real *gyroMean, real *accelVar, STATIC_DETECT_SETTING *threshold); + + +void MotionStatusImu(real *gyro, real *accel, ImuStatsStruct *imuStats, BOOL reset) +{ + static BOOL bIni = false; // indicate the routine is initialized or not + // Buffer to store input IMU data. Data in buffer is ued to calcualte filtered IMU dat. + static real dAccel[3][FILTER_ORDER]; // a section in memory as buffer to store accel data + static real dGyro[3][FILTER_ORDER]; // a section in memory as buffer to store gyro data + static Buffer bfAccel; // a ring buffer of accel + static Buffer bfGyro; // a ring buffer of gyro + // Buffer to store filtered IMU data. Data in buffer is used to calculate IMU stats. + static real dLpfAccel[3][SAMPLES_FOR_STATS]; // a section in memory as buffer to store accel data + static real dLpfGyro[3][SAMPLES_FOR_STATS]; // a section in memory as buffer to store gyro data + static Buffer bfLpfAccel; // a ring buffer of accel + static Buffer bfLpfGyro; // a ring buffer of gyro + // filter coefficients. y/x = b/a + static real b_AccelFilt[FILTER_ORDER + 1]; + static real a_AccelFilt[FILTER_ORDER + 1]; + + // reset the calculation of motion stats + if (reset) + { + bIni = false; + } + + // initialization + if (!bIni) + { + bIni = true; + // reset stats + imuStats->bValid = false; + imuStats->accelMean[0] = 0.0; + imuStats->accelMean[1] = 0.0; + imuStats->accelMean[2] = 0.0; + imuStats->accelVar[0] = 0.0; + imuStats->accelVar[1] = 0.0; + imuStats->accelVar[2] = 0.0; + imuStats->gyroMean[0] = 0.0; + imuStats->gyroMean[1] = 0.0; + imuStats->gyroMean[2] = 0.0; + imuStats->gyroVar[0] = 0.0; + imuStats->gyroVar[1] = 0.0; + imuStats->gyroVar[2] = 0.0; + // create/reset buffer + bfNew(&bfGyro, &dGyro[0][0], 3, FILTER_ORDER); + bfNew(&bfAccel, &dAccel[0][0], 3, FILTER_ORDER); + bfNew(&bfLpfGyro, &dLpfGyro[0][0], 3, SAMPLES_FOR_STATS); + bfNew(&bfLpfAccel, &dLpfAccel[0][0], 3, SAMPLES_FOR_STATS); + // Set the filter coefficients based on selected cutoff frequency and sampling rate + _PopulateFilterCoefficients(gAlgorithm.linAccelLPFType, gAlgorithm.callingFreq, b_AccelFilt, a_AccelFilt); + } + + /* Low-pass filter. + * The input IMU data is put into the buffer, and then filtered. + */ + LowPassFilter(gyro, &bfGyro, &bfLpfGyro, b_AccelFilt, a_AccelFilt, imuStats->lpfGyro); + LowPassFilter(accel, &bfAccel, &bfLpfAccel, b_AccelFilt, a_AccelFilt, imuStats->lpfAccel); + + /* Compute accel norm using raw accel data. + * The norm will be used to detect static periods via magnitude. + */ + imuStats->accelNorm = sqrtf(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]); + + /* Compute mean/variance from input data. + * The latest will be put into the buffer, and stats of data in buffer is then calculated. + * When the input data buffer is full, the stats can be assumed valid. + */ + ComputeStats(&bfLpfGyro, imuStats->lpfGyro, imuStats->gyroMean, imuStats->gyroVar); + ComputeStats(&bfLpfAccel, imuStats->lpfAccel, imuStats->accelMean, imuStats->accelVar); + imuStats->bValid = bfLpfGyro.full && bfLpfAccel.full; + + // Detect static period using var calculated above. + imuStats->bStaticIMU = DetectStaticIMU( imuStats->gyroVar, + imuStats->gyroMean, + imuStats->accelVar, + &gAlgorithm.staticDetectParam); +} + +static void ComputeStats(Buffer *bf, real *latest, real *mean, real *var) +{ + real lastMean[3]; + real lastVar[3]; + lastMean[0] = mean[0]; + lastMean[1] = mean[1]; + lastMean[2] = mean[2]; + lastVar[0] = var[0]; + lastVar[1] = var[1]; + lastVar[2] = var[2]; + if (bf->full) + { + /* when buffer is full, the var and mean are computed from + * all data in the buffer. From now on, the var and mean + * should be computed by removing the oldest data and including + * the latest data. + */ + // Get the oldest data which will be removed by following bfPut + real oldest[3]; + bfGet(bf, oldest, bf->num - 1); + // put this accel into buffer + bfPut(bf, latest); + // mean(n+1) = ( mean(n) * n - x(1) + x(n+1) ) / n + mean[0] += (latest[0] - oldest[0]) / (real)(bf->num); + mean[1] += (latest[1] - oldest[1]) / (real)(bf->num); + mean[2] += (latest[2] - oldest[2]) / (real)(bf->num); + + // naive var calculation is adopted because recursive method is numerically instable + real tmpVar[3]; + tmpVar[0] = vecVar(&(bf->d[0]), mean[0], bf->num); + tmpVar[1] = vecVar(&(bf->d[bf->n]), mean[1], bf->num); + tmpVar[2] = vecVar(&(bf->d[2 * bf->n]), mean[2], bf->num); + // make var estimation smooth + real k = 0.96f; + int i; + for (i = 0; i < 3; i++) + { + if (tmpVar[i] >= var[i]) + { + var[i] = tmpVar[i]; + } + else + { + var[i] = k * var[i] + (1.0f - k)*tmpVar[i]; + } + } + } + else + { + // put this accel into buffer + bfPut(bf, latest); + /* Recursivly include new accel. The data num used to compute mean and + * var are increasing. + */ + // mean(n+1) = mean(n) *n / (n+1) + x(n+1) / (n+1) + mean[0] = lastMean[0] + (latest[0] - lastMean[0]) / (real)(bf->num); + mean[1] = lastMean[1] + (latest[1] - lastMean[1]) / (real)(bf->num); + mean[2] = lastMean[2] + (latest[2] - lastMean[2]) / (real)(bf->num); + var[0] = lastVar[0] + lastMean[0] * lastMean[0] - mean[0] * mean[0] + + (latest[0] * latest[0] - lastVar[0] - lastMean[0] * lastMean[0]) / (real)(bf->num); + var[1] = lastVar[1] + lastMean[1] * lastMean[1] - mean[1] * mean[1] + + (latest[1] * latest[1] - lastVar[1] - lastMean[1] * lastMean[1]) / (real)(bf->num); + var[2] = lastVar[2] + lastMean[2] * lastMean[2] - mean[2] * mean[2] + + (latest[2] * latest[2] - lastVar[2] - lastMean[2] * lastMean[2]) / (real)(bf->num); + } +} + +BOOL DetectStaticIMU(real *gyroVar, real *gyroMean, real *accelVar, STATIC_DETECT_SETTING *threshold) +{ + BOOL bStatic = TRUE; + int i; + static float multiplier[3] = { 0.0, 0.0, 0.0 }; + static float gyroVarThreshold = 0.0; + static float accelVarThreshold = 0.0; + static float gyroBiasThreshold = 0.0; + // Update threshold + if (multiplier[0] != threshold->staticNoiseMultiplier[0]) + { + multiplier[0] = threshold->staticNoiseMultiplier[0]; + gyroVarThreshold = multiplier[0] * threshold->staticVarGyro; + } + if (multiplier[1] != threshold->staticNoiseMultiplier[1]) + { + multiplier[1] = threshold->staticNoiseMultiplier[1]; + accelVarThreshold = multiplier[1] * threshold->staticVarAccel; + } + if (multiplier[2] != threshold->staticNoiseMultiplier[2]) + { + multiplier[2] = threshold->staticNoiseMultiplier[2]; + gyroBiasThreshold = multiplier[2] * threshold->maxGyroBias; + } + + for (i = 0; i < 3; i++) + { + if (gyroVar[i] > gyroVarThreshold || + accelVar[i] > accelVarThreshold || + fabs(gyroMean[i]) > gyroBiasThreshold) + { + bStatic = FALSE; + break; + } + } + + return bStatic; +} + +/* Set the accelerometer filter coefficients, which are used to filter the + * accelerometer readings prior to determining the setting of the linear- + * acceleration switch and computing the roll and pitch from accelerometer + * readings. + */ +static void _PopulateFilterCoefficients(uint8_t lpfType, uint8_t callingFreq, real *b, real *a) +{ + switch (lpfType) + { + case NO_LPF: + b[0] = (real)(1.0); + b[1] = (real)(0.0); + b[2] = (real)(0.0); + b[3] = (real)(0.0); + + a[0] = (real)(0.0); + a[1] = (real)(0.0); + a[2] = (real)(0.0); + a[3] = (real)(0.0); + break; + case TWO_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(2.19606211225382e-4); + b[1] = (real)(6.58818633676145e-4); + b[2] = (real)(6.58818633676145e-4); + b[3] = (real)(2.19606211225382e-4); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.748835809214676); + a[2] = (real)(2.528231219142559); + a[3] = (real)(-0.777638560238080); + } + else + { + b[0] = (real)(2.91464944656705e-5); + b[1] = (real)(8.74394833970116e-5); + b[2] = (real)(8.74394833970116e-5); + b[3] = (real)(2.91464944656705e-5); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.874356892677485); + a[2] = (real)(2.756483195225695); + a[3] = (real)(-0.881893130592486); + } + break; + case FIVE_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(0.002898194633721); + b[1] = (real)(0.008694583901164); + b[2] = (real)(0.008694583901164); + b[3] = (real)(0.002898194633721); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.374094743709352); + a[2] = (real)(1.929355669091215); + a[3] = (real)(-0.532075368312092); + } + else + { + b[0] = (real)(0.000416546139076); + b[1] = (real)(0.001249638417227); + b[2] = (real)(0.001249638417227); + b[3] = (real)(0.000416546139076); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.686157396548143); + a[2] = (real)(2.419655110966473); + a[3] = (real)(-0.730165345305723); + } + break; + case TWENTY_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + // [B,A] = butter(3,20/(100/2)) + b[0] = (real)(0.098531160923927); + b[1] = (real)(0.295593482771781); + b[2] = (real)(0.295593482771781); + b[3] = (real)(0.098531160923927); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-0.577240524806303); + a[2] = (real)(0.421787048689562); + a[3] = (real)(-0.056297236491843); + } + else + { + // [B,A] = butter(3,20/(200/2)) + b[0] = (real)(0.018098933007514); + b[1] = (real)(0.054296799022543); + b[2] = (real)(0.054296799022543); + b[3] = (real)(0.018098933007514); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-1.760041880343169); + a[2] = (real)(1.182893262037831); + a[3] = (real)(-0.278059917634546); + } + break; + case TWENTY_FIVE_HZ_LPF: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(0.166666666666667); + b[1] = (real)(0.500000000000000); + b[2] = (real)(0.500000000000000); + b[3] = (real)(0.166666666666667); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-0.000000000000000); + a[2] = (real)(0.333333333333333); + a[3] = (real)(-0.000000000000000); + } + else + { + b[0] = (real)(0.031689343849711); + b[1] = (real)(0.095068031549133); + b[2] = (real)(0.095068031549133); + b[3] = (real)(0.031689343849711); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-1.459029062228061); + a[2] = (real)(0.910369000290069); + a[3] = (real)(-0.197825187264319); + } + break; + case TEN_HZ_LPF: + default: + if (callingFreq == FREQ_100_HZ) + { + b[0] = (real)(0.0180989330075144); + b[1] = (real)(0.0542967990225433); + b[2] = (real)(0.0542967990225433); + b[3] = (real)(0.0180989330075144); + + a[0] = (real)(1.0000000000000000); + a[1] = (real)(-1.7600418803431690); + a[2] = (real)(1.1828932620378310); + a[3] = (real)(-0.2780599176345460); + } + else + { + b[0] = (real)(0.002898194633721); + b[1] = (real)(0.008694583901164); + b[2] = (real)(0.008694583901164); + b[3] = (real)(0.002898194633721); + + a[0] = (real)(1.000000000000000); + a[1] = (real)(-2.374094743709352); + a[2] = (real)(1.929355669091215); + a[3] = (real)(-0.532075368312092); + } + break; + } +} + +static void LowPassFilter(real *in, Buffer *bfIn, Buffer *bfOut, real *b, real *a, real *filtered) +{ + // Fill the buffer with the first input. + if (!bfIn->full) + { + bfPut(bfIn, in); + filtered[0] = in[0]; + filtered[1] = in[1]; + filtered[2] = in[2]; + return; + } + + /* Filter accelerometer readings (Note: a[0] = 1.0 and the filter coefficients are symmetric) + * y = filtered output; x = raw input; + * a[0]*y(k) + a[1]*y(k-1) + a[2]*y(k-2) + a[3]*y(k-3) = + * b[0]*x(k) + b[1]*x(k-1) + b[2]*x(k-2) + b[3]*x(k-3) = + * b[0]*( x(k) + x(k-3) ) + b[1]*( x(k-1) + x(k-2) ) + */ + int i; + real tmpIn[3]; + real tmpOut[3]; + filtered[0] = b[CURRENT] * in[0]; + filtered[1] = b[CURRENT] * in[1]; + filtered[2] = b[CURRENT] * in[2]; + for (i = PASTx1; i <= PASTx3; i++) + { + bfGet(bfIn, tmpIn, i-1); + bfGet(bfOut, tmpOut, i-1); + filtered[0] += b[i] * tmpIn[0] - a[i] * tmpOut[0]; + filtered[1] += b[i] * tmpIn[1] - a[i] * tmpOut[1]; + filtered[2] += b[i] * tmpIn[2] - a[i] * tmpOut[2]; + } + + // New data into buffer + bfPut(bfIn, in); +} + +void EstimateAccelError(real *accel, real *w, real dt, uint32_t staticDelay, ImuStatsStruct *imuStats) +{ + static BOOL bIni = false; // indicate if the procedure is initialized + static real lastAccel[3]; // accel input of last step + static real lastGyro[3]; // gyro input of last step + static float lastEstimatedAccel[3]; // propagated accel of last step + static uint32_t counter = 0; // propagation counter + static uint32_t t[3]; + // initialize + if (!bIni) + { + bIni = true; + lastAccel[0] = accel[0]; + lastAccel[1] = accel[1]; + lastAccel[2] = accel[2]; + lastGyro[0] = w[0]; + lastGyro[1] = w[1]; + lastGyro[2] = w[2]; + t[0] = 0; + t[1] = 0; + t[2] = 0; + imuStats->accelErr[0] = 0.0; + imuStats->accelErr[1] = 0.0; + imuStats->accelErr[2] = 0.0; + return; + } + + /* Using gyro to propagate accel and then to detect accel error can give valid result for a + * short period of time because the inhere long-term drift of integrating gyro data. + * So, after this period of time, a new accel input will be selected. + * Beside, this method cannot detect long-time smooth linear acceleration. In this case, we + * can only hope the linear acceleration is large enough to make an obvious diffeerence from + * the Earth gravity 1g. + */ + if (counter == 0) + { + lastEstimatedAccel[0] = lastAccel[0]; + lastEstimatedAccel[1] = lastAccel[1]; + lastEstimatedAccel[2] = lastAccel[2]; + } + counter++; + if (counter == staticDelay) + { + counter = 0; + } + + // propagate accel using gyro + // a(k) = a(k-1) -w x a(k-1)*dt + real ae[3]; + lastGyro[0] *= -dt; + lastGyro[1] *= -dt; + lastGyro[2] *= -dt; + cross(lastGyro, lastEstimatedAccel, ae); + ae[0] += lastEstimatedAccel[0]; + ae[1] += lastEstimatedAccel[1]; + ae[2] += lastEstimatedAccel[2]; + + // save this estimated accel + lastEstimatedAccel[0] = ae[0]; + lastEstimatedAccel[1] = ae[1]; + lastEstimatedAccel[2] = ae[2]; + + // err = a(k) - am + ae[0] -= accel[0]; + ae[1] -= accel[1]; + ae[2] -= accel[2]; + + /* If the difference between the propagted accel and the input accel exceeds some threshold, + * we assume there is linear acceleration and set .accelErr to be a large value (0.1g). + * If the difference has been within the threshold for a period of time, we start to decrease + * estimated accel error .accelErr. + */ + int j; + imuStats->accelErrLimit = false; + for (j = 0; j < 3; j++) + { + if (fabs(ae[j]) > 0.0980665) // linear accel detected, 0.01g + { + t[j] = 0; + imuStats->accelErr[j] = (real)0.980665; // 0.1g + } + else // no linear accel detected, start to decrease estimated accel error + { + if (t[j] > staticDelay) // decrease error + { + imuStats->accelErr[j] *= 0.9f; + imuStats->accelErr[j] += 0.1f * ae[j]; + } + else // keep previous error value + { + t[j]++; + // imuStats->accelErr[j]; + } + } + // limit error, not taking effect here since the max accelErr should be 0.1g + if (imuStats->accelErr[j] > 5.0) // 0.5g + { + imuStats->accelErr[j] = 5.0; + imuStats->accelErrLimit = true; + } + if (imuStats->accelErr[j] < -5.0) + { + imuStats->accelErr[j] = -5.0; + imuStats->accelErrLimit = true; + } + } + // record accel for next step + lastAccel[0] = accel[0]; + lastAccel[1] = accel[1]; + lastAccel[2] = accel[2]; + lastGyro[0] = w[0]; + lastGyro[1] = w[1]; + lastGyro[2] = w[2]; +} + +BOOL DetectMotionFromAccel(real accelNorm, int iReset) +{ + if (iReset) + { + gAlgorithm.linAccelSwitch = false; + gAlgorithm.linAccelSwitchCntr = 0; + } + /* Check for times when the acceleration is 'close' to 1 [g]. When this occurs, + * increment a counter. When it exceeds a threshold (indicating that the system + * has been at rest for a given period) then decrease the R-values (in the + * update stage of the EKF), effectively increasing the Kalman gain. + */ + if (fabs(1.0 - accelNorm/GRAVITY) < gAlgorithm.Limit.accelSwitch) + { + gAlgorithm.linAccelSwitchCntr++; + if (gAlgorithm.linAccelSwitchCntr >= gAlgorithm.Limit.linAccelSwitchDelay) + { + gAlgorithm.linAccelSwitch = TRUE; + } + else + { + gAlgorithm.linAccelSwitch = FALSE; + } + } + else + { + gAlgorithm.linAccelSwitchCntr = 0; + gAlgorithm.linAccelSwitch = FALSE; + } + + return TRUE; +} + +BOOL DetectStaticGnssVelocity(double *vNED, real threshold, BOOL gnssValid) +{ + static uint8_t cntr = 0; + if (gnssValid) + { + if (fabs(vNED[0]) < threshold && fabs(vNED[1]) < threshold && fabs(vNED[2]) < threshold) + { + if (cntr < 3) + { + cntr++; + } + } + else + { + cntr = 0; + } + } + else + { + cntr = 0; + } + + return cntr >= 3; +} + +BOOL DetectStaticOdo(real odo) +{ + return FALSE; +} \ No newline at end of file diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/PredictFunctions.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/PredictFunctions.c index 3ac9131..e728dca 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/PredictFunctions.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/PredictFunctions.c @@ -22,19 +22,20 @@ #include "algorithm.h" #include "algorithmAPI.h" #include "SensorNoiseParameters.h" +#include "MotionStatus.h" #include "EKF_Algorithm.h" #include "PredictFunctions.h" #include "AlgorithmLimits.h" #include "WorldMagneticModel.h" -#ifdef INS_OFFLINE -#include "SimulationParameters.h" -#else +#ifndef INS_OFFLINE #ifdef DISPLAY_DIAGNOSTIC_MSG #include "debug.h" #endif #endif +extern ImuStatsStruct gImuStats; + /* F is sparse and has elements in the following locations... * There may be some more efficient ways of implementing this as this method * still performs multiplication with zero values. (Ask Andrey) @@ -77,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); @@ -100,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 ); @@ -117,9 +131,9 @@ void EKF_PredictionStage(real *filteredAccel) */ if(magUsedInAlgorithm()) { - magFieldVector[X_AXIS] = (real)gEKFInputData.magField_B[X_AXIS]; - magFieldVector[Y_AXIS] = (real)gEKFInputData.magField_B[Y_AXIS]; - magFieldVector[Z_AXIS] = (real)gEKFInputData.magField_B[Z_AXIS]; + magFieldVector[X_AXIS] = (real)gEKFInput.magField_B[X_AXIS]; + magFieldVector[Y_AXIS] = (real)gEKFInput.magField_B[Y_AXIS]; + magFieldVector[Z_AXIS] = (real)gEKFInput.magField_B[Z_AXIS]; } else { @@ -184,10 +198,6 @@ static void _PredictStateEstimate(void) { real aCorr_N[3]; real deltaQuaternion[4]; - /* Generate the transformation matrix (R_BinN) based on the past value of - * the attitude quaternion (prior to prediction at the new time-step) - */ - QuaternionToR321(gKalmanFilter.quaternion, &gKalmanFilter.R_BinN[0][0]); if( gAlgorithm.state > LOW_GAIN_AHRS ) { @@ -202,34 +212,45 @@ static void _PredictStateEstimate(void) // ================= NED Velocity (v_N) ================= // aCorr_B = aMeas_B - aBias_B - // gEKFInputData.accel_B in g's, convert to m/s^2 for integration - gKalmanFilter.correctedAccel_B[X_AXIS] = (real)(gEKFInputData.accel_B[X_AXIS] * GRAVITY) - + // gEKFInput.accel_B in g's, convert to m/s^2 for integration + gKalmanFilter.correctedAccel_B[X_AXIS] = gEKFInput.accel_B[X_AXIS] - gKalmanFilter.accelBias_B[X_AXIS]; - gKalmanFilter.correctedAccel_B[Y_AXIS] = (real)(gEKFInputData.accel_B[Y_AXIS] * GRAVITY) - + gKalmanFilter.correctedAccel_B[Y_AXIS] = gEKFInput.accel_B[Y_AXIS] - gKalmanFilter.accelBias_B[Y_AXIS]; - gKalmanFilter.correctedAccel_B[Z_AXIS] = (real)(gEKFInputData.accel_B[Z_AXIS] * GRAVITY) - + gKalmanFilter.correctedAccel_B[Z_AXIS] = gEKFInput.accel_B[Z_AXIS] - gKalmanFilter.accelBias_B[Z_AXIS]; - // Transform the corrected acceleration vector from the body to the NED-frame - // a_N = R_BinN * a_B - aCorr_N[X_AXIS] = gKalmanFilter.R_BinN[X_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + - gKalmanFilter.R_BinN[X_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + - gKalmanFilter.R_BinN[X_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; - aCorr_N[Y_AXIS] = gKalmanFilter.R_BinN[Y_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + - gKalmanFilter.R_BinN[Y_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + - gKalmanFilter.R_BinN[Y_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; - aCorr_N[Z_AXIS] = gKalmanFilter.R_BinN[Z_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + - gKalmanFilter.R_BinN[Z_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + - gKalmanFilter.R_BinN[Z_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; - - // Determine the acceleration of the system by removing the gravity vector - // v_N(k+1) = v_N(k) + dV = v_N(k) + aMotion_N*DT = v_N(k) + ( a_N - g_N )*DT - gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + - ( aCorr_N[X_AXIS] ) * gAlgorithm.dt; - gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + - ( aCorr_N[Y_AXIS] ) * gAlgorithm.dt; - gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + - ( aCorr_N[Z_AXIS] + (real)GRAVITY ) * gAlgorithm.dt; + /* Transform the corrected acceleration vector from the body to the NED-frame and remove gravity + * a_N = R_BinN * a_B + */ + aCorr_N[X_AXIS] = + gKalmanFilter.R_BinN[X_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[X_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[X_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; + aCorr_N[Y_AXIS] = + gKalmanFilter.R_BinN[Y_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[Y_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[Y_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS]; + aCorr_N[Z_AXIS] = + gKalmanFilter.R_BinN[Z_AXIS][X_AXIS] * gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Y_AXIS] * gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Z_AXIS] * gKalmanFilter.correctedAccel_B[Z_AXIS] + + (real)GRAVITY; + + /* Determine the acceleration of the system by removing the gravity vector + * v_N(k+1) = v_N(k) + dV = v_N(k) + aMotion_N*DT = v_N(k) + ( a_N - g_N )*DT + */ + gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + aCorr_N[X_AXIS] * gAlgorithm.dt; + gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + aCorr_N[Y_AXIS] * gAlgorithm.dt; + gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + aCorr_N[Z_AXIS] * gAlgorithm.dt; + + // Calculate linear acceleration in the body frame. + gKalmanFilter.linearAccel_B[X_AXIS] += (gKalmanFilter.correctedAccel_B[X_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][X_AXIS] * (real)GRAVITY)*gAlgorithm.dt; + gKalmanFilter.linearAccel_B[Y_AXIS] = gKalmanFilter.correctedAccel_B[Y_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Y_AXIS] * (real)GRAVITY; + gKalmanFilter.linearAccel_B[Z_AXIS] = gKalmanFilter.correctedAccel_B[Z_AXIS] + + gKalmanFilter.R_BinN[Z_AXIS][Z_AXIS] * (real)GRAVITY; } else { @@ -243,18 +264,18 @@ static void _PredictStateEstimate(void) gKalmanFilter.Velocity_N[Z_AXIS] = (real)0.0; // what should this be??? - gKalmanFilter.correctedAccel_B[XACCEL] = (real)gEKFInputData.accel_B[X_AXIS] * (real)GRAVITY; - gKalmanFilter.correctedAccel_B[YACCEL] = (real)gEKFInputData.accel_B[Y_AXIS] * (real)GRAVITY; - gKalmanFilter.correctedAccel_B[ZACCEL] = (real)gEKFInputData.accel_B[Z_AXIS] * (real)GRAVITY; + gKalmanFilter.correctedAccel_B[XACCEL] = gEKFInput.accel_B[X_AXIS]; + gKalmanFilter.correctedAccel_B[YACCEL] = gEKFInput.accel_B[Y_AXIS]; + gKalmanFilter.correctedAccel_B[ZACCEL] = gEKFInput.accel_B[Z_AXIS]; } // ================= Attitude quaternion ================= // Find the 'true' angular rate (wTrue_B = wCorr_B = wMeas_B - wBias_B) - gKalmanFilter.correctedRate_B[X_AXIS] = gEKFInputData.angRate_B[X_AXIS] - + gKalmanFilter.correctedRate_B[X_AXIS] = gEKFInput.angRate_B[X_AXIS] - gKalmanFilter.rateBias_B[X_AXIS]; - gKalmanFilter.correctedRate_B[Y_AXIS] = gEKFInputData.angRate_B[Y_AXIS] - + gKalmanFilter.correctedRate_B[Y_AXIS] = gEKFInput.angRate_B[Y_AXIS] - gKalmanFilter.rateBias_B[Y_AXIS]; - gKalmanFilter.correctedRate_B[Z_AXIS] = gEKFInputData.angRate_B[Z_AXIS] - + gKalmanFilter.correctedRate_B[Z_AXIS] = gEKFInput.angRate_B[Z_AXIS] - gKalmanFilter.rateBias_B[Z_AXIS]; // Placed in gKalmanFilter as wTrueTimesDtOverTwo is used to compute the Jacobian (F) @@ -637,7 +658,7 @@ static void _UpdateProcessJacobian(void) static void _UpdateProcessCovariance(void) { // Variables used to initially populate the Q-matrix - real arw, biSq[3] = {(real)1.0e-10, (real)1.0e-10, (real)1.0e-10}; + real biSq[3] = {(real)1.0e-10, (real)1.0e-10, (real)1.0e-10}; real sigDriftDot; // Variables used to populate the Q-matrix each time-step @@ -651,83 +672,16 @@ static void _UpdateProcessCovariance(void) { initQ_HG = FALSE; -#ifdef INS_OFFLINE - /* This value is set based on the version string specified in the - * simulation configuration file, ekfSim.cfg - */ - uint8_t sysRange = gSimulation.sysRange; - uint8_t rsType = gSimulation.rsType; -#else - // This value is set based on the version string loaded into the unit - // via the system configuration load - uint8_t sysRange = platformGetSysRange(); // from system config - uint8_t rsType = BMI_RS; -#endif - - /* Set the matrix, Q, based on whether the system is a -200 or -400 - * Q-values are based on the rate-sensor's ARW (noise) and BI values - * passed through the process model. - */ - switch (sysRange) - { - case _200_DPS_RANGE: // same as default - // Bias-stability value for the rate-sensors - if (rsType == BMI_RS) - { - // BMI: 1.508e-3 [deg/sec] = 2.63e-5 [rad/sec] - biSq[X_AXIS] = (real)(6.92e-10); // (2.63e-5)^2 - biSq[Y_AXIS] = biSq[X_AXIS]; - biSq[Z_AXIS] = biSq[X_AXIS]; - } - else - { - // Maxim x/y: 1.85e-3 [deg/sec] = 3.24E-05 [rad/sec] - // Maxim z: 7.25e-4 [deg/sec] = 1.27E-05 [rad/sec] - biSq[X_AXIS] = (real)(1.05e-9); // (3.25e-5)^2 - biSq[Y_AXIS] = biSq[X_AXIS]; - biSq[Z_AXIS] = (real)(1.61e-10); // (1.27e-5)^2 - } - break; - - // -400 values - case _400_DPS_RANGE: - // bi in [rad] - if (rsType == BMI_RS) - { - // BMI: 6 [deg/hr] = 1.7e-3 [deg/sec] = 2.91e-5 [rad/sec] - biSq[X_AXIS] = (real)(8.46e-10); // (2.91e-5)^2 - biSq[Y_AXIS] = biSq[X_AXIS]; - biSq[Z_AXIS] = biSq[X_AXIS]; - } - else - { - // Maxim x/y: 2.16e-3 [deg/sec] = 3.24E-05 [rad/sec] - // Maxim z: 1.07e-3 [deg/sec] = 1.86E-05 [rad/sec] - biSq[X_AXIS] = (real)(1.41e-09); // (3.24e-5)^2 - biSq[Y_AXIS] = biSq[X_AXIS]; - biSq[Z_AXIS] = (real)(3.46e-10); // (1.86e-5)^2 - } - break; - } - - // ARW is unaffected by range setting, 200/400 (or it seems). Value is in - // units of [rad/rt-sec] - if (rsType == BMI_RS) - { - // BMI: 4.39e-3 [deg/rt-sec] = 7.66e-5 [rad/rt-sec] - arw = (real)7.66e-5; - } - else - { - // Maxim: 5.63e-3 [deg/rt-sec] = 9.82E-05 [rad/rt-sec] - arw = (real)9.82e-5; - } + // squated gyro bias instability + biSq[X_AXIS] = gAlgorithm.imuSpec.biW * gAlgorithm.imuSpec.biW; + 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 */ - sigDriftDot = (real)9.064720283654388 / arw; + sigDriftDot = (real)9.064720283654388 / gAlgorithm.imuSpec.arw; // Rate-bias terms (Q is ultimately the squared value, which is done in the second line of the assignment) gKalmanFilter.Q[STATE_WBX] = sigDriftDot * biSq[X_AXIS] * gAlgorithm.dt; @@ -738,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)6.839373353251920e-10; - gKalmanFilter.Q[STATE_ABY] = (real)6.839373353251920e-10; - gKalmanFilter.Q[STATE_ABZ] = (real)6.839373353251920e-10; + // 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 ) @@ -748,7 +709,7 @@ static void _UpdateProcessCovariance(void) * = 0.5 * sqrt(dt) * sqrt(dt) * ( ARW / sqrt(dt) ) * = 0.5 * sqrt(dt) * ARW */ - multiplier_Q = (real)(0.5) * gAlgorithm.sqrtDt * arw; + multiplier_Q = (real)(0.5) * gAlgorithm.sqrtDt * gAlgorithm.imuSpec.arw; multiplier_Q_Sq = multiplier_Q * multiplier_Q; } @@ -771,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_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_WBZ] = (real)1.0e-3 * gKalmanFilter.Q[STATE_WBZ];*/ } } @@ -782,6 +743,17 @@ static void _UpdateProcessCovariance(void) * block of the Q-matrix). The rest of the elements in the matrix are set * during the transition into and between EKF states (high-gain, low-gain, * etc) or above (upon first entry into this function). + * The process cov matrix of quaternion is + * [1-q0*q0 -q0*q1 -q0*q2 -q0*q3; + * -q0*q1 1-q1*q1 -q1*q2 -q1*q3; + * -q0*q2 -q1*q2 1-q2*q2 -q2*q3; + * -q0*q3 -q1*q3 -q2*q3 1-q3*q3] * (0.5*dt*sigma_gyro)^2 + * The eigenvalue of the matrix is [1 1 1 1-q0^2-q1^2-q2^2-q3^2], which means it + * is not positive defintie when quaternion norm is above or equal to 1. Quaternion + * norm can be above 1 due to numerical accuray. A scale factor 0.99 is added here to + * make sure the positive definiteness of the covariance matrix. The eigenvalues now + * are [1 1 1 1-0.99*(q0^2+q1^2+q2^2+q3^2)]. Even if there is numerical accuracy issue, + * the cov matrix is still positive definite. */ real q0q0 = gKalmanFilter.quaternion_Past[Q0] * gKalmanFilter.quaternion_Past[Q0] * 0.99f; real q0q1 = gKalmanFilter.quaternion_Past[Q0] * gKalmanFilter.quaternion_Past[Q1] * 0.99f; @@ -799,9 +771,13 @@ static void _UpdateProcessCovariance(void) // Note: this block of the covariance matrix is symmetric real tmpQMultiplier = multiplier_Q_Sq; - if ( gAlgorithm.state == INS_SOLUTION ) + /* Only considering gyro noise can underestimate the cov of the quaternion. + * A scale factor 100 is added here. This is mainly for faster convergence + * of the heading angle in the INS solution. + */ + if (gAlgorithm.state == INS_SOLUTION) { - tmpQMultiplier = 100.0 * multiplier_Q_Sq; + tmpQMultiplier = 1.0f * multiplier_Q_Sq; } gKalmanFilter.Q[STATE_Q0] = ((real)1.0 - q0q0) * tmpQMultiplier; gKalmanFilter.Qq[0] = (-q0q1) * tmpQMultiplier; @@ -831,7 +807,7 @@ void GenerateProcessCovariance(void) */ // Acceleration based values - real dtSigAccelSq = (real)(gAlgorithm.dt * SENSOR_NOISE_ACCEL_STD_DEV); + real dtSigAccelSq = (real)(gAlgorithm.dt * gAlgorithm.imuSpec.sigmaA); dtSigAccelSq = dtSigAccelSq * dtSigAccelSq; // Position @@ -839,17 +815,99 @@ void GenerateProcessCovariance(void) gKalmanFilter.Q[STATE_RY] = gAlgorithm.dtSquared * dtSigAccelSq; gKalmanFilter.Q[STATE_RZ] = gAlgorithm.dtSquared * dtSigAccelSq; - // Velocity + /* Velocity, todo. 100 is to take under-estimated accel bias, gyro bias and + * attitude error since none of them is Gaussian. Non-Gaussian error produces + * velocity drift. High-freq vibration can also be handled by this. + */ 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/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/SelectState.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/SelectState.c index 3513336..0fe20ee 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/SelectState.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/SelectState.c @@ -11,6 +11,7 @@ #include "GlobalConstants.h" #include "platformAPI.h" +#include "TimingVars.h" #include "algorithm.h" // gAlgorithm #include "algorithmAPI.h" @@ -22,7 +23,6 @@ #include "TransformationMath.h" // FieldVectorsToEulerAngles #include "EKF_Algorithm.h" #include "PredictFunctions.h" -#include "TimingVars.h" #ifndef INS_OFFLINE #ifdef DISPLAY_DIAGNOSTIC_MSG @@ -45,8 +45,6 @@ static BOOL _AverageFieldVectors(uint16_t pointsToAverage); static void _DropToHighGainAHRS(void); static void _ResetAlgorithm(void); -static uint8_t _InitINSFilter(real* leverArmN); - // StabilizeSystem: Run for a prescribed period to let the sensors settle. void StabilizeSystem(void) { @@ -60,11 +58,11 @@ void StabilizeSystem(void) */ if (gAlgorithm.stateTimer == 0) { #ifdef INS_OFFLINE - printf("To ini att. %u\n", gEKFInputData.itow); + printf("To ini att. %u\n", gEKFInput.itow); #else #ifdef DISPLAY_DIAGNOSTIC_MSG DebugPrintString("To ini att. "); - DebugPrintInt("", gEKFInputData.itow); + DebugPrintInt("", gEKFInput.itow); DebugPrintEndline(); #endif #endif @@ -111,9 +109,9 @@ void InitializeAttitude(void) /* Quasi-static check: check for motion over threshold. If detected, reset * the accumulation variables and restart initialization phase. */ - if ((fabs(gEKFInputData.angRate_B[X_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE) || - (fabs(gEKFInputData.angRate_B[Y_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE) || - (fabs(gEKFInputData.angRate_B[Z_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE)) + if ((fabs(gEKFInput.angRate_B[X_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE) || + (fabs(gEKFInput.angRate_B[Y_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE) || + (fabs(gEKFInput.angRate_B[Z_AXIS]) > LIMIT_QUASI_STATIC_STARTUP_RATE)) { accumulatedAccelVector[X_AXIS] = (real)0.0; accumulatedAccelVector[Y_AXIS] = (real)0.0; @@ -136,11 +134,11 @@ void InitializeAttitude(void) */ if (gAlgorithm.stateTimer == 0) { #ifdef INS_OFFLINE - printf("To HG. %u\n", gEKFInputData.itow); + printf("To HG. %u\n", gEKFInput.itow); #else #ifdef DISPLAY_DIAGNOSTIC_MSG DebugPrintString("To HG. "); - DebugPrintInt("", gEKFInputData.itow); + DebugPrintInt("", gEKFInput.itow); DebugPrintEndline(); #endif #endif @@ -187,6 +185,10 @@ void InitializeAttitude(void) */ EulerAnglesToQuaternion( gKalmanFilter.measuredEulerAngles, gKalmanFilter.quaternion ); + gKalmanFilter.quaternion_Past[0] = gKalmanFilter.quaternion[0]; + gKalmanFilter.quaternion_Past[1] = gKalmanFilter.quaternion[1]; + gKalmanFilter.quaternion_Past[2] = gKalmanFilter.quaternion[2]; + gKalmanFilter.quaternion_Past[3] = gKalmanFilter.quaternion[3]; // Euler angles from the initial measurement (DEBUG: initial output of the system) gKalmanFilter.eulerAngles[ROLL] = gKalmanFilter.measuredEulerAngles[ROLL]; gKalmanFilter.eulerAngles[PITCH] = gKalmanFilter.measuredEulerAngles[PITCH]; @@ -242,11 +244,11 @@ void HG_To_LG_Transition_Test(void) */ if (gAlgorithm.stateTimer == 0) { #ifdef INS_OFFLINE - printf("To LG. %u\n", gEKFInputData.itow); + printf("To LG. %u\n", gEKFInput.itow); #else #ifdef DISPLAY_DIAGNOSTIC_MSG DebugPrintString("To LG. "); - DebugPrintInt("", gEKFInputData.itow); + DebugPrintInt("", gEKFInput.itow); DebugPrintEndline(); #endif #endif @@ -292,14 +294,14 @@ void LG_To_INS_Transition_Test(void) /* If GPS output is valid (GPS providing data with a good signal lock) * then transit to INS mode. */ - if ( gpsUsedInAlgorithm() && gEKFInputData.gpsUpdate ) + if ( gpsUsedInAlgorithm() && gEKFInput.gpsUpdate && gEKFInput.gpsFixType ) { #ifdef INS_OFFLINE - printf("To INS. %u\n", gEKFInputData.itow); + printf("To INS. %u\n", gEKFInput.itow); #else #ifdef DISPLAY_DIAGNOSTIC_MSG DebugPrintString("To INS. "); - DebugPrintInt("", gEKFInputData.itow); + DebugPrintInt("", gEKFInput.itow); DebugPrintEndline(); #endif #endif @@ -310,43 +312,10 @@ void LG_To_INS_Transition_Test(void) // Transit to INS solution gAlgorithm.state = INS_SOLUTION; - // Sync the algorithm and GPS ITOW - gAlgorithm.itow = gEKFInputData.itow; - - /* We have a good GPS reading now - set this variable so we - * don't drop into INS right away - */ - gAlgorithm.timeOfLastGoodGPSReading = gEKFInputData.itow; - - /* Prepare for INS. - * R_NinE and rGPS_E are used later. rGPS_E is used in _InitINSFilter to initialize rGPS0_E. - * R_NinE is in UpdateFunctions.c to convert position to ECEF. R_NinE is also calculated in - * UpdateFunctions.c. But it is necessary here because it is needed to calculate position - * after switching to INS and before the next GNSS measurement to trigger a Kalman update. - */ - LLA_To_Base(&gEKFInputData.llaRad[0], - &gAlgorithm.rGPS0_E[0], - &gAlgorithm.rGPS_N[0], - &gAlgorithm.R_NinE[0][0], - &gAlgorithm.rGPS_E[0]); - - /* Lever-arm is antenna position w.r.t to IMU in body. rGPS_N is antenna positive - * w.r.t to initial point in NED. IMU positive w.r.t initial point is - * rGPS_N - R_b_to_N * lever-arm - */ - float leverArmN[3]; - leverArmN[0] = gKalmanFilter.R_BinN[0][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[0][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[0][2] * gAlgorithm.leverArmB[2]; - leverArmN[1] = gKalmanFilter.R_BinN[1][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[1][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[1][2] * gAlgorithm.leverArmB[2]; - leverArmN[2] = gKalmanFilter.R_BinN[2][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[2][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[2][2] * gAlgorithm.leverArmB[2]; + gAlgoStatus.bit.attitudeOnlyAlgorithm = FALSE; // Initialize the algorithm with GNSS position and lever arm - _InitINSFilter(leverArmN); + InitINSFilter(); // Set linear-acceleration switch variables gAlgorithm.linAccelSwitchCntr = 0; @@ -354,103 +323,6 @@ void LG_To_INS_Transition_Test(void) } } - - - -// -static uint8_t _InitINSFilter(real* leverArmN) -{ - real tmp[7][7]; - int rowNum, colNum; - - gAlgorithm.insFirstTime = FALSE; - - /* Upon the first entry into INS, save off the base position and reset the - * Kalman filter variables. - */ - // Save off the base ECEF location - gAlgorithm.rGPS0_E[X_AXIS] = gAlgorithm.rGPS_E[X_AXIS]; - gAlgorithm.rGPS0_E[Y_AXIS] = gAlgorithm.rGPS_E[Y_AXIS]; - gAlgorithm.rGPS0_E[Z_AXIS] = gAlgorithm.rGPS_E[Z_AXIS]; - - /* Reset the gps position (as position is relative to starting location) - * rGPS_N is the IMU position, while starting location is the antenna position. - * rGPS_N is reset to lever-arm. - */ - gAlgorithm.rGPS_N[X_AXIS] = -leverArmN[0]; - gAlgorithm.rGPS_N[Y_AXIS] = -leverArmN[1]; - gAlgorithm.rGPS_N[Z_AXIS] = -leverArmN[2]; - - // Reset prediction values. Position_N is also IMU position. - gKalmanFilter.Position_N[X_AXIS] = (real)-leverArmN[0]; - gKalmanFilter.Position_N[Y_AXIS] = (real)-leverArmN[1]; - gKalmanFilter.Position_N[Z_AXIS] = (real)-leverArmN[2]; - - gKalmanFilter.Velocity_N[X_AXIS] = (real)gEKFInputData.vNed[X_AXIS]; - gKalmanFilter.Velocity_N[Y_AXIS] = (real)gEKFInputData.vNed[Y_AXIS]; - gKalmanFilter.Velocity_N[Z_AXIS] = (real)gEKFInputData.vNed[Z_AXIS]; - - gKalmanFilter.accelBias_B[X_AXIS] = (real)0.0; - gKalmanFilter.accelBias_B[Y_AXIS] = (real)0.0; - gKalmanFilter.accelBias_B[Z_AXIS] = (real)0.0; - - /* Extract the Quaternion and rate-bias values from the matrix before - * resetting - */ - // Save off the quaternion and rate-bias covariance values - for (rowNum = Q0; rowNum <= Q3 + Z_AXIS + 1; rowNum++) - { - for (colNum = Q0; colNum <= Q3 + Z_AXIS + 1; colNum++) - { - tmp[rowNum][colNum] = gKalmanFilter.P[rowNum + STATE_Q0][colNum + STATE_Q0]; - } - } - - // Reset P - memset(gKalmanFilter.P, 0, sizeof(gKalmanFilter.P)); - for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) - { - gKalmanFilter.P[rowNum][rowNum] = (real)INIT_P_INS; - } - - // Repopulate the P matrix with the quaternion and rate-bias values - for (rowNum = Q0; rowNum <= Q3 + Z_AXIS + 1; rowNum++) - { - for (colNum = Q0; colNum <= Q3 + Z_AXIS + 1; colNum++) - { - gKalmanFilter.P[rowNum + STATE_Q0][colNum + STATE_Q0] = tmp[rowNum][colNum]; - } - } - - /* Use the GPS-provided horizontal and vertical accuracy values to populate - * the covariance values. - */ - gKalmanFilter.P[STATE_RX][STATE_RX] = gEKFInputData.GPSHorizAcc * gEKFInputData.GPSHorizAcc; - gKalmanFilter.P[STATE_RY][STATE_RY] = gKalmanFilter.P[STATE_RX][STATE_RX]; - gKalmanFilter.P[STATE_RZ][STATE_RZ] = gEKFInputData.GPSVertAcc * gEKFInputData.GPSVertAcc; - - /* Scale the best velocity error by HDOP then multiply by the z-axis angular - * rate PLUS one (to prevent the number from being zero) so the velocity - * update during high-rate turns is reduced. - */ - float temp = (real)0.0625 * gEKFInputData.HDOP; // 0.0625 = 0.05 / 0.8 - real absFilteredYawRate = (real)fabs(gAlgorithm.filteredYawRate); - if (absFilteredYawRate > TEN_DEGREES_IN_RAD) - { - temp *= (1.0f + absFilteredYawRate); - } - gKalmanFilter.P[STATE_VX][STATE_VX] = temp;// *((real)1.0 + fabs(gAlgorithm.filteredYawRate) * (real)RAD_TO_DEG); - gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_VX] * gKalmanFilter.P[STATE_VX][STATE_VX]; - gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.P[STATE_VX][STATE_VX]; - - // z-axis velocity isn't really a function of yaw-rate and hdop - //gKalmanFilter.R[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VX][STATE_VX]; - gKalmanFilter.P[STATE_VZ][STATE_VZ] = (float)(0.1 * 0.1); - - return 1; -} - - /* INS_To_AHRS_Transition_Test: Drop back to LG AHRS operation if... * 1) GPS drops out for more than 3 seconds * 2) magnetometer data not available AND at rest too long @@ -459,21 +331,25 @@ static uint8_t _InitINSFilter(real* leverArmN) void INS_To_AHRS_Transition_Test(void) { // Record last GPS velocity large enough to give a good heading measurement - if (gEKFInputData.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING) + if (gEKFInput.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING) { - gAlgorithm.timeOfLastSufficientGPSVelocity = (int32_t)gEKFInputData.itow; + gAlgorithm.timeOfLastSufficientGPSVelocity = (int32_t)gEKFInput.itow; } /* Determine the length of time it has been since the system 'moved' -- * only linear motion considered (rotations ignored). */ - int32_t timeSinceRestBegan = (int32_t)gEKFInputData.itow - gAlgorithm.timeOfLastSufficientGPSVelocity; + int32_t timeSinceRestBegan = (int32_t)gEKFInput.itow - gAlgorithm.timeOfLastSufficientGPSVelocity; if (timeSinceRestBegan < 0) { timeSinceRestBegan = timeSinceRestBegan + MAX_ITOW; } - if (timeSinceRestBegan > LIMIT_MAX_REST_TIME_BEFORE_HEADING_INVALID) + if (timeSinceRestBegan > LIMIT_MAX_REST_TIME_BEFORE_HEADING_INVALID && gAlgorithm.headingIni != HEADING_UNINITIALIZED) { - gAlgorithm.gnssHeadingFirstTime = TRUE; + gAlgorithm.headingIni = HEADING_GNSS_LOW; +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("Rest for too long."); + DebugPrintEndline(); +#endif } // compute time since the last good GPS reading @@ -482,11 +358,15 @@ void INS_To_AHRS_Transition_Test(void) timeSinceLastGoodGPSReading = timeSinceLastGoodGPSReading + MAX_ITOW; } - if ( timeSinceLastGoodGPSReading > gAlgorithm.Limit.Max_GPS_Drop_Time ) + if ( timeSinceLastGoodGPSReading > gAlgorithm.Limit.maxGpsDropTime ) { +#ifdef INS_OFFLINE + printf("GPS outage too long\n"); +#endif // INS_OFFLINE + // Currently in INS mode but requiring a transition to AHRS / VG gAlgorithm.insFirstTime = TRUE; - gAlgorithm.gnssHeadingFirstTime = TRUE; + gAlgorithm.headingIni = HEADING_UNINITIALIZED; /* The transition from INS to AHRS and back to INS does not seem to * generate a stable solution if we transition to LG AHRS for only 30 @@ -511,7 +391,6 @@ void INS_To_AHRS_Transition_Test(void) gAlgoStatus.bit.highGain = ( gAlgorithm.state == HIGH_GAIN_AHRS ); gAlgoStatus.bit.attitudeOnlyAlgorithm = TRUE; } - } @@ -581,22 +460,22 @@ static void _DropToHighGainAHRS(void) static BOOL _AccumulateFieldVectors(void) { // Accumulate the acceleration vector readings (accels in g's) - accumulatedAccelVector[X_AXIS] += (real)gEKFInputData.accel_B[X_AXIS]; - accumulatedAccelVector[Y_AXIS] += (real)gEKFInputData.accel_B[Y_AXIS]; - accumulatedAccelVector[Z_AXIS] += (real)gEKFInputData.accel_B[Z_AXIS]; + accumulatedAccelVector[X_AXIS] += (real)gEKFInput.accel_B[X_AXIS]; + accumulatedAccelVector[Y_AXIS] += (real)gEKFInput.accel_B[Y_AXIS]; + accumulatedAccelVector[Z_AXIS] += (real)gEKFInput.accel_B[Z_AXIS]; // Accumulate the gyroscope vector readings (accels in rad/s) - accumulatedGyroVector[X_AXIS] += gEKFInputData.angRate_B[X_AXIS]; - accumulatedGyroVector[Y_AXIS] += gEKFInputData.angRate_B[Y_AXIS]; - accumulatedGyroVector[Z_AXIS] += gEKFInputData.angRate_B[Z_AXIS]; + accumulatedGyroVector[X_AXIS] += gEKFInput.angRate_B[X_AXIS]; + accumulatedGyroVector[Y_AXIS] += gEKFInput.angRate_B[Y_AXIS]; + accumulatedGyroVector[Z_AXIS] += gEKFInput.angRate_B[Z_AXIS]; // Accumulate the magnetic-field vector readings (or set to zero if the // product does not have magnetometers) if (magUsedInAlgorithm() ) { - accumulatedMagVector[X_AXIS] += (real)gEKFInputData.magField_B[X_AXIS]; - accumulatedMagVector[Y_AXIS] += (real)gEKFInputData.magField_B[Y_AXIS]; - accumulatedMagVector[Z_AXIS] += (real)gEKFInputData.magField_B[Z_AXIS]; + accumulatedMagVector[X_AXIS] += (real)gEKFInput.magField_B[X_AXIS]; + accumulatedMagVector[Y_AXIS] += (real)gEKFInput.magField_B[Y_AXIS]; + accumulatedMagVector[Z_AXIS] += (real)gEKFInput.magField_B[Z_AXIS]; } else { accumulatedMagVector[X_AXIS] = (real)0.0; accumulatedMagVector[Y_AXIS] = (real)0.0; diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/TimingVars.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/TimingVars.c index 00ba3dc..f4de8d9 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/TimingVars.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/TimingVars.c @@ -19,9 +19,6 @@ limitations under the License. #include "TimingVars.h" -#ifdef INS_OFFLINE -#include "SimulationParameters.h" -#endif TimingVars timer; // for InitTimingVars @@ -161,14 +158,11 @@ void Initialize_Timing(void) // toggles between 0 and 1 at 200 Hz (currently used in firmware) timer.oneHundredHertzFlag = 0; -#ifdef INS_OFFLINE - // This value is set based on the version string specified in the - // simulation configuration file, ekfSim.cfg - // IMU: 0/1, VG: 2, AHRS: 3, Aided-VG: 4, Aided-AHRS: 5, INS: 6 - uint8_t sysType = gSimulation.sysType; -#endif - // Override execution rate of taskDataAcquisition() based on the configuration timer.dacqFrequency = DACQ_200_HZ; // default } +void TimingVars_dacqFrequency(int freq) +{ + timer.dacqFrequency = freq; +} diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/UpdateFunctions.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/UpdateFunctions.c index ab4beb1..2759483 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/UpdateFunctions.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/UpdateFunctions.c @@ -24,20 +24,19 @@ #include "algorithm.h" #include "algorithmAPI.h" #include "AlgorithmLimits.h" +#include "MotionStatus.h" #include "EKF_Algorithm.h" #include "UpdateFunctions.h" #include "SensorNoiseParameters.h" -extern AccelStatsStruct gAccelStats; +extern ImuStatsStruct gImuStats; #if FAST_MATH #include "arm_math.h" #endif // FAST_MATH -#ifdef INS_OFFLINE -#include "SimulationParameters.h" -#else +#ifndef INS_OFFLINE #ifdef DISPLAY_DIAGNOSTIC_MSG #include "debug.h" #endif @@ -63,20 +62,36 @@ 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 + * heading is considered valid, and the eading will be initialized to be + * gEKFInput.trueCourse, and velocity will also be initiazlied as the + * corresponding NED speed. After this, the quaternion (q0 and q3) and velocity + * terms in the state covariance matrix P will be reset. Non-diagonal terms will be + * set as 0s, and diagonal terms will be set according to estimated variance. The + * cov(quaternion, velocity) should also be updated. But the positive-definiteness + * is not guaranteed this way. + * TRACE: + * @retval TRUE if heading initialized/reinitialized, FALSE if not. +******************************************************************************/ +static int InitializeHeadingFromGnss(); + /****************************************************************************** - * @brief Initializa heading when GPS velocity is above a certain threshhold. - * If the GPS velocity is above a certain threshold, a global variable useGpsHeading - * will be set to 1. And in this function, the heading will be initialized to be - * gEKFInputData.trueCourse, and velocity will also be initiazlied as the + * @brief When heading is ready for initialization, the heading angle (yaw, and + * indeed quaternion in the Kalman filter) is initialized to match the value of + * gEKFInput.trueCourse, and velocity will also be initiazlied as the * corresponding NED speed. After this, the quaternion (q0 and q3) and velocity * terms in the state covariance matrix P will be reset. Non-diagonal terms will be * set as 0s, and diagonal terms will be set according to estimated variance. The * cov(quaternion, velocity) should also be updated. But the positive-definiteness * is not guaranteed this way. * TRACE: - * @retval TRUE if heading initialized, FALSE if not. + * @retval None. ******************************************************************************/ -static int InitializeGnssHeading(); +static void InitializeEkfHeading(); // Update rates #define TEN_HERTZ_UPDATE 10 @@ -85,25 +100,28 @@ static int InitializeGnssHeading(); #define FIFTY_HERTZ_UPDATE 50 #define ONE_HUNDRED_HERTZ_UPDATE 100 -static uint32_t updateCntr[2] = { 0, 0 }; static BOOL useGpsHeading = 0; /* When GPS velocity is above a certain threshold, * this is set to 1, and GPS heading measurement * is used, otherwise, this is set to 0 and magnetic - * heading is used. */ + * heading is used. + */ static int runInsUpdate = 0; /* To enable the update to be broken up into * two sequential calculations in two sucessive - * 100 Hz periods. */ + * 100 Hz periods. + */ // Uncomment to run only AHRS-type updates //#define ATT_UPDATE_ONLY static void Update_GPS(void); +static void Update_PseudoMeasurement(void); +static void GenPseudoMeasCov(real *r); // EKF_UpdateStage.m void EKF_UpdateStage(void) { /* Perform a VG/AHRS update, regardless of GPS availability or health, - * when the state is HG AHRS or LG AHRS. Once GPS becomes healthy + * when the state is HG AHRS or LG AHRS. Once GPS becomes healthy * (and the right conditions are met) perform an INS or reduced-order GPS update. */ if( gAlgorithm.state <= LOW_GAIN_AHRS ) @@ -131,12 +149,76 @@ void EKF_UpdateStage(void) * Check for 'new GPS data'. If new, and GPS is valid, perform a * GPS-Based update and reset timer values to resync the attitude updates. */ - if( gEKFInputData.gpsUpdate ) - { - if (gEKFInputData.gpsValid) + if( gEKFInput.gpsUpdate ) + { + /* 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; + + // GNSS update + if (gEKFInput.gpsFixType) + { + // GPS heading valid? + 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 + * good, just correct the filter states with input GNSS measurement. + */ + int32_t timeSinceLastGoodGPSReading = (int32_t)gAlgorithm.itow - gAlgorithm.timeOfLastGoodGPSReading; + if (timeSinceLastGoodGPSReading < 0) + { + timeSinceLastGoodGPSReading = timeSinceLastGoodGPSReading + MAX_ITOW; + } + if (timeSinceLastGoodGPSReading > gAlgorithm.Limit.maxReliableDRTime) + { +#ifdef INS_OFFLINE + printf("GPS relocked.\n"); +#endif // INS_OFFLINE + // Since a relative long time has passed since DR begins, INS states need reinitialized. + InitINSFilter(); + } + else + { + // DR for a relative short time, no need to reinitialize the filter. + Update_GPS(); + } + // reset the "last good reading" time + gAlgorithm.timeOfLastGoodGPSReading = gEKFInput.itow; + } + + // apply motion constraints + if (gAlgorithm.velocityAlwaysAlongBodyX && gAlgorithm.headingIni>HEADING_UNINITIALIZED) { - Update_GPS(); + Update_PseudoMeasurement(); } + // At 1 Hz mark, update when GPS data is valid, else do an AHRS-update runInsUpdate = 1; } @@ -144,33 +226,12 @@ void EKF_UpdateStage(void) { Update_Att(); runInsUpdate = 0; // set up for next pass - useGpsHeading = 0; - } - - // Update LLA at 100/200 Hz - if ( gEKFInputData.gpsValid && ( gAlgorithm.insFirstTime == FALSE ) ) - { - //r_E = Base_To_ECEF( &gKalmanFilter.Position_N[0], &gAlgorithm.rGPS0_E[0], &R_NinE[0][0] ); // - double r_E[NUM_AXIS]; - float pointOfInterestN[3]; - pointOfInterestN[0] = gKalmanFilter.R_BinN[0][0] * gAlgorithm.pointOfInterestB[0] + - gKalmanFilter.R_BinN[0][1] * gAlgorithm.pointOfInterestB[1] + - gKalmanFilter.R_BinN[0][2] * gAlgorithm.pointOfInterestB[2]; - pointOfInterestN[1] = gKalmanFilter.R_BinN[1][0] * gAlgorithm.pointOfInterestB[0] + - gKalmanFilter.R_BinN[1][1] * gAlgorithm.pointOfInterestB[1] + - gKalmanFilter.R_BinN[1][2] * gAlgorithm.pointOfInterestB[2]; - pointOfInterestN[2] = gKalmanFilter.R_BinN[2][0] * gAlgorithm.pointOfInterestB[0] + - gKalmanFilter.R_BinN[2][1] * gAlgorithm.pointOfInterestB[1] + - gKalmanFilter.R_BinN[2][2] * gAlgorithm.pointOfInterestB[2]; - pointOfInterestN[0] += gKalmanFilter.Position_N[0]; - pointOfInterestN[1] += gKalmanFilter.Position_N[1]; - pointOfInterestN[2] += gKalmanFilter.Position_N[2]; - PosNED_To_PosECEF(pointOfInterestN, &gAlgorithm.rGPS0_E[0], &gAlgorithm.R_NinE[0][0], &r_E[0] ); - // 100 Hz generated once 1 Hz 100 Hz - - //gKalmanFilter.llaDeg[LAT_IDX] = ECEF2LLA( r_E ); // output variable (ned used for anything else); this is in [ deg, deg, m ] - ECEF_To_LLA(&gKalmanFilter.llaDeg[LAT], &r_E[X_AXIS]); - // 100 Hz 100 Hz + // handle P when PPS is available + if (gAlgoStatus.bit.ppsAvailable) + { + ApplyGpsDealyCorrForStateCov(); + gAlgoStatus.bit.ppsAvailable = FALSE; + } } } } @@ -179,9 +240,18 @@ void EKF_UpdateStage(void) void ComputeSystemInnovation_Pos(void) { // Position error - gKalmanFilter.nu[STATE_RX] = gAlgorithm.rGPS_N[X_AXIS] - gKalmanFilter.Position_N[X_AXIS]; - gKalmanFilter.nu[STATE_RY] = gAlgorithm.rGPS_N[Y_AXIS] - gKalmanFilter.Position_N[Y_AXIS]; - gKalmanFilter.nu[STATE_RZ] = gAlgorithm.rGPS_N[Z_AXIS] - gKalmanFilter.Position_N[Z_AXIS]; + 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); @@ -193,9 +263,18 @@ void ComputeSystemInnovation_Pos(void) void ComputeSystemInnovation_Vel(void) { // Velocity error - gKalmanFilter.nu[STATE_VX] = (real)gEKFInputData.vNed[X_AXIS] - gKalmanFilter.Velocity_N[X_AXIS]; - gKalmanFilter.nu[STATE_VY] = (real)gEKFInputData.vNed[Y_AXIS] - gKalmanFilter.Velocity_N[Y_AXIS]; - gKalmanFilter.nu[STATE_VZ] = (real)gEKFInputData.vNed[Z_AXIS] - gKalmanFilter.Velocity_N[Z_AXIS]; + 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); @@ -224,8 +303,24 @@ void ComputeSystemInnovation_Att(void) // CHANGED TO SWITCH BETWEEN GPS AND MAG UPDATES if ( useGpsHeading ) { - gKalmanFilter.nu[STATE_YAW] = (real)gEKFInputData.trueCourse * (real)DEG_TO_RAD - - gKalmanFilter.eulerAngles[YAW]; + 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; + } + } else if ( magUsedInAlgorithm() && gAlgorithm.state <= LOW_GAIN_AHRS ) { @@ -368,7 +463,6 @@ uint8_t _GenerateObservationJacobian_AHRS(void) // void _GenerateObservationCovariance_AHRS(void) { - // static real Rnom; // Only need to compute certain elements of R once @@ -382,48 +476,12 @@ void _GenerateObservationCovariance_AHRS(void) */ memset(gKalmanFilter.R, 0, sizeof(gKalmanFilter.R)); -#ifdef INS_OFFLINE - /* This value is set based on the version string specified in the - * simulation configuration file, ekfSim.cfg - */ - uint8_t sysRange = gSimulation.sysRange; -#else - /* This value is set based on the version string loaded into the unit - * via the system configuration load - */ - uint8_t sysRange = platformGetSysRange(); // from system config -#endif - - /* Set the matrix, R, based on whether the system is operating in high - * or low-gain (is the acceleration above or below the acceleration threshold) - * - * R-values are based on the variance of the roll and pitch angles - * generated from the sensor noise passed through the measurement - * model. The values are multiplied by dt^2 as well as vary with the - * angle. The value can be made large enough to work with all angles - * but it may slow the response. - * - * These values are found by passing the accelerometer VRW values - * (determined from a very limited data set) through a Matlab - * script which generates the roll and pitch noise based on the - * sensor noise. The value below is the 1-sigma value at 0 - * degrees. The quadratic correction below is meant to increase - * R as the angle increases (due to the geometry and - * mathematical function used to compute the angle). - * - * Matlab script: R_Versus_Theta.m + /* Calculate accel var when static from IMU specs. + * This accel var is the min accel var. If real-time accel var is below this value, + * the min accel var is used. + * Accel var is further converted to Euler angels measurement var. */ - switch (sysRange) - { - case _200_DPS_RANGE: - // -200 VRW value (average x/y/z): 7.2e-4 [(m/sec)/rt-sec] - Rnom = (real)9.82e-07; // (9.91e-4)^2 - break; - case _400_DPS_RANGE: - // -400 VRW value (average x/y/z): 8.8e-4 [(m/sec)/rt-sec] - Rnom = (real)1.54e-06; // (1.24e-3)^2 - break; - } + Rnom = gAlgorithm.imuSpec.sigmaA * gAlgorithm.imuSpec.sigmaA; } /* Dynamically tune measurement covariance matrix R to get proper Kalman filter @@ -433,13 +491,13 @@ void _GenerateObservationCovariance_AHRS(void) */ // Rnom, accel var and accel error - real totalAccelVar[3]; + real totalAccelVar[3]; // [m/s/s]^2 for (int i = 0; i < 3; i++) { // replace sensor noise var with vibration var - if (gAccelStats.accelVar[i] > Rnom) + if (gImuStats.accelVar[i] > Rnom) { - totalAccelVar[i] = gAccelStats.accelVar[i]; + totalAccelVar[i] = gImuStats.accelVar[i]; } else { @@ -447,7 +505,7 @@ void _GenerateObservationCovariance_AHRS(void) } // linear accel? (including noise and vibration) real errSqr; - errSqr = gAccelStats.accelErr[i] * gAccelStats.accelErr[i]; + errSqr = gImuStats.accelErr[i] * gImuStats.accelErr[i]; if (errSqr > totalAccelVar[i]) { totalAccelVar[i] = errSqr; @@ -458,13 +516,13 @@ void _GenerateObservationCovariance_AHRS(void) * Notice: totalAccelVarSum just approximates accel norm var */ real totalAccelVarSum = totalAccelVar[X_AXIS] + totalAccelVar[Y_AXIS] + totalAccelVar[Z_AXIS]; - real diff = gAccelStats.accelNorm - 1.0f; + real diff = gImuStats.accelNorm - GRAVITY; diff *= diff; real additionalR = 0.0; /* if diff is larger than estimated accel err and the estimated accel err does * not reach limit, diff will be used as additional measurement noise var. */ - if (diff > 4.0*totalAccelVarSum && gAccelStats.accelErrLimit == false) + if (diff > 4.0*totalAccelVarSum && gImuStats.accelErrLimit == false) { // the magnitude of diff is too big, there is linear acceleration additionalR = diff; @@ -481,9 +539,9 @@ void _GenerateObservationCovariance_AHRS(void) * Notice: var(kx) = k*k*var(x) */ // Get ax^2, ay^2 and az^2 of normalized accel - real axSqr = gAccelStats.lpfAccel[0] * gAccelStats.lpfAccel[0]; - real aySqr = gAccelStats.lpfAccel[1] * gAccelStats.lpfAccel[1]; - real azSqr = gAccelStats.lpfAccel[2] * gAccelStats.lpfAccel[2]; + real axSqr = gImuStats.lpfAccel[0] * gImuStats.lpfAccel[0]; + real aySqr = gImuStats.lpfAccel[1] * gImuStats.lpfAccel[1]; + real azSqr = gImuStats.lpfAccel[2] * gImuStats.lpfAccel[2]; real sumSqr = axSqr + aySqr + azSqr; axSqr /= sumSqr; aySqr /= sumSqr; @@ -505,7 +563,20 @@ void _GenerateObservationCovariance_AHRS(void) gKalmanFilter.R[STATE_ROLL] += additionalR; gKalmanFilter.R[STATE_PITCH] += additionalR; - // limit R + /* We are indeed using var of multiple accel samples to estimate the var of Euler + * angles. From the formula above, accel var should be var of normalized accel. + * However, we choose GRAVITY instead of real accel norm to normalize the accel. + * Besides, accel var is only an estimate of Euler angles var, and Euler angels + * var is indeed not Gaussian. + */ + gKalmanFilter.R[STATE_ROLL] /= GRAVITY * GRAVITY; + gKalmanFilter.R[STATE_PITCH] /= GRAVITY * GRAVITY; + + /* limit R + * In previous version, Rnom is in untis of [g]^2, and maxR = 40000.0f*Rnom. + * 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; if (gKalmanFilter.R[STATE_ROLL] > maxR) { @@ -550,7 +621,7 @@ void _GenerateObservationCovariance_AHRS(void) */ if ( useGpsHeading ) { - float temp = (float)atan( 0.05 / gEKFInputData.rawGroundSpeed ); + float temp = (float)atan( 0.05 / gEKFInput.rawGroundSpeed ); gKalmanFilter.R[STATE_YAW] = temp * temp; if (gAlgoStatus.bit.turnSwitch) { @@ -583,7 +654,7 @@ void _GenerateObservationCovariance_AHRS(void) } else { - gKalmanFilter.R[STATE_YAW] = (real)0.1; + gKalmanFilter.R[STATE_YAW] = (real)1.0; } } @@ -608,15 +679,15 @@ void _GenerateObservationCovariance_INS(void) /* Use the GPS-provided horizontal and vertical accuracy values to populate * the covariance values. */ - gKalmanFilter.R[STATE_RX] = gEKFInputData.GPSHorizAcc * gEKFInputData.GPSHorizAcc; + gKalmanFilter.R[STATE_RX] = gEKFInput.GPSHorizAcc * gEKFInput.GPSHorizAcc; gKalmanFilter.R[STATE_RY] = gKalmanFilter.R[STATE_RX]; - gKalmanFilter.R[STATE_RZ] = gEKFInputData.GPSVertAcc * gEKFInputData.GPSVertAcc; + gKalmanFilter.R[STATE_RZ] = gEKFInput.GPSVertAcc * gEKFInput.GPSVertAcc; /* Scale the best velocity error by HDOP then multiply by the z-axis angular * rate PLUS one (to prevent the number from being zero) so the velocity * update during high-rate turns is reduced. */ - float temp = (real)0.0625 * gEKFInputData.HDOP; // 0.0625 = 0.05 / 0.8 + float temp = (real)0.0625 * gEKFInput.HDOP; // 0.0625 = 0.05 / 0.8 real absFilteredYawRate = (real)fabs(gAlgorithm.filteredYawRate); if (absFilteredYawRate > TEN_DEGREES_IN_RAD) { @@ -625,6 +696,14 @@ void _GenerateObservationCovariance_INS(void) gKalmanFilter.R[STATE_VX] = temp;// *((real)1.0 + fabs(gAlgorithm.filteredYawRate) * (real)RAD_TO_DEG); gKalmanFilter.R[STATE_VX] = gKalmanFilter.R[STATE_VX] * gKalmanFilter.R[STATE_VX]; gKalmanFilter.R[STATE_VY] = gKalmanFilter.R[STATE_VX]; + if (gAlgorithm.headingIni == HEADING_UNINITIALIZED) + { + /* When heading is not initialized, velocity measurement is not able to correct + * attitude/rate bias/accel bias, the larger the velocity, the more uncertain it is. + */ + gKalmanFilter.R[STATE_VX] += SQUARE(gEKFInput.vNed[0]) + SQUARE(gEKFInput.vNed[1]); + gKalmanFilter.R[STATE_VY] += gKalmanFilter.R[STATE_VX]; + } // z-axis velocity isn't really a function of yaw-rate and hdop gKalmanFilter.R[STATE_VZ] = (float)(0.1 * 0.1); @@ -637,10 +716,10 @@ 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) { + static real lastYaw = 7.0; // a values larger than 2pi means this yaw is invalid // which state is updated in Update_Att() uint8_t updatedStatesAtt[16] = { 1, 1, 1, // Positions are not updated 1, 1, 1, // Velocities are not updated @@ -653,14 +732,67 @@ void Update_Att(void) */ _GenerateObservationJacobian_AHRS(); // gKF.H: 3x16 _GenerateObservationCovariance_AHRS(); // gKF.R: 3x3 - // If neither mag or GPS headig is available, update measuremnt matrix H to 2x16 - if (gKalmanFilter.R[STATE_YAW] > 0.9) + + // 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) + { + /* 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 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 + { + 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[2][colNum] = 0.0; + gKalmanFilter.H[0][colNum] = 0.0; + gKalmanFilter.H[1][colNum] = 0.0; } } + /* This solution consists of an integrated roll/pitch/yaw solution * S = H*P*HTrans + R (However the matrix math can be simplified since * H is very sparse! P is fully populated) @@ -751,9 +883,6 @@ void Update_Att(void) /* Update covariance: P = P + DP = P - K*H*P * KxH = gKF.K * gKF.H; */ - /* 2) Use gKalmanFilter.P as a temporary variable to hold FxPxFTranspose - * to reduce the number of "large" variables on the heap - */ memset(KxH, 0, sizeof(KxH)); for (rowNum = 0; rowNum < ROWS_IN_K; rowNum++) { @@ -768,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 */ @@ -779,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 @@ -804,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 @@ -819,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]; } } @@ -831,7 +960,7 @@ void Update_Att(void) */ void Update_Pos(void) { - // which state is updated in Update_Att() + // which state is updated in Update_Pos() uint8_t updatedStatesPos[16] = { 1, 1, 1, // Positions are updated 1, 1, 1, // Velocities are updated 1, 1, 1, 1, // Quaternions are NOT updated @@ -891,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++) { @@ -914,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]; @@ -932,21 +1062,21 @@ void Update_Pos(void) gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_VY]; gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_VZ]; - //gKalmanFilter.quaternion[Q0] = gKalmanFilter.quaternion[Q0] + gKalmanFilter.stateUpdate[STATE_Q0]; - //gKalmanFilter.quaternion[Q1] = gKalmanFilter.quaternion[Q1] + gKalmanFilter.stateUpdate[STATE_Q1]; - //gKalmanFilter.quaternion[Q2] = gKalmanFilter.quaternion[Q2] + gKalmanFilter.stateUpdate[STATE_Q2]; - //gKalmanFilter.quaternion[Q3] = gKalmanFilter.quaternion[Q3] + gKalmanFilter.stateUpdate[STATE_Q3]; - // - //// Normalize quaternion and force q0 to be positive - //QuatNormalize(gKalmanFilter.quaternion); - // - //gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; - //gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; - //gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; - // - //gKalmanFilter.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_ABX]; - //gKalmanFilter.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_ABY]; - //gKalmanFilter.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_ABZ]; + gKalmanFilter.quaternion[Q0] = gKalmanFilter.quaternion[Q0] + gKalmanFilter.stateUpdate[STATE_Q0]; + gKalmanFilter.quaternion[Q1] = gKalmanFilter.quaternion[Q1] + gKalmanFilter.stateUpdate[STATE_Q1]; + gKalmanFilter.quaternion[Q2] = gKalmanFilter.quaternion[Q2] + gKalmanFilter.stateUpdate[STATE_Q2]; + gKalmanFilter.quaternion[Q3] = gKalmanFilter.quaternion[Q3] + gKalmanFilter.stateUpdate[STATE_Q3]; + + // Normalize quaternion and force q0 to be positive + QuatNormalize(gKalmanFilter.quaternion); + + gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; + gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; + gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; + + gKalmanFilter.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_ABX]; + gKalmanFilter.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_ABY]; + gKalmanFilter.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_ABZ]; } @@ -955,7 +1085,7 @@ void Update_Pos(void) */ void Update_Vel(void) { - // which state is updated in Update_Att() + // which state is updated in Update_Vel() uint8_t updatedStatesVel[16] = { 1, 1, 1, // Positions are NOT updated 1, 1, 1, // Velocities are updated 1, 1, 1, 1, // Quaternions are updated @@ -1015,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++) { @@ -1034,21 +1164,21 @@ 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 - //gKalmanFilter.Position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_RX]; - //gKalmanFilter.Position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_RY]; - //gKalmanFilter.Position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_RZ]; + gKalmanFilter.Position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_RX]; + gKalmanFilter.Position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_RY]; + gKalmanFilter.Position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_RZ]; gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_VX]; gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_VY]; @@ -1062,59 +1192,20 @@ void Update_Vel(void) // Normalize quaternion and force q0 to be positive QuatNormalize(gKalmanFilter.quaternion); - //gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; - //gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; - //gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; - - gKalmanFilter.accelBias_B[X_AXIS] = gKalmanFilter.accelBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_ABX]; - gKalmanFilter.accelBias_B[Y_AXIS] = gKalmanFilter.accelBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_ABY]; - gKalmanFilter.accelBias_B[Z_AXIS] = gKalmanFilter.accelBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_ABZ]; + gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; + gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; + gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; + + 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) -{ - // Sync the algorithm itow to the GPS value - gAlgorithm.itow = gEKFInputData.itow; - - // Resync timer - timer.tenHertzCntr = 0; - timer.subFrameCntr = 0; - - // Debugging counter: updateCntr[0] should be about 9x less than updateCntr[1] - updateCntr[0] = updateCntr[0] + 1; - - // reset the "last good reading" time - gAlgorithm.timeOfLastGoodGPSReading = gEKFInputData.itow; - - // GPS heading valid? - useGpsHeading = (gEKFInputData.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING); +{ + // Calculate the R-values for the INS measurements + _GenerateObservationCovariance_INS(); - /* Extract what's common between the following function and the - * routines below so we aren't repeating calculations - */ - LLA_To_Base(&gEKFInputData.llaRad[0], - &gAlgorithm.rGPS0_E[0], - &gAlgorithm.rGPS_N[0], - &gAlgorithm.R_NinE[0][0], - &gAlgorithm.rGPS_E[0]); - - /* Lever-arm is antenna position w.r.t to IMU in body. rGPS_N is antenna positive - * w.r.t to initial point in NED. IMU positive w.r.t initial point is - * rGPS_N - R_b_to_N * lever-arm - */ - float leverArmN[3]; - leverArmN[0] = gKalmanFilter.R_BinN[0][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[0][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[0][2] * gAlgorithm.leverArmB[2]; - leverArmN[1] = gKalmanFilter.R_BinN[1][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[1][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[1][2] * gAlgorithm.leverArmB[2]; - leverArmN[2] = gKalmanFilter.R_BinN[2][0] * gAlgorithm.leverArmB[0] + - gKalmanFilter.R_BinN[2][1] * gAlgorithm.leverArmB[1] + - gKalmanFilter.R_BinN[2][2] * gAlgorithm.leverArmB[2]; - gAlgorithm.rGPS_N[0] -= leverArmN[0]; - gAlgorithm.rGPS_N[1] -= leverArmN[1]; - gAlgorithm.rGPS_N[2] -= leverArmN[2]; /* This Sequential-Filter (three-stage approach) is nearly as * good as the full implementation -- we also can split it * across multiple iterations to not exceed 10 ms execution on @@ -1123,22 +1214,25 @@ 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(); - ComputeSystemInnovation_Vel(); - ComputeSystemInnovation_Att(); - - // Calculate the R-values for the INS measurements - _GenerateObservationCovariance_INS(); - Update_Pos(); - + ComputeSystemInnovation_Vel(); Update_Vel(); + ComputeSystemInnovation_Att(); // Initialize heading. If getting initial heading at this step, do not update att - if (gAlgorithm.gnssHeadingFirstTime) + if (gAlgorithm.headingIni < HEADING_GNSS_HIGH) { - if (InitializeGnssHeading()) + if (InitializeHeadingFromGnss()) { + // Heading is initialized. Related elements in the EKF also need intializing. + InitializeEkfHeading(); + /* This heading measurement is used to initialize heading, and should not be * used to update heading. */ @@ -1147,6 +1241,268 @@ static void Update_GPS(void) } } +static void Update_PseudoMeasurement(void) +{ + // which state is updated in Update_Vel() + uint8_t updatedStatesPseudo[16] = { 1, 1, 1, // Positions are NOT updated + 1, 1, 1, // Velocities are updated + 1, 1, 1, 1, // Quaternions are updated + 1, 1, 1, // Gyro biases are updated + 1, 1, 1 }; // Accel biases are upated + + /* Get current rb2n. + * gKalmanFilter.R_BinN is updated every time the algo enters _PredictStateEstimate + * After prediction and GPS update, this matrix needs updated. + */ + real rb2n[3][3]; + QuaternionToR321(gKalmanFilter.quaternion, &rb2n[0][0]); + + // detect zero velocity using GNSS vNED + BOOL staticGnss = DetectStaticGnssVelocity(gEKFInput.vNed, + gAlgorithm.staticDetectParam.staticGnssVel, + gEKFInput.gpsFixType); + + // measurement cov + real r[3] = { 1.0e-4f, 1.0e-4f, 1.0e-4f }; + if (!gImuStats.bStaticIMU) + { + /* If zero velocity is not detected by IMU, the covariance for the lateral and + * vertical velocity measurement should be increased. + */ + GenPseudoMeasCov(r); + r[1] = 1.0e-1; + r[2] = 1.0e-1; + } + + /* Compute innovation (measured - estimated) of velocity expressed in the body frame: + * innovation = [odo/0.0, 0.0, 0.0] - Rn2b * v_ned + * When odometer is available, front velocity measurement is given by odometer. + * When zero velocity detected, front velocity measurement is 0. + * 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 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 (odoUsedInAlgorithm()) + { + frontVelMeaValid = TRUE; + frontVelMea = gEKFInput.odoVelocity; + r[0] = 1.0e-4; // variance of front velocity measurement should be from odo spec + } + else if (gImuStats.bStaticIMU) + { + /* Only when GNSS is invalid or zero velocity is also detected by GNSS, zero velocity + * detected by IMU (and GNSS) can be used to determine the along-track velocity. + * When front velocity measurement is not available, it is not necessary to readjust + * its variance since it will not be used. + */ + if ((!gEKFInput.gpsFixType) || staticGnss) + { + frontVelMeaValid = TRUE; + frontVelMea = 0.0; + } + } + // front vel error + gKalmanFilter.nu[STATE_VX] = frontVelMea + -rb2n[0][0] * gKalmanFilter.Velocity_N[0] + -rb2n[1][0] * gKalmanFilter.Velocity_N[1] + -rb2n[2][0] * gKalmanFilter.Velocity_N[2]; + // lateral (right) vel error + gKalmanFilter.nu[STATE_VY] = + -rb2n[0][1] * gKalmanFilter.Velocity_N[0] + -rb2n[1][1] * gKalmanFilter.Velocity_N[1] + -rb2n[2][1] * gKalmanFilter.Velocity_N[2]; + // vertical (downwards) vel erro + gKalmanFilter.nu[STATE_VZ] = + -rb2n[0][2] * gKalmanFilter.Velocity_N[0] + -rb2n[1][2] * gKalmanFilter.Velocity_N[1] + -rb2n[2][2] * gKalmanFilter.Velocity_N[2]; + gKalmanFilter.nu[STATE_VY] = _LimitValue(gKalmanFilter.nu[STATE_VY], gAlgorithm.Limit.Innov.velocityError); + gKalmanFilter.nu[STATE_VZ] = _LimitValue(gKalmanFilter.nu[STATE_VZ], gAlgorithm.Limit.Innov.velocityError); + + // p*H'. PxHTranspose is 16x3, only the last two columns are used when only lateral and vertical measurements + memset(PxHTranspose, 0, sizeof(PxHTranspose)); + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + for (colNum = 0; colNum < 3; colNum++) + { + PxHTranspose[rowNum][colNum] = + gKalmanFilter.P[rowNum][3] * rb2n[0][colNum] + + gKalmanFilter.P[rowNum][4] * rb2n[1][colNum] + + gKalmanFilter.P[rowNum][5] * rb2n[2][colNum]; + } + } + + // s = H*P*H' + R + for (rowNum = 0; rowNum < 3; rowNum++) + { + for (colNum = rowNum; colNum < 3; colNum++) + { + S_3x3[rowNum][colNum] = rb2n[0][rowNum] * PxHTranspose[3][colNum] + + rb2n[1][rowNum] * PxHTranspose[4][colNum] + + rb2n[2][rowNum] * PxHTranspose[5][colNum]; + S_3x3[colNum][rowNum] = S_3x3[rowNum][colNum]; + } + S_3x3[rowNum][rowNum] += r[rowNum]; + } + + // Calculate inv(H*P*H'+R) according to if front velocity measurement is available + if (frontVelMeaValid) + { + matrixInverse_3x3(&S_3x3[0][0], &SInverse_3x3[0][0]); + } + else + { + S_3x3[0][0] = 1.0; + S_3x3[0][1] = 0.0; + S_3x3[0][2] = 0.0; + S_3x3[1][0] = 0.0; + S_3x3[2][0] = 0.0; + matrixInverse_3x3(&S_3x3[0][0], &SInverse_3x3[0][0]); + SInverse_3x3[0][0] = 0.0; + } + + // K = P*H' * inv(H*P*H' + R). gKalmanFilter.K is 16x3, only the last two columns are used. + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + for (colNum = 0; colNum < 3; colNum++) + { + gKalmanFilter.K[rowNum][colNum] = + PxHTranspose[rowNum][0] * SInverse_3x3[0][colNum] + + PxHTranspose[rowNum][1] * SInverse_3x3[1][colNum] + + PxHTranspose[rowNum][2] * SInverse_3x3[2][colNum]; + } + } + // force uncorrected terms in K to be 0 + for (rowNum = STATE_RX; rowNum <= STATE_ABZ; rowNum++) + { + if (!updatedStatesPseudo[rowNum]) + { + for (colNum = 0; colNum < 3; colNum++) + { + gKalmanFilter.K[rowNum][colNum] = 0.0; + } + } + } + + // dx = k * nu + for (rowNum = 0; rowNum < NUMBER_OF_EKF_STATES; rowNum++) + { + gKalmanFilter.stateUpdate[rowNum] = + gKalmanFilter.K[rowNum][0] * gKalmanFilter.nu[STATE_VX] + + gKalmanFilter.K[rowNum][1] * gKalmanFilter.nu[STATE_VY] + + gKalmanFilter.K[rowNum][2] * gKalmanFilter.nu[STATE_VZ]; + } + + // update state + //gKalmanFilter.Position_N[X_AXIS] = gKalmanFilter.Position_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_RX]; + //gKalmanFilter.Position_N[Y_AXIS] = gKalmanFilter.Position_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_RY]; + //gKalmanFilter.Position_N[Z_AXIS] = gKalmanFilter.Position_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_RZ]; + + gKalmanFilter.Velocity_N[X_AXIS] = gKalmanFilter.Velocity_N[X_AXIS] + gKalmanFilter.stateUpdate[STATE_VX]; + gKalmanFilter.Velocity_N[Y_AXIS] = gKalmanFilter.Velocity_N[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_VY]; + gKalmanFilter.Velocity_N[Z_AXIS] = gKalmanFilter.Velocity_N[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_VZ]; + + gKalmanFilter.quaternion[Q0] = gKalmanFilter.quaternion[Q0] + gKalmanFilter.stateUpdate[STATE_Q0]; + gKalmanFilter.quaternion[Q1] = gKalmanFilter.quaternion[Q1] + gKalmanFilter.stateUpdate[STATE_Q1]; + gKalmanFilter.quaternion[Q2] = gKalmanFilter.quaternion[Q2] + gKalmanFilter.stateUpdate[STATE_Q2]; + gKalmanFilter.quaternion[Q3] = gKalmanFilter.quaternion[Q3] + gKalmanFilter.stateUpdate[STATE_Q3]; + + // Normalize quaternion and force q0 to be positive + QuatNormalize(gKalmanFilter.quaternion); + + gKalmanFilter.rateBias_B[X_AXIS] = gKalmanFilter.rateBias_B[X_AXIS] + gKalmanFilter.stateUpdate[STATE_WBX]; + gKalmanFilter.rateBias_B[Y_AXIS] = gKalmanFilter.rateBias_B[Y_AXIS] + gKalmanFilter.stateUpdate[STATE_WBY]; + gKalmanFilter.rateBias_B[Z_AXIS] = gKalmanFilter.rateBias_B[Z_AXIS] + gKalmanFilter.stateUpdate[STATE_WBZ]; + + 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]; + + // Update covariance: P = P + DP = P - K*H*P + // Use transpose(PxHTranspose) to hold H*P (3x16). + // Only the last two columns are used when only lateral and vertical measurements + for (colNum = 0; colNum < NUMBER_OF_EKF_STATES; colNum++) + { + for (rowNum = 0; rowNum < 3; rowNum++) + { + PxHTranspose[colNum][rowNum] = rb2n[0][rowNum] * gKalmanFilter.P[3][colNum] + + rb2n[1][rowNum] * gKalmanFilter.P[4][colNum] + + rb2n[2][rowNum] * gKalmanFilter.P[5][colNum]; + } + } + + // deltaP = KxH * gKF.P; + 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 + */ + for (rowNum = 0; rowNum <= STATE_ABZ; rowNum++) + { + if (!updatedStatesPseudo[rowNum]) + { + continue; + } + for (colNum = rowNum; colNum <= STATE_ABZ; colNum++) + { + if (!updatedStatesPseudo[colNum]) + { + continue; + } + 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]; + gKalmanFilter.deltaP_tmp[colNum][rowNum] = gKalmanFilter.deltaP_tmp[rowNum][colNum]; + } + } + + /* P is symmetric so only need to multiply one half and reflect the values + * across the diagonal + */ + for (rowNum = 0; rowNum < ROWS_IN_P; rowNum++) + { + for (colNum = rowNum; colNum < COLS_IN_P; colNum++) + { + gKalmanFilter.P[rowNum][colNum] = gKalmanFilter.P[rowNum][colNum] - + gKalmanFilter.deltaP_tmp[rowNum][colNum]; + gKalmanFilter.P[colNum][rowNum] = gKalmanFilter.P[rowNum][colNum]; + } + } +} + +static void GenPseudoMeasCov(real *r) +{ + real absYawRate = (real)fabs(gEKFInput.angRate_B[2]); + r[1] = absYawRate; + r[2] = absYawRate; + + real minVar = 1e-4; + real maxVar = 1e-2; + if (r[1] < minVar) + { + r[1] = minVar; + } + if (r[1] > maxVar) + { + r[1] = maxVar; + } + + if (r[2] < minVar) + { + r[2] = minVar; + } + if (r[2] > maxVar) + { + r[2] = maxVar; + } + //printf("rr: %f,%f\n", r[1], r[2]); +} + + /* Conversion from turn-rate threshold (values loaded into gConfiguration) to * decimal value in [rad/sec]: * @@ -1185,7 +1541,6 @@ static void _TurnSwitch(void) if ((gAlgorithm.state > HIGH_GAIN_AHRS) && (absYawRate > minSwitch)) { gAlgoStatus.bit.turnSwitch = TRUE; - // std::cout << "TurnSwitch (INS): Activated\n"; /* When the rate is below the maximum rate defined by turnSwitchThreshold, * then generate a scale-factor that is between ( 1.0 - G ) and 0.0 (based on absYawRate). @@ -1252,7 +1607,7 @@ static BOOL _CheckForUpdateTrigger(uint8_t updateRate) { // uint8_t oneHundredHzCntr; - uint8_t gpsUpdate = 0; + uint8_t updateFlag = 0; // switch( updateRate ) @@ -1261,7 +1616,7 @@ static BOOL _CheckForUpdateTrigger(uint8_t updateRate) case 10: if( timer.subFrameCntr == 0 ) { - gpsUpdate = 1; + updateFlag = 1; } break; @@ -1269,7 +1624,7 @@ static BOOL _CheckForUpdateTrigger(uint8_t updateRate) case 20: if( timer.subFrameCntr == 0 || timer.subFrameCntr == 5 ) { - gpsUpdate = 1; + updateFlag = 1; } break; @@ -1304,7 +1659,7 @@ static BOOL _CheckForUpdateTrigger(uint8_t updateRate) oneHundredHzCntr == 92 || oneHundredHzCntr == 96 ) { - gpsUpdate = 1; + updateFlag = 1; } break; @@ -1316,142 +1671,323 @@ static BOOL _CheckForUpdateTrigger(uint8_t updateRate) timer.subFrameCntr == 6 || timer.subFrameCntr == 8 ) { - gpsUpdate = 1; + updateFlag = 1; } break; // fifty-hertz update case 100: - gpsUpdate = 1; + updateFlag = 1; break; } - return gpsUpdate; + return updateFlag; } - -static int InitializeGnssHeading() +static int InitializeHeadingFromGnss() { /* enable declination correction, but the corrected magnetic yaw will not * be used if GPS is available. */ gAlgorithm.applyDeclFlag = TRUE; - if (useGpsHeading)// && gEKFInputData.rawGroundSpeed>1.0) + /* backward drive detection for heading initialization using GNSS heading. + * Detection happends every second. Velocity increment is relatively reliable + * if it is accumulated for 1sec. + */ + static real lastVelBxGnss = 0; + static uint8_t forwardDriveConfidence = 0; + static uint32_t lastTOW = 0; + uint32_t timeSinceLastDetection = gAlgorithm.itow - lastTOW; + if (timeSinceLastDetection < 0) { - // Heading is initialized with GNSS - gAlgorithm.gnssHeadingFirstTime = FALSE; + timeSinceLastDetection = timeSinceLastDetection + MAX_ITOW; + } + if (timeSinceLastDetection > 950) // 950ms is set as the threshold for 1sec + { + lastTOW = gAlgorithm.itow; + /* assume velocity is always along the body x axis. otherwise, GNSS heading + * cannot be used to initialize fusion heading + */ + real velBx = sqrtf(SQUARE(gEKFInput.vNed[0]) + SQUARE(gEKFInput.vNed[1]) + SQUARE(gEKFInput.vNed[2])); + velBx = fabs(velBx); + real dv = velBx - lastVelBxGnss; + if ((dv * gKalmanFilter.linearAccel_B[X_AXIS]) > 0.0 && fabs(gKalmanFilter.linearAccel_B[X_AXIS]) > 0.2) + { + if (forwardDriveConfidence < 255) + { + forwardDriveConfidence++; + } + } + else + { + forwardDriveConfidence = 0; + } + // record this velocity along body x axis for next run + lastVelBxGnss = velBx; + // reset accumulated x body axis velocity change. + gKalmanFilter.linearAccel_B[X_AXIS] = 0.0; + } + + // detect if GNSS heading is reliable + static uint8_t gnssHeadingGoodCntr = 0; + static float lastGnssHeading = 0.0; + static float lastFusionHeading = 0.0; + BOOL gnssHeadingGood = 0; + float angleDiff = 0.0; + if (useGpsHeading) + { + float calculatedGnssHeading = (float)(atan2(gEKFInput.vNed[1], gEKFInput.vNed[0]) * R2D); + float diffHeading = AngleErrDeg(gEKFInput.trueCourse - calculatedGnssHeading); + // input GNSS heading matches heading calculated from vNED + if (fabs(diffHeading) < 5.0) + { + // GNSS heading change matches fusion yaw angle + float gnssHeadingChange = gEKFInput.trueCourse - lastGnssHeading; + float fusionHeadingChange = gKalmanFilter.eulerAngles[2] * (float)R2D - lastFusionHeading; + angleDiff = (float)fabs( AngleErrDeg(gnssHeadingChange - fusionHeadingChange) ); + if (angleDiff < 5.0) + { + gnssHeadingGood = TRUE; + } + } + lastGnssHeading = gEKFInput.trueCourse; + lastFusionHeading = gKalmanFilter.eulerAngles[2] * (float)R2D; + } + if (gnssHeadingGood) + { + gnssHeadingGoodCntr++; + } + else + { + gnssHeadingGoodCntr = 0; + } + + // Heading initialization when drive forward and GNSS heading is reliable + BOOL thisHeadingUsedForIni = FALSE; + if (gAlgorithm.headingIni < HEADING_GNSS_LOW) // heading is immediately but maybe unreliably initialized + { + if (gnssHeadingGoodCntr >= 1 && forwardDriveConfidence >= 1) // Only one sample is checked, so heading may be unreliable + { + gnssHeadingGoodCntr = 0; + // Heading is initialized with GNSS + gAlgorithm.headingIni = HEADING_GNSS_LOW; #ifdef INS_OFFLINE - printf("gps heading: %f\n", gEKFInputData.trueCourse); + printf("quick gps heading: %f\n", gEKFInput.trueCourse); #else #ifdef DISPLAY_DIAGNOSTIC_MSG - DebugPrintString("gps heading"); - DebugPrintFloat(": ", gEKFInputData.trueCourse, 9); - DebugPrintEndline(); + DebugPrintString("quick gps heading"); + DebugPrintFloat(": ", gEKFInput.trueCourse, 9); + DebugPrintEndline(); #endif #endif - // initialize yaw angle with GPS heading - gKalmanFilter.eulerAngles[YAW] = (real)(gEKFInputData.trueCourse * D2R); - if (gKalmanFilter.eulerAngles[YAW] > PI) + thisHeadingUsedForIni = TRUE; + } + } + else + { + /* Three points are checked, and the latest ground speed is above a certian threshold. + * The latest GNSS heading should be reliable. + */ + if (gnssHeadingGoodCntr >= 3 && + forwardDriveConfidence >= 5 && + gEKFInput.rawGroundSpeed > RELIABLE_GPS_VELOCITY_HEADING) { - gKalmanFilter.eulerAngles[YAW] -= (real)TWO_PI; + gnssHeadingGoodCntr = 0; + forwardDriveConfidence = 0; + gAlgorithm.headingIni = HEADING_GNSS_HIGH; +#ifdef INS_OFFLINE + printf("reliable gps heading: %f\n", gEKFInput.trueCourse); +#else +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("reliable gps heading"); + DebugPrintFloat(": ", gEKFInput.trueCourse, 9); + DebugPrintEndline(); +#endif +#endif + thisHeadingUsedForIni = TRUE; } - EulerAnglesToQuaternion(gKalmanFilter.eulerAngles, gKalmanFilter.quaternion); - - // reinitialize NED velocity - gKalmanFilter.Velocity_N[X_AXIS] = (real)gEKFInputData.vNed[X_AXIS]; - gKalmanFilter.Velocity_N[Y_AXIS] = (real)gEKFInputData.vNed[Y_AXIS]; - gKalmanFilter.Velocity_N[Z_AXIS] = (real)gEKFInputData.vNed[Z_AXIS]; - + } + + return thisHeadingUsedForIni; +} + +static void InitializeEkfHeading() +{ + /* Compare the reliable heading with Kalamn filter heading. If the difference exceeds + * a certain threshold, this means the immediate heading initialization is unreliable, + * and the Kalman filter needs reinitialized with the reliable one. + */ + float angleDiff = (float)fabs(AngleErrDeg(gEKFInput.trueCourse - + gKalmanFilter.eulerAngles[2] * (float)R2D)); + if (angleDiff <= 2.0) + { + return; + } + +#ifdef INS_OFFLINE + printf("Reinitialize KF: %f\n", angleDiff); +#else +#ifdef DISPLAY_DIAGNOSTIC_MSG + DebugPrintString("Reinitialize KF: "); + DebugPrintFloat("", angleDiff, 9); + DebugPrintEndline(); +#endif +#endif + + // initialize yaw angle with GPS heading + gKalmanFilter.eulerAngles[YAW] = (gEKFInput.trueCourse * D2R); + if (gKalmanFilter.eulerAngles[YAW] > PI) + { + gKalmanFilter.eulerAngles[YAW] -= (real)TWO_PI; + } + EulerAnglesToQuaternion(gKalmanFilter.eulerAngles, gKalmanFilter.quaternion); + + // reinitialize NED position + gKalmanFilter.Position_N[0] = (real)gKalmanFilter.rGPS_N[0]; + gKalmanFilter.Position_N[1] = (real)gKalmanFilter.rGPS_N[1]; + gKalmanFilter.Position_N[2] = (real)gKalmanFilter.rGPS_N[2]; + + // reinitialize NED velocity + gKalmanFilter.Velocity_N[X_AXIS] = (real)gEKFInput.vNed[X_AXIS]; + gKalmanFilter.Velocity_N[Y_AXIS] = (real)gEKFInput.vNed[Y_AXIS]; + gKalmanFilter.Velocity_N[Z_AXIS] = (real)gEKFInput.vNed[Z_AXIS]; + #if 1 // mod, DXG - // reset quaternion and velocity terms in the P matrix - int i, j; - // vel row - for (i = STATE_VX; i < STATE_VZ; i++) + // reset quaternion and velocity terms in the P matrix + int i, j; + // pos row + gKalmanFilter.P[STATE_RX][STATE_RX] = gKalmanFilter.R[STATE_RX]; + gKalmanFilter.P[STATE_RY][STATE_RY] = gKalmanFilter.R[STATE_RY]; + gKalmanFilter.P[STATE_RZ][STATE_RZ] = gKalmanFilter.R[STATE_RZ]; + for (i = STATE_RX; i < STATE_RZ; i++) + { + for (j = 0; j < NUMBER_OF_EKF_STATES; j++) { - for (j = 0; j < NUMBER_OF_EKF_STATES; j++) + if (i != j) { - if (i != j) - { - gKalmanFilter.P[i][j] = 0; - gKalmanFilter.P[j][i] = 0; - } + gKalmanFilter.P[i][j] = 0; + gKalmanFilter.P[j][i] = 0; } } - // q0 row - for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + } + // vel row + gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.R[STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.R[STATE_VY]; + gKalmanFilter.P[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VZ]; + for (i = STATE_VX; i < STATE_VZ; i++) + { + for (j = 0; j < NUMBER_OF_EKF_STATES; j++) { - if (i != STATE_Q0) + if (i != j) { - gKalmanFilter.P[STATE_Q0][i] = 0; - gKalmanFilter.P[i][STATE_Q0] = 0; + gKalmanFilter.P[i][j] = 0; + gKalmanFilter.P[j][i] = 0; } } - // q3 row - for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + } + // q0 row + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + if (i != STATE_Q0) { - if (i != STATE_Q3) - { - gKalmanFilter.P[STATE_Q3][i] = 0; - gKalmanFilter.P[i][STATE_Q3] = 0; - } + gKalmanFilter.P[STATE_Q0][i] = 0; + gKalmanFilter.P[i][STATE_Q0] = 0; } - - // the initial covariance of the quaternion is estimated from ground speed. - float temp = (float)atan(0.05 / gEKFInputData.rawGroundSpeed); - temp *= temp; // heading var - if (gAlgoStatus.bit.turnSwitch) + } + // q3 row + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) + { + if (i != STATE_Q3) { - temp *= 10.0; // when rotating, heading var increases + gKalmanFilter.P[STATE_Q3][i] = 0; + gKalmanFilter.P[i][STATE_Q3] = 0; } - temp /= 4.0; // sin(heading/2) or cos(heading/2) - float sinYawSqr = (real)sin(gKalmanFilter.eulerAngles[YAW] / 2.0f); - sinYawSqr *= sinYawSqr; - // Assume roll and pitch are close to 0deg - gKalmanFilter.P[STATE_Q0][STATE_Q0] = temp * sinYawSqr; - gKalmanFilter.P[STATE_Q3][STATE_Q3] = temp * (1.0f - sinYawSqr); + } + + // the initial covariance of the quaternion is estimated from ground speed. + float temp = (float)atan(0.05 / gEKFInput.rawGroundSpeed); + temp *= temp; // heading var + if (gAlgoStatus.bit.turnSwitch) + { + temp *= 10.0; // when rotating, heading var increases + } + temp /= 4.0; // sin(heading/2) or cos(heading/2) + float sinYawSqr = (real)sin(gKalmanFilter.eulerAngles[YAW] / 2.0f); + sinYawSqr *= sinYawSqr; + // Assume roll and pitch are close to 0deg + gKalmanFilter.P[STATE_Q0][STATE_Q0] = temp * sinYawSqr; + gKalmanFilter.P[STATE_Q3][STATE_Q3] = temp * (1.0f - sinYawSqr); - gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.R[STATE_VX]; - gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.R[STATE_VY]; - gKalmanFilter.P[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VZ]; + gKalmanFilter.P[STATE_VX][STATE_VX] = gKalmanFilter.R[STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_VY] = gKalmanFilter.R[STATE_VY]; + gKalmanFilter.P[STATE_VZ][STATE_VZ] = gKalmanFilter.R[STATE_VZ]; #if 0 - // reset velocity and quaternion terms in the P matrix - real v2 = gEKFInputData.rawGroundSpeed * gEKFInputData.rawGroundSpeed; - real v3by4 = 4.0 * v2 * gEKFInputData.rawGroundSpeed; - real vn2 = gEKFInputData.vNed[0] * gEKFInputData.vNed[0]; - real q0q0 = gKalmanFilter.quaternion[0] * gKalmanFilter.quaternion[0]; - real q3q3 = gKalmanFilter.quaternion[3] * gKalmanFilter.quaternion[3]; - if (q0q0 < 1.0e-3) - { - q0q0 = 1.0e-3; - } - if (q3q3 < 1.0e-3) - { - q3q3 = 1.0e-3; - } - real multiplerQVn = (v2 - vn2) / v3by4; - multiplerQVn *= multiplerQVn; - real multiplerQVe = (gEKFInputData.vNed[0] * gEKFInputData.vNed[1]) / v3by4; - multiplerQVe *= multiplerQVe; - gKalmanFilter.P[STATE_VX][STATE_Q0] = multiplerQVn / q0q0 * gKalmanFilter.R[STATE_VX][STATE_VX]; - gKalmanFilter.P[STATE_VX][STATE_Q3] = multiplerQVn / q3q3 * gKalmanFilter.R[STATE_VX][STATE_VX]; - gKalmanFilter.P[STATE_VY][STATE_Q0] = multiplerQVe / q0q0 * gKalmanFilter.R[STATE_VY][STATE_VY]; - gKalmanFilter.P[STATE_VY][STATE_Q3] = multiplerQVe / q3q3 * gKalmanFilter.R[STATE_VY][STATE_VY]; - - - gKalmanFilter.P[STATE_Q0][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_Q0]; - gKalmanFilter.P[STATE_Q3][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_Q3]; - gKalmanFilter.P[STATE_Q0][STATE_VY] = gKalmanFilter.P[STATE_VY][STATE_Q0]; - gKalmanFilter.P[STATE_Q3][STATE_VY] = gKalmanFilter.P[STATE_VY][STATE_Q3]; + // reset velocity and quaternion terms in the P matrix + real v2 = gEKFInput.rawGroundSpeed * gEKFInput.rawGroundSpeed; + real v3by4 = 4.0 * v2 * gEKFInput.rawGroundSpeed; + real vn2 = gEKFInput.vNed[0] * gEKFInput.vNed[0]; + real q0q0 = gKalmanFilter.quaternion[0] * gKalmanFilter.quaternion[0]; + real q3q3 = gKalmanFilter.quaternion[3] * gKalmanFilter.quaternion[3]; + if (q0q0 < 1.0e-3) + { + q0q0 = 1.0e-3; + } + if (q3q3 < 1.0e-3) + { + q3q3 = 1.0e-3; + } + real multiplerQVn = (v2 - vn2) / v3by4; + multiplerQVn *= multiplerQVn; + real multiplerQVe = (gEKFInput.vNed[0] * gEKFInput.vNed[1]) / v3by4; + multiplerQVe *= multiplerQVe; + gKalmanFilter.P[STATE_VX][STATE_Q0] = multiplerQVn / q0q0 * gKalmanFilter.R[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VX][STATE_Q3] = multiplerQVn / q3q3 * gKalmanFilter.R[STATE_VX][STATE_VX]; + gKalmanFilter.P[STATE_VY][STATE_Q0] = multiplerQVe / q0q0 * gKalmanFilter.R[STATE_VY][STATE_VY]; + gKalmanFilter.P[STATE_VY][STATE_Q3] = multiplerQVe / q3q3 * gKalmanFilter.R[STATE_VY][STATE_VY]; + + + gKalmanFilter.P[STATE_Q0][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_Q0]; + gKalmanFilter.P[STATE_Q3][STATE_VX] = gKalmanFilter.P[STATE_VX][STATE_Q3]; + gKalmanFilter.P[STATE_Q0][STATE_VY] = gKalmanFilter.P[STATE_VY][STATE_Q0]; + gKalmanFilter.P[STATE_Q3][STATE_VY] = gKalmanFilter.P[STATE_VY][STATE_Q3]; #endif #endif - return 1; +} + +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]; + } } - else + + // 3) add dQ + for (i = 0; i < NUMBER_OF_EKF_STATES; i++) { - return 0; + 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]; + } } -} \ No newline at end of file +} diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/WorldMagneticModel.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/WorldMagneticModel.c index e9b20dc..c3f9403 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/WorldMagneticModel.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/WorldMagneticModel.c @@ -157,7 +157,7 @@ void TaskWorldMagneticModel(void const *argument) while(1) { OS_Delay( 5000 ); #ifdef GPS - if( gGpsDataPtr->gpsValid ) + if( gGpsDataPtr->gpsFixType ) { // caution - safety checks on the dates are fine, but we don't want // to rely on these hard-coded dates in operation @@ -204,7 +204,7 @@ void TaskWorldMagneticModel(void const *argument) // /* // % WorldMagneticModel.m // -// if( gGpsDataPtr.GPSValid ), +// if( gGpsDataPtr.gpsFixType ), // % WMM here // declinationAngle_rad = ( 13 + 36 / 60 + 43/3600 ) * CONV.DEG_TO_RAD; // @@ -216,7 +216,7 @@ void TaskWorldMagneticModel(void const *argument) // */ // // // VERY SIMPLE WMM (declination near San Jose only) -//// if (gGpsDataPtr->gpsValid) { +//// if (gGpsDataPtr->gpsFixType) { //// gWorldMagModel.decl_rad = (real)( (13.0 + 40.0 / 60.0 + 29.0 / 3600.0) * D2R ); //// } else { //// gWorldMagModel.decl_rad = (real)0.0; diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/algorithm.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/algorithm.c index 13fb6c5..c5a2087 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/algorithm.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Algorithm/src/algorithm.c @@ -15,6 +15,9 @@ ******************************************************************************/ #include +#include + +#include "SensorNoiseParameters.h" #include "platformAPI.h" #include "algorithm.h" #include "AlgorithmLimits.h" @@ -24,11 +27,17 @@ AlgorithmStruct gAlgorithm; AlgoStatus gAlgoStatus; -void InitializeAlgorithmStruct(uint16_t callingFreq) +void InitializeAlgorithmStruct(uint8_t callingFreq) { - gAlgorithm.Behavior.bit.freeIntegrate = FALSE; + 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){ + // IMU case + callingFreq = FREQ_200_HZ; + } gAlgorithm.callingFreq = callingFreq; // Set dt based on the calling frequency of the EKF @@ -50,7 +59,7 @@ void InitializeAlgorithmStruct(uint16_t callingFreq) // Set up other timing variables gAlgorithm.dtOverTwo = (real)(0.5) * gAlgorithm.dt; gAlgorithm.dtSquared = gAlgorithm.dt * gAlgorithm.dt; - gAlgorithm.sqrtDt = sqrt(gAlgorithm.dt); + gAlgorithm.sqrtDt = sqrtf(gAlgorithm.dt); // Set the algorithm duration periods gAlgorithm.Duration.Stabilize_System = (uint32_t)(gAlgorithm.callingFreq * STABILIZE_SYSTEM_DURATION); @@ -72,6 +81,7 @@ void InitializeAlgorithmStruct(uint16_t callingFreq) gAlgorithm.applyDeclFlag = FALSE; gAlgorithm.insFirstTime = TRUE; + gAlgorithm.headingIni = HEADING_UNINITIALIZED; //gAlgorithm.magAlignUnderway = FALSE; // Set and reset in mag-align code @@ -79,7 +89,8 @@ void InitializeAlgorithmStruct(uint16_t callingFreq) gAlgorithm.itow = 0; // Limit is compared to ITOW. Time must be in [msec]. - gAlgorithm.Limit.Max_GPS_Drop_Time = LIMIT_MAX_GPS_DROP_TIME * 1000; + gAlgorithm.Limit.maxGpsDropTime = LIMIT_MAX_GPS_DROP_TIME * 1000; + gAlgorithm.Limit.maxReliableDRTime = LIMIT_RELIABLE_DR_TIME * 1000; // Limit is compared to count (incremented upon loop through // taskDataAcquisition). Time must be in [count] based on ODR. @@ -105,15 +116,42 @@ void InitializeAlgorithmStruct(uint16_t callingFreq) gAlgorithm.useRawAccToDetectLinAccel = TRUE; // Set the turn-switch threshold to a default value in [deg/sec] - gAlgorithm.turnSwitchThreshold = 2.0; + gAlgorithm.turnSwitchThreshold = 6.0; // default lever arm and point of interest - gAlgorithm.leverArmB[X_AXIS] = 0.0; - gAlgorithm.leverArmB[Y_AXIS] = 0.0; - gAlgorithm.leverArmB[Z_AXIS] = 0.0; - gAlgorithm.pointOfInterestB[X_AXIS] = 0.0; - gAlgorithm.pointOfInterestB[Y_AXIS] = 0.0; - gAlgorithm.pointOfInterestB[Z_AXIS] = 0.0; + gAlgorithm.leverArmB[X_AXIS] = 0.0; + gAlgorithm.leverArmB[Y_AXIS] = 0.0; + gAlgorithm.leverArmB[Z_AXIS] = 0.0; + gAlgorithm.pointOfInterestB[X_AXIS] = 0.0; + gAlgorithm.pointOfInterestB[Y_AXIS] = 0.0; + gAlgorithm.pointOfInterestB[Z_AXIS] = 0.0; + + // For most vehicles, the velocity is always along the body x axis + gAlgorithm.velocityAlwaysAlongBodyX = TRUE; + + // get IMU specifications + 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; + gAlgorithm.imuSpec.maxBiasW = (real)MAX_BW; + gAlgorithm.imuSpec.vrw = (real)VRW_300ZA; + 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; + + // 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 = 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)); } void GetAlgoStatus(AlgoStatus *algoStatus) @@ -170,4 +208,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/OpenIMU300RI/VG_AHRS/lib/Core/GPS/gpsAPI.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/gpsAPI.h index aef45ab..3a24ea4 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/gpsAPI.h +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/gpsAPI.h @@ -43,33 +43,45 @@ void TaskGps(void const *argument); } #endif +// Fix type +#define INVALID 0 // GNSS not fixed yet +#define SPP 1 // Single Point Positioning +#define DGPS 2 // Pseudorange Differential +#define PPP 3 // Precise Point Positioning +#define RTK_FIX 4 // RTK Fixed +#define RTK_FLOAT 5 // RTK Float +#define DEAD_REC 6 // Dead Reckoning (will be considered as INVALID) +#define MANUAL 7 // Manual Input Mode (will be considered as INVALID) +#define SIMULATION 8 // Simulation Mode (will be considered as INVALID) + typedef struct { - int gpsValid; // 1 if data is valid - uint8_t gpsUpdate; // 1 if contains new data - - double latitude; // latitude , degrees - double longitude; // longitude, degrees - double vNed[3]; // velocities, m/s NED (North East Down) x, y, z - double trueCourse; // [deg] - double rawGroundSpeed; // NMEA kph, SiRf m/s - change to m/s - double altitude; // above ellipsoid [m] - double GPSSecondFraction; - float altEllipsoid; // [km] altitude above ellipsoid for WMM - - uint32_t itow; // gps Time Of Week, miliseconds + uint8_t gpsFixType; // 1 if data is valid + uint8_t gpsUpdate; // 1 if contains new data + uint8_t numSatellites; // num of satellites in the solution + uint32_t itow; // gps Time Of Week, miliseconds + + double latitude; // latitude , degrees + double longitude; // longitude, degrees + double vNed[3]; // velocities, m/s NED (North East Down) x, y, z + double trueCourse; // [deg] + double rawGroundSpeed; // [m/s] + double altitude; // above WGS84 ellipsoid [m] + double GPSSecondFraction; + - uint8_t GPSmonth; // mm - uint8_t GPSday; // dd - uint8_t GPSyear; // yy last two digits of year - char GPSHour; // hh - char GPSMinute; // mm - char GPSSecond; // ss - - float GPSHorizAcc, GPSVertAcc; - float HDOP; + uint8_t GPSmonth; // mm + uint8_t GPSday; // dd + uint8_t GPSyear; // yy last two digits of year + char GPSHour; // hh + char GPSMinute; // mm + char GPSSecond; // ss + + float GPSHorizAcc, GPSVertAcc; + float HDOP; + float geoidAboveEllipsoid; // [m] Height of geoid (mean sea level) above WGS84 ellipsoid } gpsDataStruct_t; -extern gpsDataStruct_t gGPS; +extern gpsDataStruct_t gGPS, gCanGps; /** **************************************************************************** * @name GetGPSData diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/include/GpsData.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/include/GpsData.h index 10a514b..cb21530 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/include/GpsData.h +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/include/GpsData.h @@ -43,35 +43,36 @@ This Data structure is not only used for all GPS interface and process but also used to be accessed by other modules rather than GPS files. */ typedef struct { - BOOL gpsValid; - long double lat; // concatinated from int components [deg.dec] - long double lon; - double vNed[3]; // NED North East Down [m/s] x, y, z - uint32_t itow; ///< gps milisecond Interval Time Of Week - - int updateFlagForEachCall; /// changed to 16 bits - int totalGGA; - int totalVTG; - - double trueCourse; // [deg] - double rawGroundSpeed; // NMEA kph, SiRf m/s - - double alt; // above mean sea level [m] - double filteredAlt; // FIXME should this be local? - float altEllipsoid; // [km] altitude above ellipsoid for WMM - uint8_t GPSmonth; // mm - uint8_t GPSday; // dd - uint8_t GPSyear; // yy last two digits of year - char GPSHour; // hh - char GPSMinute; // mm - char GPSSecond; // ss - double GPSSecondFraction; // FIXME used? + uint8_t gpsFixType; // GPS solution type: invalid, spp, dgps, spp, rtk_float, rtk_fix + uint8_t numSatellites; + + uint8_t GPSmonth; // mm + uint8_t GPSday; // dd + uint8_t GPSyear; // yy last two digits of year + char GPSHour; // hh + char GPSMinute; // mm + char GPSSecond; // ss + double GPSSecondFraction; // FIXME used? + + uint32_t itow; ///< gps milisecond Interval Time Of Week + int updateFlagForEachCall; /// changed to 16 bits + + int totalGGA; + int totalVTG; + + double lat; // [deg], latitude + double lon; // [lon], longitude + double alt; // [m] above WGS84 ellipsoid + double vNed[3]; // NED North East Down [m/s] x, y, z + double trueCourse; // [deg] + double rawGroundSpeed; // [m/s] + + float geoidAboveEllipsoid; // [m] Height of geoid (mean sea level) above WGS84 ellipsoid /// compatible with Ublox driver FIXME should these be seperate data structure? unsigned char ubloxClassID; unsigned char ubloxMsgID; signed long LonLatH[3]; // SiRF Lat Lon[deg] * 10^7 Alt ellipse [m]*100 <-- UNUSED - char GPSFix; float HDOP; // Horizontal Dilution Of Precision x.x double GPSVelAcc; unsigned short GPSStatusWord; /// will replace GPSfix @@ -114,9 +115,6 @@ typedef struct { float GPSHorizAcc; float GPSVertAcc; - - int numSatelites; - } GpsData_t; extern GpsData_t *gGpsDataPtr; // definition in driverGPSAllentrance.c diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/include/driverGPS.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/include/driverGPS.h index f114857..c7681a8 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/include/driverGPS.h +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/include/driverGPS.h @@ -159,23 +159,33 @@ typedef struct uint8_t hour; uint8_t min; uint8_t sec; - uint8_t valid; + uint8_t valid; // Validity flags uint32_t tAcc; // time accuracy estimate (UTC), ns int32_t nano; // fraction of second, range -1e9..1e9 (UTC), ns - uint8_t fixType; // GNSS fix type - // 0: no fix - // 1: dead reckoning only - // 2: 2D-fix - // 3: 3D-fix - // 4: GNSS + dead reckoning combined - // 5: time only fix - uint8_t flags; + uint8_t fixType; /* GNSS fix type + * 0: no fix + * 1: dead reckoning only + * 2: 2D-fix + * 3: 3D-fix + * 4: GNSS + dead reckoning combined + * 5: time only fix + */ + uint8_t flags; /* Fix status flags + * bit0: gnssFixOK, 1 = valid fix (i.e within DOP & accuracy masks) + * bit1: diffSoln, 1 = differential corrections were applied + * bit2-4: psmState, Power Save Mode state + * bit5: headVehValid, 1 = heading of vehicle is valid + * bit6-7: carrSoln, Carrier phase range solution status: + * 0: no carrier phase range solution + * 1: float solution + * 2: fixed solution + */ uint8_t flags2; uint8_t numSV; // number of satellites in Nav solution; int32_t lon; // deg, scaling is 1e-7 int32_t lat; // deg, scaling is 1e-7 int32_t height; // mm, height above ellipsoid; - int32_t hMSL; + int32_t hMSL; // mm, hegiht above mean seal level uint32_t hAcc; // mm, horizontal accuracy estimate uint32_t vAcc; // mm, vertical accuracy estimate int32_t velN; // mm/s, north velocity diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/driverGPS.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/driverGPS.c index 2cf7a92..89416a6 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/driverGPS.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/driverGPS.c @@ -273,7 +273,6 @@ int writeGps(char *data, uint16_t len) void GetGPSData(gpsDataStruct_t *data) { - data->gpsValid = gGpsDataPtr->gpsValid; data->gpsUpdate = ( gGpsDataPtr->updateFlagForEachCall >> GOT_VTG_MSG ) & 0x00000001 && ( gGpsDataPtr->updateFlagForEachCall >> GOT_GGA_MSG ) & 0x00000001; // gGpsDataPtr->updateFlagForEachCall &= 0xFFFFFFFD; @@ -281,33 +280,36 @@ void GetGPSData(gpsDataStruct_t *data) { // Reset GPS update flag only when pos and vel are both available gGpsDataPtr->updateFlagForEachCall &= 0xFFFFFFFC; - } - data->latitude = gGpsDataPtr->lat; - data->longitude = gGpsDataPtr->lon; - data->altitude = gGpsDataPtr->alt; + data->gpsFixType = gGpsDataPtr->gpsFixType; + data->numSatellites = gGpsDataPtr->numSatellites; + + data->latitude = gGpsDataPtr->lat; + data->longitude = gGpsDataPtr->lon; + data->altitude = gGpsDataPtr->alt; - data->vNed[0] = gGpsDataPtr->vNed[0]; - data->vNed[1] = gGpsDataPtr->vNed[1]; - data->vNed[2] = gGpsDataPtr->vNed[2]; + data->vNed[0] = gGpsDataPtr->vNed[0]; + data->vNed[1] = gGpsDataPtr->vNed[1]; + data->vNed[2] = gGpsDataPtr->vNed[2]; - data->trueCourse = gGpsDataPtr->trueCourse; - data->rawGroundSpeed = gGpsDataPtr->rawGroundSpeed; + data->trueCourse = gGpsDataPtr->trueCourse; + data->rawGroundSpeed = gGpsDataPtr->rawGroundSpeed; - data->GPSSecondFraction = gGpsDataPtr->GPSSecondFraction; - data->altEllipsoid = gGpsDataPtr->altEllipsoid; + data->GPSSecondFraction = gGpsDataPtr->GPSSecondFraction; - data->itow = gGpsDataPtr->itow; - data->GPSmonth = gGpsDataPtr->GPSmonth; - data->GPSday = gGpsDataPtr->GPSday; - data->GPSyear = gGpsDataPtr->GPSyear; - data->GPSHour = gGpsDataPtr->GPSHour; - data->GPSMinute = gGpsDataPtr->GPSMinute; - data->GPSSecond = gGpsDataPtr->GPSSecond; + data->itow = gGpsDataPtr->itow; + data->GPSmonth = gGpsDataPtr->GPSmonth; + data->GPSday = gGpsDataPtr->GPSday; + data->GPSyear = gGpsDataPtr->GPSyear; + data->GPSHour = gGpsDataPtr->GPSHour; + data->GPSMinute = gGpsDataPtr->GPSMinute; + data->GPSSecond = gGpsDataPtr->GPSSecond; - data->HDOP = gGpsDataPtr->HDOP; - data->GPSHorizAcc = gGpsDataPtr->GPSHorizAcc; - data->GPSVertAcc = gGpsDataPtr->GPSVertAcc; + data->HDOP = gGpsDataPtr->HDOP; + data->GPSHorizAcc = gGpsDataPtr->GPSHorizAcc; + data->GPSVertAcc = gGpsDataPtr->GPSVertAcc; + data->geoidAboveEllipsoid = gGpsDataPtr->geoidAboveEllipsoid; + } } @@ -439,9 +441,6 @@ BOOL _handleGpsMessages(GpsData_t *GPSData) } tmp = gpsUartBuf[pos++]; bytesInBuffer--; -#ifdef DETECT_USER_SERIAL_CMD - platformDetectUserSerialCmd(tmp); -#endif switch(GPSData->GPSProtocol){ case NMEA_TEXT: parseNMEAMessage(tmp, gpsMsg, GPSData); diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processNMEAGPS.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processNMEAGPS.c index 00e31d0..f51ada5 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processNMEAGPS.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processNMEAGPS.c @@ -33,6 +33,7 @@ limitations under the License. #include #include +#include "gpsAPI.h" #include "driverGPS.h" #include "BITStatus.h" @@ -42,6 +43,9 @@ char _parseRMC(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); + +int dayofweek(int day, int month, int year); + int crcError = 0; int starError = 0; /** **************************************************************************** @@ -181,6 +185,9 @@ void _handleNMEAmsg(char *msgID, GPSData->totalVTG++; _parseVTG(msgBody, GPSData); } + if( strncmp(ptr, "RMC", 3) == 0 ) { + _parseRMC(msgBody, GPSData); + } } /** **************************************************************************** @@ -244,6 +251,8 @@ char extractNMEAfield(char *msgBody, break; } } + outField[outIndex] = '\x0'; + // Leave the currentField as the one returned so the next search passes // the currrent doesn't match requested test // Back up one so the next search picks up the end ',' @@ -252,7 +261,7 @@ char extractNMEAfield(char *msgBody, } /** **************************************************************************** - * @name: _parseGPGGA LOCAL parse GPGGA message. Tiem, position, fix data + * @name: _parseGPGGA LOCAL parse GPGGA message. Time, position, fix data * @author Dong An * @param [in] msgBody - data to parse * @param [in] GPSData - data structure to parst he data into @@ -285,6 +294,7 @@ char extractNMEAfield(char *msgBody, * decimal * 02/16/15 dkh move convert to after check for flag ******************************************************************************/ +#include "debug.h" char _parseGPGGA(char *msgBody, GpsData_t *GPSData) { @@ -292,8 +302,6 @@ char _parseGPGGA(char *msgBody, char status = 0; int parseReset = true; NmeaLatLonSTRUCT nmeaLatLon; - // this is the local using the NMEA convention - char GPSFix = 0; // no fix memset(&nmeaLatLon, 0, sizeof(nmeaLatLon) ); /// Time - convert from ascii digits to decimal '0' = 48d @@ -344,7 +352,17 @@ char _parseGPGGA(char *msgBody, /// GPS quality if( extractNMEAfield(msgBody, field, 5, parseReset) ) { - GPSFix = field[0] - '0'; // convert ascii digit to decimal + GPSData->gpsFixType = field[0] - '0'; // convert ascii digit to decimal + if (GPSData->gpsFixType >= DEAD_REC) // DR, manual and simulation is considered invalid + { + GPSData->gpsFixType = INVALID; + } + } else + status = 1; + + // Number of satellites + if( extractNMEAfield(msgBody, field, 6, parseReset) ) { + GPSData->numSatellites = atoi((char *)field); } else status = 1; @@ -354,32 +372,37 @@ char _parseGPGGA(char *msgBody, } else status = 1; - if(GPSFix >= 1) { - convertItow(GPSData); // create pseudo ITOW - gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked - gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track - } else { - gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // no signal lock - gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track - } - - /// Altitude if( extractNMEAfield(msgBody, field, 8, parseReset) ) { - GPSData->alt = atof((char *)field); + GPSData->alt = atof((char *)field); // altitude above MSL + } else{ + status = 1; + } + if( extractNMEAfield(msgBody, field, 10, parseReset) ) { + GPSData->geoidAboveEllipsoid = atof((char *)field); } else{ status = 1; } - if(GPSFix >= 1) { + // if fixed, convert data + if(GPSData->gpsFixType > INVALID) + { + // Convert deg/min/sec to xxx.xxx deg _NMEA2UbloxAndLLA(&nmeaLatLon, GPSData); + // convert geiod height to ellipsoid height + GPSData->alt += GPSData->geoidAboveEllipsoid; + // create pseudo ITOW + convertItow(GPSData); + + gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked + gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track } - // flip the sense from the NMEA convention to match the propiatery binary messages - if (GPSData->HDOP < 20) { // the threshold may get set lower in algorithm.c - GPSData->GPSFix = 0; // fix - } else { - GPSData->GPSFix = 1; // no fix + else + { + gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // no signal lock + gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track } + if( status == 0) { GPSData->updateFlagForEachCall |= 1 << GOT_GGA_MSG; GPSData->LLHCounter++; @@ -422,6 +445,7 @@ char _parseVTG(char *msgBody, /// speed over ground [km/hr] if( extractNMEAfield(msgBody, field, 6, parseReset) ) { GPSData->rawGroundSpeed = atof((char *)field); // double [kph] + GPSData->rawGroundSpeed *= 0.2777777777778; // convert km/hr to m/s // the heading of the VTG of some receiver is empty when velocity is low. // a default zero will be set to the heading value. if ( status == 1 ) { @@ -474,7 +498,6 @@ char _parseRMC(char *msgBody, GPSData->GPSyear = ( (field[4] - '0') * 10) + field[5] - '0'; /// year } else status = 1; - return status; } @@ -553,10 +576,8 @@ void _NMEA2UbloxAndLLA(NmeaLatLonSTRUCT *NmeaData, ******************************************************************************/ void convertNorhEastVelocity(GpsData_t* GPSData) { - GPSData->vNed[0] = (GPSData->rawGroundSpeed * 0.2777777777778) * - cos(D2R * GPSData->trueCourse); // 0.277 = 1000/3600 kph -> m/s - GPSData->vNed[1] = (GPSData->rawGroundSpeed * 0.2777777777778) * - sin(D2R * GPSData->trueCourse); + GPSData->vNed[0] = GPSData->rawGroundSpeed * cos(D2R * GPSData->trueCourse); + GPSData->vNed[1] = GPSData->rawGroundSpeed * sin(D2R * GPSData->trueCourse); _smoothVerticalData(GPSData); // synthesize and filter down velocity } @@ -603,7 +624,6 @@ void _smoothVerticalData(GpsData_t* GPSData) } if ( OutAltCounter >= 5 ) { firstFlag = 0; // reset - GPSData->filteredAlt = filteredAlt ; GPSData->vNed[2] = filteredVd ; return; } @@ -623,7 +643,6 @@ void _smoothVerticalData(GpsData_t* GPSData) CurrentVd = -(tmpDouble / delta_T); ///down is positive } else { /// may be NMEA is lost firstFlag = 0; /// reset - GPSData->filteredAlt = filteredAlt ; GPSData->vNed[2] = filteredVd ; return; } @@ -643,7 +662,6 @@ void _smoothVerticalData(GpsData_t* GPSData) if (OutVelCounter >= 5) { firstFlag = 0; /// reset - GPSData->filteredAlt = filteredAlt ; GPSData->vNed[2] = filteredVd ; return; } @@ -655,8 +673,7 @@ void _smoothVerticalData(GpsData_t* GPSData) // save the current time local to calc delta T next time LastTotalTime = TotalTime; - /// output the filtered alt and vel values - GPSData->filteredAlt = filteredAlt; + /// output the vel GPSData->vNed[2] = filteredVd; } @@ -668,14 +685,30 @@ void _smoothVerticalData(GpsData_t* GPSData) ******************************************************************************/ void convertItow(GpsData_t* GPSData) { - long double tmp; - - tmp = ((long double)GPSData->GPSHour) * 3600.0 + ((long double)GPSData->GPSMinute) * 60.0; - tmp += (long double)GPSData->GPSSecond + (long double)GPSData->GPSSecondFraction; + double tmp; + // calculate day of week + int tow = dayofweek(GPSData->GPSday, GPSData->GPSmonth, GPSData->GPSyear+2000); + // calculate second of week + tmp = (double)tow * 86400.0; + tmp += ((double)GPSData->GPSHour) * 3600.0 + ((double)GPSData->GPSMinute) * 60.0; + tmp += (double)GPSData->GPSSecond + (double)GPSData->GPSSecondFraction; /// converting UTC to GPS time is impossible without GPS satellite /// Navigation message. using UTC time directly by scaling to ms. GPSData->itow = (unsigned long) (tmp * 1000); } +int dayofweek(int day, int month, int year) +{ + + int adjustment, mm, yy; + + adjustment = (14 - month) / 12; + mm = month + 12 * adjustment - 2; + yy = year - adjustment; + + return (day + (13 * mm - 1) / 5 + + yy + yy / 4 - yy / 100 + yy / 400) % 7; +} + #endif // GPS diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processNovAtelGPS.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processNovAtelGPS.c index a385ac9..6722855 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processNovAtelGPS.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processNovAtelGPS.c @@ -221,16 +221,17 @@ void _parseBestPosB_Fast(logBestPosB *bestPosB, { GPSData->lat = bestPosB->Lat; GPSData->lon = bestPosB->Lon; - GPSData->alt = bestPosB->hgt; ///alt + GPSData->alt = bestPosB->hgt + bestPosB->undulation; // altitude above ellipsoid + GPSData->geoidAboveEllipsoid = bestPosB->undulation; // geoid above ellipsoid if (bestPosB->sol_status != 0) // zero is good fix anything else is { // enumeration for bad fix - GPSData->GPSFix = 0;// PosVelType + GPSData->gpsFixType = 0;// PosVelType gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // locked gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track gGpsDataPtr->HDOP = 21.0f; // force to above threshold } else { - GPSData->GPSFix = bestPosB->pos_type; + GPSData->gpsFixType = bestPosB->pos_type; gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track gGpsDataPtr->HDOP = 1.0f; // force to below threshold @@ -269,7 +270,7 @@ void _parseBestPosB_Fast(logBestPosB *bestPosB, GPSData->GPSHorizAcc = sqrtf( bestPosB->lat_sigma * bestPosB->lat_sigma + bestPosB->lon_sigma * bestPosB->lon_sigma ); // [m] GPSData->GPSVertAcc = bestPosB->hgt_sigma; // [m] - GPSData->numSatelites = bestPosB->num_obs; + GPSData->numSatellites = bestPosB->num_obs; } /** **************************************************************************** @@ -329,7 +330,7 @@ void _parseBestPosB(char *completeMessage, gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track } - GPSData->GPSFix = SolStatus[0]; + GPSData->gpsFixType = SolStatus[0]; memcpy(&GPSData->itow, &completeMessage[16], 4); ///Itow @@ -371,12 +372,12 @@ void _parseBestVelB_Fast(logBestVelB *bestVelB, if (bestVelB->sol_status != 0) // zero is good fix anything else is { // enumeration for bad fix - GPSData->GPSFix = bestVelB->vel_type; + GPSData->gpsFixType = bestVelB->vel_type; gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // not locked gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track gGpsDataPtr->HDOP = 21.0f; // } else { - GPSData->GPSFix = 0; + GPSData->gpsFixType = 0; gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // not locked gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track gGpsDataPtr->HDOP = 1.0f; // @@ -437,7 +438,7 @@ void _parseBestVelB(char *completeMessage, gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // not locked gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track } - GPSData->GPSFix = SolStatus[0]; + GPSData->gpsFixType = SolStatus[0]; memcpy(&GPSData->itow, &completeMessage[16], 4); ///Itow diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processSiRFGPS.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processSiRFGPS.c index 681af14..56c579d 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processSiRFGPS.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processSiRFGPS.c @@ -398,12 +398,12 @@ static void _parseSiRFGeodeticNavMsg(char *msg, // Switched at the sazme time as PPS 5 satellites if ( geo->navValid > 0) { GPSData->HDOP = 50.0; - GPSData->GPSFix = 1; // zero is valid anything else is degraded + GPSData->gpsFixType = 1; // zero is valid anything else is degraded gBitStatus.hwStatus.bit.unlockedInternalGPS = 1; // no signal lock gBitStatus.swStatus.bit.noGPSTrackReference = 1; // no GPS track } else { GPSData->HDOP = geo->hdop; - GPSData->GPSFix = 0; + GPSData->gpsFixType = 0; gBitStatus.hwStatus.bit.unlockedInternalGPS = 0; // locked gBitStatus.swStatus.bit.noGPSTrackReference = 0; // GPS track } @@ -422,7 +422,7 @@ static void _parseSiRFGeodeticNavMsg(char *msg, GPSData->GPSday = geo->utcDay; // dd GPSData->GPSyear = byteSwap16(geo->utcYear) - 2000; // last two digits of year - GPSData->altEllipsoid = (float)byteSwap32(geo->altitudeEllipsoid) / 100.; // 0x2221201f in m * 10^2 + GPSData->geoidAboveEllipsoid = (float)byteSwap32(geo->altitudeEllipsoid - geo->altitudeMSL) / 100.; // 0x2221201f in m * 10^2 GPSData->GPSHour = geo->utcHour; GPSData->GPSMinute = geo->utcMinute; diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processUbloxGPS.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processUbloxGPS.c index 2e75a53..9354476 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processUbloxGPS.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/processUbloxGPS.c @@ -32,6 +32,7 @@ limitations under the License. #include "driverGPS.h" #include "platformAPI.h" +#include "gpsAPI.h" #ifdef DISPLAY_DIAGNOSTIC_MSG #include "debug.h" @@ -312,7 +313,7 @@ void processUbloxBinaryMessage(char *msg, GPSData->VELCounter++; break; case UBLOX_NAV_STATUS: ///NAV_STATUS - GPSData->GPSFix = msg[10]; ///fix indicator + GPSData->gpsFixType = msg[10]; ///fix indicator if((msg[11]&0x02) == 0x02) ///6+5 GPSData->GPSStatusWord |=1 << DGPS_ON; ///on else @@ -422,10 +423,34 @@ void processUbloxBinaryMessage(char *msg, void decodeNavPvt(char *msg, GpsData_t *GPSData) { + // Decode ubloxNavPvtSTRUCT *navPvt; navPvt = (ubloxNavPvtSTRUCT*)(&msg[UBLOX_BINAERY_HEADER_LEN]); + + // Get decoded results // fix type - GPSData->gpsValid = navPvt->fixType == 3; + GPSData->gpsFixType = INVALID; + if (navPvt->flags & 0x01) + { + GPSData->gpsFixType = SPP; + if (navPvt->flags & 0x02) + { + GPSData->gpsFixType = DGPS; + } + uint8_t carrSoln = navPvt->flags >> 6; + if (carrSoln == 1) + { + GPSData->gpsFixType = RTK_FLOAT; + } + else if(carrSoln == 2) + { + GPSData->gpsFixType = RTK_FIX; + } + } + + // num of satellites + GPSData->numSatellites = navPvt->numSV; + // lat GPSData->lat = navPvt->lat * 1.0e-7; @@ -444,9 +469,12 @@ void decodeNavPvt(char *msg, GpsData_t *GPSData) // fractional second GPSData->GPSSecondFraction = navPvt->nano * 1.0e-9; // ellipsoid height - GPSData->altEllipsoid = navPvt->height * 1.0e-3; + GPSData->geoidAboveEllipsoid = (navPvt->height - navPvt->hMSL) * 1.0e-3; // itow GPSData->itow = navPvt->iTOW; + if(GPSData->itow%1000 != 0){ + GPSData->itow+=1; // correction of numerical issue + } platformUpdateITOW(GPSData->itow); // year month day hour min sec GPSData->GPSmonth = navPvt->month; diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/taskGps.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/taskGps.c index 3854b95..56a62a1 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/taskGps.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/GPS/src/taskGps.c @@ -89,16 +89,6 @@ void TaskGps(void const *argument) pollSirfCnt++; } } - - // Set the GPS validity, based on the Horizontal Dilution of Precision - if (gGpsDataPtr->gpsValid && gGpsDataPtr->HDOP > 15.0) - { - gGpsDataPtr->gpsValid = false; - } - else if (gGpsDataPtr->HDOP < 10.0) - { - gGpsDataPtr->gpsValid = true; - } } } } diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Math/include/TransformationMath.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Math/include/TransformationMath.h index 910b5fc..2a01980 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Math/include/TransformationMath.h +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Math/include/TransformationMath.h @@ -73,12 +73,28 @@ real UnitGravityAndMagToYaw(real *unitGravityVector, real *magFieldVector); ******************************************************************************/ real RollPitchAndMagToYaw(real roll, real pitch, real *magFieldVector); +/****************************************************************************** +* @brief Limit angle error to be [-180, 180]. +* TRACE: +* @param [in] aErr in [deg]. +* @retval aErr within [-180, 180]deg. +******************************************************************************/ +real AngleErrDeg(real aErr); + +/****************************************************************************** +* @brief Limit angle error to be [-PI, PI]. +* TRACE: +* @param [in] aErr in [rad]. +* @retval aErr within [-2*PI, 2*PI]. +******************************************************************************/ +real AngleErrRad(real aErr); // BOOL LLA_To_R_EinN( double* llaRad, real* R_EinN); BOOL LLA_To_R_NinE( double* llaRad, real* R_NinE); -BOOL LLA_To_Base( double* llaRad, double* rECEF_Init, - real* dr_N, - real* R_NinE, double* rECEF ); +BOOL ECEF_To_Base( double* rECEF_Init, + double* rECEF , + real* R_NinE, + real* dr_N); BOOL LLA_To_ECEF( double* lla_Rad, double* ecef_m); BOOL PosNED_To_PosECEF( real* r_N, double* rECEF_Init, real* R_NinE, double* rECEF ); @@ -86,10 +102,9 @@ BOOL ECEF_To_LLA( double* llaDeg, double* ecef_m ); BOOL VelECEF_To_VelNED( double* LLA, real* VelECEF, real* VelNED ); + void printMtx(float *a, int m, int n); void printVec(float *v, int n); -int realSymmetricMtxEig(float *a, int n, float *v, float eps, int jt); - #endif /* TRANSFORMATIONMATH_H */ diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Math/src/TransformationMath.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Math/src/TransformationMath.c index 9fc4b2c..9158f9b 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Math/src/TransformationMath.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Math/src/TransformationMath.c @@ -204,68 +204,32 @@ BOOL LLA_To_R_NinE( double* llaRad, ///** *************************************************************************** -//* @name LLA2Base Express LLA in a local NED Base Frame +//* @name Calculate NED relative position of two ECEF positions. //* @details Pre calculated all non-changing constants and unfolded the matrices -//* @param [in] LLA - array with the current Latitude, Longitude and Altitude [rad] -//* @param [in] BaseECEF - start of frame position -//* @param [in] Rne - rotation matrix from ECEF to NED -//* @param [out] NED - output of the position in North East and Down coords -//* @param [out] newECEF - current position in ECEF from LLA +//* @param [in] rECEF_Init - start of frame position +//* @param [in] rECEF - current position in ECEF from LLA +//* @param [in] R_NinE - rotation matrix from NED to ECEF +//* @param [out] dr_N - output of the position in North East and Down coords //* @retval always 1 //******************************************************************************/ -BOOL LLA_To_Base( double* llaRad, // in - double* rECEF_Init, // in - real* dr_N, //NED, +BOOL ECEF_To_Base( double* rECEF_Init, + double* rECEF, real* R_NinE, - double* rECEF) // out + real* dr_N) { real dr_E[NUM_AXIS]; - double N; - double sinLat = sin(llaRad[LAT]); - double cosLat = cos(llaRad[LAT]); - double sinLon = sin(llaRad[LON]); - double cosLon = cos(llaRad[LON]); - - real sinLat_r = (real)sinLat; - real cosLat_r = (real)cosLat; - real sinLon_r = (real)sinLon; - real cosLon_r = (real)cosLon; - - N = E_MAJOR / sqrt(1.0 - (E_ECC_SQ * sinLat * sinLat)); // radius of Curvature [meters] - - //LLA_To_ECEF(llaRad, rECEF); - double temp_d = (N + llaRad[ALT]) * cosLat; - rECEF[X_AXIS] = temp_d * cosLon; - rECEF[Y_AXIS] = temp_d * sinLon; - rECEF[Z_AXIS] = ((E_MINOR_OVER_MAJOR_SQ * N) + llaRad[ALT]) * sinLat; - dr_E[X_AXIS] = (real)( rECEF[X_AXIS] - *(rECEF_Init + X_AXIS) ); dr_E[Y_AXIS] = (real)( rECEF[Y_AXIS] - *(rECEF_Init + Y_AXIS) ); dr_E[Z_AXIS] = (real)( rECEF[Z_AXIS] - *(rECEF_Init + Z_AXIS) ); - // Form R_NinE - // First row - *(R_NinE + 0 * 3 + 0) = -sinLat_r * cosLon_r; - *(R_NinE + 0 * 3 + 1) = -sinLon_r; - *(R_NinE + 0 * 3 + 2) = -cosLat_r * cosLon_r; - - // Second row - *(R_NinE + 1 * 3 + 0) = -sinLat_r * sinLon_r; - *(R_NinE + 1 * 3 + 1) = cosLon_r; - *(R_NinE + 1 * 3 + 2) = -cosLat_r * sinLon_r; - - // Third row - *(R_NinE + 2 * 3 + 0) = cosLat_r; - *(R_NinE + 2 * 3 + 1) = 0.0; - *(R_NinE + 2 * 3 + 2) = -sinLat_r; - - // Convert from delta-position in the ECEF-frame to the NED-frame (the transpose - // in the equations that followed is handled in the formulation) - // - // N E ( E N )T - // dr_N = R * dr_E = ( R ) * dr_E - // ( ) + /* Convert from delta-position in the ECEF-frame to the NED-frame (the transpose + * in the equations that followed is handled in the formulation) + * + * N E ( E N )T + * dr_N = R * dr_E = ( R ) * dr_E + * ( ) + */ dr_N[X_AXIS] = *(R_NinE + X_AXIS * 3 + X_AXIS) * dr_E[X_AXIS] + *(R_NinE + Y_AXIS * 3 + X_AXIS) * dr_E[Y_AXIS] + *(R_NinE + Z_AXIS * 3 + X_AXIS) * dr_E[Z_AXIS]; @@ -438,97 +402,36 @@ void printVec(float *v, int n) printf("%.9g\n", v[i]); } -int realSymmetricMtxEig(float *a, int n, float *v, float eps, int jt) +real AngleErrDeg(real aErr) { - int i, j, p, q, u, w, t, s, l; - double fm, cn, sn, omega, x, y, d; - l = 1; - p = 0; q = 0; // initialized AB - for (i = 0; i <= n - 1; i++) - { - v[i*n + i] = 1.0; - for (j = 0; j <= n - 1; j++) - { - if (i != j) - { - v[i*n + j] = 0.0; - } - } - } - while (1) + while (fabs(aErr) > 180.0) { - fm = 0.0; - for (i = 0; i <= n - 1; i++) + if (aErr > 180.0) { - for (j = 0; j <= n - 1; j++) - { - d = fabs(a[i*n + j]); - if ((i != j) && (d > fm)) - { - fm = d; - p = i; - q = j; - } - } + aErr -= (real)360.0; } - if (fm < eps) + else if (aErr < -180.0) { - return(1); + aErr += (real)360.0; } - if (l > jt) - { - return(-1); - } - l = l + 1; - u = p * n + q; - w = p * n + p; - t = q * n + p; - s = q * n + q; - x = -a[u]; - y = (a[s] - a[w]) / 2.0; - omega = x / sqrt(x*x + y * y); - if (y < 0.0) - { - omega = -omega; - } - sn = 1.0 + sqrt(1.0 - omega * omega); - sn = omega / sqrt(2.0*sn); - cn = sqrt(1.0 - sn * sn); - fm = a[w]; - a[w] = fm * cn*cn + a[s] * sn*sn + a[u] * omega; - a[s] = fm * sn*sn + a[s] * cn*cn - a[u] * omega; - a[u] = 0.0; - a[t] = 0.0; - for (j = 0; j <= n - 1; j++) - { - if ((j != p) && (j != q)) - { - u = p * n + j; - w = q * n + j; - fm = a[u]; - a[u] = fm * cn + a[w] * sn; - a[w] = -fm * sn + a[w] * cn; - } - } - for (i = 0; i <= n - 1; i++) + } + + return aErr; +} + +real AngleErrRad(real aErr) +{ + while (fabs(aErr) > PI) + { + if (aErr > PI) { - if ((i != p) && (i != q)) - { - u = i * n + p; - w = i * n + q; - fm = a[u]; - a[u] = fm * cn + a[w] * sn; - a[w] = -fm * sn + a[w] * cn; - } + aErr -= (real)TWO_PI; } - for (i = 0; i <= n - 1; i++) + else if (aErr < -PI) { - u = i * n + p; - w = i * n + q; - fm = v[u]; - v[u] = fm * cn + v[w] * sn; - v[w] = -fm * sn + v[w] * cn; + aErr += (real)TWO_PI; } } - return(1); -} \ No newline at end of file + + return aErr; +} diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/Odo/odoAPI.h b/examples/OpenIMU300RI/VG_AHRS/lib/Core/Odo/odoAPI.h new file mode 100644 index 0000000..458560b --- /dev/null +++ b/examples/OpenIMU300RI/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/OpenIMU300RI/VG_AHRS/lib/Core/UARTComm/CommonMessages.c b/examples/OpenIMU300RI/VG_AHRS/lib/Core/UARTComm/CommonMessages.c index 4812cfb..09cbf0b 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/UARTComm/CommonMessages.c +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/UARTComm/CommonMessages.c @@ -253,7 +253,7 @@ BOOL Fill_e1PacketPayload(uint8_t *payload, uint8_t *payloadLen) *payloadLen = sizeof(ekf1_payload_t); pld->tstmp = platformGetIMUCounter(); - pld->dbTstmp = (double)platformGetSolutionTstamp() * 0.000001; // seconds + pld->dbTstmp = platformGetSolutionTstampAsDouble() * 0.000001; // seconds EKF_GetAttitude_EA(EulerAngles); pld->roll = (float)EulerAngles[ROLL]; @@ -306,8 +306,7 @@ BOOL Fill_e2PacketPayload(uint8_t *payload, uint8_t *payloadLen) *payloadLen = sizeof(ekf2_payload_t); pld->tstmp = platformGetIMUCounter(); - pld->dbTstmp = (double)platformGetSolutionTstamp() * 0.000001; // seconds - + pld->dbTstmp = platformGetSolutionTstampAsDouble() * 0.000001; // seconds EKF_GetAttitude_EA(rData); pld->roll = (float)rData[ROLL]; @@ -347,7 +346,7 @@ BOOL Fill_e2PacketPayload(uint8_t *payload, uint8_t *payloadLen) EKF_GetEstimatedLLA(dData); for(int i = 0; i < NUM_AXIS; i++){ - pld->pos[i] = (float)dData[i]; + pld->pos[i] = dData[i]; } uint8_t opMode, linAccelSw, turnSw; diff --git a/examples/OpenIMU300RI/VG_AHRS/lib/Core/library.json b/examples/OpenIMU300RI/VG_AHRS/lib/Core/library.json index 497943a..f5babce 100644 --- a/examples/OpenIMU300RI/VG_AHRS/lib/Core/library.json +++ b/examples/OpenIMU300RI/VG_AHRS/lib/Core/library.json @@ -1,22 +1,24 @@ { - "name": "Core", - "version": "1.0.4", - "description": "Algorithm and GPS libraries", - "build": { - "flags": [ - "-I Algorithm", - "-I Algorithm/include", - "-I Buffer", - "-I Buffer/include", - "-I Common", - "-I Common/include", - "-I GPS", - "-I GPS/include", - "-I Math", - "-I Math/include", - "-I SPIComm", - "-I UARTComm", - "-I ." + "name": "Core", + "version": "1.0.4", + "description": "Algorithm and GPS libraries", + "build": { + "flags": [ + "-I Algorithm", + "-I Algorithm/include", + "-I Buffer", + "-I Buffer/include", + "-I Common", + "-I Common/include", + "-I GPS", + "-I GPS/include", + "-I Odo", + "-I Odo/include", + "-I Math", + "-I Math/include", + "-I SPIComm", + "-I UARTComm", + "-I ." ] } } diff --git a/examples/OpenIMU300RI/VG_AHRS/platformio.ini b/examples/OpenIMU300RI/VG_AHRS/platformio.ini index 48475ca..cceb861 100644 --- a/examples/OpenIMU300RI/VG_AHRS/platformio.ini +++ b/examples/OpenIMU300RI/VG_AHRS/platformio.ini @@ -14,16 +14,17 @@ description = VG_AHRS application example. A Kalman filter based algorithm that platform = aceinna_imu lib_archive = false board = OpenIMU300 -;lib_deps= ../../openIMU300-lib -lib_deps = OpenIMU300-base-library@1.0.8 +;lib_deps = ../../openIMU300-lib/openIMU300-lib +lib_deps = OpenIMU300-base-library@1.1.6 build_flags = ; -D CLI +; Comment next line for VG algorithm + -D AHRS_ALGORITHM -D SAE_J1939 -D CAN_BUS_COMM -D __FPU_PRESENT -D ARM_MATH_CM4 -I include - -I include/API -I src/user -I src -I lib/CAN/include diff --git a/examples/OpenIMU300RI/VG_AHRS/src/appVersion.h b/examples/OpenIMU300RI/VG_AHRS/src/appVersion.h index 26d4d90..69dcb01 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 1.0.1" +#define APP_VERSION_STRING "VG_AHRS_J1939 1.0.3" #endif diff --git a/examples/OpenIMU300RI/VG_AHRS/src/user/UserAlgorithm.c b/examples/OpenIMU300RI/VG_AHRS/src/user/UserAlgorithm.c index ae49480..7479a3d 100644 --- a/examples/OpenIMU300RI/VG_AHRS/src/user/UserAlgorithm.c +++ b/examples/OpenIMU300RI/VG_AHRS/src/user/UserAlgorithm.c @@ -24,166 +24,77 @@ limitations under the License. *******************************************************************************/ #include +#include -#include "algorithmAPI.h" -#include "gpsAPI.h" #include "platformAPI.h" #include "userAPI.h" #include "Indices.h" #include "GlobalConstants.h" +#if FAST_MATH +#include "arm_math.h" +#endif + +#include "UserConfiguration.h" +#include "algorithmAPI.h" #include "algorithm.h" +#include "UserAlgorithm.h" #include "EKF_Algorithm.h" -#include "BitStatus.h" -#include "UserConfiguration.h" -#include "bsp.h" -#include "debug.h" -#include "MagAlign.h" -#include "sae_j1939.h" +#ifndef INS_OFFLINE +#include "debug.h" +#endif // !INS_OFFLINE -#define DEBUG_FREQ_0HZ 0 -#define DEBUG_FREQ_1HZ 1 -#define DEBUG_FREQ_2HZ 2 -#define DEBUG_FREQ_5HZ 5 +// Declare the IMU data structure +IMUDataStruct gIMU; +gpsDataStruct_t gGPS; +odoDataStruct_t gOdo; +int initAlgo = 0;; // -void _Algorithm(int dacqRate, uint8_t algoType); +static void _Algorithm(); -// Initialize GPS algorithm variables +// Initialize algorithm variables void InitUserAlgorithm() { // Initialize built-in algorithm structure InitializeAlgorithmStruct(FREQ_200_HZ); // place additional required initialization here + /* Set the configuration variables for AHRS solution + * Enable mag + */ +#ifdef AHRS_ALGORITHM + enableMagInAlgorithm(TRUE); +#endif } -// Initialization variable -int initAlgo = 1; -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) { - static int debugOutputCntr = 0, debugOutputCntrLimit = 0, debugOutputFreq = DEBUG_FREQ_0HZ; - - if(!UseAlgorithm()){ - return NULL; - } - - - // Initialize the timer variables - if(initAlgo) { - // Reset 'initAlgo' so this is not executed more than once. - initAlgo = 0; - // Set the variables that control the debug-message output-rate (based on - // the desired calling frequency of the debug output) - debugOutputCntr = 0; - - if(debugOutputFreq != DEBUG_FREQ_0HZ){ - debugOutputCntrLimit = (int)( (float)dacqRate / (float)debugOutputFreq + 0.5 ); - }else{ - debugOutputCntrLimit = 0; - } - } - - // Populate the EKF input data structure. Load the GPS data - // structure as NULL. - EKF_SetInputStruct(accels, rates, mags, NULL); + // Populate the EKF input data structure + EKF_SetInputStruct(accels, rates, mags, gps, odo, ppsDetected); // Call the desired algorithm based on the EKF with different // calling rates and different settings. - //****************************************************************** - // NOTE: Provide algorithm type as a parameter here (VG or AHRS) - //****************************************************************** - _Algorithm(dacqRate, VG); + _Algorithm(); // Fill the output data structure with the EKF states and other // desired information EKF_SetOutputStruct(); - // Generate the debug output - if(debugOutputCntrLimit) { - debugOutputCntr++; - if(debugOutputCntr >= debugOutputCntrLimit) { - debugOutputCntr = 0; - DebugPrintFloat("Roll: ", gEKFOutputData.eulerAngs_BinN[ROLL], 5); - DebugPrintFloat(", Pitch: ", gEKFOutputData.eulerAngs_BinN[PITCH], 5); - DebugPrintFloat(", aX: ", gEKFInputData.accel_B[X_AXIS], 5); - DebugPrintFloat(", aY: ", gEKFInputData.accel_B[Y_AXIS], 5); - DebugPrintFloat(", aZ: ", gEKFInputData.accel_B[Z_AXIS], 5); - DebugPrintInt(", Mode: ", gEKFOutputData.opMode); - DebugPrintEndline(); - } - } - // The returned value from this function is unused by external functions. The // NULL pointer is returned instead of a data structure. return NULL; } - // -void _Algorithm(int dacqRate, uint8_t algoType) +static void _Algorithm() { - // - static int initAlgo = 1; - static uint8_t algoCntr = 0, algoCntrLimit = 0; - - // Initialize the configuration variables needed to make the system - // generate a VG-type solution. - 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; - - // While not needed, set hasMags to false - enableMagInAlgorithm(FALSE); - - if(algoType == VG) { - // Configuration already set for a VG solution - } else if(algoType == AHRS) { - // Set the configuration variables for AHRS solution - // (useMags = 1 and enable mags) - enableMagInAlgorithm(TRUE); - gAlgorithm.Behavior.bit.useMag = 1; - } else if(algoType == INS) { - while(1); - } else { - while(1); - } - - algoCntr = 0; - algoCntrLimit = (int)( dacqRate / (int)gAlgorithm.callingFreq ); - 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; - } - } - // Aceinna VG/AHRS/INS algorithm - if(algoCntr == 0) { EKF_Algorithm(); - } - - // 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) { - algoCntr = 0; - } } - diff --git a/examples/OpenIMU300RI/VG_AHRS/src/user/UserAlgorithm.h b/examples/OpenIMU300RI/VG_AHRS/src/user/UserAlgorithm.h index 1c8c3d6..2dd0719 100644 --- a/examples/OpenIMU300RI/VG_AHRS/src/user/UserAlgorithm.h +++ b/examples/OpenIMU300RI/VG_AHRS/src/user/UserAlgorithm.h @@ -8,6 +8,23 @@ #ifndef _USER_ALGORITHM_H_ #define _USER_ALGORITHM_H_ +#include +#include "GlobalConstants.h" + + // IMU data structure +typedef struct { + // Timer output counter + uint32_t timerCntr, dTimerCntr; + + // Algorithm states + double accel_g[3]; + double rate_radPerSec[3]; + double rate_degPerSec[3]; + double mag_G[3]; + double temp_C; +} IMUDataStruct; + +extern IMUDataStruct gIMU; #endif /* _USER_ALGORITHM_H_ */ diff --git a/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfiguration.c b/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfiguration.c index 630d38a..c4de528 100644 --- a/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfiguration.c +++ b/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfiguration.c @@ -32,6 +32,7 @@ limitations under the License. #include "userAPI.h" #include "UserConfiguration.h" +#include "UserMessagingUart.h" #include "eepromAPI.h" #include "Indices.h" #include "sae_j1939.h" @@ -365,10 +366,12 @@ void ApplySystemParameters(void *pConfig) platformSelectLPFilter(RATE_SENSOR, pEcuConfig->rate_cut_off, TRUE); platformSelectLPFilter(ACCEL_SENSOR, pEcuConfig->accel_cut_off, TRUE); platformApplyOrientation(pEcuConfig->orien_bits); + if(UseMags()){ platformForceMagUsage(); } - userInitConfigureUart(); + + UserInitConfigureUart(); } diff --git a/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfigurationUart.c b/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfigurationUart.c index 6e20226..7b79f6b 100644 --- a/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfigurationUart.c +++ b/examples/OpenIMU300RI/VG_AHRS/src/user/UserConfigurationUart.c @@ -101,7 +101,7 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) } -void userInitConfigureUart() +void UserInitConfigureUart() { uint64_t *ptr = (uint64_t*)&gUserUartConfig; diff --git a/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingCAN.c b/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingCAN.c index 65f109c..4a33a00 100644 --- a/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingCAN.c +++ b/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingCAN.c @@ -40,6 +40,7 @@ limitations under the License. #include "BITStatus.h" #include "sensorsAPI.h" #include "sae_j1939.h" +#include "math.h" // for EKFOutputDataStruct #include "EKF_Algorithm.h" @@ -276,6 +277,25 @@ void EnqeuePeriodicDataPackets(int latency, int sendPeriodicPackets) aceinna_j1939_send_angular_rate(&angle_data); } + if (packets_to_send & ACEINNA_SAE_J1939_PACKET_HEADING) { + VEHICLE_DIR_SPEED ds_data; + real velocity[3]; + double tmp; + EKF_GetEstimatedVelocity(velocity); // m/s + ds_data.altitude = 2500 * 8; // 0 m + tmp = gKalmanFilter.eulerAngles[YAW] * 57.296; + tmp = tmp < 0 ? 360.0 + tmp : tmp; + tmp = tmp * 128 + 0.5; + ds_data.bearing = (uint16_t)(tmp); + ds_data.pitch = (uint16_t)((gKalmanFilter.eulerAngles[PITCH] * 57.296 + 200.00) * 128 + 0.5); + tmp = sqrtf(powf(velocity[0],2) + powf(velocity[1],2)); + tmp = (tmp*3.6); // km/h + ds_data.speed = (uint16_t)tmp*256; + + aceinna_j1939_send_veh_dir_speed(&ds_data); + } + + forced_packets = 0; return; diff --git a/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingUART.c b/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingUART.c index 33ad77a..c7060a4 100644 --- a/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingUART.c +++ b/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingUART.c @@ -39,13 +39,6 @@ limitations under the License. #include "Indices.h" // For X_AXIS, etc #include "CommonMessages.h" -// Declare the IMU data structure -IMUDataStruct gIMU; - -// Version string -//char userVersionString[] = "IMU 1.0.0"; - - @@ -332,9 +325,8 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) pld->c = 'A'; pld->s = 1234; pld->i = -5; - if(platformGetPpsFlag()){ + if(platformGetPpsFlag(FALSE)){ ppsTstamp = platformGetPpsTimeStamp(); - platformSetPpsFlag(FALSE); } // pld->ll = platformGetDacqTimeStamp(); // in microseconds; pld->ll = ppsTstamp; // time stamp of last PPS edge in microseconds from system start diff --git a/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingUART.h b/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingUART.h index 7d96e1c..61a0c19 100644 --- a/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingUART.h +++ b/examples/OpenIMU300RI/VG_AHRS/src/user/UserMessagingUART.h @@ -151,5 +151,6 @@ extern BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloa extern BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen); extern BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen); extern BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen); +extern void UserInitConfigureUart(); #endif /* USER_CONFIGURATION_H */ diff --git a/examples/OpenIMU300RI/VG_AHRS/src/user/dataProcessingAndPresentation.c b/examples/OpenIMU300RI/VG_AHRS/src/user/dataProcessingAndPresentation.c index 1e97a1e..a467230 100644 --- a/examples/OpenIMU300RI/VG_AHRS/src/user/dataProcessingAndPresentation.c +++ b/examples/OpenIMU300RI/VG_AHRS/src/user/dataProcessingAndPresentation.c @@ -24,16 +24,26 @@ See the License for the specific language governing permissions and limitations under the License. *******************************************************************************/ +#include "BITStatus.h" + #include "userAPI.h" #include "sensorsAPI.h" #include "gpsAPI.h" -#include "UserMessagingCAN.h" +#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 -// Declare the IMU data structure -IMUDataStruct gIMU; +BOOL fAlgorithmSynced; + +// Local-function prototypes +static void _IncrementIMUTimer(uint16_t dacqRate); +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); +static void _IMUDebugMessage(void); +//static void _GPSDebugMessage(void); /* * ****************** @@ -58,6 +68,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); } @@ -67,71 +79,219 @@ void initUserDataProcessingEngine() // 2) 'DataAcquisitionTask' calls this function after retrieving samples of current // sensors data and applying corresponding calibration // 3) 'dacqRate' is rate with which new set of sensors data arrives -int algorithmInitFlag = 0; - void inertialAndPositionDataProcessing(uint16_t dacqRate) { + // + void *results; - // Initialization variable - static int initFlag = 1; + // Increment the IMU timer by the calling rate of the data-acquisition task + _IncrementIMUTimer(dacqRate); - // Variables that control the output frequency of the debug statement - static int debugFlag = 0; - static uint8_t debugOutputCntr, debugOutputCntrLimit; + 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; - if(algorithmInitFlag){ - InitUserAlgorithm(); // default implementation located in file user_algorithm.c + // Obtain accelerometer data [g] + GetAccelData_g(gIMU.accel_g); + + // Obtain rate-sensor data [rad/sec] + GetRateData_radPerSec(gIMU.rate_radPerSec); + GetRateData_degPerSec(gIMU.rate_degPerSec); + + // Obtain magnetometer data [G] + GetMagData_G(gIMU.mag_G); + + // Obtain board temperature data [degC] + 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); + + // Execute user algorithm (default implementation located in file user_algorithm.c) + 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; + GetAlgoStatus(&algoStatus); + gBitStatus.swStatus.all = algoStatus.all; + 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 } + // Generate a debug message that provide sensor data in order to verify the + // algorithm input data is as expected. + _GenerateDebugMessage(dacqRate, ZERO_HZ); +} + + +// +static void _IncrementIMUTimer(uint16_t dacqRate) +{ // Initialize timer variables (used to control the output of the debug // messages and the IMU timer value) + 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) - debugOutputCntr = 0; - - uint16_t debugOutputFreq = 4; // [Hz] - debugOutputCntrLimit = dacqRate / debugOutputFreq; - // Set the IMU output delta-counter value gIMU.dTimerCntr = (uint32_t)( 1000.0 / (float)(dacqRate) + 0.5 ); } - // Increment the timer-counter by the sampling period equivalent to the rate at which - // inertialAndPositionDataProcessing is called + // Increment the timer-counter by the sampling period equivalent to the + // rate at which inertialAndPositionDataProcessing is called gIMU.timerCntr = gIMU.timerCntr + gIMU.dTimerCntr; +} - // Obtain accelerometer data [g] - GetAccelData_g(gIMU.accel_g); - - // Obtain rate-sensor data [rad/sec] - GetRateData_radPerSec(gIMU.rate_radPerSec); +// +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) +{ + // Variables that control the output frequency of the debug statement + static uint8_t debugOutputCntr, debugOutputCntrLimit; - // Obtain magnetometer data [G] - GetMagData_G(gIMU.mag_G); + // Check debug flag. If set then generate the debug message to verify + // the loading of the GPS data into the GPS data structure + 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; - // Obtain board temperature data [degC] - GetBoardTempData(&gIMU.temp_C); + // 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; + } - // Generate the debug output to test the loading of the sensor data - // into the IMU data structure - if( debugFlag ) { debugOutputCntr++; if(debugOutputCntr >= debugOutputCntrLimit) { debugOutputCntr = 0; - DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); - DebugPrintFloat(", AccelZ: ", gIMU.accel_g[Z_AXIS], 3); - DebugPrintFloat(", RateZ: ", gIMU.rate_radPerSec[Z_AXIS] * RAD_TO_DEG, 3); - DebugPrintFloat(", MagX: ", gIMU.mag_G[X_AXIS], 3); - DebugPrintFloat(", Temp: ", gIMU.temp_C,2); - DebugPrintEndline(); + + // Reset 'new GPS data' flag (this should be done in UpdateFunctions + // to ensure the EKF can use the data) + //gGPS.updateFlag = 0; <-- This would make a difference as the input + // to the algorithm isn't set yet. + + // Create message here + static uint8_t msgType = 1; + switch( msgType ) + { + case 0: + // None + break; + + case 1: + // IMU data + _IMUDebugMessage(); + break; + +#ifdef GPS + case 2: + // GPS data + _GPSDebugMessage(); + break; +#endif } } + } +} + + +static void _IMUDebugMessage(void) +{ + // IMU Data + DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); + DebugPrintFloat(", a: [ ", (float)gIMU.accel_g[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Z_AXIS], 3); + DebugPrintFloat(" ], w: [ ", (float)gIMU.rate_radPerSec[X_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Y_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Z_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ], m: [ ", (float)gIMU.mag_G[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Z_AXIS], 3); + DebugPrintString(" ]"); + DebugPrintEndline(); +} - // Execute user algorithm (default implementation located in file user_algorithm.c) - RunUserNavAlgorithm(gIMU.accel_g, gIMU.rate_radPerSec, gIMU.mag_G, NULL, dacqRate); +#ifdef GPS +static void _GPSDebugMessage(void) +{ +#if 1 + // GPS Data + DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); + DebugPrintFloat(", Lat: ", (float)gGPS.latitude, 8); + 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); + 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 +#endif + DebugPrintEndline(); } +#endif diff --git a/examples/OpenIMU300RI/VG_AHRS/src/user/userAPI.h b/examples/OpenIMU300RI/VG_AHRS/src/user/userAPI.h index 9ca2ff2..86ca868 100644 --- a/examples/OpenIMU300RI/VG_AHRS/src/user/userAPI.h +++ b/examples/OpenIMU300RI/VG_AHRS/src/user/userAPI.h @@ -41,30 +41,17 @@ 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(); void InitUserAlgorithm(); void initUserDataProcessingEngine(); void userInitConfigureUnit(); -void userInitConfigureUart(); -// IMU data structure -typedef struct { - // Timer output counter - uint32_t timerCntr, dTimerCntr; - - // Algorithm states - double accel_g[3]; - double rate_radPerSec[3]; - double rate_degPerSec[3]; - double mag_G[3]; - double temp_C; -} IMUDataStruct; - -extern IMUDataStruct gIMU; #endif diff --git a/examples/OpenIMU300ZI/INS/platformio.ini b/examples/OpenIMU300ZI/INS/platformio.ini index bf18776..4271e75 100644 --- a/examples/OpenIMU300ZI/INS/platformio.ini +++ b/examples/OpenIMU300ZI/INS/platformio.ini @@ -19,7 +19,7 @@ description = [env:OpenIMU300ZI] platform = aceinna_imu lib_archive = false -board = openIMU300 +board = OpenIMU300 ;lib_deps= ../../OpenIMU300-lib lib_deps = OpenIMU300-base-library@1.0.9 build_flags = diff --git a/platform.json b/platform.json index d6b89e8..92d1e75 100644 --- a/platform.json +++ b/platform.json @@ -13,7 +13,7 @@ "type": "git", "url": "https://github.com/aceinna/platform-aceinna_imu.git" }, - "version": "1.2.6", + "version": "1.2.7", "packageRepositories": [ "https://dl.bintray.com/platformio/dl-packages/manifest.json", "http://dl.platformio.org/packages/manifest.json"