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:
- 11:1fdb94ae6563
- Parent:
- 10:f93407b97750
- Child:
- 12:af1d7e18b868
diff -r f93407b97750 -r 1fdb94ae6563 TCTM.cpp --- a/TCTM.cpp Mon Apr 11 17:26:46 2016 +0000 +++ b/TCTM.cpp Wed Apr 13 17:00:25 2016 +0000 @@ -10,12 +10,12 @@ #include "cassert" #include"math.h" - +/*define the pins for digital out*/ extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch -extern DigitalOut TRXY_SW; //TR XY Switch +extern DigitalOut TRXY_SW_EN; //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN extern DigitalOut TRZ_SW; //TR Z Switch extern DigitalOut CDMS_RESET; // CDMS RESET extern DigitalOut BCN_SW; //Beacon switch @@ -31,12 +31,40 @@ extern DigitalOut phase_TR_x; extern DigitalOut phase_TR_y; extern DigitalOut phase_TR_z; -extern BAE_HK_actual actual_data.power_mode; extern BAE_HK_quant quant_data; +//extern DigitalOut TRXY_SW_EN; +//extern DigitalOut TRZ_SW_EN; //same as TRZ_SW +extern uint32_t BAE_ENABLE; +//extern DigitalOut ACS_ACQ_DATA_ENABLE; + +/*given a default value as of now shuld read value from flash and increment it write it back very time it starts(bae)*/ +extern uint8_t BAE_RESET_COUNTER=0; + +extern uint8_t BCN_FAIL_COUNT; + +extern PwmOut PWM1; //x //Functions used to generate PWM signal +extern PwmOut PWM2; //y +extern PwmOut PWM3; //z //PWM output comes from pins p6 + +extern void F_ACS(); +extern void F_BCN(); +//extern void FCTN_ACS_GENPWM_MAIN(); +extern void F_EPS(); +extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]); + extern void FCTN_ATS_DATA_ACQ(); extern void FCTN_ACS_CNTRLALGO(float*,float*); + +void FCTN_CONVERT_UINT (uint8_t input[2], float* output) +{ + + *output = (float) input[1]; + *output = *output/100.0; //input[0] integer part + *output = *output + (float) input[0]; //input[1] decimal part correct to two decimal places +} + float angle(float x,float y,float z) { float mag_total=sqrt(x*x + y*y + z*z); @@ -87,7 +115,7 @@ how to get that we dont know, now we just assume it to be so*/ tm[3] = (BCN_SW); tm[3] = tm[3]|(TRXY_SW_EN<<1); - tm[3] = tm[3]|(TRZ_SW_EN<<2); + tm[3] = tm[3]|(TRZ_SW<<2); tm[3] = tm[3]|(ATS1_SW_ENABLE<<3); tm[3] = tm[3]|(ATS2_SW_ENABLE<<4); @@ -97,7 +125,9 @@ tm[3] = tm[3] & 0xDF; tm[3] = tm[3]|(BCN_FEN<<6); - tm[3] = tm[3]|(ACS_ACQ_DATA_ENABLE<<7); + uint8_t mask_val=BAE_ENABLE & 0x00000008; + /*can be a problem see if any error occurs*/ + tm[3] = tm[3]|(mask_val<<7); /*not included in the code yet*/ tm[4] = BAE_RESET_COUNTER; @@ -108,7 +138,7 @@ tm[5] = tm[5]|(phase_TR_y<<5); tm[5] = tm[5]|(phase_TR_z<<6); /*spare to be fixed*/ - tm[5] = tm[5]|(Spare))<<7); + //tm[5] = tm[5]|(Spare))<<7); /**/ uint8_t soc_powerlevel_2=50; uint8_t soc_powerlevel_3=65; @@ -139,7 +169,7 @@ uint16_t P_BCN_TX_MAIN_COUNTER=0; uint16_t P_EPS_MAIN_COUNTER=0; - tm[18] = P_BAE_I2CRX_COUNTER>>8 + tm[18] = P_BAE_I2CRX_COUNTER>>8; tm[19] = P_BAE_I2CRX_COUNTER; tm[20] = P_ACS_MAIN_COUNTER>>8; tm[21] = P_ACS_MAIN_COUNTER; @@ -158,12 +188,25 @@ tm[51] = actual_data.faultPoll_status; //Bdot Rotation Speed of Command tm[52-53] //Bdot Output Current tm[54] - float l_pmw1 = PWM1; - float l_pmw2 = PWM2; - float l_pmw3 = PWM3; - FCTN_CONVERT_FLOAT(l_pwm1, &tm[55]); - FCTN_CONVERT_FLOAT(l_pwm2, &tm[59]); - FCTN_CONVERT_FLOAT(l_pwm3, &tm[63]); + //float l_pmw1 = (PWM1); + //float l_pmw2 = PWM2; + //float l_pmw3 = PWM3; + + /*__________________________________________________________________*/ + + /*change and check if changing it to PWM1 causes problem*/ + + /*___________________________________________________________________*/ + + float PWM_measured[3]; + + PWM_measured[0] = PWM1.read(); + PWM_measured[1] = PWM2.read(); + PWM_measured[2] = PWM3.read(); + + FCTN_CONVERT_FLOAT(PWM_measured[0], &tm[55]); + FCTN_CONVERT_FLOAT(PWM_measured[1], &tm[59]); + FCTN_CONVERT_FLOAT(PWM_measured[2], &tm[63]); float attitude_ang = angle(actual_data.Bvalue_actual[0],actual_data.Bvalue_actual[1],actual_data.Bvalue_actual[2]); FCTN_CONVERT_FLOAT(attitude_ang, &tm[67]); @@ -200,54 +243,55 @@ tm[2] = ACK_CODE; for(int i;i<16;i++) - tm[i+3] = BAE_HK_min_max bae_HK_minmax.voltage_max[i]; + tm[i+3] = bae_HK_minmax.voltage_max[i]; for(int i;i<12;i++) - tm[i+18] = BAE_HK_min_max bae_HK_minmax.current_max[i]; + tm[i+18] = bae_HK_minmax.current_max[i]; - tm[29] = BAE_HK_min_max bae_HK_minmax.Batt_voltage_max;; - tm[30] = BAE_HK_min_max bae_HK_minmax.BAE_temp_max; + tm[29] = bae_HK_minmax.Batt_voltage_max;; + tm[30] = bae_HK_minmax.BAE_temp_max; /*battery soc*/ //tm[31] = BAE_HK_min_max bae_HK_minmax.voltage_max; - tm[32] = BAE_HK_min_max bae_HK_minmax.Batt_temp_max[1]; - tm[33] = BAE_HK_min_max bae_HK_minmax.Batt_temp_max[2]; + tm[32] = bae_HK_minmax.Batt_temp_max[1]; + tm[33] = bae_HK_minmax.Batt_temp_max[2]; /*BCN temp not there*/ //tm[34] = BAE_HK_min_max bae_HK_minmax.; for(int i=0; i<3; i++) - FCTN_CONVERT_FLOAT(BAE_HK_min_max bae_HK_minmax.Bvalue_max[i],&tm[35+(i*4)]); + FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[i],&tm[35+(i*4)]); for(int i=0; i<3; i++) - FCTN_CONVERT_FLOAT(BAE_HK_min_max bae_HK_minmax.AngularSpeed_max[i],&tm[47+(i*4)]); + FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[i],&tm[47+(i*4)]); /*min data*/ for(int i;i<16;i++) - tm[i+59] = BAE_HK_min_max bae_HK_minmax.voltage_min[i]; + tm[i+59] = bae_HK_minmax.voltage_min[i]; for(int i;i<12;i++) - tm[i+74] = BAE_HK_min_max bae_HK_minmax.current_min[i]; + tm[i+74] = bae_HK_minmax.current_min[i]; - tm[86] = BAE_HK_min_max bae_HK_minmax.Batt_voltage_min; - tm[87] = BAE_HK_min_max bae_HK_minmax.BAE_temp_min; + tm[86] = bae_HK_minmax.Batt_voltage_min; + tm[87] = bae_HK_minmax.BAE_temp_min; /*battery soc*/ //tm[88] = BAE_HK_min_max bae_HK_minmax.voltage_max; - tm[89] = BAE_HK_min_max bae_HK_minmax.Batt_temp_min[1]; - tm[90] = BAE_HK_min_max bae_HK_minmax.Batt_temp_min[2]; + tm[89] = bae_HK_minmax.Batt_temp_min[1]; + tm[90] = bae_HK_minmax.Batt_temp_min[2]; + //huhu// /*BCN temp not there*/ //tm[91] = BAE_HK_min_max bae_HK_minmax.; for(int i=0; i<3; i++) - FCTN_CONVERT_FLOAT(BAE_HK_min_max bae_HK_minmax.Bvalue_min[i],&tm[91+(i*4)]); + FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[i],&tm[91+(i*4)]); for(int i=0; i<3; i++) - FCTN_CONVERT_FLOAT(BAE_HK_min_max bae_HK_minmax.AngularSpeed_min[i],&tm[103+(i*4)]); + FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[i],&tm[103+(i*4)]); for (int i=115; i<132;i++) @@ -459,9 +503,9 @@ 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(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]); @@ -556,9 +600,9 @@ tm[0]=0xB0; tm[1]=tc[0]; tm[2]=ACK_CODE; - FCTN_CONVERT_UINT (M0, &moment_tc[0]); - FCTN_CONVERT_UINT (M1, &moment_tc[1]); - FCTN_CONVERT_UINT (M2, &moment_tc[2]); + FCTN_CONVERT_UINT(M0,&moment_tc[0]); + FCTN_CONVERT_UINT(M1, &moment_tc[1]); + FCTN_CONVERT_UINT(M2, &moment_tc[2]); @@ -678,9 +722,11 @@ tm[24] = tm[5]|(phase_TR_y<<5); tm[24] = tm[5]|(phase_TR_z<<6); - FCTN_CONVERT_FLOAT(l_pwm_1,&tm[25]); - FCTN_CONVERT_FLOAT(l_pwm_2,&tm[29]); - FCTN_CONVERT_FLOAT(l_pwm_3,&tm[33]); + /*___________________change / check pwm working__________________________________*/ + + FCTN_CONVERT_FLOAT(PWM1,&tm[25]); + FCTN_CONVERT_FLOAT(PWM2,&tm[29]); + FCTN_CONVERT_FLOAT(PWM3,&tm[33]); //ACK_L234_TM /* tm[0]=0xB0; @@ -796,7 +842,7 @@ //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; - TRXY_SW = 1; + TRXY_SW_EN = 1; tm[2]=1; for(uint8_t i=3;i<11;i++) { @@ -902,7 +948,7 @@ tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; - TRXY_SW = 0; + TRXY_SW_EN= 0; for(uint8_t i=3;i<11;i++) { tm[i]=0x00; @@ -1149,4 +1195,4 @@ 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; -} \ No newline at end of file +}