Omar Hagrass / CANSAT_COMBINED

IMU-pressure-tempreture sensors

Dependencies:   CMSIS_DSP_401 DHT22 MPU9150_DMP QuaternionMath MODSERIAL mbed-src FATFileSystem111 SDFileSystem11 Camera_LS_Y201_CANSAT

Dependents:   combined_F

main.cpp@1:339ebc8786ca, 2015-08-11 (annotated)

Committer:
Hagrass
Date:
Tue Aug 11 16:56:59 2015 +0000
Revision:
1:339ebc8786ca
Parent:
0:5f608863559a
Child:
3:598af964f16c
Child:
4:dbb8e901826d
combined

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hagrass 0:5f608863559a 1 #include "MPU9150.h"
Hagrass 0:5f608863559a 2 #include "Quaternion.h"
Hagrass 0:5f608863559a 3 #include "BMP085.h"
Hagrass 0:5f608863559a 4 #include "DHT22.h"
Hagrass 1:339ebc8786ca 5 #include "main.h"
Hagrass 1:339ebc8786ca 6 char ns, ew, tf, status;
Hagrass 1:339ebc8786ca 7 int fq, nst, fix, date; // fix quality, Number of satellites being tracked, 3D fix
Hagrass 1:339ebc8786ca 8 float latitude, longitude, timefix, speed, altitude;
Hagrass 1:339ebc8786ca 9
Hagrass 1:339ebc8786ca 10
Hagrass 1:339ebc8786ca 11
Hagrass 0:5f608863559a 12 DHT22 dht22(p23);
Hagrass 0:5f608863559a 13 BMP085 bmp085(p28, p27,BMP085_oss8);
Hagrass 1:339ebc8786ca 14 Serial xbee(p9,p10);
Hagrass 1:339ebc8786ca 15 DigitalOut rst(p11,PullUp);
Hagrass 0:5f608863559a 16 DigitalOut myled(LED1);
Hagrass 0:5f608863559a 17
Hagrass 0:5f608863559a 18 Serial debug(USBTX, USBRX);
Hagrass 0:5f608863559a 19 MPU9150 imu(p27, p28, p15);
Hagrass 0:5f608863559a 20
Hagrass 0:5f608863559a 21 DigitalOut led(LED1);
Hagrass 1:339ebc8786ca 22 void Init()
Hagrass 1:339ebc8786ca 23 {
Hagrass 1:339ebc8786ca 24 gps.baud(9600);
Hagrass 1:339ebc8786ca 25 // pc.baud(115200);
Hagrass 1:339ebc8786ca 26
Hagrass 1:339ebc8786ca 27 xbee.printf("Init OK\n");
Hagrass 1:339ebc8786ca 28 printf("Init OK\n");
Hagrass 1:339ebc8786ca 29 }
Hagrass 0:5f608863559a 30
Hagrass 0:5f608863559a 31 char buffer[200];
Hagrass 1:339ebc8786ca 32 int e=6;
Hagrass 1:339ebc8786ca 33 int n=0;
Hagrass 0:5f608863559a 34 int main(){
Hagrass 0:5f608863559a 35
Hagrass 1:339ebc8786ca 36 Init();
Hagrass 1:339ebc8786ca 37 char c;
Hagrass 1:339ebc8786ca 38 //debug.baud(115200);
Hagrass 0:5f608863559a 39
Hagrass 0:5f608863559a 40 if(imu.isReady()){
Hagrass 1:339ebc8786ca 41 xbee.printf("MPU9150 is ready\r\n");
Hagrass 1:339ebc8786ca 42 printf("MPU9150 is ready\r\n");
Hagrass 0:5f608863559a 43 } else {
Hagrass 1:339ebc8786ca 44 xbee.printf("MPU9150 initialisation failure\r\n");
Hagrass 1:339ebc8786ca 45 printf("MPU9150 initialisation failure\r\n");
Hagrass 0:5f608863559a 46 }
Hagrass 0:5f608863559a 47
Hagrass 0:5f608863559a 48 imu.initialiseDMP();
Hagrass 0:5f608863559a 49
Hagrass 0:5f608863559a 50 Timer timer;
Hagrass 0:5f608863559a 51 timer.start();
Hagrass 0:5f608863559a 52
Hagrass 0:5f608863559a 53 imu.setFifoReset(true);
Hagrass 0:5f608863559a 54 imu.setDMPEnabled(true);
Hagrass 0:5f608863559a 55
Hagrass 0:5f608863559a 56 Quaternion q1;
Hagrass 1:339ebc8786ca 57
Hagrass 0:5f608863559a 58 float hum,temp;
Hagrass 1:339ebc8786ca 59 // int g=10;
Hagrass 0:5f608863559a 60
Hagrass 0:5f608863559a 61 while(true){
Hagrass 1:339ebc8786ca 62 n++;
Hagrass 1:339ebc8786ca 63 // wait(0.5);
Hagrass 1:339ebc8786ca 64 bmp085.update();
Hagrass 1:339ebc8786ca 65 float allltitude=bmp085.calcAltitude(bmp085.get_pressure()*100);
Hagrass 0:5f608863559a 66 //wait(0.2);
Hagrass 1:339ebc8786ca 67 if(e==6)
Hagrass 1:339ebc8786ca 68 {
Hagrass 0:5f608863559a 69 dht22.sample() ;
Hagrass 0:5f608863559a 70 hum=dht22.getHumidity()/10.0;
Hagrass 1:339ebc8786ca 71 temp=dht22.getTemperature()/10.0;
Hagrass 0:5f608863559a 72
Hagrass 1:339ebc8786ca 73 e=1;
Hagrass 1:339ebc8786ca 74 }
Hagrass 1:339ebc8786ca 75
Hagrass 0:5f608863559a 76
Hagrass 0:5f608863559a 77 if(imu.getFifoCount() >= 48){
Hagrass 0:5f608863559a 78 imu.getFifoBuffer(buffer, 48);
Hagrass 0:5f608863559a 79 led = !led;
Hagrass 0:5f608863559a 80 }
Hagrass 0:5f608863559a 81 // debug.printf("vcvssgsgf");
Hagrass 0:5f608863559a 82 if(timer.read_ms() >50){
Hagrass 0:5f608863559a 83 timer.reset();
Hagrass 0:5f608863559a 84
Hagrass 0:5f608863559a 85 //This is the format of the data in the fifo,
Hagrass 0:5f608863559a 86 /* ================================================================================================ *
Hagrass 0:5f608863559a 87 | Default MotionApps v4.1 48-byte FIFO packet structure: |
Hagrass 0:5f608863559a 88 | |
Hagrass 0:5f608863559a 89 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
Hagrass 0:5f608863559a 90 | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
Hagrass 0:5f608863559a 91 | |
Hagrass 0:5f608863559a 92 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
Hagrass 0:5f608863559a 93 | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 |
Hagrass 0:5f608863559a 94 * ================================================================================================ */
Hagrass 0:5f608863559a 95
Hagrass 0:5f608863559a 96 /*
Hagrass 0:5f608863559a 97 debug.printf("%d, %d, %d\r\n", (int32_t)(((int32_t)buffer[34] << 24) + ((int32_t)buffer[35] << 16) + ((int32_t)buffer[36] << 8) + (int32_t)buffer[37]),
Hagrass 0:5f608863559a 98 (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
Hagrass 0:5f608863559a 99 (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
Hagrass 0:5f608863559a 100
Hagrass 0:5f608863559a 101 debug.printf("%d, %d, %d\r\n", (int32_t)(((int32_t)buffer[16] << 24) + ((int32_t)buffer[17] << 16) + ((int32_t)buffer[18] << 8) + (int32_t)buffer[19]),
Hagrass 0:5f608863559a 102 (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
Hagrass 0:5f608863559a 103 (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
Hagrass 0:5f608863559a 104
Hagrass 0:5f608863559a 105 debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28],
Hagrass 0:5f608863559a 106 (int16_t)(buffer[31] << 8) + buffer[30],
Hagrass 0:5f608863559a 107 (int16_t)(buffer[33] << 8) + buffer[32]);
Hagrass 0:5f608863559a 108
Hagrass 0:5f608863559a 109 debug.printf("%f, %f, %f, %f\r\n",
Hagrass 0:5f608863559a 110 (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)),
Hagrass 0:5f608863559a 111 (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)),
Hagrass 0:5f608863559a 112 (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)),
Hagrass 0:5f608863559a 113 (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30)));
Hagrass 0:5f608863559a 114 */
Hagrass 0:5f608863559a 115
Hagrass 1:339ebc8786ca 116 q1.decode(buffer);
Hagrass 1:339ebc8786ca 117
Hagrass 1:339ebc8786ca 118 //wait(0.5);
Hagrass 0:5f608863559a 119
Hagrass 1:339ebc8786ca 120 if(gps.readable())
Hagrass 1:339ebc8786ca 121 {
Hagrass 1:339ebc8786ca 122 if(gps.getc() == '$'); // wait a $
Hagrass 1:339ebc8786ca 123 {
Hagrass 1:339ebc8786ca 124 for(int i=0; i<sizeof(cDataBuffer); i++)
Hagrass 1:339ebc8786ca 125 {
Hagrass 1:339ebc8786ca 126 c = gps.getc();
Hagrass 1:339ebc8786ca 127 if( c == '\r' )
Hagrass 1:339ebc8786ca 128 {
Hagrass 1:339ebc8786ca 129 //pc.printf("%s\n", cDataBuffer);
Hagrass 1:339ebc8786ca 130 parse(cDataBuffer, i);
Hagrass 1:339ebc8786ca 131 i = sizeof(cDataBuffer);
Hagrass 1:339ebc8786ca 132 }
Hagrass 1:339ebc8786ca 133 else
Hagrass 1:339ebc8786ca 134 {
Hagrass 1:339ebc8786ca 135 cDataBuffer[i] = c;
Hagrass 1:339ebc8786ca 136 }
Hagrass 1:339ebc8786ca 137 }
Hagrass 1:339ebc8786ca 138 }
Hagrass 1:339ebc8786ca 139 }
Hagrass 1:339ebc8786ca 140
Hagrass 1:339ebc8786ca 141
Hagrass 1:339ebc8786ca 142 if(n==10)
Hagrass 1:339ebc8786ca 143 {
Hagrass 1:339ebc8786ca 144 xbee.printf("w:%f, v.x:%f, v.y:%f, v.z:%f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z);
Hagrass 1:339ebc8786ca 145
Hagrass 1:339ebc8786ca 146
Hagrass 1:339ebc8786ca 147 xbee.printf("p:%f hPa / t:%f / altitude=%f \n\r",bmp085.get_pressure() , bmp085.get_temperature(),allltitude);
Hagrass 1:339ebc8786ca 148
Hagrass 1:339ebc8786ca 149 xbee.printf("temp: %f , hum:%f \n\r",temp,hum);
Hagrass 1:339ebc8786ca 150
Hagrass 1:339ebc8786ca 151
Hagrass 1:339ebc8786ca 152 xbee.printf("GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
Hagrass 1:339ebc8786ca 153 xbee.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
Hagrass 1:339ebc8786ca 154 xbee.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
Hagrass 1:339ebc8786ca 155 xbee.printf("GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\n\n\r\n\n\n", timefix, status, latitude, ns, longitude, ew, speed, date);
Hagrass 1:339ebc8786ca 156 n=1;
Hagrass 1:339ebc8786ca 157
Hagrass 1:339ebc8786ca 158 }
Hagrass 1:339ebc8786ca 159
Hagrass 1:339ebc8786ca 160 printf("w:%f, v.x:%f, v.y:%f, v.z:%f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z);
Hagrass 1:339ebc8786ca 161 printf("p:%f hPa / t:%f / altitude=%f \n\r",bmp085.get_pressure() , bmp085.get_temperature(),allltitude);
Hagrass 1:339ebc8786ca 162 printf("temp: %f , hum:%f \n\r",temp,hum);
Hagrass 1:339ebc8786ca 163 printf("GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
Hagrass 1:339ebc8786ca 164 printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
Hagrass 1:339ebc8786ca 165 printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
Hagrass 1:339ebc8786ca 166 printf("GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\n\n\r\n\n\n", timefix, status, latitude, ns, longitude, ew, speed, date);
Hagrass 1:339ebc8786ca 167
Hagrass 1:339ebc8786ca 168
Hagrass 1:339ebc8786ca 169
Hagrass 1:339ebc8786ca 170
Hagrass 1:339ebc8786ca 171 imu.setFifoReset(true);
Hagrass 1:339ebc8786ca 172 imu.setDMPEnabled(true);
Hagrass 0:5f608863559a 173 //TeaPot Demo Packet for MotionFit SDK
Hagrass 0:5f608863559a 174 /*
Hagrass 0:5f608863559a 175 debug.putc('$'); //packet start
Hagrass 0:5f608863559a 176 debug.putc((char)2); //assume packet type constant
Hagrass 0:5f608863559a 177 debug.putc((char)0); //count seems unused
Hagrass 0:5f608863559a 178 for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats
Hagrass 0:5f608863559a 179 debug.putc((char)(buffer[i]));
Hagrass 0:5f608863559a 180 }
Hagrass 0:5f608863559a 181 for(int i = 0; i < 5; i++){ //no idea, padded with 0
Hagrass 0:5f608863559a 182 debug.putc((char)0);
Hagrass 0:5f608863559a 183 }
Hagrass 0:5f608863559a 184 */
Hagrass 0:5f608863559a 185 }
Hagrass 0:5f608863559a 186 //wait(1);
Hagrass 1:339ebc8786ca 187 e++;
Hagrass 1:339ebc8786ca 188 //g++;
Hagrass 1:339ebc8786ca 189 //xbee.printf("this is the end");
Hagrass 1:339ebc8786ca 190 //rst=1;
Hagrass 1:339ebc8786ca 191 //wait(0.001);
Hagrass 1:339ebc8786ca 192 //rst=0;
Hagrass 0:5f608863559a 193 }
Hagrass 1:339ebc8786ca 194
Hagrass 0:5f608863559a 195 }
Hagrass 1:339ebc8786ca 196
Hagrass 1:339ebc8786ca 197
Hagrass 1:339ebc8786ca 198
Hagrass 1:339ebc8786ca 199
Hagrass 1:339ebc8786ca 200 void parse(char *cmd, int n)
Hagrass 1:339ebc8786ca 201 {
Hagrass 1:339ebc8786ca 202
Hagrass 1:339ebc8786ca 203
Hagrass 1:339ebc8786ca 204
Hagrass 1:339ebc8786ca 205
Hagrass 1:339ebc8786ca 206 // Global Positioning System Fix Data
Hagrass 1:339ebc8786ca 207 if(strncmp(cmd,"$GPGGA", 6) == 0)
Hagrass 1:339ebc8786ca 208 {
Hagrass 1:339ebc8786ca 209 sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
Hagrass 1:339ebc8786ca 210
Hagrass 1:339ebc8786ca 211 }
Hagrass 1:339ebc8786ca 212
Hagrass 1:339ebc8786ca 213 // Satellite status
Hagrass 1:339ebc8786ca 214 if(strncmp(cmd,"$GPGSA", 6) == 0)
Hagrass 1:339ebc8786ca 215 {
Hagrass 1:339ebc8786ca 216 sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
Hagrass 1:339ebc8786ca 217
Hagrass 1:339ebc8786ca 218 }
Hagrass 1:339ebc8786ca 219
Hagrass 1:339ebc8786ca 220 // Geographic position, Latitude and Longitude
Hagrass 1:339ebc8786ca 221 if(strncmp(cmd,"$GPGLL", 6) == 0)
Hagrass 1:339ebc8786ca 222 {
Hagrass 1:339ebc8786ca 223 sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
Hagrass 1:339ebc8786ca 224
Hagrass 1:339ebc8786ca 225 }
Hagrass 1:339ebc8786ca 226
Hagrass 1:339ebc8786ca 227 // Geographic position, Latitude and Longitude
Hagrass 1:339ebc8786ca 228 if(strncmp(cmd,"$GPRMC", 6) == 0)
Hagrass 1:339ebc8786ca 229 {
Hagrass 1:339ebc8786ca 230 sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
Hagrass 1:339ebc8786ca 231
Hagrass 1:339ebc8786ca 232 }
Hagrass 1:339ebc8786ca 233
Hagrass 1:339ebc8786ca 234 }