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:
- lakshya
- Date:
- 2016-07-04
- Revision:
- 33:76f2b8735501
- Parent:
- 27:61c856be467e
- Child:
- 34:1b41c34b12ea
- Child:
- 39:670133e7ffd8
File content as of revision 33:76f2b8735501:
#include "mbed.h"
#include "rtos.h"
#include "TCTM.h"
#include "crc.h"
#include "EPS.h"
#include "ACS.h"
#include "pin_config.h"
#include "FreescaleIAP.h"
#include "inttypes.h"
#include "iostream"
#include "stdint.h"
#include "cassert"
#include"math.h"
//**********************************STATUS_PARAMETERS*****************************************************
uint8_t BCN_TX_SW_ENABLE=0x00;
//***********************************FOR STANDBY TIMER****************************************************
extern void BAE_STANDBY_TIMER_RESET();
uint8_t telemetry[135];
extern uint8_t BAE_HK_data[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 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 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 DigitalOut ACS_TR_XY_OC_FAULT;
extern DigitalOut ACS_TR_Z_OC_FAULT;
extern DigitalOut ACS_TR_XY_FAULT;
extern DigitalOut ACS_TR_Z_FAULT;
extern DigitalOut ACS_ATS1_OC_FAULT;
extern DigitalOut 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 DigitalOut 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 DigitalOut 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();
//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 DigitalOut EPS_CHARGER_FAULT;
extern DigitalOut EPS_CHARGER_STATUS;
extern DigitalOut 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()//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);
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;
//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];
}
/*chaged*/
uint8_t* tm; // without it some identifier error
uint16_t crc16=CRC::crc16_gen(telemetry,9); //implementing crc
printf("\n\r the crc is %x",crc16);
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);
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);
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:
{
printf("Memory Management Service\r\n");
uint8_t service_subtype=(tc[2]&0x0F);
switch(service_subtype)
{
case 0x02:
{
printf("\n\rRead from RAM");
uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
switch(MID)
{
case 0x0000://changed from 0001
{
printf("\n\rRead from MID 0000 \n");
printf("\n\rReading flash parameters");
/*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] = ACS_ATS_STATUS;
telemetry[4] = ACS_TR_XY_SW_STATUS;
telemetry[4] = (telemetry[4]<<2)| ACS_TR_Z_SW_STATUS;
telemetry[4] = (telemetry[4]<<4) | ACS_STATE;
telemetry[5] = ACS_DETUMBLING_ALGO_TYPE;
telemetry[5] = (telemetry[5]<<2) | BCN_TX_SW_STATUS;
telemetry[5] = (telemetry[5]<<1) | BCN_SPND_TX;
telemetry[5] = (telemetry[5]<<1) | BCN_FEN;
telemetry[5] = (telemetry[5]<<1) | BCN_LONG_MSG_TYPE;
telemetry[5] = (telemetry[5]<<1) | EPS_BTRY_HTR_AUTO;//EPS_BATTERY_HEATER_ENABLE
telemetry[5] = (telemetry[5]<<1);
//now one spares in telemetry[5]
telemetry[6] = BAE_RESET_COUNTER;
telemetry[7] = EPS_SOC_LEVEL_12;
telemetry[8] = EPS_SOC_LEVEL_23;
telemetry[9] = ACS_MAG_TIME_DELAY;
telemetry[10] = ACS_DEMAG_TIME_DELAY;
telemetry[11] = EPS_BAT_TEMP_LOW;
telemetry[12] = EPS_BAT_TEMP_HIGH;
telemetry[13] = EPS_BAT_TEMP_DEFAULT;
telemetry[14] = ACS_MM_X_COMSN >> 8;
telemetry[15] = ACS_MM_X_COMSN;
telemetry[16] = ACS_MM_Y_COMSN >> 8;
telemetry[17] = ACS_MM_Y_COMSN;
telemetry[18] = ACS_MG_X_COMSN >> 8;
telemetry[19] = ACS_MG_X_COMSN;
telemetry[20] = ACS_MG_Y_COMSN >> 8;
telemetry[21] = ACS_MG_Y_COMSN;
telemetry[22] = ACS_MM_Z_COMSN >> 8;
telemetry[23] = ACS_MM_Z_COMSN;
telemetry[24] = ACS_MG_Z_COMSN >> 8;
telemetry[25] = ACS_MG_Z_COMSN;
telemetry[26] = ACS_Z_FIXED_MOMENT >> 8;
telemetry[27] = ACS_Z_FIXED_MOMENT;
//BAE RAM PARAMETER
telemetry[28] = BAE_INIT_STATUS;
telemetry[28] = (telemetry[28]<<1) | BAE_MNG_I2C_STATUS;//changed
telemetry[28] = (telemetry[28]<<1) | BCN_INIT_STATUS;
telemetry[28] = (telemetry[28]<<1) | BCN_TX_MAIN_STATUS;
telemetry[28] = (telemetry[28]<<3) | BCN_TX_STATUS;
telemetry[28] = (telemetry[28]<<1) | ACS_INIT_STATUS;
telemetry[29] = ACS_DATA_ACQ_STATUS;
telemetry[29] = (telemetry[29]<<1) | ACS_MAIN_STATUS;
telemetry[29] = (telemetry[29]<<4) | ACS_STATUS;
telemetry[29] = (telemetry[29]<<1) | EPS_INIT_STATUS;
telemetry[30] = EPS_BATTERY_GAUGE_STATUS;
telemetry[30] = (telemetry[30]<<1) | EPS_MAIN_STATUS;
telemetry[30] = (telemetry[30]<<1) | EPS_BTRY_TMP_STATUS;
telemetry[30] = (telemetry[30]<<3) | EPS_STATUS;
telemetry[30] = (telemetry[30]<<2) | CDMS_SW_STATUS;
// telemetry[30] = (telemetry[30]<<1) | EPS_BTRY_HTR_STATUS;//new to : implement
telemetry[31] = EPS_BTRY_HTR; //new to : implement
//spare 4
telemetry[31] = (telemetry[31]<<7) | BAE_STANDBY;
// 6 next telemetries value to be given by registers
telemetry[32] = ATS1_EVENT_STATUS_RGTR;
telemetry[33] = ATS1_SENTRAL_STATUS_RGTR;
telemetry[34] = ATS1_ERROR_RGTR;
telemetry[35] = ATS2_EVENT_STATUS_RGTR;
telemetry[36] = ATS2_SENTRAL_STATUS_RGTR;
telemetry[37] = ATS2_ERROR_RGTR;
telemetry[38] = BCN_FAIL_COUNT;
telemetry[39] = actual_data.power_mode;
telemetry[40] = HTR_CYCLE_COUNTER;//new to : implement
telemetry[41] = BAE_I2C_COUNTER;
telemetry[42] = BAE_I2C_COUNTER>>8;
telemetry[43] = ACS_MAIN_COUNTER;
telemetry[44] = ACS_MAIN_COUNTER>>8;
telemetry[45] = BCN_TX_MAIN_COUNTER;
telemetry[46] = BCN_TX_MAIN_COUNTER>>8;
telemetry[47] = EPS_MAIN_COUNTER;
telemetry[48] = EPS_MAIN_COUNTER>>8;
//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
telemetry[49] = actual_data.bit_data_acs_mm[0];
telemetry[50] = actual_data.bit_data_acs_mm[0]>>8;
telemetry[51] = actual_data.bit_data_acs_mm[1];
telemetry[52] = actual_data.bit_data_acs_mm[1]>>8;
telemetry[53] = actual_data.bit_data_acs_mm[2];
telemetry[54] = actual_data.bit_data_acs_mm[2]>>8;
telemetry[55] = actual_data.bit_data_acs_mg[0];
telemetry[56] = actual_data.bit_data_acs_mg[0]>>8;
telemetry[57] = actual_data.bit_data_acs_mg[1];
telemetry[58] = actual_data.bit_data_acs_mg[1]>>8;
telemetry[59] = actual_data.bit_data_acs_mg[2];
telemetry[60] = actual_data.bit_data_acs_mg[2]>>8;
telemetry[61] = BCN_TX_OC_FAULT;
telemetry[61] = (telemetry[61]<<1) | ACS_TR_XY_ENABLE;
telemetry[61] = (telemetry[61]<<1) | ACS_TR_Z_ENABLE;
telemetry[61] = (telemetry[61]<<1) | ACS_TR_XY_OC_FAULT;
telemetry[61] = (telemetry[61]<<1) | ACS_TR_Z_OC_FAULT;
telemetry[61] = (telemetry[61]<<1) | ACS_TR_XY_FAULT;
telemetry[61] = (telemetry[61]<<1) | EPS_CHARGER_FAULT;
telemetry[61] = (telemetry[61]<<1) | EPS_CHARGER_STATUS;
telemetry[62] = EPS_BATTERY_GAUGE_ALERT;
telemetry[62] = (telemetry[62]<<1) | CDMS_OC_FAULT;
telemetry[62] = (telemetry[62]<<1) | ACS_ATS1_OC_FAULT;
telemetry[62] = (telemetry[62]<<1) | ACS_ATS2_OC_FAULT;
telemetry[62] = (telemetry[62]<<1) | ACS_TR_Z_FAULT;
telemetry[62] = (telemetry[62]<<3);
//3 spare
telemetry[63] = ACS_TR_X_PWM;
telemetry[64] = ACS_TR_Y_PWM;
telemetry[65] = ACS_TR_Z_PWM;
//spare byte
//assigned it to counter HTR_CYCLE_COUNTER
//assign it b_scz_angle
telemetry[66] = 0x00;
telemetry[66] = (telemetry[65]<<1) | alarmmode;
telemetry[66] = (telemetry[65]<<1) | controlmode_mms;
telemetry[66] = (telemetry[65]<<2);
//2 bit spare
for(int i=0;i<9;i++)
{
telemetry[67+i] = invjm_mms[i];
telemetry[80+i] = jm_mms[i];
}
for(int i=0;i<3;i++)
telemetry[76+i] = bb_mms[i];
telemetry[79] = singularity_flag_mms;
for(int i=0;i<16;i++)
{
telemetry[89+i] = quant_data.voltage_quant[i];
telemetry[105+i] = quant_data.current_quant[i];
}
telemetry[121] = quant_data.Batt_voltage_quant;
telemetry[122] = quant_data.BAE_temp_quant;
telemetry[123] = (uint8_t)(actual_data.Batt_gauge_actual[1]);
telemetry[124] = quant_data.Batt_temp_quant[0];
telemetry[125] = quant_data.Batt_temp_quant[1];
telemetry[126] = BCN_TMP;
//* ANY USE?
///if(BCN_TX_STATUS==2)
/// telemetry[3] = telemetry[3]|0x20;
///else
///telemetry[3] = telemetry[3] & 0xDF;
//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)]);
///
for (int i=127; 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:
{
printf("\r\nhk data tm");
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];
telemetry[i+19] = bae_HK_minmax.current_max[i];
}
telemetry[35] = bae_HK_minmax.Batt_voltage_max;;
telemetry[36] = bae_HK_minmax.BAE_temp_max;
telemetry[37] = bae_HK_minmax.Batt_SOC_max;
telemetry[38] = bae_HK_minmax.Batt_temp_max[0];
telemetry[39] = bae_HK_minmax.Batt_temp_max[1];
/*BCN temp there*/
telemetry[40] = bae_HK_minmax.BCN_TEMP_max;
for(int i=0; i<3; i++)
{
telemetry[41+i] = bae_HK_minmax.bit_data_acs_mm_max[i];
telemetry[44+i] = bae_HK_minmax.bit_data_acs_mg_max[i];
}
/*min data*/
for(int i;i<16;i++)
telemetry[i+47] = bae_HK_minmax.voltage_min[i];
for(int i;i<16;i++)
telemetry[i+63] = bae_HK_minmax.current_min[i];
telemetry[79] = bae_HK_minmax.Batt_voltage_min;
telemetry[80] = bae_HK_minmax.BAE_temp_min;
/*battery soc*/
telemetry[81] = bae_HK_minmax.Batt_SOC_min;
telemetry[82] = bae_HK_minmax.Batt_temp_min[0];
telemetry[83] = bae_HK_minmax.Batt_temp_min[1];
//huhu//
/*BCN temp named as BCN_TS_BUFFER there*/
telemetry[84] = bae_HK_minmax.BCN_TEMP_min;
for(int i=0; i<3; i++)
{
telemetry[85+i] = bae_HK_minmax.bit_data_acs_mm_min[i];
telemetry[88+i] = bae_HK_minmax.bit_data_acs_mg_min[i];
}
telemetry[90] = BCN_TX_OC_FAULT;
telemetry[90] = (telemetry[90]<<1) | ACS_TR_XY_OC_FAULT;
telemetry[90] = (telemetry[90]<<1) | ACS_TR_Z_OC_FAULT;
telemetry[90] = (telemetry[90]<<1) | ACS_TR_XY_FAULT;
//EPS CHARGER
telemetry[90] = (telemetry[90]<<1) | EPS_CHARGER_FAULT;//eps_charger;
telemetry[90] = (telemetry[90]<<1) | CDMS_OC_FAULT;
telemetry[90] = (telemetry[90]<<1) | ACS_ATS1_OC_FAULT;
telemetry[90] = (telemetry[90]<<1) | ACS_ATS2_OC_FAULT;
telemetry[91] = ACS_TR_Z_FAULT;
//spare 23 bits
telemetry[92] = 0x00;
telemetry[93] = 0x00;
for (int i=94; 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;
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:
{
printf("\n\rdata for mms 0x05 flash");
/*changed*/
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];
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 = (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 = (8<<(uint16_t)tc[7]) | (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 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:
{
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;
}
printf("\n\rWritten on Flash");
break;
}
default://when invalid service subtype
{
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:
{
//printf("Function Management Service\r\n");
uint8_t service_subtype=(tc[2]&0x0F);
switch(service_subtype)
{
case 0x01:
{
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 moment_comm[3];
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");
FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
printf("%f\t",moment_comm[i]);
}
}
else if(ACS_STATE == 8) // Auto Control
{
printf("\n\r Auto control mode \n");
FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
printf("%f\t",moment_comm[i]);
}
}
else if(ACS_STATE == 9) // Detumbling
{
FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
printf("%f\t",moment_comm[i]);
}
}
else
{
ACS_STATUS = 7;
}
// Control algo commissioning
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=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 init1=0;
int init2=0;
int data1=0;
int data2=0;
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;
wait_ms(5);
ATS1_SW_ENABLE = 0;
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 = 1;
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;
}
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;
if(data1 == 2)
{
ATS1_SW_ENABLE = 0;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
}
else if(data1 == 3)
{
ATS1_SW_ENABLE = 0;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
}
else if(data1 == 1)
{
ATS1_SW_ENABLE = 0;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
ats_data = 0;
}
}
else if(data2==1)
{
if(data1 == 2)
{
ATS2_SW_ENABLE = 1;
wait_ms(5);
ATS1_SW_ENABLE = 0;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
}
else if(data1 == 3)
{
ATS2_SW_ENABLE = 1;
wait_ms(5);
ATS1_SW_ENABLE = 0;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
}
else
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
ats_data = 0;
}
}
else if(data2==2)
{
if(data1 == 3)
{
ATS2_SW_ENABLE = 1;
wait_ms(5);
ATS1_SW_ENABLE = 0;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
}
else
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
}
}
else if(data2==3)
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
}
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(ats_data == 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(ats_data == 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);
}
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);
if(ats_data == 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);
}
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 = 0;
wait_ms(ACS_DEMAG_TIME_DELAY);
if(ats_data == 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);
}
FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (8*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (9*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (10*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (11*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 0xE2:
{
uint8_t BCN_SPND_STATE;
BCN_SPND_STATE=tc[4];
if(BCN_SPND_STATE==0x00)
{
BCN_SPND_TX=0;
//stop BCN_STANDBY_TIMER.stop();//create
telemetry[2]=0xA0;
}
else if(BCN_SPND_STATE==0x01)
{
FCTN_BCN_SPND_TX();
//stop BCN_STANDBY_TIMER.start();//create
if(BCN_TX_MAIN_STATUS==0)
{
telemetry[2]=0xA0;
}
else if(BCN_TX_MAIN_STATUS==1)
{
telemetry[2]=0xC0;
}
}
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:
{
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;
}
else
{
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;
}
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==0x07)
{
printf("\n\rRun P_EPS_INIT");
FCTN_EPS_INIT();
telemetry[2]=ACK_CODE;
}
else
{
printf("\n\runable to Run P_EPS_INIT as BAE_STATUS not 111 ");;
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==0x07)
{
printf("\n\rRun P_EPS_MAIN");
F_EPS();
telemetry[2]=ACK_CODE;
}
else
{
printf("\n\runable to Run P_EPS_MAIN as BAE_STATUS not 111 ");;
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==0x07)
{
printf("\n\rRun P_ACS_INIT");
FCTN_ACS_INIT();
telemetry[2]=ACK_CODE;
}
else
{
printf("\n\runable to Run P_ACS_INIT as BAE_STATUS not 111 ");;
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==0x07)
{
printf("\n\rRun P_ACS_MAIN");
F_ACS();
telemetry[2]=ACK_CODE;
}
else
{
printf("\n\runable to Run P_ACS_MAIN as BAE_STATUS not 111 ");;
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==0x07)
{
printf("\n\rRun P_BCN_INIT");
F_BCN();
telemetry[2]=ACK_CODE;
}
else
{
printf("\n\runable to Run P_BCN_INIT as BAE_STATUS not 111 ");;
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==0x07)
{
printf("\n\rRun P_BCN_TX_MAIN");
FCTN_BCN_TX_MAIN();//correct function check once
telemetry[2]=ACK_CODE;
}
else
{
printf("\n\runable to Run P_BCN_TX_MAIN as BAE_STATUS not 111 ");;
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:
{
printf("\n\rSW_ON_ACS_ATS1_SW_ENABLE");
//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 = 1; // making sure we switch off the other
ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3) | 0x0C ;
ATS1_SW_ENABLE = 0;
ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F);
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 0x12:
{
printf("\n\rSW_ON_ACS_ATS2_SW_ENABLE");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS1_SW_ENABLE = 1; //make sure u switch off the other
ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F) | 0xC0 ;
ATS2_SW_ENABLE = 0;
ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3);
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:
{
printf("\n\rSW_ON_ACS_TR_XY_ENABLE");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
TRXY_SW = 1;//1 SWITCH enable here
ACS_TR_XY_SW_STATUS=0x01;
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:
{
printf("\n\rSW_ON_ACS_TR_Z_ENABLE");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
TRZ_SW = 1;
ACS_TR_Z_SW_STATUS=0x01;
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:
{
printf("\n\rSW_ON_BCN_TX_SW_ENABLE");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
BCN_SW = 0;//here 0 is switch enable
BCN_TX_SW_ENABLE=0x01;
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:
{
printf("\n\rSW_OFF_ACS_ATS1_SW_ENABLE");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS1_SW_ENABLE = 1;
ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F) | 0xC0 ;
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 0x22:
{
printf("\n\rSW_OFF_ACS_ATS2_SW_ENABLE");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS2_SW_ENABLE = 1;
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:
{
printf("\n\rSW_OFF_ACS_TR_XY_ENABLE");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
TRXY_SW= 0;
ACS_TR_XY_SW_STATUS=0x03;
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:
{
printf("\n\rSW_OFF_ACS_TR_Z_ENABLE");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
TRZ_SW = 0;
ACS_TR_Z_SW_STATUS=0x03;
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:
{
printf("\n\rSW_OFF_BCN_TX_SW_ENABLE");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
BCN_SW = 1;
BCN_TX_SW_ENABLE=0x03;
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 0x31:
{
printf("\n\rACS_ATS1_SW_RESET");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS2_SW_ENABLE = 1;//as ats switched off
ATS1_SW_ENABLE = 1;
wait_ms(5);
ATS1_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 0x32:
{
printf("\n\rACS_ATS2_SW_RESET");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS1_SW_ENABLE = 1;//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:
{
printf("\n\rACS_TR_XY_SW_RESET");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
TRXY_SW= 0;
wait_ms(5);
TRXY_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 0x34:
{
printf("\n\rACS_TR_Z_SW_RESET");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
TRZ_SW= 0;
wait_ms(5);
TRZ_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 0x35:
{
printf("\n\rBCN_TX_SW_RESET");
//ACK_L234_TM
telemetry[0]=0xB0;
telemetry[1]=tc[0];
BCN_SW = 1;
wait_ms(5);
BCN_SW = 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 0x36:
{
printf("\n\rBAE_INTERNAL_RESET TO be done ??");
NVIC_SystemReset();
break;
}
case 0x37:
{
printf("\n\rCDMS_RESET");
//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:
{
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:
{
uint8_t STANDBY_DATA_TC;
BAE_STANDBY=0x00;
STANDBY_DATA_TC=tc[4];
if(STANDBY_DATA_TC==0x01)
{
BAE_STANDBY |=0x04;
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==0x01)
{
BAE_STANDBY |=0x02;
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==0x01)
{
BAE_STANDBY |=0x01;
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:
{
printf("\n\rexecutng BAE reset HK counter");
firstCount=true;
void minMaxHkData();
//what to do here??*************************************************
//TO BE DONE
//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:
{
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:
{
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:
{
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);
//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;
}