IMU-pressure-tempreture sensors
Dependencies: CMSIS_DSP_401 DHT22 MPU9150_DMP QuaternionMath MODSERIAL mbed-src FATFileSystem111 SDFileSystem11 Camera_LS_Y201_CANSAT
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?
User | Revision | Line number | New 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 | } |