/** 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; }