1 /*
2 * Copyright (c) 2015, Freescale Semiconductor, Inc.
3 * Copyright 2016-2017 NXP
4 * All rights reserved.
5 *
6 * SPDX-License-Identifier: BSD-3-Clause
7 */
8
9
10 /*! \file calibration_storage.c
11 \brief Provides functions to store calibration to NVM
12
13 Users who are not using NXP hardware will need to supply their own drivers
14 in place of those defined here.
15 */
16 #include <stdio.h>
17 #include "sensor_fusion.h"
18 #include "driver_KSDK_NVM.h"
19 #include "calibration_storage.h"
20
SaveMagCalibrationToNVM(SensorFusionGlobals * sfg)21 void SaveMagCalibrationToNVM(SensorFusionGlobals *sfg)
22 {
23 #if F_USING_MAG
24 uint8_t *pSrc, *pDst; // scratch pointers
25 int16_t i; // loop counter
26 uint8_t iNVMBuffer[256]; // NVM write buffer (smallest size writeable to flash)
27 uint32_t itmp32;
28
29 // copy existing magnetic, gyro and accelerometer calibrations to buffer
30 pSrc = (uint8 *) CALIBRATION_NVM_ADDR;
31 pDst = iNVMBuffer;
32 for (i = 0; i < 256; i++)
33 *(pDst++) = *(pSrc++);
34 // default to no magnetic calibration in header
35 iNVMBuffer[MAG_NVM_OFFSET] = iNVMBuffer[MAG_NVM_OFFSET + 1] = iNVMBuffer[MAG_NVM_OFFSET + 2] = iNVMBuffer[MAG_NVM_OFFSET + 3] = 0xFF;
36
37 // fill the buffer with the magnetic calibration in bytes 0 to 67 (total 68 bytes)
38 // [0-3]: four byte header denoting magnetic calibration present
39 itmp32 = 0x12345678;
40 pSrc = (uint8 *) &itmp32;
41 pDst = iNVMBuffer + MAG_NVM_OFFSET;
42 for (i = 0; i < 4; i++)
43 *(pDst++) = *(pSrc++);
44 // [4-67]: magnetic calibration: 15x float + 1x int32 total 64 bytes
45 pSrc = (uint8 *) &(sfg->MagCal);
46 for (i = 0; i < 64; i++)
47 *(pDst++) = *(pSrc++);
48
49 // write the whole buffer contents to NVM
50 NVM_SetBlockFlash(iNVMBuffer, CALIBRATION_NVM_ADDR, 256);
51 #endif // if F_USING_MAG
52 return;
53 }
54
SaveGyroCalibrationToNVM(SensorFusionGlobals * sfg)55 void SaveGyroCalibrationToNVM(SensorFusionGlobals *sfg)
56 {
57 #if F_USING_GYRO && (F_9DOF_GBY_KALMAN || F_6DOF_GY_KALMAN)
58 uint8_t *pSrc, *pDst; // scratch pointers
59 int16_t i; // loop counter
60 uint8_t iNVMBuffer[256]; // NVM write buffer
61 uint32_t itmp32;
62
63 // copy existing magnetic, gyro and accelerometer calibrations to buffer
64 pSrc = (uint8 *) CALIBRATION_NVM_ADDR;
65 pDst = iNVMBuffer;
66 for (i = 0; i < 256; i++)
67 *(pDst++) = *(pSrc++);
68 // default to no gyroscope calibration in header
69 iNVMBuffer[GYRO_NVM_OFFSET] = iNVMBuffer[GYRO_NVM_OFFSET + 1] = iNVMBuffer[GYRO_NVM_OFFSET + 2] = iNVMBuffer[GYRO_NVM_OFFSET + 3] = 0xFF;
70
71 // define the four header bytes
72 // [0-3]: four byte header denoting gyro calibration present
73 itmp32 = 0x12345678;
74 pSrc = (uint8 *) &itmp32;
75 pDst = iNVMBuffer + GYRO_NVM_OFFSET;
76 for (i = 0; i < 4; i++)
77 *(pDst++) = *(pSrc++);
78
79 // [4-15]: 3 gyro offset floats totalling 12 bytes
80 #if F_9DOF_GBY_KALMAN
81 pSrc = (uint8 *) sfg->SV_9DOF_GBY_KALMAN.fbPl;
82 #elif F_6DOF_GY_KALMAN
83 pSrc = (uint8 *) sfg->SV_6DOF_GY_KALMAN.fbPl;
84 #endif
85 for (i = 0; i < 12; i++)
86 *(pDst++) = *(pSrc++);
87
88 // write the buffer contents to NVM
89 NVM_SetBlockFlash(iNVMBuffer, CALIBRATION_NVM_ADDR, 256);
90 #endif
91 return;
92 }
93
SaveAccelCalibrationToNVM(SensorFusionGlobals * sfg)94 void SaveAccelCalibrationToNVM(SensorFusionGlobals *sfg)
95 {
96 #if F_USING_ACCEL
97 uint8_t *pSrc, *pDst; // scratch pointers
98 int16_t i; // loop counter
99 uint8_t iNVMBuffer[256]; // NVM write buffer
100 uint32_t itmp32;
101
102 // copy existing magnetic, gyro and accelerometer calibrations to buffer
103 pSrc = (uint8 *) CALIBRATION_NVM_ADDR;
104 pDst = iNVMBuffer;
105 for (i = 0; i < 256; i++)
106 *(pDst++) = *(pSrc++);
107 // default to no accelerometer calibration in header
108 iNVMBuffer[ACCEL_NVM_OFFSET] = iNVMBuffer[ACCEL_NVM_OFFSET + 1] = iNVMBuffer[ACCEL_NVM_OFFSET + 2] = iNVMBuffer[ACCEL_NVM_OFFSET + 3] = 0xFF;
109
110 // [0-3]: four byte header denoting accelerometer calibration present
111 itmp32 = 0x12345678;
112 pSrc = (uint8 *) &itmp32;
113 pDst = iNVMBuffer + ACCEL_NVM_OFFSET;
114 for (i = 0; i < 4; i++)
115 *(pDst++) = *(pSrc++);
116
117 // [4-87]: 21 precision accelerometer calibration floats totalling 84 bytes
118 pSrc = (uint8 *) &(sfg->AccelCal);
119 for (i = 0; i < 84; i++)
120 *(pDst++) = *(pSrc++);
121
122 // write the buffer contents to NVM
123 NVM_SetBlockFlash(iNVMBuffer, CALIBRATION_NVM_ADDR, 256);
124 #endif
125 return;
126 }
127
EraseMagCalibrationFromNVM(void)128 void EraseMagCalibrationFromNVM(void)
129 {
130 uint8_t *pSrc, *pDst; // scratch pointers
131 int16_t i; // loop counter
132 uint8_t iNVMBuffer[256]; // NVM write buffer
133
134 // copy existing magnetic, gyro and accelerometer calibrations to buffer
135 pSrc = (uint8 *) CALIBRATION_NVM_ADDR;
136 pDst = iNVMBuffer;
137 for (i = 0; i < 256; i++)
138 *(pDst++) = *(pSrc++);
139
140 // set no magnetic calibration in header
141 iNVMBuffer[MAG_NVM_OFFSET] = iNVMBuffer[MAG_NVM_OFFSET + 1] = iNVMBuffer[MAG_NVM_OFFSET + 2] = iNVMBuffer[MAG_NVM_OFFSET + 3] = 0xFF;
142
143 // write the buffer to flash
144 NVM_SetBlockFlash(iNVMBuffer, CALIBRATION_NVM_ADDR, 256);
145
146 return;
147 }
148
EraseGyroCalibrationFromNVM(void)149 void EraseGyroCalibrationFromNVM(void)
150 {
151 uint8_t *pSrc, *pDst; // scratch pointers
152 int16_t i; // loop counter
153 uint8_t iNVMBuffer[256]; // NVM write buffer
154
155 // copy existing magnetic, gyro and accelerometer calibrations to buffer
156 pSrc = (uint8 *) CALIBRATION_NVM_ADDR;
157 pDst = iNVMBuffer;
158 for (i = 0; i < 256; i++)
159 *(pDst++) = *(pSrc++);
160
161 // set no gyroscope calibration in header
162 iNVMBuffer[GYRO_NVM_OFFSET] = iNVMBuffer[GYRO_NVM_OFFSET + 1] = iNVMBuffer[GYRO_NVM_OFFSET + 2] = iNVMBuffer[GYRO_NVM_OFFSET + 3] = 0xFF;
163
164 // write the buffer to flash
165 NVM_SetBlockFlash(iNVMBuffer, CALIBRATION_NVM_ADDR, 256);
166
167 return;
168 }
169
EraseAccelCalibrationFromNVM(void)170 void EraseAccelCalibrationFromNVM(void)
171 {
172 uint8_t *pSrc, *pDst; // scratch pointers
173 int16_t i; // loop counter
174 uint8_t iNVMBuffer[256]; // NVM write buffer
175
176 // copy existing magnetic, gyro and accelerometer calibrations to buffer
177 pSrc = (uint8 *) CALIBRATION_NVM_ADDR;
178 pDst = iNVMBuffer;
179 for (i = 0; i < 256; i++)
180 *(pDst++) = *(pSrc++);
181
182 // set no gyroscope calibration in header
183 iNVMBuffer[ACCEL_NVM_OFFSET] = iNVMBuffer[ACCEL_NVM_OFFSET + 1] = iNVMBuffer[ACCEL_NVM_OFFSET + 2] = iNVMBuffer[ACCEL_NVM_OFFSET + 3] = 0xFF;
184
185 // write the buffer to flash
186 NVM_SetBlockFlash(iNVMBuffer, CALIBRATION_NVM_ADDR, 256);
187
188 return;
189 }
190
