test program for SIM808 GPRS + JPEG Camera library for LinkSprite LS-Y201 camera +MPU9150
Dependencies: JPEGCamera MPU9150_DMP_Nucleo QuaternionMath mbed
Fork of JPEGCamera_SIM808_STM32F401RE by
main.cpp@5:a4bb144ba3b7, 2017-07-01 (annotated)
- Committer:
- tgw
- Date:
- Sat Jul 01 06:33:25 2017 +0000
- Revision:
- 5:a4bb144ba3b7
- Parent:
- 4:c4c42e7c2298
gps
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
thesane | 0:0c9cdf074ba1 | 1 | |
thesane | 0:0c9cdf074ba1 | 2 | #include "mbed.h" |
tgw | 3:f307a059affe | 3 | #include <string> |
tgw | 3:f307a059affe | 4 | |
thesane | 0:0c9cdf074ba1 | 5 | #include "JPEGCamera.h" |
tgw | 4:c4c42e7c2298 | 6 | #include "MPU9150.h" |
tgw | 4:c4c42e7c2298 | 7 | #include "Quaternion.h" |
thesane | 0:0c9cdf074ba1 | 8 | |
tgw | 4:c4c42e7c2298 | 9 | #define buf_max 256 |
tgw | 4:c4c42e7c2298 | 10 | |
tgw | 4:c4c42e7c2298 | 11 | #define sys_idle 0 |
tgw | 4:c4c42e7c2298 | 12 | #define sys_gprs 1 |
tgw | 4:c4c42e7c2298 | 13 | #define sys_camera 2 |
tgw | 4:c4c42e7c2298 | 14 | #define sys_mpu9150 3 |
tgw | 5:a4bb144ba3b7 | 15 | #define sys_gps 4 |
tgw | 5:a4bb144ba3b7 | 16 | #define sys_ble 5 |
tgw | 3:f307a059affe | 17 | |
tgw | 3:f307a059affe | 18 | Serial pc(SERIAL_TX, SERIAL_RX); // SERIAL_TX(D1), SERIAL_RX(D0) read from bluetooth module VERIFIED |
tgw | 3:f307a059affe | 19 | Serial sim808(PA_9, PA_10); // TX(D8), RX(D2) to camera |
tgw | 3:f307a059affe | 20 | JPEGCamera camera(PA_11, PA_12); // TX6, RX6 to camera |
tgw | 3:f307a059affe | 21 | |
tgw | 4:c4c42e7c2298 | 22 | MPU9150 imu(D15, D14, D7); //scl sda int |
tgw | 3:f307a059affe | 23 | //I2C_SCL--PB_8(D15) |
tgw | 3:f307a059affe | 24 | //I2C_SDA--PB_9(D14) |
tgw | 3:f307a059affe | 25 | |
tgw | 3:f307a059affe | 26 | DigitalIn mybutton(USER_BUTTON);//PC_13 |
tgw | 4:c4c42e7c2298 | 27 | DigitalOut led(LED1);//PA_5(D13) |
tgw | 3:f307a059affe | 28 | DigitalOut sim_power(D4);//PB_5 //复位SIM808 |
thesane | 0:0c9cdf074ba1 | 29 | |
thesane | 0:0c9cdf074ba1 | 30 | //Camera time |
thesane | 0:0c9cdf074ba1 | 31 | Timer timer; |
thesane | 0:0c9cdf074ba1 | 32 | |
tgw | 4:c4c42e7c2298 | 33 | int sys_id=0; |
tgw | 3:f307a059affe | 34 | int rec_cnt = 0; //receive count |
tgw | 3:f307a059affe | 35 | char result[buf_max]; |
tgw | 3:f307a059affe | 36 | |
tgw | 4:c4c42e7c2298 | 37 | //mpu9150 |
tgw | 4:c4c42e7c2298 | 38 | Quaternion q1; |
tgw | 4:c4c42e7c2298 | 39 | char buffer[200]; |
tgw | 4:c4c42e7c2298 | 40 | |
tgw | 3:f307a059affe | 41 | void sim_callback() |
tgw | 3:f307a059affe | 42 | { |
tgw | 3:f307a059affe | 43 | char x; |
tgw | 3:f307a059affe | 44 | x = sim808.getc(); |
tgw | 3:f307a059affe | 45 | result[rec_cnt++]= x; |
tgw | 3:f307a059affe | 46 | pc.putc(x); |
tgw | 3:f307a059affe | 47 | if(rec_cnt>=buf_max-1)rec_cnt=0; |
tgw | 3:f307a059affe | 48 | } |
tgw | 4:c4c42e7c2298 | 49 | bool sim_wait(int x) |
tgw | 3:f307a059affe | 50 | { |
tgw | 4:c4c42e7c2298 | 51 | char *p; |
tgw | 4:c4c42e7c2298 | 52 | rec_cnt = 0; |
tgw | 3:f307a059affe | 53 | memset(result,0,buf_max*sizeof(char)); |
tgw | 3:f307a059affe | 54 | while(x) |
tgw | 4:c4c42e7c2298 | 55 | { |
tgw | 4:c4c42e7c2298 | 56 | wait(1); |
tgw | 4:c4c42e7c2298 | 57 | p=strstr(result,"OK"); |
tgw | 4:c4c42e7c2298 | 58 | if(p) |
tgw | 4:c4c42e7c2298 | 59 | { |
tgw | 4:c4c42e7c2298 | 60 | return true; |
tgw | 4:c4c42e7c2298 | 61 | } |
tgw | 4:c4c42e7c2298 | 62 | x--; |
tgw | 4:c4c42e7c2298 | 63 | } |
tgw | 4:c4c42e7c2298 | 64 | return false; |
tgw | 3:f307a059affe | 65 | } |
tgw | 3:f307a059affe | 66 | |
tgw | 3:f307a059affe | 67 | void sim_gprs_updata(string s) |
tgw | 3:f307a059affe | 68 | { |
tgw | 3:f307a059affe | 69 | pc.printf("%s",s); //发送内容 |
tgw | 3:f307a059affe | 70 | pc.putc(0x1a);//CtrlZ |
tgw | 3:f307a059affe | 71 | |
tgw | 3:f307a059affe | 72 | sim808.printf("%s",s); //发送内容 |
tgw | 3:f307a059affe | 73 | sim808.putc(0x1a); |
tgw | 4:c4c42e7c2298 | 74 | |
tgw | 4:c4c42e7c2298 | 75 | wait(1); |
tgw | 4:c4c42e7c2298 | 76 | // if(sim_wait(1)) { |
tgw | 3:f307a059affe | 77 | pc.printf("TCP send OK\r\n"); |
tgw | 4:c4c42e7c2298 | 78 | // } else { |
tgw | 4:c4c42e7c2298 | 79 | // pc.printf("TCP send Field\r\n"); |
tgw | 4:c4c42e7c2298 | 80 | // } |
tgw | 3:f307a059affe | 81 | } |
tgw | 3:f307a059affe | 82 | |
tgw | 3:f307a059affe | 83 | void sim_gprs_init(void) |
tgw | 3:f307a059affe | 84 | { |
tgw | 3:f307a059affe | 85 | |
tgw | 3:f307a059affe | 86 | pc.printf("AT\r\n"); |
tgw | 3:f307a059affe | 87 | sim808.printf("AT\r\n"); |
tgw | 3:f307a059affe | 88 | |
tgw | 4:c4c42e7c2298 | 89 | if(sim_wait(1)) { |
tgw | 3:f307a059affe | 90 | pc.printf("SIM AT OK.\r\n");// |
tgw | 3:f307a059affe | 91 | } |
tgw | 3:f307a059affe | 92 | else{ |
tgw | 3:f307a059affe | 93 | pc.printf("Reset SIM808.\r\n");// |
tgw | 3:f307a059affe | 94 | sim_power.write(0); |
tgw | 3:f307a059affe | 95 | wait(3); |
tgw | 3:f307a059affe | 96 | sim_power.write(1); |
tgw | 3:f307a059affe | 97 | wait(19); |
tgw | 3:f307a059affe | 98 | } |
tgw | 3:f307a059affe | 99 | |
tgw | 3:f307a059affe | 100 | pc.printf("ATE0\r\n");//关闭回显 |
tgw | 3:f307a059affe | 101 | sim808.printf("ATE0\r\n");//关闭回显 |
tgw | 3:f307a059affe | 102 | wait(1); |
tgw | 3:f307a059affe | 103 | |
tgw | 3:f307a059affe | 104 | pc.printf("AT+CPIN?\r\n"); |
tgw | 3:f307a059affe | 105 | sim808.printf("AT+CPIN?\r\n");//SIM卡状态 |
tgw | 4:c4c42e7c2298 | 106 | if(sim_wait(1)) { |
tgw | 3:f307a059affe | 107 | pc.printf("SIM CARD CPIN OK\r\n"); |
tgw | 3:f307a059affe | 108 | } else { |
tgw | 3:f307a059affe | 109 | pc.printf("SIM CARD CPIN Field\r\n"); |
tgw | 3:f307a059affe | 110 | } |
tgw | 3:f307a059affe | 111 | |
tgw | 3:f307a059affe | 112 | pc.printf("AT+CSQ\r\n"); |
tgw | 3:f307a059affe | 113 | sim808.printf("AT+CSQ\r\n");//信号状态 |
tgw | 4:c4c42e7c2298 | 114 | if(sim_wait(1)) { |
tgw | 3:f307a059affe | 115 | pc.printf("SIM CARD signal OK\r\n"); |
tgw | 3:f307a059affe | 116 | } else { |
tgw | 3:f307a059affe | 117 | pc.printf("SIM CARD signal Field\r\n"); |
tgw | 3:f307a059affe | 118 | } |
tgw | 3:f307a059affe | 119 | |
tgw | 3:f307a059affe | 120 | pc.printf("AT+CREG?\r\n"); //判断SIM卡网络是否注册成功 |
tgw | 3:f307a059affe | 121 | sim808.printf("AT+CREG?\r\n"); //判断SIM卡网络是否注册成功 |
tgw | 4:c4c42e7c2298 | 122 | if(sim_wait(1)) { |
tgw | 3:f307a059affe | 123 | pc.printf("SIM CARD REG OK\r\n"); |
tgw | 3:f307a059affe | 124 | } else { |
tgw | 3:f307a059affe | 125 | pc.printf("SIM CARD REG Field\r\n"); |
tgw | 3:f307a059affe | 126 | } |
tgw | 3:f307a059affe | 127 | |
tgw | 3:f307a059affe | 128 | pc.printf("AT+CGATT?\r\n"); //GPRS附着状态 |
tgw | 3:f307a059affe | 129 | sim808.printf("AT+CGATT?\r\n"); //GPRS附着状态 |
tgw | 3:f307a059affe | 130 | |
tgw | 4:c4c42e7c2298 | 131 | if(sim_wait(1)) { |
tgw | 3:f307a059affe | 132 | pc.printf("SIM CGATT OK\r\n"); |
tgw | 3:f307a059affe | 133 | } else { |
tgw | 3:f307a059affe | 134 | pc.printf("SIM CGATT Field\r\n"); |
tgw | 3:f307a059affe | 135 | } |
tgw | 4:c4c42e7c2298 | 136 | pc.printf("Waiting for sim808 start ......\r\n"); |
tgw | 4:c4c42e7c2298 | 137 | wait(15);//等待模块启动完成 |
tgw | 3:f307a059affe | 138 | //-----------------------组合命令------------------------------------------------------------ |
tgw | 3:f307a059affe | 139 | |
tgw | 3:f307a059affe | 140 | pc.printf("AT+CSTT=\"CMNET\"\r\n"); //set APN |
tgw | 3:f307a059affe | 141 | sim808.printf("AT+CSTT=\"CMNET\"\r\n"); //set APN |
tgw | 3:f307a059affe | 142 | |
tgw | 4:c4c42e7c2298 | 143 | if(sim_wait(5)) { |
tgw | 3:f307a059affe | 144 | pc.printf("SIM set APN OK\r\n"); |
tgw | 3:f307a059affe | 145 | } |
tgw | 3:f307a059affe | 146 | else |
tgw | 3:f307a059affe | 147 | { |
tgw | 3:f307a059affe | 148 | pc.printf("SIM set APN Field\r\n"); |
tgw | 3:f307a059affe | 149 | return ; |
tgw | 3:f307a059affe | 150 | } |
tgw | 3:f307a059affe | 151 | |
tgw | 3:f307a059affe | 152 | pc.printf("AT+CIICR\r\n"); //wireless link 建立无线链路 |
tgw | 3:f307a059affe | 153 | sim808.printf("AT+CIICR\r\n"); //wireless link 建立无线链路 |
tgw | 3:f307a059affe | 154 | |
tgw | 4:c4c42e7c2298 | 155 | if(sim_wait(5)) { |
tgw | 3:f307a059affe | 156 | pc.printf("SIM wireless link OK\r\n"); |
tgw | 3:f307a059affe | 157 | } else { |
tgw | 3:f307a059affe | 158 | pc.printf("SIM wireless link Field\r\n"); |
tgw | 3:f307a059affe | 159 | } |
tgw | 4:c4c42e7c2298 | 160 | |
tgw | 4:c4c42e7c2298 | 161 | pc.printf("Waiting for sim808 wireless link finish ......\r\n"); |
tgw | 3:f307a059affe | 162 | wait(15);//等待建立无线链路 |
tgw | 3:f307a059affe | 163 | |
tgw | 3:f307a059affe | 164 | pc.printf("AT+CIFSR\r\n"); //获取本地IP |
tgw | 3:f307a059affe | 165 | sim808.printf("AT+CIFSR\r\n"); //获取本地IP |
tgw | 3:f307a059affe | 166 | |
tgw | 3:f307a059affe | 167 | pc.printf("AT+CIPSTART=\"TCP\",\"122.5.17.146\",\"9980\"\r\n"); //TCP set 建立TCP连接 |
tgw | 3:f307a059affe | 168 | sim808.printf("AT+CIPSTART=\"TCP\",\"122.5.17.146\",\"9980\"\r\n"); //TCP set 建立TCP连接 |
tgw | 3:f307a059affe | 169 | |
tgw | 4:c4c42e7c2298 | 170 | if(sim_wait(5)) |
tgw | 3:f307a059affe | 171 | { |
tgw | 3:f307a059affe | 172 | pc.printf("SIM TCP set OK\r\n"); |
tgw | 3:f307a059affe | 173 | |
tgw | 4:c4c42e7c2298 | 174 | pc.printf("Waiting for sim808 tcp link finish ......\r\n"); |
tgw | 3:f307a059affe | 175 | wait(10);//等待建立TCP链路 |
tgw | 3:f307a059affe | 176 | |
tgw | 3:f307a059affe | 177 | pc.printf("AT+CIPSEND\r\n"); //send direction |
tgw | 3:f307a059affe | 178 | sim808.printf("AT+CIPSEND\r\n"); //send direction |
tgw | 3:f307a059affe | 179 | |
tgw | 3:f307a059affe | 180 | wait(1); |
tgw | 3:f307a059affe | 181 | sim_gprs_updata("TPC SEND:hello,sim808 tcp test ok!"); |
tgw | 3:f307a059affe | 182 | } else |
tgw | 3:f307a059affe | 183 | { |
tgw | 3:f307a059affe | 184 | pc.printf("SIM TCP set Field\r\n"); |
tgw | 3:f307a059affe | 185 | } |
tgw | 3:f307a059affe | 186 | |
tgw | 3:f307a059affe | 187 | } |
tgw | 3:f307a059affe | 188 | void sim_gprs_relink(void) |
tgw | 3:f307a059affe | 189 | { |
tgw | 3:f307a059affe | 190 | pc.printf("AT+CIPCLOSE\r\n"); |
tgw | 3:f307a059affe | 191 | sim808.printf("AT+CIPCLOSE\r\n");//关闭连接 |
tgw | 4:c4c42e7c2298 | 192 | if(sim_wait(5)) |
tgw | 3:f307a059affe | 193 | { |
tgw | 3:f307a059affe | 194 | pc.printf("SIM TCP CLOSE OK\r\n"); |
tgw | 3:f307a059affe | 195 | } else |
tgw | 3:f307a059affe | 196 | { |
tgw | 3:f307a059affe | 197 | pc.printf("SIM TCP CLOSE Field\r\n"); |
tgw | 3:f307a059affe | 198 | } |
tgw | 3:f307a059affe | 199 | pc.printf("AT+CIPSTART=\"TCP\",\"122.5.17.146\",\"9980\"\r\n"); //TCP set 建立TCP连接 |
tgw | 4:c4c42e7c2298 | 200 | sim808.printf("AT+CIPSTART=\"TCP\",\"122.5.17.146\",\"9980\"\r\n"); //TCP set 建立TCP连接 |
tgw | 3:f307a059affe | 201 | |
tgw | 4:c4c42e7c2298 | 202 | if(sim_wait(5)) |
tgw | 4:c4c42e7c2298 | 203 | { |
tgw | 4:c4c42e7c2298 | 204 | pc.printf("SIM TCP set OK\r\n"); |
tgw | 3:f307a059affe | 205 | |
tgw | 4:c4c42e7c2298 | 206 | pc.printf("Waiting for sim808 tcp link finish ......\r\n"); |
tgw | 4:c4c42e7c2298 | 207 | wait(10);//等待建立TCP链路 |
tgw | 3:f307a059affe | 208 | |
tgw | 3:f307a059affe | 209 | pc.printf("AT+CIPSEND\r\n"); //send direction |
tgw | 3:f307a059affe | 210 | sim808.printf("AT+CIPSEND\r\n"); //send direction |
tgw | 3:f307a059affe | 211 | |
tgw | 3:f307a059affe | 212 | wait(1); |
tgw | 3:f307a059affe | 213 | sim_gprs_updata("TPC SEND:hello,sim808 tcp test ok!"); |
tgw | 3:f307a059affe | 214 | } else |
tgw | 3:f307a059affe | 215 | { |
tgw | 3:f307a059affe | 216 | pc.printf("SIM TCP set Field\r\n"); |
tgw | 3:f307a059affe | 217 | pc.printf("Reset SIM808.\r\n");// |
tgw | 3:f307a059affe | 218 | sim_power.write(0); |
tgw | 3:f307a059affe | 219 | wait(3); |
tgw | 3:f307a059affe | 220 | sim_power.write(1); |
tgw | 3:f307a059affe | 221 | wait(19); |
tgw | 3:f307a059affe | 222 | sim_gprs_init(); |
tgw | 3:f307a059affe | 223 | } |
tgw | 3:f307a059affe | 224 | } |
tgw | 3:f307a059affe | 225 | |
tgw | 4:c4c42e7c2298 | 226 | void pro_gprs(void) |
thesane | 0:0c9cdf074ba1 | 227 | { |
tgw | 4:c4c42e7c2298 | 228 | if (!mybutton) |
tgw | 4:c4c42e7c2298 | 229 | { |
tgw | 4:c4c42e7c2298 | 230 | // time_t seconds = time(NULL); |
tgw | 4:c4c42e7c2298 | 231 | // //pc.printf("%s", ctime(&seconds)); |
tgw | 4:c4c42e7c2298 | 232 | // char msg[100]; |
tgw | 4:c4c42e7c2298 | 233 | // sprintf(msg,"seconds:%d", ctime(&seconds)); |
tgw | 4:c4c42e7c2298 | 234 | // sim_gprs_updata(msg); |
tgw | 4:c4c42e7c2298 | 235 | sim_gprs_relink(); |
tgw | 4:c4c42e7c2298 | 236 | } |
tgw | 4:c4c42e7c2298 | 237 | } |
tgw | 4:c4c42e7c2298 | 238 | void pro_camera(void) |
tgw | 4:c4c42e7c2298 | 239 | { |
tgw | 4:c4c42e7c2298 | 240 | wait(0.1); |
tgw | 4:c4c42e7c2298 | 241 | if (!mybutton) |
tgw | 2:9ad661c031a4 | 242 | { |
thesane | 0:0c9cdf074ba1 | 243 | if (camera.isReady()) |
thesane | 0:0c9cdf074ba1 | 244 | { |
thesane | 0:0c9cdf074ba1 | 245 | if (camera.takePicture()) |
thesane | 0:0c9cdf074ba1 | 246 | { |
thesane | 0:0c9cdf074ba1 | 247 | int image_size = camera.getImageSize(); |
thesane | 0:0c9cdf074ba1 | 248 | while (camera.isProcessing()) |
thesane | 0:0c9cdf074ba1 | 249 | { |
tgw | 3:f307a059affe | 250 | camera.processPicture(pc); |
thesane | 0:0c9cdf074ba1 | 251 | } |
thesane | 0:0c9cdf074ba1 | 252 | } |
thesane | 0:0c9cdf074ba1 | 253 | else |
thesane | 0:0c9cdf074ba1 | 254 | { |
tgw | 3:f307a059affe | 255 | pc.printf("camera takePicture fail!\r\n"); |
thesane | 0:0c9cdf074ba1 | 256 | } |
thesane | 0:0c9cdf074ba1 | 257 | } |
thesane | 0:0c9cdf074ba1 | 258 | else |
thesane | 0:0c9cdf074ba1 | 259 | { |
tgw | 3:f307a059affe | 260 | pc.printf("camera isReady fail!\r\n"); |
thesane | 0:0c9cdf074ba1 | 261 | } |
tgw | 4:c4c42e7c2298 | 262 | } |
tgw | 4:c4c42e7c2298 | 263 | } |
tgw | 4:c4c42e7c2298 | 264 | |
tgw | 4:c4c42e7c2298 | 265 | void pro_mpu9150(void) |
tgw | 4:c4c42e7c2298 | 266 | { |
tgw | 4:c4c42e7c2298 | 267 | if(imu.getFifoCount() >= 48) |
tgw | 4:c4c42e7c2298 | 268 | { |
tgw | 4:c4c42e7c2298 | 269 | imu.getFifoBuffer(buffer, 48); |
tgw | 4:c4c42e7c2298 | 270 | led = !led; |
tgw | 4:c4c42e7c2298 | 271 | } |
tgw | 4:c4c42e7c2298 | 272 | |
tgw | 4:c4c42e7c2298 | 273 | if(timer.read_ms() > 50) |
tgw | 4:c4c42e7c2298 | 274 | { |
tgw | 4:c4c42e7c2298 | 275 | timer.reset(); |
tgw | 4:c4c42e7c2298 | 276 | |
tgw | 4:c4c42e7c2298 | 277 | //This is the format of the data in the fifo, |
tgw | 4:c4c42e7c2298 | 278 | /* ================================================================================================ * |
tgw | 4:c4c42e7c2298 | 279 | | Default MotionApps v4.1 48-byte FIFO packet structure: | |
tgw | 4:c4c42e7c2298 | 280 | | | |
tgw | 4:c4c42e7c2298 | 281 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | |
tgw | 4:c4c42e7c2298 | 282 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | |
tgw | 4:c4c42e7c2298 | 283 | | | |
tgw | 4:c4c42e7c2298 | 284 | | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | |
tgw | 4:c4c42e7c2298 | 285 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | |
tgw | 4:c4c42e7c2298 | 286 | * ================================================================================================ */ |
tgw | 4:c4c42e7c2298 | 287 | |
tgw | 4:c4c42e7c2298 | 288 | |
tgw | 4:c4c42e7c2298 | 289 | pc.printf("acc :%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]), |
tgw | 4:c4c42e7c2298 | 290 | (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]), |
tgw | 4:c4c42e7c2298 | 291 | (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45])); |
tgw | 4:c4c42e7c2298 | 292 | |
tgw | 4:c4c42e7c2298 | 293 | pc.printf("gyro:%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]), |
tgw | 4:c4c42e7c2298 | 294 | (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]), |
tgw | 4:c4c42e7c2298 | 295 | (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27])); |
tgw | 4:c4c42e7c2298 | 296 | |
tgw | 4:c4c42e7c2298 | 297 | pc.printf("mag :%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28], |
tgw | 4:c4c42e7c2298 | 298 | (int16_t)(buffer[31] << 8) + buffer[30], |
tgw | 4:c4c42e7c2298 | 299 | (int16_t)(buffer[33] << 8) + buffer[32]); |
tgw | 4:c4c42e7c2298 | 300 | |
tgw | 4:c4c42e7c2298 | 301 | pc.printf("quat:%f, %f, %f, %f\r\n", |
tgw | 4:c4c42e7c2298 | 302 | (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)), |
tgw | 4:c4c42e7c2298 | 303 | (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)), |
tgw | 4:c4c42e7c2298 | 304 | (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)), |
tgw | 4:c4c42e7c2298 | 305 | (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30))); |
tgw | 4:c4c42e7c2298 | 306 | |
tgw | 4:c4c42e7c2298 | 307 | |
tgw | 4:c4c42e7c2298 | 308 | //q1.decode(buffer); |
tgw | 4:c4c42e7c2298 | 309 | //pc.printf("%f, %f, %f, %f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z); |
tgw | 4:c4c42e7c2298 | 310 | |
tgw | 4:c4c42e7c2298 | 311 | |
tgw | 4:c4c42e7c2298 | 312 | //TeaPot Demo Packet for MotionFit SDK |
tgw | 4:c4c42e7c2298 | 313 | /* |
tgw | 4:c4c42e7c2298 | 314 | pc.putc('$'); //packet start |
tgw | 4:c4c42e7c2298 | 315 | pc.putc((char)2); //assume packet type constant |
tgw | 4:c4c42e7c2298 | 316 | pc.putc((char)0); //count seems unused |
tgw | 4:c4c42e7c2298 | 317 | for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats |
tgw | 4:c4c42e7c2298 | 318 | pc.putc((char)(buffer[i])); |
tgw | 4:c4c42e7c2298 | 319 | } |
tgw | 4:c4c42e7c2298 | 320 | for(int i = 0; i < 5; i++){ //no idea, padded with 0 |
tgw | 4:c4c42e7c2298 | 321 | pc.putc((char)0); |
tgw | 4:c4c42e7c2298 | 322 | } |
tgw | 4:c4c42e7c2298 | 323 | */ |
tgw | 4:c4c42e7c2298 | 324 | } |
tgw | 4:c4c42e7c2298 | 325 | } |
tgw | 4:c4c42e7c2298 | 326 | int main() |
tgw | 4:c4c42e7c2298 | 327 | { |
tgw | 4:c4c42e7c2298 | 328 | int message = 0; |
tgw | 4:c4c42e7c2298 | 329 | |
tgw | 4:c4c42e7c2298 | 330 | pc.baud(115200); |
tgw | 4:c4c42e7c2298 | 331 | sim808.baud(115200); |
tgw | 4:c4c42e7c2298 | 332 | sim808.attach(&sim_callback,SerialBase::RxIrq); //serial interrupt attach |
tgw | 4:c4c42e7c2298 | 333 | |
tgw | 4:c4c42e7c2298 | 334 | timer.start(); |
tgw | 4:c4c42e7c2298 | 335 | |
tgw | 4:c4c42e7c2298 | 336 | //Sim808 Initialization |
tgw | 4:c4c42e7c2298 | 337 | sim_power.write(1); |
tgw | 4:c4c42e7c2298 | 338 | sim_gprs_init(); |
tgw | 4:c4c42e7c2298 | 339 | |
tgw | 4:c4c42e7c2298 | 340 | //Camera Initialization and picture sizing |
tgw | 4:c4c42e7c2298 | 341 | if(camera.setPictureSize(JPEGCamera::SIZE160x120))pc.printf("camera setPictureSize ok!\r\n"); |
tgw | 4:c4c42e7c2298 | 342 | else pc.printf("camera setPictureSize fail!\r\n"); |
tgw | 4:c4c42e7c2298 | 343 | wait(3); |
tgw | 4:c4c42e7c2298 | 344 | |
tgw | 4:c4c42e7c2298 | 345 | //MPU9150 Initialization and setting |
tgw | 4:c4c42e7c2298 | 346 | if(imu.isReady())pc.printf("MPU9150 is ready\r\n"); |
tgw | 4:c4c42e7c2298 | 347 | else pc.printf("MPU9150 initialisation failure\r\n"); |
tgw | 4:c4c42e7c2298 | 348 | |
tgw | 4:c4c42e7c2298 | 349 | imu.initialiseDMP(); |
tgw | 4:c4c42e7c2298 | 350 | |
tgw | 4:c4c42e7c2298 | 351 | imu.setFifoReset(true); |
tgw | 4:c4c42e7c2298 | 352 | imu.setDMPEnabled(true); |
tgw | 4:c4c42e7c2298 | 353 | |
tgw | 4:c4c42e7c2298 | 354 | //Continuously wait for commands |
tgw | 4:c4c42e7c2298 | 355 | while(1) |
tgw | 4:c4c42e7c2298 | 356 | { |
tgw | 5:a4bb144ba3b7 | 357 | //-------------------------E事件处理----------------------------------------------- |
tgw | 4:c4c42e7c2298 | 358 | //Check for bytes over serial from bluetooth/xbee |
tgw | 4:c4c42e7c2298 | 359 | if(pc.readable()) |
tgw | 4:c4c42e7c2298 | 360 | { |
tgw | 4:c4c42e7c2298 | 361 | message=pc.getc(); |
tgw | 4:c4c42e7c2298 | 362 | //Take a picture (note that this takes a while to complete and will only only work for 999 pictures |
tgw | 4:c4c42e7c2298 | 363 | if (message==0xd1) |
tgw | 4:c4c42e7c2298 | 364 | { |
tgw | 4:c4c42e7c2298 | 365 | sys_id=sys_gprs; |
tgw | 4:c4c42e7c2298 | 366 | pc.printf("sys_id is gprs\r\n"); |
tgw | 4:c4c42e7c2298 | 367 | } |
tgw | 4:c4c42e7c2298 | 368 | else if (message==0xd2) |
tgw | 4:c4c42e7c2298 | 369 | { |
tgw | 4:c4c42e7c2298 | 370 | sys_id=sys_camera; |
tgw | 4:c4c42e7c2298 | 371 | pc.printf("sys_id is camera\r\n"); |
tgw | 4:c4c42e7c2298 | 372 | } |
tgw | 4:c4c42e7c2298 | 373 | else if (message==0xd3) |
tgw | 4:c4c42e7c2298 | 374 | { |
tgw | 4:c4c42e7c2298 | 375 | sys_id=sys_mpu9150; |
tgw | 4:c4c42e7c2298 | 376 | pc.printf("sys_id is MPU9150\r\n"); |
thesane | 0:0c9cdf074ba1 | 377 | } |
thesane | 0:0c9cdf074ba1 | 378 | } |
tgw | 5:a4bb144ba3b7 | 379 | //-------------------------I事件处理----------------------------------------------- |
tgw | 3:f307a059affe | 380 | |
tgw | 4:c4c42e7c2298 | 381 | //-------------------------运行线程----------------------------------------------- |
tgw | 4:c4c42e7c2298 | 382 | switch(sys_id) |
tgw | 3:f307a059affe | 383 | { |
tgw | 4:c4c42e7c2298 | 384 | case sys_gprs: |
tgw | 4:c4c42e7c2298 | 385 | pro_gprs(); |
tgw | 4:c4c42e7c2298 | 386 | break; |
tgw | 4:c4c42e7c2298 | 387 | case sys_camera: |
tgw | 4:c4c42e7c2298 | 388 | pro_camera(); |
tgw | 4:c4c42e7c2298 | 389 | break; |
tgw | 4:c4c42e7c2298 | 390 | case sys_mpu9150: |
tgw | 4:c4c42e7c2298 | 391 | pro_mpu9150(); |
tgw | 4:c4c42e7c2298 | 392 | break; |
tgw | 5:a4bb144ba3b7 | 393 | case sys_gps: |
tgw | 5:a4bb144ba3b7 | 394 | break; |
tgw | 5:a4bb144ba3b7 | 395 | case sys_ble: |
tgw | 5:a4bb144ba3b7 | 396 | break; |
tgw | 4:c4c42e7c2298 | 397 | default: |
tgw | 4:c4c42e7c2298 | 398 | break; |
tgw | 3:f307a059affe | 399 | } |
thesane | 0:0c9cdf074ba1 | 400 | } |
thesane | 0:0c9cdf074ba1 | 401 | } |
tgw | 3:f307a059affe | 402 |