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@38:b5352d6f8166, 2015-03-04 (annotated)

Committer:
maximbolduc
Date:
Wed Mar 04 13:11:33 2015 +0000
Revision:
38:b5352d6f8166
Parent:
34:c2bc9f9be7ff
Child:
41:a26acd346c2f
autosteer routine

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"
maximbolduc 34:c2bc9f9be7ff 6 #include "autosteer.h"
jhedmonton 28:5905886c76ee 7
jhedmonton 28:5905886c76ee 8 int debug=1; //set to 0 to disable output to USB, set to 1 to output data to USB
jhedmonton 28:5905886c76ee 9
jhedmonton 28:5905886c76ee 10 DigitalOut ledGREEN(p11);
jhedmonton 28:5905886c76ee 11 DigitalOut ledRED(p12);
jhedmonton 28:5905886c76ee 12
jhedmonton 28:5905886c76ee 13 DigitalOut led1(LED1);
jhedmonton 28:5905886c76ee 14 DigitalOut led2(LED2);
jhedmonton 28:5905886c76ee 15 DigitalOut led3(LED3);
jhedmonton 28:5905886c76ee 16 DigitalOut led4(LED4);
jhedmonton 28:5905886c76ee 17
jhedmonton 28:5905886c76ee 18 extern "C" void mbed_mac_address(char *s);
jhedmonton 28:5905886c76ee 19 int _ID = 0; //stores this mbed's ID
jhedmonton 28:5905886c76ee 20
jhedmonton 28:5905886c76ee 21 // Connect the TX of the GPS module to p10 RX input
jhedmonton 28:5905886c76ee 22 MODSERIAL gps(p9, p10); //GPS
jhedmonton 28:5905886c76ee 23 MODSERIAL pc(USBTX, USBRX);
jhedmonton 28:5905886c76ee 24 MODSERIAL bluetooth(p13, p14);
jhedmonton 28:5905886c76ee 25
jhedmonton 28:5905886c76ee 26 // GLOBAL VALUES ONLY=====================
jhedmonton 28:5905886c76ee 27 // values which are stored in config.txt //
maximbolduc 32:c57bc701d65c 28 int gyro_pos = 0;
jhedmonton 28:5905886c76ee 29 double antennaheight = 200;
jhedmonton 28:5905886c76ee 30 int _btMode = 0; // 0=Send Everything, 1 = No GPS, 2 = Receive only
maximbolduc 30:3afafa1ef16b 31 int antenna_active = 0;
jhedmonton 28:5905886c76ee 32 //Steering
jhedmonton 28:5905886c76ee 33 double lookaheadtime = 2;
jhedmonton 28:5905886c76ee 34 double scale = 1;
jhedmonton 28:5905886c76ee 35 double phaseadv = 50;
jhedmonton 28:5905886c76ee 36 double tcenter = 5;
jhedmonton 28:5905886c76ee 37 double fgain = 125;
jhedmonton 28:5905886c76ee 38 double avgpos = -4;
jhedmonton 28:5905886c76ee 39 // in prevision of PID addition to FreePilot
maximbolduc 38:b5352d6f8166 40 double kp = 3;
maximbolduc 38:b5352d6f8166 41 double ki = 2;
maximbolduc 38:b5352d6f8166 42 double kd = 1;
jhedmonton 28:5905886c76ee 43
jhedmonton 28:5905886c76ee 44 int gps_baud = 38400; //default at 115200, but FGPS will pass the real baud-rate.
jhedmonton 28:5905886c76ee 45
jhedmonton 28:5905886c76ee 46 //offsets
jhedmonton 28:5905886c76ee 47 double w_xBias;
jhedmonton 28:5905886c76ee 48 double w_yBias;
jhedmonton 28:5905886c76ee 49 double w_zBias;
jhedmonton 28:5905886c76ee 50 double a_xBias;
jhedmonton 28:5905886c76ee 51 double a_yBias;
jhedmonton 28:5905886c76ee 52 double a_zBias;
jhedmonton 28:5905886c76ee 53
jhedmonton 28:5905886c76ee 54 bool Authenticated = 0;
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 34:c2bc9f9be7ff 103 }
maximbolduc 34:c2bc9f9be7ff 104 else if(strcmp(Data[0], "$FG") == 0)
maximbolduc 34:c2bc9f9be7ff 105 {
jhedmonton 28:5905886c76ee 106 fgain = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 107 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 34:c2bc9f9be7ff 108 }
maximbolduc 34:c2bc9f9be7ff 109 else if(strcmp(Data[0], "$SC") == 0)
maximbolduc 34:c2bc9f9be7ff 110 {
jhedmonton 28:5905886c76ee 111 scale = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 112 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 34:c2bc9f9be7ff 113 }
maximbolduc 34:c2bc9f9be7ff 114 else if(strcmp(Data[0], "$AP") == 0)
maximbolduc 34:c2bc9f9be7ff 115 {
jhedmonton 28:5905886c76ee 116 avgpos = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 117 }
maximbolduc 34:c2bc9f9be7ff 118 else if(strcmp(Data[0], "$LA") == 0)
maximbolduc 34:c2bc9f9be7ff 119 {
maximbolduc 34:c2bc9f9be7ff 120 lookaheadtime = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 121 // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
maximbolduc 34:c2bc9f9be7ff 122 }
maximbolduc 34:c2bc9f9be7ff 123 else if(strcmp(Data[0], "$PCALIVE") == 0)
maximbolduc 34:c2bc9f9be7ff 124 {
jhedmonton 28:5905886c76ee 125 //pc connection watchdog
maximbolduc 34:c2bc9f9be7ff 126 }
maximbolduc 34:c2bc9f9be7ff 127 else if(strcmp(Data[0], "$SAVE") == 0)
maximbolduc 34:c2bc9f9be7ff 128 {
jhedmonton 28:5905886c76ee 129 Config_Save();
maximbolduc 34:c2bc9f9be7ff 130 }
maximbolduc 34:c2bc9f9be7ff 131 else if(strcmp(Data[0], "$BTMODE") == 0)
maximbolduc 34:c2bc9f9be7ff 132 {
jhedmonton 28:5905886c76ee 133 _btMode = atoi(Data[1]);
maximbolduc 34:c2bc9f9be7ff 134 }
maximbolduc 34:c2bc9f9be7ff 135 else if (strcmp(Data[0],"$GPSBAUD") == 0 )
maximbolduc 34:c2bc9f9be7ff 136 {
maximbolduc 30:3afafa1ef16b 137 gps_baud = atoi(Data[1]);
maximbolduc 30:3afafa1ef16b 138 activate_antenna();
maximbolduc 34:c2bc9f9be7ff 139 }
maximbolduc 34:c2bc9f9be7ff 140 else if(strcmp(Data[0], "$GYRO") == 0)
maximbolduc 34:c2bc9f9be7ff 141 {
maximbolduc 32:c57bc701d65c 142 gyro_pos = atoi(Data[1]);
maximbolduc 34:c2bc9f9be7ff 143 }
maximbolduc 34:c2bc9f9be7ff 144 else if(strcmp(Data[0], "$HEIGHT") == 0)
maximbolduc 34:c2bc9f9be7ff 145 {
maximbolduc 30:3afafa1ef16b 146 antennaheight = atof(Data[1]);
maximbolduc 34:c2bc9f9be7ff 147 }
maximbolduc 34:c2bc9f9be7ff 148 else if(strcmp(Data[0], "$POSITION") == 0)
maximbolduc 34:c2bc9f9be7ff 149 {
maximbolduc 34:c2bc9f9be7ff 150 gyro_pos = atoi(Data[1]);
maximbolduc 34:c2bc9f9be7ff 151 }
maximbolduc 34:c2bc9f9be7ff 152 else
maximbolduc 34:c2bc9f9be7ff 153 {
jhedmonton 28:5905886c76ee 154 //bt->printf("Unrecognized config setting detected.\r\n");
jhedmonton 28:5905886c76ee 155 valid = false;
jhedmonton 28:5905886c76ee 156 }
maximbolduc 30:3afafa1ef16b 157 // if(valid && !config)
maximbolduc 30:3afafa1ef16b 158 // bt->printf("Command Accepted.");
maximbolduc 30:3afafa1ef16b 159 }
maximbolduc 30:3afafa1ef16b 160
maximbolduc 30:3afafa1ef16b 161 void activate_antenna()
maximbolduc 30:3afafa1ef16b 162 {
maximbolduc 30:3afafa1ef16b 163 gps.baud(gps_baud);
maximbolduc 30:3afafa1ef16b 164 antenna_active = 1;
jhedmonton 28:5905886c76ee 165 }
jhedmonton 28:5905886c76ee 166
maximbolduc 30:3afafa1ef16b 167 void process_GPSBAUD(char* gpsbaud)
maximbolduc 30:3afafa1ef16b 168 {
maximbolduc 30:3afafa1ef16b 169 char *token;
maximbolduc 30:3afafa1ef16b 170 int token_counter = 0;
maximbolduc 30:3afafa1ef16b 171 char *baud = (char *)NULL;
maximbolduc 30:3afafa1ef16b 172 token = strtok(gpsbaud, ",");
maximbolduc 34:c2bc9f9be7ff 173 while (token)
maximbolduc 34:c2bc9f9be7ff 174 {
maximbolduc 34:c2bc9f9be7ff 175 switch (token_counter)
maximbolduc 34:c2bc9f9be7ff 176 {
maximbolduc 30:3afafa1ef16b 177 case 1:
maximbolduc 30:3afafa1ef16b 178 baud = token;
maximbolduc 30:3afafa1ef16b 179 break;
maximbolduc 30:3afafa1ef16b 180 }
maximbolduc 30:3afafa1ef16b 181 token = strtok((char *)NULL, ",");
maximbolduc 30:3afafa1ef16b 182 token_counter++;
maximbolduc 30:3afafa1ef16b 183 }
maximbolduc 34:c2bc9f9be7ff 184 if ( baud )
maximbolduc 34:c2bc9f9be7ff 185 {
maximbolduc 30:3afafa1ef16b 186 gps_baud = atoi(baud);
maximbolduc 30:3afafa1ef16b 187 }
maximbolduc 30:3afafa1ef16b 188 activate_antenna();
maximbolduc 30:3afafa1ef16b 189 }