Diff: StateMachine/StateMachine.cpp

Revision:

33:29a4268fbc74

Parent:

32:f2f8ae34aadc

--- a/StateMachine/StateMachine.cpp	Wed Dec 20 13:52:50 2017 +0000
+++ b/StateMachine/StateMachine.cpp	Wed Dec 20 14:57:18 2017 +0000
@@ -1,5 +1,7 @@
 #include "StateMachine.hpp"
 #include "StaticDefs.hpp"
+
+#include "rtos.h"
  
 StateMachine::StateMachine() {
     _timeout = 20;            // generic timeout for every state, seconds
@@ -52,6 +54,8 @@
     _max_recorded_auto_neutral_depth = -99;
     
     _file_closed = true;
+    
+    RtosTimer save_data_timer(recordDataWithTimer, osTimerPeriodic, (void *)0);
 }
  
 //Finite State Machine (FSM)
@@ -1746,4 +1750,23 @@
     if (timer.read() >= transmit_timer) {
         is_transmit_timer_running = false; // reset the sub state timer to do one-shot actions again
     }
+}
+
+void StateMachine::recordDataWithTimer(void const *n) {        
+    _data_log[0] = timer.read();                //timer reading (shouldn't this be constant throughout the state?)
+    _data_log[1] = depthLoop().getCommand();    //depth command
+    _data_log[2] = depthLoop().getPosition();   //depth reading
+    _data_log[3] = pitchLoop().getCommand();    //pitch command
+    _data_log[4] = pitchLoop().getPosition();   //pitch reading
+    _data_log[5] = bce().getSetPosition_mm();
+    _data_log[6] = bce().getPosition_mm();
+    _data_log[7] = batt().getSetPosition_mm();
+    _data_log[8] = batt().getPosition_mm();
+        
+    //record data to the MBED every 1 second
+    mbedLogger().saveArrayToFile(_data_log);
+        
+    //record data to OpenLog every 1 seconds
+    datalogger().printf("%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f\n",_data_log[0],_data_log[1],
+    _data_log[2],_data_log[3],_data_log[4],_data_log[5],_data_log[6],_data_log[7],_data_log[8]);
 }
\ No newline at end of file