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