Boot Linux faster!

Check our new training course

Boot Linux faster!

Check our new training course
and Creative Commons CC-BY-SA
lecture and lab materials

Bootlin logo

Elixir Cross Referencer

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
/*
 * Copyright (c) 2016 Intel Corporation
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr.h>
#include <stdio.h>
#include <device.h>
#include <sensor.h>
#include <misc/printk.h>

#define MAX_TEST_TIME	15000
#define SLEEPTIME	300

/* uncomment next line for setting offsets manually */
/* #define PERFORM_MANUAL_CALIBRATION */

/* uncomment the next line for auto calibration */
/* #define PERFORM_AUTO_CALIBRATION */

#ifdef PERFORM_MANUAL_CALIBRATION
/*
 * Offset map needed for manual accelerometer calibration. These values are
 * deduced by holding the device perpendicular on one axis (usually that's Z if
 * the device lies flat on the table) and compute the difference so that the
 * values on the 3 axis look like this: X: 0, Y: 0; Z: 9.80665. Due to
 * accelerometer noise, the values will vary around these values.
 *
 * For example if the accelerometer output, without offset compensation, is :
 *
 *   Acc (m/s^2): X=-2.349435, Y=-0.488070, Z=11.158620
 *
 * then the offsets necessary to compensate the read values are:
 *   X = +2.349435, Y = +0.488070. Z = -1.351970
 */
static struct sensor_value accel_offsets[] = {
	{2, 349435},   /* X */
	{0, 488070},   /* Y */
	{-1, -351970}, /* Z */
};

/*
 * The same goes for gyro offsets but, in gyro's case, the values should
 * converge to 0 (with the device standing still).
 */
static struct sensor_value gyro_offsets[] = {
	{0, 3195}, /* X */
	{0, 3195}, /* Y */
	{0, -4260},/* Z */
};

static int manual_calibration(struct device *bmi160)
{
#if !defined(CONFIG_BMI160_ACCEL_PMU_SUSPEND)
	/* set accelerometer offsets */
	if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
			    SENSOR_ATTR_OFFSET, accel_offsets) < 0) {
		return -EIO;
	}
#endif

#if !defined(CONFIG_BMI160_GYRO_PMU_SUSPEND)
	/* set gyroscope offsets */
	if (sensor_attr_set(bmi160, SENSOR_CHAN_GYRO_XYZ,
			    SENSOR_ATTR_OFFSET, gyro_offsets) < 0) {
		return -EIO;
	}
#endif

	return 0;
}
#endif

#ifdef PERFORM_AUTO_CALIBRATION
/*
 * The values in the following map are the expected values that the
 * accelerometer needs to converge to if the device lies flat on the table. The
 * device has to stay still for about 500ms = 250ms(accel) + 250ms(gyro).
 */
struct sensor_value acc_calib[] = {
	{0, 0},      /* X */
	{0, 0},      /* Y */
	{9, 806650}, /* Z */
};
static int auto_calibration(struct device *bmi160)
{
#if !defined(CONFIG_BMI160_ACCEL_PMU_SUSPEND)
	/* calibrate accelerometer */
	if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
			      SENSOR_ATTR_CALIB_TARGET, acc_calib) < 0) {
		return -EIO;
	}
#endif

#if !defined(CONFIG_BMI160_GYRO_PMU_SUSPEND)
	/*
	 * Calibrate gyro. No calibration value needs to be passed to BMI160 as
	 * the target on all axis is set internally to 0. This is used just to
	 * trigger a gyro calibration.
	 */
	if (sensor_attr_set(bmi160, SENSOR_CHAN_GYRO_XYZ,
			      SENSOR_ATTR_CALIB_TARGET, NULL) < 0) {
		return -EIO;
	}
#endif

	return 0;
}
#endif

/**
 * @brief Helper function for printing a sensor value to a buffer
 *
 * @param buf A pointer to the buffer to which the printing is done.
 * @param len Size of buffer in bytes.
 * @param val A pointer to a sensor_value struct holding the value
 *            to be printed.
 *
 * @return The number of characters printed to the buffer.
 */
