Skip to content

Commit

Permalink
timestamp resolution
Browse files Browse the repository at this point in the history 
* bugfix in datasync
* change resolution of timestamp to nanosecs

Co-Authored-By: Yong Shen <[email protected]>
  • Loading branch information
@christianlorenz3 @yongbosch
christianlorenz3 and yongbosch committed Nov 22, 2021
1 parent 9c592b2 commit 26f39f9
Show file tree
Hide file tree
Showing 3 changed files with 46 additions and 5 deletions.
2 changes: 1 addition & 1 deletion smi230/hal/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ int main()
}

for (i = 0; i < sensorsNum; ++i) {
dev->device.batch((sensors_poll_device_1 *)dev, sSensorList[i].handle, 0, 5000000, 0);
dev->device.batch((sensors_poll_device_1 *)dev, sSensorList[i].handle, 0, 10000000, 0);
dev->device.activate((sensors_poll_device_t *)dev, sSensorList[i].handle, 1);
}

Expand Down
2 changes: 2 additions & 0 deletions smi230/sensord/inc/sensord_cfg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,8 @@ extern int trace_to_logcat;
extern long long unsigned int sensors_mask;

//#define SMI230_DATA_SYNC
//#define SMI230_NEW_DATA
#define SMI230_FIFO

#define SOLUTION_MDOF 0
#define SOLUTION_ECOMPASS 1
Expand Down
47 changes: 43 additions & 4 deletions smi230/sensord/sensord_hwcntl_implement.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -767,7 +767,7 @@ static void ap_config_phyACC(bsx_f32_t sample_rate)

ret = wr_sysfs_oneint("pwr_cfg", acc_input_dir_name, SENSOR_PM_SUSPEND);
#ifdef SMI230_DATA_SYNC
ret = wr_sysfs_oneint("pwr_cfg", gyro_input_dir_name, SENSOR_PM_SUSPEND);
ret = wr_sysfs_oneint("pwr_cfg", gyr_input_dir_name, SENSOR_PM_SUSPEND);
#endif

is_acc_open = 0;
Expand All @@ -781,7 +781,7 @@ static void ap_config_phyACC(bsx_f32_t sample_rate)
PDEBUG("set acc active");
ret = wr_sysfs_oneint("pwr_cfg", acc_input_dir_name, SENSOR_PM_NORMAL);
#ifdef SMI230_DATA_SYNC
ret = wr_sysfs_oneint("pwr_cfg", gyro_input_dir_name, SENSOR_PM_NORMAL);
ret = wr_sysfs_oneint("pwr_cfg", gyr_input_dir_name, SENSOR_PM_NORMAL);
#endif
is_acc_open = 1;

Expand Down Expand Up @@ -1407,17 +1407,29 @@ static void ap_hw_poll_smi230acc(BoschSimpleList *dest_list_acc, BoschSimpleList
static void ap_hw_poll_smi230acc(BoschSimpleList *dest_list_acc)
{
int32_t ret;
#ifdef SMI230_NEW_DATA
struct input_event event[6];
#else
struct input_event event[4];
#endif
HW_DATA_UNION *p_hwdata;

while( (ret = read(acc_input_fd, event, sizeof(event))) > 0)
{
#ifdef SMI230_NEW_DATA
if(EV_SYN != event[5].type)
#else
if(EV_SYN != event[3].type)
#endif
{
PWARN("0: %d, %d, %d;", event[0].type, event[0].code, event[0].value);
PWARN("1: %d, %d, %d;", event[1].type, event[1].code, event[1].value);
PWARN("2: %d, %d, %d;", event[2].type, event[2].code, event[2].value);
PWARN("3: %d, %d, %d;", event[3].type, event[3].code, event[3].value);
#ifdef SMI230_NEW_DATA
PWARN("4: %d, %d, %d;", event[4].type, event[4].code, event[4].value);
PWARN("5: %d, %d, %d;", event[5].type, event[5].code, event[5].value);
#endif
continue;
}

Expand All @@ -1429,11 +1441,18 @@ static void ap_hw_poll_smi230acc(BoschSimpleList *dest_list_acc)
}

p_hwdata->id = SENSOR_TYPE_ACCELEROMETER;
#ifdef SMI230_NEW_DATA
p_hwdata->x = event[2].value;
p_hwdata->y = event[3].value;
p_hwdata->z = event[4].value;
p_hwdata->timestamp = event[0].value * 1000000000LL + event[1].value;
#else
p_hwdata->x = event[0].value;
p_hwdata->y = event[1].value;
p_hwdata->z = event[2].value;
//use sync event timestamp for all data
p_hwdata->timestamp = event[3].time.tv_sec * 1000000LL + event[3].time.tv_usec;
p_hwdata->timestamp = event[3].time.tv_sec * 1000000LL + event[3].time.tv_usec;
#endif

ret = dest_list_acc->list_add_rear((void *) p_hwdata);
if (ret)
Expand All @@ -1453,17 +1472,29 @@ static void ap_hw_poll_smi230acc(BoschSimpleList *dest_list_acc)
static void ap_hw_poll_smi230gyro(BoschSimpleList *dest_list)
{
int32_t ret;
#ifdef SMI230_NEW_DATA
struct input_event event[6];
#else
struct input_event event[4];
#endif
HW_DATA_UNION *p_hwdata;

while( (ret = read(gyr_input_fd, event, sizeof(event))) > 0)
{
#ifdef SMI230_NEW_DATA
if(EV_SYN != event[5].type)
#else
if(EV_SYN != event[3].type)
#endif
{
PWARN("0: %d, %d, %d;", event[0].type, event[0].code, event[0].value);
PWARN("1: %d, %d, %d;", event[1].type, event[1].code, event[1].value);
PWARN("2: %d, %d, %d;", event[2].type, event[2].code, event[2].value);
PWARN("3: %d, %d, %d;", event[3].type, event[3].code, event[3].value);
#ifdef SMI230_NEW_DATA
PWARN("4: %d, %d, %d;", event[4].type, event[4].code, event[4].value);
PWARN("5: %d, %d, %d;", event[5].type, event[5].code, event[5].value);
#endif
continue;
}

Expand All @@ -1475,10 +1506,18 @@ static void ap_hw_poll_smi230gyro(BoschSimpleList *dest_list)
}

p_hwdata->id = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
#ifdef SMI230_NEW_DATA
p_hwdata->x_uncalib = event[2].value;
p_hwdata->y_uncalib = event[3].value;
p_hwdata->z_uncalib = event[4].value;
p_hwdata->timestamp = event[0].value * 1000000000LL + event[1].value;
#else
p_hwdata->x_uncalib = event[0].value;
p_hwdata->y_uncalib = event[1].value;
p_hwdata->z_uncalib = event[2].value;
p_hwdata->timestamp = event[3].time.tv_sec * 1000000LL + event[3].time.tv_usec;
//use sync event timestamp for all data
p_hwdata->timestamp = event[3].time.tv_sec * 1000000LL + event[3].time.tv_usec;
#endif

hw_remap_sensor_data(&(p_hwdata->x_uncalib), &(p_hwdata->y_uncalib), &(p_hwdata->z_uncalib), g_place_g);

Expand Down

0 comments on commit 26f39f9

Please sign in to comment.