Team Fox
ACS completed fully. All cases to be tested
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
ACS_Flowchart_BAE
by 
Team Fox
Diff: TCTM.cpp
- Revision:
- 18:21740620c65e
- Parent:
- 16:cc77770d787f
--- 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;
}