static inline int sensor_value_snprintf(char *buf, size_t len,
					const struct sensor_value *val)
{
	s32_t val1, val2;

	if (val->val2 == 0) {
		return snprintf(buf, len, "%d", val->val1);
	}

	/* normalize value */
	if (val->val1 < 0 && val->val2 > 0) {
		val1 = val->val1 + 1;
		val2 = val->val2 - 1000000;
	} else {
		val1 = val->val1;
		val2 = val->val2;
	}

	/* print value to buffer */
	if (val1 > 0 || (val1 == 0 && val2 > 0)) {
		return snprintf(buf, len, "%d.%06d", val1, val2);
	} else if (val1 == 0 && val2 < 0) {
		return snprintf(buf, len, "-0.%06d", -val2);
	} else {
		return snprintf(buf, len, "%d.%06d", val1, -val2);
	}
}

#if !defined(CONFIG_BMI160_GYRO_PMU_SUSPEND)
static void print_gyro_data(struct device *bmi160)
{
	struct sensor_value val[3];
	char buf_x[18], buf_y[18], buf_z[18];

	if (sensor_channel_get(bmi160, SENSOR_CHAN_GYRO_XYZ, val) < 0) {
		printk("Cannot read bmi160 gyro channels.\n");
		return;
	}

	sensor_value_snprintf(buf_x, sizeof(buf_x), &val[0]);
	sensor_value_snprintf(buf_y, sizeof(buf_y), &val[1]);
	sensor_value_snprintf(buf_z, sizeof(buf_z), &val[2]);

	printk("Gyro (rad/s): X=%s, Y=%s, Z=%s\n", buf_x, buf_y, buf_z);
}
#endif

#if !defined(CONFIG_BMI160_ACCEL_PMU_SUSPEND)
static void print_accel_data(struct device *bmi160)
{
	struct sensor_value val[3];
	char buf_x[18], buf_y[18], buf_z[18];

	if (sensor_channel_get(bmi160,
			       SENSOR_CHAN_ACCEL_XYZ, val) < 0) {
		printk("Cannot read bmi160 accel channels.\n");
		return;
	}

	sensor_value_snprintf(buf_x, sizeof(buf_x), &val[0]);
	sensor_value_snprintf(buf_y, sizeof(buf_y), &val[1]);
	sensor_value_snprintf(buf_z, sizeof(buf_z), &val[2]);

	printk("Acc (m/s^2): X=%s, Y=%s, Z=%s\n", buf_x, buf_y, buf_z);
}
#endif

static void print_temp_data(struct device *bmi160)
{
	struct sensor_value val;
	char buf[18];

	if (sensor_channel_get(bmi160, SENSOR_CHAN_TEMP, &val) < 0) {
		printk("Temperature channel read error.\n");
		return;
	}

	sensor_value_snprintf(buf, sizeof(buf), &val);

	printk("Temperature (Celsius): %s\n", buf);
}

static void test_polling_mode(struct device *bmi160)
{
	s32_t remaining_test_time = MAX_TEST_TIME;
	struct sensor_value attr;

#if defined(CONFIG_BMI160_ACCEL_ODR_RUNTIME)
	/* set sampling frequency to 100Hz for accel */
	attr.val1 = 100;
	attr.val2 = 0;

	if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
			    SENSOR_ATTR_SAMPLING_FREQUENCY, &attr) < 0) {
		printk("Cannot set sampling frequency for accelerometer.\n");
		return;
	}
#endif

#if defined(CONFIG_BMI160_GYRO_ODR_RUNTIME)
	/* set sampling frequency to 3200Hz for gyro */
	attr.val1 = 3200;
	attr.val2 = 0;

	if (sensor_attr_set(bmi160, SENSOR_CHAN_GYRO_XYZ,
			    SENSOR_ATTR_SAMPLING_FREQUENCY, &attr) < 0) {
		printk("Cannot set sampling frequency for gyroscope.\n");
		return;
	}
