Team Fox
ACS completed fully. All cases to be tested
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
ACS_Flowchart_BAE
by 
Team Fox
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;