Team Fox
Changes to be made for ATS_Fault logic and ACS_State
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
Japan_BAE_sensorworking_interrupr_reoccuring_copy
by 
Team Fox
TCTM.cpp
- Committer:
- Bragadeesh153
- Date:
- 2016-04-20
- Revision:
- 15:3239c6391ffa
- Parent:
- 14:a9588f443f1a
File content as of revision 15:3239c6391ffa:
#include "mbed.h"
#include "TCTM.h"
#include "crc.h"
#include "EPS.h"
#include "pin_config.h"
#include "FreescaleIAP.h"
#include "inttypes.h"
#include "iostream"
#include "stdint.h"
#include "cassert"
#include"math.h"
/*define the pins for digital out*/
extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
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
extern uint8_t ACS_ATS_STATUS;
extern uint8_t BCN_TX_STATUS;
extern uint8_t BCN_FEN;
extern AnalogIn CurrentInput;
extern BAE_HK_actual actual_data;
extern BAE_HK_min_max bae_HK_minmax;
extern uint32_t BAE_STATUS;
extern float data[6];
extern float moment[3];
extern uint8_t ACS_STATE;
extern DigitalOut EN_BTRY_HT;
extern DigitalOut phase_TR_x;
extern DigitalOut phase_TR_y;
extern DigitalOut phase_TR_z;
extern BAE_HK_quant quant_data;
//extern DigitalOut TRXY_SW;
//extern DigitalOut TRZ_SW_EN; //same as TRZ_SW
extern uint32_t BAE_ENABLE;
//extern DigitalOut ACS_ACQ_DATA_ENABLE;
/*given a default value as of now shuld read value from flash and increment it write it back very time it starts(bae)*/
extern uint8_t BAE_RESET_COUNTER=0;
//extern uint8_t BCN_FAIL_COUNT;
extern PwmOut PWM1; //x //Functions used to generate PWM signal
extern PwmOut PWM2; //y
extern PwmOut PWM3; //z //PWM output comes from pins p6
extern void F_ACS();
extern void F_BCN();
//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)
{
*output = (float) input[1];
*output = *output/100.0; //input[0] integer part
*output = *output + (float) input[0]; //input[1] decimal part correct to two decimal places
}
float angle(float x,float y,float z)
{
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 tm1[134];
void FCTN_BAE_TM_TC (uint8_t* tc)
{
// tm1[0] = 1;
uint8_t service_type=(tc[2]&0xF0);
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)
{
case 0x0001:
{
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;
/*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], &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, &telemetry[67]);
for (int i=0; i<16; i++)
telemetry[68+i] = quant_data.voltage_quant[i];
for (int i=0; i<12; i++)
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++)
{
telemetry[i] = 0x00;
}
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_telemetry
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;
}
case 0x80:
{
//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)
{
case 0xE0:
{
float B[3],W[3];
printf("ACS_COMSN\r\n");
//ACK_L234_telemetry
B[0]=(float)tc[4];
B[1]=(float)tc[5];
B[2] = 300; //constant value
W[0]=(float)tc[6];
W[1]=(float)tc[7];
W[2] = 300; //constant value
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("%f\n\r",W[i]);
printf("mag values\n\r");
for(int i=0; i<3; i++)
printf("%f\n\r",B[i]);
/* 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))
telemetry[28] = 1; // gyro values in correct range
else
telemetry[28] = 0;
if ((data[3] > 20 ) && (data[4] >20) && (data[5]>20)&& (data[3] < 50 ) && (data[4] <50) && (data[5]<50))
telemetry[29] = 1; // mag values in correct range
else
telemetry[29] = 0;
*/
// float B[3],W[3];
// B[0] = B0;
// B[1] = B1;
// B[2] = B2;
// W[0] = W0;
// W[1] = W1;
// W[2] = W2;
// Control algo commissioning
/* FCTN_ACS_CNTRLALGO(B,W);
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++)
{
telemetry[i]=0x00;
}
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],&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++)
{
telemetry[i]=0x00;
}
crc16 = CRC::crc16_gen(telemetry,132);
telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
telemetry[134] = (uint8_t)(crc16&0x00FF);
break;
}
case 0xE1:
{
printf("HARDWARE_COMSN\r\n");
//ACK_L234_telemetry
TRXY_SW = 1;
TRZ_SW = 1;
PWM1 = 0;
PWM2 = 0;
PWM3 = 0;
wait_ms(60); //Demagnetising time delay for torquerod
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=ACK_CODE;
float PWM_tc[3];
PWM_tc[0] = (float) tc[4];
PWM_tc[1] = (float) tc[5];
PWM_tc[2] = (float) tc[6];
ATS2_SW_ENABLE = 1;
ATS1_SW_ENABLE = 0; // making sure we switch off the other
FCTN_ATS_DATA_ACQ();
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0], &telemetry[6]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1], &telemetry[10]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2], &telemetry[14]);
FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[18]);
FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[22]);
FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[26]);
ACS_ATS_STATUS = ACS_ATS_STATUS & 0xF0;
ACS_ATS_STATUS = ACS_ATS_STATUS | 0x00;
ATS1_SW_ENABLE = 1;
ATS2_SW_ENABLE = 0; // making sure we switch off the other
FCTN_ATS_DATA_ACQ();
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0], &telemetry[30]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1], &telemetry[34]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2], &telemetry[38]);
FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[42]);
FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[46]);
FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[0], &telemetry[50]);
ACS_ATS_STATUS = ACS_ATS_STATUS & 0x0F;
ACS_ATS_STATUS = ACS_ATS_STATUS | 0x00;
ACS_ATS_STATUS = telemetry[4];
PWM1 = PWM_tc[0];
wait_ms(60);
FCTN_ATS_DATA_ACQ();
actual_data.current_actual[5]=CurrentInput.read();
actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
int resistance;
resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
if(actual_data.current_actual[5]>1.47)
{
actual_data.current_actual[5]=3694/log(24.032242*resistance);
}
else{
actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
}
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.current_actual[5], &telemetry[54]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[56]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[58]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[60]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[62]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[64]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[66]);
PWM1 = 0;
PWM2 = PWM_tc[1];
wait_ms(60);
FCTN_ATS_DATA_ACQ();
actual_data.current_actual[5]=CurrentInput.read();
actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
if(actual_data.current_actual[5]>1.47)
{
actual_data.current_actual[5]=3694/log(24.032242*resistance);
}
else{
actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
}
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.current_actual[5], &telemetry[68]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[70]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[72]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[74]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[76]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[78]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[80]);
PWM2 = 0;
PWM3 = PWM_tc[2];
wait_ms(60);
FCTN_ATS_DATA_ACQ();
resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
if(actual_data.current_actual[5]>1.47)
{
actual_data.current_actual[5]=3694/log(24.032242*resistance);
}
else{
actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
}
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.current_actual[5], &telemetry[82]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[84]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[86]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[88]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[90]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[92]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[94]);
PWM3 = 0;
wait_ms(60);
FCTN_ATS_DATA_ACQ();
actual_data.current_actual[5]=CurrentInput.read();
actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
if(actual_data.current_actual[5]>1.47)
{
actual_data.current_actual[5]=3694/log(24.032242*resistance);
}
else{
actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
}
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[0], &telemetry[96]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[1], &telemetry[98]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.Bvalue_actual[2], &telemetry[100]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[102]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[104]);
FCTN_CONVERT_FLOAT_COMPRESS(actual_data.AngularSpeed_actual[0], &telemetry[106]);
for(uint8_t i=108;i<132;i++)
{
telemetry[i]=0x00;
}
crc16 = CRC::crc16_gen(telemetry,132);
telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
telemetry[134] = (uint8_t)(crc16&0x00FF);
break;
}
case 0x02:
{
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++)
{
telemetry[i]=0x00;
}
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++)
{
telemetry[i]=0x00;
}
break;
}
case 0x03:
{
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++)
{
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;
}
case 0x04:
{
printf("Run P_ACS_ACQ_DATA\r\n");
FCTN_ATS_DATA_ACQ();
//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;
}
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.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,&telemetry[25]);
FCTN_CONVERT_FLOAT(PWM2,&telemetry[29]);
FCTN_CONVERT_FLOAT(PWM3,&telemetry[33]);
//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,37);
telemetry[37] = (uint8_t)((crc16&0xFF00)>>8);
telemetry[38] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=39;i<134;i++)
{
telemetry[i]=0x00;
}
break;
}
case 0x06:
{
F_BCN();
printf("Run P_BCN_INIT\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,0);
telemetry[0] = (uint8_t)((crc16&0xFF00)>>8);
telemetry[1] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=2;i<134;i++)
{
telemetry[i]=0x00;
}
break;
}
case 0x07:
{
printf("Run P_BCN_TX_MAIN\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;
}
case 0x11:
{
printf("SW_ON_ACS_ATS1_SW_ENABLE\r\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=1;
ATS2_SW_ENABLE = 1; // making sure we switch off the other
ATS1_SW_ENABLE = 0;
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;
}
case 0x12:
{
printf("SW_ON_ACS_ATS2_SW_ENABLE\r\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS1_SW_ENABLE = 1; //make sure u switch off the other
ATS2_SW_ENABLE = 0;
telemetry[2]=1;
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;
}
case 0x13:
{
printf("SW_ON_ACS_TR_XY_ENABLE\r\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
TRXY_SW = 1;
telemetry[2]=1;
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;
}
case 0x14:
{
printf("SW_ON_ACS_TR_Z_ENABLE\r\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=1;
TRZ_SW = 1;
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;
}
case 0x15:
{
printf("SW_ON_BCN_TX_SW_ENABLE\r\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=1;
BCN_SW = 0;
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;
}
case 0x21:
{
printf("SW_OFF_ACS_ATS1_SW_ENABLE\r\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=1;
ATS1_SW_ENABLE = 1;
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;
}
case 0x22:
{
printf("SW_OFF_ACS_ATS2_SW_ENABLE\r\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=1;
ATS2_SW_ENABLE = 1;
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;
}
case 0x23:
{
printf("SW_OFF_ACS_TR_XY_ENABLE\r\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=1;
TRXY_SW= 0;
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;
}
case 0x24:
{
printf("SW_OFF_ACS_TR_Z_ENABLE\r\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=1;
TRZ_SW = 0;
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;
}
case 0x25:
{
printf("SW_OFF_BCN_TX_SW_ENABLE\r\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=1;
BCN_SW = 1;
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;
}
case 0x31:
{
printf("ACS_ATS1_SW_RESET\r\n");
//ACK_L234_TM
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++)
{
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;
}
case 0x32:
{
printf("BCN_SW_RESET\r\n");
//ACK_L234_TM
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++)
{
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;
}
case 0x33:
{
printf("ACS_ATS2_SW_RESET\r\n");
//ACK_L234_TM
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++)
{
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;
}
case 0x34:
{
printf("CDMS_SW_RESET\r\n");
//ACK_L234_TM
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++)
{
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;
}
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;
}
}
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;
}
}
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);
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++)
{
flasharray[i]=nativeflash[i];
}
flasharray[j]=fdata;
erase_sector(strt_add);
program_flash(strt_add, (char*)&flasharray,4*8);
}
/*End*/
/*Reading from Flash*/
uint32_t FCTN_CDMS_RD_FLASH(uint16_t j)
{
for(int i=0;i<8;i++)
{
flasharray[i]=nativeflash[i];
}
return flasharray[j];
}
/*End*/
// Convert float to 4 uint8_t
void FCTN_CONVERT_FLOAT(float input, uint8_t output[4])
{
assert(sizeof(float) == sizeof(uint32_t));
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
//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;
}
void FCTN_CONVERT_FLOAT_COMPRESS(float input, uint8_t output[2])
{
int integer = (int)input;
assert(sizeof(int) == sizeof(uint16_t));
uint16_t* temp = reinterpret_cast<uint16_t*>(&integer);
//float* output1 = reinterpret_cast<float*>(temp);
printf("\n\r %d ", integer);
std::cout << "\n\r uint16"<<*temp << std::endl;
output[0] =(uint8_t ) (((*temp)>>8)&0xFF);
output[1] =(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;
}