Diff: main.cpp

Revision:

4:e27ca0c82814

Parent:

3:9b6db94aa2b3

Child:

5:d6d8ecd418cf

--- a/main.cpp	Wed Sep 14 18:19:50 2016 +0000
+++ b/main.cpp	Thu Dec 08 20:38:24 2016 +0000
@@ -29,6 +29,8 @@
 
 #define DIRECT_MODE 0 //command maps to throttle and steer commands normalized
 #define COURSE_MODE 1 //Commands map to heading and speed
+#define HIL_MODE 0 // commands map to hardware active simulation
+#define SIL_MODE 1 // commands map to software only simulation
 
 //I2C    i2c(p9, p10); // SDA, SCL
 BNO055 imu(p9, p10);
@@ -77,6 +79,13 @@
 float Kp_psi, Kp_spd, Ki_psi, Ki_spd; // controller gains
 int led1,led2,led3,led4; // neo-strip variables?
 volatile bool newpacket = false; // boolean identifier of new odroid packet
+float x0; // initial x-position when in software or hardware in the loop simulation
+float y0; // initial y-position when in software or hardware in the loop simulation
+int sim_mode = 0; // sets simulation mode, zero by default, 1 for HIL, 2 for SIL
+float x = 0.0; // simulation variables
+float y = 0.0; // simulation variables
+float psi = 0.0; // simulation variables
+float spd = 0.005; // simulation speed
 
 void rxCallback(MODSERIAL_IRQ_INFO *q)
 {
@@ -138,6 +147,7 @@
     imu_LED = 0; // turn off IMU indicator (LED 4)
     gps.setRefPoint(REF_LAT,REF_LON,REF_ALT); // set local origin of reference frame for GPS conversion
 
+
     // procedure to ensure IMU is operating correctly
     if(imu.check()) {
         pc.printf("BNO055 connected\r\n");
@@ -150,17 +160,22 @@
         imu.set_mapping(2);
 
         // Blinks light if IMU is not calibrated, stops when calibration is complete
-        while(int(imu.calib) < 0xCF) {
+        /*while(int(imu.calib) < 0xCF) {
             pc.printf("Calibration = %x.\n\r",imu.calib);
             imu.get_calib();
             wait(0.5);
             imu_LED = !imu_LED;
         } // end while(imu.calib)
+        */
         imu_LED = 1; // turns on IMU light when calibration is successful
 
     } else {
-        pc.printf("IMU BNO055 NOT connected\r\n Program Trap."); // catch statement if IMU is not connected correctly
-        // turn on all lights is IMU is not connected correctly
+        pc.printf("IMU BNO055 NOT connected\r\n Entering Simulation mode..."); // catch statement if IMU is not connected correctly
+
+        sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL)
+
+
+        /* // turn on all lights is IMU is not connected correctly
         rc_LED = 1;
         armed_LED = 1;
         imu_LED = 1;
@@ -174,8 +189,11 @@
 
             wait(0.5);
         } // end while(1) {blink if IMU is not connected}
+        */
+
     } // end if(imu.check)
 
+
     //  int colors[4] = {ORANGE,YELLOW,GREEN,CYAN};
 
     pc.printf("Emaxx Navigation Program\r\n"); // print indication that calibration is good and nav program is running
@@ -208,7 +226,7 @@
 
         if(newpacket) { // if xbee port receives a complete message, parse it
             char buf[MAX_MESSAGE_SIZE];
-            
+
             // reads from modserial port buffer, stores characters into string "buf"
             int i = 0;
             if(xbee.rxBufferGetCount()>0) {
@@ -273,10 +291,16 @@
                     } // end default status in switch
 
                 } // end switch(cmd_mode)
-                Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
-                Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
 
-            } else { // goes with if auto_ctrl
+                if(sim_mode<2) { // only actuates if in experiment or HIL modes
+                    Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
+                    Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
+                } else {
+                    // won't send command to motor and servo if in SIL mode
+                }
+
+
+            } else { // goes with if auto_ctrl, manual control mode
 
                 Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                 Throttle.write(1500); //Write Throttle Pulse
@@ -293,41 +317,72 @@
 
         }/// end  else armed
 
