Diff: main.cpp

Revision:

0:7b4c00e3912f

Child:

1:446a959e36ce

--- /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