Team Fox
ACS completed fully. All cases to be tested
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
ACS_Flowchart_BAE
by 
Team Fox
TCTM.cpp
- Committer:
- Bragadeesh153
- Date:
- 2016-06-13
- Revision:
- 18:21740620c65e
- Parent:
- 16:cc77770d787f
File content as of revision 18:21740620c65e:
#include "mbed.h"
#include "TCTM.h"
#include "crc.h"
#include "ACS.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 DRV_Z_EN;
extern DigitalOut DRV_XY_EN;
extern uint8_t ACS_STATUS;
extern float b_old[3]; // Unit: Tesla
extern float db[3];
extern uint8_t flag_firsttime;
extern uint8_t ACS_DETUMBLING_ALGO_TYPE;
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 BCN_TX_STATUS;
extern uint8_t BCN_FEN;
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 int FCTN_ACS_INIT();
extern int SENSOR_DATA_ACQ();
extern int SENSOR_INIT();
extern int FCTN_ATS_DATA_ACQ();
extern void FCTN_ACS_CNTRLALGO(float*,float*,int);
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);
/*chaged*/
uint8_t* tm; // without it some identifier error
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");
uint16_t jj;
uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
switch(MID)
{case 0x1100:jj=0x00;// using uint16_t as jj typesimilarly used in FCTN_CDMS_WR_FLASH
break;
case 0x0100:jj=0x01;
break;
case 0x0101:jj=0x02;
break;
case 0x0102:jj=0x03;
break;
case 0x0107:jj=0x04;
break;
case 0x0103:jj=0x05;
break;
case 0x0104:jj=0x05;
break;
case 0x0105:jj=0x06;
break;
case 0x0106:jj=0x07;
break;
}
/*pointer....!!!!*/
uint32_t FLASH_TEMP = FCTN_CDMS_RD_FLASH(jj);
tm[0] = 0x60;
tm[1] = tc[0];
tm[2] = ACK_CODE;
for(int i=0; i<8*4; i+=4)
{
tm[4+i] =(uint8_t )(((FLASH_TEMP)>>24)&0xFF);
tm[5+i] =(uint8_t ) (((FLASH_TEMP)>>16)&0xFF);
tm[6+i] =(uint8_t ) (((FLASH_TEMP)>>8)&0xFF);
tm[7+i] =(uint8_t ) ((FLASH_TEMP) & 0xFF);
}
for (int i=4+8*4; 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);
break;
}
case 0x02:
{
printf("Read from RAM\r\n");
uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
switch(MID)
{
case 0x0001:
{
printf("\nRead from MID 0001 hk\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;
//actual_data.AngularSpeed_actual[0]=5.32498;
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT((float)actual_data.Bvalue_actual[i],&telemetry[26+ (i*4)]);
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT((float)actual_data.AngularSpeed_actual[i],&telemetry[38+(i*4)]);
//printf("\n\rthe value is 38\t %x\n",telemetry[38]);
//printf("\n\rthe value is 39\t%x\n",telemetry[39]);
//printf("\n\rthe value is 40\t%x\n",telemetry[40]);
//printf("\n\rthe value is 41\t%x\n",telemetry[41]);
//printf("\n\rthe value true\t%f\n",actual_data.AngularSpeed_actual[0]);
//uint32_t input_stage1=0x00000000;
//uint8_t output1[4];
//output1[0]=(uint32_t)(telemetry[38]);
//output1[1]=(uint32_t)(telemetry[39]);
//output1[2]=(uint32_t)(telemetry[40]);
//output1[3]=(uint32_t)(telemetry[41]);
//input_stage1=output[3]+(output[2]*(0x100))+(output[1]*(0x10000))+(output[0]*(0x1000000));
//input_stage1=(output1[0]<<24) | (output1[1]<<16) | (output1[2]<<8) | (output1[3]);
//assert(sizeof(float) == sizeof(uint32_t));
//float* temp1 = reinterpret_cast<float*>(&input_stage1);
//printf("\n\r the value is: %f \n",*temp1);
//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:
{
printf("\r\n");
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("\nRead from MID 0001 min max\n");
/*changed*/
printf("\n\rwrite on flash\n");
uint32_t FLASH_DATA[8];
uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
switch(MID )
{
case 0x0100:
{
FLASH_DATA[0] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
FCTN_CDMS_WR_FLASH(0x00,FLASH_DATA[0]);
break;
}
case 0x0101:
{
FLASH_DATA[1] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
FCTN_CDMS_WR_FLASH(0x01,FLASH_DATA[1]);
break;
}
case 0x0102:
{
FLASH_DATA[2] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
FCTN_CDMS_WR_FLASH(0x02,FLASH_DATA[2]);
break;
}
case 0x0103:
{
FLASH_DATA[3] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
FCTN_CDMS_WR_FLASH(0x03,FLASH_DATA[3]);
break;
}
case 0x0104:
{
FLASH_DATA[4] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
FCTN_CDMS_WR_FLASH(0x04,FLASH_DATA[4]);
break;
}
case 0x0105:
{
FLASH_DATA[5] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
FCTN_CDMS_WR_FLASH(0x05,FLASH_DATA[5]);
break;
}
case 0x0106:
{
FLASH_DATA[6] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
FCTN_CDMS_WR_FLASH(0x06,FLASH_DATA[6]);
break;
}
case 0x0107:
{
FLASH_DATA[7] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
FCTN_CDMS_WR_FLASH(0x07,FLASH_DATA[7]);
break;
}
default:
{
printf("Invalid MMS\r\n");
}
}
for (int i=4; 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);
tm[0] = 0x60;
tm[1] = tc[0];
tm[2] = ACK_CODE;
printf("Written 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 mag_data[3]={0,0,0};
float gyro_data[3]={0,0,0};
printf("ACS_COMSN SOFTWARE\r\n");
//ACK_L234_telemetry
ACS_STATE = tc[4];
if(ACS_STATE == 7) // Nominal mode
{
printf("\n\r Nominal mode \n");
DRV_Z_EN = 1;
DRV_XY_EN = 1;
FCTN_ACS_CNTRLALGO(mag_data,gyro_data,1);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
printf("%f\t",moment[i]);
}
ACS_STATUS = 5; // set ACS_STATUS = ACS_NOMINAL_ONLY
}
else if(ACS_STATE == 8) // Auto Control
{
printf("\n\r Auto control mode \n");
DRV_Z_EN = 1;
DRV_XY_EN = 1;
FCTN_ACS_CNTRLALGO(mag_data,gyro_data,0);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
printf("%f\t",moment[i]);
}
}
else if(ACS_STATE == 9) // Detumbling
{
DRV_Z_EN = 1;
DRV_XY_EN = 1;
if(flag_firsttime==1)
{
for(int i=0;i<3;i++)
{
db[i]=0; // Unit: Tesla/Second
}
flag_firsttime=0;
}
else
{
for(int i=0;i<3;i++)
{
db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
}
}
if (ACS_DETUMBLING_ALGO_TYPE == 0)
{
for(int i=0;i<3;i++)
{
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
}
ACS_STATUS = 6; // set ACS_STATUS = ACS_BOMEGA_CONTROL
}
else if(ACS_DETUMBLING_ALGO_TYPE == 1)
{
for(int i=0;i<3;i++)
{
moment[i]=-kdetumble*db[i]; // Unit: Ampere*Meter^2
}
ACS_STATUS = 4; // set ACS_STATUS = ACS_BDOT_CONTROL
}
for(int i=0;i<3;i++)
{
b_old[i]= mag_data[i]; // Unit: Tesla/Second
}
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
printf("%f\t",moment[i]);
}
}
ACS_STATUS = 7;
// Control algo commissioning
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
int init;
int data;
float PWM_tc[3];
PWM_tc[0]=(float(tc[4]))*1;
PWM_tc[1]=(float(tc[4]))*1;
PWM_tc[2]=(float(tc[4]))*1;
DRV_Z_EN = 1;
DRV_XY_EN = 1;
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=ACK_CODE;
PWM1 = 0;
PWM2 = 0;
PWM3 = 0;
wait_ms(ACS_DEMAG_TIME_DELAY);
ATS2_SW_ENABLE = 1;
ATS1_SW_ENABLE = 0;
init = SENSOR_INIT();
data = SENSOR_DATA_ACQ();
ATS2_SW_ENABLE = 1;
ATS1_SW_ENABLE = 0;
init = SENSOR_INIT();
data = SENSOR_DATA_ACQ();
PWM1 = PWM_tc[0];
PWM2 = 0;
PWM3 = 0;
wait_ms(ACS_DEMAG_TIME_DELAY);
SENSOR_DATA_ACQ();
FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (0*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (1*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (2*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (3*4)]);
PWM1 = 0;
PWM2 = PWM_tc[1];
PWM3 = 0;
wait_ms(ACS_DEMAG_TIME_DELAY);
SENSOR_DATA_ACQ();
FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (4*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (5*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (6*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (7*4)]);
PWM1 = 0;
PWM2 = 0;
PWM3 = PWM_tc[2];
wait_ms(ACS_DEMAG_TIME_DELAY);
wait_ms(ACS_DEMAG_TIME_DELAY);
SENSOR_DATA_ACQ();
FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (4*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (5*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (6*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (7*4)]);
// 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
// to include commission TR as well
for(uint8_t i=35;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;
ATS1_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[1] =(uint8_t ) (((*temp)>>16)&0xFF);
output[2] =(uint8_t ) (((*temp)>>8)&0xFF);
output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic
printf("\n\rthe values generated are\n");
/*printf("\n\r%x\n",output[0]);
printf("\n\r%x\n",output[1]);
printf("\n\r%x\n",output[2]);
printf("\n\r%x\n",output[3]);
to check the values generated
*/
//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;
}