Piccard Mach Timer Implementation. (#45)

* Added machtimer and detection of liftoff from moving.

* Addressed Pull Requests comments and small bugfix

boards/cats_rev1/CMakeLists.txt

@@ -45,7 +45,7 @@ elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
     add_compile_options(-Os)
 else ()
     message(STATUS "Minimal optimization, debug info included")
-    add_compile_options(-Og -g)
+    add_compile_options(-O1 -g)
 endif ()
 
 include_directories(Core/Inc USB_DEVICE/App USB_DEVICE/Target Drivers/STM32L4xx_HAL_Driver/Inc Drivers/STM32L4xx_HAL_Driver/Inc/Legacy Middlewares/Third_Party/FreeRTOS/Source/include Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F Middlewares/ST/STM32_USB_Device_Library/Core/Inc Drivers/CMSIS/Device/ST/STM32L4xx/Include Drivers/CMSIS/Include Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc Core/Inc USB_DEVICE/App USB_DEVICE/Target Drivers/STM32L4xx_HAL_Driver/Inc Drivers/STM32L4xx_HAL_Driver/Inc/Legacy Middlewares/Third_Party/FreeRTOS/Source/include Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F Middlewares/ST/STM32_USB_Device_Library/Core/Inc Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc Drivers/CMSIS/Device/ST/STM32L4xx/Include Drivers/CMSIS/Include)

boards/cats_rev1/Core/Inc/config/globals.h

@@ -58,6 +58,7 @@ extern dt_telemetry_trigger_t dt_telemetry_trigger;
 /** Timers **/
 extern uint32_t num_timers;
 extern cats_timer_t ev_timers[8];
+extern cats_timer_t mach_timer;
 
 /** Recorder Queue **/
 extern osMessageQueueId_t rec_queue;

boards/cats_rev1/Core/Inc/control/flight_phases.h

@@ -16,6 +16,8 @@
 #define TIME_THRESHOLD_MOV_TO_IDLE 1000
 #define ALLOWED_ACC_ERROR          200
 #define ALLOWED_GYRO_ERROR         50
+#define MOV_LIFTOFF_THRESHOLD      4000.0f
+#define MOV_LIFTOFF_SAFETY_COUNTER 50
 
 /* IDLE */
 #define TIME_THRESHOLD_IDLE_TO_MOV 500

boards/cats_rev1/Core/Src/config/globals.c

@@ -84,8 +84,9 @@ drop_test_fsm_t global_drop_test_state = {.flight_state = DT_IDLE};
 dt_telemetry_trigger_t dt_telemetry_trigger = {0};
 
 /** Timers **/
-uint32_t num_timers = 8;
+uint32_t num_timers = 2;
 cats_timer_t ev_timers[8] = {};
+cats_timer_t mach_timer = {};
 
 /** Recorder Queue **/
 osMessageQueueId_t rec_queue;

boards/cats_rev1/Core/Src/control/flight_phases.c

@@ -91,6 +91,31 @@ void check_moving_phase(flight_fsm_t *fsm_state, imu_data_t *imu_data) {
     fsm_state->angular_movement[1] = 0;
     fsm_state->angular_movement[2] = 0;
   }
+
+  /* Check if we move from MOVING To THRUSTING_1 */
+  /* To Make sure that the timers start any acceleration direction is accepted
+   * here */
+  int32_t accel_x = (int32_t)imu_data->acc_x * (int32_t)imu_data->acc_x;
+  int32_t accel_y = (int32_t)imu_data->acc_y * (int32_t)imu_data->acc_y;
+  int32_t accel_z = (int32_t)imu_data->acc_z * (int32_t)imu_data->acc_z;
+  int32_t acceleration = accel_x + accel_y + accel_z;
+
+  if ((float)acceleration > (MOV_LIFTOFF_THRESHOLD * MOV_LIFTOFF_THRESHOLD)) {
+    fsm_state->memory[2]++;
+  } else {
+    fsm_state->memory[2] = 0;
+  }
+
+  if (fsm_state->memory[2] > MOV_LIFTOFF_SAFETY_COUNTER) {
+    trigger_event(EV_LIFTOFF);
+    fsm_state->flight_state = THRUSTING_1;
+    fsm_state->clock_memory = 0;
+    fsm_state->memory[1] = 0;
+    fsm_state->memory[2] = 0;
+    fsm_state->angular_movement[0] = 0;
+    fsm_state->angular_movement[1] = 0;
+    fsm_state->angular_movement[2] = 0;
+  }
 }
 
 void check_idle_phase(flight_fsm_t *fsm_state, imu_data_t *imu_data, control_settings_t *settings) {
@@ -165,8 +190,10 @@ void check_idle_phase(flight_fsm_t *fsm_state, imu_data_t *imu_data, control_set
   /* Check if we move from IDLE To THRUSTING_1 */
   /* To Make sure that the timers start any acceleration direction is accepted
    * here */
-  int32_t acceleration =
-      imu_data->acc_x * imu_data->acc_x + imu_data->acc_y * imu_data->acc_y + imu_data->acc_z * imu_data->acc_z;
+  int32_t accel_x = (int32_t)imu_data->acc_x * (int32_t)imu_data->acc_x;
+  int32_t accel_y = (int32_t)imu_data->acc_y * (int32_t)imu_data->acc_y;
+  int32_t accel_z = (int32_t)imu_data->acc_z * (int32_t)imu_data->acc_z;
+  int32_t acceleration = accel_x + accel_y + accel_z;
 
   if ((float)acceleration > (float)settings->liftoff_acc_threshold * (float)settings->liftoff_acc_threshold) {
     fsm_state->memory[2]++;
@@ -205,6 +232,10 @@ void check_thrusting_1_phase(flight_fsm_t *fsm_state, estimation_output_t *state
 }
 
 void check_coasting_phase(flight_fsm_t *fsm_state, estimation_output_t *state_data) {
+  if (osTimerIsRunning(mach_timer.timer_id)) {
+    return;
+  }
+
   if (state_data->velocity < 0) {
     fsm_state->memory[1]++;
   }

boards/cats_rev1/Core/Src/tasks/task_init.c

@@ -310,7 +310,7 @@ static void create_event_map() {
   /* number of event types + 0th element */
   /* TODO: move number of events to a constant */
   /* TODO: where to free this? */
-  event_action_map = calloc(9, sizeof(event_action_map_elem_t));
+  event_action_map = calloc(10, sizeof(event_action_map_elem_t));
 
   // Moving
   /*vent_action_map[EV_MOVING].num_actions = 1;
@@ -333,50 +333,60 @@ static void create_event_map() {
   // Apogee / Drogue
   event_action_map[EV_APOGEE].num_actions = 1;
   event_action_map[EV_APOGEE].action_list = calloc(1, sizeof(peripheral_act_t));
-  event_action_map[EV_APOGEE].action_list[0].func_ptr = action_table[ACT_HIGH_CURRENT_ONE];
-  event_action_map[EV_APOGEE].action_list[0].func_arg = 1;
+  event_action_map[EV_APOGEE].action_list[0].func_ptr = action_table[ACT_SERVO_ONE];
+  event_action_map[EV_APOGEE].action_list[0].func_arg = 180;
 
   // Low Altitude / Main
-  event_action_map[EV_POST_APOGEE].num_actions = 1;
-  event_action_map[EV_POST_APOGEE].action_list = calloc(1, sizeof(peripheral_act_t));
-  event_action_map[EV_POST_APOGEE].action_list[0].func_ptr = action_table[ACT_HIGH_CURRENT_TWO];
+  event_action_map[EV_POST_APOGEE].num_actions = 2;
+  event_action_map[EV_POST_APOGEE].action_list = calloc(2, sizeof(peripheral_act_t));
+  event_action_map[EV_POST_APOGEE].action_list[0].func_ptr = action_table[ACT_HIGH_CURRENT_ONE];
   event_action_map[EV_POST_APOGEE].action_list[0].func_arg = 1;
+  event_action_map[EV_POST_APOGEE].action_list[1].func_ptr = action_table[ACT_SERVO_TWO];
+  event_action_map[EV_POST_APOGEE].action_list[1].func_arg = 180;
 
   // Timer 1 / Drogue
   event_action_map[EV_TIMER_1].num_actions = 1;
   event_action_map[EV_TIMER_1].action_list = calloc(1, sizeof(peripheral_act_t));
-  event_action_map[EV_TIMER_1].action_list[0].func_ptr = action_table[ACT_HIGH_CURRENT_ONE];
-  event_action_map[EV_TIMER_1].action_list[0].func_arg = 1;
+  event_action_map[EV_TIMER_1].action_list[0].func_ptr = action_table[ACT_SERVO_ONE];
+  event_action_map[EV_TIMER_1].action_list[0].func_arg = 180;
 
   // Timer 2 / Main
-  event_action_map[EV_TIMER_2].num_actions = 1;
-  event_action_map[EV_TIMER_2].action_list = calloc(1, sizeof(peripheral_act_t));
-  event_action_map[EV_TIMER_2].action_list[0].func_ptr = action_table[ACT_HIGH_CURRENT_TWO];
+  event_action_map[EV_TIMER_2].num_actions = 2;
+  event_action_map[EV_TIMER_2].action_list = calloc(2, sizeof(peripheral_act_t));
+  event_action_map[EV_TIMER_2].action_list[0].func_ptr = action_table[ACT_HIGH_CURRENT_ONE];
   event_action_map[EV_TIMER_2].action_list[0].func_arg = 1;
+  event_action_map[EV_TIMER_2].action_list[1].func_ptr = action_table[ACT_SERVO_TWO];
+  event_action_map[EV_TIMER_2].action_list[1].func_arg = 180;
 
   // Touchdown
   event_action_map[EV_TOUCHDOWN].num_actions = 1;
   event_action_map[EV_TOUCHDOWN].action_list = calloc(1, sizeof(peripheral_act_t));
   event_action_map[EV_TOUCHDOWN].action_list[0].func_ptr = action_table[ACT_SET_RECORDER_STATE];
   event_action_map[EV_TOUCHDOWN].action_list[0].func_arg = REC_OFF;
-  /* ................ */
 }
 
 static void init_timers() {
-
-  uint32_t used_timers = 2;
+  uint32_t used_timers = 0;
   /* Init timers*/
-  for(uint32_t i = 0; i < num_timers; i++){
-	  if(global_cats_config.config.timers[i].start_event && global_cats_config.config.timers[i].end_event){
-		  ev_timers[i].timer_init_event = (cats_event_e)global_cats_config.config.timers[i].start_event;
-		  ev_timers[i].execute_event = (cats_event_e)global_cats_config.config.timers[i].end_event;
-		  ev_timers[i].timer_duration_ticks = (uint32_t)global_cats_config.config.timers[i].duration;
-		  used_timers++;
-		}
+  for (uint32_t i = 0; i < num_timers; i++) {
+    if (global_cats_config.config.timers[i].start_event && global_cats_config.config.timers[i].end_event) {
+      ev_timers[i].timer_init_event = (cats_event_e)global_cats_config.config.timers[i].start_event;
+      ev_timers[i].execute_event = (cats_event_e)global_cats_config.config.timers[i].end_event;
+      ev_timers[i].timer_duration_ticks = (uint32_t)global_cats_config.config.timers[i].duration;
+      used_timers++;
+    }
   }
 
+  /* Init mach Timer */
+  mach_timer.timer_init_event = EV_LIFTOFF;
+  mach_timer.execute_event = EV_MAX_V;
+  mach_timer.timer_duration_ticks = 15000;
+
   /* Create Timers */
   for (uint32_t i = 0; i < used_timers; i++) {
     ev_timers[i].timer_id = osTimerNew((void *)trigger_event, osTimerOnce, (void *)ev_timers[i].execute_event, NULL);
   }
+
+  /* Create mach timer */
+  mach_timer.timer_id = osTimerNew((void *)trigger_event, osTimerOnce, (void *)mach_timer.execute_event, NULL);
 }

boards/cats_rev1/Core/Src/tasks/task_peripherals.c

@@ -36,6 +36,12 @@ _Noreturn void task_peripherals(__attribute__((unused)) void* argument) {
           osTimerStart(ev_timers[i].timer_id, ev_timers[i].timer_duration_ticks);
         }
       }
+      /* start Mach timer if needed */
+      if (curr_event == mach_timer.timer_init_event) {
+        if (mach_timer.timer_duration_ticks > 0) {
+          osTimerStart(mach_timer.timer_id, mach_timer.timer_duration_ticks);
+        }
+      }
       peripheral_act_t* action_list = event_action_map[curr_event].action_list;
       for (uint32_t i = 0; i < event_action_map[curr_event].num_actions; ++i) {
         timestamp_t curr_ts = osKernelGetTickCount();

boards/cats_rev1Pro/src/control/flight_phases.c

@@ -106,8 +106,10 @@ static void check_moving_phase(flight_fsm_t *fsm_state, imu_data_t *imu_data) {
   /* Check if we move from MOVING To THRUSTING_1 */
   /* To Make sure that the timers start any acceleration direction is accepted
    * here */
-  int32_t acceleration =
-      imu_data->acc_x * imu_data->acc_x + imu_data->acc_y * imu_data->acc_y + imu_data->acc_z * imu_data->acc_z;
+  int32_t accel_x = (int32_t)imu_data->acc_x * (int32_t)imu_data->acc_x;
+  int32_t accel_y = (int32_t)imu_data->acc_y * (int32_t)imu_data->acc_y;
+  int32_t accel_z = (int32_t)imu_data->acc_z * (int32_t)imu_data->acc_z;
+  int32_t acceleration = accel_x + accel_y + accel_z;
 
   if ((float)acceleration > (MOV_LIFTOFF_THRESHOLD * MOV_LIFTOFF_THRESHOLD)) {
     fsm_state->memory[2]++;
@@ -197,10 +199,12 @@ static void check_idle_phase(flight_fsm_t *fsm_state, imu_data_t *imu_data, cont
 
   /* Check if we move from READY To THRUSTING_1 */
   /* The absolut value of the acceleration is used here to make sure that we detect liftoff */
-  int32_t acceleration =
-      imu_data->acc_x * imu_data->acc_x + imu_data->acc_y * imu_data->acc_y + imu_data->acc_z * imu_data->acc_z;
+  int32_t accel_x = (int32_t)imu_data->acc_x * (int32_t)imu_data->acc_x;
+  int32_t accel_y = (int32_t)imu_data->acc_y * (int32_t)imu_data->acc_y;
+  int32_t accel_z = (int32_t)imu_data->acc_z * (int32_t)imu_data->acc_z;
+  int32_t acceleration = accel_x + accel_y + accel_z;
 
-  if ((float)acceleration > (float)settings->liftoff_acc_threshold * (float)settings->liftoff_acc_threshold) {
+  if ((float)acceleration > ((float)settings->liftoff_acc_threshold * (float)settings->liftoff_acc_threshold)) {
     fsm_state->memory[2]++;
   } else {
     fsm_state->memory[2] = 0;
@@ -238,10 +242,9 @@ static void check_thrusting_1_phase(flight_fsm_t *fsm_state, estimation_output_t
 }
 
 static void check_coasting_phase(flight_fsm_t *fsm_state, estimation_output_t *state_data) {
-
-    if(osTimerIsRunning(mach_timer.timer_id)){
-        return;
-    }
+  if (osTimerIsRunning(mach_timer.timer_id)) {
+    return;
+  }
 
   if (state_data->velocity < 0) {
     fsm_state->memory[1]++;