Revision 87:6d95f853dab3, committed 2019-05-08

Comitter:

joel_ssc

Date:

Wed May 08 13:24:04 2019 +0000

Parent:

86:ba3a118b0080

Child:

88:1813f583cee9

Commit message:

caught up all but main.cpp to 18apr version

Changed in this revision

--- a/ConfigFileIO/ConfigFileIO.cpp	Thu May 02 20:34:16 2019 +0000
+++ b/ConfigFileIO/ConfigFileIO.cpp	Wed May 08 13:24:04 2019 +0000
@@ -354,7 +354,7 @@
     if (!cfg.read("/local/logvers.txt")) {   //   "/local/leg_mission.txt"
             error("Lognames version file logvers.txt  File Read Error");
     }
-    char value[BUFSIZ];
+    char value[BUFSIZ]; //  chang
  
     
     if (cfg.getValue("LogFileVers", &value[0] , sizeof(value))) {
@@ -372,7 +372,7 @@
         sprintf(buf, "in configfileIO: diag file version number is %d\n", version);
         if(print_diag == 1) {mbedLogger().appendDiagFile(buf,3);}
         //  buf[256]= {0};
-        sprintf(buf, "in ConfigFileIO:   diag file string is DIAG%03d.txt", version);
+        sprintf(buf, "in ConfigFileIO:   diag file string is DIAG%03d.txt\n", version);
         if(print_diag == 1) {mbedLogger().appendDiagFile(buf,3);}
         // buf[256]= {0};
         sprintf(buf, "DIAG%03d.txt", version);
@@ -385,6 +385,74 @@
     }
     return count;
 }
+int ConfigFileIO::load_setneutral_status() { 
+    ConfigFile cfg;
+    int count = 0;
+    int setval = 0;
+    int bce_mm = 240;
+    int batt_mm = 40;
+    if (!cfg.read("/local/neutral.txt")) {   
+       error("setneutral file neutral.txt  File Read Error");
+       //  set values in struct to defaults??
+    }
+    char value[BUFSIZ];
+ 
+    
+    if (cfg.getValue("neutral_set", &value[0] , sizeof(value))) {
+        setval = atoi(value);  
+        neutralStruct.setval = setval;
+        count++;
+    }
+    if (cfg.getValue("bce_neutral_mm", &value[0] , sizeof(value))) {
+        bce_mm = atoi(value);  
+        char buf[256];
+        sprintf(buf, "in configfileIO: bce_neutral_mm  is %d\n", bce_mm);
+        mbedLogger().appendDiagFile(buf,3);
+        //  buf[256]= {0};
+        neutralStruct.bce_neutral_mm = bce_mm;
+        count++;
+    }
+    if (cfg.getValue("batt_neutral_mm", &value[0] , sizeof(value))) {
+        batt_mm = atoi(value);  
+        char buf[256];
+        sprintf(buf, "in configfileIO: batt_neutral_mm  is %d\n", batt_mm);
+        mbedLogger().appendDiagFile(buf,3);
+        //  buf[256]= {0};
+        neutralStruct.batt_neutral_mm = batt_mm;
+        count++;
+    }
+    return count;
+}
+void ConfigFileIO::saveNeutralStatus(int setval, int bce_neutral_mm, int batt_neutral_mm) {
+    ConfigFile neutral_txt;    
+       
+    //convert input numbers into text and save them to text file
+    char string_setval[128];  
+    sprintf(string_setval, "%d", setval);
+    neutral_txt.setValue("# Neutral set status and values\nneutral_set", string_setval);
+    
+    char string_bce[128];  
+    sprintf(string_bce, "%d", bce_neutral_mm);
+    neutral_txt.setValue("bce_neutral_mm", string_bce);
+    
+    char string_batt[128];  
+    sprintf(string_batt, "%d", batt_neutral_mm);
+    neutral_txt.setValue("batt_neutral_mm", string_batt);
+
+        
+    //SAVE THE DATA!    also put the data insto the neutral struct
+    neutralStruct.batt_neutral_mm = batt_neutral_mm; 
+    neutralStruct.bce_neutral_mm = bce_neutral_mm;
+    neutralStruct.setval = setval;
+    xbee().printf("(ConfigFileIO) Saving neutral set status  parameters!");
+    
+    if (!neutral_txt.write("/local/neutral.txt")) {
+        xbee().printf("\n\rERROR: (SAVE) Failure to write neutral.txt file.");
+    }
+    else {
+        xbee().printf("\n\rFile neutral.txt successful written.\n\r");
+    }  
+}
 int ConfigFileIO::load_BATT_config() { // I could copy this mode to read one line of a leg_mission file. One line or multiple lines?
     ConfigFile cfg;
     int count = 0;
@@ -429,7 +497,57 @@
     
     return count;     
 }
-
+void ConfigFileIO::report_still_inverted( float roll_value, int yotime) {
+     ConfigFile still_inverted;
+      
+      char string_yotime[128];  
+    sprintf(string_yotime, "%d", yotime);
+    still_inverted.setValue("# Still Inverted after START_SWIM yo timeout timeout\n yo_time", string_yotime);
+    
+    char string_roll[128];  
+    sprintf(string_roll, "%f", roll_value);
+    still_inverted.setValue("inverted_roll_value", string_roll);
+    
+            
+    //SAVE THE DATA!   
+    
+    xbee().printf("(ConfigFileIO) Saving still_inverted status after start_swim timeout!");
+    
+    if (!still_inverted.write("/local/inverted.txt")) {  // I assume this will build a new file if one is not already there
+        xbee().printf("\n\rERROR: (SAVE) Failure to write inverted.txt file.");
+    }
+    else {
+        xbee().printf("\n\rFile inverted.txt  written.\n\r");  // raspberry Pi will need to delete this file on finding it
+    } 
+     
+    }
+void ConfigFileIO::report_no_neutral_found(int bce_last_pos, int batt_last_pos) {  // this will tell the Raspberry Pi that neutral was not found -- BAD
+    ConfigFile no_neutral;    
+       int setval = 0;
+    //convert input numbers into text and save them to text file
+    char string_setval[128];  
+    sprintf(string_setval, "%d", setval);
+    no_neutral.setValue("# Find_Neutral  success or failure\nneutral_found", string_setval);
+    
+    char string_batt[128];  
+    sprintf(string_batt, "%d", batt_last_pos);
+    no_neutral.setValue("batt_last_position", string_batt);
+    
+    sprintf(string_batt, "%d", bce_last_pos);
+    no_neutral.setValue("bce_last_position", string_batt);
+    
+        
+    //SAVE THE DATA!    also put the data insto the neutral struct
+    
+    xbee().printf("(ConfigFileIO) Saving find_neutral failure status!");
+    
+    if (!no_neutral.write("/local/no_float.txt")) {  // I assume this will build a new file if one is not already there
+        xbee().printf("\n\rERROR: (SAVE) Failure to write no_float.txt file.");
+    }
+    else {
+        xbee().printf("\n\rFile no_float.txt  written.\n\r");
+    } 
+}
 int ConfigFileIO::load_DEPTH_config() {
     ConfigFile cfg;
     int count = 0;

--- a/ConfigFileIO/ConfigFileIO.hpp	Thu May 02 20:34:16 2019 +0000
+++ b/ConfigFileIO/ConfigFileIO.hpp	Wed May 08 13:24:04 2019 +0000
@@ -10,14 +10,20 @@
     int logversion; 
     int diagversion;
 };
