Team Fox
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
Diff: TCTM.cpp
- Revision:
- 19:403cb36e22ed
- Parent:
- 18:21740620c65e
--- a/TCTM.cpp Mon Jun 13 13:44:31 2016 +0000
+++ b/TCTM.cpp Tue Jun 28 10:11:54 2016 +0000
@@ -18,6 +18,7 @@
extern DigitalOut DRV_Z_EN;
extern DigitalOut DRV_XY_EN;
extern uint8_t ACS_STATUS;
+extern uint8_t ACS_ATS_STATUS;
extern float b_old[3]; // Unit: Tesla
extern float db[3];
extern uint8_t flag_firsttime;
@@ -64,9 +65,16 @@
extern int SENSOR_INIT();
extern int FCTN_ATS_DATA_ACQ();
-extern void FCTN_ACS_CNTRLALGO(float*,float*,int);
uint8_t telemetry[135];
+extern AnalogIn CurrentInput; // Input from Current Multiplexer //PIN54
+
+
+extern DigitalOut SelectLineb3; // MSB of Select Lines
+extern DigitalOut SelectLineb2;
+extern DigitalOut SelectLineb1;
+extern DigitalOut SelectLineb0; // LSB of Select Lines
+
void FCTN_CONVERT_UINT (uint8_t input[2], float* output)
{
@@ -514,114 +522,67 @@
{
case 0xE0:
{
- float mag_data[3]={0,0,0};
- float gyro_data[3]={0,0,0};
+ float mag_data_comm[3]={0,0,0};
+ float gyro_data_comm[3]={0,0,0};
+ float moment_comm[3];
printf("ACS_COMSN SOFTWARE\r\n");
//ACK_L234_telemetry
ACS_STATE = tc[4];
- if(ACS_STATE == 7) // Nominal mode
+ 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);
+ FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
- printf("%f\t",moment[i]);
- }
+ printf("%f\t",moment_comm[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 Auto control mode \n");
+
+ FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
- printf("%f\t",moment[i]);
+ printf("%f\t",moment_comm[i]);
}
}
else if(ACS_STATE == 9) // Detumbling
{
- 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
- }
-
+
+ FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
- printf("%f\t",moment[i]);
- }
+ printf("%f\t",moment_comm[i]);
+ }
+
}
- ACS_STATUS = 7;
+ else
+ {
+
+ ACS_STATUS = 7;
+ }
// Control algo commissioning
- 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]
+ FCTN_CONVERT_FLOAT(moment_comm[0],&telemetry[4]); //telemetry[4] - telemetry[7]
+ FCTN_CONVERT_FLOAT(moment_comm[1],&telemetry[8]); //telemetry[8] - telemetry[11]
+ FCTN_CONVERT_FLOAT(moment_comm[2],&telemetry[12]); //telemetry[12] - telemetry[15]
// to include commission TR as well
for(uint8_t i=16;i<132;i++)
{
@@ -638,8 +599,10 @@
printf("HARDWARE_COMSN\r\n");
//ACK_L234_telemetry
- int init;
- int data;
+ int init1=0;
+ int init2=0;
+ int data1=0;
+ int data2=0;
float PWM_tc[3];
PWM_tc[0]=(float(tc[4]))*1;
@@ -662,14 +625,135 @@
wait_ms(ACS_DEMAG_TIME_DELAY);
ATS2_SW_ENABLE = 1;
+ wait_ms(5);
ATS1_SW_ENABLE = 0;
- init = SENSOR_INIT();
- data = SENSOR_DATA_ACQ();
+ wait_ms(5);
+ init1 = SENSOR_INIT();
+ if( init1 == 1)
+ {
+ data1 = SENSOR_DATA_ACQ();
+ }
+
+
+ ATS1_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS2_SW_ENABLE = 0;
+ wait_ms(5);
+
+ if(data1 == 0)
+ {
+ ATS2_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
+ }
+ else if(data1==1)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
+ }
+ else if(data1==2)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
+ }
+ else if(data1==3)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x30;
+ }
+
+
+ init2 = SENSOR_INIT();
+ if( init2 == 1)
+ {
+ data2 = SENSOR_DATA_ACQ();
+ }
+
+ uint8_t ats_data=1;
- ATS2_SW_ENABLE = 1;
- ATS1_SW_ENABLE = 0;
- init = SENSOR_INIT();
- data = SENSOR_DATA_ACQ();
+ if(data2 == 0)
+ {
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
+ if(data1 == 2)
+ {
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ }
+ else if(data1 == 3)
+ {
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
+ }
+ else if(data1 == 1)
+ {
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
+ ats_data = 0;
+ }
+
+
+ }
+
+ else if(data2==1)
+ {
+
+
+
+ if(data1 == 2)
+ {
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
+ }
+ else if(data1 == 3)
+ {
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
+ }
+ else
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
+ ats_data = 0;
+ }
+ }
+
+ else if(data2==2)
+ {
+ if(data1 == 3)
+ {
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
+ }
+ else
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ }
+ }
+ else if(data2==3)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
+ }
+
+ SelectLineb3 =0;
+ SelectLineb2 =1;
+ SelectLineb1 =0;
+ SelectLineb0 =1;
+ int resistance;
+
+
PWM1 = PWM_tc[0];
PWM2 = 0;
@@ -677,7 +761,23 @@
wait_ms(ACS_DEMAG_TIME_DELAY);
- SENSOR_DATA_ACQ();
+ if(ats_data == 0)
+ SENSOR_DATA_ACQ();
+ actual_data.current_actual[5]=CurrentInput.read();
+ actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
+
+
+ resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
+ if(actual_data.current_actual[5]>1.47)
+ {
+ actual_data.current_actual[5]=3694/log(24.032242*resistance);
+ }
+ else{
+
+ actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
+ }
+
+
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)]);
@@ -690,9 +790,23 @@
wait_ms(ACS_DEMAG_TIME_DELAY);
-
+ if(ats_data == 0)
+ SENSOR_DATA_ACQ();
+ actual_data.current_actual[5]=CurrentInput.read();
+ actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
+
+
+ resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
+ if(actual_data.current_actual[5]>1.47)
+ {
+ actual_data.current_actual[5]=3694/log(24.032242*resistance);
+ }
+ else{
+
+ actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
+ }
+
- 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)]);
@@ -704,16 +818,54 @@
wait_ms(ACS_DEMAG_TIME_DELAY);
- wait_ms(ACS_DEMAG_TIME_DELAY);
-
- SENSOR_DATA_ACQ();
+ if(ats_data == 0)
+ SENSOR_DATA_ACQ();
+ actual_data.current_actual[5]=CurrentInput.read();
+ actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
+
+
+ resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
+ if(actual_data.current_actual[5]>1.47)
+ {
+ actual_data.current_actual[5]=3694/log(24.032242*resistance);
+ }
+ else{
+
+ actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
+ }
+
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)]);
+ PWM1 = 0;
+ PWM2 = 0;
+ PWM3 = 0;
-
+ wait_ms(ACS_DEMAG_TIME_DELAY);
+
+ if(ats_data == 0)
+ SENSOR_DATA_ACQ();
+ actual_data.current_actual[5]=CurrentInput.read();
+ actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
+
+
+ resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
+ if(actual_data.current_actual[5]>1.47)
+ {
+ actual_data.current_actual[5]=3694/log(24.032242*resistance);
+ }
+ else{
+
+ actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
+ }
+
+
+ FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (8*4)]);
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (9*4)]);
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (10*4)]);
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (11*4)]);
// for(int i=0; i<12; i++)
// FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]);