jetfishteam / Mbed 2 deprecated sparkfun6dof

for interfacing the sparkfun boards

Dependencies:   ADXL345_I2C HMC5883L IMUfilter ITG3200_HelloWorld mbed

ADXL345HL.cpp@0:441caaf895d8, 2014-04-29 (annotated)

Committer:
sandwich
Date:
Tue Apr 29 00:04:48 2014 +0000
Revision:
0:441caaf895d8
imufilter works. 6dof

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sandwich 0:441caaf895d8 1 #include "ADXL345HL.h"
sandwich 0:441caaf895d8 2
sandwich 0:441caaf895d8 3 void ADXL345HL::init(int calibsamples, int readsampls, float samplrate)
sandwich 0:441caaf895d8 4 {
sandwich 0:441caaf895d8 5 accelerometer=new ADXL345_I2C(p28,p27);
sandwich 0:441caaf895d8 6 calibrationsamples=calibsamples;
sandwich 0:441caaf895d8 7 readsamples=readsampls;
sandwich 0:441caaf895d8 8 samplerate=samplrate;
sandwich 0:441caaf895d8 9 output=new double[3];
sandwich 0:441caaf895d8 10 //Go into standby mode to configure the device.
sandwich 0:441caaf895d8 11 accelerometer->setPowerControl(0x00);
sandwich 0:441caaf895d8 12 wait(0.001);
sandwich 0:441caaf895d8 13 //Full resolution, +/-16g, 4mg/LSB.
sandwich 0:441caaf895d8 14 accelerometer->setDataFormatControl(0x0B);
sandwich 0:441caaf895d8 15 wait(0.001);
sandwich 0:441caaf895d8 16 //200Hz data rate.
sandwich 0:441caaf895d8 17 accelerometer->setDataRate(ADXL345_200HZ);
sandwich 0:441caaf895d8 18 wait(0.001);
sandwich 0:441caaf895d8 19 //Measurement mode.
sandwich 0:441caaf895d8 20 accelerometer->setPowerControl(0x08);
sandwich 0:441caaf895d8 21 //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
sandwich 0:441caaf895d8 22 wait_ms(22);
sandwich 0:441caaf895d8 23 }
sandwich 0:441caaf895d8 24
sandwich 0:441caaf895d8 25 void ADXL345HL::calibrateAccelerometer(void)
sandwich 0:441caaf895d8 26 {
sandwich 0:441caaf895d8 27
sandwich 0:441caaf895d8 28 double a_xAccumulator = 0;
sandwich 0:441caaf895d8 29 double a_yAccumulator = 0;
sandwich 0:441caaf895d8 30 double a_zAccumulator = 0;
sandwich 0:441caaf895d8 31
sandwich 0:441caaf895d8 32 //Take a number of readings and average them
sandwich 0:441caaf895d8 33 //to calculate the zero g offset.
sandwich 0:441caaf895d8 34 for (int i = 0; i < calibrationsamples; i++) {
sandwich 0:441caaf895d8 35
sandwich 0:441caaf895d8 36 accelerometer->getOutput(readings);
sandwich 0:441caaf895d8 37
sandwich 0:441caaf895d8 38 a_xAccumulator += (int16_t) readings[0];
sandwich 0:441caaf895d8 39 a_yAccumulator += (int16_t) readings[1];
sandwich 0:441caaf895d8 40 a_zAccumulator += (int16_t) readings[2];
sandwich 0:441caaf895d8 41
sandwich 0:441caaf895d8 42 wait(samplerate);
sandwich 0:441caaf895d8 43
sandwich 0:441caaf895d8 44 }
sandwich 0:441caaf895d8 45
sandwich 0:441caaf895d8 46 a_xAccumulator /= calibrationsamples;
sandwich 0:441caaf895d8 47 a_yAccumulator /= calibrationsamples;
sandwich 0:441caaf895d8 48 a_zAccumulator /= calibrationsamples;
sandwich 0:441caaf895d8 49
sandwich 0:441caaf895d8 50 //At 4mg/LSB, 250 LSBs is 1g.
sandwich 0:441caaf895d8 51 xBias = a_xAccumulator;
sandwich 0:441caaf895d8 52 yBias = a_yAccumulator;
sandwich 0:441caaf895d8 53 zBias=a_zAccumulator;
sandwich 0:441caaf895d8 54 //zBias = (a_zAccumulator - 250);
sandwich 0:441caaf895d8 55
sandwich 0:441caaf895d8 56 a_xAccumulator = 0;
sandwich 0:441caaf895d8 57 a_yAccumulator = 0;
sandwich 0:441caaf895d8 58 a_zAccumulator = 0;
sandwich 0:441caaf895d8 59 }
sandwich 0:441caaf895d8 60
sandwich 0:441caaf895d8 61
sandwich 0:441caaf895d8 62 double* ADXL345HL::sampleAccelerometer(void)
sandwich 0:441caaf895d8 63 {
sandwich 0:441caaf895d8 64 double a_xAccumulator = 0;
sandwich 0:441caaf895d8 65 double a_yAccumulator = 0;
sandwich 0:441caaf895d8 66 double a_zAccumulator = 0;
sandwich 0:441caaf895d8 67 for (int i=0; i<readsamples; ++i) {
sandwich 0:441caaf895d8 68 accelerometer->getOutput(readings);
sandwich 0:441caaf895d8 69 a_xAccumulator += (int16_t) readings[0];
sandwich 0:441caaf895d8 70 a_yAccumulator += (int16_t) readings[1];
sandwich 0:441caaf895d8 71 a_zAccumulator += (int16_t) readings[2];
sandwich 0:441caaf895d8 72 }
sandwich 0:441caaf895d8 73 //Average the samples, remove the bias, and calculate the acceleration
sandwich 0:441caaf895d8 74 //in m/s/s.
sandwich 0:441caaf895d8 75 output[0] = ((a_xAccumulator / readsamples) - xBias) * ACCELEROMETER_GAIN;
sandwich 0:441caaf895d8 76 output[1] = ((a_yAccumulator / readsamples) - yBias) * ACCELEROMETER_GAIN;
sandwich 0:441caaf895d8 77 output[2] = ((a_zAccumulator / readsamples) - zBias) * ACCELEROMETER_GAIN;
sandwich 0:441caaf895d8 78 return output;
sandwich 0:441caaf895d8 79 }
sandwich 0:441caaf895d8 80
sandwich 0:441caaf895d8 81
sandwich 0:441caaf895d8 82 ADXL345HL::ADXL345HL()
sandwich 0:441caaf895d8 83 {
sandwich 0:441caaf895d8 84 }
sandwich 0:441caaf895d8 85
sandwich 0:441caaf895d8 86 ADXL345HL::~ADXL345HL()
sandwich 0:441caaf895d8 87 {
sandwich 0:441caaf895d8 88 delete[] output;
sandwich 0:441caaf895d8 89 delete accelerometer;
sandwich 0:441caaf895d8 90 }