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@46:d7d6dc429153, 2015-03-14 (annotated)

Committer:
maximbolduc
Date:
Sat Mar 14 01:01:19 2015 +0000
Revision:
46:d7d6dc429153
Parent:
42:854d8cc26bbb
Child:
47:d3123bb4f673
Before doing big changes, a little commit

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 46:d7d6dc429153 54 bool freepilot_v2 = true;
maximbolduc 30:3afafa1ef16b 55 char tx_line[80];
jhedmonton 28:5905886c76ee 56
jhedmonton 28:5905886c76ee 57 void Dispatch(char* line, bool config /* = false */)
maximbolduc 30:3afafa1ef16b 58 {
jhedmonton 28:5905886c76ee 59 char* pointer;
jhedmonton 28:5905886c76ee 60 char* Data[5];
jhedmonton 28:5905886c76ee 61 int index = 0;
jhedmonton 28:5905886c76ee 62 bool valid = true;
jhedmonton 28:5905886c76ee 63 pointer = strtok(line, ",");
jhedmonton 28:5905886c76ee 64 if(pointer == NULL)
jhedmonton 28:5905886c76ee 65 Data[0] = line;
jhedmonton 28:5905886c76ee 66 while(pointer != NULL) {
jhedmonton 28:5905886c76ee 67 Data[index] = pointer;
jhedmonton 28:5905886c76ee 68 pointer = strtok(NULL, ",");
jhedmonton 28:5905886c76ee 69 index++;
jhedmonton 28:5905886c76ee 70 }
jhedmonton 28:5905886c76ee 71 //Check ID of read data and set the corresponding variable.
maximbolduc 34:c2bc9f9be7ff 72 if(strcmp(Data[0], "$ID") == 0)
maximbolduc 34:c2bc9f9be7ff 73 {
maximbolduc 34:c2bc9f9be7ff 74 //_ID = atoi(Data[1]);
maximbolduc 34:c2bc9f9be7ff 75 // if(Config_GetID() == _ID)
maximbolduc 34:c2bc9f9be7ff 76 // {
jhedmonton 28:5905886c76ee 77 // bt->printf("Board ID Matches.\r\n");
maximbolduc 30:3afafa1ef16b 78 Authenticated = true;
maximbolduc 34:c2bc9f9be7ff 79 // }
maximbolduc 34:c2bc9f9be7ff 80 // else
maximbolduc 34:c2bc9f9be7ff 81 // {
maximbolduc 34:c2bc9f9be7ff 82 // Authenticated = false;
maximbolduc 30:3afafa1ef16b 83 // bt->printf("Board ID does not match.\r\n");
maximbolduc 34:c2bc9f9be7ff 84 // }
maximbolduc 34:c2bc9f9be7ff 85 }
maximbolduc 34:c2bc9f9be7ff 86 else if(strcmp(Data[0], "$BANY") == 0)
maximbolduc 34:c2bc9f9be7ff 87 {
maximbolduc 30:3afafa1ef16b 88 // if(!Authenticated)
jhedmonton 28:5905886c76ee 89 // RestartRequired = true;
jhedmonton 28:5905886c76ee 90 _ID = Config_GetID();
jhedmonton 28:5905886c76ee 91 Config_Save();
maximbolduc 30:3afafa1ef16b 92 // bt->printf("Adress set: %d \r\n", _ID);
maximbolduc 34:c2bc9f9be7ff 93 }
maximbolduc 34:c2bc9f9be7ff 94 else if(strcmp(Data[0], "$PA") == 0)
maximbolduc 34:c2bc9f9be7ff 95 {
jhedmonton 28:5905886c76ee 96 phaseadv = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 97 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 34:c2bc9f9be7ff 98 }
maximbolduc 34:c2bc9f9be7ff 99 else if(strcmp(Data[0], "$TC") == 0)
maximbolduc 34:c2bc9f9be7ff 100 {
jhedmonton 28:5905886c76ee 101 tcenter = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 102 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 46:d7d6dc429153 103 }
maximbolduc 46:d7d6dc429153 104 else if(strcmp(Data[0], "$V2,1") == 0)
maximbolduc 46:d7d6dc429153 105 {
maximbolduc 46:d7d6dc429153 106 freepilot_v2 = true;
maximbolduc 46:d7d6dc429153 107 }
maximbolduc 46:d7d6dc429153 108 else if(strcmp(Data[0], "$V2,0") == 0)
maximbolduc 46:d7d6dc429153 109 {
maximbolduc 46:d7d6dc429153 110 freepilot_v2 = false;
maximbolduc 34:c2bc9f9be7ff 111 }
maximbolduc 34:c2bc9f9be7ff 112 else if(strcmp(Data[0], "$FG") == 0)
maximbolduc 34:c2bc9f9be7ff 113 {
jhedmonton 28:5905886c76ee 114 fgain = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 115 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 34:c2bc9f9be7ff 116 }
maximbolduc 34:c2bc9f9be7ff 117 else if(strcmp(Data[0], "$SC") == 0)
maximbolduc 34:c2bc9f9be7ff 118 {
jhedmonton 28:5905886c76ee 119 scale = 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], "$AP") == 0)
maximbolduc 34:c2bc9f9be7ff 123 {
jhedmonton 28:5905886c76ee 124 avgpos = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 125 }
maximbolduc 34:c2bc9f9be7ff 126 else if(strcmp(Data[0], "$LA") == 0)
maximbolduc 34:c2bc9f9be7ff 127 {
maximbolduc 34:c2bc9f9be7ff 128 lookaheadtime = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 129 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 34:c2bc9f9be7ff 130 }
maximbolduc 34:c2bc9f9be7ff 131 else if(strcmp(Data[0], "$PCALIVE") == 0)
maximbolduc 34:c2bc9f9be7ff 132 {
jhedmonton 28:5905886c76ee 133 //pc connection watchdog
maximbolduc 34:c2bc9f9be7ff 134 }
maximbolduc 34:c2bc9f9be7ff 135 else if(strcmp(Data[0], "$SAVE") == 0)
maximbolduc 34:c2bc9f9be7ff 136 {
jhedmonton 28:5905886c76ee 137 Config_Save();
maximbolduc 34:c2bc9f9be7ff 138 }
maximbolduc 34:c2bc9f9be7ff 139 else if(strcmp(Data[0], "$BTMODE") == 0)
maximbolduc 34:c2bc9f9be7ff 140 {
jhedmonton 28:5905886c76ee 141 _btMode = atoi(Data[1]);
maximbolduc 34:c2bc9f9be7ff 142 }
maximbolduc 34:c2bc9f9be7ff 143 else if (strcmp(Data[0],"$GPSBAUD") == 0 )
maximbolduc 34:c2bc9f9be7ff 144 {
maximbolduc 30:3afafa1ef16b 145 gps_baud = atoi(Data[1]);
maximbolduc 30:3afafa1ef16b 146 activate_antenna();
maximbolduc 34:c2bc9f9be7ff 147 }
maximbolduc 34:c2bc9f9be7ff 148 else if(strcmp(Data[0], "$GYRO") == 0)
maximbolduc 34:c2bc9f9be7ff 149 {
maximbolduc 32:c57bc701d65c 150 gyro_pos = atoi(Data[1]);
maximbolduc 34:c2bc9f9be7ff 151 }
maximbolduc 34:c2bc9f9be7ff 152 else if(strcmp(Data[0], "$HEIGHT") == 0)
maximbolduc 34:c2bc9f9be7ff 153 {
maximbolduc 30:3afafa1ef16b 154 antennaheight = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 155 }
maximbolduc 34:c2bc9f9be7ff 156 else if(strcmp(Data[0], "$POSITION") == 0)
maximbolduc 34:c2bc9f9be7ff 157 {
maximbolduc 34:c2bc9f9be7ff 158 gyro_pos = atoi(Data[1]);
maximbolduc 34:c2bc9f9be7ff 159 }
maximbolduc 34:c2bc9f9be7ff 160 else
maximbolduc 34:c2bc9f9be7ff 161 {
jhedmonton 28:5905886c76ee 162 //bt->printf("Unrecognized config setting detected.\r\n");
jhedmonton 28:5905886c76ee 163 valid = false;
jhedmonton 28:5905886c76ee 164 }
maximbolduc 30:3afafa1ef16b 165 // if(valid && !config)
maximbolduc 30:3afafa1ef16b 166 // bt->printf("Command Accepted.");
maximbolduc 30:3afafa1ef16b 167 }
maximbolduc 30:3afafa1ef16b 168
maximbolduc 30:3afafa1ef16b 169 void activate_antenna()
maximbolduc 30:3afafa1ef16b 170 {
maximbolduc 30:3afafa1ef16b 171 gps.baud(gps_baud);
maximbolduc 30:3afafa1ef16b 172 antenna_active = 1;
jhedmonton 28:5905886c76ee 173 }
jhedmonton 28:5905886c76ee 174
maximbolduc 30:3afafa1ef16b 175 void process_GPSBAUD(char* gpsbaud)
maximbolduc 30:3afafa1ef16b 176 {
maximbolduc 30:3afafa1ef16b 177 char *token;
maximbolduc 30:3afafa1ef16b 178 int token_counter = 0;
maximbolduc 30:3afafa1ef16b 179 char *baud = (char *)NULL;
maximbolduc 30:3afafa1ef16b 180 token = strtok(gpsbaud, ",");
maximbolduc 34:c2bc9f9be7ff 181 while (token)
maximbolduc 34:c2bc9f9be7ff 182 {
maximbolduc 34:c2bc9f9be7ff 183 switch (token_counter)
maximbolduc 34:c2bc9f9be7ff 184 {
maximbolduc 30:3afafa1ef16b 185 case 1:
maximbolduc 30:3afafa1ef16b 186 baud = token;
maximbolduc 30:3afafa1ef16b 187 break;
maximbolduc 30:3afafa1ef16b 188 }
maximbolduc 30:3afafa1ef16b 189 token = strtok((char *)NULL, ",");
maximbolduc 30:3afafa1ef16b 190 token_counter++;
maximbolduc 30:3afafa1ef16b 191 }
maximbolduc 34:c2bc9f9be7ff 192 if ( baud )
maximbolduc 34:c2bc9f9be7ff 193 {
maximbolduc 30:3afafa1ef16b 194 gps_baud = atoi(baud);
maximbolduc 30:3afafa1ef16b 195 }
maximbolduc 30:3afafa1ef16b 196 activate_antenna();
maximbolduc 30:3afafa1ef16b 197 }