I'm using an accelerometer and magnetometer to take a tilt compensated heading and write it to a micro SD card. I originally tried with a delay of 100 and when the script continued to crash randomly I slowed it down to a delay of 250ms. I am still having the same issue. I am running the system on a Mini Pro 3.3v 8Mhz ATmega328 using an ADXL345 accelerometer and a HMC5883L Magnetometer connected via TWI. The SD card reader/writer is a sparkfun 3.3v version connected via SPI. Here is the code
// Reference the I2C Library
#include <Wire.h>
// Reference the HMC5883L Compass Library
#include <HMC5883L.h>
// Reference the ADXL345 Accelerometer Library
#include <ADXL345.h>
// Reference the SD Card Library
#include <SD.h>
// Store our compass as a variable.
HMC5883L compass;
// Store our accelerometer as a variable.
ADXL345 accel;
// Set up a pin we are going to use to indicate our status using an LED.
int runningPin = 2; // I'm using digital pin 2.
int failurePin = 3;
//Create Error variables
int areConnected = 0; // Store our connection status here.
int recordCount = 0;
//Set up the SD card CS on digital pin 10 and open the data file
const int chipSelect = 10;
File dataFile;
void setup()
{
Serial.begin(9600); // Initialize the serial port.
Wire.begin(); // Start the I2C interface.
pinMode(runningPin, OUTPUT); // Ready an LED to indicate our status.
pinMode(failurePin, OUTPUT);
compass = HMC5883L(); // Construct a new HMC5883 compass.
accel = ADXL345(); // Construct a new ADXL345 accelerometer.
compass.EnsureConnected();
accel.EnsureConnected();
if(compass.IsConnected && accel.IsConnected)
{
areConnected = true;
Serial.println("Connected to HMC5883L and ADXL345.");
digitalWrite(runningPin, HIGH);
digitalWrite(failurePin, LOW);
}
else
{
areConnected = false;
digitalWrite(runningPin, LOW);
digitalWrite(failurePin, HIGH);
if(compass.IsConnected == 0)
Serial.println("Could not connect to HMC5883L.");
if(accel.IsConnected == 0)
Serial.println("Could not connect to ADXL345.");
}
if(areConnected)
{
compass.SetScale(1.3); // Set the scale of the compass.
compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
accel.SetRange(2, true); // Set the range of the accelerometer to a maximum of 2G.
accel.EnableMeasurements(); // Tell the accelerometer to start taking measurements.
}
Serial.print("Initializing SD card...");
// **SD** make sure that the default chip select pin is set to output, even if you don't use it:
pinMode(SS, OUTPUT);
// **SD** see if the card is present and can be initialized:
if (!SD.begin(chipSelect)) {
Serial.println("Card failed, or not present");
// don't do anything more:
digitalWrite(runningPin, LOW);
digitalWrite(failurePin, HIGH);
while (1);
}
Serial.println("card initialized.");
// **SD** Open up the file we're going to log to!
dataFile = SD.open("datalog.txt", FILE_WRITE);
if (! dataFile) {
Serial.println("error opening datalog.txt");
digitalWrite(runningPin, LOW);
digitalWrite(failurePin, HIGH);
// Wait forever since we cant write data
while (1) ;
}
digitalWrite(runningPin, HIGH);
}
void loop()
{
if(areConnected)
{
MagnetometerScaled magnetometerReadings = compass.ReadScaledAxis();
AccelerometerScaled accelerometerReadings = accel.ReadScaledAxis();
float headingNTC = CalculateHeadingNotTiltCompensated(magnetometerReadings);
float headingTC = CalculateHeadingTiltCompensated(magnetometerReadings, accelerometerReadings);
// Output the data via the serial port.
Output(headingNTC, headingTC);
dataFile.flush();
delay(250);
}
}
float CalculateHeadingTiltCompensated(MagnetometerScaled mag, AccelerometerScaled acc)
{
// We are swapping the accelerometers axis as they are opposite to the compass the way we have them mounted.
// We are swapping the signs axis as they are opposite.
// Configure this for your setup.
float accX = -acc.XAxis;
float accY = -acc.YAxis;
float rollRadians = asin(accY);
float pitchRadians = asin(accX);
// We cannot correct for tilt over 40 degrees with this algorthem, if the board is tilted as such, return 0.
if(rollRadians > 0.78 || rollRadians < -0.78 || pitchRadians > 0.78 || pitchRadians < -0.78)
{
return 0;
}
// Some of these are used twice, so rather than computing them twice in the algorithem we precompute them before hand.
float cosRoll = cos(rollRadians);
float sinRoll = sin(rollRadians);
float cosPitch = cos(pitchRadians);
float sinPitch = sin(pitchRadians);
// The tilt compensation algorithem.
float Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;
float Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;
float heading = atan2(Yh, Xh);
return heading;
}
float CalculateHeadingNotTiltCompensated(MagnetometerScaled mag)
{
// Calculate heading when the magnetometer is level, then correct for signs of axis.
float heading = atan2(mag.YAxis, mag.XAxis);
return heading;
}
float RadiansToDegrees(float rads)
{
// Correct for when signs are reversed.
if(rads < 0)
rads += 2*PI;
// Check for wrap due to addition of declination.
if(rads > 2*PI)
rads -= 2*PI;
// Convert radians to degrees for readability.
float heading = rads * 180/PI;
return heading;
}
void Output(float headingNTC, float headingTC)
{
/*
if (recordCount <= 50) {
recordCount ++;
digitalWrite(runningPin, HIGH);
} else {
recordCount ++;
digitalWrite(runningPin, LOW);
}
*/
if (SD.exists("datalog.txt")) {
dataFile.print(RadiansToDegrees(headingNTC));
dataFile.print(", ");
dataFile.println(RadiansToDegrees(headingTC));
Serial.print(recordCount);
Serial.print(", ");
Serial.print("Heading: ");
Serial.print(RadiansToDegrees(headingNTC));
Serial.print(" Heading (Tilt Compensated): ");
Serial.println(RadiansToDegrees(headingTC));
} else {
Serial.print("Error recording at ");
Serial.println(recordCount);
digitalWrite(runningPin, LOW);
digitalWrite(failurePin, HIGH);
while(1);
}
}