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); }