#include <SPI.h>
#define CS 10
byte lowByte, highByte;
int measurements;
double gyroscope_value_roll, gyroscope_sum_measurements_roll, gyroscope_average_roll;
double gyroscope_value_pitch, gyroscope_sum_measurements_pitch, gyroscope_average_pitch;
double gyroscope_value_yaw, gyroscope_sum_measurements_yaw, gyroscope_average_yaw;
void setup() {
Serial.begin(9600);
while(!Serial);
SPI.begin();
SPI.beginTransaction(SPISettings(10000000, MSBFIRST, SPI_MODE3));
pinMode(CS, OUTPUT);
digitalWrite(CS, LOW);
SPI.transfer(0x20 & 0x7F);
SPI.transfer(0x0F);
digitalWrite(CS, HIGH);
digitalWrite(CS, LOW);
SPI.transfer(0x22 & 0x7F);
SPI.transfer(0x08);
digitalWrite(CS, HIGH);
digitalWrite(CS, LOW);
SPI.transfer(0x23 & 0x7F);
SPI.transfer(0x90);
digitalWrite(CS, HIGH);
Serial.print("Starting calibration...");
for (measurements = 0; measurements < 5000; measurements ++) {
read_gyroscope();
gyroscope_sum_measurements_roll += gyroscope_value_roll;
gyroscope_sum_measurements_pitch += gyroscope_value_pitch;
gyroscope_sum_measurements_yaw += gyroscope_value_yaw;
if(measurements%100 == 0)Serial.print(".");
delay(4);
}
gyroscope_average_roll = gyroscope_sum_measurements_roll / 5000;
gyroscope_average_pitch = gyroscope_sum_measurements_pitch / 5000;
gyroscope_average_yaw = gyroscope_sum_measurements_yaw / 5000;
}
void loop() {
read_gyroscope();
print_output();
delay(250);
}
void print_output()
{
Serial.print("Pitch:");
if(gyroscope_value_pitch >= 0)Serial.print("+");
Serial.print(gyroscope_value_pitch/57.14286); //Convert to degree per second
if(gyroscope_value_pitch/57.14286 - 2 > 0)Serial.print(" NoU");
else if(gyroscope_value_pitch/57.14286 + 2 < 0)Serial.print(" NoD");
else Serial.print(" ---");
Serial.print(" Roll:");
if(gyroscope_value_roll >= 0)Serial.print("+");
Serial.print(gyroscope_value_roll/57.14286); //Convert to degree per second
if(gyroscope_value_roll/57.14286 - 2 > 0)Serial.print(" RwD");
else if(gyroscope_value_roll/57.14286 + 2 < 0)Serial.print(" RwU");
else Serial.print(" ---");
Serial.print(" Yaw:");
if(gyroscope_value_yaw >= 0)Serial.print("+");
Serial.print(gyroscope_value_yaw/57.14286); //Convert to degree per second
if(gyroscope_value_yaw/57.14286 - 2 > 0)Serial.println(" NoR");
else if(gyroscope_value_yaw/57.14286 + 2 < 0)Serial.println(" NoL");
else Serial.println(" ---");
}
void read_gyroscope() {
digitalWrite(CS, LOW);
SPI.transfer(0x28 | 0x80);
lowByte = SPI.transfer(0x00);
digitalWrite(CS, HIGH);
digitalWrite(CS, LOW);
SPI.transfer(0x29 | 0x80);
highByte = SPI.transfer(0x00);
digitalWrite(CS, HIGH);
gyroscope_value_roll = (int16_t)((highByte << 8) | lowByte);
if (measurements == 5000)gyroscope_value_roll -= gyroscope_average_roll;
digitalWrite(CS, LOW);
SPI.transfer(0x2A | 0x80);
lowByte = SPI.transfer(0x00);
digitalWrite(CS, HIGH);
digitalWrite(CS, LOW);
SPI.transfer(0x2B | 0x80);
highByte = SPI.transfer(0x00);
digitalWrite(CS, HIGH);
gyroscope_value_pitch = (int16_t)((highByte << 8) | lowByte);
gyroscope_value_pitch *= -1;
if (measurements == 5000)gyroscope_value_pitch -= gyroscope_average_pitch;
digitalWrite(CS, LOW);
SPI.transfer(0x2C | 0x80);
lowByte = SPI.transfer(0x00);
digitalWrite(CS, HIGH);
digitalWrite(CS, LOW);
SPI.transfer(0x2D | 0x80);
highByte = SPI.transfer(0x00);
digitalWrite(CS, HIGH);
gyroscope_value_yaw = (int16_t)((highByte << 8) | lowByte);
gyroscope_value_yaw *= -1;
if (measurements == 5000)gyroscope_value_yaw -= gyroscope_average_yaw;
}