Team Fox / Mbed 2 deprecated BAE_QM_MAR9

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of workinQM_5thJan_azad by Team Fox

TCTM.cpp@19:79e69017c855, 2016-06-04 (annotated)

Committer:
lakshya
Date:
Sat Jun 04 11:29:13 2016 +0000
Revision:
19:79e69017c855
Parent:
17:fc782f7548c6
Child:
20:949d13045431
before going home

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 19:79e69017c855 13 //**********************************STATUS_PARAMETERS*****************************************************
lakshya 19:79e69017c855 14 uint8_t BCN_TX_SW_ENABLE=0x00;
lakshya 19:79e69017c855 15 //extern uint8_t BCN_TX_STATUS;?? is it same??*****************************************************************************************DOUBT
lakshya 19:79e69017c855 16 uint8_t ACS_TR_XY_SW_STATUS=0x00;
lakshya 19:79e69017c855 17 uint8_t ACS_TR_Z_SW_STATUS=0x00;
lakshya 19:79e69017c855 18 uint8_t ACS_ATS1_SW_STATUS=0x00;
lakshya 19:79e69017c855 19 uint8_t ACS_ATS2_SW_STATUS=0x00;
lakshya 19:79e69017c855 20
lakshya 19:79e69017c855 21 //***********************************FOR STANDBY TIMER****************************************************
lakshya 19:79e69017c855 22 extern void BAE_STANDBY_TIMER_RESET();
lakshya 19:79e69017c855 23
lakshya 19:79e69017c855 24
lakshya 19:79e69017c855 25 //**********************************EXTERN_PARAMETERS******************************************************
lakshya 11:1fdb94ae6563 26 /*define the pins for digital out*/
sakthipriya 0:7b4c00e3912f 27
sakthipriya 9:194afacf7449 28 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
sakthipriya 9:194afacf7449 29 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
sakthipriya 5:bb592f3185cc 30
Bragadeesh153 13:fb7facaf308b 31 extern DigitalOut TRXY_SW; //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN
sakthipriya 5:bb592f3185cc 32 extern DigitalOut TRZ_SW; //TR Z Switch
sakthipriya 5:bb592f3185cc 33 extern DigitalOut CDMS_RESET; // CDMS RESET
sakthipriya 5:bb592f3185cc 34 extern DigitalOut BCN_SW; //Beacon switch
lakshya 10:f93407b97750 35 extern uint8_t BCN_TX_STATUS;
lakshya 10:f93407b97750 36 extern uint8_t BCN_FEN;
lakshya 19:79e69017c855 37 extern uint8_t BCN_STANDBY;
lakshya 19:79e69017c855 38 uint8_t BCN_TX_MAIN_STATUS;
sakthipriya 4:39a4ae8c7ecd 39 extern BAE_HK_actual actual_data;
sakthipriya 4:39a4ae8c7ecd 40 extern BAE_HK_min_max bae_HK_minmax;
sakthipriya 4:39a4ae8c7ecd 41 extern uint32_t BAE_STATUS;
lakshya 19:79e69017c855 42 extern uint8_t BAE_STANDBY;
sakthipriya 5:bb592f3185cc 43 extern float data[6];
sakthipriya 5:bb592f3185cc 44 extern float moment[3];
lakshya 10:f93407b97750 45 extern uint8_t ACS_STATE;
lakshya 10:f93407b97750 46 extern DigitalOut EN_BTRY_HT;
lakshya 10:f93407b97750 47 extern DigitalOut phase_TR_x;
lakshya 10:f93407b97750 48 extern DigitalOut phase_TR_y;
lakshya 10:f93407b97750 49 extern DigitalOut phase_TR_z;
lakshya 10:f93407b97750 50 extern BAE_HK_quant quant_data;
Bragadeesh153 13:fb7facaf308b 51 //extern DigitalOut TRXY_SW;
lakshya 11:1fdb94ae6563 52 //extern DigitalOut TRZ_SW_EN; //same as TRZ_SW
lakshya 11:1fdb94ae6563 53 extern uint32_t BAE_ENABLE;
lakshya 11:1fdb94ae6563 54 //extern DigitalOut ACS_ACQ_DATA_ENABLE;
lakshya 11:1fdb94ae6563 55
lakshya 11:1fdb94ae6563 56 /*given a default value as of now shuld read value from flash and increment it write it back very time it starts(bae)*/
lakshya 19:79e69017c855 57 extern uint8_t BAE_RESET_COUNTER;
lakshya 11:1fdb94ae6563 58
lakshya 14:a9588f443f1a 59 //extern uint8_t BCN_FAIL_COUNT;
lakshya 11:1fdb94ae6563 60
Bragadeesh153 13:fb7facaf308b 61 extern PwmOut PWM1; //x //Functions used to generate PWM signal
lakshya 11:1fdb94ae6563 62 extern PwmOut PWM2; //y
lakshya 11:1fdb94ae6563 63 extern PwmOut PWM3; //z //PWM output comes from pins p6
lakshya 11:1fdb94ae6563 64
lakshya 11:1fdb94ae6563 65 extern void F_ACS();
lakshya 11:1fdb94ae6563 66 extern void F_BCN();
lakshya 11:1fdb94ae6563 67 //extern void FCTN_ACS_GENPWM_MAIN();
lakshya 11:1fdb94ae6563 68 extern void F_EPS();
lakshya 11:1fdb94ae6563 69 extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]);
Bragadeesh153 13:fb7facaf308b 70 extern void FCTN_ACS_INIT();
lakshya 11:1fdb94ae6563 71
sakthipriya 5:bb592f3185cc 72
sakthipriya 5:bb592f3185cc 73 extern void FCTN_ATS_DATA_ACQ();
sakthipriya 5:bb592f3185cc 74 extern void FCTN_ACS_CNTRLALGO(float*,float*);
Bragadeesh153 13:fb7facaf308b 75 uint8_t telemetry[135];
lakshya 11:1fdb94ae6563 76
lakshya 11:1fdb94ae6563 77 void FCTN_CONVERT_UINT (uint8_t input[2], float* output)
lakshya 11:1fdb94ae6563 78 {
lakshya 11:1fdb94ae6563 79
lakshya 11:1fdb94ae6563 80 *output = (float) input[1];
lakshya 11:1fdb94ae6563 81 *output = *output/100.0; //input[0] integer part
lakshya 11:1fdb94ae6563 82 *output = *output + (float) input[0]; //input[1] decimal part correct to two decimal places
lakshya 11:1fdb94ae6563 83 }
lakshya 11:1fdb94ae6563 84
lakshya 10:f93407b97750 85 float angle(float x,float y,float z)
lakshya 10:f93407b97750 86 {
lakshya 10:f93407b97750 87 float mag_total=sqrt(x*x + y*y + z*z);
lakshya 10:f93407b97750 88 float cos_z = z/mag_total;
lakshya 10:f93407b97750 89 float theta_z = acosf(cos_z);
Bragadeesh153 13:fb7facaf308b 90
lakshya 10:f93407b97750 91 return theta_z;
lakshya 10:f93407b97750 92 //printf("/n cos_zz= %f /t theta_z= %f /n",cos_z,theta_z);
lakshya 10:f93407b97750 93 }
lakshya 10:f93407b97750 94
Bragadeesh153 13:fb7facaf308b 95 //uint8_t tm1[134];
Bragadeesh153 13:fb7facaf308b 96
Bragadeesh153 13:fb7facaf308b 97 void FCTN_BAE_TM_TC (uint8_t* tc)
sakthipriya 3:07e15677a75c 98 {
lakshya 19:79e69017c855 99 //tm1[0] = 1;
lakshya 19:79e69017c855 100 //calculating crc
lakshya 19:79e69017c855 101
lakshya 15:e09aaaccf134 102 /*chaged*/
lakshya 15:e09aaaccf134 103 uint8_t* tm; // without it some identifier error
lakshya 19:79e69017c855 104 uint16_t crc16=CRC::crc16_gen((char)tc,9); //implementing crc
lakshya 15:e09aaaccf134 105
lakshya 19:79e69017c855 106 if( ( ((uint8_t)((crc16 & 0xFF00)>>8)==tc[9]) && ((uint8_t)(crc16 & 0x00FF)==tc[10]) )== 0 )
lakshya 19:79e69017c855 107 {
lakshya 19:79e69017c855 108 telemetry[0]=0xB0;
lakshya 19:79e69017c855 109 telemetry[1]=0x00;//tc psc defined for this case
lakshya 19:79e69017c855 110 telemetry[2]=0x01;//ack code for this case
lakshya 19:79e69017c855 111 for(int i=3;i<11;i++)
lakshya 19:79e69017c855 112 {
lakshya 19:79e69017c855 113 telemetry[i]=0x00;
lakshya 19:79e69017c855 114 }
lakshya 19:79e69017c855 115 //add crc
lakshya 19:79e69017c855 116 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 117 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 118 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 119 printf("\n\rincorrect crc");
lakshya 19:79e69017c855 120 for(int i=13;i<134;i++)
lakshya 19:79e69017c855 121 {
lakshya 19:79e69017c855 122 telemetry[i]=0x00;
lakshya 19:79e69017c855 123 }
lakshya 19:79e69017c855 124 }
lakshya 19:79e69017c855 125 else
lakshya 19:79e69017c855 126 {
lakshya 19:79e69017c855 127 //check apid
lakshya 19:79e69017c855 128 uint8_t apid_check=((tc[1]&0xC0)>>6);
lakshya 19:79e69017c855 129 if(apid_check!=0x01)
lakshya 19:79e69017c855 130 {
lakshya 19:79e69017c855 131 telemetry[0]=0xB0;
lakshya 19:79e69017c855 132 telemetry[1]=tc[0];//tc psc defined for this case
lakshya 19:79e69017c855 133 telemetry[2]=0x02;//ack code for this case
lakshya 19:79e69017c855 134 for(int i=3;i<11;i++)
Bragadeesh153 13:fb7facaf308b 135 {
lakshya 19:79e69017c855 136 telemetry[i]=0x00;
Bragadeesh153 13:fb7facaf308b 137 }
Bragadeesh153 13:fb7facaf308b 138 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 139 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 140 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 141 printf("\n\rillegal TC packet APID don't match");
lakshya 19:79e69017c855 142 for(int i=13;i<134;i++)
Bragadeesh153 13:fb7facaf308b 143 {
Bragadeesh153 13:fb7facaf308b 144 telemetry[i]=0x00;
Bragadeesh153 13:fb7facaf308b 145 }
sakthipriya 3:07e15677a75c 146 }
lakshya 19:79e69017c855 147 else
lakshya 19:79e69017c855 148 {// all the possible cases of fms and mms
lakshya 19:79e69017c855 149 uint8_t service_type=(tc[2]&0xF0);
lakshya 19:79e69017c855 150 //check for fms first
lakshya 19:79e69017c855 151 switch(service_type)
lakshya 19:79e69017c855 152 {
lakshya 19:79e69017c855 153 case 0x60:
lakshya 19:79e69017c855 154 {
lakshya 19:79e69017c855 155 printf("Memory Management Service\r\n");
lakshya 19:79e69017c855 156 uint8_t service_subtype=(tc[2]&0x0F);
lakshya 19:79e69017c855 157 switch(service_subtype)
lakshya 19:79e69017c855 158 {
lakshya 19:79e69017c855 159 /* no such case exist refer tc protocol mms 0x1 exist for speed/payload only*/
lakshya 19:79e69017c855 160 /* case 0x01:
lakshya 19:79e69017c855 161 {
lakshya 19:79e69017c855 162 printf("Read from Flash\r\n");
lakshya 19:79e69017c855 163 uint16_t jj; //if no problem then change it to uint8
lakshya 19:79e69017c855 164 uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
lakshya 19:79e69017c855 165 switch(MID)
lakshya 19:79e69017c855 166 {
lakshya 19:79e69017c855 167 case 0x1100:jj=0x00;// using uint16_t as jj typesimilarly used in FCTN_CDMS_WR_FLASH
lakshya 19:79e69017c855 168 break;
lakshya 19:79e69017c855 169 case 0x0100:jj=0x01;
lakshya 19:79e69017c855 170 break;
lakshya 19:79e69017c855 171 case 0x0101:jj=0x02;
lakshya 19:79e69017c855 172 break;
lakshya 19:79e69017c855 173 case 0x0102:jj=0x03;
lakshya 19:79e69017c855 174 break;
lakshya 19:79e69017c855 175 case 0x0107:jj=0x04;
lakshya 19:79e69017c855 176 break;
lakshya 19:79e69017c855 177 case 0x0103:jj=0x05;
lakshya 19:79e69017c855 178 break;
lakshya 19:79e69017c855 179 case 0x0104:jj=0x05;
lakshya 19:79e69017c855 180 break;
lakshya 19:79e69017c855 181 case 0x0105:jj=0x06;
lakshya 19:79e69017c855 182 break;
lakshya 19:79e69017c855 183 case 0x0106:jj=0x07;
lakshya 19:79e69017c855 184 break;
lakshya 19:79e69017c855 185 }
lakshya 19:79e69017c855 186 //*pointer....!!!!
lakshya 19:79e69017c855 187 uint32_t FLASH_TEMP = FCTN_CDMS_RD_FLASH(jj);
lakshya 19:79e69017c855 188
lakshya 19:79e69017c855 189 tm[0] = 0x60;
lakshya 19:79e69017c855 190 tm[1] = tc[0];
lakshya 19:79e69017c855 191 tm[2] = ACK_CODE;
lakshya 19:79e69017c855 192 for(int i=0; i<8*4; i+=4)
lakshya 19:79e69017c855 193 {
lakshya 19:79e69017c855 194 tm[4+i] =(uint8_t )(((FLASH_TEMP)>>24)&0xFF);
lakshya 19:79e69017c855 195 tm[5+i] =(uint8_t ) (((FLASH_TEMP)>>16)&0xFF);
lakshya 19:79e69017c855 196 tm[6+i] =(uint8_t ) (((FLASH_TEMP)>>8)&0xFF);
lakshya 19:79e69017c855 197 tm[7+i] =(uint8_t ) ((FLASH_TEMP) & 0xFF);
lakshya 19:79e69017c855 198
lakshya 19:79e69017c855 199 }
lakshya 19:79e69017c855 200
lakshya 19:79e69017c855 201
lakshya 19:79e69017c855 202 for (int i=4+8*4; i<132;i++)
lakshya 19:79e69017c855 203 {
lakshya 19:79e69017c855 204 tm[i] = 0x00;
lakshya 19:79e69017c855 205 }
lakshya 19:79e69017c855 206 crc16 = CRC::crc16_gen(tm,132);
lakshya 19:79e69017c855 207 tm[132] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 208 tm[133] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 209
lakshya 19:79e69017c855 210 break;
lakshya 19:79e69017c855 211 }
lakshya 19:79e69017c855 212 */
lakshya 19:79e69017c855 213 case 0x02:
lakshya 19:79e69017c855 214 {
lakshya 19:79e69017c855 215 printf("\n\rRead from RAM");
lakshya 19:79e69017c855 216 uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
lakshya 19:79e69017c855 217 switch(MID)
lakshya 19:79e69017c855 218 {
lakshya 19:79e69017c855 219 case 0x0000://changed from 0001
lakshya 19:79e69017c855 220 {
lakshya 19:79e69017c855 221 printf("\n\rRead from MID 0000 \n");
lakshya 19:79e69017c855 222 printf("\n\rReading flash parameters");
lakshya 19:79e69017c855 223
lakshya 19:79e69017c855 224 /*taking some varible till we find some thing more useful*/
lakshya 19:79e69017c855 225 //uint8_t ref_val=0x01;
lakshya 19:79e69017c855 226 telemetry[0] = 1;
lakshya 19:79e69017c855 227 telemetry[1] = tc[0];
lakshya 19:79e69017c855 228 telemetry[2] = ACK_CODE;
lakshya 19:79e69017c855 229 /*random or with bcn_tx_sw_enable assuming it is 1 bit in length
lakshya 19:79e69017c855 230 how to get that we dont know, now we just assume it to be so*/
lakshya 19:79e69017c855 231 telemetry[3] = (BCN_SW);
lakshya 19:79e69017c855 232 telemetry[3] = telemetry[3]|(TRXY_SW<<1);
lakshya 19:79e69017c855 233 telemetry[3] = telemetry[3]|(TRZ_SW<<2);
lakshya 19:79e69017c855 234 telemetry[3] = telemetry[3]|(ATS1_SW_ENABLE<<3);
lakshya 19:79e69017c855 235 telemetry[3] = telemetry[3]|(ATS2_SW_ENABLE<<4);
lakshya 19:79e69017c855 236
lakshya 19:79e69017c855 237 if(BCN_TX_STATUS==2)
lakshya 19:79e69017c855 238 telemetry[3] = telemetry[3]|0x20;
lakshya 19:79e69017c855 239 else
lakshya 19:79e69017c855 240 telemetry[3] = telemetry[3] & 0xDF;
lakshya 19:79e69017c855 241
lakshya 19:79e69017c855 242 telemetry[3] = telemetry[3]|(BCN_FEN<<6);
lakshya 19:79e69017c855 243 uint8_t mask_val=BAE_ENABLE & 0x00000008;
lakshya 19:79e69017c855 244 /*can be a problem see if any error occurs*/
lakshya 19:79e69017c855 245 telemetry[3] = telemetry[3]|(mask_val<<7);
lakshya 19:79e69017c855 246
lakshya 19:79e69017c855 247 /*not included in the code yet*/
lakshya 19:79e69017c855 248 telemetry[4] = BAE_RESET_COUNTER;
lakshya 19:79e69017c855 249 telemetry[5] = ACS_STATE;
lakshya 19:79e69017c855 250 telemetry[5] = telemetry[5]|(EN_BTRY_HT<<3);
lakshya 19:79e69017c855 251 telemetry[5] = telemetry[5]|(phase_TR_x<<4);
lakshya 19:79e69017c855 252 telemetry[5] = telemetry[5]|(phase_TR_y<<5);
lakshya 19:79e69017c855 253 telemetry[5] = telemetry[5]|(phase_TR_z<<6);
lakshya 19:79e69017c855 254 /*spare to be fixed*/
lakshya 19:79e69017c855 255 //telemetry[5] = telemetry[5]|(Spare))<<7);
lakshya 19:79e69017c855 256 /**/
lakshya 19:79e69017c855 257 uint8_t soc_powerlevel_2=50;
lakshya 19:79e69017c855 258 uint8_t soc_powerlevel_3=65;
lakshya 19:79e69017c855 259
lakshya 19:79e69017c855 260 telemetry[6] = soc_powerlevel_2;
lakshya 19:79e69017c855 261 telemetry[7] = soc_powerlevel_3;
lakshya 19:79e69017c855 262
lakshya 19:79e69017c855 263 /*to be fixed*/
lakshya 19:79e69017c855 264 telemetry[8] = 0;
lakshya 19:79e69017c855 265 telemetry[9] = 0;
lakshya 19:79e69017c855 266 telemetry[10] = 0;
lakshya 19:79e69017c855 267 telemetry[11] = 0;
lakshya 19:79e69017c855 268 //telemetry[8] = Torque Rod X Offset;
lakshya 19:79e69017c855 269 //telemetry[9] = Torque Rod Y Offset;
lakshya 19:79e69017c855 270 //telemetry[10] = Torque Rod Z Offset;
lakshya 19:79e69017c855 271 //telemetry[11] = ACS_DEMAG_TIME_DELAY;
lakshya 19:79e69017c855 272 telemetry[12] = (BAE_STATUS>>24) & 0xFF;
lakshya 19:79e69017c855 273 telemetry[13] = (BAE_STATUS>>16) & 0xFF;
lakshya 19:79e69017c855 274 telemetry[14] = (BAE_STATUS>>8) & 0xFF;
lakshya 19:79e69017c855 275 telemetry[15] = BAE_STATUS & 0xFF;
lakshya 19:79e69017c855 276
lakshya 19:79e69017c855 277 /*to be fixed*/
lakshya 19:79e69017c855 278 //telemetry[16] = BCN_FAIL_COUNT;
lakshya 19:79e69017c855 279 telemetry[17] = actual_data.power_mode;
lakshya 19:79e69017c855 280 /*to be fixed*/
lakshya 19:79e69017c855 281 uint16_t P_BAE_I2CRX_COUNTER=0;
lakshya 19:79e69017c855 282 uint16_t P_ACS_MAIN_COUNTER=0;
lakshya 19:79e69017c855 283 uint16_t P_BCN_TX_MAIN_COUNTER=0;
lakshya 19:79e69017c855 284 uint16_t P_EPS_MAIN_COUNTER=0;
lakshya 19:79e69017c855 285
lakshya 19:79e69017c855 286 telemetry[18] = P_BAE_I2CRX_COUNTER>>8;
lakshya 19:79e69017c855 287 telemetry[19] = P_BAE_I2CRX_COUNTER;
lakshya 19:79e69017c855 288 telemetry[20] = P_ACS_MAIN_COUNTER>>8;
lakshya 19:79e69017c855 289 telemetry[21] = P_ACS_MAIN_COUNTER;
lakshya 19:79e69017c855 290 telemetry[22] = P_BCN_TX_MAIN_COUNTER>>8;
lakshya 19:79e69017c855 291 telemetry[23] = P_BCN_TX_MAIN_COUNTER;
lakshya 19:79e69017c855 292 telemetry[24] = P_EPS_MAIN_COUNTER>>8;
lakshya 19:79e69017c855 293 telemetry[25] = P_EPS_MAIN_COUNTER;
lakshya 19:79e69017c855 294
lakshya 19:79e69017c855 295 //actual_data.AngularSpeed_actual[0]=5.32498;
lakshya 19:79e69017c855 296 for(int i=0; i<3; i++)
lakshya 19:79e69017c855 297 FCTN_CONVERT_FLOAT((float)actual_data.Bvalue_actual[i],&telemetry[26+ (i*4)]);
lakshya 19:79e69017c855 298 for(int i=0; i<3; i++)
lakshya 19:79e69017c855 299 FCTN_CONVERT_FLOAT((float)actual_data.AngularSpeed_actual[i],&telemetry[38+(i*4)]);
lakshya 19:79e69017c855 300
lakshya 19:79e69017c855 301 //printf("\n\rthe value is 38\t %x\n",telemetry[38]);
lakshya 19:79e69017c855 302 //printf("\n\rthe value is 39\t%x\n",telemetry[39]);
lakshya 19:79e69017c855 303 //printf("\n\rthe value is 40\t%x\n",telemetry[40]);
lakshya 19:79e69017c855 304 //printf("\n\rthe value is 41\t%x\n",telemetry[41]);
lakshya 19:79e69017c855 305 //printf("\n\rthe value true\t%f\n",actual_data.AngularSpeed_actual[0]);
lakshya 19:79e69017c855 306
lakshya 19:79e69017c855 307 //uint32_t input_stage1=0x00000000;
lakshya 19:79e69017c855 308 //uint8_t output1[4];
lakshya 19:79e69017c855 309 //output1[0]=(uint32_t)(telemetry[38]);
lakshya 19:79e69017c855 310 //output1[1]=(uint32_t)(telemetry[39]);
lakshya 19:79e69017c855 311 //output1[2]=(uint32_t)(telemetry[40]);
lakshya 19:79e69017c855 312 //output1[3]=(uint32_t)(telemetry[41]);
lakshya 19:79e69017c855 313
lakshya 19:79e69017c855 314 //input_stage1=output[3]+(output[2]*(0x100))+(output[1]*(0x10000))+(output[0]*(0x1000000));
lakshya 19:79e69017c855 315 //input_stage1=(output1[0]<<24) | (output1[1]<<16) | (output1[2]<<8) | (output1[3]);
lakshya 19:79e69017c855 316
lakshya 19:79e69017c855 317 //assert(sizeof(float) == sizeof(uint32_t));
lakshya 19:79e69017c855 318 //float* temp1 = reinterpret_cast<float*>(&input_stage1);
lakshya 19:79e69017c855 319
lakshya 19:79e69017c855 320 //printf("\n\r the value is: %f \n",*temp1);
lakshya 19:79e69017c855 321
lakshya 19:79e69017c855 322 //FAULT_FLAG();
lakshya 19:79e69017c855 323 telemetry[50] = actual_data.faultIr_status;
lakshya 19:79e69017c855 324 telemetry[51] = actual_data.faultPoll_status;
lakshya 19:79e69017c855 325 //Bdot Rotation Speed of Command telemetry[52-53]
lakshya 19:79e69017c855 326 //Bdot Output Current telemetry[54]
lakshya 19:79e69017c855 327 //float l_pmw1 = (PWM1);
lakshya 19:79e69017c855 328 //float l_pmw2 = PWM2;
lakshya 19:79e69017c855 329 //float l_pmw3 = PWM3;
lakshya 19:79e69017c855 330
lakshya 19:79e69017c855 331 /*__________________________________________________________________*/
lakshya 19:79e69017c855 332
lakshya 19:79e69017c855 333 /*change and check if changing it to PWM1 causes problem*/
lakshya 19:79e69017c855 334
lakshya 19:79e69017c855 335 /*___________________________________________________________________*/
lakshya 19:79e69017c855 336
lakshya 19:79e69017c855 337 float PWM_measured[3];
lakshya 19:79e69017c855 338
lakshya 19:79e69017c855 339 PWM_measured[0] = PWM1.read();
lakshya 19:79e69017c855 340 PWM_measured[1] = PWM2.read();
lakshya 19:79e69017c855 341 PWM_measured[2] = PWM3.read();
lakshya 19:79e69017c855 342
lakshya 19:79e69017c855 343 FCTN_CONVERT_FLOAT(PWM_measured[0], &telemetry[55]);
lakshya 19:79e69017c855 344 FCTN_CONVERT_FLOAT(PWM_measured[1], &telemetry[59]);
lakshya 19:79e69017c855 345 FCTN_CONVERT_FLOAT(PWM_measured[2], &telemetry[63]);
lakshya 19:79e69017c855 346 float attitude_ang = angle(actual_data.Bvalue_actual[0],actual_data.Bvalue_actual[1],actual_data.Bvalue_actual[2]);
lakshya 19:79e69017c855 347 FCTN_CONVERT_FLOAT(attitude_ang, &telemetry[67]);
lakshya 19:79e69017c855 348
lakshya 19:79e69017c855 349 for (int i=0; i<16; i++)
lakshya 19:79e69017c855 350 telemetry[68+i] = quant_data.voltage_quant[i];
lakshya 19:79e69017c855 351 for (int i=0; i<12; i++)
lakshya 19:79e69017c855 352 telemetry[84+i] = quant_data.current_quant[i];
lakshya 19:79e69017c855 353 //telemetry[96]
lakshya 19:79e69017c855 354 //telemetry[97]
lakshya 19:79e69017c855 355 //telemetry[98]
lakshya 19:79e69017c855 356 //telemetry[99]
lakshya 19:79e69017c855 357 telemetry[100] = quant_data.Batt_voltage_quant;
lakshya 19:79e69017c855 358 telemetry[101] = quant_data.BAE_temp_quant;
lakshya 19:79e69017c855 359 telemetry[102] = quant_data.Batt_gauge_quant[1];
lakshya 19:79e69017c855 360 telemetry[103] = quant_data.Batt_temp_quant[0];
lakshya 19:79e69017c855 361 telemetry[104] = quant_data.Batt_temp_quant[1];
lakshya 19:79e69017c855 362
lakshya 19:79e69017c855 363 //telemetry[105] = beacon temperature;
lakshya 19:79e69017c855 364
lakshya 19:79e69017c855 365 for (int i=105; i<132;i++)
lakshya 19:79e69017c855 366 {
lakshya 19:79e69017c855 367 telemetry[i] = 0x00;
lakshya 19:79e69017c855 368 }
lakshya 19:79e69017c855 369 crc16 = CRC::crc16_gen(telemetry,132);
lakshya 19:79e69017c855 370 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 371 telemetry[133] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 372
lakshya 19:79e69017c855 373 break;
lakshya 19:79e69017c855 374 }
lakshya 19:79e69017c855 375 case 0x0001:
lakshya 19:79e69017c855 376 {
lakshya 19:79e69017c855 377 printf("\r\nhk data tm");
lakshya 19:79e69017c855 378 telemetry[0] = 0x60;
lakshya 19:79e69017c855 379 telemetry[1] = tc[0];
lakshya 19:79e69017c855 380 telemetry[2] = ACK_CODE;
lakshya 19:79e69017c855 381
lakshya 19:79e69017c855 382 for(int i;i<16;i++)
lakshya 19:79e69017c855 383 telemetry[i+3] = bae_HK_minmax.voltage_max[i];
lakshya 19:79e69017c855 384
lakshya 19:79e69017c855 385 for(int i;i<12;i++)
lakshya 19:79e69017c855 386 telemetry[i+18] = bae_HK_minmax.current_max[i];
lakshya 19:79e69017c855 387
lakshya 19:79e69017c855 388 telemetry[29] = bae_HK_minmax.Batt_voltage_max;;
lakshya 19:79e69017c855 389 telemetry[30] = bae_HK_minmax.BAE_temp_max;
lakshya 19:79e69017c855 390
lakshya 19:79e69017c855 391 /*battery soc*/
lakshya 19:79e69017c855 392 //telemetry[31] = BAE_HK_min_max bae_HK_minmax.voltage_max;
lakshya 19:79e69017c855 393
lakshya 19:79e69017c855 394 telemetry[32] = bae_HK_minmax.Batt_temp_max[0];
lakshya 19:79e69017c855 395 telemetry[33] = bae_HK_minmax.Batt_temp_max[1];
lakshya 19:79e69017c855 396
lakshya 19:79e69017c855 397 /*BCN temp not there*/
lakshya 19:79e69017c855 398 //telemetry[34] = BAE_HK_min_max bae_HK_minmax.;
lakshya 19:79e69017c855 399
lakshya 19:79e69017c855 400 for(int i=0; i<3; i++)
lakshya 19:79e69017c855 401 FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[i],&telemetry[35+(i*4)]);
lakshya 19:79e69017c855 402
lakshya 19:79e69017c855 403 for(int i=0; i<3; i++)
lakshya 19:79e69017c855 404 FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[i],&telemetry[47+(i*4)]);
lakshya 19:79e69017c855 405
lakshya 19:79e69017c855 406 /*min data*/
lakshya 19:79e69017c855 407
lakshya 19:79e69017c855 408 for(int i;i<16;i++)
lakshya 19:79e69017c855 409 telemetry[i+59] = bae_HK_minmax.voltage_min[i];
lakshya 19:79e69017c855 410
lakshya 19:79e69017c855 411 for(int i;i<12;i++)
lakshya 19:79e69017c855 412 telemetry[i+74] = bae_HK_minmax.current_min[i];
lakshya 19:79e69017c855 413
lakshya 19:79e69017c855 414 telemetry[86] = bae_HK_minmax.Batt_voltage_min;
lakshya 19:79e69017c855 415 telemetry[87] = bae_HK_minmax.BAE_temp_min;
lakshya 19:79e69017c855 416
lakshya 19:79e69017c855 417 /*battery soc*/
lakshya 19:79e69017c855 418 //telemetry[88] = BAE_HK_min_max bae_HK_minmax.voltage_max;
lakshya 19:79e69017c855 419
lakshya 19:79e69017c855 420 telemetry[89] = bae_HK_minmax.Batt_temp_min[0];
lakshya 19:79e69017c855 421 telemetry[90] = bae_HK_minmax.Batt_temp_min[1];
lakshya 19:79e69017c855 422 //huhu//
lakshya 19:79e69017c855 423
lakshya 19:79e69017c855 424 /*BCN temp not there*/
lakshya 19:79e69017c855 425 //telemetry[91] = BAE_HK_min_max bae_HK_minmax.;
lakshya 19:79e69017c855 426
lakshya 19:79e69017c855 427 for(int i=0; i<3; i++)
lakshya 19:79e69017c855 428 FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[i],&telemetry[91+(i*4)]);
lakshya 19:79e69017c855 429
lakshya 19:79e69017c855 430 for(int i=0; i<3; i++)
lakshya 19:79e69017c855 431 FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[i],&telemetry[103+(i*4)]);
lakshya 19:79e69017c855 432
lakshya 19:79e69017c855 433
lakshya 19:79e69017c855 434 for (int i=115; i<132;i++)
lakshya 19:79e69017c855 435 {
lakshya 19:79e69017c855 436 telemetry[i] = 0x00;
lakshya 19:79e69017c855 437 }
lakshya 19:79e69017c855 438 crc16 = CRC::crc16_gen(telemetry,132);
lakshya 19:79e69017c855 439 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 440 telemetry[133] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 441 break;
lakshya 19:79e69017c855 442 }
lakshya 19:79e69017c855 443 default://invalid MID
lakshya 19:79e69017c855 444 {
lakshya 19:79e69017c855 445 //ACK_L234_telemetry
lakshya 19:79e69017c855 446 telemetry[0]=0xB0;
lakshya 19:79e69017c855 447 telemetry[1]=tc[0];
lakshya 19:79e69017c855 448 telemetry[2]=0x02;//for this case
lakshya 19:79e69017c855 449 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 450 {
lakshya 19:79e69017c855 451 telemetry[i]=0x00;
lakshya 19:79e69017c855 452 }
lakshya 19:79e69017c855 453 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 454 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 455 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 456 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 457 {
lakshya 19:79e69017c855 458 telemetry[i]=0x00;
lakshya 19:79e69017c855 459 }
lakshya 19:79e69017c855 460 break;
lakshya 19:79e69017c855 461 }
lakshya 19:79e69017c855 462 }
lakshya 19:79e69017c855 463 break;
lakshya 19:79e69017c855 464 }
lakshya 19:79e69017c855 465 case 0x05:
lakshya 19:79e69017c855 466 {
lakshya 19:79e69017c855 467 printf("\n\rdata for mms 0x05 flash");
lakshya 19:79e69017c855 468 /*changed*/
lakshya 19:79e69017c855 469 printf("\n\rwrite on flash\n");
lakshya 19:79e69017c855 470 uint32_t FLASH_DATA[8];//256 bits
lakshya 19:79e69017c855 471
lakshya 19:79e69017c855 472 uint8_t VALID_MID;//to determine wether mid is valid or not otherwise to much repetition of code 1 meaning valid
lakshya 19:79e69017c855 473
lakshya 19:79e69017c855 474 uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
lakshya 19:79e69017c855 475 switch(MID )
lakshya 19:79e69017c855 476 {
lakshya 19:79e69017c855 477 case 0x0100:
lakshya 19:79e69017c855 478 {
lakshya 19:79e69017c855 479 FLASH_DATA[0] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 19:79e69017c855 480 FCTN_BAE_WR_FLASH(0x00,FLASH_DATA[0]);
lakshya 19:79e69017c855 481 VALID_MID=1;
lakshya 19:79e69017c855 482 break;
lakshya 19:79e69017c855 483 }
lakshya 19:79e69017c855 484 case 0x0101:
lakshya 19:79e69017c855 485 {
lakshya 19:79e69017c855 486 FLASH_DATA[1] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 19:79e69017c855 487 FCTN_BAE_WR_FLASH(0x01,FLASH_DATA[1]);
lakshya 19:79e69017c855 488 VALID_MID=1;
lakshya 19:79e69017c855 489 break;
lakshya 19:79e69017c855 490 }
lakshya 19:79e69017c855 491
lakshya 19:79e69017c855 492 case 0x0102:
lakshya 19:79e69017c855 493 {
lakshya 19:79e69017c855 494 FLASH_DATA[2] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 19:79e69017c855 495 FCTN_BAE_WR_FLASH(0x02,FLASH_DATA[2]);
lakshya 19:79e69017c855 496 VALID_MID=1;
lakshya 19:79e69017c855 497 break;
lakshya 19:79e69017c855 498 }
lakshya 19:79e69017c855 499
lakshya 19:79e69017c855 500 case 0x0103:
lakshya 19:79e69017c855 501 {
lakshya 19:79e69017c855 502 FLASH_DATA[3] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 19:79e69017c855 503 FCTN_BAE_WR_FLASH(0x03,FLASH_DATA[3]);
lakshya 19:79e69017c855 504 VALID_MID=1;
lakshya 19:79e69017c855 505 break;
lakshya 19:79e69017c855 506 }
lakshya 19:79e69017c855 507 case 0x0104:
lakshya 19:79e69017c855 508 {
lakshya 19:79e69017c855 509 FLASH_DATA[4] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 19:79e69017c855 510 FCTN_BAE_WR_FLASH(0x04,FLASH_DATA[4]);
lakshya 19:79e69017c855 511 VALID_MID=1;
lakshya 19:79e69017c855 512 break;
lakshya 19:79e69017c855 513 }
lakshya 19:79e69017c855 514
lakshya 19:79e69017c855 515 case 0x0105:
lakshya 19:79e69017c855 516 {
lakshya 19:79e69017c855 517 FLASH_DATA[5] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 19:79e69017c855 518 FCTN_BAE_WR_FLASH(0x05,FLASH_DATA[5]);
lakshya 19:79e69017c855 519 VALID_MID=1;
lakshya 19:79e69017c855 520 break;
lakshya 19:79e69017c855 521 }
lakshya 19:79e69017c855 522 case 0x0106:
lakshya 19:79e69017c855 523 {
lakshya 19:79e69017c855 524 FLASH_DATA[6] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 19:79e69017c855 525 FCTN_BAE_WR_FLASH(0x06,FLASH_DATA[6]);
lakshya 19:79e69017c855 526 VALID_MID=1;
lakshya 19:79e69017c855 527 break;
lakshya 19:79e69017c855 528 }
lakshya 19:79e69017c855 529
lakshya 19:79e69017c855 530 case 0x0107:
lakshya 19:79e69017c855 531 {
lakshya 19:79e69017c855 532 FLASH_DATA[7] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 19:79e69017c855 533 FCTN_BAE_WR_FLASH(0x07,FLASH_DATA[7]);
lakshya 19:79e69017c855 534 VALID_MID=1;
lakshya 19:79e69017c855 535 break;
lakshya 19:79e69017c855 536 }
lakshya 19:79e69017c855 537
lakshya 19:79e69017c855 538 default:
lakshya 19:79e69017c855 539 {
lakshya 19:79e69017c855 540 printf("Invalid MMS case 0x05 invalid MID\r\n");
lakshya 19:79e69017c855 541 VALID_MID=0;
lakshya 19:79e69017c855 542 //ACK_L234_telemetry
lakshya 19:79e69017c855 543 break;
lakshya 19:79e69017c855 544
lakshya 19:79e69017c855 545 }
lakshya 19:79e69017c855 546 }
lakshya 19:79e69017c855 547
lakshya 19:79e69017c855 548 if(VALID_MID==1)//valid MID
lakshya 19:79e69017c855 549 {
lakshya 19:79e69017c855 550 telemetry[0]=0xB0;//or 0x60? check
lakshya 19:79e69017c855 551 telemetry[1]=tc[0];
lakshya 19:79e69017c855 552 telemetry[2]=0xA0;// when valid
lakshya 19:79e69017c855 553 }
lakshya 19:79e69017c855 554 else if(VALID_MID==0)//invalid MID
lakshya 19:79e69017c855 555 {
lakshya 19:79e69017c855 556 telemetry[0]=0xB0;
lakshya 19:79e69017c855 557 telemetry[1]=tc[0];
lakshya 19:79e69017c855 558 telemetry[2]=0x02;//for this case
lakshya 19:79e69017c855 559 }
lakshya 19:79e69017c855 560
lakshya 19:79e69017c855 561 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 562 {
lakshya 19:79e69017c855 563 telemetry[i]=0x00;
lakshya 19:79e69017c855 564 }
lakshya 19:79e69017c855 565 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 566 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 567 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 568 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 569 {
lakshya 19:79e69017c855 570 telemetry[i]=0x00;
lakshya 19:79e69017c855 571 }
lakshya 19:79e69017c855 572
lakshya 19:79e69017c855 573 printf("\n\rWritten on Flash");
lakshya 19:79e69017c855 574 break;
lakshya 19:79e69017c855 575 }
lakshya 19:79e69017c855 576 default://when invalid service subtype
lakshya 19:79e69017c855 577 {
lakshya 19:79e69017c855 578 printf("\n\r MMS invalid Service Subtype");
lakshya 19:79e69017c855 579 //ACK_L234_telemetry
lakshya 19:79e69017c855 580 telemetry[0]=0xB0;
lakshya 19:79e69017c855 581 telemetry[1]=tc[0];
lakshya 19:79e69017c855 582 telemetry[2]=0x02;//for this case
lakshya 19:79e69017c855 583 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 584 {
lakshya 19:79e69017c855 585 telemetry[i]=0x00;
lakshya 19:79e69017c855 586 }
lakshya 19:79e69017c855 587 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 588 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 589 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 590 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 591 {
lakshya 19:79e69017c855 592 telemetry[i]=0x00;
lakshya 19:79e69017c855 593 }
lakshya 19:79e69017c855 594 break;
lakshya 19:79e69017c855 595 }
lakshya 19:79e69017c855 596 }
lakshya 19:79e69017c855 597 break;
lakshya 19:79e69017c855 598 }
lakshya 19:79e69017c855 599 case 0x80:
lakshya 19:79e69017c855 600 {
lakshya 19:79e69017c855 601 //printf("Function Management Service\r\n");
lakshya 19:79e69017c855 602 uint8_t service_subtype=(tc[2]&0x0F);
lakshya 19:79e69017c855 603 switch(service_subtype)
lakshya 19:79e69017c855 604 {
lakshya 19:79e69017c855 605 case 0x01:
lakshya 19:79e69017c855 606 {
lakshya 19:79e69017c855 607 printf("\n\rFMS Activated");
lakshya 19:79e69017c855 608 uint8_t fid=tc[3];//changed from pid to fid
lakshya 19:79e69017c855 609 switch(fid)
lakshya 19:79e69017c855 610 {
lakshya 19:79e69017c855 611 case 0xE0:
lakshya 19:79e69017c855 612 {
lakshya 19:79e69017c855 613 float B[3],W[3];
lakshya 19:79e69017c855 614 printf("ACS_COMSN\r\n");
lakshya 19:79e69017c855 615 //ACK_L234_telemetry
lakshya 19:79e69017c855 616
lakshya 19:79e69017c855 617 uint8_t B_x[2];
lakshya 19:79e69017c855 618 uint8_t B_y[2];
lakshya 19:79e69017c855 619 uint8_t B_z[2];
lakshya 19:79e69017c855 620 uint8_t W_x[2];
lakshya 19:79e69017c855 621 uint8_t W_y[2];
lakshya 19:79e69017c855 622 uint8_t W_z[2];
lakshya 19:79e69017c855 623
lakshya 19:79e69017c855 624 B_x[0]=tc[3];
lakshya 19:79e69017c855 625 B_x[1]=tc[4];
lakshya 19:79e69017c855 626 B_y[0]=tc[5];
lakshya 19:79e69017c855 627 B_y[1]=tc[6];
lakshya 19:79e69017c855 628 B_z[0]=tc[7];
lakshya 19:79e69017c855 629 B_z[1]=tc[8];
lakshya 19:79e69017c855 630
lakshya 19:79e69017c855 631 W_x[0]=tc[9];
lakshya 19:79e69017c855 632 W_x[1]=tc[10];
lakshya 19:79e69017c855 633 W_y[0]=tc[11];
lakshya 19:79e69017c855 634 W_y[1]=tc[12];
lakshya 19:79e69017c855 635 W_z[0]=tc[13];
lakshya 19:79e69017c855 636 W_z[1]=tc[14];
lakshya 19:79e69017c855 637
lakshya 19:79e69017c855 638 FCTN_CONVERT_UINT(B_x,&B[0]);
lakshya 19:79e69017c855 639 FCTN_CONVERT_UINT(B_y,&B[1]);
lakshya 19:79e69017c855 640 FCTN_CONVERT_UINT(B_z,&B[2]);
lakshya 19:79e69017c855 641
lakshya 19:79e69017c855 642 FCTN_CONVERT_UINT (W_x, &W[0]);
lakshya 19:79e69017c855 643 FCTN_CONVERT_UINT (W_y, &W[1]);
lakshya 19:79e69017c855 644 FCTN_CONVERT_UINT (W_z, &W[2]);
lakshya 19:79e69017c855 645
lakshya 19:79e69017c855 646 telemetry[0]=0xF0;
lakshya 19:79e69017c855 647 telemetry[1]=tc[0];
lakshya 19:79e69017c855 648 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 649 //FCTN_ATS_DATA_ACQ(); //get data
lakshya 19:79e69017c855 650 printf("gyro values\n\r");
lakshya 19:79e69017c855 651 for(int i=0; i<3; i++)
lakshya 19:79e69017c855 652 printf("%f\n\r",W[i]);
lakshya 19:79e69017c855 653 printf("mag values\n\r");
lakshya 19:79e69017c855 654 for(int i=0; i<3; i++)
lakshya 19:79e69017c855 655 printf("%f\n\r",B[i]);
lakshya 19:79e69017c855 656 /* FCTN_CONVERT_FLOAT(data[0],&telemetry[4]); //telemetry[4] - telemetry[7]
lakshya 19:79e69017c855 657 FCTN_CONVERT_FLOAT(data[1],&telemetry[8]); //telemetry[8] - telemetry[11]
lakshya 19:79e69017c855 658 FCTN_CONVERT_FLOAT(data[2],&telemetry[12]); //telemetry[12] - telemetry[15]
lakshya 19:79e69017c855 659 FCTN_CONVERT_FLOAT(data[0],&telemetry[16]); //telemetry[16] - telemetry[19]
lakshya 19:79e69017c855 660 FCTN_CONVERT_FLOAT(data[1],&telemetry[20]); //telemetry[20] - telemetry[23]
lakshya 19:79e69017c855 661 FCTN_CONVERT_FLOAT(data[2],&telemetry[24]); //telemetry[24] - telemetry[27]
lakshya 19:79e69017c855 662 if((data[0]<8) && (data[1]<8) && (data[2] <8))
lakshya 19:79e69017c855 663 telemetry[28] = 1; // gyro values in correct range
lakshya 19:79e69017c855 664 else
lakshya 19:79e69017c855 665 telemetry[28] = 0;
lakshya 19:79e69017c855 666 if ((data[3] > 20 ) && (data[4] >20) && (data[5]>20)&& (data[3] < 50 ) && (data[4] <50) && (data[5]<50))
lakshya 19:79e69017c855 667 telemetry[29] = 1; // mag values in correct range
lakshya 19:79e69017c855 668 else
lakshya 19:79e69017c855 669 telemetry[29] = 0;
lakshya 19:79e69017c855 670 */
lakshya 19:79e69017c855 671 // float B[3],W[3];
lakshya 19:79e69017c855 672 // B[0] = B0;
lakshya 19:79e69017c855 673 // B[1] = B1;
lakshya 19:79e69017c855 674 // B[2] = B2;
lakshya 19:79e69017c855 675 // W[0] = W0;
lakshya 19:79e69017c855 676 // W[1] = W1;
lakshya 19:79e69017c855 677 // W[2] = W2;
lakshya 19:79e69017c855 678 // Control algo commissioning
lakshya 19:79e69017c855 679
lakshya 19:79e69017c855 680 /* FCTN_ACS_CNTRLALGO(B,W);
lakshya 19:79e69017c855 681 FCTN_CONVERT_FLOAT(moment[0],&telemetry[30]); //telemetry[30] - telemetry[33]
lakshya 19:79e69017c855 682 FCTN_CONVERT_FLOAT(moment[1],&telemetry[34]); //telemetry[34] - telemetry[37]
lakshya 19:79e69017c855 683 FCTN_CONVERT_FLOAT(moment[2],&telemetry[38]); //telemetry[38] - telemetry[41]
lakshya 19:79e69017c855 684 // to include commission TR as well
lakshya 19:79e69017c855 685 for(uint8_t i=42;i<132;i++)
lakshya 19:79e69017c855 686 {
lakshya 19:79e69017c855 687 telemetry[i]=0x00;
lakshya 19:79e69017c855 688 }
lakshya 19:79e69017c855 689
lakshya 19:79e69017c855 690 crc16 = CRC::crc16_gen(telemetry,132);
lakshya 19:79e69017c855 691 telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 692 telemetry[134] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 693 break;
lakshya 19:79e69017c855 694 */
lakshya 19:79e69017c855 695
lakshya 19:79e69017c855 696 // Control algo commissioning
lakshya 19:79e69017c855 697 FCTN_ACS_CNTRLALGO(B,W);
lakshya 19:79e69017c855 698 FCTN_CONVERT_FLOAT(moment[0],&telemetry[4]); //telemetry[4] - telemetry[7]
lakshya 19:79e69017c855 699 FCTN_CONVERT_FLOAT(moment[1],&telemetry[8]); //telemetry[8] - telemetry[11]
lakshya 19:79e69017c855 700 FCTN_CONVERT_FLOAT(moment[2],&telemetry[12]); //telemetry[12] - telemetry[15]
lakshya 19:79e69017c855 701 // to include commission TR as well
lakshya 19:79e69017c855 702 for(uint8_t i=16;i<132;i++)
lakshya 19:79e69017c855 703 {
lakshya 19:79e69017c855 704 telemetry[i]=0x00;
lakshya 19:79e69017c855 705 }
lakshya 19:79e69017c855 706
lakshya 19:79e69017c855 707 crc16 = CRC::crc16_gen(telemetry,132);
lakshya 19:79e69017c855 708 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 709 telemetry[133] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 710 break;
lakshya 19:79e69017c855 711 }
lakshya 19:79e69017c855 712 case 0xE1:
lakshya 19:79e69017c855 713 {
lakshya 19:79e69017c855 714 float moment_tc[3];
lakshya 19:79e69017c855 715 printf("\n\rHARDWARE_COMSN");
lakshya 19:79e69017c855 716 //ACK_L234_telemetry
lakshya 19:79e69017c855 717 uint8_t M0[2];
lakshya 19:79e69017c855 718 uint8_t M1[2];
lakshya 19:79e69017c855 719 uint8_t M2[2];
lakshya 19:79e69017c855 720
lakshya 19:79e69017c855 721 M0[0]=tc[3];
lakshya 19:79e69017c855 722 M0[1]=tc[4];
lakshya 19:79e69017c855 723 M1[0]=tc[5];
lakshya 19:79e69017c855 724 M1[1]=tc[6];
lakshya 19:79e69017c855 725 M2[0]=tc[7];
lakshya 19:79e69017c855 726 M2[1]=tc[8];
lakshya 19:79e69017c855 727
lakshya 19:79e69017c855 728
lakshya 19:79e69017c855 729 telemetry[0]=0xF0;
lakshya 19:79e69017c855 730 telemetry[1]=tc[0];
lakshya 19:79e69017c855 731 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 732
lakshya 19:79e69017c855 733 float PWM_measured[3];
lakshya 19:79e69017c855 734
lakshya 19:79e69017c855 735 FCTN_CONVERT_UINT(M0,&moment_tc[0]);
lakshya 19:79e69017c855 736 moment_tc[1] = 0;
lakshya 19:79e69017c855 737 moment_tc[2] = 0;
lakshya 19:79e69017c855 738 FCTN_ACS_GENPWM_MAIN(moment_tc);
lakshya 19:79e69017c855 739 PWM_measured[0] = PWM1.read();
lakshya 19:79e69017c855 740 FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (0*4)]);
lakshya 19:79e69017c855 741
lakshya 19:79e69017c855 742 FCTN_CONVERT_UINT(M1, &moment_tc[1]);
lakshya 19:79e69017c855 743 moment_tc[0] = 0;
lakshya 19:79e69017c855 744 moment_tc[2] = 0;
lakshya 19:79e69017c855 745 FCTN_ACS_GENPWM_MAIN(moment_tc);
lakshya 19:79e69017c855 746 PWM_measured[1] = PWM2.read();
lakshya 19:79e69017c855 747 FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (1*4)]);
lakshya 19:79e69017c855 748
lakshya 19:79e69017c855 749 FCTN_CONVERT_UINT(M2, &moment_tc[2]);
lakshya 19:79e69017c855 750 moment_tc[0] = 0;
lakshya 19:79e69017c855 751 moment_tc[1] = 0;
lakshya 19:79e69017c855 752 FCTN_ACS_GENPWM_MAIN(moment_tc);
lakshya 19:79e69017c855 753 PWM_measured[2] = PWM3.read();
lakshya 19:79e69017c855 754 FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (2*4)]);
lakshya 19:79e69017c855 755
lakshya 19:79e69017c855 756 FCTN_CONVERT_FLOAT(PWM_measured[0],&telemetry[4]); //4-7
lakshya 19:79e69017c855 757 FCTN_CONVERT_FLOAT(PWM_measured[1],&telemetry[8]); //8-11
lakshya 19:79e69017c855 758 FCTN_CONVERT_FLOAT(PWM_measured[2],&telemetry[12]); //12-15
lakshya 19:79e69017c855 759
lakshya 19:79e69017c855 760 // for(int i=0; i<12; i++)
lakshya 19:79e69017c855 761 // FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]);
lakshya 19:79e69017c855 762
lakshya 19:79e69017c855 763 // FCTN_ATS_DATA_ACQ(); //get data
lakshya 19:79e69017c855 764
lakshya 19:79e69017c855 765 // to include commission TR as well
lakshya 19:79e69017c855 766 for(uint8_t i=28;i<132;i++)
lakshya 19:79e69017c855 767 {
lakshya 19:79e69017c855 768 telemetry[i]=0x00;
lakshya 19:79e69017c855 769 }
lakshya 19:79e69017c855 770
lakshya 19:79e69017c855 771 crc16 = CRC::crc16_gen(telemetry,132);
lakshya 19:79e69017c855 772 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 773 telemetry[133] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 774 break;
lakshya 19:79e69017c855 775 }
lakshya 19:79e69017c855 776 case 0xE2:
lakshya 19:79e69017c855 777 {
lakshya 19:79e69017c855 778 uint8_t STANDBY_STATUS_BCN;
lakshya 19:79e69017c855 779 STANDBY_STATUS_BCN=tc[4];
lakshya 19:79e69017c855 780 if(STANDBY_STATUS_BCN==0x00)
lakshya 19:79e69017c855 781 {
lakshya 19:79e69017c855 782 BCN_STANDBY=0;
lakshya 19:79e69017c855 783 //stop BCN_STANDBY_TIMER.stop();//create
lakshya 19:79e69017c855 784 telemetry[2]=0xA0;
lakshya 19:79e69017c855 785 }
lakshya 19:79e69017c855 786 else if(STANDBY_STATUS_BCN==0x01)
lakshya 19:79e69017c855 787 {
lakshya 19:79e69017c855 788 BCN_STANDBY=1;
lakshya 19:79e69017c855 789 //stop BCN_STANDBY_TIMER.start();//create
lakshya 19:79e69017c855 790 if(BCN_TX_MAIN_STATUS==0)
lakshya 19:79e69017c855 791 {
lakshya 19:79e69017c855 792 telemetry[2]=0xA0;
lakshya 19:79e69017c855 793 }
lakshya 19:79e69017c855 794 else if(BCN_TX_MAIN_STATUS==1)
lakshya 19:79e69017c855 795 {
lakshya 19:79e69017c855 796 telemetry[2]=0xC0;
lakshya 19:79e69017c855 797 }
lakshya 19:79e69017c855 798 }
lakshya 19:79e69017c855 799 else
lakshya 19:79e69017c855 800 {
lakshya 19:79e69017c855 801 telemetry[2]=0x02;
lakshya 19:79e69017c855 802 }
lakshya 19:79e69017c855 803 //ACK_L234_telemetry
lakshya 19:79e69017c855 804 telemetry[0]=0xB0;
lakshya 19:79e69017c855 805 telemetry[1]=tc[0];
lakshya 19:79e69017c855 806 //ack_code taken care above
lakshya 19:79e69017c855 807 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 808 {
lakshya 19:79e69017c855 809 telemetry[i]=0x00;
lakshya 19:79e69017c855 810 }
lakshya 19:79e69017c855 811 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 812 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 813 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 814 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 815 {
lakshya 19:79e69017c855 816 telemetry[i]=0x00;
lakshya 19:79e69017c855 817 }
lakshya 19:79e69017c855 818 break;
lakshya 19:79e69017c855 819 }
lakshya 19:79e69017c855 820 case 0x01:
lakshya 19:79e69017c855 821 { if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 822 {
lakshya 19:79e69017c855 823 printf("\n\rRun P_EPS_INIT");
lakshya 19:79e69017c855 824 FCTN_EPS_INIT();
lakshya 19:79e69017c855 825 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 826 }
lakshya 19:79e69017c855 827 else
lakshya 19:79e69017c855 828 {
lakshya 19:79e69017c855 829 printf("\n\runable to Run P_EPS_INIT as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 830 telemetry[2]=0x87;
lakshya 19:79e69017c855 831 }
lakshya 19:79e69017c855 832
lakshya 19:79e69017c855 833 //ACK_L234_telemetry
lakshya 19:79e69017c855 834 telemetry[0]=0xB0;
lakshya 19:79e69017c855 835 telemetry[1]=tc[0];
lakshya 19:79e69017c855 836 //ACK code taken care of
lakshya 19:79e69017c855 837 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 838 {
lakshya 19:79e69017c855 839 telemetry[i]=0x00;
lakshya 19:79e69017c855 840 }
lakshya 19:79e69017c855 841 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 842 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 843 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 844 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 845 {
lakshya 19:79e69017c855 846 telemetry[i]=0x00;
lakshya 19:79e69017c855 847 }
lakshya 19:79e69017c855 848
lakshya 19:79e69017c855 849 break;
lakshya 19:79e69017c855 850 }
lakshya 19:79e69017c855 851 case 0x02:
lakshya 19:79e69017c855 852 {
lakshya 19:79e69017c855 853 if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 854 {
lakshya 19:79e69017c855 855 printf("\n\rRun P_EPS_MAIN");
lakshya 19:79e69017c855 856 F_EPS();
lakshya 19:79e69017c855 857 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 858 }
lakshya 19:79e69017c855 859 else
lakshya 19:79e69017c855 860 {
lakshya 19:79e69017c855 861 printf("\n\runable to Run P_EPS_MAIN as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 862 telemetry[2]=0x87;
lakshya 19:79e69017c855 863 }
lakshya 19:79e69017c855 864
lakshya 19:79e69017c855 865 //ACK_L234_telemetry
lakshya 19:79e69017c855 866 telemetry[0]=0xB0;
lakshya 19:79e69017c855 867 telemetry[1]=tc[0];
lakshya 19:79e69017c855 868 //ACK code taken care of
lakshya 19:79e69017c855 869 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 870 {
lakshya 19:79e69017c855 871 telemetry[i]=0x00;
lakshya 19:79e69017c855 872 }
lakshya 19:79e69017c855 873 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 874 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 875 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 876 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 877 {
lakshya 19:79e69017c855 878 telemetry[i]=0x00;
lakshya 19:79e69017c855 879 }
lakshya 19:79e69017c855 880 break;
lakshya 19:79e69017c855 881 }
lakshya 19:79e69017c855 882 case 0x03:
lakshya 19:79e69017c855 883 {
lakshya 19:79e69017c855 884 if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 885 {
lakshya 19:79e69017c855 886 printf("\n\rRun P_ACS_INIT");
lakshya 19:79e69017c855 887 FCTN_ACS_INIT();
lakshya 19:79e69017c855 888 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 889 }
lakshya 19:79e69017c855 890 else
lakshya 19:79e69017c855 891 {
lakshya 19:79e69017c855 892 printf("\n\runable to Run P_ACS_INIT as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 893 telemetry[2]=0x87;
lakshya 19:79e69017c855 894 }
lakshya 19:79e69017c855 895
lakshya 19:79e69017c855 896 //ACK_L234_telemetry
lakshya 19:79e69017c855 897 telemetry[0]=0xB0;
lakshya 19:79e69017c855 898 telemetry[1]=tc[0];
lakshya 19:79e69017c855 899 //ACK CODE TAKEN CARE OF
lakshya 19:79e69017c855 900 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 901 {
lakshya 19:79e69017c855 902 telemetry[i]=0x00;
lakshya 19:79e69017c855 903 }
lakshya 19:79e69017c855 904 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 905 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 906 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 907 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 908 {
lakshya 19:79e69017c855 909 telemetry[i]=0x00;
lakshya 19:79e69017c855 910 }
lakshya 19:79e69017c855 911 break;
lakshya 19:79e69017c855 912 }
lakshya 19:79e69017c855 913 case 0x05:
lakshya 19:79e69017c855 914 {
lakshya 19:79e69017c855 915 if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 916 {
lakshya 19:79e69017c855 917 printf("\n\rRun P_ACS_MAIN");
lakshya 19:79e69017c855 918 F_ACS();
lakshya 19:79e69017c855 919 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 920 }
lakshya 19:79e69017c855 921 else
lakshya 19:79e69017c855 922 {
lakshya 19:79e69017c855 923 printf("\n\runable to Run P_ACS_MAIN as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 924 telemetry[2]=0x87;
lakshya 19:79e69017c855 925 }
lakshya 19:79e69017c855 926
lakshya 19:79e69017c855 927 //ACK_L234_TM
lakshya 19:79e69017c855 928 telemetry[0]=0xB0;
lakshya 19:79e69017c855 929 telemetry[1]=tc[0];
lakshya 19:79e69017c855 930 //ACK CODE TAKEN CARE OF
lakshya 19:79e69017c855 931 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 932 {
lakshya 19:79e69017c855 933 telemetry[i]=0x00;
lakshya 19:79e69017c855 934 }
lakshya 19:79e69017c855 935
lakshya 19:79e69017c855 936 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 937 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 938 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 939 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 940 {
lakshya 19:79e69017c855 941 telemetry[i]=0x00;
lakshya 19:79e69017c855 942 }
lakshya 19:79e69017c855 943 break;
lakshya 19:79e69017c855 944 }
lakshya 19:79e69017c855 945 case 0x06:
lakshya 19:79e69017c855 946 {
lakshya 19:79e69017c855 947 if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 948 {
lakshya 19:79e69017c855 949 printf("\n\rRun P_BCN_INIT");
lakshya 19:79e69017c855 950 F_BCN();
lakshya 19:79e69017c855 951 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 952 }
lakshya 19:79e69017c855 953 else
lakshya 19:79e69017c855 954 {
lakshya 19:79e69017c855 955 printf("\n\runable to Run P_BCN_INIT as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 956 telemetry[2]=0x87;
lakshya 19:79e69017c855 957 }
lakshya 19:79e69017c855 958
lakshya 19:79e69017c855 959 //ACK_L234_TM
lakshya 19:79e69017c855 960 telemetry[0]=0xB0;
lakshya 19:79e69017c855 961 telemetry[1]=tc[0];
lakshya 19:79e69017c855 962 //ACK CODE TAKEN CARE OF
lakshya 19:79e69017c855 963 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 964 {
lakshya 19:79e69017c855 965 telemetry[i]=0x00;
lakshya 19:79e69017c855 966 }
lakshya 19:79e69017c855 967 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 968 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 969 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 970 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 971 {
lakshya 19:79e69017c855 972 telemetry[i]=0x00;
lakshya 19:79e69017c855 973 }
lakshya 19:79e69017c855 974 break;
lakshya 19:79e69017c855 975 }
lakshya 19:79e69017c855 976 case 0x07:
lakshya 19:79e69017c855 977 {
lakshya 19:79e69017c855 978 if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 979 {
lakshya 19:79e69017c855 980 printf("\n\rRun P_BCN_TX_MAIN");
lakshya 19:79e69017c855 981 FCTN_BCN_TX_MAIN();//correct function check once
lakshya 19:79e69017c855 982 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 983 }
lakshya 19:79e69017c855 984 else
lakshya 19:79e69017c855 985 {
lakshya 19:79e69017c855 986 printf("\n\runable to Run P_BCN_TX_MAIN as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 987 telemetry[2]=0x87;
lakshya 19:79e69017c855 988 }
lakshya 19:79e69017c855 989
lakshya 19:79e69017c855 990 //ACK_L234_TM
lakshya 19:79e69017c855 991 telemetry[0]=0xB0;
lakshya 19:79e69017c855 992 telemetry[1]=tc[0];
lakshya 19:79e69017c855 993 //ACK CODE TAKEN CARE OF
lakshya 19:79e69017c855 994 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 995 {
lakshya 19:79e69017c855 996 telemetry[i]=0x00;
lakshya 19:79e69017c855 997 }
lakshya 19:79e69017c855 998 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 999 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1000 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1001 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1002 {
lakshya 19:79e69017c855 1003 telemetry[i]=0x00;
lakshya 19:79e69017c855 1004 }
lakshya 19:79e69017c855 1005 break;
lakshya 19:79e69017c855 1006 }
lakshya 19:79e69017c855 1007 case 0x11:
lakshya 19:79e69017c855 1008 {
lakshya 19:79e69017c855 1009 printf("\n\rSW_ON_ACS_ATS1_SW_ENABLE");
lakshya 19:79e69017c855 1010 //ACK_L234_TM
lakshya 19:79e69017c855 1011 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1012 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1013 //____________________________________________************************************
lakshya 19:79e69017c855 1014 /*
lakshya 19:79e69017c855 1015 ATS PIN OR STATUS YET TO BE DECIDED. DECIDED THAT IT IS PIN TC CAN SWITCH ON/OFF THE SENSOR
lakshya 19:79e69017c855 1016 */
lakshya 19:79e69017c855 1017 ATS2_SW_ENABLE = 1; // making sure we switch off the other
lakshya 19:79e69017c855 1018 ACS_ATS2_SW_STATUS=0x00;
lakshya 19:79e69017c855 1019 ATS1_SW_ENABLE = 0;
lakshya 19:79e69017c855 1020 ACS_ATS1_SW_STATUS=0x01;
lakshya 19:79e69017c855 1021 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1022 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1023 {
lakshya 19:79e69017c855 1024 telemetry[i]=0x00;
lakshya 19:79e69017c855 1025 }
lakshya 19:79e69017c855 1026 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1027 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1028 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1029 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1030 {
lakshya 19:79e69017c855 1031 telemetry[i]=0x00;
lakshya 19:79e69017c855 1032 }
lakshya 19:79e69017c855 1033 break;
lakshya 19:79e69017c855 1034 }
lakshya 19:79e69017c855 1035 case 0x12:
lakshya 19:79e69017c855 1036 {
lakshya 19:79e69017c855 1037 printf("\n\rSW_ON_ACS_ATS2_SW_ENABLE");
lakshya 19:79e69017c855 1038 //ACK_L234_TM
lakshya 19:79e69017c855 1039 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1040 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1041 ATS1_SW_ENABLE = 1; //make sure u switch off the other
lakshya 19:79e69017c855 1042 ACS_ATS1_SW_STATUS=0x00;
lakshya 19:79e69017c855 1043 ATS2_SW_ENABLE = 0;
lakshya 19:79e69017c855 1044 ACS_ATS2_SW_STATUS=0x01;
lakshya 19:79e69017c855 1045 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1046 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1047 {
lakshya 19:79e69017c855 1048 telemetry[i]=0x00;
lakshya 19:79e69017c855 1049 }
lakshya 19:79e69017c855 1050 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1051 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1052 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1053 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1054 {
lakshya 19:79e69017c855 1055 telemetry[i]=0x00;
lakshya 19:79e69017c855 1056 }
lakshya 19:79e69017c855 1057 break;
lakshya 19:79e69017c855 1058 }
lakshya 19:79e69017c855 1059 case 0x13:
lakshya 19:79e69017c855 1060 {
lakshya 19:79e69017c855 1061 printf("\n\rSW_ON_ACS_TR_XY_ENABLE");
lakshya 19:79e69017c855 1062 //ACK_L234_TM
lakshya 19:79e69017c855 1063 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1064 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1065 TRXY_SW = 1;//1 SWITCH enable here
lakshya 19:79e69017c855 1066 ACS_TR_XY_SW_STATUS=0x01;
lakshya 19:79e69017c855 1067 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1068 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1069 {
lakshya 19:79e69017c855 1070 telemetry[i]=0x00;
lakshya 19:79e69017c855 1071 }
lakshya 19:79e69017c855 1072 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1073 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1074 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1075 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1076 {
lakshya 19:79e69017c855 1077 telemetry[i]=0x00;
lakshya 19:79e69017c855 1078 }
lakshya 19:79e69017c855 1079 break;
lakshya 19:79e69017c855 1080 }
lakshya 19:79e69017c855 1081 case 0x14:
lakshya 19:79e69017c855 1082 {
lakshya 19:79e69017c855 1083 printf("\n\rSW_ON_ACS_TR_Z_ENABLE");
lakshya 19:79e69017c855 1084 //ACK_L234_TM
lakshya 19:79e69017c855 1085 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1086 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1087 TRZ_SW = 1;
lakshya 19:79e69017c855 1088 ACS_TR_Z_SW_STATUS=0x01;
lakshya 19:79e69017c855 1089 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1090 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1091 {
lakshya 19:79e69017c855 1092 telemetry[i]=0x00;
lakshya 19:79e69017c855 1093 }
lakshya 19:79e69017c855 1094 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1095 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1096 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1097 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1098 {
lakshya 19:79e69017c855 1099 telemetry[i]=0x00;
lakshya 19:79e69017c855 1100 }
lakshya 19:79e69017c855 1101 break;
lakshya 19:79e69017c855 1102 }
lakshya 19:79e69017c855 1103 case 0x15:
lakshya 19:79e69017c855 1104 {
lakshya 19:79e69017c855 1105 printf("\n\rSW_ON_BCN_TX_SW_ENABLE");
lakshya 19:79e69017c855 1106 //ACK_L234_TM
lakshya 19:79e69017c855 1107 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1108 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1109 BCN_SW = 0;//here 0 is switch enable
lakshya 19:79e69017c855 1110 BCN_TX_SW_ENABLE=0x01;
lakshya 19:79e69017c855 1111 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1112 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1113 {
lakshya 19:79e69017c855 1114 telemetry[i]=0x00;
lakshya 19:79e69017c855 1115 }
lakshya 19:79e69017c855 1116 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1117 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1118 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1119 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1120 {
lakshya 19:79e69017c855 1121 telemetry[i]=0x00;
lakshya 19:79e69017c855 1122 }
lakshya 19:79e69017c855 1123 break;
lakshya 19:79e69017c855 1124 }
lakshya 19:79e69017c855 1125 case 0x21:
lakshya 19:79e69017c855 1126 {
lakshya 19:79e69017c855 1127 printf("\n\rSW_OFF_ACS_ATS1_SW_ENABLE");
lakshya 19:79e69017c855 1128 //ACK_L234_TM
lakshya 19:79e69017c855 1129 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1130 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1131 ATS1_SW_ENABLE = 1;
lakshya 19:79e69017c855 1132 ACS_ATS1_SW_STATUS=0x03;
lakshya 19:79e69017c855 1133 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1134 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1135 {
lakshya 19:79e69017c855 1136 telemetry[i]=0x00;
lakshya 19:79e69017c855 1137 }
lakshya 19:79e69017c855 1138 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1139 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1140 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1141 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1142 {
lakshya 19:79e69017c855 1143 telemetry[i]=0x00;
lakshya 19:79e69017c855 1144 }
lakshya 19:79e69017c855 1145 break;
lakshya 19:79e69017c855 1146 }
lakshya 19:79e69017c855 1147 case 0x22:
lakshya 19:79e69017c855 1148 {
lakshya 19:79e69017c855 1149 printf("\n\rSW_OFF_ACS_ATS2_SW_ENABLE");
lakshya 19:79e69017c855 1150 //ACK_L234_TM
lakshya 19:79e69017c855 1151 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1152 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1153 ATS2_SW_ENABLE = 1;
lakshya 19:79e69017c855 1154 ACS_ATS2_SW_STATUS=0x03;
lakshya 19:79e69017c855 1155 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1156 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1157 {
lakshya 19:79e69017c855 1158 telemetry[i]=0x00;
lakshya 19:79e69017c855 1159 }
lakshya 19:79e69017c855 1160 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1161 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1162 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1163 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1164 {
lakshya 19:79e69017c855 1165 telemetry[i]=0x00;
lakshya 19:79e69017c855 1166 }
lakshya 19:79e69017c855 1167 break;
lakshya 19:79e69017c855 1168 }
lakshya 19:79e69017c855 1169 case 0x23:
lakshya 19:79e69017c855 1170 {
lakshya 19:79e69017c855 1171 printf("\n\rSW_OFF_ACS_TR_XY_ENABLE");
lakshya 19:79e69017c855 1172 //ACK_L234_TM
lakshya 19:79e69017c855 1173 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1174 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1175 TRXY_SW= 0;
lakshya 19:79e69017c855 1176 ACS_TR_XY_SW_STATUS=0x03;
lakshya 19:79e69017c855 1177 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1178 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1179 {
lakshya 19:79e69017c855 1180 telemetry[i]=0x00;
lakshya 19:79e69017c855 1181 }
lakshya 19:79e69017c855 1182 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1183 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1184 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1185 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1186 {
lakshya 19:79e69017c855 1187 telemetry[i]=0x00;
lakshya 19:79e69017c855 1188 }
lakshya 19:79e69017c855 1189 break;
lakshya 19:79e69017c855 1190 }
lakshya 19:79e69017c855 1191 case 0x24:
lakshya 19:79e69017c855 1192 {
lakshya 19:79e69017c855 1193 printf("\n\rSW_OFF_ACS_TR_Z_ENABLE");
lakshya 19:79e69017c855 1194 //ACK_L234_TM
lakshya 19:79e69017c855 1195 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1196 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1197 TRZ_SW = 0;
lakshya 19:79e69017c855 1198 ACS_TR_Z_SW_STATUS=0x03;
lakshya 19:79e69017c855 1199 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1200 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1201 {
lakshya 19:79e69017c855 1202 telemetry[i]=0x00;
lakshya 19:79e69017c855 1203 }
lakshya 19:79e69017c855 1204 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1205 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1206 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1207 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1208 {
lakshya 19:79e69017c855 1209 telemetry[i]=0x00;
lakshya 19:79e69017c855 1210 }
lakshya 19:79e69017c855 1211 break;
lakshya 19:79e69017c855 1212 }
lakshya 19:79e69017c855 1213 case 0x25:
lakshya 19:79e69017c855 1214 {
lakshya 19:79e69017c855 1215 printf("\n\rSW_OFF_BCN_TX_SW_ENABLE");
lakshya 19:79e69017c855 1216 //ACK_L234_TM
lakshya 19:79e69017c855 1217 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1218 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1219 BCN_SW = 1;
lakshya 19:79e69017c855 1220 BCN_TX_SW_ENABLE=0x03;
lakshya 19:79e69017c855 1221 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1222 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1223 {
lakshya 19:79e69017c855 1224 telemetry[i]=0x00;
lakshya 19:79e69017c855 1225 }
lakshya 19:79e69017c855 1226 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1227 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1228 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1229 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1230 {
lakshya 19:79e69017c855 1231 telemetry[i]=0x00;
lakshya 19:79e69017c855 1232 }
lakshya 19:79e69017c855 1233 break;
lakshya 19:79e69017c855 1234 }
lakshya 19:79e69017c855 1235 case 0x31:
lakshya 19:79e69017c855 1236 {
lakshya 19:79e69017c855 1237 printf("\n\rACS_ATS1_SW_RESET");
lakshya 19:79e69017c855 1238 //ACK_L234_TM
lakshya 19:79e69017c855 1239 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1240 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1241 ATS2_SW_ENABLE = 1;//as ats switched off
lakshya 19:79e69017c855 1242 ATS1_SW_ENABLE = 1;
lakshya 19:79e69017c855 1243 wait_ms(5);
lakshya 19:79e69017c855 1244 ATS1_SW_ENABLE = 0;
lakshya 19:79e69017c855 1245 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1246 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1247 {
lakshya 19:79e69017c855 1248 telemetry[i]=0x00;
lakshya 19:79e69017c855 1249 }
lakshya 19:79e69017c855 1250 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1251 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1252 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1253 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1254 {
lakshya 19:79e69017c855 1255 telemetry[i]=0x00;
lakshya 19:79e69017c855 1256 }
lakshya 19:79e69017c855 1257 break;
lakshya 19:79e69017c855 1258 }
lakshya 19:79e69017c855 1259 case 0x32:
lakshya 19:79e69017c855 1260 {
lakshya 19:79e69017c855 1261 printf("\n\rACS_ATS2_SW_RESET");
lakshya 19:79e69017c855 1262 //ACK_L234_TM
lakshya 19:79e69017c855 1263 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1264 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1265 ATS1_SW_ENABLE = 1;//as ats1 switched off
lakshya 19:79e69017c855 1266 ATS2_SW_ENABLE = 1;
lakshya 19:79e69017c855 1267 wait_ms(5);
lakshya 19:79e69017c855 1268 ATS2_SW_ENABLE = 0;
lakshya 19:79e69017c855 1269 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1270 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1271 {
lakshya 19:79e69017c855 1272 telemetry[i]=0x00;
lakshya 19:79e69017c855 1273 }
lakshya 19:79e69017c855 1274 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1275 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1276 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1277 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1278 {
lakshya 19:79e69017c855 1279 telemetry[i]=0x00;
lakshya 19:79e69017c855 1280 }
lakshya 19:79e69017c855 1281 break;
lakshya 19:79e69017c855 1282 }
lakshya 19:79e69017c855 1283 case 0x33:
lakshya 19:79e69017c855 1284 {
lakshya 19:79e69017c855 1285 printf("\n\rACS_TR_XY_SW_RESET");
lakshya 19:79e69017c855 1286 //ACK_L234_TM
lakshya 19:79e69017c855 1287 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1288 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1289 TRXY_SW= 0;
lakshya 19:79e69017c855 1290 wait_ms(5);
lakshya 19:79e69017c855 1291 TRXY_SW= 1;
lakshya 19:79e69017c855 1292 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1293 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1294 {
lakshya 19:79e69017c855 1295 telemetry[i]=0x00;
lakshya 19:79e69017c855 1296 }
lakshya 19:79e69017c855 1297 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1298 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1299 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1300 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1301 {
lakshya 19:79e69017c855 1302 telemetry[i]=0x00;
lakshya 19:79e69017c855 1303 }
lakshya 19:79e69017c855 1304 break;
lakshya 19:79e69017c855 1305 }
lakshya 19:79e69017c855 1306 case 0x34:
lakshya 19:79e69017c855 1307 {
lakshya 19:79e69017c855 1308 printf("\n\rACS_TR_Z_SW_RESET");
lakshya 19:79e69017c855 1309 //ACK_L234_TM
lakshya 19:79e69017c855 1310 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1311 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1312 TRZ_SW= 0;
lakshya 19:79e69017c855 1313 wait_ms(5);
lakshya 19:79e69017c855 1314 TRZ_SW= 1;
lakshya 19:79e69017c855 1315 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1316 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1317 {
lakshya 19:79e69017c855 1318 telemetry[i]=0x00;
lakshya 19:79e69017c855 1319 }
lakshya 19:79e69017c855 1320 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1321 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1322 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1323 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1324 {
lakshya 19:79e69017c855 1325 telemetry[i]=0x00;
lakshya 19:79e69017c855 1326 }
lakshya 19:79e69017c855 1327 break;
lakshya 19:79e69017c855 1328 }
lakshya 19:79e69017c855 1329 case 0x35:
lakshya 19:79e69017c855 1330 {
lakshya 19:79e69017c855 1331 printf("\n\rBCN_TX_SW_RESET");
lakshya 19:79e69017c855 1332 //ACK_L234_TM
lakshya 19:79e69017c855 1333 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1334 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1335 BCN_SW = 1;
lakshya 19:79e69017c855 1336 wait_ms(5);
lakshya 19:79e69017c855 1337 BCN_SW = 0;
lakshya 19:79e69017c855 1338 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1339 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1340 {
lakshya 19:79e69017c855 1341 telemetry[i]=0x00;
lakshya 19:79e69017c855 1342 }
lakshya 19:79e69017c855 1343 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1344 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1345 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1346 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1347 {
lakshya 19:79e69017c855 1348 telemetry[i]=0x00;
lakshya 19:79e69017c855 1349 }
lakshya 19:79e69017c855 1350 break;
lakshya 19:79e69017c855 1351 }
lakshya 19:79e69017c855 1352 case 0x36:
lakshya 19:79e69017c855 1353 {
lakshya 19:79e69017c855 1354 printf("\n\rBAE_INTERNAL_RESET TO be done how??");
lakshya 19:79e69017c855 1355 //ACK_L234_TM
lakshya 19:79e69017c855 1356 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1357 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1358 /*
lakshya 19:79e69017c855 1359 logic has to be done********************************************************
lakshya 19:79e69017c855 1360 */
lakshya 19:79e69017c855 1361 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1362 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1363 {
lakshya 19:79e69017c855 1364 telemetry[i]=0x00;
lakshya 19:79e69017c855 1365 }
lakshya 19:79e69017c855 1366 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1367 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1368 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1369 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1370 {
lakshya 19:79e69017c855 1371 telemetry[i]=0x00;
lakshya 19:79e69017c855 1372 }
lakshya 19:79e69017c855 1373 break;
lakshya 19:79e69017c855 1374 }
lakshya 19:79e69017c855 1375 case 0x37:
lakshya 19:79e69017c855 1376 {
lakshya 19:79e69017c855 1377 printf("\n\rCDMS_RESET");
lakshya 19:79e69017c855 1378 //ACK_L234_TM
lakshya 19:79e69017c855 1379 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1380 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1381 CDMS_RESET = 0;
lakshya 19:79e69017c855 1382 wait_ms(5);
lakshya 19:79e69017c855 1383 CDMS_RESET = 1;
lakshya 19:79e69017c855 1384 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1385 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1386 {
lakshya 19:79e69017c855 1387 telemetry[i]=0x00;
lakshya 19:79e69017c855 1388 }
lakshya 19:79e69017c855 1389 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1390 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1391 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1392 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1393 {
lakshya 19:79e69017c855 1394 telemetry[i]=0x00;
lakshya 19:79e69017c855 1395 }
lakshya 19:79e69017c855 1396 break;
lakshya 19:79e69017c855 1397 }
lakshya 19:79e69017c855 1398 case 0x38:
lakshya 19:79e69017c855 1399 {
lakshya 19:79e69017c855 1400 printf("\n\rCDMS_SW_RESET pin yet to be decided");
lakshya 19:79e69017c855 1401 //ACK_L234_TM
lakshya 19:79e69017c855 1402 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1403 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1404 /*
lakshya 19:79e69017c855 1405 PIN to be DECIDED***************************************************************8
lakshya 19:79e69017c855 1406 */
lakshya 19:79e69017c855 1407 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1408 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1409 {
lakshya 19:79e69017c855 1410 telemetry[i]=0x00;
lakshya 19:79e69017c855 1411 }
lakshya 19:79e69017c855 1412 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1413 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1414 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1415 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1416 {
lakshya 19:79e69017c855 1417 telemetry[i]=0x00;
lakshya 19:79e69017c855 1418 }
lakshya 19:79e69017c855 1419 break;
lakshya 19:79e69017c855 1420 }
lakshya 19:79e69017c855 1421 case 0x40:
lakshya 19:79e69017c855 1422 {
lakshya 19:79e69017c855 1423 uint8_t STANDBY_DATA_TC;
lakshya 19:79e69017c855 1424 BAE_STANDBY=0x00;
lakshya 19:79e69017c855 1425 STANDBY_DATA_TC=tc[4];
lakshya 19:79e69017c855 1426 if(STANDBY_DATA_TC==0x01)
lakshya 19:79e69017c855 1427 {
lakshya 19:79e69017c855 1428 BAE_STANDBY |=0x04;
lakshya 19:79e69017c855 1429 BAE_STANDBY_TIMER_RESET();
lakshya 19:79e69017c855 1430 //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
lakshya 19:79e69017c855 1431 }
lakshya 19:79e69017c855 1432 STANDBY_DATA_TC=tc[5];
lakshya 19:79e69017c855 1433 if(STANDBY_DATA_TC==0x01)
lakshya 19:79e69017c855 1434 {
lakshya 19:79e69017c855 1435 BAE_STANDBY |=0x02;
lakshya 19:79e69017c855 1436 BAE_STANDBY_TIMER_RESET();
lakshya 19:79e69017c855 1437 //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
lakshya 19:79e69017c855 1438 }
lakshya 19:79e69017c855 1439 STANDBY_DATA_TC=tc[6];
lakshya 19:79e69017c855 1440 if(STANDBY_DATA_TC==0x01)
lakshya 19:79e69017c855 1441 {
lakshya 19:79e69017c855 1442 BAE_STANDBY |=0x01;
lakshya 19:79e69017c855 1443 BAE_STANDBY_TIMER_RESET();
lakshya 19:79e69017c855 1444 //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
lakshya 19:79e69017c855 1445 }
lakshya 19:79e69017c855 1446 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1447 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1448 telemetry[2]=0xC0;//ack_code for this case
lakshya 19:79e69017c855 1449 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1450 {
lakshya 19:79e69017c855 1451 telemetry[i]=0x00;
lakshya 19:79e69017c855 1452 }
lakshya 19:79e69017c855 1453 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1454 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1455 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1456 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1457 {
lakshya 19:79e69017c855 1458 telemetry[i]=0x00;
lakshya 19:79e69017c855 1459 }
lakshya 19:79e69017c855 1460 break;
lakshya 19:79e69017c855 1461 }
lakshya 19:79e69017c855 1462 case 0x41:
lakshya 19:79e69017c855 1463 {
lakshya 19:79e69017c855 1464 printf("\n\rexecutng BAE reset HK counter");
lakshya 19:79e69017c855 1465
lakshya 19:79e69017c855 1466 //what to do here??*************************************************
lakshya 19:79e69017c855 1467 //TO BE DONE
lakshya 19:79e69017c855 1468
lakshya 19:79e69017c855 1469 //ACK_L234_TM
lakshya 19:79e69017c855 1470 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1471 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1472 telemetry[2]=0x02;
lakshya 19:79e69017c855 1473 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1474 {
lakshya 19:79e69017c855 1475 telemetry[i]=0x00;
lakshya 19:79e69017c855 1476 }
lakshya 19:79e69017c855 1477 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1478 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1479 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1480 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1481 {
lakshya 19:79e69017c855 1482 telemetry[i]=0x00;
lakshya 19:79e69017c855 1483 }
lakshya 19:79e69017c855 1484 break;
lakshya 19:79e69017c855 1485 }
lakshya 19:79e69017c855 1486 default:
lakshya 19:79e69017c855 1487 {
lakshya 19:79e69017c855 1488 printf("\n\rInvalid TC for FMS no matching FID");
lakshya 19:79e69017c855 1489 //ACK_L234_TM
lakshya 19:79e69017c855 1490 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1491 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1492 telemetry[2]=0x02;
lakshya 19:79e69017c855 1493 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1494 {
lakshya 19:79e69017c855 1495 telemetry[i]=0x00;
lakshya 19:79e69017c855 1496 }
lakshya 19:79e69017c855 1497 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1498 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1499 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1500 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1501 {
lakshya 19:79e69017c855 1502 telemetry[i]=0x00;
lakshya 19:79e69017c855 1503 }
lakshya 19:79e69017c855 1504 break;
lakshya 19:79e69017c855 1505 }
lakshya 19:79e69017c855 1506 }
lakshya 19:79e69017c855 1507 break;
lakshya 19:79e69017c855 1508 }
lakshya 19:79e69017c855 1509 default:
lakshya 19:79e69017c855 1510 {
lakshya 19:79e69017c855 1511 printf("\n\rInvalid TC, FMS service subtype mismacth");
lakshya 19:79e69017c855 1512 //ACK_L234_TM
lakshya 19:79e69017c855 1513 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1514 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1515 telemetry[2]=0x02;
lakshya 19:79e69017c855 1516 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1517 {
lakshya 19:79e69017c855 1518 telemetry[i]=0x00;
lakshya 19:79e69017c855 1519 }
lakshya 19:79e69017c855 1520 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1521 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1522 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1523 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1524 {
lakshya 19:79e69017c855 1525 telemetry[i]=0x00;
lakshya 19:79e69017c855 1526 }
lakshya 19:79e69017c855 1527 break;
lakshya 19:79e69017c855 1528 }
lakshya 19:79e69017c855 1529 }
lakshya 19:79e69017c855 1530 break;
lakshya 19:79e69017c855 1531 }
lakshya 19:79e69017c855 1532 default:
lakshya 19:79e69017c855 1533 {
lakshya 19:79e69017c855 1534 printf("\n\rInvalid TC neither FMS nor MMS");
lakshya 19:79e69017c855 1535 //ACK_L234_TM
lakshya 19:79e69017c855 1536 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1537 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1538 telemetry[2]=0x02;
lakshya 19:79e69017c855 1539 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1540 {
lakshya 19:79e69017c855 1541 telemetry[i]=0x00;
lakshya 19:79e69017c855 1542 }
lakshya 19:79e69017c855 1543 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1544 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1545 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1546 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1547 {
lakshya 19:79e69017c855 1548 telemetry[i]=0x00;
lakshya 19:79e69017c855 1549 }
lakshya 19:79e69017c855 1550 break;
lakshya 19:79e69017c855 1551 }
lakshya 19:79e69017c855 1552 }
lakshya 19:79e69017c855 1553 }
Bragadeesh153 13:fb7facaf308b 1554 }
Bragadeesh153 13:fb7facaf308b 1555 }
sakthipriya 4:39a4ae8c7ecd 1556
sakthipriya 4:39a4ae8c7ecd 1557
sakthipriya 4:39a4ae8c7ecd 1558
sakthipriya 4:39a4ae8c7ecd 1559
Bragadeesh153 13:fb7facaf308b 1560 int strt_add = flash_size() - (2*SECTOR_SIZE);
sakthipriya 4:39a4ae8c7ecd 1561 uint32_t flasharray[8]; //256+(3*1024)
lakshya 19:79e69017c855 1562 /*corrected*/
lakshya 19:79e69017c855 1563 int *nativeflash = (int*)strt_add;
sakthipriya 4:39a4ae8c7ecd 1564
sakthipriya 4:39a4ae8c7ecd 1565 /*Writing to the Flash*/
lakshya 19:79e69017c855 1566 void FCTN_BAE_WR_FLASH(uint16_t j,uint32_t fdata) //j-position to write address ; fdata - flash data to be written
sakthipriya 4:39a4ae8c7ecd 1567 {
Bragadeesh153 13:fb7facaf308b 1568 for(int i=0;i<8;i++)
sakthipriya 4:39a4ae8c7ecd 1569 {
sakthipriya 4:39a4ae8c7ecd 1570 flasharray[i]=nativeflash[i];
sakthipriya 4:39a4ae8c7ecd 1571 }
sakthipriya 4:39a4ae8c7ecd 1572 flasharray[j]=fdata;
sakthipriya 4:39a4ae8c7ecd 1573 erase_sector(strt_add);
sakthipriya 4:39a4ae8c7ecd 1574 program_flash(strt_add, (char*)&flasharray,4*8);
sakthipriya 4:39a4ae8c7ecd 1575 }
lakshya 19:79e69017c855 1576
sakthipriya 4:39a4ae8c7ecd 1577 /*End*/
sakthipriya 4:39a4ae8c7ecd 1578
sakthipriya 4:39a4ae8c7ecd 1579 /*Reading from Flash*/
lakshya 19:79e69017c855 1580 /*return choice parameter included so that i f we want the whole 32 packet data to be sent back we can do so*/
lakshya 19:79e69017c855 1581 uint32_t FCTN_BAE_RD_FLASH_ENTITY(uint16_t entity)
sakthipriya 4:39a4ae8c7ecd 1582 {
sakthipriya 4:39a4ae8c7ecd 1583 for(int i=0;i<8;i++)
sakthipriya 4:39a4ae8c7ecd 1584 {
sakthipriya 4:39a4ae8c7ecd 1585 flasharray[i]=nativeflash[i];
sakthipriya 4:39a4ae8c7ecd 1586 }
lakshya 19:79e69017c855 1587 return flasharray[entity];
sakthipriya 4:39a4ae8c7ecd 1588 }
lakshya 19:79e69017c855 1589
lakshya 19:79e69017c855 1590 uint32_t* FCTN_BAE_RD_FLASH()
lakshya 19:79e69017c855 1591 {
lakshya 19:79e69017c855 1592 for(int i=0;i<8;i++)
lakshya 19:79e69017c855 1593 {
lakshya 19:79e69017c855 1594 flasharray[i]=nativeflash[i];
lakshya 19:79e69017c855 1595 }
lakshya 19:79e69017c855 1596 return flasharray;
lakshya 19:79e69017c855 1597 }
lakshya 19:79e69017c855 1598
sakthipriya 5:bb592f3185cc 1599 /*End*/
sakthipriya 5:bb592f3185cc 1600
sakthipriya 5:bb592f3185cc 1601 // Convert float to 4 uint8_t
sakthipriya 5:bb592f3185cc 1602
lakshya 15:e09aaaccf134 1603
sakthipriya 6:036d08b62785 1604 void FCTN_CONVERT_FLOAT(float input, uint8_t output[4])
sakthipriya 5:bb592f3185cc 1605 {
sakthipriya 6:036d08b62785 1606 assert(sizeof(float) == sizeof(uint32_t));
sakthipriya 6:036d08b62785 1607 uint32_t* temp = reinterpret_cast<uint32_t*>(&input);
sakthipriya 6:036d08b62785 1608
sakthipriya 6:036d08b62785 1609 //float* output1 = reinterpret_cast<float*>(temp);
Bragadeesh153 13:fb7facaf308b 1610
lakshya 15:e09aaaccf134 1611 //printf("\n\r %f ", input);
lakshya 15:e09aaaccf134 1612 //std::cout << "\n\r uint32"<<*temp << std::endl;
sakthipriya 6:036d08b62785 1613
sakthipriya 6:036d08b62785 1614 output[0] =(uint8_t )(((*temp)>>24)&0xFF);
lakshya 15:e09aaaccf134 1615 output[1] =(uint8_t ) (((*temp)>>16)&0xFF);
lakshya 15:e09aaaccf134 1616 output[2] =(uint8_t ) (((*temp)>>8)&0xFF);
lakshya 15:e09aaaccf134 1617 output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic
lakshya 15:e09aaaccf134 1618
lakshya 17:fc782f7548c6 1619 // printf("\n\rthe values generated are\n");
lakshya 15:e09aaaccf134 1620 /*printf("\n\r%x\n",output[0]);
lakshya 15:e09aaaccf134 1621 printf("\n\r%x\n",output[1]);
lakshya 15:e09aaaccf134 1622 printf("\n\r%x\n",output[2]);
lakshya 15:e09aaaccf134 1623 printf("\n\r%x\n",output[3]);
lakshya 15:e09aaaccf134 1624 to check the values generated
lakshya 15:e09aaaccf134 1625 */
sakthipriya 6:036d08b62785 1626 //printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]);
Bragadeesh153 13:fb7facaf308b 1627 //std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl;
Bragadeesh153 13:fb7facaf308b 1628 }