ACS completed fully. All cases to be tested

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of ACS_Flowchart_BAE by Team Fox

Files at this revision

API Documentation at this revision

Comitter:
Bragadeesh153
Date:
Mon Jun 13 13:44:31 2016 +0000
Parent:
17:1e1955f3db75
Commit message:
ACS algo commissioning done, Hardware comissioning yet to be finalised

Changed in this revision

ACS.cpp Show annotated file Show diff for this revision Revisions of this file
ACS.h Show annotated file Show diff for this revision Revisions of this file
TCTM.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 1e1955f3db75 -r 21740620c65e ACS.cpp
--- 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++)
diff -r 1e1955f3db75 -r 21740620c65e ACS.h
--- a/ACS.h	Thu Jun 09 14:12:55 2016 +0000
+++ b/ACS.h	Mon Jun 13 13:44:31 2016 +0000
@@ -9,13 +9,14 @@
 #define kdetumble 2000000
 #define MmntMax 1.1  // Unit: Ampere*Meter^2
 #define OmegaMax 1*3.1415/180.0 // Unit: Radians/Second
+#define ACS_DEMAG_TIME_DELAY 20
 
 #define senstivity_gyro 6.5536; //senstivity is obtained from 2^15/5000dps
 #define senstivity_mag  32.768; //senstivity is obtained from 2^15/1000microtesla
 #define senstivity_time 32; //senstivity is obtained from 2^16/2048dps
 
 void FCTN_ACS_GENPWM_MAIN(float*);
-void FCTN_ACS_CNTRLALGO(float*,float*);
+void FCTN_ACS_CNTRLALGO(float*,float*,int);
 void controlmodes(float*, float*, float*, uint8_t);
 void inverse(float mat[3][3],float inv[3][3]);
 extern void FLAG();
diff -r 1e1955f3db75 -r 21740620c65e TCTM.cpp
--- a/TCTM.cpp	Thu Jun 09 14:12:55 2016 +0000
+++ b/TCTM.cpp	Mon Jun 13 13:44:31 2016 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "TCTM.h"
 #include "crc.h"
+#include "ACS.h"
 #include "EPS.h"
 #include "pin_config.h"
 #include "FreescaleIAP.h"
@@ -14,6 +15,13 @@
 
 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
+extern DigitalOut DRV_Z_EN;
+extern DigitalOut DRV_XY_EN;
+extern uint8_t ACS_STATUS;
+extern float b_old[3];  // Unit: Tesla
+extern float db[3];
+extern uint8_t flag_firsttime;
+extern uint8_t ACS_DETUMBLING_ALGO_TYPE;
 
 extern DigitalOut TRXY_SW;  //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN
 extern DigitalOut TRZ_SW;  //TR Z Switch
@@ -52,10 +60,11 @@
 extern void F_EPS();
 extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]);
 extern int FCTN_ACS_INIT();
-
+extern int SENSOR_DATA_ACQ();
+extern int SENSOR_INIT();
 
 extern int FCTN_ATS_DATA_ACQ();
-extern void FCTN_ACS_CNTRLALGO(float*,float*);
+extern void FCTN_ACS_CNTRLALGO(float*,float*,int);
 uint8_t telemetry[135];
 
 void FCTN_CONVERT_UINT (uint8_t input[2], float* output)
@@ -505,90 +514,111 @@
                     {
                         case 0xE0:
                         {
-                            float B[3],W[3];
-                            printf("ACS_COMSN\r\n");
+                            float mag_data[3]={0,0,0};
+                            float gyro_data[3]={0,0,0};
+                            printf("ACS_COMSN SOFTWARE\r\n");
                             //ACK_L234_telemetry
 
-                            uint8_t B_x[2];
-                            uint8_t B_y[2];
-                            uint8_t B_z[2];
-                            uint8_t W_x[2];
-                            uint8_t W_y[2];
-                            uint8_t W_z[2];
-
-                            B_x[0]=tc[3];
-                            B_x[1]=tc[4];
-                            B_y[0]=tc[5];
-                            B_y[1]=tc[6];
-                            B_z[0]=tc[7];
-                            B_z[1]=tc[8];
-
-                            W_x[0]=tc[9];
-                            W_x[1]=tc[10];
-                            W_y[0]=tc[11];
-                            W_y[1]=tc[12];
-                            W_z[0]=tc[13];
-                            W_z[1]=tc[14];
-
-                            FCTN_CONVERT_UINT(B_x,&B[0]);
-                            FCTN_CONVERT_UINT(B_y,&B[1]);
-                            FCTN_CONVERT_UINT(B_z,&B[2]);
-
-                            FCTN_CONVERT_UINT (W_x, &W[0]);
-                            FCTN_CONVERT_UINT (W_y, &W[1]);
-                            FCTN_CONVERT_UINT (W_z, &W[2]);
+                            ACS_STATE = tc[4];
+                            
+                             if(ACS_STATE == 7)                     // Nominal mode
+                            {
+                                        
+                                        printf("\n\r Nominal mode \n");
+                                        DRV_Z_EN = 1;
+                                        DRV_XY_EN = 1;               
+                                        
+                                        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++) 
+                                            {
+                                                printf("%f\t",moment[i]);
+                                            } 
+                                        
+                                        ACS_STATUS = 5;                   // set ACS_STATUS = ACS_NOMINAL_ONLY           
+                                
+                            }
+                            
+                            else if(ACS_STATE == 8)                     // Auto Control
+                            {
+                                        
+                                        printf("\n\r Auto control mode \n");
+                                        DRV_Z_EN = 1;
+                                        DRV_XY_EN = 1;              
+                                        
+                                                           
+                                        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]);
+                                            }
 
