Agra-GPS / FreePilot_V2-3

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:   FreePilot PinDetect mbed-src

Fork of FreePilot_V2-2 by Agra-GPS

base/base.cpp@42:854d8cc26bbb, 2015-03-09 (annotated)

Committer:
maximbolduc
Date:
Mon Mar 09 15:58:28 2015 +0000
Revision:
42:854d8cc26bbb
Parent:
41:a26acd346c2f
Child:
46:d7d6dc429153
scale fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jhedmonton 28:5905886c76ee 1 #include "mbed.h"
jhedmonton 28:5905886c76ee 2 #include "MODSERIAL.h"
jhedmonton 28:5905886c76ee 3 #include <string>
jhedmonton 28:5905886c76ee 4 #include "base.h"
jhedmonton 28:5905886c76ee 5 #include "Config.h"
jhedmonton 28:5905886c76ee 6
maximbolduc 42:854d8cc26bbb 7 const int debug=1; //set to 0 to disable output to USB, set to 1 to output data to USB
jhedmonton 28:5905886c76ee 8
jhedmonton 28:5905886c76ee 9 DigitalOut ledGREEN(p11);
jhedmonton 28:5905886c76ee 10 DigitalOut ledRED(p12);
jhedmonton 28:5905886c76ee 11
jhedmonton 28:5905886c76ee 12 DigitalOut led1(LED1);
jhedmonton 28:5905886c76ee 13 DigitalOut led2(LED2);
jhedmonton 28:5905886c76ee 14 DigitalOut led3(LED3);
jhedmonton 28:5905886c76ee 15 DigitalOut led4(LED4);
jhedmonton 28:5905886c76ee 16
jhedmonton 28:5905886c76ee 17 extern "C" void mbed_mac_address(char *s);
jhedmonton 28:5905886c76ee 18 int _ID = 0; //stores this mbed's ID
jhedmonton 28:5905886c76ee 19
jhedmonton 28:5905886c76ee 20 // Connect the TX of the GPS module to p10 RX input
jhedmonton 28:5905886c76ee 21 MODSERIAL gps(p9, p10); //GPS
jhedmonton 28:5905886c76ee 22 MODSERIAL pc(USBTX, USBRX);
jhedmonton 28:5905886c76ee 23 MODSERIAL bluetooth(p13, p14);
jhedmonton 28:5905886c76ee 24
jhedmonton 28:5905886c76ee 25 // GLOBAL VALUES ONLY=====================
jhedmonton 28:5905886c76ee 26 // values which are stored in config.txt //
maximbolduc 32:c57bc701d65c 27 int gyro_pos = 0;
maximbolduc 42:854d8cc26bbb 28 double antennaheight = 78.74;
jhedmonton 28:5905886c76ee 29 int _btMode = 0; // 0=Send Everything, 1 = No GPS, 2 = Receive only
maximbolduc 30:3afafa1ef16b 30 int antenna_active = 0;
jhedmonton 28:5905886c76ee 31 //Steering
maximbolduc 42:854d8cc26bbb 32 double lookaheadtime = 4;
maximbolduc 42:854d8cc26bbb 33 double scale = 0.5;
maximbolduc 42:854d8cc26bbb 34 double phaseadv = 40;
maximbolduc 42:854d8cc26bbb 35 double tcenter = 4.6;
maximbolduc 42:854d8cc26bbb 36 double fgain = 22;
maximbolduc 42:854d8cc26bbb 37 double avgpos = 4;
jhedmonton 28:5905886c76ee 38 // in prevision of PID addition to FreePilot
maximbolduc 38:b5352d6f8166 39 double kp = 3;
maximbolduc 38:b5352d6f8166 40 double ki = 2;
maximbolduc 38:b5352d6f8166 41 double kd = 1;
jhedmonton 28:5905886c76ee 42
jhedmonton 28:5905886c76ee 43 int gps_baud = 38400; //default at 115200, but FGPS will pass the real baud-rate.
jhedmonton 28:5905886c76ee 44
jhedmonton 28:5905886c76ee 45 //offsets
jhedmonton 28:5905886c76ee 46 double w_xBias;
jhedmonton 28:5905886c76ee 47 double w_yBias;
jhedmonton 28:5905886c76ee 48 double w_zBias;
jhedmonton 28:5905886c76ee 49 double a_xBias;
jhedmonton 28:5905886c76ee 50 double a_yBias;
jhedmonton 28:5905886c76ee 51 double a_zBias;
jhedmonton 28:5905886c76ee 52
jhedmonton 28:5905886c76ee 53 bool Authenticated = 0;
maximbolduc 30:3afafa1ef16b 54 char tx_line[80];
jhedmonton 28:5905886c76ee 55
jhedmonton 28:5905886c76ee 56 void Dispatch(char* line, bool config /* = false */)
maximbolduc 30:3afafa1ef16b 57 {
jhedmonton 28:5905886c76ee 58 char* pointer;
jhedmonton 28:5905886c76ee 59 char* Data[5];
jhedmonton 28:5905886c76ee 60 int index = 0;
jhedmonton 28:5905886c76ee 61 bool valid = true;
jhedmonton 28:5905886c76ee 62 pointer = strtok(line, ",");
jhedmonton 28:5905886c76ee 63 if(pointer == NULL)
jhedmonton 28:5905886c76ee 64 Data[0] = line;
jhedmonton 28:5905886c76ee 65 while(pointer != NULL) {
jhedmonton 28:5905886c76ee 66 Data[index] = pointer;
jhedmonton 28:5905886c76ee 67 pointer = strtok(NULL, ",");
jhedmonton 28:5905886c76ee 68 index++;
jhedmonton 28:5905886c76ee 69 }
jhedmonton 28:5905886c76ee 70 //Check ID of read data and set the corresponding variable.
maximbolduc 34:c2bc9f9be7ff 71 if(strcmp(Data[0], "$ID") == 0)
maximbolduc 34:c2bc9f9be7ff 72 {
maximbolduc 34:c2bc9f9be7ff 73 //_ID = atoi(Data[1]);
maximbolduc 34:c2bc9f9be7ff 74 // if(Config_GetID() == _ID)
maximbolduc 34:c2bc9f9be7ff 75 // {
jhedmonton 28:5905886c76ee 76 // bt->printf("Board ID Matches.\r\n");
maximbolduc 30:3afafa1ef16b 77 Authenticated = true;
maximbolduc 34:c2bc9f9be7ff 78 // }
maximbolduc 34:c2bc9f9be7ff 79 // else
maximbolduc 34:c2bc9f9be7ff 80 // {
maximbolduc 34:c2bc9f9be7ff 81 // Authenticated = false;
maximbolduc 30:3afafa1ef16b 82 // bt->printf("Board ID does not match.\r\n");
maximbolduc 34:c2bc9f9be7ff 83 // }
maximbolduc 34:c2bc9f9be7ff 84 }
maximbolduc 34:c2bc9f9be7ff 85 else if(strcmp(Data[0], "$BANY") == 0)
maximbolduc 34:c2bc9f9be7ff 86 {
maximbolduc 30:3afafa1ef16b 87 // if(!Authenticated)
jhedmonton 28:5905886c76ee 88 // RestartRequired = true;
jhedmonton 28:5905886c76ee 89 _ID = Config_GetID();
jhedmonton 28:5905886c76ee 90 Config_Save();
maximbolduc 30:3afafa1ef16b 91 // bt->printf("Adress set: %d \r\n", _ID);
maximbolduc 34:c2bc9f9be7ff 92 }
maximbolduc 34:c2bc9f9be7ff 93 else if(strcmp(Data[0], "$PA") == 0)
maximbolduc 34:c2bc9f9be7ff 94 {
jhedmonton 28:5905886c76ee 95 phaseadv = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 96 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 34:c2bc9f9be7ff 97 }
maximbolduc 34:c2bc9f9be7ff 98 else if(strcmp(Data[0], "$TC") == 0)
maximbolduc 34:c2bc9f9be7ff 99 {
jhedmonton 28:5905886c76ee 100 tcenter = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 101 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 34:c2bc9f9be7ff 102 }
maximbolduc 34:c2bc9f9be7ff 103 else if(strcmp(Data[0], "$FG") == 0)
maximbolduc 34:c2bc9f9be7ff 104 {
jhedmonton 28:5905886c76ee 105 fgain = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 106 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 34:c2bc9f9be7ff 107 }
maximbolduc 34:c2bc9f9be7ff 108 else if(strcmp(Data[0], "$SC") == 0)
maximbolduc 34:c2bc9f9be7ff 109 {
jhedmonton 28:5905886c76ee 110 scale = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 111 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 34:c2bc9f9be7ff 112 }
maximbolduc 34:c2bc9f9be7ff 113 else if(strcmp(Data[0], "$AP") == 0)
maximbolduc 34:c2bc9f9be7ff 114 {
jhedmonton 28:5905886c76ee 115 avgpos = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 116 }
maximbolduc 34:c2bc9f9be7ff 117 else if(strcmp(Data[0], "$LA") == 0)
maximbolduc 34:c2bc9f9be7ff 118 {
maximbolduc 34:c2bc9f9be7ff 119 lookaheadtime = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 120 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 34:c2bc9f9be7ff 121 }
maximbolduc 34:c2bc9f9be7ff 122 else if(strcmp(Data[0], "$PCALIVE") == 0)
maximbolduc 34:c2bc9f9be7ff 123 {
jhedmonton 28:5905886c76ee 124 //pc connection watchdog
maximbolduc 34:c2bc9f9be7ff 125 }
maximbolduc 34:c2bc9f9be7ff 126 else if(strcmp(Data[0], "$SAVE") == 0)
maximbolduc 34:c2bc9f9be7ff 127 {
jhedmonton 28:5905886c76ee 128 Config_Save();
maximbolduc 34:c2bc9f9be7ff 129 }
maximbolduc 34:c2bc9f9be7ff 130 else if(strcmp(Data[0], "$BTMODE") == 0)
maximbolduc 34:c2bc9f9be7ff 131 {
jhedmonton 28:5905886c76ee 132 _btMode = atoi(Data[1]);
maximbolduc 34:c2bc9f9be7ff 133 }
maximbolduc 34:c2bc9f9be7ff 134 else if (strcmp(Data[0],"$GPSBAUD") == 0 )
maximbolduc 34:c2bc9f9be7ff 135 {
maximbolduc 30:3afafa1ef16b 136 gps_baud = atoi(Data[1]);
maximbolduc 30:3afafa1ef16b 137 activate_antenna();
maximbolduc 34:c2bc9f9be7ff 138 }
maximbolduc 34:c2bc9f9be7ff 139 else if(strcmp(Data[0], "$GYRO") == 0)
maximbolduc 34:c2bc9f9be7ff 140 {
maximbolduc 32:c57bc701d65c 141 gyro_pos = atoi(Data[1]);
maximbolduc 34:c2bc9f9be7ff 142 }
maximbolduc 34:c2bc9f9be7ff 143 else if(strcmp(Data[0], "$HEIGHT") == 0)
maximbolduc 34:c2bc9f9be7ff 144 {
maximbolduc 30:3afafa1ef16b 145 antennaheight = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 146 }
maximbolduc 34:c2bc9f9be7ff 147 else if(strcmp(Data[0], "$POSITION") == 0)
maximbolduc 34:c2bc9f9be7ff 148 {
maximbolduc 34:c2bc9f9be7ff 149 gyro_pos = atoi(Data[1]);
maximbolduc 34:c2bc9f9be7ff 150 }
maximbolduc 34:c2bc9f9be7ff 151 else
maximbolduc 34:c2bc9f9be7ff 152 {
jhedmonton 28:5905886c76ee 153 //bt->printf("Unrecognized config setting detected.\r\n");
jhedmonton 28:5905886c76ee 154 valid = false;
jhedmonton 28:5905886c76ee 155 }
maximbolduc 30:3afafa1ef16b 156 // if(valid && !config)
maximbolduc 30:3afafa1ef16b 157 // bt->printf("Command Accepted.");
maximbolduc 30:3afafa1ef16b 158 }
maximbolduc 30:3afafa1ef16b 159
maximbolduc 30:3afafa1ef16b 160 void activate_antenna()
maximbolduc 30:3afafa1ef16b 161 {
maximbolduc 30:3afafa1ef16b 162 gps.baud(gps_baud);
maximbolduc 30:3afafa1ef16b 163 antenna_active = 1;
jhedmonton 28:5905886c76ee 164 }
jhedmonton 28:5905886c76ee 165
maximbolduc 30:3afafa1ef16b 166 void process_GPSBAUD(char* gpsbaud)
maximbolduc 30:3afafa1ef16b 167 {
maximbolduc 30:3afafa1ef16b 168 char *token;
maximbolduc 30:3afafa1ef16b 169 int token_counter = 0;
maximbolduc 30:3afafa1ef16b 170 char *baud = (char *)NULL;
maximbolduc 30:3afafa1ef16b 171 token = strtok(gpsbaud, ",");
maximbolduc 34:c2bc9f9be7ff 172 while (token)
maximbolduc 34:c2bc9f9be7ff 173 {
maximbolduc 34:c2bc9f9be7ff 174 switch (token_counter)
maximbolduc 34:c2bc9f9be7ff 175 {
maximbolduc 30:3afafa1ef16b 176 case 1:
maximbolduc 30:3afafa1ef16b 177 baud = token;
maximbolduc 30:3afafa1ef16b 178 break;
maximbolduc 30:3afafa1ef16b 179 }
maximbolduc 30:3afafa1ef16b 180 token = strtok((char *)NULL, ",");
maximbolduc 30:3afafa1ef16b 181 token_counter++;
maximbolduc 30:3afafa1ef16b 182 }
maximbolduc 34:c2bc9f9be7ff 183 if ( baud )
maximbolduc 34:c2bc9f9be7ff 184 {
maximbolduc 30:3afafa1ef16b 185 gps_baud = atoi(baud);
maximbolduc 30:3afafa1ef16b 186 }
maximbolduc 30:3afafa1ef16b 187 activate_antenna();
maximbolduc 30:3afafa1ef16b 188 }