FINAL ACS TO BE USED FOR TESTING. COMMISSIONING, ACS MAIN, DATA ACQ ALL DONE.

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of ACS_FULL_Flowchart_BAE by Team Fox

Revision:
17:1e1955f3db75
Parent:
16:cc77770d787f
Child:
18:21740620c65e
--- a/main.cpp	Fri Jun 03 13:53:55 2016 +0000
+++ b/main.cpp	Thu Jun 09 14:12:55 2016 +0000
@@ -17,38 +17,25 @@
 //i2c//
 char data_send_flag = 'h'; 
 
-//.........acs...............//
-/* char ACS_INIT_STATUS = 'q';
-char ACS_DATA_ACQ_STATUS = 'q';
-char ACS_ATS_STATUS = 'q';
-char ACS_MAIN_STATUS = 'q';
-char ACS_STATUS = 'q';
+//.........ACS...............//
 
-char ACS_ATS_ENABLE = 'q';
-char ACS_DATA_ACQ_ENABLE = 'q';
-char ACS_STATE = 'q';*/
 
 uint8_t ACS_INIT_STATUS = 0;
 uint8_t ACS_DATA_ACQ_STATUS = 0;
 uint8_t ACS_ATS_STATUS = 0x60;
 uint8_t ACS_MAIN_STATUS = 0;
 uint8_t ACS_STATUS = 0;
+uint8_t ACS_DETUMBLING_ALGO_TYPE = 0;
+
+uint8_t ACS_TR_Z_SW_STATUS=1;
+uint8_t ACS_TR_XY_SW_STATUS=1;
 
 uint8_t ACS_ATS_ENABLE = 1;
 uint8_t ACS_DATA_ACQ_ENABLE = 1;
-uint8_t ACS_STATE = 4;
+uint8_t ACS_STATE = 7;
 
-//.....................eps...................//
+//.....................EPS...................//
 //eps init
-/*char EPS_INIT_STATUS = 'q';
-char EPS_BATTERY_GAUGE_STATUS = 'q';
-//eps main
-char EPS_MAIN_STATUS = 'q';
-char EPS_BATTERY_TEMP_STATUS = 'q';
-char EPS_STATUS = 'q';
-
-char EPS_BATTERY_HEAT_ENABLE = 'q';
-*/
 
 uint8_t EPS_INIT_STATUS = 0;
 uint8_t EPS_BATTERY_GAUGE_STATUS = 0;
@@ -71,11 +58,20 @@
 Timer t_start;
 Timer t_tc;
 Timer t_tm;
+
 Serial pc(USBTX, USBRX);
 int power_flag_dummy=2;
-float data[6];
+
+extern float gyro_data[3];
+extern float mag_data[3];
 
 extern float moment[3];
+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;
 extern BAE_HK_actual actual_data;
 extern BAE_HK_quant quant_data;
@@ -122,13 +118,6 @@
 InterruptIn  ir7(PIN42);//Charger IC - Fault Bar
 
 
-//DigitalOut TRXY_SW_EN(PIN71);  //TR XY Switch
-//DigitalOut DRV_Z_SLP(PIN88);    //Sleep pin of driver z
-//DigitalOut TRZ_SW(PIN40);  //TR Z Switch
-//DigitalOut CDMS_RESET(PIN7); // CDMS RESET
-//DigitalOut BCN_SW(PIN14);      //Beacon switch
-//DigitalOut DRV_XY_SLP(PIN82);
-
 
 DigitalOut TRXY_SW(PIN71);  //TR XY Switch
 DigitalOut DRV_Z_EN(PIN88);    //Sleep pin of driver z
@@ -155,202 +144,325 @@
 uint8_t iterI1;
 uint8_t iterI2;
 
