Team Fox
/
BAE_CODE_MARCH_2017
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:
- 39:670133e7ffd8
- Parent:
- 33:76f2b8735501
- Child:
- 40:c2538d97e78b
- Child:
- 45:b5bd48ffbb67
- Child:
- 47:d59ba66229ce
--- a/TCTM.cpp Mon Jul 04 04:29:59 2016 +0000
+++ b/TCTM.cpp Tue Jul 05 13:44:15 2016 +0000
@@ -41,6 +41,7 @@
extern uint8_t ACS_TR_X_PWM;
extern uint8_t ACS_TR_Y_PWM;
extern uint8_t ACS_TR_Z_PWM;
+extern uint8_t B_SCZ_ANGLE;
extern uint8_t alarmmode;
extern uint8_t controlmode_mms;
extern uint8_t invjm_mms[9];
@@ -83,36 +84,36 @@
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 DigitalInOut ACS_TR_XY_ENABLE;
+extern DigitalInOut ACS_TR_Z_ENABLE;
+extern DigitalInOut ACS_TR_XY_OC_FAULT;
+extern DigitalInOut ACS_TR_Z_OC_FAULT;
+extern DigitalInOut ACS_TR_XY_FAULT;
+extern DigitalInOut ACS_TR_Z_FAULT;
+extern DigitalInOut ACS_ATS1_OC_FAULT;
+extern DigitalInOut 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 DigitalInOut ATS1_SW_ENABLE; // enable of att sens2 switch
+extern DigitalInOut 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 DigitalInOut DRV_Z_EN;
+extern DigitalInOut DRV_XY_EN;
+extern DigitalInOut TRXY_SW; //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN
+extern DigitalInOut TRZ_SW; //TR Z Switch
-extern DigitalOut phase_TR_x;
-extern DigitalOut phase_TR_y;
-extern DigitalOut phase_TR_z;
+extern DigitalInOut phase_TR_x;
+extern DigitalInOut phase_TR_y;
+extern DigitalInOut phase_TR_z;
//CDMS
-extern DigitalOut CDMS_RESET; // CDMS RESET
+extern DigitalInOut CDMS_RESET; // CDMS RESET
extern uint8_t CDMS_SW_STATUS;
-extern DigitalOut CDMS_OC_FAULT;
+extern DigitalInOut CDMS_OC_FAULT;
//BCN
-extern DigitalOut BCN_SW; //Beacon switch
+extern DigitalInOut BCN_SW; //Beacon switch
extern uint8_t BCN_TX_STATUS;
extern uint8_t BCN_FEN;
extern uint8_t BCN_SPND_TX;
@@ -122,7 +123,7 @@
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 DigitalInOut BCN_TX_OC_FAULT;
extern uint8_t BCN_TMP;
extern void F_BCN();
extern void FCTN_BCN_TX_MAIN();
@@ -173,9 +174,9 @@
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 DigitalInOut EPS_CHARGER_FAULT;
+extern DigitalInOut EPS_CHARGER_STATUS;
+extern DigitalInOut EPS_BATTERY_GAUGE_ALERT;
extern void F_EPS();
extern AnalogIn CurrentInput;
@@ -465,10 +466,10 @@
//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);
+ telemetry[66] = B_SCZ_ANGLE>>4;
+ telemetry[66] = (telemetry[66]<<1) | alarmmode;
+ telemetry[66] = (telemetry[66]<<1) | controlmode_mms;
+ telemetry[66] = (telemetry[66]<<2);
//2 bit spare
for(int i=0;i<9;i++)
@@ -847,11 +848,24 @@
}
// 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]
+ uint16_t moment_ret;
+ telemetry[3] = 0x00;
+ telemetry[4] = ACS_STATUS;
+ moment_ret = float_to_uint16(-2.2,2.2,moment_comm[0]);
+ telemetry[5] = (uint8_t)(moment_ret>>8);
+ telemetry[6] = (uint8_t)moment_ret;
+ moment_ret = float_to_uint16(-2.2,2.2,moment_comm[0]);
+ telemetry[7] = (uint8_t)(moment_ret>>8);
+ telemetry[8] = (uint8_t)moment_ret;
+ moment_ret = float_to_uint16(-2.2,2.2,moment_comm[2]);
+ telemetry[9] = (uint8_t)(moment_ret>>8);
+ telemetry[10] = (uint8_t)moment_ret;
+
+ //FCTN_CONVERT_FLOAT(moment_comm[0],&telemetry[4]); //telemetry[4] - telemetry[7]
+ //FCTN_CONVERT_FLOAT(moment_comm[1],&telemetry[8]); //telemetry[8] - telemetry[11]
+ //FCTN_CONVERT_FLOAT(moment_comm[2],&telemetry[12]); //telemetry[12] - telemetry[15]
// to include commission TR as well
- for(uint8_t i=16;i<132;i++)
+ for(uint8_t i=11;i<132;i++)
{
telemetry[i]=0x00;
}
@@ -864,22 +878,28 @@
{
printf("HARDWARE_COMSN\r\n");
//ACK_L234_telemetry
+ uint8_t SENSOR_NO;
int init1=0;
int init2=0;
int data1=0;
int data2=0;
+
+ uint16_t assign_val;
float PWM_tc[3];
- PWM_tc[0]=(float(tc[4]))*1;
- PWM_tc[1]=(float(tc[4]))*1;
- PWM_tc[2]=(float(tc[4]))*1;
+ PWM_tc[0]=(float(tc[4]))/32768 - 1;
+ PWM_tc[1]=(float(tc[5]))/32768 - 1;
+ PWM_tc[2]=(float(tc[6]))/32768 - 1;
DRV_Z_EN = 1;
DRV_XY_EN = 1;
telemetry[0]=0xB0;
telemetry[1]=tc[0];
telemetry[2]=ACK_CODE;
+ telemetry[3] = 0x00;
+
+ SENSOR_NO = 0;
PWM1 = 0;
PWM2 = 0;
@@ -919,36 +939,65 @@
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x30;
}
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
+ telemetry[5] = (assign_val>>8);
+ telemetry[6] = assign_val;
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
+ telemetry[7] = (assign_val>>8);
+ telemetry[8] = assign_val;
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
+ telemetry[9] = (assign_val>>8);
+ telemetry[10] = assign_val;
+
+ assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[0]);
+ telemetry[11] = (assign_val>>8);
+ telemetry[12] = assign_val;
+
+ assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[1]);
+ telemetry[13] = (assign_val>>8);
+ telemetry[14] = assign_val;
+
+ assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[2]);
+ telemetry[15] = (assign_val>>8);
+ telemetry[16] = assign_val;
+
init2 = SENSOR_INIT();
if( init2 == 1)
{
data2 = SENSOR_DATA_ACQ();
}
- uint8_t ats_data=1;
+ //uint8_t ats_data=1;
if(data2 == 0)
{
ATS2_SW_ENABLE = 1;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
+ SENSOR_NO = 0;
if(data1 == 2)
{
ATS1_SW_ENABLE = 0;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ SENSOR_NO = 1;
}
else if(data1 == 3)
{
ATS1_SW_ENABLE = 0;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
+ SENSOR_NO = 1;
}
else if(data1 == 1)
{
ATS1_SW_ENABLE = 0;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
- ats_data = 0;
+ SENSOR_NO = 0;
+
}
}
@@ -962,6 +1011,7 @@
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
+ SENSOR_NO = 1;
}
else if(data1 == 3)
{
@@ -971,11 +1021,13 @@
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
+ SENSOR_NO = 1;
}
else
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
- ats_data = 0;
+ SENSOR_NO = 0;
+ //ats_data = 0;
}
}
@@ -989,16 +1041,46 @@
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
+ SENSOR_NO = 1;
}
else
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ SENSOR_NO = 2;
}
}
else if(data2==3)
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
+ SENSOR_NO = 2;
}
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
+ telemetry[17] = (assign_val>>8);
+ telemetry[18] = assign_val;
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
+ telemetry[19] = (assign_val>>8);
+ telemetry[20] = assign_val;
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
+ telemetry[21] = (assign_val>>8);
+ telemetry[22] = assign_val;
+
+ assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[0]);
+ telemetry[23] = (assign_val>>8);
+ telemetry[24] = assign_val;
+
+ assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[1]);
+ telemetry[25] = (assign_val>>8);
+ telemetry[26] = assign_val;
+
+ assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[2]);
+ telemetry[27] = (assign_val>>8);
+ telemetry[28] = assign_val;
+
+ telemetry[4] = ACS_ATS_STATUS;
+
SelectLineb3 =0;
SelectLineb2 =1;
@@ -1010,7 +1092,7 @@
PWM3 = 0;
wait_ms(ACS_DEMAG_TIME_DELAY);
- if(ats_data == 0)
+ if(SENSOR_NO != 0)
SENSOR_DATA_ACQ();
actual_data.current_actual[5]=CurrentInput.read();
actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
@@ -1026,7 +1108,7 @@
}
//to be edited final tele
- uint16_t assign_val;
+ //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;
@@ -1049,7 +1131,7 @@
wait_ms(ACS_DEMAG_TIME_DELAY);
- if(ats_data == 0)
+ if(SENSOR_NO != 0)
SENSOR_DATA_ACQ();
actual_data.current_actual[5]=CurrentInput.read();
actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
@@ -1064,10 +1146,22 @@
{
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)]);
+
+ assign_val = float_to_uint16(-100,100,actual_data.current_actual[5]);//assuming max min values for current to be diss
+ telemetry[37] = (assign_val>>8);
+ telemetry[38] = assign_val;
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
+ telemetry[39] = (assign_val>>8);
+ telemetry[40] = assign_val;
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
+ telemetry[41] = (assign_val>>8);
+ telemetry[42] = assign_val;
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
+ telemetry[43] = (assign_val>>8);
+ telemetry[44] = assign_val;
PWM1 = 0;
PWM2 = 0;
@@ -1075,7 +1169,7 @@
wait_ms(ACS_DEMAG_TIME_DELAY);
- if(ats_data == 0)
+ if(SENSOR_NO != 0)
SENSOR_DATA_ACQ();
actual_data.current_actual[5]=CurrentInput.read();
actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
@@ -1089,19 +1183,29 @@
{
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)]);
+ assign_val = float_to_uint16(-100,100,actual_data.current_actual[5]);//assuming max min values for current to be diss
+ telemetry[45] = (assign_val>>8);
+ telemetry[46] = assign_val;
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
+ telemetry[47] = (assign_val>>8);
+ telemetry[48] = assign_val;
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
+ telemetry[49] = (assign_val>>8);
+ telemetry[50] = assign_val;
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
+ telemetry[51] = (assign_val>>8);
+ telemetry[52] = assign_val;
+
PWM1 = 0;
PWM2 = 0;
PWM3 = 0;
wait_ms(ACS_DEMAG_TIME_DELAY);
- if(ats_data == 0)
+ if(SENSOR_NO != 0)
SENSOR_DATA_ACQ();
actual_data.current_actual[5]=CurrentInput.read();
actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
@@ -1115,10 +1219,18 @@
{
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)]);
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]);
+ telemetry[53] = (assign_val>>8);
+ telemetry[54] = assign_val;
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]);
+ telemetry[55] = (assign_val>>8);
+ telemetry[56] = assign_val;
+
+ assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]);
+ telemetry[57] = (assign_val>>8);
+ telemetry[58] = assign_val;
// for(int i=0; i<12; i++)
// FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]);
@@ -1126,7 +1238,10 @@
// FCTN_ATS_DATA_ACQ(); //get data
// to include commission TR as well
- for(uint8_t i=35;i<132;i++)
+
+ telemetry[58] = SENSOR_NO;
+
+ for(uint8_t i=60;i<132;i++)
{
telemetry[i]=0x00;
}
@@ -1427,10 +1542,11 @@
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 ;
-
+ //ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3) | 0x0C ;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF0) | 0x0C ;
ATS1_SW_ENABLE = 0;
- ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F);
+ //ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x0F)|0x40;
telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
@@ -1452,9 +1568,9 @@
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 ;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x0F) | 0xC0 ;
ATS2_SW_ENABLE = 0;
- ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF0)|0x04;
telemetry[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{