Team Fox / Mbed 2 deprecated BAE_ACS_FULLYDONE_153

FINAL ACS TO BE USED FOR TESTING. COMMISSIONING, ACS MAIN, DATA ACQ ALL DONE.

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of ACS_FULL_Flowchart_BAE by Team Fox

TCTM.cpp@19:403cb36e22ed, 2016-06-28 (annotated)

Committer:
Bragadeesh153
Date:
Tue Jun 28 10:11:54 2016 +0000
Revision:
19:403cb36e22ed
Parent:
18:21740620c65e 
FINAL ACS TO BE USED FOR TESTING

Who changed what in which revision?

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