-                            telemetry[0]=0xB0;
-                            telemetry[1]=tc[0];
-                            telemetry[2]=ACK_CODE;
-                            //FCTN_ATS_DATA_ACQ();  //get data
-                            printf("gyro values\n\r");
-                            for(int i=0; i<3; i++)
-                                printf("%f\n\r",W[i]);
-                            printf("mag values\n\r");
-                            for(int i=0; i<3; i++)
-                                printf("%f\n\r",B[i]);
-                   /*         FCTN_CONVERT_FLOAT(data[0],&telemetry[4]); //telemetry[4] - telemetry[7]
-                            FCTN_CONVERT_FLOAT(data[1],&telemetry[8]); //telemetry[8] - telemetry[11]
-                            FCTN_CONVERT_FLOAT(data[2],&telemetry[12]); //telemetry[12] - telemetry[15]
-                            FCTN_CONVERT_FLOAT(data[0],&telemetry[16]); //telemetry[16] - telemetry[19]
-                            FCTN_CONVERT_FLOAT(data[1],&telemetry[20]); //telemetry[20] - telemetry[23]
-                            FCTN_CONVERT_FLOAT(data[2],&telemetry[24]); //telemetry[24] - telemetry[27]
-                            if((data[0]<8) && (data[1]<8) && (data[2] <8))
-                                telemetry[28] = 1; // gyro values in correct range
-                            else
-                                telemetry[28] = 0;
-                            if ((data[3] > 20 ) && (data[4] >20) && (data[5]>20)&& (data[3] < 50 ) && (data[4] <50) && (data[5]<50))
-                                telemetry[29] = 1;   // mag values in correct range
-                            else
-                                telemetry[29] = 0;
-                     */
-                       //     float B[3],W[3];
-                       //     B[0] = B0;
-                       //     B[1] = B1;
-                       //     B[2] = B2;
-                       //     W[0] = W0;
-                       //     W[1] = W1;
-                       //     W[2] = W2;
-                            // Control algo commissioning
-                        /*    FCTN_ACS_CNTRLALGO(B,W);
-                            FCTN_CONVERT_FLOAT(moment[0],&telemetry[30]); //telemetry[30] - telemetry[33]
-                            FCTN_CONVERT_FLOAT(moment[1],&telemetry[34]); //telemetry[34] - telemetry[37]
-                            FCTN_CONVERT_FLOAT(moment[2],&telemetry[38]); //telemetry[38] - telemetry[41]
-                            // to include commission TR as well
-                            for(uint8_t i=42;i<132;i++)
+                            }
+                            
+                            else if(ACS_STATE == 9)                     // Detumbling
                             {
-                                telemetry[i]=0x00;
+                                        DRV_Z_EN = 1;
+                                        DRV_XY_EN = 1;       
+                                        
+                                        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]);
+                                            } 
                             }
-
-                            crc16 = CRC::crc16_gen(telemetry,132);
-                            telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
-                            telemetry[134] = (uint8_t)(crc16&0x00FF);
-                            break;
-                         */
+                            
+                            ACS_STATUS = 7;
 
                            // Control algo commissioning
-                            FCTN_ACS_CNTRLALGO(B,W);
                             FCTN_CONVERT_FLOAT(moment[0],&telemetry[4]); //telemetry[4] - telemetry[7]
                             FCTN_CONVERT_FLOAT(moment[1],&telemetry[8]); //telemetry[8] - telemetry[11]
                             FCTN_CONVERT_FLOAT(moment[2],&telemetry[12]); //telemetry[12] - telemetry[15]
