Team Fox
ACS completed fully. All cases to be tested
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
ACS_Flowchart_BAE
by 
Team Fox
Diff: main.cpp
- Revision:
- 17:1e1955f3db75
- Parent:
- 16:cc77770d787f
- Child:
- 18:21740620c65e
diff -r cc77770d787f -r 1e1955f3db75 main.cpp
--- a/main.cpp Fri Jun 03 13:53:55 2016 +0000
+++ b/main.cpp Thu Jun 09 14:12:55 2016 +0000
@@ -17,38 +17,25 @@
//i2c//
char data_send_flag = 'h';
-//.........acs...............//
-/* char ACS_INIT_STATUS = 'q';
-char ACS_DATA_ACQ_STATUS = 'q';
-char ACS_ATS_STATUS = 'q';
-char ACS_MAIN_STATUS = 'q';
-char ACS_STATUS = 'q';
+//.........ACS...............//
-char ACS_ATS_ENABLE = 'q';
-char ACS_DATA_ACQ_ENABLE = 'q';
-char ACS_STATE = 'q';*/
uint8_t ACS_INIT_STATUS = 0;
uint8_t ACS_DATA_ACQ_STATUS = 0;
uint8_t ACS_ATS_STATUS = 0x60;
uint8_t ACS_MAIN_STATUS = 0;
uint8_t ACS_STATUS = 0;
+uint8_t ACS_DETUMBLING_ALGO_TYPE = 0;
+
+uint8_t ACS_TR_Z_SW_STATUS=1;
+uint8_t ACS_TR_XY_SW_STATUS=1;
uint8_t ACS_ATS_ENABLE = 1;
uint8_t ACS_DATA_ACQ_ENABLE = 1;
-uint8_t ACS_STATE = 4;
+uint8_t ACS_STATE = 7;
-//.....................eps...................//
+//.....................EPS...................//
//eps init
-/*char EPS_INIT_STATUS = 'q';
-char EPS_BATTERY_GAUGE_STATUS = 'q';
-//eps main
-char EPS_MAIN_STATUS = 'q';
-char EPS_BATTERY_TEMP_STATUS = 'q';
-char EPS_STATUS = 'q';
-
-char EPS_BATTERY_HEAT_ENABLE = 'q';
-*/
uint8_t EPS_INIT_STATUS = 0;
uint8_t EPS_BATTERY_GAUGE_STATUS = 0;
@@ -71,11 +58,20 @@
Timer t_start;
Timer t_tc;
Timer t_tm;
+
Serial pc(USBTX, USBRX);
int power_flag_dummy=2;
-float data[6];
+
+extern float gyro_data[3];
+extern float mag_data[3];
extern float moment[3];
+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;
extern BAE_HK_actual actual_data;
extern BAE_HK_quant quant_data;
@@ -122,13 +118,6 @@
InterruptIn ir7(PIN42);//Charger IC - Fault Bar
-//DigitalOut TRXY_SW_EN(PIN71); //TR XY Switch
-//DigitalOut DRV_Z_SLP(PIN88); //Sleep pin of driver z
-//DigitalOut TRZ_SW(PIN40); //TR Z Switch
-//DigitalOut CDMS_RESET(PIN7); // CDMS RESET
-//DigitalOut BCN_SW(PIN14); //Beacon switch
-//DigitalOut DRV_XY_SLP(PIN82);
-
DigitalOut TRXY_SW(PIN71); //TR XY Switch
DigitalOut DRV_Z_EN(PIN88); //Sleep pin of driver z
@@ -155,202 +144,325 @@
uint8_t iterI1;
uint8_t iterI2;
-
+extern float max_array(float arr[3]);
+
void F_ACS()
{
+ pc.printf("Entered ACS.\n\r");
- //...................//
+ ACS_MAIN_STATUS = 1; //set ACS_MAIN_STATUS flag
- if(pf1check == 1)
- {
- if(iterP1 >= 3)
- {
- ATS1_SW_ENABLE = 1; // turn off ats1 permanently
- //FCTN_SWITCH_ATS(0); // switch on ATS2
- }
- else
- {
- ATS1_SW_ENABLE = 0;
- iterP1++;
- }
- pf1check = 0;
- }
- if(pf2check == 1)
- {
- if(iterP1 >= 3)
- {
- ATS2_SW_ENABLE = 1; // turn off ats2 permanently
- ACS_DATA_ACQ_ENABLE = 0;
- ACS_STATE = 2 ; // check ACS_STATE = ACS_ZAXIS_MOMENT_ONLY
- }
- else
- {
- ATS2_SW_ENABLE = 0;
- iterP2++;
- }
- pf2check = 0;
- }
- if(if1check == 1)
- {
- if(iterI1 >= 3)
- {
- TRXY_SW = 0; // turn off TRXY permanently
- }
- else
- {
- TRXY_SW = 1; //switch on TRXY
- iterI1++;
- }
- }
- if(if2check == 1)
- {
- if(iterI2 >= 3)
- {
- TRZ_SW = 0; // turn off TRZ permanently
- ACS_STATE = 2 ; // check ACS_STATE = ACS_ZAXIS_MOMENT_ONLY
- }
- else
- {
- TRZ_SW = 1; //switch on Z
- iterI2++;
- }
- }
-
- //float b1[3]={-23.376,-37.56,14.739}, omega1[3]={-1.52,2.746,0.7629}, moment1[3]= {1.0498,-1.0535,1.3246};
- //b1[3] = {22, 22,10};
- //omega1[3] = {2.1,3.0,1.5};
- // ATS1_SW_ENABLE = 0; // att sens2 switch is disabled
- // ATS2_SW_ENABLE = 0; // att sens switch is disabled
-
-
-
- //Thread::signal_wait(0x1);
- ACS_MAIN_STATUS = 1; //set ACS_MAIN_STATUS flag
PWM1 = 0; //clear pwm pins
PWM2 = 0; //clear pwm pins
PWM3 = 0; //clear pwm pins
- pc.printf("\n\rEntered ACS %f\n",t_start.read());
+
+
+ ACS_DATA_ACQ_STATUS = (uint8_t) FCTN_ATS_DATA_ACQ();
- if(ACS_DATA_ACQ_ENABLE == 1)// check if ACS_DATA_ACQ_ENABLE = 1?
- {
- //FLAG();
- FCTN_ATS_DATA_ACQ(); //the angular velocity is stored in the first 3 values and magnetic field values in next 3
- pc.printf("gyro values\n\r"); //printing the angular velocity and magnetic field values
+ //printing the angular speed and magnetic field values
+
+ pc.printf("gyro values\n\r");
for(int i=0; i<3; i++)
{
printf("%f\n\r",actual_data.AngularSpeed_actual[i]);
}
+
pc.printf("mag values\n\r");
for(int i=0; i<3; i++)
{
pc.printf("%f\n\r",actual_data.Bvalue_actual[i]);
}
- // for(int i=0;i<3;i++)
-// {
-// omega1[i]= data[i];
-// b1[i] = data[i+3];
-// }
- }//if ACS_DATA_ACQ_ENABLE = 1
- else
+
+ for(int i=0;i<3;i++)
{
- // Z axis actuation is the only final solution,
+ mag_data[i] = actual_data.Bvalue_actual[i]/1000000;
+ gyro_data[i] = actual_data.AngularSpeed_actual[i]*3.14159/180;
}
- if(ACS_STATE == 0) // check ACS_STATE = ACS_CONTROL_OFF?
+
+
+
+ if(ACS_STATE == 0) // check ACS_STATE = ACS_CONTROL_OFF?
{
printf("\n\r acs control off\n");
- FLAG();
ACS_STATUS = 0; // set ACS_STATUS = ACS_CONTROL_OFF
- PWM1 = 0; //clear pwm pins
- PWM2 = 0; //clear pwm pins
- PWM3 = 0; //clear pwm pins
+
+ ACS_MAIN_STATUS = 0;
+ return;
+ }
+
+ else if(actual_data.power_mode<2)
+ {
+ printf("\n\r Low Power \n\r");
+
+ DRV_Z_EN = 0;
+ DRV_XY_EN = 0;
+
+ ACS_STATUS = 1; // set ACS_STATUS = ACS_LOW_POWER
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
}
- else
+
+ else if(ACS_TR_Z_SW_STATUS != 1)
+ {
+ DRV_Z_EN = 0;
+ DRV_XY_EN = 0;
+
+ ACS_STATUS = 2; // set ACS_STAUS = ACS_TRZ_DISABLED
+
+ ACS_MAIN_STATUS = 0;
+ return;
+ }
+
+ else if(ACS_TR_XY_SW_STATUS != 1)
+ {
+
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 0;
+
+ ACS_STATUS = 3; // set ACS_STAUS = ACS_TRXY_DISABLED , Z axis only
+
+ moment[0] = 0;
+ moment[1] = 0;
+ moment[2] =1.3; // is a dummy value
+
+
+
+ FCTN_ACS_GENPWM_MAIN(moment) ;
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
+ }
+
+ else if(ACS_DATA_ACQ_STATUS == 1)
{
- if(actual_data.power_mode>1)
-
- {
- if(ACS_STATE == 2) // check ACS_STATE = ACS_ZAXIS_MOMENT_ONLY
- {
- FLAG();
- printf("\n\r z axis moment only\n");
- ACS_STATUS = 2; // set ACS_STATUS = ACS_ZAXIS_MOMENT_ONLY
- // FCTN_ACS_CNTRLALGO(b1, omega1);
- moment[0] = 0;
- moment[1] = 0;
- moment[2] =1.3;// is a dummy value
- FCTN_ACS_GENPWM_MAIN(moment) ;
- }
- else
- {
- if(ACS_STATE == 3) // check ACS_STATE = ACS_DATA_ACQ_FAILURE
- {
- FLAG();
- printf("\n\r acs data failure ");
- ACS_STATUS = 3; // set ACS_STATUS = ACS_DATA_ACQ_FAILURE
- PWM1 = 0; //clear pwm pins
- PWM2 = 0; //clear pwm pins
- PWM3 = 0; //clear pwm pins
- }
- else
- {
- if(ACS_STATE == 4) // check ACS_STATE = ACS_NOMINAL_ONLY
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 0;
+
+ ACS_STATUS = 3; // set Set ACS_STATUS = ACS_DATA_ACQN_FAILURE , Z axis only
+
+ moment[0] = 0;
+ moment[1] = 0;
+ moment[2] =1.3; // is a dummy value
+ FCTN_ACS_GENPWM_MAIN(moment) ;
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
+ }
+
+ else if(ACS_STATE == 5)
+ {
+
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 0;
+
+ ACS_STATUS = 3; // set ACS_STAUS = ACS_TRXY_DISABLED by ACS_STATE i.e Z axis only
+
+ moment[0] = 0;
+ moment[1] = 0;
+ moment[2] =1.3; // 1.3 is a dummy value
+ FCTN_ACS_GENPWM_MAIN(moment) ;
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
+ }
+
+ else if(ACS_DATA_ACQ_STATUS == 2) // MM only is available
+ {
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 1;
+
+ ACS_STATUS = 4; // set Set ACS_STATUS = ACS_BDOT_CONTROL
+
+ float db[3];
+
+ 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++)
{
- FLAG();
- printf("\n\r nominal");
- ACS_STATUS = 4; // set ACS_STATUS = ACS_NOMINAL_ONLY
- FCTN_ACS_CNTRLALGO(actual_data.Bvalue_actual,actual_data.AngularSpeed_actual);
- 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) ;
+ db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
}
- else
+ }
+
+
+
+ for(int i=0;i<3;i++)
+ {
+ moment[i]=-kdetumble*db[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]);
+ }
+
+ FCTN_ACS_GENPWM_MAIN(moment) ;
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
+ }
+
+ else if(ACS_STATE == 7) // Nominal mode
+ {
+
+ printf("\n\r Nominal mode \n");
+ 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++)
{
- if(ACS_STATE == 5) // check ACS_STATE = ACS_AUTO_CONTROL
- {
- FLAG();
- printf("\n\r auto control");
- ACS_STATUS = 5; // set ACS_STATUS = ACS_AUTO_CONTROL
- //FCTN_ACS_AUTOCTRL_LOGIC // gotta include this code
- }
- else
- {
- if(ACS_STATE == 6) // check ACS_STATE = ACS_DETUMBLING_ONLY
- {
- FLAG();
- printf("\n\r Entered detumbling \n");
- ACS_STATUS = 6; // set ACS_STATUS = ACS_DETUMBLING_ONLY
- FCTN_ACS_CNTRLALGO(actual_data.Bvalue_actual,actual_data.AngularSpeed_actual); // detumbling code has to be included
- FCTN_ACS_GENPWM_MAIN(moment) ;
- }
- else
- {
- FLAG();
- printf("\n\r invalid state");
- ACS_STATUS = 7 ; // set ACS_STATUS = INVALID STATE
- PWM1 = 0; //clear pwm pins
- PWM2 = 0; //clear pwm pins
- PWM3 = 0; //clear pwm pins
- }//else of invalid
- }//else of autocontrol
- }//else of nominal
- }//else of data acg failure
+ 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
+ }
+
+ 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 = 5; // set ACS_STATUS = ACS_NOMINAL_ONLY
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
+ }
+
+ else if(ACS_STATE == 8) // Auto Control
+ {
+
+ printf("\n\r Auto control mode \n");
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 1;
+
+
+ alarmmode=0;
+ FCTN_ACS_CNTRLALGO(mag_data,gyro_data);
+ 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
+
+ ACS_MAIN_STATUS = 0;
+ return;
+ }
+
+ else if(ACS_STATE == 9) // Detumbling
+ {
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 1;
- }//else fo z axis moment only
- }//if power >2
- else
- {
- FLAG();
- printf("\n\r low power");
- ACS_STATUS = 1; // set ACS_STATUS = ACS_LOW_POWER
- PWM1 = 0; //clear pwm pins
- PWM2 = 0; //clear pwm pins
- PWM3 = 0; //clear pwm pins
- }
- } //else for acs control off
+ 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]);
+ }
+ FCTN_ACS_GENPWM_MAIN(moment) ;
+
+ ACS_MAIN_STATUS = 0;
+ return;
+ }
+
+ ACS_STATUS = 7; //INVALID_STATE
+ DRV_Z_EN = 0;
+ DRV_XY_EN = 0;
ACS_MAIN_STATUS = 0; //clear ACS_MAIN_STATUS flag
}
@@ -630,10 +742,22 @@
schedcount = 1;
}
if(schedcount%1==0)
- { pc.printf("\nSTATE IS !!!!!! = %x !!\n",ACS_STATE);
- pc.printf("\niterp1 !!!!!! = %x !!\n",iterP1);
- pc.printf("\niteri2 IS !!!!!! = %x !!\n",iterI2);
- F_ACS();
+ {
+
+
+ pc.printf("\n\r\r\r\r \t\t******ACS******\r\r\r\r\r");
+
+ pc.printf("ACSSTATE IS !!!!!! = %x !!\n\r",ACS_STATE);
+
+ float acs_start = (float) t_start.read();
+
+ F_ACS();
+
+ float acs_end = float( t_start.read() - acs_start ) ;
+ printf("\nTime taken for ACS is:\t %f\n\r",acs_end);
+
+ pc.printf("\n\r\r\r\r \t\t******ACS EXIT******\r\r\r\r\r");
+
}
if(schedcount%2==0)
@@ -760,11 +884,10 @@
{
printf("\n\r Initialising BAE ");
//..........intial status....//
- ACS_STATE = 4;
+ ACS_STATE = 8;
ACS_ATS_ENABLE = 1;
ACS_DATA_ACQ_ENABLE = 1;
-
EPS_BATTERY_HEAT_ENABLE = 1;
actual_data.power_mode=3;
//............intializing pins................//