-        imu.get_angles();
-        imu.get_accel();
-        imu.get_gyro();
-        imu.get_lia();
-        float dt = t.read()-t_imu;
-        if(dt > (1/IMU_RATE)) {
+        if(sim_mode==0) {
+            imu.get_angles();
+            imu.get_accel();
+            imu.get_gyro();
+            imu.get_lia();
+            float dt = t.read()-t_imu;
+            if(dt > (1/IMU_RATE)) {
+
+                float tic = t.read();
+                ekf.setRot(wrapToPi(imu.euler.yaw),imu.euler.pitch,imu.euler.roll);
+                ekf.timeUpdate(imu.lia.x,imu.lia.y,imu.lia.z,dt);
+
+                xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
+                xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
+                pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
+                pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
+
+                t_imu = t.read();
+            }
+
+            if(t.read()-t_gps >(1/GPS_RATE)) {
+                //printf("Kp_psi: %.2f Kp_spd: %.2f Ki_spd: %.2f\r\n", Kp_psi,Kp_spd,Ki_spd);
+
+
+                float tic2 = t.read();
+                ekf.measUpdate(gps.pos_north,gps.pos_east,gps.vel_north,gps.vel_east);
+                // xbee.printf("$GPS,%.8f,%.8f,%.3f,%.3f,%.3f\r\n",gps.dec_latitude,gps.dec_longitude,gps.msl_altitude,gps.course_d,KNOTS_2_MPS*gps.speed_k);
+                // xbee.printf("$STA,%d,%d,%d,%d\r\n",);
 
-            float tic = t.read();
-            ekf.setRot(wrapToPi(imu.euler.yaw),imu.euler.pitch,imu.euler.roll);
-            ekf.timeUpdate(imu.lia.x,imu.lia.y,imu.lia.z,dt);
+                t_gps = t.read();
+
+            }
+            wait(1/LOOP_RATE);
+            // status_LED=!status_LED;
+            auto_ctrl_old = auto_ctrl;
+
+        } else { // else condition implies a simulation mode is enabled
+
+            float dt = t.read()-t_imu;
+            if(dt > (1/IMU_RATE)) {
 
-            xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
-            xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
+                float tic = t.read();
+                
+                psi_err = wrapToPi(des_psi-psi);
+                spd_err = des_spd - spd;
+                spd_err_i += spd_err;
+                str = Kp_psi*psi_err/(spd+0.05);
+                thr =  Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE;
+                
+                x = x + spd*cos(psi)*dt;
+                y = y + spd*sin(psi)*dt;
+                psi = psi + str*dt;
+                spd = spd + thr*dt;
 
-            t_imu = t.read();
+                xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
+                pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
+                t_imu = t.read();
+            }
+
+
         }
 
-        if(t.read()-t_gps >(1/GPS_RATE)) {
-            //printf("Kp_psi: %.2f Kp_spd: %.2f Ki_spd: %.2f\r\n", Kp_psi,Kp_spd,Ki_spd);
-
-
-            float tic2 = t.read();
-            ekf.measUpdate(gps.pos_north,gps.pos_east,gps.vel_north,gps.vel_east);
-            // xbee.printf("$GPS,%.8f,%.8f,%.3f,%.3f,%.3f\r\n",gps.dec_latitude,gps.dec_longitude,gps.msl_altitude,gps.course_d,KNOTS_2_MPS*gps.speed_k);
-            // xbee.printf("$STA,%d,%d,%d,%d\r\n",);
 
-            t_gps = t.read();
+    } // end while(1)
 
-        }
-        wait(1/LOOP_RATE);
-        // status_LED=!status_LED;
-        auto_ctrl_old = auto_ctrl;
-    }
-
-}
+} // end main
 
 void parseMessage(char * msg)
 {
@@ -368,23 +423,23 @@
                 Kp_psi = arg1;
                 Kp_spd = arg2;
                 Ki_spd = arg3;
-                
+
                 pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
                 //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
                 break;
             }
             case 2: { // sets origin of local reference frame
-                
+
                 //pc.printf("%s\n",msg);
 
                 float ref_lat = arg1;
                 float ref_lon = arg2;
                 float ref_alt = arg3;
                 gps.setRefPoint(ref_lat,ref_lon,ref_alt);
-                
+
                 pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
                 //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
-                
+
                 break;
             }
             default: {
@@ -402,7 +457,34 @@
         int colors[4]= {arg1,arg1,arg1,arg1};
         float brightness = arg2;
         setLED(colors,brightness);
-    }
+    } // end of $LED
+
+
+    if(!strncmp(msg, "$SIM", 4)) {
+        int arg1;
+        float arg2,arg3,arg4;
+
+        sscanf(msg,"$SIM,%d,%f,%f,%f\n",&arg1,&arg2,&arg3,&arg4);
+        sim_mode = arg1; // sets whether in hardware in the loop or software in the loop (actuators active or not during simulation)
+        auto_clock = t.read();
+        switch (cmd_mode) {
+            case HIL_MODE: {
+                auto_ctrl = true;
+                x = arg2;
+                y = arg3;
+                psi = arg4;
+            }
+            case SIL_MODE: {
+                auto_ctrl = true;
+                x = arg2;
+                y = arg3;
+                psi = arg4;
+            }
+            default: {
+            }
+        }
+    }   //emd of $SIM
+