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:
- 34:1b41c34b12ea
- Parent:
- 33:76f2b8735501
- Child:
- 35:7193e581932f
--- a/TCTM.cpp Mon Jul 04 04:29:59 2016 +0000
+++ b/TCTM.cpp Mon Jul 04 07:01:26 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];
@@ -465,7 +466,7 @@
//assigned it to counter HTR_CYCLE_COUNTER
//assign it b_scz_angle
- telemetry[66] = 0x00;
+ telemetry[66] = B_SCZ_ANGLE;
telemetry[66] = (telemetry[65]<<1) | alarmmode;
telemetry[66] = (telemetry[65]<<1) | controlmode_mms;
telemetry[66] = (telemetry[65]<<2);
@@ -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]); = 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]); = float_to_uint16(-2.2,2.2,moment_comm[1]);
+ 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;
}
@@ -865,21 +879,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;
@@ -906,6 +927,7 @@
ATS2_SW_ENABLE = 0;
wait_ms(5);
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
+
}
else if(data1==1)
{
@@ -919,36 +941,68 @@
{
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 +1016,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 +1026,12 @@
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;
}
}
@@ -989,16 +1045,45 @@
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 +1095,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 +1111,7 @@
}
//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;
@@ -1049,7 +1134,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 +1149,21 @@
{
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 +1171,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);
@@ -1090,10 +1186,21 @@
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;
@@ -1101,7 +1208,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);
@@ -1115,10 +1222,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 +1241,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;
}