Diff: main.cpp

Revision:

102:0f430de62447

Parent:

98:81db9332212d

Child:

104:426224a55f5f

--- a/main.cpp	Fri Jun 28 15:07:41 2019 +0000
+++ b/main.cpp	Fri Sep 13 16:51:48 2019 +0000
@@ -39,13 +39,15 @@
     }
 }
 
-void log_function() {    
+void log_function(int option) {    
     // log loop runs at 1 hz
+    // option =1 normal short output
+    // option = 2  longer output, state name and actual time value, etc
     if (log_loop) {  //start logloop8
         //when the state machine is not in SIT_IDLE state (or a random keyboard press)
         if((current_state != SIT_IDLE)   &&  (current_state != FLYING_IDLE)) {  //first if  - not in sit_idle/keyboard state
             if (!file_opened) {                                 //if the log file is not open, open it
-                mbedLogger().appendLogFile(current_state, 1);   //open MBED file once
+                mbedLogger().appendLogFile(current_state, option);   //open MBED file once
                 //sdLogger().appendLogFile(current_state, 0);     //open SD file once
                            
                 file_opened = true;                             //stops it from continuing to open it
@@ -56,7 +58,7 @@
             
                   //just record  to the  Mbed file system   
             
-                   mbedLogger().appendLogFile(current_state, 1);    //writing data
+                   mbedLogger().appendLogFile(current_state, option);    //writing data
                    //sdLogger().appendLogFile(current_state, 1);    //writing data
                  }
         }  //end first if
@@ -66,7 +68,7 @@
             //this can only happen once
             if (file_opened) {
                 //WRITE ONCE
-                mbedLogger().appendLogFile(current_state, 1);   //write the idle state, then close
+                mbedLogger().appendLogFile(current_state, option);   //write the idle state, then close
                 //sdLogger().appendLogFile(current_state, 1);     //write the idle state, then close
                 
                 mbedLogger().appendLogFile(current_state, 0);    //close log file
@@ -126,7 +128,7 @@
 
 void setup() {
     //  xbee().baud(115200);  // comment out so default is 9600 for USB communications
-    xbee().printf("\n\n\r 2018-11-08 FSG PCB XBee\n\n\r");
+    xbee().printf("\n\n\r 2019-13-09 FSG PCB XBee\n\n\r");
     
     //pc().baud(57600);
  
@@ -170,7 +172,14 @@
     
     configFileIO().load_RUDDER_config();    // load the rudder servo inner loop parameters from the file "SERVO.txt"
     configFileIO().load_HEADING_config();   // load the rudder servo outer loop HEADING control parameters from the file "HEADING.txt" (contains neutral position)
- 
+    
+    char buf[256];
+    sprintf(buf, "INIT  of Loop operators line 174 main.cpp   headingLoop().init() is coming\n");
+    mbedLogger().appendDiagFile(buf,3);
+    
+    
+    
+    
     // set up the linear actuators.  adc has to be running first.
     bce().setPIDHighLimit(bce().getTravelLimit());     //travel limit of this linear actuator
     bce().init();
@@ -183,7 +192,11 @@
     batt().runLinearActuator(); // _init = true;
     //batt().start();//removed start, it's handled by the interrupt
     batt().pause(); // start by not moving
- 
+     //char buf[256];
+    sprintf(buf, "INIT  of Loop operators line 196 main.cpp   headingLoop().init() is coming\n");
+    mbedLogger().appendDiagFile(buf,3);
+    
+    
     // set up the depth, pitch, and rudder outer loop controllers
     depthLoop().init();
     //removed start, it's handled by the interrupt
@@ -192,23 +205,76 @@
     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");
+    sprintf(buf, "INIT  of Loop operators line 208 main.cpp   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);  
     
-    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);
+    configFileIO().load_swim_config();      // load in more parameters  bmm_dive_mm, bce, fn_timeout, max_mbed-runtime_hr, altim_blank_m, altim_abort_m
+    sprintf(buf, "\n in setup():  after load_swim_config: fn_timeout = %d\n\r", configFileIO().swimConstants.fn_timeout);
+    mbedLogger().appendDiagFile(buf,3);
+    sprintf(buf, "\n in setup():  after load_swim_config: altim_blank_m = %.1f\n\r", configFileIO().swimConstants.altim_blank_m);
+    mbedLogger().appendDiagFile(buf,3);
+    sprintf(buf, "\n in setup():  after load_swim_config: bce_dive_mm = %.1f\n\r", configFileIO().swimConstants.bce_dive_mm);
+    mbedLogger().appendDiagFile(buf,3);
+        sprintf(buf, "depth loop:   P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), 
+         depthLoop().getControllerD(), depthLoop().getOutputOffset());
+    mbedLogger().appendDiagFile(buf,3);
+    
+    headingLoop().init(); 
+    xbee().printf("heading Loop  initialized! line 230\n\r");
+    
+    sprintf(buf, "INIT  of Loop operators line 230  headingLoop().init() is finished\n");
+    mbedLogger().appendDiagFile(buf,3);
+    // mbedLogger().appendDiagFile(buf,0);
+    
+    
+    
+    configFileIO().load_Altimeter_config();       // load the altimeter filter parameters from the file "altim.txt"  // edit by CAM
+    sprintf(buf, "loaded altimeter configuration line 237  \n");
+    mbedLogger().appendDiagFile(buf,3);
+    sprintf(buf,"line 239:  altimeter reading (meters) = %f\n", sensors().getAltimeterReading_m());
+    mbedLogger().appendDiagFile(buf,3);
+    mbedLogger().appendDiagFile(buf,0);
+  
+    xbee().printf("altimeter config loaded  line 245\n\r");
+    
+    
+    altimLoop().init();  
+    xbee().printf("altimLoop().init() run  line 247\n\r");
+    sprintf(buf, "ran altimLoop().init() after reading config line 244  \n");
+    mbedLogger().appendDiagFile(buf,3);
+    mbedLogger().appendDiagFile(buf,0);
+  
+    
+    sprintf(buf, "AltimLoop.init() succeeded!  after loading altim.txt values and init()  line 250\n");
+    mbedLogger().appendDiagFile(buf,3);  
+    
+    sprintf(buf, "in setup():  before load_setneutral_status setval = %d   line 253\n\r", setval);
+    mbedLogger().appendDiagFile(buf,3);
+    mbedLogger().appendDiagFile(buf,0);
+    
+   
+      
+ //        
+//    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);
+//    configFileIO().load_swim_config();      // load in more parameters  bmm_dive_mm, bce, fn_timeout, max_mbed-runtime_hr, altim_blank_m, altim_abort_m
+//    sprintf(buf, "\n in setup():  after load_swim_config: fn_timeout = %d\n\r", configFileIO().swimConstants.fn_timeout);
+//    mbedLogger().appendDiagFile(buf,3);
+//    sprintf(buf, "\n in setup():  after load_swim_config: altim_blank_m = %.1f\n\r", configFileIO().swimConstants.altim_blank_m);
+//    mbedLogger().appendDiagFile(buf,3);
+//    sprintf(buf, "\n in setup():  after load_swim_config: bce_dive_mm = %.1f\n\r", configFileIO().swimConstants.bce_dive_mm);
+//    mbedLogger().appendDiagFile(buf,3);
     
     //removed start, it's handled by the interrupt
     //headingLoop().setCommand(stateMachine().getHeadingCommand());         // FIX LATER
@@ -254,7 +320,7 @@
     int jj;
     long int kk;
     time_t secval;
-    
+    xbee().printf("legcontroller read  and Time  line 323\n\r");
     sprintf(buf, "sizeof int=%d    size of long int = %d  size of time_t = %d\n", (sizeof jj), (sizeof kk), (sizeof secval));
     mbedLogger().appendDiagFile(buf,3);
     
@@ -272,7 +338,7 @@
     //create log files if not present on file system
     mbedLogger().initializeLogFile();
     
-    mbedLogger().appendLogFile(current_state, 1);   //write the idle state, then close
+    mbedLogger().appendLogFile(current_state, 2);   //write the idle state, then close
                 //sdLogger().appendLogFile(current_state, 1);     //write the idle state, then close
                 
                 mbedLogger().appendLogFile(current_state, 0);    //close log file  added jcw nov 9 2018 for test
@@ -316,7 +382,7 @@
     xbee().printf("\n\n\r 2018-08-14 FSG PCB XBee (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);
-    int fn_timeout = 240;
+    int fn_timeout = 240; // need to get this into config file
     int save_timeout = 300;
     
     
@@ -391,7 +457,7 @@
                     save_state = current_state;
                     neutral_via_leg =1;
                     save_timeout = legController().legStructLoaded[0].timeout;
-                    fn_timeout = 240;
+                    fn_timeout = configFileIO().swimConstants.fn_timeout;
                     current_state = START_SWIM;  // does runStatemachine know about this value?
                     sprintf(buf, "in main(): setval==0, but upright, so pre-empt leg_pos_dive to start_swim\n  set state directly  start_swim\n");
                     mbedLogger().appendDiagFile(buf,3);
@@ -422,9 +488,10 @@
                 //LOGGING
                 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);
+                    if((tNow % 20000) ==0) {log_function(2);}  // long data string every 20 seconds
+                        else {log_function(1); }  //otherwise short
+                    // 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 % 2000) == 0 ) {   // 2 second intervals
@@ -453,35 +520,43 @@
                     //  save the new version numbers
                     // initialize new log and diagnostics files
                     log_loop = true;
-                    log_function();
+                    log_function(2);
                     led3()=1; led1()=1;
                 }
             } // end else { at  **88**
             if(current_state == FB_EXIT) {
                 log_loop=true;
-                log_function();
+                log_function(2);
                 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);
+                // and put out the end_leg  message file   
+                configFileIO().writeEndleg_reason();
             }
             if(tNow ==  90000) {  // don't wait forever  -remove this for real operations!!
+            // if(tNow == 1000 * configFileIO().swimConstants.max_mbed_runtime_hr * 3600) {    
                 keeprunning=0;
+                
                 sprintf(buf, "INSIDE  main loop: Triggered FINAL timeout at 90 seconds\n\n\r");
                 mbedLogger().appendDiagFile(buf,3);
+                sprintf(buf, "AT FINAL TIMEOUT:  before changing endleg-reason to 8: endleg_reason= %d\n\n\r", configFileIO().swimConstants.endleg_reason);
+                mbedLogger().appendDiagFile(buf,3);
+                configFileIO().swimConstants.endleg_reason = 8;  //  timed out on mbed_max_runtime_hr
+                configFileIO().writeEndleg_reason();
             }
         }  // end if(read_ticker) {
     }  // end while(keeprunning)
     
-    mbedLogger().appendLogFile(current_state, 1);
+    mbedLogger().appendLogFile(current_state, 2);
     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);
     
-    mbedLogger().appendLogFile(current_state, 1);
+    mbedLogger().appendLogFile(current_state, 2);
     mbedLogger().appendLogFile(current_state, 0);
     sprintf(buf, "wait 5 more seconds? out of the leg loop via FB_EXIT\n\n\r");
     mbedLogger().appendDiagFile(buf,0);