Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

main.cpp@23:497f8faa908e, 2014-04-22 (annotated)

Committer:
dylanembed123
Date:
Tue Apr 22 13:29:31 2014 +0000
Revision:
23:497f8faa908e
Parent:
22:9880a26886db
Child:
24:e65416d6de22
Arm motors and send take off command

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stearnsc 0:9c001c4e7bf4 1 #include "mbed.h"
stearnsc 0:9c001c4e7bf4 2 #include <string>
stearnsc 0:9c001c4e7bf4 3 #include <sstream>
dylanembed123 7:c75d5e5e6bfc 4 #include "adapt/usb.h"
dylanembed123 9:da906eeac51e 5 #include "handle/handleCamera.h"
dylanembed123 9:da906eeac51e 6 #include "handle/handleGPS.h"
dylanembed123 21:c546eab07e28 7 #include "mavcontrol.h"
stearnsc 8:28b866df62cf 8
stearnsc 0:9c001c4e7bf4 9 Serial pc(USBTX,USBRX);
stearnsc 0:9c001c4e7bf4 10 Serial xbee(p9,p10);//tx, rx
stearnsc 0:9c001c4e7bf4 11 Serial gps(p28,p27);
dylanembed123 6:434d20e99e49 12 Serial camera(p13,p14);
stearnsc 0:9c001c4e7bf4 13
stearnsc 0:9c001c4e7bf4 14 typedef struct {
stearnsc 0:9c001c4e7bf4 15 int latitude; //in .0001 minutes
stearnsc 0:9c001c4e7bf4 16 int longitude; //in .0001 minutes
stearnsc 0:9c001c4e7bf4 17 int altitude; //in decimeters
stearnsc 0:9c001c4e7bf4 18 int time; //in milliseconds
stearnsc 0:9c001c4e7bf4 19 } GpsData;
stearnsc 0:9c001c4e7bf4 20
stearnsc 8:28b866df62cf 21 void readSerial(Serial &s, char str[], int size)
stearnsc 8:28b866df62cf 22 {
stearnsc 8:28b866df62cf 23 for (int i = 0; i < size; i++) {
stearnsc 8:28b866df62cf 24 str[i] = s.getc();
stearnsc 0:9c001c4e7bf4 25 }
stearnsc 0:9c001c4e7bf4 26 }
stearnsc 0:9c001c4e7bf4 27
stearnsc 0:9c001c4e7bf4 28 //sends: "$<command>*<checksum>\r\l"
stearnsc 8:28b866df62cf 29 void sendGpsCommand(string command)
stearnsc 8:28b866df62cf 30 {
stearnsc 0:9c001c4e7bf4 31 uint8_t checksum = 0;
stearnsc 0:9c001c4e7bf4 32 pc.printf("Sending command to gps: ");
stearnsc 0:9c001c4e7bf4 33 gps.putc('$');
stearnsc 0:9c001c4e7bf4 34 pc.putc('$');
stearnsc 0:9c001c4e7bf4 35 char c;
stearnsc 8:28b866df62cf 36 for (int i = 0; i < command.length(); i++) {
stearnsc 0:9c001c4e7bf4 37 c = command[i];
stearnsc 0:9c001c4e7bf4 38 checksum ^= c;
stearnsc 0:9c001c4e7bf4 39 gps.putc(c);
stearnsc 0:9c001c4e7bf4 40 pc.putc(c);
stearnsc 0:9c001c4e7bf4 41 }
stearnsc 0:9c001c4e7bf4 42 gps.putc('*');
stearnsc 0:9c001c4e7bf4 43 pc.putc('*');
stearnsc 0:9c001c4e7bf4 44 string checkSumString;
stearnsc 8:28b866df62cf 45 while (checksum > 0) {
stearnsc 0:9c001c4e7bf4 46 uint8_t checksumChar = checksum & 0x0F;
stearnsc 8:28b866df62cf 47 if (checksumChar >= 10) {
stearnsc 0:9c001c4e7bf4 48 checksumChar -= 10;
stearnsc 8:28b866df62cf 49 checksumChar += 'A';
stearnsc 0:9c001c4e7bf4 50 } else {
stearnsc 8:28b866df62cf 51 checksumChar += '0';
stearnsc 0:9c001c4e7bf4 52 }
stearnsc 0:9c001c4e7bf4 53 checkSumString.push_back((char) checksumChar);
stearnsc 8:28b866df62cf 54 checksum = checksum >> 4;
stearnsc 0:9c001c4e7bf4 55 }
stearnsc 0:9c001c4e7bf4 56
stearnsc 8:28b866df62cf 57 for (int i = checkSumString.length() - 1; i >= 0; i--) {
stearnsc 0:9c001c4e7bf4 58 gps.putc(checkSumString[i]);
stearnsc 8:28b866df62cf 59 pc.putc(checkSumString[i]);
stearnsc 8:28b866df62cf 60 }
stearnsc 0:9c001c4e7bf4 61 gps.putc('\r');
stearnsc 0:9c001c4e7bf4 62 pc.putc('\r');
stearnsc 0:9c001c4e7bf4 63 gps.putc('\n');
stearnsc 0:9c001c4e7bf4 64 pc.putc('\n');
stearnsc 0:9c001c4e7bf4 65 }
stearnsc 8:28b866df62cf 66 //
stearnsc 8:28b866df62cf 67 ////cs: little endian parsing
stearnsc 8:28b866df62cf 68 //int nextInt(char *data, int i)
stearnsc 8:28b866df62cf 69 //{
stearnsc 8:28b866df62cf 70 // i |= data[i];
stearnsc 8:28b866df62cf 71 // i |= (data[i + 1] << 8);
stearnsc 8:28b866df62cf 72 // i |= (data[i + 2] << 16);
stearnsc 8:28b866df62cf 73 // i |= (data[i + 3] << 24);
stearnsc 8:28b866df62cf 74 // return i;
stearnsc 8:28b866df62cf 75 //}
stearnsc 0:9c001c4e7bf4 76
stearnsc 8:28b866df62cf 77 //void handleXbeeGps()
stearnsc 8:28b866df62cf 78 //{
stearnsc 8:28b866df62cf 79 // static bool reading = false;
stearnsc 8:28b866df62cf 80 // static char packet[16];
stearnsc 8:28b866df62cf 81 // static int i = 0;
stearnsc 8:28b866df62cf 82 //
stearnsc 8:28b866df62cf 83 // char c = xbee.getc();
stearnsc 8:28b866df62cf 84 // if (reading) {
stearnsc 8:28b866df62cf 85 // packet[i] = c;
stearnsc 8:28b866df62cf 86 // i++;
stearnsc 8:28b866df62cf 87 // if (i == 16) {
stearnsc 8:28b866df62cf 88 // i = 0;
stearnsc 8:28b866df62cf 89 // otherGps.latitude = nextInt(packet, 0);
stearnsc 8:28b866df62cf 90 // otherGps.longitude = nextInt(packet, 4);
stearnsc 8:28b866df62cf 91 // otherGps.altitude = nextInt(packet, 8);
stearnsc 8:28b866df62cf 92 // otherGps.time = nextInt(packet, 12);
stearnsc 8:28b866df62cf 93 //
stearnsc 8:28b866df62cf 94 // pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 8:28b866df62cf 95 // otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time
stearnsc 8:28b866df62cf 96 // );
stearnsc 8:28b866df62cf 97 // reading = false;
stearnsc 8:28b866df62cf 98 // }
stearnsc 8:28b866df62cf 99 // } else if (c == 'X') {
stearnsc 8:28b866df62cf 100 // reading = true;
stearnsc 8:28b866df62cf 101 // }
stearnsc 8:28b866df62cf 102 //}
dylanembed123 7:c75d5e5e6bfc 103
dylanembed123 21:c546eab07e28 104 typedef struct MAV_REQUEST_LIST_S{
dylanembed123 21:c546eab07e28 105 char targSys;
dylanembed123 21:c546eab07e28 106 char targComp;
dylanembed123 21:c546eab07e28 107 }MAV_REQUEST_LIST;
dylanembed123 21:c546eab07e28 108
dylanembed123 21:c546eab07e28 109 typedef struct MAV_COUNT_S{
dylanembed123 23:497f8faa908e 110 uint16_t count;
dylanembed123 21:c546eab07e28 111 char targSys;
dylanembed123 21:c546eab07e28 112 char targComp;
dylanembed123 21:c546eab07e28 113 }MAV_COUNT;
dylanembed123 21:c546eab07e28 114
dylanembed123 22:9880a26886db 115 typedef struct MAV_REQ_S{
dylanembed123 22:9880a26886db 116 char targSys;
dylanembed123 22:9880a26886db 117 char targComp;
dylanembed123 22:9880a26886db 118 char other;
dylanembed123 22:9880a26886db 119 char count;
dylanembed123 22:9880a26886db 120 }MAV_REQ;
dylanembed123 22:9880a26886db 121
dylanembed123 22:9880a26886db 122 typedef struct MAV_APM_S{
dylanembed123 21:c546eab07e28 123 float parm1;
dylanembed123 21:c546eab07e28 124 float parm2;
dylanembed123 21:c546eab07e28 125 float parm3;
dylanembed123 21:c546eab07e28 126 float parm4;
dylanembed123 21:c546eab07e28 127 float parm5;
dylanembed123 21:c546eab07e28 128 float parm6;
dylanembed123 21:c546eab07e28 129 float parm7;
dylanembed123 21:c546eab07e28 130 uint16_t cmd;
dylanembed123 21:c546eab07e28 131 uint8_t targSys;
dylanembed123 21:c546eab07e28 132 uint8_t targComp;
dylanembed123 21:c546eab07e28 133 uint8_t confirm;
dylanembed123 22:9880a26886db 134 } MAV_APM;
dylanembed123 22:9880a26886db 135
dylanembed123 22:9880a26886db 136 typedef struct MAV_MISSION_ITEM_S{
dylanembed123 22:9880a26886db 137 float parm1;
dylanembed123 22:9880a26886db 138 float parm2;
dylanembed123 22:9880a26886db 139 float parm3;
dylanembed123 22:9880a26886db 140 float parm4;
dylanembed123 22:9880a26886db 141 float lat;
dylanembed123 22:9880a26886db 142 float lon;
dylanembed123 22:9880a26886db 143 float alt;
dylanembed123 22:9880a26886db 144 uint16_t seq;
dylanembed123 22:9880a26886db 145 uint16_t cmd;
dylanembed123 22:9880a26886db 146 uint8_t targSys;
dylanembed123 22:9880a26886db 147 uint8_t targComp;
dylanembed123 22:9880a26886db 148 uint8_t frame;
dylanembed123 22:9880a26886db 149 uint8_t current; // set to true/1 to make current
dylanembed123 22:9880a26886db 150 uint8_t autoContinue;// set to true/1 to auto continue
dylanembed123 22:9880a26886db 151 uint8_t confirm;
dylanembed123 22:9880a26886db 152 } MAV_MISSION_ITEM;
dylanembed123 22:9880a26886db 153
dylanembed123 7:c75d5e5e6bfc 154
stearnsc 8:28b866df62cf 155 int main()
stearnsc 8:28b866df62cf 156 {
dylanembed123 23:497f8faa908e 157 USB::getSerial().printf("Wait 20\n");
dylanembed123 23:497f8faa908e 158 wait(20);
dylanembed123 23:497f8faa908e 159 /*
dylanembed123 21:c546eab07e28 160 char input[9];
dylanembed123 21:c546eab07e28 161 input[0]=0x00;
dylanembed123 21:c546eab07e28 162 input[1]=0x00;
dylanembed123 21:c546eab07e28 163 input[2]=0x00;
dylanembed123 21:c546eab07e28 164 input[3]=0x00;
dylanembed123 21:c546eab07e28 165 input[4]=0x02;
dylanembed123 21:c546eab07e28 166 input[5]=0x03;
dylanembed123 21:c546eab07e28 167 input[6]=0x51;
dylanembed123 21:c546eab07e28 168 input[7]=0x04;
dylanembed123 21:c546eab07e28 169 input[8]=0x03;
dylanembed123 21:c546eab07e28 170 /*
dylanembed123 21:c546eab07e28 171 while(true){
dylanembed123 21:c546eab07e28 172 char input[9];
dylanembed123 21:c546eab07e28 173 input[0]=0x00;
dylanembed123 21:c546eab07e28 174 input[1]=0x00;
dylanembed123 21:c546eab07e28 175 input[2]=0x00;
dylanembed123 21:c546eab07e28 176 input[3]=0x00;
dylanembed123 21:c546eab07e28 177 input[4]=0x02;
dylanembed123 21:c546eab07e28 178 input[5]=0x03;
dylanembed123 21:c546eab07e28 179 input[6]=0x51;
dylanembed123 21:c546eab07e28 180 input[7]=0x04;
dylanembed123 21:c546eab07e28 181 input[8]=0x03;
dylanembed123 21:c546eab07e28 182 Mav::sendOutput(0,input,9);
dylanembed123 21:c546eab07e28 183 wait_ms(50);
dylanembed123 21:c546eab07e28 184 }
dylanembed123 21:c546eab07e28 185 */
dylanembed123 21:c546eab07e28 186 //Mav::sendOutput(MAV_CMD_NAV_TAKEOFF,NULL,0);
dylanembed123 21:c546eab07e28 187 //Mav::sendOutput(MAVLINK_MSG_ID_LOITERU,NULL,0);
dylanembed123 21:c546eab07e28 188 //Mav::sendOutput(MAVLINK_MSG_ID_LOITERU,NULL,0);
dylanembed123 21:c546eab07e28 189 //Mav::sendOutput(MAV_CMD_NAV_LAND,NULL,0);
dylanembed123 21:c546eab07e28 190 //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,NULL,0);
dylanembed123 21:c546eab07e28 191 USB::getSerial().printf("System startup...\n");
dylanembed123 21:c546eab07e28 192 char nextCmd[512+1];
dylanembed123 21:c546eab07e28 193 int readIndex=0;
dylanembed123 21:c546eab07e28 194
dylanembed123 21:c546eab07e28 195 MAV_REQUEST_LIST req;req.targSys=1;req.targComp=1;
dylanembed123 23:497f8faa908e 196 // Issue arm motors
dylanembed123 23:497f8faa908e 197 MAV_APM issueArm;issueArm.targSys=1;issueArm.targComp=250;issueArm.cmd=400;issueArm.parm1=1.0f;
dylanembed123 23:497f8faa908e 198 // Issue disarm motors
dylanembed123 23:497f8faa908e 199 MAV_APM issueDisArm;issueDisArm.targSys=1;issueDisArm.targComp=250;issueDisArm.cmd=400;issueDisArm.parm1=0.0f;
dylanembed123 23:497f8faa908e 200 // Issue a mission count
dylanembed123 22:9880a26886db 201 MAV_COUNT issueCount;issueCount.targSys=1;issueCount.targComp=1;issueCount.count=1;
dylanembed123 23:497f8faa908e 202 // Issue a mission item
dylanembed123 22:9880a26886db 203 MAV_MISSION_ITEM issueItem;issueItem.targSys=1;issueItem.targComp=1;issueItem.lat=5.0f;issueItem.lon=6.0f;issueItem.alt=7.0f;issueItem.cmd=16;issueItem.seq=0;issueItem.current=1;
dylanembed123 23:497f8faa908e 204 // Issue a take off item
dylanembed123 23:497f8faa908e 205 MAV_MISSION_ITEM issueTakeOff;issueTakeOff.targSys=1;issueTakeOff.targComp=1;issueTakeOff.lat=5.0f;issueTakeOff.lon=6.0f;issueTakeOff.alt=7.0f;issueTakeOff.cmd=22;issueTakeOff.seq=0;issueTakeOff.current=1;
dylanembed123 23:497f8faa908e 206
dylanembed123 22:9880a26886db 207 bool firstRun=true;
dylanembed123 22:9880a26886db 208 bool gotNewRead=false;
dylanembed123 22:9880a26886db 209 int readState=0;
dylanembed123 22:9880a26886db 210 int realLen=0;
dylanembed123 21:c546eab07e28 211 while(true){
dylanembed123 22:9880a26886db 212 gotNewRead=false;
dylanembed123 21:c546eab07e28 213 if(Mav::getSerial().readable()>0){
dylanembed123 21:c546eab07e28 214 char input=Mav::getSerial().getc();
dylanembed123 22:9880a26886db 215 USB::getSerial().printf("> %x %d %d\n",input,readState,realLen);
dylanembed123 22:9880a26886db 216 if(readState==0&&input==0xFE){
dylanembed123 22:9880a26886db 217 readState=1;
dylanembed123 21:c546eab07e28 218 readIndex=1;
dylanembed123 22:9880a26886db 219 }else if(readState!=0){
dylanembed123 21:c546eab07e28 220 nextCmd[std::min(readIndex++,512)]=input;
dylanembed123 22:9880a26886db 221 if(readState==1){
dylanembed123 22:9880a26886db 222 realLen=input+5-1;
dylanembed123 22:9880a26886db 223 readState=2;
dylanembed123 22:9880a26886db 224 }
dylanembed123 22:9880a26886db 225 if(readState==2){
dylanembed123 22:9880a26886db 226 realLen--;
dylanembed123 22:9880a26886db 227 if(realLen==0){
dylanembed123 22:9880a26886db 228 readState=0;
dylanembed123 22:9880a26886db 229 gotNewRead=true;
dylanembed123 22:9880a26886db 230 }
dylanembed123 22:9880a26886db 231 }
dylanembed123 21:c546eab07e28 232 }
dylanembed123 21:c546eab07e28 233 }
dylanembed123 21:c546eab07e28 234 // Output debug info
dylanembed123 22:9880a26886db 235 if(gotNewRead){
dylanembed123 21:c546eab07e28 236 USB::getSerial().printf("Got CMD len %d messageid %d \n",nextCmd[1],nextCmd[5]);
dylanembed123 21:c546eab07e28 237 if(nextCmd[5]==0){
dylanembed123 21:c546eab07e28 238 //Mav::sendOutput(0,input,9);
dylanembed123 23:497f8faa908e 239 //Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM));
dylanembed123 21:c546eab07e28 240 //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
dylanembed123 22:9880a26886db 241 if(firstRun){
dylanembed123 22:9880a26886db 242 USB::getSerial().printf("Issue Command\n",nextCmd[1],nextCmd[5]);
dylanembed123 22:9880a26886db 243 Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
dylanembed123 22:9880a26886db 244 //Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
dylanembed123 22:9880a26886db 245 //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
dylanembed123 22:9880a26886db 246 }
dylanembed123 22:9880a26886db 247 firstRun=false;
dylanembed123 22:9880a26886db 248 }
dylanembed123 22:9880a26886db 249 if(nextCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
dylanembed123 22:9880a26886db 250 USB::getSerial().printf("Issue Item\n");
dylanembed123 22:9880a26886db 251 MAV_REQ* incnt=(MAV_REQ*)(&(nextCmd[6]));
dylanembed123 22:9880a26886db 252 issueItem.seq=nextCmd[6];//incnt->count;
dylanembed123 22:9880a26886db 253 USB::getSerial().printf("Got request for %d %d\n",nextCmd[9],incnt->count);
dylanembed123 22:9880a26886db 254 Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
dylanembed123 21:c546eab07e28 255 }
dylanembed123 23:497f8faa908e 256 if(nextCmd[5]==MAVLINK_MSG_ID_MISSION_ACK){
dylanembed123 23:497f8faa908e 257 // Arm motors
dylanembed123 23:497f8faa908e 258 Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM));
dylanembed123 23:497f8faa908e 259 // Take off
dylanembed123 23:497f8faa908e 260 Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueTakeOff,sizeof(MAV_MISSION_ITEM));
dylanembed123 23:497f8faa908e 261 }
dylanembed123 21:c546eab07e28 262 }
dylanembed123 21:c546eab07e28 263 }
dylanembed123 21:c546eab07e28 264
dylanembed123 21:c546eab07e28 265 int outLength;
dylanembed123 21:c546eab07e28 266 char* output=Mav::generatePacket(MAVLINK_MSG_ID_REQUEST_LIST,NULL,0,&outLength);
dylanembed123 21:c546eab07e28 267 for(int i=0;i<outLength;i++){
dylanembed123 21:c546eab07e28 268 Mav::getSerial().putc(output[i]);
dylanembed123 21:c546eab07e28 269 }
dylanembed123 21:c546eab07e28 270 while(1){}
dylanembed123 21:c546eab07e28 271
dylanembed123 14:6be57da62283 272 //getPS().sendPacket(0,NULL,0,PT_EMPTY);getPS().sendPacket(55,NULL,0,PT_IMAGE);
dylanembed123 15:e3e03a9df89e 273 //char output[256];for(int i=0;i<256;i++){output[i]=i;}getPS().sendPacket(0,NULL,0,PT_IMAGE);getPS().sendPacket(55,output,256);getPS().sendPacket(55,output,5);getPS().sendPacket(55,NULL,0,PT_END);
dylanembed123 15:e3e03a9df89e 274 //while(true){}
dylanembed123 15:e3e03a9df89e 275 /* PacketStruct* packs[4];
dylanembed123 14:6be57da62283 276 for(int i=0;i<4;i){
dylanembed123 14:6be57da62283 277 packs[i]=getPS().getNextPacket();
dylanembed123 14:6be57da62283 278 if(packs[i]!=NULL)i++;
dylanembed123 14:6be57da62283 279 }
dylanembed123 14:6be57da62283 280 for(int i=0;i<4;i++){
dylanembed123 14:6be57da62283 281 PacketStruct* pack=packs[i];
dylanembed123 14:6be57da62283 282 if(pack!=NULL){
dylanembed123 14:6be57da62283 283 USB::getSerial().printf("Got Packet!\n");
dylanembed123 14:6be57da62283 284 USB::getSerial().printf(" > %d\n",pack->type);
dylanembed123 14:6be57da62283 285 for(int i=0;i<sizeof(PacketStruct);i++){
dylanembed123 14:6be57da62283 286 USB::getSerial().printf("%d\n",((char*)pack)[i]);
dylanembed123 14:6be57da62283 287 }
dylanembed123 14:6be57da62283 288 USB::getSerial().printf("\n",pack->type);
dylanembed123 14:6be57da62283 289 }else{
dylanembed123 14:6be57da62283 290 //USB::getSerial().printf(".");
dylanembed123 14:6be57da62283 291 }
dylanembed123 14:6be57da62283 292 }
dylanembed123 14:6be57da62283 293 while(true){}
dylanembed123 14:6be57da62283 294 */
dylanembed123 13:a6d3cf2b018e 295 //getPS().sendPacket(55,NULL,0,PT_IMAGE);char output[256];for(int i=0;i<256;i++){output[i]=i;}getPS().sendPacket(55,output,256,PT_IMAGE);getPS().sendPacket(55,output,5,PT_IMAGE);getPS().sendPacket(55,NULL,0,PT_END);while(true){}
dylanembed123 7:c75d5e5e6bfc 296 ImageHandle imageHand;
dylanembed123 9:da906eeac51e 297 GPSHandle gpsHand;
dylanembed123 11:97625c27ab90 298 /// Main Loop
dylanembed123 13:a6d3cf2b018e 299 USB::getSerial().printf("Starting %d\n",sizeof(PacketStruct));
dylanembed123 13:a6d3cf2b018e 300 //while(1){
dylanembed123 13:a6d3cf2b018e 301 //USB::getSerial().printf("Test\n");
dylanembed123 13:a6d3cf2b018e 302 //XBEE::getSerial().printf("ABC\n");
dylanembed123 13:a6d3cf2b018e 303 //}
dylanembed123 14:6be57da62283 304 USB::getSerial().printf("Check GPS\n");
dylanembed123 11:97625c27ab90 305 while(1){
dylanembed123 11:97625c27ab90 306 // Run image handler
dylanembed123 15:e3e03a9df89e 307 USB::getSerial().printf("Check Image\n");
dylanembed123 15:e3e03a9df89e 308 imageHand.run();
dylanembed123 11:97625c27ab90 309 // Run GPS handler
dylanembed123 15:e3e03a9df89e 310 USB::getSerial().printf("Check GPS\n");
dylanembed123 14:6be57da62283 311 gpsHand.run();
dylanembed123 16:4f5d20b87dc3 312 USB::getSerial().printf("GPS Time: %f\n",DH::Locs().getC().getTime());
dylanembed123 15:e3e03a9df89e 313 // Read packet
dylanembed123 15:e3e03a9df89e 314 USB::getSerial().printf("Read Input\n");
dylanembed123 16:4f5d20b87dc3 315 //PacketStruct* pack=getPS().lastValid;//getPS().getNextPacket();
dylanembed123 16:4f5d20b87dc3 316 //if(pack!=NULL){
dylanembed123 16:4f5d20b87dc3 317 // USB::getSerial().printf("Received Type: %d\n",pack->type);
dylanembed123 16:4f5d20b87dc3 318 // if(pack->type==PT_REQLOC){
dylanembed123 16:4f5d20b87dc3 319 if(getPS().outputDevice.readable()>0){
dylanembed123 16:4f5d20b87dc3 320 char input=getPS().outputDevice.getc();
dylanembed123 16:4f5d20b87dc3 321 //if(getPS().outputDevice.getc()=='A'){
dylanembed123 15:e3e03a9df89e 322 // Send location
dylanembed123 15:e3e03a9df89e 323 unsigned int sID=getPS().getSuperID();
dylanembed123 15:e3e03a9df89e 324 getPS().sendPacket(0,NULL,0,PT_EMPTY);
dylanembed123 15:e3e03a9df89e 325 getPS().sendPacket(sID,NULL,0,PT_SENDLOC);
dylanembed123 15:e3e03a9df89e 326 getPS().sendPacket(sID,(char*)(&DH::Locs().getC().getLoc()),sizeof(DataLocation));
dylanembed123 15:e3e03a9df89e 327 getPS().sendPacket(sID,NULL,0,PT_END);
dylanembed123 16:4f5d20b87dc3 328 //}
dylanembed123 15:e3e03a9df89e 329 }
dylanembed123 11:97625c27ab90 330 }
dylanembed123 7:c75d5e5e6bfc 331 /// Main Loop
stearnsc 8:28b866df62cf 332 while(true) {
stearnsc 8:28b866df62cf 333 gps.baud(57600);
stearnsc 8:28b866df62cf 334 xbee.baud(9600);
stearnsc 8:28b866df62cf 335 pc.baud(57600);
stearnsc 8:28b866df62cf 336
stearnsc 8:28b866df62cf 337 sendGpsCommand("PMTK301,1");
stearnsc 8:28b866df62cf 338 while(true) {
stearnsc 8:28b866df62cf 339 pc.putc(gps.getc());
stearnsc 8:28b866df62cf 340 }
dylanembed123 9:da906eeac51e 341 //gps.attach(&handleGpsData, Serial::RxIrq);
dylanembed123 9:da906eeac51e 342 //xbee.attach(&handleXbeeGps, Serial::RxIrq)//;
dylanembed123 6:434d20e99e49 343 }
dylanembed123 9:da906eeac51e 344 }