Merge pull request #116 from Space-and-Satellite-Systems-UC-Davis/105-provide-support-for-critical-section-for-rtos-interrupts

105 provide support for critical section for rtos interrupts

Author: lichunho

ADCS.c

@@ -18,31 +18,44 @@
 
 #include <stdbool.h>
 
+adcs_main_status detumbleEX(vec3 needle, uint64_t minTime, uint64_t maxTime)
+{
+    switch (detumble(needle, false, minTime, maxTime)) {
+        case DETUMBLING_SUCCESS:
+            return ADCS_MAIN_SUCCESS;
+        case DETUMBLING_FAILURE_CURR_MILLIS:
+        case DETUMBLING_FAILURE_MAGNETOMETER:
+        case DETUMBLING_FAILURE_IMU:
+        case DETUMBLING_FAILURE_CONTROL_COILS:
+        case DETUMBLING_FAILURE_DELAY_MS:
+        case DETUMBLING_HAS_RESTARTED:
+            return ADCS_MAIN_DETUMBLE_ERR;
+    }
+}
+
 adcs_main_status ADCS_MAIN(adcs_mode mode)
 {
     switch (mode) {
+        case ADCS_INITIAL_DETUMBLE:
+            return detumbleEX((vec3){ 0, 0, 0 }, 3600, 36000);
+
         case ADCS_DETUMBLE:
-            switch (detumble((vec3){ 0, 0, 0 }, false, 0, 3600)) {
-                case DETUMBLING_SUCCESS:
-                    break;
-                case DETUMBLING_FAILURE_CURR_MILLIS:
-                case DETUMBLING_FAILURE_MAGNETOMETER:
-                case DETUMBLING_FAILURE_IMU:
-                case DETUMBLING_FAILURE_CONTROL_COILS:
-                case DETUMBLING_FAILURE_DELAY_MS:
-                    return ADCS_MAIN_DETUMBLE_ERR;
-            }
-            break;
+            return detumbleEX((vec3){ 0, 0, 0 }, 0, 3600);
 
+        // SHOULD NEVER EVER EVER BE ENGAGED IN PRODUCTION (SPACE)
         case ADCS_COILS_TESTING:
             switch (detumble((vec3){ 0, 0, 0 }, true, 0, 3600)) {
                 case DETUMBLING_SUCCESS:
                     break;
                 case DETUMBLING_FAILURE_CURR_MILLIS:
                 case DETUMBLING_FAILURE_MAGNETOMETER:
                 case DETUMBLING_FAILURE_IMU:
+                    // Do something
                 case DETUMBLING_FAILURE_CONTROL_COILS:
+                    // Do something
                 case DETUMBLING_FAILURE_DELAY_MS:
+                    // Do something
+                case DETUMBLING_HAS_RESTARTED:
                     return ADCS_MAIN_COILS_TESTING_ERR;
             }
             break;

ADCS.h

@@ -10,33 +10,32 @@
 #ifndef ADCS_H
 #define ADCS_H
 
-
 typedef enum {
-	ADCS_DETUMBLE,
-	ADCS_HDD_EXP_ANGVEL,
-	ADCS_HDD_EXP_TRIAD,
+    ADCS_DETUMBLE,
+    ADCS_INITIAL_DETUMBLE,
+    ADCS_HDD_EXP_ANGVEL,
+    ADCS_HDD_EXP_TRIAD,
     ADCS_HDD_EXP_RAMP,
     ADCS_TESTING,
     ADCS_COILS_TESTING,
     ADCS_HDD_TESTING,
     ADCS_ROTISSERIE
-} adcs_mode; //TODO: full list
+} adcs_mode; // TODO: full list
 
 typedef enum {
-   ADCS_MAIN_SUCCESS,
-   ADCS_MAIN_UNKNOWN,
-   ADCS_MAIN_ECLIPSE,
-   ADCS_MAIN_HDD_ERR,
-   ADCS_MAIN_IMU_ERR,
-   ADCS_MAIN_DETUMBLE_ERR,
-   ADCS_MAIN_COILS_TESTING_ERR,
-   ADCS_ROTISSERIE_ERR
-} adcs_main_status; //TODO: full list
-
+    ADCS_MAIN_SUCCESS,
+    ADCS_MAIN_UNKNOWN,
+    ADCS_MAIN_ECLIPSE,
+    ADCS_MAIN_HDD_ERR,
+    ADCS_MAIN_IMU_ERR,
+    ADCS_MAIN_DETUMBLE_ERR,
+    ADCS_MAIN_COILS_TESTING_ERR,
+    ADCS_ROTISSERIE_ERR
+} adcs_main_status; // TODO: full list
 
 #ifdef __cplusplus
 extern "C" {
-#endif//__cplusplus
+#endif //__cplusplus
 
 /**@brief Command ADCS to run a mode.
  *
@@ -46,20 +45,16 @@ extern "C" {
  *
  * @return The mode's status code.
  */
-adcs_main_status
-ADCS_MAIN(adcs_mode mode);
-
+adcs_main_status ADCS_MAIN(adcs_mode mode);
 
 /**@brief Ask ADCS to recommend a mode to run.
  *
  * @return The recommended mode.
  */
-adcs_mode
-ADCS_recommend_mode();
+adcs_mode ADCS_recommend_mode();
 
 #ifdef __cplusplus
 }
