![](/media/cache/group/nine_tails_kurama_by_zubair43-d4zyf5s.jpg.50x50_q85.jpg)
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
Diff: main.cpp
- Revision:
- 0:7b4c00e3912f
- Child:
- 1:446a959e36ce
diff -r 000000000000 -r 7b4c00e3912f main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Dec 24 19:15:43 2015 +0000 @@ -0,0 +1,505 @@ +#include "mbed.h" +#include "rtos.h" +#include "pin_config.h" +#include "ACS.h" +#include "EPS.h" +#include "BCN.h" +#include "TCTM.h" + +#define tm_len 134 +#define tc_len 135 +#define bae_data_len 150 +#define batt_heat_low 20 +//***************************************************** flags *************************************************************// +uint32_t BAE_STATUS = 0x00000000; +uint32_t BAE_ENABLE = 0xFFFFFFFF; + +//.........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'; + +char ACS_ATS_ENABLE = 'q'; +char ACS_DATA_ACQ_ENABLE = 'q'; +char ACS_STATE = 'q'; + +//.....................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'; + +//.......................global variables..................................................................// new hk structure- everything has to changed based on this +char BAE_data[bae_data_len]; + + +//*************************************Global declarations************************************************// +const int addr = 0x20; //slave address + +Timer t_rfsilence; +Timer t_start; +Serial pc(USBTX, USBRX); +int power_flag_dummy=2; +float data[6]; + +extern float moment[3]; +extern SensorData Sensor; +extern uint8_t BCN_FEN; + +bool write_ack = 1; +bool read_ack = 1; +char telecommand[tc_len]; +char telemetry[tm_len]; +char data_send_flag = 'h'; + +//*****************************************************Assigning pins******************************************************// +DigitalOut gpo1(PTC0); // enable of att sens2 switch +DigitalOut gpo2(PTC16); // enable of att sens switch +InterruptIn irpt_4m_mstr(PIN38); //I2c interrupt from CDMS +DigitalOut irpt_2_mstr(PIN4); //I2C interrupt to CDMS +I2CSlave slave (PIN1,PIN2); +DigitalOut batt_heat(PIN96); + +//gpo1 = 0; +PwmOut PWM1(PIN93); //x //Functions used to generate PWM signal +PwmOut PWM2(PIN94); //y +PwmOut PWM3(PIN95); //z //PWM output comes from pins p6 + + +/*****************************************************************Threads USed***********************************************************************************/ +Thread *ptr_t_acs; +Thread *ptr_t_eps; +Thread *ptr_t_bcn; +Thread *ptr_t_i2c; + +/*********************************************************FCTN HEADERS***********************************************************************************/ + +void FCTN_ISR_I2C(); +void FCTN_TM(); + +//*******************************************ACS THREAD**************************************************// + +void T_ACS(void const *args) +{ + 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}; + // gpo1 = 0; // att sens2 switch is disabled + // gpo2 = 0; // att sens switch is disabled + + while(1) + { + + Thread::signal_wait(0x1); + ACS_MAIN_STATUS = 's'; //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()); + + if(ACS_DATA_ACQ_ENABLE == 'e')// 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 + for(int i=0; i<3; i++) + { + pc.printf("%f\n\r",data[i]); + } + pc.printf("mag values\n\r"); + for(int i=3; i<6; i++) + { + pc.printf("%f\n\r",data[i]); + + for(int i=0;i<3;i++) + { + omega1[i]= data[i]; + b1[i] = data[i+3]; + } + } + }//if ACS_DATA_ACQ_ENABLE = 1 + else + { + // Z axis actuation is the only final solution, + } + 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 + } + else + { + if(Sensor.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 + { + FLAG(); + printf("\n\r nominal"); + ACS_STATUS = '4'; // set ACS_STATUS = ACS_NOMINAL_ONLY + FCTN_ACS_CNTRLALGO(b1,omega1); + 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) ; + } + else + { + 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(b1,omega1); // 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 + + }//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 + + + + ACS_MAIN_STATUS = 'c'; //clear ACS_MAIN_STATUS flag + }//while ends + + +} +//***************************************************EPS THREAD***********************************************// + +void T_EPS(void const *args) +{ + while(1) + { + Thread::signal_wait(0x2); + pc.printf("\n\rEntered EPS %f\n",t_start.read()); + EPS_MAIN_STATUS = 's'; // Set EPS main status + //FCTN_READ_HK(); + //FCTN_APPEND_HKDATA(); + FCTN_BATTERYGAUGE_MAIN(actual_data.Batt_gauge_actual); + if (actual_data.Battery_gauge_actual[1] == 200) //data not received + { + actual_data.power_mode = 1; + EPS_BATTERY_GAUGE_STATUS = 'c'; //clear EPS_BATTERY_GAUGE_STATUS + + } + else + { + FCTN_EPS_POWERMODE(actual_data.Battery_gauge_actual[1]); //updating power level + EPS_BATTERY_GAUGE_STATUS = 's'; //set EPS_BATTERY_GAUGE_STATUS + } + /* if( Temperature data received) + { + FCTN_BATT_TEMP_SENSOR_MAIN(); + EPS_BATTERY_TEMP_STATUS = 's'; //set EPS_BATTERY_TEMP_STATUS + if(EPS_BATTERY_HEAT_ENABLE = 'e') + { + if(actual_data.Batt_temp_actual[0] < batt_heat_low) + { + batt_heat = 1; //turn on battery heater + } + else + { + batt_heat = 0; //turn off battery heater + } + + } + else if(EPS_BATTERY_HEAT_ENABLE = 'd) + { + EPS_STATUS = EPS_BATTERY_HEATER_DISABLED; + } + + } + else + { + Set battery temp to XX + EPS_BATTERY_TEMP_STATUS = 'c'; //clear EPS_BATTERY_TEMP_STATUS + EPS_STATUS = EPS_ERR_BATTERY_TEMP; + } + + EPS_MAIN_STATUS = 'c'; // clear EPS main status */ + + } + +} + +//**************************************************BCN THREAD*******************************************************************// + +void T_BCN(void const *args) +{ + while(1) + { + Thread::signal_wait(0x3); + pc.printf("\n\rEntered BCN %f\n",t_start.read()); + + P_BCN_TX_MAIN(); + } + +} + +//**************************************************TCTM THREAD*******************************************************************// + +void T_TC(void const * args) +{ + while(1) + { + Thread::signal_wait(0x4); + wait_us(200); // can be between 38 to 15700 + if( slave.receive() == 0) + slave.stop(); + else if( slave.receive() == 1) // slave writes to master + { + write_ack=slave.write(telemetry,tm_len); + } + else if( slave.receive()==3 || slave.receive()==2) // slave read + { + read_ack=slave.read(telecommand,tc_len); + pc.printf("\n\rTELECOMMAND received from CDMS is %s \n",telecommand); + pc.printf("\n\r Executing Telecommand \n"); + FCTN_TC_DECODE((uint8_t*) telecommand); + FCTN_TM(); + } + + } +} + +void FCTN_TM() +{ + irpt_2_mstr = 0; + pc.printf("\n\r Telemetry Generation \n"); + irpt_2_mstr = 1; +} + + +//******************************************************* I2C *******************************************************************// + +void FCTN_I2C_ISR() +{ + ptr_t_i2c->signal_set(0x4); +} + + +//------------------------------------------------------------------------------------------------------------------------------------------------ +//SCHEDULER +//------------------------------------------------------------------------------------------------------------------------------------------------ +uint8_t schedcount=1; +void T_SC(void const *args) +{ + printf("\n\r in scheduler"); + + if(schedcount == 7) //to reset the counter + { + schedcount = 1; + } + if(schedcount%1==0) + { + ptr_t_acs -> signal_set(0x1); + } + + if(schedcount%2==0) + { + // ptr_t_eps -> signal_set(0x2); + + } + if(schedcount%3==0) + { + //ptr_t_bcn -> signal_set(0x3); + } + schedcount++; + printf("\n\r exited scheduler"); +} + +Timer t_flag; +void FLAG() +{ + +//.............acs..................// + if(ACS_INIT_STATUS == 's') + BAE_STATUS = BAE_STATUS | 0x00000080; //set ACS_INIT_STATUS flag + else if(ACS_INIT_STATUS == 'c') + BAE_STATUS &= 0xFFFFFF7F; //clear ACS_INIT_STATUS flag + + if(ACS_DATA_ACQ_STATUS == 's') + BAE_STATUS =BAE_STATUS | 0x00000100; //set ACS_DATA_ACQ_STATUS flag + else if(ACS_DATA_ACQ_STATUS == 'c') + BAE_STATUS &= 0xFFFFFEFF; //clear ACS_DATA_ACQ_STATUS flag + + if(ACS_ATS_ENABLE == 'e') + BAE_ENABLE |= 0x00000004; + else if(ACS_ATS_ENABLE == 'd') + BAE_ENABLE = BAE_ENABLE &0xFFFFFFFB | 0x00000004; + + if(ACS_DATA_ACQ_STATUS == 'f') + BAE_STATUS |= 0x00000200; + + if(ACS_MAIN_STATUS == 's') + BAE_STATUS = (BAE_STATUS | 0x00001000); //set ACS_MAIN_STATUS flag + else if(ACS_MAIN_STATUS == 'c') + BAE_STATUS &= 0xFFFFEFFF; //clear ACS_MAIN_STATUS flag + + if(ACS_STATUS == '0') + BAE_STATUS = (BAE_STATUS & 0xFFFF1FFF); // set ACS_STATUS = ACS_CONTROL_OFF + else if(ACS_STATUS == '1') + BAE_STATUS =(BAE_STATUS & 0xFFFF1FFF) | 0x00002000; // set ACS_STATUS = ACS_LOW_POWER + else if(ACS_STATUS == '2') + BAE_STATUS = (BAE_STATUS & 0xFFFF1FFF)| 0x00004000; // set ACS_STATUS = ACS_ZAXIS_MOMENT_ONLY + else if(ACS_STATUS == '3') + BAE_STATUS = (BAE_STATUS & 0xFFFF1FFF) | 0x00006000; // set ACS_STATUS = ACS_DATA_ACQ_FAILURE + else if(ACS_STATUS == '4') + BAE_STATUS = (BAE_STATUS & 0xFFFF1FFF) | 0x00008000; // set ACS_STATUS = ACS_NOMINAL_ONLY + else if(ACS_STATUS == '5') + BAE_STATUS =(BAE_STATUS & 0xFFFF1FFF) | 0x0000A000; // set ACS_STATUS = ACS_AUTO_CONTROL + else if(ACS_STATUS == '6') + BAE_STATUS =(BAE_STATUS & 0xFFFF1FFF) | 0x0000C000; // set ACS_STATUS = ACS_DETUMBLING_ONLY + else + BAE_STATUS =(BAE_STATUS & 0xFFFF1FFF) | 0x0000E000; // set ACS_STATUS = INVALID STATE + + if(ACS_STATE == '0') + BAE_ENABLE = (BAE_ENABLE & 0xFFFFFF8F); //ACS_STATE = ACS_CONTROL_OFF + else if(ACS_STATE == '2') + BAE_ENABLE = ((BAE_ENABLE & 0xFFFFFF8F)| 0x00000020); // ACS_STATE = ACS_ZAXIS_MOMENT_ONLY + else if(ACS_STATE == '3') + BAE_ENABLE = ((BAE_ENABLE & 0xFFFFFF8F)| 0x00000030); // set ACS_STATUS = ACS_DATA_ACQ_FAILURE + else if(ACS_STATE == '4') + BAE_ENABLE = ((BAE_ENABLE & 0xFFFFFF8F)| 0x00000040); // ACS_STATE = ACS_NOMINAL_ONLY + else if(ACS_STATE == '5') + BAE_ENABLE = ((BAE_ENABLE & 0xFFFFFF8F)| 0x00000050); // ACS_STATE = ACS_AUTO_CONTROL + else if(ACS_STATE == '6') + BAE_ENABLE = ((BAE_ENABLE & 0xFFFFFF8F)| 0x00000060); //ACS_STATE = ACS_DETUMBLING_CONTROL + +//...............eps......................// + if(EPS_INIT_STATUS == 's') + BAE_STATUS |= 0x00010000; //set EPS_INIT_STATUS flag + else if(EPS_INIT_STATUS == 'c') + BAE_STATUS &= 0xFFFEFFFF; //clear EPS_INIT_STATUS flag + if(EPS_BATTERY_GAUGE_STATUS == 'c') + BAE_STATUS &= 0xFFFDFFFF; //clear EPS_BATTERY_GAUGE_STATUS + else if(EPS_BATTERY_GAUGE_STATUS == 's') + BAE_STATUS |= 0x00020000; //set EPS_BATTERY_GAUGE_STATUS + + + pc.printf("\n\r BAE status %x BAE ENABLE %x ",BAE_STATUS,BAE_ENABLE); +} + +void FCTN_BAE_INIT() +{ + printf("\n\r Initialising BAE "); + FCTN_ACS_INIT(); + FCTN_EPS_INIT(); + //P_BCN_INIT(); + FLAG(); +} + +int main() +{ + pc.printf("\n\r BAE Activated. Testing Version 1.1 \n"); + + /* if (BCN_FEN == 0) //dummy implementation + { + pc.printf("\n\r RF silence "); + P_BCN_FEN(); + t_rfsilence.start();//Start the timer for RF_Silence + while(t_rfsilence.read() < RF_SILENCE_TIME); + } + */ + ACS_STATE = '4'; + //ACS_INIT_STATUS = 'c'; + //ACS_DATA_ACQ_STATUS = 'c'; + gpo1 = 0; + FLAG(); + FCTN_BAE_INIT(); + ACS_ATS_ENABLE = 'e'; + ACS_DATA_ACQ_ENABLE = 'e'; + + //...i2c.. + strcpy(telemetry,"This is telemetry THis is sample telemetry. ffffffffffffffffffffffffffffff end"); + slave.address(addr); + irpt_2_mstr = 0; + + ptr_t_i2c = new Thread(T_TC); + ptr_t_i2c->set_priority(osPriorityHigh); + ptr_t_acs = new Thread(T_ACS); + ptr_t_acs->set_priority(osPriorityAboveNormal); + ptr_t_eps = new Thread(T_EPS); + ptr_t_eps->set_priority(osPriorityAboveNormal); + ptr_t_bcn = new Thread(T_BCN); + ptr_t_bcn->set_priority(osPriorityAboveNormal); + + irpt_4m_mstr.enable_irq(); + irpt_4m_mstr.rise(&FCTN_I2C_ISR); + RtosTimer t_sc_timer(T_SC,osTimerPeriodic); // Initiating the scheduler thread + t_sc_timer.start(10000); + t_start.start(); + pc.printf("\n\rStarted scheduler %f\n\r",t_start.read()); + + gpo1 = 0; // att sens2 switch is enabled + //FCTN_BAE_INIT(); + while(1); //required to prevent main from terminating +} \ No newline at end of file