Diff: main.cpp

Revision:

61:9348606edee2

Parent:

59:0fc0bfafaa9f

Child:

63:55d32e7dcad7

--- a/main.cpp	Sat Oct 08 09:19:13 2016 +0000
+++ b/main.cpp	Fri Oct 28 10:02:40 2016 +0000
@@ -613,6 +613,8 @@
             //#endif
             DRV_Z_EN = 0;
             DRV_XY_EN = 0;
+            ACS_TR_Z_ENABLE = 0;
+            ACS_TR_XY_ENABLE = 0;
             ACS_STATUS = 1;                    // set ACS_STATUS = ACS_LOW_POWER
             ACS_MAIN_STATUS = 0;
             return;
@@ -623,6 +625,8 @@
             pc.printf("\n\r Z disabled \n\r");
             DRV_Z_EN = 0;
             DRV_XY_EN = 0;
+            ACS_TR_Z_ENABLE = 0;
+            ACS_TR_XY_ENABLE = 0;
             ACS_STATUS = 2;                 // set ACS_STAUS = ACS_TRZ_DISABLED
             ACS_MAIN_STATUS = 0;
             return;       
@@ -632,6 +636,8 @@
             pc.printf("\n\r Z only \n\r");              
             DRV_Z_EN = 1;
             DRV_XY_EN = 0;
+            ACS_TR_Z_ENABLE = 1;
+    ACS_TR_XY_ENABLE = 0;
             ACS_STATUS = 3;                 // set ACS_STAUS = ACS_TRXY_DISABLED , Z axis only
             moment[0] = 0;
             moment[1] = 0;
@@ -650,6 +656,8 @@
             pc.printf("\n\r Z only no data \n\r");
             DRV_Z_EN = 1;
             DRV_XY_EN = 0;
+            ACS_TR_Z_ENABLE = 1;
+    ACS_TR_XY_ENABLE = 0;
             ACS_STATUS = 3;                 // set Set ACS_STATUS = ACS_DATA_ACQN_FAILURE , Z axis only
             
             moment[0] = 0;
@@ -669,6 +677,8 @@
             pc.printf("\n\r Z only by state \n\r");
             DRV_Z_EN = 1;
             DRV_XY_EN = 0;
+            ACS_TR_Z_ENABLE = 1;
+    ACS_TR_XY_ENABLE = 0;
             ACS_STATUS = 3;                 // set ACS_STAUS = ACS_TRXY_DISABLED by ACS_STATE i.e Z axis only
             
             moment[0] = 0;
@@ -684,6 +694,8 @@
             pc.printf("\n\r MM only BDOT \n\r");
             DRV_Z_EN = 1;
             DRV_XY_EN = 1;
+            ACS_TR_Z_ENABLE = 1;
+    ACS_TR_XY_ENABLE = 1;
                               
             ACS_STATUS = 4;                 // set Set ACS_STATUS = ACS_BDOT_CONTROL
             ACS_DETUMBLING_ALGO_TYPE = 0x01;
@@ -706,7 +718,9 @@
                 pc.printf("\n\r Nominal mode \n");
             #endif
             DRV_Z_EN = 1;
-            DRV_XY_EN = 1;            
+            DRV_XY_EN = 1;  
+            ACS_TR_Z_ENABLE = 1;
+    ACS_TR_XY_ENABLE = 1;          
             
             //timer_FCTN_ACS_CNTRLALGO.start();   
             FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE);
@@ -736,7 +750,10 @@
                 pc.printf("\n\r Auto control mode \n");
             #endif 
             DRV_Z_EN = 1;
-            DRV_XY_EN = 1;              
+            DRV_XY_EN = 1;     
+            
+            ACS_TR_Z_ENABLE = 1;
+    ACS_TR_XY_ENABLE = 1;         
             
             timer_FCTN_ACS_CNTRLALGO.start();   
             FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE);
@@ -764,7 +781,11 @@
         {
             pc.printf("\n\r Detumbling \n\r");
             DRV_Z_EN = 1;
-            DRV_XY_EN = 1;       
+            DRV_XY_EN = 1; 
+            
+            ACS_TR_Z_ENABLE = 1;
+    ACS_TR_XY_ENABLE = 1;
+          
             FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
             controlmode_mms = 0x00;
             FCTN_ACS_GENPWM_MAIN(moment) ;  
@@ -774,6 +795,8 @@
     ACS_STATUS = 7;                    //INVALID_STATE
     DRV_Z_EN = 0;
     DRV_XY_EN = 0;    
+    ACS_TR_Z_ENABLE = 0;
+    ACS_TR_XY_ENABLE = 0;
     ACS_MAIN_STATUS = 0; //clear ACS_MAIN_STATUS flag 
 }