Team Fox
Updated EPS code with flowchart v2.3 (CDMS and HW faults)
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
QM_BAE_review_1_EPS_faults
by 
Mohamed Azad
Revision 5:bb592f3185cc, committed 2015-12-31
- Comitter:
- sakthipriya
- Date:
- Thu Dec 31 17:12:52 2015 +0000
- Parent:
- 4:39a4ae8c7ecd
- Child:
- 6:036d08b62785
- Commit message:
- final version of BAE
Changed in this revision
--- a/EPS.cpp Wed Dec 30 04:57:46 2015 +0000
+++ b/EPS.cpp Thu Dec 31 17:12:52 2015 +0000
@@ -3,7 +3,7 @@
/***********************************************global variable declaration***************************************************************/
extern uint32_t BAE_STATUS;
extern uint32_t BAE_ENABLE;
-
+extern uint8_t BAE_data[73];
//m_I2C.frequency(10000)
const char RCOMP0= 0x97;
@@ -12,6 +12,7 @@
BAE_HK_min_max bae_HK_minmax;
BAE_HK_arch arch_data;
+
//......................................Peripheral declarations.........................................................//
Serial pc_eps(USBTX,USBRX);
@@ -191,7 +192,7 @@
quant_data.BAE_temp_quant=quantiz(tstart,tstep,actual_data.BAE_temp_actual);
for(Iteration=0;Iteration<3;Iteration++){
- quant_data.AngularSpeed_quant[Iteration]=quantiz(AngularSpeed_start,AngularSpeed_step,actual_data.AngularSpeed_actual[Iteration]);
+ quant_data.AngularSpeed_quant[Iteration]=actual_data.AngularSpeed_actual[Iteration];
}
for(Iteration=0;Iteration<3;Iteration++){
@@ -206,12 +207,50 @@
arch_data.EPS_PCB_temp=quant_data.voltage_quant[14];
arch_data.Batt_SOC=quant_data.Batt_gauge_quant[1];
arch_data.power_mode=actual_data.power_mode;
+ arch_data.faultPoll_status=actual_data.faultPoll_status;
+ arch_data.faultIr_status=actual_data.faultIr_status;
arch_data.Batt_voltage=quant_data.Batt_voltage_quant;
}
-int quantiz(float start,float step,float x)
+void FCTN_APPEND_HKDATA()
+{
+ // quantized data
+ for (int i=0;i<16;i++)
+ BAE_data[i] = quant_data.voltage_quant[i];
+ for (int i=16;i<28;i++)
+ BAE_data[i] = quant_data.current_quant[i-16];
+ BAE_data[28] = quant_data.Batt_temp_quant[0];
+ BAE_data[29] = quant_data.Batt_temp_quant[1];
+ BAE_data[30] = quant_data.Batt_gauge_quant[1];
+ BAE_data[31] = quant_data.Batt_gauge_quant[1];
+ BAE_data[32] = quant_data.Batt_gauge_quant[1];
+ FCTN_CONVERT_FLOAT(quant_data.Batt_gauge_alerts,&BAE_data[33]);
+ BAE_data[37] = quant_data.BAE_temp_quant;
+ FCTN_CONVERT_FLOAT(quant_data.AngularSpeed_quant[0],&BAE_data[38]);
+ FCTN_CONVERT_FLOAT(quant_data.AngularSpeed_quant[1],&BAE_data[42]);
+ FCTN_CONVERT_FLOAT(quant_data.AngularSpeed_quant[2],&BAE_data[46]);
+ FCTN_CONVERT_FLOAT(quant_data.Bvalue_quant[0],&BAE_data[50]);
+ FCTN_CONVERT_FLOAT(quant_data.Bvalue_quant[1],&BAE_data[54]);
+ FCTN_CONVERT_FLOAT(quant_data.Bvalue_quant[2],&BAE_data[58]);
+ BAE_data[62] = quant_data.Batt_voltage_quant;
+ BAE_data[63] = (uint8_t)actual_data.power_mode;
+ BAE_data[64] = actual_data.faultPoll_status;
+ BAE_data[65] = actual_data.faultIr_status;
+ // archived data
+ BAE_data[66] = arch_data.Batt_1_temp; //verify if uint8_t is right
+ BAE_data[67] = arch_data.Batt_2_temp;
+ BAE_data[68] = arch_data.EPS_PCB_temp;
+ BAE_data[69] = arch_data.Batt_SOC;
+ BAE_data[70] = (uint8_t)arch_data.power_mode;
+ BAE_data[71] = arch_data.faultPoll_status;
+ BAE_data[72] = arch_data.faultIr_status;
+ BAE_data[73] = arch_data.Batt_voltage;
+
+}
+
+uint8_t quantiz(float start,float step,float x)
{
int y=(x-start)/step;
if(y<=0)y=0;
--- a/EPS.h Wed Dec 30 04:57:46 2015 +0000
+++ b/EPS.h Thu Dec 31 17:12:52 2015 +0000
@@ -42,12 +42,15 @@
//#define VBATT PIN20
#define BATT_HEAT PIN96
//#define BATT_HEAT_OUTPUT
+extern void FCTN_CONVERT_FLOAT(float input, uint8_t* output);
+
void FCTN_EPS_INIT();
void FCTN_EPS_POWERMODE(float soc) ;
void FCTN_HK_MAIN();
-int quantiz(float start,float step,float x);
+void FCTN_APPEND_HKDATA();
+uint8_t quantiz(float start,float step,float x);
void saveMin();
void saveMax();
@@ -90,32 +93,37 @@
float Batt_gauge_actual[4];
float BAE_temp_actual;
char power_mode;
- char fault_status;
+ uint8_t faultPoll_status;
+ uint8_t faultIr_status;
float AngularSpeed_actual[3];
float Bvalue_actual[3];
float Batt_voltage_actual;
+
}BAE_HK_actual;
typedef struct BAE_HK_quant{
- char voltage_quant[16]; //power_mode should be appended to sd card
- char current_quant[12];
- char Batt_temp_quant[2];
- char Batt_gauge_quant[3];
+ uint8_t voltage_quant[16]; //power_mode + faults should be appended to sd card
+ uint8_t current_quant[12];
+ uint8_t Batt_temp_quant[2];
+ uint8_t Batt_gauge_quant[3]; // why only 3 here??
float Batt_gauge_alerts;
- char BAE_temp_quant;
- char AngularSpeed_quant[3];
+ uint8_t BAE_temp_quant;
+ float AngularSpeed_quant[3];
float Bvalue_quant[3];
- char Batt_voltage_quant;
+ uint8_t Batt_voltage_quant;
}BAE_HK_quant;
-typedef struct BAE_HK_arch{
+typedef struct BAE_HK_arch{ //power_mode + faults should be appended to sd card
- char Batt_1_temp;
- char Batt_2_temp;
- char EPS_PCB_temp;
- char Batt_SOC;
+ uint8_t Batt_1_temp; //verify if uint8_t is right
+ uint8_t Batt_2_temp;
+ uint8_t EPS_PCB_temp;
+ uint8_t Batt_SOC;
char power_mode;
- char Batt_voltage;
+ uint8_t faultPoll_status;
+ uint8_t faultIr_status;
+ uint8_t Batt_voltage;
+
//char Batt_voltage2;
// char Digital power bus voltage;
@@ -123,23 +131,23 @@
}BAE_HK_arch;
typedef struct BAE_HK_min_max{
- char voltage_max[16];
- char current_max[12];
- char Batt_temp_max[2];
- char Batt_gauge_max[3];
- char BAE_temp_max;
- char AngularSpeed_max[3];
+ uint8_t voltage_max[16];
+ uint8_t current_max[12];
+ uint8_t Batt_temp_max[2];
+ uint8_t Batt_gauge_max[3];
+ uint8_t BAE_temp_max;
+ float AngularSpeed_max[3];
float Bvalue_max[3];
- char Batt_voltage_max;
+ uint8_t Batt_voltage_max;
- char voltage_min[16];
- char current_min[12];
- char Batt_temp_min[2];
- char Batt_gauge_min[3];
- char BAE_temp_min;
- char AngularSpeed_min[3];
+ uint8_t voltage_min[16];
+ uint8_t current_min[12];
+ uint8_t Batt_temp_min[2];
+ uint8_t Batt_gauge_min[3];
+ uint8_t BAE_temp_min;
+ float AngularSpeed_min[3];
float Bvalue_min[3];
- char Batt_voltage_min;
+ uint8_t Batt_voltage_min;
}BAE_HK_min_max;
--- a/TCTM.cpp Wed Dec 30 04:57:46 2015 +0000
+++ b/TCTM.cpp Thu Dec 31 17:12:52 2015 +0000
@@ -2,12 +2,25 @@
#include "TCTM.h"
#include "crc.h"
#include "EPS.h"
+#include "pin_config.h"
+#include "FreescaleIAP.h"
-#include "FreescaleIAP.h"
+extern DigitalOut gpo1; // enable of att sens2 switch
+extern DigitalOut gpo2; // enable of att sens switch
+
+extern DigitalOut TRXY_SW; //TR XY Switch
+extern DigitalOut TRZ_SW; //TR Z Switch
+extern DigitalOut CDMS_RESET; // CDMS RESET
+extern DigitalOut BCN_SW; //Beacon switch
extern BAE_HK_actual actual_data;
extern BAE_HK_min_max bae_HK_minmax;
extern uint32_t BAE_STATUS;
+extern float data[6];
+extern float moment[3];
+
+extern void FCTN_ATS_DATA_ACQ();
+extern void FCTN_ACS_CNTRLALGO(float*,float*);
uint8_t* FCTN_BAE_TM_TC (uint8_t* tc)
@@ -35,29 +48,124 @@
printf("Read from RAM\r\n");
switch(tc[3])
{
- case 0x00:
+ case 0x01:
+ {
+ printf("Read MUX DATA\r\n");
+ tm[0] = 0x60;
+ tm[1] = tc[0];
+ tm[2] = ACK_CODE;
+ for(int i=0; i<16; i++) //16*4 = 64 bytes //tm[4] to tm[67] filled
+ FCTN_CONVERT_FLOAT(actual_data.voltage_actual[i], &tm[4+(i*4)]);
+ for(int i=0; i<12; i++) //12*4 = 48 //tm[68] to tm[115] filled
+ FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&tm[68 + (i*4)]);
+ for (int i=116; i<132;i++)
+ {
+ tm[i] = 0x00;
+ }
+ crc16 = CRC::crc16_gen(tm,132);
+ tm[132] = (uint8_t)((crc16&0xFF00)>>8);
+ tm[133] = (uint8_t)(crc16&0x00FF);
+ return tm;
+ }
+ case 0x02:
{
printf("Read HK\r\n");
tm[0] = 0x60;
tm[1] = tc[0];
tm[2] = ACK_CODE;
- for (int i=4; i<132;i++)
+ FCTN_CONVERT_FLOAT(actual_data.Batt_temp_actual[0],&tm[4]); //tm[4]-tm[7]
+ FCTN_CONVERT_FLOAT(actual_data.Batt_temp_actual[1],&tm[8]); //tm[8]- tm[11]
+ for(int i=0; i<4; i++)
+ FCTN_CONVERT_FLOAT(actual_data.Batt_gauge_actual[i],&tm[12+(i*4)]); //tm[12] - tm[27]
+ FCTN_CONVERT_FLOAT(actual_data.BAE_temp_actual,&tm[28]); //tm[28] - tm[31]
+ tm[32] = (uint8_t)actual_data.power_mode;
+ tm[33] = actual_data.faultPoll_status;
+ tm[34] = actual_data.faultIr_status;
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[35+(i*4)]); //35 -46
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[47+(i*4)]); //47 -58
+ FCTN_CONVERT_FLOAT(actual_data.Batt_voltage_actual,&tm[59]); //59 - 62
+ for (int i=63; i<132;i++)
{
-
+ tm[i] = 0x00;
}
crc16 = CRC::crc16_gen(tm,132);
tm[132] = (uint8_t)((crc16&0xFF00)>>8);
tm[133] = (uint8_t)(crc16&0x00FF);
-
+ return tm;
- }
- case 0x01:
+ }
+ case 0x03:
{
printf("Read min max data");
+ tm[0] = 0x60;
+ tm[1] = tc[0];
+ tm[2] = ACK_CODE;
+ for(int i=4; i<20; i++)
+ tm[i] = (uint8_t)bae_HK_minmax.voltage_max[i-4];
+ for(int i=20; i<32; i++)
+ tm[i] = (uint8_t)bae_HK_minmax.current_max[i-20];
+ tm[32] = (uint8_t)bae_HK_minmax.Batt_temp_max[0];
+ tm[33] = (uint8_t)bae_HK_minmax.Batt_temp_max[1];
+ tm[34] = (uint8_t)bae_HK_minmax.Batt_gauge_max[0];
+ tm[35] = (uint8_t)bae_HK_minmax.Batt_gauge_max[1];
+ tm[36] = (uint8_t)bae_HK_minmax.Batt_gauge_max[2];
+ tm[37] = (uint8_t)bae_HK_minmax.BAE_temp_max;
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[0],&tm[38]); //tm[38] - tm[41]
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[1],&tm[42]); //tm[42] - tm[45]
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[2],&tm[46]); //tm[46] - tm[49]
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[0],&tm[50]); //tm[50] - tm[53]
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[1],&tm[54]); //tm[54] - tm[57]
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[2],&tm[58]); //tm[58] - tm[61]
+ tm[62] = (uint8_t)bae_HK_minmax.Bvalue_max[0];
+ tm[63] = (uint8_t)bae_HK_minmax.Bvalue_max[1];
+ tm[64] = (uint8_t)bae_HK_minmax.Bvalue_max[2];
+ tm[65] = (uint8_t)bae_HK_minmax.Batt_voltage_max;
+ for(int i=66; i<82; i++)
+ tm[i] = (uint8_t)bae_HK_minmax.voltage_min[i-66];
+ for(int i=82; i<94; i++)
+ tm[i] = (uint8_t)bae_HK_minmax.current_min[i-82];
+ tm[94] = (uint8_t)bae_HK_minmax.Batt_temp_min[0];
+ tm[95] = (uint8_t)bae_HK_minmax.Batt_temp_min[1];
+ tm[96] = (uint8_t)bae_HK_minmax.Batt_gauge_min[0];
+ tm[97] = (uint8_t)bae_HK_minmax.Batt_gauge_min[1];
+ tm[98] = (uint8_t)bae_HK_minmax.Batt_gauge_min[2];
+ tm[99] = (uint8_t)bae_HK_minmax.BAE_temp_min;
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[0],&tm[100]); //tm[100] - tm[103]
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[1],&tm[104]); //tm[104] - tm[107]
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[2],&tm[108]); //tm[108] - tm[111]
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[0],&tm[112]); //tm[112] - tm[115]
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[1],&tm[116]); //tm[116] - tm[119]
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[2],&tm[120]); //tm[120] - tm[123]
+ tm[124] = (uint8_t)bae_HK_minmax.Batt_voltage_min;
+ for (int i=125; i<132;i++)
+ {
+ tm[i] = 0x00;
+ }
+ crc16 = CRC::crc16_gen(tm,132);
+ tm[132] = (uint8_t)((crc16&0xFF00)>>8);
+ tm[133] = (uint8_t)(crc16&0x00FF);
+ return tm;
}
- case 0x02:
+ case 0x04:
{
printf("Read status");
+ tm[0] = 0x60;
+ tm[1] = tc[0];
+ tm[2] = ACK_CODE;
+ tm[4] = (BAE_STATUS>>24) & 0xFF;
+ tm[5] = (BAE_STATUS>>16) & 0xFF;
+ tm[6] = (BAE_STATUS>>8) & 0xFF;
+ tm[7] = BAE_STATUS & 0xFF;
+ for (int i=8; i<132;i++)
+ {
+ tm[i] = 0x00;
+ }
+ crc16 = CRC::crc16_gen(tm,132);
+ tm[132] = (uint8_t)((crc16&0xFF00)>>8);
+ tm[133] = (uint8_t)(crc16&0x00FF);
+ return tm;
}
}
@@ -103,28 +211,60 @@
uint8_t pid=tc[3];
switch(pid)
{
- case 0x01:
+ case 0xE0:
{
- printf("Run P_EPS_INIT\r\n");
+ printf("ACS_COMSN\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
- for(uint8_t i=3;i<11;i++)
+ FCTN_ATS_DATA_ACQ(); //get data
+ printf("gyro values\n\r");
+ for(int i=0; i<3; i++)
+ printf("%f\n\r",data[i]);
+ printf("mag values\n\r");
+ for(int i=3; i<6; i++)
+ printf("%f\n\r",data[i]);
+ FCTN_CONVERT_FLOAT(data[0],&tm[4]); //tm[4] - tm[7]
+ FCTN_CONVERT_FLOAT(data[1],&tm[8]); //tm[8] - tm[11]
+ FCTN_CONVERT_FLOAT(data[2],&tm[12]); //tm[12] - tm[15]
+ FCTN_CONVERT_FLOAT(data[0],&tm[16]); //tm[16] - tm[19]
+ FCTN_CONVERT_FLOAT(data[1],&tm[20]); //tm[20] - tm[23]
+ FCTN_CONVERT_FLOAT(data[2],&tm[24]); //tm[24] - tm[27]
+ if((data[0]<8) && (data[1]<8) && (data[2] <8))
+ tm[28] = 1; // gyro values in correct range
+ else
+ tm[28] = 0;
+ if ((data[3] > 20 ) && (data[4] >20) && (data[5]>20)&& (data[3] < 50 ) && (data[4] <50) && (data[5]<50))
+ tm[29] = 1; // mag values in correct range
+ else
+ tm[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],&tm[30]); //tm[30] - tm[33]
+ FCTN_CONVERT_FLOAT(moment[1],&tm[34]); //tm[34] - tm[37]
+ FCTN_CONVERT_FLOAT(moment[2],&tm[38]); //tm[38] - tm[41]
+ // to include commission TR as well
+ for(uint8_t i=42;i<132;i++)
{
tm[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
- for(uint8_t i=13;i<134;i++)
- {
- tm[i]=0x00;
- }
+
+ crc16 = CRC::crc16_gen(tm,132);
+ tm[133] = (uint8_t)((crc16&0xFF00)>>8);
+ tm[134] = (uint8_t)(crc16&0x00FF);
return tm;
}
- case 0x02:
+ /* case 0x02:
{
printf("Run P_EPS_MAIN\r\n");
//ACK_L234_TM
@@ -243,14 +383,15 @@
tm[i]=0x00;
}
return tm;
- }
+ }*/
case 0x11:
{
printf("SW_ON_ACS_ATS1_SW_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ gpo1 = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -270,7 +411,8 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ gpo2 = 0;
+ tm[2]=1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -290,7 +432,8 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ TRXY_SW = 1;
+ tm[2]=1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -310,7 +453,8 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ TRZ_SW = 1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -330,7 +474,8 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ BCN_SW = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -350,7 +495,8 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ gpo1 = 1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -370,7 +516,8 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ gpo2 = 1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -390,7 +537,8 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ TRXY_SW = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -410,7 +558,8 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ TRZ_SW = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -430,7 +579,8 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ BCN_SW = 1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -446,11 +596,14 @@
}
case 0x31:
{
- printf("ACS_ATS_SW_RESET\r\n");
+ printf("ACS_ATS1_SW_RESET\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ gpo1 = 1;
+ wait_us(1);
+ gpo1 = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -470,7 +623,10 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ BCN_SW = 1;
+ wait_us(1);
+ BCN_SW = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -486,11 +642,14 @@
}
case 0x33:
{
- printf("BAE_RESET\r\n");
+ printf("ACS_ATS2_SW_RESET\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ gpo1 = 1;
+ wait_us(1);
+ gpo1 = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -510,7 +669,10 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ tm[2]=1;
+ CDMS_RESET = 0;
+ wait_us(1);
+ CDMS_RESET = 1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -524,58 +686,8 @@
}
return tm;
}
- case 0xE0:
- {
- printf("CMSN_ACS_ALGO\r\n");
- //FMS_TM
- tm[0]=0xF0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
- for(uint8_t i=3;i<132;i++)
- {
- tm[i]=0x00;
- }
- crc16 = CRC::crc16_gen(tm,132);
- tm[132] = (uint8_t)((crc16&0xFF00)>>8);
- tm[133] = (uint8_t)(crc16&0x00FF);
- return tm;
- }
- case 0xE1:
- {
- printf("CMSN_ACS_HW\r\n");
- //FMS_TM
- tm[0]=0xF0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
- for(uint8_t i=3;i<132;i++)
- {
- tm[i]=0x00;
- }
- crc16 = CRC::crc16_gen(tm,132);
- tm[132] = (uint8_t)((crc16&0xFF00)>>8);
- tm[133] = (uint8_t)(crc16&0x00FF);
- return tm;
- }
- case 0xC1:
- {
- printf("Reset HK_Counter\r\n");
- //ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
- for(uint8_t i=3;i<11;i++)
- {
- tm[i]=0x00;
- }
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
- for(uint8_t i=13;i<134;i++)
- {
- tm[i]=0x00;
- }
- return tm;
- }
+
+
default:
{
printf("Invalid TC\r\n");
@@ -653,4 +765,16 @@
}
return flasharray[j];
}
-/*End*/
\ No newline at end of file
+/*End*/
+
+// Convert float to 4 uint8_t
+
+void FCTN_CONVERT_FLOAT(float input, uint8_t* output)
+{
+ uint32_t temp;
+ temp = (uint32_t)input;
+ output[0] = (temp>>24)&0xFF;
+ output[2] = (temp>>16)&0xFF;
+ output[1] = (temp>>8)&0xFF;
+ output[3] = temp & 0xFF; // verify the logic
+}
\ No newline at end of file
--- a/TCTM.h Wed Dec 30 04:57:46 2015 +0000 +++ b/TCTM.h Thu Dec 31 17:12:52 2015 +0000 @@ -2,4 +2,13 @@ uint8_t* FCTN_BAE_TM_TC(uint8_t*); void FCTN_CDMS_WR_FLASH(uint16_t ,uint32_t); -uint32_t FCTN_CDMS_RD_FLASH(uint16_t ); \ No newline at end of file +uint32_t FCTN_CDMS_RD_FLASH(uint16_t ); +void FCTN_CONVERT_FLOAT(float input, uint8_t* output); + +#define B0 -23 +#define B1 -2 +#define B2 5 +#define W0 -5.65 +#define W1 0.45 +#define W2 -1.97 +
--- a/main.cpp Wed Dec 30 04:57:46 2015 +0000
+++ b/main.cpp Thu Dec 31 17:12:52 2015 +0000
@@ -8,12 +8,14 @@
#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;
+//i2c//
+char data_send_flag = 'h';
+
//.........acs...............//
char ACS_INIT_STATUS = 'q';
char ACS_DATA_ACQ_STATUS = 'q';
@@ -37,7 +39,7 @@
char EPS_BATTERY_HEAT_ENABLE = 'q';
//.......................global variables..................................................................// new hk structure- everything has to changed based on this
-char BAE_data[bae_data_len];
+uint8_t BAE_data[73];
//*************************************Global declarations************************************************//
@@ -56,11 +58,15 @@
extern BAE_HK_min_max bae_HK_minmax;
extern BAE_HK_arch arch_data;
-bool write_ack = 1;
-bool read_ack = 1;
+int write_ack = 1;
+int read_ack = 1;
char telecommand[tc_len];
char* telemetry;
-char data_send_flag = 'h';
+
+bool pf1check = 0;
+bool pf2check = 0;
+bool if1check = 0;
+bool if2check = 1;
//*****************************************************Assigning pins******************************************************//
DigitalOut gpo1(PTC0); // enable of att sens2 switch
@@ -75,6 +81,30 @@
PwmOut PWM2(PIN94); //y
PwmOut PWM3(PIN95); //z //PWM output comes from pins p6
+//........faults
+//Polled Faults
+DigitalIn pf1(PIN5);//Attitude Sensor 1 OC bar fault signal
+DigitalIn pf2(PIN97);//Attitude Sensor 2 OC bar fault signal
+DigitalIn pf3(PIN83);//Fault Bar for TRXY driver
+
+//Interrupt based faults
+//InterruptIn ir1(PIN73);//Battery Gauge - Alert Bar Signal
+InterruptIn ir2(PIN72);//TRXY Driver TR switch Fault
+InterruptIn ir3(PIN89);//TRZ Driver Fault Bar
+InterruptIn ir4(PIN91);//TRZ Driver TR switch Fault
+InterruptIn ir5(PIN79);//CDMS - Switch Fault
+InterruptIn ir6(PIN80);//Beacon- Switch OC bar
+InterruptIn ir7(PIN42);//Charger IC - Fault Bar
+
+DigitalOut TRXY_SW(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);
+
+
+
/*****************************************************************Threads USed***********************************************************************************/
@@ -89,9 +119,67 @@
void F_BCN();
//*******************************************ACS THREAD**************************************************//
+uint8_t iterP1;
+uint8_t iterP2;
+uint8_t iterI1;
+uint8_t iterI2;
+
void F_ACS()
{
+ if(pf1check == 1)
+ {
+ if(iterP1 >= 3)
+ {
+ gpo1 = 1; // turn off ats1 permanently
+ //FCTN_SWITCH_ATS(0); // switch on ATS2
+ }
+ else
+ {
+ gpo1 = 0;
+ iterP1++;
+ }
+ }
+ if(pf2check == 1)
+ {
+ if(iterP1 >= 3)
+ {
+ gpo2 = 1; // turn off ats2 permanently
+ ACS_DATA_ACQ_ENABLE == 'd';
+ ACS_STATE == '2' ; // check ACS_STATE = ACS_ZAXIS_MOMENT_ONLY
+ }
+ else
+ {
+ gpo2 = 0;
+ iterP2++;
+ }
+ }
+ 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};
@@ -120,13 +208,12 @@
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
{
@@ -245,7 +332,7 @@
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) & (actual_data.Batt_temp_actual[1] < batt_heat_low)) // to confirm
+ if((actual_data.Batt_temp_actual[0] < batt_heat_low) && (actual_data.Batt_temp_actual[1] < batt_heat_low)) // to confirm
{
batt_heat = 1; //turn on battery heater
}
@@ -283,8 +370,8 @@
// EPS_STATUS = EPS_ERR_BATTERY_TEMP;
// }
FCTN_HK_MAIN();
- //FCTN_APPEND_HKDATA();
- //minMaxHkData();
+ FCTN_APPEND_HKDATA();
+ minMaxHkData();
EPS_MAIN_STATUS = 'c'; // clear EPS main status
@@ -313,18 +400,24 @@
slave.stop();
else if( slave.receive() == 1) // slave writes to master
{
- write_ack=slave.write((char*)telemetry,tm_len);
- irpt_2_mstr = 0;
+ if(data_send_flag == 'h')
+ write_ack=slave.write((char*)BAE_data,73);
+ else if(data_send_flag == 't')
+ {
+ write_ack=slave.write(telemetry,tm_len);
+ data_send_flag = 'h';
+ irpt_2_mstr = 0;
+ }
}
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");
+ 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);
uint8_t* temp = FCTN_BAE_TM_TC((uint8_t*) telecommand);
telemetry = (char*)temp;
- FCTN_TM();
+ FCTN_TM();
// for(int i = 0; i<134; i++)
//pc.printf("%c", telemetry[i]);
}
@@ -335,6 +428,7 @@
void FCTN_TM()
{
//irpt_2_mstr = 0;
+ data_send_flag = 't';
pc.printf("\n\r Telemetry Generation \n");
irpt_2_mstr = 1;
}
@@ -348,6 +442,125 @@
}
+//***********************************************************FAULTS***************************************************************//
+/*void ir1clear()
+{
+ actual_data.faultIr_status |= 0x01; // alert
+
+}*/
+
+
+
+void ir2clear()
+{
+ actual_data.faultIr_status |= 0x02;
+ TRXY_SW = 0; // Switch off TR XY
+ if1check = 1;
+}
+
+void ir3clear()
+{
+ actual_data.faultIr_status |= 0x04;
+ DRV_Z_SLP = 0;
+ wait_us(1);
+ DRV_Z_SLP = 1;
+
+}
+
+void ir4clear()
+{
+ if2check = 1;
+ actual_data.faultIr_status |= 0x08;
+ TRZ_SW = 0;
+}
+
+void ir5clear()
+{
+ actual_data.faultIr_status |= 0x10;
+ CDMS_RESET = 0;
+ wait_us(1);
+ CDMS_RESET = 1;
+}
+
+void ir6clear()
+{
+ actual_data.faultIr_status |= 0x20;
+ BCN_SW = 0;
+ wait_us(1);
+ BCN_SW = 1;
+}
+
+void ir7clear()
+{
+ actual_data.faultIr_status |= 0x40;
+}
+uint8_t iter2=0,iter4 = 0;
+
+
+
+void pollfault()
+{
+
+
+
+ if (pf1==0) // OC_ATS1
+ {
+ pf1check=1;
+ actual_data.faultPoll_status |=0x01 ;
+ gpo1 = 1; // turn off ats1 // to be turned on next cycle in ACS
+ }
+ else actual_data.faultPoll_status &= 0xFE;
+
+ if (pf2==0)
+ {
+ pf2check=1;
+ actual_data.faultPoll_status |=0x02 ;
+ gpo2 = 1; // turn off ats2 // turn on in ACS
+ }
+ else actual_data.faultPoll_status &= 0xFD;
+
+ if (pf3==0)
+ { actual_data.faultPoll_status |=0x04 ;
+ DRV_XY_SLP = 0;
+ wait_us(1);
+ DRV_XY_SLP = 1;
+ }
+ else actual_data.faultPoll_status &= 0xFB;
+
+
+
+
+ /*if (ir1==1)
+ {
+ actual_data.faultIr_status &=0xFE;
+ }*/
+ if (ir2==1)
+ {
+ actual_data.faultIr_status &=0xFD;
+ }
+ if (ir3==1)
+ {
+ actual_data.faultIr_status &=0xFB;
+ }
+ if (ir4==1)
+ {
+ actual_data.faultIr_status &=0xF7;
+ }
+ if (ir5==1)
+ {
+ actual_data.faultIr_status &=0xEF;
+ }
+ if (ir6==1)
+ {
+ actual_data.faultIr_status &=0xDF;
+ }if (ir7==1)
+ {
+ actual_data.faultIr_status &=0xBF;
+ }
+
+}
+
+
//------------------------------------------------------------------------------------------------------------------------------------------------
//SCHEDULER
//------------------------------------------------------------------------------------------------------------------------------------------------
@@ -436,15 +649,49 @@
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
-
+
+
+if (EPS_INIT_STATUS=='s') // Set EPS_INIT_STATUS
+ BAE_STATUS |= 0x00010000;
+else if(EPS_INIT_STATUS=='c') // Clear
+ BAE_STATUS &= 0xFFFEFFFF;
+
+
+if (EPS_MAIN_STATUS=='s') // Set EPS_MAIIN_STATUS
+ BAE_STATUS |= 0x00040000;
+else if(EPS_MAIN_STATUS=='c') // Clear
+ BAE_STATUS &= 0xFFFBFFFF;
+
+
+if (EPS_BATTERY_GAUGE_STATUS=='s') // Set EPS_BATTERY_GAUGE_STATUS
+ BAE_STATUS |= 0x00020000;
+else if(EPS_BATTERY_GAUGE_STATUS=='c') // Clear
+ BAE_STATUS &= 0xFFFDFFFF;
+
+
+if (EPS_BATTERY_TEMP_STATUS=='s') // Set EPS_BATTERY_TEMP_STATUS
+ BAE_STATUS |= 0x00080000;
+else if(EPS_BATTERY_TEMP_STATUS=='c') // Clear
+ BAE_STATUS &= 0xFFF7FFFF;
+
+if (EPS_STATUS==0)
+ BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF); // Set EPS_ERR_BATTERY_TEMP
+else if (EPS_STATUS==1)
+ BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF)|0x00010000; // Set EPS_BATTERY_HEATER_DISABLED
+else if (EPS_STATUS==2)
+ BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF)|0x00020000; // Set EPS_ERR_HEATER_SWITCH_OFF
+else if (EPS_STATUS==3)
+ BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF)|0x00030000; // Set EPS_ERR_HEATER_SWITCH_ON
+else if (EPS_STATUS==4)
+ BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF)|0x00040000; // Set EPS_BATTERY_HEATER_OFF
+else if (EPS_STATUS==5)
+ BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF)|0x00050000; // Set EPS_BATTERY_HEATER_ON
+
+
+ if(EPS_BATTERY_HEAT_ENABLE == 'e')
+ BAE_ENABLE |= 0x00000080;
+ else if(EPS_BATTERY_HEAT_ENABLE == 'd')
+ BAE_ENABLE = BAE_ENABLE &0xFFFFFF7;
@@ -495,9 +742,16 @@
ptr_t_i2c = new Thread(T_TC);
ptr_t_i2c->set_priority(osPriorityHigh);
-
+
irpt_4m_mstr.enable_irq();
irpt_4m_mstr.rise(&FCTN_I2C_ISR);
+ // ir1.fall(&ir1clear); //Battery Gauge - Alert Bar Signal
+ ir2.fall(&ir2clear); //TRXY Driver TR switch Fault
+ ir3.fall(&ir3clear); //TRZ Driver Fault Bar
+ ir4.fall(&ir4clear); //TRZ Driver TR switch Fault
+ ir5.fall(&ir5clear); //CDMS - Switch Fault
+ ir6.fall(&ir6clear); //Beacon- Switch OC bar
+ ir7.fall(&ir7clear); //Charger IC - Fault Bar
RtosTimer t_sc_timer(T_SC,osTimerPeriodic); // Initiating the scheduler thread
t_sc_timer.start(10000);
t_start.start();