Team Fox / Mbed 2 deprecated ACS_Flowchart_BAE

ACS data acq changed completely. Tested and working. Deals all faults.

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of QM_BAE_review_1 by Team Fox

TCTM.cpp@10:f93407b97750, 2016-04-11 (annotated)

Committer:
lakshya
Date:
Mon Apr 11 17:26:46 2016 +0000
Revision:
10:f93407b97750
Parent:
9:194afacf7449
Child:
11:1fdb94ae6563
11april

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