/**
* @brief This function will enable/disable sensor.
* @param[in] handle
* which sensor to enable/disable.
* @param[in] en
* en=1, enable;
* en=0, disable
* @return if the operation is successful.
*/
int CompassSensor::enable(int32_t handle, int en)
{
VFUNC_LOG;
mEnable = en;
int tempFd;
int res = 0;
/* reset master enable */
res = masterEnable(0);
if (res < 0) {
return res;
}
if (en) {
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, compassSysFs.compass_x_fifo_enable, getTimestamp());
res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en);
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, compassSysFs.compass_y_fifo_enable, getTimestamp());
res += write_sysfs_int(compassSysFs.compass_y_fifo_enable, en);
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, compassSysFs.compass_z_fifo_enable, getTimestamp());
res += write_sysfs_int(compassSysFs.compass_z_fifo_enable, en);
res = masterEnable(en);
if (res < en) {
return res;
}
}
return res;
}
int enable_dmp_features(int en)
{
int res= -1;
if (dmp_fw_loaded()) {
/* Currently there's no info regarding DMP's supported features/capabilities
An error in enabling features below could be an indication of the feature
not supported in current loaded DMP firmware */
if (write_sysfs_int(mpu.enable, 0) < 0) {
printf("GT:ERR-can't write 'buffer/enable'");
return -1;
}
enable_tap(en);
enable_display_orientation(en);
//enable_orientation(en); // not available
res= 0;
if (write_sysfs_int(mpu.enable, 1) < 0) {
printf("GT:ERR-can't write 'buffer/enable'");
return -1;
}
} else {
printf("GT:ERR-No DMP firmware\n");
res= -1;
}
return res;
}
static void setup_dmp(char *dev_path){
char sysfs_path[200];
int ret;
FILE *fd;
sprintf(sysfs_path, "%s", dev_path);
printf("sysfs: %s\n", sysfs_path);
ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1);
if (ret < 0)
return;
ret = write_sysfs_int("in_accel_scale", dev_path, 0);
if (ret < 0)
return;
ret = write_sysfs_int("in_anglvel_scale", dev_path, 3);
if (ret < 0)
return;
ret = write_sysfs_int("sampling_frequency", sysfs_path, 200);
if (ret < 0)
return;
ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0);
if (ret < 0)
return;
sprintf(dmp_path, "%s/dmp_firmware", dev_path);
if ((fd = fopen(dmp_path, "wb")) < 0 ) {
perror("dmp fail");
}
inv_load_dmp(fd);
fclose(fd);
printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path));
ret = write_sysfs_int_and_verify("dmp_on", sysfs_path, 1);
if (ret < 0)
return;
ret = write_sysfs_int_and_verify("dmp_int_on", sysfs_path, 1);
if (ret < 0)
return;
/* selelct which event to enable and interrupt on/off here */
//enable_glu(sysfs_path, 0);
ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 0);
if (ret < 0)
return;
ret = write_sysfs_int_and_verify("pedometer_int_on", sysfs_path, 1);
ret = write_sysfs_int_and_verify("pedometer_on", sysfs_path, 1);
ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, 1);
write_sysfs_int64("pedometer_steps", sysfs_path, 0x3ffffffff);
write_sysfs_int64("pedometer_time", sysfs_path, 0xffffffff);
if (ret < 0)
return;
ret = setup_offset_and_bias();
return;
}
/**
* Initialize an ADC device
* @param[in] *adc The ADC device to initialize
*/
static inline void adc_dev_init(struct adc_t *adc)
{
char filename[32];
uint8_t i;
/* Enable all linked channels */
for(i = 0; i < adc->channels_cnt; i++) {
sprintf(filename, "scan_elemens/in_voltage%d_en", adc->channels[i]);
write_sysfs_int(adc->dev_id, filename, 1);
}
/* Set the buffer length */
write_sysfs_int(adc->dev_id, "buffer/length", adc->buf_length);
}
/**
* iio_fixup_sampling_frequency: Fixup devices *sampling_frequency attributes
* @dev: the IIO device to fix the sampling frequencies for
*
* Make sure devices with *sampling_frequency attributes are sampling at
* 10Hz or more. This fixes 2 problems:
* 1) Some buffered devices default their sampling_frequency to 0Hz and then
* never produce any readings.
* 2) Some polled devices default to 1Hz and wait for a fresh sample before
* returning from sysfs *_raw reads, blocking all of iio-sensor-proxy for
* multiple seconds
**/
gboolean
iio_fixup_sampling_frequency (GUdevDevice *dev)
{
GDir *dir;
const char *device_dir;
const char *name;
GError *error = NULL;
double sample_freq;
device_dir = g_udev_device_get_sysfs_path (dev);
dir = g_dir_open (g_udev_device_get_sysfs_path (dev), 0, &error);
if (!dir) {
g_warning ("Failed to open directory '%s': %s", device_dir, error->message);
g_error_free (error);
return FALSE;
}
while ((name = g_dir_read_name (dir))) {
if (g_str_has_suffix (name, "sampling_frequency") == FALSE)
continue;
sample_freq = g_udev_device_get_sysfs_attr_as_double (dev, name);
if (sample_freq >= IIO_MIN_SAMPLING_FREQUENCY)
continue; /* Continue with pre-set sample freq. */
/* Sample freq too low, set it to 10Hz */
if (write_sysfs_int (name, device_dir, IIO_MIN_SAMPLING_FREQUENCY) < 0)
g_warning ("Could not fix sample-freq for %s/%s", device_dir, name);
}
g_dir_close (dir);
return TRUE;
}
int CompassSensor::masterEnable(int en)
{
VFUNC_LOG;
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, compassSysFs.chip_enable, getTimestamp());
return write_sysfs_int(compassSysFs.chip_enable, en);
}
/**
* @brief This function will enable/disable sensor.
* @param[in] handle
* which sensor to enable/disable.
* @param[in] en
* en=1, enable;
* en=0, disable
* @return if the operation is successful.
*/
int CompassSensor::enable(int32_t handle, int en)
{
VFUNC_LOG;
int res = 0;
res = write_sysfs_int(compassSysFs.compass_enable, en);
return res;
}
int CompassSensor::turnOnCompassFifo(void)
{
int i, res = 0, tempFd;
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1, compassSysFs.compass_fifo_enable, getTimestamp());
res += write_sysfs_int(compassSysFs.compass_fifo_enable, 1);
return res;
}
int enable_orient(int en)
{
if (write_sysfs_int(mpu.orientation_on, en) < 0) {
printf("GT:ERR-can't write 'orientation_on'");
return -1;
}
return 0;
}
/**
* @brief This function will enable/disable sensor.
* @param[in] handle
* which sensor to enable/disable.
* @param[in] en
* en=1, enable;
* en=0, disable
* @return if the operation is successful.
*/
int CompassSensor::enable(int32_t handle, int en)
{
VFUNC_LOG;
int res = 0;
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, compassSysFs.compass_enable, getTimestamp());
res = write_sysfs_int(compassSysFs.compass_enable, en);
return res;
}
static void
disable_ring_buffer (BufferDrvData *data)
{
/* Stop the buffer */
write_sysfs_int ("buffer/enable", data->dev_dir_name, 0);
/* Disconnect the trigger - just write a dummy name. */
write_sysfs_string ("trigger/current_trigger", data->dev_dir_name, "NULL");
}
int enable_displ_orient(int en)
{
if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
printf("GT:ERR-can't write 'display_orientation_en'");
return -1;
}
return 0;
}
int enable_tap(int en)
{
if (write_sysfs_int(mpu.tap_on, en) < 0) {
printf("GT:ERR-can't write 'tap_on'");
return -1;
}
return 0;
}
/**
* enable_sensors: enable all the sensors in a device
* @device_dir: the IIO device directory in sysfs
* @
**/
static gboolean
enable_sensors (GUdevDevice *dev,
int enable)
{
GDir *dir;
char *device_dir;
const char *name;
gboolean ret = FALSE;
GError *error = NULL;
device_dir = g_build_filename (g_udev_device_get_sysfs_path (dev), "scan_elements", NULL);
dir = g_dir_open (device_dir, 0, &error);
if (!dir) {
g_warning ("Failed to open directory '%s': %s", device_dir, error->message);
g_free (device_dir);
g_error_free (error);
return FALSE;
}
while ((name = g_dir_read_name (dir))) {
char *path;
if (g_str_has_suffix (name, "_en") == FALSE)
continue;
/* Already enabled? */
path = g_strdup_printf ("scan_elements/%s", name);
if (g_udev_device_get_sysfs_attr_as_boolean (dev, path)) {
g_debug ("Already enabled sensor %s/%s", device_dir, name);
ret = TRUE;
g_free (path);
continue;
}
g_free (path);
/* Enable */
if (write_sysfs_int (name, device_dir, enable) < 0) {
g_warning ("Could not enable sensor %s/%s", device_dir, name);
continue;
}
ret = TRUE;
g_debug ("Enabled sensor %s/%s", device_dir, name);
}
g_dir_close (dir);
g_free (device_dir);
if (!ret) {
g_warning ("Failed to enable any sensors for device '%s'",
g_udev_device_get_sysfs_path (dev));
}
return ret;
}
/**
* @brief This function will enable/disable sensor.
* @param[in] handle
* which sensor to enable/disable.
* @param[in] en
* en=1, enable;
* en=0, disable
* @return if the operation is successful.
*/
int PressureSensor::enable(int32_t handle, int en)
{
VFUNC_LOG;
int res = 0;
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: echo %d > %s (%lld)",
en, pressureSysFs.pressure_enable, getTimestamp());
res = write_sysfs_int(pressureSysFs.pressure_enable, en);
return res;
}
int save_n_write_sysfs_int(char *filename, int data, int *old_value)
{
int res;
res = read_sysfs_int(filename, old_value);
if (res < 0) {
return res;
}
//printf("saved %s = %d\n", filename, *old_value);
res = write_sysfs_int(filename, data);
if (res < 0) {
return res;
}
return 0;
}
static gboolean
enable_ring_buffer (BufferDrvData *data)
{
int ret;
/* Setup ring buffer parameters */
ret = write_sysfs_int("buffer/length", data->dev_dir_name, 128);
if (ret < 0) {
g_warning ("Failed to set ring buffer length for %s", data->dev_dir_name);
return FALSE;
}
/* Enable the buffer */
ret = write_sysfs_int_and_verify("buffer/enable", data->dev_dir_name, 1);
if (ret < 0) {
g_warning ("Unable to enable ring buffer for %s", data->dev_dir_name);
return FALSE;
}
return TRUE;
}
int PressureSensor::setDelay(int32_t handle, int64_t ns)
{
VFUNC_LOG;
int res = 0;
mDelay = int(1000000000.f / ns);
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: echo %lld > %s (%lld)",
mDelay, pressureSysFs.pressure_rate, getTimestamp());
res = write_sysfs_int(pressureSysFs.pressure_rate, mDelay);
#ifdef TIMER
int t_poll_time = (int)(ns / 1000000LL);
if (t_poll_time > min_poll_time) {
s_poll_time = t_poll_time;
} else {
s_poll_time = min_poll_time;
}
LOGV_IF(PROCESS_VERBOSE,
"HAL:setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f/ns);
#endif
return res;
}
int enable_flick(int en)
{
int res=0;
int flickUpper=0, flickLower=0, flickCounter=0;
if (write_sysfs_int(mpu.flick_int_on, en) < 0) {
printf("GT:ERR-can't write 'flick_int_on'");
res= -1;
}
if (en) {
flickUpper= FLICK_UPPER_THRES;
flickLower= FLICK_LOWER_THRES;
flickCounter= FLICK_COUNTER;
}
if (write_sysfs_int(mpu.flick_upper, flickUpper) < 0) {
printf("GT:ERR-can't write 'flick_upper'");
res= -1;
}
if (write_sysfs_int(mpu.flick_lower, flickLower) < 0) {
printf("GT:ERR-can't write 'flick_lower'");
res= -1;
}
if (write_sysfs_int(mpu.flick_counter, flickCounter) < 0) {
printf("GT:ERR-can't write 'flick_counter'");
res= -1;
}
if (write_sysfs_int(mpu.flick_message_on, 0) < 0) {
printf("GT:ERR-can't write 'flick_message_on'");
res= -1;
}
if (write_sysfs_int(mpu.flick_axis, 0) < 0) {
printf("GT:ERR_can't write 'flick_axis'");
res= -1;
}
return res;
}
int main(int argc, char **argv)
{
int i, j, k, toread;
FILE *fp_ev;
int fp;
char *data;
size_t read_size;
struct iio_event_data dat;
char *BaseDirectoryName,
*TriggerDirectoryName,
*RingBufferDirectoryName;
BaseDirectoryName = find_type_by_name(device_name, "device");
if (BaseDirectoryName == NULL) {
printf("Failed to find the %s \n", device_name);
return -1;
}
TriggerDirectoryName = find_type_by_name(trigger_name, "trigger");
if (TriggerDirectoryName == NULL) {
printf("Failed to find the %s\n", trigger_name);
return -1;
}
RingBufferDirectoryName = find_ring_subelement(BaseDirectoryName,
"ring_buffer");
if (RingBufferDirectoryName == NULL) {
printf("Failed to find ring buffer\n");
return -1;
}
if (write_sysfs_string_and_verify("trigger/current_trigger",
BaseDirectoryName,
(char *)trigger_name) < 0) {
printf("Failed to write current_trigger file \n");
return -1;
}
/* Setup ring buffer parameters */
if (write_sysfs_int("length", RingBufferDirectoryName,
RingLength) < 0) {
printf("Failed to open the ring buffer length file \n");
return -1;
}
/* Enable the ring buffer */
if (write_sysfs_int("ring_enable", RingBufferDirectoryName, 1) < 0) {
printf("Failed to open the ring buffer control file \n");
return -1;
};
data = malloc(size_from_scanmode(NumVals, scan_ts)*RingLength);
if (!data) {
printf("Could not allocate space for usespace data store\n");
return -1;
}
/* Attempt to open non blocking the access dev */
fp = open(ring_access, O_RDONLY | O_NONBLOCK);
if (fp == -1) { /*If it isn't there make the node */
printf("Failed to open %s\n", ring_access);
return -1;
}
/* Attempt to open the event access dev (blocking this time) */
fp_ev = fopen(ring_event, "rb");
if (fp_ev == NULL) {
printf("Failed to open %s\n", ring_event);
return -1;
}
/* Wait for events 10 times */
for (j = 0; j < 10; j++) {
read_size = fread(&dat, 1, sizeof(struct iio_event_data),
fp_ev);
switch (dat.id) {
case IIO_EVENT_CODE_RING_100_FULL:
toread = RingLength;
break;
case IIO_EVENT_CODE_RING_75_FULL:
toread = RingLength*3/4;
break;
case IIO_EVENT_CODE_RING_50_FULL:
toread = RingLength/2;
break;
default:
printf("Unexpecteded event code\n");
continue;
}
read_size = read(fp,
data,
toread*size_from_scanmode(NumVals, scan_ts));
if (read_size == -EAGAIN) {
printf("nothing available \n");
continue;
}
for (i = 0;
i < read_size/size_from_scanmode(NumVals, scan_ts);
i++) {
for (k = 0; k < NumVals; k++) {
__s16 val = *(__s16 *)(&data[i*size_from_scanmode(NumVals, scan_ts)
+ (k)*2]);
printf("%05d ", val);
}
printf(" %lld\n",
*(__s64 *)(&data[(i+1)*size_from_scanmode(NumVals, scan_ts)
- sizeof(__s64)]));
}
}
/* Stop the ring buffer */
if (write_sysfs_int("ring_enable", RingBufferDirectoryName, 0) < 0) {
printf("Failed to open the ring buffer control file \n");
return -1;
};
/* Disconnect from the trigger - writing something that doesn't exist.*/
write_sysfs_string_and_verify("trigger/current_trigger",
BaseDirectoryName, "NULL");
free(BaseDirectoryName);
free(TriggerDirectoryName);
free(RingBufferDirectoryName);
free(data);
return 0;
}
int main(int argc, char **argv)
{
unsigned long buf_len = 240;
int ret, c, i;
char *trigger_name = NULL;
int datardytrigger = 1;
int trig_num;
char *dummy;
char chip_name[10];
char device_name[10];
char sysfs[100];
gyro_data_is_enabled = 0;
accel_data_is_enabled = 0;
compass_data_is_enabled = 0;
quaternion_data_is_enabled = 0;
while ((c = getopt(argc, argv, "lcd:e:rmp")) != -1) {
switch (c) {
case 'c':
has_compass = 1;
break;
case 'p':
has_pressure = 1;
break;
case 'd':
disable_delay = strtoul(optarg, &dummy, 10);
break;
case 'e':
enable_delay = strtoul(optarg, &dummy, 10);
break;
case 'r':
enable_random_delay = 1;
break;
case 'm':
enable_motion_on = 1;
break;
case '?':
return -1;
}
}
inv_get_sysfs_path(sysfs);
printf("sss:::%s\n", sysfs);
if (inv_get_chip_name(chip_name) != INV_SUCCESS) {
printf("get chip name fail\n");
exit(0);
}
printf("chip_name=%s\n", chip_name);
if (INV_SUCCESS != inv_check_key())
printf("key check fail\n");
else
printf("key authenticated\n");
for (i=0; i<strlen(chip_name); i++) {
device_name[i] = tolower(chip_name[i]);
}
device_name[strlen(chip_name)] = '\0';
printf("device name: %s\n", device_name);
/* Find the device requested */
dev_num = find_type_by_name(device_name, "iio:device");
if (dev_num < 0) {
printf("Failed to find the %s\n", device_name);
ret = -ENODEV;
goto error_ret;
}
printf("iio device number being used is %d\n", dev_num);
asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num);
printf("allco=%x\n", (int)dev_dir_name);
if (trigger_name == NULL) {
/*
* Build the trigger name. If it is device associated it's
* name is <device_name>_dev[n] where n matches the device
* number found above
*/
ret = asprintf(&trigger_name,
"%s-dev%d", device_name, dev_num);
if (ret < 0) {
ret = -ENOMEM;
goto error_ret;
}
}
/* Verify the trigger exists */
trig_num = find_type_by_name(trigger_name, "trigger");
if (trig_num < 0) {
printf("Failed to find the trigger %s\n", trigger_name);
ret = -ENODEV;
goto error_free_triggername;
}
printf("iio trigger number being used is %d\n", trig_num);
ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num);
if (ret < 0) {
ret = -ENOMEM;
goto error_free_triggername;
}
enable_enable_main(0);
ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1);
/*
* Parse the files in scan_elements to identify what channels are
* present
*/
ret = 0;
setup_dmp(dev_dir_name);
printf("%s %s\n", dev_dir_name, trigger_name);
/* Set the device trigger to be the data rdy trigger found above */
ret = write_sysfs_string_and_verify("trigger/current_trigger",
dev_dir_name,
trigger_name);
if (ret < 0) {
printf("Failed to write current_trigger file\n");
goto error_free_buf_dir_name;
}
/* Setup ring buffer parameters */
/* length must be even number because iio_store_to_sw_ring is expecting
half pointer to be equal to the read pointer, which is impossible
when buflen is odd number. This is actually a bug in the code */
ret = write_sysfs_int("length", buf_dir_name, buf_len*2);
if (ret < 0)
goto exit_here;
enable_enable_main(1);
inv_create_thread();
exit_here:
error_free_buf_dir_name:
free(buf_dir_name);
error_free_triggername:
if (datardytrigger)
free(trigger_name);
error_ret:
return ret;
}
void CompassSensor::enable_iio_sysfs()
{
VFUNC_LOG;
int tempFd = 0;
char iio_device_node[MAX_CHIP_ID_LEN];
FILE *tempFp = NULL;
const char* compass = dev_full_name;
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1, compassSysFs.in_timestamp_en, getTimestamp());
write_sysfs_int(compassSysFs.in_timestamp_en, 1);
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
IIO_BUFFER_LENGTH, compassSysFs.buffer_length, getTimestamp());
tempFp = fopen(compassSysFs.buffer_length, "w");
if (tempFp == NULL) {
LOGE("HAL:could not open buffer length");
} else {
if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0 || fclose(tempFp) < 0) {
LOGE("HAL:could not write buffer length");
}
}
sprintf(iio_device_node, "%s%d", "/dev/iio:device",
find_type_by_name(compass, "iio:device"));
compass_fd = open(iio_device_node, O_RDONLY);
int res = errno;
if (compass_fd < 0) {
LOGE("HAL:could not open '%s' iio device node in path '%s' - "
"error '%s' (%d)",
compass, iio_device_node, strerror(res), res);
} else {
LOGV_IF(EXTRA_VERBOSE,
"HAL:iio %s, compass_fd opened : %d", compass, compass_fd);
}
/* TODO: need further tests for optimization to reduce context-switch
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
compassSysFs.compass_x_fifo_enable, getTimestamp());
tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR);
res = errno;
if (tempFd > 0) {
res = enable_sysfs_sensor(tempFd, 1);
} else {
LOGE("HAL:open of %s failed with '%s' (%d)",
compassSysFs.compass_x_fifo_enable, strerror(res), res);
}
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
compassSysFs.compass_y_fifo_enable, getTimestamp());
tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR);
res = errno;
if (tempFd > 0) {
res = enable_sysfs_sensor(tempFd, 1);
} else {
LOGE("HAL:open of %s failed with '%s' (%d)",
compassSysFs.compass_y_fifo_enable, strerror(res), res);
}
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
compassSysFs.compass_z_fifo_enable, getTimestamp());
tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR);
res = errno;
if (tempFd > 0) {
res = enable_sysfs_sensor(tempFd, 1);
} else {
LOGE("HAL:open of %s failed with '%s' (%d)",
compassSysFs.compass_z_fifo_enable, strerror(res), res);
}
*/
}
/**
* Start or stop the ADC readings
* @param[in] *adc The ADC to start the readings for
* @param[in] value 1 to enable and 0 to disable
*/
void adc_enable(struct adc_t *adc, uint8_t value)
{
/* Write 1 or 0 to enable/disable the ADC */
write_sysfs_int(adc->dev_id, "buffer/enable", value);
}
int main(int argc, char **argv)
{
int ret;
int i, j, k, toread;
FILE *fp_ev;
int fp;
char *trigger_name, *dev_dir_name, *buf_dir_name;
char *data;
size_t read_size;
struct iio_event_data dat;
int dev_num, trig_num;
char *buffer_access, *buffer_event;
const char *iio_dir = "/sys/bus/iio/devices/";
int scan_size;
float gain = 1;
/* Find out which iio device is the accelerometer. */
dev_num = find_type_by_name(device_name, "device");
if (dev_num < 0) {
printf("Failed to find the %s\n", device_name);
ret = -ENODEV;
goto error_ret;
}
printf("iio device number being used is %d\n", dev_num);
asprintf(&dev_dir_name, "%sdevice%d", iio_dir, dev_num);
/*
* Build the trigger name.
* In this case we want the lis3l02dq's data ready trigger
* for this lis3l02dq. The naming is lis3l02dq_dev[n], where
* n matches the device number found above.
*/
ret = asprintf(&trigger_name, "%s%d", trigger_name_base, dev_num);
if (ret < 0) {
ret = -ENOMEM;
goto error_free_dev_dir_name;
}
/*
* Find the trigger by name.
* This is techically unecessary here as we only need to
* refer to the trigger by name and that name is already
* known.
*/
trig_num = find_type_by_name(trigger_name, "trigger");
if (trig_num < 0) {
printf("Failed to find the %s\n", trigger_name);
ret = -ENODEV;
goto error_free_triggername;
}
printf("iio trigger number being used is %d\n", trig_num);
/*
* Read in the scale value - in a more generic case, first
* check for accel_scale, then the indivual channel scales
*/
ret = read_sysfs_float("accel_scale", dev_dir_name, &gain);
if (ret)
goto error_free_triggername;;
/*
* Construct the directory name for the associated buffer.
* As we know that the lis3l02dq has only one buffer this may
* be built rather than found.
*/
ret = asprintf(&buf_dir_name, "%sdevice%d:buffer0", iio_dir, dev_num);
if (ret < 0) {
ret = -ENOMEM;
goto error_free_triggername;
}
/* Set the device trigger to be the data rdy trigger found above */
ret = write_sysfs_string_and_verify("trigger/current_trigger",
dev_dir_name,
trigger_name);
if (ret < 0) {
printf("Failed to write current_trigger file\n");
goto error_free_buf_dir_name;
}
/* Setup ring buffer parameters */
ret = write_sysfs_int("length", buf_dir_name, buf_len);
if (ret < 0)
goto error_free_buf_dir_name;
/* Enable the buffer */
ret = write_sysfs_int("ring_enable", buf_dir_name, 1);
if (ret < 0)
goto error_free_buf_dir_name;
data = malloc(size_from_scanmode(num_vals, scan_ts)*buf_len);
if (!data) {
ret = -ENOMEM;
goto error_free_buf_dir_name;
}
ret = asprintf(&buffer_access,
"/dev/device%d:buffer0:access0",
dev_num);
if (ret < 0) {
ret = -ENOMEM;
goto error_free_data;
}
ret = asprintf(&buffer_event, "/dev/device%d:buffer0:event0", dev_num);
if (ret < 0) {
ret = -ENOMEM;
goto error_free_data;
}
/* Attempt to open non blocking the access dev */
fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
if (fp == -1) { /*If it isn't there make the node */
printf("Failed to open %s\n", buffer_access);
ret = -errno;
goto error_free_buffer_event;
}
/* Attempt to open the event access dev (blocking this time) */
fp_ev = fopen(buffer_event, "rb");
if (fp_ev == NULL) {
printf("Failed to open %s\n", buffer_event);
ret = -errno;
goto error_close_buffer_access;
}
/* Wait for events 10 times */
for (j = 0; j < num_loops; j++) {
read_size = fread(&dat, 1, sizeof(struct iio_event_data),
fp_ev);
switch (dat.id) {
case IIO_EVENT_CODE_RING_100_FULL:
toread = buf_len;
break;
case IIO_EVENT_CODE_RING_75_FULL:
toread = buf_len*3/4;
break;
case IIO_EVENT_CODE_RING_50_FULL:
toread = buf_len/2;
break;
default:
printf("Unexpecteded event code\n");
continue;
}
read_size = read(fp,
data,
toread*size_from_scanmode(num_vals, scan_ts));
if (read_size == -EAGAIN) {
printf("nothing available\n");
continue;
}
scan_size = size_from_scanmode(num_vals, scan_ts);
for (i = 0; i < read_size/scan_size; i++) {
for (k = 0; k < num_vals; k++) {
__s16 val = *(__s16 *)(&data[i*scan_size
+ (k)*2]);
printf("%05f ", (float)val*gain);
}
printf(" %lld\n",
*(__s64 *)(&data[(i + 1)
*size_from_scanmode(num_vals,
scan_ts)
- sizeof(__s64)]));
}
}
/* Stop the ring buffer */
ret = write_sysfs_int("ring_enable", buf_dir_name, 0);
if (ret < 0)
goto error_close_buffer_event;
/* Disconnect from the trigger - just write a dummy name.*/
write_sysfs_string("trigger/current_trigger",
dev_dir_name, "NULL");
error_close_buffer_event:
fclose(fp_ev);
error_close_buffer_access:
close(fp);
error_free_data:
free(data);
error_free_buffer_access:
free(buffer_access);
error_free_buffer_event:
free(buffer_event);
error_free_buf_dir_name:
free(buf_dir_name);
error_free_triggername:
free(trigger_name);
error_free_dev_dir_name:
free(dev_dir_name);
error_ret:
return ret;
}
