Diff: base/base.cpp

Revision:

28:5905886c76ee

Child:

30:3afafa1ef16b

diff -r 9ac59b261d87 -r 5905886c76ee base/base.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/base/base.cpp	Wed Jan 21 02:54:26 2015 +0000
@@ -0,0 +1,144 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include <string>
+#include "base.h"
+#include "Config.h"
+
+
+
+int debug=1; //set to 0 to disable output to USB, set to 1 to output data to USB
+
+DigitalOut ledGREEN(p11);
+DigitalOut ledRED(p12);
+
+
+
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+
+extern "C" void mbed_mac_address(char *s);
+int _ID = 0; //stores this mbed's ID
+
+// Connect the TX of the GPS module to p10 RX input
+MODSERIAL gps(p9, p10); //GPS
+MODSERIAL pc(USBTX, USBRX);
+MODSERIAL bluetooth(p13, p14);
+
+
+// GLOBAL VALUES ONLY=====================
+// values which are stored in config.txt         //
+int gyro_position = 0;
+double antennaheight = 200;
+int _btMode = 0; // 0=Send Everything, 1 = No GPS, 2 = Receive only
+
+//Steering
+double lookaheadtime = 2;
+double scale = 1;
+double phaseadv = 50;
+double tcenter = 5;
+double fgain = 125;
+double avgpos = -4;
+// in prevision of PID addition to FreePilot
+double kp = 0;
+double ki = 0;
+double kd = 0;
+
+int gps_baud = 38400; //default at 115200, but FGPS will pass the real baud-rate.
+
+//offsets
+double w_xBias;
+double w_yBias;
+double w_zBias;
+double a_xBias;
+double a_yBias;
+double a_zBias;
+
+bool Authenticated = 0;
+
+void Dispatch(char* line, bool config /* = false */)
+{   
+    char* pointer;
+    char* Data[5];
+    int index = 0;
+    
+    bool valid = true;
+    
+    //Split data at commas
+    pointer = strtok(line, ",");
+
+    if(pointer == NULL)
+        Data[0] = line;
+
+    while(pointer != NULL) {
+        Data[index] = pointer;
+        pointer = strtok(NULL, ",");
+        index++;
+    }
+
+    //Check ID of read data and set the corresponding variable.
+    if(strcmp(Data[0], "$ID") == 0) {
+        _ID = atoi(Data[1]);
+        
+        if(Config_GetID() == _ID)
+        {
+            // bt->printf("Board ID Matches.\r\n");
+             Authenticated = true;
+        }
+        else
+        {
+             Authenticated = false;
+           //  bt->printf("Board ID does not match.\r\n");
+        }
+        
+    } else if(strcmp(Data[0], "$BANY") == 0) {
+        
+       // if(!Authenticated)
+        //    RestartRequired = true;
+        
+        _ID = Config_GetID();
+        Config_Save();
+        
+       // bt->printf("Adress set: %d \r\n", _ID);
+
+    } else if(strcmp(Data[0], "$PA") == 0) {
+        phaseadv = atof(Data[1]);
+
+    } else if(strcmp(Data[0], "$TC") == 0) {
+        tcenter = atof(Data[1]);
+
+    } else if(strcmp(Data[0], "$FG") == 0) {
+        fgain = atof(Data[1]);
+
+    } else if(strcmp(Data[0], "$SC") == 0) {
+        scale = atof(Data[1]);
+
+    } else if(strcmp(Data[0], "$AP") == 0) {
+        avgpos = atof(Data[1]);
+
+    //} 
+    //else if(strcmp(Data[0], "$ASTEER") == 0) {
+    //    j_guidance = atoi(Data[1]);
+
+    } 
+    else if(strcmp(Data[0], "$PCALIVE") == 0) {
+        //pc connection watchdog
+
+    } else if(strcmp(Data[0], "$SAVE") == 0) {
+        Config_Save();
+
+    } else if(strcmp(Data[0], "$BTMODE") == 0) {
+        _btMode = atoi(Data[1]);
+
+    } else {
+        //bt->printf("Unrecognized config setting detected.\r\n");
+        valid = false;
+    }
+        
+   // if(valid && !config)
+   //     bt->printf("Command Accepted.");    
+}
+