Team Fox
ACS data acq changed completely. Tested and working. Deals all faults.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
QM_BAE_review_1
by 
Team Fox
Revision 10:f93407b97750, committed 2016-04-11
- Comitter:
- lakshya
- Date:
- Mon Apr 11 17:26:46 2016 +0000
- Parent:
- 9:194afacf7449
- Child:
- 11:1fdb94ae6563
- Commit message:
- 11april
Changed in this revision
| ACS.cpp | Show annotated file Show diff for this revision Revisions of this file |
| TCTM.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/ACS.cpp Fri Apr 01 21:13:16 2016 +0000
+++ b/ACS.cpp Mon Apr 11 17:26:46 2016 +0000
@@ -11,15 +11,15 @@
//********************************flags******************************************//
extern uint32_t BAE_STATUS;
extern uint32_t BAE_ENABLE;
-extern uint8_t ACS_INIT_STATUS;
-extern uint8_t ACS_DATA_ACQ_STATUS;
-extern uint8_t ACS_ATS_STATUS;
-extern uint8_t ACS_MAIN_STATUS;
-extern uint8_t ACS_STATUS;
+extern char ACS_INIT_STATUS;
+extern char ACS_DATA_ACQ_STATUS;
+extern char ACS_ATS_STATUS;
+extern char ACS_MAIN_STATUS;
+extern char ACS_STATUS;
-extern uint8_t ACS_ATS_ENABLE;
-extern uint8_t ACS_DATA_ACQ_ENABLE;
-extern uint8_t ACS_STATE;
+extern char ACS_ATS_ENABLE;
+extern char ACS_DATA_ACQ_ENABLE;
+extern char ACS_STATE;
DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
@@ -37,122 +37,138 @@
extern BAE_HK_actual actual_data;
-//DigitalOut ATS1_SW_ENABLE(PTC0); // enable of att sens2 switch
-//DigitalOut ATS2_SW_ENABLE(PTC16); // enable of att sens switch
+//DigitalOut gpo1(PTC0); // enable of att sens2 switch
+//DigitalOut gpo2(PTC16); // enable of att sens switch
Serial pc_acs(USBTX,USBRX); //for usb communication
+//CONTROL_ALGO
+
+float moment[3]; // Unit: Ampere*Meter^2
+float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla
+int flag_firsttime=1, controlmode, alarmmode=0;
+
+
+
+void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode);
+void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax);
+float max_array(float arr[3]);
void inverse(float mat[3][3],float inv[3][3]);
-
-int ctrl_count = 0;
-float bcopy[3];
-float moment[3];
- ///////algo working well
+
+//CONTROLALGO PARAMETERS
+
+
void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
{
- float db[3];
- float bb[3]={0,0,0};
- float d[3]={0,0,0};
- float Jm[3][3]={{0.2730,0,0},{0,0.3018,0},{0,0,0.3031}};
- float den=0,den2;
- int i,j; //temporary variables
- float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0}; //outputs
- float invJm[3][3];
- float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4;
+ float b1[3];
+ float omega1[3];
+ b1[0] = b[0]/1000000.0;
+ b1[1] = b[1]/1000000.0;
+ b1[2] = b[2]/1000000.0;
+
+ omega1[0] = omega[0]*3.14159/180;
+ omega1[1] = omega[1]*3.14159/180;
+ omega1[2] = omega[2]*3.14159/180;
+ controller (moment, b1, omega1, b_old, alarmmode, flag_firsttime, controlmode);
- //................. calculating db values...........................
- if(ctrl_count!=0)
- {
- for(i=0;i<3;i++)
- db[i]= (b[i]-bcopy[i])/10;
- }
+}
+void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode)
+{
+ float db1[3]; // Unit: Tesla/Second
+ float sampling_time=10; // Unit: Seconds. Digital Control law excuted once in 10 seconds
+ float MmntMax=1.1; // Unit: Ampere*Meter^2
+ float OmegaMax=1*3.1415/180.0; // Unit: Radians/Second
+ float normalising_fact;
+ float b1_copy[3], omega1_copy[3], db1_copy[3];
+ int i;
+ if(flag_firsttime==1)
+ {
+ for(i=0;i<3;i++)
+ {
+ db1[i]=0; // Unit: Tesla/Second
+ }
+ flag_firsttime=0;
+ }
else
{
for(i=0;i<3;i++)
- db[i]= 0;
- }
- ctrl_count++;
- //..................................................................
- printf("\n\r Entered cntrl algo\n\r");
- for(int i=0; i<3; i++)
{
- printf("%f\t",omega[i]);
+ db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
}
- for(int i=0; i<3; i++)
- {
- printf("%f\t",b[i]);
- }
-
- //.........................algo......................................
- den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
- den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
- for(i=0;i<3;i++)
+ }
+
+ if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1)
{
- db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3));
- //db[i]/=den*den*den;
+ alarmmode=0;
}
- for(i=0;i<3;i++)
- {
- b[i]/=den;
- }
- // select kz, kmu, gamma
- if(b[0]>0.9||b[0]<-0.9)
+ else if(max_array(omega1)>OmegaMax && alarmmode==0)
{
- kz=kz2;
- kmu=kmu2;
- gamma=gamma2;
+ alarmmode=1;
}
- // calculate Mu, v, dv, z, u
- for(i=0;i<2;i++)
+
+ for (i=0;i<3;i++)
{
- Mu[i]=b[i+1];
- v[i]=-kmu*Mu[i];
- dv[i]=-kmu*db[i+1];
- z[i]=db[i+1]-v[i];
- u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
+ b1_copy[i]=b1[i];
+ db1_copy[i]=db1[i];
+ omega1_copy[i]=omega1[i];
}
- inverse(Jm,invJm);
- for(i=0;i<3;i++)
- {
- for(j=0;j<3;j++)
- bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
- }
- for(i=0;i<3;i++)
+
+ if(alarmmode==0)
+ {
+ controlmode=0;
+ controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
+ for (i=0;i<3;i++)
+ {
+ b1[i]=b1_copy[i];
+ db1[i]=db1_copy[i];
+ omega1[i]=omega1_copy[i];
+ }
+ if(max_array(moment)>MmntMax)
+ {
+ controlmode=1;
+ controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
+ for (i=0;i<3;i++)
+ {
+ b1[i]=b1_copy[i];
+ db1[i]=db1_copy[i];
+ omega1[i]=omega1_copy[i];
+ }
+ if(max_array(moment)>MmntMax)
+ {
+ normalising_fact=max_array(moment)/MmntMax;
+ for(i=0;i<3;i++)
+ {
+ moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
+ }
+ }
+ }
+ }
+ else
+ {
+ controlmode=1;
+ controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
+ for (i=0;i<3;i++)
+ {
+ b1[i]=b1_copy[i];
+ db1[i]=db1_copy[i];
+ omega1[i]=omega1_copy[i];
+ }
+ if(max_array(moment)>MmntMax)
+ {
+ normalising_fact=max_array(moment)/MmntMax;
+ for(i=0;i<3;i++)
+ {
+ moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
+ }
+ }
+
+ }
+ for (i=0;i<3;i++)
{
- for(j=0;j<3;j++)
- d[i]+=bb[j]*invJm[i][j];
- }
- bb[1]=u[0]+(d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2])+(omega[2]*db[0]);
- bb[2]=u[1]-(d[0]*b[1])+(d[1]*b[0])+(omega[0]*db[1])-(omega[1]*db[0]);
- bb[0]=0;
- for(i=0;i<3;i++)
- {
- d[i]=invJm[1][i];
- invJm[1][i]=b[2]*invJm[0][i]-b[0]*invJm[2][i];
- invJm[2][i]=-b[1]*invJm[0][i]+b[0]*d[i];
- invJm[0][i]=b[i];
+ b_old[i]=b1[i];
}
- inverse(invJm,Jm);
- printf("\n \r calculating tauc");
- for(i=0;i<3;i++)
- {
- for(j=0;j<3;j++)
- tauc[i]+=Jm[i][j]*bb[j]; // calculating torque values
- printf(" %f \t",tauc[i]);
- }
- //..........................tauc to moment conversion..........................
- printf("\n \r calculating moment");
- for(i=0;i<3;i++)
- bcopy[i]=b[i]*den;
- for(i=0;i<3;i++)
- {
- moment[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
- moment[i]/=den;
- printf(" %f \t",moment[i]);
- }
- printf("\n\r exited control algo\n");
}
-//..........................function to find inverse..................
+
void inverse(float mat[3][3],float inv[3][3])
{
int i,j;
@@ -160,16 +176,141 @@
for(i=0;i<3;i++)
{
for(j=0;j<3;j++)
+ {
inv[j][i]=(mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3])-(mat[(i+2)%3][(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
+ }
}
det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
for(i=0;i<3;i++)
- {
+ {
for(j=0;j<3;j++)
+ {
inv[i][j]/=det;
+ }
}
}
+float max_array(float arr[3])
+{
+ int i;
+ float temp_max=fabs(arr[0]);
+ for(i=1;i<3;i++)
+ {
+ if(fabs(arr[i])>temp_max)
+ {
+ temp_max=fabs(arr[i]);
+ }
+ }
+ return temp_max;
+}
+
+
+void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax)
+{
+ float bb[3]={0,0,0};
+ float d[3]={0,0,0};
+ float Jm[3][3]={{0.2271,0.0014,-0.0026},{0.0014,0.2167,-0.004},{-0.0026,-0.004,0.2406}}; // Unit: Kilogram*Meter^2. Jm may change depending on the final satellite structure
+ float den=0,den2;
+ float bcopy[3];
+ int i, j;//temporary variables
+ float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs
+ float invJm[3][3];
+ float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4,kdetumble=2000000;
+ int infflag; // Flag variable to check if the moment value is infinity or NaN
+
+ if(controlmode1==0)
+ {
+ den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
+ den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
+ for(i=0;i<3;i++)
+ {
+ db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
+ }
+ for(i=0;i<3;i++)
+ {
+ b[i]/=den; // Mormalized b. Hence no unit.
+ }
+ if(b[2]>0.9 || b[2]<-0.9)
+ {
+ kz=kz2;
+ kmu=kmu2;
+ gamma=gamma2;
+ }
+ for(i=0;i<2;i++)
+ {
+ Mu[i]=b[i];
+ v[i]=-kmu*Mu[i];
+ dv[i]=-kmu*db[i];
+ z[i]=db[i]-v[i];
+ u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
+ }
+ inverse(Jm,invJm);
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ {
+ bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
+ }
+ }
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ {
+ d[i]+=bb[j]*invJm[i][j];
+ }
+ }
+ bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]);
+ bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]);
+ bb[0]=0;
+ for(i=0;i<3;i++)
+ {
+ d[i]=invJm[2][i];
+ invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i];
+ invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
+ invJm[0][i]=b[i];
+ }
+ inverse(invJm,Jm);
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ {
+ tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
+ }
+ }
+ for(i=0;i<3;i++)
+ {
+ bcopy[i]=b[i]*den;
+ }
+ for(i=0;i<3;i++)
+ {
+ Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
+ Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2
+ }
+ infflag=0;
+ for (i=0; i<3 && infflag==0; i++)
+ {
+ if (isinf(Mmnt[i])==1 || isnan(Mmnt[i])==1)
+ infflag=1;
+ }
+ if (infflag==1)
+ {
+ for (i=0; i<3; i++)
+ Mmnt[i]=2*MmntMax;
+ }
+
+ }
+ else if(controlmode1==1)
+ {
+ for(i=0;i<3;i++)
+ {
+ Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2
+ }
+ }
+ for(i=0;i<3;i++)
+ {
+ moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2
+ }
+}
I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
@@ -199,7 +340,7 @@
void FCTN_ACS_INIT()
{
- ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
+ ACS_INIT_STATUS = 's'; //set ACS_INIT_STATUS flag
//FLAG();
pc_acs.printf("Attitude sensor init called \n \r");
//FLAG();
@@ -247,13 +388,13 @@
cmd[1]=BIT_EVT_ENB;
i2c.write(SLAVE_ADDR,cmd,2);
wait_ms(100);
- ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag
+ ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag
}
void FCTN_ATS_DATA_ACQ()
{
- ACS_DATA_ACQ_STATUS = 1; //set ACS_DATA_ACQ_STATUS flag for att sens 2
- if( ACS_ATS_ENABLE == 1)
+ ACS_DATA_ACQ_STATUS = 's'; //set ACS_DATA_ACQ_STATUS flag for att sens 2
+ if( ACS_ATS_ENABLE == 'e')
{
FLAG();
pc_acs.printf("attitude sensor execution called \n \r");
@@ -307,9 +448,9 @@
}
else //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
{
- ACS_DATA_ACQ_STATUS = 1;
+ ACS_DATA_ACQ_STATUS = 'f';
}
- ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
+ ACS_DATA_ACQ_STATUS = 'c'; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
}
void FCTN_ACS_GENPWM_MAIN(float Moment[3])
@@ -342,6 +483,176 @@
}
l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
+ printf("current in trx is %f \r \n",l_current_x);
+ if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
+ {
+ l_duty_cycle_x = 3*10000000*pow(l_current_x,3)- 90216*pow(l_current_x,2) + 697.78*l_current_x - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
+ printf("DC for trx is %f \r \n",l_duty_cycle_x);
+ PWM1.period(TIME_PERIOD);
+ PWM1 = l_duty_cycle_x/100 ;
+ }
+ else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
+ {
+ l_duty_cycle_x = - 76880*pow(l_current_x,3) + 1280.8*pow(l_current_x,2) + 583.78*l_current_x + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
+ printf("DC for trx is %f \r \n",l_duty_cycle_x);
+ PWM1.period(TIME_PERIOD);
+ PWM1 = l_duty_cycle_x/100 ;
+ }
+ else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
+ {
+ l_duty_cycle_x = 275.92*pow(l_current_x,2) + 546.13*l_current_x + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
+ printf("DC for trx is %f \r \n",l_duty_cycle_x);
+ PWM1.period(TIME_PERIOD);
+ PWM1 = l_duty_cycle_x/100 ;
+ }
+ else if(l_current_x==0)
+ {
+ printf("\n \r l_current_x====0");
+ l_duty_cycle_x = 0; // default value of duty cycle
+ printf("DC for trx is %f \r \n",l_duty_cycle_x);
+ PWM1.period(TIME_PERIOD);
+ PWM1 = l_duty_cycle_x/100 ;
+ }
+ else //not necessary
+ {
+ g_err_flag_TR_x = 1;
+ }
+
+ //------------------------------------- y-direction TR--------------------------------------//
+
+
+ float l_moment_y = Moment[1]; //Moment in y direction
+
+ phase_TR_y = 1; // setting the default current direction
+ if (l_moment_y <0)
+ {
+ phase_TR_y = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
+ l_moment_y = abs(l_moment_y);
+ }
+
+
+ l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
+ printf("current in try is %f \r \n",l_current_y);
+ if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
+ {
+ l_duty_cycle_y = 3*10000000*pow(l_current_y,3)- 90216*pow(l_current_y,2) + 697.78*l_current_y - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
+ printf("DC for try is %f \r \n",l_duty_cycle_y);
+ PWM2.period(TIME_PERIOD);
+ PWM2 = l_duty_cycle_y/100 ;
+ }
+ else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
+ {
+ l_duty_cycle_y = - 76880*pow(l_current_y,3) + 1280.8*pow(l_current_y,2) + 583.78*l_current_y + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
+ printf("DC for try is %f \r \n",l_duty_cycle_y);
+ PWM2.period(TIME_PERIOD);
+ PWM2 = l_duty_cycle_y/100 ;
+ }
+ else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
+ {
+ l_duty_cycle_y = 275.92*pow(l_current_y,2) + 546.13*l_current_y + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
+ printf("DC for try is %f \r \n",l_duty_cycle_y);
+ PWM2.period(TIME_PERIOD);
+ PWM2 = l_duty_cycle_y/100 ;
+ }
+ else if(l_current_y==0)
+ {
+ printf("\n \r l_current_y====0");
+ l_duty_cycle_y = 0; // default value of duty cycle
+ printf("DC for try is %f \r \n",l_duty_cycle_y);
+ PWM2.period(TIME_PERIOD);
+ PWM2 = l_duty_cycle_y/100 ;
+ }
+ else // not necessary
+ {
+ g_err_flag_TR_y = 1;
+ }
+
+ //----------------------------------------------- z-direction TR -------------------------//
+
+
+ float l_moment_z = Moment[2]; //Moment in z direction
+
+ phase_TR_z = 1; // setting the default current direction
+ if (l_moment_z <0)
+ {
+ phase_TR_z = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
+ l_moment_z = abs(l_moment_z);
+ }
+
+
+ l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
+ printf("current in trz is %f \r \n",l_current_z);
+ if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
+ {
+ l_duty_cycle_z = 3*10000000*pow(l_current_z,3)- 90216*pow(l_current_z,2) + 697.78*l_current_z - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
+ printf("DC for trz is %f \r \n",l_duty_cycle_z);
+ PWM3.period(TIME_PERIOD);
+ PWM3 = l_duty_cycle_z/100 ;
+ }
+ else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
+ {
+ l_duty_cycle_z = - 76880*pow(l_current_z,3) + 1280.8*pow(l_current_z,2) + 583.78*l_current_z + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
+ printf("DC for trz is %f \r \n",l_duty_cycle_z);
+ PWM3.period(TIME_PERIOD);
+ PWM3 = l_duty_cycle_z/100 ;
+ }
+ else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
+ {
+ l_duty_cycle_z = 275.92*pow(l_current_z,2) + 546.13*l_current_z + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
+ printf("DC for trz is %f \r \n",l_duty_cycle_z);
+ PWM3.period(TIME_PERIOD);
+ PWM3 = l_duty_cycle_z/100 ;
+ }
+ else if(l_current_z==0)
+ {
+ printf("\n \r l_current_z====0");
+ l_duty_cycle_z = 0; // default value of duty cycle
+ printf("DC for trz is %f \r \n",l_duty_cycle_z);
+ PWM3.period(TIME_PERIOD);
+ PWM3 = l_duty_cycle_z/100 ;
+ }
+ else // not necessary
+ {
+ g_err_flag_TR_z = 1;
+ }
+
+ //-----------------------------------------exiting the function-----------------------------------//
+
+ printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
+
+}
+
+
+/*void FCTN_ACS_GENPWM_MAIN(float Moment[3])
+{
+ printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
+
+ float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
+ float l_current_x=0; //Current sent in x TR's
+ float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
+ float l_current_y=0; //Current sent in y TR's
+ float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
+ float l_current_z=0; //Current sent in z TR's
+
+
+ for(int i = 0 ; i<3;i++)
+ {
+ printf("pwm %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
+ }
+
+ //----------------------------- x-direction TR --------------------------------------------//
+
+
+ float l_moment_x = Moment[0]; //Moment in x direction
+
+ phase_TR_x = 1; // setting the default current direction
+ if (l_moment_x <0)
+ {
+ phase_TR_x = 0; // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
+ l_moment_x = abs(l_moment_x);
+ }
+
+ l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
pc_acs.printf("current in trx is %f \r \n",l_current_x);
if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
{
@@ -349,7 +660,6 @@
pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
PWM1.period(TIME_PERIOD);
PWM1 = l_duty_cycle_x/100 ;
-
}
else if( l_current_x >= 0.006 && l_current_x < 0.0116)
{
@@ -501,7 +811,7 @@
printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
-}
+}*/
\ No newline at end of file
--- a/TCTM.cpp Fri Apr 01 21:13:16 2016 +0000
+++ b/TCTM.cpp Mon Apr 11 17:26:46 2016 +0000
@@ -8,6 +8,9 @@
#include "iostream"
#include "stdint.h"
#include "cassert"
+#include"math.h"
+
+
extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
@@ -16,23 +19,41 @@
extern DigitalOut TRZ_SW; //TR Z Switch
extern DigitalOut CDMS_RESET; // CDMS RESET
extern DigitalOut BCN_SW; //Beacon switch
-
+extern uint8_t BCN_TX_STATUS;
+extern uint8_t BCN_FEN;
extern BAE_HK_actual actual_data;
extern BAE_HK_min_max bae_HK_minmax;
extern uint32_t BAE_STATUS;
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_actual actual_data.power_mode;
+extern BAE_HK_quant quant_data;
extern void FCTN_ATS_DATA_ACQ();
extern void FCTN_ACS_CNTRLALGO(float*,float*);
+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* FCTN_BAE_TM_TC (uint8_t* tc)
{
uint8_t service_type=(tc[2]&0xF0);
uint8_t* tm;
uint16_t crc16;
-
+
switch(service_type)
{
@@ -49,27 +70,120 @@
}
case 0x02:
{
+ uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
+ switch(MID)
+ {
+
+ case 0x0001:
+ {
printf("Read from RAM\r\n");
- uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
- switch(MID )
- {
- case 0x0010:
- {
- printf("Read MUX DATA\r\n");
+ /*taking some varible till we find some thing more useful*/
+ //uint8_t ref_val=0x01;
tm[0] = 0x60;
tm[1] = tc[0];
tm[2] = ACK_CODE;
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[4+ (i*4)]); //tm[4] - tm[7]
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[35+(i*4)]); //
+ /*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*/
+ tm[3] = (BCN_SW);
+ tm[3] = tm[3]|(TRXY_SW_EN<<1);
+ tm[3] = tm[3]|(TRZ_SW_EN<<2);
+ tm[3] = tm[3]|(ATS1_SW_ENABLE<<3);
+ tm[3] = tm[3]|(ATS2_SW_ENABLE<<4);
+
+ if(BCN_TX_STATUS==2)
+ tm[3] = tm[3]|0x20;
+ else
+ tm[3] = tm[3] & 0xDF;
+
+ tm[3] = tm[3]|(BCN_FEN<<6);
+ tm[3] = tm[3]|(ACS_ACQ_DATA_ENABLE<<7);
+
+ /*not included in the code yet*/
+ tm[4] = BAE_RESET_COUNTER;
+
+ tm[5] = ACS_STATE;
+ tm[5] = tm[5]|(EN_BTRY_HT<<3);
+ tm[5] = tm[5]|(phase_TR_x<<4);
+ 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);
+ /**/
+ uint8_t soc_powerlevel_2=50;
+ uint8_t soc_powerlevel_3=65;
+
+ tm[6] = soc_powerlevel_2;
+ tm[7] = soc_powerlevel_3;
+
+ /*to be fixed*/
+ tm[8] = 0;
+ tm[9] = 0;
+ tm[10] = 0;
+ tm[11] = 0;
+ //tm[8] = Torque Rod X Offset;
+ //tm[9] = Torque Rod Y Offset;
+ //tm[10] = Torque Rod Z Offset;
+ //tm[11] = ACS_DEMAG_TIME_DELAY;
+ tm[12] = (BAE_STATUS>>24) & 0xFF;
+ tm[13] = (BAE_STATUS>>16) & 0xFF;
+ tm[14] = (BAE_STATUS>>8) & 0xFF;
+ tm[15] = BAE_STATUS & 0xFF;
- for(int i=0; i<16; i++) //16*4 = 64 bytes //tm[4] to tm[67] filled
- FCTN_CONVERT_FLOAT(actual_data.voltage_actual[i], &tm[4+(i*4)]);
- for(int i=0; i<12; i++) //12*4 = 48 //tm[68] to tm[115] filled
- FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&tm[68 + (i*4)]);
- for (int i=116; i<132;i++)
+ /*to be fixed*/
+ tm[16] = BCN_FAIL_COUNT;
+ tm[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;
+
+ 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;
+ tm[22] = P_BCN_TX_MAIN_COUNTER>>8;
+ tm[23] = P_BCN_TX_MAIN_COUNTER;
+ tm[24] = P_EPS_MAIN_COUNTER>>8;
+ tm[25] = P_EPS_MAIN_COUNTER;
+
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[26+ (i*4)]);
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[38+(i*4)]);
+
+ //FAULT_FLAG();
+ tm[50] = actual_data.faultIr_status;
+ 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 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]);
+
+ for (int i=0; i<16; i++)
+ tm[68+i] = quant_data.voltage_quant[i];
+ for (int i=0; i<12; i++)
+ tm[84+i] = quant_data.current_quant[i];
+ //tm[96]
+ //tm[97]
+ //tm[98]
+ //tm[99]
+ tm[100] = quant_data.Batt_voltage_quant;
+ tm[101] = quant_data.BAE_temp_quant;
+ tm[102] = quant_data.Batt_gauge_quant[1];
+ tm[103] = quant_data.Batt_temp_quant[0];
+ tm[104] = quant_data.Batt_temp_quant[1];
+
+ //tm[105] = beacon temperature;
+
+ for (int i=105; i<132;i++)
{
tm[i] = 0x00;
}
@@ -77,9 +191,84 @@
tm[132] = (uint8_t)((crc16&0xFF00)>>8);
tm[133] = (uint8_t)(crc16&0x00FF);
return tm;
- }
- //.........................
- /* case 0x0010:
+
+ }
+ case 0x0002:
+ {
+ tm[0] = 0x60;
+ tm[1] = tc[0];
+ tm[2] = ACK_CODE;
+
+ for(int i;i<16;i++)
+ tm[i+3] = BAE_HK_min_max 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[29] = BAE_HK_min_max bae_HK_minmax.Batt_voltage_max;;
+ tm[30] = BAE_HK_min_max 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];
+
+ /*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)]);
+
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(BAE_HK_min_max 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];
+
+ for(int i;i<12;i++)
+ tm[i+74] = BAE_HK_min_max 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;
+
+ /*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];
+
+ /*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)]);
+
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(BAE_HK_min_max bae_HK_minmax.AngularSpeed_min[i],&tm[103+(i*4)]);
+
+
+ for (int i=115; 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);
+ return tm;
+
+ }
+
+
+ }
+ }
+
+ /*
+ switch(tc[3])
+ {
+ case 0x01:
{
printf("Read MUX DATA\r\n");
tm[0] = 0x60;
@@ -97,19 +286,13 @@
tm[132] = (uint8_t)((crc16&0xFF00)>>8);
tm[133] = (uint8_t)(crc16&0x00FF);
return tm;
- }*/
- //..........................................
- case 0x2:
+ }
+ case 0x02:
{
printf("Read HK\r\n");
tm[0] = 0x60;
tm[1] = tc[0];
tm[2] = ACK_CODE;
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[4+ (i*4)]); //tm[4] - tm[7]
- for(int i=0; i<3; i++)
- FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[35+(i*4)]); //
-
FCTN_CONVERT_FLOAT(actual_data.Batt_temp_actual[0],&tm[4]); //tm[4]-tm[7]
FCTN_CONVERT_FLOAT(actual_data.Batt_temp_actual[1],&tm[8]); //tm[8]- tm[11]
for(int i=0; i<4; i++)
@@ -118,6 +301,10 @@
tm[32] = (uint8_t)actual_data.power_mode;
tm[33] = actual_data.faultPoll_status;
tm[34] = actual_data.faultIr_status;
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[35+(i*4)]); //35 -46
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[47+(i*4)]); //47 -58
FCTN_CONVERT_FLOAT(actual_data.Batt_voltage_actual,&tm[59]); //59 - 62
for (int i=63; i<132;i++)
{
@@ -129,7 +316,7 @@
return tm;
}
- case 0x3:
+ case 0x03:
{
printf("Read min max data");
tm[0] = 0x60;
@@ -181,7 +368,7 @@
tm[133] = (uint8_t)(crc16&0x00FF);
return tm;
}
- case 0x4:
+ case 0x04:
{
printf("Read status");
tm[0] = 0x60;
@@ -202,7 +389,8 @@
}
}
- }
+ */
+
case 0x05:
{
printf("Write on Flash\r\n");
@@ -246,19 +434,50 @@
{
case 0xE0:
{
+ float B[3],W[3];
printf("ACS_COMSN\r\n");
//ACK_L234_TM
+
+ 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]);
+
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
- FCTN_ATS_DATA_ACQ(); //get data
+ //FCTN_ATS_DATA_ACQ(); //get data
printf("gyro values\n\r");
for(int i=0; i<3; i++)
- printf("%f\n\r",data[i]);
+ printf("%f\n\r",W[i]);
printf("mag values\n\r");
- for(int i=3; i<6; i++)
- printf("%f\n\r",data[i]);
- FCTN_CONVERT_FLOAT(data[0],&tm[4]); //tm[4] - tm[7]
+ for(int i=0; i<3; i++)
+ printf("%f\n\r",B[i]);
+ /* FCTN_CONVERT_FLOAT(data[0],&tm[4]); //tm[4] - tm[7]
FCTN_CONVERT_FLOAT(data[1],&tm[8]); //tm[8] - tm[11]
FCTN_CONVERT_FLOAT(data[2],&tm[12]); //tm[12] - tm[15]
FCTN_CONVERT_FLOAT(data[0],&tm[16]); //tm[16] - tm[19]
@@ -272,16 +491,16 @@
tm[29] = 1; // mag values in correct range
else
tm[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;
+ */
+ // 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_ACS_CNTRLALGO(B,W);
FCTN_CONVERT_FLOAT(moment[0],&tm[30]); //tm[30] - tm[33]
FCTN_CONVERT_FLOAT(moment[1],&tm[34]); //tm[34] - tm[37]
FCTN_CONVERT_FLOAT(moment[2],&tm[38]); //tm[38] - tm[41]
@@ -295,31 +514,115 @@
tm[133] = (uint8_t)((crc16&0xFF00)>>8);
tm[134] = (uint8_t)(crc16&0x00FF);
return tm;
+ */
+
+ // Control algo commissioning
+ FCTN_ACS_CNTRLALGO(B,W);
+ FCTN_CONVERT_FLOAT(moment[0],&tm[4]); //tm[4] - tm[7]
+ FCTN_CONVERT_FLOAT(moment[1],&tm[8]); //tm[8] - tm[11]
+ FCTN_CONVERT_FLOAT(moment[2],&tm[12]); //tm[12] - tm[15]
+ // to include commission TR as well
+ for(uint8_t i=16;i<132;i++)
+ {
+ tm[i]=0x00;
+ }
+
+ crc16 = CRC::crc16_gen(tm,132);
+ tm[133] = (uint8_t)((crc16&0xFF00)>>8);
+ tm[134] = (uint8_t)(crc16&0x00FF);
+ return tm;
}
- /* case 0x02:
+
+
+
+ case 0xE1:
{
- printf("Run P_EPS_MAIN\r\n");
+ float moment_tc[3];
+ printf("HARDWARE_COMSN\r\n");
+ //ACK_L234_TM
+ uint8_t M0[2];
+ uint8_t M1[2];
+ uint8_t M2[2];
+
+ M0[0]=tc[3];
+ M0[1]=tc[4];
+ M1[0]=tc[5];
+ M1[1]=tc[6];
+ M2[0]=tc[7];
+ M2[1]=tc[8];
+
+
+ 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_ACS_GENPWM_MAIN(moment_tc);
+
+ 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[4]); //4-7
+ FCTN_CONVERT_FLOAT(PWM_measured[1],&tm[8]); //8-11
+ FCTN_CONVERT_FLOAT(PWM_measured[2],&tm[12]); //12-15
+ for(int i=0; i<12; i++)
+ FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&tm[16 + (i*4)]);
+
+
+ FCTN_ATS_DATA_ACQ(); //get data
+
+
+ // to include commission TR as well
+ for(uint8_t i=64;i<132;i++)
+ {
+ tm[i]=0x00;
+ }
+
+ crc16 = CRC::crc16_gen(tm,132);
+ tm[133] = (uint8_t)((crc16&0xFF00)>>8);
+ tm[134] = (uint8_t)(crc16&0x00FF);
+ return tm;
+
+ }
+ case 0x02:
+ {
+ F_EPS();
+ /* printf("Run P_EPS_MAIN\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
- for(uint8_t i=3;i<11;i++)
+ */
+ for(uint8_t i=0;i<133;i++)
{
tm[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
+ crc16 = CRC::crc16_gen(tm,132);
+ tm[132] = (uint8_t)((crc16&0xFF00)>>8);
+ tm[133] = (uint8_t)(crc16&0x00FF);
+ /*
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
+ */
+
return tm;
}
- case 0x03:
+
+ /* case 0x03:
{
- printf("Run P_ACS_INIT\r\n");
+
+ /* printf("Run P_ACS_INIT\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
@@ -357,9 +660,53 @@
}
return tm;
}
+ */
case 0x05:
{
printf("Run P_ACS_MAIN\r\n");
+ F_ACS();
+
+
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[(i*4)]);
+ for(int i=0; i<3; i++)
+ FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[12+(i*4)]);
+
+ tm[24] = ACS_STATE;
+ tm[24] = tm[5]|(EN_BTRY_HT<<3);
+ tm[24] = tm[5]|(phase_TR_x<<4);
+ 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]);
+
+ //ACK_L234_TM
+ /* tm[0]=0xB0;
+ tm[1]=tc[0];
+ tm[2]=ACK_CODE;
+ for(uint8_t i=3;i<11;i++)
+ {
+ tm[i]=0x00;
+ }
+ */
+ crc16 = CRC::crc16_gen(tm,37);
+ tm[37] = (uint8_t)((crc16&0xFF00)>>8);
+ tm[38] = (uint8_t)(crc16&0x00FF);
+ for(uint8_t i=39;i<134;i++)
+ {
+ tm[i]=0x00;
+ }
+ return tm;
+ }
+
+
+
+ case 0x06:
+ {
+ F_BCN();
+ /* printf("Run P_BCN_INIT\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
@@ -368,35 +715,17 @@
{
tm[i]=0x00;
}
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
- for(uint8_t i=13;i<134;i++)
+ */
+ crc16 = CRC::crc16_gen(tm,0);
+ tm[0] = (uint8_t)((crc16&0xFF00)>>8);
+ tm[1] = (uint8_t)(crc16&0x00FF);
+ for(uint8_t i=2;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
- case 0x06:
- {
- printf("Run P_BCN_INIT\r\n");
- //ACK_L234_TM
- tm[0]=0xB0;
- tm[1]=tc[0];
- tm[2]=ACK_CODE;
- for(uint8_t i=3;i<11;i++)
- {
- tm[i]=0x00;
- }
- crc16 = CRC::crc16_gen(tm,11);
- tm[11] = (uint8_t)((crc16&0xFF00)>>8);
- tm[12] = (uint8_t)(crc16&0x00FF);
- for(uint8_t i=13;i<134;i++)
- {
- tm[i]=0x00;
- }
- return tm;
- }
+ /*
case 0x07:
{
printf("Run P_BCN_TX_MAIN\r\n");
@@ -417,13 +746,14 @@
}
return tm;
}*/
- case 0x11:
+ case 0x11:
{
printf("SW_ON_ACS_ATS1_SW_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
+ ATS1_SW_ENABLE = 1; // making sure we switch off the other
ATS1_SW_ENABLE = 0;
for(uint8_t i=3;i<11;i++)
{
@@ -444,6 +774,7 @@
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
+ ATS1_SW_ENABLE = 1; //make sure u switch off the other
ATS2_SW_ENABLE = 0;
tm[2]=1;
for(uint8_t i=3;i<11;i++)