![](/media/cache/group/nine_tails_kurama_by_zubair43-d4zyf5s.jpg.50x50_q85.jpg)
ACS data acq changed completely. Tested and working. Deals all faults.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of QM_BAE_review_1 by
TCTM.cpp
- Committer:
- Bragadeesh153
- Date:
- 2016-04-13
- Revision:
- 12:af1d7e18b868
- Parent:
- 11:1fdb94ae6563
- Child:
- 13:fb7facaf308b
File content as of revision 12:af1d7e18b868:
#include "mbed.h" #include "TCTM.h" #include "crc.h" #include "EPS.h" #include "pin_config.h" #include "FreescaleIAP.h" #include "inttypes.h" #include "iostream" #include "stdint.h" #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_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 extern uint8_t BCN_TX_STATUS; extern uint8_t BCN_FEN; 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 uint8_t ACS_STATE; extern DigitalOut EN_BTRY_HT; extern DigitalOut phase_TR_x; extern DigitalOut phase_TR_y; extern DigitalOut phase_TR_z; 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); float cos_z = z/mag_total; float theta_z = acosf(cos_z); return theta_z; //printf("/n cos_zz= %f /t theta_z= %f /n",cos_z,theta_z); } uint8_t* FCTN_BAE_TM_TC (uint8_t* tc) { uint8_t service_type=(tc[2]&0xF0); uint8_t* tm; uint16_t crc16; switch(service_type) { case 0x60: { printf("Memory Management Service\r\n"); uint8_t service_subtype=(tc[2]&0x0F); switch(service_subtype) { case 0x01: { printf("Read from Flash\r\n"); } case 0x02: { uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4]; switch(MID) { case 0x0001: { printf("Read from RAM\r\n"); /*taking some varible till we find some thing more useful*/ //uint8_t ref_val=0x01; tm[0] = 0x60; tm[1] = tc[0]; tm[2] = ACK_CODE; /*random or with bcn_tx_sw_enable assuming it is 1 bit in length 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<<2); tm[3] = tm[3]|(ATS1_SW_ENABLE<<3); tm[3] = tm[3]|(ATS2_SW_ENABLE<<4); if(BCN_TX_STATUS==2) tm[3] = tm[3]|0x20; else tm[3] = tm[3] & 0xDF; tm[3] = tm[3]|(BCN_FEN<<6); 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; tm[5] = ACS_STATE; tm[5] = tm[5]|(EN_BTRY_HT<<3); tm[5] = tm[5]|(phase_TR_x<<4); 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); /**/ uint8_t soc_powerlevel_2=50; uint8_t soc_powerlevel_3=65; tm[6] = soc_powerlevel_2; tm[7] = soc_powerlevel_3; /*to be fixed*/ tm[8] = 0; tm[9] = 0; tm[10] = 0; tm[11] = 0; //tm[8] = Torque Rod X Offset; //tm[9] = Torque Rod Y Offset; //tm[10] = Torque Rod Z Offset; //tm[11] = ACS_DEMAG_TIME_DELAY; tm[12] = (BAE_STATUS>>24) & 0xFF; tm[13] = (BAE_STATUS>>16) & 0xFF; tm[14] = (BAE_STATUS>>8) & 0xFF; tm[15] = BAE_STATUS & 0xFF; /*to be fixed*/ tm[16] = BCN_FAIL_COUNT; tm[17] = actual_data.power_mode; /*to be fixed*/ uint16_t P_BAE_I2CRX_COUNTER=0; uint16_t P_ACS_MAIN_COUNTER=0; uint16_t FCTN_BCN_TX_MAIN_COUNTER=0; uint16_t P_EPS_MAIN_COUNTER=0; 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; tm[22] = FCTN_BCN_TX_MAIN_COUNTER>>8; tm[23] = FCTN_BCN_TX_MAIN_COUNTER; tm[24] = P_EPS_MAIN_COUNTER>>8; tm[25] = P_EPS_MAIN_COUNTER; for(int i=0; i<3; i++) FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[26+ (i*4)]); for(int i=0; i<3; i++) FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[38+(i*4)]); //FAULT_FLAG(); tm[50] = actual_data.faultIr_status; 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; /*__________________________________________________________________*/ /*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]); for (int i=0; i<16; i++) tm[68+i] = quant_data.voltage_quant[i]; for (int i=0; i<12; i++) tm[84+i] = quant_data.current_quant[i]; //tm[96] //tm[97] //tm[98] //tm[99] tm[100] = quant_data.Batt_voltage_quant; tm[101] = quant_data.BAE_temp_quant; tm[102] = quant_data.Batt_gauge_quant[1]; tm[103] = quant_data.Batt_temp_quant[0]; tm[104] = quant_data.Batt_temp_quant[1]; //tm[105] = beacon temperature; for (int i=105; 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 0x0002: { tm[0] = 0x60; tm[1] = tc[0]; tm[2] = ACK_CODE; for(int i;i<16;i++) tm[i+3] = bae_HK_minmax.voltage_max[i]; for(int i;i<12;i++) tm[i+18] = bae_HK_minmax.current_max[i]; 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_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_minmax.Bvalue_max[i],&tm[35+(i*4)]); for(int i=0; i<3; i++) 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_minmax.voltage_min[i]; for(int i;i<12;i++) tm[i+74] = bae_HK_minmax.current_min[i]; 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_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_minmax.Bvalue_min[i],&tm[91+(i*4)]); for(int i=0; i<3; i++) FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[i],&tm[103+(i*4)]); for (int i=115; 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; } } } /* switch(tc[3]) { 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; 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 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 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; } } */ case 0x05: { printf("Write on Flash\r\n"); } default: { printf("Invalid TC"); //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; } } } case 0x80: { printf("Function Management Service\r\n"); uint8_t service_subtype=(tc[2]&0x0F); switch(service_subtype) { case 0x01: { printf("FMS Activated\r\n"); uint8_t pid=tc[3]; switch(pid) { case 0xE0: { float B[3],W[3]; printf("ACS_COMSN\r\n"); //ACK_L234_TM 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]); tm[0]=0xB0; tm[1]=tc[0]; tm[2]=ACK_CODE; //FCTN_ATS_DATA_ACQ(); //get data printf("gyro values\n\r"); for(int i=0; i<3; i++) printf("%f\n\r",W[i]); printf("mag values\n\r"); for(int i=0; i<3; i++) printf("%f\n\r",B[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,132); tm[133] = (uint8_t)((crc16&0xFF00)>>8); tm[134] = (uint8_t)(crc16&0x00FF); return tm; */ // Control algo commissioning FCTN_ACS_CNTRLALGO(B,W); FCTN_CONVERT_FLOAT(moment[0],&tm[4]); //tm[4] - tm[7] FCTN_CONVERT_FLOAT(moment[1],&tm[8]); //tm[8] - tm[11] FCTN_CONVERT_FLOAT(moment[2],&tm[12]); //tm[12] - tm[15] // to include commission TR as well for(uint8_t i=16;i<132;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 0xE1: { float moment_tc[3]; printf("HARDWARE_COMSN\r\n"); //ACK_L234_TM 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]; 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_ACS_GENPWM_MAIN(moment_tc); 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[4]); //4-7 FCTN_CONVERT_FLOAT(PWM_measured[1],&tm[8]); //8-11 FCTN_CONVERT_FLOAT(PWM_measured[2],&tm[12]); //12-15 for(int i=0; i<12; i++) FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&tm[16 + (i*4)]); FCTN_ATS_DATA_ACQ(); //get data // to include commission TR as well for(uint8_t i=64;i<132;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: { F_EPS(); /* printf("Run P_EPS_MAIN\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=ACK_CODE; */ for(uint8_t i=0;i<133;i++) { tm[i]=0x00; } crc16 = CRC::crc16_gen(tm,132); tm[132] = (uint8_t)((crc16&0xFF00)>>8); tm[133] = (uint8_t)(crc16&0x00FF); /* for(uint8_t i=13;i<134;i++) { tm[i]=0x00; } */ return tm; } /* case 0x03: { /* printf("Run P_ACS_INIT\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; } case 0x04: { printf("Run P_ACS_ACQ_DATA\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; } */ case 0x05: { printf("Run P_ACS_MAIN\r\n"); F_ACS(); for(int i=0; i<3; i++) FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[(i*4)]); for(int i=0; i<3; i++) FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[12+(i*4)]); tm[24] = ACS_STATE; tm[24] = tm[5]|(EN_BTRY_HT<<3); tm[24] = tm[5]|(phase_TR_x<<4); tm[24] = tm[5]|(phase_TR_y<<5); tm[24] = tm[5]|(phase_TR_z<<6); /*___________________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; tm[1]=tc[0]; tm[2]=ACK_CODE; for(uint8_t i=3;i<11;i++) { tm[i]=0x00; } */ crc16 = CRC::crc16_gen(tm,37); tm[37] = (uint8_t)((crc16&0xFF00)>>8); tm[38] = (uint8_t)(crc16&0x00FF); for(uint8_t i=39;i<134;i++) { tm[i]=0x00; } return tm; } case 0x06: { F_BCN(); /* printf("Run FCTN_BCN_INIT\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,0); tm[0] = (uint8_t)((crc16&0xFF00)>>8); tm[1] = (uint8_t)(crc16&0x00FF); for(uint8_t i=2;i<134;i++) { tm[i]=0x00; } return tm; } /* case 0x07: { printf("Run FCTN_BCN_TX_MAIN\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; }*/ case 0x11: { printf("SW_ON_ACS_ATS1_SW_ENABLE\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; ATS1_SW_ENABLE = 1; // making sure we switch off the other ATS1_SW_ENABLE = 0; 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; } case 0x12: { printf("SW_ON_ACS_ATS2_SW_ENABLE\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; ATS1_SW_ENABLE = 1; //make sure u switch off the other ATS2_SW_ENABLE = 0; tm[2]=1; 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; } case 0x13: { printf("SW_ON_ACS_TR_XY_ENABLE\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; TRXY_SW_EN = 1; tm[2]=1; 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; } case 0x14: { printf("SW_ON_ACS_TR_Z_ENABLE\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; TRZ_SW = 1; 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; } case 0x15: { printf("SW_ON_BCN_TX_SW_ENABLE\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; BCN_SW = 0; 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; } case 0x21: { printf("SW_OFF_ACS_ATS1_SW_ENABLE\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; ATS1_SW_ENABLE = 1; 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; } case 0x22: { printf("SW_OFF_ACS_ATS2_SW_ENABLE\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; ATS2_SW_ENABLE = 1; 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; } case 0x23: { printf("SW_OFF_ACS_TR_XY_ENABLE\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; TRXY_SW_EN= 0; 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; } case 0x24: { printf("SW_OFF_ACS_TR_Z_ENABLE\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; TRZ_SW = 0; 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; } case 0x25: { printf("SW_OFF_BCN_TX_SW_ENABLE\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; BCN_SW = 1; 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; } case 0x31: { printf("ACS_ATS1_SW_RESET\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; ATS1_SW_ENABLE = 1; wait_us(1); ATS1_SW_ENABLE = 0; 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; } case 0x32: { printf("BCN_SW_RESET\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; BCN_SW = 1; wait_us(1); BCN_SW = 0; 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; } case 0x33: { printf("ACS_ATS2_SW_RESET\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; ATS1_SW_ENABLE = 1; wait_us(1); ATS1_SW_ENABLE = 0; 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; } case 0x34: { printf("CDMS_SW_RESET\r\n"); //ACK_L234_TM tm[0]=0xB0; tm[1]=tc[0]; tm[2]=1; CDMS_RESET = 0; wait_us(1); CDMS_RESET = 1; 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"); //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"); //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; } } } } } } int strt_add = flash_size() - (2*SECTOR_SIZE); uint32_t flasharray[8]; //256+(3*1024) char *nativeflash = (char*)strt_add; /*Writing to the Flash*/ void FCTN_CDMS_WR_FLASH(uint16_t j,uint32_t fdata) //j-position to write address ; fdata - flash data to be written { for(int i=0;i<8;i++) { flasharray[i]=nativeflash[i]; } flasharray[j]=fdata; erase_sector(strt_add); program_flash(strt_add, (char*)&flasharray,4*8); } /*End*/ /*Reading from Flash*/ uint32_t FCTN_CDMS_RD_FLASH(uint16_t j) { for(int i=0;i<8;i++) { flasharray[i]=nativeflash[i]; } return flasharray[j]; } /*End*/ // Convert float to 4 uint8_t void FCTN_CONVERT_FLOAT(float input, uint8_t output[4]) { assert(sizeof(float) == sizeof(uint32_t)); uint32_t* temp = reinterpret_cast<uint32_t*>(&input); //float* output1 = reinterpret_cast<float*>(temp); printf("\n\r %f ", input); std::cout << "\n\r uint32"<<*temp << std::endl; output[0] =(uint8_t )(((*temp)>>24)&0xFF); output[2] =(uint8_t ) (((*temp)>>16)&0xFF); output[1] =(uint8_t ) (((*temp)>>8)&0xFF); 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; }