Team Fox / Mbed 2 deprecated BAE_Updated_Commissioning

Changes to be made for ATS_Fault logic and ACS_State

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of Japan_BAE_sensorworking_interrupr_reoccuring_copy by Team Fox

TCTM.cpp@15:3239c6391ffa, 2016-04-20 (annotated)

Committer:
Bragadeesh153
Date:
Wed Apr 20 17:47:05 2016 +0000
Revision:
15:3239c6391ffa
Parent:
14:a9588f443f1a 
Upated commissioning.. Changes to be made in ACS_MAIN logic and checking the faults of ATS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakthipriya 0:7b4c00e3912f 1 #include "mbed.h"
sakthipriya 0:7b4c00e3912f 2 #include "TCTM.h"
sakthipriya 3:07e15677a75c 3 #include "crc.h"
sakthipriya 4:39a4ae8c7ecd 4 #include "EPS.h"
sakthipriya 5:bb592f3185cc 5 #include "pin_config.h"
sakthipriya 5:bb592f3185cc 6 #include "FreescaleIAP.h"
sakthipriya 6:036d08b62785 7 #include "inttypes.h"
sakthipriya 6:036d08b62785 8 #include "iostream"
sakthipriya 6:036d08b62785 9 #include "stdint.h"
sakthipriya 6:036d08b62785 10 #include "cassert"
lakshya 10:f93407b97750 11 #include"math.h"
lakshya 10:f93407b97750 12
lakshya 11:1fdb94ae6563 13 /*define the pins for digital out*/
sakthipriya 0:7b4c00e3912f 14
sakthipriya 9:194afacf7449 15 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
sakthipriya 9:194afacf7449 16 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
sakthipriya 5:bb592f3185cc 17
Bragadeesh153 13:fb7facaf308b 18 extern DigitalOut TRXY_SW; //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN
sakthipriya 5:bb592f3185cc 19 extern DigitalOut TRZ_SW; //TR Z Switch
sakthipriya 5:bb592f3185cc 20 extern DigitalOut CDMS_RESET; // CDMS RESET
sakthipriya 5:bb592f3185cc 21 extern DigitalOut BCN_SW; //Beacon switch
Bragadeesh153 15:3239c6391ffa 22 extern uint8_t ACS_ATS_STATUS;
lakshya 10:f93407b97750 23 extern uint8_t BCN_TX_STATUS;
lakshya 10:f93407b97750 24 extern uint8_t BCN_FEN;
Bragadeesh153 15:3239c6391ffa 25 extern AnalogIn CurrentInput;
Bragadeesh153 15:3239c6391ffa 26
sakthipriya 4:39a4ae8c7ecd 27 extern BAE_HK_actual actual_data;
sakthipriya 4:39a4ae8c7ecd 28 extern BAE_HK_min_max bae_HK_minmax;
sakthipriya 4:39a4ae8c7ecd 29 extern uint32_t BAE_STATUS;
sakthipriya 5:bb592f3185cc 30 extern float data[6];
sakthipriya 5:bb592f3185cc 31 extern float moment[3];
lakshya 10:f93407b97750 32 extern uint8_t ACS_STATE;
lakshya 10:f93407b97750 33 extern DigitalOut EN_BTRY_HT;
lakshya 10:f93407b97750 34 extern DigitalOut phase_TR_x;
lakshya 10:f93407b97750 35 extern DigitalOut phase_TR_y;
lakshya 10:f93407b97750 36 extern DigitalOut phase_TR_z;
lakshya 10:f93407b97750 37 extern BAE_HK_quant quant_data;
Bragadeesh153 13:fb7facaf308b 38 //extern DigitalOut TRXY_SW;
lakshya 11:1fdb94ae6563 39 //extern DigitalOut TRZ_SW_EN; //same as TRZ_SW
lakshya 11:1fdb94ae6563 40 extern uint32_t BAE_ENABLE;
lakshya 11:1fdb94ae6563 41 //extern DigitalOut ACS_ACQ_DATA_ENABLE;
lakshya 11:1fdb94ae6563 42
lakshya 11:1fdb94ae6563 43 /*given a default value as of now shuld read value from flash and increment it write it back very time it starts(bae)*/
lakshya 11:1fdb94ae6563 44 extern uint8_t BAE_RESET_COUNTER=0;
lakshya 11:1fdb94ae6563 45
lakshya 14:a9588f443f1a 46 //extern uint8_t BCN_FAIL_COUNT;
lakshya 11:1fdb94ae6563 47
Bragadeesh153 13:fb7facaf308b 48 extern PwmOut PWM1; //x //Functions used to generate PWM signal
lakshya 11:1fdb94ae6563 49 extern PwmOut PWM2; //y
lakshya 11:1fdb94ae6563 50 extern PwmOut PWM3; //z //PWM output comes from pins p6
lakshya 11:1fdb94ae6563 51
lakshya 11:1fdb94ae6563 52 extern void F_ACS();
lakshya 11:1fdb94ae6563 53 extern void F_BCN();
lakshya 11:1fdb94ae6563 54 //extern void FCTN_ACS_GENPWM_MAIN();
lakshya 11:1fdb94ae6563 55 extern void F_EPS();
lakshya 11:1fdb94ae6563 56 extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]);
Bragadeesh153 13:fb7facaf308b 57 extern void FCTN_ACS_INIT();
lakshya 11:1fdb94ae6563 58
sakthipriya 5:bb592f3185cc 59
sakthipriya 5:bb592f3185cc 60 extern void FCTN_ATS_DATA_ACQ();
sakthipriya 5:bb592f3185cc 61 extern void FCTN_ACS_CNTRLALGO(float*,float*);
Bragadeesh153 13:fb7facaf308b 62 uint8_t telemetry[135];
lakshya 11:1fdb94ae6563 63
lakshya 11:1fdb94ae6563 64 void FCTN_CONVERT_UINT (uint8_t input[2], float* output)
lakshya 11:1fdb94ae6563 65 {
lakshya 11:1fdb94ae6563 66
lakshya 11:1fdb94ae6563 67 *output = (float) input[1];
lakshya 11:1fdb94ae6563 68 *output = *output/100.0; //input[0] integer part
lakshya 11:1fdb94ae6563 69 *output = *output + (float) input[0]; //input[1] decimal part correct to two decimal places
lakshya 11:1fdb94ae6563 70 }
lakshya 11:1fdb94ae6563 71
lakshya 10:f93407b97750 72 float angle(float x,float y,float z)
lakshya 10:f93407b97750 73 {
lakshya 10:f93407b97750 74 float mag_total=sqrt(x*x + y*y + z*z);
lakshya 10:f93407b97750 75 float cos_z = z/mag_total;
lakshya 10:f93407b97750 76 float theta_z = acosf(cos_z);
Bragadeesh153 13:fb7facaf308b 77
lakshya 10:f93407b97750 78 return theta_z;
lakshya 10:f93407b97750 79 //printf("/n cos_zz= %f /t theta_z= %f /n",cos_z,theta_z);
lakshya 10:f93407b97750 80 }
lakshya 10:f93407b97750 81
Bragadeesh153 13:fb7facaf308b 82 //uint8_t tm1[134];
Bragadeesh153 13:fb7facaf308b 83
Bragadeesh153 13:fb7facaf308b 84 void FCTN_BAE_TM_TC (uint8_t* tc)
sakthipriya 0:7b4c00e3912f 85
sakthipriya 3:07e15677a75c 86 {
Bragadeesh153 13:fb7facaf308b 87 // tm1[0] = 1;
sakthipriya 3:07e15677a75c 88 uint8_t service_type=(tc[2]&0xF0);
Bragadeesh153 13:fb7facaf308b 89
sakthipriya 3:07e15677a75c 90 uint16_t crc16;
Bragadeesh153 13:fb7facaf308b 91
Bragadeesh153 13:fb7facaf308b 92
sakthipriya 3:07e15677a75c 93 switch(service_type)
sakthipriya 3:07e15677a75c 94 {
sakthipriya 3:07e15677a75c 95 case 0x60:
sakthipriya 3:07e15677a75c 96 {
sakthipriya 3:07e15677a75c 97 printf("Memory Management Service\r\n");
sakthipriya 3:07e15677a75c 98 uint8_t service_subtype=(tc[2]&0x0F);
Bragadeesh153 13:fb7facaf308b 99
sakthipriya 3:07e15677a75c 100 switch(service_subtype)
sakthipriya 3:07e15677a75c 101 {
sakthipriya 3:07e15677a75c 102 case 0x01:
sakthipriya 3:07e15677a75c 103 {
sakthipriya 3:07e15677a75c 104 printf("Read from Flash\r\n");
Bragadeesh153 13:fb7facaf308b 105 break;
sakthipriya 3:07e15677a75c 106 }
sakthipriya 3:07e15677a75c 107 case 0x02:
sakthipriya 3:07e15677a75c 108 {
Bragadeesh153 13:fb7facaf308b 109 uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
Bragadeesh153 13:fb7facaf308b 110 switch(MID)
Bragadeesh153 13:fb7facaf308b 111 {
Bragadeesh153 13:fb7facaf308b 112
lakshya 10:f93407b97750 113 case 0x0001:
Bragadeesh153 13:fb7facaf308b 114 {
Bragadeesh153 13:fb7facaf308b 115 printf("Read from RAM\r\n");
Bragadeesh153 13:fb7facaf308b 116
Bragadeesh153 13:fb7facaf308b 117 /*taking some varible till we find some thing more useful*/
Bragadeesh153 13:fb7facaf308b 118 //uint8_t ref_val=0x01;
Bragadeesh153 13:fb7facaf308b 119 telemetry[0] = 1;
Bragadeesh153 13:fb7facaf308b 120 telemetry[1] = tc[0];
Bragadeesh153 13:fb7facaf308b 121 telemetry[2] = ACK_CODE;
Bragadeesh153 13:fb7facaf308b 122 /*random or with bcn_tx_sw_enable assuming it is 1 bit in length
Bragadeesh153 13:fb7facaf308b 123 how to get that we dont know, now we just assume it to be so*/
Bragadeesh153 13:fb7facaf308b 124 telemetry[3] = (BCN_SW);
Bragadeesh153 13:fb7facaf308b 125 telemetry[3] = telemetry[3]|(TRXY_SW<<1);
Bragadeesh153 13:fb7facaf308b 126 telemetry[3] = telemetry[3]|(TRZ_SW<<2);
Bragadeesh153 13:fb7facaf308b 127 telemetry[3] = telemetry[3]|(ATS1_SW_ENABLE<<3);
Bragadeesh153 13:fb7facaf308b 128 telemetry[3] = telemetry[3]|(ATS2_SW_ENABLE<<4);
Bragadeesh153 13:fb7facaf308b 129
Bragadeesh153 13:fb7facaf308b 130 if(BCN_TX_STATUS==2)
Bragadeesh153 13:fb7facaf308b 131 telemetry[3] = telemetry[3]|0x20;
Bragadeesh153 13:fb7facaf308b 132 else
Bragadeesh153 13:fb7facaf308b 133 telemetry[3] = telemetry[3] & 0xDF;
Bragadeesh153 13:fb7facaf308b 134
Bragadeesh153 13:fb7facaf308b 135 telemetry[3] = telemetry[3]|(BCN_FEN<<6);
Bragadeesh153 13:fb7facaf308b 136 uint8_t mask_val=BAE_ENABLE & 0x00000008;
Bragadeesh153 13:fb7facaf308b 137 /*can be a problem see if any error occurs*/
Bragadeesh153 13:fb7facaf308b 138 telemetry[3] = telemetry[3]|(mask_val<<7);
Bragadeesh153 13:fb7facaf308b 139
Bragadeesh153 13:fb7facaf308b 140 /*not included in the code yet*/
Bragadeesh153 13:fb7facaf308b 141 telemetry[4] = BAE_RESET_COUNTER;
Bragadeesh153 13:fb7facaf308b 142 telemetry[5] = ACS_STATE;
Bragadeesh153 13:fb7facaf308b 143 telemetry[5] = telemetry[5]|(EN_BTRY_HT<<3);
Bragadeesh153 13:fb7facaf308b 144 telemetry[5] = telemetry[5]|(phase_TR_x<<4);
Bragadeesh153 13:fb7facaf308b 145 telemetry[5] = telemetry[5]|(phase_TR_y<<5);
Bragadeesh153 13:fb7facaf308b 146 telemetry[5] = telemetry[5]|(phase_TR_z<<6);
Bragadeesh153 13:fb7facaf308b 147 /*spare to be fixed*/
Bragadeesh153 13:fb7facaf308b 148 //telemetry[5] = telemetry[5]|(Spare))<<7);
Bragadeesh153 13:fb7facaf308b 149 /**/
Bragadeesh153 13:fb7facaf308b 150 uint8_t soc_powerlevel_2=50;
Bragadeesh153 13:fb7facaf308b 151 uint8_t soc_powerlevel_3=65;
Bragadeesh153 13:fb7facaf308b 152
Bragadeesh153 13:fb7facaf308b 153 telemetry[6] = soc_powerlevel_2;
Bragadeesh153 13:fb7facaf308b 154 telemetry[7] = soc_powerlevel_3;
lakshya 10:f93407b97750 155
Bragadeesh153 13:fb7facaf308b 156 /*to be fixed*/
Bragadeesh153 13:fb7facaf308b 157 telemetry[8] = 0;
Bragadeesh153 13:fb7facaf308b 158 telemetry[9] = 0;
Bragadeesh153 13:fb7facaf308b 159 telemetry[10] = 0;
Bragadeesh153 13:fb7facaf308b 160 telemetry[11] = 0;
Bragadeesh153 13:fb7facaf308b 161 //telemetry[8] = Torque Rod X Offset;
Bragadeesh153 13:fb7facaf308b 162 //telemetry[9] = Torque Rod Y Offset;
Bragadeesh153 13:fb7facaf308b 163 //telemetry[10] = Torque Rod Z Offset;
Bragadeesh153 13:fb7facaf308b 164 //telemetry[11] = ACS_DEMAG_TIME_DELAY;
Bragadeesh153 13:fb7facaf308b 165 telemetry[12] = (BAE_STATUS>>24) & 0xFF;
Bragadeesh153 13:fb7facaf308b 166 telemetry[13] = (BAE_STATUS>>16) & 0xFF;
Bragadeesh153 13:fb7facaf308b 167 telemetry[14] = (BAE_STATUS>>8) & 0xFF;
Bragadeesh153 13:fb7facaf308b 168 telemetry[15] = BAE_STATUS & 0xFF;
Bragadeesh153 13:fb7facaf308b 169
Bragadeesh153 13:fb7facaf308b 170 /*to be fixed*/
lakshya 14:a9588f443f1a 171 //telemetry[16] = BCN_FAIL_COUNT;
Bragadeesh153 13:fb7facaf308b 172 telemetry[17] = actual_data.power_mode;
Bragadeesh153 13:fb7facaf308b 173 /*to be fixed*/
Bragadeesh153 13:fb7facaf308b 174 uint16_t P_BAE_I2CRX_COUNTER=0;
Bragadeesh153 13:fb7facaf308b 175 uint16_t P_ACS_MAIN_COUNTER=0;
Bragadeesh153 13:fb7facaf308b 176 uint16_t P_BCN_TX_MAIN_COUNTER=0;
Bragadeesh153 13:fb7facaf308b 177 uint16_t P_EPS_MAIN_COUNTER=0;
Bragadeesh153 13:fb7facaf308b 178
Bragadeesh153 13:fb7facaf308b 179 telemetry[18] = P_BAE_I2CRX_COUNTER>>8;
Bragadeesh153 13:fb7facaf308b 180 telemetry[19] = P_BAE_I2CRX_COUNTER;
Bragadeesh153 13:fb7facaf308b 181 telemetry[20] = P_ACS_MAIN_COUNTER>>8;
Bragadeesh153 13:fb7facaf308b 182 telemetry[21] = P_ACS_MAIN_COUNTER;
Bragadeesh153 13:fb7facaf308b 183 telemetry[22] = P_BCN_TX_MAIN_COUNTER>>8;
Bragadeesh153 13:fb7facaf308b 184 telemetry[23] = P_BCN_TX_MAIN_COUNTER;
Bragadeesh153 13:fb7facaf308b 185 telemetry[24] = P_EPS_MAIN_COUNTER>>8;
Bragadeesh153 13:fb7facaf308b 186 telemetry[25] = P_EPS_MAIN_COUNTER;
Bragadeesh153 13:fb7facaf308b 187
Bragadeesh153 13:fb7facaf308b 188 for(int i=0; i<3; i++)
Bragadeesh153 13:fb7facaf308b 189 FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&telemetry[26+ (i*4)]);
Bragadeesh153 13:fb7facaf308b 190 for(int i=0; i<3; i++)
Bragadeesh153 13:fb7facaf308b 191 FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&telemetry[38+(i*4)]);
Bragadeesh153 13:fb7facaf308b 192
Bragadeesh153 13:fb7facaf308b 193 //FAULT_FLAG();
Bragadeesh153 13:fb7facaf308b 194 telemetry[50] = actual_data.faultIr_status;
Bragadeesh153 13:fb7facaf308b 195 telemetry[51] = actual_data.faultPoll_status;
Bragadeesh153 13:fb7facaf308b 196 //Bdot Rotation Speed of Command telemetry[52-53]
Bragadeesh153 13:fb7facaf308b 197 //Bdot Output Current telemetry[54]
Bragadeesh153 13:fb7facaf308b 198 //float l_pmw1 = (PWM1);
Bragadeesh153 13:fb7facaf308b 199 //float l_pmw2 = PWM2;
Bragadeesh153 13:fb7facaf308b 200 //float l_pmw3 = PWM3;
Bragadeesh153 13:fb7facaf308b 201
lakshya 11:1fdb94ae6563 202 /*__________________________________________________________________*/
Bragadeesh153 13:fb7facaf308b 203
lakshya 11:1fdb94ae6563 204 /*change and check if changing it to PWM1 causes problem*/
Bragadeesh153 13:fb7facaf308b 205
lakshya 11:1fdb94ae6563 206 /*___________________________________________________________________*/
Bragadeesh153 13:fb7facaf308b 207
lakshya 11:1fdb94ae6563 208 float PWM_measured[3];
Bragadeesh153 13:fb7facaf308b 209
Bragadeesh153 13:fb7facaf308b 210 PWM_measured[0] = PWM1.read();
Bragadeesh153 13:fb7facaf308b 211 PWM_measured[1] = PWM2.read();
Bragadeesh153 13:fb7facaf308b 212 PWM_measured[2] = PWM3.read();
Bragadeesh153 13:fb7facaf308b 213
Bragadeesh153 13:fb7facaf308b 214 FCTN_CONVERT_FLOAT(PWM_measured[0], &telemetry[55]);
Bragadeesh153 13:fb7facaf308b 215 FCTN_CONVERT_FLOAT(PWM_measured[1], &telemetry[59]);
Bragadeesh153 13:fb7facaf308b 216 FCTN_CONVERT_FLOAT(PWM_measured[2], &telemetry[63]);
lakshya 10:f93407b97750 217 float attitude_ang = angle(actual_data.Bvalue_actual[0],actual_data.Bvalue_actual[1],actual_data.Bvalue_actual[2]);
Bragadeesh153 13:fb7facaf308b 218 FCTN_CONVERT_FLOAT(attitude_ang, &telemetry[67]);
lakshya 10:f93407b97750 219
lakshya 10:f93407b97750 220 for (int i=0; i<16; i++)
Bragadeesh153 13:fb7facaf308b 221 telemetry[68+i] = quant_data.voltage_quant[i];
lakshya 10:f93407b97750 222 for (int i=0; i<12; i++)
Bragadeesh153 13:fb7facaf308b 223 telemetry[84+i] = quant_data.current_quant[i];
Bragadeesh153 13:fb7facaf308b 224 //telemetry[96]
Bragadeesh153 13:fb7facaf308b 225 //telemetry[97]
Bragadeesh153 13:fb7facaf308b 226 //telemetry[98]
Bragadeesh153 13:fb7facaf308b 227 //telemetry[99]
Bragadeesh153 13:fb7facaf308b 228 telemetry[100] = quant_data.Batt_voltage_quant;
Bragadeesh153 13:fb7facaf308b 229 telemetry[101] = quant_data.BAE_temp_quant;
Bragadeesh153 13:fb7facaf308b 230 telemetry[102] = quant_data.Batt_gauge_quant[1];
Bragadeesh153 13:fb7facaf308b 231 telemetry[103] = quant_data.Batt_temp_quant[0];
Bragadeesh153 13:fb7facaf308b 232 telemetry[104] = quant_data.Batt_temp_quant[1];
Bragadeesh153 13:fb7facaf308b 233
Bragadeesh153 13:fb7facaf308b 234 //telemetry[105] = beacon temperature;
Bragadeesh153 13:fb7facaf308b 235
lakshya 10:f93407b97750 236 for (int i=105; i<132;i++)
sakthipriya 8:82250e41da81 237 {
Bragadeesh153 13:fb7facaf308b 238 telemetry[i] = 0x00;
sakthipriya 5:bb592f3185cc 239 }
Bragadeesh153 13:fb7facaf308b 240 crc16 = CRC::crc16_gen(telemetry,132);
Bragadeesh153 13:fb7facaf308b 241 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 242 telemetry[133] = (uint8_t)(crc16&0x00FF);
Bragadeesh153 13:fb7facaf308b 243
Bragadeesh153 13:fb7facaf308b 244 break;
Bragadeesh153 13:fb7facaf308b 245 }
Bragadeesh153 13:fb7facaf308b 246 case 0x0002:
Bragadeesh153 13:fb7facaf308b 247 {
Bragadeesh153 13:fb7facaf308b 248 telemetry[0] = 0x60;
Bragadeesh153 13:fb7facaf308b 249 telemetry[1] = tc[0];
Bragadeesh153 13:fb7facaf308b 250 telemetry[2] = ACK_CODE;
Bragadeesh153 13:fb7facaf308b 251
Bragadeesh153 13:fb7facaf308b 252 for(int i;i<16;i++)
Bragadeesh153 13:fb7facaf308b 253 telemetry[i+3] = bae_HK_minmax.voltage_max[i];
Bragadeesh153 13:fb7facaf308b 254
Bragadeesh153 13:fb7facaf308b 255 for(int i;i<12;i++)
Bragadeesh153 13:fb7facaf308b 256 telemetry[i+18] = bae_HK_minmax.current_max[i];
Bragadeesh153 13:fb7facaf308b 257
Bragadeesh153 13:fb7facaf308b 258 telemetry[29] = bae_HK_minmax.Batt_voltage_max;;
Bragadeesh153 13:fb7facaf308b 259 telemetry[30] = bae_HK_minmax.BAE_temp_max;
Bragadeesh153 13:fb7facaf308b 260
Bragadeesh153 13:fb7facaf308b 261 /*battery soc*/
Bragadeesh153 13:fb7facaf308b 262 //telemetry[31] = BAE_HK_min_max bae_HK_minmax.voltage_max;
Bragadeesh153 13:fb7facaf308b 263
Bragadeesh153 13:fb7facaf308b 264 telemetry[32] = bae_HK_minmax.Batt_temp_max[0];
Bragadeesh153 13:fb7facaf308b 265 telemetry[33] = bae_HK_minmax.Batt_temp_max[1];
Bragadeesh153 13:fb7facaf308b 266
Bragadeesh153 13:fb7facaf308b 267 /*BCN temp not there*/
Bragadeesh153 13:fb7facaf308b 268 //telemetry[34] = BAE_HK_min_max bae_HK_minmax.;
Bragadeesh153 13:fb7facaf308b 269
Bragadeesh153 13:fb7facaf308b 270 for(int i=0; i<3; i++)
Bragadeesh153 13:fb7facaf308b 271 FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[i],&telemetry[35+(i*4)]);
Bragadeesh153 13:fb7facaf308b 272
Bragadeesh153 13:fb7facaf308b 273 for(int i=0; i<3; i++)
Bragadeesh153 13:fb7facaf308b 274 FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[i],&telemetry[47+(i*4)]);
Bragadeesh153 13:fb7facaf308b 275
Bragadeesh153 13:fb7facaf308b 276 /*min data*/
Bragadeesh153 13:fb7facaf308b 277
Bragadeesh153 13:fb7facaf308b 278 for(int i;i<16;i++)
Bragadeesh153 13:fb7facaf308b 279 telemetry[i+59] = bae_HK_minmax.voltage_min[i];
Bragadeesh153 13:fb7facaf308b 280
Bragadeesh153 13:fb7facaf308b 281 for(int i;i<12;i++)
Bragadeesh153 13:fb7facaf308b 282 telemetry[i+74] = bae_HK_minmax.current_min[i];
Bragadeesh153 13:fb7facaf308b 283
Bragadeesh153 13:fb7facaf308b 284 telemetry[86] = bae_HK_minmax.Batt_voltage_min;
Bragadeesh153 13:fb7facaf308b 285 telemetry[87] = bae_HK_minmax.BAE_temp_min;
Bragadeesh153 13:fb7facaf308b 286
Bragadeesh153 13:fb7facaf308b 287 /*battery soc*/
Bragadeesh153 13:fb7facaf308b 288 //telemetry[88] = BAE_HK_min_max bae_HK_minmax.voltage_max;
Bragadeesh153 13:fb7facaf308b 289
Bragadeesh153 13:fb7facaf308b 290 telemetry[89] = bae_HK_minmax.Batt_temp_min[0];
Bragadeesh153 13:fb7facaf308b 291 telemetry[90] = bae_HK_minmax.Batt_temp_min[1];
Bragadeesh153 13:fb7facaf308b 292 //huhu//
Bragadeesh153 13:fb7facaf308b 293
Bragadeesh153 13:fb7facaf308b 294 /*BCN temp not there*/
Bragadeesh153 13:fb7facaf308b 295 //telemetry[91] = BAE_HK_min_max bae_HK_minmax.;
Bragadeesh153 13:fb7facaf308b 296
Bragadeesh153 13:fb7facaf308b 297 for(int i=0; i<3; i++)
Bragadeesh153 13:fb7facaf308b 298 FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[i],&telemetry[91+(i*4)]);
Bragadeesh153 13:fb7facaf308b 299
Bragadeesh153 13:fb7facaf308b 300 for(int i=0; i<3; i++)
Bragadeesh153 13:fb7facaf308b 301 FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[i],&telemetry[103+(i*4)]);
Bragadeesh153 13:fb7facaf308b 302
Bragadeesh153 13:fb7facaf308b 303
Bragadeesh153 13:fb7facaf308b 304 for (int i=115; i<132;i++)
Bragadeesh153 13:fb7facaf308b 305 {
Bragadeesh153 13:fb7facaf308b 306 telemetry[i] = 0x00;
Bragadeesh153 13:fb7facaf308b 307 }
Bragadeesh153 13:fb7facaf308b 308 crc16 = CRC::crc16_gen(telemetry,132);
Bragadeesh153 13:fb7facaf308b 309 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 310 telemetry[133] = (uint8_t)(crc16&0x00FF);
Bragadeesh153 13:fb7facaf308b 311 break;
Bragadeesh153 13:fb7facaf308b 312 }
Bragadeesh153 13:fb7facaf308b 313 }
Bragadeesh153 13:fb7facaf308b 314 break;
Bragadeesh153 13:fb7facaf308b 315 }
sakthipriya 3:07e15677a75c 316 case 0x05:
sakthipriya 3:07e15677a75c 317 {
sakthipriya 3:07e15677a75c 318 printf("Write on Flash\r\n");
Bragadeesh153 13:fb7facaf308b 319 break;
sakthipriya 3:07e15677a75c 320 }
sakthipriya 3:07e15677a75c 321 default:
sakthipriya 3:07e15677a75c 322 {
sakthipriya 3:07e15677a75c 323 printf("Invalid TC");
Bragadeesh153 13:fb7facaf308b 324 //ACK_L234_telemetry
Bragadeesh153 13:fb7facaf308b 325 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 326 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 327 telemetry[2]=ACK_CODE;
sakthipriya 3:07e15677a75c 328 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 329 {
Bragadeesh153 13:fb7facaf308b 330 telemetry[i]=0x00;
sakthipriya 0:7b4c00e3912f 331 }
Bragadeesh153 13:fb7facaf308b 332 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 333 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 334 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 335 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 336 {
Bragadeesh153 13:fb7facaf308b 337 telemetry[i]=0x00;
sakthipriya 0:7b4c00e3912f 338 }
Bragadeesh153 13:fb7facaf308b 339 break;
sakthipriya 3:07e15677a75c 340 }
sakthipriya 3:07e15677a75c 341 }
Bragadeesh153 13:fb7facaf308b 342 break;
sakthipriya 0:7b4c00e3912f 343 }
sakthipriya 3:07e15677a75c 344 case 0x80:
sakthipriya 3:07e15677a75c 345 {
Bragadeesh153 13:fb7facaf308b 346 //printf("Function Management Service\r\n");
sakthipriya 3:07e15677a75c 347 uint8_t service_subtype=(tc[2]&0x0F);
sakthipriya 3:07e15677a75c 348 switch(service_subtype)
sakthipriya 3:07e15677a75c 349 {
sakthipriya 3:07e15677a75c 350 case 0x01:
sakthipriya 3:07e15677a75c 351 {
sakthipriya 3:07e15677a75c 352 printf("FMS Activated\r\n");
sakthipriya 3:07e15677a75c 353 uint8_t pid=tc[3];
sakthipriya 3:07e15677a75c 354 switch(pid)
sakthipriya 3:07e15677a75c 355 {
sakthipriya 5:bb592f3185cc 356 case 0xE0:
sakthipriya 3:07e15677a75c 357 {
lakshya 10:f93407b97750 358 float B[3],W[3];
sakthipriya 5:bb592f3185cc 359 printf("ACS_COMSN\r\n");
Bragadeesh153 13:fb7facaf308b 360 //ACK_L234_telemetry
Bragadeesh153 13:fb7facaf308b 361
Bragadeesh153 13:fb7facaf308b 362
Bragadeesh153 15:3239c6391ffa 363 B[0]=(float)tc[4];
Bragadeesh153 15:3239c6391ffa 364 B[1]=(float)tc[5];
Bragadeesh153 15:3239c6391ffa 365 B[2] = 300; //constant value
Bragadeesh153 15:3239c6391ffa 366
Bragadeesh153 15:3239c6391ffa 367 W[0]=(float)tc[6];
Bragadeesh153 15:3239c6391ffa 368 W[1]=(float)tc[7];
Bragadeesh153 15:3239c6391ffa 369 W[2] = 300; //constant value
Bragadeesh153 13:fb7facaf308b 370
Bragadeesh153 13:fb7facaf308b 371 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 372 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 373 telemetry[2]=ACK_CODE;
lakshya 10:f93407b97750 374 //FCTN_ATS_DATA_ACQ(); //get data
Bragadeesh153 13:fb7facaf308b 375 printf("gyro values\n\r");
Bragadeesh153 13:fb7facaf308b 376 for(int i=0; i<3; i++)
lakshya 10:f93407b97750 377 printf("%f\n\r",W[i]);
sakthipriya 5:bb592f3185cc 378 printf("mag values\n\r");
Bragadeesh153 13:fb7facaf308b 379 for(int i=0; i<3; i++)
lakshya 10:f93407b97750 380 printf("%f\n\r",B[i]);
Bragadeesh153 13:fb7facaf308b 381 /* FCTN_CONVERT_FLOAT(data[0],&telemetry[4]); //telemetry[4] - telemetry[7]
Bragadeesh153 13:fb7facaf308b 382 FCTN_CONVERT_FLOAT(data[1],&telemetry[8]); //telemetry[8] - telemetry[11]
Bragadeesh153 13:fb7facaf308b 383 FCTN_CONVERT_FLOAT(data[2],&telemetry[12]); //telemetry[12] - telemetry[15]
Bragadeesh153 13:fb7facaf308b 384 FCTN_CONVERT_FLOAT(data[0],&telemetry[16]); //telemetry[16] - telemetry[19]
Bragadeesh153 13:fb7facaf308b 385 FCTN_CONVERT_FLOAT(data[1],&telemetry[20]); //telemetry[20] - telemetry[23]
Bragadeesh153 13:fb7facaf308b 386 FCTN_CONVERT_FLOAT(data[2],&telemetry[24]); //telemetry[24] - telemetry[27]
sakthipriya 5:bb592f3185cc 387 if((data[0]<8) && (data[1]<8) && (data[2] <8))
Bragadeesh153 13:fb7facaf308b 388 telemetry[28] = 1; // gyro values in correct range
sakthipriya 5:bb592f3185cc 389 else
Bragadeesh153 13:fb7facaf308b 390 telemetry[28] = 0;
sakthipriya 5:bb592f3185cc 391 if ((data[3] > 20 ) && (data[4] >20) && (data[5]>20)&& (data[3] < 50 ) && (data[4] <50) && (data[5]<50))
Bragadeesh153 13:fb7facaf308b 392 telemetry[29] = 1; // mag values in correct range
sakthipriya 5:bb592f3185cc 393 else
Bragadeesh153 13:fb7facaf308b 394 telemetry[29] = 0;
Bragadeesh153 13:fb7facaf308b 395 */
Bragadeesh153 13:fb7facaf308b 396 // float B[3],W[3];
lakshya 10:f93407b97750 397 // B[0] = B0;
Bragadeesh153 13:fb7facaf308b 398 // B[1] = B1;
lakshya 10:f93407b97750 399 // B[2] = B2;
lakshya 10:f93407b97750 400 // W[0] = W0;
Bragadeesh153 13:fb7facaf308b 401 // W[1] = W1;
Bragadeesh153 13:fb7facaf308b 402 // W[2] = W2;
sakthipriya 5:bb592f3185cc 403 // Control algo commissioning
lakshya 10:f93407b97750 404 /* FCTN_ACS_CNTRLALGO(B,W);
Bragadeesh153 13:fb7facaf308b 405 FCTN_CONVERT_FLOAT(moment[0],&telemetry[30]); //telemetry[30] - telemetry[33]
Bragadeesh153 13:fb7facaf308b 406 FCTN_CONVERT_FLOAT(moment[1],&telemetry[34]); //telemetry[34] - telemetry[37]
Bragadeesh153 13:fb7facaf308b 407 FCTN_CONVERT_FLOAT(moment[2],&telemetry[38]); //telemetry[38] - telemetry[41]
sakthipriya 5:bb592f3185cc 408 // to include commission TR as well
sakthipriya 5:bb592f3185cc 409 for(uint8_t i=42;i<132;i++)
sakthipriya 3:07e15677a75c 410 {
Bragadeesh153 13:fb7facaf308b 411 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 412 }
Bragadeesh153 13:fb7facaf308b 413
Bragadeesh153 13:fb7facaf308b 414 crc16 = CRC::crc16_gen(telemetry,132);
Bragadeesh153 13:fb7facaf308b 415 telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 416 telemetry[134] = (uint8_t)(crc16&0x00FF);
Bragadeesh153 13:fb7facaf308b 417 break;
Bragadeesh153 13:fb7facaf308b 418 */
Bragadeesh153 13:fb7facaf308b 419
lakshya 10:f93407b97750 420 // Control algo commissioning
lakshya 10:f93407b97750 421 FCTN_ACS_CNTRLALGO(B,W);
Bragadeesh153 13:fb7facaf308b 422 FCTN_CONVERT_FLOAT(moment[0],&telemetry[4]); //telemetry[4] - telemetry[7]
Bragadeesh153 13:fb7facaf308b 423 FCTN_CONVERT_FLOAT(moment[1],&telemetry[8]); //telemetry[8] - telemetry[11]
Bragadeesh153 13:fb7facaf308b 424 FCTN_CONVERT_FLOAT(moment[2],&telemetry[12]); //telemetry[12] - telemetry[15]
lakshya 10:f93407b97750 425 // to include commission TR as well
lakshya 10:f93407b97750 426 for(uint8_t i=16;i<132;i++)
lakshya 10:f93407b97750 427 {
Bragadeesh153 13:fb7facaf308b 428 telemetry[i]=0x00;
lakshya 10:f93407b97750 429 }
Bragadeesh153 13:fb7facaf308b 430
Bragadeesh153 13:fb7facaf308b 431 crc16 = CRC::crc16_gen(telemetry,132);
Bragadeesh153 13:fb7facaf308b 432 telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 433 telemetry[134] = (uint8_t)(crc16&0x00FF);
Bragadeesh153 13:fb7facaf308b 434 break;
sakthipriya 3:07e15677a75c 435 }
lakshya 10:f93407b97750 436 case 0xE1:
sakthipriya 3:07e15677a75c 437 {
Bragadeesh153 15:3239c6391ffa 438
lakshya 10:f93407b97750 439 printf("HARDWARE_COMSN\r\n");
Bragadeesh153 13:fb7facaf308b 440 //ACK_L234_telemetry
Bragadeesh153 13:fb7facaf308b 441
Bragadeesh153 15:3239c6391ffa 442
Bragadeesh153 15:3239c6391ffa 443 TRXY_SW = 1;
Bragadeesh153 15:3239c6391ffa 444 TRZ_SW = 1;
Bragadeesh153 15:3239c6391ffa 445
Bragadeesh153 15:3239c6391ffa 446 PWM1 = 0;
Bragadeesh153 15:3239c6391ffa 447 PWM2 = 0;
Bragadeesh153 15:3239c6391ffa 448 PWM3 = 0;
Bragadeesh153 15:3239c6391ffa 449
Bragadeesh153 15:3239c6391ffa 450 wait_ms(60); //Demagnetising time delay for torquerod
Bragadeesh153 13:fb7facaf308b 451
Bragadeesh153 13:fb7facaf308b 452 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 453 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 454 telemetry[2]=ACK_CODE;
lakshya 10:f93407b97750 455
Bragadeesh153 15:3239c6391ffa 456 float PWM_tc[3];
Bragadeesh153 15:3239c6391ffa 457
Bragadeesh153 15:3239c6391ffa 458 PWM_tc[0] = (float) tc[4];
Bragadeesh153 15:3239c6391ffa 459 PWM_tc[1] = (float) tc[5];
Bragadeesh153 15:3239c6391ffa 460 PWM_tc[2] = (float) tc[6];
Bragadeesh153 15:3239c6391ffa 461
Bragadeesh153 15:3239c6391ffa 462 ATS2_SW_ENABLE = 1;
Bragadeesh153 15:3239c6391ffa 463 ATS1_SW_ENABLE = 0; // making sure we switch off the other
Bragadeesh153 15:3239c6391ffa 464
Bragadeesh153 15:3239c6391ffa 465
Bragadeesh153 15:3239c6391ffa 466 FCTN_ATS_DATA_ACQ();
Bragadeesh153 15:3239c6391ffa 467
Bragadeesh153 15:3239c6391ffa 468
Bragadeesh153 15:3239c6391ffa 469 FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0], &telemetry[6]);
Bragadeesh153 15:3239c6391ffa 470 FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1], &telemetry[10]);
Bragadeesh153 15:3239c6391ffa 471 FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2], &telemetry[14]);
Bragadeesh153 15:3239c6391ffa 472
Bragadeesh153 15:3239c6391ffa 473 FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[18]);
Bragadeesh153 15:3239c6391ffa 474 FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[22]);
Bragadeesh153 15:3239c6391ffa 475 FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[26]);
Bragadeesh153 15:3239c6391ffa 476
Bragadeesh153 15:3239c6391ffa 477 ACS_ATS_STATUS = ACS_ATS_STATUS & 0xF0;
Bragadeesh153 15:3239c6391ffa 478 ACS_ATS_STATUS = ACS_ATS_STATUS | 0x00;
Bragadeesh153 15:3239c6391ffa 479
Bragadeesh153 15:3239c6391ffa 480
Bragadeesh153 15:3239c6391ffa 481 ATS1_SW_ENABLE = 1;
Bragadeesh153 15:3239c6391ffa 482 ATS2_SW_ENABLE = 0; // making sure we switch off the other
Bragadeesh153 15:3239c6391ffa 483
Bragadeesh153 15:3239c6391ffa 484
Bragadeesh153 15:3239c6391ffa 485 FCTN_ATS_DATA_ACQ();
Bragadeesh153 15:3239c6391ffa 486
Bragadeesh153 15:3239c6391ffa 487
Bragadeesh153 15:3239c6391ffa 488 FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0], &telemetry[30]);
Bragadeesh153 15:3239c6391ffa 489 FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1], &telemetry[34]);
Bragadeesh153 15:3239c6391ffa 490 FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2], &telemetry[38]);
Bragadeesh153 15:3239c6391ffa 491
Bragadeesh153 15:3239c6391ffa 492 FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[42]);
Bragadeesh153 15:3239c6391ffa 493 FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[46]);
Bragadeesh153 15:3239c6391ffa 494 FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[50]);
Bragadeesh153 15:3239c6391ffa 495
Bragadeesh153 15:3239c6391ffa 496 ACS_ATS_STATUS = ACS_ATS_STATUS & 0x0F;
Bragadeesh153 15:3239c6391ffa 497 ACS_ATS_STATUS = ACS_ATS_STATUS | 0x00;
Bragadeesh153 15:3239c6391ffa 498
Bragadeesh153 15:3239c6391ffa 499 ACS_ATS_STATUS = telemetry[4];
Bragadeesh153 15:3239c6391ffa 500
Bragadeesh153 15:3239c6391ffa 501 PWM1 = PWM_tc[0];
Bragadeesh153 15:3239c6391ffa 502 wait_ms(60);
Bragadeesh153 15:3239c6391ffa 503 FCTN_ATS_DATA_ACQ();
Bragadeesh153 15:3239c6391ffa 504
Bragadeesh153 15:3239c6391ffa 505 actual_data.current_actual[5]=CurrentInput.read();
Bragadeesh153 15:3239c6391ffa 506 actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
Bragadeesh153 15:3239c6391ffa 507 int resistance;
Bragadeesh153 15:3239c6391ffa 508
Bragadeesh153 15:3239c6391ffa 509 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
Bragadeesh153 15:3239c6391ffa 510 if(actual_data.current_actual[5]>1.47)
Bragadeesh153 15:3239c6391ffa 511 {
Bragadeesh153 15:3239c6391ffa 512 actual_data.current_actual[5]=3694/log(24.032242*resistance);
Bragadeesh153 15:3239c6391ffa 513 }
Bragadeesh153 15:3239c6391ffa 514 else{
Bragadeesh153 15:3239c6391ffa 515
Bragadeesh153 15:3239c6391ffa 516 actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
Bragadeesh153 15:3239c6391ffa 517 }
Bragadeesh153 15:3239c6391ffa 518
Bragadeesh153 15:3239c6391ffa 519 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.current_actual[5], &telemetry[54]);
Bragadeesh153 15:3239c6391ffa 520
Bragadeesh153 15:3239c6391ffa 521 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[56]);
Bragadeesh153 15:3239c6391ffa 522 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[58]);
Bragadeesh153 15:3239c6391ffa 523 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[60]);
Bragadeesh153 15:3239c6391ffa 524
Bragadeesh153 15:3239c6391ffa 525 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[62]);
Bragadeesh153 15:3239c6391ffa 526 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[64]);
Bragadeesh153 15:3239c6391ffa 527 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[66]);
Bragadeesh153 15:3239c6391ffa 528
Bragadeesh153 15:3239c6391ffa 529
Bragadeesh153 15:3239c6391ffa 530
Bragadeesh153 15:3239c6391ffa 531 PWM1 = 0;
lakshya 10:f93407b97750 532
lakshya 10:f93407b97750 533
Bragadeesh153 15:3239c6391ffa 534 PWM2 = PWM_tc[1];
Bragadeesh153 15:3239c6391ffa 535 wait_ms(60);
Bragadeesh153 15:3239c6391ffa 536 FCTN_ATS_DATA_ACQ();
Bragadeesh153 13:fb7facaf308b 537
lakshya 10:f93407b97750 538
Bragadeesh153 15:3239c6391ffa 539 actual_data.current_actual[5]=CurrentInput.read();
Bragadeesh153 15:3239c6391ffa 540 actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
Bragadeesh153 15:3239c6391ffa 541
Bragadeesh153 15:3239c6391ffa 542 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
Bragadeesh153 15:3239c6391ffa 543 if(actual_data.current_actual[5]>1.47)
Bragadeesh153 15:3239c6391ffa 544 {
Bragadeesh153 15:3239c6391ffa 545 actual_data.current_actual[5]=3694/log(24.032242*resistance);
Bragadeesh153 15:3239c6391ffa 546 }
Bragadeesh153 15:3239c6391ffa 547 else{
Bragadeesh153 15:3239c6391ffa 548
Bragadeesh153 15:3239c6391ffa 549 actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
Bragadeesh153 15:3239c6391ffa 550 }
Bragadeesh153 15:3239c6391ffa 551
Bragadeesh153 15:3239c6391ffa 552 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.current_actual[5], &telemetry[68]);
Bragadeesh153 15:3239c6391ffa 553
Bragadeesh153 15:3239c6391ffa 554 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[70]);
Bragadeesh153 15:3239c6391ffa 555 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[72]);
Bragadeesh153 15:3239c6391ffa 556 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[74]);
Bragadeesh153 15:3239c6391ffa 557
Bragadeesh153 15:3239c6391ffa 558 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[76]);
Bragadeesh153 15:3239c6391ffa 559 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[78]);
Bragadeesh153 15:3239c6391ffa 560 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[80]);
Bragadeesh153 15:3239c6391ffa 561
Bragadeesh153 15:3239c6391ffa 562 PWM2 = 0;
Bragadeesh153 15:3239c6391ffa 563
Bragadeesh153 15:3239c6391ffa 564 PWM3 = PWM_tc[2];
Bragadeesh153 15:3239c6391ffa 565
Bragadeesh153 15:3239c6391ffa 566 wait_ms(60);
Bragadeesh153 15:3239c6391ffa 567 FCTN_ATS_DATA_ACQ();
Bragadeesh153 15:3239c6391ffa 568
Bragadeesh153 15:3239c6391ffa 569 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
Bragadeesh153 15:3239c6391ffa 570 if(actual_data.current_actual[5]>1.47)
Bragadeesh153 15:3239c6391ffa 571 {
Bragadeesh153 15:3239c6391ffa 572 actual_data.current_actual[5]=3694/log(24.032242*resistance);
Bragadeesh153 15:3239c6391ffa 573 }
Bragadeesh153 15:3239c6391ffa 574 else{
Bragadeesh153 15:3239c6391ffa 575
Bragadeesh153 15:3239c6391ffa 576 actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
Bragadeesh153 15:3239c6391ffa 577 }
Bragadeesh153 15:3239c6391ffa 578
Bragadeesh153 15:3239c6391ffa 579
Bragadeesh153 15:3239c6391ffa 580
Bragadeesh153 15:3239c6391ffa 581 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.current_actual[5], &telemetry[82]);
Bragadeesh153 15:3239c6391ffa 582
Bragadeesh153 15:3239c6391ffa 583 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[84]);
Bragadeesh153 15:3239c6391ffa 584 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[86]);
Bragadeesh153 15:3239c6391ffa 585 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[88]);
Bragadeesh153 15:3239c6391ffa 586
Bragadeesh153 15:3239c6391ffa 587 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[90]);
Bragadeesh153 15:3239c6391ffa 588 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[92]);
Bragadeesh153 15:3239c6391ffa 589 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[94]);
Bragadeesh153 15:3239c6391ffa 590
Bragadeesh153 15:3239c6391ffa 591 PWM3 = 0;
Bragadeesh153 15:3239c6391ffa 592
Bragadeesh153 15:3239c6391ffa 593 wait_ms(60);
Bragadeesh153 15:3239c6391ffa 594
Bragadeesh153 15:3239c6391ffa 595 FCTN_ATS_DATA_ACQ();
Bragadeesh153 15:3239c6391ffa 596 actual_data.current_actual[5]=CurrentInput.read();
Bragadeesh153 15:3239c6391ffa 597 actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
Bragadeesh153 15:3239c6391ffa 598
Bragadeesh153 15:3239c6391ffa 599 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
Bragadeesh153 15:3239c6391ffa 600 if(actual_data.current_actual[5]>1.47)
Bragadeesh153 15:3239c6391ffa 601 {
Bragadeesh153 15:3239c6391ffa 602 actual_data.current_actual[5]=3694/log(24.032242*resistance);
Bragadeesh153 15:3239c6391ffa 603 }
Bragadeesh153 15:3239c6391ffa 604 else{
Bragadeesh153 15:3239c6391ffa 605
Bragadeesh153 15:3239c6391ffa 606 actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
Bragadeesh153 15:3239c6391ffa 607 }
Bragadeesh153 15:3239c6391ffa 608
Bragadeesh153 15:3239c6391ffa 609 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[96]);
Bragadeesh153 15:3239c6391ffa 610 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[98]);
Bragadeesh153 15:3239c6391ffa 611 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[100]);
Bragadeesh153 15:3239c6391ffa 612
Bragadeesh153 15:3239c6391ffa 613 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[102]);
Bragadeesh153 15:3239c6391ffa 614 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[104]);
Bragadeesh153 15:3239c6391ffa 615 FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[106]);
Bragadeesh153 15:3239c6391ffa 616
Bragadeesh153 15:3239c6391ffa 617
Bragadeesh153 15:3239c6391ffa 618
Bragadeesh153 15:3239c6391ffa 619 for(uint8_t i=108;i<132;i++)
lakshya 10:f93407b97750 620 {
Bragadeesh153 13:fb7facaf308b 621 telemetry[i]=0x00;
lakshya 10:f93407b97750 622 }
Bragadeesh153 13:fb7facaf308b 623
Bragadeesh153 13:fb7facaf308b 624 crc16 = CRC::crc16_gen(telemetry,132);
Bragadeesh153 13:fb7facaf308b 625 telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 626 telemetry[134] = (uint8_t)(crc16&0x00FF);
Bragadeesh153 13:fb7facaf308b 627 break;
lakshya 10:f93407b97750 628 }
Bragadeesh153 13:fb7facaf308b 629 case 0x02:
lakshya 10:f93407b97750 630 {
Bragadeesh153 13:fb7facaf308b 631
Bragadeesh153 13:fb7facaf308b 632 printf("Run P_EPS_MAIN\r\n");
Bragadeesh153 13:fb7facaf308b 633 F_EPS();
Bragadeesh153 13:fb7facaf308b 634 //ACK_L234_telemetry
Bragadeesh153 13:fb7facaf308b 635 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 636 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 637 telemetry[2]=ACK_CODE;
lakshya 10:f93407b97750 638 for(uint8_t i=0;i<133;i++)
sakthipriya 3:07e15677a75c 639 {
Bragadeesh153 13:fb7facaf308b 640 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 641 }
Bragadeesh153 13:fb7facaf308b 642 crc16 = CRC::crc16_gen(telemetry,132);
Bragadeesh153 13:fb7facaf308b 643 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 644 telemetry[133] = (uint8_t)(crc16&0x00FF);
Bragadeesh153 13:fb7facaf308b 645
sakthipriya 3:07e15677a75c 646 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 647 {
Bragadeesh153 13:fb7facaf308b 648 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 649 }
Bragadeesh153 13:fb7facaf308b 650 break;
sakthipriya 3:07e15677a75c 651 }
Bragadeesh153 13:fb7facaf308b 652 case 0x03:
sakthipriya 3:07e15677a75c 653 {
Bragadeesh153 13:fb7facaf308b 654 printf("Run P_ACS_INIT\r\n");
Bragadeesh153 13:fb7facaf308b 655 FCTN_ACS_INIT();
Bragadeesh153 13:fb7facaf308b 656 //ACK_L234_telemetry
Bragadeesh153 13:fb7facaf308b 657 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 658 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 659 telemetry[2]=ACK_CODE;
sakthipriya 3:07e15677a75c 660 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 661 {
Bragadeesh153 13:fb7facaf308b 662 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 663 }
Bragadeesh153 13:fb7facaf308b 664 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 665 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 666 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 667 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 668 {
Bragadeesh153 13:fb7facaf308b 669 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 670 }
Bragadeesh153 13:fb7facaf308b 671 break;
sakthipriya 3:07e15677a75c 672 }
sakthipriya 3:07e15677a75c 673 case 0x04:
sakthipriya 3:07e15677a75c 674 {
Bragadeesh153 13:fb7facaf308b 675
sakthipriya 3:07e15677a75c 676 printf("Run P_ACS_ACQ_DATA\r\n");
Bragadeesh153 13:fb7facaf308b 677 FCTN_ATS_DATA_ACQ();
sakthipriya 3:07e15677a75c 678 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 679 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 680 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 681 telemetry[2]=ACK_CODE;
sakthipriya 3:07e15677a75c 682 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 683 {
Bragadeesh153 13:fb7facaf308b 684 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 685 }
Bragadeesh153 13:fb7facaf308b 686 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 687 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 688 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 689 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 690 {
Bragadeesh153 13:fb7facaf308b 691 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 692 }
Bragadeesh153 13:fb7facaf308b 693 break;
sakthipriya 3:07e15677a75c 694 }
sakthipriya 3:07e15677a75c 695 case 0x05:
sakthipriya 3:07e15677a75c 696 {
sakthipriya 3:07e15677a75c 697 printf("Run P_ACS_MAIN\r\n");
lakshya 10:f93407b97750 698 F_ACS();
Bragadeesh153 13:fb7facaf308b 699 for(int i=0; i<3; i++)
Bragadeesh153 13:fb7facaf308b 700 FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&telemetry[(i*4)]);
lakshya 10:f93407b97750 701 for(int i=0; i<3; i++)
Bragadeesh153 13:fb7facaf308b 702 FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&telemetry[12+(i*4)]);
Bragadeesh153 13:fb7facaf308b 703
Bragadeesh153 13:fb7facaf308b 704 telemetry[24] = ACS_STATE;
Bragadeesh153 13:fb7facaf308b 705 telemetry[24] = telemetry[5]|(EN_BTRY_HT<<3);
Bragadeesh153 13:fb7facaf308b 706 telemetry[24] = telemetry[5]|(phase_TR_x<<4);
Bragadeesh153 13:fb7facaf308b 707 telemetry[24] = telemetry[5]|(phase_TR_y<<5);
Bragadeesh153 13:fb7facaf308b 708 telemetry[24] = telemetry[5]|(phase_TR_z<<6);
Bragadeesh153 13:fb7facaf308b 709
lakshya 11:1fdb94ae6563 710 /*___________________change / check pwm working__________________________________*/
Bragadeesh153 13:fb7facaf308b 711
Bragadeesh153 13:fb7facaf308b 712 FCTN_CONVERT_FLOAT(PWM1,&telemetry[25]);
Bragadeesh153 13:fb7facaf308b 713 FCTN_CONVERT_FLOAT(PWM2,&telemetry[29]);
Bragadeesh153 13:fb7facaf308b 714 FCTN_CONVERT_FLOAT(PWM3,&telemetry[33]);
Bragadeesh153 13:fb7facaf308b 715
lakshya 10:f93407b97750 716 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 717 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 718 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 719 telemetry[2]=ACK_CODE;
lakshya 10:f93407b97750 720 for(uint8_t i=3;i<11;i++)
lakshya 10:f93407b97750 721 {
Bragadeesh153 13:fb7facaf308b 722 telemetry[i]=0x00;
lakshya 10:f93407b97750 723 }
Bragadeesh153 13:fb7facaf308b 724
Bragadeesh153 13:fb7facaf308b 725 crc16 = CRC::crc16_gen(telemetry,37);
Bragadeesh153 13:fb7facaf308b 726 telemetry[37] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 727 telemetry[38] = (uint8_t)(crc16&0x00FF);
lakshya 10:f93407b97750 728 for(uint8_t i=39;i<134;i++)
lakshya 10:f93407b97750 729 {
Bragadeesh153 13:fb7facaf308b 730 telemetry[i]=0x00;
lakshya 10:f93407b97750 731 }
Bragadeesh153 13:fb7facaf308b 732 break;
lakshya 10:f93407b97750 733 }
lakshya 10:f93407b97750 734 case 0x06:
lakshya 10:f93407b97750 735 {
lakshya 10:f93407b97750 736 F_BCN();
Bragadeesh153 13:fb7facaf308b 737 printf("Run P_BCN_INIT\r\n");
sakthipriya 3:07e15677a75c 738 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 739 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 740 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 741 telemetry[2]=ACK_CODE;
sakthipriya 3:07e15677a75c 742 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 743 {
Bragadeesh153 13:fb7facaf308b 744 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 745 }
Bragadeesh153 13:fb7facaf308b 746 crc16 = CRC::crc16_gen(telemetry,0);
Bragadeesh153 13:fb7facaf308b 747 telemetry[0] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 748 telemetry[1] = (uint8_t)(crc16&0x00FF);
lakshya 10:f93407b97750 749 for(uint8_t i=2;i<134;i++)
sakthipriya 3:07e15677a75c 750 {
Bragadeesh153 13:fb7facaf308b 751 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 752 }
Bragadeesh153 13:fb7facaf308b 753 break;
sakthipriya 3:07e15677a75c 754 }
sakthipriya 3:07e15677a75c 755 case 0x07:
sakthipriya 3:07e15677a75c 756 {
Bragadeesh153 13:fb7facaf308b 757 printf("Run P_BCN_TX_MAIN\r\n");
sakthipriya 3:07e15677a75c 758 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 759 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 760 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 761 telemetry[2]=ACK_CODE;
sakthipriya 3:07e15677a75c 762 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 763 {
Bragadeesh153 13:fb7facaf308b 764 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 765 }
Bragadeesh153 13:fb7facaf308b 766 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 767 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 768 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 769 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 770 {
Bragadeesh153 13:fb7facaf308b 771 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 772 }
Bragadeesh153 13:fb7facaf308b 773 break;
Bragadeesh153 13:fb7facaf308b 774 }
Bragadeesh153 13:fb7facaf308b 775 case 0x11:
sakthipriya 3:07e15677a75c 776 {
sakthipriya 3:07e15677a75c 777 printf("SW_ON_ACS_ATS1_SW_ENABLE\r\n");
sakthipriya 3:07e15677a75c 778 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 779 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 780 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 781 telemetry[2]=1;
Bragadeesh153 15:3239c6391ffa 782 ATS2_SW_ENABLE = 1; // making sure we switch off the other
sakthipriya 9:194afacf7449 783 ATS1_SW_ENABLE = 0;
sakthipriya 3:07e15677a75c 784 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 785 {
Bragadeesh153 13:fb7facaf308b 786 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 787 }
Bragadeesh153 13:fb7facaf308b 788 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 789 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 790 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 791 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 792 {
Bragadeesh153 13:fb7facaf308b 793 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 794 }
Bragadeesh153 13:fb7facaf308b 795 break;
sakthipriya 3:07e15677a75c 796 }
sakthipriya 3:07e15677a75c 797 case 0x12:
sakthipriya 3:07e15677a75c 798 {
sakthipriya 3:07e15677a75c 799 printf("SW_ON_ACS_ATS2_SW_ENABLE\r\n");
sakthipriya 3:07e15677a75c 800 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 801 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 802 telemetry[1]=tc[0];
Bragadeesh153 15:3239c6391ffa 803
lakshya 10:f93407b97750 804 ATS1_SW_ENABLE = 1; //make sure u switch off the other
sakthipriya 9:194afacf7449 805 ATS2_SW_ENABLE = 0;
Bragadeesh153 13:fb7facaf308b 806 telemetry[2]=1;
sakthipriya 3:07e15677a75c 807 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 808 {
Bragadeesh153 13:fb7facaf308b 809 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 810 }
Bragadeesh153 13:fb7facaf308b 811 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 812 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 813 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 814 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 815 {
Bragadeesh153 13:fb7facaf308b 816 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 817 }
Bragadeesh153 13:fb7facaf308b 818 break;
sakthipriya 3:07e15677a75c 819 }
sakthipriya 3:07e15677a75c 820 case 0x13:
sakthipriya 3:07e15677a75c 821 {
sakthipriya 3:07e15677a75c 822 printf("SW_ON_ACS_TR_XY_ENABLE\r\n");
sakthipriya 3:07e15677a75c 823 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 824 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 825 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 826 TRXY_SW = 1;
Bragadeesh153 13:fb7facaf308b 827 telemetry[2]=1;
sakthipriya 3:07e15677a75c 828 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 829 {
Bragadeesh153 13:fb7facaf308b 830 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 831 }
Bragadeesh153 13:fb7facaf308b 832 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 833 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 834 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 835 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 836 {
Bragadeesh153 13:fb7facaf308b 837 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 838 }
Bragadeesh153 13:fb7facaf308b 839 break;
sakthipriya 3:07e15677a75c 840 }
sakthipriya 3:07e15677a75c 841 case 0x14:
sakthipriya 3:07e15677a75c 842 {
sakthipriya 3:07e15677a75c 843 printf("SW_ON_ACS_TR_Z_ENABLE\r\n");
sakthipriya 3:07e15677a75c 844 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 845 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 846 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 847 telemetry[2]=1;
sakthipriya 5:bb592f3185cc 848 TRZ_SW = 1;
sakthipriya 3:07e15677a75c 849 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 850 {
Bragadeesh153 13:fb7facaf308b 851 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 852 }
Bragadeesh153 13:fb7facaf308b 853 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 854 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 855 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 856 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 857 {
Bragadeesh153 13:fb7facaf308b 858 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 859 }
Bragadeesh153 13:fb7facaf308b 860 break;
sakthipriya 3:07e15677a75c 861 }
Bragadeesh153 13:fb7facaf308b 862 case 0x15:
sakthipriya 3:07e15677a75c 863 {
sakthipriya 3:07e15677a75c 864 printf("SW_ON_BCN_TX_SW_ENABLE\r\n");
sakthipriya 3:07e15677a75c 865 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 866 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 867 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 868 telemetry[2]=1;
Bragadeesh153 13:fb7facaf308b 869 BCN_SW = 0;
sakthipriya 3:07e15677a75c 870 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 871 {
Bragadeesh153 13:fb7facaf308b 872 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 873 }
Bragadeesh153 13:fb7facaf308b 874 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 875 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 876 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 877 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 878 {
Bragadeesh153 13:fb7facaf308b 879 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 880 }
Bragadeesh153 13:fb7facaf308b 881 break;
sakthipriya 3:07e15677a75c 882 }
sakthipriya 3:07e15677a75c 883 case 0x21:
sakthipriya 3:07e15677a75c 884 {
sakthipriya 3:07e15677a75c 885 printf("SW_OFF_ACS_ATS1_SW_ENABLE\r\n");
sakthipriya 3:07e15677a75c 886 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 887 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 888 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 889 telemetry[2]=1;
sakthipriya 9:194afacf7449 890 ATS1_SW_ENABLE = 1;
sakthipriya 3:07e15677a75c 891 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 892 {
Bragadeesh153 13:fb7facaf308b 893 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 894 }
Bragadeesh153 13:fb7facaf308b 895 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 896 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 897 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 898 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 899 {
Bragadeesh153 13:fb7facaf308b 900 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 901 }
Bragadeesh153 13:fb7facaf308b 902 break;
sakthipriya 3:07e15677a75c 903 }
sakthipriya 3:07e15677a75c 904 case 0x22:
sakthipriya 3:07e15677a75c 905 {
sakthipriya 3:07e15677a75c 906 printf("SW_OFF_ACS_ATS2_SW_ENABLE\r\n");
sakthipriya 3:07e15677a75c 907 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 908 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 909 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 910 telemetry[2]=1;
sakthipriya 9:194afacf7449 911 ATS2_SW_ENABLE = 1;
sakthipriya 3:07e15677a75c 912 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 913 {
Bragadeesh153 13:fb7facaf308b 914 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 915 }
Bragadeesh153 13:fb7facaf308b 916 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 917 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 918 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 919 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 920 {
Bragadeesh153 13:fb7facaf308b 921 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 922 }
Bragadeesh153 13:fb7facaf308b 923 break;
sakthipriya 3:07e15677a75c 924 }
sakthipriya 3:07e15677a75c 925 case 0x23:
sakthipriya 3:07e15677a75c 926 {
sakthipriya 3:07e15677a75c 927 printf("SW_OFF_ACS_TR_XY_ENABLE\r\n");
sakthipriya 3:07e15677a75c 928 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 929 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 930 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 931 telemetry[2]=1;
Bragadeesh153 13:fb7facaf308b 932 TRXY_SW= 0;
sakthipriya 3:07e15677a75c 933 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 934 {
Bragadeesh153 13:fb7facaf308b 935 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 936 }
Bragadeesh153 13:fb7facaf308b 937 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 938 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 939 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 940 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 941 {
Bragadeesh153 13:fb7facaf308b 942 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 943 }
Bragadeesh153 13:fb7facaf308b 944 break;
sakthipriya 3:07e15677a75c 945 }
sakthipriya 3:07e15677a75c 946 case 0x24:
sakthipriya 3:07e15677a75c 947 {
sakthipriya 3:07e15677a75c 948 printf("SW_OFF_ACS_TR_Z_ENABLE\r\n");
sakthipriya 3:07e15677a75c 949 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 950 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 951 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 952 telemetry[2]=1;
sakthipriya 5:bb592f3185cc 953 TRZ_SW = 0;
sakthipriya 3:07e15677a75c 954 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 955 {
Bragadeesh153 13:fb7facaf308b 956 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 957 }
Bragadeesh153 13:fb7facaf308b 958 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 959 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 960 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 961 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 962 {
Bragadeesh153 13:fb7facaf308b 963 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 964 }
Bragadeesh153 13:fb7facaf308b 965 break;
sakthipriya 3:07e15677a75c 966 }
Bragadeesh153 13:fb7facaf308b 967 case 0x25:
sakthipriya 3:07e15677a75c 968 {
sakthipriya 3:07e15677a75c 969 printf("SW_OFF_BCN_TX_SW_ENABLE\r\n");
sakthipriya 3:07e15677a75c 970 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 971 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 972 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 973 telemetry[2]=1;
sakthipriya 5:bb592f3185cc 974 BCN_SW = 1;
sakthipriya 3:07e15677a75c 975 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 976 {
Bragadeesh153 13:fb7facaf308b 977 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 978 }
Bragadeesh153 13:fb7facaf308b 979 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 980 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 981 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 982 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 983 {
Bragadeesh153 13:fb7facaf308b 984 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 985 }
Bragadeesh153 13:fb7facaf308b 986 break;
sakthipriya 3:07e15677a75c 987 }
sakthipriya 3:07e15677a75c 988 case 0x31:
sakthipriya 3:07e15677a75c 989 {
sakthipriya 5:bb592f3185cc 990 printf("ACS_ATS1_SW_RESET\r\n");
sakthipriya 3:07e15677a75c 991 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 992 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 993 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 994 telemetry[2]=1;
sakthipriya 9:194afacf7449 995 ATS1_SW_ENABLE = 1;
sakthipriya 5:bb592f3185cc 996 wait_us(1);
sakthipriya 9:194afacf7449 997 ATS1_SW_ENABLE = 0;
sakthipriya 3:07e15677a75c 998 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 999 {
Bragadeesh153 13:fb7facaf308b 1000 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 1001 }
Bragadeesh153 13:fb7facaf308b 1002 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 1003 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 1004 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 1005 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 1006 {
Bragadeesh153 13:fb7facaf308b 1007 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 1008 }
Bragadeesh153 13:fb7facaf308b 1009 break;
sakthipriya 3:07e15677a75c 1010 }
sakthipriya 3:07e15677a75c 1011 case 0x32:
sakthipriya 3:07e15677a75c 1012 {
sakthipriya 3:07e15677a75c 1013 printf("BCN_SW_RESET\r\n");
sakthipriya 3:07e15677a75c 1014 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 1015 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 1016 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 1017 telemetry[2]=1;
sakthipriya 5:bb592f3185cc 1018 BCN_SW = 1;
sakthipriya 5:bb592f3185cc 1019 wait_us(1);
sakthipriya 5:bb592f3185cc 1020 BCN_SW = 0;
sakthipriya 3:07e15677a75c 1021 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 1022 {
Bragadeesh153 13:fb7facaf308b 1023 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 1024 }
Bragadeesh153 13:fb7facaf308b 1025 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 1026 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 1027 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 1028 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 1029 {
Bragadeesh153 13:fb7facaf308b 1030 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 1031 }
Bragadeesh153 13:fb7facaf308b 1032 break;
sakthipriya 3:07e15677a75c 1033 }
sakthipriya 3:07e15677a75c 1034 case 0x33:
sakthipriya 3:07e15677a75c 1035 {
sakthipriya 5:bb592f3185cc 1036 printf("ACS_ATS2_SW_RESET\r\n");
sakthipriya 3:07e15677a75c 1037 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 1038 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 1039 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 1040 telemetry[2]=1;
sakthipriya 9:194afacf7449 1041 ATS1_SW_ENABLE = 1;
sakthipriya 5:bb592f3185cc 1042 wait_us(1);
sakthipriya 9:194afacf7449 1043 ATS1_SW_ENABLE = 0;
sakthipriya 3:07e15677a75c 1044 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 1045 {
Bragadeesh153 13:fb7facaf308b 1046 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 1047 }
Bragadeesh153 13:fb7facaf308b 1048 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 1049 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 1050 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 1051 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 1052 {
Bragadeesh153 13:fb7facaf308b 1053 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 1054 }
Bragadeesh153 13:fb7facaf308b 1055 break;
sakthipriya 3:07e15677a75c 1056 }
Bragadeesh153 13:fb7facaf308b 1057 case 0x34:
sakthipriya 3:07e15677a75c 1058 {
sakthipriya 3:07e15677a75c 1059 printf("CDMS_SW_RESET\r\n");
sakthipriya 3:07e15677a75c 1060 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 1061 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 1062 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 1063 telemetry[2]=1;
sakthipriya 5:bb592f3185cc 1064 CDMS_RESET = 0;
sakthipriya 5:bb592f3185cc 1065 wait_us(1);
sakthipriya 5:bb592f3185cc 1066 CDMS_RESET = 1;
sakthipriya 3:07e15677a75c 1067 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 1068 {
Bragadeesh153 13:fb7facaf308b 1069 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 1070 }
Bragadeesh153 13:fb7facaf308b 1071 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 1072 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 1073 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 1074 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 1075 {
Bragadeesh153 13:fb7facaf308b 1076 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 1077 }
Bragadeesh153 13:fb7facaf308b 1078 break;
sakthipriya 3:07e15677a75c 1079 }
sakthipriya 3:07e15677a75c 1080 default:
Bragadeesh153 13:fb7facaf308b 1081 {
sakthipriya 3:07e15677a75c 1082 printf("Invalid TC\r\n");
sakthipriya 3:07e15677a75c 1083 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 1084 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 1085 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 1086 telemetry[2]=ACK_CODE;
sakthipriya 3:07e15677a75c 1087 for(uint8_t i=3;i<11;i++)
sakthipriya 3:07e15677a75c 1088 {
Bragadeesh153 13:fb7facaf308b 1089 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 1090 }
Bragadeesh153 13:fb7facaf308b 1091 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 1092 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 1093 telemetry[12] = (uint8_t)(crc16&0x00FF);
sakthipriya 3:07e15677a75c 1094 for(uint8_t i=13;i<134;i++)
sakthipriya 3:07e15677a75c 1095 {
Bragadeesh153 13:fb7facaf308b 1096 telemetry[i]=0x00;
sakthipriya 3:07e15677a75c 1097 }
Bragadeesh153 13:fb7facaf308b 1098 break;
sakthipriya 3:07e15677a75c 1099 }
sakthipriya 3:07e15677a75c 1100 }
Bragadeesh153 13:fb7facaf308b 1101 break;
Bragadeesh153 13:fb7facaf308b 1102 }
sakthipriya 3:07e15677a75c 1103 default:
Bragadeesh153 13:fb7facaf308b 1104 {
Bragadeesh153 13:fb7facaf308b 1105 printf("Invalid TC\r\n");
Bragadeesh153 13:fb7facaf308b 1106 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 1107 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 1108 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 1109 telemetry[2]=ACK_CODE;
Bragadeesh153 13:fb7facaf308b 1110 for(uint8_t i=3;i<11;i++)
Bragadeesh153 13:fb7facaf308b 1111 {
Bragadeesh153 13:fb7facaf308b 1112 telemetry[i]=0x00;
Bragadeesh153 13:fb7facaf308b 1113 }
Bragadeesh153 13:fb7facaf308b 1114 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 1115 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 1116 telemetry[12] = (uint8_t)(crc16&0x00FF);
Bragadeesh153 13:fb7facaf308b 1117 for(uint8_t i=13;i<134;i++)
Bragadeesh153 13:fb7facaf308b 1118 {
Bragadeesh153 13:fb7facaf308b 1119 telemetry[i]=0x00;
Bragadeesh153 13:fb7facaf308b 1120 }
Bragadeesh153 13:fb7facaf308b 1121 break;
sakthipriya 3:07e15677a75c 1122 }
sakthipriya 3:07e15677a75c 1123 }
Bragadeesh153 13:fb7facaf308b 1124 break;
Bragadeesh153 13:fb7facaf308b 1125 }
Bragadeesh153 13:fb7facaf308b 1126 default:
Bragadeesh153 13:fb7facaf308b 1127 {
Bragadeesh153 13:fb7facaf308b 1128 printf("Invalid TC\r\n");
Bragadeesh153 13:fb7facaf308b 1129 //ACK_L234_TM
Bragadeesh153 13:fb7facaf308b 1130 telemetry[0]=0xB0;
Bragadeesh153 13:fb7facaf308b 1131 telemetry[1]=tc[0];
Bragadeesh153 13:fb7facaf308b 1132 telemetry[2]=ACK_CODE;
Bragadeesh153 13:fb7facaf308b 1133 for(uint8_t i=3;i<11;i++)
Bragadeesh153 13:fb7facaf308b 1134 {
Bragadeesh153 13:fb7facaf308b 1135 telemetry[i]=0x00;
Bragadeesh153 13:fb7facaf308b 1136 }
Bragadeesh153 13:fb7facaf308b 1137 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 1138 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 1139 telemetry[12] = (uint8_t)(crc16&0x00FF);
Bragadeesh153 13:fb7facaf308b 1140 for(uint8_t i=13;i<134;i++)
Bragadeesh153 13:fb7facaf308b 1141 {
Bragadeesh153 13:fb7facaf308b 1142 telemetry[i]=0x00;
Bragadeesh153 13:fb7facaf308b 1143 }
Bragadeesh153 13:fb7facaf308b 1144 break;
sakthipriya 3:07e15677a75c 1145 }
sakthipriya 3:07e15677a75c 1146 }
Bragadeesh153 13:fb7facaf308b 1147 }
sakthipriya 4:39a4ae8c7ecd 1148
sakthipriya 4:39a4ae8c7ecd 1149
sakthipriya 4:39a4ae8c7ecd 1150
sakthipriya 4:39a4ae8c7ecd 1151
Bragadeesh153 13:fb7facaf308b 1152 int strt_add = flash_size() - (2*SECTOR_SIZE);
sakthipriya 4:39a4ae8c7ecd 1153 uint32_t flasharray[8]; //256+(3*1024)
sakthipriya 4:39a4ae8c7ecd 1154 char *nativeflash = (char*)strt_add;
sakthipriya 4:39a4ae8c7ecd 1155
sakthipriya 4:39a4ae8c7ecd 1156 /*Writing to the Flash*/
sakthipriya 4:39a4ae8c7ecd 1157 void FCTN_CDMS_WR_FLASH(uint16_t j,uint32_t fdata) //j-position to write address ; fdata - flash data to be written
sakthipriya 4:39a4ae8c7ecd 1158 {
Bragadeesh153 13:fb7facaf308b 1159 for(int i=0;i<8;i++)
sakthipriya 4:39a4ae8c7ecd 1160 {
sakthipriya 4:39a4ae8c7ecd 1161 flasharray[i]=nativeflash[i];
sakthipriya 4:39a4ae8c7ecd 1162 }
sakthipriya 4:39a4ae8c7ecd 1163 flasharray[j]=fdata;
sakthipriya 4:39a4ae8c7ecd 1164 erase_sector(strt_add);
sakthipriya 4:39a4ae8c7ecd 1165 program_flash(strt_add, (char*)&flasharray,4*8);
sakthipriya 4:39a4ae8c7ecd 1166 }
sakthipriya 4:39a4ae8c7ecd 1167 /*End*/
sakthipriya 4:39a4ae8c7ecd 1168
sakthipriya 4:39a4ae8c7ecd 1169 /*Reading from Flash*/
sakthipriya 4:39a4ae8c7ecd 1170 uint32_t FCTN_CDMS_RD_FLASH(uint16_t j)
sakthipriya 4:39a4ae8c7ecd 1171 {
sakthipriya 4:39a4ae8c7ecd 1172 for(int i=0;i<8;i++)
sakthipriya 4:39a4ae8c7ecd 1173 {
sakthipriya 4:39a4ae8c7ecd 1174 flasharray[i]=nativeflash[i];
sakthipriya 4:39a4ae8c7ecd 1175 }
sakthipriya 4:39a4ae8c7ecd 1176 return flasharray[j];
sakthipriya 4:39a4ae8c7ecd 1177 }
sakthipriya 5:bb592f3185cc 1178 /*End*/
sakthipriya 5:bb592f3185cc 1179
sakthipriya 5:bb592f3185cc 1180 // Convert float to 4 uint8_t
sakthipriya 5:bb592f3185cc 1181
sakthipriya 6:036d08b62785 1182 void FCTN_CONVERT_FLOAT(float input, uint8_t output[4])
sakthipriya 5:bb592f3185cc 1183 {
sakthipriya 6:036d08b62785 1184 assert(sizeof(float) == sizeof(uint32_t));
sakthipriya 6:036d08b62785 1185 uint32_t* temp = reinterpret_cast<uint32_t*>(&input);
sakthipriya 6:036d08b62785 1186
sakthipriya 6:036d08b62785 1187 //float* output1 = reinterpret_cast<float*>(temp);
Bragadeesh153 13:fb7facaf308b 1188
sakthipriya 6:036d08b62785 1189 printf("\n\r %f ", input);
sakthipriya 6:036d08b62785 1190 std::cout << "\n\r uint32"<<*temp << std::endl;
sakthipriya 6:036d08b62785 1191
sakthipriya 6:036d08b62785 1192 output[0] =(uint8_t )(((*temp)>>24)&0xFF);
sakthipriya 6:036d08b62785 1193 output[2] =(uint8_t ) (((*temp)>>16)&0xFF);
Bragadeesh153 13:fb7facaf308b 1194 output[1] =(uint8_t ) (((*temp)>>8)&0xFF);
Bragadeesh153 13:fb7facaf308b 1195 output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic
sakthipriya 6:036d08b62785 1196 //printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]);
Bragadeesh153 13:fb7facaf308b 1197 //std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl;
Bragadeesh153 15:3239c6391ffa 1198 }
Bragadeesh153 15:3239c6391ffa 1199
Bragadeesh153 15:3239c6391ffa 1200
Bragadeesh153 15:3239c6391ffa 1201 void FCTN_CONVERT_FLOAT_COMPRESS(float input, uint8_t output[2])
Bragadeesh153 15:3239c6391ffa 1202 {
Bragadeesh153 15:3239c6391ffa 1203 int integer = (int)input;
Bragadeesh153 15:3239c6391ffa 1204 assert(sizeof(int) == sizeof(uint16_t));
Bragadeesh153 15:3239c6391ffa 1205 uint16_t* temp = reinterpret_cast<uint16_t*>(&integer);
Bragadeesh153 15:3239c6391ffa 1206
Bragadeesh153 15:3239c6391ffa 1207 //float* output1 = reinterpret_cast<float*>(temp);
Bragadeesh153 15:3239c6391ffa 1208
Bragadeesh153 15:3239c6391ffa 1209 printf("\n\r %d ", integer);
Bragadeesh153 15:3239c6391ffa 1210 std::cout << "\n\r uint16"<<*temp << std::endl;
Bragadeesh153 15:3239c6391ffa 1211
Bragadeesh153 15:3239c6391ffa 1212 output[0] =(uint8_t ) (((*temp)>>8)&0xFF);
Bragadeesh153 15:3239c6391ffa 1213 output[1] =(uint8_t ) ((*temp) & 0xFF); // verify the logic
Bragadeesh153 15:3239c6391ffa 1214 //printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]);
Bragadeesh153 15:3239c6391ffa 1215 //std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl;
Bragadeesh153 13:fb7facaf308b 1216 }