ACS data acq changed completely. Tested and working. Deals all faults.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of QM_BAE_review_1 by
Diff: TCTM.cpp
- Revision:
- 5:bb592f3185cc
- Parent:
- 4:39a4ae8c7ecd
- Child:
- 6:036d08b62785
diff -r 39a4ae8c7ecd -r bb592f3185cc TCTM.cpp --- 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