-
Notifications
You must be signed in to change notification settings - Fork 0
/
samplebuffer.c
118 lines (111 loc) · 4.18 KB
/
samplebuffer.c
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
/**
* Written by Icewire Technologies
*/
#include "samplebuffer.h"
/*
* Set the sample's dt and loggers' axes to 0.
*
* sample: The sample to be cleared
*/
void clear_sample(volatile struct Sample *sample);
void construct_sample_buffer(struct SampleBuffer *sample_buffer, volatile struct Sample *samples, uint16_t size) {
sample_buffer->samples = samples;
sample_buffer->size = size;
sample_buffer->start = 0;
sample_buffer->end = 0;
sample_buffer->count = 0;
}
void clear_sample_buffer(struct SampleBuffer *sample_buffer) {
for (uint16_t i = 0; i < sample_buffer->size; ++i) {
volatile struct Sample *sample = &sample_buffer->samples[i];
clear_sample(sample);
}
sample_buffer->start = 0;
sample_buffer->end = 0;
sample_buffer->count = 0;
}
bool add_sample(struct SampleBuffer *sample_buffer, uint8_t delta_time_h, uint8_t delta_time_m, uint8_t delta_time_l,
uint8_t accel_x_axis_h, uint8_t accel_x_axis_l, uint8_t accel_y_axis_h, uint8_t accel_y_axis_l, uint8_t accel_z_axis_h, uint8_t accel_z_axis_l,
uint8_t gyro_x_axis_h, uint8_t gyro_x_axis_l, uint8_t gyro_y_axis_h, uint8_t gyro_y_axis_l, uint8_t gyro_z_axis_h, uint8_t gyro_z_axis_l) {
/* The buffer is full */
if (sample_buffer->count == sample_buffer->size) {
return false;
}
/* Set data for new sample based on parameters */
volatile struct Sample *sample = &sample_buffer->samples[sample_buffer->end];
/* Delta time */
sample->delta_time[0] = delta_time_h;
sample->delta_time[1] = delta_time_m;
sample->delta_time[2] = delta_time_l;
/* From accelerometer */
sample->accel.x_axis[0] = accel_x_axis_h;
sample->accel.x_axis[1] = accel_x_axis_l;
sample->accel.y_axis[0] = accel_y_axis_h;
sample->accel.y_axis[1] = accel_y_axis_l;
sample->accel.z_axis[0] = accel_z_axis_h;
sample->accel.z_axis[1] = accel_z_axis_l;
/* From gyroscope */
sample->gyro.x_axis[0] = gyro_x_axis_h;
sample->gyro.x_axis[1] = gyro_x_axis_l;
sample->gyro.y_axis[0] = gyro_y_axis_h;
sample->gyro.y_axis[1] = gyro_y_axis_l;
sample->gyro.z_axis[0] = gyro_z_axis_h;
sample->gyro.z_axis[1] = gyro_z_axis_l;
/* Increment the index for the next sample */
sample_buffer->end = (sample_buffer->end + 1) % sample_buffer->size;
++sample_buffer->count;
return true;
}
bool remove_sample(struct SampleBuffer *sample_buffer, struct Sample *sample_ret) {
/* The buffer is empty */
if (sample_buffer->count == 0) {
return false;
}
/*
* Store the sample in a new variable so the data won't be overwritten by a new sample when read
* instead of returning the existing sample in the buffer
*/
volatile struct Sample *sample = &sample_buffer->samples[sample_buffer->start];
/* Delta time */
sample_ret->delta_time[0] = sample->delta_time[0];
sample_ret->delta_time[1] = sample->delta_time[1];
sample_ret->delta_time[2] = sample->delta_time[2];
/* From accelerometer */
sample_ret->accel.x_axis[0] = sample->accel.x_axis[0];
sample_ret->accel.x_axis[1] = sample->accel.x_axis[1];
sample_ret->accel.y_axis[0] = sample->accel.y_axis[0];
sample_ret->accel.y_axis[1] = sample->accel.y_axis[1];
sample_ret->accel.z_axis[0] = sample->accel.z_axis[0];
sample_ret->accel.z_axis[1] = sample->accel.z_axis[1];
/* From gyroscope */
sample_ret->gyro.x_axis[0] = sample->gyro.x_axis[0];
sample_ret->gyro.x_axis[1] = sample->gyro.x_axis[1];
sample_ret->gyro.y_axis[0] = sample->gyro.y_axis[0];
sample_ret->gyro.y_axis[1] = sample->gyro.y_axis[1];
sample_ret->gyro.z_axis[0] = sample->gyro.z_axis[0];
sample_ret->gyro.z_axis[1] = sample->gyro.z_axis[1];
/* Set next sample to be removed as next sample in buffer */
sample_buffer->start = (sample_buffer->start + 1) % sample_buffer->size;
--sample_buffer->count;
return true;
}
void clear_sample(volatile struct Sample *sample) {
/* Delta time */
sample->delta_time[0] = 0;
sample->delta_time[1] = 0;
sample->delta_time[2] = 0;
/* From accelerometer */
sample->accel.x_axis[0] = 0;
sample->accel.x_axis[1] = 0;
sample->accel.y_axis[0] = 0;
sample->accel.y_axis[1] = 0;
sample->accel.z_axis[0] = 0;
sample->accel.z_axis[1] = 0;
/* From gyroscope */
sample->gyro.x_axis[0] = 0;
sample->gyro.x_axis[1] = 0;
sample->gyro.y_axis[0] = 0;
sample->gyro.y_axis[1] = 0;
sample->gyro.z_axis[0] = 0;
sample->gyro.z_axis[1] = 0;
}