static int mantis_exit(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
if (pdata->private_data)
return inv_free(pdata->private_data);
else
return INV_SUCCESS;
}
/*
* sam_fsaclose_inv - close inventory, writing any unsaved status to
* inventory file.
* invp - address of inventory pointer created using sam_fsa_open_inv
*
* postcond -
* inventory data is saved to disk
* invp is freed and set to NULL
*
* returns 0 on success, -1 on failure
*/
int
sam_fsa_close_inv(sam_fsa_inv_t **invp)
{
int rst = 0;
if (*invp != NULL) {
rst = inv_close_log(*invp, FALSE);
rst |= inv_save(*invp);
rst |= close((*invp)->fd_inv);
inv_free(invp);
}
return (rst);
}
void zone_free(zone * z)
{
int i, j;
for (i = 0; i < z->width; i++) {
for (j = 0; j < z->height; j++) {
// TODO free creature?
inv_free(z->tiles[i][j].inv);
}
free(z->tiles[i]);
}
free(z->tiles);
free(z);
}
/**
* @brief Store runtime calibration data to a file
*
* @pre Must be in INV_STATE_DMP_OPENED state.
* inv_dmp_open() or inv_dmp_stop() must have been called.
* inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
* been called.
*
* @return 0 or error code.
*/
inv_error_t inv_store_calibration(void)
{
unsigned char *calData;
inv_error_t result;
size_t length;
result = inv_get_mpl_state_size(&length);
calData = (unsigned char *)inv_malloc(length);
if (!calData) {
MPL_LOGE("Could not allocate buffer of %d bytes - "
"aborting\n", length);
return INV_ERROR_MEMORY_EXAUSTED;
}
else {
MPL_LOGI("mpl state size = %d", length);
}
result = inv_save_mpl_states(calData, length);
if (result != INV_SUCCESS) {
MPL_LOGE("Could not save mpl states - "
"error %d - aborting\n", result);
goto free_mem_n_exit;
}
else {
MPL_LOGE("calData from inv_save_mpl_states, size=%d",
strlen((char *)calData));
}
result = inv_write_cal(calData, length);
if (result != INV_SUCCESS) {
MPL_LOGE("Could not store calibrated data on file - "
"error %d - aborting\n", result);
goto free_mem_n_exit;
}
free_mem_n_exit:
inv_free(calData);
return result;
}
/**
* @brief Load a calibration file.
*
* @pre Must be in INV_STATE_DMP_OPENED state.
* inv_dmp_open() or inv_dmp_stop() must have been called.
* inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
* been called.
*
* @return 0 or error code.
*/
inv_error_t inv_load_calibration(void)
{
unsigned char *calData= NULL;
inv_error_t result = 0;
size_t bytesRead = 0;
result = inv_read_cal(&calData, &bytesRead);
if(result != INV_SUCCESS) {
MPL_LOGE("Could not load cal file - "
"aborting\n");
goto free_mem_n_exit;
}
result = inv_load_mpl_states(calData, bytesRead);
if (result != INV_SUCCESS) {
MPL_LOGE("Could not load the calibration data - "
"error %d - aborting\n", result);
goto free_mem_n_exit;
}
free_mem_n_exit:
inv_free(calData);
return result;
}
/**
* @brief If requested via inv_test_setup_accel(), test the accelerometer
* biases and calculate the necessary bias correction.
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @param enable_axis
* specify which axis has to be checked and corrected: provides
* a switch mode between 3 axis calibration and Z axis only
* calibration.
* @param bias
* output pointer to store the initial bias calculation provided
* by the MPU Self Test. Requires 3 elements to store accel X, Y,
* and Z axis bias.
* @param gravity
* The gravity value given the parts' sensitivity: for example
* if the accelerometer is set to +/- 2 gee ==> the gravity
* value will be 2^14 = 16384.
* @param perform_full_test
* If 1:
* calculates offsets and noise and compare it against set
* thresholds. The final exist status will reflect if any of the
* value is outside of the expected range.
* When 0;
* skip the noise calculation and pass/fail assessment; simply
* calculates the accel biases.
*
* @return 0 on success. A non-zero error code on error.
*/
int test_accel(void *mlsl_handle, int enable_axes,
short *bias, long gravity,
uint_fast8_t perform_full_test)
{
short *p_vals;
float avg[3] = {0.f, 0.f, 0.f}, zg = 0.f;
float rms[3];
float accel_rms_thresh = 1000000.f; /* enourmous to make the test always
passes - future deployment */
int accel_error = false;
const long sample_period = inv_get_sample_step_size_ms() * 1000;
int ii;
p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples);
/* collect the samples */
for(ii = 0; ii < test_setup.accel_samples; ii++) {
unsigned result = INV_ERROR_ACCEL_DATA_NOT_READY;
int tries = 0;
long accel_data[3];
short *vals = &p_vals[3 * ii];
/* ignore data not ready errors but don't try more than 5 times */
while (result == INV_ERROR_ACCEL_DATA_NOT_READY && tries++ < 5) {
result = inv_get_accel_data(accel_data);
usleep(sample_period);
}
if (result || tries >= 5) {
MPL_LOGV("cannot reliably fetch data from the accelerometer");
accel_error = true;
goto accel_early_exit;
}
vals[X] = (short)accel_data[X];
vals[Y] = (short)accel_data[Y];
vals[Z] = (short)accel_data[Z];
avg[X] += 1.f * vals[X] / test_setup.accel_samples;
avg[Y] += 1.f * vals[Y] / test_setup.accel_samples;
avg[Z] += 1.f * vals[Z] / test_setup.accel_samples;
if (VERBOSE_OUT)
MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n",
vals[X], vals[Y], vals[Z]);
}
if (((enable_axes << 4) & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL) {
MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n",
avg[X], avg[Y], avg[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n",
avg[X] / gravity, avg[Y] / gravity, avg[Z] / gravity);
bias[X] = FLOAT_TO_SHORT(avg[X]);
bias[Y] = FLOAT_TO_SHORT(avg[Y]);
zg = avg[Z] - g_z_sign * gravity;
bias[Z] = FLOAT_TO_SHORT(zg);
MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n",
bias[X], bias[Y], bias[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel correct.: "
"%+13.3f %+13.3f %+13.3f (gee)\n",
1.f * bias[X] / gravity,
1.f * bias[Y] / gravity,
1.f * bias[Z] / gravity);
if (perform_full_test) {
/* accel RMS - for now the threshold is only indicative */
for (ii = 0,
rms[X] = 0.f, rms[Y] = 0.f, rms[Z] = 0.f;
ii < test_setup.accel_samples; ii++) {
short *vals = &p_vals[3 * ii];
rms[X] += (vals[X] - avg[X]) * (vals[X] - avg[X]);
rms[Y] += (vals[Y] - avg[Y]) * (vals[Y] - avg[Y]);
rms[Z] += (vals[Z] - avg[Z]) * (vals[Z] - avg[Z]);
}
for (ii = 0; ii < 3; ii++) {
if (rms[ii] > accel_rms_thresh * accel_rms_thresh
* test_setup.accel_samples) {
MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold "
"(threshold = %.2f)\n", a_name[ii],
sqrt(rms[ii] / test_setup.accel_samples),
accel_rms_thresh);
accel_error = true;
goto accel_early_exit;
}
}
MPL_LOGI("Accel RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n",
sqrt(rms[X] / DEF_N_ACCEL_SAMPLES),
sqrt(rms[Y] / DEF_N_ACCEL_SAMPLES),
sqrt(rms[Z] / DEF_N_ACCEL_SAMPLES));
}
} else {
MPL_LOGI("Accel Z bias : %+13.3f (LSB)\n", avg[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel Z bias : %+13.3f (gee)\n", avg[Z] / gravity);
zg = avg[Z] - g_z_sign * gravity;
bias[Z] = FLOAT_TO_SHORT(zg);
MPL_LOGI("Accel Z correct.: %+13d (LSB)\n", bias[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel Z correct.: "
"%+13.3f (gee)\n", 1.f * bias[Z] / gravity);
}
accel_early_exit:
if (accel_error) {
bias[0] = bias[1] = bias[2] = 0;
return (1); /* error */
}
inv_free(p_vals);
return (0); /* success */
}
void InterruptPollDone(struct mpuirq_data ** data)
{
inv_free(data);
}
/*
* sam_fsa_open_inv - opens inventory for event log path
* inv - address of pointer for inventory to create
* path - absolute path to directory with events logs
* fs_name - family set name for filesystem events
* appname - unique identifier for application using this fsa api
*
* precond -
* inv pointer is null
* fs_name is valid family set name
* path points to valid directory with event logs for fs_name
* postcond -
* inv is allocated and populated with complete event log information
* inventory file is created in path with name 'fs_name.appname.inv'
*
* Returns 0 on success, -1 on failure
*/
int
sam_fsa_open_inv(
sam_fsa_inv_t **invp,
char *path,
char *fs_name,
char *appname)
{
sam_fsa_inv_t *inv;
char *inv_path;
int inv_fd;
int inv_sz;
int rst = 0;
if (*invp != NULL) {
Trace(TR_ERR, "Inventory table already allocated.");
return (-1);
}
inv_path = inv_build_path(path, fs_name, appname);
inv_fd = inv_open(inv_path, &inv_sz);
if (inv_fd < 0) {
Trace(TR_ERR, "Open inventory file failed.");
return (-1);
}
/*
* Allocate inventory table. Initially inv_sz+100 file entries
* allocated (one in fsalog_inv_t)
*/
SamMalloc(inv, sizeof (sam_fsa_inv_t) + inv_sz +
99*sizeof (sam_fsa_log_t));
memset(inv, 0, sizeof (sam_fsa_inv_t) + inv_sz +
99*sizeof (sam_fsa_log_t));
/* Initialize inventory */
strncpy(inv->fs_name, fs_name, sizeof (inv->fs_name));
inv->l_fsn = strlen(fs_name);
inv->n_logs = inv_sz / sizeof (sam_fsa_log_t);
inv->n_alloc = inv->n_logs+100;
inv->c_log = -1;
inv->fd_log = -1;
inv->fd_inv = inv_fd;
SamStrdup(inv->path_fsa, path);
inv->path_inv = inv_path;
/* Read inventory file. */
if (inv_sz > 0) {
rst = read(inv->fd_inv, &inv->logs[0], inv_sz);
if (rst < 0) {
Trace(TR_ERR, "read(%s) failed", inv->path_inv);
goto error;
}
if (rst != inv_sz) {
Trace(TR_ERR, "read(%s) failed: incomplete read",
inv->path_inv);
goto error;
}
}
if (inv_update(&inv) < 0) {
goto error;
}
*invp = inv;
return (0);
error:
inv_free(&inv);
return (-1);
}
