Revision 18:21740620c65e, committed 2016-06-13

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

--- 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++)

--- 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();

--- 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;
                             }

--- 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;