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@3:598af964f16c, 2015-08-18 (annotated)

Committer:
Hagrass
Date:
Tue Aug 18 14:40:03 2015 +0000
Revision:
3:598af964f16c
Parent:
1:339ebc8786ca 
cansat;

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 3:598af964f16c 6
Hagrass 3:598af964f16c 7 #include "Camera_LS_Y201.h"
Hagrass 3:598af964f16c 8 #include "SDFileSystem.h"
Hagrass 1:339ebc8786ca 9 char ns, ew, tf, status;
Hagrass 1:339ebc8786ca 10 int fq, nst, fix, date; // fix quality, Number of satellites being tracked, 3D fix
Hagrass 1:339ebc8786ca 11 float latitude, longitude, timefix, speed, altitude;
Hagrass 3:598af964f16c 12 Serial xbee(p28,p27);
Hagrass 1:339ebc8786ca 13
Hagrass 1:339ebc8786ca 14
Hagrass 0:5f608863559a 15 DHT22 dht22(p23);
Hagrass 3:598af964f16c 16 BMP085 bmp085(p9, p10,BMP085_oss8);
Hagrass 3:598af964f16c 17
Hagrass 3:598af964f16c 18
Hagrass 3:598af964f16c 19 //DigitalOut myled(LED1);
Hagrass 0:5f608863559a 20
Hagrass 0:5f608863559a 21 Serial debug(USBTX, USBRX);
Hagrass 3:598af964f16c 22 MPU9150 imu(p10, p9, p15);
Hagrass 3:598af964f16c 23 /////////////////////////////////////////////////////
Hagrass 3:598af964f16c 24 #define DEBMSG debug.printf
Hagrass 3:598af964f16c 25 #define NEWLINE() debug.printf("\r\n")
Hagrass 3:598af964f16c 26
Hagrass 3:598af964f16c 27 #define USE_SDCARD 0
Hagrass 3:598af964f16c 28
Hagrass 3:598af964f16c 29 #if USE_SDCARD
Hagrass 3:598af964f16c 30 #define FILENAME "/sd/IMG_%04d.jpg"
Hagrass 3:598af964f16c 31 SDFileSystem fs(p5, p6, p7, p8, "sd");
Hagrass 3:598af964f16c 32 #else
Hagrass 3:598af964f16c 33 #define FILENAME "/local/IMG_%04d.jpg"
Hagrass 3:598af964f16c 34 LocalFileSystem fs("local");
Hagrass 3:598af964f16c 35 #endif
Hagrass 3:598af964f16c 36 Camera_LS_Y201 cam1(p17, p18);
Hagrass 3:598af964f16c 37
Hagrass 3:598af964f16c 38 typedef struct work {
Hagrass 3:598af964f16c 39 FILE *fp;
Hagrass 3:598af964f16c 40 } work_t;
Hagrass 3:598af964f16c 41
Hagrass 3:598af964f16c 42 work_t work;
Hagrass 3:598af964f16c 43
Hagrass 3:598af964f16c 44
Hagrass 3:598af964f16c 45
Hagrass 3:598af964f16c 46 uint8_t send[16] = {
Hagrass 3:598af964f16c 47 0x56,
Hagrass 3:598af964f16c 48 0x00,
Hagrass 3:598af964f16c 49 0x32,
Hagrass 3:598af964f16c 50 0x0C,
Hagrass 3:598af964f16c 51 0x00,
Hagrass 3:598af964f16c 52 0x0A,
Hagrass 3:598af964f16c 53 0x00,
Hagrass 3:598af964f16c 54 0x00,
Hagrass 3:598af964f16c 55 0x00, // MH
Hagrass 3:598af964f16c 56 0x00, // ML
Hagrass 3:598af964f16c 57 0x00,
Hagrass 3:598af964f16c 58 0x00,
Hagrass 3:598af964f16c 59 0x00, // KH
Hagrass 3:598af964f16c 60 0x00, // KL
Hagrass 3:598af964f16c 61 0x00, // XX
Hagrass 3:598af964f16c 62 0x00 // XX
Hagrass 3:598af964f16c 63 };
Hagrass 3:598af964f16c 64
Hagrass 3:598af964f16c 65 uint16_t x = 10; // Interval time. XX XX * 0.01m[sec]
Hagrass 3:598af964f16c 66 bool end = true;
Hagrass 3:598af964f16c 67 uint16_t m = 0; // Staring address.
Hagrass 3:598af964f16c 68 /*
Hagrass 3:598af964f16c 69 * Get the data size.
Hagrass 3:598af964f16c 70 */
Hagrass 3:598af964f16c 71 uint8_t body[16000];
Hagrass 3:598af964f16c 72 uint16_t k = sizeof(body);
Hagrass 3:598af964f16c 73 int siz_done = 0;
Hagrass 3:598af964f16c 74 int siz_total = 0;
Hagrass 3:598af964f16c 75 int cnt = 0;
Hagrass 3:598af964f16c 76 int z;
Hagrass 3:598af964f16c 77 //////////////////////////////////////////
Hagrass 3:598af964f16c 78 void callback_func(int done, int total, uint8_t *buf, size_t siz) {
Hagrass 3:598af964f16c 79
Hagrass 3:598af964f16c 80 fwrite(buf, siz, 1, work.fp);
Hagrass 3:598af964f16c 81
Hagrass 3:598af964f16c 82 //for(int i=0;i<siz;i++)
Hagrass 3:598af964f16c 83 //{
Hagrass 3:598af964f16c 84 //xbee.printf("%x",buf[i]);
Hagrass 3:598af964f16c 85 //fprintf(work.fp,"%c",buf[i]);
Hagrass 3:598af964f16c 86 //}
Hagrass 3:598af964f16c 87
Hagrass 3:598af964f16c 88 static int n = 0;
Hagrass 3:598af964f16c 89 int tmp = done * 100 / total;
Hagrass 3:598af964f16c 90 if (n != tmp) {
Hagrass 3:598af964f16c 91 n = tmp;
Hagrass 3:598af964f16c 92 DEBMSG("Writ%3d%%", n);
Hagrass 3:598af964f16c 93 NEWLINE();
Hagrass 3:598af964f16c 94 }
Hagrass 3:598af964f16c 95 }
Hagrass 3:598af964f16c 96
Hagrass 3:598af964f16c 97
Hagrass 3:598af964f16c 98
Hagrass 3:598af964f16c 99 ////////////////////////////////////////////
Hagrass 3:598af964f16c 100
Hagrass 3:598af964f16c 101
Hagrass 3:598af964f16c 102
Hagrass 3:598af964f16c 103
Hagrass 3:598af964f16c 104 void readdJpegFileContent(void (*func)(int done, int total, uint8_t *buf, size_t siz)) {
Hagrass 3:598af964f16c 105
Hagrass 3:598af964f16c 106
Hagrass 3:598af964f16c 107
Hagrass 3:598af964f16c 108 if(m==0)
Hagrass 3:598af964f16c 109 {
Hagrass 3:598af964f16c 110 z=sizeof(body);
Hagrass 3:598af964f16c 111 }
Hagrass 3:598af964f16c 112
Hagrass 3:598af964f16c 113 //if (r != NoError) {
Hagrass 3:598af964f16c 114 // return r;
Hagrass 3:598af964f16c 115 //}
Hagrass 0:5f608863559a 116
Hagrass 3:598af964f16c 117
Hagrass 3:598af964f16c 118 send[8] = (m >> 8) & 0xff;
Hagrass 3:598af964f16c 119 send[9] = (m >> 0) & 0xff;
Hagrass 3:598af964f16c 120 send[12] = (k >> 8) & 0xff;
Hagrass 3:598af964f16c 121 send[13] = (k >> 0) & 0xff;
Hagrass 3:598af964f16c 122 send[14] = (x >> 8) & 0xff;
Hagrass 3:598af964f16c 123 send[15] = (x >> 0) & 0xff;
Hagrass 3:598af964f16c 124 /*
Hagrass 3:598af964f16c 125 * Send a command.
Hagrass 3:598af964f16c 126 */
Hagrass 3:598af964f16c 127 cam1.sendBytes(send, sizeof(send), 200 * 1000) ;
Hagrass 3:598af964f16c 128 // printf("sended header");
Hagrass 3:598af964f16c 129 // return SendError;
Hagrass 3:598af964f16c 130
Hagrass 3:598af964f16c 131 /*
Hagrass 3:598af964f16c 132 * Read the header of the response.
Hagrass 3:598af964f16c 133 */
Hagrass 3:598af964f16c 134 uint8_t header[5];
Hagrass 3:598af964f16c 135 cam1.recvBytes(header, sizeof(header), 2 * 1000 * 1000);
Hagrass 3:598af964f16c 136 // return RecvError;
Hagrass 3:598af964f16c 137 //printf("recved header");
Hagrass 3:598af964f16c 138 /*
Hagrass 3:598af964f16c 139 * Check the response and fetch an image data.
Hagrass 3:598af964f16c 140 */
Hagrass 3:598af964f16c 141
Hagrass 3:598af964f16c 142 if ((header[0] == 0x76)
Hagrass 3:598af964f16c 143 && (header[1] == 0x00)
Hagrass 3:598af964f16c 144 && (header[2] == 0x32)
Hagrass 3:598af964f16c 145 && (header[3] == 0x00)
Hagrass 3:598af964f16c 146 && (header[4] == 0x00)) {
Hagrass 3:598af964f16c 147
Hagrass 3:598af964f16c 148 cam1.recvBytes(body, z, 2 * 1000 * 1000); //{
Hagrass 3:598af964f16c 149 //return RecvError;
Hagrass 3:598af964f16c 150 // }
Hagrass 3:598af964f16c 151 // printf("saved");
Hagrass 3:598af964f16c 152
Hagrass 3:598af964f16c 153 siz_done += z;
Hagrass 3:598af964f16c 154
Hagrass 3:598af964f16c 155 if (func != NULL) {
Hagrass 3:598af964f16c 156 if (siz_done > siz_total) {
Hagrass 3:598af964f16c 157 z=sizeof(body)-siz_done+siz_total+3;
Hagrass 3:598af964f16c 158 siz_done = siz_total;
Hagrass 3:598af964f16c 159
Hagrass 3:598af964f16c 160
Hagrass 3:598af964f16c 161 }
Hagrass 3:598af964f16c 162 func(siz_done, siz_total, body, z);
Hagrass 3:598af964f16c 163 }
Hagrass 3:598af964f16c 164 for (int i = 1; i < z; i++) {
Hagrass 3:598af964f16c 165 if ((body[i - 1] == 0xFF) && (body[i - 0] == 0xD9)) {
Hagrass 3:598af964f16c 166 end = true;
Hagrass 3:598af964f16c 167 //printf("saved%d",i);
Hagrass 3:598af964f16c 168 }
Hagrass 3:598af964f16c 169 }
Hagrass 3:598af964f16c 170 } //else {
Hagrass 3:598af964f16c 171 // return UnexpectedReply;
Hagrass 3:598af964f16c 172 //}
Hagrass 3:598af964f16c 173 /*
Hagrass 3:598af964f16c 174 * Read the footer of the response.
Hagrass 3:598af964f16c 175 */
Hagrass 3:598af964f16c 176 uint8_t footer[5];
Hagrass 3:598af964f16c 177 cam1.recvBytes(footer, sizeof(footer), 2 * 1000 * 1000);// {
Hagrass 3:598af964f16c 178 // return RecvError;
Hagrass 3:598af964f16c 179 //}
Hagrass 3:598af964f16c 180
Hagrass 3:598af964f16c 181 m += z;
Hagrass 3:598af964f16c 182
Hagrass 3:598af964f16c 183 // return NoError;
Hagrass 3:598af964f16c 184 }
Hagrass 3:598af964f16c 185
Hagrass 3:598af964f16c 186 //////////////////
Hagrass 3:598af964f16c 187 /**
Hagrass 3:598af964f16c 188 * Capture.
Hagrass 3:598af964f16c 189 *
Hagrass 3:598af964f16c 190 * @param cam A pointer to a camera object.
Hagrass 3:598af964f16c 191 * @param filename The file name.
Hagrass 3:598af964f16c 192 *
Hagrass 3:598af964f16c 193 * @return Return 0 if it succeed.
Hagrass 3:598af964f16c 194 */
Hagrass 3:598af964f16c 195 int capture(Camera_LS_Y201 *cam, char *filename) {
Hagrass 3:598af964f16c 196 /*
Hagrass 3:598af964f16c 197 * Take a picture.
Hagrass 3:598af964f16c 198 */
Hagrass 3:598af964f16c 199 if(end==true)
Hagrass 3:598af964f16c 200 {
Hagrass 3:598af964f16c 201 if (cam->takePicture() != 0) {
Hagrass 3:598af964f16c 202 return -1;
Hagrass 3:598af964f16c 203 }
Hagrass 3:598af964f16c 204 cam1.readJpegFileSize(&siz_total);
Hagrass 3:598af964f16c 205 // printf("%d",siz_total);
Hagrass 3:598af964f16c 206 siz_done = 0;
Hagrass 3:598af964f16c 207
Hagrass 3:598af964f16c 208
Hagrass 3:598af964f16c 209 DEBMSG("Captured.");
Hagrass 3:598af964f16c 210 NEWLINE();
Hagrass 3:598af964f16c 211 work.fp = fopen(filename, "wb");
Hagrass 3:598af964f16c 212 //p1= fopen("/sd/IMG_11111.txt", "wb");
Hagrass 3:598af964f16c 213 if (work.fp == NULL) {
Hagrass 3:598af964f16c 214 return -2;
Hagrass 3:598af964f16c 215 }
Hagrass 3:598af964f16c 216
Hagrass 3:598af964f16c 217 /*
Hagrass 3:598af964f16c 218 * Read the content.
Hagrass 3:598af964f16c 219 */
Hagrass 3:598af964f16c 220 DEBMSG("%s", filename);
Hagrass 3:598af964f16c 221 NEWLINE();
Hagrass 3:598af964f16c 222 end=false;
Hagrass 3:598af964f16c 223 }
Hagrass 3:598af964f16c 224 /*
Hagrass 3:598af964f16c 225 * Open file.
Hagrass 3:598af964f16c 226 */
Hagrass 3:598af964f16c 227
Hagrass 3:598af964f16c 228 readdJpegFileContent(callback_func);
Hagrass 3:598af964f16c 229 // fclose(work.fp);
Hagrass 3:598af964f16c 230 //return -3;
Hagrass 3:598af964f16c 231
Hagrass 3:598af964f16c 232
Hagrass 3:598af964f16c 233
Hagrass 3:598af964f16c 234 /*
Hagrass 3:598af964f16c 235 * Stop taking pictures.
Hagrass 3:598af964f16c 236 */
Hagrass 3:598af964f16c 237 if(end== true)
Hagrass 3:598af964f16c 238 {
Hagrass 3:598af964f16c 239 fclose(work.fp);
Hagrass 3:598af964f16c 240 cam->stopTakingPictures();
Hagrass 3:598af964f16c 241
Hagrass 3:598af964f16c 242 DEBMSG("[%04d]:OK.", cnt);
Hagrass 3:598af964f16c 243 NEWLINE();
Hagrass 3:598af964f16c 244 m=0;
Hagrass 3:598af964f16c 245 cnt++;
Hagrass 3:598af964f16c 246 //wait(1);
Hagrass 3:598af964f16c 247 //xbee.printf("saved");
Hagrass 3:598af964f16c 248 }
Hagrass 3:598af964f16c 249 return 0;
Hagrass 3:598af964f16c 250 }
Hagrass 3:598af964f16c 251 //////////////////////////////////////////////////////
Hagrass 0:5f608863559a 252 DigitalOut led(LED1);
Hagrass 1:339ebc8786ca 253 void Init()
Hagrass 1:339ebc8786ca 254 {
Hagrass 1:339ebc8786ca 255 gps.baud(9600);
Hagrass 3:598af964f16c 256 debug.baud(115200);
Hagrass 1:339ebc8786ca 257
Hagrass 1:339ebc8786ca 258 xbee.printf("Init OK\n");
Hagrass 1:339ebc8786ca 259 printf("Init OK\n");
Hagrass 1:339ebc8786ca 260 }
Hagrass 0:5f608863559a 261
Hagrass 0:5f608863559a 262 char buffer[200];
Hagrass 1:339ebc8786ca 263 int e=6;
Hagrass 1:339ebc8786ca 264 int n=0;
Hagrass 0:5f608863559a 265 int main(){
Hagrass 3:598af964f16c 266 xbee.baud(115200);
Hagrass 3:598af964f16c 267 pc.baud(115200);
Hagrass 0:5f608863559a 268
Hagrass 3:598af964f16c 269 DEBMSG("Camera module");
Hagrass 3:598af964f16c 270 NEWLINE();
Hagrass 3:598af964f16c 271 DEBMSG("Resetting...");
Hagrass 3:598af964f16c 272 NEWLINE();
Hagrass 3:598af964f16c 273 wait(1);
Hagrass 3:598af964f16c 274
Hagrass 3:598af964f16c 275 if (cam1.reset() == 0) {
Hagrass 3:598af964f16c 276 DEBMSG("Reset OK.");
Hagrass 3:598af964f16c 277 NEWLINE();
Hagrass 3:598af964f16c 278 } else {
Hagrass 3:598af964f16c 279 DEBMSG("Reset fail.");
Hagrass 3:598af964f16c 280 NEWLINE();
Hagrass 3:598af964f16c 281 error("Reset fail.");
Hagrass 3:598af964f16c 282 }
Hagrass 3:598af964f16c 283
Hagrass 3:598af964f16c 284
Hagrass 3:598af964f16c 285 wait(1);
Hagrass 3:598af964f16c 286 cam1.baud();
Hagrass 3:598af964f16c 287 int j=1;
Hagrass 3:598af964f16c 288 int g=1;
Hagrass 1:339ebc8786ca 289 Init();
Hagrass 1:339ebc8786ca 290 char c;
Hagrass 1:339ebc8786ca 291 //debug.baud(115200);
Hagrass 0:5f608863559a 292
Hagrass 0:5f608863559a 293 if(imu.isReady()){
Hagrass 1:339ebc8786ca 294 xbee.printf("MPU9150 is ready\r\n");
Hagrass 1:339ebc8786ca 295 printf("MPU9150 is ready\r\n");
Hagrass 0:5f608863559a 296 } else {
Hagrass 1:339ebc8786ca 297 xbee.printf("MPU9150 initialisation failure\r\n");
Hagrass 1:339ebc8786ca 298 printf("MPU9150 initialisation failure\r\n");
Hagrass 0:5f608863559a 299 }
Hagrass 0:5f608863559a 300
Hagrass 0:5f608863559a 301 imu.initialiseDMP();
Hagrass 0:5f608863559a 302
Hagrass 0:5f608863559a 303 Timer timer;
Hagrass 0:5f608863559a 304 timer.start();
Hagrass 0:5f608863559a 305
Hagrass 0:5f608863559a 306 imu.setFifoReset(true);
Hagrass 0:5f608863559a 307 imu.setDMPEnabled(true);
Hagrass 0:5f608863559a 308
Hagrass 0:5f608863559a 309 Quaternion q1;
Hagrass 1:339ebc8786ca 310
Hagrass 0:5f608863559a 311 float hum,temp;
Hagrass 1:339ebc8786ca 312 // int g=10;
Hagrass 0:5f608863559a 313
Hagrass 0:5f608863559a 314 while(true){
Hagrass 3:598af964f16c 315 Timer timer1;
Hagrass 3:598af964f16c 316 timer1.start();
Hagrass 3:598af964f16c 317 // n++;
Hagrass 1:339ebc8786ca 318 // wait(0.5);
Hagrass 1:339ebc8786ca 319 bmp085.update();
Hagrass 1:339ebc8786ca 320 float allltitude=bmp085.calcAltitude(bmp085.get_pressure()*100);
Hagrass 0:5f608863559a 321 //wait(0.2);
Hagrass 3:598af964f16c 322 //if(e==6)
Hagrass 3:598af964f16c 323 //{
Hagrass 0:5f608863559a 324 dht22.sample() ;
Hagrass 0:5f608863559a 325 hum=dht22.getHumidity()/10.0;
Hagrass 1:339ebc8786ca 326 temp=dht22.getTemperature()/10.0;
Hagrass 0:5f608863559a 327
Hagrass 3:598af964f16c 328 //e=1;
Hagrass 3:598af964f16c 329 //}
Hagrass 1:339ebc8786ca 330
Hagrass 0:5f608863559a 331
Hagrass 0:5f608863559a 332 if(imu.getFifoCount() >= 48){
Hagrass 0:5f608863559a 333 imu.getFifoBuffer(buffer, 48);
Hagrass 0:5f608863559a 334 led = !led;
Hagrass 0:5f608863559a 335 }
Hagrass 0:5f608863559a 336 // debug.printf("vcvssgsgf");
Hagrass 0:5f608863559a 337 if(timer.read_ms() >50){
Hagrass 0:5f608863559a 338 timer.reset();
Hagrass 0:5f608863559a 339
Hagrass 0:5f608863559a 340 //This is the format of the data in the fifo,
Hagrass 0:5f608863559a 341 /* ================================================================================================ *
Hagrass 0:5f608863559a 342 | Default MotionApps v4.1 48-byte FIFO packet structure: |
Hagrass 0:5f608863559a 343 | |
Hagrass 0:5f608863559a 344 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
Hagrass 0:5f608863559a 345 | 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 346 | |
Hagrass 0:5f608863559a 347 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
Hagrass 0:5f608863559a 348 | 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 349 * ================================================================================================ */
Hagrass 0:5f608863559a 350
Hagrass 0:5f608863559a 351 /*
Hagrass 0:5f608863559a 352 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 353 (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
Hagrass 0:5f608863559a 354 (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
Hagrass 0:5f608863559a 355
Hagrass 0:5f608863559a 356 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 357 (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
Hagrass 0:5f608863559a 358 (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
Hagrass 0:5f608863559a 359
Hagrass 0:5f608863559a 360 debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28],
Hagrass 0:5f608863559a 361 (int16_t)(buffer[31] << 8) + buffer[30],
Hagrass 0:5f608863559a 362 (int16_t)(buffer[33] << 8) + buffer[32]);
Hagrass 0:5f608863559a 363
Hagrass 0:5f608863559a 364 debug.printf("%f, %f, %f, %f\r\n",
Hagrass 0:5f608863559a 365 (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 366 (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 367 (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 368 (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 369 */
Hagrass 0:5f608863559a 370
Hagrass 1:339ebc8786ca 371 q1.decode(buffer);
Hagrass 1:339ebc8786ca 372
Hagrass 1:339ebc8786ca 373 //wait(0.5);
Hagrass 0:5f608863559a 374
Hagrass 1:339ebc8786ca 375 if(gps.readable())
Hagrass 1:339ebc8786ca 376 {
Hagrass 1:339ebc8786ca 377 if(gps.getc() == '$'); // wait a $
Hagrass 1:339ebc8786ca 378 {
Hagrass 1:339ebc8786ca 379 for(int i=0; i<sizeof(cDataBuffer); i++)
Hagrass 1:339ebc8786ca 380 {
Hagrass 1:339ebc8786ca 381 c = gps.getc();
Hagrass 1:339ebc8786ca 382 if( c == '\r' )
Hagrass 1:339ebc8786ca 383 {
Hagrass 1:339ebc8786ca 384 //pc.printf("%s\n", cDataBuffer);
Hagrass 1:339ebc8786ca 385 parse(cDataBuffer, i);
Hagrass 1:339ebc8786ca 386 i = sizeof(cDataBuffer);
Hagrass 1:339ebc8786ca 387 }
Hagrass 1:339ebc8786ca 388 else
Hagrass 1:339ebc8786ca 389 {
Hagrass 1:339ebc8786ca 390 cDataBuffer[i] = c;
Hagrass 1:339ebc8786ca 391 }
Hagrass 1:339ebc8786ca 392 }
Hagrass 1:339ebc8786ca 393 }
Hagrass 1:339ebc8786ca 394 }
Hagrass 1:339ebc8786ca 395
Hagrass 1:339ebc8786ca 396
Hagrass 1:339ebc8786ca 397 if(n==10)
Hagrass 1:339ebc8786ca 398 {
Hagrass 1:339ebc8786ca 399 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 400
Hagrass 1:339ebc8786ca 401
Hagrass 1:339ebc8786ca 402 xbee.printf("p:%f hPa / t:%f / altitude=%f \n\r",bmp085.get_pressure() , bmp085.get_temperature(),allltitude);
Hagrass 1:339ebc8786ca 403
Hagrass 1:339ebc8786ca 404 xbee.printf("temp: %f , hum:%f \n\r",temp,hum);
Hagrass 1:339ebc8786ca 405
Hagrass 1:339ebc8786ca 406
Hagrass 1:339ebc8786ca 407 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 408 xbee.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
Hagrass 1:339ebc8786ca 409 xbee.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
Hagrass 1:339ebc8786ca 410 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 411 n=1;
Hagrass 1:339ebc8786ca 412
Hagrass 1:339ebc8786ca 413 }
Hagrass 1:339ebc8786ca 414
Hagrass 1:339ebc8786ca 415 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 416 printf("p:%f hPa / t:%f / altitude=%f \n\r",bmp085.get_pressure() , bmp085.get_temperature(),allltitude);
Hagrass 1:339ebc8786ca 417 printf("temp: %f , hum:%f \n\r",temp,hum);
Hagrass 1:339ebc8786ca 418 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 419 printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
Hagrass 1:339ebc8786ca 420 printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
Hagrass 1:339ebc8786ca 421 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 422
Hagrass 3:598af964f16c 423 char fname[64];
Hagrass 3:598af964f16c 424 // xbee.printf("hello%d\n\r",g);
Hagrass 3:598af964f16c 425
Hagrass 3:598af964f16c 426 if((end==true)||(j==1))
Hagrass 3:598af964f16c 427 {
Hagrass 3:598af964f16c 428
Hagrass 3:598af964f16c 429 snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
Hagrass 3:598af964f16c 430 j=0;
Hagrass 3:598af964f16c 431 }
Hagrass 3:598af964f16c 432 capture(&cam1, fname);
Hagrass 3:598af964f16c 433 //if (r == 0) {
Hagrass 3:598af964f16c 434
Hagrass 3:598af964f16c 435
Hagrass 3:598af964f16c 436 // }
Hagrass 3:598af964f16c 437 //} else {
Hagrass 3:598af964f16c 438 //DEBMSG("[%04d]:NG. (code=%d)", cnt, r);
Hagrass 3:598af964f16c 439 // NEWLINE();
Hagrass 3:598af964f16c 440 // error("Failure.");
Hagrass 3:598af964f16c 441 //}
Hagrass 3:598af964f16c 442 g++;
Hagrass 1:339ebc8786ca 443
Hagrass 1:339ebc8786ca 444
Hagrass 1:339ebc8786ca 445
Hagrass 1:339ebc8786ca 446 imu.setFifoReset(true);
Hagrass 1:339ebc8786ca 447 imu.setDMPEnabled(true);
Hagrass 0:5f608863559a 448 //TeaPot Demo Packet for MotionFit SDK
Hagrass 0:5f608863559a 449 /*
Hagrass 0:5f608863559a 450 debug.putc('$'); //packet start
Hagrass 0:5f608863559a 451 debug.putc((char)2); //assume packet type constant
Hagrass 0:5f608863559a 452 debug.putc((char)0); //count seems unused
Hagrass 0:5f608863559a 453 for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats
Hagrass 0:5f608863559a 454 debug.putc((char)(buffer[i]));
Hagrass 0:5f608863559a 455 }
Hagrass 0:5f608863559a 456 for(int i = 0; i < 5; i++){ //no idea, padded with 0
Hagrass 0:5f608863559a 457 debug.putc((char)0);
Hagrass 0:5f608863559a 458 }
Hagrass 0:5f608863559a 459 */
Hagrass 3:598af964f16c 460 }
Hagrass 3:598af964f16c 461
Hagrass 3:598af964f16c 462 while(1)
Hagrass 3:598af964f16c 463 {
Hagrass 3:598af964f16c 464 if(timer1.read_ms()>=2000)
Hagrass 3:598af964f16c 465 {
Hagrass 3:598af964f16c 466 timer1.reset();
Hagrass 3:598af964f16c 467 break;
Hagrass 3:598af964f16c 468 }
Hagrass 3:598af964f16c 469 }
Hagrass 3:598af964f16c 470
Hagrass 3:598af964f16c 471 //e++;
Hagrass 1:339ebc8786ca 472 //g++;
Hagrass 1:339ebc8786ca 473 //xbee.printf("this is the end");
Hagrass 1:339ebc8786ca 474 //rst=1;
Hagrass 1:339ebc8786ca 475 //wait(0.001);
Hagrass 1:339ebc8786ca 476 //rst=0;
Hagrass 0:5f608863559a 477 }
Hagrass 1:339ebc8786ca 478
Hagrass 0:5f608863559a 479 }
Hagrass 1:339ebc8786ca 480
Hagrass 1:339ebc8786ca 481
Hagrass 1:339ebc8786ca 482
Hagrass 1:339ebc8786ca 483
Hagrass 1:339ebc8786ca 484 void parse(char *cmd, int n)
Hagrass 1:339ebc8786ca 485 {
Hagrass 1:339ebc8786ca 486
Hagrass 1:339ebc8786ca 487
Hagrass 1:339ebc8786ca 488
Hagrass 1:339ebc8786ca 489
Hagrass 1:339ebc8786ca 490 // Global Positioning System Fix Data
Hagrass 1:339ebc8786ca 491 if(strncmp(cmd,"$GPGGA", 6) == 0)
Hagrass 1:339ebc8786ca 492 {
Hagrass 1:339ebc8786ca 493 sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
Hagrass 1:339ebc8786ca 494
Hagrass 1:339ebc8786ca 495 }
Hagrass 1:339ebc8786ca 496
Hagrass 1:339ebc8786ca 497 // Satellite status
Hagrass 1:339ebc8786ca 498 if(strncmp(cmd,"$GPGSA", 6) == 0)
Hagrass 1:339ebc8786ca 499 {
Hagrass 1:339ebc8786ca 500 sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
Hagrass 1:339ebc8786ca 501
Hagrass 1:339ebc8786ca 502 }
Hagrass 1:339ebc8786ca 503
Hagrass 1:339ebc8786ca 504 // Geographic position, Latitude and Longitude
Hagrass 1:339ebc8786ca 505 if(strncmp(cmd,"$GPGLL", 6) == 0)
Hagrass 1:339ebc8786ca 506 {
Hagrass 1:339ebc8786ca 507 sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
Hagrass 1:339ebc8786ca 508
Hagrass 1:339ebc8786ca 509 }
Hagrass 1:339ebc8786ca 510
Hagrass 1:339ebc8786ca 511 // Geographic position, Latitude and Longitude
Hagrass 1:339ebc8786ca 512 if(strncmp(cmd,"$GPRMC", 6) == 0)
Hagrass 1:339ebc8786ca 513 {
Hagrass 1:339ebc8786ca 514 sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
Hagrass 1:339ebc8786ca 515
Hagrass 1:339ebc8786ca 516 }
Hagrass 1:339ebc8786ca 517
Hagrass 1:339ebc8786ca 518 }