- 
+extern float max_array(float arr[3]);
+
 void F_ACS()
 {
     
+    pc.printf("Entered ACS.\n\r");
 
-    //...................//
+    ACS_MAIN_STATUS = 1; //set ACS_MAIN_STATUS flag 
     
-    if(pf1check == 1)
-    {
-        if(iterP1 >= 3)
-        {
-            ATS1_SW_ENABLE = 1;  // turn off ats1 permanently
-            //FCTN_SWITCH_ATS(0);  // switch on ATS2    
-        }
-        else    
-        {
-        ATS1_SW_ENABLE = 0;
-        iterP1++;
-        }
-        pf1check = 0;
-    }
-    if(pf2check == 1)
-    {
-        if(iterP1 >= 3)
-        {
-            ATS2_SW_ENABLE = 1;  // turn off ats2 permanently
-            ACS_DATA_ACQ_ENABLE = 0;
-            ACS_STATE = 2 ; // check ACS_STATE = ACS_ZAXIS_MOMENT_ONLY    
-        }
-        else    
-        {
-        ATS2_SW_ENABLE = 0;
-        iterP2++;
-        }
-        pf2check = 0;
-    }
-     if(if1check == 1)
-    {
-        if(iterI1 >= 3)
-        {
-            TRXY_SW = 0;  // turn off TRXY permanently
-        }
-        else    
-        {
-         TRXY_SW = 1;   //switch on TRXY
-         iterI1++;
-        }
-    }    
-    if(if2check == 1)
-    {
-        if(iterI2 >= 3)
-        {
-            TRZ_SW = 0;  // turn off TRZ permanently
-            ACS_STATE = 2 ; // check ACS_STATE = ACS_ZAXIS_MOMENT_ONLY  
-        }
-        else    
-        {
-         TRZ_SW = 1;   //switch on Z
-         iterI2++;
-        }
-    }
-    
-    //float b1[3]={-23.376,-37.56,14.739}, omega1[3]={-1.52,2.746,0.7629}, moment1[3]= {1.0498,-1.0535,1.3246};
-    //b1[3] = {22, 22,10};
-    //omega1[3] = {2.1,3.0,1.5};
-    // ATS1_SW_ENABLE = 0;  // att sens2 switch is disabled
-    // ATS2_SW_ENABLE = 0; // att sens switch is disabled
-     
-    
-        
-    //Thread::signal_wait(0x1);  
-    ACS_MAIN_STATUS = 1; //set ACS_MAIN_STATUS flag 
     PWM1 = 0;                     //clear pwm pins
     PWM2 = 0;                     //clear pwm pins
     PWM3 = 0;                     //clear pwm pins
-    pc.printf("\n\rEntered ACS   %f\n",t_start.read());
+    
+
+    ACS_DATA_ACQ_STATUS = (uint8_t) FCTN_ATS_DATA_ACQ();
     
-    if(ACS_DATA_ACQ_ENABLE == 1)// check if ACS_DATA_ACQ_ENABLE = 1?
-    {
-    //FLAG();
-    FCTN_ATS_DATA_ACQ(); //the angular velocity is stored in the first 3 values and magnetic field values in next 3
-    pc.printf("gyro values\n\r"); //printing the angular velocity and magnetic field values
+    //printing the angular speed and magnetic field values
+    
+    pc.printf("gyro values\n\r"); 
     for(int i=0; i<3; i++) 
     {
         printf("%f\n\r",actual_data.AngularSpeed_actual[i]);
     }
+    
     pc.printf("mag values\n\r");
     for(int i=0; i<3; i++) 
     {
         pc.printf("%f\n\r",actual_data.Bvalue_actual[i]);
     }
-      //  for(int i=0;i<3;i++)
-//    {
-//    omega1[i]= data[i];
-//    b1[i] = data[i+3];
-//    }
-    }//if ACS_DATA_ACQ_ENABLE = 1
-     else
+    
+    for(int i=0;i<3;i++)
     {
-        // Z axis actuation is the only final solution,
+        mag_data[i] = actual_data.Bvalue_actual[i]/1000000;
+        gyro_data[i] = actual_data.AngularSpeed_actual[i]*3.14159/180;
     }
-    if(ACS_STATE == 0)        // check ACS_STATE = ACS_CONTROL_OFF?
+    
+
+
+    if(ACS_STATE == 0)                      // check ACS_STATE = ACS_CONTROL_OFF?
     {
           printf("\n\r acs control off\n");
-          FLAG();
           ACS_STATUS = 0;                // set ACS_STATUS = ACS_CONTROL_OFF
-          PWM1 = 0;                     //clear pwm pins
-          PWM2 = 0;                     //clear pwm pins
-          PWM3 = 0;                     //clear pwm pins
+          
+          ACS_MAIN_STATUS = 0;
+          return;
+    }
+    
+    else if(actual_data.power_mode<2)
+    {
+                printf("\n\r Low Power \n\r");
+                
+                DRV_Z_EN = 0;
+                DRV_XY_EN = 0;
+                
+                ACS_STATUS = 1;                    // set ACS_STATUS = ACS_LOW_POWER
+                      
+                ACS_MAIN_STATUS = 0;
+                return;
+        
     }
-    else
+    
+    else if(ACS_TR_Z_SW_STATUS != 1)
+    {
+                DRV_Z_EN = 0;
+                DRV_XY_EN = 0;
+                
+                ACS_STATUS = 2;                 // set ACS_STAUS = ACS_TRZ_DISABLED
+                
+                ACS_MAIN_STATUS = 0;
+                return;       
+    }
+    
+    else if(ACS_TR_XY_SW_STATUS != 1)
+    {                 
+                                
+                DRV_Z_EN = 1;
+                DRV_XY_EN = 0;
+                
+                ACS_STATUS = 3;                 // set ACS_STAUS = ACS_TRXY_DISABLED , Z axis only
+                
+                moment[0] = 0;
+                moment[1] = 0;
+                moment[2] =1.3;                 // is a dummy value 
+                
+
+                
+                FCTN_ACS_GENPWM_MAIN(moment) ;
+                
+                ACS_MAIN_STATUS = 0;
+                return;
+        
+    }
+    
+    else if(ACS_DATA_ACQ_STATUS == 1)
     {
-            if(actual_data.power_mode>1)
-            
-            {
-                if(ACS_STATE == 2)   // check ACS_STATE = ACS_ZAXIS_MOMENT_ONLY 
-                {
-                    FLAG();
-                    printf("\n\r z axis moment only\n");
-                    ACS_STATUS = 2;                    // set ACS_STATUS = ACS_ZAXIS_MOMENT_ONLY   
-                    //   FCTN_ACS_CNTRLALGO(b1, omega1);
-                    moment[0] = 0;
-                    moment[1] = 0;
-                    moment[2] =1.3;// is a dummy value 
-                    FCTN_ACS_GENPWM_MAIN(moment) ; 
-                 }
-                 else
-                {
-                if(ACS_STATE == 3) // check ACS_STATE = ACS_DATA_ACQ_FAILURE
-                {
-                     FLAG();
-                     printf("\n\r acs data failure "); 
-                     ACS_STATUS = 3;                    // set ACS_STATUS = ACS_DATA_ACQ_FAILURE
-                        PWM1 = 0;                     //clear pwm pins
-                        PWM2 = 0;                     //clear pwm pins
-                        PWM3 = 0;                     //clear pwm pins
-                 }
-                 else
-                 {
-                     if(ACS_STATE == 4)       // check ACS_STATE = ACS_NOMINAL_ONLY
+                DRV_Z_EN = 1;
+                DRV_XY_EN = 0;
+                
+                ACS_STATUS = 3;                 // set Set ACS_STATUS = ACS_DATA_ACQN_FAILURE , Z axis only
+                
+                moment[0] = 0;
+                moment[1] = 0;
+                moment[2] =1.3;                 // is a dummy value 
+                FCTN_ACS_GENPWM_MAIN(moment) ;
+                
+                ACS_MAIN_STATUS = 0;
+                return; 
+        
+    }
+    
+    else if(ACS_STATE == 5)
+    {
+                
+                DRV_Z_EN = 1;
+                DRV_XY_EN = 0;
+                
+                ACS_STATUS = 3;                 // set ACS_STAUS = ACS_TRXY_DISABLED by ACS_STATE i.e Z axis only
+                
+                moment[0] = 0;
+                moment[1] = 0;
+                moment[2] =1.3;                 // 1.3 is a dummy value 
+                FCTN_ACS_GENPWM_MAIN(moment) ;
+                
+                ACS_MAIN_STATUS = 0;
+                return;
+        
+    }
+    
+    else if(ACS_DATA_ACQ_STATUS == 2)           // MM only is available
+    {
+                DRV_Z_EN = 1;
+                DRV_XY_EN = 1;
+                              
+                ACS_STATUS = 4;                 // set Set ACS_STATUS = ACS_BDOT_CONTROL
+                
+                float db[3];
+                
+                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++)
                         {
-                            FLAG();
-                            printf("\n\r nominal");
-                            ACS_STATUS = 4;                    // set ACS_STATUS = ACS_NOMINAL_ONLY
-                            FCTN_ACS_CNTRLALGO(actual_data.Bvalue_actual,actual_data.AngularSpeed_actual);
-                            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) ;   
+                            db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
                         }
-                        else
+                    }
+                    
+                    
+                    
+                for(int i=0;i<3;i++)
+                {
+                    moment[i]=-kdetumble*db[i];
+                    b_old[i]= mag_data[i]; // Unit: Tesla/Second
+                }
+                
+                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_MAIN_STATUS = 0;
+                return; 
+        
+    }
+    
+    else if(ACS_STATE == 7)                     // Nominal mode
+    {
+                
+                printf("\n\r Nominal mode \n");
+                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++)
                         {
-                            if(ACS_STATE == 5)       // check ACS_STATE = ACS_AUTO_CONTROL
-                            {
-                                FLAG();
-                                printf("\n\r auto control");
-                                ACS_STATUS = 5;                    // set ACS_STATUS = ACS_AUTO_CONTROL
-                                //FCTN_ACS_AUTOCTRL_LOGIC                    // gotta include this code
-                            }
-                            else
-                            {
-                                if(ACS_STATE == 6)       // check ACS_STATE = ACS_DETUMBLING_ONLY
-                                {
-                                    FLAG();
-                                    printf("\n\r Entered detumbling \n");
-                                    ACS_STATUS = 6;                    // set ACS_STATUS = ACS_DETUMBLING_ONLY  
-                                    FCTN_ACS_CNTRLALGO(actual_data.Bvalue_actual,actual_data.AngularSpeed_actual);  // detumbling code has to be included
-                                    FCTN_ACS_GENPWM_MAIN(moment) ; 
-                                }
-                                else
-                                {
-                                    FLAG();
-                                    printf("\n\r invalid state");
-                                    ACS_STATUS = 7 ;                    // set ACS_STATUS = INVALID STATE 
-                                    PWM1 = 0;                     //clear pwm pins
-                                    PWM2 = 0;                     //clear pwm pins
-                                    PWM3 = 0;                     //clear pwm pins
-                                }//else of invalid 
-                            }//else of autocontrol 
-                        }//else of nominal
-                 }//else of data acg failure
+                            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
+                }
+                
+                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 = 5;                   // set ACS_STATUS = ACS_NOMINAL_ONLY
+                
+                ACS_MAIN_STATUS = 0;
+                return; 
+        
+    }
+    
+    else if(ACS_STATE == 8)                     // Auto Control
+    {
+                
+                printf("\n\r Auto control mode \n");
+                DRV_Z_EN = 1;
+                DRV_XY_EN = 1;              
+                
+                     
+                alarmmode=0;              
+                FCTN_ACS_CNTRLALGO(mag_data,gyro_data);
+                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
+                
+                ACS_MAIN_STATUS = 0;
+                return; 
+    }
+    
+    else if(ACS_STATE == 9)                     // Detumbling
+    {
+                DRV_Z_EN = 1;
+                DRV_XY_EN = 1;       
                 
-                }//else fo z axis moment only
-            }//if power >2
-            else
-            {
-                FLAG();
-                printf("\n\r low power");
-                ACS_STATUS = 1;                    // set ACS_STATUS = ACS_LOW_POWER
-                PWM1 = 0;                     //clear pwm pins
-                PWM2 = 0;                     //clear pwm pins
-                PWM3 = 0;                     //clear pwm pins
-            }
-    } //else for acs control off
+                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
+                        }
+                    }
+                            
+                
+                if (ACS_DETUMBLING_ALGO_TYPE == 0)
+                {
+                    
+                    for(int i=0;i<3;i++)
+                    {
+                        moment[i]=-kdetumble*(mag_data[(i+1)%3]*gyro_data[(i+2)%3]-mag_data[(i+2)%3]*gyro_data[(i+1)%3]); // Unit: Ampere*Meter^2
+                    }
+                    
+                    
+                    ACS_STATUS = 6;                         // set ACS_STATUS = ACS_BOMEGA_CONTROL
+                }
+                
+                else if(ACS_DETUMBLING_ALGO_TYPE == 1)
+                {
+                    
+                    for(int i=0;i<3;i++)
+                    {
+                        moment[i]=-kdetumble*db[i];          // Unit: Ampere*Meter^2
+                    }
+                    
+                    ACS_STATUS = 4;                         // set ACS_STATUS = ACS_BDOT_CONTROL
+                }
+                
+                for(int i=0;i<3;i++)
+                {
+                    
+                    b_old[i]= mag_data[i]; // Unit: Tesla/Second
+                }
+                                    
+                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_MAIN_STATUS = 0;
+                return; 
+    }
+    
+    ACS_STATUS = 7;                    //INVALID_STATE
+    DRV_Z_EN = 0;
+    DRV_XY_EN = 0;    
     ACS_MAIN_STATUS = 0; //clear ACS_MAIN_STATUS flag 
         
 }
