From bbb333d74b6f98b9c6d1510582d9a2901fb70976 Mon Sep 17 00:00:00 2001 From: dw-aceinna Date: Tue, 10 Mar 2020 22:26:56 +0800 Subject: [PATCH] major update to have v1.0.2 --- Core/Algorithm/algorithmAPI.h | 98 - Core/Algorithm/include/TimingVars.h | 75 - Core/Algorithm/include/algorithm.h | 538 -- Core/Algorithm/src/TimingVars.c | 164 - Core/Algorithm/src/algorithm.c | 4802 ----------------- Core/Buffer/include/buffer.h | 62 - Core/Buffer/src/buffer.c | 83 - Core/Cjson/src/json_parse.c | 179 - Core/Common/include/scaling.h | 78 - Core/Common/include/ucb_packet_struct.h | 119 - Core/Common/src/utilities.c | 224 - Core/DmaMemcpy/inc/memcpy_dma.h | 35 - Core/DmaMemcpy/src/memcpy_dma.c | 120 - Core/GPS/gpsAPI.h | 229 - Core/Logg/inc/log.h | 124 - Core/Logg/src/log.c | 178 - Core/UARTComm/CommonMessages.c | 441 -- Core/UARTComm/CommonMessages.h | 164 - Core/console/inc/shell.h | 34 - Core/console/src/shell.c | 562 -- Core/library.json | 24 - FreeRTOS/src/cmsis_os.c | 2 - FreeRTOS_M4/library.json | 12 - LWIP/lwip-1.4.1/src/netif/ethernetif.c | 2 +- LWIP/lwip_app/NtripClient/inc/ntripClient.h | 14 +- LWIP/lwip_app/NtripClient/src/ntripClient.c | 119 +- LWIP/lwip_app/user/inc/LwipComm.h | 2 +- LWIP/lwip_app/user/src/LwipComm.c | 3 +- LWIP/lwip_app/webserver/src/httpd_handler.c | 50 +- Platform/Board/include/bsp.h | 2 +- Platform/Board/src/bsp.c | 6 - Platform/Board/src/configureGPIO.c | 2 +- Platform/Board/src/stm32f4xx_it.c | 17 +- Platform/Core/include/Indices.h | 2 +- Platform/Core/include/debug.h | 2 +- Platform/Core/include/serial_port.h | 10 +- Platform/Core/include/ucb_packet.h | 76 +- Platform/Core/include/xbowsp_algorithm.h | 2 +- Platform/Core/src/debug.c | 5 +- Platform/Core/src/handle_packet.c | 173 +- Platform/Core/src/parameters.c | 39 +- Platform/Core/src/platform.c | 14 +- Platform/Core/src/send_packet.c | 781 +-- Platform/Core/src/serial_port.c | 117 +- Platform/Core/src/ucb_packet.c | 98 +- Platform/Driver/inc/RingBuffer.h | 66 - Platform/Driver/{inc => include}/driver.h | 2 +- Platform/Driver/{inc => include}/exit.h | 0 Platform/Driver/{inc => include}/led.h | 0 Platform/Driver/{inc => include}/timer.h | 15 +- Platform/Driver/{inc => include}/uart.h | 95 +- Platform/Driver/src/RingBuffer.c | 206 - Platform/Driver/src/exit.c | 54 +- Platform/Driver/src/led.c | 6 +- Platform/Driver/src/timer.c | 26 +- Platform/Driver/src/uart.c | 60 +- Platform/Filter/src/filter.c | 9 +- {Core/Cjson => Platform/cJSON}/inc/cJSON.h | 0 .../Cjson => Platform/cJSON}/inc/json_parse.h | 0 {Core/Cjson => Platform/cJSON}/src/cJSON.c | 0 Platform/cJSON/src/json_parse.c | 171 + Platform/commAPI.h | 2 +- .../common/include/constants.h | 85 +- .../common/include/utils.h | 51 +- Platform/common/src/utils.c | 47 + Platform/configurationAPI.h | 5 +- .../GPS => Platform/gnss_data}/include/rtcm.h | 21 +- .../gnss_data}/include/rtklib_core.h | 0 {Core/GPS => Platform/gnss_data}/src/rtcm.c | 52 +- Platform/gnss_data_api.h | 72 + Platform/hwAPI.h | 2 +- .../math}/include/FastInvTrigFuncs.h | 0 .../math}/include/MatrixMath.h | 2 +- .../math}/include/QuaternionMath.h | 2 +- .../math}/include/TransformationMath.h | 2 +- .../math}/include/VectorMath.h | 2 +- .../math}/include/arm_common_tables.h | 0 .../math}/include/arm_const_structs.h | 0 .../Math => Platform/math}/include/arm_math.h | 0 {Core/Math => Platform/math}/include/qmath.h | 0 {Core/Math => Platform/math}/include/xmath.h | 0 .../math}/src/FastInvTrigFuncs.c | 0 {Core/Math => Platform/math}/src/MatrixMath.c | 0 .../math}/src/QuaternionMath.c | 0 .../math}/src/TransformationMath.c | 0 {Core/Math => Platform/math}/src/VectorMath.c | 0 .../math}/src/arm_common_tables.c | 0 .../math}/src/arm_const_structs.c | 0 .../Math => Platform/math}/src/arm_cos_f32.c | 0 .../Math => Platform/math}/src/arm_sin_f32.c | 0 {Core/Math => Platform/math}/src/qmath.c | 0 Platform/platformAPI.h | 13 +- Platform/platform_version.h | 14 +- {Platform => Sensors}/eepromAPI.h | 2 +- Sensors/sensorsAPI.h | 2 +- library.json | 23 +- 96 files changed, 646 insertions(+), 10346 deletions(-) delete mode 100644 Core/Algorithm/algorithmAPI.h delete mode 100644 Core/Algorithm/include/TimingVars.h delete mode 100644 Core/Algorithm/include/algorithm.h delete mode 100644 Core/Algorithm/src/TimingVars.c delete mode 100644 Core/Algorithm/src/algorithm.c delete mode 100644 Core/Buffer/include/buffer.h delete mode 100644 Core/Buffer/src/buffer.c delete mode 100644 Core/Cjson/src/json_parse.c delete mode 100644 Core/Common/include/scaling.h delete mode 100644 Core/Common/include/ucb_packet_struct.h delete mode 100644 Core/Common/src/utilities.c delete mode 100644 Core/DmaMemcpy/inc/memcpy_dma.h delete mode 100644 Core/DmaMemcpy/src/memcpy_dma.c delete mode 100644 Core/GPS/gpsAPI.h delete mode 100644 Core/Logg/inc/log.h delete mode 100644 Core/Logg/src/log.c delete mode 100644 Core/UARTComm/CommonMessages.c delete mode 100644 Core/UARTComm/CommonMessages.h delete mode 100644 Core/console/inc/shell.h delete mode 100644 Core/console/src/shell.c delete mode 100644 Core/library.json delete mode 100644 FreeRTOS_M4/library.json delete mode 100644 Platform/Driver/inc/RingBuffer.h rename Platform/Driver/{inc => include}/driver.h (97%) rename Platform/Driver/{inc => include}/exit.h (100%) rename Platform/Driver/{inc => include}/led.h (100%) rename Platform/Driver/{inc => include}/timer.h (86%) rename Platform/Driver/{inc => include}/uart.h (53%) delete mode 100644 Platform/Driver/src/RingBuffer.c rename {Core/Cjson => Platform/cJSON}/inc/cJSON.h (100%) rename {Core/Cjson => Platform/cJSON}/inc/json_parse.h (100%) rename {Core/Cjson => Platform/cJSON}/src/cJSON.c (100%) create mode 100644 Platform/cJSON/src/json_parse.c rename Core/Common/include/GlobalConstants.h => Platform/common/include/constants.h (63%) rename Core/Common/include/utilities.h => Platform/common/include/utils.h (50%) create mode 100644 Platform/common/src/utils.c rename {Core/GPS => Platform/gnss_data}/include/rtcm.h (97%) rename {Core/GPS => Platform/gnss_data}/include/rtklib_core.h (100%) rename {Core/GPS => Platform/gnss_data}/src/rtcm.c (99%) create mode 100644 Platform/gnss_data_api.h rename {Core/Math => Platform/math}/include/FastInvTrigFuncs.h (100%) rename {Core/Math => Platform/math}/include/MatrixMath.h (97%) rename {Core/Math => Platform/math}/include/QuaternionMath.h (94%) rename {Core/Math => Platform/math}/include/TransformationMath.h (99%) rename {Core/Math => Platform/math}/include/VectorMath.h (97%) rename {Core/Math => Platform/math}/include/arm_common_tables.h (100%) rename {Core/Math => Platform/math}/include/arm_const_structs.h (100%) rename {Core/Math => Platform/math}/include/arm_math.h (100%) rename {Core/Math => Platform/math}/include/qmath.h (100%) rename {Core/Math => Platform/math}/include/xmath.h (100%) rename {Core/Math => Platform/math}/src/FastInvTrigFuncs.c (100%) rename {Core/Math => Platform/math}/src/MatrixMath.c (100%) rename {Core/Math => Platform/math}/src/QuaternionMath.c (100%) rename {Core/Math => Platform/math}/src/TransformationMath.c (100%) rename {Core/Math => Platform/math}/src/VectorMath.c (100%) rename {Core/Math => Platform/math}/src/arm_common_tables.c (100%) rename {Core/Math => Platform/math}/src/arm_const_structs.c (100%) rename {Core/Math => Platform/math}/src/arm_cos_f32.c (100%) rename {Core/Math => Platform/math}/src/arm_sin_f32.c (100%) rename {Core/Math => Platform/math}/src/qmath.c (100%) rename {Platform => Sensors}/eepromAPI.h (99%) diff --git a/Core/Algorithm/algorithmAPI.h b/Core/Algorithm/algorithmAPI.h deleted file mode 100644 index 1e1f0db..0000000 --- a/Core/Algorithm/algorithmAPI.h +++ /dev/null @@ -1,98 +0,0 @@ -/** ****************************************************************************** - * @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 enableGpsInAlgorithm(BOOL enable); - -BOOL gpsUsedInAlgorithm(); - -void SetAlgorithmUseDgps(BOOL d); - -void updateAlgorithmTimings(int corr, uint32_t tmrVal ); - -uint32_t getAlgorithmTimer(); - -uint16_t getAlgorithmCounter(); - -/****************************************************************************** - * @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/Core/Algorithm/include/TimingVars.h b/Core/Algorithm/include/TimingVars.h deleted file mode 100644 index 1799cbc..0000000 --- a/Core/Algorithm/include/TimingVars.h +++ /dev/null @@ -1,75 +0,0 @@ -/****************************************************************************** - * @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_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/Core/Algorithm/include/algorithm.h b/Core/Algorithm/include/algorithm.h deleted file mode 100644 index c772ac7..0000000 --- a/Core/Algorithm/include/algorithm.h +++ /dev/null @@ -1,538 +0,0 @@ -/***************************************************************************** -* @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" -#include "gpsAPI.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 - -#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 - -// IMU spec -/* [Hz], The data sampling rate when calculate ARW and VRW. -* ARW = sigma * sqrt(dt) = sigma * sqrt(1/ODR) -*/ -#define RW_ODR 100.0 -/* [rad/sqrt(s)], gyro angular random walk, sampled at 100Hz -* 0.3deg/sqrt(Hr) = 0.3 / 60 * D2R = 8.72664625997165e-05rad/sqrt(s) -*/ -#define ARW_300ZA 8.73e-5 - -#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 - -// 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 - -// Size of EKF matrices -#define ROWS_IN_P 16 -#define COLS_IN_P 16 - -#define ROWS_IN_F 16 -#define COLS_IN_F 16 - -// 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]; - - // The following two are used in more than one functions, so they are pre-computed. - real wTrueTimesDtOverTwo[NUM_AXIS]; - real turnSwitchMultiplier; -} 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 - mcu_time_base_t rovTime; //gnss time rov->time - uint16_t week; - uint32_t itow; - float age; - 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. -} 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; - - uint16_t week; - uint32_t itow; - uint8_t gnss_sol_type; -} EKF_OutputDataStruct; - -extern EKF_OutputDataStruct gEKFOutput; - -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; - -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] -//#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; - - InnovationStruct Innov; -} LimitStruct; - -/// specifying how the user sets up the device algorithm -struct algoBehavior_BITS { /// bits description - uint16_t freeIntegrate : 1; /// 0 - uint16_t useMag : 1; /// 1 - uint16_t useGPS : 1; /// 2 - Not used yet - uint16_t stationaryLockYaw : 1; /// 3 - Not used yet - uint16_t restartOnOverRange : 1; /// 4 - uint16_t dynamicMotion : 1; /// 5 - Not used - uint16_t rsvd : 10; /// 6:15 -}; - -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 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 -}; - -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 { - uint16_t week; - 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; - - -/****************************************************************************** - * @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); - - -void EKF_PredictionStage(real *filteredAccel); -void GenerateProcessCovariance(void); -void GenerateProcessJacobian(void); - - -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); - - -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); - - -// 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); -void EKF_SetOutputStruct(void); - - -#endif diff --git a/Core/Algorithm/src/TimingVars.c b/Core/Algorithm/src/TimingVars.c deleted file mode 100644 index 2942dfb..0000000 --- a/Core/Algorithm/src/TimingVars.c +++ /dev/null @@ -1,164 +0,0 @@ -/****************************************************************************** - * 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 -} - diff --git a/Core/Algorithm/src/algorithm.c b/Core/Algorithm/src/algorithm.c deleted file mode 100644 index 2d106ef..0000000 --- a/Core/Algorithm/src/algorithm.c +++ /dev/null @@ -1,4802 +0,0 @@ -/****************************************************************************** - * @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 "TransformationMath.h" -#include "QuaternionMath.h" -#include "MatrixMath.h" -#include "VectorMath.h" - -#include "platformAPI.h" -#include "algorithm.h" -#include "algorithmAPI.h" -#include "TimingVars.h" -#include "buffer.h" - - -AlgorithmStruct gAlgorithm; -AlgoStatus gAlgoStatus; - -KalmanFilterStruct gKalmanFilter; -EKF_InputDataStruct gEKFInput; -EKF_OutputDataStruct gEKFOutput; -ImuStatsStruct gImuStats; - -static real accumulatedAccelVector[3]; -static real accumulatedGyroVector[3]; -// static real accumulatedMagVector[3]; -static real averagedAccelVector[3]; -static real averagedGyroVector[3]; -// static real averagedMagVector[3]; - -#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) - -//============================================================================= -// 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. - ******************************************************************************/ -/* 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; - } -} - - -/****************************************************************************** - * @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) -{ - // 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); -} - -/****************************************************************************** - * @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) -{ - 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); - } -} - - -void InitializeAlgorithmStruct(uint8_t callingFreq) -{ - gAlgorithm.Behavior.bit.freeIntegrate = FALSE; - // 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 = 2.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 = MAX_BW; - gAlgorithm.staticDetectParam.staticGnssVel = 0.2; - gAlgorithm.staticDetectParam.staticNoiseMultiplier[0] = 4.0; - gAlgorithm.staticDetectParam.staticNoiseMultiplier[1] = 4.0; - gAlgorithm.staticDetectParam.staticNoiseMultiplier[2] = 1.0; -} - -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; -} - - -/* predict function.c */ - - - -// -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()); - return 1; -} - - -/****************************************************************************** -* @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. - -* 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()); - return 1; -} - - -// -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(); -} - -// 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 == 1 && 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; - - // 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 - } - } - } -} - - - -/* updatefunction.c */ -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); - -/****************************************************************************** - * @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 == 1) - { - // Sync the algorithm itow to the GPS value - gAlgorithm.week = gEKFInput.week; - gAlgorithm.itow = gEKFInput.itow; - // Resync timer - timer.tenHertzCntr = 0; - timer.subFrameCntr = 0; - - // GNSS update - if (gEKFInput.gpsFixType) - { - // GPS heading valid? - useGpsHeading = (gEKFInput.rawGroundSpeed >= LIMIT_MIN_GPS_VELOCITY_HEADING); - - /* 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; - } - //else - 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 - useGpsHeading = 0; - } - } -} - -// ----Compute the innovation vector, nu---- -void ComputeSystemInnovation_Pos(void) -{ - // Position error - 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 - 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 - { - 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 }}; -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 - 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) - { - if (!gImuStats.bStaticIMU) - { - // If neither mag or GPS headig is available, update measuremnt matrix H to 2x16 - if (gKalmanFilter.R[STATE_YAW] > 0.9) - { - for (colNum = 0; colNum < COLS_IN_H; colNum++) - { - gKalmanFilter.H[2][colNum] = 0.0; - } - } - lastYaw = 7.0; - } - else - { - if (lastYaw > TWO_PI) - { - lastYaw = gKalmanFilter.eulerAngles[YAW]; - } - else - { - gKalmanFilter.nu[STATE_YAW] = lastYaw - gKalmanFilter.eulerAngles[YAW]; - gKalmanFilter.R[STATE_YAW] = 1e-4; - } - } - 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(deltaP_tmp, 0, sizeof(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++) - { - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + - KxH[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; - } - deltaP_tmp[colNum][rowNum] = 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; - } - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + - KxH[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; - } - deltaP_tmp[colNum][rowNum] = 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] - - 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(deltaP_tmp, 0, sizeof(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; - } - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + - gKalmanFilter.K[rowNum][multIndex] * gKalmanFilter.P[multIndex][colNum]; - } - deltaP_tmp[colNum][rowNum] = 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]); - - // 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(deltaP_tmp, 0, sizeof(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++) - { - deltaP_tmp[rowNum][colNum] = deltaP_tmp[rowNum][colNum] + - gKalmanFilter.K[rowNum][multIndex - STATE_VX] * gKalmanFilter.P[multIndex][colNum]; - } - deltaP_tmp[colNum][rowNum] = 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]); - // ++++++++++++++++++++++ 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 - */ - 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 hasOdo = FALSE; - BOOL frontVelMeaValid = FALSE; - real frontVelMea = 0.0; - /* Front velocity is first determined by odometer. If odometer is not available, zero velocity - * detection results are used to determine if front velocity is zero. If neither odometer is - * available nor zero velocity detected, front velocity measurement is not valid. - */ - if (hasOdo) - { - frontVelMeaValid = TRUE; - frontVelMea = 0.0; // replace with real odo output - 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(deltaP_tmp, 0, sizeof(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; - } - deltaP_tmp[rowNum][colNum] = gKalmanFilter.K[rowNum][0] * PxHTranspose[colNum][0] + - gKalmanFilter.K[rowNum][1] * PxHTranspose[colNum][1] + - gKalmanFilter.K[rowNum][2] * PxHTranspose[colNum][2]; - deltaP_tmp[colNum][rowNum] = deltaP_tmp[rowNum][colNum]; - } - } - - /* 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] - - 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 gpsUpdate = 0; - - // - switch( updateRate ) - { - // ten-hertz update - case 10: - if( timer.subFrameCntr == 0 ) - { - gpsUpdate = 1; - } - break; - - // twenty-hertz update - case 20: - if( timer.subFrameCntr == 0 || timer.subFrameCntr == 5 ) - { - gpsUpdate = 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 ) - { - gpsUpdate = 1; - } - break; - - // fifty-hertz update - case 50: - if( timer.subFrameCntr == 0 || - timer.subFrameCntr == 2 || - timer.subFrameCntr == 4 || - timer.subFrameCntr == 6 || - timer.subFrameCntr == 8 ) - { - gpsUpdate = 1; - } - break; - - // fifty-hertz update - case 100: - gpsUpdate = 1; - break; - } - - return gpsUpdate; -} - -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 -} - - -/****************************************************************************** - * @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); - -//============================================================================= - -/* This routine is called at either 100 or 200 Hz based upon the system configuration: - * -- Unaided soln: 200 Hz - * -- Aided soln: 100 Hz - */ - -void enableFreeIntegration(BOOL enable) -{ - gAlgorithm.Behavior.bit.freeIntegrate = enable; -} - - -BOOL freeIntegrationEnabled() -{ - return (BOOL)gAlgorithm.Behavior.bit.freeIntegrate; -} - -void enableMagInAlgorithm(BOOL enable) -{ - if(1) - { - gAlgorithm.Behavior.bit.useMag = enable; - } - else - { - gAlgorithm.Behavior.bit.useMag = FALSE; - } -} - -// BOOL magUsedInAlgorithm() -// { -// return gAlgorithm.Behavior.bit.useMag != 0; -// } - -BOOL gpsUsedInAlgorithm(void) -{ - return (BOOL)gAlgorithm.Behavior.bit.useGPS; -} - -void enableGpsInAlgorithm(BOOL enable) -{ - gAlgorithm.Behavior.bit.useGPS = enable; -} - -// 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 -static void calc_mn(double a, double e2, double lat, double *M, double *N) -{ - double slat = sin(lat); - double v = 1.0 - e2*slat*slat; - *M = a*(1.0 - e2) / pow(v, 1.5); - *N = a / sqrt(v); - -} - -// Populate the EKF input structure with sensor and GPS data (if used) -void EKF_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStruct_t *gps) -{ - // 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; - // gps->gpsUpdate = 0; //Prevent duplicate execution - if (gEKFInput.gpsUpdate == 1) - { - gps->gpsUpdate = 0; //Prevent duplicate execution - - // Validity data - gEKFInput.gpsFixType = gps->gpsFixType; - - // num of satellites - gEKFInput.numSatellites = gps->numSatellites; - - gEKFInput.rovTime.time = gps->rovTime.time; - gEKFInput.rovTime.msec = gps->rovTime.msec; - - // DW DEBUG - uint32_t delay = 0; - double dlat = 0.0, dlon = 0.0, dhgt = 0.0; - // delay = (g_MCU_time.time - gEKFInput.rovTime.time)*1000 + g_MCU_time.msec - gEKFInput.rovTime.msec; - // if (delay <= 0 && delay >= 2000) { - // gEKFInput.gpsFixType = 0; - // return; - // } - - - // double M = 0.0, N = 0.0; - - // double ds = delay*0.001; - // calc_mn(E_MAJOR, E_ECC_SQ, gps->latitude, &M, &N); - // dlat = (gps->vNed[0]*ds) / (M + gps->altitude); - // dlon = (gps->vNed[1]*ds) / ((N + gps->altitude) * cos(gps->latitude)); - // dhgt = gps->vNed[2]*ds; - - // ITOW data - gEKFInput.week = gps->week; - gEKFInput.itow = gps->itow + delay; - - - // Lat/Lon/Alt data - gEKFInput.llaAnt[LAT] = gps->latitude + dlat; - gEKFInput.llaAnt[LON] = gps->longitude + dlon; - gEKFInput.llaAnt[ALT] = gps->altitude + dhgt; - gEKFInput.geoidAboveEllipsoid = gps->geoidAboveEllipsoid; - - // Data quality measures - gEKFInput.GPSHorizAcc = gps->GPSHorizAcc; - gEKFInput.GPSVertAcc = gps->GPSVertAcc; - gEKFInput.HDOP = gps->HDOP; - gEKFInput.age = gps->sol_age; - - - // 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(gEKFInput.vNed[0]) + - SQUARE(gEKFInput.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]); - } - } -} - - -// 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]; - - gEKFOutput.week = gAlgorithm.week; - gEKFOutput.itow = gAlgorithm.itow; - gEKFOutput.gnss_sol_type = gEKFInput.gpsFixType; -} - - -// -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; -} - - -/* prediction functions */ -/* 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 _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 - - // 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]; - - gKalmanFilter.Q[STATE_ABX] = (real)1.0e-12; - gKalmanFilter.Q[STATE_ABY] = (real)1.0e-12; - gKalmanFilter.Q[STATE_ABZ] = (real)1.0e-12; - - /* 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; - - // 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; -} - - -/** **************************************************************************** - * @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; -} - - - - - - - -/****************************************************************************** - * @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); -} - -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; -} - - - - -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; -} - -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); - } - - // 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); - - /* 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(); -} diff --git a/Core/Buffer/include/buffer.h b/Core/Buffer/include/buffer.h deleted file mode 100644 index 678144b..0000000 --- a/Core/Buffer/include/buffer.h +++ /dev/null @@ -1,62 +0,0 @@ -/***************************************************************************** - * 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/Core/Buffer/src/buffer.c b/Core/Buffer/src/buffer.c deleted file mode 100644 index 0b7ff40..0000000 --- a/Core/Buffer/src/buffer.c +++ /dev/null @@ -1,83 +0,0 @@ -#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/Core/Cjson/src/json_parse.c b/Core/Cjson/src/json_parse.c deleted file mode 100644 index fddf92e..0000000 --- a/Core/Cjson/src/json_parse.c +++ /dev/null @@ -1,179 +0,0 @@ -/******************************************************************************* -* File Name : json_parse.c -* Author : Daich -* Revision : 1.0 -* Date : 29/09/2019 -* Description : json_parse.c -* -* HISTORY*********************************************************************** -* 29/09/2019 | | Daich -* Description: add send_rtk_json_message -*******************************************************************************/ - -#include -#include -#include -#include "cJSON.h" -#include "json_parse.h" -#include "log.h" -#include "appVersion.h" -#include "configuration.h" -#include "ucb_packet.h" -#include "uart.h" -#include "platformAPI.h" -#ifndef SENSOR_UNUSED - #include "calibrationAPI.h" -#endif -#ifndef BAREMETAL_OS - #include "UserConfiguration.h" -#endif -int change_item(char *text,char* json_to_write,char* key,char* value) -{ - char *out; - cJSON *root; - root = cJSON_Parse(text); - if (!root) - { -#ifdef DEVICE_DEBUG - printf("not json format\r\n"); -#endif - return json_false; - } - else - { - cJSON* item_replace = cJSON_GetObjectItem(root,"openrtk config"); - cJSON_ReplaceItemInObject(item_replace,key,cJSON_CreateString(value)); -#if 0 - item = cJSON_GetObjectItem(root,"rtk config"); - memcpy(cJSON_GetObjectItem(item,key)->valuestring,value,strlen(value)); // how to modify value safety? -#endif - out = cJSON_Print(root); - { - memcpy(json_to_write,out,strlen(out) + 1); - } - cJSON_Delete(root); - free(out); - return json_true; - } -} - - -void create_json_object(cJSON** json) -{ - *json = cJSON_CreateObject(); -} - - -char* get_rtk_json_item_value(cJSON *json,char* key) //TODO: -{ - //int json_array_size = cJSON_GetArraySize(); -#if 1 - if(json == NULL) - { - return NULL; - } - int json_item_count = cJSON_GetArraySize(json); - for(int i=0; i < json_item_count; i++) - { - cJSON * item = cJSON_GetArrayItem(json, i); - if(item->type == cJSON_Object) - { - cJSON* item_sub = cJSON_GetObjectItem(item,key); - if(item_sub != NULL) - { - return item_sub->valuestring; - } -#if 0 - for(int j=0;j < cJSON_GetArraySize(item); j++) - { - if - } -#endif - } - } -#endif - return NULL; -} - -void send_rtk_json_message(cJSON* root) -{ - cJSON *fmt; - char *out; - //root=cJSON_CreateObject(); - if(root != NULL) - { -#ifdef DEVICE_DEBUG - printf("root = %p\n",root); -#endif - char compile_time[50] = __DATE__; - //root = cJSON_CreateObject(); - cJSON_AddItemToObject(root, "openrtk config", fmt=cJSON_CreateObject()); - cJSON_AddItemToObject(fmt, "Product Name", cJSON_CreateString(PRODUCT_NAME_STRING)); - cJSON_AddItemToObject(fmt, "Product PN", cJSON_CreateString((const char *)platformBuildInfo())); - cJSON_AddItemToObject(fmt, "Product SN", cJSON_CreateNumber(GetUnitSerialNum())); - cJSON_AddItemToObject(fmt, "Version", cJSON_CreateString(APP_VERSION_STRING)); - cJSON_AddItemToObject(fmt, "Compile Time", cJSON_CreateString(compile_time)); - int packet_type = configGetPacketCode(); - char packet_type_str[5] = {0}; - packet_type_str[0] = packet_type >> 8; - packet_type_str[1] = packet_type; - cJSON_AddItemToObject(fmt, "userPacketType", cJSON_CreateString(packet_type_str)); -#ifndef BAREMETAL_OS - float* ins_para = get_user_ins_para(); - cJSON_AddItemToObject(fmt, "leverArmBx", cJSON_CreateNumber(*ins_para)); - cJSON_AddItemToObject(fmt, "leverArmBy", cJSON_CreateNumber(*(ins_para+1))); - cJSON_AddItemToObject(fmt, "leverArmBz", cJSON_CreateNumber(*(ins_para+2))); - cJSON_AddItemToObject(fmt, "pointOfInterestBx", cJSON_CreateNumber(*(ins_para+3))); - cJSON_AddItemToObject(fmt, "pointOfInterestBy", cJSON_CreateNumber(*(ins_para+4))); - cJSON_AddItemToObject(fmt, "pointOfInterestBz", cJSON_CreateNumber(*(ins_para+5))); - cJSON_AddItemToObject(fmt, "rotationRbvx", cJSON_CreateNumber(*(ins_para+6))); - cJSON_AddItemToObject(fmt, "rotationRbvy", cJSON_CreateNumber(*(ins_para+7))); - cJSON_AddItemToObject(fmt, "rotationRbvz", cJSON_CreateNumber(*(ins_para+8))); -#endif - out=cJSON_Print(root); - //cJSON_Delete(root); //not delete other code use -#ifdef DEVICE_DEBUG - printf("%s\n",out); -#endif - uart_write_bytes(UART_BT,out,strlen(out),1); - free(out); - } -} -void send_rtk_json_to_esp32() -{ - cJSON *root,*fmt; - char *out; - root=cJSON_CreateObject(); - char compile_time[50] = __DATE__; - root = cJSON_CreateObject(); - cJSON_AddItemToObject(root, "openrtk config", fmt=cJSON_CreateObject()); - cJSON_AddItemToObject(fmt, "Product Name", cJSON_CreateString(PRODUCT_NAME_STRING)); - cJSON_AddItemToObject(fmt, "Product PN", cJSON_CreateString((const char *)platformBuildInfo())); - cJSON_AddItemToObject(fmt, "Product SN", cJSON_CreateNumber(GetUnitSerialNum())); - cJSON_AddItemToObject(fmt, "Version", cJSON_CreateString(APP_VERSION_STRING)); - cJSON_AddItemToObject(fmt, "Compile Time", cJSON_CreateString(compile_time)); - int packet_type = configGetPacketCode(); - char packet_type_str[5] = {0}; - packet_type_str[0] = packet_type >> 8; - packet_type_str[1] = packet_type; - cJSON_AddItemToObject(fmt, "userPacketType", cJSON_CreateString(packet_type_str)); -#ifndef BAREMETAL_OS - float* ins_para = get_user_ins_para(); - cJSON_AddItemToObject(fmt, "leverArmBx", cJSON_CreateNumber(*ins_para)); - cJSON_AddItemToObject(fmt, "leverArmBy", cJSON_CreateNumber(*(ins_para+1))); - cJSON_AddItemToObject(fmt, "leverArmBz", cJSON_CreateNumber(*(ins_para+2))); - cJSON_AddItemToObject(fmt, "pointOfInterestBx", cJSON_CreateNumber(*(ins_para+3))); - cJSON_AddItemToObject(fmt, "pointOfInterestBy", cJSON_CreateNumber(*(ins_para+4))); - cJSON_AddItemToObject(fmt, "pointOfInterestBz", cJSON_CreateNumber(*(ins_para+5))); - cJSON_AddItemToObject(fmt, "rotationRbvx", cJSON_CreateNumber(*(ins_para+6))); - cJSON_AddItemToObject(fmt, "rotationRbvy", cJSON_CreateNumber(*(ins_para+7))); - cJSON_AddItemToObject(fmt, "rotationRbvz", cJSON_CreateNumber(*(ins_para+8))); -#endif - out=cJSON_Print(root); - cJSON_Delete(root); -#ifdef DEVICE_DEBUG - printf("%s\n",out); -#endif - uart_write_bytes(UART_BT,out,strlen(out),1); - free(out); -} diff --git a/Core/Common/include/scaling.h b/Core/Common/include/scaling.h deleted file mode 100644 index 169123f..0000000 --- a/Core/Common/include/scaling.h +++ /dev/null @@ -1,78 +0,0 @@ -/** *************************************************************************** - * @file scaling.h - * @Author - * @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. - * - * macros to define various power of 2, fixed point and constant scalsing - ******************************************************************************/ -#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/Core/Common/include/ucb_packet_struct.h b/Core/Common/include/ucb_packet_struct.h deleted file mode 100644 index 754fc30..0000000 --- a/Core/Common/include/ucb_packet_struct.h +++ /dev/null @@ -1,119 +0,0 @@ -/** *************************************************************************** - * @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; - -typedef enum -{ - USR_IN_NONE = 0, - USR_IN_PING, - USR_IN_UPDATE_PARAM, - USR_IN_SAVE_CONFIG, - USR_IN_GET_ALL, - USR_IN_GET_VERSION, - // 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, - USR_OUT_TEST, - USR_OUT_DATA1, - USR_OUT_ANG1, - USR_OUT_ANG2, -// add new output packet type here, before USR_OUT_MAX - USR_OUT_SCALED1, - USR_OUT_EKF1, - USR_OUT_EKF2, - USR_OUT_RTK1, - USR_OUT_POS, - USR_OUT_SKY, - USR_OUT_C1, - USR_OUT_MAX -} UserOutPacketType; - -#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/Core/Common/src/utilities.c b/Core/Common/src/utilities.c deleted file mode 100644 index 0ffe768..0000000 --- a/Core/Common/src/utilities.c +++ /dev/null @@ -1,224 +0,0 @@ -/** *************************************************************************** - * @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/Core/DmaMemcpy/inc/memcpy_dma.h b/Core/DmaMemcpy/inc/memcpy_dma.h deleted file mode 100644 index 0da0dc8..0000000 --- a/Core/DmaMemcpy/inc/memcpy_dma.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************* -* File Name : dma_memcpy.h -* Author : Daich -* Revision : 1.0 -* Date : 30/09/2019 -* Description : dma_memcpy.h -* -* HISTORY*********************************************************************** -* 30/09/2019 | | Daich -* Description: create - -* 15/10/2019 | | Daich -* Description: modify dma_memcpy_test -*******************************************************************************/ -#ifndef _DMA_MEMCPY_H_ -#define _DMA_MEMCPY_H_ -//#pragma once -#include - -typedef enum -{ - DMA_MEM_STATE_RESET = 0x00U, /*!< DMA not yet initialized or disabled */ - DMA_MEM_STATE_READY = 0x01U, /*!< DMA initialized and ready for use */ - DMA_MEM_STATE_BUSY = 0x02U, /*!< DMA process is ongoing */ - DMA_MEM_STATE_TIMEOUT = 0x03U, /*!< DMA timeout state */ - DMA_MEM_STATE_ERROR = 0x04U, /*!< DMA error state */ - DMA_MEM_STATE_ABORT = 0x05U, /*!< DMA Abort state */ -}DMA_MEM_STATE_E; - -DMA_MEM_STATE_E get_dma_mem_state(); -void dma_memcpy(uint32_t dst_address, uint32_t src_address, uint32_t data_len); -void dma_memcpy_init(void); -int dma_memcpy_test(void); -int buffer_cmp(const uint32_t* pBuffer, uint32_t* pBuffer1, uint16_t BufferLength); -#endif diff --git a/Core/DmaMemcpy/src/memcpy_dma.c b/Core/DmaMemcpy/src/memcpy_dma.c deleted file mode 100644 index eb8aadd..0000000 --- a/Core/DmaMemcpy/src/memcpy_dma.c +++ /dev/null @@ -1,120 +0,0 @@ -/******************************************************************************* -* File Name : dma_memcpy.h -* Author : Daich -* Revision : 1.0 -* Date : 30/09/2019 -* Description : zero-copy by dma -* -* HISTORY*********************************************************************** -* 30/09/2019 | | Daich -* Description: create -* 15/10/2019 | | Daich -* Description: modify dma_memcpy_test -*******************************************************************************/ - -#include "stm32f4xx_hal.h" -#ifndef BAREMETAL_OS - #include "osapi.h" - #include "osresources.h" -#else - #include "bare_osapi.h" -#endif - -#include -#include "memcpy_dma.h" - -#define DMA_MEMCPY_IRQ DMA2_Stream0_IRQHandler - -DMA_HandleTypeDef hdma_memtomem; -const uint8_t src_const_buffer[24] = { - 0x31,0x31,0x31,0x31, - 0x31,0x31,0x31,0x31, - 0x31,0x31,0x31,0x31, - 0x31,0x31,0x31,0x31, - 0x31,0x31,0x31,0x31, - 0x31,0x31,0x31,0x31, - }; - -static void delay_ms(uint32_t time) -{ - uint32_t i=0; - while(time--) - { - i = 18000; - while(i--) - { - asm("nop"); - } - } -} - -int dma_memcpy_test(void) -{ - dma_memcpy_init(); - printf("start test\n"); - while (1) - { - uint8_t dst_buffer[32]; - dma_memcpy((uint32_t)dst_buffer,(uint32_t)src_const_buffer,24); - printf("test = %s\n",dst_buffer); - delay_ms(2000); - } -} - - -void dma_memcpy_init(void) -{ - __HAL_RCC_DMA2_CLK_ENABLE(); - - hdma_memtomem.Instance = DMA2_Stream0; - hdma_memtomem.Init.Channel = DMA_CHANNEL_1; - hdma_memtomem.Init.Direction = DMA_MEMORY_TO_MEMORY; - hdma_memtomem.Init.PeriphInc = DMA_PINC_ENABLE; - hdma_memtomem.Init.MemInc = DMA_MINC_ENABLE; - hdma_memtomem.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - hdma_memtomem.Init.MemDataAlignment = DMA_PDATAALIGN_BYTE; - hdma_memtomem.Init.Mode = DMA_NORMAL; - hdma_memtomem.Init.Priority = DMA_PRIORITY_LOW ; - hdma_memtomem.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - __HAL_DMA_ENABLE_IT(&hdma_memtomem,DMA_FLAG_TCIF1_5); - HAL_DMA_Init(&hdma_memtomem); -} - -int buffer_cmp(const uint32_t* pBuffer, uint32_t* pBuffer1, uint16_t BufferLength) -{ - while(BufferLength--) - { - if(*pBuffer != *pBuffer1) - { - return -1; - } - - pBuffer++; - pBuffer1++; - } - return 1; -} - -void dma_memcpy(uint32_t dst_address, uint32_t src_address, uint32_t data_len) -{ - while(HAL_DMA_GetState(&hdma_memtomem) == HAL_DMA_STATE_BUSY) - { - ; - } - HAL_DMA_Start_IT(&hdma_memtomem, src_address, dst_address, data_len); - //HAL_DMA_Start(&hdma_memtomem,SrcAddress,DstAddress,DataLength); -} - -DMA_MEM_STATE_E get_dma_mem_state() -{ - return HAL_DMA_GetState(&hdma_memtomem); -} - -void DMA_MEMCPY_IRQ(void) //TODO: -{ - /* USER CODE BEGIN DMA2_Stream0_IRQn 0 */ - - HAL_DMA_IRQHandler(&hdma_memtomem); - - /* USER CODE END DMA2_Stream0_IRQn 1 */ -} diff --git a/Core/GPS/gpsAPI.h b/Core/GPS/gpsAPI.h deleted file mode 100644 index e4d55b6..0000000 --- a/Core/GPS/gpsAPI.h +++ /dev/null @@ -1,229 +0,0 @@ -/** *************************************************************************** - * @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" -#include "rtcm.h" -#include "timer.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 - -void initGPSDataStruct(void); /// note: should pass in SPI or UART - -// FIXME ECW: implement GpsWhoAmI and GpsSelfTest -uint8_t GpsWhoAmI(uint32_t *whoami); /// returns true if value is as expected -uint8_t GpsSelfTest(); - -// debugging stream out the debug port -#define GPS_NO_STREAM 0 -#define GPS_NMEA_DEBUG_STREAM 1 - -// #define MAX_ITOW (604800000) // ms - -#ifndef STREAM_GPS -//#define STREAM_GPS GPS_NO_STREAM -#define STREAM_GPS GPS_NMEA_DEBUG_STREAM -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -void GnssDataAcqTask(void const *argument); - -#ifdef __cplusplus -} -#endif - -/** \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; - -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 - 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 mean sea level [m] - float altEllipsoid; // [km] altitude above ellipsoid for WMM - - uint16_t week; - uint32_t itow; // gps Time Of Week, miliseconds - mcu_time_base_t rovTime; //gnss time rov->time - - float GPSHorizAcc, GPSVertAcc; - float HDOP, sol_age; - float velHorAcc; - float geoidAboveEllipsoid; // [m] Height of geoid (mean sea level) above WGS84 ellipsoid - float latitude_standard_deviation; //!< latitude standard deviation (m) - float longitude_standard_deviation; //!< longitude standard deviation (m) - float height_standard_deviation; //!< height standard deviation (m) - - uint8_t num_gnss_psr; - uint8_t num_gnss_adr; - -} gpsDataStruct_t; - -extern gpsDataStruct_t *gPtrGnssSol; -extern gpsDataStruct_t gGnssSol; - - -/** @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 { - BOOL gpsFixType; - int latSign; - int lonSign; - 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? - - /// compatible with Ublox driver FIXME should these be seperate data structure? -#ifdef USE_UBLOX - unsigned char ubloxClassID; - unsigned char ubloxMsgID; - unsigned char ubloxOldVersion; - float UbloxSoftwareVer; -#endif - 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 - unsigned char isGPSFWVerKnown; - unsigned char isGPSBaudrateKnown; - unsigned long Timer100Hz10ms; - - 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; - - int numSatelites; - - unsigned char gnss_data_flag; - unsigned char new_eph_flag; - gnss_rtcm_t rtcm; /* store RTCM data struct for RTK and PPP */ - - /* for algorithm */ - obs_t rov; - obs_t ref; - nav_t nav; - -} GpsData_t; - - -BOOL SetGpsBaudRate(int rate, int fApply); -BOOL SetGpsProtocol(int protocol, int fApply); - -#endif /* GPS_API_H */ diff --git a/Core/Logg/inc/log.h b/Core/Logg/inc/log.h deleted file mode 100644 index 87db566..0000000 --- a/Core/Logg/inc/log.h +++ /dev/null @@ -1,124 +0,0 @@ -/******************************************************************************* -* File Name : log.h -* Author : Daich -* Revision : 1.0 -* Date : 19/09/2019 -* Description : log head file -* -* HISTORY*********************************************************************** -* 19/09/2019 | | Daich -* -*******************************************************************************/ -#ifndef _LOG_H_ -#define _LOG_H_ -//#pragma once - - - -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -#define BYTES_PER_LINE 16 - -#ifdef DEBUG - #define DbgPrintf(format,args...) \ - fprintf(stderr, format, ##args) -#else - #define DbgPrintf(format,args...) -#endif - -extern const char* log_type[7] ; - -typedef enum { - ATS_LOG_NONE, /*!< No log output */ - ATS_LOG_ERROR, /*!< Critical errors, software module can not recover on its own */ - ATS_LOG_WARN, /*!< Error conditions from which recovery measures have been taken */ - ATS_LOG_INFO, /*!< Information messages which describe normal flow of events */ - ATS_LOG_MES, - ATS_LOG_DEBUG, /*!< Extra information which is not necessary for normal use (values, pointers, sizes, etc). */ - ATS_LOG_VERBOSE /*!< Bigger chunks of debugging information, or frequent messages which can potentially flood the output. */ -} ats_log_level_t; -#define LOG_LOCAL_LEVEL ATS_LOG_DEBUG - -#define CONFIG_LOG_COLORS -#ifdef CONFIG_LOG_COLORS -#define LOG_COLOR_BLACK "30" -#define LOG_COLOR_RED "31" -#define LOG_COLOR_GREEN "32" -#define LOG_COLOR_BROWN "33" -#define LOG_COLOR_BLUE "34" -#define LOG_COLOR_PURPLE "35" -#define LOG_COLOR_CYAN "36" -#define LOG_COLOR_WHITE "37" -#define LOG_COLOR(COLOR) "\033[0;" COLOR "m" //高亮 -//#define LOG_COLOR(COLOR) "\033[5;" COLOR "m" //闪烁 可用分号叠加 - -#define LOG_BOLD(COLOR) "\033[1;" COLOR "m" -#define LOG_DEFAULT_COLOR "\033[0m" -#define LOG_COLOR_E LOG_COLOR(LOG_COLOR_RED)//LOG_COLOR(LOG_COLOR_RED) -#define LOG_COLOR_W LOG_COLOR(LOG_COLOR_BROWN) -#define LOG_COLOR_I LOG_COLOR(LOG_COLOR_GREEN) -#define LOG_COLOR_M LOG_COLOR(LOG_COLOR_CYAN) -#define LOG_COLOR_D LOG_COLOR(LOG_COLOR_WHITE) -#define LOG_COLOR_V -#else //CONFIG_LOG_COLORS -#define LOG_COLOR_E -#define LOG_COLOR_W -#define LOG_COLOR_I -#define LOG_COLOR_D -#define LOG_COLOR_V -#define LOG_DEFAULT_COLOR -#endif //CONFIG_LOG_COLORS - -uint32_t ats_log_timestamp(void); -void ats_log_write(ats_log_level_t level, const char* tag, const char* format, ...) __attribute__ ((format (printf, 3, 4))); - -#ifdef PUT_TIME -#define LOG_FORMAT(letter, format) LOG_COLOR_ ## letter #letter " (%d) %s: " format LOG_DEFAULT_COLOR "\r\n" - -#define ATS_LOGE( tag, format, ... ) if (LOG_LOCAL_LEVEL >= ATS_LOG_ERROR) { ats_log_write(ATS_LOG_ERROR, tag, LOG_FORMAT(E, format), ats_log_timestamp(), tag, ##__VA_ARGS__); } -#define ATS_LOGI( tag, format, ... ) ATS_LOG_LEVEL_LOCAL(ATS_LOG_INFO, tag, format, ##__VA_ARGS__) -#define ATS_LOGV( tag, format, ... ) if (LOG_LOCAL_LEVEL >= ATS_LOG_VERBOSE) { ats_log_write(ATS_LOG_VERBOSE, tag, LOG_FORMAT(V, format), ats_log_timestamp(), tag, ##__VA_ARGS__); } - -#define ATS_LOGD( tag, format, ... ) if (LOG_LOCAL_LEVEL >= ATS_LOG_DEBUG) { ats_log_write(ATS_LOG_DEBUG, tag, LOG_FORMAT(D, format), ats_log_timestamp(), tag, ##__VA_ARGS__); } - -#define ATS_LOGW( tag, format, ... ) if (LOG_LOCAL_LEVEL >= ATS_LOG_WARN) { ats_log_write(ATS_LOG_WARN, tag, LOG_FORMAT(W, format), ats_log_timestamp(), tag, ##__VA_ARGS__); } -#else -#define LOG_FORMAT(letter, format) LOG_COLOR_ ## letter #letter " %s: " format LOG_DEFAULT_COLOR "\r\n" -//#define LOG_FORMAT_RTK(letter, format) log_type[letter], "%s %s: " format LOG_DEFAULT_COLOR "\r\n" - - -#define LOG_FORMAT_RTK(letter, format) letter "%s %s: " format LOG_DEFAULT_COLOR "\r\n" //加上tag和mes字符串 -#define LOG_MES_TYPE(prio) log_type[prio] - -//#define ATS_LOG(tag, prio, format, ... ) if (LOG_LOCAL_LEVEL >= ATS_LOG_ERROR) { ats_log_write(ATS_LOG_ERROR, tag ,LOG_FORMAT_RTK(prio, format), LOG_MES_TYPE(prio),tag, ##__VA_ARGS__); } -//#define ATS_LOG(tag, prio, format, ... ) if (LOG_LOCAL_LEVEL >= ATS_LOG_ERROR) { ats_log_write(ATS_LOG_ERROR, tag ,LOG_FORMAT_RTK(LOG_COLOR_W, format), LOG_MES_TYPE(prio),tag, ##__VA_ARGS__); } -#define ATS_LOG(tag, prio, format, ... ) if (LOG_LOCAL_LEVEL >= ATS_LOG_ERROR) { RTK_LOG(prio, tag ,format, ##__VA_ARGS__); } - -#define ATS_LOGE( tag, format, ... ) if (LOG_LOCAL_LEVEL >= ATS_LOG_ERROR) { ats_log_write(ATS_LOG_ERROR, tag, LOG_FORMAT(E, format), tag, ##__VA_ARGS__); } -#define ATS_LOGV( tag, format, ... ) if (LOG_LOCAL_LEVEL >= ATS_LOG_VERBOSE) { ats_log_write(ATS_LOG_VERBOSE, tag, LOG_FORMAT(V, format), tag, ##__VA_ARGS__); } - -#define ATS_LOGD( tag, format, ... ) if (LOG_LOCAL_LEVEL >= ATS_LOG_DEBUG) { ats_log_write(ATS_LOG_DEBUG, tag, LOG_FORMAT(D, format), tag, ##__VA_ARGS__); } - -#define ATS_LOGW( tag, format, ... ) if (LOG_LOCAL_LEVEL >= ATS_LOG_WARN) { ats_log_write(ATS_LOG_WARN, tag, LOG_FORMAT(W, format), tag, ##__VA_ARGS__); } -#endif - - - - - - -void ats_log_level_set(ats_log_level_t log_level_set); -void RTK_LOG(ats_log_level_t level,const char *tag,const char *format, ...); -void RTK_LOG_BUFF(ats_log_level_t log_level,const char *tag, const void *buffer, uint16_t buff_len); -void RTK_LOG_HEXDUMP(ats_log_level_t log_level,const char *tag, const void *buffer, uint16_t buff_len); - -#ifdef __cplusplus -} -#endif - -#endif \ No newline at end of file diff --git a/Core/Logg/src/log.c b/Core/Logg/src/log.c deleted file mode 100644 index 2becf6a..0000000 --- a/Core/Logg/src/log.c +++ /dev/null @@ -1,178 +0,0 @@ -/******************************************************************************* -* File Name : log.c -* Author : Daich -* Revision : 1.0 -* Date : 19/09/2019 -* Description : log source file -* -* HISTORY*********************************************************************** -* 19/09/2019 | | Daich -* -*******************************************************************************/ -#include -#include -#include -#include -#include -#include -#include "stm32f4xx_hal.h" -#include "log.h" -#include "uart.h" - -#define APP_TAG "ACEINNA_OPENRTK" - - - -ats_log_level_t LOG_LEVEL = ATS_LOG_DEBUG; - -const char* log_type[7] = { - "none", - "error", - "warn", - "info", - "mes", - "dbg", - "test", -}; - - - -void ats_log_write(ats_log_level_t level, const char *tag, const char *format, ...) -{ - va_list arg; - va_start(arg, format); - vprintf(format, arg); - va_end(arg); -} -#define LOG_BUF_SIZE (1024) - -void ats_log_level_set(ats_log_level_t log_level_set) -{ - RTK_LOG(ATS_LOG_INFO,APP_TAG,"log level: %s",log_type[log_level_set]); - LOG_LEVEL = log_level_set; -} - - -void RTK_LOG(ats_log_level_t level,const char *tag,const char *format, ...) -{ - char buf[512]; - char mes_type[50]; - char* corlor_type = NULL; - char log_str[1024]; - if((level > ATS_LOG_DEBUG) || (level < ATS_LOG_ERROR) || (LOG_LEVEL < level)) - { - return; - } - switch(level) - { - case ATS_LOG_NONE: - break; - case ATS_LOG_ERROR: - strcpy(mes_type,"ERROR:"); - corlor_type = LOG_COLOR_E; - break; - case ATS_LOG_WARN: - strcpy(mes_type,"WARNING:"); - corlor_type = LOG_COLOR_W; - break; - case ATS_LOG_INFO: - strcpy(mes_type,"INFORMATION:"); - corlor_type = LOG_COLOR_I; - break; - case ATS_LOG_MES: - strcpy(mes_type,"MESSAGE:"); - corlor_type = LOG_COLOR_M; - break; - case ATS_LOG_DEBUG: - strcpy(mes_type,"DEBUG:"); - corlor_type = LOG_COLOR_D; - break; - case ATS_LOG_VERBOSE: - break; - default: - sprintf(mes_type,"MESSAGE:"); - corlor_type = LOG_COLOR_E; - break; - } - va_list arg; - va_start(arg, format); - vsnprintf(buf, LOG_BUF_SIZE, format, arg); - va_end(arg); - sprintf(log_str,"%s%s %s: %s %s\r\n",corlor_type,mes_type,tag,buf,LOG_DEFAULT_COLOR); - printf("%s",log_str); -} - - -uint32_t ats_log_timestamp() -{ - return 0; -} - -void RTK_LOG_BUFF(ats_log_level_t log_level,const char *tag, const void *buffer, uint16_t buff_len) -{ - if ( buff_len == 0 ) return; - char hex_buffer[3*BYTES_PER_LINE+1]; - const char *ptr_line; - int bytes_cur_line; - - do { - if ( buff_len > BYTES_PER_LINE ) { - bytes_cur_line = BYTES_PER_LINE; - } else { - bytes_cur_line = buff_len; - } - ptr_line = buffer; - for( int i = 0; i < bytes_cur_line; i ++ ) { - sprintf(hex_buffer + 3*i, "%02x ", ptr_line[i] ); - } - RTK_LOG(log_level, tag, "%s", hex_buffer); - buffer += bytes_cur_line; - buff_len -= bytes_cur_line; - } while( buff_len ); -} - - -void RTK_LOG_HEXDUMP(ats_log_level_t log_level,const char *tag, const void *buffer, uint16_t buff_len) -{ - - if ( buff_len == 0 ) return; - const char *ptr_line; - char hd_buffer[10+3+BYTES_PER_LINE*3+3+BYTES_PER_LINE+1+1]; - char *ptr_hd; - int bytes_cur_line; - - do { - if ( buff_len > BYTES_PER_LINE ) { - bytes_cur_line = BYTES_PER_LINE; - } else { - bytes_cur_line = buff_len; - } - ptr_line = buffer; - ptr_hd = hd_buffer; - ptr_hd += sprintf( ptr_hd, "%p ", buffer ); - for( int i = 0; i < BYTES_PER_LINE; i ++ ) { - if ( (i&7)==0 ) { - ptr_hd += sprintf( ptr_hd, " " ); - } - if ( i < bytes_cur_line ) { - ptr_hd += sprintf( ptr_hd, " %02x", ptr_line[i] ); - } else { - ptr_hd += sprintf( ptr_hd, " " ); - } - } - ptr_hd += sprintf( ptr_hd, " ||" ); - for( int i = 0; i < bytes_cur_line; i ++ ) { - if (isprint((int)ptr_line[i]) ) { - ptr_hd += sprintf( ptr_hd, "%c", ptr_line[i] ); - } else { - ptr_hd += sprintf( ptr_hd, "." ); - } - } - ptr_hd += sprintf( ptr_hd, "||" ); - - RTK_LOG(log_level, tag, "%s", hd_buffer); - buffer += bytes_cur_line; - buff_len -= bytes_cur_line; - } while( buff_len ); -} - diff --git a/Core/UARTComm/CommonMessages.c b/Core/UARTComm/CommonMessages.c deleted file mode 100644 index 0267761..0000000 --- a/Core/UARTComm/CommonMessages.c +++ /dev/null @@ -1,441 +0,0 @@ -/** *************************************************************************** - * @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. -* HISTORY*********************************************************************** -* 15/10/2019 | | Daich -* Description: solve the compile warning by adding calibrationAPI.h -*******************************************************************************/ - -#include -#include -#include -#include "algorithmAPI.h" -#include "CommonMessages.h" -#include "algorithm.h" -#include "configurationAPI.h" -#include "appVersion.h" -#include "platformAPI.h" -#include "timer.h" -#ifndef BAREMETAL_OS - #include "sensorsAPI.h" - #include "UserMessaging.h" - #include "calibrationAPI.h" -#else - #include "bare_osapi.h" -#endif - - -IMUDataStruct gIMU; - -/****************************************************************************** - * @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*)GetUnitVersion(); - uint8_t *rev = (uint8_t*)platformBuildInfo(); - unsigned int serialNum = GetUnitSerialNum(); - len = snprintf((char*)payload, 250, "%s %s %s SN:%u", PRODUCT_NAME_STRING, 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 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 = 0; - double accels[NUM_AXIS] = {0}; - double mags[NUM_AXIS] = {0}; - double rates[NUM_AXIS] = {0}; - - 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] = {0}; - real CorrRates_B[NUM_AXIS] = {0}; - double accels[NUM_AXIS] = {0}; - // 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] = {0}; - real CorrRates_B[NUM_AXIS] = {0}; - double accels[NUM_AXIS] = {0}; - - 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] = {0}; - double mags[NUM_AXIS] = {0}; - double rates[NUM_AXIS] = {0}; - double temp = 0; - scaled1_payload_t *pld = (scaled1_payload_t *)payload; - *payloadLen = sizeof(scaled1_payload_t); - - pld->dbTstmp = (double)imu_time.time + (float)imu_time.msec / 1000; - pld->temp_C = GetUnitTemp(); - GetAccelData_g(pld->accel_g); - GetRateData_radPerSec(pld->rate_dps); - - 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; - float data[NUM_AXIS] = {0}; - real EulerAngles[NUM_AXIS]; - - ekf1_payload_t *pld = (ekf1_payload_t *)payload; - - *payloadLen = sizeof(ekf1_payload_t); - pld->tstmp = gIMU.timerCntr; - // pld->dbTstmp = imu_time.time + imu_time.msec*0.0001;// 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 = gIMU.timerCntr; - // pld->dbTstmp = imu_time.time + imu_time.msec*0.0001;// seconds - - - EKF_GetAttitude_EA(rData); - pld->roll = (float)rData[ROLL]; - pld->pitch = (float)rData[PITCH]; - pld->yaw = (float)rData[YAW]; - - GetAccelData_g(fData); - for(int i = 0; i < NUM_AXIS; i++){ - pld->accels_g[i] = (float)fData[i]; - } - - EKF_GetEstimatedAccelBias(fData); - for(int i = 0; i < NUM_AXIS; i++){ - pld->accelBias[i] = fData[i]; - } - - GetRateData_degPerSec(fData); - for(int i = 0; i < NUM_AXIS; i++){ - pld->rates_dps[i] = (float)fData[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(fData); - for(int i = 0; i < NUM_AXIS; i++){ - pld->mags[i] = (float)fData[i]; - } - - EKF_GetEstimatedLLA(dData); - for(int i = 0; i < NUM_AXIS; i++){ - pld->pos[i] = (double)dData[i]; - } - - // // debug - // double *algoData_6 = (double *)(algoData_4); - // *algoData_6++ = (double)gEKFInput.lla[LAT]; - // *algoData_6++ = (double)gEKFInput.lla[LON]; - // *algoData_6++ = (double)gEKFInput.lla[ALT]; //(double)gEKFInput.trueCourse; // DW DEBUG - - uint8_t opMode, linAccelSw, turnSw; - EKF_GetOperationalMode(&opMode); - EKF_GetOperationalSwitches(&linAccelSw, &turnSw); - - pld->opMode = opMode; - pld->accelLinSwitch = linAccelSw; - pld->turnSwitch = turnSw; - - return TRUE; -} - - -/****************************************************************************** - * @name Fill_k1PacketPayload - 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 - ******************************************************************************/ -#ifdef TEST_RTK_TASK -BOOL Fill_k1PacketPayload(uint8_t *payload, uint8_t *payloadLen) -{ - *payloadLen = USR_OUT_RTK1_PAYLOAD_LEN; - // Output time as represented by gLeveler.timerCntr (uint32_t - // incremented at each call of the algorithm) - uint32_t *algoData_1 = (uint32_t *)(payload); - *algoData_1++ = gIMU.timerCntr; - - // Set the pointer of the algoData array to the payload - // uint32_t *algoData_2 = (uint32_t*)(algoData_1); - *algoData_1++ = gGPS.itow; - - // Set the pointer of the algoData array to the payload - double *algoData_2 = (double *)(algoData_1); - // double lla[NUM_AXIS]; - // EKF_GetEstimatedLLA(lla); - *algoData_2++ = gGPS.latitude; - *algoData_2++ = gGPS.longitude; - *algoData_2++ = gGPS.altitude; - - // *algoData_2++ = gGPS.base_lat; - // *algoData_2++ = gGPS.base_lon; - // *algoData_2++ = gGPS.base_hgt; - - // Set the pointer of the algoData array to the payload - float *algoData_3 = (float *)(algoData_2); - - *algoData_3++ = (float)gGPS.vNed[0]; - *algoData_3++ = (float)gGPS.vNed[1]; - *algoData_3++ = (float)gGPS.vNed[2]; - - *algoData_3++ = gGPS.HDOP; - *algoData_3++ = gGPS.GPSHorizAcc; - *algoData_3++ = gGPS.GPSVertAcc; - *algoData_3++ = gGPS.sol_age; - - float accels[NUM_AXIS]; - GetAccelData_g(accels); - *algoData_3++ = (float)accels[X_AXIS]; - *algoData_3++ = (float)accels[Y_AXIS]; - *algoData_3++ = (float)accels[Z_AXIS]; - - float rates[NUM_AXIS]; - GetRateData_degPerSec(rates); - *algoData_3++ = (float)rates[X_AXIS]; - *algoData_3++ = (float)rates[Y_AXIS]; - *algoData_3++ = (float)rates[Z_AXIS]; - - uint8_t *algoData_4 = (uint8_t *)(algoData_3); - *algoData_4++ = gGPS.gpsFixType; //gGPS.gnss_nav_quality; - *algoData_4++ = gGPS.num_gnss_psr; - *algoData_4++ = gGPS.num_gnss_adr; -} -#endif - - - -/****************************************************************************** - * @name Fill_posPacketPayload - 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_posPacketPayload(uint8_t *payload, uint8_t *payloadLen) -{ - return TRUE; -} - -/****************************************************************************** - * @name Fill_skyviewPacketPayload - 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_skyviewPacketPayload(uint8_t *payload, uint8_t *payloadLen) -{ - return true; -} diff --git a/Core/UARTComm/CommonMessages.h b/Core/UARTComm/CommonMessages.h deleted file mode 100644 index d172ea7..0000000 --- a/Core/UARTComm/CommonMessages.h +++ /dev/null @@ -1,164 +0,0 @@ -/** *************************************************************************** - * @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 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 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; - -// payload structure of alternative IMU data message -typedef struct { - double dbTstmp; - float accel_g[3]; - float rate_dps[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 rtk test data message -typedef struct { - -}rtk1_payload_t; - -// payload structure of standard pos data message -typedef struct { - uint32_t systemTime; - double timeOfWeek; - uint32_t positionMode; - double latitude; - double longitude; - double ellipsoidalHeight; - double mslHeight; - double positionRMS; - uint32_t velocityMode; - float velocityNorth; - float velocityEast; - float velocityDown; - float velocityRMS; - uint32_t insStatus; - uint32_t insPositionType; - float roll; - float pitch; - float heading; - float rollRMS; - float pitchRMS; - float headingRMS; -}pos_payload_t; - -// payload structure of standard skyview data message -typedef struct { - double timeOfWeek; - uint8_t satelliteId; - uint8_t systemId; - uint8_t antennaId; - uint8_t l1cn0; - uint8_t l2cn0; - float azimuth; - float elevation; -}skyview_payload_t; - - -#pragma pack() - -BOOL Fill_PingPacketPayload(uint8_t *payload, uint8_t *payloadLen); -BOOL Fill_VersionPacketPayload(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); -BOOL Fill_k1PacketPayload(uint8_t *payload, uint8_t *payloadLen); -BOOL Fill_posPacketPayload(uint8_t *payload, uint8_t *payloadLen); -BOOL Fill_snrPacketPayload(uint8_t *payload, uint8_t *payloadLen); -BOOL Fill_skyviewPacketPayload(uint8_t *payload, uint8_t *payloadLen); - -#endif diff --git a/Core/console/inc/shell.h b/Core/console/inc/shell.h deleted file mode 100644 index b204e96..0000000 --- a/Core/console/inc/shell.h +++ /dev/null @@ -1,34 +0,0 @@ -/******************************************************************************* -* File Name : shell.h -* Author : daich -* Revision : 1.0 -* Date : -* Description : shell.h -* -* HISTORY*********************************************************************** -* 10/12/2019 | | Daich -* Description: created -* HISTORY*********************************************************************** -* 16/12/2019 | | Daich -* Description: add marco SHELL_TASK -*******************************************************************************/ - -#ifndef _SHELL_H_ -#define _SHELL_H_ -#include "stdint.h" -#include "GlobalConstants.h" -#include "stdio.h" - - -#define SHELL_ENABLE false -#define SHELL_TASK false -#define SHELL_RX_MAX (256+32) -#define SHELL_TX_MAX (512) - -bool is_cmd_right(void * buffer,void * cmd); -void shell_service(void); -void debug_uart_handle(void); -void ConsoleTask(void const *argument); -void parse_debug_cmd(); -int bt_uart_parse(uint8_t* bt_buff); -#endif \ No newline at end of file diff --git a/Core/console/src/shell.c b/Core/console/src/shell.c deleted file mode 100644 index 304be17..0000000 --- a/Core/console/src/shell.c +++ /dev/null @@ -1,562 +0,0 @@ -/******************************************************************************* -* File Name : shell.c -* Author : daich -* Revision : 1.0 -* Date : -* Description : shell.c -* -* HISTORY*********************************************************************** -* 10/12/2019 | | Daich -* Description: created -* 16/12/2019 | | Daich -* Description: add json cmd parse -* 06/01/2020 | | Daich -* Description: add parse_json_cmd function to parse mobile bt app cmd -*******************************************************************************/ -#include -#include -#include "shell.h" -#include "uart.h" -#include "log.h" -#include "cJSON.h" -#include "driver.h" -#include "configuration.h" -#include "uart.h" -#include "json_parse.h" -#ifndef BAREMETAL_OS - #include "UserConfiguration.h" -#endif -#define LINUX_CONSOLE -#define BT_CMD_NUM 2 -#define CMD_MAX_LEN 20 -#define DEBUG_CMD_NUM 2 - - -volatile uint16_t shell_rx_rdy = 0; -volatile uint16_t shell_rdy_flag = 0; -volatile uint8_t shell_rx_buff[SHELL_RX_MAX+1]="\0"; -cJSON* rtk_json = NULL; - - -static volatile uint16_t shell_rx_index = 0; - -static volatile uint8_t shell_tx_buff[SHELL_TX_MAX+1]="\0"; -static volatile uint16_t shell_tx_size = 0; -static volatile uint16_t shell_tx_index = 0; -static int bt_cmd_index = -1; - -#ifndef BAREMETAL_OS -extern osSemaphoreId bt_uart_sem; -extern osSemaphoreId debug_uart_sem; -extern osMutexId bt_mutex; -extern CCMRAM UserConfigurationStruct gUserConfiguration; -#endif - -static void bt_app_json_parse(cJSON* root); - -enum BT_CMD_TYPE -{ - RTK_MES_CMD = 0, - RTK_SN_CMD, - BT_CMD_MAX -}; -enum DEBUG_CMD_TYPE -{ - BT_NAME_CMD_DEBUG = 0, - RTK_MES_CMD_DEBUG, - DEBUG_CMD_MAX -}; - -char debug_uart_cmd[DEBUG_CMD_MAX][CMD_MAX_LEN] = -{ - "ls bt", - "get rtk message" -}; -char esp_bt_cmd[BT_CMD_MAX][CMD_MAX_LEN] = -{ - "get rtk message", - "get rtk sn" -}; -extern ConfigurationStruct gConfiguration; -void debug_uart_handle(void) //TODO: -{ - uint8_t dataBuffer[512]; - int bytes_in_buffer = 0; - do - { - bytes_in_buffer = uart_read_bytes(UART_DEBUG,dataBuffer, 1,0); - if(bytes_in_buffer > 0) - { - if( (shell_rx_index > 0) && ('\b' == dataBuffer[0])) //\b - { - shell_rx_index = shell_rx_index <2? 0:shell_rx_index - 1; - shell_rx_buff[shell_rx_index] = '\0'; -#ifdef DEVICE_DEBUG - uart_write_bytes(UART_DEBUG,(const char*)dataBuffer,1,1); - uart_write_bytes(UART_DEBUG,(const char*)"\033[K",strlen("\033[K"),1); //set back -#endif - } - else if( shell_rx_index < SHELL_RX_MAX) - { - shell_rx_buff[shell_rx_index] = dataBuffer[0]; - shell_rx_index++; -#ifdef DEVICE_DEBUG - uart_write_bytes(UART_DEBUG,(const char*)dataBuffer,1,1); -#endif - } - else - { - shell_rx_index = 0; - } -#ifdef LINUX_CONSOLE - if( (shell_rx_index >= 1) && ('\r' == shell_rx_buff[shell_rx_index-1])) //\r - { - shell_rx_buff[shell_rx_index + 1] = '\0'; - shell_rx_rdy = shell_rx_index; - shell_rx_index = 0; - shell_rdy_flag = 1; - } -#if 0 - if( (shell_rx_index > 0) && ('\b' == shell_rx_buff[shell_rx_index-1])) //\b - { - shell_rx_index = shell_rx_index <2? 0:shell_rx_index - 1; - uart_write_bytes(UART_DEBUG,(const char*)"\033[K",strlen("\033[K"),1); - } -#endif -#else - if( (shell_rx_index >=2) && ('\r' == shell_rx_buff[shell_rx_index-2]) && - ('\n' == shell_rx_buff[shell_rx_index-1]) ) - { - shell_rx_rdy = shell_rx_index; - shell_rx_index = 0; - } - else if( (shell_rx_index > 0) && ('\b' == shell_rx_buff[shell_rx_index-1]) ) - { - shell_rx_index = shell_rx_index <2? 0:shell_rx_index-2; - printf(" \b"); - } -#endif - } - }while(bytes_in_buffer); -} - -void shell_service(void) -{ - uint8_t* ptSrc; - if(shell_rdy_flag == 1) - { - shell_rdy_flag = 0; -#ifdef LINUX_CONSOLE - if(1 > shell_rx_rdy) - { - shell_rx_buff[0] = 0; - return; - } -#else - if(2 > shell_rx_rdy) - { - shell_rx_buff[0] = 0; - return; - } -#endif -#ifdef LINUX_CONSOLE - else if('\r' == shell_rx_buff[shell_rx_rdy-1]) -#else - else if( ('\r' == shell_rx_buff[shell_rx_rdy-2]) && ('\n' == shell_rx_buff[shell_rx_rdy-1]) ) -#endif - { - ptSrc = (uint8_t *)shell_rx_buff; -#ifdef DEVICE_DEBUG - printf("\r\n"); //new line -#endif -#ifdef LINUX_CONSOLE - if(1 == shell_rx_rdy) -#else - if(2 == shell_rx_rdy) -#endif - { -#ifdef DEVICE_DEBUG - printf("Shell:OK!\r\n"); -#endif - } - else if(is_cmd_right(ptSrc,debug_uart_cmd[BT_NAME_CMD_DEBUG])) - { - //uint8_t bt_buff[GPS_BUFF_SIZE]; - shell_rx_buff[0] = 0; - shell_rx_rdy = 0; - bt_cmd_index = BT_NAME_CMD_DEBUG; -#if 0 - char* bt_name = get_rtk_json_item_value(rtk_json,"bt_name"); - - printf("bt_name = %s\n",bt_name); -#endif -#if 1 -#ifndef BAREMETAL_OS - osMutexWait(bt_mutex,portMAX_DELAY); -#endif - uart_write_bytes(UART_BT, (char *)"esp32_config", sizeof("esp32_config"), 1); -#if 0 - OS_Delay(200); - int BtLen = uart_read_bytes(UART_BT, bt_buff, GPS_BUFF_SIZE, 0); - osMutexRelease(bt_mutex); - if(BtLen > 0) - { - cJSON *root; - root = cJSON_Parse((const char *)bt_buff); - if (root != NULL) - { - cJSON *esp_config,*item_btname,*item_bt_mac; - esp_config = cJSON_GetObjectItem(root,"esp32 config"); - item_btname = cJSON_GetObjectItem(esp_config, "bt_name"); - item_bt_mac = cJSON_GetObjectItem(esp_config, "esp32_serial"); - RTK_LOG(ATS_LOG_MES,"ESP32 BT NAME","%s",item_btname->valuestring); - RTK_LOG(ATS_LOG_MES,"ESP32 BT MAC","%s",item_bt_mac->valuestring); - free(esp_config); - free(item_btname); - free(item_bt_mac); - } - free(root); - } - else - { - RTK_LOG(ATS_LOG_MES,"console","%s","time out"); - } -#endif -#endif - } - else - { -#ifdef DEVICE_DEBUG - printf("\r\nAT: Cmd Error:\r\n"); -#endif - } - shell_rx_buff[0] = 0; - shell_rx_rdy = 0; - } - } -} - - -char get_strtrim(char *pstr,char *dest,uint32_t len) -{ - char cmp[len + 1]; - int j = 0; - for(int i = 0;i < len;i++) - { - if((*(pstr + i) != ' ') && (*(pstr + i) != '\r') && (*(pstr + i) != '\n')) - { - cmp[j++] = *(pstr + i); - } - } - cmp[j] = '\0'; - strcpy((char *)dest,(const char *)cmp); - int len_now = strlen((const char*)dest); - int count = len - len_now; - return count; -} - -char strtrimall(unsigned char *pstr,uint32_t len) //p ' ' q vallue -{ - char cmp[len + 1]; - int j = 0; - for(int i = 0;i < len;i++) - { - if((*(pstr + i) != ' ') && (*(pstr + i) != '\r') && (*(pstr + i) != '\n')) - { - cmp[j++] = *(pstr + i); - } - } - cmp[j] = '\0'; - strcpy((char *)pstr,(const char *)cmp); - int len_now = strlen((const char*)pstr); - int count = len - len_now; - return count; -} - - -bool is_cmd_right(void * buffer,void * cmd) -{ - strtrimall(buffer,strlen(buffer)); - strtrimall(cmd,strlen(cmd)); - if(strcmp((char*)buffer,(char*)cmd) == 0) - { - return true; - } - return false; -} - -void ConsoleTask(void const *argument) -{ - while (1) - { - debug_uart_handle(); - shell_service(); - OS_Delay(20); - } -} - -int get_bt_cmd_index(void* cmd) -{ - static int is_get_rtk_mes_rev_flag = 0; - char cmd_trim[strlen(cmd) + 1]; - char bt_cmd_trim[CMD_MAX_LEN] = {0}; - get_strtrim(cmd,cmd_trim,strlen(cmd)); - for(int i = 0;i < BT_CMD_NUM;i++) - { - get_strtrim(esp_bt_cmd[i],bt_cmd_trim,strlen(esp_bt_cmd[i])); - //if(strcmp(bt_cmd_trim,cmd_trim) == 0) - if(((is_get_rtk_mes_rev_flag == 0)&&(strstr(cmd_trim,bt_cmd_trim) != NULL)) \ - || ((is_get_rtk_mes_rev_flag == 1)&&(strcmp(cmd_trim,bt_cmd_trim) == 0))) - { - if((i == RTK_MES_CMD)&&(is_get_rtk_mes_rev_flag == 0)) - { - is_get_rtk_mes_rev_flag = 1; - } - return i; - } - memset(bt_cmd_trim,0,sizeof(bt_cmd_trim)); - } - return -1; -} - -int bt_uart_parse(uint8_t* bt_buff) //TODO: -{ - static int is_first_rev_bt_mes = 0; - if (bt_buff != NULL) - { - if(is_first_rev_bt_mes == 0) - { - uart_write_bytes(UART_BT,"openrtk start run\r\n",strlen("openrtk start run\r\n"),1); - is_first_rev_bt_mes = 1; - return BT_CMD; - } - cJSON *root; - root = cJSON_Parse((const char *)bt_buff); - if (root != NULL) - { - bt_app_json_parse(root); - switch(bt_cmd_index) - { - case BT_NAME_CMD_DEBUG: - { - get_rtk_json_item_value(root,"bt_name"); -#ifdef DEVICE_DEBUG - printf("bt_name = %s\r\n",bt_name); -#endif - cJSON_Delete(root); - return RTK_JSON; - break; - } - default: - cJSON_Delete(root); - return RTK_JSON; - break; - } - } -#if 0 - cJSON *item_packet_type; - item_packet_type = cJSON_GetObjectItem(root,"UserPacketType"); - if(item_packet_type != NULL) - { - RTK_LOG(ATS_LOG_MES,"UserPacketType","%s",item_packet_type->valuestring); - int packet_type = 0; - sscanf(item_packet_type->valuestring, "%x", &packet_type); - gConfiguration.packetCode = packet_type; - printf("gConfiguration.packetCode = %x\r\n",gConfiguration.packetCode); - } - free(item_packet_type); - return RTK_JSON; -#endif - else - { - int cmd_type = get_bt_cmd_index(bt_buff); - if(cmd_type == -1) - { - return BT_BASE; - } - switch(cmd_type) - { - case RTK_SN_CMD: - break; - case RTK_MES_CMD: - if(rtk_json == NULL) - { - create_json_object(&rtk_json); - } - send_rtk_json_message(rtk_json); - break; - default: - break; - } - return BT_CMD; - } - - } - return BT_BASE; -} - -void parse_debug_cmd() -{ - //res = osSemaphoreWait(debug_uart_sem, 0); - //if(osSemaphoreWait(p_uart_obj[UART_DEBUG]->uart_idle_sem,0) == osOK) - if(uart_sem_wait(UART_DEBUG,0) == RTK_SEM_OK) - { - debug_uart_handle(); - shell_service(); - } -} - -#if 0 -int get_bt_cmd_index() -{ - return bt_cmd_index; -} -#endif - -static void bt_app_json_parse(cJSON* root) -{ - -#ifndef BAREMETAL_OS - cJSON *leverArmBx, *leverArmBy, *leverArmBz; - cJSON *pointOfInterestBx, *pointOfInterestBy, *pointOfInterestBz; - cJSON *rotationRbvx, *rotationRbvy, *rotationRbvz; - cJSON *packetCode; - packetCode = cJSON_GetObjectItem(root,"UserPacketType"); - leverArmBx = cJSON_GetObjectItem(root, "leverArmBx"); - leverArmBy = cJSON_GetObjectItem(root, "leverArmBy"); - leverArmBz = cJSON_GetObjectItem(root, "leverArmBz"); - pointOfInterestBx = cJSON_GetObjectItem(root, "pointOfInterestBx"); - pointOfInterestBy = cJSON_GetObjectItem(root, "pointOfInterestBy"); - pointOfInterestBz = cJSON_GetObjectItem(root, "pointOfInterestBz"); - rotationRbvx = cJSON_GetObjectItem(root, "rotationRbvx"); - rotationRbvy = cJSON_GetObjectItem(root, "rotationRbvy"); - rotationRbvz = cJSON_GetObjectItem(root, "rotationRbvz"); - if(packetCode != NULL) - { - gConfiguration.packetCode = ((uint16_t)*(packetCode->valuestring) << 8) + *(packetCode->valuestring + 1); - uint16_t type = ((uint16_t)*(packetCode->valuestring + 1) << 8) + *(packetCode->valuestring); - setUserPacketType((uint8_t *)&type,TRUE); - strcpy((char*)gUserConfiguration.userPacketType, (const char*)packetCode->valuestring); - SaveUserConfig(); - } - if(leverArmBx != NULL) - { - if(leverArmBx->type == cJSON_String) - { - gUserConfiguration.leverArmBx = (strlen(leverArmBx->valuestring) == 0)? \ - gUserConfiguration.leverArmBx : atof(leverArmBx->valuestring); - } - else if(leverArmBx->type == cJSON_Number) - { - gUserConfiguration.leverArmBx = (leverArmBx->valuedouble); - } - } - if(leverArmBy != NULL) - { - if(leverArmBy->type == cJSON_String) - { - gUserConfiguration.leverArmBy = (strlen(leverArmBy->valuestring) == 0)? \ - gUserConfiguration.leverArmBy : atof(leverArmBy->valuestring); - } - else if(leverArmBy->type == cJSON_Number) - { - gUserConfiguration.leverArmBy = leverArmBy->valuedouble; - } - } - if(leverArmBz != NULL) - { - if(leverArmBz->type == cJSON_String) - { - gUserConfiguration.leverArmBz = (strlen(leverArmBz->valuestring) == 0)? \ - gUserConfiguration.leverArmBz : atof(leverArmBz->valuestring); - } - else if(leverArmBz->type == cJSON_Number) - { - gUserConfiguration.leverArmBz = leverArmBz->valuedouble; - } - } - if(pointOfInterestBx != NULL) - { - if(pointOfInterestBx->type == cJSON_String) - { - gUserConfiguration.pointOfInterestBx = (strlen(pointOfInterestBx->valuestring) == 0)? \ - gUserConfiguration.pointOfInterestBx : atof(pointOfInterestBx->valuestring); - } - else if(pointOfInterestBx->type == cJSON_Number) - { - gUserConfiguration.pointOfInterestBx = pointOfInterestBx->valuedouble; - } - } - if(pointOfInterestBy != NULL) - { - if(pointOfInterestBy->type == cJSON_String) - { - gUserConfiguration.pointOfInterestBy = (strlen(pointOfInterestBy->valuestring) == 0)? \ - gUserConfiguration.pointOfInterestBy : atof(pointOfInterestBy->valuestring); - } - else if(pointOfInterestBy->type == cJSON_Number) - { - gUserConfiguration.pointOfInterestBy = pointOfInterestBy->valuedouble; - } - } - if(pointOfInterestBz != NULL) - { - if(pointOfInterestBz->type == cJSON_String) - { - gUserConfiguration.pointOfInterestBz = (strlen(pointOfInterestBz->valuestring) == 0)? \ - gUserConfiguration.pointOfInterestBz : atof(pointOfInterestBz->valuestring); - } - else if(pointOfInterestBz->type == cJSON_Number) - { - gUserConfiguration.pointOfInterestBz = pointOfInterestBz->valuedouble; - } - } - if(rotationRbvx != NULL) - { - if(rotationRbvx->type == cJSON_String) - { - gUserConfiguration.rotationRbvx = (strlen(rotationRbvx->valuestring) == 0)? \ - gUserConfiguration.rotationRbvx : atof(rotationRbvx->valuestring); - } - else if(rotationRbvx->type == cJSON_Number) - { - gUserConfiguration.rotationRbvx = rotationRbvx->valuedouble; - } - } - if(rotationRbvy != NULL) - { - if(rotationRbvy->type == cJSON_String) - { - gUserConfiguration.rotationRbvy = (strlen(rotationRbvy->valuestring) == 0)? \ - gUserConfiguration.rotationRbvy : atof(rotationRbvy->valuestring); - } - else if(rotationRbvy->type == cJSON_Number) - { - gUserConfiguration.rotationRbvy = rotationRbvy->valuedouble; - } - } - if(rotationRbvz != NULL) - { - if(rotationRbvz->type == cJSON_String) - { - gUserConfiguration.rotationRbvz = (strlen(rotationRbvz->valuestring) == 0)? \ - gUserConfiguration.rotationRbvz : atof(rotationRbvz->valuestring); - } - else if(rotationRbvz->type == cJSON_Number) - { - gUserConfiguration.rotationRbvz = rotationRbvz->valuedouble; - } - } - if((leverArmBx != NULL) || (leverArmBy != NULL) || (leverArmBz != NULL) || \ - (pointOfInterestBx != NULL) || (pointOfInterestBy != NULL) || (pointOfInterestBz != NULL) || \ - (rotationRbvx != NULL) || (rotationRbvy != NULL) || (rotationRbvz != NULL)) - { - SaveUserConfig(); - int size = sizeof(gUserConfiguration); - EEPROM_LoadUserConfig((void*)&gUserConfiguration, &size); - uart_write_bytes(UART_BT,"##para received!##",strlen("##para received!##"),1); - send_rtk_json_to_esp32(); - } - -#endif -} \ No newline at end of file diff --git a/Core/library.json b/Core/library.json deleted file mode 100644 index 956f5ae..0000000 --- a/Core/library.json +++ /dev/null @@ -1,24 +0,0 @@ -{ - "name": "Core", - "version": "1.0.4", - "description": "Algorithm, utility and GNSS reciever PVT decoding libraries", - "build": { - "flags": [ - "-I Algorithm/include", - "-I Algorithm", - "-I Buffer/include", - "-I Common/include", - "-I GPS", - "-I GPS/include", - "-I Math/include", - "-I Logg/inc", - "-I Cjson/inc", - "-I DmaMemcpy/inc", - "-I UARTComm", - "-I console/inc" , - "-I ." - ], - "libArchive": true, - "platforms": "aceinna_imu" - } - } diff --git a/FreeRTOS/src/cmsis_os.c b/FreeRTOS/src/cmsis_os.c index 98e1ff6..d8cf925 100644 --- a/FreeRTOS/src/cmsis_os.c +++ b/FreeRTOS/src/cmsis_os.c @@ -1405,7 +1405,6 @@ osStatus osMailFree (osMailQId queue_id, void *mail) * @param none. * @retval none. */ -#ifndef BAREMETAL_OS void osSystickHandler(void) { @@ -1419,7 +1418,6 @@ void osSystickHandler(void) } #endif /* INCLUDE_xTaskGetSchedulerState */ } -#endif #if ( INCLUDE_eTaskGetState == 1 ) /** * @brief Obtain the state of any thread. diff --git a/FreeRTOS_M4/library.json b/FreeRTOS_M4/library.json deleted file mode 100644 index fdd00bd..0000000 --- a/FreeRTOS_M4/library.json +++ /dev/null @@ -1,12 +0,0 @@ -{ - "name": "FreeRTOS_M4", - "version": "1.0.4", - "description": "FreeRTOS port functions for M4", - "build": { - "flags": [ - "-I include" - ], - "libArchive": true, - "platforms": "aceinna_imu" - } - } diff --git a/LWIP/lwip-1.4.1/src/netif/ethernetif.c b/LWIP/lwip-1.4.1/src/netif/ethernetif.c index 3d81548..cdc5c94 100644 --- a/LWIP/lwip-1.4.1/src/netif/ethernetif.c +++ b/LWIP/lwip-1.4.1/src/netif/ethernetif.c @@ -4,7 +4,7 @@ #include "cmsis_os.h" #include "osapi.h" #include "string.h" -#include "UserConfiguration.h" +#include "user_config.h" #include "boardDefinition.h" // Declare the ETH structure diff --git a/LWIP/lwip_app/NtripClient/inc/ntripClient.h b/LWIP/lwip_app/NtripClient/inc/ntripClient.h index 51a9685..05626e1 100644 --- a/LWIP/lwip_app/NtripClient/inc/ntripClient.h +++ b/LWIP/lwip_app/NtripClient/inc/ntripClient.h @@ -8,7 +8,7 @@ #include "LwipComm.h" #include "lwip/sys.h" #include "lwip/api.h" -#include "RingBuffer.h" +#include "utils.h" // ntrip buffer size #define NTRIP_TX_BUFSIZE 2000 @@ -39,8 +39,8 @@ extern uint8_t NTRIP_client_start; extern uint8_t NTRIP_client_state; extern uint8_t NTRIP_base_stream; -extern FIFO_Type ntrip_tx_fifo; -extern FIFO_Type ntrip_rx_fifo; +extern fifo_type ntrip_tx_fifo; +extern fifo_type ntrip_rx_fifo; extern uint8_t ntripTxBuf[NTRIP_TX_BUFSIZE]; extern uint8_t ntripRxBuf[NTRIP_RX_BUFSIZE]; extern uint16_t ntripTxLen; @@ -50,11 +50,11 @@ extern uint32_t ntripStreamCount; #define NTRIP_STREAM_CONNECTED_MAX_COUNT 20 void Fill_EthLocalRTKPacketPayload(uint8_t* payload, uint16_t *payloadLen); -void Fill_EthCloudRTKPacketPayload(uint8_t* payload, uint16_t *payloadLen); -err_t NTRIP_GET_Data(uint8_t *rxBuf, uint16_t *rxLen); -err_t NTRIP_Recieve_Data(FIFO_Type* fifo); -err_t NTRIP_Write_Data(uint8_t *txBuf, uint16_t txLen, uint8_t apiflags); +err_t ntrip_read_data(uint8_t *rxBuf, uint16_t *rxLen); +err_t ntrip_push_rx_data(fifo_type* fifo); +err_t ntrip_write_data(uint8_t *txBuf, uint16_t txLen, uint8_t apiflags); +uint8_t ntrip_push_tx_data(uint8_t* buf, uint16_t len); void ntrip_link_down(void); void NTRIP_interface(void); diff --git a/LWIP/lwip_app/NtripClient/src/ntripClient.c b/LWIP/lwip_app/NtripClient/src/ntripClient.c index 8194aac..b93e897 100644 --- a/LWIP/lwip_app/NtripClient/src/ntripClient.c +++ b/LWIP/lwip_app/NtripClient/src/ntripClient.c @@ -1,9 +1,10 @@ -#include "ntripClient.h" #include + +#include "ntripClient.h" #include "base64.h" #include "stm32f4xx_hal.h" #include "osapi.h" -#include "UserConfiguration.h" +#include "user_config.h" #include "calibrationAPI.h" #include "platformAPI.h" @@ -15,8 +16,8 @@ uint8_t NTRIP_client_state = NTRIP_STATE_OFF; uint8_t NTRIP_base_stream = BSAE_ON; // ntrip buf -FIFO_Type ntrip_tx_fifo; -FIFO_Type ntrip_rx_fifo; +fifo_type ntrip_tx_fifo; +fifo_type ntrip_rx_fifo; CCMRAM uint8_t ntripTxBuf[NTRIP_TX_BUFSIZE]; CCMRAM uint8_t ntripRxBuf[NTRIP_RX_BUFSIZE]; uint16_t ntripTxLen = 0; @@ -74,41 +75,13 @@ void Fill_EthLocalRTKPacketPayload(uint8_t *payload, uint16_t *payloadLen) } /** *************************************************************************** - * @name Fill_EthCloudRTKPacketPayload() - * @brief fill Cloud RTK Request - * @param *payload point to buffer - * *payloadLen point to buffer length - * @retval N/A - ******************************************************************************/ -void Fill_EthCloudRTKPacketPayload(uint8_t *payload, uint16_t *payloadLen) -{ - uint8_t sn[15]; - - strcpy((char *)payload, "SOURCE "); - strcat((char *)payload, (const char *)gUserConfiguration.password); - strcat((char *)payload, " "); - strcat((char *)payload, (const char *)gUserConfiguration.mountPoint); - strcat((char *)payload, "\r\n"); - - strcat((char *)payload, "Aceinna-Sn:"); - sprintf((char *)sn, "%ld", GetUnitSerialNum()); - strcat((char *)payload, (const char *)sn); - strcat((char *)payload, "\r\n"); - - strcat((char *)payload, "Source-Agent: NTRIP Aceinna/0.1\r\n\r\n"); - - *payloadLen = strlen((const char *)payload); -} - - -/** *************************************************************************** - * @name NTRIP_GET_Data() + * @name ntrip_read_data() * @brief ntrip client revieve function * @param *rxBuf point to ntrip client recieve buffer * *rxLen point to buffer length * @retval success(ERR_OK) fail(other) ******************************************************************************/ -err_t NTRIP_GET_Data(uint8_t *rxBuf, uint16_t *rxLen) +err_t ntrip_read_data(uint8_t *rxBuf, uint16_t *rxLen) { struct netbuf *rxNetbuf; uint16_t len = 0; @@ -137,7 +110,7 @@ err_t NTRIP_GET_Data(uint8_t *rxBuf, uint16_t *rxLen) if (ERR_IS_FATAL(err)) { #ifdef DEVICE_DEBUG - printf("NTRIP_GET_Data fail %d\r\n", err); + printf("ntrip_read_data fail %d\r\n", err); #endif NTRIP_client_state = NTRIP_STATE_CONNECT; } @@ -145,7 +118,13 @@ err_t NTRIP_GET_Data(uint8_t *rxBuf, uint16_t *rxLen) return err; } -err_t NTRIP_Recieve_Data(FIFO_Type* fifo) +/** *************************************************************************** + * @name ntrip_push_rx_data() + * @brief ntrip client revieve data and push to rx fifo + * @param *fifo point to rx fifo + * @retval success(ERR_OK) fail(other) + ******************************************************************************/ +err_t ntrip_push_rx_data(fifo_type* fifo) { struct netbuf *rxNetbuf; struct pbuf *q; @@ -158,18 +137,18 @@ err_t NTRIP_Recieve_Data(FIFO_Type* fifo) taskENTER_CRITICAL(); for (q = rxNetbuf->p; q != NULL; q = q->next) { - FifoPush(fifo, q->payload, q->len); + fifo_push(fifo, q->payload, q->len); len += q->len; } taskEXIT_CRITICAL(); - //printf("NTRIP_Recieve_Data=%d\r\n", len); + //printf("ntrip_push_rx_data=%d\r\n", len); } netbuf_delete(rxNetbuf); if (ERR_IS_FATAL(err)) { #ifdef DEVICE_DEBUG - printf("NTRIP_Recieve_Data fail %d\r\n", err); + printf("ntrip_push_rx_data fail %d\r\n", err); #endif NTRIP_client_state = NTRIP_STATE_CONNECT; } @@ -178,14 +157,14 @@ err_t NTRIP_Recieve_Data(FIFO_Type* fifo) } /** *************************************************************************** - * @name NTRIP_Write_Data + * @name ntrip_write_data * @brief ntrip client write function * @param *txBuf point to ntrip client send buffer * [in] txLen buffer length * [in] apiflags write flag * @retval success(ERR_OK) fail(other) ******************************************************************************/ -err_t NTRIP_Write_Data(uint8_t *txBuf, uint16_t txLen, uint8_t apiflags) +err_t ntrip_write_data(uint8_t *txBuf, uint16_t txLen, uint8_t apiflags) { err_t err; @@ -193,7 +172,7 @@ err_t NTRIP_Write_Data(uint8_t *txBuf, uint16_t txLen, uint8_t apiflags) if (ERR_IS_FATAL(err)) { #ifdef DEVICE_DEBUG - printf("NTRIP_Write_Data fail %d\r\n", err); + printf("ntrip_write_data fail %d\r\n", err); #endif if (NTRIP_client_state == NTRIP_STATE_REQUEST || NTRIP_client_state == NTRIP_STATE_INTERACTIVE) { @@ -204,6 +183,23 @@ err_t NTRIP_Write_Data(uint8_t *txBuf, uint16_t txLen, uint8_t apiflags) return err; } +/** *************************************************************************** + * @name ntrip_push_tx_data + * @brief push data to tx fifo + * @param *buf point to send data buffer + * len send length + * @retval success(1) fail(0) + ******************************************************************************/ +uint8_t ntrip_push_tx_data(uint8_t* buf, uint16_t len) +{ + if (NTRIP_client_start == NTRIP_START_ON && NTRIP_client_state == NTRIP_STATE_INTERACTIVE) + { + fifo_push(&ntrip_tx_fifo, (uint8_t*)buf, len); + return 1; + } + return 0; +} + void ntrip_link_down(void) { if (NTRIP_client_state >= NTRIP_STATE_CONNECT && NTRIP_client_state <= NTRIP_STATE_INTERACTIVE) @@ -292,21 +288,14 @@ void NTRIP_interface(void) printf("ntrip:request...\r\n"); #endif OS_Delay(100); - if (gUserConfiguration.rtkType == LocalRTK) - { - Fill_EthLocalRTKPacketPayload(ntripTxBuf, &txLen); - } - else if (gUserConfiguration.rtkType == CloudRTK) - { - Fill_EthCloudRTKPacketPayload(ntripTxBuf, &txLen); - } + Fill_EthLocalRTKPacketPayload(ntripTxBuf, &txLen); - err = NTRIP_Write_Data(ntripTxBuf, txLen, NETCONN_COPY); + err = ntrip_write_data(ntripTxBuf, txLen, NETCONN_COPY); if (err == ERR_OK) { OS_Delay(1000); uint16_t rxLen = 0; - err = NTRIP_GET_Data(ntripRxBuf, &rxLen); + err = ntrip_read_data(ntripRxBuf, &rxLen); if (err == ERR_OK) { ntripRxBuf[rxLen] = 0; @@ -323,30 +312,12 @@ void NTRIP_interface(void) break; case NTRIP_STATE_INTERACTIVE: - if (gUserConfiguration.rtkType == LocalRTK) // LocalRTK - { - NTRIP_Recieve_Data(&ntrip_rx_fifo); + ntrip_push_rx_data(&ntrip_rx_fifo); - txLen = FifoGet(&ntrip_tx_fifo, txBuf, 512); - if (txLen) - { - NTRIP_Write_Data(txBuf, txLen, NETCONN_NOFLAG); - } - } - else // CloudRTK + txLen = fifo_get(&ntrip_tx_fifo, txBuf, 512); + if (txLen) { - err = NTRIP_GET_Data(ntripRxBuf, &ntripRxLen); - if (err == ERR_OK) - { - if (ntripRxLen) - { - ntripRxBuf[ntripRxLen] = 0; -#ifdef DEVICE_DEBUG - printf("%s", ntripRxBuf); //print #GPGGA -#endif - ntripRxLen = 0; - } - } + ntrip_write_data(txBuf, txLen, NETCONN_NOFLAG); } break; diff --git a/LWIP/lwip_app/user/inc/LwipComm.h b/LWIP/lwip_app/user/inc/LwipComm.h index 84efa42..f1d064d 100644 --- a/LWIP/lwip_app/user/inc/LwipComm.h +++ b/LWIP/lwip_app/user/inc/LwipComm.h @@ -4,7 +4,7 @@ #include #include "lwipopts.h" #include "lwip/ip_addr.h" -#include "UserConfiguration.h" +#include "user_config.h" /*Static IP ADDRESS*/ diff --git a/LWIP/lwip_app/user/src/LwipComm.c b/LWIP/lwip_app/user/src/LwipComm.c index 1aed8f2..74b026e 100644 --- a/LWIP/lwip_app/user/src/LwipComm.c +++ b/LWIP/lwip_app/user/src/LwipComm.c @@ -13,13 +13,12 @@ #include "ethernetif.h" #include "stm32f4xx_hal.h" #include "osapi.h" -#include "osresources.h" #include #include #include "httpd.h" #include "netbios.h" #include "ntripClient.h" -#include "UserConfiguration.h" +#include "user_config.h" #include "cmsis_os.h" /* network interface structure */ diff --git a/LWIP/lwip_app/webserver/src/httpd_handler.c b/LWIP/lwip_app/webserver/src/httpd_handler.c index 275add7..ea9f2ae 100644 --- a/LWIP/lwip_app/webserver/src/httpd_handler.c +++ b/LWIP/lwip_app/webserver/src/httpd_handler.c @@ -1,22 +1,16 @@ +#include +#include + #include "httpd.h" #include "lwip/tcp.h" #include "fs.h" #include "LwipComm.h" -#include "appVersion.h" -#include -#include +#include "app_version.h" #include "stm32f4xx_hal.h" #include "ntripClient.h" - #include "calibrationAPI.h" #include "platformAPI.h" -#include "UserConfiguration.h" - - -const char radioRTKType[2][15] = { - "radioLocalRTK", - "radioCloudRTK", -}; +#include "user_config.h" const char radioEthMode[2][15] = { "radioDhcp", @@ -283,22 +277,13 @@ const char *NTRIP_CONFIG_CGI_Handler(int iIndex, int iNumParams, char *pcParam[] const char *USER_CONFIG_CGI_Handler(int iIndex, int iNumParams, char *pcParam[], char *pcValue[]) { - int index_userUartBaudRate, index_userPacketType, index_userPacketRate; - int index_lpfAccelFilterFreq, index_lpfRateFilterFreq; - int index_orientation, index_profile; + int index_profile; int index_leverArmBx, index_leverArmBy, index_leverArmBz; int index_pointOfInterestBx, index_pointOfInterestBy, index_pointOfInterestBz; int index_rotationRbvx, index_rotationRbvy, index_rotationRbvz; - uint8_t ori[7] = "+x+y+z"; - if (iNumParams == 16) + if (iNumParams == 10) { - index_userUartBaudRate = FindCGIParameter("userUartBaudRate", pcParam, iNumParams); - index_userPacketType = FindCGIParameter("userPacketType", pcParam, iNumParams); - index_userPacketRate = FindCGIParameter("userPacketRate", pcParam, iNumParams); - index_lpfAccelFilterFreq = FindCGIParameter("lpfAccelFilterFreq", pcParam, iNumParams); - index_lpfRateFilterFreq = FindCGIParameter("lpfRateFilterFreq", pcParam, iNumParams); - index_orientation = FindCGIParameter("orientation", pcParam, iNumParams); index_profile = FindCGIParameter("profile", pcParam, iNumParams); index_leverArmBx = FindCGIParameter("leverArmBx", pcParam, iNumParams); index_leverArmBy = FindCGIParameter("leverArmBy", pcParam, iNumParams); @@ -310,21 +295,10 @@ const char *USER_CONFIG_CGI_Handler(int iIndex, int iNumParams, char *pcParam[], index_rotationRbvy = FindCGIParameter("rotationRbvy", pcParam, iNumParams); index_rotationRbvz = FindCGIParameter("rotationRbvz", pcParam, iNumParams); - if (index_userUartBaudRate != -1 && index_userPacketType != -1 && index_userPacketRate != -1 - && index_lpfAccelFilterFreq != -1 && index_lpfRateFilterFreq != -1 - && index_orientation != -1 && index_profile != -1 - && index_leverArmBx != -1 && index_leverArmBy != -1 && index_leverArmBz != -1 + if (index_profile != -1 && index_leverArmBx != -1 && index_leverArmBy != -1 && index_leverArmBz != -1 && index_pointOfInterestBx != -1 && index_pointOfInterestBy != -1 && index_pointOfInterestBz != -1 && index_rotationRbvx != -1 && index_rotationRbvy != -1 && index_rotationRbvz != -1) { - gUserConfiguration.userUartBaudRate = atol(pcValue[index_userUartBaudRate]); - memset(gUserConfiguration.userPacketType, 0, sizeof(gUserConfiguration.userPacketType)); - strcpy((char*)gUserConfiguration.userPacketType, (const char*)pcValue[index_userPacketType]); - gUserConfiguration.userPacketRate = atol(pcValue[index_userPacketRate]); - gUserConfiguration.lpfAccelFilterFreq = atol(pcValue[index_lpfAccelFilterFreq]); - gUserConfiguration.lpfRateFilterFreq = atol(pcValue[index_lpfRateFilterFreq]); - memset(gUserConfiguration.orientation, 0, sizeof(gUserConfiguration.orientation)); - strcpy((char*)gUserConfiguration.orientation, (const char*)ori); gUserConfiguration.profile = atoi(pcValue[index_profile]); gUserConfiguration.leverArmBx = atof(pcValue[index_leverArmBx]); gUserConfiguration.leverArmBy = atof(pcValue[index_leverArmBy]); @@ -401,13 +375,7 @@ const char *USER_CONFIG_JS_Handler(int iIndex, int iNumParams, char *pcParam[], memset(http_response, 0, HTTP_JS_RESPONSE_SIZE); memset(http_response_body, 0, HTTP_JS_RESPONSE_SIZE); - sprintf((char *)http_response_body, "UserConfigCallback({\"userUartBaudRate\":\"%llu\",\"userPacketType\":\"%s\",\"userPacketRate\":\"%llu\",\"lpfAccelFilterFreq\":\"%llu\",\"lpfRateFilterFreq\":\"%llu\",\"orientation\":\"%s\",\"profile\":\"%lu\",\"leverArmBx\":\"%f\",\"leverArmBy\":\"%f\",\"leverArmBz\":\"%f\",\"pointOfInterestBx\":\"%f\",\"pointOfInterestBy\":\"%f\",\"pointOfInterestBz\":\"%f\",\"rotationRbvx\":\"%f\",\"rotationRbvy\":\"%f\",\"rotationRbvz\":\"%f\"})", - gUserConfiguration.userUartBaudRate, - gUserConfiguration.userPacketType, - gUserConfiguration.userPacketRate, - gUserConfiguration.lpfAccelFilterFreq, - gUserConfiguration.lpfRateFilterFreq, - gUserConfiguration.orientation, + sprintf((char *)http_response_body, "UserConfigCallback({\"profile\":\"%lu\",\"leverArmBx\":\"%f\",\"leverArmBy\":\"%f\",\"leverArmBz\":\"%f\",\"pointOfInterestBx\":\"%f\",\"pointOfInterestBy\":\"%f\",\"pointOfInterestBz\":\"%f\",\"rotationRbvx\":\"%f\",\"rotationRbvy\":\"%f\",\"rotationRbvz\":\"%f\"})", gUserConfiguration.profile, gUserConfiguration.leverArmBx, gUserConfiguration.leverArmBy, diff --git a/Platform/Board/include/bsp.h b/Platform/Board/include/bsp.h index 3d5a5aa..efb8151 100644 --- a/Platform/Board/include/bsp.h +++ b/Platform/Board/include/bsp.h @@ -8,7 +8,7 @@ * *****************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. diff --git a/Platform/Board/src/bsp.c b/Platform/Board/src/bsp.c index 4ae13f5..da3580d 100644 --- a/Platform/Board/src/bsp.c +++ b/Platform/Board/src/bsp.c @@ -20,12 +20,6 @@ #include "boardDefinition.h" #include "configureGPIO.h" #include "eepromAPI.h" -#ifndef BAREMETAL_OS - #include "FreeRTOS.h" - #include "osapi.h" -#else - #include "bare_osapi.h" -#endif #include "bsp.h" #include "led.h" #include "rtcm.h" diff --git a/Platform/Board/src/configureGPIO.c b/Platform/Board/src/configureGPIO.c index cb7d24e..7670385 100644 --- a/Platform/Board/src/configureGPIO.c +++ b/Platform/Board/src/configureGPIO.c @@ -13,7 +13,7 @@ ******************************************************************************/ #include "configureGPIO.h" // #include "boardAPI.h" -#include "GlobalConstants.h" +#include "constants.h" // #include "platformAPI.h" #include "stm32f4xx_hal_conf.h" #include "bsp.h" diff --git a/Platform/Board/src/stm32f4xx_it.c b/Platform/Board/src/stm32f4xx_it.c index 434abaf..8d4bb4e 100644 --- a/Platform/Board/src/stm32f4xx_it.c +++ b/Platform/Board/src/stm32f4xx_it.c @@ -40,14 +40,7 @@ #include "stm32f4xx_it.h" #include "Indices.h" #include "boardDefinition.h" -#ifndef BAREMETAL_OS - #include "sensorsAPI.h" - #include "osapi.h" - #include "osresources.h" -#else - #include "bare_osapi.h" -#endif -#include "RingBuffer.h" +#include "utils.h" #include "rtcm.h" #include "led.h" #include "timer.h" @@ -244,14 +237,6 @@ void DebugMon_Handler(void) /* USER CODE END DebugMonitor_IRQn 1 */ } - -#ifdef BAREMETAL_OS -void SysTick_Handler(void) -{ - HAL_IncTick(); -} -#endif - /******************************************************************************/ /* STM32F4xx Peripheral Interrupt Handlers */ /* Add here the Interrupt Handlers for the used peripherals. */ diff --git a/Platform/Core/include/Indices.h b/Platform/Core/include/Indices.h index c352b73..68cce59 100644 --- a/Platform/Core/include/Indices.h +++ b/Platform/Core/include/Indices.h @@ -5,7 +5,7 @@ * Created on April 10, 2016, 10:12 PM *******************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. diff --git a/Platform/Core/include/debug.h b/Platform/Core/include/debug.h index e75c4e1..dda5a4b 100644 --- a/Platform/Core/include/debug.h +++ b/Platform/Core/include/debug.h @@ -34,7 +34,7 @@ * #include "debug.h" *****************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. diff --git a/Platform/Core/include/serial_port.h b/Platform/Core/include/serial_port.h index 2da5496..17835d8 100644 --- a/Platform/Core/include/serial_port.h +++ b/Platform/Core/include/serial_port.h @@ -10,7 +10,7 @@ * external communication port structures *****************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -28,15 +28,11 @@ limitations under the License. #ifndef EXTERN_PORT_H #define EXTERN_PORT_H - #include -#include "ucb_packet_struct.h" -#include "GlobalConstants.h" -typedef uint16_t ExternPortTypeEnum; +#include "constants.h" +#include "ucb_packet.h" -extern void ExternPortInit (void); extern BOOL HandleUcbRx (UcbPacketStruct *ptrUcbPacket); extern void HandleUcbTx (int port, UcbPacketStruct *ptrUcbPacket); -extern void ExternPortWaitOnTxIdle (void); #endif diff --git a/Platform/Core/include/ucb_packet.h b/Platform/Core/include/ucb_packet.h index 4123199..831ea3d 100644 --- a/Platform/Core/include/ucb_packet.h +++ b/Platform/Core/include/ucb_packet.h @@ -17,8 +17,47 @@ #ifndef UCB_PACKET_H #define UCB_PACKET_H -#include "GlobalConstants.h" -#include "ucb_packet_struct.h" +#include "stdio.h" +#include "constants.h" + +#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) + + +#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; // NEEDS TO BE CHECKED // Xbow Packet Code @@ -34,13 +73,9 @@ typedef enum UCB_UNLOCK_EEPROM, // 7 UE 0x5545 UCB_READ_EEPROM, // 8 RE 0x5245 UCB_WRITE_EEPROM, // 9 WE 0x4745 - UCB_PROGRAM_RESET, // 10 PR 0x5052 UCB_SOFTWARE_RESET, // 11 SR 0x5352 UCB_WRITE_CAL, // 12 WC 0x5743 UCB_READ_CAL, // 13 RC 0x5243 - UCB_WRITE_APP, // 14 WA 0x5743 - UCB_JUMP2_BOOT, - UCB_JUMP2_APP, UCB_J2BOOT, // 15 JB 0x4A42 UCB_J2IAP, // 16 JI 0x4A49 UCB_J2APP, // 17 JA 0x4A41 @@ -57,43 +92,23 @@ typedef enum UCB_FACTORY_1, // 25 F1 0x4631 UCB_FACTORY_2, // 26 F2 0x4632 UCB_FACTORY_M, // 27 F3 0x464D - UCB_ANGLE_1, // 28 A1 0x4131 - UCB_MAG_CAL_1_COMPLETE, // 30 CB 0x4342 - UCB_MAG_CAL_3_COMPLETE, // 31 CD 0x4344 +//************************************************** UCB_PKT_NONE, // 27 marker after last valid packet UCB_NAK, // 28 - UCB_ERROR_TIMEOUT, // 29 - UCB_ERROR_CRC_FAIL, // 30 } UcbPacketType; #define UCB_IDENTIFICATION_LENGTH 69 #define UCB_VERSION_DATA_LENGTH 5 #define UCB_VERSION_ALL_DATA_LENGTH 15 -#define UCB_ANGLE_1_LENGTH 32 -#define UCB_ANGLE_2_LENGTH 30 -#define UCB_ANGLE_3_LENGTH 30 -#define UCB_ANGLE_4_LENGTH 42 // std A4 = 38 this is custom -#define UCB_ANGLE_5_LENGTH 62 -#define UCB_ANGLE_U_LENGTH 42 #define UCB_SCALED_0_LENGTH 30 #define UCB_SCALED_1_LENGTH 24 #define UCB_SCALED_M_LENGTH 60 #define UCB_TEST_0_LENGTH 28 -#define UCB_TEST_1_LENGTH 32 #define UCB_FACTORY_1_LENGTH 54 #define UCB_FACTORY_2_LENGTH 66 #define UCB_FACTORY_M_LENGTH 85 -#define UCB_FACTORY_4_LENGTH 54 -#define UCB_FACTORY_5_LENGTH 70 -#define UCB_FACTORY_6_LENGTH 66 -#define UCB_FACTORY_7_LENGTH 134 -#define UCB_MAG_CAL_1_COMPLETE_LENGTH 4 -#define UCB_MAG_CAL_3_COMPLETE_LENGTH 10 -#define UCB_NAV_0_LENGTH 32 -#define UCB_NAV_1_LENGTH 42 -#define UCB_NAV_2_LENGTH 46 // with ITOW -#define UCB_KT_LENGTH 18 + /// UCB packet-specific utility functions ucb_packet.c extern UcbPacketType UcbPacketBytesToPacketType (const uint8_t bytes []); @@ -105,11 +120,6 @@ extern BOOL UcbPacketIsAnOutputPacket(UcbPacketType type); extern void SendUcbPacket(uint16_t port, UcbPacketStruct *ptrUcbPacket); // handle packet.c extern void HandleUcbPacket(UcbPacketStruct *ptrUcbPacket); -extern int HandleUserInputPacket (UcbPacketStruct *ptrUcbPacket); -extern BOOL HandleUserOutputPacket (uint8_t *payload, uint8_t *payloadLen); -// Function used to write the Mag-Align parameters to the EEPROM by field -extern void WriteMagAlignParamsToMemory( uint16_t port, - UcbPacketStruct *ptrUcbPacket ); #endif diff --git a/Platform/Core/include/xbowsp_algorithm.h b/Platform/Core/include/xbowsp_algorithm.h index f1fe160..a50067c 100644 --- a/Platform/Core/include/xbowsp_algorithm.h +++ b/Platform/Core/include/xbowsp_algorithm.h @@ -18,7 +18,7 @@ #ifndef XBOWSP_ALGORITHM_H #define XBOWSP_ALGORITHM_H -#include "GlobalConstants.h" +#include "constants.h" #include "BITStatus.h" // BITStatusStruct #include "Indices.h" diff --git a/Platform/Core/src/debug.c b/Platform/Core/src/debug.c index a5b550a..41dbb0e 100644 --- a/Platform/Core/src/debug.c +++ b/Platform/Core/src/debug.c @@ -11,7 +11,7 @@ * console. ******************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -28,8 +28,7 @@ limitations under the License. #include "debug.h" -#include "utilities.h" // for itoa -#include "GlobalConstants.h" +#include "constants.h" #include "platformAPI.h" #include "uart.h" #include "string.h" diff --git a/Platform/Core/src/handle_packet.c b/Platform/Core/src/handle_packet.c index e06c8f8..e8e649b 100644 --- a/Platform/Core/src/handle_packet.c +++ b/Platform/Core/src/handle_packet.c @@ -21,11 +21,6 @@ #include "parameters.h" #include "eepromAPI.h" #include "crc16.h" -#ifndef BAREMETAL_OS - #include "osapi.h" -#else - #include "bare_osapi.h" -#endif #include "BITStatus.h" #ifndef SENSOR_UNUSED #include "calibrationAPI.h" @@ -34,7 +29,7 @@ #include "hwAPI.h" #include "platformAPI.h" #include "configureGPIO.h" -//#include "UserMessaging.h" +#include "user_message.h" #include "bsp.h" #include "commAPI.h" #include "uart.h" @@ -623,75 +618,6 @@ void _UcbWriteCal (uint16_t port, // RestoreDelay_Watchdog(); } -/** **************************************************************************** - * @name _UcbWriteApp - * @brief Write data as 16 bit cells into an unlocked EEPROM. - * - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -static void _UcbWriteApp (uint16_t port, UcbPacketStruct *ptrUcbPacket) -#if 0 -{ - uint32_t startAddress; - uint16_t bytesToWrite; - - startAddress = (uint32_t)((ptrUcbPacket->payload[0] << 24) | - ptrUcbPacket->payload[1] << 16 | - ptrUcbPacket->payload[2] << 8 | - ptrUcbPacket->payload[3]); - - bytesToWrite = ptrUcbPacket->payload[4]; - - /// verify that the packet length matches packet specification - if (ptrUcbPacket->payloadLength == (bytesToWrite + 5)) { - /// 0 means no errors - if (writeToEEPROMAppPartition(startAddress, bytesToWrite, - &(ptrUcbPacket->payload[5])) != 0) { - ptrUcbPacket->payloadLength = 5; - } else { - _SetNak(port, ptrUcbPacket); - } - } else { - _SetNak(port, ptrUcbPacket); - } - - HandleUcbTx(port, ptrUcbPacket); -// RestoreDelay_Watchdog(); -} -#endif -{ - uint32_t startAddress; - uint8_t wordsToWrite; - uint16_t bytesToWrite; -// packet structure -// header code payload len start addr numbytes crc -// 5555 5747 [x] [yyyy] [z] [payload] [cc] - startAddress = (uint32_t)((ptrUcbPacket->payload[0] << 24) | - ptrUcbPacket->payload[1] << 16 | - ptrUcbPacket->payload[2] << 8 | - ptrUcbPacket->payload[3]); - wordsToWrite = ptrUcbPacket->payload[4]; - bytesToWrite = (uint16_t)wordsToWrite; - -// SetMaxDelay_Watchdog(); - - /// verify that the packet length matches packet specification - if ((ptrUcbPacket->payloadLength == (bytesToWrite + 5))) { - - //if(!writeFlash(startAddress,&ptrUcbPacket->payload[5],bytesToWrite)){ //TODO: - if(writeFlash(startAddress,&ptrUcbPacket->payload[5],bytesToWrite)){ //TODO: - ptrUcbPacket->payloadLength = 5; - } else { - _SetNak(port, ptrUcbPacket); - } - } else { - _SetNak(port, ptrUcbPacket); - } - HandleUcbTx(port, ptrUcbPacket); -// RestoreDelay_Watchdog(); -} /** **************************************************************************** * @name _UcbJump2BOOT * @brief @@ -850,7 +776,7 @@ void HandleUcbPacket (UcbPacketStruct *ptrUcbPacket) { int result; - ExternPortTypeEnum port = UART_USER; + uint16_t port = UART_USER; if (ptrUcbPacket) { switch (ptrUcbPacket->packetType) { @@ -866,7 +792,6 @@ void HandleUcbPacket (UcbPacketStruct *ptrUcbPacket) case UCB_J2IAP: _UcbJump2BOOT (port, ptrUcbPacket); break; case UCB_J2APP: //TODO: - case UCB_JUMP2_APP: _UcbJump2APP (port, ptrUcbPacket); break; case UCB_HARDWARE_TEST: _Ucb_HARDWARE_TEST (port, ptrUcbPacket); break; @@ -874,8 +799,6 @@ void HandleUcbPacket (UcbPacketStruct *ptrUcbPacket) _UcbReadCal(port, ptrUcbPacket); break; case UCB_SOFTWARE_RESET: _UcbSoftwareReset(port, ptrUcbPacket); break; - case UCB_WRITE_APP: - _UcbWriteApp(port, ptrUcbPacket); break; #ifndef BOOT_MODE case UCB_SET_FIELDS: _UcbSetFields(port, ptrUcbPacket); break; @@ -904,7 +827,6 @@ void HandleUcbPacket (UcbPacketStruct *ptrUcbPacket) } break; #endif // BOOT_MODE -// case UCB_READ_APP:break; default: _UcbError(port, ptrUcbPacket); break; break; /// default handler - unknown send NAK @@ -913,43 +835,6 @@ void HandleUcbPacket (UcbPacketStruct *ptrUcbPacket) } /* end HandleUcbPacket() */ -/** **************************************************************************** - * @name _WriteMagAlignParamsToMemory - * @brief writes the magnetic alignment parameters to the EEPROM - * Trace: [SDD_HANDLE_PKT <-- SRC_HANDLE_PACKET] - * @retval N/A - ******************************************************************************/ -void WriteMagAlignParamsToMemory( uint16_t port, - UcbPacketStruct *ptrUcbPacket ) -{ - // Array sizes are based on maximum number of fields to change - uint16_t fieldId [UCB_MAX_PAYLOAD_LENGTH / 4]; - fieldId[0] = 0x0009; // X-Axis Hard-Iron - fieldId[1] = 0x000A; // Y-Axis Hard-Iron - fieldId[2] = 0x000B; // Soft-Iron Scale-Ratio - fieldId[3] = 0x000E; // Soft-Iron Angle - - uint16_t fieldData [UCB_MAX_PAYLOAD_LENGTH / 4]; - fieldData[0] = gConfiguration.hardIronBias[0]; // [G] - fieldData[1] = gConfiguration.hardIronBias[1]; // [G] - fieldData[2] = gConfiguration.softIronScaleRatio; // [N/A] - fieldData[3] = gConfiguration.softIronAngle; // [deg] - - uint8_t numFields = 0x4; - ptrUcbPacket->payloadLength = 1 + 4*numFields; - ptrUcbPacket->payload[0] = numFields; - - // first two are field ID, second two are data - for( uint8_t fieldNum = 0; fieldNum < numFields; fieldNum++ ) { - ptrUcbPacket->payload[(fieldNum*numFields)+1] = fieldId[fieldNum] >> 8; - ptrUcbPacket->payload[(fieldNum*numFields)+2] = fieldId[fieldNum] & 0x00FF; - ptrUcbPacket->payload[(fieldNum*numFields)+3] = fieldData[fieldNum] >> 8; - ptrUcbPacket->payload[(fieldNum*numFields)+4] = fieldData[fieldNum] & 0x00FF; - } - - _UcbWriteFields( port, ptrUcbPacket ); -} - /** **************************************************************************** * @name _ProcessUcbCommands @@ -975,57 +860,3 @@ void ProcessUserCommands (void) } /* end ProcessUcbCommands() */ -extern int get_input_packet_type(); -extern BOOL Fill_PingPacketPayload(uint8_t *payload, uint8_t *payloadLen); -extern BOOL Fill_VersionPacketPayload(uint8_t *payload, uint8_t *payloadLen); -__weak int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) -{ - BOOL valid = TRUE; - int ret = USER_PACKET_OK; - /// call appropriate function based on packet type - switch (get_input_packet_type()) { - 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; -#if 1 - case USR_IN_UPDATE_PARAM: - //UpdateUserParam((userParamPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); - break; - case USR_IN_SAVE_CONFIG: - // payload length does not change - //if(!SaveUserConfig()) - { - valid = FALSE; - } - break; - case USR_IN_GET_ALL: - //if(!GetAllUserParams((allUserParamsPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength)) - { - valid = FALSE; - } - break; -#endif - 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; -} \ No newline at end of file diff --git a/Platform/Core/src/parameters.c b/Platform/Core/src/parameters.c index e139ac6..94396a2 100644 --- a/Platform/Core/src/parameters.c +++ b/Platform/Core/src/parameters.c @@ -16,10 +16,11 @@ //*************************** #include #include + #include "configuration.h" #include "parameters.h" #include "eepromAPI.h" -#include "scaling.h" +#include "constants.h" #include "qmath.h" #include "Indices.h" #ifndef SENSOR_UNUSED @@ -39,7 +40,6 @@ UART_HandleTypeDef huart5; static ConfigurationStruct proposedRamConfiguration; static ConfigurationStruct proposedEepromConfiguration; ConfigurationStruct *proposedEepromConfigurationPtr = &proposedEepromConfiguration; -extern void InitUserAlgorithm(); float GetUnitTemp(); static BOOL portConfigurationChanged; // port settings are to be changed @@ -51,10 +51,6 @@ static BOOL portConfigurationChanged; // port settings are to be changed -__weak void InitUserAlgorithm() -{ - -} /** **************************************************************************** * @name DefaultPortConfiguration * @brief Set initial ports: @@ -141,9 +137,6 @@ BOOL CheckContPacketRate (UcbPacketType outputPacket, case UCB_TEST_0: bytesPerPacket += UCB_TEST_0_LENGTH; break; -// case UCB_TEST_1: -// bytesPerPacket += UCB_TEST_1_LENGTH; -// break; case UCB_FACTORY_1: bytesPerPacket += UCB_FACTORY_1_LENGTH; break; @@ -153,21 +146,6 @@ BOOL CheckContPacketRate (UcbPacketType outputPacket, case UCB_FACTORY_M: bytesPerPacket += UCB_FACTORY_M_LENGTH; break; - case UCB_ANGLE_1: - bytesPerPacket += UCB_ANGLE_1_LENGTH; - break; -// case UCB_ANGLE_2: -// bytesPerPacket += UCB_ANGLE_2_LENGTH; -// break; -// case UCB_ANGLE_3: -// bytesPerPacket += UCB_ANGLE_3_LENGTH; -// break; -// case UCB_ANGLE_5: -// bytesPerPacket += UCB_ANGLE_5_LENGTH; -// break; -// case UCB_ANGLE_U: -// bytesPerPacket += UCB_ANGLE_U_LENGTH; -// break; case UCB_VERSION_DATA: bytesPerPacket += UCB_VERSION_DATA_LENGTH; break; @@ -183,18 +161,6 @@ BOOL CheckContPacketRate (UcbPacketType outputPacket, case UCB_SCALED_M: bytesPerPacket += UCB_SCALED_M_LENGTH; break; -// case UCB_NAV_0: -// bytesPerPacket += UCB_NAV_0_LENGTH; -// break; -// case UCB_NAV_1: -// bytesPerPacket += UCB_NAV_1_LENGTH; -// break; -// case UCB_NAV_2: -// bytesPerPacket += UCB_NAV_2_LENGTH; -// break; -// case UCB_KT: -// bytesPerPacket += UCB_KT_LENGTH; -// break; default: valid = FALSE; } @@ -415,7 +381,6 @@ static uint8_t CheckFieldData (ConfigurationStruct *currentConfiguration, /// add to valid list validFields[validFieldIndex++] = fieldId[fieldIndex]; // Set the flags to RESTART the algorithm - InitUserAlgorithm(); } break; case OFFSET_ROLL_ALIGN_FIELD_ID: diff --git a/Platform/Core/src/platform.c b/Platform/Core/src/platform.c index ea12cad..469d358 100644 --- a/Platform/Core/src/platform.c +++ b/Platform/Core/src/platform.c @@ -17,7 +17,7 @@ //***************************** #include "BitStatus.h" #include "platform_version.h" -#include "GlobalConstants.h" +#include "constants.h" int userSerialChan; int gpsSerialChan; @@ -98,16 +98,4 @@ void platformSetMode(BOOL isBoot) bootMode = isBoot; } -static BOOL _useGpsPps = FALSE; - -BOOL platformIsGpsPPSUsed(void) -{ - return _useGpsPps; -} - -void platformEnableGpsPps(BOOL enable) -{ - _useGpsPps = enable; -} - /*end void initConfigureUnit(void) */ diff --git a/Platform/Core/src/send_packet.c b/Platform/Core/src/send_packet.c index 997b9f2..21c28c5 100644 --- a/Platform/Core/src/send_packet.c +++ b/Platform/Core/src/send_packet.c @@ -35,65 +35,30 @@ #include "configureGPIO.h" #include "bsp.h" #include "uart.h" -#ifdef USE_ALGORITHM -#include "algorithmAPI.h" -#endif -#include "gpsAPI.h" -//#include "taskDataAcquisition.h" -//#include "UserMessaging.h" -#ifndef BAREMETAL_OS - #include "osapi.h" - #include "calibrationAPI.h" - #include "sensorsAPI.h" -#else - #include "bare_osapi.h" -#endif +#include "timer.h" +#include "serial_port.h" + +#include "osapi.h" +#include "calibrationAPI.h" +#include "sensorsAPI.h" +#include "user_message.h" #include "Indices.h" -extern mcu_time_base_t imu_time; -extern UART_HandleTypeDef huart_user; -extern UART_HandleTypeDef huart_bt; -void HandleUcbTx(uint16_t port, UcbPacketStruct *ptrUcbPacket); + void _UcbIdentification(uint16_t port, UcbPacketStruct *ptrUcbPacket); void _UcbVersionData(uint16_t port, UcbPacketStruct *ptrUcbPacket); void _UcbVersionAllData(uint16_t port, UcbPacketStruct *ptrUcbPacket); -void _UcbAngle1(uint16_t port, UcbPacketStruct *ptrUcbPacket); -void _UcbAngle2(uint16_t port, UcbPacketStruct *ptrUcbPacket); -void _UcbAngle5(uint16_t port, UcbPacketStruct *ptrUcbPacket); -void _UcbAngleU(uint16_t port, UcbPacketStruct *ptrUcbPacket); void _UcbScaled0(uint16_t port, UcbPacketStruct *ptrUcbPacket); void _UcbScaled1(uint16_t port, UcbPacketStruct *ptrUcbPacket); void _UcbTest0(uint16_t port, UcbPacketStruct *ptrUcbPacket); -void _UcbTest1(uint16_t port, UcbPacketStruct *ptrUcbPacket); void _UcbFactory1(uint16_t port, UcbPacketStruct *ptrUcbPacket); void _UcbFactory2(uint16_t port, UcbPacketStruct *ptrUcbPacket); void _UcbFactory3(uint16_t port, UcbPacketStruct *ptrUcbPacket); -void _UcbMagCal1Complete(uint16_t port, UcbPacketStruct *ptrUcbPacket); -void _UcbMagCal3Complete(uint16_t port, UcbPacketStruct *ptrUcbPacket); -void _UcbNav0(uint16_t port, UcbPacketStruct *ptrUcbPacket); -void _UcbNav1(uint16_t port, UcbPacketStruct *ptrUcbPacket); -void _UcbNav2(uint16_t port, UcbPacketStruct *ptrUcbPacket); uint8_t divideCount = 10; /// continuous packet rate divider - set initial delay uint8_t divideCount_py = 10; static UcbPacketStruct continuousUcbPacket; - - -__weak int checkUserOutPacketType(uint16_t receivedCode) -{ - return RTK_OK; -} - -__weak BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) -{ - return RTK_OK; -} - -__weak uint16_t get_gpsUpdate_state() -{ - return 0; -} /** **************************************************************************** * @name _UcbIdentification send ID packet * @brief @@ -207,410 +172,6 @@ void _UcbVersionAllData(uint16_t port, HandleUcbTx(port, ptrUcbPacket); ///< send version all data packet } -#ifdef USE_ALGORITHM -/** **************************************************************************** - * @name _UcbAngle1 send A1 packet (typical output packet used in 525 - * configuration) - * @brief load (SPI/UART) and send (UART) Angle 1 message - * Trace: [SDD_UCB_TX_A1 <-- SRC_UCB_TX_A1] - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -void _UcbAngle1(uint16_t port, - UcbPacketStruct *ptrUcbPacket) -{ - uint16_t packetIndex = 0; - Crc32Type payloadCrc; - - ptrUcbPacket->payloadLength = UCB_ANGLE_1_LENGTH; /// set packet length - // xbowsp_fields.c - /// roll angle, pitch angle, magnetometer yaw angle - packetIndex = appendAttitudeTrue(ptrUcbPacket->payload, packetIndex); - - // Append raw or EKF derived data into the packet. If sest to zero then the - // raw data is provided (FIXME: remove before release). - if (0) - { //gConfiguration.analogFilterClocks[0] & 0x8000 ) { - /// X-angular rate, Y, Z - packetIndex = appendRates(ptrUcbPacket->payload, packetIndex); - - /// X-accelerometer, Y, Z - packetIndex = appendAccels(ptrUcbPacket->payload, packetIndex); - } - else - { - /// X-angular rate, Y, Z - packetIndex = appendCorrectedRates(ptrUcbPacket->payload, packetIndex); - - /// X-accelerometer, Y, Z (according to the serial-interface spec, the - /// accelerometer signal is uncorrected). - // packetIndex = appendCorrectedAccels(ptrUcbPacket->payload, packetIndex); - packetIndex = appendAccels(ptrUcbPacket->payload, packetIndex); - } - - /// magnetometer X, Y, Z - packetIndex = appendMagReadings(ptrUcbPacket->payload, packetIndex); - - /// X rate temp - packetIndex = appendRateTemp(ptrUcbPacket->payload, packetIndex); - - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, /// timer - packetIndex, - getAlgorithmTimer()); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// BIT status - packetIndex, - gBitStatus.BITStatus.all); - /// compute Universal payload CRC - payloadCrc = Crc32(ptrUcbPacket->payload, - (UCB_ANGLE_U_LENGTH - CRC_32_LENGTH), - CRC_32_INITIAL_SEED); - /// Universal payload CRC - Crc32TypeToBytes(payloadCrc, &(ptrUcbPacket->payload[packetIndex])); - - if (platformGetUnitCommunicationType() == UART_COMM) - { - HandleUcbTx(port, ptrUcbPacket); /// send Angle 1 packet - } // load only for SPI don't send -} - -/** **************************************************************************** - * @name _UcbAngle2 send A2 packet (typical output packet used in 525 - * configuration) - * @brief - * Trace: [SDD_UCB_TX_A2 <-- SRC_UCB_TX_A2] - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -/* -void _UcbAngle2 (uint16_t port, - UcbPacketStruct *ptrUcbPacket) -{ - uint16_t packetIndex = 0; - uint32_t time = TimeNow(); - Crc32Type payloadCrc; - - ptrUcbPacket->payloadLength = UCB_ANGLE_2_LENGTH; /// set packet length - // xbowsp_fields.c - /// roll angle, pitch angle, magnetometer yaw angle - packetIndex = appendAttitudeTrue(ptrUcbPacket->payload, packetIndex); - /// X-angular rate, Y, Z - packetIndex = appendCorrectedRates(ptrUcbPacket->payload, packetIndex); - /// X-accelerometer, Y, Z - packetIndex = appendAccels(ptrUcbPacket->payload, packetIndex); - - /// X,Y,Z rate temp (do not include board temp per Serial Interface Spec, i.e. do not - /// use appendTemps) - packetIndex = appendRateTemp(ptrUcbPacket->payload, packetIndex); - packetIndex = appendRateTemp(ptrUcbPacket->payload, packetIndex); - packetIndex = appendRateTemp(ptrUcbPacket->payload, packetIndex); - - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, /// timer - packetIndex, - gAlgorithm.timer); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// BIT status - packetIndex, - gBitStatus.BITStatus.all ); - /// compute Universal payload CRC - payloadCrc = Crc32(ptrUcbPacket->payload, - (UCB_ANGLE_U_LENGTH - CRC_32_LENGTH), - CRC_32_INITIAL_SEED); - /// Universal payload CRC - Crc32TypeToBytes (payloadCrc, &(ptrUcbPacket->payload[packetIndex])); - HandleUcbTx(port, ptrUcbPacket); /// send Angle 2 packet -} -*/ - -/** **************************************************************************** - * @name _UcbAngle3 send A3 packet (typical output packet used in 525 - * configuration) - * @brief - * Trace: - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -/* -void _UcbAngle3 (uint16_t port, - UcbPacketStruct *ptrUcbPacket) -{ - uint16_t packetIndex = 0; - uint32_t time = TimeNow(); - Crc32Type payloadCrc; - - ptrUcbPacket->payloadLength = UCB_ANGLE_3_LENGTH; /// set packet length - // xbowsp_fields.c - /// roll angle, pitch angle, magnetometer yaw angle - packetIndex = appendAttitudeTrue(ptrUcbPacket->payload, packetIndex); - /// X-angular rate, Y, Z - packetIndex = appendRates(ptrUcbPacket->payload, packetIndex); - /// X-accelerometer, Y, Z - packetIndex = appendAccels(ptrUcbPacket->payload, packetIndex); - - /// X,Y,Z rate temp (do not include board temp per Serial Interface Spec, i.e. do not - /// use appendTemps) - packetIndex = appendRateTemp(ptrUcbPacket->payload, packetIndex); - packetIndex = appendRateTemp(ptrUcbPacket->payload, packetIndex); - packetIndex = appendRateTemp(ptrUcbPacket->payload, packetIndex); - - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, /// timer - packetIndex, - gAlgorithm.timer); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// BIT status - packetIndex, - gBitStatus.BITStatus.all ); - /// compute Universal payload CRC - payloadCrc = Crc32(ptrUcbPacket->payload, - (UCB_ANGLE_U_LENGTH - CRC_32_LENGTH), - CRC_32_INITIAL_SEED); - /// Universal payload CRC - Crc32TypeToBytes (payloadCrc, &(ptrUcbPacket->payload[packetIndex])); - HandleUcbTx(port, ptrUcbPacket); /// send Angle 3 packet -} -*/ - -/** **************************************************************************** - * @name _UcbAngle4 send modified A4 DEBUG packet - * @brief Used to load SPI message payload NO INFRASTRUCTURE CONNECTED FOR UCB - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -/* todo tm20160602 - need to fix this, if we support this packet -void _UcbAngle4 (uint16_t port, - UcbPacketStruct *ptrUcbPacket) -{ - uint16_t packetIndex = 0; - int16_t tmp; - - // set packet length - ptrUcbPacket->payloadLength = UCB_ANGLE_4_LENGTH; - - // add roll angle, pitch angle, magnetometer yaw angle - packetIndex = appendAttitudeTrue((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // add X-angular rate, Y-angular rate, Z-angular rate - packetIndex = appendCorrectedRates((uint8_t *)ptrUcbPacket->payload, packetIndex); - //packetIndex = appendRates((char *)packetPtr.ucbPacketPtr->payload, packetIndex); - - // add X-accelerometer, Y-accelerometer, Z-accelerometer - packetIndex = appendAccels((uint8_t *)ptrUcbPacket->payload, packetIndex); - - /// X-magnetometer, Y, Z - packetIndex = appendMagReadings(ptrUcbPacket->payload, packetIndex); - - // X-Rate Temperature - tmp = _qmul( TWO_POW16_OVER_128_q21, gAlgorithm.scaledSensors_q27[XRTEMP+0], 21, 27, 16 ) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - - // Y-Rate Temperature - tmp = _qmul( TWO_POW16_OVER_2_q15, gKalmanFilter.Quaternion_q30[1], 15, 30, 16 ) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - // Z-Rate Temperature - tmp = _qmul( TWO_POW16_OVER_2_q15, gKalmanFilter.Quaternion_q30[2], 15, 30, 16 ) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - // mag Temperature - tmp = _qmul( TWO_POW16_OVER_2_q15, gKalmanFilter.Quaternion_q30[3], 15, 30, 16 ) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - - /// time ITOW X-angular, Y, Z rate - packetIndex = appendRates(ptrUcbPacket->payload, packetIndex); - - /// add timer - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, /// timer - packetIndex, - gAlgorithm.timer); - - // NO XMIT: this is to load buffer for SPI -// HandleUcbTx(port, ptrUcbPacket); /// send Angle U packet -} */ - -/** **************************************************************************** - * @name _UcbAngle5 send A5 DEBUG packet - * @brief Borrowing messages sent to NavView - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -/* todo tm20160602 - need to fix this, if we support this packet -void _UcbAngle5 (uint16_t port, - UcbPacketStruct *ptrUcbPacket) -{ - uint16_t packetIndex = 0; - int16_t tmp; - - Crc32Type payloadCrc; - - // set packet length - ptrUcbPacket->payloadLength = UCB_ANGLE_5_LENGTH; - - // add roll angle, pitch angle, magnetometer yaw angle - packetIndex = appendAttitudeTrue((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // add X-angular rate, Y-angular rate, Z-angular rate - packetIndex = appendCorrectedRates((uint8_t *)ptrUcbPacket->payload, packetIndex); - //packetIndex = appendRates((char *)packetPtr.ucbPacketPtr->payload, packetIndex); - - // add X-accelerometer, Y-accelerometer, Z-accelerometer - packetIndex = appendAccels((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // X-Rate Temperature - tmp = _qmul( TWO_POW16_OVER_128_q21, gAlgorithm.scaledSensors_q27[XRTEMP+0], 21, 27, 16 ) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - // prediction covariance - tmp = _qmul( TWO_POW16_OVER_128_q21, doubleToQ30( gKalmanFilter.P[1][6] ), 21, 30, 16 ) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - // - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, - packetIndex, - 0x000000001); - - // more covariance - tmp = _qmul( TWO_POW16_OVER_512_q23, doubleToQ30( gKalmanFilter.P[1][6]*10000 ), 23, 30, 16 ) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - - // add X-magnetometer, Y-magnetometer, Z-magnetometer - packetIndex = appendMagReadings((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // X-Rate Temperature - tmp = _qmul( TWO_POW16_OVER_7PI_q19, gKalmanFilter.rateBias_q27[X_AXIS] << 5, 19, 27, 16) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - // X-Rate Temperature - tmp = _qmul( TWO_POW16_OVER_7PI_q19, gKalmanFilter.rateBias_q27[Y_AXIS] << 5, 19, 27, 16) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - // X-Rate Temperature - tmp = _qmul( TWO_POW16_OVER_7PI_q19, gKalmanFilter.rateBias_q27[Z_AXIS] << 5, 19, 27, 16) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - // X-Accel Temperature - tmp = _qmul( TWO_POW16_OVER_2_q15, gKalmanFilter.Quaternion_q30[1], 15, 30, 16 ) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - // X-Rate Temperature - tmp = _qmul( TWO_POW16_OVER_2_q15, gKalmanFilter.Quaternion_q30[2], 15, 30, 16 ) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); - // X-Rate Temperature - tmp = _qmul( TWO_POW16_OVER_2_q15, gKalmanFilter.Quaternion_q30[3], 15, 30, 16 ) >> 16; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - tmp ); -//GOOD TO HERE - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - 0x0001 ); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - 0x0002 ); - - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, - packetIndex, - (uint16_t)gKalmanFilter.P[6][6]); - - // X-Rate Temperature - tmp = _qmul( TWO_POW16_OVER_20_q19, gAlgorithm.scaledSensors_q27[XRTEMP+0], 19, 27, 15 ) >> 15; - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// BIT status - packetIndex, - tmp ); - /// add timer - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, /// timer - packetIndex, - gAlgorithm.timer); - - /// add BIT status - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// BIT status - packetIndex, - gBitStatus.BITStatus.all ); - /// Universal payload CRC - payloadCrc = Crc32(ptrUcbPacket->payload, - (UCB_ANGLE_5_LENGTH - CRC_32_LENGTH), - CRC_32_INITIAL_SEED); - Crc32TypeToBytes (payloadCrc, &(ptrUcbPacket->payload[packetIndex])); - /// send Angle 5 packet - HandleUcbTx(port, ptrUcbPacket); /// send Angle U packet -} -*/ - -/** **************************************************************************** - * @name _UcbAngleU send AU packet (typical output packet used in 525 - * configuration) - * @brief - * Trace: [SDD_UCB_TX_AU <-- SRC_UCB_TX_AU] - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -/* -void _UcbAngleU (uint16_t port, - UcbPacketStruct *ptrUcbPacket) -{ - uint16_t packetIndex = 0; - uint32_t time = TimeNow(); - Crc32Type payloadCrc; - - ptrUcbPacket->payloadLength = UCB_ANGLE_U_LENGTH; /// set packet length - // xbowsp_fields.c - /// roll angle, pitch angle, magnetometer yaw angle - packetIndex = appendAttitudeTrue(ptrUcbPacket->payload, packetIndex); - /// X-angular rate, Y, Z - packetIndex = appendCorrectedRates(ptrUcbPacket->payload, packetIndex); - /// X-accelerometer, Y, Z - packetIndex = appendAccels(ptrUcbPacket->payload, packetIndex); - /// tangent X-rate, Y, Z - packetIndex = appendTangentRates(ptrUcbPacket->payload, packetIndex); - /// tangent X-accel, Y, Z - packetIndex = appendTangentAccels(ptrUcbPacket->payload, packetIndex); - - /// compass heading - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - ( (int)(SCALE_BY_2POW16_OVER_2PI(gAlgorithm.compassHeading))) ); - - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, /// timer - packetIndex, - gAlgorithm.timer); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// BIT status - packetIndex, - gBitStatus.BITStatus.all ); - /// compute Universal payload CRC - payloadCrc = Crc32(ptrUcbPacket->payload, - (UCB_ANGLE_U_LENGTH - CRC_32_LENGTH), - CRC_32_INITIAL_SEED); - /// Universal payload CRC - Crc32TypeToBytes (payloadCrc, &(ptrUcbPacket->payload[packetIndex])); - HandleUcbTx(port, ptrUcbPacket); /// send Angle U packet -} -*/ -#endif //USE_ALGORITHM - /** **************************************************************************** * @name _UcbScaled0 send S0 packet * @brief Scaled sensor 0 message load (SPI / UART) send (UART) scaled and @@ -816,123 +377,6 @@ void _UcbTest0(uint16_t port, HandleUcbTx(port, ptrUcbPacket); /// send Test 0 packet } -/** **************************************************************************** - * @name _UcbKT send KT packet - * @brief - * Trace: [SDD_UCB_TX_KT <-- SRC_UCB_TX_KT] - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -/* -void _UcbKT (uint16_t port, - UcbPacketStruct *ptrUcbPacket) -{ - uint8_t packetIndex = 0; - uint32_t sPtr = 0; - uint32_t sTop = 0; - uint16_t sSize = buildTstamp.year; - uint16_t sPrio = buildTstamp.month; - uint16_t tId = 0; - uint16_t tState = buildTstamp.day; - uint16_t tSlice = buildTstamp.hour*100+buildTstamp.min; - - /// set packet length - ptrUcbPacket->payloadLength = UCB_KT_LENGTH; - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, packetIndex, sPtr); - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, packetIndex, sTop); - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, packetIndex, sSize); - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, packetIndex, sPrio); - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, packetIndex, tId); - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, packetIndex, tState); - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, packetIndex, tSlice); - - HandleUcbTx(port, ptrUcbPacket); /// send Test 0 packet -} -*/ - -/** **************************************************************************** - * @name _UcbTest1 send T1 packet - * @brief - * Trace: [SDD_UCB_TX_T1 <-- SRC_UCB_TX_T1] - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -void _UcbTest1(uint16_t port, - UcbPacketStruct *ptrUcbPacket) -{ - uint8_t packetIndex = 0; - - /// set packet length - ptrUcbPacket->payloadLength = UCB_TEST_1_LENGTH; - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// BIT - packetIndex, - gBitStatus.BITStatus.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// hardware BIT - packetIndex, - gBitStatus.hwBIT.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// hardware power BIT - packetIndex, - 0X0000); // Place holder for NavView - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// hardware environmental BIT - packetIndex, - gBitStatus.hwEnvBIT.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// hardware sensor BIT - packetIndex, - 0x0000); // Place holder for NavView - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// hardware internal comm BIT - packetIndex, - 0x0000); // Place holder for NavView - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// comm BIT - packetIndex, - gBitStatus.comBIT.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// comm serial A BIT - packetIndex, - gBitStatus.comSABIT.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// comm serial B BIT - packetIndex, - gBitStatus.comSBBIT.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// software BIT - packetIndex, - gBitStatus.swBIT.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// software algorithm BIT - packetIndex, - gBitStatus.swAlgBIT.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// software data BIT - packetIndex, - gBitStatus.swDataBIT.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// hardware status - packetIndex, - gBitStatus.hwStatus.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// com status - packetIndex, - gBitStatus.comStatus.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// software status - packetIndex, - gBitStatus.swStatus.all); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// sensor status - packetIndex, - gBitStatus.sensorStatus.all); - HandleUcbTx(port, ptrUcbPacket); /// send Test 1 packet -} - /** **************************************************************************** * @name _UcbFactory1 send F1 packet Factory (Raw) sensor data * @brief Raw data 1 load (SPI / UART) and send (UART) raw sensor counts @@ -999,159 +443,6 @@ void _UcbFactoryM(uint16_t port, UcbPacketStruct *ptrUcbPacket) HandleUcbTx(port, ptrUcbPacket); /// send Factory 2 packet } -/** **************************************************************************** - * @name _UcbNav0 load UCB Navigaion data out SPi and UART - * @brief Nav 1 load (SPI / UART) and send (UART) kalman filter and GPS data - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -void _UcbNav0(uint16_t port, - UcbPacketStruct *ptrUcbPacket) -{ - uint16_t packetIndex = 0; - - // set packet length - ptrUcbPacket->payloadLength = UCB_NAV_0_LENGTH; - - // add roll angle, pitch angle, magnetometer yaw angle - packetIndex = appendAttitudeTrue((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // add X-angular rate, Y-angular rate, Z-angular rate - packetIndex = appendCorrectedRates((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // GPS NED Velocities - packetIndex = appendGpsVel((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // GPS position Lat, Lon, alt - packetIndex = appendGpsPos((uint8_t *)ptrUcbPacket->payload, packetIndex); - - /// time ITOW Ttruncated - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// timer - packetIndex, - GetSensorsSamplingTstamp()); - - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// BIT status - packetIndex, - gBitStatus.BITStatus.all ); - - if( platformGetUnitCommunicationType() == UART_COMM ) { - HandleUcbTx(port, ptrUcbPacket); /// send NAV 0 packet - } -} - -/** **************************************************************************** - * @name _UcbNav1 load UCB Navigaion data out SPi and UART - * @brief Nav 1 load (SPI / UART) and send (UART) kalman filter and GPS data - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -void _UcbNav1(uint16_t port, - UcbPacketStruct *ptrUcbPacket) -{ - uint16_t packetIndex = 0; - - // set packet length - ptrUcbPacket->payloadLength = UCB_NAV_1_LENGTH; - - // add roll angle, pitch angle, magnetometer yaw angle - packetIndex = appendAttitudeTrue((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // Kalman X-angular rate, Y-angular rate, Z-angular rate - packetIndex = appendCorrectedRates((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // kalman accels [m/s^2] - //packetIndex = appendKalmanAccels((uint8_t *)ptrUcbPacket->payload, packetIndex); // alt off 1 - packetIndex = appendAccels((uint8_t *)ptrUcbPacket->payload, packetIndex); // todo - tm20160318 - do we need Kalman accls? - - // Kalman estimated NED Velocities [m/s] - packetIndex = appendKalmanVel((uint8_t *)ptrUcbPacket->payload, packetIndex); // alt off 2 - - // Kalman estimated position Lat, Lon, alt[m] -#if 1 - packetIndex = appendKalmanPos((uint8_t *)ptrUcbPacket->payload, packetIndex); -#else - // GPS position Lat, Lon, alt[m] - packetIndex = appendGpsPos((uint8_t *)ptrUcbPacket->payload, packetIndex); -#endif - - // Rate Temperature - packetIndex = appendRateTemp((uint8_t *)ptrUcbPacket->payload, packetIndex); - - /// time ITOW - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, - packetIndex, - GetSensorsSamplingTstamp()); //gGpsDataPtr->itow ); - -#if 1 - // BIT status - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, - packetIndex, - gBitStatus.BITStatus.all); -#else - // replace bit status with counter - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// BIT status - packetIndex, - gAlgorithm.counter); -#endif - - if (platformGetUnitCommunicationType() == UART_COMM) - { - HandleUcbTx(port, ptrUcbPacket); /// send NAV 0 packet - } -} - -/** **************************************************************************** - * @name _UcbNav2 load UCB Navigaion data out SPi and UART - * @brief Nav 1 load (SPI / UART) and send (UART) kalman filter and GPS data - * @param [in] port - number request came in on, the reply will go out this port - * @param [out] packetPtr - data part of packet - * @retval N/A - ******************************************************************************/ -void _UcbNav2(uint16_t port, - UcbPacketStruct *ptrUcbPacket) -{ - uint16_t packetIndex = 0; - - // set packet length - ptrUcbPacket->payloadLength = UCB_NAV_2_LENGTH; - - // add roll angle, pitch angle, magnetometer yaw angle - packetIndex = appendAttitudeTrue((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // add Raw X-angular rate, Y-angular rate, Z-angular rate - packetIndex = appendRates((uint8_t *)ptrUcbPacket->payload, packetIndex); - //packetIndex = appendCorrectedRates((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // scaled Accels - packetIndex = appendAccels((uint8_t *)ptrUcbPacket->payload, packetIndex); - - /// scaled magnetometer X, Y, Z - packetIndex = appendMagReadings((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // GPS estimated NED Velocities [m/s] - packetIndex = appendGpsVel((uint8_t *)ptrUcbPacket->payload, packetIndex); - - // GPS position Lat, Lon, alt[m] - packetIndex = appendGpsPos((uint8_t *)ptrUcbPacket->payload, packetIndex); - // Rate Temperature - // packetIndex = appendRateTemp((uint8_t *)ptrUcbPacket->payload, packetIndex); - - /// time ITOW (gAlgorithm.itow updates at 10 msec, the GPS ITOW updates at 1 sec) - packetIndex = uint32ToBuffer(ptrUcbPacket->payload, - packetIndex, GetSensorsSamplingTstamp()); //gGpsDataPtr->itow); - - // Use counter in place of bit status - packetIndex = uint16ToBuffer(ptrUcbPacket->payload, /// BIT status - packetIndex, gBitStatus.BITStatus.all); - - if (platformGetUnitCommunicationType() == UART_COMM) - { - HandleUcbTx(port, ptrUcbPacket); /// send NAV 0 packet - } -} - /** **************************************************************************** * @name SendUcbPacket API - taskUserCommunication.c * @brief top level send packet routine - calls other send routines based on @@ -1182,24 +473,6 @@ void SendUcbPacket(uint16_t port, _UcbVersionAllData(port, ptrUcbPacket); break; #ifndef BOOT_MODE -#ifdef USE_ALGORITHM - case UCB_ANGLE_1: // A1 0x4131 - _UcbAngle1(port, ptrUcbPacket); - break; -#endif - // case UCB_ANGLE_2: // A2 0x4132 - // _UcbAngle2(port, ptrUcbPacket); - // break; - // case UCB_ANGLE_3: // A3 0x4133 - // _UcbAngle3(port, ptrUcbPacket); - // break; - // case UCB_ANGLE_5: // A2 0x4135 - // - todo tm20160603 - FIX!!! - //_UcbAngle5(port, ptrUcbPacket); - // break; - // case UCB_ANGLE_U: // AU 0x4155 - // _UcbAngleU(port, ptrUcbPacket); - // break; case UCB_SCALED_0: // S0 0x5330 _UcbScaled0(port, ptrUcbPacket); break; @@ -1221,30 +494,7 @@ void SendUcbPacket(uint16_t port, case UCB_FACTORY_M: // F2 0x464D _UcbFactoryM(port, ptrUcbPacket); break; - // case UCB_KT: // KT 0x4b54 - // _UcbKT(port, ptrUcbPacket); - // break; - // case UCB_TEST_1: // T1 0x5431 - // _UcbTest1(port, ptrUcbPacket); - // break; - // case UCB_FACTORY_3: // F3 0x4533 - // _UcbFactory3(port, ptrUcbPacket); - // break; - // case UCB_MAG_CAL_1_COMPLETE: // CB 0x4342 - // _UcbMagCal1Complete(port, ptrUcbPacket); - // break; - // case UCB_MAG_CAL_3_COMPLETE: // CD 0x4344 - // _UcbMagCal3Complete(port, ptrUcbPacket); - // break; - // case UCB_NAV_0: // N0 0x4e30 - // _UcbNav0(port, ptrUcbPacket); - // break; - // case UCB_NAV_1: // N1 0x4e31 - // _UcbNav1(port, ptrUcbPacket); - // break; - // case UCB_NAV_2: // N2 0x4e32 - // _UcbNav2(port, ptrUcbPacket); - // break; + case UCB_USER_OUT: result = HandleUserOutputPacket(ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); if (!result) @@ -1260,19 +510,6 @@ void SendUcbPacket(uint16_t port, } } -/** **************************************************************************** - * @name ExternPortWaitOnTxIdle - * @brief Drain all pending output on all port types - * Trace: [SDD_EXT_PORT_DRAIN <-- SRC_EXT_PORT_DRAIN] - * [SDD_WATCHDOG <-- SRC_EXT_PORT_DRAIN] - * @param N/A - * @retval N/A - *******************************************************************************/ -void ExternPortWaitOnTxIdle(void) -{ - DelayMs(10); -} - /** **************************************************************************** * @name SendContinuousPacket * diff --git a/Platform/Core/src/serial_port.c b/Platform/Core/src/serial_port.c index 9d797ae..00785ce 100644 --- a/Platform/Core/src/serial_port.c +++ b/Platform/Core/src/serial_port.c @@ -9,7 +9,7 @@ * @brief UCB (Unified Code Base) external serial interface *****************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -25,25 +25,22 @@ limitations under the License. *******************************************************************************/ #include -#include "GlobalConstants.h" +#include "constants.h" #include "serial_port.h" #include "uart.h" #include "crc16.h" #include "ucb_packet.h" -#include "ucb_packet_struct.h" #include "platformAPI.h" #include "configuration.h" +#include "user_message.h" typedef struct{ int type; uint16_t code; }ucbInputSyncTableEntry_t; -extern ConfigurationStruct gConfiguration; // NEEDS TO BE CHECKED /// List of allowed input packet codes ucbInputSyncTableEntry_t ucbInputSyncTable[] = { - {UCB_JUMP2_BOOT, 0x4A42}, // "JB" - {UCB_JUMP2_APP, 0x4A41}, // "JA" {UCB_PING, 0x504B}, // "PK" {UCB_ECHO, 0x4348}, // "CH" {UCB_GET_PACKET, 0x4750}, // "GP" @@ -57,7 +54,6 @@ ucbInputSyncTableEntry_t ucbInputSyncTable[] = { {UCB_SOFTWARE_RESET, 0x5352}, // "SR" {UCB_WRITE_CAL, 0x5743}, // "WC" {UCB_READ_CAL, 0x5243}, // "RC" - {UCB_WRITE_APP, 0x5741}, // "WA" {UCB_J2BOOT, 0x4A42}, // "JB" {UCB_J2IAP, 0x4A49}, // "JI" {UCB_J2APP, 0x4A41}, // "JA" @@ -65,43 +61,8 @@ ucbInputSyncTableEntry_t ucbInputSyncTable[] = { {UCB_INPUT_PACKET_MAX, 0x00000000}, // " " }; -__weak usr_packet_t userInputPackets[] = { - {USR_IN_NONE, {0,0}}, - {USR_IN_PING, "pG"}, - {USR_IN_UPDATE_PARAM, "uP"}, - {USR_IN_SAVE_CONFIG, "sC"}, - {USR_IN_GET_ALL, "gA"}, - {USR_IN_GET_VERSION, "gV"}, -// place new input packet code here, before USR_IN_MAX - {USR_IN_MAX, {0xff, 0xff}}, // "" -}; - -__weak usr_packet_t userOutputPackets[] = { -// Packet Type Packet Code - {USR_OUT_NONE, {0x00, 0x00}}, - {USR_OUT_TEST, "zT"}, - {USR_OUT_DATA1, "z1"}, - {USR_OUT_ANG1, "a1"}, - {USR_OUT_ANG2, "a2"}, -// place new type and code here - {USR_OUT_SCALED1, "s1"}, - {USR_OUT_EKF1, "e1"}, - {USR_OUT_EKF2, "e2"}, - {USR_OUT_RTK1, "K1"}, - {USR_OUT_POS, "pS"}, - {USR_OUT_SKY, "sK"}, - {USR_OUT_C1, "C1"}, //4331 - {USR_OUT_MAX, {0xff, 0xff}}, // "" -}; -static int _outputPacketType = USR_OUT_MAX; -static int _inputPacketType = USR_IN_MAX; uint8_t dataBuffer[512]; -int get_input_packet_type() -{ - return _inputPacketType; -} - /** **************************************************************************** * @name HandleUcbRx * @brief handles received ucb packets @@ -120,7 +81,6 @@ int get_input_packet_type() * FALSE if needing more to fill in a packet ******************************************************************************/ BOOL HandleUcbRx (UcbPacketStruct *ucbPacket) - { static int bytesInBuffer = 0, state = 0, crcError = 0, len = 0; static uint8_t *ptr; @@ -238,11 +198,9 @@ BOOL HandleUcbRx (UcbPacketStruct *ucbPacket) * @retval valid packet in packetPtr TRUE ******************************************************************************/ void HandleUcbTx (int port, UcbPacketStruct *ptrUcbPacket) - { - - UcbPacketCrcType crc; - uint8_t data[2]; + uint16_t crc; + uint8_t data[2]; /// get byte representation of packet type, index adjust required since sync /// isn't placed in data array @@ -257,69 +215,6 @@ void HandleUcbTx (int port, UcbPacketStruct *ptrUcbPacket) ptrUcbPacket->payload[ptrUcbPacket->payloadLength+1] = (crc >> 8) & 0xff; ptrUcbPacket->payload[ptrUcbPacket->payloadLength] = crc & 0xff; - if (gConfiguration.packetCode != 0x4331) //C1 - { - uart_write_bytes(port, (const char *)&ptrUcbPacket->sync_MSB, ptrUcbPacket->payloadLength + 7,1); - } - else - { - uart_write_bytes(UART_USER, (const char *)&ptrUcbPacket->payload, ptrUcbPacket->payloadLength,1); - } + uart_write_bytes(port, (const char *)&ptrUcbPacket->sync_MSB, ptrUcbPacket->payloadLength + 7,1); } /* end HandleUcbTx */ - -__weak 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; -} - -__weak void userPacketTypeToBytes(uint8_t *bytes) //TODO: -{ - 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; - } - -} -__weak int getUserPayloadLength(void) -{ - return RTK_OK; -} \ No newline at end of file diff --git a/Platform/Core/src/ucb_packet.c b/Platform/Core/src/ucb_packet.c index 94eb582..cecfee2 100644 --- a/Platform/Core/src/ucb_packet.c +++ b/Platform/Core/src/ucb_packet.c @@ -10,7 +10,7 @@ * PARTICULAR PURPOSE. *****************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -27,18 +27,13 @@ limitations under the License. #include // strcmp, strstr - -//#include "UserConfiguration.h" #include "ucb_packet.h" - +#include "user_message.h" // NEEDS TO BE CHECKED /// List of allowed packet codes ucb_packet_t ucbPackets[] = { // - {UCB_WRITE_APP, 0x5741}, // "WA" - {UCB_JUMP2_BOOT, 0x4A42}, // "JB" {UCB_J2IAP, 0x4A49}, // "JI" - {UCB_JUMP2_APP, 0x4A41}, // "JA" {UCB_PING, 0x504B}, // "PK" {UCB_ECHO, 0x4348}, // "CH" {UCB_GET_PACKET, 0x4750}, // "GP" @@ -49,7 +44,6 @@ ucb_packet_t ucbPackets[] = { // {UCB_UNLOCK_EEPROM, 0x5545}, // "UE" {UCB_READ_EEPROM, 0x5245}, // "RE" {UCB_WRITE_EEPROM, 0x5745}, // "WE" - {UCB_PROGRAM_RESET, 0x5052}, // "PR" {UCB_SOFTWARE_RESET, 0x5352}, // "SR" {UCB_WRITE_CAL, 0x5743}, // "WC" {UCB_IDENTIFICATION, 0x4944}, // "ID" @@ -62,8 +56,6 @@ ucb_packet_t ucbPackets[] = { // {UCB_FACTORY_1, 0x4631}, // "F1" {UCB_FACTORY_2, 0x4632}, // "F2" {UCB_FACTORY_M, 0x464D}, // "FM" - {UCB_MAG_CAL_1_COMPLETE, 0x4342}, // "CB" - {UCB_MAG_CAL_3_COMPLETE, 0x4344}, // "CD" {UCB_USER_OUT, 0x5550}, // "UP" {UCB_PKT_NONE, 0x0000} // " " should be last in the table as a end marker }; @@ -84,7 +76,7 @@ UcbPacketType UcbPacketBytesToPacketType (const uint8_t bytes []) { UcbPacketType packetType = UCB_ERROR_INVALID_TYPE; ucb_packet_t *pkt = ucbPackets; - UcbPacketCodeType receivedCode = (UcbPacketCodeType)(((bytes[0] & 0xff) << 8) | + uint16_t receivedCode = (uint16_t)(((bytes[0] & 0xff) << 8) | (bytes[1] & 0xff)); /// search through the packet code table for a matching code - check type @@ -143,89 +135,6 @@ void UcbPacketPacketTypeToBytes (UcbPacketType type, } /* end UcbPacketPacketTypeToBytes */ -/****************************************************************************** - * Function name: UcbPacketTypeToCode -* @brief Convert the packet type enum into packet code - For example UCB_PING ->0x504B ("PK") - * Trace: - * [SDD_UCB_UNKNOWN_01 <-- SRC_UCB_PKT_STR] - * [SDD_HANDLE_PKT <-- SRC_UCB_PKT_STR] - * @param [in] byte array, containing one byte - * @Retval length - ******************************************************************************/ -void UcbPacketPacketTypeToCode (UcbPacketType type, uint16_t *code) -{ -// carefull here, since for sake of speed it's indexing but not lookup through ucbPackets structure - if (type < UCB_PKT_NONE) { - *code = ucbPackets[type].packetCode; - }else{ - *code = ucbPackets[UCB_PKT_NONE].packetCode; - } -} -/* end UcbPacketPacketTypeToCode */ - - - -/** **************************************************************************** - * Function name: UcbPacketBytesToPayloadLength - * @brief Convert the packet bytes into the packet payload length - * Trace: - * [SDD_UCB_STORE_DATA <-- SRC_UCB_PAYLEN] - * [SDD_UCB_PKT_PAYLEN <-- SRC_UCB_PAYLEN] - * @param [in] byte array, containing one byte - * @Retval length - ******************************************************************************/ -uint8_t UcbPacketBytesToPayloadLength (const uint8_t bytes []) -{ - return (uint8_t)(bytes[0] & 0xff); -} -/* end UcbPacketBytesToPayloadLength */ - -/** **************************************************************************** - * @name UcbPacketPayloadLengthToBytes - * @brief Convert the payload length into bytes - * Trace: - * [SDD_UCB_PROCESS_OUT <-- SRC_UCB_PKT_LENBYT] - * [SDD_UCB_PKT_LENBYT <-- SRC_UCB_PKT_LENBYT] - * @param [in] type - payload type - * @param [out] byte array, containing one byte - * @Retval none - ******************************************************************************/ -void UcbPacketPayloadLengthToBytes (uint8_t length, - uint8_t bytes []) -{ - bytes[0] = (uint8_t)(length & 0xff); -} -/* end UcbPacketBytesToPayloadLength */ - -/** **************************************************************************** - * @name UcbPacketBytesToCrc - * Description: - * @param [in] byte array with byte[0] containing input value - * @retval N/A - ******************************************************************************/ -UcbPacketCrcType UcbPacketBytesToCrc (const uint8_t bytes []) -{ - return ((UcbPacketCrcType)(((bytes[0] & 0xff) << 8) | (bytes[1] & 0xff))); -} -/* end UcbPacketBytesToCrc */ - -/** **************************************************************************** - * @name UcbPacketCrcToBytes - * @broef This function converts a value into a 2 byte string. - * Trace: [SDD_UCB_PROCESS_OUT <-- SRC_UCB_PKT_CRCSTR] - * @param [in] crc - 16-bit value (crc value) - * @param [out] byte array with byte[0] containing the upper byte of input value - * @retval N/A - ******************************************************************************/ -void UcbPacketCrcToBytes (const UcbPacketCrcType crc, - uint8_t bytes []) -{ - bytes[0] = (uint8_t)((crc >> 8) & 0xff); - bytes[1] = (uint8_t)(crc & 0xff); -} -/* end UcbPacketCrcToBytes */ - /** **************************************************************************** * @name UcbPacketIsAnInputPacket @@ -252,7 +161,6 @@ BOOL UcbPacketIsAnInputPacket (UcbPacketType type) case UCB_UNLOCK_EEPROM: case UCB_READ_EEPROM: case UCB_WRITE_EEPROM: - case UCB_PROGRAM_RESET: case UCB_SOFTWARE_RESET: case UCB_WRITE_CAL: isAnInputPacket = TRUE; diff --git a/Platform/Driver/inc/RingBuffer.h b/Platform/Driver/inc/RingBuffer.h deleted file mode 100644 index ecafa43..0000000 --- a/Platform/Driver/inc/RingBuffer.h +++ /dev/null @@ -1,66 +0,0 @@ -/******************************************************************************* -* File Name : led.h -* Author : jacksun -* Revision : 1.0 -* Date : 12/11/2017 -* Description : led drive head file -* -* HISTORY*********************************************************************** -* 11/10/2019 | | Daich -* -*******************************************************************************/ -// RingBuffer.h writed by jacksun 2017.11.22 -#ifndef _RINGBUFFER_H_ -#define _RINGBUFFER_H_ - -#include - -typedef unsigned char u8; -typedef unsigned short u16; -// typedef unsigned int u32; - -typedef signed char s8; -typedef signed short s16; -// typedef signed int s32; - -typedef u8 ELEMENT_TYPE; -typedef struct -{ - ELEMENT_TYPE *pRing_buf; - u16 length; - u16 write_index; - u16 read_index; - -}RingBuffer; - -typedef struct - { - uint8_t* buffer; //FIFO数据 - uint16_t in; //入口下标 - uint16_t out; //出口下标 - uint16_t size; //FIFO大小 -}FIFO_Type; - -#define GPS_BUFF_SIZE (2000) -#define IMU_BUFF_SIZE 512 -#define DEBUG_BUFF_SIZE 128 - - - -int InitRingBuffer(RingBuffer *pRing,ELEMENT_TYPE * buff,u16 len); -int ReadRingBuffer(RingBuffer *pRing,ELEMENT_TYPE *pReadbuf,u16 rd_len); -int ReadRecentDataInRingBuffer(RingBuffer *pRing,ELEMENT_TYPE *pReadbuf,u16 rd_len); -int WriteRingBuffer(RingBuffer *pRing,ELEMENT_TYPE *pWrbuf,u16 wr_len); -int ReadAllDataNoRead(RingBuffer *pRing,ELEMENT_TYPE *pReadbuf); -int ReadAllDataNoReadLength(RingBuffer *pRing); -int WriteOneElementRingBuffer(RingBuffer *pRing,ELEMENT_TYPE element); -u16 GetLastWriteIndex(RingBuffer *pRing); -void FifoInit(FIFO_Type* fifo, uint8_t* buffer, uint16_t size); -uint16_t FifoGet(FIFO_Type* fifo, uint8_t* buffer, uint16_t len); -uint16_t FifoStatus(FIFO_Type* fifo); -void FifoPush(FIFO_Type* fifo, uint8_t* buffer, uint16_t size); - - -#endif - - diff --git a/Platform/Driver/inc/driver.h b/Platform/Driver/include/driver.h similarity index 97% rename from Platform/Driver/inc/driver.h rename to Platform/Driver/include/driver.h index 5dced47..1e14cfd 100644 --- a/Platform/Driver/inc/driver.h +++ b/Platform/Driver/include/driver.h @@ -16,7 +16,7 @@ #ifndef _DRIVER_H_ #define _DRIVER_H_ //#pragma once -#include "GlobalConstants.h" +#include "constants.h" typedef enum rtk_ret_e_ { diff --git a/Platform/Driver/inc/exit.h b/Platform/Driver/include/exit.h similarity index 100% rename from Platform/Driver/inc/exit.h rename to Platform/Driver/include/exit.h diff --git a/Platform/Driver/inc/led.h b/Platform/Driver/include/led.h similarity index 100% rename from Platform/Driver/inc/led.h rename to Platform/Driver/include/led.h diff --git a/Platform/Driver/inc/timer.h b/Platform/Driver/include/timer.h similarity index 86% rename from Platform/Driver/inc/timer.h rename to Platform/Driver/include/timer.h index 2de58bc..224c19a 100644 --- a/Platform/Driver/inc/timer.h +++ b/Platform/Driver/include/timer.h @@ -17,27 +17,22 @@ #define _TIMER_H_ //#pragma once #include -#ifndef BAREMETAL_OS - #include "cmsis_os.h" -#endif +#include "cmsis_os.h" typedef struct mcu_time_base_t_ -{ - time_t time; - time_t msec; +{ + time_t time; + time_t msec; } mcu_time_base_t; extern volatile mcu_time_base_t g_MCU_time; extern mcu_time_base_t imu_time; extern volatile mcu_time_base_t next_obs_time; - void MX_TIM1_Init(void); void MX_TIM_SENSOR_Init(void); time_t get_time_of_msec(); -volatile mcu_time_base_t* get_mcu_time(); +volatile mcu_time_base_t *get_mcu_time(); -#ifndef BAREMETAL_OS void release_sem(osSemaphoreId sem); -#endif #endif diff --git a/Platform/Driver/inc/uart.h b/Platform/Driver/include/uart.h similarity index 53% rename from Platform/Driver/inc/uart.h rename to Platform/Driver/include/uart.h index 61a0a38..9d4a059 100644 --- a/Platform/Driver/inc/uart.h +++ b/Platform/Driver/include/uart.h @@ -15,27 +15,23 @@ #define _UART_H_ //#pragma once #include "driver.h" -#include "RingBuffer.h" +#include "utils.h" #include -#ifndef BAREMETAL_OS - #include "osapi.h" - #include "osresources.h" -#else - #include "bare_osapi.h" -#endif +#include "osapi.h" + #include "stm32f4xx_hal.h" //#define OFFLINE_DEBUG #if 0 -#define UART_USER_BASE USART1 -#define UART_BT_BASE USART2 -#define UART_GPS_BASE USART3 -#define UART_DEBUG_BASE UART5 +#define UART_USER_BASE USART1 +#define UART_BT_BASE USART2 +#define UART_GPS_BASE USART3 +#define UART_DEBUG_BASE UART5 #endif -#define UART_USER_BASE UART5 //TODO: -#define UART_BT_BASE USART2 -#define UART_GPS_BASE USART3 -#define UART_DEBUG_BASE USART1 +#define UART_USER_BASE UART5 //TODO: +#define UART_BT_BASE USART2 +#define UART_GPS_BASE USART3 +#define UART_DEBUG_BASE USART1 extern UART_HandleTypeDef huart_debug; extern UART_HandleTypeDef huart_user; @@ -50,67 +46,66 @@ extern DMA_HandleTypeDef hdma_usart_gps_tx; extern DMA_HandleTypeDef hdma_usart_bt_rx; extern DMA_HandleTypeDef hdma_usart_bt_tx; -extern FIFO_Type uart_gps_rx_fifo; -extern FIFO_Type uart_debug_rx_fifo; -extern FIFO_Type uart_bt_rx_fifo; -extern FIFO_Type uart_user_rx_fifo; +extern fifo_type uart_gps_rx_fifo; +extern fifo_type uart_debug_rx_fifo; +extern fifo_type uart_bt_rx_fifo; +extern fifo_type uart_user_rx_fifo; #define USER_UART_DMA_FIFO #ifdef USER_UART_DMA_FIFO -#define UART_TX_FIFO_MANAGE_NUM 1 +#define UART_TX_FIFO_MANAGE_NUM 1 -#define DMA_TX_FIFO_BUF_SIZE 2048 +#define DMA_TX_FIFO_BUF_SIZE 2048 typedef struct _uart_tx_fifo { - FIFO_Type uart_tx_fifo; + fifo_type uart_tx_fifo; uint32_t frame_num; uint32_t data_total_num; uint32_t is_data_available; - uint8_t is_dma_busy; //TODO: -}uart_tx_dma_fifo_s; + uint8_t is_dma_busy; //TODO: +} uart_tx_dma_fifo_s; #endif - - -//#define UART_BLOCK -typedef enum { - UART_USER = 0x00, - UART_BT = 0x01, - UART_GPS = 0x02, - UART_DEBUG = 0x03, +//#define UART_BLOCK +typedef enum +{ + UART_USER = 0x00, + UART_BT = 0x01, + UART_GPS = 0x02, + UART_DEBUG = 0x03, UART_MAX, } uart_port_e; -struct uart_config_t { +struct uart_config_t +{ int uart_base_addr; uint32_t rec_buff_size; - uint8_t* rec_buff; - DMA_HandleTypeDef* hdma_usart_rx; - DMA_HandleTypeDef* hdma_usart_tx; + uint8_t *rec_buff; + DMA_HandleTypeDef *hdma_usart_rx; + DMA_HandleTypeDef *hdma_usart_tx; }; -typedef struct { - uart_port_e uart_num; +typedef struct +{ + uart_port_e uart_num; int baudrate; #ifdef UART_BLOCK osSemaphoreId rx_sem; - osSemaphoreId tx_sem; -#endif -#ifndef BAREMETAL_OS - osSemaphoreId uart_idle_sem; + osSemaphoreId tx_sem; #endif + osSemaphoreId uart_idle_sem; bool init_flag; - FIFO_Type* uart_rx_fifo; - UART_HandleTypeDef* huart; - DMA_HandleTypeDef* hdma_usart_rx; - DMA_HandleTypeDef* hdma_usart_tx; + fifo_type *uart_rx_fifo; + UART_HandleTypeDef *huart; + DMA_HandleTypeDef *hdma_usart_rx; + DMA_HandleTypeDef *hdma_usart_tx; } uart_obj_t; -int uart_read_bytes(uart_port_e uart_num, uint8_t* buf, uint32_t len, TickType_t ticks_to_wait); -int uart_driver_install(uart_port_e uart_num, FIFO_Type* uart_rx_fifo,UART_HandleTypeDef* huart,int baudrate); +int uart_read_bytes(uart_port_e uart_num, uint8_t *buf, uint32_t len, TickType_t ticks_to_wait); +int uart_driver_install(uart_port_e uart_num, fifo_type *uart_rx_fifo, UART_HandleTypeDef *huart, int baudrate); rtk_ret_e uart_driver_delete(uart_port_e uart_num); -int uart_write_bytes(uart_port_e uart_num, const char* src, size_t size, bool is_wait); +int uart_write_bytes(uart_port_e uart_num, const char *src, size_t size, bool is_wait); void update_fifo_in(uart_port_e uart_num); -rtk_ret_e uart_sem_wait(uart_port_e uart_num,uint32_t millisec); +rtk_ret_e uart_sem_wait(uart_port_e uart_num, uint32_t millisec); #endif \ No newline at end of file diff --git a/Platform/Driver/src/RingBuffer.c b/Platform/Driver/src/RingBuffer.c deleted file mode 100644 index 23624a0..0000000 --- a/Platform/Driver/src/RingBuffer.c +++ /dev/null @@ -1,206 +0,0 @@ -// RingBuffer.c needed by park_algo_jack.c writed by jacksun 2017.11.12 -#include "RingBuffer.h" -#include - - -void FifoInit(FIFO_Type* fifo, uint8_t* buffer, uint16_t size) -{ - fifo->buffer = buffer; - fifo->in = 0; - fifo->out = 0; - fifo->size = size; -} - -void FifoPush(FIFO_Type* fifo, uint8_t* buffer, uint16_t size) -{ - uint16_t i; - - for (i = 0; i < size; i++) - { - fifo->buffer[(fifo->in+i)%fifo->size] = buffer[i]; - } - fifo->in = (fifo->in + size)%fifo->size; -} - -uint16_t FifoGet(FIFO_Type* fifo, uint8_t* buffer, uint16_t len) -{ - uint16_t lenght; - uint16_t in = fifo->in; - uint16_t i; - lenght = (in + fifo->size - fifo->out)%fifo->size; - if(lenght > len) - lenght = len; - for(i = 0; i < lenght; i++) - { - buffer[i] = fifo->buffer[(fifo->out + i)%fifo->size]; - } - fifo->out = (fifo->out + lenght)%fifo->size; - return lenght; -} - -uint16_t FifoStatus(FIFO_Type* fifo) -{ - uint16_t lenght; - lenght = (fifo->in + fifo->size - fifo->out)%fifo->size; - return lenght; -} - - - -int InitRingBuffer(RingBuffer *pRing,ELEMENT_TYPE * buff,u16 len) -{ - memset(pRing->pRing_buf,0,sizeof(ELEMENT_TYPE) * pRing->length); - pRing->pRing_buf =buff; - pRing->write_index =0; - pRing->read_index =0; - pRing->length =len; - return 0; -} -int testdata=0; -int ReadRingBuffer(RingBuffer *pRing,ELEMENT_TYPE *pReadbuf,u16 rd_len) -{ - if(rd_len > pRing->length) - return -1; - - - if((pRing->read_index+rd_len) >= pRing->length) - { - memcpy(pReadbuf,&pRing->pRing_buf[pRing->read_index],sizeof(ELEMENT_TYPE) * (pRing->length - pRing->read_index)); - memcpy(&pReadbuf[pRing->length - pRing->read_index],&pRing->pRing_buf[0],sizeof(ELEMENT_TYPE) * (rd_len - (pRing->length - pRing->read_index)) ); - - pRing->read_index = ((pRing->read_index+rd_len )- pRing->length)%pRing->length ; - } - else - { - memcpy(pReadbuf,&pRing->pRing_buf[pRing->read_index],sizeof(ELEMENT_TYPE) * rd_len); - pRing->read_index= (pRing->read_index+rd_len)%pRing->length; - } - return 0; -} -int ReadRecentDataInRingBuffer(RingBuffer *pRing,ELEMENT_TYPE *pReadbuf,u16 rd_len) -{ - - int num,rd_tail_num=0; - - if(rd_len > pRing->length) - return -1; - if(rd_len <= pRing->write_index) - { - memcpy(pReadbuf,&pRing->pRing_buf[pRing->write_index-rd_len],sizeof(ELEMENT_TYPE) *rd_len); - } - else - { - rd_tail_num=rd_len-pRing->write_index; - - memcpy(pReadbuf,&pRing->pRing_buf[pRing->length-rd_tail_num],sizeof(ELEMENT_TYPE) * rd_tail_num); - memcpy(&pReadbuf[rd_tail_num],&pRing->pRing_buf[0],sizeof(ELEMENT_TYPE) * pRing->write_index); - } - - num = rd_len; - - return num; -} - -int ReadAllDataNoRead(RingBuffer *pRing,ELEMENT_TYPE *pReadbuf) -{ - int num; - - if(pRing->write_index==pRing->read_index) - { - return 0; - } - - if(pRing->write_index>pRing->read_index) - { - memcpy(pReadbuf,&pRing->pRing_buf[pRing->read_index],sizeof(ELEMENT_TYPE) * (pRing->write_index-pRing->read_index)); - num = pRing->write_index-pRing->read_index; - if(num==1200) - { - testdata=1; - - } - - - } - else - { - memcpy(pReadbuf,&pRing->pRing_buf[pRing->read_index],sizeof(ELEMENT_TYPE) * (pRing->length - pRing->read_index)); - memcpy(&pReadbuf[pRing->length - pRing->read_index],&pRing->pRing_buf[0],sizeof(ELEMENT_TYPE) * pRing->write_index); - num = pRing->write_index+( pRing->length - pRing->read_index); - if(num==1200) - { - testdata=2; - } - } - pRing->read_index = pRing->write_index; - - return num; -} - -int ReadAllDataNoReadLength(RingBuffer *pRing) -{ - int num; - - if(pRing->write_index>=pRing->read_index) - { - num = pRing->write_index-pRing->read_index; - } - else - { - num = pRing->write_index+( pRing->length - pRing->read_index); - } - - return num; -} - - -int WriteOneElementRingBuffer(RingBuffer *pRing,ELEMENT_TYPE element) -{ - - if(pRing->write_indexlength) - pRing->pRing_buf[pRing->write_index++]=element; - else - pRing->write_index=0; - - return 0; -} - -u16 GetLastWriteIndex(RingBuffer *pRing) -{ - u16 index=0; - if(pRing->write_index==0) - index=pRing->length-1; - else - index=pRing->write_index-1; - - return index; - -} - - -int WriteRingBuffer(RingBuffer *pRing,ELEMENT_TYPE *pWrbuf,u16 wr_len) -{ - if(wr_len>pRing->length) - { - return -1; - - } - if(wr_len==0) - { - return 0; - } - - if((pRing->write_index + wr_len) > pRing->length) - { - memcpy(&pRing->pRing_buf[pRing->write_index], pWrbuf, sizeof(ELEMENT_TYPE) *(pRing->length - pRing->write_index)); - memcpy(pRing->pRing_buf,&pWrbuf[ pRing->length - pRing->write_index],sizeof(ELEMENT_TYPE) *(wr_len-(pRing->length - pRing->write_index))); - pRing->write_index=((pRing->write_index + wr_len) -pRing->length)%pRing->length; - } - else - { - memcpy(&pRing->pRing_buf[pRing->write_index], pWrbuf, sizeof(ELEMENT_TYPE) *wr_len); - pRing->write_index = (pRing->write_index + wr_len)%pRing->length; - } - - return wr_len; -} diff --git a/Platform/Driver/src/exit.c b/Platform/Driver/src/exit.c index 1947387..abc71c9 100644 --- a/Platform/Driver/src/exit.c +++ b/Platform/Driver/src/exit.c @@ -13,18 +13,13 @@ #include "boardDefinition.h" #include "exit.h" #include "timer.h" +#include "gnss_data_api.h" #include "led.h" -#include "gpsAPI.h" -#include "osresources.h" #include "bsp.h" -//#include "taskDataAcquisition.h" #include "uart.h" -#ifndef BAREMETAL_OS #include "osapi.h" -#endif -volatile mcu_time_base_t g_obs_rcv_time; - +volatile mcu_time_base_t g_obs_rcv_time; __weak uint8_t get_gnss_signal_flag() @@ -51,7 +46,6 @@ void pps_exit_init(void) } -extern GpsData_t *gGpsDataPtr; extern volatile mcu_time_base_t g_MCU_time; uint8_t g_pps_flag = 0; extern uint32_t usCnt; @@ -68,45 +62,19 @@ void ST_PPS_IRQ(void) { if (g_MCU_time.msec < 500) { -#ifndef BAREMETAL_OS - if(dataAcqSem != 0) - { - release_sem(dataAcqSem); - } -#endif + g_MCU_time.msec = 499; + htim_sensor.Instance->CNT = 44960; + } + else if(g_MCU_time.msec > 500) + { + g_MCU_time.msec = 500; + htim_sensor.Instance->CNT = 1; } - usCnt = 0; - - - - g_MCU_time.msec = 500; - htim_sensor.Instance->CNT = 0; if(get_gnss_signal_flag() && (g_MCU_time.msec - g_obs_rcv_time.msec) >= 0 && (g_MCU_time.time == g_obs_rcv_time.time)) - //g_MCU_time.time = gGpsDataPtr->rtcm.obs[0].time.time; - g_MCU_time.time = get_obs_time(); - } - // else - // { - // g_MCU_time.msec = 0; - // usCnt = 100; - // g_MCU_time.time = gGpsDataPtr->rtcm.obs[0].time.time + 1; - // release_sem(dataAcqSem); - // } + g_MCU_time.time = get_obs_time(); - - // if (gGpsDataPtr->rtcm.obs[0].time.sec < 0.5) - // { - // g_MCU_time.msec = 500; - // usCnt = 0; - // g_MCU_time.time = gGpsDataPtr->rtcm.obs[0].time.time; - // } - // else - // { - // g_MCU_time.msec = 0; - // usCnt = 0; - // g_MCU_time.time = gGpsDataPtr->rtcm.obs[0].time.time + 1; - // } + } HAL_GPIO_EXTI_IRQHandler(ST_PPS_PIN); OSExitISR(); } diff --git a/Platform/Driver/src/led.c b/Platform/Driver/src/led.c index 124d60c..9c3de29 100644 --- a/Platform/Driver/src/led.c +++ b/Platform/Driver/src/led.c @@ -16,11 +16,7 @@ #include "stm32f4xx_hal.h" #include "led.h" #include "driver.h" -#include "log.h" -#ifndef BAREMETAL_OS -#include "FreeRTOS.h" -#include "osapi.h" -#endif + rtk_ret_e led_driver_install() { diff --git a/Platform/Driver/src/timer.c b/Platform/Driver/src/timer.c index b7e94a2..7c41c1d 100644 --- a/Platform/Driver/src/timer.c +++ b/Platform/Driver/src/timer.c @@ -14,16 +14,11 @@ * Description: modify double get_time_of_msec(),Otherwise, a data overflow will occur *******************************************************************************/ #include "timer.h" -#include "GlobalConstants.h" -#ifndef BAREMETAL_OS - #include "osapi.h" -#else - #include "bare_osapi.h" -#endif +#include "constants.h" +#include "osapi.h" #include "rtcm.h" -#include "gpsAPI.h" #include "stm32f4xx_hal.h" - +#include "main.h" #define SENSOR_TIMER_IRQ TIM2_IRQHandler TIM_HandleTypeDef htim1; @@ -104,16 +99,12 @@ void MX_TIM_SENSOR_Init(void) } volatile mcu_time_base_t g_MCU_time; -#ifndef BAREMETAL_OS -extern osSemaphoreId dataAcqSem; -#endif -#ifndef BAREMETAL_OS + void release_sem(osSemaphoreId sem) { osSemaphoreRelease(sem); } -#endif time_t get_time_of_msec() { return (g_MCU_time.time * 1000 + g_MCU_time.msec); @@ -129,23 +120,16 @@ static void timer_isr_if(TIM_HandleTypeDef* timer) if(timer == &htim_sensor) { usCnt ++; - // if(usCnt >= 100) { - usCnt = 0; g_MCU_time.msec += 1; if(g_MCU_time.msec >= 1000) { g_MCU_time.msec = 0; g_MCU_time.time ++; - // if(gPtrGnssSol -> gpsUpdate == 1) - // gPtrGnssSol->gpsUpdate = 2; } if(g_MCU_time.msec % 20 == 0) { -#ifndef BAREMETAL_OS - release_sem(dataAcqSem); -#endif - // NSS_Toggle(); + release_sem(g_sem_imu_data_acq); } } diff --git a/Platform/Driver/src/uart.c b/Platform/Driver/src/uart.c index cff4479..e4623c7 100644 --- a/Platform/Driver/src/uart.c +++ b/Platform/Driver/src/uart.c @@ -26,26 +26,15 @@ #include "stm32f4xx_hal.h" #include "uart.h" #include "driver.h" -#include "log.h" -#include "RingBuffer.h" +#include "utils.h" #include "boardDefinition.h" -#include "shell.h" -#ifndef BAREMETAL_OS #include "FreeRTOS.h" #include "osapi.h" -#else -#include "bare_osapi.h" -#endif + // osSemaphoreDef(RX_ACQ_SEM); // osSemaphoreDef(TX_ACQ_SEM); -#define UART_CHECK(a, str, ret_val) \ - if (!(a)) { \ - RTK_LOG(ATS_LOG_INFO,UART_TAG,"%s(%d): %s", __FUNCTION__, __LINE__, str); \ - return (ret_val); \ - } - static uint8_t uart_user_buff[IMU_BUFF_SIZE]; static uint8_t uart_bt_buff[GPS_BUFF_SIZE]; @@ -53,9 +42,6 @@ static uint8_t uart_gps_buff[GPS_BUFF_SIZE]; static uint8_t uart_debug_buff[GPS_BUFF_SIZE]; -static const char* UART_TAG = "drive_uart"; - - DMA_HandleTypeDef hdma_usart_user_rx; DMA_HandleTypeDef hdma_usart_user_tx; DMA_HandleTypeDef hdma_uart_debug_rx; @@ -65,10 +51,10 @@ DMA_HandleTypeDef hdma_usart_gps_tx; DMA_HandleTypeDef hdma_usart_bt_rx; DMA_HandleTypeDef hdma_usart_bt_tx; -FIFO_Type uart_gps_rx_fifo; -FIFO_Type uart_debug_rx_fifo; -FIFO_Type uart_bt_rx_fifo; -FIFO_Type uart_user_rx_fifo; +fifo_type uart_gps_rx_fifo; +fifo_type uart_debug_rx_fifo; +fifo_type uart_bt_rx_fifo; +fifo_type uart_user_rx_fifo; #ifdef USER_UART_DMA_FIFO static uint8_t user_uart_dma_tx_buff[DMA_TX_FIFO_BUF_SIZE]; @@ -113,28 +99,24 @@ uart_obj_t uart_obj[UART_MAX]; static int uart_receive_dma(uart_port_e uart_num) { - UART_CHECK((p_uart_obj[uart_num]), "uart driver error", (-1)); HAL_UART_Receive_DMA(p_uart_obj[uart_num]->huart, p_uart_obj[uart_num]->uart_rx_fifo->buffer, p_uart_obj[uart_num]->uart_rx_fifo->size); return RTK_OK; } static int uart_rx_dma_enable(uart_port_e uart_num) { - UART_CHECK((p_uart_obj[uart_num]),"uart driver error",(-1)); __HAL_DMA_ENABLE(p_uart_obj[uart_num]->hdma_usart_rx); return RTK_OK; } static int uart_dma_stop(uart_port_e uart_num) { - UART_CHECK((p_uart_obj[uart_num]),"uart driver error",(-1)); HAL_UART_DMAStop(p_uart_obj[uart_num]->huart); return RTK_OK; } static int uart_dma_enanle_it(uart_port_e uart_num,uint32_t it_type) { - UART_CHECK((p_uart_obj[uart_num]),"uart driver error",(-1)); __HAL_UART_ENABLE_IT(p_uart_obj[uart_num]->huart,it_type); return RTK_OK; } @@ -145,9 +127,7 @@ int uart_read_bytes(uart_port_e uart_num, uint8_t* buf, uint32_t len, TickType_t uint16_t lenght; uint16_t in = p_uart_obj[uart_num]->uart_rx_fifo->in; uint16_t i; - UART_CHECK((uart_num < UART_MAX), "uart_num error", (-1)); - UART_CHECK((buf), "uart data null", (-1)); - UART_CHECK((p_uart_obj[uart_num]), "uart driver error", (-1)); + #ifdef UART_BLOCK if(ticks_to_wait > 0) { @@ -174,22 +154,18 @@ int uart_read_bytes(uart_port_e uart_num, uint8_t* buf, uint32_t len, TickType_t static uint8_t data_to_write[DMA_TX_FIFO_BUF_SIZE]; int uart_write_bytes(uart_port_e uart_num, const char* src, size_t size, bool is_wait) //TODO: { - UART_CHECK((uart_num < UART_MAX), "uart_num error", (-1)); - UART_CHECK((p_uart_obj[uart_num]), "uart driver error", (-1)); - UART_CHECK(src, "buffer null", (-1)); #ifdef USER_UART_DMA_FIFO if(uart_num == UART_USER) { - FifoPush(&user_uart_dma_tx_fifo.uart_tx_fifo,(uint8_t*)src,size); + fifo_push(&user_uart_dma_tx_fifo.uart_tx_fifo,(uint8_t*)src,size); user_uart_dma_tx_fifo.frame_num+= 1; user_uart_dma_tx_fifo.data_total_num+= size; if(user_uart_dma_tx_fifo.data_total_num > DMA_TX_FIFO_BUF_SIZE) { - RTK_LOG(ATS_LOG_INFO,UART_TAG, "UART tx buf overflow"); } if(p_uart_obj[UART_USER]->huart->gState == HAL_UART_STATE_READY) { - int data_len = FifoGet(&user_uart_dma_tx_fifo.uart_tx_fifo,data_to_write,DMA_TX_FIFO_BUF_SIZE); + int data_len = fifo_get(&user_uart_dma_tx_fifo.uart_tx_fifo,data_to_write,DMA_TX_FIFO_BUF_SIZE); user_uart_dma_tx_fifo.frame_num = 0; user_uart_dma_tx_fifo.data_total_num = 0; HAL_UART_Transmit_DMA(p_uart_obj[UART_USER]->huart, data_to_write, data_len); @@ -203,35 +179,28 @@ int uart_write_bytes(uart_port_e uart_num, const char* src, size_t size, bool is { if(is_wait == 0) return 0; -#ifndef BAREMETAL_OS osDelay(1); -#endif } } return RTK_OK; } -int uart_driver_install(uart_port_e uart_num, FIFO_Type* uart_rx_fifo,UART_HandleTypeDef* huart,int baudrate) +int uart_driver_install(uart_port_e uart_num, fifo_type* uart_rx_fifo,UART_HandleTypeDef* huart,int baudrate) { rtk_ret_e ret; - UART_CHECK((uart_num < UART_MAX), "uart_num error", RTK_FAIL); if(p_uart_obj[uart_num] == NULL) { p_uart_obj[uart_num] = &uart_obj[uart_num]; if(p_uart_obj[uart_num] == NULL) { - RTK_LOG(ATS_LOG_INFO,UART_TAG, "UART driver malloc error"); return RTK_FAIL; } -#ifndef BAREMETAL_OS osSemaphoreDef(UART_IDLE_SEM); if((uart_num == UART_BT) || (uart_num == UART_DEBUG)) - //if((uart_num == UART_BT)) { p_uart_obj[uart_num]->uart_idle_sem = osSemaphoreCreate(osSemaphore(UART_IDLE_SEM), 1); } -#endif p_uart_obj[uart_num]->hdma_usart_rx = uart_config[uart_num].hdma_usart_rx; p_uart_obj[uart_num]->hdma_usart_tx = uart_config[uart_num].hdma_usart_tx; p_uart_obj[uart_num]->uart_num = uart_num; @@ -239,7 +208,7 @@ int uart_driver_install(uart_port_e uart_num, FIFO_Type* uart_rx_fifo,UART_Handl //uint8_t* uart_x_buff = (uint8_t*)malloc(sizeof(uint8_t) * uart_config[uart_num].rec_buff_size); - FifoInit(uart_rx_fifo, uart_config[uart_num].rec_buff, uart_config[uart_num].rec_buff_size); + fifo_init(uart_rx_fifo, uart_config[uart_num].rec_buff, uart_config[uart_num].rec_buff_size); p_uart_obj[uart_num]->uart_rx_fifo = uart_rx_fifo; p_uart_obj[uart_num]->huart = huart; @@ -253,7 +222,6 @@ int uart_driver_install(uart_port_e uart_num, FIFO_Type* uart_rx_fifo,UART_Handl } else { - RTK_LOG(ATS_LOG_INFO,UART_TAG, "UART driver already installed"); return RTK_FAIL; } p_uart_obj[uart_num]->huart->Instance = (USART_TypeDef*)(uart_config[uart_num].uart_base_addr); @@ -289,7 +257,7 @@ int uart_driver_install(uart_port_e uart_num, FIFO_Type* uart_rx_fifo,UART_Handl user_uart_dma_tx_fifo.is_dma_busy = 0; user_uart_dma_tx_fifo.frame_num = 0; user_uart_dma_tx_fifo.is_data_available = 0; - FifoInit(&user_uart_dma_tx_fifo.uart_tx_fifo, user_uart_dma_tx_buff, DMA_TX_FIFO_BUF_SIZE); + fifo_init(&user_uart_dma_tx_fifo.uart_tx_fifo, user_uart_dma_tx_buff, DMA_TX_FIFO_BUF_SIZE); } #endif return ret; @@ -324,7 +292,6 @@ static void uart_isr_if(uart_port_e uart_num) uart_dma_enanle_it(uart_num,UART_IT_IDLE); } HAL_UART_IRQHandler(p_uart_obj[uart_num]->huart); -#ifndef BAREMETAL_OS if((uart_num == UART_BT) || (uart_num == UART_DEBUG)) { if (RESET == __HAL_UART_GET_FLAG(p_uart_obj[uart_num]->huart, UART_FLAG_IDLE)) @@ -332,7 +299,6 @@ static void uart_isr_if(uart_port_e uart_num) osSemaphoreRelease(p_uart_obj[uart_num]->uart_idle_sem); } } -#endif #ifdef UART_BLOCK osSemaphoreRelease(p_uart_obj[uart_num]->rx_sem); #endif @@ -456,9 +422,7 @@ rtk_ret_e uart_sem_wait(uart_port_e uart_num,uint32_t millisec) { if((uart_num == UART_BT) || (uart_num == UART_DEBUG)) { -#ifndef BAREMETAL_OS if(osSemaphoreWait(p_uart_obj[uart_num]->uart_idle_sem,millisec) == osOK) -#endif { return RTK_SEM_OK; } diff --git a/Platform/Filter/src/filter.c b/Platform/Filter/src/filter.c index e42f19f..fd789eb 100644 --- a/Platform/Filter/src/filter.c +++ b/Platform/Filter/src/filter.c @@ -15,17 +15,12 @@ #include // malloc #include // fabs() -#include "GlobalConstants.h" +#include "constants.h" #include "sensorsAPI.h" #include "Indices.h" #include "filter.h" #include "xbowsp_algorithm.h" -#ifndef BAREMETAL_OS - //#include "sensor.h" - //#include "sensors_data.h" -#else - #include "bare_osapi.h" -#endif + // Butterworth (IIR) low-pass filter coefficients Q27 // 200 Hz Sampling static int32_t b_2_Hz_iir_200HzSamp[] = {134217728, -256516528, 122805978}; diff --git a/Core/Cjson/inc/cJSON.h b/Platform/cJSON/inc/cJSON.h similarity index 100% rename from Core/Cjson/inc/cJSON.h rename to Platform/cJSON/inc/cJSON.h diff --git a/Core/Cjson/inc/json_parse.h b/Platform/cJSON/inc/json_parse.h similarity index 100% rename from Core/Cjson/inc/json_parse.h rename to Platform/cJSON/inc/json_parse.h diff --git a/Core/Cjson/src/cJSON.c b/Platform/cJSON/src/cJSON.c similarity index 100% rename from Core/Cjson/src/cJSON.c rename to Platform/cJSON/src/cJSON.c diff --git a/Platform/cJSON/src/json_parse.c b/Platform/cJSON/src/json_parse.c new file mode 100644 index 0000000..5b0ba51 --- /dev/null +++ b/Platform/cJSON/src/json_parse.c @@ -0,0 +1,171 @@ +/******************************************************************************* +* File Name : json_parse.c +* Author : Daich +* Revision : 1.0 +* Date : 29/09/2019 +* Description : json_parse.c +* +* HISTORY*********************************************************************** +* 29/09/2019 | | Daich +* Description: add send_rtk_json_message +*******************************************************************************/ + +#include +#include +#include + +#include "cJSON.h" +#include "json_parse.h" +#include "app_version.h" +#include "configuration.h" +#include "ucb_packet.h" +#include "uart.h" +#include "platformAPI.h" +#include "user_config.h" + +int change_item(char *text, char *json_to_write, char *key, char *value) +{ + char *out; + cJSON *root; + root = cJSON_Parse(text); + if (!root) + { +#ifdef DEVICE_DEBUG + printf("not json format\r\n"); +#endif + return json_false; + } + else + { + cJSON *item_replace = cJSON_GetObjectItem(root, "openrtk config"); + cJSON_ReplaceItemInObject(item_replace, key, cJSON_CreateString(value)); +#if 0 + item = cJSON_GetObjectItem(root,"rtk config"); + memcpy(cJSON_GetObjectItem(item,key)->valuestring,value,strlen(value)); // how to modify value safety? +#endif + out = cJSON_Print(root); + { + memcpy(json_to_write, out, strlen(out) + 1); + } + cJSON_Delete(root); + free(out); + return json_true; + } +} + +void create_json_object(cJSON **json) +{ + *json = cJSON_CreateObject(); +} + +char *get_rtk_json_item_value(cJSON *json, char *key) //TODO: +{ + //int json_array_size = cJSON_GetArraySize(); +#if 1 + if (json == NULL) + { + return NULL; + } + int json_item_count = cJSON_GetArraySize(json); + for (int i = 0; i < json_item_count; i++) + { + cJSON *item = cJSON_GetArrayItem(json, i); + if (item->type == cJSON_Object) + { + cJSON *item_sub = cJSON_GetObjectItem(item, key); + if (item_sub != NULL) + { + return item_sub->valuestring; + } +#if 0 + for(int j=0;j < cJSON_GetArraySize(item); j++) + { + if + } +#endif + } + } +#endif + return NULL; +} +uint32_t GetUnitSerialNum(); +void send_rtk_json_message(cJSON *root) +{ + cJSON *fmt; + char *out; + //root=cJSON_CreateObject(); + if (root != NULL) + { +#ifdef DEVICE_DEBUG + printf("root = %p\n", root); +#endif + char compile_time[50] = __DATE__; + //root = cJSON_CreateObject(); + cJSON_AddItemToObject(root, "openrtk config", fmt = cJSON_CreateObject()); + cJSON_AddItemToObject(fmt, "Product Name", cJSON_CreateString(PRODUCT_NAME_STRING)); + cJSON_AddItemToObject(fmt, "Product PN", cJSON_CreateString((const char *)platformBuildInfo())); + cJSON_AddItemToObject(fmt, "Product SN", cJSON_CreateNumber(GetUnitSerialNum())); + cJSON_AddItemToObject(fmt, "Version", cJSON_CreateString(APP_VERSION_STRING)); + cJSON_AddItemToObject(fmt, "Compile Time", cJSON_CreateString(compile_time)); + int packet_type = configGetPacketCode(); + char packet_type_str[5] = {0}; + packet_type_str[0] = packet_type >> 8; + packet_type_str[1] = packet_type; + cJSON_AddItemToObject(fmt, "userPacketType", cJSON_CreateString(packet_type_str)); + + float *ins_para = get_user_ins_para(); + cJSON_AddItemToObject(fmt, "leverArmBx", cJSON_CreateNumber(*ins_para)); + cJSON_AddItemToObject(fmt, "leverArmBy", cJSON_CreateNumber(*(ins_para + 1))); + cJSON_AddItemToObject(fmt, "leverArmBz", cJSON_CreateNumber(*(ins_para + 2))); + cJSON_AddItemToObject(fmt, "pointOfInterestBx", cJSON_CreateNumber(*(ins_para + 3))); + cJSON_AddItemToObject(fmt, "pointOfInterestBy", cJSON_CreateNumber(*(ins_para + 4))); + cJSON_AddItemToObject(fmt, "pointOfInterestBz", cJSON_CreateNumber(*(ins_para + 5))); + cJSON_AddItemToObject(fmt, "rotationRbvx", cJSON_CreateNumber(*(ins_para + 6))); + cJSON_AddItemToObject(fmt, "rotationRbvy", cJSON_CreateNumber(*(ins_para + 7))); + cJSON_AddItemToObject(fmt, "rotationRbvz", cJSON_CreateNumber(*(ins_para + 8))); + out = cJSON_Print(root); + //cJSON_Delete(root); //not delete other code use +#ifdef DEVICE_DEBUG + printf("%s\n", out); +#endif + uart_write_bytes(UART_BT, out, strlen(out), 1); + free(out); + } +} +void send_rtk_json_to_esp32() +{ + cJSON *root, *fmt; + char *out; + root = cJSON_CreateObject(); + char compile_time[50] = __DATE__; + root = cJSON_CreateObject(); + cJSON_AddItemToObject(root, "openrtk config", fmt = cJSON_CreateObject()); + cJSON_AddItemToObject(fmt, "Product Name", cJSON_CreateString(PRODUCT_NAME_STRING)); + cJSON_AddItemToObject(fmt, "Product PN", cJSON_CreateString((const char *)platformBuildInfo())); + cJSON_AddItemToObject(fmt, "Product SN", cJSON_CreateNumber(GetUnitSerialNum())); + cJSON_AddItemToObject(fmt, "Version", cJSON_CreateString(APP_VERSION_STRING)); + cJSON_AddItemToObject(fmt, "Compile Time", cJSON_CreateString(compile_time)); + int packet_type = configGetPacketCode(); + char packet_type_str[5] = {0}; + packet_type_str[0] = packet_type >> 8; + packet_type_str[1] = packet_type; + cJSON_AddItemToObject(fmt, "userPacketType", cJSON_CreateString(packet_type_str)); + + float *ins_para = get_user_ins_para(); + cJSON_AddItemToObject(fmt, "leverArmBx", cJSON_CreateNumber(*ins_para)); + cJSON_AddItemToObject(fmt, "leverArmBy", cJSON_CreateNumber(*(ins_para + 1))); + cJSON_AddItemToObject(fmt, "leverArmBz", cJSON_CreateNumber(*(ins_para + 2))); + cJSON_AddItemToObject(fmt, "pointOfInterestBx", cJSON_CreateNumber(*(ins_para + 3))); + cJSON_AddItemToObject(fmt, "pointOfInterestBy", cJSON_CreateNumber(*(ins_para + 4))); + cJSON_AddItemToObject(fmt, "pointOfInterestBz", cJSON_CreateNumber(*(ins_para + 5))); + cJSON_AddItemToObject(fmt, "rotationRbvx", cJSON_CreateNumber(*(ins_para + 6))); + cJSON_AddItemToObject(fmt, "rotationRbvy", cJSON_CreateNumber(*(ins_para + 7))); + cJSON_AddItemToObject(fmt, "rotationRbvz", cJSON_CreateNumber(*(ins_para + 8))); + out = cJSON_Print(root); + cJSON_Delete(root); +#ifdef DEVICE_DEBUG + printf("%s\n", out); +#endif + uart_write_bytes(UART_BT, out, strlen(out), 1); + free(out); +} diff --git a/Platform/commAPI.h b/Platform/commAPI.h index a885143..c908db0 100644 --- a/Platform/commAPI.h +++ b/Platform/commAPI.h @@ -8,7 +8,7 @@ * *****************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. diff --git a/Core/Common/include/GlobalConstants.h b/Platform/common/include/constants.h similarity index 63% rename from Core/Common/include/GlobalConstants.h rename to Platform/common/include/constants.h index 17d0e5e..ebea2fb 100644 --- a/Core/Common/include/GlobalConstants.h +++ b/Platform/common/include/constants.h @@ -1,8 +1,8 @@ /******************************************************************************* - * File: GlobalConstants.h + * @file: constants.h *******************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -18,8 +18,8 @@ limitations under the License. *******************************************************************************/ -#ifndef GLOBALCONSTANTS_H -#define GLOBALCONSTANTS_H +#ifndef _CONSTANTS_H +#define _CONSTANTS_H #define BMI_RS 1 #define MAXIM_RS 2 @@ -136,20 +136,6 @@ typedef unsigned char BOOL; #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 - RTCM3 = 5, - UNKNOWN = 0xFF -} enumGPSProtocol; - // Algorithm specifiers #define IMU 0 @@ -186,6 +172,65 @@ typedef enum{ #define MAXINT16 ( 32767) ///< max signed 16 bit int=> ((2^15)-1) #define MININT16 (-32768) ///< max negative signed 16 bit int=> (-(2^15)) - -#endif /* GLOBALCONSTANTS_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 /* _CONSTANTS_H */ diff --git a/Core/Common/include/utilities.h b/Platform/common/include/utils.h similarity index 50% rename from Core/Common/include/utilities.h rename to Platform/common/include/utils.h index 0e09559..e5d9b38 100644 --- a/Core/Common/include/utilities.h +++ b/Platform/common/include/utils.h @@ -1,15 +1,8 @@ -/** *************************************************************************** - * @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 + * @file: utils.h + *******************************************************************************/ +/******************************************************************************* +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -24,15 +17,33 @@ See the License for the specific language governing permissions and limitations under the License. *******************************************************************************/ -#ifndef UTILITIES_H -#define UTILITIES_H + +#ifndef _UTILS_H +#define _UTILS_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 +#define GPS_BUFF_SIZE (2000) +#define IMU_BUFF_SIZE (512) +#define DEBUG_BUFF_SIZE (128) + + +typedef struct + { + uint8_t* buffer; + uint16_t in; + uint16_t out; + uint16_t size; +} fifo_type; + + +void fifo_init(fifo_type* fifo, uint8_t* buffer, uint16_t size); +uint16_t fifo_get(fifo_type* fifo, uint8_t* buffer, uint16_t len); +uint16_t fifo_status(fifo_type* fifo); +void fifo_push(fifo_type* fifo, uint8_t* buffer, uint16_t size); + + +#endif /* _UTILS_H */ + + diff --git a/Platform/common/src/utils.c b/Platform/common/src/utils.c new file mode 100644 index 0000000..e7c429a --- /dev/null +++ b/Platform/common/src/utils.c @@ -0,0 +1,47 @@ + +#include + +#include "utils.h" + + +void fifo_init(fifo_type* fifo, uint8_t* buffer, uint16_t size) +{ + fifo->buffer = buffer; + fifo->in = 0; + fifo->out = 0; + fifo->size = size; +} + +void fifo_push(fifo_type* fifo, uint8_t* buffer, uint16_t size) +{ + uint16_t i; + + for (i = 0; i < size; i++) + { + fifo->buffer[(fifo->in+i)%fifo->size] = buffer[i]; + } + fifo->in = (fifo->in + size)%fifo->size; +} + +uint16_t fifo_get(fifo_type* fifo, uint8_t* buffer, uint16_t len) +{ + uint16_t lenght; + uint16_t in = fifo->in; + uint16_t i; + lenght = (in + fifo->size - fifo->out)%fifo->size; + if(lenght > len) + lenght = len; + for(i = 0; i < lenght; i++) + { + buffer[i] = fifo->buffer[(fifo->out + i)%fifo->size]; + } + fifo->out = (fifo->out + lenght)%fifo->size; + return lenght; +} + +uint16_t fifo_status(fifo_type* fifo) +{ + uint16_t lenght; + lenght = (fifo->in + fifo->size - fifo->out)%fifo->size; + return lenght; +} \ No newline at end of file diff --git a/Platform/configurationAPI.h b/Platform/configurationAPI.h index 2987477..9fc2b92 100644 --- a/Platform/configurationAPI.h +++ b/Platform/configurationAPI.h @@ -8,7 +8,7 @@ * *****************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -27,7 +27,8 @@ limitations under the License. #ifndef _CONFIG_API_H #define _GONFIG_API_H #include -#include "GlobalConstants.h" + +#include "constants.h" // serial port related functions int configGetPacketRate(void); diff --git a/Core/GPS/include/rtcm.h b/Platform/gnss_data/include/rtcm.h similarity index 97% rename from Core/GPS/include/rtcm.h rename to Platform/gnss_data/include/rtcm.h index c22471c..abce8ef 100644 --- a/Core/GPS/include/rtcm.h +++ b/Platform/gnss_data/include/rtcm.h @@ -1,5 +1,5 @@ -#ifndef _RTCM_H_ -#define _RTCM_H_ +#ifndef _RTCM_H +#define _RTCM_H #include #include @@ -201,20 +201,11 @@ extern "C" { #define CODE_L9X 55 /* obs code: SB+C (IRN) */ #define MAXCODE 55 /* max number of obs code */ -// #ifdef WIN32 -// #define MAXOBS 70 /* max number of observed satellites */ -// #else #define MAXOBS 45 /* max number of observed satellites */ -// #endif -//#define _PC_ -#ifdef _PC_ -#define MAXEPH 100 /* max number of GPS/BDS/GAL ephemeris(PC) */ -#define MAXEPH_R 24 /* max number of GLO ephemeris(PC) */ -#else #define MAXEPH 55 /* max number of GPS/BDS/GAL ephemeris(real-time) */ #define MAXEPH_R 15 /* max number of GLO ephemeris(real-time) */ -#endif + #define MAXSSR 24 /* max number of SSR */ #define MAXANT 2 /* max number of antenna */ @@ -383,6 +374,7 @@ typedef struct { /* rtcm data struct */ double time; } gnss_rtcm_t; + /* ssr update intervals ------------------------------------------------------*/ static const double ssrudint[16] = { 1, 2, 5, 10, 15, 30, 60, 120, 240, 300, 600, 900, 1800, 3600, 7200, 10800 @@ -452,11 +444,8 @@ static const int codes_sbs[] = { }; -extern void trace (int level, const char *format, ...); -extern int input_rtcm3_data(rtcm_t *rtcm, unsigned char data, obs_t *obs, nav_t *nav); +extern void trace (int level, const char *format, ...); -/* interface to GNSS db */ -extern int input_rtcm3(unsigned char data, unsigned int stnID, gnss_rtcm_t *gnss); extern unsigned int rtcm_getbitu(const unsigned char *buff, int pos, int len); /* glo frquent number function */ diff --git a/Core/GPS/include/rtklib_core.h b/Platform/gnss_data/include/rtklib_core.h similarity index 100% rename from Core/GPS/include/rtklib_core.h rename to Platform/gnss_data/include/rtklib_core.h diff --git a/Core/GPS/src/rtcm.c b/Platform/gnss_data/src/rtcm.c similarity index 99% rename from Core/GPS/src/rtcm.c rename to Platform/gnss_data/src/rtcm.c index 6cc4941..c53531c 100644 --- a/Core/GPS/src/rtcm.c +++ b/Platform/gnss_data/src/rtcm.c @@ -1,12 +1,4 @@ -/******************************************************************************* -* HISTORY*********************************************************************** -* 16/10/2019 | | Daich -* Description: package some _hal function -*******************************************************************************/ -#include "rtcm.h" - -/*--------------------------------------------------------*/ - + #include #include #include @@ -16,14 +8,8 @@ #include #include -#ifndef WIN32 -//#ifdef INT_SEC_SEND - //#include "main.h" -//#endif -#endif - -// #define D2R (0.017453292519943295) /* deg to rad */ -// #define R2D (57.29577951308232) /* rad to deg */ +#include "rtcm.h" +#include "gnss_data_api.h" #define SC2RAD 3.1415926535898 /* semi-circle to radian (IS-GPS) */ #define AU 149597870691.0 /* 1 AU (m) */ @@ -4830,7 +4816,6 @@ extern int input_rtcm3_data(rtcm_t *rtcm, unsigned char data, obs_t *obs, nav_t extern int input_rtcm3(unsigned char data, unsigned int stnID, gnss_rtcm_t *gnss) { - //trace(5, "input_rtcm3: data=%02x\n", data); rtcm_t *rtcm = NULL; obs_t *obs = NULL; nav_t *nav = NULL; @@ -4843,39 +4828,8 @@ extern int input_rtcm3(unsigned char data, unsigned int stnID, gnss_rtcm_t *gnss nav = &gnss->nav; obs = gnss->obs + stnID; ret = input_rtcm3_data(rtcm, data, obs, nav); -#if 0 - if (ret == 1 && obs!=NULL) { - int week = 0; - double time = time2gpst(obs->time, &week); - printf("%4i,%10.3f,%3i\n", week, time, obs->n); - for (int i = 0; i < obs->n; ++i) { - int prn = 0; - char sys = satid(obs->data[i].sat, &prn); - printf("%c%02i,%14.3f,%14.3f,%14.3f,%7.3f,%14.3f,%14.3f,%14.3f,%7.3f\n" - , sys, prn - , obs->data[i].P[0], obs->data[i].L[0], obs->data[i].D[0], obs->data[i].SNR[0]/4.0 - , obs->data[i].P[1], obs->data[i].L[1], obs->data[i].D[1], obs->data[i].SNR[1]/4.0); - } - week = 0; - } -#endif } return ret; } - -void set_ms(uint32_t ms) -{ -#ifdef INT_SEC_SEND - sensor_time_s.ms += ms; -#endif -} - -#ifdef INT_SEC_SEND -TIME_S *get_time() -{ - return &sensor_time_s; -} -#endif - diff --git a/Platform/gnss_data_api.h b/Platform/gnss_data_api.h new file mode 100644 index 0000000..f95c553 --- /dev/null +++ b/Platform/gnss_data_api.h @@ -0,0 +1,72 @@ +/** *************************************************************************** + * @file gnss_data_api.h GNSS data API for algorithm and I/O layer + * + * 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 2020 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 _GNSS_DATA_API_H +#define _GNSS_DATA_API_H + +#include + +#include "constants.h" +#include "rtcm.h" +#include "timer.h" + +typedef struct { + uint16_t gps_week; + uint32_t gps_tow; // gps Time Of Week, miliseconds + + uint8_t gnss_fix_type; // 1 if data is valid + uint8_t gnss_update; // 1 if contains new data + uint8_t num_sats; // num of satellites in the solution + + double latitude; // latitude , degrees + double longitude; // longitude, degrees + double altitude; // above mean sea level [m] + float vel_ned[3]; // velocities, m/s NED (North East Down) x, y, z + float heading; // [deg] + + float dops[5]; + float sol_age; + + float std_lat; //!< latitude standard deviation (m) + float std_lon; //!< longitude standard deviation (m) + float std_hgt; //!< height standard deviation (m) + float std_vn; + float std_ve; + float std_vd; + +} gnss_solution_t; + +typedef struct { /* rtcm data struct */ + gnss_rtcm_t rtcm; /* store RTCM data struct for RTK and PPP */ + + /* for algorithm */ + obs_t rov; + obs_t ref; + nav_t nav; +} gnss_raw_data_t; + +extern int input_rtcm3_data(rtcm_t *rtcm, unsigned char data, obs_t *obs, nav_t *nav); +extern int input_rtcm3(unsigned char data, unsigned int stnID, gnss_rtcm_t *gnss); + +#endif /* _GNSS_DATA_API_H */ diff --git a/Platform/hwAPI.h b/Platform/hwAPI.h index dba9362..03a8993 100644 --- a/Platform/hwAPI.h +++ b/Platform/hwAPI.h @@ -7,7 +7,7 @@ #define __HW_API_H #include "stdint.h" -#include "GlobalConstants.h" +#include "constants.h" // GPIO - related fucntions diff --git a/Core/Math/include/FastInvTrigFuncs.h b/Platform/math/include/FastInvTrigFuncs.h similarity index 100% rename from Core/Math/include/FastInvTrigFuncs.h rename to Platform/math/include/FastInvTrigFuncs.h diff --git a/Core/Math/include/MatrixMath.h b/Platform/math/include/MatrixMath.h similarity index 97% rename from Core/Math/include/MatrixMath.h rename to Platform/math/include/MatrixMath.h index f4eae23..2bcf703 100644 --- a/Core/Math/include/MatrixMath.h +++ b/Platform/math/include/MatrixMath.h @@ -15,7 +15,7 @@ #define MATRIXMATH_H #include // uint8_t, etc. -#include "GlobalConstants.h" // for real +#include "constants.h" // for real // Function declarations uint8_t AxBTranspose(real *A, real *B, uint8_t rowsInA, uint8_t colsInA, uint8_t rowsInB, real *C); diff --git a/Core/Math/include/QuaternionMath.h b/Platform/math/include/QuaternionMath.h similarity index 94% rename from Core/Math/include/QuaternionMath.h rename to Platform/math/include/QuaternionMath.h index d5211b2..e2e2a11 100644 --- a/Core/Math/include/QuaternionMath.h +++ b/Platform/math/include/QuaternionMath.h @@ -8,7 +8,7 @@ #ifndef QUATERNIONMATH_H #define QUATERNIONMATH_H -#include "GlobalConstants.h" +#include "constants.h" BOOL EulerAnglesToQuaternion(real* EulerAngles, real* Quaternion); BOOL EulerAnglesToR321(real* EulerAngles, real* R321); diff --git a/Core/Math/include/TransformationMath.h b/Platform/math/include/TransformationMath.h similarity index 99% rename from Core/Math/include/TransformationMath.h rename to Platform/math/include/TransformationMath.h index daabf24..be6cd03 100644 --- a/Core/Math/include/TransformationMath.h +++ b/Platform/math/include/TransformationMath.h @@ -8,7 +8,7 @@ #ifndef TRANSFORMATIONMATH_H #define TRANSFORMATIONMATH_H -#include "GlobalConstants.h" +#include "constants.h" #include #define E_MAJOR 6.378137e+6 // semi major axis a diff --git a/Core/Math/include/VectorMath.h b/Platform/math/include/VectorMath.h similarity index 97% rename from Core/Math/include/VectorMath.h rename to Platform/math/include/VectorMath.h index 1dd09e3..70bd4f4 100644 --- a/Core/Math/include/VectorMath.h +++ b/Platform/math/include/VectorMath.h @@ -8,7 +8,7 @@ #ifndef VECTORMATH_H #define VECTORMATH_H -#include "GlobalConstants.h" +#include "constants.h" // Declare the function definitions for real void VectorNormalize(real *vectorIn, real *vectorOut); diff --git a/Core/Math/include/arm_common_tables.h b/Platform/math/include/arm_common_tables.h similarity index 100% rename from Core/Math/include/arm_common_tables.h rename to Platform/math/include/arm_common_tables.h diff --git a/Core/Math/include/arm_const_structs.h b/Platform/math/include/arm_const_structs.h similarity index 100% rename from Core/Math/include/arm_const_structs.h rename to Platform/math/include/arm_const_structs.h diff --git a/Core/Math/include/arm_math.h b/Platform/math/include/arm_math.h similarity index 100% rename from Core/Math/include/arm_math.h rename to Platform/math/include/arm_math.h diff --git a/Core/Math/include/qmath.h b/Platform/math/include/qmath.h similarity index 100% rename from Core/Math/include/qmath.h rename to Platform/math/include/qmath.h diff --git a/Core/Math/include/xmath.h b/Platform/math/include/xmath.h similarity index 100% rename from Core/Math/include/xmath.h rename to Platform/math/include/xmath.h diff --git a/Core/Math/src/FastInvTrigFuncs.c b/Platform/math/src/FastInvTrigFuncs.c similarity index 100% rename from Core/Math/src/FastInvTrigFuncs.c rename to Platform/math/src/FastInvTrigFuncs.c diff --git a/Core/Math/src/MatrixMath.c b/Platform/math/src/MatrixMath.c similarity index 100% rename from Core/Math/src/MatrixMath.c rename to Platform/math/src/MatrixMath.c diff --git a/Core/Math/src/QuaternionMath.c b/Platform/math/src/QuaternionMath.c similarity index 100% rename from Core/Math/src/QuaternionMath.c rename to Platform/math/src/QuaternionMath.c diff --git a/Core/Math/src/TransformationMath.c b/Platform/math/src/TransformationMath.c similarity index 100% rename from Core/Math/src/TransformationMath.c rename to Platform/math/src/TransformationMath.c diff --git a/Core/Math/src/VectorMath.c b/Platform/math/src/VectorMath.c similarity index 100% rename from Core/Math/src/VectorMath.c rename to Platform/math/src/VectorMath.c diff --git a/Core/Math/src/arm_common_tables.c b/Platform/math/src/arm_common_tables.c similarity index 100% rename from Core/Math/src/arm_common_tables.c rename to Platform/math/src/arm_common_tables.c diff --git a/Core/Math/src/arm_const_structs.c b/Platform/math/src/arm_const_structs.c similarity index 100% rename from Core/Math/src/arm_const_structs.c rename to Platform/math/src/arm_const_structs.c diff --git a/Core/Math/src/arm_cos_f32.c b/Platform/math/src/arm_cos_f32.c similarity index 100% rename from Core/Math/src/arm_cos_f32.c rename to Platform/math/src/arm_cos_f32.c diff --git a/Core/Math/src/arm_sin_f32.c b/Platform/math/src/arm_sin_f32.c similarity index 100% rename from Core/Math/src/arm_sin_f32.c rename to Platform/math/src/arm_sin_f32.c diff --git a/Core/Math/src/qmath.c b/Platform/math/src/qmath.c similarity index 100% rename from Core/Math/src/qmath.c rename to Platform/math/src/qmath.c diff --git a/Platform/platformAPI.h b/Platform/platformAPI.h index 401686b..6c1aea2 100644 --- a/Platform/platformAPI.h +++ b/Platform/platformAPI.h @@ -8,7 +8,7 @@ * *****************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -27,9 +27,8 @@ limitations under the License. #ifndef _PLATFORM_API_H #define _PLATFORM_API_H #include -#include "GlobalConstants.h" +#include "constants.h" -// NEEDS TO BE CHECKED BOOL eepromLocked(void); BOOL lockEeprom(void); BOOL unlockEeprom(void); @@ -65,14 +64,6 @@ int platformEnableExtSync(BOOL enable); void platformUpdateInterfaceTestStatus(BOOL fGood); void platformSetMode(BOOL isBoot); BOOL platformIsInBootMode(); -BOOL platformIsGpsPPSUsed(void); -void platformEnableGpsPps(BOOL enable); -#define kick_dog() - -#define UART_CHANNEL_NONE -1 // undefined channel -#define UART_CHANNEL_0 0 // pins 3 and 4 on the 20-pin connector. NOTE: not available in SPI interface mode -#define UART_CHANNEL_1 1 // pins 5 and 6 on the 20-pin connector. NOTE: not available in SPI interface mode -#define UART_CHANNEL_2 2 // pins 17 and 19 on the 20-pin connector. Always available #endif diff --git a/Platform/platform_version.h b/Platform/platform_version.h index ef38e61..19cef24 100644 --- a/Platform/platform_version.h +++ b/Platform/platform_version.h @@ -3,8 +3,8 @@ * * @brief Platfor version definition ******************************************************************************/ -#ifndef PLATFORM_VERSION_H -#define PLATFORM_VERSION_H +#ifndef _PLATFORM_VERSION_H +#define _PLATFORM_VERSION_H // DO NOT CHANGE THESE NUMBERS FROM ZERO! CAUSES A CONFLICT WITH // IMUTest RESULTING IN ACCELEROMETER VALUES THAT ARE FLIPPED (WHAT @@ -31,14 +31,14 @@ // 12345678901234567890 #ifdef IMU383 -#define SOFTWARE_PART "5020-1398-01 0.0.20" // IMU383 + #define SOFTWARE_PART "5020-1398-01 0.0.20" // IMU383 #else -#define SOFTWARE_PART "8350-3021-01 0.1.0" // openrtk330 + #define SOFTWARE_PART "8350-3021-01 0.1.1" // openrtk330 #endif #ifdef BOOT_MODE -#define BOOT_SOFTWARE_PART "0.0.1" // Bootloader version -#define VERSION_STRING "IMU383_Bootloader" // Bootloader version + #define BOOT_SOFTWARE_PART "0.0.1" // Bootloader version + #define VERSION_STRING "IMU383_Bootloader" // Bootloader version #endif @@ -47,4 +47,4 @@ #define VERSION_STR SOFTWARE_PART #define N_VERSION_STR 128 -#endif +#endif /* _PLATFORM_VERSION_H */ diff --git a/Platform/eepromAPI.h b/Sensors/eepromAPI.h similarity index 99% rename from Platform/eepromAPI.h rename to Sensors/eepromAPI.h index d2ff268..f0d9e40 100644 --- a/Platform/eepromAPI.h +++ b/Sensors/eepromAPI.h @@ -13,7 +13,7 @@ #ifndef _EEPROM_API_H #define _EEPROM_API_H -#include "GlobalConstants.h" +#include "constants.h" #define APP_STACK_START 0x2000FF00 #define EEPROM_APP_UPDATE_ADDR 0x0801FFF8 diff --git a/Sensors/sensorsAPI.h b/Sensors/sensorsAPI.h index 3ce849b..6201cbb 100644 --- a/Sensors/sensorsAPI.h +++ b/Sensors/sensorsAPI.h @@ -8,7 +8,7 @@ * *****************************************************************************/ /******************************************************************************* -Copyright 2018 ACEINNA, INC +Copyright 2020 ACEINNA, INC Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. diff --git a/library.json b/library.json index 1c71fbc..86bfe8b 100644 --- a/library.json +++ b/library.json @@ -18,19 +18,7 @@ "-I STM32F469/HAL/Src", "-I STM32F469/OS/Inc", "-I STM32F469/", - "-I Core/Algorithm/include", - "-I Core/Algorithm", - "-I Core/Buffer/include", - "-I Core/Common/include", - "-I Core/GPS", - "-I Core/GPS/include", - "-I Core/Math/include", - "-I Core/Logg/inc", - "-I Core/Cjson/inc", - "-I Core/DmaMemcpy/inc", - "-I Core/UARTComm", - "-I Core/console/inc", - "-I Core/", + "-I LWIP/arch", "-I LWIP/lwip-1.4.1/src/include", "-I LWIP/lwip-1.4.1/src/include/ipv4", @@ -39,14 +27,21 @@ "-I LWIP/lwip_app/webserver/inc", "-I LWIP/lwip_app/NtripClient/inc" , "-I LWIP/", + "-I Platform/", "-I Platform/Core/include", "-I Platform/Board/include", "-I Platform/Filter/include", - "-I Platform/Driver/inc", + "-I Platform/Driver/include", + "-I Platform/cJSON/inc", + "-I Platform/gnss_data/include", + "-I Platform/common/include", + "-I Platform/math/include", + "-I FreeRTOS/include", "-I FreeRTOS/", "-I FreeRtos_M4/include", + "-I Sensors/Core", "-I Sensors/Core/include", "-I Sensors/",