Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

main.cpp@35:a6177e5ca00c, 2014-04-23 (annotated)

Committer:
dylanembed123
Date:
Wed Apr 23 01:54:11 2014 +0000
Revision:
35:a6177e5ca00c
Parent:
34:c0b13ce5408c
Child:
39:1acea80563cf
Child:
41:df156ae5631b
Read last commit...

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"
krobertson 20:81d5655fecc2 7 #include "handle/handleCommand.h"
dylanembed123 26:06f1c9d70e9f 8 #include "handle/mavcommands.h"
dylanembed123 33:ad63e7013801 9 #include "handle/dataLocation.h"
dylanembed123 33:ad63e7013801 10 #define DELAYBOOT 1
stearnsc 8:28b866df62cf 11
stearnsc 0:9c001c4e7bf4 12 Serial pc(USBTX,USBRX);
stearnsc 0:9c001c4e7bf4 13 Serial xbee(p9,p10);//tx, rx
stearnsc 0:9c001c4e7bf4 14 Serial gps(p28,p27);
dylanembed123 6:434d20e99e49 15 Serial camera(p13,p14);
stearnsc 0:9c001c4e7bf4 16
stearnsc 0:9c001c4e7bf4 17 typedef struct {
stearnsc 0:9c001c4e7bf4 18 int latitude; //in .0001 minutes
stearnsc 0:9c001c4e7bf4 19 int longitude; //in .0001 minutes
stearnsc 0:9c001c4e7bf4 20 int altitude; //in decimeters
stearnsc 0:9c001c4e7bf4 21 int time; //in milliseconds
stearnsc 0:9c001c4e7bf4 22 } GpsData;
stearnsc 0:9c001c4e7bf4 23
stearnsc 8:28b866df62cf 24 void readSerial(Serial &s, char str[], int size)
stearnsc 8:28b866df62cf 25 {
stearnsc 8:28b866df62cf 26 for (int i = 0; i < size; i++) {
stearnsc 8:28b866df62cf 27 str[i] = s.getc();
stearnsc 0:9c001c4e7bf4 28 }
stearnsc 0:9c001c4e7bf4 29 }
stearnsc 0:9c001c4e7bf4 30
krobertson 18:e72ee7aed088 31 void connection_lost(){
krobertson 18:e72ee7aed088 32 USB::getSerial().printf("TCP connection lost!\r\n");
krobertson 18:e72ee7aed088 33 }
krobertson 18:e72ee7aed088 34
stearnsc 0:9c001c4e7bf4 35 //sends: "$<command>*<checksum>\r\l"
stearnsc 8:28b866df62cf 36 void sendGpsCommand(string command)
stearnsc 8:28b866df62cf 37 {
stearnsc 0:9c001c4e7bf4 38 uint8_t checksum = 0;
stearnsc 0:9c001c4e7bf4 39 pc.printf("Sending command to gps: ");
stearnsc 0:9c001c4e7bf4 40 gps.putc('$');
stearnsc 0:9c001c4e7bf4 41 pc.putc('$');
stearnsc 0:9c001c4e7bf4 42 char c;
stearnsc 8:28b866df62cf 43 for (int i = 0; i < command.length(); i++) {
stearnsc 0:9c001c4e7bf4 44 c = command[i];
stearnsc 0:9c001c4e7bf4 45 checksum ^= c;
stearnsc 0:9c001c4e7bf4 46 gps.putc(c);
stearnsc 0:9c001c4e7bf4 47 pc.putc(c);
stearnsc 0:9c001c4e7bf4 48 }
stearnsc 0:9c001c4e7bf4 49 gps.putc('*');
stearnsc 0:9c001c4e7bf4 50 pc.putc('*');
stearnsc 0:9c001c4e7bf4 51 string checkSumString;
stearnsc 8:28b866df62cf 52 while (checksum > 0) {
stearnsc 0:9c001c4e7bf4 53 uint8_t checksumChar = checksum & 0x0F;
stearnsc 8:28b866df62cf 54 if (checksumChar >= 10) {
stearnsc 0:9c001c4e7bf4 55 checksumChar -= 10;
stearnsc 8:28b866df62cf 56 checksumChar += 'A';
stearnsc 0:9c001c4e7bf4 57 } else {
stearnsc 8:28b866df62cf 58 checksumChar += '0';
stearnsc 0:9c001c4e7bf4 59 }
stearnsc 0:9c001c4e7bf4 60 checkSumString.push_back((char) checksumChar);
stearnsc 8:28b866df62cf 61 checksum = checksum >> 4;
stearnsc 0:9c001c4e7bf4 62 }
stearnsc 0:9c001c4e7bf4 63
stearnsc 8:28b866df62cf 64 for (int i = checkSumString.length() - 1; i >= 0; i--) {
stearnsc 0:9c001c4e7bf4 65 gps.putc(checkSumString[i]);
stearnsc 8:28b866df62cf 66 pc.putc(checkSumString[i]);
stearnsc 8:28b866df62cf 67 }
stearnsc 0:9c001c4e7bf4 68 gps.putc('\r');
stearnsc 0:9c001c4e7bf4 69 pc.putc('\r');
stearnsc 0:9c001c4e7bf4 70 gps.putc('\n');
stearnsc 0:9c001c4e7bf4 71 pc.putc('\n');
stearnsc 0:9c001c4e7bf4 72 }
stearnsc 8:28b866df62cf 73 //
stearnsc 8:28b866df62cf 74 ////cs: little endian parsing
stearnsc 8:28b866df62cf 75 //int nextInt(char *data, int i)
stearnsc 8:28b866df62cf 76 //{
stearnsc 8:28b866df62cf 77 // i |= data[i];
stearnsc 8:28b866df62cf 78 // i |= (data[i + 1] << 8);
stearnsc 8:28b866df62cf 79 // i |= (data[i + 2] << 16);
stearnsc 8:28b866df62cf 80 // i |= (data[i + 3] << 24);
stearnsc 8:28b866df62cf 81 // return i;
stearnsc 8:28b866df62cf 82 //}
stearnsc 0:9c001c4e7bf4 83
stearnsc 8:28b866df62cf 84 //void handleXbeeGps()
stearnsc 8:28b866df62cf 85 //{
stearnsc 8:28b866df62cf 86 // static bool reading = false;
stearnsc 8:28b866df62cf 87 // static char packet[16];
stearnsc 8:28b866df62cf 88 // static int i = 0;
stearnsc 8:28b866df62cf 89 //
stearnsc 8:28b866df62cf 90 // char c = xbee.getc();
stearnsc 8:28b866df62cf 91 // if (reading) {
stearnsc 8:28b866df62cf 92 // packet[i] = c;
stearnsc 8:28b866df62cf 93 // i++;
stearnsc 8:28b866df62cf 94 // if (i == 16) {
stearnsc 8:28b866df62cf 95 // i = 0;
stearnsc 8:28b866df62cf 96 // otherGps.latitude = nextInt(packet, 0);
stearnsc 8:28b866df62cf 97 // otherGps.longitude = nextInt(packet, 4);
stearnsc 8:28b866df62cf 98 // otherGps.altitude = nextInt(packet, 8);
stearnsc 8:28b866df62cf 99 // otherGps.time = nextInt(packet, 12);
stearnsc 8:28b866df62cf 100 //
stearnsc 8:28b866df62cf 101 // pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 8:28b866df62cf 102 // otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time
stearnsc 8:28b866df62cf 103 // );
stearnsc 8:28b866df62cf 104 // reading = false;
stearnsc 8:28b866df62cf 105 // }
stearnsc 8:28b866df62cf 106 // } else if (c == 'X') {
stearnsc 8:28b866df62cf 107 // reading = true;
stearnsc 8:28b866df62cf 108 // }
stearnsc 8:28b866df62cf 109 //}
dylanembed123 7:c75d5e5e6bfc 110
stearnsc 8:28b866df62cf 111 int main()
stearnsc 8:28b866df62cf 112 {
dylanembed123 24:e65416d6de22 113 // Start Mav test
dylanembed123 35:a6177e5ca00c 114 /*
dylanembed123 23:497f8faa908e 115 USB::getSerial().printf("Wait 20\n");
dylanembed123 33:ad63e7013801 116 wait(DELAYBOOT);
dylanembed123 25:b7f861fc8ddd 117 while(true){
dylanembed123 25:b7f861fc8ddd 118 MavCmd::get().run();
dylanembed123 25:b7f861fc8ddd 119 }
dylanembed123 35:a6177e5ca00c 120 */
dylanembed123 24:e65416d6de22 121 // End mav test
dylanembed123 21:c546eab07e28 122
krobertson 20:81d5655fecc2 123 //handlers
krobertson 20:81d5655fecc2 124 //ImageHandle imageHand;
krobertson 20:81d5655fecc2 125 //GPSHandle gpsHand;
krobertson 20:81d5655fecc2 126 //CommandHandle commHand;
krobertson 20:81d5655fecc2 127
dylanembed123 13:a6d3cf2b018e 128 USB::getSerial().printf("Starting %d\n",sizeof(PacketStruct));
dylanembed123 14:6be57da62283 129 USB::getSerial().printf("Check GPS\n");
krobertson 20:81d5655fecc2 130 USB::getSerial().printf("Connect to the wifly network now!\r\n");
krobertson 20:81d5655fecc2 131 //XBEE::getTCPInterrupt().fall(&connection_lost);
krobertson 20:81d5655fecc2 132 //checking connection to egg before continuing
krobertson 19:8c1f2a2204fb 133 getPS().openConnection();
krobertson 19:8c1f2a2204fb 134 getPS().closeConnection();
krobertson 20:81d5655fecc2 135 //Main Loop
krobertson 19:8c1f2a2204fb 136 while(1){
krobertson 32:9cb7bc3fc9e0 137 GPSHandle::getGPSHand().run();
krobertson 20:81d5655fecc2 138 USB::getSerial().printf("Requesting commands from egg...\r\n");
krobertson 20:81d5655fecc2 139 wait_us(100000);
krobertson 20:81d5655fecc2 140 CommandHandle::getCommandHand().run();
krobertson 20:81d5655fecc2 141 wait_us(100000);
krobertson 20:81d5655fecc2 142 if(GPSHandle::getGPSHand().if_image_location()){
krobertson 20:81d5655fecc2 143 USB::getSerial().printf("Taking picture and sending...\r\n");
krobertson 20:81d5655fecc2 144 wait_us(100000);
krobertson 20:81d5655fecc2 145 ImageHandle::getImageHand().run();
krobertson 20:81d5655fecc2 146 USB::getSerial().printf("sent all data\r\n");
krobertson 20:81d5655fecc2 147 wait_us(100000);
krobertson 32:9cb7bc3fc9e0 148 GPSHandle::getGPSHand().next_waypoint();
krobertson 32:9cb7bc3fc9e0 149 }else{
krobertson 32:9cb7bc3fc9e0 150 USB::getSerial().printf("Not close enough to waypoint for image\r\n");
dylanembed123 21:c546eab07e28 151 }
krobertson 20:81d5655fecc2 152 wait_us(1000000);
krobertson 20:81d5655fecc2 153 //}
krobertson 18:e72ee7aed088 154
dylanembed123 11:97625c27ab90 155 // Run image handler
krobertson 20:81d5655fecc2 156 //USB::getSerial().printf("Check Image\n");
krobertson 20:81d5655fecc2 157 // imageHand.run();
dylanembed123 11:97625c27ab90 158 // Run GPS handler
krobertson 20:81d5655fecc2 159 // USB::getSerial().printf("Check GPS\n");
krobertson 20:81d5655fecc2 160 //gpsHand.run();
krobertson 20:81d5655fecc2 161 //USB::getSerial().printf("GPS Time: %f\n",DH::Locs().getC().getTime());
dylanembed123 15:e3e03a9df89e 162 // Read packet
krobertson 20:81d5655fecc2 163 //USB::getSerial().printf("Read Input\n");
dylanembed123 16:4f5d20b87dc3 164 //PacketStruct* pack=getPS().lastValid;//getPS().getNextPacket();
dylanembed123 16:4f5d20b87dc3 165 //if(pack!=NULL){
dylanembed123 16:4f5d20b87dc3 166 // USB::getSerial().printf("Received Type: %d\n",pack->type);
dylanembed123 16:4f5d20b87dc3 167 // if(pack->type==PT_REQLOC){
krobertson 20:81d5655fecc2 168 //if(getPS().outputDevice.readable()>0){
krobertson 20:81d5655fecc2 169 // char input=getPS().outputDevice.getc();
krobertson 20:81d5655fecc2 170 // //if(getPS().outputDevice.getc()=='A'){
krobertson 20:81d5655fecc2 171 // // Send location
krobertson 20:81d5655fecc2 172 // unsigned int sID=getPS().getSuperID();
krobertson 20:81d5655fecc2 173 // getPS().sendPacket(0,NULL,0,PT_EMPTY);
krobertson 20:81d5655fecc2 174 // getPS().sendPacket(sID,NULL,0,PT_SENDLOC);
krobertson 20:81d5655fecc2 175 // getPS().sendPacket(sID,(char*)(&DH::Locs().getC().getLoc()),sizeof(DataLocation));
krobertson 20:81d5655fecc2 176 // getPS().sendPacket(sID,NULL,0,PT_END);
krobertson 20:81d5655fecc2 177 // //}
krobertson 20:81d5655fecc2 178 // }
dylanembed123 11:97625c27ab90 179 }
dylanembed123 7:c75d5e5e6bfc 180 /// Main Loop
krobertson 20:81d5655fecc2 181 // while(true) {
krobertson 20:81d5655fecc2 182 // gps.baud(57600);
krobertson 20:81d5655fecc2 183 // xbee.baud(9600);
krobertson 20:81d5655fecc2 184 // pc.baud(57600);
krobertson 20:81d5655fecc2 185 //
krobertson 20:81d5655fecc2 186 // sendGpsCommand("PMTK301,1");
krobertson 20:81d5655fecc2 187 // while(true) {
krobertson 20:81d5655fecc2 188 // pc.putc(gps.getc());
krobertson 20:81d5655fecc2 189 // }
dylanembed123 9:da906eeac51e 190 //gps.attach(&handleGpsData, Serial::RxIrq);
dylanembed123 9:da906eeac51e 191 //xbee.attach(&handleXbeeGps, Serial::RxIrq)//;
krobertson 20:81d5655fecc2 192 //}
dylanembed123 9:da906eeac51e 193 }