Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

main.cpp@66:5d43988d100c, 2014-05-05 (annotated)

Committer:
dylanembed123
Date:
Mon May 05 13:20:35 2014 +0000
Revision:
66:5d43988d100c
Parent:
64:d4818fb7813c 
Final Project;

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"
krobertson 44:2ae602b89961 10 #include "handle/handleCompass.h"
dylanembed123 33:ad63e7013801 11 #define DELAYBOOT 1
stearnsc 8:28b866df62cf 12
stearnsc 0:9c001c4e7bf4 13 Serial pc(USBTX,USBRX);
stearnsc 0:9c001c4e7bf4 14 Serial xbee(p9,p10);//tx, rx
stearnsc 0:9c001c4e7bf4 15 Serial gps(p28,p27);
dylanembed123 6:434d20e99e49 16 Serial camera(p13,p14);
stearnsc 0:9c001c4e7bf4 17
stearnsc 0:9c001c4e7bf4 18 typedef struct {
stearnsc 0:9c001c4e7bf4 19 int latitude; //in .0001 minutes
stearnsc 0:9c001c4e7bf4 20 int longitude; //in .0001 minutes
stearnsc 0:9c001c4e7bf4 21 int altitude; //in decimeters
stearnsc 0:9c001c4e7bf4 22 int time; //in milliseconds
stearnsc 0:9c001c4e7bf4 23 } GpsData;
stearnsc 0:9c001c4e7bf4 24
stearnsc 8:28b866df62cf 25 void readSerial(Serial &s, char str[], int size)
stearnsc 8:28b866df62cf 26 {
stearnsc 8:28b866df62cf 27 for (int i = 0; i < size; i++) {
stearnsc 8:28b866df62cf 28 str[i] = s.getc();
stearnsc 0:9c001c4e7bf4 29 }
stearnsc 0:9c001c4e7bf4 30 }
stearnsc 0:9c001c4e7bf4 31
krobertson 18:e72ee7aed088 32 void connection_lost(){
krobertson 18:e72ee7aed088 33 USB::getSerial().printf("TCP connection lost!\r\n");
krobertson 18:e72ee7aed088 34 }
krobertson 18:e72ee7aed088 35
stearnsc 0:9c001c4e7bf4 36 //sends: "$<command>*<checksum>\r\l"
stearnsc 8:28b866df62cf 37 void sendGpsCommand(string command)
stearnsc 8:28b866df62cf 38 {
stearnsc 0:9c001c4e7bf4 39 uint8_t checksum = 0;
stearnsc 0:9c001c4e7bf4 40 pc.printf("Sending command to gps: ");
stearnsc 0:9c001c4e7bf4 41 gps.putc('$');
stearnsc 0:9c001c4e7bf4 42 pc.putc('$');
stearnsc 0:9c001c4e7bf4 43 char c;
stearnsc 8:28b866df62cf 44 for (int i = 0; i < command.length(); i++) {
stearnsc 0:9c001c4e7bf4 45 c = command[i];
stearnsc 0:9c001c4e7bf4 46 checksum ^= c;
stearnsc 0:9c001c4e7bf4 47 gps.putc(c);
stearnsc 0:9c001c4e7bf4 48 pc.putc(c);
stearnsc 0:9c001c4e7bf4 49 }
stearnsc 0:9c001c4e7bf4 50 gps.putc('*');
stearnsc 0:9c001c4e7bf4 51 pc.putc('*');
stearnsc 0:9c001c4e7bf4 52 string checkSumString;
stearnsc 8:28b866df62cf 53 while (checksum > 0) {
stearnsc 0:9c001c4e7bf4 54 uint8_t checksumChar = checksum & 0x0F;
stearnsc 8:28b866df62cf 55 if (checksumChar >= 10) {
stearnsc 0:9c001c4e7bf4 56 checksumChar -= 10;
stearnsc 8:28b866df62cf 57 checksumChar += 'A';
stearnsc 0:9c001c4e7bf4 58 } else {
stearnsc 8:28b866df62cf 59 checksumChar += '0';
stearnsc 0:9c001c4e7bf4 60 }
stearnsc 0:9c001c4e7bf4 61 checkSumString.push_back((char) checksumChar);
stearnsc 8:28b866df62cf 62 checksum = checksum >> 4;
stearnsc 0:9c001c4e7bf4 63 }
stearnsc 0:9c001c4e7bf4 64
stearnsc 8:28b866df62cf 65 for (int i = checkSumString.length() - 1; i >= 0; i--) {
stearnsc 0:9c001c4e7bf4 66 gps.putc(checkSumString[i]);
stearnsc 8:28b866df62cf 67 pc.putc(checkSumString[i]);
stearnsc 8:28b866df62cf 68 }
stearnsc 0:9c001c4e7bf4 69 gps.putc('\r');
stearnsc 0:9c001c4e7bf4 70 pc.putc('\r');
stearnsc 0:9c001c4e7bf4 71 gps.putc('\n');
stearnsc 0:9c001c4e7bf4 72 pc.putc('\n');
stearnsc 0:9c001c4e7bf4 73 }
stearnsc 8:28b866df62cf 74 //
stearnsc 8:28b866df62cf 75 ////cs: little endian parsing
stearnsc 8:28b866df62cf 76 //int nextInt(char *data, int i)
stearnsc 8:28b866df62cf 77 //{
stearnsc 8:28b866df62cf 78 // i |= data[i];
stearnsc 8:28b866df62cf 79 // i |= (data[i + 1] << 8);
stearnsc 8:28b866df62cf 80 // i |= (data[i + 2] << 16);
stearnsc 8:28b866df62cf 81 // i |= (data[i + 3] << 24);
stearnsc 8:28b866df62cf 82 // return i;
stearnsc 8:28b866df62cf 83 //}
stearnsc 0:9c001c4e7bf4 84
stearnsc 8:28b866df62cf 85 //void handleXbeeGps()
stearnsc 8:28b866df62cf 86 //{
stearnsc 8:28b866df62cf 87 // static bool reading = false;
stearnsc 8:28b866df62cf 88 // static char packet[16];
stearnsc 8:28b866df62cf 89 // static int i = 0;
stearnsc 8:28b866df62cf 90 //
stearnsc 8:28b866df62cf 91 // char c = xbee.getc();
stearnsc 8:28b866df62cf 92 // if (reading) {
stearnsc 8:28b866df62cf 93 // packet[i] = c;
stearnsc 8:28b866df62cf 94 // i++;
stearnsc 8:28b866df62cf 95 // if (i == 16) {
stearnsc 8:28b866df62cf 96 // i = 0;
stearnsc 8:28b866df62cf 97 // otherGps.latitude = nextInt(packet, 0);
stearnsc 8:28b866df62cf 98 // otherGps.longitude = nextInt(packet, 4);
stearnsc 8:28b866df62cf 99 // otherGps.altitude = nextInt(packet, 8);
stearnsc 8:28b866df62cf 100 // otherGps.time = nextInt(packet, 12);
stearnsc 8:28b866df62cf 101 //
stearnsc 8:28b866df62cf 102 // pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 8:28b866df62cf 103 // otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time
stearnsc 8:28b866df62cf 104 // );
stearnsc 8:28b866df62cf 105 // reading = false;
stearnsc 8:28b866df62cf 106 // }
stearnsc 8:28b866df62cf 107 // } else if (c == 'X') {
stearnsc 8:28b866df62cf 108 // reading = true;
stearnsc 8:28b866df62cf 109 // }
stearnsc 8:28b866df62cf 110 //}
dylanembed123 7:c75d5e5e6bfc 111
stearnsc 8:28b866df62cf 112 int main()
stearnsc 8:28b866df62cf 113 {
dylanembed123 40:7b4d6043f533 114
dylanembed123 24:e65416d6de22 115 // Start Mav test
dylanembed123 64:d4818fb7813c 116 //DH::Locs().add(LHType_targ,DataLocation(27.175015,78.042155,5.0));
dylanembed123 52:b4dddb28dffa 117 USB::getSerial().printf("Wait %d seconds\n",DELAYBOOT);
dylanembed123 33:ad63e7013801 118 wait(DELAYBOOT);
dylanembed123 64:d4818fb7813c 119 //while(true){
dylanembed123 64:d4818fb7813c 120 // USB::getSerial().printf(".\n",DELAYBOOT);
dylanembed123 64:d4818fb7813c 121 // MavCmd::get().run();
dylanembed123 66:5d43988d100c 122 // compassHandle::getCompassHand().run();
dylanembed123 64:d4818fb7813c 123 // wait(5);
dylanembed123 64:d4818fb7813c 124 //}
krobertson 20:81d5655fecc2 125
dylanembed123 13:a6d3cf2b018e 126 USB::getSerial().printf("Starting %d\n",sizeof(PacketStruct));
dylanembed123 14:6be57da62283 127 USB::getSerial().printf("Check GPS\n");
krobertson 20:81d5655fecc2 128 USB::getSerial().printf("Connect to the wifly network now!\r\n");
krobertson 20:81d5655fecc2 129 //XBEE::getTCPInterrupt().fall(&connection_lost);
krobertson 20:81d5655fecc2 130 //checking connection to egg before continuing
krobertson 19:8c1f2a2204fb 131 getPS().openConnection();
krobertson 19:8c1f2a2204fb 132 getPS().closeConnection();
krobertson 20:81d5655fecc2 133 //Main Loop
krobertson 41:df156ae5631b 134 int count = 0;
krobertson 19:8c1f2a2204fb 135 while(1){
krobertson 49:06721139d298 136 //USB::getSerial().printf("Running GPS...\r\n");
krobertson 58:ea73523cf04b 137 //GPSHandle::getGPSHand().run();
krobertson 58:ea73523cf04b 138 MavCmd::get().run();
krobertson 49:06721139d298 139 //USB::getSerial().printf("Running Compass...\r\n");
krobertson 44:2ae602b89961 140 compassHandle::getCompassHand().run();
krobertson 58:ea73523cf04b 141 if(count % 10 == 0){
krobertson 49:06721139d298 142 USB::getSerial().printf("Requesting commands from egg...\r\n");
krobertson 41:df156ae5631b 143 wait_us(100000);
krobertson 41:df156ae5631b 144 CommandHandle::getCommandHand().run();
krobertson 41:df156ae5631b 145 wait_us(100000);
krobertson 58:ea73523cf04b 146 }
krobertson 58:ea73523cf04b 147 if(GPSHandle::getGPSHand().if_image_location()){
krobertson 58:ea73523cf04b 148 USB::getSerial().printf("Taking picture and sending...\r\n");
krobertson 58:ea73523cf04b 149 wait_us(100000);
krobertson 58:ea73523cf04b 150 ImageHandle::getImageHand().run();
krobertson 58:ea73523cf04b 151 USB::getSerial().printf("sent all data\r\n");
krobertson 58:ea73523cf04b 152 wait_us(100000);
krobertson 58:ea73523cf04b 153 GPSHandle::getGPSHand().next_waypoint();
krobertson 58:ea73523cf04b 154 wait_us(100000);
krobertson 58:ea73523cf04b 155 }else{
krobertson 58:ea73523cf04b 156 //USB::getSerial().printf("Not close enough to waypoint for image\r\n");
dylanembed123 21:c546eab07e28 157 }
krobertson 41:df156ae5631b 158 count++;
dylanembed123 11:97625c27ab90 159 }
dylanembed123 9:da906eeac51e 160 }