@@ -630,10 +742,22 @@
         schedcount = 1;
     }
     if(schedcount%1==0)
-    { pc.printf("\nSTATE IS !!!!!! = %x !!\n",ACS_STATE);
-      pc.printf("\niterp1 !!!!!! = %x !!\n",iterP1);
-      pc.printf("\niteri2 IS !!!!!! = %x !!\n",iterI2);
-       F_ACS();
+    { 
+    
+    
+    pc.printf("\n\r\r\r\r \t\t******ACS******\r\r\r\r\r");
+    
+    pc.printf("ACSSTATE IS !!!!!! = %x !!\n\r",ACS_STATE);
+    
+    float acs_start = (float) t_start.read();
+      
+    F_ACS();
+    
+    float  acs_end = float( t_start.read() - acs_start ) ;
+    printf("\nTime taken for ACS is:\t %f\n\r",acs_end);
+    
+    pc.printf("\n\r\r\r\r \t\t******ACS EXIT******\r\r\r\r\r");
+       
     }
     
     if(schedcount%2==0)
@@ -760,11 +884,10 @@
 {
     printf("\n\r Initialising BAE ");
     //..........intial status....//
-    ACS_STATE = 4;
+    ACS_STATE = 8;
     ACS_ATS_ENABLE = 1;
     ACS_DATA_ACQ_ENABLE = 1;
     
-    
     EPS_BATTERY_HEAT_ENABLE = 1;
     actual_data.power_mode=3;
     //............intializing pins................//