inv_error_t inv_write_cal(unsigned char *cal, size_t len)
{
FILE *fp;
int bytesWritten;
inv_error_t result = INV_SUCCESS;
if (len <= 0) {
MPL_LOGE("Nothing to write");
return INV_ERROR_FILE_WRITE;
}
else {
MPL_LOGI("cal data size to write = %d", len);
}
fp = fopen(MLCAL_FILE,"wb");
if (fp == NULL) {
MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
return INV_ERROR_FILE_OPEN;
}
bytesWritten = fwrite(cal, 1, len, fp);
if (bytesWritten != len) {
MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
bytesWritten, len);
result = INV_ERROR_FILE_WRITE;
}
else {
MPL_LOGI("Bytes written = %d", bytesWritten);
}
fclose(fp);
return result;
}
inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len)
{
inv_error_t result = INV_SUCCESS;
int bytesWritten = 0;
if (len <= 0) {
MPL_LOGE("Nothing to write");
return INV_ERROR_FILE_WRITE;
}
else {
MPL_LOGI("dmp firmware size to write = %d", len);
}
if ( fd == NULL ) {
return INV_ERROR_FILE_OPEN;
}
bytesWritten = fwrite(dmp, 1, len, fd);
if (bytesWritten != len) {
MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
bytesWritten, len);
result = INV_ERROR_FILE_WRITE;
}
else {
MPL_LOGI("Bytes written = %d", bytesWritten);
}
return result;
}
int inv_init_sysfs_attributes(void)
{
unsigned char i = 0;
char sysfs_path[MAX_SYSFS_NAME_LEN];
char *sptr;
char **dptr;
sysfs_names_ptr =
(char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
sptr = sysfs_names_ptr;
if (sptr != NULL) {
dptr = (char**)&mpu;
do {
*dptr++ = sptr;
sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
} while (++i < MAX_SYSFS_ATTRB);
} else {
MPL_LOGE("couldn't alloc mem for sysfs paths");
return -1;
}
// get proper (in absolute/relative) IIO path & build MPU's sysfs paths
inv_get_sysfs_path(sysfs_path);
sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable");
sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
sprintf(mpu.orientation_on, "%s%s", sysfs_path, "/orientation_on");
sprintf(mpu.event_flick, "%s%s", sysfs_path, "/event_flick");
sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
sprintf(mpu.event_orientation, "%s%s", sysfs_path, "/event_orientation");
sprintf(mpu.event_tap, "%s%s", sysfs_path, "/event_tap");
sprintf(mpu.flick_axis, "%s%s", sysfs_path, "/flick_axis");
sprintf(mpu.flick_counter, "%s%s", sysfs_path, "/flick_counter");
sprintf(mpu.flick_int_on, "%s%s", sysfs_path, "/flick_int_on");
sprintf(mpu.flick_lower, "%s%s", sysfs_path, "/flick_lower");
sprintf(mpu.flick_upper, "%s%s", sysfs_path, "/flick_upper");
sprintf(mpu.flick_message_on, "%s%s", sysfs_path, "/flick_message_on");
sprintf(mpu.tap_min_count, "%s%s", sysfs_path, "/tap_min_count");
sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
sprintf(mpu.tap_threshold, "%s%s", sysfs_path, "/tap_threshold");
sprintf(mpu.tap_time, "%s%s", sysfs_path, "/tap_time");
#if 0
// test print sysfs paths
dptr = (char**)&mpu;
for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
MPL_LOGE("sysfs path: %s", *dptr++);
}
#endif
return 0;
}
/**
* @brief An MPL wrapper for the main MPU Self Test API inv_factory_calibrate().
* See inv_factory_calibrate() function for more details.
*
* @pre inv_dmp_open() <b>must</b> have been called to populate the mldl_cfg
* data structure.
* On Windows, SetupPlatform() is also a madatory pre condition to
* ensure the accelerometer is properly configured before running the
* test.
*
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @param provide_result
* If 1:
* perform and analyze the offset, drive frequency, and noise
* calculation and compare it against set thresholds. Report
* also the final result using a bit-mask like error code as
* described in the inv_test_gyro_xxxx() functions.
* When 0:
* skip the noise and drive frequency calculation and pass/fail
* assessment. It simply calculates the gyro and accel biases.
* NOTE: for MPU6050 devices, this parameter is currently
* ignored.
*
* @return INV_SUCCESS or first non-zero error code otherwise.
*/
inv_error_t inv_self_test_factory_calibrate(void *mlsl_handle,
unsigned char provide_result)
{
INVENSENSE_FUNC_START;
inv_error_t firstError = INV_SUCCESS;
inv_error_t result;
unsigned char initState = inv_get_state();
if (initState < INV_STATE_DMP_OPENED) {
MPL_LOGE("Self Test cannot run before inv_dmp_open()\n");
return INV_ERROR_SM_IMPROPER_STATE;
}
/* obtain a pointer to the 'struct mldl_cfg' data structure. */
mputestCfgPtr = inv_get_dl_config();
if(initState == INV_STATE_DMP_STARTED) {
result = inv_dmp_stop();
ERROR_CHECK_FIRST(firstError, result);
}
result = inv_factory_calibrate(mlsl_handle, provide_result);
ERROR_CHECK_FIRST(firstError, result);
if(initState == INV_STATE_DMP_STARTED) {
result = inv_dmp_start();
ERROR_CHECK_FIRST(firstError, result);
}
return firstError;
}
/**
* @brief Loads a set of calibration data.
* It parses a binary data set containing calibration data.
* The binary data set is intended to be loaded from a file.
*
* @pre
*
*
* @param calData
* A pointer to an array of bytes to be parsed.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t inv_load_cal(unsigned char *calData)
{
int calType = 0;
int len = 0;
//int ptr;
//uint32_t chk = 0;
//uint32_t cmp_chk = 0;
/*load_func_t loaders[] = {
inv_load_cal_V0,
inv_load_cal_V1,
};
*/
inv_load_cal_V0(calData, len);
/* read the header (type and len)
len is the total record length including header and checksum */
len = 0;
len += 16777216L * ((int)calData[0]);
len += 65536L * ((int)calData[1]);
len += 256 * ((int)calData[2]);
len += (int)calData[3];
calType = ((int)calData[4]) * 256 + ((int)calData[5]);
if (calType > 5) {
MPL_LOGE("Unsupported calibration file format %d. "
"Valid types 0..5\n", calType);
return INV_ERROR_INVALID_PARAMETER;
}
/* call the proper method to read in the data */
//return loaders[calType] (calData, len);
return 0;
}
/**
* @brief umplStop stops the uMPL state machine. This function
* in turn calls the necessary MPL functions like inv_dmp_stop()
* and inv_dmp_close() to stop the motion processing algorithms.
*
*
* @pre umplInit() and umplStartMPU() must have been called.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t umplStop(void)
{
inv_error_t result;
if (umplState == UMPL_RUN)
{
#ifndef UMPL_DISABLE_STORE_CAL
result = inv_ustore_calibration();
if (result != INV_SUCCESS)
MPL_LOGE("inv_ustore_calibration failed with %d in umplStop\n",result);
#endif
}
if (umplState == UMPL_RUN || umplState == UMPL_ACCEL_ONLY ||
umplState == UMPL_LPACCEL_ONLY)
{
result = inv_dmp_stop();
if (result != INV_SUCCESS) return result;
result = inv_dmp_close();
if (result != INV_SUCCESS) return result;
umplSetState(UMPL_STOP);
return INV_SUCCESS;
}
return INV_ERROR_SM_IMPROPER_STATE;
}
/**
* @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;
}
inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
{
FILE *fp;
inv_error_t result = INV_SUCCESS;
size_t fsize;
fp = fopen(MLCAL_FILE,"rb");
if (fp == NULL) {
MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
return INV_ERROR_FILE_OPEN;
}
// obtain file size
fseek (fp, 0 , SEEK_END);
fsize = ftell (fp);
rewind (fp);
*calData = (unsigned char *)inv_malloc(fsize);
if (*calData==NULL) {
MPL_LOGE("Could not allocate buffer of %d bytes - "
"aborting\n", fsize);
fclose(fp);
return INV_ERROR_MEMORY_EXAUSTED;
}
*bytesRead = fread(*calData, 1, fsize, fp);
if (*bytesRead != fsize) {
MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
*bytesRead, fsize);
result = INV_ERROR_FILE_READ;
goto read_cal_end;
}
else {
MPL_LOGI("Bytes read = %d", *bytesRead);
}
read_cal_end:
fclose(fp);
return result;
}
/**
* find_type_by_name() - function to match top level types by name
* @name: top level type instance name
* @type: the type of top level instance being sort
*
* Typical types this is used for are device and trigger.
**/
int find_type_by_name(const char *name, const char *type)
{
const struct dirent *ent;
int number, numstrlen;
FILE *nameFile;
DIR *dp;
char thisname[IIO_MAX_NAME_LENGTH];
char *filename;
dp = opendir(iio_dir);
if (dp == NULL) {
MPL_LOGE("No industrialio devices available");
return -ENODEV;
}
while (ent = readdir(dp), ent != NULL) {
if (strcmp(ent->d_name, ".") != 0 &&
strcmp(ent->d_name, "..") != 0 &&
strlen(ent->d_name) > strlen(type) &&
strncmp(ent->d_name, type, strlen(type)) == 0) {
numstrlen = sscanf(ent->d_name + strlen(type),
"%d",
&number);
/* verify the next character is not a colon */
if (strncmp(ent->d_name + strlen(type) + numstrlen,
":",
1) != 0) {
filename = malloc(strlen(iio_dir)
+ strlen(type)
+ numstrlen
+ 6);
if (filename == NULL)
return -ENOMEM;
sprintf(filename, "%s%s%d/name",
iio_dir,
type,
number);
nameFile = fopen(filename, "r");
if (!nameFile)
continue;
free(filename);
fscanf(nameFile, "%s", thisname);
if (strcmp(name, thisname) == 0)
return number;
fclose(nameFile);
}
}
}
return -ENODEV;
}
int read_sysfs_int(char *filename, int *var)
{
int res=0;
FILE *fp;
fp = fopen(filename, "r");
if (fp!=NULL) {
fscanf(fp, "%d\n", var);
fclose(fp);
} else {
MPL_LOGE("ERR open file to read");
res= -1;
}
return res;
}
int write_sysfs_int(char *filename, int data)
{
int res=0;
FILE *fp;
fp = fopen(filename, "w");
if (fp!=NULL) {
fprintf(fp, "%d\n", data);
fclose(fp);
} else {
MPL_LOGE("ERR open file to write");
res= -1;
}
return res;
}
/**
* @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;
}
inv_error_t inv_load_lite_fusion_data(void)
{
inv_error_t result_load, result_set;
result_load = inv_uload_mem(accel_bias, sizeof(accel_bias));
if (result_load != INV_SUCCESS) {
MPL_LOGE("inv_uload_mem failed with accel_bias\n");
LOG_RESULT_LOCATION(result_load);
return result_load;
}
result_set = inv_set_accel_bias(accel_bias);
if (result_set != INV_SUCCESS) {
LOG_RESULT_LOCATION(result_set);
return result_set;
}
return INV_SUCCESS;
}
/** Registers to receive a callback when there is new sensor data.
* @internal
* @param[in] func Function pointer to receive callback when there is new sensor data
* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority
* numbers must be unique.
* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be
* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW =
* gyro data, INV_MAG_NEW = compass data. So passing in
* INV_ACCEL_NEW | INV_MAG_NEW, a
* callback would be generated if there was new magnetomer data OR new accel data.
*/
inv_error_t inv_register_data_cb(
inv_error_t (*func)(struct inv_sensor_cal_t *data),
int priority, int sensor_type)
{
inv_error_t result = INV_SUCCESS;
int kk, nn;
// Make sure we haven't registered this function already
// Or used the same priority
for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
if ((inv_data_builder.process[kk].func == func) ||
(inv_data_builder.process[kk].priority == priority)) {
return INV_ERROR_INVALID_PARAMETER; //fixme give a warning
}
}
// Make sure we have not filled up our number of allowable callbacks
if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) {
kk = 0;
if (inv_data_builder.num_cb != 0) {
// set kk to be where this new callback goes in the array
while ((kk < inv_data_builder.num_cb) &&
(inv_data_builder.process[kk].priority < priority)) {
kk++;
}
if (kk != inv_data_builder.num_cb) {
// We need to move the others
for (nn = inv_data_builder.num_cb; nn > kk; --nn) {
inv_data_builder.process[nn] =
inv_data_builder.process[nn - 1];
}
}
}
// Add new callback
inv_data_builder.process[kk].func = func;
inv_data_builder.process[kk].priority = priority;
inv_data_builder.process[kk].data_required = sensor_type;
inv_data_builder.num_cb++;
} else {
MPL_LOGE("Unable to add feature callback as too many were already registered\n");
result = INV_ERROR_MEMORY_EXAUSTED;
}
return result;
}
/**
* @brief umplStartMPU kicks starts the uMPL state machine. This function
* in turn calls the MPL functions like inv_dmp_open() and inv_dmp_start() which starts
* the motion processing algorithms. This function also enables the required features
* such as turning on the bias trackers and temperature compensation.
* This function enables the required type of data to be put in FIFO.
*
*
*
* @pre umplInit() must have been called.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t umplStartMPU(void)
{
inv_error_t result;
if (umplState == UMPL_STOP)
{
MPL_LOGV("UMPL_STOP to UMPL_RUN\n");
result = inv_dmp_open();
if (result != INV_SUCCESS) return result;
result = umplDmpSetup();
if (result != INV_SUCCESS) return result;
result = inv_dmp_start();
if (result != INV_SUCCESS) return result;
#ifndef UMPL_DISABLE_LOAD_CAL
result = inv_uload_calibration();
if (result != INV_SUCCESS)
MPL_LOGE("inv_uload_calibration failed with %d in umplStartMPU\n",result);
#endif
umplSetState(UMPL_RUN);
return INV_SUCCESS;
}
else if( umplState == UMPL_ACCEL_ONLY )
{
struct mldl_cfg * mldl_cfg = inv_get_dl_config();
MPL_LOGD("UMPL_ACCEL_ONLY (or UMPL_LPACCEL_ONLY) to UMPL_RUN\n");
if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]) {
inv_set_mpu_sensors( INV_NINE_AXIS );
} else {
inv_set_mpu_sensors( INV_SIX_AXIS_GYRO_ACCEL );
}
inv_set_fifo_rate(fifo_rate);
umplSetState(UMPL_RUN);
return INV_SUCCESS;
}
else if( umplState == UMPL_LPACCEL_ONLY )
{
umplStartAccelOnly(0.0);
umplStartMPU();
}
return INV_ERROR_SM_IMPROPER_STATE;
}
inv_error_t inv_store_lite_fusion_data(void)
{
inv_error_t result_get, result_store;
result_get = inv_get_accel_bias(accel_bias);
if (result_get != INV_SUCCESS) {
LOG_RESULT_LOCATION(result_get);
/* populate with dummy vals and do not return:
* call to inv_ustore_mem is required for ustore to work properly. */
accel_bias[0] = 0; accel_bias[1] = 0; accel_bias[2] = 0;
}
result_store = inv_ustore_mem(accel_bias, sizeof(accel_bias));
if (result_store != INV_SUCCESS) {
MPL_LOGE("inv_ustore_mem failed with accel_bias\n");
LOG_RESULT_LOCATION(result_store);
return result_store;
}
return result_get;
}
/**
* Prints the new or current orientation using MPL_LOGI and remembers the last
* orientation to print orientation flips.
*
* @param orientation the new or current orientation. 0 to reset.
*/
void PrintOrientation(unsigned short orientation)
{
// Determine if it was a flip
static int sLastOrientation = 0;
int flip = orientation | sLastOrientation;
if ((ML_X_UP | ML_X_DOWN) == flip) {
MPL_LOGI("Flip about the X Axis: \n");
} else if ((ML_Y_UP | ML_Y_DOWN) == flip) {
MPL_LOGI("Flip about the Y axis: \n");
} else if ((ML_Z_UP | ML_Z_DOWN) == flip) {
MPL_LOGI("Flip about the Z axis: \n");
}
sLastOrientation = orientation;
switch (orientation) {
case ML_X_UP:
MPL_LOGI("X Axis is up\n");
break;
case ML_X_DOWN:
MPL_LOGI("X Axis is down\n");
break;
case ML_Y_UP:
MPL_LOGI("Y Axis is up\n");
break;
case ML_Y_DOWN:
MPL_LOGI("Y Axis is down\n");
break;
case ML_Z_UP:
MPL_LOGI("Z Axis is up\n");
break;
case ML_Z_DOWN:
MPL_LOGI("Z Axis is down\n");
break;
case 0:
break; /* Not an error. Resets sLastOrientation */
default:
MPL_LOGE("%s: Unreconized orientation %hx\n", __func__, orientation);
break;
}
}
/**
* @brief Stores a set of calibration data.
* It generates a binary data set containing calibration data.
* The binary data set is intended to be stored into a file.
*
* @pre inv_dmp_open()
*
* @param calData
* A pointer to an array of bytes to be stored.
* @param length
* The amount of bytes available in the array.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t inv_store_cal(unsigned char *calData, size_t length)
{
inv_error_t res = 0;
size_t size;
STORECAL_LOG("Entering inv_store_cal\n");
inv_get_mpl_state_size(&size);
MPL_LOGI("inv_get_mpl_state_size() : size=%d", size);
/* store data */
res = inv_save_mpl_states(calData, size);
if(res != 0)
{
MPL_LOGE("inv_save_mpl_states() failed");
}
STORECAL_LOG("Exiting inv_store_cal\n");
return INV_SUCCESS;
}
int inv_init_sysfs_attributes(void)
{
unsigned char i = 0;
char sysfs_path[MAX_SYSFS_NAME_LEN];
char *sptr;
char **dptr;
sysfs_names_ptr =
(char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
sptr = sysfs_names_ptr;
if (sptr != NULL) {
dptr = (char**)&mpu;
do {
*dptr++ = sptr;
sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
} while (++i < MAX_SYSFS_ATTRB);
} else {
MPL_LOGE("inv_self_test: couldn't alloc mem for sysfs paths");
return -1;
}
// Setup IIO sysfs path & build MPU's sysfs paths
sprintf(sysfs_path, "%s", IIO_SYSFS_PATH);
sprintf(mpu.name, "%s%s", sysfs_path, "/name");
sprintf(mpu.buffer_enable, "%s%s", sysfs_path, "/buffer/enable");
sprintf(mpu.master_enable, "%s%s", sysfs_path, "/master_enable");
sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
sprintf(mpu.gyro_x_calibbias, "%s%s",
sysfs_path, "/in_anglvel_x_calibbias");
sprintf(mpu.gyro_y_calibbias, "%s%s",
sysfs_path, "/in_anglvel_y_calibbias");
sprintf(mpu.gyro_z_calibbias, "%s%s",
sysfs_path, "/in_anglvel_z_calibbias");
sprintf(mpu.gyro_scale, "%s%s", sysfs_path, "/in_anglvel_self_test_scale");
sprintf(mpu.gyro_x_offset, "%s%s", sysfs_path, "/in_anglvel_x_offset");
sprintf(mpu.gyro_y_offset, "%s%s", sysfs_path, "/in_anglvel_y_offset");
sprintf(mpu.gyro_z_offset, "%s%s", sysfs_path, "/in_anglvel_z_offset");
sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accel_enable");
sprintf(mpu.accel_x_calibbias, "%s%s", sysfs_path, "/in_accel_x_calibbias");
sprintf(mpu.accel_y_calibbias, "%s%s", sysfs_path, "/in_accel_y_calibbias");
sprintf(mpu.accel_z_calibbias, "%s%s", sysfs_path, "/in_accel_z_calibbias");
sprintf(mpu.accel_scale, "%s%s", sysfs_path, "/in_accel_self_test_scale");
sprintf(mpu.accel_x_offset, "%s%s", sysfs_path, "/in_accel_x_offset");
sprintf(mpu.accel_y_offset, "%s%s", sysfs_path, "/in_accel_y_offset");
sprintf(mpu.accel_z_offset, "%s%s", sysfs_path, "/in_accel_z_offset");
sprintf(mpu.compass_enable, "%s%s", sysfs_path, "/compass_enable");
#if 0
// test print sysfs paths
dptr = (char**)&mpu;
for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
printf("inv_self_test: sysfs path: %s\n", *dptr++);
}
#endif
return 0;
}
/**
* @brief close the file.
* @param fp handle to file to close.
* @return error code.
*/
void inv_fclose(FILE *fp)
{
MPL_LOGE("inv_fclose should not be used!\n");
return;
}
/**
* @brief open file
* @param filename name of the file to open.
* @return error code.
*/
FILE *inv_fopen(char *filename)
{
MPL_LOGE("inv_fopen should not be used!\n");
return NULL;
}
/**
* @brief Free allocated space.
* @param ptr pointer to space to deallocate
* @return error code.
*/
inv_error_t inv_free(void *ptr)
{
MPL_LOGE("inv_free should not be used!\n");
return INV_SUCCESS;
}
/**
* @brief Allocate space.
* @param numBytes
* number of bytes.
* @return pointer to allocated space.
*/
void *inv_malloc(unsigned int numBytes)
{
MPL_LOGE("inv_malloc should not be used!\n");
return NULL;
}
/**
* Handles a keyboard input and updates an appropriate threshold, prints then
* menu or returns false if the character is not processed.
*
* @param params The parameters to modify if the thresholds are updated
* @param ch The input character
*
* @return TRUE if the character was processed, FALSE otherwise
*/
tMLError GestureMenuProcessChar(tGestureMenuParams * const params, char ch)
{
int result = ML_SUCCESS;
/* Dynamic keyboard processing */
switch (ch) {
case 'j':
params->xTapThreshold += 20;
// Intentionally fall through
case 'J': {
params->xTapThreshold -= 10;
if (params->xTapThreshold < 0) params->xTapThreshold = 0;
result = MLSetTapThreshByAxis(ML_TAP_AXIS_X, params->xTapThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapThresh returned :%d\n", result);
}
MPL_LOGI("MLSetTapThreshByAxis(ML_TAP_AXIS_X, %d)\n",
params->xTapThreshold);
} break;
case 'k':
params->yTapThreshold += 20;
// Intentionally fall through
case 'K': {
params->yTapThreshold -= 10;
if (params->yTapThreshold < 0) params->yTapThreshold = 0;
result = MLSetTapThreshByAxis(ML_TAP_AXIS_Y, params->yTapThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapThresh returned :%d\n", result);
}
MPL_LOGI("MLSetTapThreshByAxis(ML_TAP_AXIS_Y, %d)\n",
params->yTapThreshold);
} break;
case 'i':
params->zTapThreshold += 20;
// Intentionally fall through
case 'I': {
params->zTapThreshold -= 10;
if (params->zTapThreshold < 0) params->zTapThreshold = 0;
result = MLSetTapThreshByAxis(ML_TAP_AXIS_Z, params->zTapThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapThresh returned :%d\n", result);
}
MPL_LOGI("MLSetTapThreshByAxis(ML_TAP_AXIS_Z, %d)\n",
params->zTapThreshold);
} break;
case 'l':
params->tapTime += 20;
// Intentionally fall through
case 'L': {
params->tapTime -= 10;
if (params->tapTime < 0) params->tapTime = 0;
result = MLSetTapTime(params->tapTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapTime returned :%d\n", result);
}
MPL_LOGI("MLSetTapTime(%d)\n", params->tapTime);
} break;
case 'o':
params->nextTapTime += 20;
// Intentionally fall through
case 'O': {
params->nextTapTime -= 10;
if (params->nextTapTime < 0) params->nextTapTime = 0;
result = MLSetNextTapTime(params->nextTapTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetNextTapTime returned :%d\n", result);
}
MPL_LOGI("MLSetNextTapTime(%d)\n", params->nextTapTime);
} break;
case 'u':
params->maxTaps += 2;
// Intentionally fall through
case 'U': {
params->maxTaps -= 1;
if (params->maxTaps < 0) params->maxTaps = 0;
result = MLSetMaxTaps(params->maxTaps);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetMaxTaps returned :%d\n", result);
}
MPL_LOGI("MLSetMaxTaps(%d)\n", params->maxTaps);
} break;
case 's': {
int shakeParam;
params->shakeFunction = (params->shakeFunction + 1) % 2;
switch (params->shakeFunction)
{
case 0:
shakeParam = ML_NO_RETRACTION;
MPL_LOGE("MLSetShakeFunc(ML_NO_RETRACTION)\n");
break;
case 1:
shakeParam = ML_RETRACTION;
MPL_LOGI("MLSetShakeFunc(ML_RETRACTION)\n");
break;
};
result = MLSetShakeFunc(shakeParam);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeFunc returned :%d\n", result);
}
} break;
case 'x':
params->xShakeThresh += 200;
// Intentionally fall through
case 'X': {
params->xShakeThresh -= 100;
result = MLSetShakeThresh(ML_PITCH_SHAKE, params->xShakeThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetShakeThresh(ML_PITCH_SHAKE, %d)\n", params->xShakeThresh);
} break;
case 'y':
params->yShakeThresh += 200;
// Intentionally fall through
case 'Y': {
params->yShakeThresh -= 100;
result = MLSetShakeThresh(ML_ROLL_SHAKE, params->yShakeThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetShakeThresh(ML_ROLL_SHAKE, %d)\n", params->yShakeThresh);
} break;
case 'z':
params->zShakeThresh += 200;
// Intentionally fall through
case 'Z':{
params->zShakeThresh -= 100;
result = MLSetShakeThresh(ML_YAW_SHAKE, params->zShakeThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetShakeThresh(ML_YAW_SHAKE, %d)\n",params->zShakeThresh);
} break;
case 'r':
params->ySnapThresh += 20;
// Intentionally fall through
case 'R': {
params->ySnapThresh -= 10;
result = MLSetHardShakeThresh(ML_ROLL_SHAKE, params->ySnapThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetHardShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetHardShakeThresh(ML_ROLL_SHAKE, %d)\n",params->ySnapThresh);
} break;
case 'p':
params->xSnapThresh += 20;
// Intentionally fall through
case 'P': {
params->xSnapThresh -= 10;
result = MLSetHardShakeThresh(ML_PITCH_SHAKE, params->xSnapThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetHardShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetHardShakeThresh(ML_PITCH_SHAKE, %d)\n",
params->xSnapThresh);
} break;
case 'a':
params->zSnapThresh += 20;
case 'A': {
params->zSnapThresh -= 10;
result = MLSetHardShakeThresh(ML_YAW_SHAKE, params->zSnapThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetHardShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetHardShakeThresh(ML_YAW_SHAKE, %d)\n",params->zSnapThresh);
} break;
case 't':
params->shakeTime += 20;
case 'T':{
params->shakeTime -= 10;
result = MLSetShakeTime(params->shakeTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeTime returned :%d\n", result);
}
MPL_LOGI("MLSetShakeTime(%d)\n", params->shakeTime);
} break;
case 'n':
params->nextShakeTime += 20;
case 'N':{
params->nextShakeTime -= 10;
result = MLSetNextShakeTime(params->nextShakeTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetNextShakeTime returned :%d\n", result);
}
MPL_LOGI("MLSetNextShakeTime(%d)\n", params->nextShakeTime);
} break;
case 'm':
params->maxShakes += 2;
case 'M':{
params->maxShakes -= 1;
result = MLSetMaxShakes(ML_SHAKE_ALL, params->maxShakes);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetMaxShakes returned :%d\n", result);
}
MPL_LOGI("MLSetMaxShakes(%d)\n", params->maxShakes);
} break;
case 'e':
params->yawRotateTime += 20;
case 'E':{
params->yawRotateTime -= 10;
result = MLSetYawRotateTime(params->yawRotateTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetYawRotateTime returned :%d\n", result);
}
MPL_LOGI("MLSetYawRotateTime(%d)\n", params->yawRotateTime);
} break;
case 'w':
params->yawRotateThreshold += 2;
case 'W':{
params->yawRotateThreshold -= 1;
result = MLSetYawRotateThresh(params->yawRotateThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetYawRotateThresh returned :%d\n", result);
}
MPL_LOGI("MLSetYawRotateThresh(%d)\n", params->yawRotateThreshold);
} break;
case 'c':
params->shakeRejectValue += 0.20f;
case 'C':{
params->shakeRejectValue -= 0.10f;
result = MLSetTapShakeReject(params->shakeRejectValue);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapShakeReject returned :%d\n", result);
}
MPL_LOGI("MLSetTapShakeReject(%f)\n", params->shakeRejectValue);
} break;
case 'd':
params->orientationThreshold += 10;
case 'D':{
params->orientationThreshold -= 5;
if (params->orientationThreshold > 90) {
params->orientationThreshold = 90;
}
if (params->orientationThreshold < 0 ) {
params->orientationThreshold = 0;
}
result = MLSetOrientationThreshold(params->orientationThreshold,
5, 80,
ML_X_AXIS | ML_Y_AXIS | ML_Z_AXIS);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetOrientationThreshold returned :%d\n", result);
}
MPL_LOGI("MLSetOrientationThreshold(%f, %d, %d,"
" ML_X_AXIS | ML_Y_AXIS | ML_Z_AXIS)\n",
params->orientationThreshold, 5, 80);
} break;
case 'f':
result = MLSetFIFORate(MLGetFIFORate() + 1);
MPL_LOGI("MLSetFIFORate(%d)\n",MLGetFIFORate());
break;
case 'F':
{
unsigned short newRate = MLGetFIFORate();
if (newRate > 0)
newRate--;
result = MLSetFIFORate(newRate);
MPL_LOGI("MLSetFIFORate(%d)\n",MLGetFIFORate());
break;
}
case 'S':
params->sensorsIndex++;
if (params->sensorsIndex >= DIM(sensors)) {
params->sensorsIndex = 0;
}
result = MLSetMPUSensors(sensors[params->sensorsIndex]);
ERROR_CHECK(result);
MPL_LOGI("%d = MLSetMPUSensors(%s)\n", result,
sensors_string[params->sensorsIndex]);
break;
case 'h':
case 'H': {
PrintGestureMenu(params);
} break;
default: {
result = ML_ERROR;
} break;
};
return result;
}
/**
* Call the appropriate MPL set threshold functions and checkes the error codes
*
* @param params The parametrs to use in setting the thresholds
*
* @return ML_SUCCESS or the first error code encountered.
*/
tMLError GestureMenuSetMpl(tGestureMenuParams const * const params)
{
tMLError result = ML_SUCCESS;
result = MLSetTapThreshByAxis(ML_TAP_AXIS_X, params->xTapThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapThreshByAxis returned :%d\n", result);
return result;
}
result = MLSetTapThreshByAxis(ML_TAP_AXIS_Y, params->yTapThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapThreshByAxis returned :%d\n", result);
return result;
}
result = MLSetTapThreshByAxis(ML_TAP_AXIS_Z, params->zTapThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapThreshByAxis returned :%d\n", result);
return result;
}
result = MLSetTapTime(params->tapTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapTime returned :%d\n", result);
return result;
}
result = MLSetNextTapTime(params->nextTapTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetNextTapTime returned :%d\n", result);
return result;
}
result = MLSetMaxTaps(params->maxTaps);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetMaxTaps returned :%d\n", result);
return result;
}
result = MLSetTapShakeReject(params->shakeRejectValue);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapShakeReject returned :%d\n", result);
return result;
}
//Set up shake gesture
result = MLSetShakeFunc(params->shakeFunction);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeFunc returned :%d\n", result);
return result;
}
result = MLSetShakeThresh(ML_ROLL_SHAKE, params->xShakeThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeThresh returned :%d\n", result);
return result;
}
result = MLSetShakeThresh(ML_PITCH_SHAKE, params->yShakeThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeThresh returned :%d\n", result);
return result;
}
result = MLSetShakeThresh(ML_YAW_SHAKE, params->zShakeThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeThresh returned :%d\n", result);
return result;
}
result = MLSetShakeTime(params->shakeTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeTime returned :%d\n", result);
return result;
}
result = MLSetNextShakeTime(params->nextShakeTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetNextShakeTime returned :%d\n", result);
return result;
}
result = MLSetMaxShakes(ML_SHAKE_ALL,params->maxShakes);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetMaxShakes returned :%d\n", result);
return result;
}
// Yaw rotate settings
result = MLSetYawRotateTime(params->yawRotateTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetYawRotateTime returned :%d\n", result);
return result;
}
result = MLSetYawRotateThresh(params->yawRotateThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetYawRotateThresh returned :%d\n", result);
return result;
}
// Orientation settings
result = MLSetOrientationThreshold(params->orientationThreshold, 5, 80,
ML_X_AXIS | ML_Y_AXIS | ML_Z_AXIS);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetOrientationThreshold returned: %d\n", result);
return result;
}
// Requested Sensors
result = MLSetMPUSensors(sensors[params->sensorsIndex]);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetMPUSesnors returned: %d %lx\n", result,
sensors[params->sensorsIndex]);
return result;
}
return ML_SUCCESS;
}
void PrintGesture(tGesture* gesture)
{
float speed;
char type[1024];
switch (gesture->type)
{
case ML_TAP:
{
if (gesture->meta < 0) {
snprintf(type,sizeof(type),"-");
} else {
snprintf(type,sizeof(type),"+");
}
switch (ABS(gesture->meta))
{
case 1:
strcat(type,"X");
break;
case 2:
strcat(type,"Y");
break;
case 3:
strcat(type,"Z");
break;
default:
strcat(type,"ERROR");
break;
};
MPL_LOGI("TAP: %s %2d, X: %6d Y: %6d Z: %6d XY: %6.2f, YZ: %6.2f, XZ: %6.2f\n",
type,
gesture->num,
gesture->strength,
gesture->speed,
gesture->reserved,
(180 / M_PI) * atan2(
(float)gesture->strength, (float)gesture->speed),
(180 / M_PI) * atan2(
(float)gesture->speed, (float)gesture->reserved),
(180 / M_PI) * atan2(
(float)gesture->strength, (float)gesture->reserved)
);
}
break;
case ML_ROLL_SHAKE:
case ML_PITCH_SHAKE:
case ML_YAW_SHAKE:
{
if (gesture->strength){
snprintf(type, sizeof(type), "Snap : ");
} else {
snprintf(type, sizeof(type), "Shake: ");
}
if (gesture->meta==0) {
strcat(type, "+");
} else {
strcat(type, "-");
}
if (gesture->type == ML_ROLL_SHAKE) {
strcat(type, "Roll ");
} else if (gesture->type == ML_PITCH_SHAKE) {
strcat(type, "Pitch ");
} else if (gesture->type == ML_YAW_SHAKE) {
strcat(type, "Yaw ");
}
speed = (float)gesture->speed +
(float)(gesture->reserved / (float)(1 << 16));
MPL_LOGI("%s:%3d (speed: %8.2f)\n",type, gesture->num, speed);
}
break;
case ML_YAW_IMAGE_ROTATE:
{
if (gesture->meta == 0) {
snprintf(type, sizeof(type), "Positive ");
} else {
snprintf(type, sizeof(type), "Negative ");
}
MPL_LOGI("%s Yaw Image Rotation\n", type);
}
break;
default:
MPL_LOGE("Unknown Gesture received\n");
break;
}
}
int find_name_by_sensor_type(const char *sensor_type, const char *type, char *sensor_name)
{
const struct dirent *ent;
int number, numstrlen;
FILE *nameFile;
DIR *dp;
char *filename;
dp = opendir(iio_dir);
if (dp == NULL) {
MPL_LOGE("No industrialio devices available");
return -ENODEV;
}
while (ent = readdir(dp), ent != NULL) {
if (strcmp(ent->d_name, ".") != 0 &&
strcmp(ent->d_name, "..") != 0 &&
strlen(ent->d_name) > strlen(type) &&
strncmp(ent->d_name, type, strlen(type)) == 0) {
numstrlen = sscanf(ent->d_name + strlen(type),
"%d",
&number);
/* verify the next character is not a colon */
if (strncmp(ent->d_name + strlen(type) + numstrlen,
":",
1) != 0) {
filename = malloc(strlen(iio_dir)
+ strlen(type)
+ numstrlen
+ 6
+ strlen(sensor_type));
if (filename == NULL)
return -ENOMEM;
sprintf(filename, "%s%s%d/%s",
iio_dir,
type,
number,
sensor_type);
nameFile = fopen(filename, "r");
MPL_LOGI("sensor type path: %s\n", filename);
free(filename);
//fscanf(nameFile, "%s", thisname);
//if (strcmp(name, thisname) == 0) {
if(nameFile == NULL) {
MPL_LOGI("keeps searching");
continue;
} else{
MPL_LOGI("found directory");
}
filename = malloc(strlen(iio_dir)
+ strlen(type)
+ numstrlen
+ 6);
sprintf(filename, "%s%s%d/name",
iio_dir,
type,
number);
nameFile = fopen(filename, "r");
MPL_LOGI("name path: %s\n", filename);
free(filename);
if (!nameFile)
continue;
fscanf(nameFile, "%s", sensor_name);
MPL_LOGI("name found: %s now test for mpuxxxx", sensor_name);
if( !strncmp("mpu",sensor_name, 3) ) {
char secondaryFileName[200];
sprintf(secondaryFileName, "%s%s%d/secondary_name",
iio_dir,
type,
number);
nameFile = fopen(secondaryFileName, "r");
MPL_LOGI("name path: %s\n", secondaryFileName);
if(!nameFile)
continue;
fscanf(nameFile, "%s", sensor_name);
MPL_LOGI("secondary name found: %s\n", sensor_name);
}
else {
fscanf(nameFile, "%s", sensor_name);
MPL_LOGI("name found: %s\n", sensor_name);
}
return 0;
//}
fclose(nameFile);
}
}
}
return -ENODEV;
}
