Team Fox
I2C working only once, sensor working well, commissioning updated to be tested, Wohoooo!!! :-D
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
Japan_BAE1
by 
Team Fox
Diff: TCTM.cpp
- Revision:
- 11:1fdb94ae6563
- Parent:
- 10:f93407b97750
- Child:
- 12:af1d7e18b868
--- a/TCTM.cpp Mon Apr 11 17:26:46 2016 +0000
+++ b/TCTM.cpp Wed Apr 13 17:00:25 2016 +0000
@@ -10,12 +10,12 @@
#include "cassert"
#include"math.h"
-
+/*define the pins for digital out*/
extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
-extern DigitalOut TRXY_SW; //TR XY Switch
+extern DigitalOut TRXY_SW_EN; //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN
extern DigitalOut TRZ_SW; //TR Z Switch
extern DigitalOut CDMS_RESET; // CDMS RESET
extern DigitalOut BCN_SW; //Beacon switch
@@ -31,12 +31,40 @@
extern DigitalOut phase_TR_x;
extern DigitalOut phase_TR_y;
extern DigitalOut phase_TR_z;
-extern BAE_HK_actual actual_data.power_mode;
extern BAE_HK_quant quant_data;
+//extern DigitalOut TRXY_SW_EN;
+//extern DigitalOut TRZ_SW_EN; //same as TRZ_SW
+extern uint32_t BAE_ENABLE;
+//extern DigitalOut ACS_ACQ_DATA_ENABLE;
+
+/*given a default value as of now shuld read value from flash and increment it write it back very time it starts(bae)*/
+extern uint8_t BAE_RESET_COUNTER=0;
+
+extern uint8_t BCN_FAIL_COUNT;
+
+extern PwmOut PWM1; //x //Functions used to generate PWM signal
+extern PwmOut PWM2; //y
+extern PwmOut PWM3; //z //PWM output comes from pins p6
+
+extern void F_ACS();
+extern void F_BCN();
+//extern void FCTN_ACS_GENPWM_MAIN();
+extern void F_EPS();
+extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]);
+
extern void FCTN_ATS_DATA_ACQ();
extern void FCTN_ACS_CNTRLALGO(float*,float*);
+
+void FCTN_CONVERT_UINT (uint8_t input[2], float* output)
+{
+
+ *output = (float) input[1];
+ *output = *output/100.0; //input[0] integer part
+ *output = *output + (float) input[0]; //input[1] decimal part correct to two decimal places
+}
+
float angle(float x,float y,float z)
{
float mag_total=sqrt(x*x + y*y + z*z);
@@ -87,7 +115,7 @@
how to get that we dont know, now we just assume it to be so*/
tm[3] = (BCN_SW);
tm[3] = tm[3]|(TRXY_SW_EN<<1);
- tm[3] = tm[3]|(TRZ_SW_EN<<2);
+ tm[3] = tm[3]|(TRZ_SW<<2);
tm[3] = tm[3]|(ATS1_SW_ENABLE<<3);
tm[3] = tm[3]|(ATS2_SW_ENABLE<<4);
@@ -97,7 +125,9 @@
tm[3] = tm[3] & 0xDF;
tm[3] = tm[3]|(BCN_FEN<<6);
- tm[3] = tm[3]|(ACS_ACQ_DATA_ENABLE<<7);
+ uint8_t mask_val=BAE_ENABLE & 0x00000008;
+ /*can be a problem see if any error occurs*/
+ tm[3] = tm[3]|(mask_val<<7);
/*not included in the code yet*/
tm[4] = BAE_RESET_COUNTER;
@@ -108,7 +138,7 @@
tm[5] = tm[5]|(phase_TR_y<<5);
tm[5] = tm[5]|(phase_TR_z<<6);
/*spare to be fixed*/
- tm[5] = tm[5]|(Spare))<<7);
+ //tm[5] = tm[5]|(Spare))<<7);
/**/
uint8_t soc_powerlevel_2=50;
uint8_t soc_powerlevel_3=65;
@@ -139,7 +169,7 @@
uint16_t P_BCN_TX_MAIN_COUNTER=0;
uint16_t P_EPS_MAIN_COUNTER=0;
- tm[18] = P_BAE_I2CRX_COUNTER>>8
+ tm[18] = P_BAE_I2CRX_COUNTER>>8;
tm[19] = P_BAE_I2CRX_COUNTER;
tm[20] = P_ACS_MAIN_COUNTER>>8;
tm[21] = P_ACS_MAIN_COUNTER;
@@ -158,12 +188,25 @@
tm[51] = actual_data.faultPoll_status;
//Bdot Rotation Speed of Command tm[52-53]
//Bdot Output Current tm[54]
- float l_pmw1 = PWM1;
- float l_pmw2 = PWM2;
- float l_pmw3 = PWM3;
- FCTN_CONVERT_FLOAT(l_pwm1, &tm[55]);
- FCTN_CONVERT_FLOAT(l_pwm2, &tm[59]);
- FCTN_CONVERT_FLOAT(l_pwm3, &tm[63]);
+ //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], &tm[55]);
+ FCTN_CONVERT_FLOAT(PWM_measured[1], &tm[59]);
+ FCTN_CONVERT_FLOAT(PWM_measured[2], &tm[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, &tm[67]);
@@ -200,54 +243,55 @@
tm[2] = ACK_CODE;
for(int i;i<16;i++)
- tm[i+3] = BAE_HK_min_max bae_HK_minmax.voltage_max[i];
+ tm[i+3] = bae_HK_minmax.voltage_max[i];
for(int i;i<12;i++)
- tm[i+18] = BAE_HK_min_max bae_HK_minmax.current_max[i];
+ tm[i+18] = bae_HK_minmax.current_max[i];
- tm[29] = BAE_HK_min_max bae_HK_minmax.Batt_voltage_max;;
- tm[30] = BAE_HK_min_max bae_HK_minmax.BAE_temp_max;
+ tm[29] = bae_HK_minmax.Batt_voltage_max;;
+ tm[30] = bae_HK_minmax.BAE_temp_max;
/*battery soc*/
//tm[31] = BAE_HK_min_max bae_HK_minmax.voltage_max;
- tm[32] = BAE_HK_min_max bae_HK_minmax.Batt_temp_max[1];
- tm[33] = BAE_HK_min_max bae_HK_minmax.Batt_temp_max[2];
+ tm[32] = bae_HK_minmax.Batt_temp_max[1];
+ tm[33] = bae_HK_minmax.Batt_temp_max[2];
/*BCN temp not there*/
//tm[34] = BAE_HK_min_max bae_HK_minmax.;
for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(BAE_HK_min_max bae_HK_minmax.Bvalue_max[i],&tm[35+(i*4)]);
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[i],&tm[35+(i*4)]);
for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(BAE_HK_min_max bae_HK_minmax.AngularSpeed_max[i],&tm[47+(i*4)]);
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[i],&tm[47+(i*4)]);
/*min data*/
for(int i;i<16;i++)
- tm[i+59] = BAE_HK_min_max bae_HK_minmax.voltage_min[i];
+ tm[i+59] = bae_HK_minmax.voltage_min[i];
for(int i;i<12;i++)
- tm[i+74] = BAE_HK_min_max bae_HK_minmax.current_min[i];
+ tm[i+74] = bae_HK_minmax.current_min[i];
- tm[86] = BAE_HK_min_max bae_HK_minmax.Batt_voltage_min;
- tm[87] = BAE_HK_min_max bae_HK_minmax.BAE_temp_min;
+ tm[86] = bae_HK_minmax.Batt_voltage_min;
+ tm[87] = bae_HK_minmax.BAE_temp_min;
/*battery soc*/
//tm[88] = BAE_HK_min_max bae_HK_minmax.voltage_max;
- tm[89] = BAE_HK_min_max bae_HK_minmax.Batt_temp_min[1];
- tm[90] = BAE_HK_min_max bae_HK_minmax.Batt_temp_min[2];
+ tm[89] = bae_HK_minmax.Batt_temp_min[1];
+ tm[90] = bae_HK_minmax.Batt_temp_min[2];
+ //huhu//
/*BCN temp not there*/
//tm[91] = BAE_HK_min_max bae_HK_minmax.;
for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(BAE_HK_min_max bae_HK_minmax.Bvalue_min[i],&tm[91+(i*4)]);
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[i],&tm[91+(i*4)]);
for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(BAE_HK_min_max bae_HK_minmax.AngularSpeed_min[i],&tm[103+(i*4)]);
+ FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[i],&tm[103+(i*4)]);
for (int i=115; i<132;i++)
@@ -459,9 +503,9 @@
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(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]);
@@ -556,9 +600,9 @@
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
- FCTN_CONVERT_UINT (M0, &moment_tc[0]);
- FCTN_CONVERT_UINT (M1, &moment_tc[1]);
- FCTN_CONVERT_UINT (M2, &moment_tc[2]);
+ FCTN_CONVERT_UINT(M0,&moment_tc[0]);
+ FCTN_CONVERT_UINT(M1, &moment_tc[1]);
+ FCTN_CONVERT_UINT(M2, &moment_tc[2]);
@@ -678,9 +722,11 @@
tm[24] = tm[5]|(phase_TR_y<<5);
tm[24] = tm[5]|(phase_TR_z<<6);
- FCTN_CONVERT_FLOAT(l_pwm_1,&tm[25]);
- FCTN_CONVERT_FLOAT(l_pwm_2,&tm[29]);
- FCTN_CONVERT_FLOAT(l_pwm_3,&tm[33]);
+ /*___________________change / check pwm working__________________________________*/
+
+ FCTN_CONVERT_FLOAT(PWM1,&tm[25]);
+ FCTN_CONVERT_FLOAT(PWM2,&tm[29]);
+ FCTN_CONVERT_FLOAT(PWM3,&tm[33]);
//ACK_L234_TM
/* tm[0]=0xB0;
@@ -796,7 +842,7 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
- TRXY_SW = 1;
+ TRXY_SW_EN = 1;
tm[2]=1;
for(uint8_t i=3;i<11;i++)
{
@@ -902,7 +948,7 @@
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
- TRXY_SW = 0;
+ TRXY_SW_EN= 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
@@ -1149,4 +1195,4 @@
output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic
//printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]);
//std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl;
-}
\ No newline at end of file
+}