Changes to be made for ATS_Fault logic and ACS_State
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of Japan_BAE_sensorworking_interrupr_reoccuring_copy by
Revision 15:3239c6391ffa, committed 2016-04-20
- Comitter:
- Bragadeesh153
- Date:
- Wed Apr 20 17:47:05 2016 +0000
- Parent:
- 14:a9588f443f1a
- Commit message:
- Upated commissioning.. Changes to be made in ACS_MAIN logic and checking the faults of ATS
Changed in this revision
TCTM.cpp | Show annotated file Show diff for this revision Revisions of this file |
TCTM.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/TCTM.cpp Thu Apr 14 09:34:06 2016 +0000 +++ b/TCTM.cpp Wed Apr 20 17:47:05 2016 +0000 @@ -19,8 +19,11 @@ extern DigitalOut TRZ_SW; //TR Z Switch extern DigitalOut CDMS_RESET; // CDMS RESET extern DigitalOut BCN_SW; //Beacon switch +extern uint8_t ACS_ATS_STATUS; extern uint8_t BCN_TX_STATUS; extern uint8_t BCN_FEN; +extern AnalogIn CurrentInput; + extern BAE_HK_actual actual_data; extern BAE_HK_min_max bae_HK_minmax; extern uint32_t BAE_STATUS; @@ -356,34 +359,14 @@ printf("ACS_COMSN\r\n"); //ACK_L234_telemetry - uint8_t B_x[2]; - uint8_t B_y[2]; - uint8_t B_z[2]; - uint8_t W_x[2]; - uint8_t W_y[2]; - uint8_t W_z[2]; - B_x[0]=tc[3]; - B_x[1]=tc[4]; - B_y[0]=tc[5]; - B_y[1]=tc[6]; - B_z[0]=tc[7]; - B_z[1]=tc[8]; - - W_x[0]=tc[9]; - W_x[1]=tc[10]; - W_y[0]=tc[11]; - W_y[1]=tc[12]; - W_z[0]=tc[13]; - W_z[1]=tc[14]; - - FCTN_CONVERT_UINT(B_x,&B[0]); - FCTN_CONVERT_UINT(B_y,&B[1]); - FCTN_CONVERT_UINT(B_z,&B[2]); - - FCTN_CONVERT_UINT (W_x, &W[0]); - FCTN_CONVERT_UINT (W_y, &W[1]); - FCTN_CONVERT_UINT (W_z, &W[2]); + B[0]=(float)tc[4]; + B[1]=(float)tc[5]; + B[2] = 300; //constant value + + W[0]=(float)tc[6]; + W[1]=(float)tc[7]; + W[2] = 300; //constant value telemetry[0]=0xB0; telemetry[1]=tc[0]; @@ -452,63 +435,188 @@ } case 0xE1: { - float moment_tc[3]; + printf("HARDWARE_COMSN\r\n"); //ACK_L234_telemetry - uint8_t M0[2]; - uint8_t M1[2]; - uint8_t M2[2]; - M0[0]=tc[3]; - M0[1]=tc[4]; - M1[0]=tc[5]; - M1[1]=tc[6]; - M2[0]=tc[7]; - M2[1]=tc[8]; - + + TRXY_SW = 1; + TRZ_SW = 1; + + PWM1 = 0; + PWM2 = 0; + PWM3 = 0; + + wait_ms(60); //Demagnetising time delay for torquerod telemetry[0]=0xB0; telemetry[1]=tc[0]; telemetry[2]=ACK_CODE; - float PWM_measured[3]; + float PWM_tc[3]; + + PWM_tc[0] = (float) tc[4]; + PWM_tc[1] = (float) tc[5]; + PWM_tc[2] = (float) tc[6]; + + ATS2_SW_ENABLE = 1; + ATS1_SW_ENABLE = 0; // making sure we switch off the other + + + FCTN_ATS_DATA_ACQ(); + + + FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0], &telemetry[6]); + FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1], &telemetry[10]); + FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2], &telemetry[14]); + + FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[18]); + FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[22]); + FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[26]); + + ACS_ATS_STATUS = ACS_ATS_STATUS & 0xF0; + ACS_ATS_STATUS = ACS_ATS_STATUS | 0x00; + + + ATS1_SW_ENABLE = 1; + ATS2_SW_ENABLE = 0; // making sure we switch off the other + + + FCTN_ATS_DATA_ACQ(); + + + FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0], &telemetry[30]); + FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1], &telemetry[34]); + FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2], &telemetry[38]); + + FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[42]); + FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[46]); + FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[50]); + + ACS_ATS_STATUS = ACS_ATS_STATUS & 0x0F; + ACS_ATS_STATUS = ACS_ATS_STATUS | 0x00; + + ACS_ATS_STATUS = telemetry[4]; + + PWM1 = PWM_tc[0]; + wait_ms(60); + FCTN_ATS_DATA_ACQ(); + + actual_data.current_actual[5]=CurrentInput.read(); + actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens); + int resistance; + + resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); + if(actual_data.current_actual[5]>1.47) + { + actual_data.current_actual[5]=3694/log(24.032242*resistance); + } + else{ + + actual_data.current_actual[5]=3365.4/log(7.60573*resistance); + } + + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.current_actual[5], &telemetry[54]); + + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[56]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[58]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[60]); + + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[62]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[64]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[66]); + + + + PWM1 = 0; - FCTN_CONVERT_UINT(M0,&moment_tc[0]); - moment_tc[1] = 0; - moment_tc[2] = 0; - FCTN_ACS_GENPWM_MAIN(moment_tc); - PWM_measured[0] = PWM1.read(); - FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (0*4)]); - - FCTN_CONVERT_UINT(M1, &moment_tc[1]); - moment_tc[0] = 0; - moment_tc[2] = 0; - FCTN_ACS_GENPWM_MAIN(moment_tc); - PWM_measured[1] = PWM2.read(); - FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (1*4)]); - - FCTN_CONVERT_UINT(M2, &moment_tc[2]); - moment_tc[0] = 0; - moment_tc[1] = 0; - FCTN_ACS_GENPWM_MAIN(moment_tc); - PWM_measured[2] = PWM3.read(); - FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (2*4)]); - - FCTN_CONVERT_FLOAT(PWM_measured[0],&telemetry[4]); //4-7 - FCTN_CONVERT_FLOAT(PWM_measured[1],&telemetry[8]); //8-11 - FCTN_CONVERT_FLOAT(PWM_measured[2],&telemetry[12]); //12-15 + PWM2 = PWM_tc[1]; + wait_ms(60); + FCTN_ATS_DATA_ACQ(); - // for(int i=0; i<12; i++) - // FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]); - - - // FCTN_ATS_DATA_ACQ(); //get data - - - // to include commission TR as well - for(uint8_t i=28;i<132;i++) + actual_data.current_actual[5]=CurrentInput.read(); + actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens); + + resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); + if(actual_data.current_actual[5]>1.47) + { + actual_data.current_actual[5]=3694/log(24.032242*resistance); + } + else{ + + actual_data.current_actual[5]=3365.4/log(7.60573*resistance); + } + + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.current_actual[5], &telemetry[68]); + + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[70]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[72]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[74]); + + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[76]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[78]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[80]); + + PWM2 = 0; + + PWM3 = PWM_tc[2]; + + wait_ms(60); + FCTN_ATS_DATA_ACQ(); + + resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); + if(actual_data.current_actual[5]>1.47) + { + actual_data.current_actual[5]=3694/log(24.032242*resistance); + } + else{ + + actual_data.current_actual[5]=3365.4/log(7.60573*resistance); + } + + + + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.current_actual[5], &telemetry[82]); + + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[84]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[86]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[88]); + + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[90]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[92]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[94]); + + PWM3 = 0; + + wait_ms(60); + + FCTN_ATS_DATA_ACQ(); + actual_data.current_actual[5]=CurrentInput.read(); + actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens); + + resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); + if(actual_data.current_actual[5]>1.47) + { + actual_data.current_actual[5]=3694/log(24.032242*resistance); + } + else{ + + actual_data.current_actual[5]=3365.4/log(7.60573*resistance); + } + + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[96]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[98]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[100]); + + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[102]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[104]); + FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[106]); + + + + for(uint8_t i=108;i<132;i++) { telemetry[i]=0x00; } @@ -671,7 +779,7 @@ telemetry[0]=0xB0; telemetry[1]=tc[0]; telemetry[2]=1; - ATS1_SW_ENABLE = 1; // making sure we switch off the other + ATS2_SW_ENABLE = 1; // making sure we switch off the other ATS1_SW_ENABLE = 0; for(uint8_t i=3;i<11;i++) { @@ -692,6 +800,7 @@ //ACK_L234_TM telemetry[0]=0xB0; telemetry[1]=tc[0]; + ATS1_SW_ENABLE = 1; //make sure u switch off the other ATS2_SW_ENABLE = 0; telemetry[2]=1; @@ -1086,4 +1195,22 @@ output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic //printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]); //std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl; +} + + +void FCTN_CONVERT_FLOAT_COMPRESS(float input, uint8_t output[2]) +{ + int integer = (int)input; + assert(sizeof(int) == sizeof(uint16_t)); + uint16_t* temp = reinterpret_cast<uint16_t*>(&integer); + + //float* output1 = reinterpret_cast<float*>(temp); + + printf("\n\r %d ", integer); + std::cout << "\n\r uint16"<<*temp << std::endl; + + output[0] =(uint8_t ) (((*temp)>>8)&0xFF); + output[1] =(uint8_t ) ((*temp) & 0xFF); // verify the logic + //printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]); + //std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl; } \ No newline at end of file
--- a/TCTM.h Thu Apr 14 09:34:06 2016 +0000 +++ b/TCTM.h Wed Apr 20 17:47:05 2016 +0000 @@ -1,9 +1,12 @@ #define ACK_CODE 0x02; +#define rsens 0.095 + void FCTN_BAE_TM_TC(uint8_t*); void FCTN_CDMS_WR_FLASH(uint16_t ,uint32_t); uint32_t FCTN_CDMS_RD_FLASH(uint16_t ); void FCTN_CONVERT_FLOAT(float input, uint8_t* output); +void FCTN_CONVERT_FLOAT_COMPRESS(float input, uint8_t* output); #define B0 -23 #define B1 -2