/** This function fills up a block of memory to be stored in non-volatile memory.
* @param[out] data Place to store data, size of sz, must be at least size
* returned by inv_get_mpl_state_size()
* @param[in] sz Size of data.
* @return Returns INV_SUCCESS if successful or an error code if not.
*/
inv_error_t inv_save_mpl_states(unsigned char *data, size_t sz)
{
unsigned char *cur;
int kk;
struct data_header_t *hd;
if (sz >= ds.total_size) {
cur = data + sizeof(struct data_header_t);
for (kk = 0; kk < ds.num; ++kk) {
hd = (struct data_header_t *)cur;
cur += sizeof(struct data_header_t);
ds.save[kk](cur);
hd->checksum = inv_checksum(cur, ds.hd[kk].size);
hd->size = ds.hd[kk].size;
hd->key = ds.hd[kk].key;
cur += ds.hd[kk].size;
}
} else {
return INV_ERROR_CALIBRATION_LOAD;
}
hd = (struct data_header_t *)data;
hd->checksum = inv_checksum(data + sizeof(struct data_header_t),
ds.total_size - sizeof(struct data_header_t));
hd->key = DEFAULT_KEY;
hd->size = ds.total_size;
return INV_SUCCESS;
}
/** This function takes a block of data that has been saved in non-volatile memory and pushes
* to the proper locations. Multiple error checks are performed on the data.
* @param[in] data Data that was saved to be loaded up by MPL
* @param[in] length Length of data vector in bytes
* @return Returns INV_SUCCESS if successful or an error code if not.
*/
inv_error_t inv_load_mpl_states(const unsigned char *data, size_t length)
{
struct data_header_t *hd;
int entry;
uint32_t checksum;
long len;
len = length; // Important so we get negative numbers
if (len < sizeof(struct data_header_t))
return INV_ERROR_CALIBRATION_LOAD; // No data
hd = (struct data_header_t *)data;
if (hd->key != DEFAULT_KEY)
return INV_ERROR_CALIBRATION_LOAD; // Key changed or data corruption
len = MIN(hd->size, len);
len = hd->size;
len -= sizeof(struct data_header_t);
data += sizeof(struct data_header_t);
checksum = inv_checksum(data, len);
if (checksum != hd->checksum)
return INV_ERROR_CALIBRATION_LOAD; // Data corruption
while (len > (long)sizeof(struct data_header_t)) {
hd = (struct data_header_t *)data;
entry = inv_find_entry(hd->key);
data += sizeof(struct data_header_t);
len -= sizeof(struct data_header_t);
if (entry >= 0 && len >= hd->size) {
if (hd->size == ds.hd[entry].size) {
checksum = inv_checksum(data, hd->size);
if (checksum == hd->checksum) {
ds.load[entry](data);
} else {
return INV_ERROR_CALIBRATION_LOAD;
}
}
}
len -= hd->size;
if (len >= 0)
data = data + hd->size;
}
return INV_SUCCESS;
}
/**
* @brief
*/
int populate_data_store(unsigned long sensor_mask,
short temp_avg, short gyro_biases[],
short accel_biases[], long accel_sens[])
{
int ptr = 0;
int tmp;
long long lltmp;
uint32_t chk;
/* total len of factory cal */
data_store[ptr++] = 0;
data_store[ptr++] = 0;
data_store[ptr++] = 0;
data_store[ptr++] = ML_INIT_CAL_LEN;
/* record type 5 - initial calibration */
data_store[ptr++] = 0;
data_store[ptr++] = 5;
if (sensor_mask & INV_THREE_AXIS_GYRO) {
/* temperature */
tmp = temp_avg;
if (tmp < 0)
tmp += 2 << 16;
inv_int16_to_big8(tmp, &data_store[ptr]);
ptr += 2;
/* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */
/* x gyro avg */
lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs;
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
ptr += 4;
/* y gyro avg */
lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs;
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
ptr += 4;
/* z gyro avg */
lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs;
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
ptr += 4;
} else {
ptr += (2 + 4 + 4 + 4);
}
if (sensor_mask & INV_THREE_AXIS_ACCEL) {
signed char *mtx =
mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]->orientation;
short tmp[3];
int ii;
/* need store the biases in chip frame - that is,
inverting the rotation applied in inv_get_accel_data */
memcpy(tmp, accel_biases, sizeof(tmp));
for (ii = 0; ii < ARRAY_SIZE(accel_biases); ii++) {
accel_biases[ii] = tmp[0] * mtx[3 * 0 + ii] +
tmp[1] * mtx[3 * 1 + ii] +
tmp[2] * mtx[3 * 2 + ii];
}
/*
if (sensor_mask & INV_X_ACCEL) {
// x accel avg
lltmp = (long)accel_biases[0] * 65536L / accel_sens[0];
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
}
*/
ptr += 4;
if (sensor_mask & INV_Y_ACCEL) {
/* y accel avg */
lltmp = (long)accel_biases[1] * 65536L / accel_sens[1];
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
}
ptr += 4;
/* z accel avg */
if (sensor_mask & INV_Z_ACCEL) {
lltmp = (long)accel_biases[2] * 65536L / accel_sens[2];
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
}
ptr += 4;
} else {
ptr += 12;
}
/* add a checksum for data */
chk = inv_checksum(
data_store + INV_CAL_HDR_LEN,
ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN);
inv_int32_to_big8(chk, &data_store[ptr]);
ptr += 4;
if (ptr != ML_INIT_CAL_LEN) {
MPL_LOGI("Invalid calibration data length: exp %d, got %d\n",
ML_INIT_CAL_LEN, ptr);
return -1;
}
return 0;
}