+struct neutralStatus {
+     int setval;
+     int bce_neutral_mm;
+     int batt_neutral_mm;
+};
 
     
 class ConfigFileIO {
 public:
     ConfigFileIO();
-    logFileNames logFilesStruct;
-    //modified this to save frequency and deadband 7/11/2018
-    
+    logFileNames logFilesStruct; 
+    neutralStatus neutralStruct;
+      
+    //modified savebatt to save frequency and deadband 7/11/2018
     void saveBattData(float batt_p_gain, float batt_i_gain, float batt_d_gain, int batt_zeroOffset, float batt_filter_freq, float batt_deadband); //modified this because zero offsets are integers
     void savePitchData(float pitch_p_gain, float pitch_i_gain, float pitch_d_gain, float pitch_zeroOffset, float pitch_filter_freq, float pitch_deadband);
     
@@ -30,6 +36,10 @@
     //ConfigFunctions
     int load_BCE_config();
     int load_LogVers_config(int print_diag);    // jcw feb 2019
+    void saveNeutralStatus(int setval, int bce_neutral_mm, int batt_neutral_mm);
+    int load_setneutral_status();
+    void report_no_neutral_found(int bce_last_pos, int batt_last_pos);
+    void report_still_inverted( float roll_value, int yotime);
     void saveLogVersData(int logversion,  int diagversion);
     int load_BATT_config();
     int load_DEPTH_config();

--- a/MbedLogger/MbedLogger.cpp	Thu May 02 20:34:16 2019 +0000
+++ b/MbedLogger/MbedLogger.cpp	Wed May 08 13:24:04 2019 +0000
@@ -29,6 +29,7 @@
     _default_timestamp_time = 1518467832;
     
     _end_sequence_transmission = false;
+    _time_set =0;
 }
 
 //this function has to be called for the time to function correctly
@@ -38,6 +39,7 @@
         else { set_time(setting_time); _default_timestamp_time = setting_time; } 
     xbee().printf("\n%s log time set.\n\r", _file_system_string.c_str());
     // set_time(time_stamp);   // Set RTC time to Mon, 12 FEB 2018 15:37
+    _time_set =1;
 }
 
 //in the future create the ability to set the start time
@@ -50,6 +52,7 @@
 void MbedLogger::recordData(int current_state) {
     int data_log_time = mbedLogger().getSystemTime();                 //read the system timer to get unix timestamp
     int start_time = 1518467832;
+    if(_time_set) { start_time = _default_timestamp_time;}
     _data_log[0] = depthLoop().getCommand();        //depth command
     _data_log[1] = depthLoop().getPosition();       //depth reading (filtered depth)
     _data_log[2] = pitchLoop().getCommand();        //pitch command
@@ -95,7 +98,8 @@
     _data_log[34] = headingLoop().getFilterFrequency();
     _data_log[35] = headingLoop().getDeadband();
     
-    string string_state;
+    string string_state;           string_state= "UNKNOWN";  //default just in case.
+    
     if (current_state == SIT_IDLE)
         string_state = "SIT_IDLE";
     else if (current_state == FIND_NEUTRAL)
@@ -132,6 +136,10 @@
         string_state = "LEG_POS_RISE";
     else if (current_state == FB_EXIT) 
         string_state = "FB_EXIT"; 
+    else if (current_state == START_SWIM) 
+        string_state = "START_SWIM"; 
+    else if (current_state == FLYING_IDLE) 
+        string_state = "FLYING_IDLE"; 
     else if (current_state == ENDLEG_WAIT) 
         string_state = "ENDLEG_WAIT";        
         
@@ -1275,7 +1283,8 @@
 void MbedLogger::appendDiagFile(char *printf_string, int flushclose) {
     //
     
- int start_time = 1518467832;
+    int start_time = 1518467832;
+    if(_time_set) { start_time = _default_timestamp_time;}
  //in the future create the ability to set the start time
 
     int time_now = mbedLogger().getSystemTime(); 

--- a/MbedLogger/MbedLogger.hpp	Thu May 02 20:34:16 2019 +0000
+++ b/MbedLogger/MbedLogger.hpp	Wed May 08 13:24:04 2019 +0000
@@ -225,6 +225,7 @@
     int _confirmed_packet_number;
     
     int _default_timestamp_time;
+    int _time_set;
     
     bool _still_transmitting_data;
     

--- a/OuterLoop/OuterLoop.cpp	Thu May 02 20:34:16 2019 +0000
+++ b/OuterLoop/OuterLoop.cpp	Wed May 08 13:24:04 2019 +0000
@@ -22,6 +22,7 @@
     
     if (_sensor == 2) {
         _pid.setHeadingFlag(true);  //used to update the handling for the heading PID control loop
+        _filter.setHeadingFlag(true);
     }
 }
 
@@ -62,6 +63,9 @@
         _sensorVal = imu().getPitch();
     } else if (_sensor == 2) {
         _sensorVal = imu().getHeading();
+    } else if (_sensor == 3) {
+      _sensorVal = sensors().getAltimeterChannelReadings();  // 27mar2019    this is still in counts
+     //    _sensorVal = imu().getPitch();  // 27mar2019   testing sensors error?
     } else {
         error("\n\r This sensor option does not exist");
     }

--- a/PosVelFilter/PosVelFilter.cpp	Thu May 02 20:34:16 2019 +0000
+++ b/PosVelFilter/PosVelFilter.cpp	Wed May 08 13:24:04 2019 +0000
@@ -1,25 +1,61 @@
 #include "PosVelFilter.hpp"
 //#include "StaticDefs.hpp"
 