-#endif//__cplusplus
-
+#endif //__cplusplus
 
-#endif//ADCS_H
+#endif // ADCS_H

config.mk

@@ -7,6 +7,7 @@ CFLAGS += -Wextra
 CFLAGS += -Wno-unused-variable 
 CFLAGS += -Wno-unused-value 
 CFLAGS += -Wno-unused-but-set-variable
+CFLAGS += -Wswitch-enum
 
 ifneq ($(V), 1)
 Q = @

control/detumble/detumble.c

@@ -12,6 +12,7 @@
 #include "control/detumble/bdot_control.h"
 #include "control/detumble/detumble_util.h"
 #include "virtual_intellisat.h"
+#include "virtual_ros.h"
 
 #include <math.h>
 
@@ -46,6 +47,12 @@ detumble_status detumble(vec3 needle, bool isTesting, uint64_t maxTime,
     startTime = curr_millis;
 
     do {
+        vi_enter_critical();
+
+        if (vi_task_has_restarted()) {
+            // Return to Schedulers to restart Detumbling
+            return DETUMBLING_HAS_RESTARTED;
+        }
 
         // Perform delay for the coil magnetic field decay
         if (vi_control_coil(0, 0, 0))
@@ -85,6 +92,8 @@ detumble_status detumble(vec3 needle, bool isTesting, uint64_t maxTime,
 
         keepDetumbling = isTooSoon || (!isTimeOut && isTooFast);
 
+        vi_exit_critical();
+
     } while (isTesting || keepDetumbling);
 
     // Increment generation on successful execution

control/detumble/detumble.h

@@ -30,6 +30,7 @@ typedef enum detumble {
     DETUMBLING_FAILURE_CONTROL_COILS,
     DETUMBLING_FAILURE_IMU,
     DETUMBLING_FAILURE_DELAY_MS,
+    DETUMBLING_HAS_RESTARTED
 
 } detumble_status;
 

control/experiment/PID_experiment.c

@@ -1,6 +1,7 @@
 #include "control/experiment/PID_experiment.h"
 #include "adcs_math/sensors.h"
 #include "virtual_intellisat.h"
+#include "virtual_ros.h"
 
 #include <math.h>
 #include <stdint.h>
@@ -46,6 +47,12 @@ PID_status PID_experiment(double target, int infinite)
     // Run a while loop
     while (infinite || (fabs(target - angVel_curr.z) > 0.1)) {
 
+        vi_enter_critical();
+        if (vi_task_has_restarted()) {
+            // Return to Schedulers to restart Detumbling
+            return PID_EXPERIMENT_HAS_RESTARTED;
+        }
+
         if (getIMU(imu, angVel_prev, &angVel_curr))
             return PID_EXPERIMENT_ANGVEL_FAILURE;
         // Get the current time (Virtual Intellisat)
@@ -72,6 +79,8 @@ PID_status PID_experiment(double target, int infinite)
             return PID_EXPERIMENT_COMMAND_FAILURE;
 
         angVel_prev = angVel_curr;
+
+        vi_exit_critical();
     }
 
     // Increment generation on successful execution

control/experiment/PID_experiment.h

@@ -5,11 +5,12 @@
 #include "control/PID/PID.h"
 #include "virtual_intellisat.h"
 
-typedef enum PID_experiment{
+typedef enum PID_experiment {
     PID_EXPERIMENT_SUCCESS,
     PID_EXPERIMENT_ANGVEL_FAILURE,
     PID_EXPERIMENT_MILLIS_FAILURE,
-    PID_EXPERIMENT_COMMAND_FAILURE
+    PID_EXPERIMENT_COMMAND_FAILURE,
+    PID_EXPERIMENT_HAS_RESTARTED
 } PID_status;
 
 /**@brief Performs an experiement for the PID function.
@@ -18,5 +19,4 @@ typedef enum PID_experiment{
  */
 PID_status PID_experiment(double target, int infinite);
 
-
-#endif//PID_EXPERIMENT_H
+#endif // PID_EXPERIMENT_H

control/experiment/determination_experiment.c

@@ -41,6 +41,13 @@ determination_exp_status determination_experiment()
     // Run a while loop
     while (fabs(target - angvel_z) > 0.1) {
         vi_delay_ms(100);
+      
+        vi_enter_critical();
+        if (vi_task_has_restarted()) {
+            // Return to Schedulers to restart Detumbling
+            return DETERMINATION_EXPERIMENT_HAS_RESTARTED;
+        }
+
 
         // default values for now, waiting for sun sensors to implement get_sun
         sun.x = 0;
@@ -68,6 +75,8 @@ determination_exp_status determination_experiment()
             return DETERMINATION_EXPERIMENT_HDD_COMMAND_FAILURE;
         prevAttitude = currAttitude;
         prev_millis = curr_millis;
+
+        vi_exit_critical();
     }
 
     // Increment generation on successful execution

control/experiment/determination_experiment.h

@@ -4,22 +4,22 @@
 #include "adcs_math/vector.h"
 #include "control/PID/PID.h"
 #include "virtual_intellisat.h"
+#include "virtual_ros.h"
 
-
-typedef enum determination_experiment{
+typedef enum determination_experiment {
     DETERMINATION_EXPERIMENT_SUCCESS,
     DETERMINATION_EXPERIMENT_MILLIS_FAILURE,
-    DETERMINATION_EXPERIMENT_HDD_COMMAND_FAILURE
-}determination_exp_status;
+    DETERMINATION_EXPERIMENT_HDD_COMMAND_FAILURE,
+    DETERMINATION_EXPERIMENT_HAS_RESTARTED
+} determination_exp_status;
 
 /*
  * @brief Performs an experiement using the function.
- *        Uses determination to calculate the angular velocity 
+ *        Uses determination to calculate the angular velocity
  *        instead of the IMU
  *
  * @return PID_status A return code.
  */
 determination_exp_status determination_experiment();
 
-
-#endif//DETERMINATION_EXPERIMENT_H
+#endif // DETERMINATION_EXPERIMENT_H

control/experiment/ramp_experiment.c

@@ -2,6 +2,7 @@
 #include "adcs_math/calibration.h"
 #include "adcs_math/sensors.h"
 #include "virtual_intellisat.h"
+#include "virtual_ros.h"
 
 // TODO: HDD alternation?
 #define HDD_CHOICE VI_HDD1
@@ -31,12 +32,20 @@ run_ramp_experiment_status ramp_experiment()
             return RUN_RAMP_EXPERIMENT_MILLIS_FAILURE;
         }
 
+        vi_enter_critical();
+        if (vi_task_has_restarted()) {
+            // Return to Schedulers to restart ramp_experiment
+            return RUN_RAMP_EXPERIMENT_HAS_RESTARTED;
+        }
+
         double command = linear_ramp_command(ti, &controller);
 
         command_status = vi_hdd_command(Hdd, command);
         if (command_status == HDD_COMMAND_FAILURE) {
             return RUN_RAMP_EXPERIMENT_COMMAND_FAILURE;
         }
+
+        vi_exit_critical();
     }
 
     return RUN_RAMP_EXPERIMENT_SUCCESS;