Team Fox / Mbed 2 deprecated RAJANGAM_REVIEW_BAE_CODE

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 QM_BAE_review_1 by Team Fox

TCTM.cpp@41:5df2bed2157d, 2016-07-08 (annotated)

Committer:
Bragadeesh153
Date:
Fri Jul 08 08:14:29 2016 +0000
Revision:
41:5df2bed2157d
Parent:
40:c2538d97e78b 
BAE CODE used for testing telecommands

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakthipriya 0:7b4c00e3912f 1 #include "mbed.h"
lakshya 20:949d13045431 2 #include "rtos.h"
sakthipriya 0:7b4c00e3912f 3 #include "TCTM.h"
sakthipriya 3:07e15677a75c 4 #include "crc.h"
sakthipriya 4:39a4ae8c7ecd 5 #include "EPS.h"
lakshya 20:949d13045431 6 #include "ACS.h"
sakthipriya 5:bb592f3185cc 7 #include "pin_config.h"
sakthipriya 5:bb592f3185cc 8 #include "FreescaleIAP.h"
sakthipriya 6:036d08b62785 9 #include "inttypes.h"
sakthipriya 6:036d08b62785 10 #include "iostream"
sakthipriya 6:036d08b62785 11 #include "stdint.h"
sakthipriya 6:036d08b62785 12 #include "cassert"
lakshya 10:f93407b97750 13 #include"math.h"
lakshya 10:f93407b97750 14
lakshya 19:79e69017c855 15 //**********************************STATUS_PARAMETERS*****************************************************
lakshya 19:79e69017c855 16 uint8_t BCN_TX_SW_ENABLE=0x00;
lakshya 19:79e69017c855 17
lakshya 19:79e69017c855 18 //***********************************FOR STANDBY TIMER****************************************************
lakshya 19:79e69017c855 19 extern void BAE_STANDBY_TIMER_RESET();
lakshya 19:79e69017c855 20
Bragadeesh153 41:5df2bed2157d 21 uint8_t telemetry[134];
lakshya 20:949d13045431 22 extern uint8_t BAE_HK_data[134];
lakshya 19:79e69017c855 23
lakshya 20:949d13045431 24 //*****************PARA******************************
lakshya 20:949d13045431 25
lakshya 20:949d13045431 26 //ACS
lakshya 20:949d13045431 27 extern float db[3];
lakshya 20:949d13045431 28 extern float data[6];
lakshya 20:949d13045431 29 extern float b_old[3]; // Unit: Tesla
lakshya 20:949d13045431 30 extern float moment[3];
lakshya 20:949d13045431 31 extern uint8_t ACS_STATE;
lakshya 20:949d13045431 32 extern uint8_t ACS_STATUS;
lakshya 20:949d13045431 33 extern uint8_t flag_firsttime;
lakshya 20:949d13045431 34 extern uint8_t ACS_DETUMBLING_ALGO_TYPE;
lakshya 20:949d13045431 35 extern uint8_t ACS_INIT_STATUS;
lakshya 20:949d13045431 36 extern uint8_t ACS_DATA_ACQ_STATUS;
lakshya 20:949d13045431 37 extern uint8_t ACS_MAIN_STATUS;
lakshya 20:949d13045431 38 extern uint8_t ACS_MAG_TIME_DELAY;
lakshya 20:949d13045431 39 extern uint8_t ACS_DEMAG_TIME_DELAY;
lakshya 20:949d13045431 40 extern uint8_t ACS_Z_FIXED_MOMENT;
lakshya 20:949d13045431 41 extern uint8_t ACS_TR_X_PWM;
lakshya 20:949d13045431 42 extern uint8_t ACS_TR_Y_PWM;
lakshya 20:949d13045431 43 extern uint8_t ACS_TR_Z_PWM;
lakshya 39:670133e7ffd8 44 extern uint8_t B_SCZ_ANGLE;
lakshya 20:949d13045431 45 extern uint8_t alarmmode;
lakshya 20:949d13045431 46 extern uint8_t controlmode_mms;
lakshya 20:949d13045431 47 extern uint8_t invjm_mms[9];
lakshya 20:949d13045431 48 extern uint8_t jm_mms[9];
lakshya 20:949d13045431 49 extern uint8_t bb_mms[3];
lakshya 20:949d13045431 50 extern uint8_t singularity_flag_mms;
lakshya 20:949d13045431 51 extern uint8_t ACS_TR_Z_SW_STATUS;
lakshya 20:949d13045431 52 extern uint8_t ACS_TR_XY_SW_STATUS;
lakshya 20:949d13045431 53
lakshya 20:949d13045431 54 extern uint8_t ATS1_EVENT_STATUS_RGTR;
lakshya 20:949d13045431 55 extern uint8_t ATS1_SENTRAL_STATUS_RGTR;
lakshya 20:949d13045431 56 extern uint8_t ATS1_ERROR_RGTR;
lakshya 20:949d13045431 57 extern uint8_t ATS2_EVENT_STATUS_RGTR;
lakshya 20:949d13045431 58 extern uint8_t ATS2_SENTRAL_STATUS_RGTR;
lakshya 20:949d13045431 59 extern uint8_t ATS2_ERROR_RGTR;
lakshya 20:949d13045431 60 //change
lakshya 20:949d13045431 61 extern uint16_t ACS_MM_X_COMSN;
lakshya 20:949d13045431 62 extern uint16_t ACS_MM_Y_COMSN;
lakshya 20:949d13045431 63 extern uint16_t ACS_MG_X_COMSN;
lakshya 20:949d13045431 64 extern uint16_t ACS_MG_Y_COMSN;
lakshya 20:949d13045431 65 extern uint16_t ACS_MM_Z_COMSN;
lakshya 20:949d13045431 66 extern uint16_t ACS_MG_Z_COMSN;
lakshya 20:949d13045431 67 //acs func
lakshya 20:949d13045431 68 extern void F_ACS();
lakshya 20:949d13045431 69 extern int SENSOR_INIT();
lakshya 20:949d13045431 70 extern int FCTN_ACS_INIT();
lakshya 20:949d13045431 71 extern int SENSOR_DATA_ACQ();
lakshya 20:949d13045431 72 extern int FCTN_ATS_DATA_ACQ();
lakshya 20:949d13045431 73 extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]);
lakshya 20:949d13045431 74 extern void FCTN_ACS_CNTRLALGO(float*,float*,int);
lakshya 20:949d13045431 75
lakshya 20:949d13045431 76
lakshya 20:949d13045431 77 //main
lakshya 20:949d13045431 78 extern uint8_t ACS_ATS_STATUS;
lakshya 20:949d13045431 79 extern uint16_t ACS_MAIN_COUNTER;//change\apply
lakshya 20:949d13045431 80 extern uint8_t HTR_CYCLE_COUNTER;
lakshya 20:949d13045431 81 extern RtosTimer *HTR_CYCLE;
lakshya 20:949d13045431 82 extern uint8_t HTR_CYCLE_COUNTS; //Count of heater cycles
lakshya 20:949d13045431 83 extern uint8_t HTR_CYCLE_START_DLY; //EPS_HTR_DLY_TIMER timer duration in minutes
lakshya 20:949d13045431 84 extern uint8_t HTR_ON_DURATION; //EPS_HTR_OFF timer duration in minutes
lakshya 20:949d13045431 85 extern uint16_t HTR_CYCLE_PERIOD;
lakshya 20:949d13045431 86
Bragadeesh153 40:c2538d97e78b 87 extern DigitalOut ACS_TR_XY_ENABLE;
Bragadeesh153 40:c2538d97e78b 88 extern DigitalOut ACS_TR_Z_ENABLE;
Bragadeesh153 40:c2538d97e78b 89 extern DigitalIn ACS_TR_XY_OC_FAULT;
Bragadeesh153 40:c2538d97e78b 90 extern DigitalIn ACS_TR_Z_OC_FAULT;
Bragadeesh153 40:c2538d97e78b 91 extern DigitalIn ACS_TR_XY_FAULT;
Bragadeesh153 40:c2538d97e78b 92 extern DigitalIn ACS_TR_Z_FAULT;
Bragadeesh153 40:c2538d97e78b 93 extern DigitalIn ACS_ATS1_OC_FAULT;
Bragadeesh153 40:c2538d97e78b 94 extern DigitalIn ACS_ATS2_OC_FAULT;
sakthipriya 0:7b4c00e3912f 95
Bragadeesh153 40:c2538d97e78b 96 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
Bragadeesh153 40:c2538d97e78b 97 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
sakthipriya 5:bb592f3185cc 98
Bragadeesh153 40:c2538d97e78b 99 extern DigitalOut DRV_Z_EN;
Bragadeesh153 40:c2538d97e78b 100 extern DigitalOut DRV_XY_EN;
Bragadeesh153 40:c2538d97e78b 101 extern DigitalOut TRXY_SW; //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN
Bragadeesh153 40:c2538d97e78b 102 extern DigitalOut TRZ_SW; //TR Z Switch
lakshya 20:949d13045431 103
Bragadeesh153 40:c2538d97e78b 104 extern DigitalOut phase_TR_x;
Bragadeesh153 40:c2538d97e78b 105 extern DigitalOut phase_TR_y;
Bragadeesh153 40:c2538d97e78b 106 extern DigitalOut phase_TR_z;
lakshya 20:949d13045431 107
lakshya 20:949d13045431 108
lakshya 20:949d13045431 109 //CDMS
Bragadeesh153 40:c2538d97e78b 110 extern DigitalOut CDMS_RESET; // CDMS RESET
lakshya 20:949d13045431 111 extern uint8_t CDMS_SW_STATUS;
Bragadeesh153 40:c2538d97e78b 112 extern DigitalIn CDMS_OC_FAULT;
lakshya 20:949d13045431 113
lakshya 20:949d13045431 114
lakshya 20:949d13045431 115 //BCN
Bragadeesh153 40:c2538d97e78b 116 extern DigitalOut BCN_SW; //Beacon switch
lakshya 10:f93407b97750 117 extern uint8_t BCN_TX_STATUS;
lakshya 10:f93407b97750 118 extern uint8_t BCN_FEN;
lakshya 20:949d13045431 119 extern uint8_t BCN_SPND_TX;
lakshya 20:949d13045431 120 extern uint8_t BCN_TX_MAIN_STATUS;
lakshya 20:949d13045431 121 extern uint8_t BCN_TX_SW_STATUS;
lakshya 20:949d13045431 122 extern uint8_t BCN_LONG_MSG_TYPE;
lakshya 20:949d13045431 123 extern uint8_t BCN_INIT_STATUS;
lakshya 20:949d13045431 124 extern uint8_t BCN_FAIL_COUNT;
lakshya 20:949d13045431 125 extern uint16_t BCN_TX_MAIN_COUNTER;
Bragadeesh153 40:c2538d97e78b 126 extern DigitalIn BCN_TX_OC_FAULT;
lakshya 20:949d13045431 127 extern uint8_t BCN_TMP;
lakshya 20:949d13045431 128 extern void F_BCN();
lakshya 20:949d13045431 129 extern void FCTN_BCN_TX_MAIN();
lakshya 33:76f2b8735501 130 extern uint8_t SHORT_HK_data[15];
lakshya 33:76f2b8735501 131 extern void FCTN_BCN_SPND_TX();
lakshya 20:949d13045431 132
lakshya 20:949d13045431 133
lakshya 20:949d13045431 134 //BAE
lakshya 20:949d13045431 135 extern uint8_t BAE_STANDBY;
lakshya 20:949d13045431 136 extern uint8_t BAE_INIT_STATUS;
lakshya 20:949d13045431 137 extern uint8_t BAE_RESET_COUNTER;
lakshya 27:61c856be467e 138 extern uint8_t BAE_MNG_I2C_STATUS;
lakshya 20:949d13045431 139 /*given a default value as of now shuld read value from flash and increment it write it back very time it starts(bae)*/
lakshya 20:949d13045431 140
lakshya 20:949d13045431 141 extern uint32_t BAE_STATUS;//extern uint32_t BAE_STATUS;
lakshya 20:949d13045431 142 extern BAE_HK_quant quant_data;
sakthipriya 4:39a4ae8c7ecd 143 extern BAE_HK_actual actual_data;
sakthipriya 4:39a4ae8c7ecd 144 extern BAE_HK_min_max bae_HK_minmax;
Bragadeesh153 13:fb7facaf308b 145 //extern DigitalOut TRXY_SW;
lakshya 11:1fdb94ae6563 146 //extern DigitalOut TRZ_SW_EN; //same as TRZ_SW
lakshya 11:1fdb94ae6563 147 extern uint32_t BAE_ENABLE;
lakshya 20:949d13045431 148 extern uint16_t BAE_I2C_COUNTER;
lakshya 33:76f2b8735501 149 extern uint8_t LONG_HK_data[2][134];
lakshya 20:949d13045431 150 //extern uint8_t BCN_FAIL_COUNT;
lakshya 20:949d13045431 151
lakshya 11:1fdb94ae6563 152
lakshya 20:949d13045431 153 //EPS
lakshya 20:949d13045431 154 extern bool firstCount;
lakshya 20:949d13045431 155 extern uint8_t EPS_BTRY_HTR_AUTO;
lakshya 20:949d13045431 156 extern uint8_t EPS_BATT_TEMP_LOW;
lakshya 20:949d13045431 157 extern uint8_t EPS_BATT_TEMP_HIGH;
lakshya 20:949d13045431 158 extern uint8_t EPS_BATT_TEMP_DEFAULT;
lakshya 33:76f2b8735501 159 extern DigitalOut BTRY_HTR_ENABLE;
lakshya 20:949d13045431 160 extern uint8_t EPS_SOC_LEVEL_12;
lakshya 20:949d13045431 161 extern uint8_t EPS_SOC_LEVEL_23;
lakshya 20:949d13045431 162 extern uint8_t EPS_INIT_STATUS;
lakshya 20:949d13045431 163 extern uint8_t EPS_BATTERY_GAUGE_STATUS ;
lakshya 20:949d13045431 164 extern uint8_t EPS_MAIN_STATUS;
lakshya 33:76f2b8735501 165 extern uint8_t EPS_BTRY_TMP_STATUS ;
lakshya 20:949d13045431 166 extern uint8_t EPS_STATUS ;
lakshya 20:949d13045431 167 extern uint8_t EPS_BAT_TEMP_LOW;
lakshya 20:949d13045431 168 extern uint8_t EPS_BAT_TEMP_HIGH;
lakshya 20:949d13045431 169 extern uint8_t EPS_BAT_TEMP_DEFAULT;
lakshya 20:949d13045431 170 extern uint16_t EPS_MAIN_COUNTER;
lakshya 33:76f2b8735501 171 extern uint8_t EPS_BTRY_HTR;
lakshya 11:1fdb94ae6563 172
lakshya 20:949d13045431 173 extern DigitalOut SelectLineb3; // MSB of Select Lines
lakshya 20:949d13045431 174 extern DigitalOut SelectLineb2;
lakshya 20:949d13045431 175 extern DigitalOut SelectLineb1;
lakshya 20:949d13045431 176 extern DigitalOut SelectLineb0;
Bragadeesh153 40:c2538d97e78b 177 extern DigitalIn EPS_CHARGER_FAULT;
Bragadeesh153 40:c2538d97e78b 178 extern DigitalIn EPS_CHARGER_STATUS;
Bragadeesh153 40:c2538d97e78b 179 extern DigitalIn EPS_BATTERY_GAUGE_ALERT;
lakshya 20:949d13045431 180
lakshya 20:949d13045431 181 extern void F_EPS();
lakshya 20:949d13045431 182 extern AnalogIn CurrentInput;
lakshya 20:949d13045431 183
lakshya 20:949d13045431 184 //--------------------check this refer prashant
lakshya 11:1fdb94ae6563 185
Bragadeesh153 13:fb7facaf308b 186 extern PwmOut PWM1; //x //Functions used to generate PWM signal
lakshya 11:1fdb94ae6563 187 extern PwmOut PWM2; //y
lakshya 11:1fdb94ae6563 188 extern PwmOut PWM3; //z //PWM output comes from pins p6
lakshya 11:1fdb94ae6563 189
lakshya 20:949d13045431 190 //included after home
lakshya 11:1fdb94ae6563 191 //extern void FCTN_ACS_GENPWM_MAIN();
lakshya 20:949d13045431 192
lakshya 20:949d13045431 193 uint16_t crc_hk_data()//gencrc16_for_me()
lakshya 20:949d13045431 194 {
lakshya 33:76f2b8735501 195 uint16_t crc = CRC::crc16_gen(&LONG_HK_data[1][0],132);//BAE_chardata i.e char data type usesd earlier BAE_HK_data
lakshya 33:76f2b8735501 196 return crc;
lakshya 33:76f2b8735501 197 }
lakshya 33:76f2b8735501 198
lakshya 33:76f2b8735501 199 uint8_t crc8_short()
lakshya 33:76f2b8735501 200 {
lakshya 33:76f2b8735501 201 uint8_t crc = CRC::crc8_gen(SHORT_HK_data,14);
lakshya 20:949d13045431 202 return crc;
lakshya 20:949d13045431 203 }
lakshya 20:949d13045431 204
lakshya 20:949d13045431 205 float uint16_to_float(float min,float max,uint16_t scale)
lakshya 20:949d13045431 206 {
lakshya 20:949d13045431 207 float div=max-min;
lakshya 20:949d13045431 208 div=(div/(65535.0));
lakshya 20:949d13045431 209 return ((div*(float)scale)+ min);
lakshya 20:949d13045431 210 }
lakshya 11:1fdb94ae6563 211
lakshya 20:949d13045431 212 uint16_t float_to_uint16(float min,float max,float val) //takes care of -ve num as its scale with min and max as reference
lakshya 20:949d13045431 213 {
lakshya 20:949d13045431 214 if(val>max)
lakshya 20:949d13045431 215 {return 0xffff;
lakshya 20:949d13045431 216 }
lakshya 20:949d13045431 217 if(val<min)
lakshya 20:949d13045431 218 {return 0x0000;
lakshya 20:949d13045431 219 }
lakshya 20:949d13045431 220 float div=max-min;
lakshya 20:949d13045431 221 div=(65535.0/div);
lakshya 20:949d13045431 222 val=((val-min)*div);
lakshya 20:949d13045431 223 printf("\n\r the scale is %x",(uint16_t)val);
lakshya 20:949d13045431 224 return (uint16_t)val;
lakshya 20:949d13045431 225 }
sakthipriya 5:bb592f3185cc 226
lakshya 20:949d13045431 227 void gen_I_TM()
lakshya 20:949d13045431 228 {
lakshya 20:949d13045431 229 telemetry[0] = 0xB0;
lakshya 20:949d13045431 230 for(int i=1;i<11;i++)
lakshya 20:949d13045431 231 telemetry[i] = 0x00;
lakshya 20:949d13045431 232 uint16_t crc = CRC::crc16_gen(telemetry,11);//BAE_chardata i.e char data type usesd earlier
lakshya 20:949d13045431 233 telemetry[11] = (uint8_t)(crc >> 8);
lakshya 20:949d13045431 234 telemetry[12] = (uint8_t)crc ;
lakshya 20:949d13045431 235 for(int i=13;i<135;i++)
lakshya 20:949d13045431 236 telemetry[i] = 0x00;
lakshya 20:949d13045431 237 }
lakshya 20:949d13045431 238 void FCTN__UINT (uint8_t input[2], float* output)
lakshya 11:1fdb94ae6563 239 {
lakshya 11:1fdb94ae6563 240
lakshya 11:1fdb94ae6563 241 *output = (float) input[1];
lakshya 11:1fdb94ae6563 242 *output = *output/100.0; //input[0] integer part
lakshya 11:1fdb94ae6563 243 *output = *output + (float) input[0]; //input[1] decimal part correct to two decimal places
lakshya 11:1fdb94ae6563 244 }
lakshya 11:1fdb94ae6563 245
lakshya 10:f93407b97750 246 float angle(float x,float y,float z)
lakshya 10:f93407b97750 247 {
lakshya 10:f93407b97750 248 float mag_total=sqrt(x*x + y*y + z*z);
lakshya 10:f93407b97750 249 float cos_z = z/mag_total;
lakshya 10:f93407b97750 250 float theta_z = acosf(cos_z);
Bragadeesh153 13:fb7facaf308b 251
lakshya 10:f93407b97750 252 return theta_z;
lakshya 10:f93407b97750 253 //printf("/n cos_zz= %f /t theta_z= %f /n",cos_z,theta_z);
lakshya 10:f93407b97750 254 }
lakshya 10:f93407b97750 255
Bragadeesh153 13:fb7facaf308b 256 //uint8_t tm1[134];
Bragadeesh153 13:fb7facaf308b 257
Bragadeesh153 13:fb7facaf308b 258 void FCTN_BAE_TM_TC (uint8_t* tc)
sakthipriya 3:07e15677a75c 259 {
lakshya 19:79e69017c855 260 //tm1[0] = 1;
lakshya 19:79e69017c855 261 //calculating crc
lakshya 20:949d13045431 262 for(int i=0;i<134;i++)
lakshya 20:949d13045431 263 {
lakshya 20:949d13045431 264 telemetry[i]=tc[i];
lakshya 20:949d13045431 265 }
lakshya 20:949d13045431 266
lakshya 19:79e69017c855 267
lakshya 15:e09aaaccf134 268 /*chaged*/
lakshya 15:e09aaaccf134 269 uint8_t* tm; // without it some identifier error
lakshya 20:949d13045431 270 uint16_t crc16=CRC::crc16_gen(telemetry,9); //implementing crc
lakshya 20:949d13045431 271 printf("\n\r the crc is %x",crc16);
lakshya 19:79e69017c855 272 if( ( ((uint8_t)((crc16 & 0xFF00)>>8)==tc[9]) && ((uint8_t)(crc16 & 0x00FF)==tc[10]) )== 0 )
lakshya 19:79e69017c855 273 {
lakshya 19:79e69017c855 274 telemetry[0]=0xB0;
lakshya 19:79e69017c855 275 telemetry[1]=0x00;//tc psc defined for this case
lakshya 19:79e69017c855 276 telemetry[2]=0x01;//ack code for this case
lakshya 19:79e69017c855 277 for(int i=3;i<11;i++)
lakshya 20:949d13045431 278 {
lakshya 19:79e69017c855 279 telemetry[i]=0x00;
lakshya 19:79e69017c855 280 }
lakshya 19:79e69017c855 281 //add crc
lakshya 19:79e69017c855 282 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 283 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 284 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 285 printf("\n\rincorrect crc");
lakshya 19:79e69017c855 286 for(int i=13;i<134;i++)
lakshya 19:79e69017c855 287 {
lakshya 19:79e69017c855 288 telemetry[i]=0x00;
lakshya 19:79e69017c855 289 }
lakshya 19:79e69017c855 290 }
lakshya 19:79e69017c855 291 else
lakshya 19:79e69017c855 292 {
lakshya 19:79e69017c855 293 //check apid
lakshya 19:79e69017c855 294 uint8_t apid_check=((tc[1]&0xC0)>>6);
lakshya 19:79e69017c855 295 if(apid_check!=0x01)
lakshya 19:79e69017c855 296 {
lakshya 19:79e69017c855 297 telemetry[0]=0xB0;
lakshya 19:79e69017c855 298 telemetry[1]=tc[0];//tc psc defined for this case
lakshya 19:79e69017c855 299 telemetry[2]=0x02;//ack code for this case
lakshya 19:79e69017c855 300 for(int i=3;i<11;i++)
Bragadeesh153 13:fb7facaf308b 301 {
lakshya 19:79e69017c855 302 telemetry[i]=0x00;
Bragadeesh153 13:fb7facaf308b 303 }
Bragadeesh153 13:fb7facaf308b 304 crc16 = CRC::crc16_gen(telemetry,11);
Bragadeesh153 13:fb7facaf308b 305 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 13:fb7facaf308b 306 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 307 printf("\n\rillegal TC packet APID don't match");
lakshya 19:79e69017c855 308 for(int i=13;i<134;i++)
Bragadeesh153 13:fb7facaf308b 309 {
Bragadeesh153 13:fb7facaf308b 310 telemetry[i]=0x00;
Bragadeesh153 13:fb7facaf308b 311 }
sakthipriya 3:07e15677a75c 312 }
lakshya 19:79e69017c855 313 else
lakshya 19:79e69017c855 314 {// all the possible cases of fms and mms
lakshya 19:79e69017c855 315 uint8_t service_type=(tc[2]&0xF0);
lakshya 19:79e69017c855 316 //check for fms first
lakshya 19:79e69017c855 317 switch(service_type)
lakshya 19:79e69017c855 318 {
lakshya 19:79e69017c855 319 case 0x60:
lakshya 19:79e69017c855 320 {
lakshya 19:79e69017c855 321 printf("Memory Management Service\r\n");
lakshya 19:79e69017c855 322 uint8_t service_subtype=(tc[2]&0x0F);
lakshya 19:79e69017c855 323 switch(service_subtype)
lakshya 19:79e69017c855 324 {
lakshya 19:79e69017c855 325 case 0x02:
lakshya 19:79e69017c855 326 {
lakshya 19:79e69017c855 327 printf("\n\rRead from RAM");
lakshya 19:79e69017c855 328 uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
lakshya 19:79e69017c855 329 switch(MID)
lakshya 19:79e69017c855 330 {
lakshya 19:79e69017c855 331 case 0x0000://changed from 0001
lakshya 19:79e69017c855 332 {
lakshya 19:79e69017c855 333 printf("\n\rRead from MID 0000 \n");
lakshya 19:79e69017c855 334 printf("\n\rReading flash parameters");
lakshya 19:79e69017c855 335
lakshya 19:79e69017c855 336 /*taking some varible till we find some thing more useful*/
lakshya 19:79e69017c855 337 //uint8_t ref_val=0x01;
lakshya 19:79e69017c855 338 telemetry[0] = 1;
lakshya 19:79e69017c855 339 telemetry[1] = tc[0];
lakshya 19:79e69017c855 340 telemetry[2] = ACK_CODE;
lakshya 19:79e69017c855 341 /*random or with bcn_tx_sw_enable assuming it is 1 bit in length
lakshya 19:79e69017c855 342 how to get that we dont know, now we just assume it to be so*/
lakshya 20:949d13045431 343 telemetry[3] = ACS_ATS_STATUS;
lakshya 20:949d13045431 344 telemetry[4] = ACS_TR_XY_SW_STATUS;
lakshya 20:949d13045431 345 telemetry[4] = (telemetry[4]<<2)| ACS_TR_Z_SW_STATUS;
lakshya 27:61c856be467e 346 telemetry[4] = (telemetry[4]<<4) | ACS_STATE;
lakshya 27:61c856be467e 347 telemetry[5] = ACS_DETUMBLING_ALGO_TYPE;
lakshya 27:61c856be467e 348 telemetry[5] = (telemetry[5]<<2) | BCN_TX_SW_STATUS;
lakshya 20:949d13045431 349 telemetry[5] = (telemetry[5]<<1) | BCN_SPND_TX;
lakshya 20:949d13045431 350 telemetry[5] = (telemetry[5]<<1) | BCN_FEN;
lakshya 20:949d13045431 351 telemetry[5] = (telemetry[5]<<1) | BCN_LONG_MSG_TYPE;
lakshya 20:949d13045431 352 telemetry[5] = (telemetry[5]<<1) | EPS_BTRY_HTR_AUTO;//EPS_BATTERY_HEATER_ENABLE
lakshya 27:61c856be467e 353 telemetry[5] = (telemetry[5]<<1);
lakshya 27:61c856be467e 354 //now one spares in telemetry[5]
lakshya 20:949d13045431 355 telemetry[6] = BAE_RESET_COUNTER;
Bragadeesh153 41:5df2bed2157d 356 printf("telemetry[6] flash read is %x:\r\r\n ",telemetry[6]);
lakshya 20:949d13045431 357 telemetry[7] = EPS_SOC_LEVEL_12;
lakshya 20:949d13045431 358 telemetry[8] = EPS_SOC_LEVEL_23;
lakshya 20:949d13045431 359 telemetry[9] = ACS_MAG_TIME_DELAY;
lakshya 20:949d13045431 360 telemetry[10] = ACS_DEMAG_TIME_DELAY;
lakshya 20:949d13045431 361 telemetry[11] = EPS_BAT_TEMP_LOW;
lakshya 20:949d13045431 362 telemetry[12] = EPS_BAT_TEMP_HIGH;
lakshya 20:949d13045431 363 telemetry[13] = EPS_BAT_TEMP_DEFAULT;
lakshya 20:949d13045431 364
lakshya 20:949d13045431 365 telemetry[14] = ACS_MM_X_COMSN >> 8;
lakshya 20:949d13045431 366 telemetry[15] = ACS_MM_X_COMSN;
lakshya 20:949d13045431 367
lakshya 20:949d13045431 368 telemetry[16] = ACS_MM_Y_COMSN >> 8;
lakshya 20:949d13045431 369 telemetry[17] = ACS_MM_Y_COMSN;
lakshya 20:949d13045431 370
lakshya 20:949d13045431 371 telemetry[18] = ACS_MG_X_COMSN >> 8;
lakshya 20:949d13045431 372 telemetry[19] = ACS_MG_X_COMSN;
Bragadeesh153 41:5df2bed2157d 373 printf("telemetry[19] flash read is %x: \r\n",telemetry[19]);
lakshya 20:949d13045431 374
lakshya 20:949d13045431 375 telemetry[20] = ACS_MG_Y_COMSN >> 8;
lakshya 20:949d13045431 376 telemetry[21] = ACS_MG_Y_COMSN;
lakshya 20:949d13045431 377
lakshya 20:949d13045431 378 telemetry[22] = ACS_MM_Z_COMSN >> 8;
lakshya 20:949d13045431 379 telemetry[23] = ACS_MM_Z_COMSN;
lakshya 20:949d13045431 380
lakshya 20:949d13045431 381 telemetry[24] = ACS_MG_Z_COMSN >> 8;
lakshya 20:949d13045431 382 telemetry[25] = ACS_MG_Z_COMSN;
lakshya 20:949d13045431 383
lakshya 20:949d13045431 384 telemetry[26] = ACS_Z_FIXED_MOMENT >> 8;
lakshya 20:949d13045431 385 telemetry[27] = ACS_Z_FIXED_MOMENT;
lakshya 20:949d13045431 386
lakshya 20:949d13045431 387 //BAE RAM PARAMETER
lakshya 20:949d13045431 388 telemetry[28] = BAE_INIT_STATUS;
lakshya 27:61c856be467e 389 telemetry[28] = (telemetry[28]<<1) | BAE_MNG_I2C_STATUS;//changed
lakshya 20:949d13045431 390 telemetry[28] = (telemetry[28]<<1) | BCN_INIT_STATUS;
lakshya 20:949d13045431 391 telemetry[28] = (telemetry[28]<<1) | BCN_TX_MAIN_STATUS;
lakshya 20:949d13045431 392 telemetry[28] = (telemetry[28]<<3) | BCN_TX_STATUS;
lakshya 27:61c856be467e 393 telemetry[28] = (telemetry[28]<<1) | ACS_INIT_STATUS;
lakshya 20:949d13045431 394
lakshya 20:949d13045431 395 telemetry[29] = ACS_DATA_ACQ_STATUS;
lakshya 20:949d13045431 396 telemetry[29] = (telemetry[29]<<1) | ACS_MAIN_STATUS;
lakshya 27:61c856be467e 397 telemetry[29] = (telemetry[29]<<4) | ACS_STATUS;
lakshya 20:949d13045431 398 telemetry[29] = (telemetry[29]<<1) | EPS_INIT_STATUS;
lakshya 20:949d13045431 399
lakshya 27:61c856be467e 400 telemetry[30] = EPS_BATTERY_GAUGE_STATUS;
lakshya 27:61c856be467e 401 telemetry[30] = (telemetry[30]<<1) | EPS_MAIN_STATUS;
lakshya 33:76f2b8735501 402 telemetry[30] = (telemetry[30]<<1) | EPS_BTRY_TMP_STATUS;
lakshya 20:949d13045431 403 telemetry[30] = (telemetry[30]<<3) | EPS_STATUS;
lakshya 20:949d13045431 404 telemetry[30] = (telemetry[30]<<2) | CDMS_SW_STATUS;
lakshya 20:949d13045431 405 // telemetry[30] = (telemetry[30]<<1) | EPS_BTRY_HTR_STATUS;//new to : implement
lakshya 33:76f2b8735501 406
lakshya 33:76f2b8735501 407 telemetry[31] = EPS_BTRY_HTR; //new to : implement
lakshya 27:61c856be467e 408 //spare 4
lakshya 27:61c856be467e 409 telemetry[31] = (telemetry[31]<<7) | BAE_STANDBY;
lakshya 20:949d13045431 410 // 6 next telemetries value to be given by registers
lakshya 20:949d13045431 411 telemetry[32] = ATS1_EVENT_STATUS_RGTR;
lakshya 20:949d13045431 412 telemetry[33] = ATS1_SENTRAL_STATUS_RGTR;
lakshya 20:949d13045431 413 telemetry[34] = ATS1_ERROR_RGTR;
lakshya 20:949d13045431 414 telemetry[35] = ATS2_EVENT_STATUS_RGTR;
lakshya 20:949d13045431 415 telemetry[36] = ATS2_SENTRAL_STATUS_RGTR;
lakshya 20:949d13045431 416 telemetry[37] = ATS2_ERROR_RGTR;
lakshya 20:949d13045431 417
lakshya 20:949d13045431 418 telemetry[38] = BCN_FAIL_COUNT;
lakshya 20:949d13045431 419 telemetry[39] = actual_data.power_mode;
lakshya 20:949d13045431 420 telemetry[40] = HTR_CYCLE_COUNTER;//new to : implement
lakshya 20:949d13045431 421
lakshya 20:949d13045431 422 telemetry[41] = BAE_I2C_COUNTER;
lakshya 20:949d13045431 423 telemetry[42] = BAE_I2C_COUNTER>>8;
lakshya 20:949d13045431 424 telemetry[43] = ACS_MAIN_COUNTER;
lakshya 20:949d13045431 425 telemetry[44] = ACS_MAIN_COUNTER>>8;
lakshya 20:949d13045431 426 telemetry[45] = BCN_TX_MAIN_COUNTER;
lakshya 20:949d13045431 427 telemetry[46] = BCN_TX_MAIN_COUNTER>>8;
lakshya 20:949d13045431 428 telemetry[47] = EPS_MAIN_COUNTER;
lakshya 20:949d13045431 429 telemetry[48] = EPS_MAIN_COUNTER>>8;
lakshya 20:949d13045431 430 //sending in uint can be converted back to int by direct conversion for +values
lakshya 20:949d13045431 431 //make sure to convert baack to int for getting negative values
lakshya 20:949d13045431 432 //algo for that done
lakshya 20:949d13045431 433 telemetry[49] = actual_data.bit_data_acs_mm[0];
lakshya 20:949d13045431 434 telemetry[50] = actual_data.bit_data_acs_mm[0]>>8;
lakshya 20:949d13045431 435 telemetry[51] = actual_data.bit_data_acs_mm[1];
lakshya 20:949d13045431 436 telemetry[52] = actual_data.bit_data_acs_mm[1]>>8;
lakshya 20:949d13045431 437 telemetry[53] = actual_data.bit_data_acs_mm[2];
lakshya 20:949d13045431 438 telemetry[54] = actual_data.bit_data_acs_mm[2]>>8;
lakshya 20:949d13045431 439
lakshya 20:949d13045431 440 telemetry[55] = actual_data.bit_data_acs_mg[0];
lakshya 20:949d13045431 441 telemetry[56] = actual_data.bit_data_acs_mg[0]>>8;
lakshya 20:949d13045431 442 telemetry[57] = actual_data.bit_data_acs_mg[1];
lakshya 20:949d13045431 443 telemetry[58] = actual_data.bit_data_acs_mg[1]>>8;
lakshya 20:949d13045431 444 telemetry[59] = actual_data.bit_data_acs_mg[2];
lakshya 20:949d13045431 445 telemetry[60] = actual_data.bit_data_acs_mg[2]>>8;
lakshya 20:949d13045431 446
lakshya 20:949d13045431 447 telemetry[61] = BCN_TX_OC_FAULT;
lakshya 20:949d13045431 448 telemetry[61] = (telemetry[61]<<1) | ACS_TR_XY_ENABLE;
lakshya 20:949d13045431 449 telemetry[61] = (telemetry[61]<<1) | ACS_TR_Z_ENABLE;
lakshya 20:949d13045431 450 telemetry[61] = (telemetry[61]<<1) | ACS_TR_XY_OC_FAULT;
lakshya 20:949d13045431 451 telemetry[61] = (telemetry[61]<<1) | ACS_TR_Z_OC_FAULT;
lakshya 20:949d13045431 452 telemetry[61] = (telemetry[61]<<1) | ACS_TR_XY_FAULT;
lakshya 20:949d13045431 453 telemetry[61] = (telemetry[61]<<1) | EPS_CHARGER_FAULT;
lakshya 20:949d13045431 454 telemetry[61] = (telemetry[61]<<1) | EPS_CHARGER_STATUS;
lakshya 20:949d13045431 455
lakshya 27:61c856be467e 456 telemetry[62] = EPS_BATTERY_GAUGE_ALERT;
lakshya 20:949d13045431 457 telemetry[62] = (telemetry[62]<<1) | CDMS_OC_FAULT;
lakshya 20:949d13045431 458 telemetry[62] = (telemetry[62]<<1) | ACS_ATS1_OC_FAULT;
lakshya 20:949d13045431 459 telemetry[62] = (telemetry[62]<<1) | ACS_ATS2_OC_FAULT;
lakshya 20:949d13045431 460 telemetry[62] = (telemetry[62]<<1) | ACS_TR_Z_FAULT;
lakshya 27:61c856be467e 461 telemetry[62] = (telemetry[62]<<3);
lakshya 20:949d13045431 462 //3 spare
lakshya 20:949d13045431 463
lakshya 20:949d13045431 464 telemetry[63] = ACS_TR_X_PWM;
lakshya 20:949d13045431 465 telemetry[64] = ACS_TR_Y_PWM;
lakshya 20:949d13045431 466 telemetry[65] = ACS_TR_Z_PWM;
lakshya 20:949d13045431 467 //spare byte
lakshya 20:949d13045431 468 //assigned it to counter HTR_CYCLE_COUNTER
lakshya 20:949d13045431 469
lakshya 20:949d13045431 470 //assign it b_scz_angle
lakshya 39:670133e7ffd8 471 telemetry[66] = B_SCZ_ANGLE>>4;
lakshya 39:670133e7ffd8 472 telemetry[66] = (telemetry[66]<<1) | alarmmode;
lakshya 39:670133e7ffd8 473 telemetry[66] = (telemetry[66]<<1) | controlmode_mms;
lakshya 39:670133e7ffd8 474 telemetry[66] = (telemetry[66]<<2);
lakshya 20:949d13045431 475 //2 bit spare
lakshya 20:949d13045431 476
lakshya 20:949d13045431 477 for(int i=0;i<9;i++)
lakshya 20:949d13045431 478 {
lakshya 20:949d13045431 479 telemetry[67+i] = invjm_mms[i];
lakshya 20:949d13045431 480 telemetry[80+i] = jm_mms[i];
lakshya 20:949d13045431 481 }
lakshya 20:949d13045431 482
lakshya 20:949d13045431 483 for(int i=0;i<3;i++)
lakshya 20:949d13045431 484 telemetry[76+i] = bb_mms[i];
lakshya 20:949d13045431 485
lakshya 20:949d13045431 486 telemetry[79] = singularity_flag_mms;
lakshya 20:949d13045431 487
lakshya 20:949d13045431 488 for(int i=0;i<16;i++)
lakshya 20:949d13045431 489 {
lakshya 20:949d13045431 490 telemetry[89+i] = quant_data.voltage_quant[i];
lakshya 20:949d13045431 491 telemetry[105+i] = quant_data.current_quant[i];
lakshya 20:949d13045431 492 }
lakshya 20:949d13045431 493
lakshya 20:949d13045431 494 telemetry[121] = quant_data.Batt_voltage_quant;
lakshya 20:949d13045431 495 telemetry[122] = quant_data.BAE_temp_quant;
lakshya 20:949d13045431 496 telemetry[123] = (uint8_t)(actual_data.Batt_gauge_actual[1]);
lakshya 20:949d13045431 497 telemetry[124] = quant_data.Batt_temp_quant[0];
lakshya 20:949d13045431 498 telemetry[125] = quant_data.Batt_temp_quant[1];
lakshya 20:949d13045431 499 telemetry[126] = BCN_TMP;
lakshya 20:949d13045431 500
lakshya 20:949d13045431 501 //* ANY USE?
lakshya 20:949d13045431 502 ///if(BCN_TX_STATUS==2)
lakshya 20:949d13045431 503 /// telemetry[3] = telemetry[3]|0x20;
lakshya 20:949d13045431 504 ///else
lakshya 20:949d13045431 505 ///telemetry[3] = telemetry[3] & 0xDF;
lakshya 19:79e69017c855 506
lakshya 19:79e69017c855 507 //actual_data.AngularSpeed_actual[0]=5.32498;
lakshya 20:949d13045431 508 ///for(int i=0; i<3; i++)
lakshya 20:949d13045431 509 /// FCTN_CONVERT_FLOAT((float)actual_data.Bvalue_actual[i],&telemetry[26+ (i*4)]);
lakshya 20:949d13045431 510 ///for(int i=0; i<3; i++)
lakshya 20:949d13045431 511 /// FCTN_CONVERT_FLOAT((float)actual_data.AngularSpeed_actual[i],&telemetry[38+(i*4)]);
lakshya 20:949d13045431 512 ///
lakshya 20:949d13045431 513 for (int i=127; i<132;i++)
lakshya 19:79e69017c855 514 {
lakshya 19:79e69017c855 515 telemetry[i] = 0x00;
lakshya 19:79e69017c855 516 }
lakshya 19:79e69017c855 517 crc16 = CRC::crc16_gen(telemetry,132);
lakshya 19:79e69017c855 518 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 519 telemetry[133] = (uint8_t)(crc16&0x00FF);
Bragadeesh153 41:5df2bed2157d 520 printf("CRC %x %x: \r\n",telemetry[132],telemetry[133]);
lakshya 19:79e69017c855 521
lakshya 19:79e69017c855 522 break;
lakshya 19:79e69017c855 523 }
lakshya 19:79e69017c855 524 case 0x0001:
lakshya 19:79e69017c855 525 {
lakshya 19:79e69017c855 526 printf("\r\nhk data tm");
lakshya 19:79e69017c855 527 telemetry[0] = 0x60;
lakshya 19:79e69017c855 528 telemetry[1] = tc[0];
lakshya 19:79e69017c855 529 telemetry[2] = ACK_CODE;
lakshya 19:79e69017c855 530
lakshya 19:79e69017c855 531 for(int i;i<16;i++)
lakshya 20:949d13045431 532 {
lakshya 20:949d13045431 533 telemetry[i+3] = bae_HK_minmax.voltage_max[i];
lakshya 20:949d13045431 534 telemetry[i+19] = bae_HK_minmax.current_max[i];
lakshya 20:949d13045431 535 }
lakshya 19:79e69017c855 536
lakshya 20:949d13045431 537 telemetry[35] = bae_HK_minmax.Batt_voltage_max;;
lakshya 20:949d13045431 538 telemetry[36] = bae_HK_minmax.BAE_temp_max;
lakshya 19:79e69017c855 539
lakshya 20:949d13045431 540 telemetry[37] = bae_HK_minmax.Batt_SOC_max;
lakshya 19:79e69017c855 541
lakshya 20:949d13045431 542 telemetry[38] = bae_HK_minmax.Batt_temp_max[0];
lakshya 20:949d13045431 543 telemetry[39] = bae_HK_minmax.Batt_temp_max[1];
lakshya 19:79e69017c855 544
lakshya 20:949d13045431 545 /*BCN temp there*/
lakshya 20:949d13045431 546 telemetry[40] = bae_HK_minmax.BCN_TEMP_max;
lakshya 19:79e69017c855 547
lakshya 19:79e69017c855 548 for(int i=0; i<3; i++)
lakshya 20:949d13045431 549 {
lakshya 20:949d13045431 550 telemetry[41+i] = bae_HK_minmax.bit_data_acs_mm_max[i];
lakshya 20:949d13045431 551 telemetry[44+i] = bae_HK_minmax.bit_data_acs_mg_max[i];
lakshya 20:949d13045431 552 }
lakshya 19:79e69017c855 553
lakshya 19:79e69017c855 554 /*min data*/
lakshya 19:79e69017c855 555
lakshya 19:79e69017c855 556 for(int i;i<16;i++)
lakshya 20:949d13045431 557 telemetry[i+47] = bae_HK_minmax.voltage_min[i];
lakshya 19:79e69017c855 558
lakshya 20:949d13045431 559 for(int i;i<16;i++)
lakshya 20:949d13045431 560 telemetry[i+63] = bae_HK_minmax.current_min[i];
lakshya 19:79e69017c855 561
lakshya 20:949d13045431 562 telemetry[79] = bae_HK_minmax.Batt_voltage_min;
lakshya 20:949d13045431 563 telemetry[80] = bae_HK_minmax.BAE_temp_min;
lakshya 19:79e69017c855 564
lakshya 19:79e69017c855 565 /*battery soc*/
lakshya 20:949d13045431 566 telemetry[81] = bae_HK_minmax.Batt_SOC_min;
lakshya 19:79e69017c855 567
lakshya 20:949d13045431 568 telemetry[82] = bae_HK_minmax.Batt_temp_min[0];
lakshya 20:949d13045431 569 telemetry[83] = bae_HK_minmax.Batt_temp_min[1];
lakshya 19:79e69017c855 570 //huhu//
lakshya 19:79e69017c855 571
lakshya 20:949d13045431 572 /*BCN temp named as BCN_TS_BUFFER there*/
lakshya 20:949d13045431 573 telemetry[84] = bae_HK_minmax.BCN_TEMP_min;
lakshya 19:79e69017c855 574
lakshya 19:79e69017c855 575 for(int i=0; i<3; i++)
lakshya 20:949d13045431 576 {
lakshya 20:949d13045431 577 telemetry[85+i] = bae_HK_minmax.bit_data_acs_mm_min[i];
lakshya 20:949d13045431 578 telemetry[88+i] = bae_HK_minmax.bit_data_acs_mg_min[i];
lakshya 20:949d13045431 579 }
lakshya 20:949d13045431 580
lakshya 20:949d13045431 581 telemetry[90] = BCN_TX_OC_FAULT;
lakshya 20:949d13045431 582 telemetry[90] = (telemetry[90]<<1) | ACS_TR_XY_OC_FAULT;
lakshya 20:949d13045431 583 telemetry[90] = (telemetry[90]<<1) | ACS_TR_Z_OC_FAULT;
lakshya 20:949d13045431 584 telemetry[90] = (telemetry[90]<<1) | ACS_TR_XY_FAULT;
lakshya 20:949d13045431 585 //EPS CHARGER
lakshya 20:949d13045431 586 telemetry[90] = (telemetry[90]<<1) | EPS_CHARGER_FAULT;//eps_charger;
lakshya 20:949d13045431 587 telemetry[90] = (telemetry[90]<<1) | CDMS_OC_FAULT;
lakshya 20:949d13045431 588 telemetry[90] = (telemetry[90]<<1) | ACS_ATS1_OC_FAULT;
lakshya 20:949d13045431 589 telemetry[90] = (telemetry[90]<<1) | ACS_ATS2_OC_FAULT;
lakshya 20:949d13045431 590
lakshya 20:949d13045431 591 telemetry[91] = ACS_TR_Z_FAULT;
lakshya 20:949d13045431 592 //spare 23 bits
lakshya 20:949d13045431 593 telemetry[92] = 0x00;
lakshya 20:949d13045431 594 telemetry[93] = 0x00;
lakshya 20:949d13045431 595
lakshya 20:949d13045431 596 for (int i=94; i<132;i++)
lakshya 19:79e69017c855 597 {
lakshya 19:79e69017c855 598 telemetry[i] = 0x00;
lakshya 19:79e69017c855 599 }
lakshya 19:79e69017c855 600 crc16 = CRC::crc16_gen(telemetry,132);
lakshya 19:79e69017c855 601 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 602 telemetry[133] = (uint8_t)(crc16&0x00FF);
Bragadeesh153 41:5df2bed2157d 603 printf("CRC %x %x: \r\n",telemetry[132],telemetry[133]);
lakshya 19:79e69017c855 604 break;
lakshya 19:79e69017c855 605 }
lakshya 19:79e69017c855 606 default://invalid MID
lakshya 19:79e69017c855 607 {
lakshya 19:79e69017c855 608 //ACK_L234_telemetry
lakshya 19:79e69017c855 609 telemetry[0]=0xB0;
lakshya 19:79e69017c855 610 telemetry[1]=tc[0];
lakshya 19:79e69017c855 611 telemetry[2]=0x02;//for this case
lakshya 19:79e69017c855 612 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 613 {
lakshya 19:79e69017c855 614 telemetry[i]=0x00;
lakshya 19:79e69017c855 615 }
lakshya 19:79e69017c855 616 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 617 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 618 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 619 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 620 {
lakshya 19:79e69017c855 621 telemetry[i]=0x00;
lakshya 19:79e69017c855 622 }
lakshya 19:79e69017c855 623 break;
lakshya 19:79e69017c855 624 }
lakshya 19:79e69017c855 625 }
lakshya 19:79e69017c855 626 break;
lakshya 19:79e69017c855 627 }
lakshya 19:79e69017c855 628 case 0x05:
lakshya 19:79e69017c855 629 {
lakshya 19:79e69017c855 630 printf("\n\rdata for mms 0x05 flash");
lakshya 19:79e69017c855 631 /*changed*/
lakshya 19:79e69017c855 632 printf("\n\rwrite on flash\n");
lakshya 20:949d13045431 633 uint32_t FLASH_DATA;//256 bits
lakshya 19:79e69017c855 634
lakshya 19:79e69017c855 635 uint8_t VALID_MID;//to determine wether mid is valid or not otherwise to much repetition of code 1 meaning valid
lakshya 19:79e69017c855 636
lakshya 19:79e69017c855 637 uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
lakshya 19:79e69017c855 638 switch(MID )
lakshya 19:79e69017c855 639 {
lakshya 20:949d13045431 640 case 0x1100:
lakshya 19:79e69017c855 641 {
lakshya 20:949d13045431 642 //FLASH_DATA[0] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[8]);
lakshya 20:949d13045431 643 //FCTN_BAE_WR_FLASH(0,FLASH_DATA[0]);
lakshya 20:949d13045431 644 BCN_LONG_MSG_TYPE = tc[8];
Bragadeesh153 41:5df2bed2157d 645 printf("BCN_LONG_MSG_TYPE = tc[8] = : %X",tc[8]);
lakshya 20:949d13045431 646 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
lakshya 27:61c856be467e 647 FLASH_DATA = (FLASH_DATA & 0xFFFFFBFF) | (10<<(uint32_t)tc[8]);//see if uint8 to uint32 conversion works
lakshya 20:949d13045431 648 FCTN_BAE_WR_FLASH(0,FLASH_DATA);
lakshya 19:79e69017c855 649 VALID_MID=1;
lakshya 19:79e69017c855 650 break;
lakshya 19:79e69017c855 651 }
lakshya 19:79e69017c855 652 case 0x0101:
lakshya 19:79e69017c855 653 {
lakshya 20:949d13045431 654 //FLASH_DATA[1] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 27:61c856be467e 655 ACS_DETUMBLING_ALGO_TYPE = (tc[8] & 0x01);
Bragadeesh153 41:5df2bed2157d 656 printf("ACS_DETUMBLING_ALGO_TYPE = tc[8] = : %X",tc[8]);
Bragadeesh153 41:5df2bed2157d 657 //ACS_STATE = (tc[8]>>1) & 0x0F;
Bragadeesh153 41:5df2bed2157d 658 ACS_STATE = 9;
lakshya 20:949d13045431 659 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
lakshya 27:61c856be467e 660 FLASH_DATA = (FLASH_DATA & 0xFFF07FFF) | (15<<(uint32_t)tc[8]);
lakshya 20:949d13045431 661 FCTN_BAE_WR_FLASH(0,FLASH_DATA);
lakshya 19:79e69017c855 662 VALID_MID=1;
lakshya 19:79e69017c855 663 break;
lakshya 19:79e69017c855 664 }
lakshya 19:79e69017c855 665
lakshya 19:79e69017c855 666 case 0x0102:
lakshya 19:79e69017c855 667 {
lakshya 20:949d13045431 668 //FLASH_DATA[2] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 20:949d13045431 669 //EPS_BATTERY_HEATER_ENABLE = tc[8];
lakshya 20:949d13045431 670 EPS_BTRY_HTR_AUTO = tc[8];
Bragadeesh153 41:5df2bed2157d 671 printf("EPS_BTRY_HTR_AUTO = tc[8] = : %X",tc[8]);
lakshya 20:949d13045431 672 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
lakshya 27:61c856be467e 673 FLASH_DATA = (FLASH_DATA & 0xFFFFFDFF) | (9<<(uint32_t)tc[8]);
lakshya 20:949d13045431 674 FCTN_BAE_WR_FLASH(0,FLASH_DATA);
lakshya 19:79e69017c855 675 VALID_MID=1;
lakshya 19:79e69017c855 676 break;
lakshya 19:79e69017c855 677 }
lakshya 19:79e69017c855 678
lakshya 19:79e69017c855 679 case 0x0103:
lakshya 19:79e69017c855 680 {
lakshya 20:949d13045431 681 //FLASH_DATA[3] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 20:949d13045431 682 ACS_MAG_TIME_DELAY = tc[7];
lakshya 20:949d13045431 683 ACS_DEMAG_TIME_DELAY = tc[8];
Bragadeesh153 41:5df2bed2157d 684 printf("ACS_MAG_TIME_DELAY= tc[7] = : %X",tc[7]);
Bragadeesh153 41:5df2bed2157d 685 printf("ACS_DEMAG_TIME_DELAY = tc[8] = : %X",tc[8]);
lakshya 20:949d13045431 686 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(1);
lakshya 20:949d13045431 687 FLASH_DATA = (FLASH_DATA & 0xFFFF0000) | (8<<(uint32_t)tc[7]) | ((uint32_t)tc[8]);
lakshya 20:949d13045431 688 FCTN_BAE_WR_FLASH(1,FLASH_DATA);
lakshya 19:79e69017c855 689 VALID_MID=1;
lakshya 19:79e69017c855 690 break;
lakshya 19:79e69017c855 691 }
lakshya 19:79e69017c855 692 case 0x0104:
lakshya 19:79e69017c855 693 {
lakshya 20:949d13045431 694 //FLASH_DATA[4] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
Bragadeesh153 41:5df2bed2157d 695
lakshya 20:949d13045431 696 ACS_Z_FIXED_MOMENT = (8<<(uint16_t)tc[7]) | (uint16_t)tc[8];
Bragadeesh153 41:5df2bed2157d 697 printf("ACS_Z_FIXED_MOMENT = tc[7]tc[8] = : %X %X",tc[7], tc[8]);
lakshya 20:949d13045431 698 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(6);
lakshya 20:949d13045431 699 FLASH_DATA = (FLASH_DATA & 0x0000FFFF) | ((uint32_t)ACS_Z_FIXED_MOMENT<<16);
lakshya 20:949d13045431 700 FCTN_BAE_WR_FLASH(6,FLASH_DATA);
lakshya 19:79e69017c855 701 VALID_MID=1;
lakshya 19:79e69017c855 702 break;
lakshya 19:79e69017c855 703 }
lakshya 20:949d13045431 704 case 0x0106:
lakshya 19:79e69017c855 705 {
lakshya 20:949d13045431 706 //FLASH_DATA[6] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 20:949d13045431 707 ACS_MM_Z_COMSN = ((uint16_t)tc[5]<<8) | (uint16_t)tc[6];
lakshya 20:949d13045431 708 ACS_MG_Z_COMSN = ((uint16_t)tc[7]<<8) | (uint16_t)tc[8];
Bragadeesh153 41:5df2bed2157d 709 printf("ACS_MM_Z_COMSN = tc[5]tc[6] = : %X %X",tc[5], tc[6]);
Bragadeesh153 41:5df2bed2157d 710 printf("ACS_MG_Z_COMSN = tc[7]tc[8] = : %X %X",tc[7], tc[8]);
lakshya 20:949d13045431 711 FLASH_DATA = ((uint32_t)ACS_MM_Z_COMSN<<16) | (uint32_t)ACS_MG_Z_COMSN;
lakshya 20:949d13045431 712 FCTN_BAE_WR_FLASH(5,FLASH_DATA);
lakshya 20:949d13045431 713 VALID_MID=1;
lakshya 20:949d13045431 714 break;
lakshya 20:949d13045431 715 }
lakshya 20:949d13045431 716 case 0x0107:
lakshya 20:949d13045431 717 {
lakshya 20:949d13045431 718 //FLASH_DATA[5] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 20:949d13045431 719 EPS_SOC_LEVEL_12 = tc[7];
lakshya 20:949d13045431 720 EPS_SOC_LEVEL_23 = tc[8];
Bragadeesh153 41:5df2bed2157d 721 printf("EPS_SOC_LEVEL_12 = : %X",tc[7]);
Bragadeesh153 41:5df2bed2157d 722 printf("EPS_SOC_LEVEL_23 = : %X",tc[8]);
Bragadeesh153 41:5df2bed2157d 723 printf("ACS_MG_Z_COMSN = tc[7]tc[8] = : %X %X",tc[7], tc[8]);
lakshya 20:949d13045431 724 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(1);
lakshya 20:949d13045431 725 FLASH_DATA = (FLASH_DATA & 0x0000FFFF) | ((uint32_t)tc[7]<<24) | ((uint32_t)tc[8]<<16);
lakshya 20:949d13045431 726 FCTN_BAE_WR_FLASH(1,FLASH_DATA);
lakshya 19:79e69017c855 727 VALID_MID=1;
lakshya 19:79e69017c855 728 break;
lakshya 19:79e69017c855 729 }
lakshya 20:949d13045431 730 case 0x0108:
lakshya 19:79e69017c855 731 {
lakshya 20:949d13045431 732 //FLASH_DATA[6] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 20:949d13045431 733 ACS_MM_X_COMSN = ((uint16_t)tc[5]<<8) | (uint16_t)tc[6];
lakshya 20:949d13045431 734 ACS_MM_Y_COMSN = ((uint16_t)tc[7]<<8) | (uint16_t)tc[8];
Bragadeesh153 41:5df2bed2157d 735 printf("ACS_MM_X_COMSN = tc[5]tc[6] = : %X %X",tc[5], tc[6]);
Bragadeesh153 41:5df2bed2157d 736 printf("ACS_MM_Y_COMSN = tc[7]tc[8] = : %X %X",tc[7], tc[8]);
lakshya 20:949d13045431 737 FLASH_DATA = ((uint32_t)ACS_MM_X_COMSN<<16) | (uint32_t)ACS_MM_Y_COMSN;
lakshya 20:949d13045431 738 FCTN_BAE_WR_FLASH(3,FLASH_DATA);
lakshya 19:79e69017c855 739 VALID_MID=1;
lakshya 19:79e69017c855 740 break;
lakshya 19:79e69017c855 741 }
lakshya 20:949d13045431 742 case 0x0109:
lakshya 19:79e69017c855 743 {
lakshya 20:949d13045431 744 //FLASH_DATA[7] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
lakshya 20:949d13045431 745 ACS_MG_X_COMSN = ((uint16_t)tc[5]<<8) | (uint16_t)tc[6];
lakshya 20:949d13045431 746 ACS_MG_Y_COMSN = ((uint16_t)tc[7]<<8) | (uint16_t)tc[8];
Bragadeesh153 41:5df2bed2157d 747 printf("ACS_MG_X_COMSN = tc[5]tc[6] = : %X %X",tc[5], tc[6]);
Bragadeesh153 41:5df2bed2157d 748 printf("ACS_MG_Y_COMSN = tc[7]tc[8] = : %X %X",tc[7], tc[8]);
lakshya 20:949d13045431 749 FLASH_DATA = ((uint32_t)ACS_MG_X_COMSN<<16) | (uint32_t)ACS_MG_Y_COMSN;
lakshya 20:949d13045431 750 FCTN_BAE_WR_FLASH(4,FLASH_DATA);
lakshya 19:79e69017c855 751 VALID_MID=1;
lakshya 19:79e69017c855 752 break;
lakshya 19:79e69017c855 753 }
lakshya 19:79e69017c855 754
lakshya 19:79e69017c855 755 default:
lakshya 19:79e69017c855 756 {
lakshya 19:79e69017c855 757 printf("Invalid MMS case 0x05 invalid MID\r\n");
lakshya 19:79e69017c855 758 VALID_MID=0;
lakshya 19:79e69017c855 759 //ACK_L234_telemetry
lakshya 19:79e69017c855 760 break;
lakshya 19:79e69017c855 761
lakshya 19:79e69017c855 762 }
lakshya 19:79e69017c855 763 }
lakshya 19:79e69017c855 764
lakshya 19:79e69017c855 765 if(VALID_MID==1)//valid MID
lakshya 19:79e69017c855 766 {
lakshya 19:79e69017c855 767 telemetry[0]=0xB0;//or 0x60? check
lakshya 19:79e69017c855 768 telemetry[1]=tc[0];
lakshya 19:79e69017c855 769 telemetry[2]=0xA0;// when valid
lakshya 19:79e69017c855 770 }
lakshya 19:79e69017c855 771 else if(VALID_MID==0)//invalid MID
lakshya 19:79e69017c855 772 {
lakshya 19:79e69017c855 773 telemetry[0]=0xB0;
lakshya 19:79e69017c855 774 telemetry[1]=tc[0];
lakshya 19:79e69017c855 775 telemetry[2]=0x02;//for this case
lakshya 19:79e69017c855 776 }
lakshya 19:79e69017c855 777
lakshya 19:79e69017c855 778 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 779 {
lakshya 19:79e69017c855 780 telemetry[i]=0x00;
lakshya 19:79e69017c855 781 }
lakshya 19:79e69017c855 782 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 783 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 784 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 785 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 786 {
lakshya 19:79e69017c855 787 telemetry[i]=0x00;
lakshya 19:79e69017c855 788 }
lakshya 19:79e69017c855 789
lakshya 19:79e69017c855 790 printf("\n\rWritten on Flash");
lakshya 19:79e69017c855 791 break;
lakshya 19:79e69017c855 792 }
lakshya 19:79e69017c855 793 default://when invalid service subtype
lakshya 19:79e69017c855 794 {
lakshya 19:79e69017c855 795 printf("\n\r MMS invalid Service Subtype");
lakshya 19:79e69017c855 796 //ACK_L234_telemetry
lakshya 19:79e69017c855 797 telemetry[0]=0xB0;
lakshya 19:79e69017c855 798 telemetry[1]=tc[0];
lakshya 19:79e69017c855 799 telemetry[2]=0x02;//for this case
lakshya 19:79e69017c855 800 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 801 {
lakshya 19:79e69017c855 802 telemetry[i]=0x00;
lakshya 19:79e69017c855 803 }
lakshya 19:79e69017c855 804 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 805 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 806 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 807 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 808 {
lakshya 19:79e69017c855 809 telemetry[i]=0x00;
lakshya 19:79e69017c855 810 }
lakshya 19:79e69017c855 811 break;
lakshya 19:79e69017c855 812 }
lakshya 19:79e69017c855 813 }
lakshya 19:79e69017c855 814 break;
lakshya 19:79e69017c855 815 }
lakshya 19:79e69017c855 816 case 0x80:
lakshya 19:79e69017c855 817 {
lakshya 19:79e69017c855 818 //printf("Function Management Service\r\n");
lakshya 19:79e69017c855 819 uint8_t service_subtype=(tc[2]&0x0F);
lakshya 19:79e69017c855 820 switch(service_subtype)
lakshya 19:79e69017c855 821 {
lakshya 19:79e69017c855 822 case 0x01:
lakshya 19:79e69017c855 823 {
lakshya 19:79e69017c855 824 printf("\n\rFMS Activated");
lakshya 19:79e69017c855 825 uint8_t fid=tc[3];//changed from pid to fid
lakshya 19:79e69017c855 826 switch(fid)
lakshya 19:79e69017c855 827 {
lakshya 19:79e69017c855 828 case 0xE0:
lakshya 19:79e69017c855 829 {
lakshya 20:949d13045431 830 float mag_data_comm[3]={uint16_to_float(-1000,1000,ACS_MM_X_COMSN),uint16_to_float(-1000,1000,ACS_MM_Y_COMSN),uint16_to_float(-1000,1000,ACS_MM_Z_COMSN)};
lakshya 20:949d13045431 831 float gyro_data_comm[3]={uint16_to_float(-5000,5000,ACS_MG_X_COMSN),uint16_to_float(-5000,5000,ACS_MG_Y_COMSN),uint16_to_float(-5000,5000,ACS_MG_Z_COMSN)};
lakshya 20:949d13045431 832 float moment_comm[3];
lakshya 20:949d13045431 833 printf("ACS_COMSN SOFTWARE\r\n");
lakshya 19:79e69017c855 834 //ACK_L234_telemetry
lakshya 20:949d13045431 835 ACS_STATE = tc[4];
lakshya 20:949d13045431 836 if(ACS_STATE == 7) // Nominal mode
lakshya 20:949d13045431 837 {
lakshya 20:949d13045431 838 printf("\n\r Nominal mode \n");
lakshya 20:949d13045431 839 FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE);
lakshya 20:949d13045431 840 printf("\n\r Moment values returned by control algo \n");
lakshya 20:949d13045431 841 for(int i=0; i<3; i++)
lakshya 20:949d13045431 842 {
lakshya 20:949d13045431 843 printf("%f\t",moment_comm[i]);
lakshya 20:949d13045431 844 }
lakshya 20:949d13045431 845
lakshya 20:949d13045431 846 }
lakshya 20:949d13045431 847 else if(ACS_STATE == 8) // Auto Control
lakshya 20:949d13045431 848 {
lakshya 20:949d13045431 849 printf("\n\r Auto control mode \n");
lakshya 20:949d13045431 850 FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE);
lakshya 20:949d13045431 851 printf("\n\r Moment values returned by control algo \n");
lakshya 20:949d13045431 852 for(int i=0; i<3; i++)
lakshya 20:949d13045431 853 {
lakshya 20:949d13045431 854 printf("%f\t",moment_comm[i]);
lakshya 20:949d13045431 855 }
lakshya 20:949d13045431 856 }
lakshya 20:949d13045431 857 else if(ACS_STATE == 9) // Detumbling
lakshya 20:949d13045431 858 {
lakshya 20:949d13045431 859 FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
lakshya 20:949d13045431 860 printf("\n\r Moment values returned by control algo \n");
lakshya 20:949d13045431 861 for(int i=0; i<3; i++)
lakshya 20:949d13045431 862 {
lakshya 20:949d13045431 863 printf("%f\t",moment_comm[i]);
lakshya 20:949d13045431 864 }
lakshya 20:949d13045431 865 }
lakshya 19:79e69017c855 866 else
lakshya 19:79e69017c855 867 {
lakshya 20:949d13045431 868 ACS_STATUS = 7;
lakshya 19:79e69017c855 869 }
lakshya 20:949d13045431 870
lakshya 20:949d13045431 871 // Control algo commissioning
lakshya 39:670133e7ffd8 872 uint16_t moment_ret;
Bragadeesh153 41:5df2bed2157d 873 telemetry[0] = 0x78;
Bragadeesh153 40:c2538d97e78b 874 telemetry[1] = tc[0];
Bragadeesh153 40:c2538d97e78b 875 telemetry[2] = ACK_CODE;
lakshya 39:670133e7ffd8 876 telemetry[3] = 0x00;
lakshya 39:670133e7ffd8 877 telemetry[4] = ACS_STATUS;
lakshya 39:670133e7ffd8 878 moment_ret = float_to_uint16(-2.2,2.2,moment_comm[0]);
lakshya 39:670133e7ffd8 879 telemetry[5] = (uint8_t)(moment_ret>>8);
lakshya 39:670133e7ffd8 880 telemetry[6] = (uint8_t)moment_ret;
lakshya 39:670133e7ffd8 881 moment_ret = float_to_uint16(-2.2,2.2,moment_comm[0]);
lakshya 39:670133e7ffd8 882 telemetry[7] = (uint8_t)(moment_ret>>8);
lakshya 39:670133e7ffd8 883 telemetry[8] = (uint8_t)moment_ret;
lakshya 39:670133e7ffd8 884 moment_ret = float_to_uint16(-2.2,2.2,moment_comm[2]);
lakshya 39:670133e7ffd8 885 telemetry[9] = (uint8_t)(moment_ret>>8);
lakshya 39:670133e7ffd8 886 telemetry[10] = (uint8_t)moment_ret;
lakshya 39:670133e7ffd8 887
lakshya 39:670133e7ffd8 888 //FCTN_CONVERT_FLOAT(moment_comm[0],&telemetry[4]); //telemetry[4] - telemetry[7]
lakshya 39:670133e7ffd8 889 //FCTN_CONVERT_FLOAT(moment_comm[1],&telemetry[8]); //telemetry[8] - telemetry[11]
lakshya 39:670133e7ffd8 890 //FCTN_CONVERT_FLOAT(moment_comm[2],&telemetry[12]); //telemetry[12] - telemetry[15]
lakshya 19:79e69017c855 891 // to include commission TR as well
lakshya 39:670133e7ffd8 892 for(uint8_t i=11;i<132;i++)
lakshya 19:79e69017c855 893 {
lakshya 19:79e69017c855 894 telemetry[i]=0x00;
lakshya 19:79e69017c855 895 }
lakshya 19:79e69017c855 896 crc16 = CRC::crc16_gen(telemetry,132);
Bragadeesh153 41:5df2bed2157d 897 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 41:5df2bed2157d 898 telemetry[133] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 899 break;
lakshya 19:79e69017c855 900 }
lakshya 19:79e69017c855 901 case 0xE1:
lakshya 19:79e69017c855 902 {
lakshya 20:949d13045431 903 printf("HARDWARE_COMSN\r\n");
lakshya 19:79e69017c855 904 //ACK_L234_telemetry
lakshya 39:670133e7ffd8 905 uint8_t SENSOR_NO;
lakshya 20:949d13045431 906
lakshya 20:949d13045431 907 int init1=0;
lakshya 20:949d13045431 908 int init2=0;
lakshya 20:949d13045431 909 int data1=0;
lakshya 20:949d13045431 910 int data2=0;
lakshya 39:670133e7ffd8 911
lakshya 39:670133e7ffd8 912 uint16_t assign_val;
lakshya 19:79e69017c855 913
lakshya 20:949d13045431 914 float PWM_tc[3];
lakshya 39:670133e7ffd8 915 PWM_tc[0]=(float(tc[4]))/32768 - 1;
lakshya 39:670133e7ffd8 916 PWM_tc[1]=(float(tc[5]))/32768 - 1;
lakshya 39:670133e7ffd8 917 PWM_tc[2]=(float(tc[6]))/32768 - 1;
lakshya 20:949d13045431 918
lakshya 20:949d13045431 919 DRV_Z_EN = 1;
lakshya 20:949d13045431 920 DRV_XY_EN = 1;
Bragadeesh153 41:5df2bed2157d 921 telemetry[0]=0x78;
lakshya 19:79e69017c855 922 telemetry[1]=tc[0];
lakshya 19:79e69017c855 923 telemetry[2]=ACK_CODE;
lakshya 39:670133e7ffd8 924 telemetry[3] = 0x00;
lakshya 39:670133e7ffd8 925
lakshya 39:670133e7ffd8 926 SENSOR_NO = 0;
lakshya 19:79e69017c855 927
lakshya 20:949d13045431 928 PWM1 = 0;
lakshya 20:949d13045431 929 PWM2 = 0;
lakshya 20:949d13045431 930 PWM3 = 0;
lakshya 20:949d13045431 931
lakshya 20:949d13045431 932 wait_ms(ACS_DEMAG_TIME_DELAY);
lakshya 20:949d13045431 933 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 934 wait_ms(5);
lakshya 20:949d13045431 935 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 936 wait_ms(5);
lakshya 20:949d13045431 937 //will it lead to causing delay in i2c interrupt
lakshya 20:949d13045431 938 init1 = SENSOR_INIT();
lakshya 20:949d13045431 939 if( init1 == 1)
lakshya 20:949d13045431 940 {
lakshya 20:949d13045431 941 data1 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 942 }
lakshya 20:949d13045431 943 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 944 wait_ms(5);
lakshya 20:949d13045431 945 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 946 wait_ms(5);
lakshya 20:949d13045431 947
lakshya 20:949d13045431 948 if(data1 == 0)
lakshya 20:949d13045431 949 {
lakshya 20:949d13045431 950 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 951 wait_ms(5);
lakshya 20:949d13045431 952 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 953 }
lakshya 20:949d13045431 954 else if(data1==1)
lakshya 20:949d13045431 955 {
lakshya 20:949d13045431 956 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
lakshya 20:949d13045431 957 }
lakshya 20:949d13045431 958 else if(data1==2)
lakshya 20:949d13045431 959 {
lakshya 20:949d13045431 960 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
lakshya 20:949d13045431 961 }
lakshya 20:949d13045431 962 else if(data1==3)
lakshya 20:949d13045431 963 {
lakshya 20:949d13045431 964 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x30;
lakshya 20:949d13045431 965 }
lakshya 39:670133e7ffd8 966
lakshya 39:670133e7ffd8 967 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
lakshya 39:670133e7ffd8 968 telemetry[5] = (assign_val>>8);
lakshya 39:670133e7ffd8 969 telemetry[6] = assign_val;
lakshya 39:670133e7ffd8 970
lakshya 39:670133e7ffd8 971 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
lakshya 39:670133e7ffd8 972 telemetry[7] = (assign_val>>8);
lakshya 39:670133e7ffd8 973 telemetry[8] = assign_val;
lakshya 39:670133e7ffd8 974
lakshya 39:670133e7ffd8 975 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
lakshya 39:670133e7ffd8 976 telemetry[9] = (assign_val>>8);
lakshya 39:670133e7ffd8 977 telemetry[10] = assign_val;
lakshya 39:670133e7ffd8 978
lakshya 39:670133e7ffd8 979 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[0]);
lakshya 39:670133e7ffd8 980 telemetry[11] = (assign_val>>8);
lakshya 39:670133e7ffd8 981 telemetry[12] = assign_val;
lakshya 39:670133e7ffd8 982
lakshya 39:670133e7ffd8 983 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[1]);
lakshya 39:670133e7ffd8 984 telemetry[13] = (assign_val>>8);
lakshya 39:670133e7ffd8 985 telemetry[14] = assign_val;
lakshya 39:670133e7ffd8 986
lakshya 39:670133e7ffd8 987 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[2]);
lakshya 39:670133e7ffd8 988 telemetry[15] = (assign_val>>8);
lakshya 39:670133e7ffd8 989 telemetry[16] = assign_val;
lakshya 39:670133e7ffd8 990
lakshya 20:949d13045431 991 init2 = SENSOR_INIT();
lakshya 20:949d13045431 992 if( init2 == 1)
lakshya 20:949d13045431 993 {
lakshya 20:949d13045431 994 data2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 995 }
lakshya 39:670133e7ffd8 996 //uint8_t ats_data=1;
lakshya 19:79e69017c855 997
lakshya 20:949d13045431 998 if(data2 == 0)
lakshya 20:949d13045431 999 {
lakshya 20:949d13045431 1000 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1001 wait_ms(5);
lakshya 20:949d13045431 1002 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 39:670133e7ffd8 1003 SENSOR_NO = 0;
lakshya 20:949d13045431 1004 if(data1 == 2)
lakshya 20:949d13045431 1005 {
lakshya 20:949d13045431 1006 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1007 wait_ms(5);
lakshya 20:949d13045431 1008 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 39:670133e7ffd8 1009 SENSOR_NO = 1;
lakshya 20:949d13045431 1010 }
lakshya 20:949d13045431 1011 else if(data1 == 3)
lakshya 20:949d13045431 1012 {
lakshya 20:949d13045431 1013 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1014 wait_ms(5);
lakshya 20:949d13045431 1015 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 39:670133e7ffd8 1016 SENSOR_NO = 1;
lakshya 20:949d13045431 1017 }
lakshya 20:949d13045431 1018 else if(data1 == 1)
lakshya 20:949d13045431 1019 {
lakshya 20:949d13045431 1020 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1021 wait_ms(5);
lakshya 20:949d13045431 1022 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
lakshya 39:670133e7ffd8 1023 SENSOR_NO = 0;
lakshya 39:670133e7ffd8 1024
lakshya 20:949d13045431 1025 }
lakshya 20:949d13045431 1026
lakshya 20:949d13045431 1027 }
lakshya 20:949d13045431 1028 else if(data2==1)
lakshya 20:949d13045431 1029 {
lakshya 20:949d13045431 1030 if(data1 == 2)
lakshya 20:949d13045431 1031 {
lakshya 20:949d13045431 1032 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1033 wait_ms(5);
lakshya 20:949d13045431 1034 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1035 wait_ms(5);
lakshya 20:949d13045431 1036 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
lakshya 20:949d13045431 1037 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 39:670133e7ffd8 1038 SENSOR_NO = 1;
lakshya 20:949d13045431 1039 }
lakshya 20:949d13045431 1040 else if(data1 == 3)
lakshya 20:949d13045431 1041 {
lakshya 20:949d13045431 1042 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1043 wait_ms(5);
lakshya 20:949d13045431 1044 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1045 wait_ms(5);
lakshya 20:949d13045431 1046 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
lakshya 20:949d13045431 1047 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 39:670133e7ffd8 1048 SENSOR_NO = 1;
lakshya 20:949d13045431 1049 }
lakshya 20:949d13045431 1050 else
lakshya 20:949d13045431 1051 {
lakshya 20:949d13045431 1052 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
lakshya 39:670133e7ffd8 1053 SENSOR_NO = 0;
lakshya 39:670133e7ffd8 1054 //ats_data = 0;
lakshya 20:949d13045431 1055 }
lakshya 20:949d13045431 1056 }
lakshya 20:949d13045431 1057
lakshya 20:949d13045431 1058 else if(data2==2)
lakshya 20:949d13045431 1059 {
lakshya 20:949d13045431 1060 if(data1 == 3)
lakshya 20:949d13045431 1061 {
lakshya 20:949d13045431 1062 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1063 wait_ms(5);
lakshya 20:949d13045431 1064 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1065 wait_ms(5);
lakshya 20:949d13045431 1066 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
lakshya 20:949d13045431 1067 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 39:670133e7ffd8 1068 SENSOR_NO = 1;
lakshya 20:949d13045431 1069 }
lakshya 20:949d13045431 1070 else
lakshya 20:949d13045431 1071 {
lakshya 20:949d13045431 1072 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 39:670133e7ffd8 1073 SENSOR_NO = 2;
lakshya 20:949d13045431 1074 }
lakshya 20:949d13045431 1075 }
lakshya 20:949d13045431 1076 else if(data2==3)
lakshya 20:949d13045431 1077 {
lakshya 20:949d13045431 1078 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 39:670133e7ffd8 1079 SENSOR_NO = 2;
lakshya 20:949d13045431 1080 }
lakshya 39:670133e7ffd8 1081
lakshya 39:670133e7ffd8 1082 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
lakshya 39:670133e7ffd8 1083 telemetry[17] = (assign_val>>8);
lakshya 39:670133e7ffd8 1084 telemetry[18] = assign_val;
lakshya 39:670133e7ffd8 1085
lakshya 39:670133e7ffd8 1086 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
lakshya 39:670133e7ffd8 1087 telemetry[19] = (assign_val>>8);
lakshya 39:670133e7ffd8 1088 telemetry[20] = assign_val;
lakshya 39:670133e7ffd8 1089
lakshya 39:670133e7ffd8 1090 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
lakshya 39:670133e7ffd8 1091 telemetry[21] = (assign_val>>8);
lakshya 39:670133e7ffd8 1092 telemetry[22] = assign_val;
lakshya 39:670133e7ffd8 1093
lakshya 39:670133e7ffd8 1094 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[0]);
lakshya 39:670133e7ffd8 1095 telemetry[23] = (assign_val>>8);
lakshya 39:670133e7ffd8 1096 telemetry[24] = assign_val;
lakshya 39:670133e7ffd8 1097
lakshya 39:670133e7ffd8 1098 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[1]);
lakshya 39:670133e7ffd8 1099 telemetry[25] = (assign_val>>8);
lakshya 39:670133e7ffd8 1100 telemetry[26] = assign_val;
lakshya 39:670133e7ffd8 1101
lakshya 39:670133e7ffd8 1102 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[2]);
lakshya 39:670133e7ffd8 1103 telemetry[27] = (assign_val>>8);
lakshya 39:670133e7ffd8 1104 telemetry[28] = assign_val;
lakshya 39:670133e7ffd8 1105
lakshya 39:670133e7ffd8 1106 telemetry[4] = ACS_ATS_STATUS;
lakshya 39:670133e7ffd8 1107
lakshya 19:79e69017c855 1108
lakshya 20:949d13045431 1109 SelectLineb3 =0;
lakshya 20:949d13045431 1110 SelectLineb2 =1;
lakshya 20:949d13045431 1111 SelectLineb1 =0;
lakshya 20:949d13045431 1112 SelectLineb0 =1;
lakshya 20:949d13045431 1113 int resistance;
lakshya 20:949d13045431 1114 PWM1 = PWM_tc[0];
lakshya 20:949d13045431 1115 PWM2 = 0;
lakshya 20:949d13045431 1116 PWM3 = 0;
lakshya 20:949d13045431 1117
lakshya 20:949d13045431 1118 wait_ms(ACS_DEMAG_TIME_DELAY);
lakshya 39:670133e7ffd8 1119 if(SENSOR_NO != 0)
lakshya 20:949d13045431 1120 SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1121 actual_data.current_actual[5]=CurrentInput.read();
lakshya 20:949d13045431 1122 actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
lakshya 20:949d13045431 1123
lakshya 20:949d13045431 1124 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
lakshya 20:949d13045431 1125 if(actual_data.current_actual[5]>1.47)
lakshya 20:949d13045431 1126 {
lakshya 20:949d13045431 1127 actual_data.current_actual[5]=3694/log(24.032242*resistance);
lakshya 20:949d13045431 1128 }
lakshya 20:949d13045431 1129 else
lakshya 20:949d13045431 1130 {
lakshya 20:949d13045431 1131 actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
lakshya 20:949d13045431 1132 }
lakshya 20:949d13045431 1133
lakshya 20:949d13045431 1134 //to be edited final tele
lakshya 39:670133e7ffd8 1135 //uint16_t assign_val;
lakshya 20:949d13045431 1136 assign_val = float_to_uint16(-100,100,actual_data.current_actual[5]);//assuming max min values for current to be diss
lakshya 20:949d13045431 1137 telemetry[29] = (assign_val>>8);
lakshya 20:949d13045431 1138 telemetry[30] = assign_val;
lakshya 20:949d13045431 1139
lakshya 20:949d13045431 1140 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
lakshya 20:949d13045431 1141 telemetry[31] = (assign_val>>8);
lakshya 20:949d13045431 1142 telemetry[32] = assign_val;
lakshya 20:949d13045431 1143
lakshya 20:949d13045431 1144 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
lakshya 20:949d13045431 1145 telemetry[33] = (assign_val>>8);
lakshya 20:949d13045431 1146 telemetry[34] = assign_val;
lakshya 20:949d13045431 1147
lakshya 20:949d13045431 1148 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
lakshya 20:949d13045431 1149 telemetry[35] = (assign_val>>8);
lakshya 20:949d13045431 1150 telemetry[36] = assign_val;
lakshya 20:949d13045431 1151
lakshya 20:949d13045431 1152 PWM1 = 0;
lakshya 20:949d13045431 1153 PWM2 = PWM_tc[1];
lakshya 20:949d13045431 1154 PWM3 = 0;
lakshya 20:949d13045431 1155
lakshya 20:949d13045431 1156 wait_ms(ACS_DEMAG_TIME_DELAY);
lakshya 20:949d13045431 1157
lakshya 39:670133e7ffd8 1158 if(SENSOR_NO != 0)
lakshya 20:949d13045431 1159 SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1160 actual_data.current_actual[5]=CurrentInput.read();
lakshya 20:949d13045431 1161 actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
lakshya 20:949d13045431 1162
lakshya 20:949d13045431 1163
lakshya 20:949d13045431 1164 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
lakshya 20:949d13045431 1165 if(actual_data.current_actual[5]>1.47)
lakshya 20:949d13045431 1166 {
lakshya 20:949d13045431 1167 actual_data.current_actual[5]=3694/log(24.032242*resistance);
lakshya 20:949d13045431 1168 }
lakshya 20:949d13045431 1169 else
lakshya 20:949d13045431 1170 {
lakshya 20:949d13045431 1171 actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
lakshya 20:949d13045431 1172 }
lakshya 39:670133e7ffd8 1173
lakshya 39:670133e7ffd8 1174 assign_val = float_to_uint16(-100,100,actual_data.current_actual[5]);//assuming max min values for current to be diss
lakshya 39:670133e7ffd8 1175 telemetry[37] = (assign_val>>8);
lakshya 39:670133e7ffd8 1176 telemetry[38] = assign_val;
lakshya 39:670133e7ffd8 1177
lakshya 39:670133e7ffd8 1178 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
lakshya 39:670133e7ffd8 1179 telemetry[39] = (assign_val>>8);
lakshya 39:670133e7ffd8 1180 telemetry[40] = assign_val;
lakshya 39:670133e7ffd8 1181
lakshya 39:670133e7ffd8 1182 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
lakshya 39:670133e7ffd8 1183 telemetry[41] = (assign_val>>8);
lakshya 39:670133e7ffd8 1184 telemetry[42] = assign_val;
lakshya 39:670133e7ffd8 1185
lakshya 39:670133e7ffd8 1186 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
lakshya 39:670133e7ffd8 1187 telemetry[43] = (assign_val>>8);
lakshya 39:670133e7ffd8 1188 telemetry[44] = assign_val;
lakshya 20:949d13045431 1189
lakshya 20:949d13045431 1190 PWM1 = 0;
lakshya 20:949d13045431 1191 PWM2 = 0;
lakshya 20:949d13045431 1192 PWM3 = PWM_tc[2];
lakshya 20:949d13045431 1193
lakshya 20:949d13045431 1194 wait_ms(ACS_DEMAG_TIME_DELAY);
lakshya 20:949d13045431 1195
lakshya 39:670133e7ffd8 1196 if(SENSOR_NO != 0)
lakshya 20:949d13045431 1197 SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1198 actual_data.current_actual[5]=CurrentInput.read();
lakshya 20:949d13045431 1199 actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
lakshya 20:949d13045431 1200
lakshya 20:949d13045431 1201 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
lakshya 20:949d13045431 1202 if(actual_data.current_actual[5]>1.47)
lakshya 20:949d13045431 1203 {
lakshya 20:949d13045431 1204 actual_data.current_actual[5]=3694/log(24.032242*resistance);
lakshya 20:949d13045431 1205 }
lakshya 20:949d13045431 1206 else
lakshya 20:949d13045431 1207 {
lakshya 20:949d13045431 1208 actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
lakshya 20:949d13045431 1209 }
lakshya 39:670133e7ffd8 1210 assign_val = float_to_uint16(-100,100,actual_data.current_actual[5]);//assuming max min values for current to be diss
lakshya 39:670133e7ffd8 1211 telemetry[45] = (assign_val>>8);
lakshya 39:670133e7ffd8 1212 telemetry[46] = assign_val;
lakshya 39:670133e7ffd8 1213
lakshya 39:670133e7ffd8 1214 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
lakshya 39:670133e7ffd8 1215 telemetry[47] = (assign_val>>8);
lakshya 39:670133e7ffd8 1216 telemetry[48] = assign_val;
lakshya 20:949d13045431 1217
lakshya 39:670133e7ffd8 1218 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
lakshya 39:670133e7ffd8 1219 telemetry[49] = (assign_val>>8);
lakshya 39:670133e7ffd8 1220 telemetry[50] = assign_val;
lakshya 39:670133e7ffd8 1221
lakshya 39:670133e7ffd8 1222 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
lakshya 39:670133e7ffd8 1223 telemetry[51] = (assign_val>>8);
lakshya 39:670133e7ffd8 1224 telemetry[52] = assign_val;
lakshya 39:670133e7ffd8 1225
lakshya 20:949d13045431 1226 PWM1 = 0;
lakshya 20:949d13045431 1227 PWM2 = 0;
lakshya 20:949d13045431 1228 PWM3 = 0;
lakshya 20:949d13045431 1229
lakshya 20:949d13045431 1230 wait_ms(ACS_DEMAG_TIME_DELAY);
lakshya 20:949d13045431 1231
lakshya 39:670133e7ffd8 1232 if(SENSOR_NO != 0)
lakshya 20:949d13045431 1233 SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1234 actual_data.current_actual[5]=CurrentInput.read();
lakshya 20:949d13045431 1235 actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
lakshya 20:949d13045431 1236
lakshya 20:949d13045431 1237 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
lakshya 20:949d13045431 1238 if(actual_data.current_actual[5]>1.47)
lakshya 20:949d13045431 1239 {
lakshya 20:949d13045431 1240 actual_data.current_actual[5]=3694/log(24.032242*resistance);
lakshya 20:949d13045431 1241 }
lakshya 20:949d13045431 1242 else
lakshya 20:949d13045431 1243 {
lakshya 20:949d13045431 1244 actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
lakshya 20:949d13045431 1245 }
lakshya 39:670133e7ffd8 1246
lakshya 39:670133e7ffd8 1247 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
lakshya 39:670133e7ffd8 1248 telemetry[53] = (assign_val>>8);
lakshya 39:670133e7ffd8 1249 telemetry[54] = assign_val;
lakshya 39:670133e7ffd8 1250
lakshya 39:670133e7ffd8 1251 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
lakshya 39:670133e7ffd8 1252 telemetry[55] = (assign_val>>8);
lakshya 39:670133e7ffd8 1253 telemetry[56] = assign_val;
lakshya 39:670133e7ffd8 1254
lakshya 39:670133e7ffd8 1255 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
lakshya 39:670133e7ffd8 1256 telemetry[57] = (assign_val>>8);
lakshya 39:670133e7ffd8 1257 telemetry[58] = assign_val;
lakshya 20:949d13045431 1258
lakshya 20:949d13045431 1259 // for(int i=0; i<12; i++)
lakshya 20:949d13045431 1260 // FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]);
lakshya 20:949d13045431 1261
lakshya 20:949d13045431 1262 // FCTN_ATS_DATA_ACQ(); //get data
lakshya 19:79e69017c855 1263
lakshya 20:949d13045431 1264 // to include commission TR as well
lakshya 39:670133e7ffd8 1265
Bragadeesh153 40:c2538d97e78b 1266 telemetry[59] = SENSOR_NO;
lakshya 39:670133e7ffd8 1267
lakshya 39:670133e7ffd8 1268 for(uint8_t i=60;i<132;i++)
lakshya 19:79e69017c855 1269 {
lakshya 20:949d13045431 1270 telemetry[i]=0x00;
lakshya 19:79e69017c855 1271 }
lakshya 19:79e69017c855 1272
lakshya 19:79e69017c855 1273 crc16 = CRC::crc16_gen(telemetry,132);
Bragadeesh153 41:5df2bed2157d 1274 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
Bragadeesh153 41:5df2bed2157d 1275 telemetry[133] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1276 break;
lakshya 19:79e69017c855 1277 }
lakshya 19:79e69017c855 1278 case 0xE2:
lakshya 19:79e69017c855 1279 {
lakshya 20:949d13045431 1280 uint8_t BCN_SPND_STATE;
lakshya 20:949d13045431 1281 BCN_SPND_STATE=tc[4];
lakshya 20:949d13045431 1282 if(BCN_SPND_STATE==0x00)
lakshya 19:79e69017c855 1283 {
lakshya 20:949d13045431 1284 BCN_SPND_TX=0;
lakshya 19:79e69017c855 1285 //stop BCN_STANDBY_TIMER.stop();//create
lakshya 19:79e69017c855 1286 telemetry[2]=0xA0;
lakshya 19:79e69017c855 1287 }
lakshya 20:949d13045431 1288 else if(BCN_SPND_STATE==0x01)
lakshya 19:79e69017c855 1289 {
lakshya 33:76f2b8735501 1290 FCTN_BCN_SPND_TX();
lakshya 19:79e69017c855 1291 //stop BCN_STANDBY_TIMER.start();//create
lakshya 19:79e69017c855 1292 if(BCN_TX_MAIN_STATUS==0)
lakshya 19:79e69017c855 1293 {
lakshya 19:79e69017c855 1294 telemetry[2]=0xA0;
lakshya 19:79e69017c855 1295 }
lakshya 19:79e69017c855 1296 else if(BCN_TX_MAIN_STATUS==1)
lakshya 19:79e69017c855 1297 {
lakshya 19:79e69017c855 1298 telemetry[2]=0xC0;
lakshya 19:79e69017c855 1299 }
lakshya 19:79e69017c855 1300 }
lakshya 19:79e69017c855 1301 else
lakshya 19:79e69017c855 1302 {
lakshya 19:79e69017c855 1303 telemetry[2]=0x02;
lakshya 19:79e69017c855 1304 }
lakshya 19:79e69017c855 1305 //ACK_L234_telemetry
lakshya 19:79e69017c855 1306 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1307 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1308 //ack_code taken care above
lakshya 19:79e69017c855 1309 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1310 {
lakshya 19:79e69017c855 1311 telemetry[i]=0x00;
lakshya 19:79e69017c855 1312 }
lakshya 19:79e69017c855 1313 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1314 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1315 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1316 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1317 {
lakshya 19:79e69017c855 1318 telemetry[i]=0x00;
lakshya 19:79e69017c855 1319 }
lakshya 19:79e69017c855 1320 break;
lakshya 19:79e69017c855 1321 }
lakshya 20:949d13045431 1322 case 0xE3:
lakshya 20:949d13045431 1323 {
lakshya 20:949d13045431 1324 if(EPS_BTRY_HTR_AUTO != 0)
lakshya 20:949d13045431 1325 telemetry[2]=0x87;
lakshya 20:949d13045431 1326 else
lakshya 20:949d13045431 1327 {
lakshya 20:949d13045431 1328 HTR_CYCLE_COUNTS = tc[4];
lakshya 20:949d13045431 1329 if(HTR_CYCLE_COUNTS==0x00)
lakshya 20:949d13045431 1330 {
lakshya 33:76f2b8735501 1331 BTRY_HTR_ENABLE = 0;
lakshya 20:949d13045431 1332 HTR_CYCLE->stop();
lakshya 20:949d13045431 1333 //clear EPS_BTRY_HTR is it
lakshya 20:949d13045431 1334 EPS_BTRY_HTR_AUTO = 0;
lakshya 20:949d13045431 1335
lakshya 20:949d13045431 1336 }
lakshya 20:949d13045431 1337 else
lakshya 20:949d13045431 1338 {
lakshya 20:949d13045431 1339 if(HTR_CYCLE_COUNTS != 0xFF)
lakshya 20:949d13045431 1340 {
lakshya 20:949d13045431 1341 HTR_CYCLE_COUNTER = 0;
lakshya 20:949d13045431 1342 }
lakshya 20:949d13045431 1343 //uint8_t HTR_CYCLE_START_DLY = tc[5];
lakshya 20:949d13045431 1344 HTR_CYCLE_START_DLY = tc[5];
lakshya 20:949d13045431 1345 HTR_ON_DURATION = tc[6];
lakshya 20:949d13045431 1346
lakshya 20:949d13045431 1347 //make it uint16_t
lakshya 20:949d13045431 1348 HTR_CYCLE_PERIOD = (tc[7]<<8) | tc[8];
lakshya 20:949d13045431 1349 //start BTRY_HTR_DLY_TIMER;
lakshya 20:949d13045431 1350 }
lakshya 20:949d13045431 1351 telemetry[2]=0xA0;
lakshya 20:949d13045431 1352 }
lakshya 20:949d13045431 1353 //ACK_L234_telemetry
lakshya 20:949d13045431 1354 telemetry[0]=0xB0;
lakshya 20:949d13045431 1355 telemetry[1]=tc[0];
lakshya 20:949d13045431 1356 //ACK code taken care of
lakshya 20:949d13045431 1357 for(uint8_t i=3;i<11;i++)
lakshya 20:949d13045431 1358 {
lakshya 20:949d13045431 1359 telemetry[i]=0x00;
lakshya 20:949d13045431 1360 }
lakshya 20:949d13045431 1361 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 20:949d13045431 1362 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 20:949d13045431 1363 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 20:949d13045431 1364 for(uint8_t i=13;i<134;i++)
lakshya 20:949d13045431 1365 {
lakshya 20:949d13045431 1366 telemetry[i]=0x00;
lakshya 20:949d13045431 1367 }
lakshya 20:949d13045431 1368
lakshya 20:949d13045431 1369 break;
lakshya 20:949d13045431 1370 }
lakshya 19:79e69017c855 1371 case 0x01:
lakshya 19:79e69017c855 1372 { if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 1373 {
lakshya 19:79e69017c855 1374 printf("\n\rRun P_EPS_INIT");
lakshya 19:79e69017c855 1375 FCTN_EPS_INIT();
lakshya 19:79e69017c855 1376 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1377 }
lakshya 19:79e69017c855 1378 else
lakshya 19:79e69017c855 1379 {
lakshya 19:79e69017c855 1380 printf("\n\runable to Run P_EPS_INIT as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 1381 telemetry[2]=0x87;
lakshya 19:79e69017c855 1382 }
lakshya 19:79e69017c855 1383
lakshya 19:79e69017c855 1384 //ACK_L234_telemetry
lakshya 19:79e69017c855 1385 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1386 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1387 //ACK code taken care of
lakshya 19:79e69017c855 1388 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1389 {
lakshya 19:79e69017c855 1390 telemetry[i]=0x00;
lakshya 19:79e69017c855 1391 }
lakshya 19:79e69017c855 1392 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1393 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1394 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1395 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1396 {
lakshya 19:79e69017c855 1397 telemetry[i]=0x00;
lakshya 19:79e69017c855 1398 }
lakshya 19:79e69017c855 1399
lakshya 19:79e69017c855 1400 break;
lakshya 19:79e69017c855 1401 }
lakshya 19:79e69017c855 1402 case 0x02:
lakshya 19:79e69017c855 1403 {
lakshya 19:79e69017c855 1404 if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 1405 {
lakshya 19:79e69017c855 1406 printf("\n\rRun P_EPS_MAIN");
lakshya 19:79e69017c855 1407 F_EPS();
lakshya 19:79e69017c855 1408 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1409 }
lakshya 19:79e69017c855 1410 else
lakshya 19:79e69017c855 1411 {
lakshya 19:79e69017c855 1412 printf("\n\runable to Run P_EPS_MAIN as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 1413 telemetry[2]=0x87;
lakshya 19:79e69017c855 1414 }
lakshya 19:79e69017c855 1415
lakshya 19:79e69017c855 1416 //ACK_L234_telemetry
lakshya 19:79e69017c855 1417 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1418 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1419 //ACK code taken care of
lakshya 19:79e69017c855 1420 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1421 {
lakshya 19:79e69017c855 1422 telemetry[i]=0x00;
lakshya 19:79e69017c855 1423 }
lakshya 19:79e69017c855 1424 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1425 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1426 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1427 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1428 {
lakshya 19:79e69017c855 1429 telemetry[i]=0x00;
lakshya 19:79e69017c855 1430 }
lakshya 19:79e69017c855 1431 break;
lakshya 19:79e69017c855 1432 }
lakshya 19:79e69017c855 1433 case 0x03:
lakshya 19:79e69017c855 1434 {
lakshya 19:79e69017c855 1435 if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 1436 {
lakshya 19:79e69017c855 1437 printf("\n\rRun P_ACS_INIT");
lakshya 19:79e69017c855 1438 FCTN_ACS_INIT();
lakshya 19:79e69017c855 1439 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1440 }
lakshya 19:79e69017c855 1441 else
lakshya 19:79e69017c855 1442 {
lakshya 19:79e69017c855 1443 printf("\n\runable to Run P_ACS_INIT as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 1444 telemetry[2]=0x87;
lakshya 19:79e69017c855 1445 }
lakshya 19:79e69017c855 1446
lakshya 19:79e69017c855 1447 //ACK_L234_telemetry
lakshya 19:79e69017c855 1448 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1449 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1450 //ACK CODE TAKEN CARE OF
lakshya 19:79e69017c855 1451 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1452 {
lakshya 19:79e69017c855 1453 telemetry[i]=0x00;
lakshya 19:79e69017c855 1454 }
lakshya 19:79e69017c855 1455 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1456 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1457 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1458 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1459 {
lakshya 19:79e69017c855 1460 telemetry[i]=0x00;
lakshya 19:79e69017c855 1461 }
lakshya 19:79e69017c855 1462 break;
lakshya 19:79e69017c855 1463 }
lakshya 19:79e69017c855 1464 case 0x05:
lakshya 19:79e69017c855 1465 {
lakshya 19:79e69017c855 1466 if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 1467 {
lakshya 19:79e69017c855 1468 printf("\n\rRun P_ACS_MAIN");
lakshya 19:79e69017c855 1469 F_ACS();
lakshya 19:79e69017c855 1470 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1471 }
lakshya 19:79e69017c855 1472 else
lakshya 19:79e69017c855 1473 {
lakshya 19:79e69017c855 1474 printf("\n\runable to Run P_ACS_MAIN as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 1475 telemetry[2]=0x87;
lakshya 19:79e69017c855 1476 }
lakshya 19:79e69017c855 1477
lakshya 19:79e69017c855 1478 //ACK_L234_TM
lakshya 19:79e69017c855 1479 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1480 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1481 //ACK CODE TAKEN CARE OF
lakshya 19:79e69017c855 1482 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1483 {
lakshya 19:79e69017c855 1484 telemetry[i]=0x00;
lakshya 19:79e69017c855 1485 }
lakshya 19:79e69017c855 1486
lakshya 19:79e69017c855 1487 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1488 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1489 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1490 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1491 {
lakshya 19:79e69017c855 1492 telemetry[i]=0x00;
lakshya 19:79e69017c855 1493 }
lakshya 19:79e69017c855 1494 break;
lakshya 19:79e69017c855 1495 }
lakshya 19:79e69017c855 1496 case 0x06:
lakshya 19:79e69017c855 1497 {
lakshya 19:79e69017c855 1498 if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 1499 {
lakshya 19:79e69017c855 1500 printf("\n\rRun P_BCN_INIT");
lakshya 19:79e69017c855 1501 F_BCN();
lakshya 19:79e69017c855 1502 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1503 }
lakshya 19:79e69017c855 1504 else
lakshya 19:79e69017c855 1505 {
lakshya 19:79e69017c855 1506 printf("\n\runable to Run P_BCN_INIT as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 1507 telemetry[2]=0x87;
lakshya 19:79e69017c855 1508 }
lakshya 19:79e69017c855 1509
lakshya 19:79e69017c855 1510 //ACK_L234_TM
lakshya 19:79e69017c855 1511 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1512 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1513 //ACK CODE TAKEN CARE OF
lakshya 19:79e69017c855 1514 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1515 {
lakshya 19:79e69017c855 1516 telemetry[i]=0x00;
lakshya 19:79e69017c855 1517 }
lakshya 19:79e69017c855 1518 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1519 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1520 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1521 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1522 {
lakshya 19:79e69017c855 1523 telemetry[i]=0x00;
lakshya 19:79e69017c855 1524 }
lakshya 19:79e69017c855 1525 break;
lakshya 19:79e69017c855 1526 }
lakshya 19:79e69017c855 1527 case 0x07:
lakshya 19:79e69017c855 1528 {
lakshya 19:79e69017c855 1529 if(BAE_STANDBY==0x07)
lakshya 19:79e69017c855 1530 {
lakshya 19:79e69017c855 1531 printf("\n\rRun P_BCN_TX_MAIN");
lakshya 19:79e69017c855 1532 FCTN_BCN_TX_MAIN();//correct function check once
lakshya 19:79e69017c855 1533 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1534 }
lakshya 19:79e69017c855 1535 else
lakshya 19:79e69017c855 1536 {
lakshya 19:79e69017c855 1537 printf("\n\runable to Run P_BCN_TX_MAIN as BAE_STATUS not 111 ");;
lakshya 19:79e69017c855 1538 telemetry[2]=0x87;
lakshya 19:79e69017c855 1539 }
lakshya 19:79e69017c855 1540
lakshya 19:79e69017c855 1541 //ACK_L234_TM
lakshya 19:79e69017c855 1542 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1543 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1544 //ACK CODE TAKEN CARE OF
lakshya 19:79e69017c855 1545 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1546 {
lakshya 19:79e69017c855 1547 telemetry[i]=0x00;
lakshya 19:79e69017c855 1548 }
lakshya 19:79e69017c855 1549 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1550 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1551 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1552 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1553 {
lakshya 19:79e69017c855 1554 telemetry[i]=0x00;
lakshya 19:79e69017c855 1555 }
lakshya 19:79e69017c855 1556 break;
lakshya 19:79e69017c855 1557 }
lakshya 19:79e69017c855 1558 case 0x11:
lakshya 19:79e69017c855 1559 {
lakshya 19:79e69017c855 1560 printf("\n\rSW_ON_ACS_ATS1_SW_ENABLE");
lakshya 19:79e69017c855 1561 //ACK_L234_TM
lakshya 19:79e69017c855 1562 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1563 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1564 //____________________________________________************************************
lakshya 19:79e69017c855 1565 /*
lakshya 19:79e69017c855 1566 ATS PIN OR STATUS YET TO BE DECIDED. DECIDED THAT IT IS PIN TC CAN SWITCH ON/OFF THE SENSOR
lakshya 19:79e69017c855 1567 */
lakshya 19:79e69017c855 1568 ATS2_SW_ENABLE = 1; // making sure we switch off the other
Bragadeesh153 41:5df2bed2157d 1569 wait_ms(5);
lakshya 39:670133e7ffd8 1570 //ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3) | 0x0C ;
lakshya 39:670133e7ffd8 1571 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF0) | 0x0C ;
lakshya 19:79e69017c855 1572 ATS1_SW_ENABLE = 0;
lakshya 39:670133e7ffd8 1573 //ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F);
lakshya 39:670133e7ffd8 1574 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x0F)|0x40;
lakshya 19:79e69017c855 1575 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1576 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1577 {
lakshya 19:79e69017c855 1578 telemetry[i]=0x00;
lakshya 19:79e69017c855 1579 }
lakshya 19:79e69017c855 1580 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1581 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1582 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1583 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1584 {
lakshya 19:79e69017c855 1585 telemetry[i]=0x00;
lakshya 19:79e69017c855 1586 }
lakshya 19:79e69017c855 1587 break;
lakshya 19:79e69017c855 1588 }
lakshya 19:79e69017c855 1589 case 0x12:
lakshya 19:79e69017c855 1590 {
lakshya 19:79e69017c855 1591 printf("\n\rSW_ON_ACS_ATS2_SW_ENABLE");
lakshya 19:79e69017c855 1592 //ACK_L234_TM
lakshya 19:79e69017c855 1593 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1594 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1595 ATS1_SW_ENABLE = 1; //make sure u switch off the other
Bragadeesh153 41:5df2bed2157d 1596 wait_ms(5);
lakshya 39:670133e7ffd8 1597 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x0F) | 0xC0 ;
lakshya 19:79e69017c855 1598 ATS2_SW_ENABLE = 0;
lakshya 39:670133e7ffd8 1599 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF0)|0x04;
lakshya 19:79e69017c855 1600 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1601 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1602 {
lakshya 19:79e69017c855 1603 telemetry[i]=0x00;
lakshya 19:79e69017c855 1604 }
lakshya 19:79e69017c855 1605 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1606 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1607 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1608 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1609 {
lakshya 19:79e69017c855 1610 telemetry[i]=0x00;
lakshya 19:79e69017c855 1611 }
lakshya 19:79e69017c855 1612 break;
lakshya 19:79e69017c855 1613 }
lakshya 19:79e69017c855 1614 case 0x13:
lakshya 19:79e69017c855 1615 {
lakshya 19:79e69017c855 1616 printf("\n\rSW_ON_ACS_TR_XY_ENABLE");
lakshya 19:79e69017c855 1617 //ACK_L234_TM
lakshya 19:79e69017c855 1618 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1619 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1620 TRXY_SW = 1;//1 SWITCH enable here
lakshya 19:79e69017c855 1621 ACS_TR_XY_SW_STATUS=0x01;
lakshya 19:79e69017c855 1622 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1623 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1624 {
lakshya 19:79e69017c855 1625 telemetry[i]=0x00;
lakshya 19:79e69017c855 1626 }
lakshya 19:79e69017c855 1627 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1628 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1629 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1630 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1631 {
lakshya 19:79e69017c855 1632 telemetry[i]=0x00;
lakshya 19:79e69017c855 1633 }
lakshya 19:79e69017c855 1634 break;
lakshya 19:79e69017c855 1635 }
lakshya 19:79e69017c855 1636 case 0x14:
lakshya 19:79e69017c855 1637 {
lakshya 19:79e69017c855 1638 printf("\n\rSW_ON_ACS_TR_Z_ENABLE");
lakshya 19:79e69017c855 1639 //ACK_L234_TM
lakshya 19:79e69017c855 1640 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1641 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1642 TRZ_SW = 1;
lakshya 19:79e69017c855 1643 ACS_TR_Z_SW_STATUS=0x01;
lakshya 19:79e69017c855 1644 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1645 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1646 {
lakshya 19:79e69017c855 1647 telemetry[i]=0x00;
lakshya 19:79e69017c855 1648 }
lakshya 19:79e69017c855 1649 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1650 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1651 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1652 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1653 {
lakshya 19:79e69017c855 1654 telemetry[i]=0x00;
lakshya 19:79e69017c855 1655 }
lakshya 19:79e69017c855 1656 break;
lakshya 19:79e69017c855 1657 }
lakshya 19:79e69017c855 1658 case 0x15:
lakshya 19:79e69017c855 1659 {
lakshya 19:79e69017c855 1660 printf("\n\rSW_ON_BCN_TX_SW_ENABLE");
lakshya 19:79e69017c855 1661 //ACK_L234_TM
lakshya 19:79e69017c855 1662 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1663 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1664 BCN_SW = 0;//here 0 is switch enable
lakshya 19:79e69017c855 1665 BCN_TX_SW_ENABLE=0x01;
lakshya 19:79e69017c855 1666 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1667 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1668 {
lakshya 19:79e69017c855 1669 telemetry[i]=0x00;
lakshya 19:79e69017c855 1670 }
lakshya 19:79e69017c855 1671 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1672 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1673 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1674 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1675 {
lakshya 19:79e69017c855 1676 telemetry[i]=0x00;
lakshya 19:79e69017c855 1677 }
lakshya 19:79e69017c855 1678 break;
lakshya 19:79e69017c855 1679 }
lakshya 19:79e69017c855 1680 case 0x21:
lakshya 19:79e69017c855 1681 {
lakshya 19:79e69017c855 1682 printf("\n\rSW_OFF_ACS_ATS1_SW_ENABLE");
lakshya 19:79e69017c855 1683 //ACK_L234_TM
lakshya 19:79e69017c855 1684 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1685 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1686 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1687 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F) | 0xC0 ;
lakshya 19:79e69017c855 1688 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1689 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1690 {
lakshya 19:79e69017c855 1691 telemetry[i]=0x00;
lakshya 19:79e69017c855 1692 }
lakshya 19:79e69017c855 1693 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1694 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1695 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1696 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1697 {
lakshya 19:79e69017c855 1698 telemetry[i]=0x00;
lakshya 19:79e69017c855 1699 }
lakshya 19:79e69017c855 1700 break;
lakshya 19:79e69017c855 1701 }
lakshya 19:79e69017c855 1702 case 0x22:
lakshya 19:79e69017c855 1703 {
lakshya 19:79e69017c855 1704 printf("\n\rSW_OFF_ACS_ATS2_SW_ENABLE");
lakshya 19:79e69017c855 1705 //ACK_L234_TM
lakshya 19:79e69017c855 1706 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1707 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1708 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1709 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3) | 0x0C ;
lakshya 19:79e69017c855 1710 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1711 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1712 {
lakshya 19:79e69017c855 1713 telemetry[i]=0x00;
lakshya 19:79e69017c855 1714 }
lakshya 19:79e69017c855 1715 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1716 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1717 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1718 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1719 {
lakshya 19:79e69017c855 1720 telemetry[i]=0x00;
lakshya 19:79e69017c855 1721 }
lakshya 19:79e69017c855 1722 break;
lakshya 19:79e69017c855 1723 }
lakshya 19:79e69017c855 1724 case 0x23:
lakshya 19:79e69017c855 1725 {
lakshya 19:79e69017c855 1726 printf("\n\rSW_OFF_ACS_TR_XY_ENABLE");
lakshya 19:79e69017c855 1727 //ACK_L234_TM
lakshya 19:79e69017c855 1728 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1729 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1730 TRXY_SW= 0;
lakshya 19:79e69017c855 1731 ACS_TR_XY_SW_STATUS=0x03;
lakshya 19:79e69017c855 1732 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1733 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1734 {
lakshya 19:79e69017c855 1735 telemetry[i]=0x00;
lakshya 19:79e69017c855 1736 }
lakshya 19:79e69017c855 1737 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1738 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1739 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1740 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1741 {
lakshya 19:79e69017c855 1742 telemetry[i]=0x00;
lakshya 19:79e69017c855 1743 }
lakshya 19:79e69017c855 1744 break;
lakshya 19:79e69017c855 1745 }
lakshya 19:79e69017c855 1746 case 0x24:
lakshya 19:79e69017c855 1747 {
lakshya 19:79e69017c855 1748 printf("\n\rSW_OFF_ACS_TR_Z_ENABLE");
lakshya 19:79e69017c855 1749 //ACK_L234_TM
lakshya 19:79e69017c855 1750 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1751 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1752 TRZ_SW = 0;
lakshya 19:79e69017c855 1753 ACS_TR_Z_SW_STATUS=0x03;
lakshya 19:79e69017c855 1754 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1755 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1756 {
lakshya 19:79e69017c855 1757 telemetry[i]=0x00;
lakshya 19:79e69017c855 1758 }
lakshya 19:79e69017c855 1759 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1760 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1761 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1762 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1763 {
lakshya 19:79e69017c855 1764 telemetry[i]=0x00;
lakshya 19:79e69017c855 1765 }
lakshya 19:79e69017c855 1766 break;
lakshya 19:79e69017c855 1767 }
lakshya 19:79e69017c855 1768 case 0x25:
lakshya 19:79e69017c855 1769 {
lakshya 19:79e69017c855 1770 printf("\n\rSW_OFF_BCN_TX_SW_ENABLE");
lakshya 19:79e69017c855 1771 //ACK_L234_TM
lakshya 19:79e69017c855 1772 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1773 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1774 BCN_SW = 1;
lakshya 19:79e69017c855 1775 BCN_TX_SW_ENABLE=0x03;
lakshya 19:79e69017c855 1776 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1777 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1778 {
lakshya 19:79e69017c855 1779 telemetry[i]=0x00;
lakshya 19:79e69017c855 1780 }
lakshya 19:79e69017c855 1781 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1782 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1783 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1784 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1785 {
lakshya 19:79e69017c855 1786 telemetry[i]=0x00;
lakshya 19:79e69017c855 1787 }
lakshya 19:79e69017c855 1788 break;
lakshya 19:79e69017c855 1789 }
lakshya 19:79e69017c855 1790 case 0x31:
lakshya 19:79e69017c855 1791 {
lakshya 19:79e69017c855 1792 printf("\n\rACS_ATS1_SW_RESET");
lakshya 19:79e69017c855 1793 //ACK_L234_TM
lakshya 19:79e69017c855 1794 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1795 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1796 ATS2_SW_ENABLE = 1;//as ats switched off
lakshya 19:79e69017c855 1797 ATS1_SW_ENABLE = 1;
lakshya 19:79e69017c855 1798 wait_ms(5);
lakshya 19:79e69017c855 1799 ATS1_SW_ENABLE = 0;
lakshya 19:79e69017c855 1800 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1801 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1802 {
lakshya 19:79e69017c855 1803 telemetry[i]=0x00;
lakshya 19:79e69017c855 1804 }
lakshya 19:79e69017c855 1805 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1806 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1807 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1808 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1809 {
lakshya 19:79e69017c855 1810 telemetry[i]=0x00;
lakshya 19:79e69017c855 1811 }
lakshya 19:79e69017c855 1812 break;
lakshya 19:79e69017c855 1813 }
lakshya 19:79e69017c855 1814 case 0x32:
lakshya 19:79e69017c855 1815 {
lakshya 19:79e69017c855 1816 printf("\n\rACS_ATS2_SW_RESET");
lakshya 19:79e69017c855 1817 //ACK_L234_TM
lakshya 19:79e69017c855 1818 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1819 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1820 ATS1_SW_ENABLE = 1;//as ats1 switched off
lakshya 19:79e69017c855 1821 ATS2_SW_ENABLE = 1;
lakshya 19:79e69017c855 1822 wait_ms(5);
lakshya 19:79e69017c855 1823 ATS2_SW_ENABLE = 0;
lakshya 19:79e69017c855 1824 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1825 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1826 {
lakshya 19:79e69017c855 1827 telemetry[i]=0x00;
lakshya 19:79e69017c855 1828 }
lakshya 19:79e69017c855 1829 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1830 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1831 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1832 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1833 {
lakshya 19:79e69017c855 1834 telemetry[i]=0x00;
lakshya 19:79e69017c855 1835 }
lakshya 19:79e69017c855 1836 break;
lakshya 19:79e69017c855 1837 }
Bragadeesh153 41:5df2bed2157d 1838 case 0x44:
lakshya 19:79e69017c855 1839 {
lakshya 19:79e69017c855 1840 printf("\n\rACS_TR_XY_SW_RESET");
lakshya 19:79e69017c855 1841 //ACK_L234_TM
lakshya 19:79e69017c855 1842 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1843 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1844 TRXY_SW= 0;
lakshya 19:79e69017c855 1845 wait_ms(5);
lakshya 19:79e69017c855 1846 TRXY_SW= 1;
lakshya 19:79e69017c855 1847 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1848 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1849 {
lakshya 19:79e69017c855 1850 telemetry[i]=0x00;
lakshya 19:79e69017c855 1851 }
lakshya 19:79e69017c855 1852 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1853 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1854 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1855 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1856 {
lakshya 19:79e69017c855 1857 telemetry[i]=0x00;
lakshya 19:79e69017c855 1858 }
lakshya 19:79e69017c855 1859 break;
lakshya 19:79e69017c855 1860 }
lakshya 19:79e69017c855 1861 case 0x34:
lakshya 19:79e69017c855 1862 {
lakshya 19:79e69017c855 1863 printf("\n\rACS_TR_Z_SW_RESET");
lakshya 19:79e69017c855 1864 //ACK_L234_TM
lakshya 19:79e69017c855 1865 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1866 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1867 TRZ_SW= 0;
lakshya 19:79e69017c855 1868 wait_ms(5);
lakshya 19:79e69017c855 1869 TRZ_SW= 1;
lakshya 19:79e69017c855 1870 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1871 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1872 {
lakshya 19:79e69017c855 1873 telemetry[i]=0x00;
lakshya 19:79e69017c855 1874 }
lakshya 19:79e69017c855 1875 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1876 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1877 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1878 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1879 {
lakshya 19:79e69017c855 1880 telemetry[i]=0x00;
lakshya 19:79e69017c855 1881 }
lakshya 19:79e69017c855 1882 break;
lakshya 19:79e69017c855 1883 }
lakshya 19:79e69017c855 1884 case 0x35:
lakshya 19:79e69017c855 1885 {
lakshya 19:79e69017c855 1886 printf("\n\rBCN_TX_SW_RESET");
lakshya 19:79e69017c855 1887 //ACK_L234_TM
lakshya 19:79e69017c855 1888 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1889 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1890 BCN_SW = 1;
lakshya 19:79e69017c855 1891 wait_ms(5);
lakshya 19:79e69017c855 1892 BCN_SW = 0;
lakshya 19:79e69017c855 1893 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1894 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1895 {
lakshya 19:79e69017c855 1896 telemetry[i]=0x00;
lakshya 19:79e69017c855 1897 }
lakshya 19:79e69017c855 1898 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1899 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1900 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1901 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1902 {
lakshya 19:79e69017c855 1903 telemetry[i]=0x00;
lakshya 19:79e69017c855 1904 }
lakshya 19:79e69017c855 1905 break;
lakshya 19:79e69017c855 1906 }
lakshya 19:79e69017c855 1907 case 0x36:
lakshya 19:79e69017c855 1908 {
lakshya 20:949d13045431 1909 printf("\n\rBAE_INTERNAL_RESET TO be done ??");
lakshya 20:949d13045431 1910 NVIC_SystemReset();
lakshya 19:79e69017c855 1911 break;
lakshya 19:79e69017c855 1912 }
lakshya 19:79e69017c855 1913 case 0x37:
lakshya 19:79e69017c855 1914 {
lakshya 19:79e69017c855 1915 printf("\n\rCDMS_RESET");
lakshya 19:79e69017c855 1916 //ACK_L234_TM
lakshya 19:79e69017c855 1917 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1918 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1919 CDMS_RESET = 0;
lakshya 19:79e69017c855 1920 wait_ms(5);
lakshya 19:79e69017c855 1921 CDMS_RESET = 1;
lakshya 19:79e69017c855 1922 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1923 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1924 {
lakshya 19:79e69017c855 1925 telemetry[i]=0x00;
lakshya 19:79e69017c855 1926 }
lakshya 19:79e69017c855 1927 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1928 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1929 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1930 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1931 {
lakshya 19:79e69017c855 1932 telemetry[i]=0x00;
lakshya 19:79e69017c855 1933 }
lakshya 19:79e69017c855 1934 break;
lakshya 19:79e69017c855 1935 }
lakshya 19:79e69017c855 1936 case 0x38:
lakshya 19:79e69017c855 1937 {
lakshya 19:79e69017c855 1938 printf("\n\rCDMS_SW_RESET pin yet to be decided");
lakshya 19:79e69017c855 1939 //ACK_L234_TM
lakshya 19:79e69017c855 1940 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1941 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1942 /*
lakshya 19:79e69017c855 1943 PIN to be DECIDED***************************************************************8
lakshya 19:79e69017c855 1944 */
lakshya 19:79e69017c855 1945 telemetry[2]=ACK_CODE;
lakshya 19:79e69017c855 1946 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1947 {
lakshya 19:79e69017c855 1948 telemetry[i]=0x00;
lakshya 19:79e69017c855 1949 }
lakshya 19:79e69017c855 1950 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1951 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1952 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1953 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1954 {
lakshya 19:79e69017c855 1955 telemetry[i]=0x00;
lakshya 19:79e69017c855 1956 }
lakshya 19:79e69017c855 1957 break;
lakshya 19:79e69017c855 1958 }
Bragadeesh153 41:5df2bed2157d 1959 case 0x33:
lakshya 19:79e69017c855 1960 {
lakshya 19:79e69017c855 1961 uint8_t STANDBY_DATA_TC;
lakshya 19:79e69017c855 1962 BAE_STANDBY=0x00;
lakshya 19:79e69017c855 1963 STANDBY_DATA_TC=tc[4];
lakshya 19:79e69017c855 1964 if(STANDBY_DATA_TC==0x01)
lakshya 19:79e69017c855 1965 {
lakshya 19:79e69017c855 1966 BAE_STANDBY |=0x04;
Bragadeesh153 41:5df2bed2157d 1967 //BAE_STANDBY_TIMER_RESET();
lakshya 19:79e69017c855 1968 //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
lakshya 19:79e69017c855 1969 }
lakshya 19:79e69017c855 1970 STANDBY_DATA_TC=tc[5];
lakshya 19:79e69017c855 1971 if(STANDBY_DATA_TC==0x01)
lakshya 19:79e69017c855 1972 {
lakshya 19:79e69017c855 1973 BAE_STANDBY |=0x02;
Bragadeesh153 41:5df2bed2157d 1974 //BAE_STANDBY_TIMER_RESET();
lakshya 19:79e69017c855 1975 //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
lakshya 19:79e69017c855 1976 }
lakshya 19:79e69017c855 1977 STANDBY_DATA_TC=tc[6];
lakshya 19:79e69017c855 1978 if(STANDBY_DATA_TC==0x01)
lakshya 19:79e69017c855 1979 {
lakshya 19:79e69017c855 1980 BAE_STANDBY |=0x01;
Bragadeesh153 41:5df2bed2157d 1981 //BAE_STANDBY_TIMER_RESET();
lakshya 19:79e69017c855 1982 //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
lakshya 19:79e69017c855 1983 }
Bragadeesh153 41:5df2bed2157d 1984 printf("BAE_STANDBY : %X\r\n",BAE_STANDBY);
lakshya 19:79e69017c855 1985 telemetry[0]=0xB0;
lakshya 19:79e69017c855 1986 telemetry[1]=tc[0];
lakshya 19:79e69017c855 1987 telemetry[2]=0xC0;//ack_code for this case
lakshya 19:79e69017c855 1988 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 1989 {
lakshya 19:79e69017c855 1990 telemetry[i]=0x00;
lakshya 19:79e69017c855 1991 }
lakshya 19:79e69017c855 1992 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 1993 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 1994 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 1995 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 1996 {
lakshya 19:79e69017c855 1997 telemetry[i]=0x00;
lakshya 19:79e69017c855 1998 }
lakshya 19:79e69017c855 1999 break;
lakshya 19:79e69017c855 2000 }
lakshya 19:79e69017c855 2001 case 0x41:
lakshya 19:79e69017c855 2002 {
lakshya 19:79e69017c855 2003 printf("\n\rexecutng BAE reset HK counter");
lakshya 20:949d13045431 2004 firstCount=true;
lakshya 20:949d13045431 2005 void minMaxHkData();
lakshya 19:79e69017c855 2006
lakshya 19:79e69017c855 2007 //what to do here??*************************************************
lakshya 19:79e69017c855 2008 //TO BE DONE
lakshya 19:79e69017c855 2009
lakshya 19:79e69017c855 2010 //ACK_L234_TM
lakshya 19:79e69017c855 2011 telemetry[0]=0xB0;
lakshya 19:79e69017c855 2012 telemetry[1]=tc[0];
lakshya 19:79e69017c855 2013 telemetry[2]=0x02;
lakshya 19:79e69017c855 2014 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 2015 {
lakshya 19:79e69017c855 2016 telemetry[i]=0x00;
lakshya 19:79e69017c855 2017 }
lakshya 19:79e69017c855 2018 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 2019 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 2020 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 2021 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 2022 {
lakshya 19:79e69017c855 2023 telemetry[i]=0x00;
lakshya 19:79e69017c855 2024 }
lakshya 19:79e69017c855 2025 break;
lakshya 19:79e69017c855 2026 }
lakshya 19:79e69017c855 2027 default:
lakshya 19:79e69017c855 2028 {
lakshya 19:79e69017c855 2029 printf("\n\rInvalid TC for FMS no matching FID");
lakshya 19:79e69017c855 2030 //ACK_L234_TM
lakshya 19:79e69017c855 2031 telemetry[0]=0xB0;
lakshya 19:79e69017c855 2032 telemetry[1]=tc[0];
lakshya 19:79e69017c855 2033 telemetry[2]=0x02;
lakshya 19:79e69017c855 2034 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 2035 {
lakshya 19:79e69017c855 2036 telemetry[i]=0x00;
lakshya 19:79e69017c855 2037 }
lakshya 19:79e69017c855 2038 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 2039 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 2040 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 2041 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 2042 {
lakshya 19:79e69017c855 2043 telemetry[i]=0x00;
lakshya 19:79e69017c855 2044 }
lakshya 19:79e69017c855 2045 break;
lakshya 19:79e69017c855 2046 }
lakshya 19:79e69017c855 2047 }
lakshya 19:79e69017c855 2048 break;
lakshya 19:79e69017c855 2049 }
lakshya 19:79e69017c855 2050 default:
lakshya 19:79e69017c855 2051 {
lakshya 19:79e69017c855 2052 printf("\n\rInvalid TC, FMS service subtype mismacth");
lakshya 19:79e69017c855 2053 //ACK_L234_TM
lakshya 19:79e69017c855 2054 telemetry[0]=0xB0;
lakshya 19:79e69017c855 2055 telemetry[1]=tc[0];
lakshya 19:79e69017c855 2056 telemetry[2]=0x02;
lakshya 19:79e69017c855 2057 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 2058 {
lakshya 19:79e69017c855 2059 telemetry[i]=0x00;
lakshya 19:79e69017c855 2060 }
lakshya 19:79e69017c855 2061 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 2062 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 2063 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 2064 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 2065 {
lakshya 19:79e69017c855 2066 telemetry[i]=0x00;
lakshya 19:79e69017c855 2067 }
lakshya 19:79e69017c855 2068 break;
lakshya 19:79e69017c855 2069 }
lakshya 19:79e69017c855 2070 }
lakshya 19:79e69017c855 2071 break;
lakshya 19:79e69017c855 2072 }
lakshya 19:79e69017c855 2073 default:
lakshya 19:79e69017c855 2074 {
lakshya 19:79e69017c855 2075 printf("\n\rInvalid TC neither FMS nor MMS");
lakshya 19:79e69017c855 2076 //ACK_L234_TM
lakshya 19:79e69017c855 2077 telemetry[0]=0xB0;
lakshya 19:79e69017c855 2078 telemetry[1]=tc[0];
lakshya 19:79e69017c855 2079 telemetry[2]=0x02;
lakshya 19:79e69017c855 2080 for(uint8_t i=3;i<11;i++)
lakshya 19:79e69017c855 2081 {
lakshya 19:79e69017c855 2082 telemetry[i]=0x00;
lakshya 19:79e69017c855 2083 }
lakshya 19:79e69017c855 2084 crc16 = CRC::crc16_gen(telemetry,11);
lakshya 19:79e69017c855 2085 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
lakshya 19:79e69017c855 2086 telemetry[12] = (uint8_t)(crc16&0x00FF);
lakshya 19:79e69017c855 2087 for(uint8_t i=13;i<134;i++)
lakshya 19:79e69017c855 2088 {
lakshya 19:79e69017c855 2089 telemetry[i]=0x00;
lakshya 19:79e69017c855 2090 }
lakshya 19:79e69017c855 2091 break;
lakshya 19:79e69017c855 2092 }
lakshya 19:79e69017c855 2093 }
lakshya 19:79e69017c855 2094 }
Bragadeesh153 13:fb7facaf308b 2095 }
Bragadeesh153 13:fb7facaf308b 2096 }
sakthipriya 4:39a4ae8c7ecd 2097
sakthipriya 4:39a4ae8c7ecd 2098
Bragadeesh153 13:fb7facaf308b 2099 int strt_add = flash_size() - (2*SECTOR_SIZE);
sakthipriya 4:39a4ae8c7ecd 2100 uint32_t flasharray[8]; //256+(3*1024)
lakshya 19:79e69017c855 2101 /*corrected*/
lakshya 19:79e69017c855 2102 int *nativeflash = (int*)strt_add;
sakthipriya 4:39a4ae8c7ecd 2103
sakthipriya 4:39a4ae8c7ecd 2104 /*Writing to the Flash*/
lakshya 19:79e69017c855 2105 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 2106 {
Bragadeesh153 13:fb7facaf308b 2107 for(int i=0;i<8;i++)
sakthipriya 4:39a4ae8c7ecd 2108 {
sakthipriya 4:39a4ae8c7ecd 2109 flasharray[i]=nativeflash[i];
sakthipriya 4:39a4ae8c7ecd 2110 }
sakthipriya 4:39a4ae8c7ecd 2111 flasharray[j]=fdata;
sakthipriya 4:39a4ae8c7ecd 2112 erase_sector(strt_add);
lakshya 20:949d13045431 2113 program_flash(strt_add, (char*)flasharray,32);
sakthipriya 4:39a4ae8c7ecd 2114 }
lakshya 19:79e69017c855 2115
sakthipriya 4:39a4ae8c7ecd 2116 /*End*/
sakthipriya 4:39a4ae8c7ecd 2117
sakthipriya 4:39a4ae8c7ecd 2118 /*Reading from Flash*/
lakshya 19:79e69017c855 2119 /*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 2120 uint32_t FCTN_BAE_RD_FLASH_ENTITY(uint16_t entity)
sakthipriya 4:39a4ae8c7ecd 2121 {
sakthipriya 4:39a4ae8c7ecd 2122 for(int i=0;i<8;i++)
sakthipriya 4:39a4ae8c7ecd 2123 {
sakthipriya 4:39a4ae8c7ecd 2124 flasharray[i]=nativeflash[i];
sakthipriya 4:39a4ae8c7ecd 2125 }
lakshya 19:79e69017c855 2126 return flasharray[entity];
sakthipriya 4:39a4ae8c7ecd 2127 }
lakshya 19:79e69017c855 2128
lakshya 19:79e69017c855 2129 uint32_t* FCTN_BAE_RD_FLASH()
lakshya 19:79e69017c855 2130 {
lakshya 19:79e69017c855 2131 for(int i=0;i<8;i++)
lakshya 19:79e69017c855 2132 {
lakshya 19:79e69017c855 2133 flasharray[i]=nativeflash[i];
lakshya 19:79e69017c855 2134 }
lakshya 19:79e69017c855 2135 return flasharray;
lakshya 19:79e69017c855 2136 }
lakshya 19:79e69017c855 2137
sakthipriya 5:bb592f3185cc 2138 /*End*/
sakthipriya 5:bb592f3185cc 2139
sakthipriya 5:bb592f3185cc 2140 // Convert float to 4 uint8_t
sakthipriya 5:bb592f3185cc 2141
lakshya 15:e09aaaccf134 2142
sakthipriya 6:036d08b62785 2143 void FCTN_CONVERT_FLOAT(float input, uint8_t output[4])
sakthipriya 5:bb592f3185cc 2144 {
sakthipriya 6:036d08b62785 2145 assert(sizeof(float) == sizeof(uint32_t));
sakthipriya 6:036d08b62785 2146 uint32_t* temp = reinterpret_cast<uint32_t*>(&input);
sakthipriya 6:036d08b62785 2147
sakthipriya 6:036d08b62785 2148 //float* output1 = reinterpret_cast<float*>(temp);
Bragadeesh153 13:fb7facaf308b 2149
lakshya 15:e09aaaccf134 2150 //printf("\n\r %f ", input);
lakshya 15:e09aaaccf134 2151 //std::cout << "\n\r uint32"<<*temp << std::endl;
sakthipriya 6:036d08b62785 2152
sakthipriya 6:036d08b62785 2153 output[0] =(uint8_t )(((*temp)>>24)&0xFF);
lakshya 15:e09aaaccf134 2154 output[1] =(uint8_t ) (((*temp)>>16)&0xFF);
lakshya 15:e09aaaccf134 2155 output[2] =(uint8_t ) (((*temp)>>8)&0xFF);
lakshya 15:e09aaaccf134 2156 output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic
lakshya 15:e09aaaccf134 2157
lakshya 17:fc782f7548c6 2158 // printf("\n\rthe values generated are\n");
lakshya 15:e09aaaccf134 2159 /*printf("\n\r%x\n",output[0]);
lakshya 15:e09aaaccf134 2160 printf("\n\r%x\n",output[1]);
lakshya 15:e09aaaccf134 2161 printf("\n\r%x\n",output[2]);
lakshya 15:e09aaaccf134 2162 printf("\n\r%x\n",output[3]);
lakshya 15:e09aaaccf134 2163 to check the values generated
lakshya 15:e09aaaccf134 2164 */
sakthipriya 6:036d08b62785 2165 //printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]);
Bragadeesh153 13:fb7facaf308b 2166 //std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl;
Bragadeesh153 13:fb7facaf308b 2167 }