static const unsigned int AVERAGING_BUFFER_LENGTH = 16;
static const int PRINT_LOOP_FREQUENCY = 40;
static const L3G::DPSMode DEGREES_PER_SECOND = L3G::DPS_250;
static const unsigned int PERIOD = SECOND / PRINT_LOOP_FREQUENCY;
static const Port::Mask SCLK = Port::P0;
static const Port::Mask MOSI = Port::P1;
static const Port::Mask MISO = Port::P2;
static const Port::Mask CS = Port::P4;
void read_average (const L3G &gyro, float *result);
void print_graph (const int markerIndex);
SPI spi = SPI::get_instance();
spi.set_mosi(MOSI);
spi.set_miso(MISO);
spi.set_sclk(SCLK);
spi.set_mode(L3G::SPI_MODE);
spi.set_bit_mode(L3G::SPI_BITMODE);
L3G gyro(spi, CS);
gyro.set_dps(DEGREES_PER_SECOND);
gyro.write(L3G::Register::CTRL_REG1, 0b11001111);
gyro.write(L3G::Register::CTRL_REG2, 6);
gyro.write(L3G::Register::CTRL_REG5, PropWare::BIT_6 | PropWare::BIT_4);
gyro.write(L3G::Register::FIFO_CTRL_REG, PropWare::BIT_6);
auto timer =
CNT + PERIOD;
while (1) {
float values[L3G::AXES];
read_average(gyro, values);
print_graph(static_cast<const int>(values[L3G::X]));
}
}
void read_average (const L3G &gyro, float *result) {
int16_t buffer[AVERAGING_BUFFER_LENGTH][L3G::AXES];
int32_t totals[] = {0, 0, 0};
for (unsigned int i = 0; i < AVERAGING_BUFFER_LENGTH; ++i)
gyro.read(buffer[i]);
for (unsigned int i = 0; i < AVERAGING_BUFFER_LENGTH; ++i)
for (unsigned int axis = 0; axis < L3G::AXES; ++axis)
totals[axis] += buffer[i][axis];
for (unsigned int axis = 0; axis < L3G::AXES; ++axis)
result[axis] = L3G::to_dps(totals[axis], DEGREES_PER_SECOND) / AVERAGING_BUFFER_LENGTH;
}
void print_graph (const int markerIndex) {
for (int i = -50; i <= 50; ++i) {
if (i == markerIndex)
else if (i < 0)
else if (i == 0)
else
}
}