#endif
	/* wait for the change to take effect */
	k_sleep(SLEEPTIME);

	/* poll the data and print it */
	do {
		if (sensor_sample_fetch(bmi160) < 0) {
			printk("Sample update error.\n");
			return;
		}

#if !defined(CONFIG_BMI160_GYRO_PMU_SUSPEND)
		print_gyro_data(bmi160);
#endif
#if !defined(CONFIG_BMI160_ACCEL_PMU_SUSPEND)
		print_accel_data(bmi160);
#endif
		print_temp_data(bmi160);

		/* wait a while */
		k_sleep(SLEEPTIME);

		remaining_test_time -= SLEEPTIME;
	} while (remaining_test_time > 0);
}

#ifdef CONFIG_BMI160_TRIGGER
static void trigger_hdlr(struct device *bmi160,
			 struct sensor_trigger *trigger)
{
	if (trigger->type != SENSOR_TRIG_DELTA &&
	    trigger->type != SENSOR_TRIG_DATA_READY) {
		return;
	}

	if (sensor_sample_fetch(bmi160) < 0) {
		printk("Sample update error.\n");
		return;
	}

#if !defined(CONFIG_BMI160_GYRO_PMU_SUSPEND)
	if (trigger->chan == SENSOR_CHAN_GYRO_XYZ) {
		print_gyro_data(bmi160);
	}
#endif

#if !defined(CONFIG_BMI160_ACCEL_PMU_SUSPEND)
	if (trigger->chan == SENSOR_CHAN_ACCEL_XYZ) {
		print_accel_data(bmi160);
	}
#endif
}

static void test_anymotion_trigger(struct device *bmi160)
{
	s32_t remaining_test_time = MAX_TEST_TIME;
	struct sensor_value attr;
	struct sensor_trigger trig;

	/* set up anymotion trigger */

	/*
	 * Set slope threshold to 0.1G (0.1 * 9.80665 = 4.903325 m/s^2).
	 * This depends on the chosen range. One cannot choose a threshold
	 * bigger than half the range. For example, for a 16G range, the
	 * threshold must not exceed 8G.
	 */
	attr.val1 = 0;
	attr.val2 = 980665;
	if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
			    SENSOR_ATTR_SLOPE_TH, &attr) < 0) {
		printk("Cannot set anymotion slope threshold.\n");
		return;
	}

	/*
	 * Set slope duration to 2 consecutive samples (after which the
	 * anymotion interrupt will trigger.
	 *
	 * Allowed values are from 1 to 4.
	 */
	attr.val1 = 2;
	attr.val2 = 0;
	if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
			    SENSOR_ATTR_SLOPE_DUR, &attr) < 0) {
		printk("Cannot set anymotion slope duration.\n");
		return;
	}

	/* enable anymotion trigger */
	trig.type = SENSOR_TRIG_DELTA;
	trig.chan = SENSOR_CHAN_ACCEL_XYZ;

	if (sensor_trigger_set(bmi160, &trig, trigger_hdlr) < 0) {
		printk("Cannot enable anymotion trigger.\n");
		return;
	}

	printk("Anymotion test: shake the device to get anymotion events.\n");
	do {
		/* wait a while */
		k_sleep(SLEEPTIME);
		remaining_test_time -= SLEEPTIME;

	} while (remaining_test_time > 0);

	printk("Anymotion test: finished, removing anymotion trigger...\n");

	if (sensor_trigger_set(bmi160, &trig, NULL) < 0) {
		printk("Cannot remove anymotion trigger.\n");
		return;
	}
}

