Team Fox / Mbed 2 deprecated ACS_FULL_Flowchart_BAE

ACS completed fully. All cases to be tested

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of ACS_Flowchart_BAE by Team Fox

TCTM.cpp@18:21740620c65e, 2016-06-13 (annotated)

Committer:
Bragadeesh153
Date:
Mon Jun 13 13:44:31 2016 +0000
Revision:
18:21740620c65e
Parent:
16:cc77770d787f 
ACS algo commissioning done, Hardware comissioning yet to be finalised

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