@@ -605,53 +635,85 @@
                         }
                         case 0xE1:
                         {
-                            float moment_tc[3];
                             printf("HARDWARE_COMSN\r\n");
                             //ACK_L234_telemetry
-                            uint8_t M0[2];
-                            uint8_t M1[2];
-                            uint8_t M2[2];
+                            
+                            int init;
+                            int data;
 
-                            M0[0]=tc[3];
-                            M0[1]=tc[4];
-                            M1[0]=tc[5];
-                            M1[1]=tc[6];
-                            M2[0]=tc[7];
-                            M2[1]=tc[8];
+                            float PWM_tc[3];
+                            PWM_tc[0]=(float(tc[4]))*1;
+                            PWM_tc[1]=(float(tc[4]))*1;
+                            PWM_tc[2]=(float(tc[4]))*1;
+                            
+                            DRV_Z_EN = 1;
+                            DRV_XY_EN = 1;  
+
 
 
                             telemetry[0]=0xB0;
                             telemetry[1]=tc[0];
                             telemetry[2]=ACK_CODE;
                             
-                            float PWM_measured[3];
+                            PWM1 = 0;
+                            PWM2 = 0;
+                            PWM3 = 0;
+                            
+                            wait_ms(ACS_DEMAG_TIME_DELAY);
+                            
+                            ATS2_SW_ENABLE = 1;
+                            ATS1_SW_ENABLE = 0;
+                            init = SENSOR_INIT();
+                            data = SENSOR_DATA_ACQ();
+                            
+                            ATS2_SW_ENABLE = 1;
+                            ATS1_SW_ENABLE = 0;
+                            init = SENSOR_INIT();
+                            data = SENSOR_DATA_ACQ();
+
+                            PWM1 = PWM_tc[0];
+                            PWM2 = 0;
+                            PWM3 = 0;
+                            
+                            wait_ms(ACS_DEMAG_TIME_DELAY);
+                            
+                            SENSOR_DATA_ACQ();
+                            FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (0*4)]);
+                            FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (1*4)]);
+                            FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (2*4)]);
+                            FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (3*4)]);
                             
                             
-                            FCTN_CONVERT_UINT(M0,&moment_tc[0]);
-                            moment_tc[1] = 0;
-                            moment_tc[2] = 0;
-                            FCTN_ACS_GENPWM_MAIN(moment_tc);
-                            PWM_measured[0] = PWM1.read();
-                            FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (0*4)]);
+                            PWM1 = 0;
+                            PWM2 = PWM_tc[1];
+                            PWM3 = 0;
+                            
+                            wait_ms(ACS_DEMAG_TIME_DELAY);
+                            
+                            
+                            
+                            SENSOR_DATA_ACQ();
+                            FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (4*4)]);
+                            FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (5*4)]);
+                            FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (6*4)]);
+                            FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (7*4)]);
                             
-                            FCTN_CONVERT_UINT(M1, &moment_tc[1]);
-                            moment_tc[0] = 0;
-                            moment_tc[2] = 0;
-                            FCTN_ACS_GENPWM_MAIN(moment_tc);
-                            PWM_measured[1] = PWM2.read();
-                            FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (1*4)]);
+                            PWM1 = 0;
+                            PWM2 = 0;
+                            PWM3 = PWM_tc[2];
+                            
+                            wait_ms(ACS_DEMAG_TIME_DELAY);
+                            
+                            wait_ms(ACS_DEMAG_TIME_DELAY);
                             
-                            FCTN_CONVERT_UINT(M2, &moment_tc[2]);
-                            moment_tc[0] = 0;
-                            moment_tc[1] = 0;
-                            FCTN_ACS_GENPWM_MAIN(moment_tc);
-                            PWM_measured[2] = PWM3.read();
-                            FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (2*4)]);
+                            SENSOR_DATA_ACQ();
+                            FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (4*4)]);
+                            FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (5*4)]);
+                            FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (6*4)]);
+                            FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (7*4)]);
+                            
+                            
 
-                            FCTN_CONVERT_FLOAT(PWM_measured[0],&telemetry[4]);    //4-7
-                            FCTN_CONVERT_FLOAT(PWM_measured[1],&telemetry[8]);    //8-11
-                            FCTN_CONVERT_FLOAT(PWM_measured[2],&telemetry[12]);  //12-15
-                            
                             
                             // for(int i=0; i<12; i++)
                              //   FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]);
@@ -661,7 +723,7 @@
 
 
                             // to include commission TR as well
-                            for(uint8_t i=28;i<132;i++)
+                            for(uint8_t i=35;i<132;i++)
                             {
                                 telemetry[i]=0x00;
                             }
diff -r 1e1955f3db75 -r 21740620c65e main.cpp
--- 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;