static void test_data_ready_trigger(struct device *bmi160)
{
	s32_t remaining_test_time = MAX_TEST_TIME;
	struct sensor_trigger trig;

	/* enable data ready trigger */
	trig.type = SENSOR_TRIG_DATA_READY;
	trig.chan = SENSOR_CHAN_ACCEL_XYZ;

	if (sensor_trigger_set(bmi160, &trig, trigger_hdlr) < 0) {
		printk("Cannot enable data ready trigger.\n");
		return;
	}

	printk("Data ready test:\n");
	do {
		/* wait a while */
		k_sleep(SLEEPTIME);
		remaining_test_time -= SLEEPTIME;

	} while (remaining_test_time > 0);

	printk("Data ready test: finished, removing data ready trigger...\n");

	if (sensor_trigger_set(bmi160, &trig, NULL) < 0) {
		printk("Cannot remove data ready trigger.\n");
		return;
	}
}

static void test_trigger_mode(struct device *bmi160)
{
	struct sensor_value attr;

#if defined(CONFIG_BMI160_ACCEL_ODR_RUNTIME)
	/* set sampling frequency to 100Hz for accel */
	attr.val1 = 100;
	attr.val2 = 0;

	if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
			    SENSOR_ATTR_SAMPLING_FREQUENCY, &attr) < 0) {
		printk("Cannot set sampling frequency for accelerometer.\n");
		return;
	}
#endif

#if defined(CONFIG_BMI160_GYRO_ODR_RUNTIME)
	/* set sampling frequency to 100Hz for gyro */
	attr.val1 = 100;
	attr.val2 = 0;

	if (sensor_attr_set(bmi160, SENSOR_CHAN_GYRO_XYZ,
			    SENSOR_ATTR_SAMPLING_FREQUENCY, &attr) < 0) {
		printk("Cannot set sampling frequency for gyroscope.\n");
		return;
	}
#endif

	test_anymotion_trigger(bmi160);

	test_data_ready_trigger(bmi160);
}
#endif /* CONFIG_BMI160_TRIGGER */

extern u8_t pbuf[1024];
extern u8_t *pos;
void main(void)
{
	struct device *bmi160;
#if defined(CONFIG_BMI160_ACCEL_RANGE_RUNTIME) ||\
		defined(CONFIG_BMI160_GYRO_RANGE_RUNTIME)
	struct sensor_value attr;
#endif

	printk("IMU: Binding...\n");
	bmi160 = device_get_binding(CONFIG_BMI160_NAME);
	if (!bmi160) {
		printk("Gyro: Device not found.\n");
		return;
	}

#if defined(CONFIG_BMI160_ACCEL_RANGE_RUNTIME)
	/*
	 * Set accelerometer range to +/- 16G. Since the sensor API needs SI
	 * units, convert the range to m/s^2.
	 */
	sensor_g_to_ms2(16, &attr);

	if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
			    SENSOR_ATTR_FULL_SCALE, &attr) < 0) {
		printk("Cannot set accelerometer range.\n");
		return;
	}
#endif

#if defined(CONFIG_BMI160_GYRO_RANGE_RUNTIME)
	/*
	 * Set gyro range to +/- 250 degrees/s. Since the sensor API needs SI
	 * units, convert the range to rad/s.
	 */
	sensor_degrees_to_rad(250, &attr);

	if (sensor_attr_set(bmi160, SENSOR_CHAN_GYRO_XYZ,
			    SENSOR_ATTR_FULL_SCALE, &attr) < 0) {
		printk("Cannot set gyro range.\n");
		return;
	}
#endif

#ifdef PERFORM_MANUAL_CALIBRATION
	/* manually adjust accelerometer and gyro offsets */
	if (manual_calibration(bmi160) < 0) {
		printk("Manual calibration failed.\n");
		return;
	}
#endif

#ifdef PERFORM_AUTO_CALIBRATION
	/* auto calibrate accelerometer and gyro */
	if (auto_calibration(bmi160) < 0) {
		printk("HW calibration failed.\n");
		return;
	}
#endif

	printk("Testing the polling mode.\n");
	test_polling_mode(bmi160);
	printk("Testing the polling mode finished.\n");

#ifdef CONFIG_BMI160_TRIGGER
	printk("Testing the trigger mode.\n");
	test_trigger_mode(bmi160);
	printk("Testing the trigger mode finished.\n");
#endif
}