Colin Stearns / Mbed 2 deprecated qcControl

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of dgps by Colin Stearns

main.cpp.orig@24:e65416d6de22, 2014-04-22 (annotated)

Committer:
dylanembed123
Date:
Tue Apr 22 14:15:27 2014 +0000
Revision:
24:e65416d6de22
Abstract mav commands and merge

Who changed what in which revision?

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