Diff: Config/Config.cpp

Revision:

0:826c6171fc1b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Config/Config.cpp	Wed Jun 20 14:57:48 2012 +0000
@@ -0,0 +1,151 @@
+#define MAXBUF 64
+
+#include "Config.h"
+#include "SDHCFileSystem.h"
+#include "GeoPosition.h"
+#include "globals.h"
+#include "util.h"
+
+extern Serial pc;
+
+// TODO: 3: mod waypoints to include speed after waypoint
+
+Config::Config()
+{
+    // not much to do here, yet.
+}
+
+// load configuration from filesystem
+bool Config::load()
+{
+    LocalFileSystem local("etc");         // Create the local filesystem under the name "local"
+    FILE *fp;
+    char buf[MAXBUF];   // buffer to read in data
+    char tmp[MAXBUF];   // temp buffer
+    char *p;
+    double lat, lon;
+    bool declFound = false;
+    bool confStatus = false;
+    
+    // Just to be safe let's wait
+    //wait(2.0);
+
+    pc.printf("opening config file...\n");
+    
+    fp = fopen("/etc/config.txt", "r");
+    if (fp == 0) {
+        pc.printf("Could not open config.txt\n");
+    } else {
+        wptCount = 0;
+        declination = 0.0;
+        while (!feof(fp)) {
+            fgets(buf, MAXBUF-1, fp);
+            p = split(tmp, buf, MAXBUF, ',');           // split off the first field
+            switch (tmp[0]) {
+                case 'B' :
+                    p = split(tmp, p, MAXBUF, ',');     // threshold distance for curb avoid
+                    curbThreshold = cvstof(tmp);
+                    p = split(tmp, p, MAXBUF, ',');     // steering gain for curb avoid
+                    curbGain = cvstof(tmp);
+                    break;
+                case 'W' :                              // Waypoint
+                    p = split(tmp, p, MAXBUF, ',');     // split off the latitude to tmp
+                    lat = cvstof(tmp);
+                    p = split(tmp, p, MAXBUF, ',');     // split off the longitude to tmp
+                    lon = cvstof(tmp);
+                    if (wptCount < MAXWPT) {
+                        wpt[wptCount].set(lat, lon);
+                        wptCount++;
+                    }
+                    break;
+                case 'G' :                              // GPS
+                    p = split(tmp, p, MAXBUF, ',');
+                    gpsBaud = atoi(tmp);                // baud rate for gps
+                    p = split(tmp, p, MAXBUF, ',');
+                    gpsType = atoi(tmp);
+                    break;
+                case 'Y' :                              // Gyro Bias
+                    p = split(tmp, p, MAXBUF, ',');     // split off the declination to tmp
+                    gyroBias = (float) cvstof(tmp);
+                    break;
+                case 'D' :                              // Compass Declination
+                    p = split(tmp, p, MAXBUF, ',');     // split off the declination to tmp
+                    declination = (float) cvstof(tmp);
+                    declFound = true;
+                    break;
+                case 'N' :                                  // Navigation and Turning    
+                    p = split(tmp, p, MAXBUF, ',');
+                    interceptDist = (float) cvstof(tmp);    // intercept distance for steering algorithm
+                    p = split(tmp, p, MAXBUF, ',');
+                    waypointDist = (float) cvstof(tmp);     // distance before waypoint switch
+                    p = split(tmp, p, MAXBUF, ',');
+                    brakeDist = (float) cvstof(tmp);        // distance at which braking starts
+                    p = split(tmp, p, MAXBUF, ',');
+                    minRadius = (float) cvstof(tmp);        // minimum turning radius
+                    break;
+                case 'M' :                              // Magnetometer
+                    for (int i=0; i < 3; i++) {
+                        p = split(tmp, p, MAXBUF, ',');
+                        magOffset[i] = (float) cvstof(tmp);
+                        pc.printf("magOffset[%d]: %.2f\n", i, magOffset[i]);
+                    }
+                    for (int i=0; i < 3; i++) {
+                        p = split(tmp, p, MAXBUF, ',');
+                        magScale[i] = (float) cvstof(tmp);
+                        pc.printf("magScale[%d]: %.2f\n", i, magScale[i]);
+                    }                    
+                case 'R' : // Steering configuration
+                    p = split(tmp, p, MAXBUF, ',');
+                    steerZero = cvstof(tmp);            // servo center setting
+                    p = split(tmp, p, MAXBUF, ',');
+                    steerGain = cvstof(tmp);            // steering angle multiplier
+                    p = split(tmp, p, MAXBUF, ',');
+                    steerGainAngle = cvstof(tmp);       // angle below which steering gain takes effect
+                    p = split(tmp, p, MAXBUF, ',');
+                    cteKi = cvstof(tmp);                // integral term for cross track
+                    p = split(tmp, p, MAXBUF, ',');
+                    usePP = ( atoi(tmp) == 1 );         // use pure pursuit algorithm
+                    break;
+                case 'S' :                              // Throttle configuration
+                    p = split(tmp, p, MAXBUF, ',');
+                    escMin = atoi(tmp);                 // minimum esc (brake) setting
+                    p = split(tmp, p, MAXBUF, ',');     
+                    escZero = atoi(tmp);                // esc center/zero setting
+                    p = split(tmp, p, MAXBUF, ',');     
+                    escMax = atoi(tmp);                 // maximum esc setting
+                    p = split(tmp, p, MAXBUF, ',');   
+                    topSpeed = cvstof(tmp);             // desired top speed
+                    p = split(tmp, p, MAXBUF, ',');   
+                    turnSpeed = cvstof(tmp);            // speed to use within brake distance of waypoint
+                    p = split(tmp, p, MAXBUF, ',');
+                    startSpeed = cvstof(tmp);            // speed to use at start
+                    p = split(tmp, p, MAXBUF, ',');
+                    speedKp = cvstof(tmp);              // speed PID: proportional gain
+                    p = split(tmp, p, MAXBUF, ',');     
+                    speedKi = cvstof(tmp);              // speed PID: integral gain
+                    p = split(tmp, p, MAXBUF, ',');     
+                    speedKd = cvstof(tmp);              // speed PID: derivative gain
+    
+                    break;
+                case 'E' :
+                    p = split(tmp, p, MAXBUF, ',');     
+                    compassGain = (float) cvstof(tmp);  // not used (DCM)
+                    p = split(tmp, p, MAXBUF, ',');    
+                    yawGain = (float) cvstof(tmp);      // not used (DCM)
+                default :
+                    break;
+            } // switch
+        } // while
+
+        // Did we get the values we were looking for?
+        if (wptCount > 0 && declFound) {
+            confStatus = true;
+        }
+        
+    } // if fp
+    
+    if (fp != 0)
+        fclose(fp);
+
+    return confStatus;
+}