-PosVelFilter::PosVelFilter() {
+PosVelFilter::PosVelFilter() {  // if heading_flag is set, operate on sine and cosine, return atan2(sin, cos) as postion and velocity??
     x1 = 0; // pseudo position state
     x2 = 0; // pseudo velocity state
-    
-    w_n = 1.0; // natural frequency of the filter bigger increases frequency response
+    x1c = 0;
+    x2c = 0;
+    x1s = 0;
+    x2s = 0;
+    w_n = 1.0; // natural frequency of the filter:   bigger increases frequency response
+    _headingFlag = false;
 }
 
 //run the pos-vel estimate filter
 void PosVelFilter::update(float deltaT, float counts) {
     dt = deltaT;
+    
+    if(!_headingFlag) {
+      x1_dot = x2;
+      x2_dot = (-2.0*w_n*x2) - (w_n*w_n)*x1 + (w_n*w_n)*counts;
 
-    x1_dot = x2;
-    x2_dot = (-2.0*w_n*x2) - (w_n*w_n)*x1 + (w_n*w_n)*counts;
+      position = x1;
+      velocity = x2;
+
+      x1 += x1_dot*dt;
+      x2 += x2_dot*dt;
+    }
+    if(_headingFlag) {
+        counts_c = cos(counts * _PI/180.0);  // math.h cos and sine operate on radians , counts is heading degrees
+        counts_s = sin(counts * _PI/180.0);
+        // the cosine part
+        x1c_dot = x2c;
+        x2c_dot = (-2.0*w_n*x2c) - (w_n*w_n)*x1c + (w_n*w_n)*counts_c;
+
+        position_c = x1c;
+        velocity_c = x2c;
 
-    position = x1;
-    velocity = x2;
+        x1c += x1c_dot*dt;
+        x2c += x2c_dot*dt;
+          //   now the sine portion
+        x1s_dot = x2s;
+        x2s_dot = (-2.0*w_n*x2s) - (w_n*w_n)*x1s + (w_n*w_n)*counts_s;
+
+        position_s = x1s;
+        velocity_s = x2s;
 
-    x1 += x1_dot*dt;
-    x2 += x2_dot*dt;
+        x1s += x1s_dot*dt;
+        x2s += x2s_dot*dt;
+      // now reconstitute the angle and the angular velocity
+        position = atan2(position_s, position_c)  * 180.0/_PI;
+        pnew = atan2(x1s,x1c) * 180/_PI;  // the next value, since the update has happened by now.
+        velocity = (pnew - position) ;  // don't multiply by dt yet, now just an angular difference
+        if(position < 0 ) { position = position +360; }
+        if(velocity  > 180 ) { velocity -= 360;  }
+        if(velocity <= -180)  { velocity += 360 ;}
+        velocity *= (1.0/dt);
+    }
 }
 
 float PosVelFilter::getPosition() {
@@ -29,6 +65,11 @@
 float PosVelFilter::getVelocity() {
     return velocity;
 }
+void PosVelFilter::setHeadingFlag(bool heading_flag) {
+    _headingFlag = heading_flag;
+}
+
+    
 
 float PosVelFilter::getDt() {
     return dt;
@@ -36,4 +77,4 @@
 
 void PosVelFilter::writeWn(float wn) {
     w_n = wn;
-}
\ No newline at end of file
+}

--- a/PosVelFilter/PosVelFilter.hpp	Thu May 02 20:34:16 2019 +0000
+++ b/PosVelFilter/PosVelFilter.hpp	Wed May 08 13:24:04 2019 +0000
@@ -2,6 +2,8 @@
 #define POSVELFILTER_H
 
 #include "mbed.h"
+#include <deque>
+#define _PI ((float) 3.14159265359)
 
 class PosVelFilter
 {
@@ -15,19 +17,21 @@
     float getPosition();
     float getVelocity();
     float getDt();
-    
+    void setHeadingFlag(bool heading_flag);    
     void writeWn(float wn);
     
 protected:
-    float x1;
-    float x2;
-    float x2_dot;
-    float x1_dot;
+    float x1, x1c,x1s;
+    float x2, x2c,x2s;
+    float x1_dot, x1c_dot, x1s_dot;
+    float x2_dot, x2c_dot, x2s_dot;
     float w_n; 
+    float counts_c, counts_s;
     
     float dt;
-    float position;
-    float velocity;
+    float position,pnew,  position_c, position_s;
+    float velocity, velocity_c, velocity_s;
+    bool _headingFlag;
 };
 
 #endif
\ No newline at end of file

--- a/SequenceController/SequenceController.cpp	Thu May 02 20:34:16 2019 +0000
+++ b/SequenceController/SequenceController.cpp	Wed May 08 13:24:04 2019 +0000
@@ -228,7 +228,7 @@
         
             if (read_leg_cfg.getValue(buf, &value[0], sizeof(value))) {
                 xbee().printf("\n\rsequence %d = %s",i,value);
-                sprintf(bux2, "\n\r leg values sequence %d = %s",i,value);
+                sprintf(bux2, "\n\r leg values sequence %d = %s\n",i,value);
                 mbedLogger().appendDiagFile(bux2,0);
                 
                 legStructLoaded[i] = process(value); //create the structs using process(string randomstring)
@@ -279,20 +279,30 @@
     }
     /* LPD */
     
-    /* neutral */
-    if ((signed int) randomstring.find("neutral") != -1) {
-        loadStruct.title = "neutral";
-        xbee().printf("\n\rLOAD neutral. %d", randomstring.find("neutral"));
-        loadStruct.state = FIND_NEUTRAL;
+    /* start-swim */
+    if ((signed int) randomstring.find("start_swim") != -1) {
+        loadStruct.title = "start_swim";
+        xbee().printf("\n\rLOAD start-swim %d", randomstring.find("start_swim"));
+        loadStruct.state = START_SWIM;
+        sprintf(bux2, "\n\r process leg file: found START-SWIM  label, setting state to same\n");
+                mbedLogger().appendDiagFile(bux2,3);
     }
-    /* neutral */
-    
+    /* start-swim */
+    /* flying_idle */
+    if ((signed int) randomstring.find("flying_idle") != -1) {
+        loadStruct.title = "flying_idle";
+        xbee().printf("\n\rLOAD flying-idle found at %d", randomstring.find("flying_idle"));
+        loadStruct.state = FLYING_IDLE;
+        sprintf(bux2, "\n\r process leg file: found FLYING_IDLE  label, setting state to FLYING_IDLE \n");
+                mbedLogger().appendDiagFile(bux2,3);
+    }
+    /* flying_idle */
     /* EXIT */
     if ((signed int) randomstring.find("exit") != -1) {
         loadStruct.title = "exit";
         xbee().printf("\n\rReminder. Exit command is state FLOAT_BROADCAST\n\r");
         loadStruct.state = FLOAT_BROADCAST; //this is the new exit condition of the dive-rise sequence (11/4/17)
-                sprintf(bux2, "\n\r process legfile: found exit key callcount=%d\n", callcount);
+                sprintf(bux2, "\n\r process(valuestring) legfile: found exit key.  Callcount=%d\n", callcount);
                 mbedLogger().appendDiagFile(bux2,3);
     }
     /* EXIT */
@@ -300,67 +310,67 @@
     /*  max DEPTH TO  cycle to */
  
     if ((signed int) randomstring.find("max_depth") != -1) {
-        if (randomstring.find("neutral") || randomstring.find("leg")) {
-            int depth_pos = randomstring.find("max_depth") + 10;     //11 in example literally "depth="
-            char depth_array[256] = {0};    //clear memory
-            int depth_counter = 0;
-            for (int i = depth_pos; i < randomstring.length(); i++) {
-                if (cstr[i] == ',') 
-                    break;
-                else if (cstr[i] == ';') 
-                    break;
-                else {
-                    depth_array[depth_counter] = cstr[i]; 
-                    depth_counter++;
-                }
+
+        int depth_pos = randomstring.find("max_depth") + 10;     //11 in example literally "depth="
+        char depth_array[256] = {0};    //clear memory
+        int depth_counter = 0;
+        for (int i = depth_pos; i < randomstring.length(); i++) {
+            if (cstr[i] == ',')
+                break;
+            else if (cstr[i] == ';')
+                break;
+            else {
+                depth_array[depth_counter] = cstr[i];
+                depth_counter++;
             }
-            loadStruct.max_depth = atof(depth_array);
-                sprintf(bux2, "\n\r process legfile: key=max_depth val=%g  process(legstring)count=%d \n", atof(depth_array), callcount);
-                mbedLogger().appendDiagFile(bux2,3);
         }
-    }    
+        loadStruct.max_depth = atof(depth_array);
+        sprintf(bux2, "\n\r process legfile: key=max_depth val=%g  process(legstring)count=%d \n", atof(depth_array), callcount);
+        mbedLogger().appendDiagFile(bux2,3);
+
+    }
     
  
-       if ((signed int) randomstring.find("min_depth") != -1) {
-        if (randomstring.find("neutral") || randomstring.find("leg")) {
-            int depth_pos = randomstring.find("min_depth") + 10;     //11 in example literally "depth="
-            char depth_array[256] = {0};    //clear memory
-            int depth_counter = 0;
-            for (int i = depth_pos; i < randomstring.length(); i++) {
-                if (cstr[i] == ',') 
-                    break;
-                else if (cstr[i] == ';') 
-                    break;
-                else {
-                    depth_array[depth_counter] = cstr[i]; 
-                    depth_counter++;
-                }
+    if ((signed int) randomstring.find("min_depth") != -1) {
+
+        int depth_pos = randomstring.find("min_depth") + 10;     //11 in example literally "depth="
+        char depth_array[256] = {0};    //clear memory
+        int depth_counter = 0;
+        for (int i = depth_pos; i < randomstring.length(); i++) {
+            if (cstr[i] == ',')
+                break;
+            else if (cstr[i] == ';')
+                break;
+            else {
+                depth_array[depth_counter] = cstr[i];
+                depth_counter++;
             }
-            loadStruct.min_depth = atof(depth_array);
-            sprintf(bux2, "\n\r process legfile: key=min_depth val=%g \n", atof(depth_array));
-                mbedLogger().appendDiagFile(bux2,3);
-            }
-    }    
-          if ((signed int) randomstring.find("heading") != -1) {
-        if (randomstring.find("neutral") || randomstring.find("leg")) {
+        }
+        loadStruct.min_depth = atof(depth_array);
+        sprintf(bux2, "\n\r process legfile: key=min_depth val=%g \n", atof(depth_array));
+        mbedLogger().appendDiagFile(bux2,3);
+
+    }
+    if ((signed int) randomstring.find("heading") != -1) {
+        
             int depth_pos = randomstring.find("heading") + 8;     //11 in example literally "depth="
             char depth_array[256] = {0};    //clear memory
             int depth_counter = 0;
             for (int i = depth_pos; i < randomstring.length(); i++) {
-                if (cstr[i] == ',') 
+                if (cstr[i] == ',')
                     break;
-                else if (cstr[i] == ';') 
+                else if (cstr[i] == ';')
                     break;
                 else {
-                    depth_array[depth_counter] = cstr[i]; 
+                    depth_array[depth_counter] = cstr[i];
                     depth_counter++;
-                    }
+                }
             }
             loadStruct.heading = atof(depth_array);
             sprintf(bux2, "\n\r process legfile: key=heading val=%g \n", atof(depth_array));
-                mbedLogger().appendDiagFile(bux2,3);
-        }
-    }    
+            mbedLogger().appendDiagFile(bux2,3);
+        
+    }
     /* DEPTH TO FLOAT */
     
     

--- a/SequenceController/SequenceController.hpp	Thu May 02 20:34:16 2019 +0000
+++ b/SequenceController/SequenceController.hpp	Wed May 08 13:24:04 2019 +0000
@@ -61,7 +61,7 @@
 public:
     LegController();
     
-    legStruct legStructLoaded[256];
+    legStruct legStructLoaded[32];
     
     int loadLeg();
     

--- a/StateMachine/StateMachine.cpp	Thu May 02 20:34:16 2019 +0000
+++ b/StateMachine/StateMachine.cpp	Wed May 08 13:24:04 2019 +0000
@@ -9,18 +9,25 @@
  
     _bceFloatPosition = bce().getTravelLimit();      // bce position for "float" states                  (max travel limit for BCE is 320 mm)
     _battFloatPosition = batt().getTravelLimit();    // batt position tail high for "broadcast" state    (max travel limit for battery is 75 mm)
- 
+    
+    _disconnect_batt_pos_mm = _battFloatPosition;  // all the way forward
+    _batt_flying_pos_mm  = 22.0;
+    _timeout_splashdown = 120;  // two minutes??
+    _motorDisconnect_triggered = 0;
     _depth_command = 2.0;                        // user keyboard depth (default)
     _pitch_command = -20.0;                      // user keyboard pitch (default)
     _heading_command = 0.0;
-    
+    _start_swim_entry = SIT_IDLE;
+    _neutral_entry_state = SIT_IDLE;
     //new commands
     _BCE_dive_offset = 0.0;     //starting at the limits
     _BMM_dive_offset = 0.0;
     //new commands
     
     _neutral_timer        = 0;                  //timer used in FIND_NEUTRAL sub-FSM
+    _neutral_success = 0;    // used if find neutral has not been run and neutral_setval is also not 1 from neutral.txt file
 ////////////////////////////// 
+
     _state = SIT_IDLE;                          // select starting state here
     _isTimeoutRunning = false;                  // default timer to not running
     _isSubStateTimerRunning = false;            // default timer to not running
@@ -79,7 +86,51 @@
           _isTimeoutRunning = 1;
           }
           keyboard();  //keyboard function now needs to know about this state and its timeout
-          break;  
+          break; 
+        case FLYING_IDLE:   // wait for some signal and the move the battery by a known amount
+            //  then wait until you drop to START_SWIM on a timeout
+            if (!_isTimeoutRunning)  {      //     start in the correct state, tell it once where to move
+                _fsm_timer.reset();
+                _fsm_timer.start();
+                _isTimeoutRunning = 1;
+                batt().unpause();
+                batt().setPosition_mm(_batt_flying_pos_mm); // do nothing - may include not bothering to log data - yes both SIT_IDLE and FLYING_IDLE do not log  27mar2019
+                sprintf(buf, "FLYING_IDLE started in statemachine\n\n\r");
+                mbedLogger().appendDiagFile(buf,0);
+            }
+            if (motorDisconnect().read() || (_fsm_timer >  13))  {     //  for real case, only test is motorDisconnect().read(), timeout check is for testing only
+                _motorDisconnect_triggered = 1;  // once hardware has switched once, do not need it again
+                // move the battery
+                // reset and start the timer for post disconnect wait
+                sprintf(buf, "FLYING_IDLE:  motorDisconnect.read() at timeout=13sec, to end\n\r");
+                mbedLogger().appendDiagFile(buf,0);
+                _fsm_timer.reset();
+                _fsm_timer.start();
+                _isTimeoutRunning = 1;
+                batt().unpause();   //this is now active
+                batt().setPosition_mm(_disconnect_batt_pos_mm);
+
+            }
+            _timeout_splashdown = 6;    // timeout_splashdown reduced to 6 seconds for testing, usually 120 sec
+            if( _motorDisconnect_triggered && _isTimeoutRunning) {
+                // how end  - timeout
+                if (_fsm_timer > _timeout_splashdown) {  // timeout_splashdown reduced to 6 seconds for testing, usually 120 sec
+                    xbee().printf("FLYING_IDLE ended: timed out! Go to START_SWIM \r\n");  // go to start_swim
+                    //tare the  pressure sensor
+                    depth().tare(); // tares to ambient (do on surface)
+                    _state = START_SWIM;  // alternately, go into the legStruct and set start_swim now? rather than leg, or leg_position_dive
+                    _start_swim_entry = FLYING_IDLE;
+                    sprintf(buf, "FLYING_IDLE ended on splashdown timeout  go to Start_swim\n\n\r");
+                    mbedLogger().appendDiagFile(buf,3);
+
+                    //sprintf(buf, "FLYING_IDLE ended on splashdown timeout.  FOR testing go to ENDLEG_WAIT instead of   go to Start_swim\n\n\r");
+                    //mbedLogger().appendDiagFile(buf,0);
+                    //_state = ENDLEG_WAIT;
+                    _fsm_timer.reset();
+                    _isTimeoutRunning = false;
+                }
+            }
+            break; 
     case SIT_IDLE:   // sit_idle and fb_exit fall through to keyboard actions, since they do not have break statements
     case FB_EXIT:               
     case KEYBOARD :
@@ -177,6 +228,8 @@
             _state = FLOAT_BROADCAST;
             _fsm_timer.reset(); _yotimer.reset();
             _isTimeoutRunning = false;
+            sprintf(buf, "EMERGENCY_CLIMB - at surface ... now go to  FLOAT_BROADCAST\n\n\r");
+                   mbedLogger().appendDiagFile(buf,3);
         }
         
         //WHAT IS ACTIVE?
@@ -219,8 +272,14 @@
         // how exit? (exit with the timer, if timer still running continue processing sub FSM)
         if (_fsm_timer > _timeout) {
             xbee().printf("FN: timed out [time: %0.1f sec]\r\n", _fsm_timer.read());
+            sprintf(buf, "FIND_NEUTRAL  - timed out  roll=%f  depth = %f   now exit to EMERG_CLIMB\n\n", imu().getRoll(), depthLoop().getPosition());
+                   mbedLogger().appendDiagFile(buf,3);
             _state = EMERGENCY_CLIMB;         //new behavior (if this times out it emergency surfaces)
             _fsm_timer.reset();
+            sprintf(buf, "FN: timed out [time:%0.1f sec], _neutral_success = -1 for info to float_broadcast\n", _fsm_timer.read());
+                   mbedLogger().appendDiagFile(buf,3);
+            _fsm_timer.reset();
+            _neutral_success = -1;
             _isTimeoutRunning = false;
             
             //record this to the NEUTRAL sub-FSM tracker
@@ -228,15 +287,28 @@
             _substate_array_counter++;
         }
         
+        
         //what is active? (neutral finding sub-function runs until completion)        
         //check if substate returned exit state, if so stop running the sub-FSM
-        else if (runNeutralStateMachine() == NEUTRAL_EXIT) { 
+        
+        else if (runNeutralStateMachine() == NEUTRAL_EXIT) {   // but this line will repeatedly runneutralstatemachine as long as the overall state is FIND_NEUTRAL
             //if successful, FIND_NEUTRAL then goes to RISE
             xbee().printf("*************************************** FIND_NEUTRAL sequence complete.  Rising.\r\n\n");
-            _state = RISE;
+             sprintf(buf, "FIND_NEUTRAL - at Neutral EXIT  now go to RISE\n\n");
+                   mbedLogger().appendDiagFile(buf,3);
+            if(_neutral_success == 0 ) {   // failed in finding neutral - bad exit states
+                 configFileIO().report_no_neutral_found( bce().getPosition_mm(), batt().getPosition_mm());   // this is a message "no_float.txt"
+                                                                                                             //to the raspberry Pi control computer
+                 }
+            _state = RISE;  // adjust this to leg_position_dive  with an if statement - if not sent via keyboard
+                            // also set flags/structures that neutral was found successfully.
+              if (_neutral_entry_state == START_SWIM ) { 
+                   _state = EMERGENCY_CLIMB;  finish_leg =1;
+                   sprintf(buf, "FN  neutral_EXIT: but entry via start_swim, so radio call is next\n\n\r");
+                   mbedLogger().appendDiagFile(buf,3); 
+                }   //exit case when not entered by keyboard
             _isTimeoutRunning = false;
         }
-        
         break;   
         
     case DIVE :
@@ -295,7 +367,128 @@
         }
         
         break;
-    
+    case START_SWIM :
+        // start local state timer and init any other one-shot actions
+            if (!_isTimeoutRunning) {
+                xbee().printf("\r\n\nstate: START_SWIM\r\n");
+                _fsm_timer.reset(); // timer goes back to zero
+                _fsm_timer.start(); // background timer starts running
+                      sprintf(buf, "START_SWIM begun \n\r");
+                      mbedLogger().appendDiagFile(buf,3);
+                _yotimer.reset();
+                _yotimer.start();
+                _isTimeoutRunning = true;
+                //tare the  pressure sensor
+                    depth().tare(); // tares to ambient (do on surface)     HOW certain am I that this tare only happens at surface?
+                if( fabs(imu().getRoll()) > 70  && depthLoop().getPosition() < 9.0) {  //inversion operations
+                    // what needs to be started?
+                    bce().unpause();
+                    batt().unpause();
+                    rudder().unpause();
+                    _depth_command = 10;  // this should be meters depth
+                    _heading_command = 90;
+                    _timeout = 180;
+                    sprintf(buf, "START_SWIM found upside down, starting to descend imu().getRoll()= %f \n\r", imu().getRoll());
+                   mbedLogger().appendDiagFile(buf,3);
+
+                    // what are the commands? (using inner loops except for heading outer loop)
+                    // These actions happen ONCE in the POSITION_DIVE sequence
+
+                    //DEPTH and HEADING  COMMANDS first
+                    depthLoop().setCommand(_depth_command);
+                    headingLoop().setCommand(_heading_command);     //ACTIVE HEADING (mimic of dive and rise code)
+
+                     // move BCE and batt to new target positions                     
+                     bce().setPosition_mm(depthLoop().getOutput());  //constantly checking the Outer Loop output to move the motors
+                     // depthloop().getOutput()  will tell bce() to basically hold at neutral if you are at the desired depth ( assuming that zeroOffset for depth is correct)
+                     
+                     batt().setPosition_mm(pitchLoop().getOutput());  //these together should hold a depth, rather than turn around and rise
+                                        //batt().setPosition_mm(_neutral_batt_pos_mm + _BMM_dive_offset); // dare not use neutral postions before knowing what neutral is
+                                        //bce().setPosition_mm(_neutral_bce_pos_mm - _BCE_dive_offset);    // dare not use neutral postions before knowing what neutral is
+
+                    
+
+                    xbee().printf("START_SWIM: inversion  BATT cmd: %3.1f\r\n",batt().getSetPosition_mm());  //g
+                    xbee().printf("START_SWIM: inversion  BCE cmd: %3.1f\r\n",bce().getSetPosition_mm());  //g
+                    xbee().printf("START_SWIM: inversion  starting roll = : %3.1f\r\n",imu().getRoll() );  //g
+                }  //end of inversion section
+                if( fabs(imu().getRoll()) >70  && depthLoop().getPosition() > 9.0) {  //still inverted
+                    // what needs to be started?
+                    bce().unpause();
+                    batt().unpause();
+                    rudder().unpause();
+                    _depth_command = 9;
+                    _heading_command = 90;
+                    _timeout = 120;
+                    _pitch_command = 0;
+                    
+                    //DEPTH COMMAND
+                    depthLoop().setCommand(_depth_command);
+                    headingLoop().setCommand(_heading_command);     //ACTIVE HEADING (mimic of dive and rise code)
+
+                    // what are the commands? (using inner loops except for heading outer loop)
+                    // These actions happen ONCE in the POSITION_DIVE sequence
+                                      //batt().setPosition_mm(_neutral_batt_pos_mm - _BMM_dive_offset);  // is this right ??  looks like climb commands
+                                      //bce().setPosition_mm(_neutral_bce_pos_mm + _BCE_dive_offset); // dare not use neutral postions before knowing what neutral is
+                     bce().setPosition_mm(depthLoop().getOutput());  //constantly checking the Outer Loop output to move the motors
+                     batt().setPosition_mm(pitchLoop().getOutput());  //these together should hold a depth, rather than turn around and rise
+
+                    
+                    xbee().printf("START_SWIM: inversion  BATT cmd: %3.1f\r\n",batt().getSetPosition_mm());  //g
+                    xbee().printf("START_SWIM: inversion  BCE cmd: %3.1f\r\n",bce().getSetPosition_mm());  //g
+                    xbee().printf("START_SWIM: inversion  starting roll = : %3.1f\r\n",imu().getRoll() );  //g
+                sprintf(buf, "START_SWIM still upside down but depth>9m, set up to limit descent:  depth = %f   roll=%f\n\n\r", depthLoop().getPosition(), imu().getRoll());
+                   mbedLogger().appendDiagFile(buf,3);
+                }
+            if( fabs(imu().getRoll()) <70 && _start_swim_entry == FLYING_IDLE ) {  // starts out right side up,  still inside first call to start_swim
+                finish_leg = 1;   //this works to get to FB_EXIT, 
+                _state = FLOAT_BROADCAST;
+                _fsm_timer.reset(); _yotimer.reset();
+                _isTimeoutRunning = false;
+                sprintf(buf, "START_SWIM now upright, called by flying_idle, so end now.  roll= %f  depth = %f\n\r", imu().getRoll(), depthLoop().getPosition());
+                   mbedLogger().appendDiagFile(buf,3);
+             }
+             if( fabs(imu().getRoll()) <70 && _start_swim_entry != FLYING_IDLE ) {  // starts out right side up,  on second on/off cycle
+                _state = FIND_NEUTRAL;
+                _neutral_entry_state = START_SWIM;
+                _fsm_timer.reset(); _yotimer.reset();
+                _isTimeoutRunning = false;
+                sprintf(buf, "START_SWIM not called by FLYING_IDLE, so go to find_neutral: depth= %f\n\r", depthLoop().getPosition());
+                   mbedLogger().appendDiagFile(buf,3);
+             }
+        } //end of timeout running ==0
+        
+        // how exit?   keep diving and watching depth while also watching imu().getRoll()
+        if ((_fsm_timer > _timeout ) || (_yotimer > _yo_time)) {  // this is bad, still upside down.
+            xbee().printf("start_swim: timed out\r\n");
+            sprintf(buf, "start_swim timed out on yo_time or timeout - still upside down bad - go to FLOAT_BROADCAST  roll=%f \n\n\r", imu().getRoll());
+                   mbedLogger().appendDiagFile(buf,3);
+            configFileIO().report_still_inverted( fabs(imu().getRoll()), _yo_time);  // tells how long you waited, puts in file "inverted.txt"
+            _state = FLOAT_BROADCAST;
+            _fsm_timer.reset(); _yotimer.reset();
+            _isTimeoutRunning = false;
+        }
+        else if (fabs(imu().getRoll()) <30 ) { // pretty much righted itself  go to FIND_NEUTRAL
+            _state = FIND_NEUTRAL;
+            _neutral_entry_state = START_SWIM;
+            sprintf(buf, "START_SWIM - turned self upright  roll=%f  depth = %f ,    now go toFIND_NEUTRAL\n\n\r", imu().getRoll(), depthLoop().getPosition());
+                   mbedLogger().appendDiagFile(buf,3);
+            // print message ?
+            _fsm_timer.reset(); _yotimer.reset();
+            _isTimeoutRunning = false;
+        }
+        
+        //WHAT IS ACTIVE?
+        //print status to screen continuously
+        xbee().printf("START_SWIM: BcePos (cmd):%6.1f mm(%0.1f), BattPos:%6.1f mm(%0.1f), RUD_deg_cmd: %5.1f <<current depth:%6.1f m [cmd:%6.1f]), pitch:%6.1f deg [cmd:%6.1f], heading_imu:%6.1f deg>>[%0.2f sec]                                         \r", 
+        bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_deg(),
+        depthLoop().getPosition(),depthLoop().getCommand(),pitchLoop().getPosition(),pitchLoop().getCommand(),imu().getHeading(),_fsm_timer.read());
+        
+        bce().setPosition_mm(depthLoop().getOutput());  //constantly checking the Outer Loop output to move the motors
+        batt().setPosition_mm(pitchLoop().getOutput());  //these together should hold a depth, rather than turn around and rise
+        
+        break;
+   
     case RISE :
         // start local state timer and init any other one-shot actions
         
@@ -332,6 +525,8 @@
         //did not work correctly in bench test (stuck in rise state)
         else if (depthLoop().getPosition() < 0.5) {
             xbee().printf("RISE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
+            sprintf(buf, "in RISE: at surface ...  depth =%g  ... now go to  FLOAT_BROADCAST\n\n\r", depthLoop().getPosition());
+                   mbedLogger().appendDiagFile(buf,3);
             _state = FLOAT_BROADCAST;
             _fsm_timer.reset();
             _isTimeoutRunning = false;
@@ -777,6 +972,8 @@
         
         if (_fsm_timer > _timeout || ( _yotimer > _state_transition_time)) {
             xbee().printf("FB: timed out\r\n");
+            sprintf(buf, "in FLOAT_BRADCAST timed out  ...  go to SIT_IDLE\n\n");
+            mbedLogger().appendDiagFile(buf,3);
             _state = SIT_IDLE;
             _fsm_timer.reset();
             
@@ -784,7 +981,11 @@
             //mbedLogger().closeFile();
             
             _isTimeoutRunning = false;
-            if(finish_leg == 1) { _state = ENDLEG_WAIT; }  // allows wait at surface for xbee messaging to not close program
+            if(finish_leg == 1) { 
+                _state = ENDLEG_WAIT;
+                sprintf(buf, "in FLOAT_BROADCAST still timed out .... but also finish_leg==1 so go to ENDLEG_WAIT\n\n");
+                mbedLogger().appendDiagFile(buf,3); 
+            }  // allows wait at surface for xbee messaging to not close program
         }
         
         //fix on float_broadcast to account for BCE stopping early in current hardware
@@ -1095,7 +1296,8 @@
 //FIND_NEUTRAL sub-Finite State Machine (sub-FSM)
 // Note: the sub-FSM only moves the pistons once at the start of each timer loop
 //  (timer completes, moves piston, timer completes, moves piston, etc)
-int StateMachine::runNeutralStateMachine() {                
+int StateMachine::runNeutralStateMachine() {  
+       char buf[256];              
     switch (_substate) {
         case NEUTRAL_SINKING :
             //start the 10 second timer
@@ -1116,16 +1318,23 @@
             //once reached the travel limit, no need to keep trying, so exit
             if (bce().getPosition_mm() <= 0) {
                 xbee().printf("\r\nDEBUG: BCE current position is %0.1f mm (NEXT SUBSTATE NEUTRAL EXIT)\r\n", bce().getPosition_mm());
+                sprintf(buf, "in RUNneutral: state failed at bce.getPosition()\n\n");
+                mbedLogger().appendDiagFile(buf,3);
                 _substate = NEUTRAL_EXIT;
+                 _neutral_success = 0;
                 _isSubStateTimerRunning = false; // reset the sub state timer
             }
+            
             //Troy: Pressure vessel went beyond set depth limit, goes to next state
             //once deeper than the commanded setpoint...
             else if (depthLoop().getPosition() > _depth_command) {
                 _substate = NEUTRAL_SLOWLY_RISE; // next state
                 _isSubStateTimerRunning = false; //reset the sub state timer
             }
- 
+            else if ( (depthLoop().getVelocity()  > 0.05) && (depthLoop().getPosition() >  0.6 * _depth_command) )  {  // need to code in FILTERED vertical velocity
+                _substate = NEUTRAL_SLOWLY_RISE; // next state
+                _isSubStateTimerRunning = false; //reset the sub state timer
+            }
             // what is active?
             //once the 10 second timer is complete, reset the timeout so the state one-shot entry will move the setpoint
             if (_fsm_timer.read() >= _neutral_timer) {
@@ -1164,7 +1373,10 @@
             // how exit?
             //once at full travel limit (setPosition) and haven't yet risen, time to give up and exit
             if (bce().getSetPosition_mm() >= bce().getTravelLimit()) {
-                _substate = NEUTRAL_EXIT;     
+                _substate = NEUTRAL_EXIT;  
+                sprintf(buf, "in RUNneutral: SLOWLY RISE state failed at bce.getsetPosition()\n\n");
+                mbedLogger().appendDiagFile(buf,3);
+                _neutral_success =0;  //another failed state   
                 _isSubStateTimerRunning = false; // reset the sub state timer
             }
             //Troy: Depth rate will go negative as the pressure vessel starts rising
@@ -1173,8 +1385,7 @@
                 xbee().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Sink Rate < 0 m/s [time: %0.1f]\r\n", _fsm_timer.read());
                 _substate = NEUTRAL_CHECK_PITCH;
                 _isSubStateTimerRunning = false; // reset the sub state timer
-            }
-            
+            }        
             // what is active?
             //once 5 second timer complete, reset the timeout so the state one-shot entry will move the setpoint
             if (_fsm_timer.read() >= _neutral_timer) {
@@ -1236,13 +1447,20 @@
 
                     xbee().printf("\r\n\n>>> Saving Positions: BCE: %0.1f mm, BATT: %0.1f <<<\r\n\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
                     
+                    _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again  
                     _substate = NEUTRAL_EXIT;
-                    _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
+                    sprintf(buf, "in RUNneutral: state succeeded! at NEUTRAL_CHECK PITCH line 1452\n\n");
+                mbedLogger().appendDiagFile(buf,3);
+                    _neutral_success = 1;
+                    configFileIO().saveNeutralStatus(_neutral_success, _neutral_bce_pos_mm, _neutral_batt_pos_mm);  // saves to tell re-started program neutral has been found
+                    
                 }
                 
                 else {
                     xbee().printf("\r\nDid not find NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH, how did I get here?!\r\n");
                     _substate = NEUTRAL_EXIT;
+                    sprintf(buf, "in RUNneutral: state failed at final fallthrough point line 1462\n\n");
+                    mbedLogger().appendDiagFile(buf,3);
                 }
             }
             
@@ -1328,7 +1546,7 @@
             _state = FB_EXIT;
             }
       }
-    if (xbee().readable() && (_state == SIT_IDLE || _state == KEYBOARD  || _state == ENDLEG_WAIT)) {  //ends at very end of function  1684     
+    if (xbee().readable() && (_state == SIT_IDLE || _state == KEYBOARD  || _state == ENDLEG_WAIT)) {  //ends at very end of function  1740     
         // then get the key
         user_input = xbee().getc();   
         // and exit from ENDLEG_WAIT - you got a keyboard input

--- a/StateMachine/StateMachine.hpp	Thu May 02 20:34:16 2019 +0000
+++ b/StateMachine/StateMachine.hpp	Wed May 08 13:24:04 2019 +0000
@@ -25,6 +25,8 @@
     LEG_POSITION_RISE,
     TX_MBED_LOG,
     RX_SEQUENCE,
+    START_SWIM,
+    FLYING_IDLE,
     FB_EXIT,
     ENDLEG_WAIT
 };
@@ -127,7 +129,14 @@
     float _pitchTolerance;       // pitch angle tolerance for neutral finding exit criteria
     float _bceFloatPosition;     // bce position for "float" states
     float _battFloatPosition;    // batt position for "broadcast" state
+    float _disconnect_batt_pos_mm;  // all the way forward
+    float _batt_flying_pos_mm;
     
+    float _timeout_splashdown;  // two minutes??
+    int _motorDisconnect_triggered;
+    
+    int _start_swim_entry;
+    int _neutral_entry_state;
     float _depth_command;       // user keyboard depth
     float _pitch_command;       // user keyboard pitch
     float _heading_command;     // user keyboard heading
@@ -144,7 +153,7 @@
     int _sub_state;             // substate on find_neutral function
     int _previous_sub_state;    // previous substate so that what goes into the sub-state is not being changed as it is processed
     float _neutral_timer;  // keep time for rise/sink/level timer incremnets
-    
+    int _neutral_success;
     bool _isTimeoutRunning;
     
     bool _isSubStateTimerRunning;

--- a/System/StaticDefs.cpp	Thu May 02 20:34:16 2019 +0000
+++ b/System/StaticDefs.cpp	Wed May 08 13:24:04 2019 +0000
@@ -64,6 +64,10 @@
     static IMU imu(p28, p27);       // tx, rx pin
     return imu;    
 }
+Sensors & sensors() {
+    static Sensors sensors;
+    return sensors;
+}
 
 OuterLoop & depthLoop() {
     static OuterLoop depthLoop(0.1, 0); // interval, sensor type
@@ -79,6 +83,11 @@
     static OuterLoop headingLoop(0.1, 2); // interval, sensor type
     return headingLoop;
 }
+OuterLoop & altimLoop() {
+    static OuterLoop altimLoop(0.1, 3); // interval, sensor type
+    return  altimLoop;
+}
+
 
 StateMachine & stateMachine() {
     static StateMachine stateMachine;
@@ -99,20 +108,16 @@
     return legController;
 }
 
-Sensors & sensors() {
-    static Sensors sensors;
-    return sensors;
-}
 
 MbedLogger & mbedLogger() {
     static MbedLogger mbedLogger("/local/");        //local file system
     return mbedLogger;
 }
 
-MbedLogger & sdLogger() {
-    static MbedLogger sdLogger("/sd/");
-    return sdLogger;
-}
+//MbedLogger & sdLogger() {             // leaving this in with no SD card might be a problem.
+//    static MbedLogger sdLogger("/sd/");
+//    return sdLogger;
+//}
 
 DigitalOut & led1() {
     static DigitalOut led1(LED1);
@@ -133,7 +138,10 @@
     static DigitalOut led4(LED4);
     return led4;
 }
-
+DigitalIn & motorDisconnect() {
+     static DigitalIn motorDisconnect(p24, PullDown);
+     return motorDisconnect;
+}
 Gui & gui() {
     static Gui pythonGUI;
     return pythonGUI;

--- a/System/StaticDefs.hpp	Thu May 02 20:34:16 2019 +0000
+++ b/System/StaticDefs.hpp	Wed May 08 13:24:04 2019 +0000
@@ -38,6 +38,8 @@
 OuterLoop                   &   depthLoop();
 
 IMU                         &   imu();
+Sensors                     &   sensors();
+
 OuterLoop                   &   pitchLoop();
 
 StateMachine                &   stateMachine();
@@ -45,10 +47,7 @@
 MbedLogger                  &   mbedLogger();       //internal memory log files
 
 //SDFileSystem                &   sd_card();          //SD card file system
-
-Sensors                     &   sensors();
-
-MbedLogger                  &   sdLogger();         //sd log files
+//MbedLogger                  &   sdLogger();         //sd log files
 
 ConfigFileIO                &   configFileIO();
 
@@ -58,12 +57,14 @@
 //servo driver
 ServoDriver & rudder();     //new 06/06/2018
 OuterLoop                   &   headingLoop();  //change 06/08/2018
+OuterLoop                   &   altimLoop();  //change 03may2019
 
 // leds for debugging, global for use in any function
 DigitalOut                  &   led1();
 DigitalOut                  &   led2();
 DigitalOut                  &   led3();
 DigitalOut                  &   led4();
+DigitalIn                   &  motorDisconnect();    // is this right?  is it basically global?
 
 Gui                         &   gui();
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/exPosVelFilter/exPosVelFilter.txt	Wed May 08 13:24:04 2019 +0000
@@ -0,0 +1,39 @@
+#include "PosVelFilter.hpp"
+//#include "StaticDefs.hpp"
+
+PosVelFilter::PosVelFilter() {
+    x1 = 0; // pseudo position state
+    x2 = 0; // pseudo velocity state
+    
+    w_n = 1.0; // natural frequency of the filter bigger increases frequency response
+}
+
+//run the pos-vel estimate filter
+void PosVelFilter::update(float deltaT, float counts) {
+    dt = deltaT;
+
+    x1_dot = x2;
+    x2_dot = (-2.0*w_n*x2) - (w_n*w_n)*x1 + (w_n*w_n)*counts;
+
+    position = x1;
+    velocity = x2;
+
+    x1 += x1_dot*dt;
+    x2 += x2_dot*dt;
+}
+
+float PosVelFilter::getPosition() {
+    return position;
+}
+
+float PosVelFilter::getVelocity() {
+    return velocity;
+}
+
+float PosVelFilter::getDt() {
+    return dt;
+}
+
+void PosVelFilter::writeWn(float wn) {
+    w_n = wn;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/exPosVelFilter/ex_hPosVelFilter.txt	Wed May 08 13:24:04 2019 +0000
@@ -0,0 +1,33 @@
+#ifndef POSVELFILTER_H
+#define POSVELFILTER_H
+
+#include "mbed.h"
+
+class PosVelFilter
+{
+public:
+    PosVelFilter();
+    
+    void update(float deltaT, float counts);
+    
+    void init();
+    
+    float getPosition();
+    float getVelocity();
+    float getDt();
+    
+    void writeWn(float wn);
+    
+protected:
+    float x1;
+    float x2;
+    float x2_dot;
+    float x1_dot;
+    float w_n; 
+    
+    float dt;
+    float position;
+    float velocity;
+};
+
+#endif
\ No newline at end of file

--- a/main.cpp	Thu May 02 20:34:16 2019 +0000
+++ b/main.cpp	Wed May 08 13:24:04 2019 +0000
@@ -22,7 +22,12 @@
 void log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)
 
 static int current_state = 0; 
-static int have_legfile = 0;  
+static int have_legfile = 0; 
+
+static int save_state = 0;   // state to hold while find_neutral runs ( if necessary)
+       int begin_state = 0;
+static int neutral_via_leg = 0;  // flag for entry of find_neutral via leg_pos and setval==0
+static int setval = -1;    // has neutral been set, found in neutral.txt   
 
 static bool file_opened = false;
 
@@ -107,6 +112,7 @@
             depthLoop().runOuterLoop();
             pitchLoop().runOuterLoop();
             headingLoop().runOuterLoop();
+            altimLoop().runOuterLoop();
         }
         
         if ( (timer_counter % 1000) == 0) {        // update at 1.0 second intervals
@@ -155,7 +161,7 @@
     print_diag=1;
     configFileIO().load_LogVers_config(print_diag);    // Cnow print info to diag file
     mbedLogger().initializeDiagFile(print_diag);
-    
+        
     configFileIO().load_BCE_config();       // load the buoyancy engine parameters from the file "bce.txt"
     configFileIO().load_BATT_config();      // load the battery mass mover parameters from the file "batt.txt"
     
@@ -186,8 +192,24 @@
     pitchLoop().init();
     //removed start, it's handled by the interrupt
     pitchLoop().setCommand(stateMachine().getPitchCommand());
+    char buf[256];
+    sprintf(buf, "INIT  of Loop operators   headingLoop().init() is coming\n");
+    mbedLogger().appendDiagFile(buf,3);
+    headingLoop().init();  
+    altimLoop().init();   
+    sprintf(buf, "AltimLOOP.init() succeeded!  even with just default values - no starting file\n");
+    mbedLogger().appendDiagFile(buf,3);  
     
-    headingLoop().init();           
+    sprintf(buf, "in setup():  before load_setneutral_status setval = %d \n\r", setval);
+    mbedLogger().appendDiagFile(buf,3);
+            
+    configFileIO().load_setneutral_status();   // "neutral.txt" has flag for whether neutral has been set, and neutral values
+    sprintf(buf, "\n in setup():  load_setneutral_status succeeded:    setval = %d \n\r", configFileIO().neutralStruct.setval);
+    mbedLogger().appendDiagFile(buf,3);
+    setval = configFileIO().neutralStruct.setval;
+    sprintf(buf, "\n in setup():  after load_setneutral_status setval = %d \n\r", setval);
+    mbedLogger().appendDiagFile(buf,3);
+    
     //removed start, it's handled by the interrupt
     //headingLoop().setCommand(stateMachine().getHeadingCommand());         // FIX LATER
     //heading flag that adjust the PID error is set in the constructor
@@ -214,12 +236,19 @@
          
     //load sequence from file
     sequenceController().loadSequence();
-    char buf[256];
+    
     //xbee().printf("\n\n\r 2018-08-14 FSG PCB XBee (setup complete) \n\n\r");
-    sprintf(buf, "\n\n\r in setup():  starting legController().loadleg() \n\n\r");
+    sprintf(buf, " in setup():  starting legController().loadleg() \n\n\r");
     mbedLogger().appendDiagFile(buf,3);
     have_legfile = legController().loadLeg();  // this should be 1 if the legfile reader has found 1 or more legs
-    sprintf(buf, "Time is amystery, here is a message before the basic call\n");
+    
+    current_state = legController().legStructLoaded[0].state;
+    sprintf(buf, "in setup(): have_legfile = %d    current_state = %d (sit_idle= 0 or 1)\n", have_legfile, current_state);
+    mbedLogger().appendDiagFile(buf,3);
+    sprintf(buf, "in setup(): LEG_POSITION_DIVE = %d   START_SWIM = %d \n", LEG_POSITION_DIVE, START_SWIM);
+    mbedLogger().appendDiagFile(buf,3);
+    
+    sprintf(buf, "Time is a mystery, here is a message before the basic call\n");
     mbedLogger().appendDiagFile(buf,3);
     int jj;
     long int kk;
@@ -284,13 +313,13 @@
     int diagnum=0;
      char buf[256];
     xbee().printf("\n\n\r 2018-08-14 FSG PCB XBee (setup complete) \n\n\r");
-    sprintf(buf, "\n\n\r 2019-feb-25 FSG PCB XBee line271main (setup complete) \n\n\r");
+    sprintf(buf, "\n\n\r 2019-may-07 FSG PCB XBee line315  in main (setup complete) \n\n\r");
     mbedLogger().appendDiagFile(buf,3);
-    sprintf(buf, "finished setting up and will exit\n\n\r");
-    mbedLogger().appendDiagFile(buf,3);
+    
+    
     //tNow=5; sprintf(buf, "log file config values logfile= %s     diag file= %s\n", configFileIO().logFilesStruct.logFileName,
      //        configFileIO().logFilesStruct.diagFileName); tNow=0;
-    mbedLogger().appendDiagFile(buf,3);
+    //         mbedLogger().appendDiagFile(buf,3);
     vernum = configFileIO().logFilesStruct.logversion;
     diagnum = configFileIO().logFilesStruct.diagversion;
     sprintf(buf, "translated values LOG FILE VERSION number (vernum)=%d           diag file version number(diagnum) = %d\n", vernum, diagnum);
@@ -315,14 +344,23 @@
         if(have_legfile) {
             //install the leg variables in a structure, and set the state there.
             stateMachine().getLegParams(); //should set up everything with proper LEG_POSITION_DIVE state
-            sprintf(buf, "have_legfile succeeded \n\r");
+            sprintf(buf, "have_legfile succeeded  main: line 318 \n\r");
              mbedLogger().appendDiagFile(buf,3);
             }
+        
         if(!have_legfile) {
             sprintf(buf, "have_legfile failed!   .... will exit\n\n\r");
             mbedLogger().appendDiagFile(buf,3);
             keeprunning = 0;
             }
+        if(motorDisconnect().read() == 1) {
+            sprintf(buf, "motorDisconnect.read() = 1  surprising!\n\n\r");
+            mbedLogger().appendDiagFile(buf,3); led1()=1; led4()=1;
+            }
+        if(motorDisconnect().read() == 0) {
+            sprintf(buf, "motorDisconnect.read() = 0  I expected that\n\n\r");
+            mbedLogger().appendDiagFile(buf,3); led1()=1; led3()=1;
+            }
   
     while (keeprunning) {
         if( read_ticker() ) {                       // read_ticker runs at the speed of 10 kHz (adc timing)
@@ -351,16 +389,21 @@
                 if ( (tNow % 1000) == 0 ) {   // 1.0 second intervals
                     log_loop = true;
                     log_function();
-                    sprintf(buf, "hit 1 second  log interval in main loop tNow =%d  imu.roll = %f not-unsampled \n\n\r", tNow, imu().getRoll());  
-                    mbedLogger().appendDiagFile(buf,3);
-                    led3()=0; led2()=0;
+                    //sprintf(buf, "hit 1 second  log interval in main loop tNow =%d  imu.roll = %f not-unsampled \n\n\r", tNow, imu().getRoll());  
+                    //mbedLogger().appendDiagFile(buf,3);
+                    led3()=0; led2()=0; led1()=0; led4()=0;
                 }
-                if ( (tNow % 5000) == 0 ) {   // 5 second intervals
+                if ( (tNow % 2000) == 0 ) {   // 2 second intervals
                     
-                    led3()=1; led2()=1;
+                    led3()=1; 
+                    sprintf(buf, "hit 2 second  log interval in main loop current_state=%d tNow =%d  imu.roll = %f not-unsampled \n\n\r", current_state, tNow, imu().getRoll());  
+                    mbedLogger().appendDiagFile(buf,3);
                 }
+                if ((tNow % 3000) == 0) {
+                    led2()=1;
+                    }
                 //update the log and diagnostics files
-                if ( (tNow % 31000) == 0 ) {   // 1.0 hour intervals= 3600*1000  check for testing via 4 second intervals
+                if ( (tNow % 31000) == 0 ) {   // 1.0 hour intervals= 3600*1000  check for testing via 31 second intervals
                 sprintf(buf, "hit cycle seconds  replace_logfiles interval in main loop tNow =%d  \n\n\r", tNow);
                     mbedLogger().appendDiagFile(buf,3);
                     vernum = configFileIO().logFilesStruct.logversion;
@@ -376,21 +419,25 @@
                     // initialize new log and diagnostics files
                     log_loop = true;
                     log_function();
-                    led3()=1;
+                    led3()=1; led1()=1;
                 }
             } // end else { at  **88**
             if(current_state == FB_EXIT) {
                 log_loop=true;
-                log_function(); led2()=1;
+                log_function(); led2()=1; led4()=1;
                 keeprunning=0;  // and this will force exit to the main while() loop.
                 //  switch state to EOL_WAIT, spit out xbee messages,  wait 60 seconds or so. if keyboard comes back, do not exit
                 sprintf(buf, "INSIDE  main loop: BUT now called to exit out of it via FB_EXIT\n\n\r");
                 mbedLogger().appendDiagFile(buf,3);
             }
+                          if(tNow ==  90000) {  // don't wait forever  -remove this for real operations!!
+                              keeprunning=0;
+                              }
         }  // end if(read_ticker) {
     }  // end while(keeprunning)
+    
     mbedLogger().appendLogFile(current_state, 1);
-    sprintf(buf, "outside of main loop:  exiting out of the leg loop via FB_EXIT\n\n\r");
+    sprintf(buf, "outside of main loop:  exiting out of the leg loop via FB_EXIT  tNow= %d \n\n\r", tNow);
     mbedLogger().appendDiagFile(buf,3);
     led1()=1; led2() =1; led3()=1 ;led4() = 1;
     wait(5);