|
| 1 | +// Aya Helmy wrote this example for her 4B25 coursework. James Meech adapted it to run over sunflower and take input from the sigsrc command |
| 2 | +#include <stdio.h> |
| 3 | +#include <string.h> |
| 4 | +#include <stdlib.h> |
| 5 | +#include <limits.h> |
| 6 | +#include <stdbool.h> |
| 7 | +#include "sf-types.h" |
| 8 | +#include "tag.h" |
| 9 | +#include "devsim7708.h" |
| 10 | +#include "sh7708.h" |
| 11 | +#include "devscc.h" |
| 12 | +#include "devrtc.h" |
| 13 | +#include "devexcp.h" |
| 14 | +#include "devlog.h" |
| 15 | +#include "devloc.h" |
| 16 | +#include "devsensor.h" |
| 17 | +#include "misc.h" |
| 18 | +#include "print.h" |
| 19 | + |
| 20 | +#define BUFFER_SIZE 400 |
| 21 | +struct accel_axis |
| 22 | +{ |
| 23 | + // Hold data and info about single acceleration axis: |
| 24 | + |
| 25 | + float data[BUFFER_SIZE]; //array to store filtered data |
| 26 | + float max; //maximum data entry |
| 27 | + float min; //minimum data entry |
| 28 | + float p2p; //peak to peak value |
| 29 | + float thresh; //current value of dynamic threshold |
| 30 | + |
| 31 | + |
| 32 | +}; |
| 33 | + |
| 34 | +void findAxisProperties(struct accel_axis *axis) |
| 35 | +{ |
| 36 | + // Find peak-to-peak value and threshold for single-axis data |
| 37 | + |
| 38 | + float max = axis->data[0]; |
| 39 | + float min = axis->data[0]; |
| 40 | + float p2p; |
| 41 | + float thresh; |
| 42 | + |
| 43 | + for (int i=0; i<BUFFER_SIZE; i++){ |
| 44 | + if (axis->data[i] > max){ |
| 45 | + max = axis->data[i]; |
| 46 | + } |
| 47 | + if (axis->data[i] < min){ |
| 48 | + min = axis->data[i]; |
| 49 | + } |
| 50 | + } |
| 51 | + |
| 52 | + p2p = max-min; |
| 53 | + thresh = (max+min)/2.0; |
| 54 | + |
| 55 | + // Write values into struct |
| 56 | + axis->max = max; |
| 57 | + axis->min = min; |
| 58 | + axis->p2p = p2p; |
| 59 | + axis->thresh = thresh; |
| 60 | +} |
| 61 | + |
| 62 | +int chooseAxis(struct accel_axis *x, struct accel_axis *y, struct accel_axis *z, float calib) |
| 63 | +{ |
| 64 | + // Perform maximum activity axis selection |
| 65 | + |
| 66 | + float p2p[3]; |
| 67 | + float max_p2p = 0; |
| 68 | + int max_index; |
| 69 | + |
| 70 | + findAxisProperties(x); |
| 71 | + findAxisProperties(y); |
| 72 | + findAxisProperties(z); |
| 73 | + |
| 74 | + p2p[0] = x->p2p; |
| 75 | + p2p[1] = y->p2p; |
| 76 | + p2p[2] = z->p2p; |
| 77 | + |
| 78 | + // Find axis with greatest peak-to-peak (p2p) amplitude |
| 79 | + for (int i = 0; i < 3; i++){ |
| 80 | + if (p2p[i] > max_p2p){ |
| 81 | + max_p2p = p2p[i]; |
| 82 | + max_index = i; |
| 83 | + } |
| 84 | + } |
| 85 | + |
| 86 | + // If p2p value of chosen axis is above amplitude calibration value then return chosen axis |
| 87 | + if (max_p2p > calib) { |
| 88 | + return (max_index + 1); |
| 89 | + } |
| 90 | + |
| 91 | + // If p2p value is less than calibration value then we assume invalid vibrations |
| 92 | + // so return "invalid" 0 index - no steps are counted for this time window |
| 93 | + else { |
| 94 | + return 0; |
| 95 | + } |
| 96 | +} |
| 97 | + |
| 98 | +float detectSteps(struct accel_axis *chosen) |
| 99 | +{ |
| 100 | + // Finds where threshold is crossed in negative slope direction |
| 101 | + |
| 102 | + float steps = 0; |
| 103 | + float current, next; |
| 104 | + for(int i=0; i<BUFFER_SIZE-1;i++){ |
| 105 | + current = chosen->data[i]; |
| 106 | + next = chosen->data[i+1]; |
| 107 | + if((current > chosen->thresh) && (next < chosen->thresh)) |
| 108 | + { |
| 109 | + steps = steps + 1; |
| 110 | + } |
| 111 | + } |
| 112 | + return steps; |
| 113 | +} |
| 114 | + |
| 115 | +void readRawData(float *t_buffer, float *x_buffer, float *y_buffer, float *z_buffer) |
| 116 | +{ |
| 117 | + for(int j = 0; j < BUFFER_SIZE; j++) |
| 118 | + { |
| 119 | +// Insert delay of 2500 uSeconds to simulate 400 Hz sample rate |
| 120 | +xudelay(2500); |
| 121 | +// Read sensor readings from sigsrc 0 which is the x-axis accelerometer readings |
| 122 | + x_buffer[j] = devsignal_read(0); |
| 123 | +// Read sensor readings from sigsrc 1 which is the y-axis accelerometer readings |
| 124 | + y_buffer[j] = devsignal_read(1); |
| 125 | +// Read sensor readings from sigsrc 2 which is the z-axis accelerometer readings |
| 126 | + z_buffer[j] = devsignal_read(2); |
| 127 | +/* Please note that sigsrc simulates a real signal which changes in time. The value returned by devsignal_read() will be different at different simulation times. |
| 128 | + Here we use delay to wait for 1/f seconds where f is the 400 Hz sample frequency specified in the run.m file. |
| 129 | + If we don't wait for this amount of time and instead sample as quickly as we can we will see the same sensor value many times. |
| 130 | + If we set xudelay to a higher value we will start to miss some samples in the file as we are looking at the return value of devsignal_read() at a frequency lower than 400 Hz. |
| 131 | + */ |
| 132 | + } |
| 133 | +} |
| 134 | + |
| 135 | +void MovingAvgFilter(float input[], struct accel_axis *output) |
| 136 | +{ |
| 137 | + // Implement moving average low pass filtering |
| 138 | + // by using unweighted average of frames of 4 samples |
| 139 | + |
| 140 | + float sum = 0, mean; |
| 141 | + |
| 142 | + // calculate mean of input array |
| 143 | + for (int i=0; i<BUFFER_SIZE; i++) { |
| 144 | + sum += input[i]; |
| 145 | + } |
| 146 | + mean = sum/BUFFER_SIZE; |
| 147 | + |
| 148 | + for (int i=0; i<BUFFER_SIZE; i++) { |
| 149 | + // Find mean-subtracted input |
| 150 | + input[i] = input[i] - mean; |
| 151 | + // Initialise output to zero |
| 152 | + output->data[i] = 0; |
| 153 | + } |
| 154 | + |
| 155 | + for (int i=0; i<BUFFER_SIZE; i++) { |
| 156 | + output->data[i] = input[i]; |
| 157 | + } |
| 158 | +} |
| 159 | + |
| 160 | + |
| 161 | +int |
| 162 | +main(void) |
| 163 | +{ |
| 164 | + int chosen_axis; |
| 165 | + float steps; |
| 166 | + float steps_total = 0; |
| 167 | + float calib_max; // used in calibration stage |
| 168 | + bool use_butterworth; // if true use Butterworth filter, else use Moving Average filter |
| 169 | + |
| 170 | +// Declare buffers to hold raw unfiltered acceleration data |
| 171 | + float t_buffer[BUFFER_SIZE]; |
| 172 | + float x_buffer[BUFFER_SIZE]; |
| 173 | + float y_buffer[BUFFER_SIZE]; |
| 174 | + float z_buffer[BUFFER_SIZE]; |
| 175 | + |
| 176 | +// Declare struct for each acceleration axis (for use after filtering) |
| 177 | + struct accel_axis x_accel; |
| 178 | + struct accel_axis y_accel; |
| 179 | + struct accel_axis z_accel; |
| 180 | + |
| 181 | +// Select type of filtering |
| 182 | + use_butterworth = false; |
| 183 | + |
| 184 | +//empirically calculated minimum allowable calibration value |
| 185 | + // in units of 0.25 mg |
| 186 | + calib_max = 1; |
| 187 | + |
| 188 | +/* Stage 2: Step Detection */ |
| 189 | + |
| 190 | + // Read raw tri-axial accelerometer data and store in buffers |
| 191 | + readRawData(t_buffer, x_buffer, y_buffer, z_buffer); |
| 192 | + |
| 193 | +MovingAvgFilter(x_buffer, &x_accel); |
| 194 | + MovingAvgFilter(y_buffer, &y_accel); |
| 195 | + MovingAvgFilter(z_buffer, &z_accel); |
| 196 | + |
| 197 | + // Peform maximal activity axis selection |
| 198 | + chosen_axis = chooseAxis(&x_accel, &y_accel, &z_accel, calib_max); |
| 199 | + if(chosen_axis == 1){ |
| 200 | + steps = detectSteps(&x_accel); |
| 201 | + } |
| 202 | + |
| 203 | + else if(chosen_axis == 2){ |
| 204 | + steps = detectSteps(&y_accel); |
| 205 | + } |
| 206 | + |
| 207 | + else if(chosen_axis == 3){ |
| 208 | + steps = detectSteps(&z_accel); |
| 209 | + } |
| 210 | + |
| 211 | + else { |
| 212 | + steps = 0; |
| 213 | + } |
| 214 | + steps_total += steps; |
| 215 | + |
| 216 | + // Reset count to 0 if it reaches 100 |
| 217 | + if (steps_total > 99){ |
| 218 | + steps_total = 0; |
| 219 | + } |
| 220 | + printf("Total steps = %f\n", steps_total); |
| 221 | +return 0; |
| 222 | +} |
0 commit comments