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@16:cc77770d787f, 2016-06-03 (annotated)

Committer:
Bragadeesh153
Date:
Fri Jun 03 13:53:55 2016 +0000
Revision:
16:cc77770d787f
Parent:
15:e09aaaccf134 
ACS Data_Acq updated. Tested and working.

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