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
Diff: TCTM.cpp
- Revision:
- 20:949d13045431
- Parent:
- 19:79e69017c855
- Child:
- 27:61c856be467e
--- a/TCTM.cpp Sat Jun 04 11:29:13 2016 +0000
+++ b/TCTM.cpp Fri Jul 01 17:55:30 2016 +0000
@@ -1,7 +1,9 @@
#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"
@@ -12,69 +14,216 @@
//**********************************STATUS_PARAMETERS*****************************************************
uint8_t BCN_TX_SW_ENABLE=0x00;
-//extern uint8_t BCN_TX_STATUS;?? is it same??*****************************************************************************************DOUBT
-uint8_t ACS_TR_XY_SW_STATUS=0x00;
-uint8_t ACS_TR_Z_SW_STATUS=0x00;
-uint8_t ACS_ATS1_SW_STATUS=0x00;
-uint8_t ACS_ATS2_SW_STATUS=0x00;
//***********************************FOR STANDBY TIMER****************************************************
extern void BAE_STANDBY_TIMER_RESET();
+uint8_t telemetry[135];
+extern uint8_t BAE_HK_data[134];
-//**********************************EXTERN_PARAMETERS******************************************************
-/*define the pins for digital out*/
+//*****************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_STANDBY;
-uint8_t BCN_TX_MAIN_STATUS;
+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();
+
+
+//BAE
+extern uint8_t BAE_STANDBY;
+extern uint8_t BAE_INIT_STATUS;
+extern uint8_t BAE_RESET_COUNTER;
+/*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 uint32_t BAE_STATUS;
-extern uint8_t BAE_STANDBY;
-extern float data[6];
-extern float moment[3];
-extern uint8_t ACS_STATE;
-extern DigitalOut EN_BTRY_HT;
-extern DigitalOut phase_TR_x;
-extern DigitalOut phase_TR_y;
-extern DigitalOut phase_TR_z;
-extern BAE_HK_quant quant_data;
//extern DigitalOut TRXY_SW;
//extern DigitalOut TRZ_SW_EN; //same as TRZ_SW
extern uint32_t BAE_ENABLE;
-//extern DigitalOut ACS_ACQ_DATA_ENABLE;
+extern uint16_t BAE_I2C_COUNTER;
+//extern uint8_t BCN_FAIL_COUNT;
+
-/*given a default value as of now shuld read value from flash and increment it write it back very time it starts(bae)*/
-extern uint8_t BAE_RESET_COUNTER;
+//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 EN_BTRY_HT;
+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_BATTERY_TEMP_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 BCN_FAIL_COUNT;
+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
-extern void F_ACS();
-extern void F_BCN();
+//included after home
//extern void FCTN_ACS_GENPWM_MAIN();
-extern void F_EPS();
-extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]);
-extern void FCTN_ACS_INIT();
+
+uint16_t crc_hk_data()//gencrc16_for_me()
+{
+ uint16_t crc = CRC::crc16_gen(BAE_HK_data,132);//BAE_chardata i.e char data type usesd earlier
+ 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;
+}
-extern void FCTN_ATS_DATA_ACQ();
-extern void FCTN_ACS_CNTRLALGO(float*,float*);
-uint8_t telemetry[135];
-
-void FCTN_CONVERT_UINT (uint8_t input[2], float* output)
+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];
@@ -98,18 +247,23 @@
{
//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((char)tc,9); //implementing crc
-
+ 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
@@ -156,60 +310,6 @@
uint8_t service_subtype=(tc[2]&0x0F);
switch(service_subtype)
{
- /* no such case exist refer tc protocol mms 0x1 exist for speed/payload only*/
-/* case 0x01:
- {
- printf("Read from Flash\r\n");
- uint16_t jj; //if no problem then change it to uint8
- uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
- switch(MID)
- {
- case 0x1100:jj=0x00;// using uint16_t as jj typesimilarly used in FCTN_CDMS_WR_FLASH
- break;
- case 0x0100:jj=0x01;
- break;
- case 0x0101:jj=0x02;
- break;
- case 0x0102:jj=0x03;
- break;
- case 0x0107:jj=0x04;
- break;
- case 0x0103:jj=0x05;
- break;
- case 0x0104:jj=0x05;
- break;
- case 0x0105:jj=0x06;
- break;
- case 0x0106:jj=0x07;
- break;
- }
- //*pointer....!!!!
- uint32_t FLASH_TEMP = FCTN_CDMS_RD_FLASH(jj);
-
- tm[0] = 0x60;
- tm[1] = tc[0];
- tm[2] = ACK_CODE;
- for(int i=0; i<8*4; i+=4)
- {
- tm[4+i] =(uint8_t )(((FLASH_TEMP)>>24)&0xFF);
- tm[5+i] =(uint8_t ) (((FLASH_TEMP)>>16)&0xFF);
- tm[6+i] =(uint8_t ) (((FLASH_TEMP)>>8)&0xFF);
- tm[7+i] =(uint8_t ) ((FLASH_TEMP) & 0xFF);
-
- }
-
-
- for (int i=4+8*4; i<132;i++)
- {
- tm[i] = 0x00;
- }
- crc16 = CRC::crc16_gen(tm,132);
- tm[132] = (uint8_t)((crc16&0xFF00)>>8);
- tm[133] = (uint8_t)(crc16&0x00FF);
-
- break;
- }
-*/
case 0x02:
{
printf("\n\rRead from RAM");
@@ -228,141 +328,171 @@
telemetry[2] = ACK_CODE;
/*random or with bcn_tx_sw_enable assuming it is 1 bit in length
how to get that we dont know, now we just assume it to be so*/
- telemetry[3] = (BCN_SW);
- telemetry[3] = telemetry[3]|(TRXY_SW<<1);
- telemetry[3] = telemetry[3]|(TRZ_SW<<2);
- telemetry[3] = telemetry[3]|(ATS1_SW_ENABLE<<3);
- telemetry[3] = telemetry[3]|(ATS2_SW_ENABLE<<4);
-
- if(BCN_TX_STATUS==2)
- telemetry[3] = telemetry[3]|0x20;
- else
- telemetry[3] = telemetry[3] & 0xDF;
-
- telemetry[3] = telemetry[3]|(BCN_FEN<<6);
- uint8_t mask_val=BAE_ENABLE & 0x00000008;
- /*can be a problem see if any error occurs*/
- telemetry[3] = telemetry[3]|(mask_val<<7);
-
- /*not included in the code yet*/
- telemetry[4] = BAE_RESET_COUNTER;
- telemetry[5] = ACS_STATE;
- telemetry[5] = telemetry[5]|(EN_BTRY_HT<<3);
- telemetry[5] = telemetry[5]|(phase_TR_x<<4);
- telemetry[5] = telemetry[5]|(phase_TR_y<<5);
- telemetry[5] = telemetry[5]|(phase_TR_z<<6);
- /*spare to be fixed*/
- //telemetry[5] = telemetry[5]|(Spare))<<7);
- /**/
- uint8_t soc_powerlevel_2=50;
- uint8_t soc_powerlevel_3=65;
-
- telemetry[6] = soc_powerlevel_2;
- telemetry[7] = soc_powerlevel_3;
-
- /*to be fixed*/
- telemetry[8] = 0;
- telemetry[9] = 0;
- telemetry[10] = 0;
- telemetry[11] = 0;
- //telemetry[8] = Torque Rod X Offset;
- //telemetry[9] = Torque Rod Y Offset;
- //telemetry[10] = Torque Rod Z Offset;
- //telemetry[11] = ACS_DEMAG_TIME_DELAY;
- telemetry[12] = (BAE_STATUS>>24) & 0xFF;
- telemetry[13] = (BAE_STATUS>>16) & 0xFF;
- telemetry[14] = (BAE_STATUS>>8) & 0xFF;
- telemetry[15] = BAE_STATUS & 0xFF;
-
- /*to be fixed*/
- //telemetry[16] = BCN_FAIL_COUNT;
- telemetry[17] = actual_data.power_mode;
- /*to be fixed*/
- uint16_t P_BAE_I2CRX_COUNTER=0;
- uint16_t P_ACS_MAIN_COUNTER=0;
- uint16_t P_BCN_TX_MAIN_COUNTER=0;
- uint16_t P_EPS_MAIN_COUNTER=0;
-
- telemetry[18] = P_BAE_I2CRX_COUNTER>>8;
- telemetry[19] = P_BAE_I2CRX_COUNTER;
- telemetry[20] = P_ACS_MAIN_COUNTER>>8;
- telemetry[21] = P_ACS_MAIN_COUNTER;
- telemetry[22] = P_BCN_TX_MAIN_COUNTER>>8;
- telemetry[23] = P_BCN_TX_MAIN_COUNTER;
- telemetry[24] = P_EPS_MAIN_COUNTER>>8;
- telemetry[25] = P_EPS_MAIN_COUNTER;
+ 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]<<1) | ACS_DETUMBLING_ALGO_TYPE;
+ telemetry[4] = (telemetry[4]<<3) | ACS_STATE;
+ telemetry[5] = 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
+ //now two 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) | 0;//change it
+ 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]<<3) | ACS_INIT_STATUS;
+
+ telemetry[29] = ACS_DATA_ACQ_STATUS;
+ telemetry[29] = (telemetry[29]<<1) | ACS_MAIN_STATUS;
+ telemetry[29] = (telemetry[29]<<3) | ACS_STATUS;
+ telemetry[29] = (telemetry[29]<<1) | EPS_INIT_STATUS;
+ telemetry[29] = (telemetry[29]<<3) | EPS_BATTERY_GAUGE_STATUS;
+
+ telemetry[30] = EPS_MAIN_STATUS;
+ telemetry[30] = (telemetry[30]<<1) | EPS_BATTERY_TEMP_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
+ //spare 1
+ //spare 5
+ telemetry[31] = 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] = (telemetry[62]<<1) | 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;
+ //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;
+ //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)]);
-
- //printf("\n\rthe value is 38\t %x\n",telemetry[38]);
- //printf("\n\rthe value is 39\t%x\n",telemetry[39]);
- //printf("\n\rthe value is 40\t%x\n",telemetry[40]);
- //printf("\n\rthe value is 41\t%x\n",telemetry[41]);
- //printf("\n\rthe value true\t%f\n",actual_data.AngularSpeed_actual[0]);
-
- //uint32_t input_stage1=0x00000000;
- //uint8_t output1[4];
- //output1[0]=(uint32_t)(telemetry[38]);
- //output1[1]=(uint32_t)(telemetry[39]);
- //output1[2]=(uint32_t)(telemetry[40]);
- //output1[3]=(uint32_t)(telemetry[41]);
-
- //input_stage1=output[3]+(output[2]*(0x100))+(output[1]*(0x10000))+(output[0]*(0x1000000));
- //input_stage1=(output1[0]<<24) | (output1[1]<<16) | (output1[2]<<8) | (output1[3]);
-
- //assert(sizeof(float) == sizeof(uint32_t));
- //float* temp1 = reinterpret_cast<float*>(&input_stage1);
-
- //printf("\n\r the value is: %f \n",*temp1);
-
- //FAULT_FLAG();
- telemetry[50] = actual_data.faultIr_status;
- telemetry[51] = actual_data.faultPoll_status;
- //Bdot Rotation Speed of Command telemetry[52-53]
- //Bdot Output Current telemetry[54]
- //float l_pmw1 = (PWM1);
- //float l_pmw2 = PWM2;
- //float l_pmw3 = PWM3;
-
- /*__________________________________________________________________*/
-
- /*change and check if changing it to PWM1 causes problem*/
-
- /*___________________________________________________________________*/
-
- float PWM_measured[3];
-
- PWM_measured[0] = PWM1.read();
- PWM_measured[1] = PWM2.read();
- PWM_measured[2] = PWM3.read();
-
- FCTN_CONVERT_FLOAT(PWM_measured[0], &telemetry[55]);
- FCTN_CONVERT_FLOAT(PWM_measured[1], &telemetry[59]);
- FCTN_CONVERT_FLOAT(PWM_measured[2], &telemetry[63]);
- float attitude_ang = angle(actual_data.Bvalue_actual[0],actual_data.Bvalue_actual[1],actual_data.Bvalue_actual[2]);
- FCTN_CONVERT_FLOAT(attitude_ang, &telemetry[67]);
-
- for (int i=0; i<16; i++)
- telemetry[68+i] = quant_data.voltage_quant[i];
- for (int i=0; i<12; i++)
- telemetry[84+i] = quant_data.current_quant[i];
- //telemetry[96]
- //telemetry[97]
- //telemetry[98]
- //telemetry[99]
- telemetry[100] = quant_data.Batt_voltage_quant;
- telemetry[101] = quant_data.BAE_temp_quant;
- telemetry[102] = quant_data.Batt_gauge_quant[1];
- telemetry[103] = quant_data.Batt_temp_quant[0];
- telemetry[104] = quant_data.Batt_temp_quant[1];
-
- //telemetry[105] = beacon temperature;
-
- for (int i=105; i<132;i++)
+ ///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;
}
@@ -380,58 +510,71 @@
telemetry[2] = ACK_CODE;
for(int i;i<16;i++)
- telemetry[i+3] = bae_HK_minmax.voltage_max[i];
+ {
+ telemetry[i+3] = bae_HK_minmax.voltage_max[i];
+ telemetry[i+19] = bae_HK_minmax.current_max[i];
+ }
- for(int i;i<12;i++)
- telemetry[i+18] = bae_HK_minmax.current_max[i];
-
- telemetry[29] = bae_HK_minmax.Batt_voltage_max;;
- telemetry[30] = bae_HK_minmax.BAE_temp_max;
+ telemetry[35] = bae_HK_minmax.Batt_voltage_max;;
+ telemetry[36] = bae_HK_minmax.BAE_temp_max;
- /*battery soc*/
- //telemetry[31] = BAE_HK_min_max bae_HK_minmax.voltage_max;
+ telemetry[37] = bae_HK_minmax.Batt_SOC_max;
- telemetry[32] = bae_HK_minmax.Batt_temp_max[0];
- telemetry[33] = bae_HK_minmax.Batt_temp_max[1];
+ telemetry[38] = bae_HK_minmax.Batt_temp_max[0];
+ telemetry[39] = bae_HK_minmax.Batt_temp_max[1];
- /*BCN temp not there*/
- //telemetry[34] = BAE_HK_min_max bae_HK_minmax.;
+ /*BCN temp there*/
+ telemetry[40] = bae_HK_minmax.BCN_TEMP_max;
for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[i],&telemetry[35+(i*4)]);
-
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[i],&telemetry[47+(i*4)]);
+ {
+ 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+59] = bae_HK_minmax.voltage_min[i];
+ telemetry[i+47] = bae_HK_minmax.voltage_min[i];
- for(int i;i<12;i++)
- telemetry[i+74] = bae_HK_minmax.current_min[i];
+ for(int i;i<16;i++)
+ telemetry[i+63] = bae_HK_minmax.current_min[i];
- telemetry[86] = bae_HK_minmax.Batt_voltage_min;
- telemetry[87] = bae_HK_minmax.BAE_temp_min;
+ telemetry[79] = bae_HK_minmax.Batt_voltage_min;
+ telemetry[80] = bae_HK_minmax.BAE_temp_min;
/*battery soc*/
- //telemetry[88] = BAE_HK_min_max bae_HK_minmax.voltage_max;
+ telemetry[81] = bae_HK_minmax.Batt_SOC_min;
- telemetry[89] = bae_HK_minmax.Batt_temp_min[0];
- telemetry[90] = bae_HK_minmax.Batt_temp_min[1];
+ telemetry[82] = bae_HK_minmax.Batt_temp_min[0];
+ telemetry[83] = bae_HK_minmax.Batt_temp_min[1];
//huhu//
- /*BCN temp not there*/
- //telemetry[91] = BAE_HK_min_max bae_HK_minmax.;
+ /*BCN temp named as BCN_TS_BUFFER there*/
+ telemetry[84] = bae_HK_minmax.BCN_TEMP_min;
for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[i],&telemetry[91+(i*4)]);
-
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[i],&telemetry[103+(i*4)]);
-
-
- for (int i=115; i<132;i++)
+ {
+ telemetry[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;
}
@@ -467,70 +610,107 @@
printf("\n\rdata for mms 0x05 flash");
/*changed*/
printf("\n\rwrite on flash\n");
- uint32_t FLASH_DATA[8];//256 bits
+ 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 0x0100:
+ case 0x1100:
{
- FLASH_DATA[0] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
- FCTN_BAE_WR_FLASH(0x00,FLASH_DATA[0]);
+ //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 & 0xFFFFF7FF) | (11<<(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]);
- FCTN_BAE_WR_FLASH(0x01,FLASH_DATA[1]);
+ //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]>>3;
+ ACS_STATE = (tc[8] & 0x07);
+ FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
+ FLASH_DATA = (FLASH_DATA & 0xFFF0FFFF) | (16<<(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]);
- FCTN_BAE_WR_FLASH(0x02,FLASH_DATA[2]);
+ //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 & 0xFFFFFBFF) | (10<<(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]);
- FCTN_BAE_WR_FLASH(0x03,FLASH_DATA[3]);
+ //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]);
- FCTN_BAE_WR_FLASH(0x04,FLASH_DATA[4]);
+ //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 0x0105:
+ case 0x0106:
{
- FLASH_DATA[5] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
- FCTN_BAE_WR_FLASH(0x05,FLASH_DATA[5]);
+ //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 0x0106:
+ case 0x0108:
{
- FLASH_DATA[6] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
- FCTN_BAE_WR_FLASH(0x06,FLASH_DATA[6]);
+ //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 0x0107:
+ case 0x0109:
{
- FLASH_DATA[7] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]);
- FCTN_BAE_WR_FLASH(0x07,FLASH_DATA[7]);
+ //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;
}
@@ -610,182 +790,350 @@
{
case 0xE0:
{
- float B[3],W[3];
- printf("ACS_COMSN\r\n");
+ 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
-
- uint8_t B_x[2];
- uint8_t B_y[2];
- uint8_t B_z[2];
- uint8_t W_x[2];
- uint8_t W_y[2];
- uint8_t W_z[2];
-
- B_x[0]=tc[3];
- B_x[1]=tc[4];
- B_y[0]=tc[5];
- B_y[1]=tc[6];
- B_z[0]=tc[7];
- B_z[1]=tc[8];
-
- W_x[0]=tc[9];
- W_x[1]=tc[10];
- W_y[0]=tc[11];
- W_y[1]=tc[12];
- W_z[0]=tc[13];
- W_z[1]=tc[14];
-
- FCTN_CONVERT_UINT(B_x,&B[0]);
- FCTN_CONVERT_UINT(B_y,&B[1]);
- FCTN_CONVERT_UINT(B_z,&B[2]);
-
- FCTN_CONVERT_UINT (W_x, &W[0]);
- FCTN_CONVERT_UINT (W_y, &W[1]);
- FCTN_CONVERT_UINT (W_z, &W[2]);
-
- telemetry[0]=0xF0;
- telemetry[1]=tc[0];
- telemetry[2]=ACK_CODE;
- //FCTN_ATS_DATA_ACQ(); //get data
- printf("gyro values\n\r");
- for(int i=0; i<3; i++)
- printf("%f\n\r",W[i]);
- printf("mag values\n\r");
- for(int i=0; i<3; i++)
- printf("%f\n\r",B[i]);
- /* FCTN_CONVERT_FLOAT(data[0],&telemetry[4]); //telemetry[4] - telemetry[7]
- FCTN_CONVERT_FLOAT(data[1],&telemetry[8]); //telemetry[8] - telemetry[11]
- FCTN_CONVERT_FLOAT(data[2],&telemetry[12]); //telemetry[12] - telemetry[15]
- FCTN_CONVERT_FLOAT(data[0],&telemetry[16]); //telemetry[16] - telemetry[19]
- FCTN_CONVERT_FLOAT(data[1],&telemetry[20]); //telemetry[20] - telemetry[23]
- FCTN_CONVERT_FLOAT(data[2],&telemetry[24]); //telemetry[24] - telemetry[27]
- if((data[0]<8) && (data[1]<8) && (data[2] <8))
- telemetry[28] = 1; // gyro values in correct range
+ 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
- telemetry[28] = 0;
- if ((data[3] > 20 ) && (data[4] >20) && (data[5]>20)&& (data[3] < 50 ) && (data[4] <50) && (data[5]<50))
- telemetry[29] = 1; // mag values in correct range
- else
- telemetry[29] = 0;
- */
- // float B[3],W[3];
- // B[0] = B0;
- // B[1] = B1;
- // B[2] = B2;
- // W[0] = W0;
- // W[1] = W1;
- // W[2] = W2;
- // Control algo commissioning
-
- /* FCTN_ACS_CNTRLALGO(B,W);
- FCTN_CONVERT_FLOAT(moment[0],&telemetry[30]); //telemetry[30] - telemetry[33]
- FCTN_CONVERT_FLOAT(moment[1],&telemetry[34]); //telemetry[34] - telemetry[37]
- FCTN_CONVERT_FLOAT(moment[2],&telemetry[38]); //telemetry[38] - telemetry[41]
- // to include commission TR as well
- for(uint8_t i=42;i<132;i++)
{
- telemetry[i]=0x00;
+ ACS_STATUS = 7;
}
-
- crc16 = CRC::crc16_gen(telemetry,132);
- telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
- telemetry[134] = (uint8_t)(crc16&0x00FF);
- break;
- */
-
- // Control algo commissioning
- FCTN_ACS_CNTRLALGO(B,W);
- FCTN_CONVERT_FLOAT(moment[0],&telemetry[4]); //telemetry[4] - telemetry[7]
- FCTN_CONVERT_FLOAT(moment[1],&telemetry[8]); //telemetry[8] - telemetry[11]
- FCTN_CONVERT_FLOAT(moment[2],&telemetry[12]); //telemetry[12] - telemetry[15]
+
+ // 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[132] = (uint8_t)((crc16&0xFF00)>>8);
- telemetry[133] = (uint8_t)(crc16&0x00FF);
+ telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[134] = (uint8_t)(crc16&0x00FF);
break;
}
case 0xE1:
{
- float moment_tc[3];
- printf("\n\rHARDWARE_COMSN");
+ printf("HARDWARE_COMSN\r\n");
//ACK_L234_telemetry
- uint8_t M0[2];
- uint8_t M1[2];
- uint8_t M2[2];
+
+ int init1=0;
+ int init2=0;
+ int data1=0;
+ int data2=0;
- M0[0]=tc[3];
- M0[1]=tc[4];
- M1[0]=tc[5];
- M1[1]=tc[6];
- M2[0]=tc[7];
- M2[1]=tc[8];
-
-
- telemetry[0]=0xF0;
+ 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;
- float PWM_measured[3];
-
- FCTN_CONVERT_UINT(M0,&moment_tc[0]);
- moment_tc[1] = 0;
- moment_tc[2] = 0;
- FCTN_ACS_GENPWM_MAIN(moment_tc);
- PWM_measured[0] = PWM1.read();
- FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (0*4)]);
+ 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;
- FCTN_CONVERT_UINT(M1, &moment_tc[1]);
- moment_tc[0] = 0;
- moment_tc[2] = 0;
- FCTN_ACS_GENPWM_MAIN(moment_tc);
- PWM_measured[1] = PWM2.read();
- FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (1*4)]);
+ 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;
+ }
- FCTN_CONVERT_UINT(M2, &moment_tc[2]);
- moment_tc[0] = 0;
- moment_tc[1] = 0;
- FCTN_ACS_GENPWM_MAIN(moment_tc);
- PWM_measured[2] = PWM3.read();
- FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[16 + (2*4)]);
+ 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
- FCTN_CONVERT_FLOAT(PWM_measured[0],&telemetry[4]); //4-7
- FCTN_CONVERT_FLOAT(PWM_measured[1],&telemetry[8]); //8-11
- FCTN_CONVERT_FLOAT(PWM_measured[2],&telemetry[12]); //12-15
-
- // for(int i=0; i<12; i++)
- // FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]);
-
- // FCTN_ATS_DATA_ACQ(); //get data
-
- // to include commission TR as well
- for(uint8_t i=28;i<132;i++)
+ // to include commission TR as well
+ for(uint8_t i=35;i<132;i++)
{
- telemetry[i]=0x00;
+ telemetry[i]=0x00;
}
crc16 = CRC::crc16_gen(telemetry,132);
- telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
- telemetry[133] = (uint8_t)(crc16&0x00FF);
+ telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
+ telemetry[134] = (uint8_t)(crc16&0x00FF);
break;
}
case 0xE2:
{
- uint8_t STANDBY_STATUS_BCN;
- STANDBY_STATUS_BCN=tc[4];
- if(STANDBY_STATUS_BCN==0x00)
+ uint8_t BCN_SPND_STATE;
+ BCN_SPND_STATE=tc[4];
+ if(BCN_SPND_STATE==0x00)
{
- BCN_STANDBY=0;
+ BCN_SPND_TX=0;
//stop BCN_STANDBY_TIMER.stop();//create
telemetry[2]=0xA0;
}
- else if(STANDBY_STATUS_BCN==0x01)
+ else if(BCN_SPND_STATE==0x01)
{
- BCN_STANDBY=1;
+ BCN_SPND_TX=1;
//stop BCN_STANDBY_TIMER.start();//create
if(BCN_TX_MAIN_STATUS==0)
{
@@ -817,6 +1165,55 @@
}
break;
}
+ case 0xE3:
+ {
+ if(EPS_BTRY_HTR_AUTO != 0)
+ telemetry[2]=0x87;
+ else
+ {
+ HTR_CYCLE_COUNTS = tc[4];
+ if(HTR_CYCLE_COUNTS==0x00)
+ {
+ EN_BTRY_HT = 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)
{
@@ -1015,9 +1412,10 @@
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_ATS2_SW_STATUS=0x00;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3) | 0x0C ;
+
ATS1_SW_ENABLE = 0;
- ACS_ATS1_SW_STATUS=0x01;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F);
telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
@@ -1039,9 +1437,9 @@
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS1_SW_ENABLE = 1; //make sure u switch off the other
- ACS_ATS1_SW_STATUS=0x00;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F) | 0xC0 ;
ATS2_SW_ENABLE = 0;
- ACS_ATS2_SW_STATUS=0x01;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3);
telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
@@ -1129,7 +1527,7 @@
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS1_SW_ENABLE = 1;
- ACS_ATS1_SW_STATUS=0x03;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F) | 0xC0 ;
telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
@@ -1151,7 +1549,7 @@
telemetry[0]=0xB0;
telemetry[1]=tc[0];
ATS2_SW_ENABLE = 1;
- ACS_ATS2_SW_STATUS=0x03;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3) | 0x0C ;
telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
@@ -1351,25 +1749,8 @@
}
case 0x36:
{
- printf("\n\rBAE_INTERNAL_RESET TO be done how??");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- /*
- logic has to be done********************************************************
- */
- 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;
- }
+ printf("\n\rBAE_INTERNAL_RESET TO be done ??");
+ NVIC_SystemReset();
break;
}
case 0x37:
@@ -1462,6 +1843,8 @@
case 0x41:
{
printf("\n\rexecutng BAE reset HK counter");
+ firstCount=true;
+ void minMaxHkData();
//what to do here??*************************************************
//TO BE DONE
@@ -1555,8 +1938,6 @@
}
-
-
int strt_add = flash_size() - (2*SECTOR_SIZE);
uint32_t flasharray[8]; //256+(3*1024)
/*corrected*/
@@ -1571,7 +1952,7 @@
}
flasharray[j]=fdata;
erase_sector(strt_add);
- program_flash(strt_add, (char*)&flasharray,4*8);
+ program_flash(strt_add, (char*)flasharray,32);
}
/*End*/