278 lines
6.7 KiB
C++
278 lines
6.7 KiB
C++
/**! @brief Class for managing storing "Adafruit" sensor calibration
|
|
in internal Flash memory with KVStore
|
|
for Arduino Nano 33 BLE
|
|
* **/
|
|
|
|
//https://os.mbed.com/docs/mbed-os/v6.12/apis/kvstore.html
|
|
//KVStore. TDBStore - Default implementation of the KVStore API. It provides static wear-leveling and quick access for when you have a small number of KV pairs.
|
|
//https://os.mbed.com/docs/mbed-os/v6.12/apis/data-architecture.html
|
|
|
|
#include "KVStore.h"
|
|
#include "kvstore_global_api.h"
|
|
|
|
/**! XYZ vector of offsets for zero-g, in m/s^2 */
|
|
float accel_zerog[3] = {0, 0, 0};
|
|
|
|
/**! XYZ vector of offsets for zero-rate, in rad/s */
|
|
float gyro_zerorate[3] = {0, 0, 0};
|
|
|
|
/**! XYZ vector of offsets for hard iron calibration (in uT) */
|
|
float mag_hardiron[3] = {0, 0, 0};
|
|
|
|
/**! The 3x3 matrix for soft-iron calibration (unitless) */
|
|
float mag_softiron[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
|
|
|
|
/**! The magnetic field magnitude in uTesla */
|
|
float mag_field = 50;
|
|
|
|
const char* const ASC_KV_Key0 = "store_flag";
|
|
const char* const ASC_KV_Key1 = "store_offsets";
|
|
|
|
void setGyroCalibration(float x, float y, float z) {
|
|
gyro_zerorate[0]=x;
|
|
gyro_zerorate[1]=y;
|
|
gyro_zerorate[2]=z;
|
|
}
|
|
|
|
float getGyroXCal() {
|
|
return gyro_zerorate[0];
|
|
}
|
|
|
|
float getGyroYCal() {
|
|
return gyro_zerorate[1];
|
|
}
|
|
|
|
float getGyroZCal() {
|
|
return gyro_zerorate[2];
|
|
}
|
|
|
|
bool saveCalibration(void) {
|
|
Serial.println("Save Cal");
|
|
|
|
kv_reset("/kv/");
|
|
|
|
uint8_t flag=42;
|
|
|
|
kv_set(ASC_KV_Key0,&flag,sizeof(flag),0);
|
|
|
|
float offsets[16];
|
|
memcpy(offsets, accel_zerog, 12); // 3 x 4-byte floats
|
|
memcpy(offsets + 3, gyro_zerorate, 12); // 3 x 4-byte floats
|
|
memcpy(offsets + 6, mag_hardiron, 12); // 3 x 4-byte floats
|
|
|
|
offsets[9] = mag_field;
|
|
|
|
offsets[10] = mag_softiron[0];
|
|
offsets[11] = mag_softiron[4];
|
|
offsets[12] = mag_softiron[8];
|
|
offsets[13] = mag_softiron[1];
|
|
offsets[14] = mag_softiron[2];
|
|
offsets[15] = mag_softiron[5];
|
|
|
|
kv_set(ASC_KV_Key1, (uint8_t*)&offsets, sizeof(offsets), 0);
|
|
|
|
return true;
|
|
}
|
|
|
|
bool loadCalibration(void) {
|
|
|
|
uint8_t flag;
|
|
|
|
kv_get(ASC_KV_Key0,&flag,sizeof(flag),0);
|
|
|
|
if (flag=!42) {
|
|
Serial.print("FLAG NOT SET, CALIBRATION NOT FOUND");
|
|
return false;
|
|
}
|
|
|
|
float offsets[16];
|
|
|
|
kv_get(ASC_KV_Key1,(byte*)&offsets,sizeof(offsets),0);
|
|
|
|
accel_zerog[0] = offsets[0];
|
|
accel_zerog[1] = offsets[1];
|
|
accel_zerog[2] = offsets[2];
|
|
|
|
gyro_zerorate[0] = offsets[3];
|
|
gyro_zerorate[1] = offsets[4];
|
|
gyro_zerorate[2] = offsets[5];
|
|
|
|
mag_hardiron[0] = offsets[6];
|
|
mag_hardiron[1] = offsets[7];
|
|
mag_hardiron[2] = offsets[8];
|
|
|
|
mag_field = offsets[9];
|
|
|
|
mag_softiron[0] = offsets[10];
|
|
mag_softiron[1] = offsets[13];
|
|
mag_softiron[2] = offsets[14];
|
|
mag_softiron[3] = offsets[13];
|
|
mag_softiron[4] = offsets[11];
|
|
mag_softiron[5] = offsets[15];
|
|
mag_softiron[6] = offsets[14];
|
|
mag_softiron[7] = offsets[15];
|
|
mag_softiron[8] = offsets[12];
|
|
|
|
return true;
|
|
}
|
|
|
|
bool printCalibration(void) {
|
|
Serial.println(F("------------"));
|
|
|
|
Serial.print("Accelerometer: ");
|
|
Serial.print(accel_zerog[0]);
|
|
Serial.print(", ");
|
|
Serial.print(accel_zerog[1]);
|
|
Serial.print(", ");
|
|
Serial.print(accel_zerog[2]);
|
|
Serial.println();
|
|
|
|
Serial.print("Gyroscope: ");
|
|
Serial.print(gyro_zerorate[0]);
|
|
Serial.print(", ");
|
|
Serial.print(gyro_zerorate[1]);
|
|
Serial.print(", ");
|
|
Serial.print(gyro_zerorate[2]);
|
|
Serial.println();
|
|
|
|
Serial.print("Magnetometer Hard Iron: ");
|
|
Serial.print(mag_hardiron[0]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_hardiron[1]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_hardiron[2]);
|
|
Serial.println();
|
|
|
|
Serial.print("Magnetic Field: ");
|
|
Serial.print(mag_field);
|
|
Serial.println();
|
|
|
|
Serial.print("Magnetometer Soft Iron: ");
|
|
Serial.print(mag_softiron[0]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[1]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[2]);
|
|
Serial.println();
|
|
Serial.print(mag_softiron[3]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[4]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[5]);
|
|
Serial.println();
|
|
Serial.print(mag_softiron[6]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[7]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[8]);
|
|
Serial.println();
|
|
|
|
Serial.println(F("\n------------"));
|
|
return true;
|
|
}
|
|
|
|
bool printSavedCalibration(void) {
|
|
Serial.println(F("------------"));
|
|
|
|
uint8_t flag;
|
|
|
|
kv_get(ASC_KV_Key0,&flag,sizeof(flag),0);
|
|
|
|
if (flag=!42) {
|
|
Serial.print("FLAG NOT SET, CALIBRATION NOT FOUND");
|
|
return false;
|
|
}
|
|
|
|
float offsets[16];
|
|
|
|
kv_get(ASC_KV_Key1,(byte*)&offsets,sizeof(offsets),0);
|
|
|
|
accel_zerog[0] = offsets[0];
|
|
accel_zerog[1] = offsets[1];
|
|
accel_zerog[2] = offsets[2];
|
|
|
|
Serial.print("Accelerometer: ");
|
|
Serial.print(accel_zerog[0]);
|
|
Serial.print(", ");
|
|
Serial.print(accel_zerog[1]);
|
|
Serial.print(", ");
|
|
Serial.print(accel_zerog[2]);
|
|
Serial.println();
|
|
|
|
gyro_zerorate[0] = offsets[3];
|
|
gyro_zerorate[1] = offsets[4];
|
|
gyro_zerorate[2] = offsets[5];
|
|
|
|
Serial.print("Gyroscope: ");
|
|
Serial.print(gyro_zerorate[0]);
|
|
Serial.print(", ");
|
|
Serial.print(gyro_zerorate[1]);
|
|
Serial.print(", ");
|
|
Serial.print(gyro_zerorate[2]);
|
|
Serial.println();
|
|
|
|
mag_hardiron[0] = offsets[6];
|
|
mag_hardiron[1] = offsets[7];
|
|
mag_hardiron[2] = offsets[8];
|
|
|
|
Serial.println("Magnetometer Hard Iron: ");
|
|
Serial.print(mag_hardiron[0]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_hardiron[1]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_hardiron[2]);
|
|
Serial.println();
|
|
|
|
mag_field = offsets[9];
|
|
|
|
Serial.print("Magnetic Field: ");
|
|
Serial.print(mag_field);
|
|
Serial.println();
|
|
|
|
mag_softiron[0] = offsets[10];
|
|
mag_softiron[1] = offsets[13];
|
|
mag_softiron[2] = offsets[14];
|
|
mag_softiron[3] = offsets[13];
|
|
mag_softiron[4] = offsets[11];
|
|
mag_softiron[5] = offsets[15];
|
|
mag_softiron[6] = offsets[14];
|
|
mag_softiron[7] = offsets[15];
|
|
mag_softiron[8] = offsets[12];
|
|
|
|
Serial.print("Magnetometer Soft Iron: ");
|
|
Serial.print(mag_softiron[0]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[1]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[2]);
|
|
Serial.println();
|
|
Serial.print(mag_softiron[3]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[4]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[5]);
|
|
Serial.println();
|
|
Serial.print(mag_softiron[6]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[7]);
|
|
Serial.print(", ");
|
|
Serial.print(mag_softiron[8]);
|
|
Serial.println();
|
|
|
|
/* for (uint16_t a = ee_addr; a < ee_addr + KVStore_CAL_SIZE; a++) {
|
|
uint8_t c = KVStore.read(a);
|
|
Serial.print("0x");
|
|
if (c < 0x10)
|
|
Serial.print('0');
|
|
Serial.print(c, HEX);
|
|
Serial.print(", ");
|
|
if ((a - ee_addr) % 16 == 15) {
|
|
Serial.println();
|
|
}
|
|
}
|
|
*/
|
|
|
|
Serial.println(F("\n------------"));
|
|
return true;
|
|
}
|