most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown

Dependencies:   mbed MODSERIAL FATFileSystem

Revision:
66:0f20870117b7
Parent:
65:2ac186553959
Child:
67:c86a4b464682
--- a/main.cpp	Tue Jun 19 18:10:50 2018 +0000
+++ b/main.cpp	Tue Jun 19 20:14:23 2018 +0000
@@ -178,7 +178,7 @@
 
 void setup() {
     pc().baud(57600);
-    pc().printf("\n\n\r 2018_03_28_wireless (FSG bench test)\n\n\r");
+    pc().printf("\n\n\r FSG PCB Bench Test V1.5)\n\n\r");
  
     // start up the system timer
     //systemTimer().start();
@@ -281,19 +281,33 @@
     systemTicker.attach_us(&system_timer, 1000);         // Interrupt timer running at 0.001 seconds       (slower than original ADC time interval)
         
     while (1) {        
-        if( read_ticker() )                         // read_ticker runs at the speed of 10 kHz
+        if( read_ticker() )                         // read_ticker runs at the speed of 10 kHz (adc timing)
         {
             ++tNow;
 
-            if ( (tNow % 10) == 0 ) {   // 0.001 second intervals               //if ( (tNow % 100) == 0 ) {   // 0.01 second intervals
-                fsm_loop = true;
-                FSM();
+            //run finite state machine fast when transmitting data
+            if (current_state == TRANSMIT_MBED_LOG or current_state == RECEIVE_SEQUENCE) {
+                if ( (tNow % 10) == 0 ) {   // 0.001 second intervals  (1000 Hz)
+                    fsm_loop = true;
+                    FSM();
+                }
+                if ( (tNow % 1000) == 0 ) {   // 1.0 second intervals
+                   log_loop = true;
+                    log_function();
+                }
             }
             
-//            if ( (tNow % 1000) == 0 ) {   // 1.0 second intervals
-//                log_loop = true;
-//                log_function();
-//            }
+            else {
+                if ( (tNow % 100) == 0 ) {   // 0.1 second intervals (10 Hz)
+                    fsm_loop = true;
+                    FSM();
+                }
+                
+                if ( (tNow % 1000) == 0 ) {   // 1.0 second intervals
+                    log_loop = true;
+                    log_function();
+                }
+            }
         }
     }
 }
\ No newline at end of file