Diff: ACS.cpp

Revision:

18:21740620c65e

Parent:

17:1e1955f3db75

Child:

19:403cb36e22ed

--- a/ACS.cpp	Thu Jun 09 14:12:55 2016 +0000
+++ b/ACS.cpp	Mon Jun 13 13:44:31 2016 +0000
@@ -61,7 +61,7 @@
 
 
 
-void FCTN_ACS_CNTRLALGO ( float b[3] , float omega[3])
+void FCTN_ACS_CNTRLALGO ( float b[3] , float omega[3],int nominal)
 {
 
     float normalising_fact;
@@ -83,14 +83,23 @@
         }
     }
     
-        if(max_array(omega)<(0.8*OmegaMax) && alarmmode==1)
-    {
-            alarmmode=0;
-    }
-        else if(max_array(omega)>OmegaMax&& alarmmode==0)
-    {
-            alarmmode=1;
-    }
+        if(nominal == 0)
+        
+        {
+    
+                if(max_array(omega)<(0.8*OmegaMax) && alarmmode==1)
+            {
+                    alarmmode=0;
+            }
+                else if(max_array(omega)>OmegaMax&& alarmmode==0)
+            {
+                    alarmmode=1;
+            }
+            
+        }
+        
+    
+
 
     for (i=0;i<3;i++)
     {
@@ -99,7 +108,7 @@
         omega_copy[i]=omega[i];
     }
 
-    if(alarmmode==0)
+    if((alarmmode==0)|| (nominal == 1))
         {
             controlmodes(b,db,omega,0);
         for (i=0;i<3;i++)
@@ -126,6 +135,7 @@
                 }
                 }
             }
+            ACS_STATUS = 5;
         }
         else
         {   
@@ -303,6 +313,7 @@
             {
                 Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2
             }
+            ACS_STATUS = 6;
         }
         else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo
         {
@@ -310,6 +321,7 @@
             {
                 Mmnt[i]=-kdetumble*db[i];
             }
+            ACS_STATUS = 4;
         }
     }
     for(i=0;i<3;i++)