Team Fox
/
ACS_Flowchart_BAE
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
QM_BAE_review_1
by 
Team Fox
Embed:
(wiki syntax)
Show/hide line numbers
TCTM.cpp
00001 #include "mbed.h" 00002 #include "TCTM.h" 00003 #include "crc.h" 00004 #include "EPS.h" 00005 #include "pin_config.h" 00006 #include "FreescaleIAP.h" 00007 #include "inttypes.h" 00008 #include "iostream" 00009 #include "stdint.h" 00010 #include "cassert" 00011 #include"math.h" 00012 00013 /*define the pins for digital out*/ 00014 00015 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch 00016 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch 00017 00018 extern DigitalOut TRXY_SW; //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN 00019 extern DigitalOut TRZ_SW; //TR Z Switch 00020 extern DigitalOut CDMS_RESET; // CDMS RESET 00021 extern DigitalOut BCN_SW; //Beacon switch 00022 extern uint8_t BCN_TX_STATUS; 00023 extern uint8_t BCN_FEN; 00024 extern BAE_HK_actual actual_data; 00025 extern BAE_HK_min_max bae_HK_minmax; 00026 extern uint32_t BAE_STATUS; 00027 extern float data[6]; 00028 extern float moment[3]; 00029 extern uint8_t ACS_STATE; 00030 extern DigitalOut EN_BTRY_HT; 00031 extern DigitalOut phase_TR_x; 00032 extern DigitalOut phase_TR_y; 00033 extern DigitalOut phase_TR_z; 00034 extern BAE_HK_quant quant_data; 00035 //extern DigitalOut TRXY_SW; 00036 //extern DigitalOut TRZ_SW_EN; //same as TRZ_SW 00037 extern uint32_t BAE_ENABLE; 00038 //extern DigitalOut ACS_ACQ_DATA_ENABLE; 00039 00040 /*given a default value as of now shuld read value from flash and increment it write it back very time it starts(bae)*/ 00041 extern uint8_t BAE_RESET_COUNTER=0; 00042 00043 //extern uint8_t BCN_FAIL_COUNT; 00044 00045 extern PwmOut PWM1; //x //Functions used to generate PWM signal 00046 extern PwmOut PWM2; //y 00047 extern PwmOut PWM3; //z //PWM output comes from pins p6 00048 00049 extern void F_ACS(); 00050 extern void F_BCN(); 00051 //extern void FCTN_ACS_GENPWM_MAIN(); 00052 extern void F_EPS(); 00053 extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]); 00054 extern int FCTN_ACS_INIT(); 00055 00056 00057 extern int FCTN_ATS_DATA_ACQ(); 00058 extern void FCTN_ACS_CNTRLALGO(float*,float*); 00059 uint8_t telemetry[135]; 00060 00061 void FCTN_CONVERT_UINT (uint8_t input[2], float* output) 00062 { 00063 00064 *output = (float) input[1]; 00065 *output = *output/100.0; //input[0] integer part 00066 *output = *output + (float) input[0]; //input[1] decimal part correct to two decimal places 00067 } 00068 00069 float angle(float x,float y,float z) 00070 { 00071 float mag_total=sqrt(x*x + y*y + z*z); 00072 float cos_z = z/mag_total; 00073 float theta_z = acosf(cos_z); 00074 00075 return theta_z; 00076 //printf("/n cos_zz= %f /t theta_z= %f /n",cos_z,theta_z); 00077 } 00078 00079 //uint8_t tm1[134]; 00080 00081 void FCTN_BAE_TM_TC (uint8_t* tc) 00082 00083 { 00084 // tm1[0] = 1; 00085 uint8_t service_type=(tc[2]&0xF0); 00086 /*chaged*/ 00087 uint8_t* tm; // without it some identifier error 00088 uint16_t crc16; 00089 00090 00091 switch(service_type) 00092 { 00093 case 0x60: 00094 { 00095 printf("Memory Management Service\r\n"); 00096 uint8_t service_subtype=(tc[2]&0x0F); 00097 00098 switch(service_subtype) 00099 { 00100 case 0x01: 00101 { 00102 printf("Read from Flash\r\n"); 00103 uint16_t jj; 00104 uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4]; 00105 switch(MID) 00106 {case 0x1100:jj=0x00;// using uint16_t as jj typesimilarly used in FCTN_CDMS_WR_FLASH 00107 break; 00108 case 0x0100:jj=0x01; 00109 break; 00110 case 0x0101:jj=0x02; 00111 break; 00112 case 0x0102:jj=0x03; 00113 break; 00114 case 0x0107:jj=0x04; 00115 break; 00116 case 0x0103:jj=0x05; 00117 break; 00118 case 0x0104:jj=0x05; 00119 break; 00120 case 0x0105:jj=0x06; 00121 break; 00122 case 0x0106:jj=0x07; 00123 break; 00124 } 00125 /*pointer....!!!!*/ 00126 uint32_t FLASH_TEMP = FCTN_CDMS_RD_FLASH(jj); 00127 00128 tm[0] = 0x60; 00129 tm[1] = tc[0]; 00130 tm[2] = ACK_CODE; 00131 for(int i=0; i<8*4; i+=4) 00132 { 00133 tm[4+i] =(uint8_t )(((FLASH_TEMP)>>24)&0xFF); 00134 tm[5+i] =(uint8_t ) (((FLASH_TEMP)>>16)&0xFF); 00135 tm[6+i] =(uint8_t ) (((FLASH_TEMP)>>8)&0xFF); 00136 tm[7+i] =(uint8_t ) ((FLASH_TEMP) & 0xFF); 00137 00138 } 00139 00140 00141 for (int i=4+8*4; i<132;i++) 00142 { 00143 tm[i] = 0x00; 00144 } 00145 crc16 = CRC::crc16_gen(tm,132); 00146 tm[132] = (uint8_t)((crc16&0xFF00)>>8); 00147 tm[133] = (uint8_t)(crc16&0x00FF); 00148 00149 break; 00150 } 00151 case 0x02: 00152 { 00153 printf("Read from RAM\r\n"); 00154 uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4]; 00155 switch(MID) 00156 { 00157 00158 case 0x0001: 00159 { 00160 printf("\nRead from MID 0001 hk\n"); 00161 00162 /*taking some varible till we find some thing more useful*/ 00163 //uint8_t ref_val=0x01; 00164 telemetry[0] = 1; 00165 telemetry[1] = tc[0]; 00166 telemetry[2] = ACK_CODE; 00167 /*random or with bcn_tx_sw_enable assuming it is 1 bit in length 00168 how to get that we dont know, now we just assume it to be so*/ 00169 telemetry[3] = (BCN_SW); 00170 telemetry[3] = telemetry[3]|(TRXY_SW<<1); 00171 telemetry[3] = telemetry[3]|(TRZ_SW<<2); 00172 telemetry[3] = telemetry[3]|(ATS1_SW_ENABLE<<3); 00173 telemetry[3] = telemetry[3]|(ATS2_SW_ENABLE<<4); 00174 00175 if(BCN_TX_STATUS==2) 00176 telemetry[3] = telemetry[3]|0x20; 00177 else 00178 telemetry[3] = telemetry[3] & 0xDF; 00179 00180 telemetry[3] = telemetry[3]|(BCN_FEN<<6); 00181 uint8_t mask_val=BAE_ENABLE & 0x00000008; 00182 /*can be a problem see if any error occurs*/ 00183 telemetry[3] = telemetry[3]|(mask_val<<7); 00184 00185 /*not included in the code yet*/ 00186 telemetry[4] = BAE_RESET_COUNTER; 00187 telemetry[5] = ACS_STATE; 00188 telemetry[5] = telemetry[5]|(EN_BTRY_HT<<3); 00189 telemetry[5] = telemetry[5]|(phase_TR_x<<4); 00190 telemetry[5] = telemetry[5]|(phase_TR_y<<5); 00191 telemetry[5] = telemetry[5]|(phase_TR_z<<6); 00192 /*spare to be fixed*/ 00193 //telemetry[5] = telemetry[5]|(Spare))<<7); 00194 /**/ 00195 uint8_t soc_powerlevel_2=50; 00196 uint8_t soc_powerlevel_3=65; 00197 00198 telemetry[6] = soc_powerlevel_2; 00199 telemetry[7] = soc_powerlevel_3; 00200 00201 /*to be fixed*/ 00202 telemetry[8] = 0; 00203 telemetry[9] = 0; 00204 telemetry[10] = 0; 00205 telemetry[11] = 0; 00206 //telemetry[8] = Torque Rod X Offset; 00207 //telemetry[9] = Torque Rod Y Offset; 00208 //telemetry[10] = Torque Rod Z Offset; 00209 //telemetry[11] = ACS_DEMAG_TIME_DELAY; 00210 telemetry[12] = (BAE_STATUS>>24) & 0xFF; 00211 telemetry[13] = (BAE_STATUS>>16) & 0xFF; 00212 telemetry[14] = (BAE_STATUS>>8) & 0xFF; 00213 telemetry[15] = BAE_STATUS & 0xFF; 00214 00215 /*to be fixed*/ 00216 //telemetry[16] = BCN_FAIL_COUNT; 00217 telemetry[17] = actual_data.power_mode; 00218 /*to be fixed*/ 00219 uint16_t P_BAE_I2CRX_COUNTER=0; 00220 uint16_t P_ACS_MAIN_COUNTER=0; 00221 uint16_t P_BCN_TX_MAIN_COUNTER=0; 00222 uint16_t P_EPS_MAIN_COUNTER=0; 00223 00224 telemetry[18] = P_BAE_I2CRX_COUNTER>>8; 00225 telemetry[19] = P_BAE_I2CRX_COUNTER; 00226 telemetry[20] = P_ACS_MAIN_COUNTER>>8; 00227 telemetry[21] = P_ACS_MAIN_COUNTER; 00228 telemetry[22] = P_BCN_TX_MAIN_COUNTER>>8; 00229 telemetry[23] = P_BCN_TX_MAIN_COUNTER; 00230 telemetry[24] = P_EPS_MAIN_COUNTER>>8; 00231 telemetry[25] = P_EPS_MAIN_COUNTER; 00232 00233 //actual_data.AngularSpeed_actual[0]=5.32498; 00234 for(int i=0; i<3; i++) 00235 FCTN_CONVERT_FLOAT((float)actual_data.Bvalue_actual[i],&telemetry[26+ (i*4)]); 00236 for(int i=0; i<3; i++) 00237 FCTN_CONVERT_FLOAT((float)actual_data.AngularSpeed_actual[i],&telemetry[38+(i*4)]); 00238 00239 //printf("\n\rthe value is 38\t %x\n",telemetry[38]); 00240 //printf("\n\rthe value is 39\t%x\n",telemetry[39]); 00241 //printf("\n\rthe value is 40\t%x\n",telemetry[40]); 00242 //printf("\n\rthe value is 41\t%x\n",telemetry[41]); 00243 //printf("\n\rthe value true\t%f\n",actual_data.AngularSpeed_actual[0]); 00244 00245 00246 //uint32_t input_stage1=0x00000000; 00247 //uint8_t output1[4]; 00248 //output1[0]=(uint32_t)(telemetry[38]); 00249 //output1[1]=(uint32_t)(telemetry[39]); 00250 //output1[2]=(uint32_t)(telemetry[40]); 00251 //output1[3]=(uint32_t)(telemetry[41]); 00252 00253 //input_stage1=output[3]+(output[2]*(0x100))+(output[1]*(0x10000))+(output[0]*(0x1000000)); 00254 //input_stage1=(output1[0]<<24) | (output1[1]<<16) | (output1[2]<<8) | (output1[3]); 00255 00256 00257 //assert(sizeof(float) == sizeof(uint32_t)); 00258 //float* temp1 = reinterpret_cast<float*>(&input_stage1); 00259 00260 //printf("\n\r the value is: %f \n",*temp1); 00261 00262 //FAULT_FLAG(); 00263 telemetry[50] = actual_data.faultIr_status; 00264 telemetry[51] = actual_data.faultPoll_status; 00265 //Bdot Rotation Speed of Command telemetry[52-53] 00266 //Bdot Output Current telemetry[54] 00267 //float l_pmw1 = (PWM1); 00268 //float l_pmw2 = PWM2; 00269 //float l_pmw3 = PWM3; 00270 00271 /*__________________________________________________________________*/ 00272 00273 /*change and check if changing it to PWM1 causes problem*/ 00274 00275 /*___________________________________________________________________*/ 00276 00277 float PWM_measured[3]; 00278 00279 PWM_measured[0] = PWM1.read(); 00280 PWM_measured[1] = PWM2.read(); 00281 PWM_measured[2] = PWM3.read(); 00282 00283 FCTN_CONVERT_FLOAT(PWM_measured[0], &telemetry[55]); 00284 FCTN_CONVERT_FLOAT(PWM_measured[1], &telemetry[59]); 00285 FCTN_CONVERT_FLOAT(PWM_measured[2], &telemetry[63]); 00286 float attitude_ang = angle(actual_data.Bvalue_actual[0],actual_data.Bvalue_actual[1],actual_data.Bvalue_actual[2]); 00287 FCTN_CONVERT_FLOAT(attitude_ang, &telemetry[67]); 00288 00289 for (int i=0; i<16; i++) 00290 telemetry[68+i] = quant_data.voltage_quant[i]; 00291 for (int i=0; i<12; i++) 00292 telemetry[84+i] = quant_data.current_quant[i]; 00293 //telemetry[96] 00294 //telemetry[97] 00295 //telemetry[98] 00296 //telemetry[99] 00297 telemetry[100] = quant_data.Batt_voltage_quant; 00298 telemetry[101] = quant_data.BAE_temp_quant; 00299 telemetry[102] = quant_data.Batt_gauge_quant[1]; 00300 telemetry[103] = quant_data.Batt_temp_quant[0]; 00301 telemetry[104] = quant_data.Batt_temp_quant[1]; 00302 00303 //telemetry[105] = beacon temperature; 00304 00305 for (int i=105; i<132;i++) 00306 { 00307 telemetry[i] = 0x00; 00308 } 00309 crc16 = CRC::crc16_gen(telemetry,132); 00310 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8); 00311 telemetry[133] = (uint8_t)(crc16&0x00FF); 00312 00313 break; 00314 } 00315 case 0x0002: 00316 { 00317 printf("\r\n"); 00318 telemetry[0] = 0x60; 00319 telemetry[1] = tc[0]; 00320 telemetry[2] = ACK_CODE; 00321 00322 for(int i;i<16;i++) 00323 telemetry[i+3] = bae_HK_minmax.voltage_max[i]; 00324 00325 for(int i;i<12;i++) 00326 telemetry[i+18] = bae_HK_minmax.current_max[i]; 00327 00328 telemetry[29] = bae_HK_minmax.Batt_voltage_max;; 00329 telemetry[30] = bae_HK_minmax.BAE_temp_max; 00330 00331 /*battery soc*/ 00332 //telemetry[31] = BAE_HK_min_max bae_HK_minmax.voltage_max; 00333 00334 telemetry[32] = bae_HK_minmax.Batt_temp_max[0]; 00335 telemetry[33] = bae_HK_minmax.Batt_temp_max[1]; 00336 00337 /*BCN temp not there*/ 00338 //telemetry[34] = BAE_HK_min_max bae_HK_minmax.; 00339 00340 for(int i=0; i<3; i++) 00341 FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[i],&telemetry[35+(i*4)]); 00342 00343 for(int i=0; i<3; i++) 00344 FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[i],&telemetry[47+(i*4)]); 00345 00346 /*min data*/ 00347 00348 for(int i;i<16;i++) 00349 telemetry[i+59] = bae_HK_minmax.voltage_min[i]; 00350 00351 for(int i;i<12;i++) 00352 telemetry[i+74] = bae_HK_minmax.current_min[i]; 00353 00354 telemetry[86] = bae_HK_minmax.Batt_voltage_min; 00355 telemetry[87] = bae_HK_minmax.BAE_temp_min; 00356 00357 /*battery soc*/ 00358 //telemetry[88] = BAE_HK_min_max bae_HK_minmax.voltage_max; 00359 00360 telemetry[89] = bae_HK_minmax.Batt_temp_min[0]; 00361 telemetry[90] = bae_HK_minmax.Batt_temp_min[1]; 00362 //huhu// 00363 00364 /*BCN temp not there*/ 00365 //telemetry[91] = BAE_HK_min_max bae_HK_minmax.; 00366 00367 for(int i=0; i<3; i++) 00368 FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[i],&telemetry[91+(i*4)]); 00369 00370 for(int i=0; i<3; i++) 00371 FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[i],&telemetry[103+(i*4)]); 00372 00373 00374 for (int i=115; i<132;i++) 00375 { 00376 telemetry[i] = 0x00; 00377 } 00378 crc16 = CRC::crc16_gen(telemetry,132); 00379 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8); 00380 telemetry[133] = (uint8_t)(crc16&0x00FF); 00381 break; 00382 } 00383 } 00384 break; 00385 } 00386 case 0x05: 00387 { 00388 printf("\nRead from MID 0001 min max\n"); 00389 /*changed*/ 00390 printf("\n\rwrite on flash\n"); 00391 uint32_t FLASH_DATA[8]; 00392 00393 uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4]; 00394 switch(MID ) 00395 { 00396 00397 case 0x0100: 00398 { 00399 FLASH_DATA[0] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00400 FCTN_CDMS_WR_FLASH(0x00,FLASH_DATA[0]); 00401 break; 00402 } 00403 case 0x0101: 00404 { 00405 FLASH_DATA[1] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00406 FCTN_CDMS_WR_FLASH(0x01,FLASH_DATA[1]); 00407 break; 00408 } 00409 00410 case 0x0102: 00411 { 00412 FLASH_DATA[2] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00413 FCTN_CDMS_WR_FLASH(0x02,FLASH_DATA[2]); 00414 break; 00415 } 00416 00417 case 0x0103: 00418 { 00419 FLASH_DATA[3] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00420 FCTN_CDMS_WR_FLASH(0x03,FLASH_DATA[3]); 00421 break; 00422 } 00423 case 0x0104: 00424 { 00425 FLASH_DATA[4] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00426 FCTN_CDMS_WR_FLASH(0x04,FLASH_DATA[4]); 00427 break; 00428 } 00429 00430 case 0x0105: 00431 { 00432 FLASH_DATA[5] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00433 FCTN_CDMS_WR_FLASH(0x05,FLASH_DATA[5]); 00434 break; 00435 } 00436 case 0x0106: 00437 { 00438 FLASH_DATA[6] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00439 FCTN_CDMS_WR_FLASH(0x06,FLASH_DATA[6]); 00440 break; 00441 } 00442 00443 case 0x0107: 00444 { 00445 FLASH_DATA[7] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00446 FCTN_CDMS_WR_FLASH(0x07,FLASH_DATA[7]); 00447 break; 00448 } 00449 00450 default: 00451 { 00452 printf("Invalid MMS\r\n"); 00453 } 00454 } 00455 00456 00457 for (int i=4; i<132;i++) 00458 { 00459 tm[i] = 0x00; 00460 } 00461 crc16 = CRC::crc16_gen(tm,132); 00462 tm[132] = (uint8_t)((crc16&0xFF00)>>8); 00463 tm[133] = (uint8_t)(crc16&0x00FF); 00464 tm[0] = 0x60; 00465 tm[1] = tc[0]; 00466 tm[2] = ACK_CODE; 00467 00468 printf("Written on Flash\r\n"); 00469 break; 00470 } 00471 default: 00472 { 00473 printf("Invalid TC"); 00474 //ACK_L234_telemetry 00475 telemetry[0]=0xB0; 00476 telemetry[1]=tc[0]; 00477 telemetry[2]=ACK_CODE; 00478 for(uint8_t i=3;i<11;i++) 00479 { 00480 telemetry[i]=0x00; 00481 } 00482 crc16 = CRC::crc16_gen(telemetry,11); 00483 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00484 telemetry[12] = (uint8_t)(crc16&0x00FF); 00485 for(uint8_t i=13;i<134;i++) 00486 { 00487 telemetry[i]=0x00; 00488 } 00489 break; 00490 } 00491 } 00492 break; 00493 } 00494 case 0x80: 00495 { 00496 //printf("Function Management Service\r\n"); 00497 uint8_t service_subtype=(tc[2]&0x0F); 00498 switch(service_subtype) 00499 { 00500 case 0x01: 00501 { 00502 printf("FMS Activated\r\n"); 00503 uint8_t pid=tc[3]; 00504 switch(pid) 00505 { 00506 case 0xE0: 00507 { 00508 float B[3],W[3]; 00509 printf("ACS_COMSN\r\n"); 00510 //ACK_L234_telemetry 00511 00512 uint8_t B_x[2]; 00513 uint8_t B_y[2]; 00514 uint8_t B_z[2]; 00515 uint8_t W_x[2]; 00516 uint8_t W_y[2]; 00517 uint8_t W_z[2]; 00518 00519 B_x[0]=tc[3]; 00520 B_x[1]=tc[4]; 00521 B_y[0]=tc[5]; 00522 B_y[1]=tc[6]; 00523 B_z[0]=tc[7]; 00524 B_z[1]=tc[8]; 00525 00526 W_x[0]=tc[9]; 00527 W_x[1]=tc[10]; 00528 W_y[0]=tc[11]; 00529 W_y[1]=tc[12]; 00530 W_z[0]=tc[13]; 00531 W_z[1]=tc[14]; 00532 00533 FCTN_CONVERT_UINT(B_x,&B[0]); 00534 FCTN_CONVERT_UINT(B_y,&B[1]); 00535 FCTN_CONVERT_UINT(B_z,&B[2]); 00536 00537 FCTN_CONVERT_UINT (W_x, &W[0]); 00538 FCTN_CONVERT_UINT (W_y, &W[1]); 00539 FCTN_CONVERT_UINT (W_z, &W[2]); 00540 00541 telemetry[0]=0xB0; 00542 telemetry[1]=tc[0]; 00543 telemetry[2]=ACK_CODE; 00544 //FCTN_ATS_DATA_ACQ(); //get data 00545 printf("gyro values\n\r"); 00546 for(int i=0; i<3; i++) 00547 printf("%f\n\r",W[i]); 00548 printf("mag values\n\r"); 00549 for(int i=0; i<3; i++) 00550 printf("%f\n\r",B[i]); 00551 /* FCTN_CONVERT_FLOAT(data[0],&telemetry[4]); //telemetry[4] - telemetry[7] 00552 FCTN_CONVERT_FLOAT(data[1],&telemetry[8]); //telemetry[8] - telemetry[11] 00553 FCTN_CONVERT_FLOAT(data[2],&telemetry[12]); //telemetry[12] - telemetry[15] 00554 FCTN_CONVERT_FLOAT(data[0],&telemetry[16]); //telemetry[16] - telemetry[19] 00555 FCTN_CONVERT_FLOAT(data[1],&telemetry[20]); //telemetry[20] - telemetry[23] 00556 FCTN_CONVERT_FLOAT(data[2],&telemetry[24]); //telemetry[24] - telemetry[27] 00557 if((data[0]<8) && (data[1]<8) && (data[2] <8)) 00558 telemetry[28] = 1; // gyro values in correct range 00559 else 00560 telemetry[28] = 0; 00561 if ((data[3] > 20 ) && (data[4] >20) && (data[5]>20)&& (data[3] < 50 ) && (data[4] <50) && (data[5]<50)) 00562 telemetry[29] = 1; // mag values in correct range 00563 else 00564 telemetry[29] = 0; 00565 */ 00566 // float B[3],W[3]; 00567 // B[0] = B0; 00568 // B[1] = B1; 00569 // B[2] = B2; 00570 // W[0] = W0; 00571 // W[1] = W1; 00572 // W[2] = W2; 00573 // Control algo commissioning 00574 /* FCTN_ACS_CNTRLALGO(B,W); 00575 FCTN_CONVERT_FLOAT(moment[0],&telemetry[30]); //telemetry[30] - telemetry[33] 00576 FCTN_CONVERT_FLOAT(moment[1],&telemetry[34]); //telemetry[34] - telemetry[37] 00577 FCTN_CONVERT_FLOAT(moment[2],&telemetry[38]); //telemetry[38] - telemetry[41] 00578 // to include commission TR as well 00579 for(uint8_t i=42;i<132;i++) 00580 { 00581 telemetry[i]=0x00; 00582 } 00583 00584 crc16 = CRC::crc16_gen(telemetry,132); 00585 telemetry[133] = (uint8_t)((crc16&0xFF00)>>8); 00586 telemetry[134] = (uint8_t)(crc16&0x00FF); 00587 break; 00588 */ 00589 00590 // Control algo commissioning 00591 FCTN_ACS_CNTRLALGO(B,W); 00592 FCTN_CONVERT_FLOAT(moment[0],&telemetry[4]); //telemetry[4] - telemetry[7] 00593 FCTN_CONVERT_FLOAT(moment[1],&telemetry[8]); //telemetry[8] - telemetry[11] 00594 FCTN_CONVERT_FLOAT(moment[2],&telemetry[12]); //telemetry[12] - telemetry[15] 00595 // to include commission TR as well 00596 for(uint8_t i=16;i<132;i++) 00597 { 00598 telemetry[i]=0x00; 00599 } 00600 00601 crc16 = CRC::crc16_gen(telemetry,132); 00602 telemetry[133] = (uint8_t)((crc16&0xFF00)>>8); 00603 telemetry[134] = (uint8_t)(crc16&0x00FF); 00604 break; 00605 } 00606 case 0xE1: 00607 { 00608 float moment_tc[3]; 00609 printf("HARDWARE_COMSN\r\n"); 00610 //ACK_L234_telemetry 00611 uint8_t M0[2]; 00612 uint8_t M1[2]; 00613 uint8_t M2[2]; 00614 00615 M0[0]=tc[3]; 00616 M0[1]=tc[4]; 00617 M1[0]=tc[5]; 00618 M1[1]=tc[6]; 00619 M2[0]=tc[7]; 00620 M2[1]=tc[8]; 00621 00622 00623 telemetry[0]=0xB0; 00624 telemetry[1]=tc[0]; 00625 telemetry[2]=ACK_CODE; 00626 00627 float PWM_measured[3]; 00628 00629 00630 FCTN_CONVERT_UINT(M0,&moment_tc[0]); 00631 moment_tc[1] = 0; 00632 moment_tc[2] = 0; 00633 FCTN_ACS_GENPWM_MAIN(moment_tc); 00634 PWM_measured[0] = PWM1.read(); 00635 FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (0*4)]); 00636 00637 FCTN_CONVERT_UINT(M1, &moment_tc[1]); 00638 moment_tc[0] = 0; 00639 moment_tc[2] = 0; 00640 FCTN_ACS_GENPWM_MAIN(moment_tc); 00641 PWM_measured[1] = PWM2.read(); 00642 FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (1*4)]); 00643 00644 FCTN_CONVERT_UINT(M2, &moment_tc[2]); 00645 moment_tc[0] = 0; 00646 moment_tc[1] = 0; 00647 FCTN_ACS_GENPWM_MAIN(moment_tc); 00648 PWM_measured[2] = PWM3.read(); 00649 FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (2*4)]); 00650 00651 FCTN_CONVERT_FLOAT(PWM_measured[0],&telemetry[4]); //4-7 00652 FCTN_CONVERT_FLOAT(PWM_measured[1],&telemetry[8]); //8-11 00653 FCTN_CONVERT_FLOAT(PWM_measured[2],&telemetry[12]); //12-15 00654 00655 00656 // for(int i=0; i<12; i++) 00657 // FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]); 00658 00659 00660 // FCTN_ATS_DATA_ACQ(); //get data 00661 00662 00663 // to include commission TR as well 00664 for(uint8_t i=28;i<132;i++) 00665 { 00666 telemetry[i]=0x00; 00667 } 00668 00669 crc16 = CRC::crc16_gen(telemetry,132); 00670 telemetry[133] = (uint8_t)((crc16&0xFF00)>>8); 00671 telemetry[134] = (uint8_t)(crc16&0x00FF); 00672 break; 00673 } 00674 case 0x02: 00675 { 00676 00677 printf("Run P_EPS_MAIN\r\n"); 00678 F_EPS(); 00679 //ACK_L234_telemetry 00680 telemetry[0]=0xB0; 00681 telemetry[1]=tc[0]; 00682 telemetry[2]=ACK_CODE; 00683 for(uint8_t i=0;i<133;i++) 00684 { 00685 telemetry[i]=0x00; 00686 } 00687 crc16 = CRC::crc16_gen(telemetry,132); 00688 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8); 00689 telemetry[133] = (uint8_t)(crc16&0x00FF); 00690 00691 for(uint8_t i=13;i<134;i++) 00692 { 00693 telemetry[i]=0x00; 00694 } 00695 break; 00696 } 00697 case 0x03: 00698 { 00699 printf("Run P_ACS_INIT\r\n"); 00700 FCTN_ACS_INIT(); 00701 //ACK_L234_telemetry 00702 telemetry[0]=0xB0; 00703 telemetry[1]=tc[0]; 00704 telemetry[2]=ACK_CODE; 00705 for(uint8_t i=3;i<11;i++) 00706 { 00707 telemetry[i]=0x00; 00708 } 00709 crc16 = CRC::crc16_gen(telemetry,11); 00710 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00711 telemetry[12] = (uint8_t)(crc16&0x00FF); 00712 for(uint8_t i=13;i<134;i++) 00713 { 00714 telemetry[i]=0x00; 00715 } 00716 break; 00717 } 00718 case 0x04: 00719 { 00720 00721 printf("Run P_ACS_ACQ_DATA\r\n"); 00722 FCTN_ATS_DATA_ACQ(); 00723 //ACK_L234_TM 00724 telemetry[0]=0xB0; 00725 telemetry[1]=tc[0]; 00726 telemetry[2]=ACK_CODE; 00727 for(uint8_t i=3;i<11;i++) 00728 { 00729 telemetry[i]=0x00; 00730 } 00731 crc16 = CRC::crc16_gen(telemetry,11); 00732 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00733 telemetry[12] = (uint8_t)(crc16&0x00FF); 00734 for(uint8_t i=13;i<134;i++) 00735 { 00736 telemetry[i]=0x00; 00737 } 00738 break; 00739 } 00740 case 0x05: 00741 { 00742 printf("Run P_ACS_MAIN\r\n"); 00743 F_ACS(); 00744 for(int i=0; i<3; i++) 00745 FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&telemetry[(i*4)]); 00746 for(int i=0; i<3; i++) 00747 FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&telemetry[12+(i*4)]); 00748 00749 telemetry[24] = ACS_STATE; 00750 telemetry[24] = telemetry[5]|(EN_BTRY_HT<<3); 00751 telemetry[24] = telemetry[5]|(phase_TR_x<<4); 00752 telemetry[24] = telemetry[5]|(phase_TR_y<<5); 00753 telemetry[24] = telemetry[5]|(phase_TR_z<<6); 00754 00755 /*___________________change / check pwm working__________________________________*/ 00756 00757 FCTN_CONVERT_FLOAT(PWM1,&telemetry[25]); 00758 FCTN_CONVERT_FLOAT(PWM2,&telemetry[29]); 00759 FCTN_CONVERT_FLOAT(PWM3,&telemetry[33]); 00760 00761 //ACK_L234_TM 00762 telemetry[0]=0xB0; 00763 telemetry[1]=tc[0]; 00764 telemetry[2]=ACK_CODE; 00765 for(uint8_t i=3;i<11;i++) 00766 { 00767 telemetry[i]=0x00; 00768 } 00769 00770 crc16 = CRC::crc16_gen(telemetry,37); 00771 telemetry[37] = (uint8_t)((crc16&0xFF00)>>8); 00772 telemetry[38] = (uint8_t)(crc16&0x00FF); 00773 for(uint8_t i=39;i<134;i++) 00774 { 00775 telemetry[i]=0x00; 00776 } 00777 break; 00778 } 00779 case 0x06: 00780 { 00781 F_BCN(); 00782 printf("Run P_BCN_INIT\r\n"); 00783 //ACK_L234_TM 00784 telemetry[0]=0xB0; 00785 telemetry[1]=tc[0]; 00786 telemetry[2]=ACK_CODE; 00787 for(uint8_t i=3;i<11;i++) 00788 { 00789 telemetry[i]=0x00; 00790 } 00791 crc16 = CRC::crc16_gen(telemetry,0); 00792 telemetry[0] = (uint8_t)((crc16&0xFF00)>>8); 00793 telemetry[1] = (uint8_t)(crc16&0x00FF); 00794 for(uint8_t i=2;i<134;i++) 00795 { 00796 telemetry[i]=0x00; 00797 } 00798 break; 00799 } 00800 case 0x07: 00801 { 00802 printf("Run P_BCN_TX_MAIN\r\n"); 00803 //ACK_L234_TM 00804 telemetry[0]=0xB0; 00805 telemetry[1]=tc[0]; 00806 telemetry[2]=ACK_CODE; 00807 for(uint8_t i=3;i<11;i++) 00808 { 00809 telemetry[i]=0x00; 00810 } 00811 crc16 = CRC::crc16_gen(telemetry,11); 00812 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00813 telemetry[12] = (uint8_t)(crc16&0x00FF); 00814 for(uint8_t i=13;i<134;i++) 00815 { 00816 telemetry[i]=0x00; 00817 } 00818 break; 00819 } 00820 case 0x11: 00821 { 00822 printf("SW_ON_ACS_ATS1_SW_ENABLE\r\n"); 00823 //ACK_L234_TM 00824 telemetry[0]=0xB0; 00825 telemetry[1]=tc[0]; 00826 telemetry[2]=1; 00827 ATS1_SW_ENABLE = 1; // making sure we switch off the other 00828 ATS1_SW_ENABLE = 0; 00829 for(uint8_t i=3;i<11;i++) 00830 { 00831 telemetry[i]=0x00; 00832 } 00833 crc16 = CRC::crc16_gen(telemetry,11); 00834 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00835 telemetry[12] = (uint8_t)(crc16&0x00FF); 00836 for(uint8_t i=13;i<134;i++) 00837 { 00838 telemetry[i]=0x00; 00839 } 00840 break; 00841 } 00842 case 0x12: 00843 { 00844 printf("SW_ON_ACS_ATS2_SW_ENABLE\r\n"); 00845 //ACK_L234_TM 00846 telemetry[0]=0xB0; 00847 telemetry[1]=tc[0]; 00848 ATS1_SW_ENABLE = 1; //make sure u switch off the other 00849 ATS2_SW_ENABLE = 0; 00850 telemetry[2]=1; 00851 for(uint8_t i=3;i<11;i++) 00852 { 00853 telemetry[i]=0x00; 00854 } 00855 crc16 = CRC::crc16_gen(telemetry,11); 00856 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00857 telemetry[12] = (uint8_t)(crc16&0x00FF); 00858 for(uint8_t i=13;i<134;i++) 00859 { 00860 telemetry[i]=0x00; 00861 } 00862 break; 00863 } 00864 case 0x13: 00865 { 00866 printf("SW_ON_ACS_TR_XY_ENABLE\r\n"); 00867 //ACK_L234_TM 00868 telemetry[0]=0xB0; 00869 telemetry[1]=tc[0]; 00870 TRXY_SW = 1; 00871 telemetry[2]=1; 00872 for(uint8_t i=3;i<11;i++) 00873 { 00874 telemetry[i]=0x00; 00875 } 00876 crc16 = CRC::crc16_gen(telemetry,11); 00877 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00878 telemetry[12] = (uint8_t)(crc16&0x00FF); 00879 for(uint8_t i=13;i<134;i++) 00880 { 00881 telemetry[i]=0x00; 00882 } 00883 break; 00884 } 00885 case 0x14: 00886 { 00887 printf("SW_ON_ACS_TR_Z_ENABLE\r\n"); 00888 //ACK_L234_TM 00889 telemetry[0]=0xB0; 00890 telemetry[1]=tc[0]; 00891 telemetry[2]=1; 00892 TRZ_SW = 1; 00893 for(uint8_t i=3;i<11;i++) 00894 { 00895 telemetry[i]=0x00; 00896 } 00897 crc16 = CRC::crc16_gen(telemetry,11); 00898 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00899 telemetry[12] = (uint8_t)(crc16&0x00FF); 00900 for(uint8_t i=13;i<134;i++) 00901 { 00902 telemetry[i]=0x00; 00903 } 00904 break; 00905 } 00906 case 0x15: 00907 { 00908 printf("SW_ON_BCN_TX_SW_ENABLE\r\n"); 00909 //ACK_L234_TM 00910 telemetry[0]=0xB0; 00911 telemetry[1]=tc[0]; 00912 telemetry[2]=1; 00913 BCN_SW = 0; 00914 for(uint8_t i=3;i<11;i++) 00915 { 00916 telemetry[i]=0x00; 00917 } 00918 crc16 = CRC::crc16_gen(telemetry,11); 00919 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00920 telemetry[12] = (uint8_t)(crc16&0x00FF); 00921 for(uint8_t i=13;i<134;i++) 00922 { 00923 telemetry[i]=0x00; 00924 } 00925 break; 00926 } 00927 case 0x21: 00928 { 00929 printf("SW_OFF_ACS_ATS1_SW_ENABLE\r\n"); 00930 //ACK_L234_TM 00931 telemetry[0]=0xB0; 00932 telemetry[1]=tc[0]; 00933 telemetry[2]=1; 00934 ATS1_SW_ENABLE = 1; 00935 for(uint8_t i=3;i<11;i++) 00936 { 00937 telemetry[i]=0x00; 00938 } 00939 crc16 = CRC::crc16_gen(telemetry,11); 00940 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00941 telemetry[12] = (uint8_t)(crc16&0x00FF); 00942 for(uint8_t i=13;i<134;i++) 00943 { 00944 telemetry[i]=0x00; 00945 } 00946 break; 00947 } 00948 case 0x22: 00949 { 00950 printf("SW_OFF_ACS_ATS2_SW_ENABLE\r\n"); 00951 //ACK_L234_TM 00952 telemetry[0]=0xB0; 00953 telemetry[1]=tc[0]; 00954 telemetry[2]=1; 00955 ATS2_SW_ENABLE = 1; 00956 for(uint8_t i=3;i<11;i++) 00957 { 00958 telemetry[i]=0x00; 00959 } 00960 crc16 = CRC::crc16_gen(telemetry,11); 00961 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00962 telemetry[12] = (uint8_t)(crc16&0x00FF); 00963 for(uint8_t i=13;i<134;i++) 00964 { 00965 telemetry[i]=0x00; 00966 } 00967 break; 00968 } 00969 case 0x23: 00970 { 00971 printf("SW_OFF_ACS_TR_XY_ENABLE\r\n"); 00972 //ACK_L234_TM 00973 telemetry[0]=0xB0; 00974 telemetry[1]=tc[0]; 00975 telemetry[2]=1; 00976 TRXY_SW= 0; 00977 for(uint8_t i=3;i<11;i++) 00978 { 00979 telemetry[i]=0x00; 00980 } 00981 crc16 = CRC::crc16_gen(telemetry,11); 00982 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00983 telemetry[12] = (uint8_t)(crc16&0x00FF); 00984 for(uint8_t i=13;i<134;i++) 00985 { 00986 telemetry[i]=0x00; 00987 } 00988 break; 00989 } 00990 case 0x24: 00991 { 00992 printf("SW_OFF_ACS_TR_Z_ENABLE\r\n"); 00993 //ACK_L234_TM 00994 telemetry[0]=0xB0; 00995 telemetry[1]=tc[0]; 00996 telemetry[2]=1; 00997 TRZ_SW = 0; 00998 for(uint8_t i=3;i<11;i++) 00999 { 01000 telemetry[i]=0x00; 01001 } 01002 crc16 = CRC::crc16_gen(telemetry,11); 01003 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01004 telemetry[12] = (uint8_t)(crc16&0x00FF); 01005 for(uint8_t i=13;i<134;i++) 01006 { 01007 telemetry[i]=0x00; 01008 } 01009 break; 01010 } 01011 case 0x25: 01012 { 01013 printf("SW_OFF_BCN_TX_SW_ENABLE\r\n"); 01014 //ACK_L234_TM 01015 telemetry[0]=0xB0; 01016 telemetry[1]=tc[0]; 01017 telemetry[2]=1; 01018 BCN_SW = 1; 01019 for(uint8_t i=3;i<11;i++) 01020 { 01021 telemetry[i]=0x00; 01022 } 01023 crc16 = CRC::crc16_gen(telemetry,11); 01024 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01025 telemetry[12] = (uint8_t)(crc16&0x00FF); 01026 for(uint8_t i=13;i<134;i++) 01027 { 01028 telemetry[i]=0x00; 01029 } 01030 break; 01031 } 01032 case 0x31: 01033 { 01034 printf("ACS_ATS1_SW_RESET\r\n"); 01035 //ACK_L234_TM 01036 telemetry[0]=0xB0; 01037 telemetry[1]=tc[0]; 01038 telemetry[2]=1; 01039 ATS1_SW_ENABLE = 1; 01040 wait_us(1); 01041 ATS1_SW_ENABLE = 0; 01042 for(uint8_t i=3;i<11;i++) 01043 { 01044 telemetry[i]=0x00; 01045 } 01046 crc16 = CRC::crc16_gen(telemetry,11); 01047 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01048 telemetry[12] = (uint8_t)(crc16&0x00FF); 01049 for(uint8_t i=13;i<134;i++) 01050 { 01051 telemetry[i]=0x00; 01052 } 01053 break; 01054 } 01055 case 0x32: 01056 { 01057 printf("BCN_SW_RESET\r\n"); 01058 //ACK_L234_TM 01059 telemetry[0]=0xB0; 01060 telemetry[1]=tc[0]; 01061 telemetry[2]=1; 01062 BCN_SW = 1; 01063 wait_us(1); 01064 BCN_SW = 0; 01065 for(uint8_t i=3;i<11;i++) 01066 { 01067 telemetry[i]=0x00; 01068 } 01069 crc16 = CRC::crc16_gen(telemetry,11); 01070 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01071 telemetry[12] = (uint8_t)(crc16&0x00FF); 01072 for(uint8_t i=13;i<134;i++) 01073 { 01074 telemetry[i]=0x00; 01075 } 01076 break; 01077 } 01078 case 0x33: 01079 { 01080 printf("ACS_ATS2_SW_RESET\r\n"); 01081 //ACK_L234_TM 01082 telemetry[0]=0xB0; 01083 telemetry[1]=tc[0]; 01084 telemetry[2]=1; 01085 ATS1_SW_ENABLE = 1; 01086 wait_us(1); 01087 ATS1_SW_ENABLE = 0; 01088 for(uint8_t i=3;i<11;i++) 01089 { 01090 telemetry[i]=0x00; 01091 } 01092 crc16 = CRC::crc16_gen(telemetry,11); 01093 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01094 telemetry[12] = (uint8_t)(crc16&0x00FF); 01095 for(uint8_t i=13;i<134;i++) 01096 { 01097 telemetry[i]=0x00; 01098 } 01099 break; 01100 } 01101 case 0x34: 01102 { 01103 printf("CDMS_SW_RESET\r\n"); 01104 //ACK_L234_TM 01105 telemetry[0]=0xB0; 01106 telemetry[1]=tc[0]; 01107 telemetry[2]=1; 01108 CDMS_RESET = 0; 01109 wait_us(1); 01110 CDMS_RESET = 1; 01111 for(uint8_t i=3;i<11;i++) 01112 { 01113 telemetry[i]=0x00; 01114 } 01115 crc16 = CRC::crc16_gen(telemetry,11); 01116 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01117 telemetry[12] = (uint8_t)(crc16&0x00FF); 01118 for(uint8_t i=13;i<134;i++) 01119 { 01120 telemetry[i]=0x00; 01121 } 01122 break; 01123 } 01124 default: 01125 { 01126 printf("Invalid TC\r\n"); 01127 //ACK_L234_TM 01128 telemetry[0]=0xB0; 01129 telemetry[1]=tc[0]; 01130 telemetry[2]=ACK_CODE; 01131 for(uint8_t i=3;i<11;i++) 01132 { 01133 telemetry[i]=0x00; 01134 } 01135 crc16 = CRC::crc16_gen(telemetry,11); 01136 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01137 telemetry[12] = (uint8_t)(crc16&0x00FF); 01138 for(uint8_t i=13;i<134;i++) 01139 { 01140 telemetry[i]=0x00; 01141 } 01142 break; 01143 } 01144 } 01145 break; 01146 } 01147 default: 01148 { 01149 printf("Invalid TC\r\n"); 01150 //ACK_L234_TM 01151 telemetry[0]=0xB0; 01152 telemetry[1]=tc[0]; 01153 telemetry[2]=ACK_CODE; 01154 for(uint8_t i=3;i<11;i++) 01155 { 01156 telemetry[i]=0x00; 01157 } 01158 crc16 = CRC::crc16_gen(telemetry,11); 01159 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01160 telemetry[12] = (uint8_t)(crc16&0x00FF); 01161 for(uint8_t i=13;i<134;i++) 01162 { 01163 telemetry[i]=0x00; 01164 } 01165 break; 01166 } 01167 } 01168 break; 01169 } 01170 default: 01171 { 01172 printf("Invalid TC\r\n"); 01173 //ACK_L234_TM 01174 telemetry[0]=0xB0; 01175 telemetry[1]=tc[0]; 01176 telemetry[2]=ACK_CODE; 01177 for(uint8_t i=3;i<11;i++) 01178 { 01179 telemetry[i]=0x00; 01180 } 01181 crc16 = CRC::crc16_gen(telemetry,11); 01182 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01183 telemetry[12] = (uint8_t)(crc16&0x00FF); 01184 for(uint8_t i=13;i<134;i++) 01185 { 01186 telemetry[i]=0x00; 01187 } 01188 break; 01189 } 01190 } 01191 } 01192 01193 01194 01195 01196 int strt_add = flash_size() - (2*SECTOR_SIZE); 01197 uint32_t flasharray[8]; //256+(3*1024) 01198 char *nativeflash = (char*)strt_add; 01199 01200 /*Writing to the Flash*/ 01201 void FCTN_CDMS_WR_FLASH(uint16_t j,uint32_t fdata) //j-position to write address ; fdata - flash data to be written 01202 { 01203 for(int i=0;i<8;i++) 01204 { 01205 flasharray[i]=nativeflash[i]; 01206 } 01207 flasharray[j]=fdata; 01208 erase_sector(strt_add); 01209 program_flash(strt_add, (char*)&flasharray,4*8); 01210 } 01211 /*End*/ 01212 01213 /*Reading from Flash*/ 01214 uint32_t FCTN_CDMS_RD_FLASH(uint16_t j) 01215 { 01216 for(int i=0;i<8;i++) 01217 { 01218 flasharray[i]=nativeflash[i]; 01219 } 01220 return flasharray[j]; 01221 } 01222 /*End*/ 01223 01224 // Convert float to 4 uint8_t 01225 01226 01227 void FCTN_CONVERT_FLOAT(float input, uint8_t output[4]) 01228 { 01229 assert(sizeof(float) == sizeof(uint32_t)); 01230 uint32_t* temp = reinterpret_cast<uint32_t*>(&input); 01231 01232 //float* output1 = reinterpret_cast<float*>(temp); 01233 01234 //printf("\n\r %f ", input); 01235 //std::cout << "\n\r uint32"<<*temp << std::endl; 01236 01237 output[0] =(uint8_t )(((*temp)>>24)&0xFF); 01238 output[1] =(uint8_t ) (((*temp)>>16)&0xFF); 01239 output[2] =(uint8_t ) (((*temp)>>8)&0xFF); 01240 output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic 01241 01242 printf("\n\rthe values generated are\n"); 01243 /*printf("\n\r%x\n",output[0]); 01244 printf("\n\r%x\n",output[1]); 01245 printf("\n\r%x\n",output[2]); 01246 printf("\n\r%x\n",output[3]); 01247 to check the values generated 01248 */ 01249 //printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]); 01250 //std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl; 01251 }
Generated on Tue Jul 12 2022 17:24:01 by
1.7.2