Team Fox / Mbed 2 deprecated BAE_QM_MAR9

Important changes to repositories hosted on mbed.com

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

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

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

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of workinQM_5thJan_azad by Team Fox

TCTM.cpp@53:459b71b1861c, 2016-08-05 (annotated)

Committer:
lakshya
Date:
Fri Aug 05 19:33:09 2016 +0000
Revision:
53:459b71b1861c
Parent:
52:daa685b0e390
Child:
54:d6a45982ace1
code+minor bug solved; watch dog to be implemented completely

Who changed what in which revision?

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