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@32:c57bc701d65c, 2015-01-29 (annotated)

Committer:
maximbolduc
Date:
Thu Jan 29 02:49:07 2015 +0000
Revision:
32:c57bc701d65c
Parent:
30:3afafa1ef16b
Child:
33:3e71c418e90d
Got checksum working and some other chnages;

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
jhedmonton 28:5905886c76ee 7 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;
jhedmonton 28:5905886c76ee 28 double antennaheight = 200;
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
jhedmonton 28:5905886c76ee 32 double lookaheadtime = 2;
jhedmonton 28:5905886c76ee 33 double scale = 1;
jhedmonton 28:5905886c76ee 34 double phaseadv = 50;
jhedmonton 28:5905886c76ee 35 double tcenter = 5;
jhedmonton 28:5905886c76ee 36 double fgain = 125;
jhedmonton 28:5905886c76ee 37 double avgpos = -4;
jhedmonton 28:5905886c76ee 38 // in prevision of PID addition to FreePilot
jhedmonton 28:5905886c76ee 39 double kp = 0;
jhedmonton 28:5905886c76ee 40 double ki = 0;
jhedmonton 28:5905886c76ee 41 double kd = 0;
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;
maximbolduc 30:3afafa1ef16b 61
jhedmonton 28:5905886c76ee 62 bool valid = true;
maximbolduc 30:3afafa1ef16b 63
jhedmonton 28:5905886c76ee 64 //Split data at commas
jhedmonton 28:5905886c76ee 65 pointer = strtok(line, ",");
jhedmonton 28:5905886c76ee 66
jhedmonton 28:5905886c76ee 67 if(pointer == NULL)
jhedmonton 28:5905886c76ee 68 Data[0] = line;
jhedmonton 28:5905886c76ee 69
jhedmonton 28:5905886c76ee 70 while(pointer != NULL) {
jhedmonton 28:5905886c76ee 71 Data[index] = pointer;
jhedmonton 28:5905886c76ee 72 pointer = strtok(NULL, ",");
jhedmonton 28:5905886c76ee 73 index++;
jhedmonton 28:5905886c76ee 74 }
jhedmonton 28:5905886c76ee 75
jhedmonton 28:5905886c76ee 76 //Check ID of read data and set the corresponding variable.
jhedmonton 28:5905886c76ee 77 if(strcmp(Data[0], "$ID") == 0) {
jhedmonton 28:5905886c76ee 78 _ID = atoi(Data[1]);
maximbolduc 30:3afafa1ef16b 79
maximbolduc 30:3afafa1ef16b 80 if(Config_GetID() == _ID) {
jhedmonton 28:5905886c76ee 81 // bt->printf("Board ID Matches.\r\n");
maximbolduc 30:3afafa1ef16b 82 Authenticated = true;
maximbolduc 30:3afafa1ef16b 83 } else {
maximbolduc 30:3afafa1ef16b 84 Authenticated = false;
maximbolduc 30:3afafa1ef16b 85 // bt->printf("Board ID does not match.\r\n");
jhedmonton 28:5905886c76ee 86 }
maximbolduc 30:3afafa1ef16b 87
jhedmonton 28:5905886c76ee 88 } else if(strcmp(Data[0], "$BANY") == 0) {
maximbolduc 30:3afafa1ef16b 89
maximbolduc 30:3afafa1ef16b 90 // if(!Authenticated)
jhedmonton 28:5905886c76ee 91 // RestartRequired = true;
maximbolduc 30:3afafa1ef16b 92
jhedmonton 28:5905886c76ee 93 _ID = Config_GetID();
jhedmonton 28:5905886c76ee 94 Config_Save();
maximbolduc 30:3afafa1ef16b 95
maximbolduc 30:3afafa1ef16b 96 // bt->printf("Adress set: %d \r\n", _ID);
jhedmonton 28:5905886c76ee 97
jhedmonton 28:5905886c76ee 98 } else if(strcmp(Data[0], "$PA") == 0) {
jhedmonton 28:5905886c76ee 99 phaseadv = atof(Data[1]);
jhedmonton 28:5905886c76ee 100
jhedmonton 28:5905886c76ee 101 } else if(strcmp(Data[0], "$TC") == 0) {
jhedmonton 28:5905886c76ee 102 tcenter = atof(Data[1]);
jhedmonton 28:5905886c76ee 103
jhedmonton 28:5905886c76ee 104 } else if(strcmp(Data[0], "$FG") == 0) {
jhedmonton 28:5905886c76ee 105 fgain = atof(Data[1]);
jhedmonton 28:5905886c76ee 106
jhedmonton 28:5905886c76ee 107 } else if(strcmp(Data[0], "$SC") == 0) {
jhedmonton 28:5905886c76ee 108 scale = atof(Data[1]);
jhedmonton 28:5905886c76ee 109
jhedmonton 28:5905886c76ee 110 } else if(strcmp(Data[0], "$AP") == 0) {
jhedmonton 28:5905886c76ee 111 avgpos = atof(Data[1]);
jhedmonton 28:5905886c76ee 112
maximbolduc 30:3afafa1ef16b 113 //}
maximbolduc 30:3afafa1ef16b 114 //else if(strcmp(Data[0], "$ASTEER") == 0) {
maximbolduc 30:3afafa1ef16b 115 // j_guidance = atoi(Data[1]);
jhedmonton 28:5905886c76ee 116
maximbolduc 30:3afafa1ef16b 117 } else if(strcmp(Data[0], "$PCALIVE") == 0) {
jhedmonton 28:5905886c76ee 118 //pc connection watchdog
jhedmonton 28:5905886c76ee 119
jhedmonton 28:5905886c76ee 120 } else if(strcmp(Data[0], "$SAVE") == 0) {
jhedmonton 28:5905886c76ee 121 Config_Save();
jhedmonton 28:5905886c76ee 122
jhedmonton 28:5905886c76ee 123 } else if(strcmp(Data[0], "$BTMODE") == 0) {
jhedmonton 28:5905886c76ee 124 _btMode = atoi(Data[1]);
maximbolduc 30:3afafa1ef16b 125 } else if (strcmp(Data[0],"$GPSBAUD") == 0 ) {
maximbolduc 30:3afafa1ef16b 126 gps_baud = atoi(Data[1]);
maximbolduc 30:3afafa1ef16b 127 activate_antenna();
maximbolduc 30:3afafa1ef16b 128 } else if(strcmp(Data[0], "$GYRO") == 0) {
maximbolduc 32:c57bc701d65c 129 gyro_pos = atoi(Data[1]);
maximbolduc 30:3afafa1ef16b 130 } else if(strcmp(Data[0], "$HEIGHT") == 0) {
maximbolduc 30:3afafa1ef16b 131 antennaheight = atof(Data[1]);
maximbolduc 30:3afafa1ef16b 132 } else if(strcmp(Data[0], "$a_zBias") == 0) {
maximbolduc 30:3afafa1ef16b 133 a_zBias = atof(Data[1]);
maximbolduc 30:3afafa1ef16b 134 } else if(strcmp(Data[0], "$a_xBias") == 0) {
maximbolduc 30:3afafa1ef16b 135 a_xBias = atof(Data[1]);
maximbolduc 30:3afafa1ef16b 136 } else if(strcmp(Data[0], "$a_yBias") == 0) {
maximbolduc 30:3afafa1ef16b 137 a_yBias = atof(Data[1]);
maximbolduc 30:3afafa1ef16b 138 } else if(strcmp(Data[0], "$w_zBias") == 0) {
maximbolduc 30:3afafa1ef16b 139 // w_zBias = atof(Data[1]);
maximbolduc 30:3afafa1ef16b 140 } else if(strcmp(Data[0], "$w_xBias") == 0) {
maximbolduc 30:3afafa1ef16b 141 // w_xBias = atof(Data[1]);
maximbolduc 30:3afafa1ef16b 142 } else if(strcmp(Data[0], "$w_yBias") == 0) {
maximbolduc 30:3afafa1ef16b 143 // w_yBias = atof(Data[1]);
maximbolduc 32:c57bc701d65c 144 }else if(strcmp(Data[0], "$POSITION") == 0) {
maximbolduc 32:c57bc701d65c 145 gyro_pos = atoi(Data[1]);
maximbolduc 30:3afafa1ef16b 146 }else {
jhedmonton 28:5905886c76ee 147 //bt->printf("Unrecognized config setting detected.\r\n");
jhedmonton 28:5905886c76ee 148 valid = false;
jhedmonton 28:5905886c76ee 149 }
maximbolduc 30:3afafa1ef16b 150 // if(valid && !config)
maximbolduc 30:3afafa1ef16b 151 // bt->printf("Command Accepted.");
maximbolduc 30:3afafa1ef16b 152 }
maximbolduc 30:3afafa1ef16b 153
maximbolduc 30:3afafa1ef16b 154 void activate_antenna()
maximbolduc 30:3afafa1ef16b 155 {
maximbolduc 30:3afafa1ef16b 156 gps.baud(gps_baud);
maximbolduc 30:3afafa1ef16b 157 antenna_active = 1;
jhedmonton 28:5905886c76ee 158 }
jhedmonton 28:5905886c76ee 159
maximbolduc 30:3afafa1ef16b 160 void process_GPSBAUD(char* gpsbaud)
maximbolduc 30:3afafa1ef16b 161 {
maximbolduc 30:3afafa1ef16b 162 char *token;
maximbolduc 30:3afafa1ef16b 163 int token_counter = 0;
maximbolduc 30:3afafa1ef16b 164 char *baud = (char *)NULL;
maximbolduc 30:3afafa1ef16b 165 token = strtok(gpsbaud, ",");
maximbolduc 30:3afafa1ef16b 166 while (token) {
maximbolduc 30:3afafa1ef16b 167 switch (token_counter) {
maximbolduc 30:3afafa1ef16b 168 case 1:
maximbolduc 30:3afafa1ef16b 169 baud = token;
maximbolduc 30:3afafa1ef16b 170 break;
maximbolduc 30:3afafa1ef16b 171 }
maximbolduc 30:3afafa1ef16b 172 token = strtok((char *)NULL, ",");
maximbolduc 30:3afafa1ef16b 173 token_counter++;
maximbolduc 30:3afafa1ef16b 174 }
maximbolduc 30:3afafa1ef16b 175 if ( baud ) {
maximbolduc 30:3afafa1ef16b 176 gps_baud = atoi(baud);
maximbolduc 30:3afafa1ef16b 177 }
maximbolduc 30:3afafa1ef16b 178 activate_antenna();
maximbolduc 30:3afafa1ef16b 179 }