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@28:5905886c76ee, 2015-01-21 (annotated)

Committer:
jhedmonton
Date:
Wed Jan 21 02:54:26 2015 +0000
Revision:
28:5905886c76ee
Child:
30:3afafa1ef16b
Changes to config, adding a base.cpp to move globals

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
jhedmonton 28:5905886c76ee 8
jhedmonton 28:5905886c76ee 9 int debug=1; //set to 0 to disable output to USB, set to 1 to output data to USB
jhedmonton 28:5905886c76ee 10
jhedmonton 28:5905886c76ee 11 DigitalOut ledGREEN(p11);
jhedmonton 28:5905886c76ee 12 DigitalOut ledRED(p12);
jhedmonton 28:5905886c76ee 13
jhedmonton 28:5905886c76ee 14
jhedmonton 28:5905886c76ee 15
jhedmonton 28:5905886c76ee 16
jhedmonton 28:5905886c76ee 17 DigitalOut led1(LED1);
jhedmonton 28:5905886c76ee 18 DigitalOut led2(LED2);
jhedmonton 28:5905886c76ee 19 DigitalOut led3(LED3);
jhedmonton 28:5905886c76ee 20 DigitalOut led4(LED4);
jhedmonton 28:5905886c76ee 21
jhedmonton 28:5905886c76ee 22
jhedmonton 28:5905886c76ee 23 extern "C" void mbed_mac_address(char *s);
jhedmonton 28:5905886c76ee 24 int _ID = 0; //stores this mbed's ID
jhedmonton 28:5905886c76ee 25
jhedmonton 28:5905886c76ee 26 // Connect the TX of the GPS module to p10 RX input
jhedmonton 28:5905886c76ee 27 MODSERIAL gps(p9, p10); //GPS
jhedmonton 28:5905886c76ee 28 MODSERIAL pc(USBTX, USBRX);
jhedmonton 28:5905886c76ee 29 MODSERIAL bluetooth(p13, p14);
jhedmonton 28:5905886c76ee 30
jhedmonton 28:5905886c76ee 31
jhedmonton 28:5905886c76ee 32 // GLOBAL VALUES ONLY=====================
jhedmonton 28:5905886c76ee 33 // values which are stored in config.txt //
jhedmonton 28:5905886c76ee 34 int gyro_position = 0;
jhedmonton 28:5905886c76ee 35 double antennaheight = 200;
jhedmonton 28:5905886c76ee 36 int _btMode = 0; // 0=Send Everything, 1 = No GPS, 2 = Receive only
jhedmonton 28:5905886c76ee 37
jhedmonton 28:5905886c76ee 38 //Steering
jhedmonton 28:5905886c76ee 39 double lookaheadtime = 2;
jhedmonton 28:5905886c76ee 40 double scale = 1;
jhedmonton 28:5905886c76ee 41 double phaseadv = 50;
jhedmonton 28:5905886c76ee 42 double tcenter = 5;
jhedmonton 28:5905886c76ee 43 double fgain = 125;
jhedmonton 28:5905886c76ee 44 double avgpos = -4;
jhedmonton 28:5905886c76ee 45 // in prevision of PID addition to FreePilot
jhedmonton 28:5905886c76ee 46 double kp = 0;
jhedmonton 28:5905886c76ee 47 double ki = 0;
jhedmonton 28:5905886c76ee 48 double kd = 0;
jhedmonton 28:5905886c76ee 49
jhedmonton 28:5905886c76ee 50 int gps_baud = 38400; //default at 115200, but FGPS will pass the real baud-rate.
jhedmonton 28:5905886c76ee 51
jhedmonton 28:5905886c76ee 52 //offsets
jhedmonton 28:5905886c76ee 53 double w_xBias;
jhedmonton 28:5905886c76ee 54 double w_yBias;
jhedmonton 28:5905886c76ee 55 double w_zBias;
jhedmonton 28:5905886c76ee 56 double a_xBias;
jhedmonton 28:5905886c76ee 57 double a_yBias;
jhedmonton 28:5905886c76ee 58 double a_zBias;
jhedmonton 28:5905886c76ee 59
jhedmonton 28:5905886c76ee 60 bool Authenticated = 0;
jhedmonton 28:5905886c76ee 61
jhedmonton 28:5905886c76ee 62 void Dispatch(char* line, bool config /* = false */)
jhedmonton 28:5905886c76ee 63 {
jhedmonton 28:5905886c76ee 64 char* pointer;
jhedmonton 28:5905886c76ee 65 char* Data[5];
jhedmonton 28:5905886c76ee 66 int index = 0;
jhedmonton 28:5905886c76ee 67
jhedmonton 28:5905886c76ee 68 bool valid = true;
jhedmonton 28:5905886c76ee 69
jhedmonton 28:5905886c76ee 70 //Split data at commas
jhedmonton 28:5905886c76ee 71 pointer = strtok(line, ",");
jhedmonton 28:5905886c76ee 72
jhedmonton 28:5905886c76ee 73 if(pointer == NULL)
jhedmonton 28:5905886c76ee 74 Data[0] = line;
jhedmonton 28:5905886c76ee 75
jhedmonton 28:5905886c76ee 76 while(pointer != NULL) {
jhedmonton 28:5905886c76ee 77 Data[index] = pointer;
jhedmonton 28:5905886c76ee 78 pointer = strtok(NULL, ",");
jhedmonton 28:5905886c76ee 79 index++;
jhedmonton 28:5905886c76ee 80 }
jhedmonton 28:5905886c76ee 81
jhedmonton 28:5905886c76ee 82 //Check ID of read data and set the corresponding variable.
jhedmonton 28:5905886c76ee 83 if(strcmp(Data[0], "$ID") == 0) {
jhedmonton 28:5905886c76ee 84 _ID = atoi(Data[1]);
jhedmonton 28:5905886c76ee 85
jhedmonton 28:5905886c76ee 86 if(Config_GetID() == _ID)
jhedmonton 28:5905886c76ee 87 {
jhedmonton 28:5905886c76ee 88 // bt->printf("Board ID Matches.\r\n");
jhedmonton 28:5905886c76ee 89 Authenticated = true;
jhedmonton 28:5905886c76ee 90 }
jhedmonton 28:5905886c76ee 91 else
jhedmonton 28:5905886c76ee 92 {
jhedmonton 28:5905886c76ee 93 Authenticated = false;
jhedmonton 28:5905886c76ee 94 // bt->printf("Board ID does not match.\r\n");
jhedmonton 28:5905886c76ee 95 }
jhedmonton 28:5905886c76ee 96
jhedmonton 28:5905886c76ee 97 } else if(strcmp(Data[0], "$BANY") == 0) {
jhedmonton 28:5905886c76ee 98
jhedmonton 28:5905886c76ee 99 // if(!Authenticated)
jhedmonton 28:5905886c76ee 100 // RestartRequired = true;
jhedmonton 28:5905886c76ee 101
jhedmonton 28:5905886c76ee 102 _ID = Config_GetID();
jhedmonton 28:5905886c76ee 103 Config_Save();
jhedmonton 28:5905886c76ee 104
jhedmonton 28:5905886c76ee 105 // bt->printf("Adress set: %d \r\n", _ID);
jhedmonton 28:5905886c76ee 106
jhedmonton 28:5905886c76ee 107 } else if(strcmp(Data[0], "$PA") == 0) {
jhedmonton 28:5905886c76ee 108 phaseadv = atof(Data[1]);
jhedmonton 28:5905886c76ee 109
jhedmonton 28:5905886c76ee 110 } else if(strcmp(Data[0], "$TC") == 0) {
jhedmonton 28:5905886c76ee 111 tcenter = atof(Data[1]);
jhedmonton 28:5905886c76ee 112
jhedmonton 28:5905886c76ee 113 } else if(strcmp(Data[0], "$FG") == 0) {
jhedmonton 28:5905886c76ee 114 fgain = atof(Data[1]);
jhedmonton 28:5905886c76ee 115
jhedmonton 28:5905886c76ee 116 } else if(strcmp(Data[0], "$SC") == 0) {
jhedmonton 28:5905886c76ee 117 scale = atof(Data[1]);
jhedmonton 28:5905886c76ee 118
jhedmonton 28:5905886c76ee 119 } else if(strcmp(Data[0], "$AP") == 0) {
jhedmonton 28:5905886c76ee 120 avgpos = atof(Data[1]);
jhedmonton 28:5905886c76ee 121
jhedmonton 28:5905886c76ee 122 //}
jhedmonton 28:5905886c76ee 123 //else if(strcmp(Data[0], "$ASTEER") == 0) {
jhedmonton 28:5905886c76ee 124 // j_guidance = atoi(Data[1]);
jhedmonton 28:5905886c76ee 125
jhedmonton 28:5905886c76ee 126 }
jhedmonton 28:5905886c76ee 127 else if(strcmp(Data[0], "$PCALIVE") == 0) {
jhedmonton 28:5905886c76ee 128 //pc connection watchdog
jhedmonton 28:5905886c76ee 129
jhedmonton 28:5905886c76ee 130 } else if(strcmp(Data[0], "$SAVE") == 0) {
jhedmonton 28:5905886c76ee 131 Config_Save();
jhedmonton 28:5905886c76ee 132
jhedmonton 28:5905886c76ee 133 } else if(strcmp(Data[0], "$BTMODE") == 0) {
jhedmonton 28:5905886c76ee 134 _btMode = atoi(Data[1]);
jhedmonton 28:5905886c76ee 135
jhedmonton 28:5905886c76ee 136 } else {
jhedmonton 28:5905886c76ee 137 //bt->printf("Unrecognized config setting detected.\r\n");
jhedmonton 28:5905886c76ee 138 valid = false;
jhedmonton 28:5905886c76ee 139 }
jhedmonton 28:5905886c76ee 140
jhedmonton 28:5905886c76ee 141 // if(valid && !config)
jhedmonton 28:5905886c76ee 142 // bt->printf("Command Accepted.");
jhedmonton 28:5905886c76ee 143 }
jhedmonton 28:5905886c76ee 144