Team Fox
/
BAE_QM_MAR9
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
workinQM_5thJan_azad
by 
Team Fox
TCTM.cpp
- Committer:
- samp1234
- Date:
- 2017-03-09
- Revision:
- 99:2cc5c0c98cb3
- Parent:
- 97:36a91c432cb1
- Child:
- 100:af43bc82d3eb
File content as of revision 99:2cc5c0c98cb3:
#include "mbed.h"
#include "rtos.h"
#include "TCTM.h"
#include "crc.h"
#include "EPS.h"
#include "ACS.h"
#include "BCN.h"
#include "pin_config.h"
#include "FreescaleIAP.h"
#include "inttypes.h"
#include "iostream"
#include "stdint.h"
#include "cassert"
#include"math.h"
//Serial pc(USBTX,USBRX);
Serial gpc(USBTX, USBRX);
//**********************************STATUS_PARAMETERS*****************************************************
//uint8_t BCN_TX_SW_ENABLE=0x00;
//***********************************FOR STANDBY TIMER****************************************************
extern void BAE_STANDBY_TIMER_RESET();
uint8_t telemetry[134];
//*****************PARA******************************
//ACS
extern float db[3];
extern float data[6];
extern float b_old[3]; // Unit: Tesla
extern float moment[3];
extern uint8_t ACS_STATE;
extern uint8_t ACS_STATUS;
extern uint8_t flag_firsttime;
extern uint8_t ACS_DETUMBLING_ALGO_TYPE;
extern uint8_t ACS_INIT_STATUS;
extern uint8_t ACS_DATA_ACQ_STATUS;
extern uint8_t ACS_MAIN_STATUS;
extern uint8_t ACS_MAG_TIME_DELAY;
extern uint8_t ACS_DEMAG_TIME_DELAY;
extern uint8_t ACS_Z_FIXED_MOMENT;
extern uint8_t ACS_TR_X_PWM;
extern uint8_t ACS_TR_Y_PWM;
extern uint8_t ACS_TR_Z_PWM;
extern uint8_t B_SCZ_ANGLE;
extern uint8_t alarmmode;
extern uint8_t controlmode_mms;
extern uint8_t invjm_mms[9];
extern uint8_t jm_mms[9];
extern uint8_t bb_mms[3];
extern uint8_t singularity_flag_mms;
extern uint8_t ACS_TR_Z_SW_STATUS;
extern uint8_t ACS_TR_XY_SW_STATUS;
extern uint8_t ATS1_EVENT_STATUS_RGTR;
extern uint8_t ATS1_SENTRAL_STATUS_RGTR;
extern uint8_t ATS1_ERROR_RGTR;
extern uint8_t ATS2_EVENT_STATUS_RGTR;
extern uint8_t ATS2_SENTRAL_STATUS_RGTR;
extern uint8_t ATS2_ERROR_RGTR;
//change
extern uint16_t ACS_MM_X_COMSN;
extern uint16_t ACS_MM_Y_COMSN;
extern uint16_t ACS_MG_X_COMSN;
extern uint16_t ACS_MG_Y_COMSN;
extern uint16_t ACS_MM_Z_COMSN;
extern uint16_t ACS_MG_Z_COMSN;
//acs func
extern void F_ACS();
extern int SENSOR_INIT();
extern int FCTN_ACS_INIT();
extern int SENSOR_DATA_ACQ();
extern int FCTN_ATS_DATA_ACQ();
extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]);
extern void FCTN_ACS_CNTRLALGO(float*,float*,int);
//main
extern Timer BAE_uptime;
extern Timer I2C_last;
extern uint8_t BAE_RESET_COUNTER;
extern void RETURN_UPTIME(float time, uint8_t *day,uint8_t *hour,uint8_t *min);
extern uint8_t ACS_ATS_STATUS;
extern uint16_t ACS_MAIN_COUNTER;//change\apply
extern uint8_t HTR_CYCLE_COUNTER;
extern RtosTimer *HTR_CYCLE;
extern uint8_t HTR_CYCLE_COUNTS; //Count of heater cycles
extern uint8_t HTR_CYCLE_START_DLY; //EPS_HTR_DLY_TIMER timer duration in minutes
extern uint8_t HTR_ON_DURATION; //EPS_HTR_OFF timer duration in minutes
extern uint16_t HTR_CYCLE_PERIOD;
extern DigitalOut ACS_TR_XY_ENABLE;
extern DigitalOut ACS_TR_Z_ENABLE;
extern DigitalIn ACS_TR_XY_OC_FAULT;
extern DigitalIn ACS_TR_Z_OC_FAULT;
extern DigitalIn ACS_TR_XY_FAULT;
extern DigitalIn ACS_TR_Z_FAULT;
extern DigitalIn ACS_ATS1_OC_FAULT;
extern DigitalIn ACS_ATS2_OC_FAULT;
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 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 phase_TR_x;
extern DigitalOut phase_TR_y;
extern DigitalOut phase_TR_z;
//CDMS
extern DigitalOut CDMS_RESET; // CDMS RESET
extern uint8_t CDMS_SW_STATUS;
extern DigitalIn CDMS_OC_FAULT;
//BCN
extern DigitalOut BCN_SW; //Beacon switch
extern uint8_t BCN_TX_STATUS;
extern uint8_t BCN_FEN;
extern uint8_t BCN_SPND_TX;
extern uint8_t BCN_TX_MAIN_STATUS;
extern uint8_t BCN_TX_SW_STATUS;
extern uint8_t BCN_LONG_MSG_TYPE;
extern uint8_t BCN_INIT_STATUS;
extern uint8_t BCN_FAIL_COUNT;
extern uint16_t BCN_TX_MAIN_COUNTER;
extern DigitalIn BCN_TX_OC_FAULT;
extern uint8_t BCN_TMP;
extern void F_BCN();
extern void FCTN_BCN_TX_MAIN();
extern uint8_t SHORT_HK_data[15];
extern void FCTN_BCN_SPND_TX();
extern void FCTN_BCN_INIT();
extern uint8_t readreg(uint8_t reg);
extern void writereg(uint8_t reg,uint8_t val);
//BAE
extern uint8_t BAE_STANDBY;
extern uint8_t BAE_INIT_STATUS;
extern uint8_t BAE_RESET_COUNTER;
extern uint8_t BAE_MNG_I2C_STATUS;
/*given a default value as of now shuld read value from flash and increment it write it back very time it starts(bae)*/
extern uint32_t BAE_STATUS;//extern uint32_t BAE_STATUS;
extern BAE_HK_quant quant_data;
extern BAE_HK_actual actual_data;
extern BAE_HK_min_max bae_HK_minmax;
//extern DigitalOut TRXY_SW;
//extern DigitalOut TRZ_SW_EN; //same as TRZ_SW
extern uint32_t BAE_ENABLE;
extern uint16_t BAE_I2C_COUNTER;
extern uint8_t LONG_HK_data[2][134];
//extern uint8_t BCN_FAIL_COUNT;
//EPS
extern bool firstCount;
extern uint8_t EPS_BTRY_HTR_AUTO;
extern uint8_t EPS_BATT_TEMP_LOW;
extern uint8_t EPS_BATT_TEMP_HIGH;
extern uint8_t EPS_BATT_TEMP_DEFAULT;
extern DigitalOut BTRY_HTR_ENABLE;
extern uint8_t EPS_SOC_LEVEL_12;
extern uint8_t EPS_SOC_LEVEL_23;
extern uint8_t EPS_INIT_STATUS;
extern uint8_t EPS_BATTERY_GAUGE_STATUS ;
extern uint8_t EPS_MAIN_STATUS;
extern uint8_t EPS_BTRY_TMP_STATUS ;
extern uint8_t EPS_STATUS ;
extern uint8_t EPS_BAT_TEMP_LOW;
extern uint8_t EPS_BAT_TEMP_HIGH;
extern uint8_t EPS_BAT_TEMP_DEFAULT;
extern uint16_t EPS_MAIN_COUNTER;
extern uint8_t EPS_BTRY_HTR;
extern DigitalOut SelectLineb3; // MSB of Select Lines
extern DigitalOut SelectLineb2;
extern DigitalOut SelectLineb1;
extern DigitalOut SelectLineb0;
extern DigitalIn EPS_CHARGER_FAULT;
extern DigitalIn EPS_CHARGER_STATUS;
extern DigitalIn EPS_BATTERY_GAUGE_ALERT;
extern void F_EPS();
extern AnalogIn CurrentInput;
//--------------------check this refer prashant
extern PwmOut PWM1; //x //Functions used to generate PWM signal
extern PwmOut PWM2; //y
extern PwmOut PWM3; //z //PWM output comes from pins p6
//included after home
//extern void FCTN_ACS_GENPWM_MAIN();
uint16_t crc_hk_data_type()//gencrc16_for_me()
{
uint16_t crc = CRC::crc16_gen(&LONG_HK_data[0][0],132);//BAE_chardata i.e char data type usesd earlier BAE_HK_data
return crc;
}
uint16_t crc_hk_data()//gencrc16_for_me()
{
uint16_t crc = CRC::crc16_gen(&LONG_HK_data[1][0],132);//BAE_chardata i.e char data type usesd earlier BAE_HK_data
return crc;
}
uint8_t crc8_short()
{
uint8_t crc = CRC::crc8_gen(SHORT_HK_data,14);
return crc;
}
float uint16_to_float(float min,float max,uint16_t scale)
{
float div=max-min;
div=(div/(65535.0));
return ((div*(float)scale)+ min);
}
uint16_t float_to_uint16(float min,float max,float val) //takes care of -ve num as its scale with min and max as reference
{
if(val>max)
{return 0xffff;
}
if(val<min)
{return 0x0000;
}
float div=max-min;
div=(65535.0/div);
val=((val-min)*div);
//gpc.printf("\n\r the scale is %x",(uint16_t)val);
return (uint16_t)val;
}
void gen_I_TM()
{
telemetry[0] = 0xB0;
for(int i=1;i<11;i++)
telemetry[i] = 0x00;
uint16_t crc = CRC::crc16_gen(telemetry,11);//BAE_chardata i.e char data type usesd earlier
telemetry[11] = (uint8_t)(crc >> 8);
telemetry[12] = (uint8_t)crc ;
for(int i=13;i<135;i++)
telemetry[i] = 0x00;
}
void FCTN__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;
//gpc.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;
//calculating crc
for(int i=0;i<134;i++)
{
telemetry[i]=tc[i];
}
/*changed*/
uint8_t* tm; // without it some identifier error
uint16_t crc16=CRC::crc16_gen(telemetry,9); //implementing crc
gpc.printf("\n\r the crc is %x",crc16);
// for(int qw=0;qw<11;qw++);
// gpc.printf("\n\r the telecommand %d \t %x",qw,tc[qw]);
// pc.printf("\n the tc provided is %s \n" , tc");
if( ( ((uint8_t)((crc16 & 0xFF00)>>8)==tc[9]) && ((uint8_t)(crc16 & 0x00FF)==tc[10]) )== 0 )
{
telemetry[0]=0xB0;
telemetry[1]=0x00;//tc psc defined for this case
telemetry[2]=0x01;//ack code for this case
for(int i=3;i<11;i++)
{
telemetry[i]=0x00;
}
//add crc
crc16 = CRC::crc16_gen(telemetry,11);
telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
telemetry[12] = (uint8_t)(crc16&0x00FF);
gpc.printf("\n\rincorrect crc");
for(int i=13;i<134;i++)
{
telemetry[i]=0x00;
}
}
else
{
//check apid
uint8_t apid_check=((tc[1]&0xC0)>>6);
if(apid_check!=0x01)
{
telemetry[0]=0xB0;
telemetry[1]=tc[0];//tc psc defined for this case
telemetry[2]=0x02;//ack code for this case
for(int 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);
gpc.printf("\n\rillegal TC packet APID don't match");
for(int i=13;i<134;i++)
{
telemetry[i]=0x00;
}
}
else
{// all the possible cases of fms and mms
uint8_t service_type=(tc[2]&0xF0);
//check for fms first
switch(service_type)
{
case 0x60:
{
gpc.printf("Memory Management Service\r\n");
uint8_t service_subtype=(tc[2]&0x0F);
switch(service_subtype)
{
case 0x02:
{
gpc.printf("\n\rRead from RAM");
uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
switch(MID)
{
case 0x0000://changed from 0001
{
gpc.printf("\n\rRead from MID 0000 \n");
gpc.printf("\n\rReading flash parameters");
/*taking some varible till we find some thing more useful*/
//uint8_t ref_val=0x01;
telemetry[0] = 0x30;
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] = 0x00;
telemetry[4] = ACS_ATS_STATUS;
telemetry[5] = ACS_TR_XY_SW_STATUS;
telemetry[5] = (telemetry[5]<<2)| ACS_TR_Z_SW_STATUS;
telemetry[5] = (telemetry[5]<<4) | ACS_STATE;
telemetry[6] = ACS_DETUMBLING_ALGO_TYPE;
telemetry[6] = (telemetry[6]<<2) | BCN_TX_SW_STATUS;
telemetry[6] = (telemetry[6]<<1) | BCN_SPND_TX;
telemetry[6] = (telemetry[6]<<1) | BCN_FEN;
telemetry[6] = (telemetry[6]<<1) | BCN_LONG_MSG_TYPE;
telemetry[6] = (telemetry[6]<<1) | EPS_BTRY_HTR_AUTO;//EPS_BATTERY_HEATER_ENABLE
telemetry[6] = (telemetry[6]<<1);
//now one spares in telemetry[6]
telemetry[7] = BAE_RESET_COUNTER;
telemetry[8] = EPS_SOC_LEVEL_12;
telemetry[9] = EPS_SOC_LEVEL_23;
telemetry[10] = ACS_MAG_TIME_DELAY;
telemetry[11] = ACS_DEMAG_TIME_DELAY;
telemetry[12] = EPS_BAT_TEMP_LOW;
telemetry[13] = EPS_BAT_TEMP_HIGH;
telemetry[14] = EPS_BAT_TEMP_DEFAULT;
telemetry[15] = ACS_MM_X_COMSN >> 8;
telemetry[16] = ACS_MM_X_COMSN;
printf("\n\r the mm x commission %d",ACS_MM_X_COMSN);
telemetry[17] = ACS_MM_Y_COMSN >> 8;
telemetry[18] = ACS_MM_Y_COMSN;
telemetry[19] = ACS_MG_X_COMSN >> 8;
telemetry[20] = ACS_MG_X_COMSN;
telemetry[21] = ACS_MG_Y_COMSN >> 8;
telemetry[22] = ACS_MG_Y_COMSN;
telemetry[23] = ACS_MM_Z_COMSN >> 8;
telemetry[24] = ACS_MM_Z_COMSN;
telemetry[25] = ACS_MG_Z_COMSN >> 8;
telemetry[26] = ACS_MG_Z_COMSN;
telemetry[27] = ACS_Z_FIXED_MOMENT >> 8;
telemetry[28] = ACS_Z_FIXED_MOMENT;
printf("\n\r the z_fixed_moment %d",ACS_Z_FIXED_MOMENT);
//BAE RAM PARAMETER
telemetry[29] = BAE_INIT_STATUS;
telemetry[29] = (telemetry[29]<<1) | BAE_MNG_I2C_STATUS;//changed
telemetry[29] = (telemetry[29]<<1) | BCN_INIT_STATUS;
telemetry[29] = (telemetry[29]<<1) | BCN_TX_MAIN_STATUS;
telemetry[29] = (telemetry[29]<<3) | BCN_TX_STATUS;
telemetry[29] = (telemetry[29]<<1) | ACS_INIT_STATUS;
telemetry[30] = ACS_DATA_ACQ_STATUS;
telemetry[30] = (telemetry[30]<<1) | ACS_MAIN_STATUS;
telemetry[30] = (telemetry[30]<<4) | ACS_STATUS;
telemetry[30] = (telemetry[30]<<1) | EPS_INIT_STATUS;
telemetry[31] = EPS_BATTERY_GAUGE_STATUS;
telemetry[31] = (telemetry[31]<<1) | EPS_MAIN_STATUS;
telemetry[31] = (telemetry[31]<<1) | EPS_BTRY_TMP_STATUS;
telemetry[31] = (telemetry[31]<<3) | EPS_STATUS;
telemetry[31] = (telemetry[31]<<2) | CDMS_SW_STATUS;
// telemetry[30] = (telemetry[30]<<1) | EPS_BTRY_HTR_STATUS;//new to : implement
telemetry[32] = EPS_BTRY_HTR; //new to : implement
//spare 4
telemetry[32] = (telemetry[32]<<7) | BAE_STANDBY;
// 6 next telemetries value to be given by registers
telemetry[33] = ATS1_EVENT_STATUS_RGTR;
telemetry[34] = ATS1_SENTRAL_STATUS_RGTR;
telemetry[35] = ATS1_ERROR_RGTR;
telemetry[36] = ATS2_EVENT_STATUS_RGTR;
telemetry[37] = ATS2_SENTRAL_STATUS_RGTR;
telemetry[38] = ATS2_ERROR_RGTR;
telemetry[39] = BCN_FAIL_COUNT;
telemetry[40] = actual_data.power_mode;
telemetry[41] = HTR_CYCLE_COUNTER;//new to : implement // starts from zero and then increases
// GS changed order has to be updated in the telemetry sheet or jithin should change the order
telemetry[43] = BAE_I2C_COUNTER;
telemetry[42] = BAE_I2C_COUNTER>>8;
telemetry[45] = ACS_MAIN_COUNTER;
telemetry[44] = ACS_MAIN_COUNTER>>8;
telemetry[47] = BCN_TX_MAIN_COUNTER;
telemetry[46] = BCN_TX_MAIN_COUNTER>>8;
telemetry[49] = EPS_MAIN_COUNTER;
telemetry[48] = EPS_MAIN_COUNTER>>8;
uint8_t days,hours,mins;
RETURN_UPTIME(BAE_uptime.read(),&days,&hours,&mins);
printf("\n\r the values bae of days,hour,min %d, %d, %d",days,hours,mins);
telemetry[50] = days;
RETURN_UPTIME(I2C_last.read(),&days,&hours,&mins);
telemetry[50] = (telemetry[50]<<3) | (hours>>2);
telemetry[51] = hours;
telemetry[51] = (telemetry[51]<<6) | mins;
printf("\n\r the values of bae uptime %f",BAE_uptime.read());
printf("\n\r the values i2c of days,hour,min %d, %d, %d",days,hours,mins);
//sending in uint can be converted back to int by direct conversion for +values
//make sure to convert baack to int for getting negative values
//algo for that done
uint16_t scaled;
scaled = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
telemetry[52] = (scaled>>8);
telemetry[53] = scaled;
scaled = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
telemetry[54] = (scaled>>8);
telemetry[55] = scaled;
scaled = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
telemetry[56] = (scaled>>8);
telemetry[57] = scaled;
scaled = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[0]);
telemetry[58] = (scaled>>8);
telemetry[59] = scaled;
scaled = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[1]);
telemetry[60] = (scaled>>8);
telemetry[61] = scaled;
scaled = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[2]);
telemetry[62] = (scaled>>8);
telemetry[63] = scaled;
telemetry[64] = (~BCN_TX_OC_FAULT);
telemetry[64] = (telemetry[64]<<1) | ACS_TR_XY_ENABLE;
telemetry[64] = (telemetry[64]<<1) | ACS_TR_Z_ENABLE;
telemetry[64] = (telemetry[64]<<1) | (~ACS_TR_XY_OC_FAULT);
telemetry[64] = (telemetry[64]<<1) | (~ACS_TR_Z_OC_FAULT);
telemetry[64] = (telemetry[64]<<1) | (~ACS_TR_XY_FAULT);
telemetry[64] = (telemetry[64]<<1) | (~EPS_CHARGER_FAULT);
telemetry[64] = (telemetry[64]<<1) | EPS_CHARGER_STATUS;
telemetry[65] = EPS_BATTERY_GAUGE_ALERT;
telemetry[65] = (telemetry[65]<<1) | (~CDMS_OC_FAULT);
telemetry[65] = (telemetry[65]<<1) | (~ACS_ATS1_OC_FAULT);
telemetry[65] = (telemetry[65]<<1) | (~ACS_ATS2_OC_FAULT);
telemetry[65] = (telemetry[65]<<1) | (~ACS_TR_Z_FAULT);
telemetry[65] = (telemetry[65]<<3);
//3 spare
telemetry[66] = ACS_TR_X_PWM;
telemetry[67] = ACS_TR_Y_PWM;
telemetry[68] = ACS_TR_Z_PWM;
//spare byte
//assigned it to counter HTR_CYCLE_COUNTER
//assign it b_scz_angle
telemetry[69] = B_SCZ_ANGLE>>4;
telemetry[69] = (telemetry[69]<<1) | alarmmode;
telemetry[69] = (telemetry[69]<<1) | controlmode_mms;
telemetry[69] = (telemetry[69]<<1) | singularity_flag_mms;
telemetry[69] = (telemetry[69]<<1);
//1 bit spare
for(int i=0;i<9;i++)
{
telemetry[70+i] = invjm_mms[i];
telemetry[81+i] = jm_mms[i];
}
for(int i=1;i<3;i++)
telemetry[79+i] = bb_mms[i];
for(int i=0;i<16;i++)
{
telemetry[90+i] = (uint8_t)(quant_data.voltage_quant[i]);//changed (uint8_t)(quant_data.voltage_quant[i]<<3)+(quant_data.voltage_quant[i]<<1); /*multiplied by 10 */
//printf("\n\r %d \t %d",i,telemetry[90+i]);
telemetry[106+i] = quant_data.current_quant[i];
/*check the working*/
}
telemetry[122] = quant_data.Batt_voltage_quant;// changed (quant_data.Batt_voltage_quant<<3)+(quant_data.Batt_voltage_quant<<1);
telemetry[123] = quant_data.BAE_temp_quant;
telemetry[124] = (uint8_t)(actual_data.Batt_gauge_actual[1]);
telemetry[125] = quant_data.Batt_temp_quant[0];
telemetry[126] = quant_data.Batt_temp_quant[1];
telemetry[127] = BCN_TMP+50;
for (int i=128; 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 0x0001:
{
gpc.printf("\r\nHK data min max");
telemetry[0] = 0x30; // changed0x06 shifted by 1 bit 0x03
telemetry[1] = tc[0];
telemetry[2] = ACK_CODE;
telemetry[3] = 0x00;
for(int i;i<16;i++)
{
telemetry[i+4] = (uint8_t)bae_HK_minmax.voltage_max[i];//(bae_HK_minmax.voltage_max[i]<<3)+(bae_HK_minmax.voltage_max[i]<<2);
telemetry[i+20] = bae_HK_minmax.current_max[i];
}
telemetry[36] = (uint8_t)bae_HK_minmax.Batt_voltage_max;//(bae_HK_minmax.Batt_voltage_max<<3)+(bae_HK_minmax.Batt_voltage_max<<2);
telemetry[37] = bae_HK_minmax.BAE_temp_max;
telemetry[38] = bae_HK_minmax.Batt_SOC_max;
telemetry[39] = bae_HK_minmax.Batt_temp_max[0];
telemetry[40] = bae_HK_minmax.Batt_temp_max[1];
/*BCN temp there*/
telemetry[41] = bae_HK_minmax.BCN_TEMP_max;
for(int i=0; i<3; i++)
{
telemetry[42+(2*i)] = (bae_HK_minmax.bit_data_acs_mm_max[i]>>8);
telemetry[43+(2*i)] = bae_HK_minmax.bit_data_acs_mm_max[i];
telemetry[48+(2*i)] = (bae_HK_minmax.bit_data_acs_mg_max[i]>>8);
telemetry[49+(2*i)] = bae_HK_minmax.bit_data_acs_mg_max[i];
}
/*min data*/
for(int i;i<16;i++)
{ telemetry[i+54] = (uint8_t)bae_HK_minmax.voltage_min[i];//(bae_HK_minmax.voltage_min[i]<<3)+(bae_HK_minmax.voltage_min[i]<<1);
//for(int i;i<16;i++)
telemetry[i+70] = bae_HK_minmax.current_min[i];
}
telemetry[86] = (uint8_t)bae_HK_minmax.Batt_voltage_min;//(bae_HK_minmax.Batt_voltage_min<<3)+(bae_HK_minmax.Batt_voltage_min<<1);
telemetry[87] = bae_HK_minmax.BAE_temp_min;
/*battery soc*/
telemetry[88] = bae_HK_minmax.Batt_SOC_min;
telemetry[89] = bae_HK_minmax.Batt_temp_min[0];
telemetry[90] = bae_HK_minmax.Batt_temp_min[1];
//huhu//
/*BCN temp named as BCN_TS_BUFFER there*/
telemetry[91] = bae_HK_minmax.BCN_TEMP_min;
for(int i=0; i<3; i++)
{
telemetry[92+(2*i)] = (bae_HK_minmax.bit_data_acs_mm_min[i]>>8);
telemetry[93+(2*i)] = bae_HK_minmax.bit_data_acs_mm_min[i];
telemetry[98+(2*i)] = (bae_HK_minmax.bit_data_acs_mg_min[i]>>8);
telemetry[99+(2*i)] = bae_HK_minmax.bit_data_acs_mg_min[i];
}
telemetry[104] = (~(BCN_TX_OC_FAULT));
telemetry[104] = (telemetry[104]<<1) | (~(ACS_TR_XY_OC_FAULT));
telemetry[104] = (telemetry[104]<<1) | (~(ACS_TR_Z_OC_FAULT));
telemetry[104] = (telemetry[104]<<1) | (~(ACS_TR_XY_FAULT));
//EPS CHARGER
telemetry[104] = (telemetry[104]<<1) | (~(EPS_CHARGER_FAULT));//eps_charger;
telemetry[104] = (telemetry[104]<<1) | (~(CDMS_OC_FAULT));
telemetry[104] = (telemetry[104]<<1) | (~(ACS_ATS1_OC_FAULT));
telemetry[104] = (telemetry[104]<<1) | (~(ACS_ATS2_OC_FAULT));
telemetry[105] = (ACS_TR_Z_FAULT<<7);
//spare 23 bits
telemetry[106] = 0x00;
telemetry[107] = 0x00;
for (int i=108; 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;
}
default://invalid MID
{
//ACK_L234_telemetry
telemetry[0]=0xB0; // check if this needs to be changed
telemetry[1]=tc[0];
telemetry[2]=0x02;//for this case
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 0x05:
{
gpc.printf("\n\rDATA for mms 0x05 flash");
/*changed*/
gpc.printf("\n\rWrite on flash\n");
uint32_t FLASH_DATA;//256 bits
uint8_t VALID_MID;//to determine wether mid is valid or not otherwise to much repetition of code 1 meaning valid
uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
switch(MID )
{
case 0x1100:
{
//FLASH_DATA[0] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[8]);
//FCTN_BAE_WR_FLASH(0,FLASH_DATA[0]);
BCN_LONG_MSG_TYPE = tc[8];
gpc.printf("\n\rReceived BCN_LMB_TYPE tc, new BCN_LONG_MSG_TYPE = %d",tc[8]);
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = (FLASH_DATA & 0xFFFFFBFF) | (10<<(uint32_t)tc[8]);//see if uint8 to uint32 conversion works
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
VALID_MID=1;
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]);
ACS_DETUMBLING_ALGO_TYPE = (tc[8] & 0x01);
//===============================================
ACS_STATE = 4;
//tc[8] = 1001_
//ACS_STATE = (tc[8]>>1) & 0x0F;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = (FLASH_DATA & 0xFFF07FFF) | (15<<(uint32_t)tc[8]);
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
VALID_MID=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]);
//EPS_BATTERY_HEATER_ENABLE = tc[8];
EPS_BTRY_HTR_AUTO = tc[8];
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = (FLASH_DATA & 0xFFFFFDFF) | (9<<(uint32_t)tc[8]);
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
VALID_MID=1;
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]);
ACS_MAG_TIME_DELAY = tc[7];
ACS_DEMAG_TIME_DELAY = tc[8];
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(1);
FLASH_DATA = (FLASH_DATA & 0xFFFF0000) | (8<<(uint32_t)tc[7]) | ((uint32_t)tc[8]);
FCTN_BAE_WR_FLASH(1,FLASH_DATA);
VALID_MID=1;
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]);
ACS_Z_FIXED_MOMENT = ((uint16_t)tc[7]<<8) | (uint16_t)tc[8];
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(6);
FLASH_DATA = (FLASH_DATA & 0x0000FFFF) | ((uint32_t)ACS_Z_FIXED_MOMENT<<16);
FCTN_BAE_WR_FLASH(6,FLASH_DATA);
VALID_MID=1;
break;
}
case 0x0105:
{
//FLASH_DATA[4] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
EPS_BAT_TEMP_DEFAULT=(uint8_t)tc[6];
EPS_BAT_TEMP_LOW=(uint8_t)tc[7];
EPS_BAT_TEMP_HIGH=(uint8_t)tc[8];
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(2);
FLASH_DATA = (FLASH_DATA & 0x000000FF) | ((uint32_t)EPS_BAT_TEMP_DEFAULT<<24) | ((uint32_t)EPS_BAT_TEMP_LOW<<16) | ((uint32_t)EPS_BAT_TEMP_LOW<<8);
FCTN_BAE_WR_FLASH(6,FLASH_DATA);
VALID_MID=1;
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]);
ACS_MM_Z_COMSN = ((uint16_t)tc[5]<<8) | (uint16_t)tc[6];
ACS_MG_Z_COMSN = ((uint16_t)tc[7]<<8) | (uint16_t)tc[8];
FLASH_DATA = ((uint32_t)ACS_MM_Z_COMSN<<16) | (uint32_t)ACS_MG_Z_COMSN;
FCTN_BAE_WR_FLASH(5,FLASH_DATA);
VALID_MID=1;
break;
}
case 0x0107:
{
//FLASH_DATA[5] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
EPS_SOC_LEVEL_12 = tc[7];
EPS_SOC_LEVEL_23 = tc[8];
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(1);
FLASH_DATA = (FLASH_DATA & 0x0000FFFF) | ((uint32_t)tc[7]<<24) | ((uint32_t)tc[8]<<16);
FCTN_BAE_WR_FLASH(1,FLASH_DATA);
VALID_MID=1;
break;
}
case 0x0108:
{
//FLASH_DATA[6] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
ACS_MM_X_COMSN = ((uint16_t)tc[5]<<8) | (uint16_t)tc[6];
ACS_MM_Y_COMSN = ((uint16_t)tc[7]<<8) | (uint16_t)tc[8];
FLASH_DATA = ((uint32_t)ACS_MM_X_COMSN<<16) | (uint32_t)ACS_MM_Y_COMSN;
FCTN_BAE_WR_FLASH(3,FLASH_DATA);
VALID_MID=1;
break;
}
case 0x0109:
{
//FLASH_DATA[7] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
ACS_MG_X_COMSN = ((uint16_t)tc[5]<<8) | (uint16_t)tc[6];
ACS_MG_Y_COMSN = ((uint16_t)tc[7]<<8) | (uint16_t)tc[8];
FLASH_DATA = ((uint32_t)ACS_MG_X_COMSN<<16) | (uint32_t)ACS_MG_Y_COMSN;
FCTN_BAE_WR_FLASH(4,FLASH_DATA);
VALID_MID=1;
break;
}
default:
{
gpc.printf("Invalid MMS case 0x05 invalid MID\r\n");
VALID_MID=0;
//ACK_L234_telemetry
break;
}
}
if(VALID_MID==1)//valid MID
{
telemetry[0]=0xB0;//or 0x60? check
telemetry[1]=tc[0];
telemetry[2]=0xA0;// when valid
}
else if(VALID_MID==0)//invalid MID
{
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=0x02;//for this case
}
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;
}
gpc.printf("\n\rWritten on Flash");
break;
}
default://when invalid service subtype
{
gpc.printf("\n\r MMS invalid Service Subtype");
//ACK_L234_telemetry
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=0x02;//for this case
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:
{
gpc.printf("Function Management Service\r\n");//changed
uint8_t service_subtype=(tc[2]&0x0F);
switch(service_subtype)
{
case 0x01:
{
gpc.printf("\n\rFMS Activated");
uint8_t fid=tc[3];//changed from pid to fid
switch(fid)
{
case 0xE0:
{
//float mag_data_comm[3]={uint16_to_float(-1000,1000,ACS_MM_X_COMSN),uint16_to_float(-1000,1000,ACS_MM_Y_COMSN),uint16_to_float(-1000,1000,ACS_MM_Z_COMSN)};
//float gyro_data_comm[3]={uint16_to_float(-5000,5000,ACS_MG_X_COMSN),uint16_to_float(-5000,5000,ACS_MG_Y_COMSN),uint16_to_float(-5000,5000,ACS_MG_Z_COMSN)};
float mag_data_comm[3] = {ACS_MM_X_COMSN,ACS_MM_Y_COMSN,ACS_MM_Z_COMSN};
float gyro_data_comm[3] = {ACS_MG_X_COMSN,ACS_MG_Y_COMSN,ACS_MG_Z_COMSN};
float moment_comm[3];
gpc.printf("ACS_COMSN SOFTWARE\r\n");
//ACK_L234_telemetry
ACS_STATE = tc[4];
if(ACS_STATE == 2) // Nominal mode
{
gpc.printf("\n\r Nominal mode \n");
FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE);
gpc.printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
gpc.printf("%f\t",moment_comm[i]);
}
}
else if(ACS_STATE == 3) // Auto Control
{
gpc.printf("\n\r Auto control mode \n");
FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE);
gpc.printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
gpc.printf("%f\t",moment_comm[i]);
}
}
else if(ACS_STATE == 4) // Detumbling
{
FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
gpc.printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
gpc.printf("%f\t",moment_comm[i]);
}
}
else
{
ACS_STATUS = 7;
}
// Control algo commissioning
uint16_t moment_ret;
telemetry[0] = 0x78;
telemetry[1] = tc[0];
telemetry[2] = ACK_CODE;
telemetry[3] = 0x00;
telemetry[4] = ACS_STATUS;
moment_ret = float_to_uint16(-2.2,2.2,moment_comm[0]);
telemetry[5] = (uint8_t)(moment_ret>>8);
telemetry[6] = (uint8_t)moment_ret;
moment_ret = float_to_uint16(-2.2,2.2,moment_comm[0]);
telemetry[7] = (uint8_t)(moment_ret>>8);
telemetry[8] = (uint8_t)moment_ret;
moment_ret = float_to_uint16(-2.2,2.2,moment_comm[2]);
telemetry[9] = (uint8_t)(moment_ret>>8);
telemetry[10] = (uint8_t)moment_ret;
//FCTN_CONVERT_FLOAT(moment_comm[0],&telemetry[4]); //telemetry[4] - telemetry[7]
//FCTN_CONVERT_FLOAT(moment_comm[1],&telemetry[8]); //telemetry[8] - telemetry[11]
//FCTN_CONVERT_FLOAT(moment_comm[2],&telemetry[12]); //telemetry[12] - telemetry[15]
// to include commission TR as well
for(uint8_t i=11;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 0xE1:
{
gpc.printf("HARDWARE_COMSN\r\n");
//ACK_L234_telemetry
uint8_t SENSOR_NO;
int init1=0;
int init2=0;
int data1=0;
int data2=0;
uint16_t assign_val;
float PWM_tc[3];
PWM_tc[0]=(float(tc[4]))/256;
PWM_tc[1]=(float(tc[5]))/256;
PWM_tc[2]=(float(tc[6]))/256;
DRV_Z_EN = 1;
DRV_XY_EN = 1;
telemetry[0]=0x78;
telemetry[1]=tc[0];
telemetry[2]=ACK_CODE;
telemetry[3] = 0x00;
SENSOR_NO = 0;
PWM1 = 0;
PWM2 = 0;
PWM3 = 0;
wait_ms(ACS_DEMAG_TIME_DELAY);
ATS2_SW_ENABLE = 1;
wait_ms(5);
ATS1_SW_ENABLE = 1;
wait_ms(5);
//will it lead to causing delay in i2c interrupt
init1 = SENSOR_INIT();
if( init1 == 1)
{
data1 = SENSOR_DATA_ACQ();
}
ATS1_SW_ENABLE = 0;
wait_ms(5);
ATS2_SW_ENABLE = 0;
wait_ms(5);
if(data1 == 0)
{
ATS2_SW_ENABLE = 0;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
}
else if(data1==1)
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
}
else if(data1==2)
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
}
else if(data1==3)
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x30;
}
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
telemetry[5] = (assign_val>>8);
telemetry[6] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
telemetry[7] = (assign_val>>8);
telemetry[8] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
telemetry[9] = (assign_val>>8);
telemetry[10] = assign_val;
assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[0]);
telemetry[11] = (assign_val>>8);
telemetry[12] = assign_val;
assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[1]);
telemetry[13] = (assign_val>>8);
telemetry[14] = assign_val;
assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[2]);
telemetry[15] = (assign_val>>8);
telemetry[16] = assign_val;
init2 = SENSOR_INIT();
if( init2 == 1)
{
data2 = SENSOR_DATA_ACQ();
}
//uint8_t ats_data=1;
if(data2 == 0)
{
ATS2_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
SENSOR_NO = 0;
if(data1 == 2)
{
ATS1_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
SENSOR_NO = 1;
}
else if(data1 == 3)
{
ATS1_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
SENSOR_NO = 1;
}
else if(data1 == 1)
{
ATS1_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
SENSOR_NO = 0;
}
}
else if(data2==1)
{
if(data1 == 2)
{
ATS2_SW_ENABLE = 1;
wait_ms(5);
ATS1_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
SENSOR_NO = 1;
}
else if(data1 == 3)
{
ATS2_SW_ENABLE = 1;
wait_ms(5);
ATS1_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
SENSOR_NO = 1;
}
else
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
SENSOR_NO = 0;
//ats_data = 0;
}
}
else if(data2==2)
{
if(data1 == 3)
{
ATS2_SW_ENABLE = 1;
wait_ms(5);
ATS1_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
SENSOR_NO = 1;
}
else
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
SENSOR_NO = 2;
}
}
else if(data2==3)
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
SENSOR_NO = 2;
}
SENSOR_NO = 1;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
telemetry[17] = (assign_val>>8);
telemetry[18] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
telemetry[19] = (assign_val>>8);
telemetry[20] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
telemetry[21] = (assign_val>>8);
telemetry[22] = assign_val;
assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[0]);
telemetry[23] = (assign_val>>8);
telemetry[24] = assign_val;
assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[1]);
telemetry[25] = (assign_val>>8);
telemetry[26] = assign_val;
assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[2]);
telemetry[27] = (assign_val>>8);
telemetry[28] = assign_val;
telemetry[4] = ACS_ATS_STATUS;
SelectLineb3 =0;
SelectLineb2 =1;
SelectLineb1 =0;
SelectLineb0 =1;
int resistance;
PWM1 = PWM_tc[0];
PWM2 = 0;
PWM3 = 0;
wait_ms(ACS_DEMAG_TIME_DELAY);
if(SENSOR_NO != 0)
SENSOR_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);
}
//to be edited final tele
//uint16_t assign_val;
assign_val = float_to_uint16(-100,100,actual_data.current_actual[5]);//assuming max min values for current to be diss
telemetry[29] = (assign_val>>8);
telemetry[30] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
telemetry[31] = (assign_val>>8);
telemetry[32] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
telemetry[33] = (assign_val>>8);
telemetry[34] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
telemetry[35] = (assign_val>>8);
telemetry[36] = assign_val;
PWM1 = 0;
PWM2 = PWM_tc[1];
PWM3 = 0;
wait_ms(ACS_DEMAG_TIME_DELAY);
if(SENSOR_NO != 0)
SENSOR_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);
}
assign_val = float_to_uint16(-100,100,actual_data.current_actual[5]);//assuming max min values for current to be diss
telemetry[37] = (assign_val>>8);
telemetry[38] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
telemetry[39] = (assign_val>>8);
telemetry[40] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
telemetry[41] = (assign_val>>8);
telemetry[42] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
telemetry[43] = (assign_val>>8);
telemetry[44] = assign_val;
PWM1 = 0;
PWM2 = 0;
PWM3 = PWM_tc[2];
wait_ms(ACS_DEMAG_TIME_DELAY);
if(SENSOR_NO != 0)
SENSOR_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);
}
assign_val = float_to_uint16(-100,100,actual_data.current_actual[5]);//assuming max min values for current to be diss
telemetry[45] = (assign_val>>8);
telemetry[46] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
telemetry[47] = (assign_val>>8);
telemetry[48] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
telemetry[49] = (assign_val>>8);
telemetry[50] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
telemetry[51] = (assign_val>>8);
telemetry[52] = assign_val;
PWM1 = 0;
PWM2 = 0;
PWM3 = 0;
wait_ms(ACS_DEMAG_TIME_DELAY);
if(SENSOR_NO != 0)
SENSOR_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);
}
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
telemetry[53] = (assign_val>>8);
telemetry[54] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
telemetry[55] = (assign_val>>8);
telemetry[56] = assign_val;
assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
telemetry[57] = (assign_val>>8);
telemetry[58] = assign_val;
// 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
telemetry[59] = SENSOR_NO;
for(uint8_t i=60;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);
if(data1 == 0)
{
ATS1_SW_ENABLE = 0;
//wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
}
else if(data1==1)
{
ATS1_SW_ENABLE = 1;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40;
}
else if(data1==2)
{
ATS1_SW_ENABLE = 1;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
}
else if(data1==3)
{
ATS1_SW_ENABLE = 1;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
}
printf("Exited HW\r\n");
break;
}
case 0xE2:
{ //BCN_SW active high - samp
uint8_t BCN_SPND_STATE;
BCN_SPND_STATE=tc[4];
if(BCN_SPND_STATE==0x00)// to switch on bcn
{
BCN_SPND_TX=0;
if( BCN_TX_SW_STATUS != 3 )
{
BCN_SW = 1;
BCN_TX_SW_STATUS = 1;
}
//stop BCN_STANDBY_TIMER.stop();//create
telemetry[2]=0xA0;
}
else if(BCN_SPND_STATE==0x01)
{
FCTN_BCN_SPND_TX();//direct implementation of function
/*{
//printf("BCN_SPND\n\r");
BCN_SPND_TX = 1;
if( BCN_TX_SW_STATUS != 3 )
{
writereg(RF22_REG_07_OPERATING_MODE1,0x00); //standby mode
//if( readreg(RF22_REG_07_OPERATING_MODE1) & 0x08 == 0x08 )
{
BCN_SW = 0; //active high
BCN_TX_SW_STATUS = 0; //working but powered off
uint32_t FLASH_DATA;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); //updated in flash
FLASH_DATA = (FLASH_DATA | 0x00006000);
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
}
}*/
//}
//BCN_SW =1;
//stop BCN_STANDBY_TIMER.start();//create
if(BCN_TX_MAIN_STATUS==0)
{
telemetry[2]=0xA0;
}
else if(BCN_TX_MAIN_STATUS==1)
{
telemetry[2]=0xA0; //changed
}
}
else
{
telemetry[2]=0x02;
}
//ACK_L234_telemetry
telemetry[0]=0xB0;
telemetry[1]=tc[0];
//ack_code taken care above
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 0xE3:
{
gpc.printf("\n\n\rE3 batter heater\n");
if(EPS_BTRY_HTR_AUTO != 0)
telemetry[2]=0x87;
else
{
HTR_CYCLE_COUNTS = tc[4];
if(HTR_CYCLE_COUNTS==0x00)
{
BTRY_HTR_ENABLE = 0;
HTR_CYCLE->stop();
//clear EPS_BTRY_HTR is it
EPS_BTRY_HTR_AUTO = 0;
gpc.printf("\n\n\rheater OFF\n");
}
else
{
gpc.printf("\n\n\rheater ON\n");
if(HTR_CYCLE_COUNTS != 0xFF)
{
HTR_CYCLE_COUNTER = 0;
}
//uint8_t HTR_CYCLE_START_DLY = tc[5];
HTR_CYCLE_START_DLY = tc[5];
HTR_ON_DURATION = tc[6];
//make it uint16_t
HTR_CYCLE_PERIOD = (tc[7]<<8) | tc[8];
//start BTRY_HTR_DLY_TIMER;
gpc.printf("\n\n\cycle_start_dly:%d on_duration:%d: cycle_period:%d \n",HTR_CYCLE_START_DLY,HTR_ON_DURATION,HTR_CYCLE_PERIOD);
}
telemetry[2]=0xA0;
}
//ACK_L234_telemetry
telemetry[0]=0xB0;
telemetry[1]=tc[0];
//ACK code taken care of
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 0x01:
{ if(BAE_STANDBY==0x00)
{
gpc.printf("\n\n\rRun P_EPS_INIT\n");
FCTN_EPS_INIT();
telemetry[2]=ACK_CODE;
}
else
{
gpc.printf("\n\runable to Run P_EPS_INIT as BAE_STATUS not 000 ");;
telemetry[2]=0x87;
}
//ACK_L234_telemetry
telemetry[0]=0xB0;
telemetry[1]=tc[0];
//ACK code taken care of
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 0x02:
{
if(BAE_STANDBY==0x00)
{
gpc.printf("\n\n\rRun P_EPS_MAIN\n");
F_EPS();
telemetry[2]=ACK_CODE;
}
else
{
gpc.printf("\n\runable to Run P_EPS_MAIN as BAE_STATUS not 000 ");;
telemetry[2]=0x87;
}
//ACK_L234_telemetry
telemetry[0]=0xB0;
telemetry[1]=tc[0];
//ACK code taken care of
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 0x03:
{
if(BAE_STANDBY==0x00)
{
gpc.printf("\n\n\rRun P_ACS_INIT\n");
FCTN_ACS_INIT();
telemetry[2]=ACK_CODE;
}
else
{
gpc.printf("\n\runable to Run P_ACS_INIT as BAE_STATUS not 000 ");;
telemetry[2]=0x87;
}
//ACK_L234_telemetry
telemetry[0]=0xB0;
telemetry[1]=tc[0];
//ACK CODE TAKEN CARE OF
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:
{
if(BAE_STANDBY==0x00)
{
gpc.printf("\n\n\rRun P_ACS_MAIN\n");
F_ACS();
telemetry[2]=ACK_CODE;
}
else
{
gpc.printf("\n\runable to Run P_ACS_MAIN as BAE_STATUS not 000 ");;
telemetry[2]=0x87;
}
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
//ACK CODE TAKEN CARE OF
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 0x06:
{
if(BAE_STANDBY==0x00)
{
gpc.printf("\n\n\rRun P_BCN_INIT\n");
// F_BCN(); why was this written?
FCTN_BCN_INIT(); // does this makes sense??
telemetry[2]=ACK_CODE;
}
else
{
gpc.printf("\n\runable to Run P_BCN_INIT as BAE_STATUS not 000 ");;
telemetry[2]=0x87;
}
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
//ACK CODE TAKEN CARE OF
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 0x07:
{
if(BAE_STANDBY==0x00)
{
gpc.printf("\n\n\rRun P_BCN_TX_MAIN\n");
FCTN_BCN_TX_MAIN();//correct function check once
telemetry[2]=ACK_CODE;
}
else
{
gpc.printf("\n\runable to Run P_BCN_TX_MAIN as BAE_STATUS not 000 ");;
telemetry[2]=0x87;
}
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
//ACK CODE TAKEN CARE OF
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:
{
gpc.printf("\n\n\rSW_ON_ACS_ATS1_SW_ENABLE\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
//____________________________________________************************************
/*
ATS PIN OR STATUS YET TO BE DECIDED. DECIDED THAT IT IS PIN TC CAN SWITCH ON/OFF THE SENSOR
*/
ATS2_SW_ENABLE = 0; // making sure we switch off the other
//ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3) | 0x0C ;
ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF0) | 0x0C ;
ATS1_SW_ENABLE = 1; // what should the signal be for unabling 0 or 1
//ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F);
ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x0F);// state now 00|0x40; //changed device status
telemetry[2]=ACK_CODE;
/*update in flash here*/
uint32_t FLASH_DATA;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = (FLASH_DATA & 0x3FFFFFFF);
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
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:
{
gpc.printf("\n\rSW_ON_ACS_ATS2_SW_ENABLE");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS1_SW_ENABLE = 0; //make sure u switch off the other
ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x0F) | 0xC0 ;
ATS2_SW_ENABLE = 1;
ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF0);//|0x04; changed here
/*update in flash here*/
uint32_t FLASH_DATA;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = (FLASH_DATA & 0xF3FFFFFF);
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
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 0x13:
{
gpc.printf("\n\n\rSW_ON_ACS_TR_XY_ENABLE\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
DRV_XY_EN = 1;//1 SWITCH enable here
ACS_TR_XY_ENABLE = 1;
ACS_TR_XY_SW_STATUS=0x01;
/*update in flash here*/
uint32_t FLASH_DATA;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = ((FLASH_DATA & 0xFF3FFFFF) | 0x00400000 ) ;
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
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 0x14:
{
gpc.printf("\n\n\rSW_ON_ACS_TR_Z_ENABLE\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
DRV_Z_EN = 1;
ACS_TR_Z_ENABLE = 1;
ACS_TR_Z_SW_STATUS=0x01;
/*update in flash here*/
uint32_t FLASH_DATA;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = ((FLASH_DATA & 0xFFCFFFFF) | 0x00100000 ) ;
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
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 0x15:
{
gpc.printf("\n\n\rSW_ON_BCN_TX_SW_ENABLE\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
BCN_SW = 1;//here 1 is switch enable 5th dec
BCN_TX_SW_STATUS=0x03;//BCN_TX_SW_ENABLE CHANGED TO BCN_TX_SW_STATUS // changed to 3:: 4th march
/*update in flash here*/
uint32_t FLASH_DATA;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = ((FLASH_DATA & 0xFFFF9FFF) | 0x00002000 ) ;
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
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 0x21:
{
gpc.printf("\n\n\rSW_OFF_ACS_ATS1_SW_ENABLE\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS1_SW_ENABLE = 0;
ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F) | 0xC0 ;
telemetry[2]=ACK_CODE;
/*update in flaash here*/
uint32_t FLASH_DATA;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = (FLASH_DATA | 0xC0000000 ) ;
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
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:
{
gpc.printf("\n\rSW_OFF_ACS_ATS2_SW_ENABLE");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS2_SW_ENABLE = 0;
uint32_t FLASH_DATA;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = (FLASH_DATA | 0x0C000000);
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3) | 0x0C ;
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 0x23:
{
gpc.printf("\n\n\rSW_OFF_ACS_TR_XY_ENABLE\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ACS_TR_XY_ENABLE = 0;
ACS_TR_XY_SW_STATUS=0x03;
/*update in flash here*/
uint32_t FLASH_DATA;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = (FLASH_DATA | 0x00C00000 ) ;
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
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 0x24:
{
gpc.printf("\n\n\rSW_OFF_ACS_TR_Z_ENABLE\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ACS_TR_Z_ENABLE = 0;
ACS_TR_Z_SW_STATUS=0x03;
/*update in flash here*/
uint32_t FLASH_DATA;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = (FLASH_DATA | 0x00300000 ) ;
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
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 0x25:
{
gpc.printf("\n\n\rSW_OFF_BCN_TX_SW_ENABLE\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
BCN_SW = 0;// same convention active high 5th dec
BCN_TX_SW_STATUS=0x03; /// changed
/*update in the flash*/
uint32_t FLASH_DATA;
FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
FLASH_DATA = (FLASH_DATA | 0x00006000 ) ;
FCTN_BAE_WR_FLASH(0,FLASH_DATA);
telemetry[2]=0xA0;//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 0x31:
{
gpc.printf("\n\n\rACS_ATS1_SW_RESET\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS2_SW_ENABLE = 1;//as ats switched off
ATS1_SW_ENABLE = 0;
wait_ms(5);
ATS1_SW_ENABLE = 1;
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 0x32:
{
gpc.printf("\n\n\rACS_ATS2_SW_RESET\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS1_SW_ENABLE = 0;//as ats1 switched off
ATS2_SW_ENABLE = 1;
wait_ms(5);
ATS2_SW_ENABLE = 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 0x33:
{
gpc.printf("\n\n\rACS_TR_XY_SW_RESET\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ACS_TR_XY_ENABLE= 0;
wait_ms(5);
ACS_TR_XY_ENABLE= 1;
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 0x34:
{
gpc.printf("\n\n\rACS_TR_Z_SW_RESET\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ACS_TR_Z_ENABLE= 0;
wait_ms(5);
ACS_TR_Z_ENABLE= 1;
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 0x35:
{
gpc.printf("\n\n\rBCN_TX_SW_RESET\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
BCN_SW = 0; //active high
wait_ms(5);
BCN_SW = 1;
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 0x36:
{
gpc.printf("\n\n\rBAE_INTERNAL_RESET TO be done \n");
NVIC_SystemReset();
break;
}
case 0x37:
{
gpc.printf("\n\n\rCDMS_RESET\n");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
CDMS_RESET = 0;
wait_ms(5);
CDMS_RESET = 1;
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 0x38://not in IF present in SBC
{
gpc.printf("\n\rCDMS_SW_RESET pin yet to be decided");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
/*
PIN to be DECIDED***************************************************************8
*/
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 0x40://condition changed by AK47 now 00 means off in TC
{
printf("\n\n\r EN_PRCS\n");
uint8_t STANDBY_DATA_TC;
BAE_STANDBY=0x07;
STANDBY_DATA_TC=tc[4];
if(STANDBY_DATA_TC==0x00)
{
BAE_STANDBY &=0x03;
//$ L BAE_STANDBY_TIMER_RESET();
//BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
}
STANDBY_DATA_TC=tc[5];
if(STANDBY_DATA_TC==0x00)
{
BAE_STANDBY &=0x05;
//$ L BAE_STANDBY_TIMER_RESET();
//BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
}
STANDBY_DATA_TC=tc[6];
if(STANDBY_DATA_TC==0x00)
{
BAE_STANDBY &=0x06;
//$ L BAE_STANDBY_TIMER_RESET();
//BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
}
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=0xC0;//ack_code for this case
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 0x41:
{
gpc.printf("\n\n\rexecutng BAE reset HK counter\n");
firstCount=true;
void minMaxHkData();
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=0x02;
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:
{
gpc.printf("\n\rInvalid TC for FMS no matching FID");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=0x02;
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:
{
gpc.printf("\n\rInvalid TC, FMS service subtype mismacth");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=0x02;
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:
{
gpc.printf("\n\rInvalid TC neither FMS nor MMS");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=0x02;
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)
/*corrected*/
int *nativeflash = (int*)strt_add;
/*Writing to the Flash*/
void FCTN_BAE_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,32);
}
/*End*/
/*Reading from Flash*/
/*return choice parameter included so that i f we want the whole 32 packet data to be sent back we can do so*/
uint32_t FCTN_BAE_RD_FLASH_ENTITY(uint16_t entity)
{
for(int i=0;i<8;i++)
{
flasharray[i]=nativeflash[i];
}
return flasharray[entity];
}
uint32_t* FCTN_BAE_RD_FLASH()
{
for(int i=0;i<8;i++)
{
flasharray[i]=nativeflash[i];
}
return flasharray;
}
/*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);
//gpc.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
// gpc.printf("\n\rthe values generated are\n");
/*gpc.printf("\n\r%x\n",output[0]);
gpc.printf("\n\r%x\n",output[1]);
gpc.printf("\n\r%x\n",output[2]);
gpc.printf("\n\r%x\n",output[3]);
to check the values generated
*/
//gpc.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;
}