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@4:dbb8e901826d, 2015-09-09 (annotated)

Committer:
Hagrass
Date:
Wed Sep 09 14:38:04 2015 +0000
Revision:
4:dbb8e901826d
Parent:
1:339ebc8786ca 
cansat 3;

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