Diff: main.cpp

Revision:

18:21740620c65e

Parent:

17:1e1955f3db75

--- a/main.cpp	Thu Jun 09 14:12:55 2016 +0000
+++ b/main.cpp	Mon Jun 13 13:44:31 2016 +0000
@@ -69,7 +69,6 @@
 extern float b_old[3];  // Unit: Tesla
 extern float db[3];
 extern uint8_t flag_firsttime;
-extern uint8_t alarmmode;
 
 
 extern uint8_t BCN_FEN;
@@ -326,40 +325,7 @@
                 DRV_Z_EN = 1;
                 DRV_XY_EN = 1;               
                 
-                alarmmode = 0;
-                float normalising_fact;
-                
-                if(flag_firsttime==1)
-                    {
-                        for(int i=0;i<3;i++)
-                        {
-                            db[i]=0; // Unit: Tesla/Second
-                        }
-                        flag_firsttime=0;
-                    }
-                else
-                    {
-                        for(int i=0;i<3;i++)
-                        {
-                            db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
-                        }
-                    }   
-                                
-                controlmodes(mag_data,db,gyro_data, 0);
-                
-                if(max_array(moment)>MmntMax)
-                {
-                    normalising_fact=max_array(moment)/MmntMax;
-                    for(int i=0;i<3;i++)
-                    {
-                        moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
-                    }
-                }
-                
-                 for(int i=0;i<3;i++)
-                {
-                    b_old[i]= mag_data[i]; // Unit: Tesla/Second
-                }
+                FCTN_ACS_CNTRLALGO(mag_data,gyro_data,1);
                 
                 printf("\n\r Moment values returned by control algo \n");
                 for(int i=0; i<3; i++) 
@@ -381,17 +347,15 @@
                 printf("\n\r Auto control mode \n");
                 DRV_Z_EN = 1;
                 DRV_XY_EN = 1;              
-                
-                     
-                alarmmode=0;              
-                FCTN_ACS_CNTRLALGO(mag_data,gyro_data);
+                             
+                FCTN_ACS_CNTRLALGO(mag_data,gyro_data,0);
                 printf("\n\r Moment values returned by control algo \n");
                 for(int i=0; i<3; i++) 
                     {
                         printf("%f\t",moment[i]);
                     }
                 FCTN_ACS_GENPWM_MAIN(moment) ; 
-                ACS_STATUS = 6;                     // set ACS_STATUS = ACS_AUTO_CONTROL
+                                     // set ACS_STATUS in function
                 
                 ACS_MAIN_STATUS = 0;
                 return;