U of Calegary - Okeef
d
Dependencies: General C12832 FatFileSystemCpp mbed
Fork of
MPU9150AHRS
by 
Kris Winer
Diff: main.cpp
- Revision:
- 1:4523d7cda75e
- Parent:
- 0:39935bb3c1a1
--- a/main.cpp Sun Jun 29 22:48:08 2014 +0000
+++ b/main.cpp Fri Jun 17 20:35:28 2016 +0000
@@ -25,35 +25,51 @@
We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
*/
-
-//#include "ST_F401_84MHZ.h"
-//F401_init84 myinit(0);
+
#include "mbed.h"
#include "MPU9150.h"
-#include "N5110.h"
+#include "C12832.h"
+#include "MSCFileSystem.h"
+#include "BMP180.h"
+#include "DW1000.h"
+#include "MM2WayRanging.h"
+#include "stdlib.h"
-// Using NOKIA 5110 monochrome 84 x 48 pixel display
-// pin 9 - Serial clock out (SCLK)
-// pin 8 - Serial data out (DIN)
-// pin 7 - Data/Command select (D/C)
-// pin 5 - LCD chip select (CS)
-// pin 6 - LCD reset (RST)
-//Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
+#define LAST(k, n) ((k)&((1<<(n))-1))
+#define MID(k,m,n) LAST((k)>>(m),((n)-(m)))
+#define myprintf pc.printf
float sum = 0;
uint32_t sumCount = 0, mcount = 0;
char buffer[14];
- MPU9150 MPU9150;
+ //IMU
+ MPU9150 MPU9150;
+
+ //BARO
+ BMP180 bmp180(p28, p27); // sda, scl
+
+ // DWM1000
+ DW1000 dw(p5, p6, p7, p11, p17); // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
+ MM2WayRanging node(dw); // Ranging algorithm
+
+ //USB
+ MSCFileSystem fs("fs");
+
+ //Timer
+ Timer t, timer;
- Timer t;
-
- Serial pc(USBTX, USBRX); // tx, rx
-
- // VCC, SCE, RST, D/C, MOSI,S CLK, LED
- N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
+ // Application System
+ Serial pc(USBTX, USBRX); // tx, rx
+
+ // joystick
+ BusIn joystick(p15,p12,p13,p16);
-
+ // LCD display
+ //C12832 lcd(p5, p7, p6, p8, p11);
+
+ // joystick
+ DigitalIn fire(p14);
int main()
{
@@ -62,86 +78,88 @@
//Set up I2C
i2c.frequency(400000); // use fast (400 kHz) I2C
- pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
+ //lcd.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
t.start();
- lcd.init();
-// lcd.setBrightness(0.05);
-
+ // Connection Test of the DecaWave Ranging Measurement Unit
+
+ dw.setEUI(0xFAEDCD01FAEDCD01); // basic methods called to check if we have a working SPI connection
+ if (!(dw.getEUI() == 0xFAEDCD01FAEDCD01 && dw.getDeviceID() == 0xDECA0130))
+ { myprintf("DWM1000 Identification error:\n DeviceID = %d\n EUI = %d", dw.getDeviceID(), dw.getEUI());
+ while(1);}
+ else
+ {myprintf("DWM100 Connection established\n\r");}
+ // Anchor or Beacon?
+myprintf("Set the Chip as an Anchor: Up\n\r Set the Chip as a Beacon: Down\n\r");
+while (joystick.read() != 1 && joystick.read() != 2){wait(0.001);}
+
+if (joystick.read() == 1) {
+ node.isAnchor = true;
+ node.address = 2;
+ myprintf("This node is Anchor node %d \r\n", node.address);
+ while(1);
+ } else {
+ node.isAnchor = false;
+ node.address = 0;
+ myprintf("This node is a Beacon.\n\r ");
+ }
+
// Read the WHO_AM_I register, this is a good test of communication
uint8_t whoami = MPU9150.readByte(MPU9150_ADDRESS, WHO_AM_I_MPU9150); // Read WHO_AM_I register for MPU-9250
- pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
if (whoami == 0x68) // WHO_AM_I should be 0x68
- {
- pc.printf("MPU9150 WHO_AM_I is 0x%x\n\r", whoami);
- pc.printf("MPU9150 is online...\n\r");
- lcd.clear();
- lcd.printString("MPU9150 is", 0, 0);
- sprintf(buffer, "0x%x", whoami);
- lcd.printString(buffer, 0, 1);
- lcd.printString("shoud be 0x68", 0, 2);
- wait(1);
+ {
+ myprintf("IMU Connection established, Initialization...\n\r");
+ MPU9150.MPU9150SelfTest(SelfTest); // Accelerometer and gyroscope self test; check calibration wrt factory settings
+ //lcd.printf("x-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[0]);
- MPU9150.MPU9150SelfTest(SelfTest);
- pc.printf("x-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[0]);
- pc.printf("y-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[1]);
- pc.printf("z-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[2]);
- pc.printf("x-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[3]);
- pc.printf("y-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[4]);
- pc.printf("z-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[5]);
- wait(1);
MPU9150.resetMPU9150(); // Reset registers to default in preparation for device calibration
MPU9150.calibrateMPU9150(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
- pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
- pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
- pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
- pc.printf("x accel bias = %f\n\r", accelBias[0]);
- pc.printf("y accel bias = %f\n\r", accelBias[1]);
- pc.printf("z accel bias = %f\n\r", accelBias[2]);
- wait(1);
+ //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
+
MPU9150.initMPU9150();
- pc.printf("MPU9150 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+ myprintf("MPU9150 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
MPU9150.initAK8975A(magCalibration);
- pc.printf("AK8975 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+ myprintf("AK8975 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
}
else
{
- pc.printf("Could not connect to MPU9150: \n\r");
- pc.printf("%#x \n", whoami);
-
- lcd.clear();
- lcd.printString("MPU9150", 0, 0);
- lcd.printString("no connection", 0, 1);
- sprintf(buffer, "WHO_AM_I 0x%x", whoami);
- lcd.printString(buffer, 0, 2);
-
+ myprintf("Could not connect to MPU9150: \n\r");
+ myprintf("%#x \n", whoami);
while(1) ; // Loop forever if communication doesn't happen
}
+ bmp180.Initialize(64, BMP180_OSS_ULTRA_LOW_POWER); // 64m altitude compensation and low power oversampling
+
uint8_t MagRate = 10; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values
MPU9150.getAres(); // Get accelerometer sensitivity
MPU9150.getGres(); // Get gyro sensitivity
- mRes = 10.*1229./4096.; // Conversion from 1229 microTesla full scale (4096) to 12.29 Gauss full scale
+ mRes = 10.*1229./4096.; // Conversion from binary to microtesla and from 1229 microTesla full scale (4096) to 12.29 Gauss full scale
// So far, magnetometer bias is calculated and subtracted here manually, should construct an algorithm to do it automatically
// like the gyro and accelerometer biases
magbias[0] = -5.; // User environmental x-axis correction in milliGauss
magbias[1] = -95.; // User environmental y-axis correction in milliGauss
magbias[2] = -260.; // User environmental z-axis correction in milliGauss
-
-
- while(1) {
+FILE *fic= fopen("/fs/test.txt","w");
+int button = 0;
+float Start;
+float pressure, temperature;
+float Distance[3] = {7,8,9};
+int Points = 0;
+ while(joystick.read() != 4) {
+ // Get Ranging measurements
+ (Distance[0], Distance[1], Distance[2]) = node.rangeAndDisplayAll();
// If intPin goes high, all data registers have new data
if(MPU9150.readByte(MPU9150_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
MPU9150.readAccelData(accelCount); // Read the x/y/z adc values
// Now we'll calculate the accleration value into actual g's
- ax = (float)accelCount[0]*aRes; // - accelBias[0]; // get actual g value, this depends on scale being set
- ay = (float)accelCount[1]*aRes; // - accelBias[1];
- az = (float)accelCount[2]*aRes; // - accelBias[2];
+ ax = (float)accelCount[0]*aRes; - accelBias[0]; // get actual g value, this depends on scale being set
+ ay = (float)accelCount[1]*aRes; - accelBias[1];
+ az = (float)accelCount[2]*aRes; - accelBias[2];
MPU9150.readGyroData(gyroCount); // Read the x/y/z adc values
// Calculate the gyro value into actual degrees per second
@@ -160,13 +178,14 @@
mcount = 0;
}
}
+
- Now = t.read_us();
- deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
- lastUpdate = Now;
+ //Now = t.read_us();
+ //deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+ //lastUpdate = Now;
- sum += deltat;
- sumCount++;
+ //sum += deltat;
+ //sumCount++;
// if(lastUpdate - firstUpdate > 10000000.0f) {
// beta = 0.04; // decrease filter gain after stabilized
@@ -175,32 +194,72 @@
// Pass gyro rate as rad/s
// MPU9150.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
- MPU9150.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+ //MPU9150.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
// Serial print and/or display at 0.5 s rate independent of data rates
- delt_t = t.read_ms() - count;
- if (delt_t > 500) { // update LCD once per half-second independent of read rate
+
+ //myprintf("\r\n CIR= %u, %016llX", MID(dw.getCIR_PWR(),48,64), dw.getCIR_PWR());
+ //myprintf("\r\n RXPACC = %u, %X", MID(dw.getRXPACC(), 20,32), dw.getRXPACC());
- pc.printf("ax = %f", 1000*ax);
- pc.printf(" ay = %f", 1000*ay);
- pc.printf(" az = %f mg\n\r", 1000*az);
- pc.printf("gx = %f", gx);
- pc.printf(" gy = %f", gy);
- pc.printf(" gz = %f deg/s\n\r", gz);
+ //while(1);
- pc.printf("gx = %f", mx);
- pc.printf(" gy = %f", my);
- pc.printf(" gz = %f mG\n\r", mz);
+ delt_t = t.read_ms() - count;
+ timer.start();
+ while (fire)
+ { Start = timer.read();
+ myprintf("#");
+ fprintf(fic, "#\n");
+ wait(0.3);}
+
+ if (Start>0.1)
+ { button = !button;
+ Points = 0;}
+
+ if (fic != NULL && button && Points < 501)
+ {
+ //fprintf(fic, "%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f\n", t.read(),1000*ax, 1000*ay, 1000*az, gx, gy, gz, mx, my, mz, pressure, temperature, Distance);
+ myprintf("%d", Points);
+ fprintf(fic, "%.2f %.2f %.2f %u %u\n", Distance[0], Distance[1], Distance[2], MID(dw.getRXPACC(), 20,32), MID(dw.getCIR_PWR(),48,64));
+ myprintf(" Distance 2 = %.2f Distance 3 = %.2f Distance 4 = %.2f %u %u\n\r", node.distances[2], node.distances[3], node.distances[4], MID(dw.getRXPACC(), 20,32), MID(dw.getCIR_PWR(),48,64));
+ Start = 0;
+ wait(0.0005);
+ Points ++;}
+ else
+ //{if (fic != NULL && button)
+ //{fprintf(fic, "%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;No data;No data\n",t.read(),1000*ax, 1000*ay, 1000*az, gx, gy, gz, mx, my, mz);
+ // Start = 0;}
+ // else {
+ {
+ myprintf(".");
+ Start = 0;}
+
+ if (delt_t > 500) { // update LCD once per half-second independent of read rate
+
+
+
+ //lcd.locate(0,0);
+ //lcd.printf("ax = %.3f", 1000*ax);
+ //myprintf(" ay = %f", 1000*ay);
+ //pc.printf(" az = %f mg\n\r", 1000*az);
+ //lcd.locate(0,9);
+ //lcd.printf("gx = %f", gx);
+ // myprintf(" gy = %f", gy);
+ //pc.printf(" gz = %f deg/s\n\r", gz);
+ //lcd.locate(0,18);
+ //lcd.printf("gx = %f", mx);
+ //myprintf(" gy = %f", my);
+ //pc.printf(" gz = %f mG\n\r", mz);
tempCount = MPU9150.readTempData(); // Read the adc values
temperature = ((float) tempCount) / 340.0f + 36.53f; // Temperature in degrees Centigrade
- pc.printf(" temperature = %f C\n\r", temperature);
+ //myprintf(" temperature = %f C", temperature);
+ //myprintf("Ranging = %f\n\r ", Distance);
- pc.printf("q0 = %f\n\r", q[0]);
- pc.printf("q1 = %f\n\r", q[1]);
- pc.printf("q2 = %f\n\r", q[2]);
- pc.printf("q3 = %f\n\r", q[3]);
+ //pc.printf("q0 = %f\n\r", q[0]);
+ //pc.printf("q1 = %f\n\r", q[1]);
+ //pc.printf("q2 = %f\n\r", q[2]);
+ //pc.printf("q3 = %f\n\r", q[3]);
/* lcd.clear();
lcd.printString("MPU9150", 0, 0);
@@ -221,16 +280,16 @@
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
- yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
- pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
- roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
- pitch *= 180.0f / PI;
- yaw *= 180.0f / PI;
- yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
- roll *= 180.0f / PI;
+ //yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+ //pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+ //roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+ //pitch *= 180.0f / PI;
+ //yaw *= 180.0f / PI;
+ //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
+ //roll *= 180.0f / PI;
- pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
- pc.printf("average rate = %f\n\r", (float) sumCount/sum);
+ //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+ //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
// sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
// lcd.printString(buffer, 0, 4);
// sprintf(buffer, "rate = %f", (float) sumCount/sum);
@@ -244,10 +303,11 @@
count = 0;
deltat= 0;
lastUpdate = t.read_us();
- }
- sum = 0;
- sumCount = 0;
+ }
+ //sum = 0;
+ //sumCount = 0; *
}
}
+fclose(fic);
}
\ No newline at end of file