Team Fox
/
workinQM_10thDec
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
RAJANGAM_REVIEW_BAE_CODE
by 
Team Fox
Diff: TCTM.cpp
- Revision:
- 13:fb7facaf308b
- Parent:
- 12:af1d7e18b868
- Child:
- 14:a9588f443f1a
--- a/TCTM.cpp Wed Apr 13 18:34:28 2016 +0000
+++ b/TCTM.cpp Wed Apr 13 21:48:21 2016 +0000
@@ -15,7 +15,7 @@
extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
-extern DigitalOut TRXY_SW_EN; //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN
+extern DigitalOut TRXY_SW; //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN
extern DigitalOut TRZ_SW; //TR Z Switch
extern DigitalOut CDMS_RESET; // CDMS RESET
extern DigitalOut BCN_SW; //Beacon switch
@@ -32,7 +32,7 @@
extern DigitalOut phase_TR_y;
extern DigitalOut phase_TR_z;
extern BAE_HK_quant quant_data;
-//extern DigitalOut TRXY_SW_EN;
+//extern DigitalOut TRXY_SW;
//extern DigitalOut TRZ_SW_EN; //same as TRZ_SW
extern uint32_t BAE_ENABLE;
//extern DigitalOut ACS_ACQ_DATA_ENABLE;
@@ -42,7 +42,7 @@
extern uint8_t BCN_FAIL_COUNT;
-extern PwmOut PWM1; //x //Functions used to generate PWM signal
+extern PwmOut PWM1; //x //Functions used to generate PWM signal
extern PwmOut PWM2; //y
extern PwmOut PWM3; //z //PWM output comes from pins p6
@@ -51,11 +51,12 @@
//extern void FCTN_ACS_GENPWM_MAIN();
extern void F_EPS();
extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]);
+extern void FCTN_ACS_INIT();
extern void FCTN_ATS_DATA_ACQ();
extern void FCTN_ACS_CNTRLALGO(float*,float*);
-
+uint8_t telemetry[135];
void FCTN_CONVERT_UINT (uint8_t input[2], float* output)
{
@@ -70,409 +71,282 @@
float mag_total=sqrt(x*x + y*y + z*z);
float cos_z = z/mag_total;
float theta_z = acosf(cos_z);
-
+
return theta_z;
//printf("/n cos_zz= %f /t theta_z= %f /n",cos_z,theta_z);
}
-uint8_t* FCTN_BAE_TM_TC (uint8_t* tc)
+//uint8_t tm1[134];
+
+void FCTN_BAE_TM_TC (uint8_t* tc)
{
+ // tm1[0] = 1;
uint8_t service_type=(tc[2]&0xF0);
- uint8_t* tm;
+
uint16_t crc16;
-
-
+
+
switch(service_type)
{
case 0x60:
{
printf("Memory Management Service\r\n");
uint8_t service_subtype=(tc[2]&0x0F);
-
+
switch(service_subtype)
{
case 0x01:
{
printf("Read from Flash\r\n");
+ break;
}
case 0x02:
{
- uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
- switch(MID)
- {
-
+ uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
+ switch(MID)
+ {
+
case 0x0001:
- {
- printf("Read from RAM\r\n");
-
- /*taking some varible till we find some thing more useful*/
- //uint8_t ref_val=0x01;
- tm[0] = 0x60;
- tm[1] = tc[0];
- tm[2] = ACK_CODE;
- /*random or with bcn_tx_sw_enable assuming it is 1 bit in length
- how to get that we dont know, now we just assume it to be so*/
- tm[3] = (BCN_SW);
- tm[3] = tm[3]|(TRXY_SW_EN<<1);
- tm[3] = tm[3]|(TRZ_SW<<2);
- tm[3] = tm[3]|(ATS1_SW_ENABLE<<3);
- tm[3] = tm[3]|(ATS2_SW_ENABLE<<4);
+ {
+ printf("Read from RAM\r\n");
+
+ /*taking some varible till we find some thing more useful*/
+ //uint8_t ref_val=0x01;
+ telemetry[0] = 1;
+ telemetry[1] = tc[0];
+ telemetry[2] = ACK_CODE;
+ /*random or with bcn_tx_sw_enable assuming it is 1 bit in length
+ how to get that we dont know, now we just assume it to be so*/
+ telemetry[3] = (BCN_SW);
+ telemetry[3] = telemetry[3]|(TRXY_SW<<1);
+ telemetry[3] = telemetry[3]|(TRZ_SW<<2);
+ telemetry[3] = telemetry[3]|(ATS1_SW_ENABLE<<3);
+ telemetry[3] = telemetry[3]|(ATS2_SW_ENABLE<<4);
+
+ if(BCN_TX_STATUS==2)
+ telemetry[3] = telemetry[3]|0x20;
+ else
+ telemetry[3] = telemetry[3] & 0xDF;
+
+ telemetry[3] = telemetry[3]|(BCN_FEN<<6);
+ uint8_t mask_val=BAE_ENABLE & 0x00000008;
+ /*can be a problem see if any error occurs*/
+ telemetry[3] = telemetry[3]|(mask_val<<7);
+
+ /*not included in the code yet*/
+ telemetry[4] = BAE_RESET_COUNTER;
+ telemetry[5] = ACS_STATE;
+ telemetry[5] = telemetry[5]|(EN_BTRY_HT<<3);
+ telemetry[5] = telemetry[5]|(phase_TR_x<<4);
+ telemetry[5] = telemetry[5]|(phase_TR_y<<5);
+ telemetry[5] = telemetry[5]|(phase_TR_z<<6);
+ /*spare to be fixed*/
+ //telemetry[5] = telemetry[5]|(Spare))<<7);
+ /**/
+ uint8_t soc_powerlevel_2=50;
+ uint8_t soc_powerlevel_3=65;
+
+ telemetry[6] = soc_powerlevel_2;
+ telemetry[7] = soc_powerlevel_3;
- if(BCN_TX_STATUS==2)
- tm[3] = tm[3]|0x20;
- else
- tm[3] = tm[3] & 0xDF;
-
- tm[3] = tm[3]|(BCN_FEN<<6);
- uint8_t mask_val=BAE_ENABLE & 0x00000008;
- /*can be a problem see if any error occurs*/
- tm[3] = tm[3]|(mask_val<<7);
-
- /*not included in the code yet*/
- tm[4] = BAE_RESET_COUNTER;
-
- tm[5] = ACS_STATE;
- tm[5] = tm[5]|(EN_BTRY_HT<<3);
- tm[5] = tm[5]|(phase_TR_x<<4);
- tm[5] = tm[5]|(phase_TR_y<<5);
- tm[5] = tm[5]|(phase_TR_z<<6);
- /*spare to be fixed*/
- //tm[5] = tm[5]|(Spare))<<7);
- /**/
- uint8_t soc_powerlevel_2=50;
- uint8_t soc_powerlevel_3=65;
-
- tm[6] = soc_powerlevel_2;
- tm[7] = soc_powerlevel_3;
-
- /*to be fixed*/
- tm[8] = 0;
- tm[9] = 0;
- tm[10] = 0;
- tm[11] = 0;
- //tm[8] = Torque Rod X Offset;
- //tm[9] = Torque Rod Y Offset;
- //tm[10] = Torque Rod Z Offset;
- //tm[11] = ACS_DEMAG_TIME_DELAY;
- tm[12] = (BAE_STATUS>>24) & 0xFF;
- tm[13] = (BAE_STATUS>>16) & 0xFF;
- tm[14] = (BAE_STATUS>>8) & 0xFF;
- tm[15] = BAE_STATUS & 0xFF;
-
- /*to be fixed*/
- tm[16] = BCN_FAIL_COUNT;
- tm[17] = actual_data.power_mode;
- /*to be fixed*/
- uint16_t P_BAE_I2CRX_COUNTER=0;
- uint16_t P_ACS_MAIN_COUNTER=0;
- uint16_t FCTN_BCN_TX_MAIN_COUNTER=0;
- uint16_t P_EPS_MAIN_COUNTER=0;
-
- tm[18] = P_BAE_I2CRX_COUNTER>>8;
- tm[19] = P_BAE_I2CRX_COUNTER;
- tm[20] = P_ACS_MAIN_COUNTER>>8;
- tm[21] = P_ACS_MAIN_COUNTER;
- tm[22] = FCTN_BCN_TX_MAIN_COUNTER>>8;
- tm[23] = FCTN_BCN_TX_MAIN_COUNTER;
- tm[24] = P_EPS_MAIN_COUNTER>>8;
- tm[25] = P_EPS_MAIN_COUNTER;
-
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[26+ (i*4)]);
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[38+(i*4)]);
-
- //FAULT_FLAG();
- tm[50] = actual_data.faultIr_status;
- tm[51] = actual_data.faultPoll_status;
- //Bdot Rotation Speed of Command tm[52-53]
- //Bdot Output Current tm[54]
- //float l_pmw1 = (PWM1);
- //float l_pmw2 = PWM2;
- //float l_pmw3 = PWM3;
-
+ /*to be fixed*/
+ telemetry[8] = 0;
+ telemetry[9] = 0;
+ telemetry[10] = 0;
+ telemetry[11] = 0;
+ //telemetry[8] = Torque Rod X Offset;
+ //telemetry[9] = Torque Rod Y Offset;
+ //telemetry[10] = Torque Rod Z Offset;
+ //telemetry[11] = ACS_DEMAG_TIME_DELAY;
+ telemetry[12] = (BAE_STATUS>>24) & 0xFF;
+ telemetry[13] = (BAE_STATUS>>16) & 0xFF;
+ telemetry[14] = (BAE_STATUS>>8) & 0xFF;
+ telemetry[15] = BAE_STATUS & 0xFF;
+
+ /*to be fixed*/
+ telemetry[16] = BCN_FAIL_COUNT;
+ telemetry[17] = actual_data.power_mode;
+ /*to be fixed*/
+ uint16_t P_BAE_I2CRX_COUNTER=0;
+ uint16_t P_ACS_MAIN_COUNTER=0;
+ uint16_t P_BCN_TX_MAIN_COUNTER=0;
+ uint16_t P_EPS_MAIN_COUNTER=0;
+
+ telemetry[18] = P_BAE_I2CRX_COUNTER>>8;
+ telemetry[19] = P_BAE_I2CRX_COUNTER;
+ telemetry[20] = P_ACS_MAIN_COUNTER>>8;
+ telemetry[21] = P_ACS_MAIN_COUNTER;
+ telemetry[22] = P_BCN_TX_MAIN_COUNTER>>8;
+ telemetry[23] = P_BCN_TX_MAIN_COUNTER;
+ telemetry[24] = P_EPS_MAIN_COUNTER>>8;
+ telemetry[25] = P_EPS_MAIN_COUNTER;
+
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&telemetry[26+ (i*4)]);
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&telemetry[38+(i*4)]);
+
+ //FAULT_FLAG();
+ telemetry[50] = actual_data.faultIr_status;
+ telemetry[51] = actual_data.faultPoll_status;
+ //Bdot Rotation Speed of Command telemetry[52-53]
+ //Bdot Output Current telemetry[54]
+ //float l_pmw1 = (PWM1);
+ //float l_pmw2 = PWM2;
+ //float l_pmw3 = PWM3;
+
/*__________________________________________________________________*/
-
+
/*change and check if changing it to PWM1 causes problem*/
-
+
/*___________________________________________________________________*/
-
+
float PWM_measured[3];
-
- PWM_measured[0] = PWM1.read();
- PWM_measured[1] = PWM2.read();
- PWM_measured[2] = PWM3.read();
-
- FCTN_CONVERT_FLOAT(PWM_measured[0], &tm[55]);
- FCTN_CONVERT_FLOAT(PWM_measured[1], &tm[59]);
- FCTN_CONVERT_FLOAT(PWM_measured[2], &tm[63]);
+
+ PWM_measured[0] = PWM1.read();
+ PWM_measured[1] = PWM2.read();
+ PWM_measured[2] = PWM3.read();
+
+ FCTN_CONVERT_FLOAT(PWM_measured[0], &telemetry[55]);
+ FCTN_CONVERT_FLOAT(PWM_measured[1], &telemetry[59]);
+ FCTN_CONVERT_FLOAT(PWM_measured[2], &telemetry[63]);
float attitude_ang = angle(actual_data.Bvalue_actual[0],actual_data.Bvalue_actual[1],actual_data.Bvalue_actual[2]);
- FCTN_CONVERT_FLOAT(attitude_ang, &tm[67]);
+ FCTN_CONVERT_FLOAT(attitude_ang, &telemetry[67]);
for (int i=0; i<16; i++)
- tm[68+i] = quant_data.voltage_quant[i];
+ telemetry[68+i] = quant_data.voltage_quant[i];
for (int i=0; i<12; i++)
- tm[84+i] = quant_data.current_quant[i];
- //tm[96]
- //tm[97]
- //tm[98]
- //tm[99]
- tm[100] = quant_data.Batt_voltage_quant;
- tm[101] = quant_data.BAE_temp_quant;
- tm[102] = quant_data.Batt_gauge_quant[1];
- tm[103] = quant_data.Batt_temp_quant[0];
- tm[104] = quant_data.Batt_temp_quant[1];
-
- //tm[105] = beacon temperature;
-
+ telemetry[84+i] = quant_data.current_quant[i];
+ //telemetry[96]
+ //telemetry[97]
+ //telemetry[98]
+ //telemetry[99]
+ telemetry[100] = quant_data.Batt_voltage_quant;
+ telemetry[101] = quant_data.BAE_temp_quant;
+ telemetry[102] = quant_data.Batt_gauge_quant[1];
+ telemetry[103] = quant_data.Batt_temp_quant[0];
+ telemetry[104] = quant_data.Batt_temp_quant[1];
+
+ //telemetry[105] = beacon temperature;
+
for (int i=105; i<132;i++)
{
- tm[i] = 0x00;
- }
- crc16 = CRC::crc16_gen(tm,132);
- tm[132] = (uint8_t)((crc16&0xFF00)>>8);
- tm[133] = (uint8_t)(crc16&0x00FF);
- return tm;
-
- }
- case 0x0002:
- {
- tm[0] = 0x60;
- tm[1] = tc[0];
- tm[2] = ACK_CODE;
-
- for(int i;i<16;i++)
- tm[i+3] = bae_HK_minmax.voltage_max[i];
-
- for(int i;i<12;i++)
- tm[i+18] = bae_HK_minmax.current_max[i];
-
- tm[29] = bae_HK_minmax.Batt_voltage_max;;
- tm[30] = bae_HK_minmax.BAE_temp_max;
-
- /*battery soc*/
- //tm[31] = BAE_HK_min_max bae_HK_minmax.voltage_max;
-
- tm[32] = bae_HK_minmax.Batt_temp_max[1];
- tm[33] = bae_HK_minmax.Batt_temp_max[2];
-
- /*BCN temp not there*/
- //tm[34] = BAE_HK_min_max bae_HK_minmax.;
-
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[i],&tm[35+(i*4)]);
-
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[i],&tm[47+(i*4)]);
-
- /*min data*/
-
- for(int i;i<16;i++)
- tm[i+59] = bae_HK_minmax.voltage_min[i];
-
- for(int i;i<12;i++)
- tm[i+74] = bae_HK_minmax.current_min[i];
-
- tm[86] = bae_HK_minmax.Batt_voltage_min;
- tm[87] = bae_HK_minmax.BAE_temp_min;
-
- /*battery soc*/
- //tm[88] = BAE_HK_min_max bae_HK_minmax.voltage_max;
-
- tm[89] = bae_HK_minmax.Batt_temp_min[1];
- tm[90] = bae_HK_minmax.Batt_temp_min[2];
- //huhu//
-
- /*BCN temp not there*/
- //tm[91] = BAE_HK_min_max bae_HK_minmax.;
-
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[i],&tm[91+(i*4)]);
-
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[i],&tm[103+(i*4)]);
-
-
- for (int i=115; i<132;i++)
- {
- tm[i] = 0x00;
- }
- crc16 = CRC::crc16_gen(tm,132);
- tm[132] = (uint8_t)((crc16&0xFF00)>>8);
- tm[133] = (uint8_t)(crc16&0x00FF);
- return tm;
-
- }
-
-
- }
- }
-
- /*
- switch(tc[3])
- {
- case 0x01:
- {
- printf("Read MUX DATA\r\n");
- tm[0] = 0x60;
- tm[1] = tc[0];
- tm[2] = ACK_CODE;
- for(int i=0; i<16; i++) //16*4 = 64 bytes //tm[4] to tm[67] filled
- FCTN_CONVERT_FLOAT(actual_data.voltage_actual[i], &tm[4+(i*4)]);
- for(int i=0; i<12; i++) //12*4 = 48 //tm[68] to tm[115] filled
- FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&tm[68 + (i*4)]);
- for (int i=116; i<132;i++)
- {
- tm[i] = 0x00;
+ telemetry[i] = 0x00;
}
- crc16 = CRC::crc16_gen(tm,132);
- tm[132] = (uint8_t)((crc16&0xFF00)>>8);
- tm[133] = (uint8_t)(crc16&0x00FF);
- return tm;
- }
- case 0x02:
- {
- printf("Read HK\r\n");
- tm[0] = 0x60;
- tm[1] = tc[0];
- tm[2] = ACK_CODE;
- FCTN_CONVERT_FLOAT(actual_data.Batt_temp_actual[0],&tm[4]); //tm[4]-tm[7]
- FCTN_CONVERT_FLOAT(actual_data.Batt_temp_actual[1],&tm[8]); //tm[8]- tm[11]
- for(int i=0; i<4; i++)
- FCTN_CONVERT_FLOAT(actual_data.Batt_gauge_actual[i],&tm[12+(i*4)]); //tm[12] - tm[27]
- FCTN_CONVERT_FLOAT(actual_data.BAE_temp_actual,&tm[28]); //tm[28] - tm[31]
- tm[32] = (uint8_t)actual_data.power_mode;
- tm[33] = actual_data.faultPoll_status;
- tm[34] = actual_data.faultIr_status;
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[35+(i*4)]); //35 -46
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[47+(i*4)]); //47 -58
- FCTN_CONVERT_FLOAT(actual_data.Batt_voltage_actual,&tm[59]); //59 - 62
- for (int i=63; i<132;i++)
- {
- tm[i] = 0x00;
- }
- crc16 = CRC::crc16_gen(tm,132);
- tm[132] = (uint8_t)((crc16&0xFF00)>>8);
- tm[133] = (uint8_t)(crc16&0x00FF);
- return tm;
-
- }
- case 0x03:
- {
- printf("Read min max data");
- tm[0] = 0x60;
- tm[1] = tc[0];
- tm[2] = ACK_CODE;
- for(int i=4; i<20; i++)
- tm[i] = (uint8_t)bae_HK_minmax.voltage_max[i-4];
- for(int i=20; i<32; i++)
- tm[i] = (uint8_t)bae_HK_minmax.current_max[i-20];
- tm[32] = (uint8_t)bae_HK_minmax.Batt_temp_max[0];
- tm[33] = (uint8_t)bae_HK_minmax.Batt_temp_max[1];
- tm[34] = (uint8_t)bae_HK_minmax.Batt_gauge_max[0];
- tm[35] = (uint8_t)bae_HK_minmax.Batt_gauge_max[1];
- tm[36] = (uint8_t)bae_HK_minmax.Batt_gauge_max[2];
- tm[37] = (uint8_t)bae_HK_minmax.BAE_temp_max;
- FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[0],&tm[38]); //tm[38] - tm[41]
- FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[1],&tm[42]); //tm[42] - tm[45]
- FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[2],&tm[46]); //tm[46] - tm[49]
- FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[0],&tm[50]); //tm[50] - tm[53]
- FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[1],&tm[54]); //tm[54] - tm[57]
- FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[2],&tm[58]); //tm[58] - tm[61]
- tm[62] = (uint8_t)bae_HK_minmax.Bvalue_max[0];
- tm[63] = (uint8_t)bae_HK_minmax.Bvalue_max[1];
- tm[64] = (uint8_t)bae_HK_minmax.Bvalue_max[2];
- tm[65] = (uint8_t)bae_HK_minmax.Batt_voltage_max;
- for(int i=66; i<82; i++)
- tm[i] = (uint8_t)bae_HK_minmax.voltage_min[i-66];
- for(int i=82; i<94; i++)
- tm[i] = (uint8_t)bae_HK_minmax.current_min[i-82];
- tm[94] = (uint8_t)bae_HK_minmax.Batt_temp_min[0];
- tm[95] = (uint8_t)bae_HK_minmax.Batt_temp_min[1];
- tm[96] = (uint8_t)bae_HK_minmax.Batt_gauge_min[0];
- tm[97] = (uint8_t)bae_HK_minmax.Batt_gauge_min[1];
- tm[98] = (uint8_t)bae_HK_minmax.Batt_gauge_min[2];
- tm[99] = (uint8_t)bae_HK_minmax.BAE_temp_min;
- FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[0],&tm[100]); //tm[100] - tm[103]
- FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[1],&tm[104]); //tm[104] - tm[107]
- FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[2],&tm[108]); //tm[108] - tm[111]
- FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[0],&tm[112]); //tm[112] - tm[115]
- FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[1],&tm[116]); //tm[116] - tm[119]
- FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[2],&tm[120]); //tm[120] - tm[123]
- tm[124] = (uint8_t)bae_HK_minmax.Batt_voltage_min;
- for (int i=125; i<132;i++)
- {
- tm[i] = 0x00;
- }
- crc16 = CRC::crc16_gen(tm,132);
- tm[132] = (uint8_t)((crc16&0xFF00)>>8);
- tm[133] = (uint8_t)(crc16&0x00FF);
- return tm;
- }
- case 0x04:
- {
- printf("Read status");
- tm[0] = 0x60;
- tm[1] = tc[0];
- tm[2] = ACK_CODE;
- tm[4] = (BAE_STATUS>>24) & 0xFF;
- tm[5] = (BAE_STATUS>>16) & 0xFF;
- tm[6] = (BAE_STATUS>>8) & 0xFF;
- tm[7] = BAE_STATUS & 0xFF;
- for (int i=8; i<132;i++)
- {
- tm[i] = 0x00;
- }
- crc16 = CRC::crc16_gen(tm,132);
- tm[132] = (uint8_t)((crc16&0xFF00)>>8);
- tm[133] = (uint8_t)(crc16&0x00FF);
- return tm;
-
- }
- }
- */
-
+ crc16 = CRC::crc16_gen(telemetry,132);
+ telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[133] = (uint8_t)(crc16&0x00FF);
+
+ break;
+ }
+ case 0x0002:
+ {
+ telemetry[0] = 0x60;
+ telemetry[1] = tc[0];
+ telemetry[2] = ACK_CODE;
+
+ for(int i;i<16;i++)
+ telemetry[i+3] = bae_HK_minmax.voltage_max[i];
+
+ for(int i;i<12;i++)
+ telemetry[i+18] = bae_HK_minmax.current_max[i];
+
+ telemetry[29] = bae_HK_minmax.Batt_voltage_max;;
+ telemetry[30] = bae_HK_minmax.BAE_temp_max;
+
+ /*battery soc*/
+ //telemetry[31] = BAE_HK_min_max bae_HK_minmax.voltage_max;
+
+ telemetry[32] = bae_HK_minmax.Batt_temp_max[0];
+ telemetry[33] = bae_HK_minmax.Batt_temp_max[1];
+
+ /*BCN temp not there*/
+ //telemetry[34] = BAE_HK_min_max bae_HK_minmax.;
+
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[i],&telemetry[35+(i*4)]);
+
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[i],&telemetry[47+(i*4)]);
+
+ /*min data*/
+
+ for(int i;i<16;i++)
+ telemetry[i+59] = bae_HK_minmax.voltage_min[i];
+
+ for(int i;i<12;i++)
+ telemetry[i+74] = bae_HK_minmax.current_min[i];
+
+ telemetry[86] = bae_HK_minmax.Batt_voltage_min;
+ telemetry[87] = bae_HK_minmax.BAE_temp_min;
+
+ /*battery soc*/
+ //telemetry[88] = BAE_HK_min_max bae_HK_minmax.voltage_max;
+
+ telemetry[89] = bae_HK_minmax.Batt_temp_min[0];
+ telemetry[90] = bae_HK_minmax.Batt_temp_min[1];
+ //huhu//
+
+ /*BCN temp not there*/
+ //telemetry[91] = BAE_HK_min_max bae_HK_minmax.;
+
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[i],&telemetry[91+(i*4)]);
+
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[i],&telemetry[103+(i*4)]);
+
+
+ for (int i=115; i<132;i++)
+ {
+ telemetry[i] = 0x00;
+ }
+ crc16 = CRC::crc16_gen(telemetry,132);
+ telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[133] = (uint8_t)(crc16&0x00FF);
+ break;
+ }
+ }
+ break;
+ }
case 0x05:
{
printf("Write on Flash\r\n");
+ break;
}
default:
{
printf("Invalid TC");
- //ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ //ACK_L234_telemetry
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
}
+ break;
}
-
case 0x80:
{
- printf("Function Management Service\r\n");
+ //printf("Function Management Service\r\n");
uint8_t service_subtype=(tc[2]&0x0F);
-
switch(service_subtype)
{
case 0x01:
{
printf("FMS Activated\r\n");
-
uint8_t pid=tc[3];
switch(pid)
{
@@ -480,683 +354,700 @@
{
float B[3],W[3];
printf("ACS_COMSN\r\n");
- //ACK_L234_TM
-
+ //ACK_L234_telemetry
+
uint8_t B_x[2];
uint8_t B_y[2];
uint8_t B_z[2];
uint8_t W_x[2];
uint8_t W_y[2];
uint8_t W_z[2];
-
+
B_x[0]=tc[3];
B_x[1]=tc[4];
B_y[0]=tc[5];
B_y[1]=tc[6];
B_z[0]=tc[7];
B_z[1]=tc[8];
-
+
W_x[0]=tc[9];
W_x[1]=tc[10];
W_y[0]=tc[11];
W_y[1]=tc[12];
W_z[0]=tc[13];
W_z[1]=tc[14];
-
+
FCTN_CONVERT_UINT(B_x,&B[0]);
FCTN_CONVERT_UINT(B_y,&B[1]);
FCTN_CONVERT_UINT(B_z,&B[2]);
-
+
FCTN_CONVERT_UINT (W_x, &W[0]);
FCTN_CONVERT_UINT (W_y, &W[1]);
FCTN_CONVERT_UINT (W_z, &W[2]);
-
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
+
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
//FCTN_ATS_DATA_ACQ(); //get data
- printf("gyro values\n\r");
- for(int i=0; i<3; i++)
+ printf("gyro values\n\r");
+ for(int i=0; i<3; i++)
printf("%f\n\r",W[i]);
printf("mag values\n\r");
- for(int i=0; i<3; i++)
+ for(int i=0; i<3; i++)
printf("%f\n\r",B[i]);
- /* FCTN_CONVERT_FLOAT(data[0],&tm[4]); //tm[4] - tm[7]
- FCTN_CONVERT_FLOAT(data[1],&tm[8]); //tm[8] - tm[11]
- FCTN_CONVERT_FLOAT(data[2],&tm[12]); //tm[12] - tm[15]
- FCTN_CONVERT_FLOAT(data[0],&tm[16]); //tm[16] - tm[19]
- FCTN_CONVERT_FLOAT(data[1],&tm[20]); //tm[20] - tm[23]
- FCTN_CONVERT_FLOAT(data[2],&tm[24]); //tm[24] - tm[27]
+ /* FCTN_CONVERT_FLOAT(data[0],&telemetry[4]); //telemetry[4] - telemetry[7]
+ FCTN_CONVERT_FLOAT(data[1],&telemetry[8]); //telemetry[8] - telemetry[11]
+ FCTN_CONVERT_FLOAT(data[2],&telemetry[12]); //telemetry[12] - telemetry[15]
+ FCTN_CONVERT_FLOAT(data[0],&telemetry[16]); //telemetry[16] - telemetry[19]
+ FCTN_CONVERT_FLOAT(data[1],&telemetry[20]); //telemetry[20] - telemetry[23]
+ FCTN_CONVERT_FLOAT(data[2],&telemetry[24]); //telemetry[24] - telemetry[27]
if((data[0]<8) && (data[1]<8) && (data[2] <8))
- tm[28] = 1; // gyro values in correct range
+ telemetry[28] = 1; // gyro values in correct range
else
- tm[28] = 0;
+ telemetry[28] = 0;
if ((data[3] > 20 ) && (data[4] >20) && (data[5]>20)&& (data[3] < 50 ) && (data[4] <50) && (data[5]<50))
- tm[29] = 1; // mag values in correct range
+ telemetry[29] = 1; // mag values in correct range
else
- tm[29] = 0;
- */
- // float B[3],W[3];
+ telemetry[29] = 0;
+ */
+ // float B[3],W[3];
// B[0] = B0;
- // B[1] = B1;
+ // B[1] = B1;
// B[2] = B2;
// W[0] = W0;
- // W[1] = W1;
- // W[2] = W2;
+ // W[1] = W1;
+ // W[2] = W2;
// Control algo commissioning
/* FCTN_ACS_CNTRLALGO(B,W);
- FCTN_CONVERT_FLOAT(moment[0],&tm[30]); //tm[30] - tm[33]
- FCTN_CONVERT_FLOAT(moment[1],&tm[34]); //tm[34] - tm[37]
- FCTN_CONVERT_FLOAT(moment[2],&tm[38]); //tm[38] - tm[41]
+ FCTN_CONVERT_FLOAT(moment[0],&telemetry[30]); //telemetry[30] - telemetry[33]
+ FCTN_CONVERT_FLOAT(moment[1],&telemetry[34]); //telemetry[34] - telemetry[37]
+ FCTN_CONVERT_FLOAT(moment[2],&telemetry[38]); //telemetry[38] - telemetry[41]
// to include commission TR as well
for(uint8_t i=42;i<132;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
-
- crc16 = CRC::crc16_gen(tm,132);
- tm[133] = (uint8_t)((crc16&0xFF00)>>8);
- tm[134] = (uint8_t)(crc16&0x00FF);
- return tm;
- */
-
+
+ crc16 = CRC::crc16_gen(telemetry,132);
+ telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[134] = (uint8_t)(crc16&0x00FF);
+ break;
+ */
+
// Control algo commissioning
FCTN_ACS_CNTRLALGO(B,W);
- FCTN_CONVERT_FLOAT(moment[0],&tm[4]); //tm[4] - tm[7]
- FCTN_CONVERT_FLOAT(moment[1],&tm[8]); //tm[8] - tm[11]
- FCTN_CONVERT_FLOAT(moment[2],&tm[12]); //tm[12] - tm[15]
+ FCTN_CONVERT_FLOAT(moment[0],&telemetry[4]); //telemetry[4] - telemetry[7]
+ FCTN_CONVERT_FLOAT(moment[1],&telemetry[8]); //telemetry[8] - telemetry[11]
+ FCTN_CONVERT_FLOAT(moment[2],&telemetry[12]); //telemetry[12] - telemetry[15]
// to include commission TR as well
for(uint8_t i=16;i<132;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
-
- crc16 = CRC::crc16_gen(tm,132);
- tm[133] = (uint8_t)((crc16&0xFF00)>>8);
- tm[134] = (uint8_t)(crc16&0x00FF);
- return tm;
-
+
+ crc16 = CRC::crc16_gen(telemetry,132);
+ telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[134] = (uint8_t)(crc16&0x00FF);
+ break;
}
-
-
-
case 0xE1:
{
- float moment_tc[3];
+ float moment_tc[3];
printf("HARDWARE_COMSN\r\n");
- //ACK_L234_TM
+ //ACK_L234_telemetry
uint8_t M0[2];
uint8_t M1[2];
uint8_t M2[2];
-
+
M0[0]=tc[3];
M0[1]=tc[4];
M1[0]=tc[5];
M1[1]=tc[6];
M2[0]=tc[7];
M2[1]=tc[8];
-
+
+
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
- FCTN_CONVERT_UINT(M0,&moment_tc[0]);
- FCTN_CONVERT_UINT(M1, &moment_tc[1]);
- FCTN_CONVERT_UINT(M2, &moment_tc[2]);
-
+ float PWM_measured[3];
- FCTN_ACS_GENPWM_MAIN(moment_tc);
-
- float PWM_measured[3];
-
- PWM_measured[0] = PWM1.read();
- PWM_measured[1] = PWM2.read();
- PWM_measured[2] = PWM3.read();
+ FCTN_CONVERT_UINT(M0,&moment_tc[0]);
+ moment_tc[1] = 0;
+ moment_tc[2] = 0;
+ FCTN_ACS_GENPWM_MAIN(moment_tc);
+ PWM_measured[0] = PWM1.read();
+ FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (0*4)]);
+ FCTN_CONVERT_UINT(M1, &moment_tc[1]);
+ moment_tc[0] = 0;
+ moment_tc[2] = 0;
+ FCTN_ACS_GENPWM_MAIN(moment_tc);
+ PWM_measured[1] = PWM2.read();
+ FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (1*4)]);
+
+ FCTN_CONVERT_UINT(M2, &moment_tc[2]);
+ moment_tc[0] = 0;
+ moment_tc[1] = 0;
+ FCTN_ACS_GENPWM_MAIN(moment_tc);
+ PWM_measured[2] = PWM3.read();
+ FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (2*4)]);
- FCTN_CONVERT_FLOAT(PWM_measured[0],&tm[4]); //4-7
- FCTN_CONVERT_FLOAT(PWM_measured[1],&tm[8]); //8-11
- FCTN_CONVERT_FLOAT(PWM_measured[2],&tm[12]); //12-15
- for(int i=0; i<12; i++)
- FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&tm[16 + (i*4)]);
+ FCTN_CONVERT_FLOAT(PWM_measured[0],&telemetry[4]); //4-7
+ FCTN_CONVERT_FLOAT(PWM_measured[1],&telemetry[8]); //8-11
+ FCTN_CONVERT_FLOAT(PWM_measured[2],&telemetry[12]); //12-15
+
+ // for(int i=0; i<12; i++)
+ // FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]);
- FCTN_ATS_DATA_ACQ(); //get data
-
+
+ // FCTN_ATS_DATA_ACQ(); //get data
+
// to include commission TR as well
- for(uint8_t i=64;i<132;i++)
+ for(uint8_t i=28;i<132;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
-
- crc16 = CRC::crc16_gen(tm,132);
- tm[133] = (uint8_t)((crc16&0xFF00)>>8);
- tm[134] = (uint8_t)(crc16&0x00FF);
- return tm;
-
+
+ crc16 = CRC::crc16_gen(telemetry,132);
+ telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[134] = (uint8_t)(crc16&0x00FF);
+ break;
}
- case 0x02:
+ case 0x02:
{
- F_EPS();
- /* printf("Run P_EPS_MAIN\r\n");
- //ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
- */
+
+ printf("Run P_EPS_MAIN\r\n");
+ F_EPS();
+ //ACK_L234_telemetry
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
for(uint8_t i=0;i<133;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,132);
- tm[132] = (uint8_t)((crc16&0xFF00)>>8);
- tm[133] = (uint8_t)(crc16&0x00FF);
- /*
+ crc16 = CRC::crc16_gen(telemetry,132);
+ telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[133] = (uint8_t)(crc16&0x00FF);
+
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- */
-
- return tm;
+ break;
}
-
- /* case 0x03:
+ case 0x03:
{
-
- /* printf("Run P_ACS_INIT\r\n");
- //ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ printf("Run P_ACS_INIT\r\n");
+ FCTN_ACS_INIT();
+ //ACK_L234_telemetry
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
case 0x04:
{
+
printf("Run P_ACS_ACQ_DATA\r\n");
+ FCTN_ATS_DATA_ACQ();
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
- */
case 0x05:
{
printf("Run P_ACS_MAIN\r\n");
F_ACS();
-
-
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&telemetry[(i*4)]);
for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[(i*4)]);
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[12+(i*4)]);
-
- tm[24] = ACS_STATE;
- tm[24] = tm[5]|(EN_BTRY_HT<<3);
- tm[24] = tm[5]|(phase_TR_x<<4);
- tm[24] = tm[5]|(phase_TR_y<<5);
- tm[24] = tm[5]|(phase_TR_z<<6);
-
+ FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&telemetry[12+(i*4)]);
+
+ telemetry[24] = ACS_STATE;
+ telemetry[24] = telemetry[5]|(EN_BTRY_HT<<3);
+ telemetry[24] = telemetry[5]|(phase_TR_x<<4);
+ telemetry[24] = telemetry[5]|(phase_TR_y<<5);
+ telemetry[24] = telemetry[5]|(phase_TR_z<<6);
+
/*___________________change / check pwm working__________________________________*/
-
- FCTN_CONVERT_FLOAT(PWM1,&tm[25]);
- FCTN_CONVERT_FLOAT(PWM2,&tm[29]);
- FCTN_CONVERT_FLOAT(PWM3,&tm[33]);
-
+
+ FCTN_CONVERT_FLOAT(PWM1,&telemetry[25]);
+ FCTN_CONVERT_FLOAT(PWM2,&telemetry[29]);
+ FCTN_CONVERT_FLOAT(PWM3,&telemetry[33]);
+
//ACK_L234_TM
- /* tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- */
- crc16 = CRC::crc16_gen(tm,37);
- tm[37] = (uint8_t)((crc16&0xFF00)>>8);
- tm[38] = (uint8_t)(crc16&0x00FF);
+
+ crc16 = CRC::crc16_gen(telemetry,37);
+ telemetry[37] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[38] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=39;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
-
-
-
case 0x06:
{
F_BCN();
- /* printf("Run FCTN_BCN_INIT\r\n");
+ printf("Run P_BCN_INIT\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- */
- crc16 = CRC::crc16_gen(tm,0);
- tm[0] = (uint8_t)((crc16&0xFF00)>>8);
- tm[1] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,0);
+ telemetry[0] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[1] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=2;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
- /*
case 0x07:
{
- printf("Run FCTN_BCN_TX_MAIN\r\n");
+ printf("Run P_BCN_TX_MAIN\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
- }*/
- case 0x11:
+ break;
+ }
+ case 0x11:
{
printf("SW_ON_ACS_ATS1_SW_ENABLE\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
ATS1_SW_ENABLE = 1; // making sure we switch off the other
ATS1_SW_ENABLE = 0;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
case 0x12:
{
printf("SW_ON_ACS_ATS2_SW_ENABLE\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
ATS1_SW_ENABLE = 1; //make sure u switch off the other
ATS2_SW_ENABLE = 0;
- tm[2]=1;
+ telemetry[2]=1;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
case 0x13:
{
printf("SW_ON_ACS_TR_XY_ENABLE\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- TRXY_SW_EN = 1;
- tm[2]=1;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ TRXY_SW = 1;
+ telemetry[2]=1;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
case 0x14:
{
printf("SW_ON_ACS_TR_Z_ENABLE\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
TRZ_SW = 1;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
- case 0x15:
+ case 0x15:
{
printf("SW_ON_BCN_TX_SW_ENABLE\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
- BCN_SW = 0;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
+ BCN_SW = 0;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
case 0x21:
{
printf("SW_OFF_ACS_ATS1_SW_ENABLE\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
ATS1_SW_ENABLE = 1;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
case 0x22:
{
printf("SW_OFF_ACS_ATS2_SW_ENABLE\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
ATS2_SW_ENABLE = 1;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
case 0x23:
{
printf("SW_OFF_ACS_TR_XY_ENABLE\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
- TRXY_SW_EN= 0;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
+ TRXY_SW= 0;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
case 0x24:
{
printf("SW_OFF_ACS_TR_Z_ENABLE\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
TRZ_SW = 0;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
- case 0x25:
+ case 0x25:
{
printf("SW_OFF_BCN_TX_SW_ENABLE\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
BCN_SW = 1;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
case 0x31:
{
printf("ACS_ATS1_SW_RESET\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
ATS1_SW_ENABLE = 1;
wait_us(1);
ATS1_SW_ENABLE = 0;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
case 0x32:
{
printf("BCN_SW_RESET\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
BCN_SW = 1;
wait_us(1);
BCN_SW = 0;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
case 0x33:
{
printf("ACS_ATS2_SW_RESET\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
ATS1_SW_ENABLE = 1;
wait_us(1);
ATS1_SW_ENABLE = 0;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
- case 0x34:
+ case 0x34:
{
printf("CDMS_SW_RESET\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=1;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=1;
CDMS_RESET = 0;
wait_us(1);
CDMS_RESET = 1;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
-
-
default:
- {
+ {
printf("Invalid TC\r\n");
//ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
- tm[i]=0x00;
+ telemetry[i]=0x00;
}
- return tm;
+ break;
}
}
-
+ break;
+ }
default:
- {
- printf("Invalid TC\r\n");
- //ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
- for(uint8_t i=3;i<11;i++)
- {
- tm[i]=0x00;
- }
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
- for(uint8_t i=13;i<134;i++)
- {
- tm[i]=0x00;
- }
- return tm;
- }
+ {
+ printf("Invalid TC\r\n");
+ //ACK_L234_TM
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
+ for(uint8_t i=3;i<11;i++)
+ {
+ telemetry[i]=0x00;
+ }
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
+ for(uint8_t i=13;i<134;i++)
+ {
+ telemetry[i]=0x00;
+ }
+ break;
}
}
+ break;
+ }
+ default:
+ {
+ printf("Invalid TC\r\n");
+ //ACK_L234_TM
+ telemetry[0]=0xB0;
+ telemetry[1]=tc[0];
+ telemetry[2]=ACK_CODE;
+ for(uint8_t i=3;i<11;i++)
+ {
+ telemetry[i]=0x00;
+ }
+ crc16 = CRC::crc16_gen(telemetry,11);
+ telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[12] = (uint8_t)(crc16&0x00FF);
+ for(uint8_t i=13;i<134;i++)
+ {
+ telemetry[i]=0x00;
+ }
+ break;
}
}
-}
+}
-int strt_add = flash_size() - (2*SECTOR_SIZE);
+int strt_add = flash_size() - (2*SECTOR_SIZE);
uint32_t flasharray[8]; //256+(3*1024)
char *nativeflash = (char*)strt_add;
/*Writing to the Flash*/
void FCTN_CDMS_WR_FLASH(uint16_t j,uint32_t fdata) //j-position to write address ; fdata - flash data to be written
{
- for(int i=0;i<8;i++)
+ for(int i=0;i<8;i++)
{
flasharray[i]=nativeflash[i];
}
@@ -1185,14 +1076,14 @@
uint32_t* temp = reinterpret_cast<uint32_t*>(&input);
//float* output1 = reinterpret_cast<float*>(temp);
-
+
printf("\n\r %f ", input);
std::cout << "\n\r uint32"<<*temp << std::endl;
output[0] =(uint8_t )(((*temp)>>24)&0xFF);
output[2] =(uint8_t ) (((*temp)>>16)&0xFF);
- output[1] =(uint8_t ) (((*temp)>>8)&0xFF);
- output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic
+ output[1] =(uint8_t ) (((*temp)>>8)&0xFF);
+ output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic
//printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]);
- //std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl;
-}
+ //std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl;
+}
\ No newline at end of file