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
Revision 21:81c888bb6125, committed 2016-07-01
- Comitter:
- lakshya
- Date:
- Fri Jul 01 17:59:04 2016 +0000
- Parent:
- 20:949d13045431
- Commit message:
- just checking
Changed in this revision
--- a/ACS.cpp Fri Jul 01 17:55:30 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,1590 +0,0 @@
-/*------------------------------------------------------------------------------------------------------------------------------------------------------
--------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
-#include <mbed.h>
-#include <math.h>
-
-#include "pni.h" //pni header file
-#include "pin_config.h"
-#include "ACS.h"
-#include "EPS.h"
-/*variables will get get updated value from FLash
-in case flash cups while testing i.e initial defaul values are kept as of now
-*/
-//********************************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 uint8_t ACS_DETUMBLING_ALGO_TYPE;//////
-
-extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
-extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
-
-extern uint8_t ACS_ATS_ENABLE;
-extern uint8_t ACS_DATA_ACQ_ENABLE;
-extern uint8_t ACS_STATE;
-
-DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
-DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
-DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
-
-extern PwmOut PWM1; //x //Functions used to generate PWM signal
-extern PwmOut PWM2; //y
-extern PwmOut PWM3; //z //PWM output comes from pins p6
-
-int g_err_flag_TR_x=0; // setting x-flag to zero
-int g_err_flag_TR_y=0; // setting y-flag to zero
-int g_err_flag_TR_z=0; // setting z-flag to zero
-
-extern float data[6];
-extern BAE_HK_actual actual_data;
-
-//global para
-//FUNCTION
-float max_invjm [9]= {1.0000,1.0000,1.0000,0.0471,4.6159,4.1582,4.4047,0.0755,4.1582};
-float min_invjm[9] = {-1.0000,-1.0000,-1.0000,-0.0471,-4.6159,-4.1582,-4.4047,-0.0755,-4.1582};
-float max_jm[9] = {0.3755,0.0176,0.2672,0.4895,0.2174,0.0452,1.0000,0.1209,0.0572};
-float min_jm[9] = {-0.2491,-0.0457,0.2271,0.1556,0.2222,0.0175,0.9998,0.0361,0.0922};
-//se some other better way
-/*
-float max_bb[3] = {0,1.0*e-04*0.1633,1.0*e-04*0.1528};
-float min_bb[3] = {0,1.0*e-04*(-0.1736),1.0*e-04*(-0.1419)};
-*/
-float max_bb[3] = {0,1.0*0.0001*0.1633,1.0*0.0001*0.1528};
-float min_bb[3] = {0,1.0*0.0001*(-0.1736),1.0*0.0001*(-0.1419)};
-
-//ACS
-uint8_t controlmode_mms = 0;
-uint8_t ATS1_EVENT_STATUS_RGTR=0x00;
-uint8_t ATS1_SENTRAL_STATUS_RGTR=0x00;
-uint8_t ATS1_ERROR_RGTR=0x00;
-uint8_t ATS2_EVENT_STATUS_RGTR=0x00;
-uint8_t ATS2_SENTRAL_STATUS_RGTR=0x00;
-uint8_t ATS2_ERROR_RGTR=0x00;
-uint8_t invjm_mms[9];
-uint8_t jm_mms[9];
-uint8_t bb_mms[3];
-uint8_t singularity_flag=0;
-
-uint8_t ACS_MAG_TIME_DELAY;// = 65;
-uint8_t ACS_DEMAG_TIME_DELAY;// = 65;
-uint16_t ACS_Z_FIXED_MOMENT;// = 1.3;
-uint8_t ACS_TR_Z_SW_STATUS;//=1;
-uint8_t ACS_TR_XY_SW_STATUS;//=1;
-//GLOBAL PARA
-uint8_t ACS_TR_X_PWM; //*
-uint8_t ACS_TR_Y_PWM; //*
-uint8_t ACS_TR_Z_PWM; //*
-//change
-uint16_t ACS_MM_X_COMSN = 1;
-uint16_t ACS_MM_Y_COMSN = 1;
-uint16_t ACS_MG_X_COMSN = 1;
-uint16_t ACS_MG_Y_COMSN = 1;
-uint16_t ACS_MM_Z_COMSN = 1;
-uint16_t ACS_MG_Z_COMSN = 1;
-
-uint8_t float_to_uint8(float min,float max,float val)
-{
- if(val>max)
- {return 0xff;
- }
- if(val<min)
- {return 0x00;
- }
- float div=max-min;div=(255.0/div);val=((val-min)*div);
- return (uint8_t)val;
-}
-
-
-void float_to_uint8_ARRAY(int d1,int d2, float *arr,float max[], float min[], uint8_t *valarr)
-{
- for(int i=0;i<d1;i++)
- for(int j=0;j<d2;j++)
- {
- printf("\n\r%f",*((arr+(i*d1))+j));
- valarr[i*d1+j] = (uint8_t)float_to_uint8(min[i*d1+j],max[i*d1+j],*((arr+(i*d1))+j));
- printf("\n\r%d",valarr[i*d1+j]);
- }
-}
-
-
-
-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
-float db[3];//*
-uint8_t flag_firsttime=1, alarmmode=0;
-
-
-void controlmodes(float b[3], float db[3], float omega[3], uint8_t controlmode1); //*
-float max_array(float arr[3]);
-void inverse(float mat[3][3],float inv[3][3]);
-
-//CONTROLALGO PARAMETERS
-void FCTN_ACS_CNTRLALGO (float moment[3],float b[3] ,float omega[3],uint8_t nominal,uint8_t detumbling,uint8_t ACS_DETUMBLING_ALGO_TYPE)
-{
-
- float normalising_fact;
- float b_copy[3], omega_copy[3], db_copy[3];
- int i;
- if(flag_firsttime==1)
- {
- for(i=0;i<3;i++)
- {
- db[i]=0; // Unit: Tesla/Second
- }
- flag_firsttime=0;
- }
- else
- {
- for(i=0;i<3;i++)
- {
- db[i]= (b[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
- }
- }
-
- if(nominal == 0)
-
- {
-
- if(max_array(omega)<(0.8*OmegaMax) && alarmmode==1)
- {
- alarmmode=0;
- }
- else if(max_array(omega)>OmegaMax&& alarmmode==0)
- {
- alarmmode=1;
- }
-
- }
-
- for (i=0;i<3;i++)
- {
- b_copy[i]=b[i];
- db_copy[i]=db[i];
- omega_copy[i]=omega[i];
- }
-
- if(((alarmmode==0)|| (nominal == 1))&&(detumbling==0))
- {
- controlmode_mms = 0;
- controlmodes(moment,b,db,omega,0x00,ACS_DETUMBLING_ALGO_TYPE);
- for (i=0;i<3;i++)
- {
- b[i]=b_copy[i];
- db[i]=db_copy[i];
- omega[i]=omega_copy[i];
- }
- if(max_array(moment)>MmntMax)
- {
- controlmode_mms = 1;
- controlmodes(moment,b,db,omega,0x01,ACS_DETUMBLING_ALGO_TYPE);
- for (i=0;i<3;i++)
- {
- b[i]=b_copy[i];
- db[i]=db_copy[i];
- omega[i]=omega_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
- }
- }
- }
- ACS_STATUS = 5;//*is this changed now
- }
- else
- {
- controlmode_mms = 1;
- controlmodes(moment,b,db,omega,0x01,ACS_DETUMBLING_ALGO_TYPE);
- for (i=0;i<3;i++)
- {
- b[i]=b_copy[i];
- db[i]=db_copy[i];
- omega[i]=omega_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++)
- {
- b_old[i]=b[i];
- }
-}
-
-void inverse(float mat[3][3],float inv[3][3],uint8_t &singularity_flag)
-{
- int i,j;
- float det=0;
- 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]);
- if (det==0)
- {
- singularity_flag=1;
- }
- else
- {
- singularity_flag=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;
-}
-
-uint8_t singularity_flag_mms=0;
-
-void controlmodes(float moment[3],float b[3], float db[3], float omega[3], uint8_t controlmode1,uint8_t ACS_DETUMBLING_ALGO_TYPE)
-{
- 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;
- //uint8_t singularity_flag=0;
-
- 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]);
- if (den==0)
- {
- singularity_flag_mms=1;
- }
- if (singularity_flag_mms==0)
- {
- 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,singularity_flag_mms);
- 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,singularity_flag_mms);
-
- //00000
- float_to_uint8_ARRAY(3,3, (float*)invJm,max_invjm, min_invjm, invjm_mms);
- float_to_uint8_ARRAY(3,3, (float*)Jm,max_jm, min_jm, jm_mms);
- float_to_uint8_ARRAY(1,3, (float*)bb,max_bb, min_bb, bb_mms);
-
- if (singularity_flag_mms==0)
- {
- 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
- }
- }
- }
- if (singularity_flag_mms==1)
- {
- for (i=0;i<3;i++)
- {
- Mmnt[i]=2*MmntMax;
- }
- }
- ACS_STATUS = 5;
- }
- else if(controlmode1==1)
- {
- if (ACS_DETUMBLING_ALGO_TYPE==0) // BOmega Algo
- {
- 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
- }
- ACS_STATUS = 6;
- }
- else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo
- {
- for(i=0;i<3;i++)
- {
- Mmnt[i]=-kdetumble*db[i];
- }
- ACS_STATUS = 4;
- }
- }
- 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
-
-int FCTN_ACS_INIT(); //initialization of registers happens
-int SENSOR_INIT();
-int FCTN_ATS_DATA_ACQ(); //data is obtained
-int SENSOR_DATA_ACQ();
-//void T_OUT(); //timeout function to stop infinite loop
-
-int CONFIG_UPLOAD();
-//Timeout to; //Timeout variable to
-int toFlag;
-
-int count =0; // Time for which the BAE uC is running (in seconds)
-//void T_OUT()
-//{
-// toFlag=0; //as T_OUT function gets called the while loop gets terminated
-//}
-
-
-//DEFINING VARIABLES
-char cmd[2];
-char raw_gyro[6];
-char raw_mag[6];
-char reg_data[24];
-char store,status;
-//int16_t bit_data done in actual_data structure itself;
-
-uint16_t time_data;
-float gyro_data[3], mag_data[3];
-//float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
-
-int ack;
-int CONFIG_UPLOAD()
-{
- cmd[0]=RESETREQ;
- cmd[1]=BIT_RESREQ;
- i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
- wait_ms(575);
-
- //Verify magic number
-
- cmd[0]=HOST_CTRL; //0x02 is written in HOST CONTROL register to enable upload
- cmd[1]=BIT_HOST_UPLD_ENB;
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(20);
-
- cmd[0]=UPLOAD_ADDR; //0x02 is written in HOST CONTROL register to enable upload
- cmd[1]=0x0000;
- i2c.write(SLAVE_ADDR,cmd,3);
- wait_ms(20);
-
- cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to free upload
- cmd[1]=0x00;
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(20);
-
- return 0;
-}
-
-int SENSOR_INIT()
-{
-/// pc_acs.printf("Entered sensor init\n \r");
- cmd[0]=RESETREQ;
- cmd[1]=BIT_RESREQ;
- ack = i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
- //wait_ms(575); //waiting for loading configuration file stored in EEPROM
-
-/// pc_acs.printf("ACK for reset is %d\r\n",ack); //waiting for loading configuration file stored in EEPROM
-
- if( ack!=0)
- {
- cmd[0]=RESETREQ;
- cmd[1]=BIT_RESREQ;
- ack = i2c.write(SLAVE_ADDR,cmd,2); //repeat
- if(ack !=0)
- return 0;
- }
-
- wait_ms(575);
-
- cmd[0]=SENTRALSTATUS;
- ack = i2c.write(SLAVE_ADDR,cmd,1);
-
- if( ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- return 0;
- }
-
- ack = i2c.read(SLAVE_ADDR_READ,&store,1);
-
- if( ack!=0)
- {
- ack = i2c.read(SLAVE_ADDR_READ,&store,1);
- if(ack!=0)
- return 0;
- }
-
-/// pc_acs.printf("Sentral Status is %x\n \r",(int)store);
-
- //to check whether EEPROM is uploaded properly
- switch((int)store) {
- case(3): {
- break;
- }
- case(11): {
- break;
- }
- default: {
- cmd[0]=RESETREQ;
- cmd[1]=BIT_RESREQ;
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if( ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if(ack!=0)
- return 0;
- }
- wait_ms(575);//should be 600
-
- cmd[0]=SENTRALSTATUS;
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if( ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- return 0;
- }
- ack = i2c.read(SLAVE_ADDR_READ,&store,1);
- if( ack!=0)
- {
- ack = i2c.read(SLAVE_ADDR_READ,&store,1);
- if(ack!=0)
- return 0;
- }
-/// pc_acs.printf("Sentral Status is %x\n \r",(int)store);
-
- }
- }
-
- int manual=0;
- if( ((int)store != 11 )&&((int)store != 3))
- {
-
- cmd[0]=RESETREQ;
- cmd[1]=BIT_RESREQ;
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if( ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if(ack!=0)
- return 0;
- }
- wait_ms(575);
-
- manual = CONFIG_UPLOAD();
-
- if(manual == 0)
- {
- //MANUAL CONFIGURATION FAILED
- return 0;
- }
-
- }
- cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
- cmd[1]=BIT_RUN_ENB;
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if( ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if(ack!=0)
- return 0;
- }
-
- cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
- cmd[1]=BIT_MAGODR;
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if( ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if(ack!=0)
- return 0;
- }
-
- cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
- cmd[1]=BIT_GYROODR;
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if( ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if(ack!=0)
- return 0;
- }
-
- cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
- cmd[1]=0x00;
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if( ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if(ack!=0)
- return 0;
- }
- //wait_ms(20);
- cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to scaled sensor values
- cmd[1]=0x00;
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if( ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if(ack!=0)
- return 0;
- }
-
- cmd[0]=ENB_EVT; //Enabling the CPU reset , error,gyro values and magnetometer values
- cmd[1]=BIT_EVT_ENB;
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if( ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,2);
- if(ack!=0)
- return 0;
- }
-
- cmd[0]=SENTRALSTATUS;
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if( ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- return 0;
- }
-
- ack = i2c.read(SLAVE_ADDR_READ,&store,1);
- if( ack!=0)
- {
- ack= i2c.read(SLAVE_ADDR_READ,&store,1);
- if(ack!=0)
- return 0;
- }
-
-/// pc_acs.printf("Sentral Status after initialising is %x\n \r",(int)store);
-
- if( (int)store == 3) //Check if initialised properly and not in idle state
- {
-/// pc_acs.printf("Exited sensor init successfully\n \r");
- return 1;
- }
-
-
-//// pc_acs.printf("Sensor init failed \n \r") ;
- return 0;
-}
-
-int FCTN_ACS_INIT()
-{
- ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
-
-
- int working=0;
-
-/// pc_acs.printf("Attitude sensor init called \n \r");
-/// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
-
-
- if(((ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) //Sensor1 status is not 10 or 11
- {
-
-/// pc_acs.printf("Sensor 1 marked working \n \r");
- working = SENSOR_INIT();
- if(working ==1)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
-/// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); //Sensor 1 INIT successful
-/// pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r");
- ACS_INIT_STATUS = 0;
- return 1;
- }
-
-
-
-/// pc_acs.printf("Sensor 1 not working.Powering off.\n \r"); //Sensor 1 INIT failure and power off
- ATS1_SW_ENABLE = 1;
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
-
- }
-
-/// pc_acs.printf("Sensor 1 not working. Trying Sensor 2\n \r");
-
- if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) //Sensor1 status is not 10 or 11
- {
-
-
- ATS2_SW_ENABLE = 0;
- wait_ms(5);
- working = SENSOR_INIT();
- if(working ==1)
- {
-/// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
-/// pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r"); //Sensor2 INIT successful
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
- ACS_INIT_STATUS = 0;
- return 2;
- }
-
- ATS2_SW_ENABLE = 1;
- wait_ms(5);
-
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
-
-
- }
-
-/// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
-/// pc_acs.printf("Sensor 2 also not working.Exit init.\n \r");
-
- ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working
- return 0;
-}
-
-
-int SENSOR_DATA_ACQ()
-{
- //int mag_only=0;
-/// pc_acs.printf("Entering Sensor data acq.\n \r");
- char status;
- int sentral;
- int event;
- int sensor;
- int error;
- int init;
-
- uint8_t gyro_error=0;
- uint8_t mag_error=0;
-
- //int ack1;
- //int ack2;
-
- cmd[0]=EVT_STATUS;
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- return 0;
- }
-
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- {
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- return 0;
- }
-
- event = (int)status;
-
- if(ACS_ATS_STATUS&0xC0 == 0x40)
- {
- ATS1_EVENT_STATUS_RGTR = (uint8_t)event;
- }
- else if(ACS_ATS_STATUS&0x0C == 0x04)
- {
- ATS2_EVENT_STATUS_RGTR = (uint8_t)event;
- }
-
- cmd[0]=SENTRALSTATUS;
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- return 0;
- }
-
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- {
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- return 0;
- }
-
-
- sentral = (int) status;
-
- if(ACS_ATS_STATUS&0xC0 == 0x40)
- {
- ATS1_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
- }
- else if(ACS_ATS_STATUS&0x0C == 0x04)
- {
- ATS2_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
- }
-
-/// pc_acs.printf("Event Status is %x\n \r",event);
-/// pc_acs.printf("Sentral Status is %x\n \r",sentral);
-
-
-
- if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 )|| (sentral!= 3)) //check for any error in event status register
- {
-
-
- init = SENSOR_INIT();
-
- cmd[0]=EVT_STATUS;
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- return 0;
- }
-
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- {
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- return 0;
- }
-
- event = (int)status;
-
- cmd[0]=SENTRALSTATUS;
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- return 0;
- }
-
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- {
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- return 0;
- }
-
- sentral = (int)status;
-
-/// pc_acs.printf("Event Status after resetting and init is %x\n \r",event);
-
- if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 ) || (init == 0)||(sentral != 3)) //check for any error in event status
- {
-
- cmd[0]=ERROR;
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- return 0;
- }
-
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- {
-
- if(ACS_ATS_STATUS&0xC0 == 0x40)
- {
- ATS1_ERROR_RGTR = 0x01;
- }
- else if(ACS_ATS_STATUS&0x0C == 0x04)
- {
- ATS2_ERROR_RGTR = 0x01;
- }
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- return 0;
- }
-
- error = (int)status;
-
- if(ACS_ATS_STATUS&0xC0 == 0x40)
- {
- ATS1_ERROR_RGTR = (uint8_t)error;
- }
- else if(ACS_ATS_STATUS&0x0C == 0x04)
- {
- ATS2_ERROR_RGTR = (uint8_t)error;
- }
-
- cmd[0]=SENSORSTATUS;
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- return 0;
- }
-
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- {
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- return 0;
- }
-
- sensor = (int)status;
-
-
- if((error!=0) || (sensor!=0))
- {
- if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) )
- {
- pc_acs.printf("error in gyro alone..\n \r");
- gyro_error = 1;
- }
-
- if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) )
- {
-
- pc_acs.printf("error in mag alone.Exiting.\n \r");
- mag_error = 1;
- }
- if( (gyro_error!=1)&&(mag_error!=1))
- {
- pc_acs.printf("error in something else.Exiting.\n \r");
- return 0;
-
- }
- }
-
-
- if((event & 1 == 1 ))
- {
-/// pc_acs.printf("error in CPU Reset.\n \r");
- return 0;
-
- }
-
- if((event & 8 != 8 )||(event & 32 != 32 ))
- {
- pc_acs.printf("Data not ready waiting...\n \r");
- //POLL
- wait_ms(200);
-
- cmd[0]=EVT_STATUS;
-
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- {
- ack = i2c.write(SLAVE_ADDR,cmd,1);
- if(ack!=0)
- return 0;
- }
-
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- {
- ack = i2c.read(SLAVE_ADDR_READ,&status,1);
- if(ack!=0)
- return 0;
- }
-
- event = (int)status;
- if(event & 32 != 32 )
- {
-
- pc_acs.printf("Mag data only ready.Read..\n \r");
- gyro_error = 1;
-
- }
-
- if(event & 8 != 8 )
- {
- pc_acs.printf("Both data still not ready.Exiting..\n \r");
- mag_error=1;
- }
-
-
- }
-
-
- }
-
- if((mag_error !=1)&&(gyro_error!=1))
- {
- pc_acs.printf("Error in something else.Exiting.\n \r");
- return 0;
- }
-
- if((mag_error ==1)&&(gyro_error==1))
- {
- pc_acs.printf("Error in both gyro and mag.Exiting.\n \r");
- return 0;
- }
-
- }
-
-
- cmd[0]=MAG_XOUT_H; //LSB of x
- i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
- ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
- if(ack != 0)
- {
- cmd[0]=MAG_XOUT_H; //LSB of x
- i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
- ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
- if(ack !=1)
- return 0;
-
- }
-
-
- // pc_acs.printf("\nGyro Values:\n");
- if (gyro_error!=1)
- {
- for(int i=0; i<3; i++) {
- //concatenating gyro LSB and MSB to get 16 bit signed data values
- actual_data.bit_data_acs_mg[i]= ((int16_t)reg_data[16+2*i+1]<<8)|(int16_t)reg_data[16+2*i];
- gyro_data[i]=(float)actual_data.bit_data_acs_mg[i];
- gyro_data[i]=gyro_data[i]/senstivity_gyro;
- actual_data.AngularSpeed_actual[i] = gyro_data[i];
- }
- }
-
- if(mag_error!=1)
- {
- for(int i=0; i<3; i++) {
- //concatenating mag LSB and MSB to get 16 bit signed data values Extract data
- actual_data.bit_data_acs_mm[i]= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i];
-
- mag_data[i]=(float)actual_data.bit_data_acs_mm[i];
- mag_data[i]=mag_data[i]/senstivity_mag;
- actual_data.Bvalue_actual[i] = mag_data[i];
- }
- }
-
-
- if(mag_error == 1)
- {
-
- pc_acs.printf("Gyro only successful.\n \r");
- return 1;
- }
- if(gyro_error == 1)
- {
- pc_acs.printf("Mag only successful.\n \r");
- return 2;
- }
-
- pc_acs.printf("Reading data success.\n \r");
- return 3;
-}
-
-
-int FCTN_ATS_DATA_ACQ()
-{
- for(int i=0; i<3; i++) {
- actual_data.AngularSpeed_actual[i] = 0;
- actual_data.Bvalue_actual[i] = 0;
- }
-
- int acq;
- int init;
-
-//// pc_acs.printf("DATA_ACQ called \n \r");
-//// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
-
-
- if(( (ACS_ATS_STATUS & 0xC0) == 0x40))
- {
-
- acq = SENSOR_DATA_ACQ();
- if(acq == 3)
- {
-
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
-
- //??ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
-//// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
-//// pc_acs.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
- return 3;
- }
- else if((acq == 2)||(acq==1))
- {
- pc_acs.printf(" Sensor 1 data partial success.Try other sensor.\n \r");
- if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) )
- {
- //other sensor both working, off or
- //other sensor gyro working, this sensor not working , off
- //other sensor mag working, this sensor not working,off
-
- ATS1_SW_ENABLE = 1; //switch off sensor 1
- wait_ms(5);
- if(acq == 1)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status
- }
- if(acq==2)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
- }
-
- ATS2_SW_ENABLE = 0; //switch on sensor 2
- wait_ms(5);
-
- init = SENSOR_INIT(); //sensor 2 init
- if( init == 0)
- {
- pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
- ATS2_SW_ENABLE = 1;
- wait_ms(5);
- ATS1_SW_ENABLE = 0;
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
- if(acq == 1)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status
- }
- if(acq==2)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
- }
- return acq;
- }
-
- int acq2;
- acq2 = SENSOR_DATA_ACQ();
- if(acq2 == 3)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
- pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
- return 3;
- }
- else if(acq2 == 1)
- {
- if(acq==2)
- {
- ATS2_SW_ENABLE = 1;
- wait_ms(5);
- ATS1_SW_ENABLE = 0; //Sensor 2 gyro only,sensor 1 mag only
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
- return 3;
- }
- else
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only
- return 1;
- }
- }
-
- else if(acq2==2) //Sensor 2 mag only, exit in both cases
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
- return 2;
- }
- else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1
- {
- pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
- ATS2_SW_ENABLE = 1;
- wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
- ATS1_SW_ENABLE = 0;
- wait_ms(5);
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
- return acq;
- }
-
- }
- else //Sensor 2 not working or both sensors gyro/mag ONLY
- {
- if(acq == 1)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq
- return 1;
- }
- if(acq==2)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
- return 2;
- }
- pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
- return acq;
-
- }
- }
-
- else if(acq == 0)
- {
- pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
- ATS1_SW_ENABLE = 1;
- wait_ms(5); //Switch ON sensor 2
- ATS2_SW_ENABLE = 0;
- wait_ms(5);
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
- if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX
- {
- init = SENSOR_INIT();
- if( init == 0)
- {
- pc_acs.printf(" Sensor 2 also data acq failure.\n \r");
- ATS2_SW_ENABLE = 1;
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit
- return 0;
- }
-
- int acq2;
- acq2 = SENSOR_DATA_ACQ();
- if(acq2 == 3)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
- pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
- return 3;
- }
- else if(acq2 == 1)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
- return 1;
- }
- else if(acq2 == 2)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
- return 2;
- }
- else if(acq2 == 0)
- {
- pc_acs.printf(" Sensor 2 data acq failure..\n \r");
- ATS2_SW_ENABLE = 1;
-
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
- return 0;
- }
-
- }
-
- }
-
-
- }
-
- if(( (ACS_ATS_STATUS & 0x0C) == 0x04))
- {
- acq = SENSOR_DATA_ACQ(); //ATS2 should already be on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only
- if(acq == 3) //Both available read and exit
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
- pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
- pc_acs.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
- return 3;
- }
- else if((acq == 2)||(acq==1)) //Only mag or only gyro
- {
- pc_acs.printf(" Sensor 2 data partial success.Try other sensor.\n \r");
- if((ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) )
- {
- //other sensor both working, off or
- //other sensor gyro working, this sensor not working , off
- //other sensor mag working, this sensor not working,off
- ATS2_SW_ENABLE = 1; //switch off sensor 2
- wait_ms(5);
- if(acq == 1)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status
- }
- if(acq==2)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
- }
-
- ATS1_SW_ENABLE = 0; //switch on sensor 1
- wait_ms(5);
- init = SENSOR_INIT(); //sensor 2 init
-
- if( init == 0)
- {
- pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
- ATS1_SW_ENABLE = 1;
- wait_ms(5);
- ATS2_SW_ENABLE = 0;
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
- if(acq == 1)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status
- }
- if(acq==2)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
- }
- return acq;
- }
-
- int acq2;
- acq2 = SENSOR_DATA_ACQ();
-
- if(acq2 == 3)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
- pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
- return 3;
- }
-
- else if(acq2 == 1)
- {
- if(acq==2)
- {
- ATS1_SW_ENABLE = 1;
- wait_ms(5);
- ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
- return 3;
- }
- else
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only
- return 1;
- }
- }
-
- else if(acq2==2) //Sensor 1 mag only, exit in both cases
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
- return 2;
- }
- else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
- {
- pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
- ATS1_SW_ENABLE = 1;
- wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
- ATS2_SW_ENABLE = 0;
- wait_ms(5);
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04;
- return acq;
- }
-
- }
- else //Sensor 1 not working or both sensors gyro/mag ONLY
- {
- if(acq == 1)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq
- return 1;
- }
- if(acq==2)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
- return 2;
- }
- pc_acs.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
- return acq;
-
- }
- }
- else if(acq == 0)
- {
- pc_acs.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
- ATS2_SW_ENABLE = 1;
- wait_ms(5); //Switch ON sensor 1
- ATS1_SW_ENABLE = 0;
- wait_ms(5);
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
- if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
- {
- init = SENSOR_INIT();
- if( init == 0)
- {
- pc_acs.printf(" Sensor 1 also data acq failure.\n \r");
- ATS2_SW_ENABLE = 1;
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit
- return 0;
- }
-
- int acq2;
- acq2 = SENSOR_DATA_ACQ();
- if(acq2 == 3)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
- pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
- return 3;
- }
- else if(acq2 == 1)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50;
- return 1;
- }
- else if(acq2 == 2)
- {
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
- return 2;
- }
- else if(acq2 == 0)
- {
- pc_acs.printf(" Sensor 1 data acq failure..\n \r");
- ATS1_SW_ENABLE = 1;
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
- return 0;
- }
- }
- }
- }
- pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
- pc_acs.printf(" Both sensors data acq failure.Exiting.\n \r");
- return 0;
-}
-
-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
-
-
-//// printf("\r\r");
-
- //----------------------------- 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
-//// 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;
- }
- pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
-
- //------------------------------------- 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;
- }
- pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
- //----------------------------------------------- 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;
- }
- pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
-
-//changed
- if(phase_TR_x)
- ACS_TR_X_PWM = float_to_uint8(-1,1,PWM1);
- else
- ACS_TR_X_PWM = float_to_uint8(-1,1,-PWM1);
- if(phase_TR_y)
- ACS_TR_Y_PWM = float_to_uint8(-1,1,PWM2);
- else
- ACS_TR_Y_PWM = float_to_uint8(-1,1,-PWM2);
- if(phase_TR_z)
- ACS_TR_Z_PWM = float_to_uint8(-1,1,PWM3);
- else
- ACS_TR_Z_PWM = float_to_uint8(-1,1,-PWM2);
-
- //-----------------------------------------exiting the function-----------------------------------//
-
-//// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
-
-}
-
\ No newline at end of file
--- a/ACS.h Fri Jul 01 17:55:30 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,28 +0,0 @@ -#include "mbed.h" -#include "math.h" -#include "pni.h" - -//........................................... -#define TIME_PERIOD 0.02 -#define TR_CONSTANT 0.3 -#define sampling_time 10 -#define kdetumble 2000000 -#define MmntMax 1.1 // Unit: Ampere*Meter^2 -#define OmegaMax 1*3.1415/180.0 // Unit: Radians/Second -//#define ACS_DEMAG_TIME_DELAY 65 -//#define ACS_Z_FIXED_MOMENT 1.3 ka lvl assign kar de;;;;;;;; - -#define senstivity_gyro 6.5536; //senstivity is obtained from 2^15/5000dps -#define senstivity_mag 32.768; //senstivity is obtained from 2^15/1000microtesla -#define senstivity_time 32; //senstivity is obtained from 2^16/2048dps - -void FCTN_ACS_GENPWM_MAIN(float*); -void FCTN_ACS_CNTRLALGO(float*,float*,float*,uint8_t,uint8_t,uint8_t); -void controlmodes(float*,float*, float*, float*, uint8_t,uint8_t); -void inverse(float mat[3][3],float inv[3][3]); -extern void FLAG(); - -void FCTN_ATS_SWITCH(bool); -int FCTN_ACS_INIT(); //initialization of registers happens -void FCTN_T_OUT(); //timeout function to stop infinite loop -int FCTN_ATS_DATA_ACQ();
--- a/BCN.cpp Fri Jul 01 17:55:30 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,693 +0,0 @@
-#include "BCN.h"
-#include <stdio.h>
-#include "pin_config.h"
-//Check the pin names
-//Takes max 4.3 sec in void FCTN_BCN_TX_MAIN() (temp.calc. + long_beacon + short_beacon)
-
-Serial pc_bcn(USBTX, USBRX); //tx,rx
-SPI spi(D11, D12, D13); // mosi, miso, sclk
-DigitalOut cs(D10); //slave select or chip select
-//SPI spi(PIN16, PIN17, PIN15); // mosi, miso, sclk
-//DigitalOut cs(PIN6); //slave select or chip select
-Timer t_i;//timer for checking the time taken by (temp.calc. + long_beacon + short_beacon)
-//Timer t_is;//timer for short_beacon
-//Timer t_il;//timer for long_beacon
-Timeout rf_sl_timeout;//RF_silence timer
-Ticker loop;//for transmitting every 10 secs
-
-//GLOBAL VARIABLES
-uint8_t BCN_INIT_STATUS = 0;
-uint8_t BCN_TX_MAIN_STATUS = 0;
-uint8_t BCN_TX_STATUS = 0;
-uint8_t BCN_TX_ENABLE = 1; //hardcoding for now //check where is this variable toggled??
-uint8_t BCN_TX_SW_STATUS = 1;
-uint8_t BCN_FEN = 0; //hardcoding for now //write this value to flash
-uint8_t BCN_SPND_TX = 0; //hardcoding for now //check where is this variable toggled??
-uint8_t BCN_TMP = 0; // For Temperature
-uint8_t ERROR_CHECK = 0;
-uint8_t BCN_FAIL_COUNT = 0; //Flag for keeping count of no. of times of BCN failure in init or one transmission in 30 secs (failure in spi communication)
- //This Flag when exceeds a threshold, uC should reset.
-uint16_t BCN_TX_MAIN_COUNTER = 0;
-
-void FCTN_BCN_INIT()
-{
- pc_bcn.printf("FCTN_BCN_INIT\n");
- BCN_INIT_STATUS = 1;
- if( BCN_TX_SW_STATUS == 0b00000001 )
- Init_BEACON_HW();
- else Set_BCN_TX_STATUS(BCN_TX_DISABLED);
- // if(BCN_FEN == 0)//BCN_FEN is in flash
- // rf_sl_timeout.attach(&FCTN_BCN_FEN, RF_SILENCE_TIME);
- BCN_INIT_STATUS = 0;
-}
-
-extern uint32_t FCTN_BAE_RD_FLASH_ENTITY(uint16_t);
-extern void FCTN_BAE_WR_FLASH(uint16_t ,uint32_t );
-
-void FCTN_BCN_FEN(const void*)
-{
- pc_bcn.printf("FCTN_FEN\n\r");
- BCN_FEN = 1;//write this value to flash
- uint32_t FLASH_DATA;
- FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0);
- FLASH_DATA = (FLASH_DATA | 0x00001000);//see if uint8 to uint32 conversion works
- FCTN_BAE_WR_FLASH(0,FLASH_DATA);
-}
-void FCTN_BCN_TX_MAIN()
-{
- ERROR_CHECK=0;
- pc_bcn.printf("\n\nFCTN_BCN_TX_MAIN\n\r");
- t_i.start();
- int begin = t_i.read_us();
- //int begins,beginl,endl;
- int begintx,endtx;
- BCN_TX_MAIN_STATUS = 1;
- BCN_TX_MAIN_COUNTER++;
- if(BCN_FEN == 1)
- {
- if(BCN_TX_SW_STATUS == 0b00000001)
- {
- //Measure and store BCN temperature in BCN_TS_BUFFER
- BCN_TMP = check_Temperature();
- pc_bcn.printf("temperature = %d\n\r",BCN_TMP);
- //Get BCN_HK data from BCN HW(SPI) //Store BCN_HK data in BCN_HK_BUFFER
- if(BCN_SPND_TX == 1 )
- {
- Set_BCN_TX_STATUS(BCN_TX_STANDBY);
- BCN_TX_MAIN_STATUS = 0;
-
- // break;
- }
- else
- {
- //transmit short beacon and long beacon
- begintx = t_i.read_us();
- BCN_TX();
- endtx = t_i.read_us();
- /*
- begins = t_i.read_us();
- SHORT_BCN_TX();
- //ends = t_i.read_us();
-
- beginl = t_i.read_us();
- LONG_BCN_TX();
- endl = t_i.read_us();
- */
-
- if(Check_ACK_RECEIVED() == 1)
- {
- Set_BCN_TX_STATUS(BCN_TX_SUCCESS);
- BCN_TX_MAIN_STATUS = 0;
- }
- else
- {
- Set_BCN_TX_STATUS(BCN_TX_FAILURE);
- BCN_FAIL_COUNT++;
- Init_BEACON_HW();
- BCN_TX_MAIN_STATUS = 0;
-
- }
- }
- }
- else
- {
- Set_BCN_TX_STATUS(BCN_TX_DISABLED);
- BCN_TX_MAIN_STATUS = 0;
- }
- }
- else
- {
- Set_BCN_TX_STATUS(BCN_RF_SILENCE); //Window of RF Silence: None of the Txs should be on.
- BCN_TX_MAIN_STATUS = 0;
- }
- t_i.stop();
- int end = t_i.read_us();
- pc_bcn.printf("The time required for FCTN_BCN_TX_MAIN is %d useconds\r\n", end-begin);
- pc_bcn.printf("The time required for FCTN_BCN_TX is %d useconds\r\n", endtx-begintx);
- /*
- pc_bcn.printf("The time required for Short_BCN is %d useconds\r\n", beginl-begins);
- pc_bcn.printf("The time required for Long_BCN is %d useconds\r\n", endtx-beginl);
- */
- ERROR_CHECK = 0;
-
-}
-
-void Set_BCN_TX_STATUS(uint8_t STATUS)
-{
- BCN_TX_STATUS = STATUS;
-}
-
-uint8_t check_Temperature()
-{
- uint8_t temperature;
- writereg(RF22_REG_0F_ADC_CONFIGURATION,0x00);//set ADC to temp measurement
- writereg(RF22_REG_12_Temperature_Sensor_Calibration,0x20);//measure in degree celsius
- writereg(RF22_REG_0F_ADC_CONFIGURATION,0x80);//start adc
- wait(0.1);
- temperature = readreg(RF22_REG_11_ADC_Value);
- temperature = (float)temperature*0.5 - 64;
- return temperature;
-}
-#if 0
-void SHORT_BCN_TX()
-{
- /*
- writereg(RF22_REG_6E_TX_DATA_RATE,0x01);
- writereg(RF22_REG_6F_TX_DATA_RATE,0x50);//160bps
- */
- writereg(RF22_REG_6E_TX_DATA_RATE,0x0A);
- writereg(RF22_REG_6F_TX_DATA_RATE,0x7C);//1280bps
-
- //writereg(RF22_REG_3E_PACKET_LENGTH,1); //short packet length
- wait(0.02);
-
- uint32_t timeout_count = 10e5;
- //extract values from short_beacon[]
-
- struct Short_beacon
- {
- uint8_t Voltage[1];
- uint8_t AngularSpeed[2];
- uint8_t SubsystemStatus[1];
- uint8_t Temp[3];
- uint8_t ErrorFlag[1];
- }Shortbeacon = { {0xFF}, {0xFF, 0xFF} , {0xFF},{0xFF,0xFF,0xFF}, {0xFF} };
-
- //filling hk data
- //uint8_t short_beacon[] = { 0xAB, 0x8A, 0xE2, 0xBB, 0xB8, 0xA2, 0x8E,Shortbeacon.Voltage[0],Shortbeacon.AngularSpeed[0], Shortbeacon.AngularSpeed[1],Shortbeacon.SubsystemStatus[0],Shortbeacon.Temp[0],Shortbeacon.Temp[1],Shortbeacon.Temp[2],Shortbeacon.ErrorFlag[0]};
- uint8_t short_beacon[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,Shortbeacon.Voltage[0],Shortbeacon.AngularSpeed[0], Shortbeacon.AngularSpeed[1],Shortbeacon.SubsystemStatus[0],Shortbeacon.Temp[0],Shortbeacon.Temp[1],Shortbeacon.Temp[2],Shortbeacon.ErrorFlag[0]};
-
- //writereg(RF22_REG_07_OPERATING_MODE1,0x01); //ready mode ??
- clearTxBuf(); //writing data first time
-
- int byte_count=0;
- uint8_t onebyte[4] = {0x81,0xA6,0xBE,0x4E};
- uint8_t zerobyte[4] = {0x7E,0x59,0x41,0xB1};
- for (int byte_counter = 0; byte_counter <15 ; byte_counter++)
- {
- //pc_bcn.printf("byte-counter=%d\r\n", byte_counter);
- /*
- for(int j = 3; j >= 0 ; j--)
- {
- if((short_beacon[byte_counter] & (uint8_t) pow(2.0,(j*2+1)))!= pow(2.0,(j*2+1)))
- {
- byte=0x00;
- }
- else
- {
- byte=0xF0;
- }
- if((short_beacon[byte_counter] & (uint8_t) pow(2.0,j*2))!= pow(2.0,j*2))
- {
- byte=byte | 0x00;
- }
- else
- {
- byte=byte | 0x0F;
- }
- cs = 0;
- spi.write(0xFF);
- spi.write(byte);
- cs = 1;1
- }
- */
- for(int j = 7; j >= 0 ; j--)
- {
- cs = 0;
- spi.write(0xFF);
- if((short_beacon[byte_counter] & (uint8_t) pow(2.0,j))!= pow(2.0,j))
- {
- //byte=0x00;
- spi.write(zerobyte[0]);
- spi.write(zerobyte[1]);
- spi.write(zerobyte[2]);
- spi.write(zerobyte[3]);
- }
- else
- {
- //byte=0xFF;
- spi.write(onebyte[0]);
- spi.write(onebyte[1]);
- spi.write(onebyte[2]);
- spi.write(onebyte[3]);
- }
- cs = 1;
- /*
- spi.write(byte);
- spi.write(byte);
- spi.write(byte);
- spi.write(byte); //Each bit is written 32 times
- */
- byte_count+=4;
-
- }
-
- if(byte_counter == 1)
- {
-
- //Set to Tx mode
- writereg(RF22_REG_07_OPERATING_MODE1,0x08); //txon
- wait(0.1);// takes time to set to tx mode hence the delay of 0.1.
-
- }
- //testing level
- /*if(byte_counter > 0)
- while(1)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x40) == 0x00)break;else
- {
- pc_bcn.printf("w_f_empty1\n\r");
- //reset_rfm(1);
- }*/
-
-
- //Check for fifoThresh
-
- if(byte_counter > 0)
- {wait_ms(25);
- while(1)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x20) == 0x20)break;else
- {
- //pc_bcn.printf("w_f_empty\n");
- //reset_rfm(1);
- }
- }
-
- /* if(byte_counter%2==0 && byte_counter)
- while(1)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x40) == 0x00)break;else
- {
- pc_bcn.printf("W = %d\r\n",byte_counter);
- }*/
- pc_bcn.printf("W = %d\r\n",byte_counter);
- }
-
- //Check for fifoThresh
- while(1)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x40) == 0x00)break;else
- {
- pc_bcn.printf("Waiting for fifo to empty\r\n");
- }
-
- //Check for packetsent interrupt
- while(timeout_count--)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x04) == 0x04)break;else if(timeout_count == 1) reset_rfm(1);
-
- pc_bcn.printf("Short packet sent, bytes written = %d\r\n",byte_count);
-
- writereg(RF22_REG_07_OPERATING_MODE1,0x00); //standby mode
-
-}
-#endif
-void BCN_TX()
-{
- printf("BCN_TX\n\r");
- writereg(RF22_REG_6E_TX_DATA_RATE,0x0A);
- writereg(RF22_REG_6F_TX_DATA_RATE,0x7C);//1280bps
-
- wait(0.02);
-
- uint32_t timeout_count = 10e5;
- //extract values from short_beacon[]
-
- uint8_t Long_beacon[LONG_TX_DATA];
- for(int i = 0;i<LONG_TX_DATA;i++)
- {
- Long_beacon[i] = 0xFF;
-}
-
- struct Short_beacon
- {
- uint8_t Voltage[1];
- uint8_t AngularSpeed[2];
- uint8_t SubsystemStatus[1];
- uint8_t Temp[3];
- uint8_t ErrorFlag[1];
- }Shortbeacon = { {0xFF}, {0xFF, 0xFF} , {0xFF},{0xFF,0xFF,0xFF}, {0xFF} };
-
- //filling hk data
- //uint8_t short_beacon[] = { 0xAB, 0x8A, 0xE2, 0xBB, 0xB8, 0xA2, 0x8E,Shortbeacon.Voltage[0],Shortbeacon.AngularSpeed[0], Shortbeacon.AngularSpeed[1],Shortbeacon.SubsystemStatus[0],Shortbeacon.Temp[0],Shortbeacon.Temp[1],Shortbeacon.Temp[2],Shortbeacon.ErrorFlag[0]};
- uint8_t short_beacon[] = { 0xF3, 0x02, 0xFA, 0xC6, 0xD4, 0x28, 0x8A,Shortbeacon.Voltage[0],Shortbeacon.AngularSpeed[0], Shortbeacon.AngularSpeed[1],Shortbeacon.SubsystemStatus[0],Shortbeacon.Temp[0],Shortbeacon.Temp[1],Shortbeacon.Temp[2],Shortbeacon.ErrorFlag[0]};
-
- //writereg(RF22_REG_07_OPERATING_MODE1,0x01); //ready mode ??
- clearTxBuf(); //writing data first time
-
- int byte_count=0, byte_counter;
- uint8_t onebyte[4] = {0x81,0xA6,0xBE,0x4E};
- uint8_t zerobyte[4] = {0x7E,0x59,0x41,0xB1};
- for (byte_counter = 0; byte_counter <15 ; byte_counter++)
- {
- for(int j = 7; j >= 0 ; j--)
- {
- cs = 0;
- spi.write(0xFF);
- if((short_beacon[byte_counter] & (uint8_t) pow(2.0,j))!= pow(2.0,j))
- {
- //byte=0x00;
- spi.write(zerobyte[0]);
- spi.write(zerobyte[1]);
- spi.write(zerobyte[2]);
- spi.write(zerobyte[3]);
- }
- else
- {
- //byte=0xFF;
- spi.write(onebyte[0]);
- spi.write(onebyte[1]);
- spi.write(onebyte[2]);
- spi.write(onebyte[3]);
- }
- cs = 1;
- /*
- spi.write(byte);
- spi.write(byte);
- spi.write(byte);
- spi.write(byte); //Each bit is written 32 times
- */
- byte_count+=4;
-
- }
-
- if(byte_counter == 1)
- {
- //Set to Tx mode
- writereg(RF22_REG_07_OPERATING_MODE1,0x08); //txon
- wait(0.1);// takes time to set to tx mode hence the delay of 0.1.
-
- }
- //testing level
- /*if(byte_counter > 0)
- while(1)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x40) == 0x00)break;else
- {
- pc_bcn.printf("w_f_empty1\n\r");
- //reset_rfm(1);
- }*/
-
-
- //Check for fifoThresh
-
- if(byte_counter > 0)
- {wait_ms(25);
- while(1)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x20) == 0x20)break;else
- {
- //pc_bcn.printf("w_f_empty\n");
- //reset_rfm(1);
- }
- }
-
- /* if(byte_counter%2==0 && byte_counter)
- while(1)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x40) == 0x00)break;else
- {
- pc_bcn.printf("W = %d\r\n",byte_counter);
- }*/
- //pc_bcn.printf("W = %d\r\n",byte_counter);
- }
-
- for(byte_counter = 15;byte_counter<142;byte_counter++)
- {
- cs = 0;
- spi.write(0xFF);
- spi.write(Long_beacon[byte_counter-15]);
- cs = 1;
-
- if((byte_counter-15)%32==0)
- { //Check for fifoThresh
- wait_ms(25);
- while(1)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x20) == 0x20)break;else //if(timeout_count == 1)
- {
- //pc_bcn.printf("Reset\n");
- //reset_rfm(1);
- }
- }
-
- }
- wait_ms(70);
- //Check for fifoThresh
- while(1)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x40) == 0x00)break;else
- {
- pc_bcn.printf("Waiting for fifo to empty\r\n");
- }
-
- //Check for packetsent interrupt
- while(timeout_count--)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x04) == 0x04)break;else if(timeout_count == 1) reset_rfm(1);
-
- //pc_bcn.printf("Short packet sent, bytes written = %d\r\n",byte_count);
-
- writereg(RF22_REG_07_OPERATING_MODE1,0x00); //standby mode
-
-}
-#if 0
-void LONG_BCN_TX()
-{
- /*
- writereg(RF22_REG_6E_TX_DATA_RATE,0x04);
- writereg(RF22_REG_6F_TX_DATA_RATE,0xEA);//600 bps
- */
- writereg(RF22_REG_3E_PACKET_LENGTH,LONG_TX_DATA); //long packet length
- wait(0.02);
- uint32_t timeout_count=10e5;
-
- //get long_beacon array
-
- uint8_t Long_beacon[LONG_TX_DATA];
- for(int i = 0;i<LONG_TX_DATA;i++)
- {
- Long_beacon[i] = 0xFF;
- }
-
- clearTxBuf();
- //writing data first time
- cs = 0;
- spi.write(0xFF);
- for(int i=0; i<64;i++)
- {
- spi.write(Long_beacon[i]);
- }
- cs = 1;
-
- //Set to Tx mode
- writereg(RF22_REG_07_OPERATING_MODE1,0x08);
- wait(0.1);//necessary
-
- //Check for fifoThresh
- while(timeout_count--)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x20) == 0x20)break;else if(timeout_count == 1)
- {
- pc_bcn.printf("Reset\n");
- reset_rfm(1);
- }
- timeout_count=10e5;
-
- cs = 0;
- spi.write(0xFF);
- for(int i=64; i<127;i++)
- {
- spi.write(Long_beacon[i]);
- }
- cs = 1;
- wait(0.1);
- //Check for fifoThresh
- while(timeout_count--)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x20) == 0x20)break;else if(timeout_count == 1)
- {
- pc_bcn.printf("Reset\n");
- reset_rfm(1);}
- timeout_count=10e5;
-
- //Check for packetsent interrupt
- while(timeout_count--)if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x04) == 0x04)break;else if(timeout_count == 1) reset_rfm(1);
-
- //pc_bcn.printf("Long packet sent\r\n");
-
- writereg(RF22_REG_07_OPERATING_MODE1,0x00); //standby mode
-}
-#endif
-void reset_rfm(uint8_t fl)
-{
- if (fl ==1 && ERROR_CHECK ==0)
- { BCN_FAIL_COUNT++;
- ERROR_CHECK=1;
- printf("BCN_FAIL_COUNT++\n");
- }
- else if(fl == 0)
- {
- BCN_FAIL_COUNT = 0;
- ERROR_CHECK=0;
- }
-}
-void writereg(uint8_t reg,uint8_t val)
-{
-
- uint8_t count = 0;
- for(;;count++)
- {
- int read_val =0; cs = 0;spi.write(reg | 0x80);spi.write(val);cs = 1;
- if(reg != 0x7 && reg != 0x58 && reg != 0xF)
- {
- read_val = readreg(reg);
- if (read_val == val)
- {
- break;
- }
- else if(count == 5)
- {
- reset_rfm(1); printf("reg = 0x%X\n",reg);break;
- }
- else init_spi();
- }
- else
- break;
- }
-}
-uint8_t readreg(uint8_t reg)
-{
- uint8_t val;cs = 0;spi.write(reg & ~0x80);val = spi.write(0);cs = 1;return val;
-}
-void clearTxBuf()
-{
- writereg(RF22_REG_08_OPERATING_MODE2,0x01);
- writereg(RF22_REG_08_OPERATING_MODE2,0x00);
-}
-uint8_t setFrequency(double centre)
-{
- uint8_t fbsel = 0x40;
- if (centre >= 480.0) {
- centre /= 2;
- fbsel |= 0x20;
- }
- centre /= 10.0;
- double integerPart = floor(centre);
- double fractionalPart = centre - integerPart;
-
- uint8_t fb = (uint8_t)integerPart - 24; // Range 0 to 23
- fbsel |= fb;
- uint16_t fc = fractionalPart * 64000;
- writereg(RF22_REG_73_FREQUENCY_OFFSET1, 0); // REVISIT
- writereg(RF22_REG_74_FREQUENCY_OFFSET2, 0);
- writereg(RF22_REG_75_FREQUENCY_BAND_SELECT, fbsel);
- writereg(RF22_REG_76_NOMINAL_CARRIER_FREQUENCY1, fc >> 8);
- writereg(RF22_REG_77_NOMINAL_CARRIER_FREQUENCY0, fc & 0xff);
- return 0;
-}
-void init_spi()
-{
- cs=1; // chip must be deselected
- wait(0.1);
- spi.format(8,0);
- spi.frequency(10000000); //10MHz SCLK
-}
-void Init_BEACON_HW()
-{
- printf("Init HW\n\r");
- ERROR_CHECK=0;
- wait(0.1);
- int BCN_INIT_COUNTER = 1;
- while(BCN_INIT_COUNTER)
- {
- init_spi();
- printf("init spi\r\n");
- //should either have a flag for invalid SPI or discard this for actual case or add reset
- if (readreg(RF22_REG_00_DEVICE_TYPE) == 0x08)
- {
- pc_bcn.printf("spi connection valid\r\n");
- reset_rfm(0);
- Set_BCN_TX_STATUS(BCN_TX_INITIALIZED);
- break;
- }
- else if (BCN_INIT_COUNTER == 1)
- {
- pc_bcn.printf("error in spi connection\r\n");
- reset_rfm(1);
- writereg(RF22_REG_07_OPERATING_MODE1,0x80); //sw_reset
- wait(0.1); //takes time to reset
- }
- else if (BCN_INIT_COUNTER == 2)
- {
- pc_bcn.printf("error in spi connection\r\n");
- reset_rfm(1);
- BCN_TX_SW_STATUS = 0b00000011; //Device disabled
- //reset BCN HW using switch here
- }
- else
- {
- pc_bcn.printf("BCN_TX_FAILURE\r\n");
- Set_BCN_TX_STATUS(BCN_TX_FAILURE);
- break;
- }
- BCN_INIT_COUNTER++;
- }
-
- if(BCN_TX_STATUS == BCN_TX_INITIALIZED)
- {
- writereg(RF22_REG_07_OPERATING_MODE1,0x80); //sw_reset
- wait(0.1); //takes time to reset
-
- clearTxBuf();
-
- writereg(RF22_REG_07_OPERATING_MODE1,0x00); //standby mode
-
- //txfifoalmostempty
- writereg(RF22_REG_7D_TX_FIFO_CONTROL2,30);
-
- //txfifoalmostfull
- writereg(RF22_REG_7C_TX_FIFO_CONTROL1,50);
-
-
- //Packet-engine registers
- writereg(RF22_REG_30_DATA_ACCESS_CONTROL,0x00);
-
- writereg(RF22_REG_33_HEADER_CONTROL2,0x08);
- writereg(RF22_REG_34_PREAMBLE_LENGTH,0x00);
-
- writereg(RF22_REG_0B_GPIO_CONFIGURATION0,0x15); // TX state
- writereg(RF22_REG_0C_GPIO_CONFIGURATION1,0x12); // RX state
-
- setFrequency(435.0);
-
- int i=3;
- while(i--)
- if((readreg(RF22_REG_02_DEVICE_STATUS)& 0x08)!= 0x00)
- {
- setFrequency(435.0);
- if (i==1)
- {pc_bcn.printf("frequency not set properly\r\n");
- reset_rfm(1);
- }
- }
-
- //set Modem Configuration
- writereg(RF22_REG_58,0x80);
-
- //Set Data rate - same for both long and short beacon
-
- writereg(RF22_REG_6E_TX_DATA_RATE,0x0A);
- writereg(RF22_REG_6F_TX_DATA_RATE,0x7C);//1280bps
-
-
- writereg(RF22_REG_70_MODULATION_CONTROL1,0x20);
- writereg(RF22_REG_71_MODULATION_CONTROL2,0x21);//ook = 0x21
-
- //set tx power
- writereg(RF22_REG_6D_TX_POWER,0x07); //20dbm
-
- //TX_packet_length written later
- ERROR_CHECK = 0;
- printf("Done Init HW\n\r");
- }
-}
-bool Check_ACK_RECEIVED()
-{
- if((readreg(RF22_REG_03_INTERRUPT_STATUS1) & 0x04) == 0x04)
- {
- printf("Packet sent: ACK received\r\n");
- return 1;
- }
- else
- {
- pc_bcn.printf("Packet not sent\r\n");
- return 0;
- }
-}
-/*
-int main()
-{
- FCTN_BCN_INIT();
-
- loop.attach(&FCTN_BCN_TX_MAIN, 10.0);//in actual case its 30.0
-
- while(1);
-
-}
-*/
\ No newline at end of file
--- a/BCN.h Fri Jul 01 17:55:30 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,135 +0,0 @@ -#include "mbed.h" - -//STATES -#define BCN_RF_SILENCE 0 -#define BCN_TX_DISABLED 1 -#define BCN_TX_STANDBY 2 -#define BCN_TX_FAILURE 3 -#define BCN_TX_SUCCESS 4 -#define BCN_TX_INITIALIZED 5 - -//Size of tx data -/* -#define SHORT_TX_DATA 60 //in bytes -#define LONG_TX_DATA 75 //in bytes -*/ - -#define SHORT_TX_DATA 480 //in bytes -#define LONG_TX_DATA 127 //in bytes - -//#define RF_SILENCE_TIME 35*60 -#define RF_SILENCE_TIME 5 //changed for testing - -//FUNCTION PROTOTYPING -void FCTN_BCN_INIT(); -void FCTN_BCN_FEN(); -void FCTN_BCN_TX_MAIN(); -void Set_BCN_TX_STATUS(uint8_t); -uint8_t check_Temperature(); -//void SHORT_BCN_TX(); -//void LONG_BCN_TX(); -void BCN_TX(); -void Init_BEACON_HW(); -void writereg(uint8_t,uint8_t); -uint8_t readreg(uint8_t); -void clearTxBuf(); -uint8_t setFrequency(double); -bool Check_ACK_RECEIVED(); -void init_spi(); -void reset_rfm(uint8_t); - -#define RF22_MAX_MESSAGE_LEN 255 - -//These values we set for FIFO thresholds -#define RF22_TXFFAEM_THRESHOLD 40 - -//Registers -#define RF22_REG_00_DEVICE_TYPE 0x00 -#define RF22_REG_02_DEVICE_STATUS 0x02 -#define RF22_REG_03_INTERRUPT_STATUS1 0x03 -#define RF22_REG_04_INTERRUPT_STATUS2 0x04 -#define RF22_REG_07_OPERATING_MODE1 0x07 -#define RF22_REG_08_OPERATING_MODE2 0x08 -#define RF22_REG_09_OSCILLATOR_LOAD_CAPACITANCE 0x09 -#define RF22_REG_0B_GPIO_CONFIGURATION0 0x0b -#define RF22_REG_0C_GPIO_CONFIGURATION1 0x0c -#define RF22_REG_0D_GPIO_CONFIGURATION2 0x0d -#define RF22_REG_0F_ADC_CONFIGURATION 0x0f -#define RF22_REG_11_ADC_Value 0x11 -#define RF22_REG_12_Temperature_Sensor_Calibration 0x12 -#define RF22_REG_1C_IF_FILTER_BANDWIDTH 0x1c -#define RF22_REG_1F_CLOCK_RECOVERY_GEARSHIFT_OVERRIDE 0x1f -#define RF22_REG_20_CLOCK_RECOVERY_OVERSAMPLING_RATE 0x20 -#define RF22_REG_21_CLOCK_RECOVERY_OFFSET2 0x21 -#define RF22_REG_22_CLOCK_RECOVERY_OFFSET1 0x22 -#define RF22_REG_23_CLOCK_RECOVERY_OFFSET0 0x23 -#define RF22_REG_24_CLOCK_RECOVERY_TIMING_LOOP_GAIN1 0x24 -#define RF22_REG_25_CLOCK_RECOVERY_TIMING_LOOP_GAIN0 0x25 -#define RF22_REG_26_RSSI 0x26 -#define RF22_REG_27_RSSI_THRESHOLD 0x27 -#define RF22_REG_28_ANTENNA_DIVERSITY1 0x28 -#define RF22_REG_29_ANTENNA_DIVERSITY2 0x29 -#define RF22_REG_2A_AFC_LIMITER 0x2a -#define RF22_REG_2B_AFC_CORRECTION_READ 0x2b -#define RF22_REG_2C_OOK_COUNTER_VALUE_1 0x2c -#define RF22_REG_2D_OOK_COUNTER_VALUE_2 0x2d -#define RF22_REG_2E_SLICER_PEAK_HOLD 0x2e -#define RF22_REG_30_DATA_ACCESS_CONTROL 0x30 -#define RF22_REG_31_EZMAC_STATUS 0x31 -#define RF22_REG_32_HEADER_CONTROL1 0x32 -#define RF22_REG_33_HEADER_CONTROL2 0x33 -#define RF22_REG_34_PREAMBLE_LENGTH 0x34 -#define RF22_REG_35_PREAMBLE_DETECTION_CONTROL1 0x35 -#define RF22_REG_36_SYNC_WORD3 0x36 -#define RF22_REG_37_SYNC_WORD2 0x37 -#define RF22_REG_38_SYNC_WORD1 0x38 -#define RF22_REG_39_SYNC_WORD0 0x39 -#define RF22_REG_3A_TRANSMIT_HEADER3 0x3a -#define RF22_REG_3B_TRANSMIT_HEADER2 0x3b -#define RF22_REG_3C_TRANSMIT_HEADER1 0x3c -#define RF22_REG_3D_TRANSMIT_HEADER0 0x3d -#define RF22_REG_3E_PACKET_LENGTH 0x3e -#define RF22_REG_3F_CHECK_HEADER3 0x3f -#define RF22_REG_40_CHECK_HEADER2 0x40 -#define RF22_REG_41_CHECK_HEADER1 0x41 -#define RF22_REG_42_CHECK_HEADER0 0x42 -#define RF22_REG_43_HEADER_ENABLE3 0x43 -#define RF22_REG_44_HEADER_ENABLE2 0x44 -#define RF22_REG_45_HEADER_ENABLE1 0x45 -#define RF22_REG_46_HEADER_ENABLE0 0x46 -#define RF22_REG_47_RECEIVED_HEADER3 0x47 -#define RF22_REG_48_RECEIVED_HEADER2 0x48 -#define RF22_REG_49_RECEIVED_HEADER1 0x49 -#define RF22_REG_4A_RECEIVED_HEADER0 0x4a -#define RF22_REG_4B_RECEIVED_PACKET_LENGTH 0x4b -#define RF22_REG_58 0x58 -#define RF22_REG_60_CHANNEL_FILTER_COEFFICIENT_ADDRESS 0x60 -#define RF22_REG_61_CHANNEL_FILTER_COEFFICIENT_VALUE 0x61 -#define RF22_REG_62_CRYSTAL_OSCILLATOR_POR_CONTROL 0x62 -#define RF22_REG_63_RC_OSCILLATOR_COARSE_CALIBRATION 0x63 -#define RF22_REG_64_RC_OSCILLATOR_FINE_CALIBRATION 0x64 -#define RF22_REG_65_LDO_CONTROL_OVERRIDE 0x65 -#define RF22_REG_66_LDO_LEVEL_SETTINGS 0x66 -#define RF22_REG_67_DELTA_SIGMA_ADC_TUNING1 0x67 -#define RF22_REG_68_DELTA_SIGMA_ADC_TUNING2 0x68 -#define RF22_REG_69_AGC_OVERRIDE1 0x69 -#define RF22_REG_6A_AGC_OVERRIDE2 0x6a -#define RF22_REG_6B_GFSK_FIR_FILTER_COEFFICIENT_ADDRESS 0x6b -#define RF22_REG_6C_GFSK_FIR_FILTER_COEFFICIENT_VALUE 0x6c -#define RF22_REG_6D_TX_POWER 0x6d -#define RF22_REG_6E_TX_DATA_RATE 0x6e -#define RF22_REG_6F_TX_DATA_RATE 0x6f -#define RF22_REG_70_MODULATION_CONTROL1 0x70 -#define RF22_REG_71_MODULATION_CONTROL2 0x71 -#define RF22_REG_72_FREQUENCY_DEVIATION 0x72 -#define RF22_REG_73_FREQUENCY_OFFSET1 0x73 -#define RF22_REG_74_FREQUENCY_OFFSET2 0x74 -#define RF22_REG_75_FREQUENCY_BAND_SELECT 0x75 -#define RF22_REG_76_NOMINAL_CARRIER_FREQUENCY1 0x76 -#define RF22_REG_77_NOMINAL_CARRIER_FREQUENCY0 0x77 -#define RF22_REG_79_FREQUENCY_HOPPING_CHANNEL_SELECT 0x79 -#define RF22_REG_7A_FREQUENCY_HOPPING_STEP_SIZE 0x7a -#define RF22_REG_7C_TX_FIFO_CONTROL1 0x7c -#define RF22_REG_7D_TX_FIFO_CONTROL2 0x7d -#define RF22_REG_7E_RX_FIFO_CONTROL 0x7e -#define RF22_REG_7F_FIFO_ACCESS 0x7f \ No newline at end of file
--- a/EPS.cpp Fri Jul 01 17:55:30 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,1156 +0,0 @@
-#include "EPS.h"
-#include "pin_config.h"
-#include "iostream"
-
-//FOR APPEDING HK DATA===================================
-
-extern uint16_t crc_hk_data();
-
-//acs
-extern uint8_t ACS_DETUMBLING_ALGO_TYPE;
-extern uint8_t ACS_STATE;
-extern uint8_t ACS_MAG_TIME_DELAY;
-extern uint8_t ACS_DEMAG_TIME_DELAY;
-extern uint8_t ACS_Z_FIXED_MOMENT;
-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;
-extern uint8_t ACS_MAIN_STATUS;
-extern uint8_t ACS_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;
-extern uint8_t ACS_DATA_ACQ_STATUS;
-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;
-
-
-//bcn
-extern uint8_t BCN_SPND_TX;
-extern uint8_t BCN_FEN;
-extern uint8_t BCN_LONG_MSG_TYPE;
-extern uint8_t BCN_TX_MAIN_STATUS;
-extern uint8_t BCN_TX_STATUS;
-extern uint8_t BCN_INIT_STATUS;
-extern uint8_t BCN_FAIL_COUNT;
-extern uint16_t BCN_TX_MAIN_COUNTER;
-
-//bae
-extern uint8_t BAE_RESET_COUNTER;
-extern uint8_t BAE_INIT_STATUS;
-extern uint8_t BAE_STANDBY;
-extern uint16_t BAE_I2C_COUNTER;
-
-//eps
-extern uint8_t ACS_INIT_STATUS;
-extern uint16_t EPS_MAIN_COUNTER;
-
-//main
-extern uint8_t HTR_CYCLE_COUNTER;
-extern uint16_t ACS_MAIN_COUNTER;
-
-
-//=======================================================
-
-
-/***********************************************global variable declaration***************************************************************/
-extern uint32_t BAE_STATUS;
-extern uint32_t BAE_ENABLE;
-extern uint8_t BAE_HK_data[134];
-//extern char BAE_chardata[74];
-
-//implement them assign them default values
-uint8_t EPS_BATT_TEMP_LOW;
-uint8_t EPS_BATT_TEMP_HIGH;
-uint8_t EPS_BATT_TEMP_DEFAULT;
-uint8_t EPS_SOC_LEVEL_12 = 70;
-uint8_t EPS_SOC_LEVEL_23 = 90;
-uint8_t EPS_BAT_TEMP_LOW;
-uint8_t EPS_BAT_TEMP_HIGH;
-uint8_t EPS_BAT_TEMP_DEFAULT;
-DigitalOut EPS_CHARGER_FAULT(PIN42);
-DigitalOut EPS_CHARGER_STATUS(PIN31);
-DigitalOut EPS_BATTERY_GAUGE_ALERT(PIN73);
-
-//m_I2C.frequency(10000)
-const char RCOMP0= 0x97;// don't know what it is now
-BAE_HK_actual actual_data;
-BAE_HK_quant quant_data;
-BAE_HK_min_max bae_HK_minmax;
-BAE_HK_arch arch_data;
-
-/************************battery temperature var*********************************/
-//instead just return the approprate value.. test it
-
-//......................................Peripheral declarations.........................................................//
-Serial pc_eps(USBTX,USBRX);
-
-I2C m_I2C(PIN85,PIN84);
-DigitalOut TRXY(TRXY_DR_EN); //active high
-DigitalOut TRZ(TRZ_DR_EN); //active high
-DigitalOut EN3V3A(ENBL3V3A);
-DigitalOut EN_BTRY_HT(BATT_HEAT);
-//DigitalIn BTRY_HT_OUTPUT(BATT_HEAT_OUTPUT);
-//AnalogIn Vbatt_ang(VBATT);
-AnalogIn Batt_voltage(PIN20); //Battery voltage
-
-SPI spi_bt(PIN99,PIN100,PIN98); //MOSI,MISO,SLK // battery temp something 3
-DigitalOut ssn1(PIN19); //Slave select1 // low line master talks
-DigitalOut ssn2(PIN21);//Slave select2
-//DigitalOut PS(PTB0);
-//DigitalOut HS(PTB1);
-
-AnalogIn CurrentInput(PIN54); // Input from Current Multiplexer //PIN54
-AnalogIn VoltageInput(PIN53); // Input from Voltage Multiplexer //PIN53
-AnalogIn BAE_temp_sensor(PIN55); //Input from BAE temp sensor
-
-/*mux for reading value one by one*/
-DigitalOut SelectLinea3 (PIN46); // MSB of Select Lines
-DigitalOut SelectLinea2 (PIN45);
-DigitalOut SelectLinea1 (PIN44);
-DigitalOut SelectLinea0 (PIN43); // LSB of Select Lines
-
-DigitalOut SelectLineb3 (PIN56); // MSB of Select Lines
-DigitalOut SelectLineb2 (PIN57);
-DigitalOut SelectLineb1 (PIN58);
-DigitalOut SelectLineb0 (PIN59); // LSB of Select Lines
-
-//*********************************************************flags********************************************************//
-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_BTRY_HTR_AUTO ;
-
-//eps cdms fault
-extern uint8_t CDMS_SW_STATUS;
-extern DigitalOut CDMS_OC_FAULT;
-extern bool CDMS_SW_ENABLE;
-extern int CDMS_FAULT_COUNTER;
-
-//eps hw faults
-
-/********* EXTERN ACS VAR ********************/
-extern uint8_t ACS_ATS_STATUS;
-extern uint8_t ACS_TR_Z_SW_STATUS;
-extern DigitalOut ACS_TR_Z_ENABLE;
-extern DigitalOut ACS_TR_Z_OC_FAULT;
-extern DigitalOut ACS_TR_Z_FAULT; //Driver IC fault
-extern int ACS_TR_Z_FAULT_COUNTER;
-
-extern uint8_t ACS_TR_XY_SW_STATUS;
-extern DigitalOut ACS_TR_XY_ENABLE;
-extern DigitalOut ACS_TR_XY_OC_FAULT;
-extern DigitalOut ACS_TR_XY_FAULT; //Driver IC fault
-extern int ACS_TR_XY_FAULT_COUNTER;
-
-//extern uint8_t ACS_ATS1_SW_STATUS;
-extern DigitalOut ATS1_SW_ENABLE;
-extern DigitalOut ACS_ATS1_OC_FAULT;
-extern int ACS_ATS1_FAULT_COUNTER;
-
-//extern uint8_t ACS_ATS2_SW_STATUS;
-
-extern DigitalOut ATS2_SW_ENABLE;
-extern DigitalOut ACS_ATS2_OC_FAULT;
-extern int ACS_ATS2_FAULT_COUNTER;
-
-/********* EXTERN BCN VAR ********************/
-extern uint8_t BCN_TX_SW_STATUS;
-extern bool BCN_TX_ENABLE;
-extern DigitalOut BCN_TX_OC_FAULT;
-extern int BCN_TX_FAULT_COUNTER;
-extern uint8_t BCN_TMP;
-
-//........................................... FUCTIONS.................................................//
-
-void FCTN_EPS_INIT()
-{
-//// printf("\n\r eps init \n");
- EPS_INIT_STATUS = 1 ; //set EPS_INIT_STATUS flag
- // FLAG();
- FCTN_BATTERYGAUGE_INIT();
- EN3V3A = 1; //enable dc dc converter A
- char value=alertFlags(); // initialization part of battery gauge
- unsigned short value_u= (short int )value;
- value_u &=0x0001;
- if(value_u ==0x0001) // battery gauge not initialised
- {
- actual_data.power_mode = 1;
- EPS_BATTERY_GAUGE_STATUS = 0; //clear EPS_BATTERY_GAUGE_STATUS
- }
- else
- {
- actual_data.Batt_gauge_actual[1] = soc();
- actual_data.Batt_voltage_actual = Batt_voltage.read()*3.3; //1 corresponds to 3.3 scaling factor
- FCTN_EPS_POWERMODE(actual_data.Batt_gauge_actual[1]);
- EPS_BATTERY_GAUGE_STATUS = 1; //set EPS_BATTERY_GAUGE_STATUS
- }
-
- FCTN_BATTTEMP_INIT();
- EPS_BATTERY_GAUGE_STATUS = 1;
-
- EPS_INIT_STATUS = 0 ; //clear EPS_INIT_STATUS flag
-
-}
-
-void FCTN_EPS_HANDLE_CDMS_FAULT()
-{ //Variable names from MMS structure, if not, marked as //Temp name
- if(CDMS_SW_STATUS == 0b11) //powered on and oc fault
- if(!CDMS_OC_FAULT)
- CDMS_SW_STATUS = 0b01; //powered on and working
- else
- {
- if(CDMS_SW_STATUS == 0b10) //powered off and oc fault
- CDMS_SW_ENABLE = 1; //Temp name
- if(CDMS_OC_FAULT == 0)
- {
- CDMS_FAULT_COUNTER = 0; //Temp name
- CDMS_SW_STATUS = 0b01; //powered on and working
- }
- else
- {
- CDMS_FAULT_COUNTER++;
- if(CDMS_FAULT_COUNTER == 3)
- CDMS_SW_STATUS = 0b11; //powered on and oc fault
- else
- {
- CDMS_SW_ENABLE = 0; //power OFF switch
- CDMS_SW_STATUS = 0b10; //powered off and oc fault
- }
- }
- }
-}
-
-void FCTN_EPS_HANDLE_HW_FAULTS()
-{ //Variable names from MMS structure, if not, marked as //Temp name
-
- //--------ACS_TR_Z--------//
- if(ACS_TR_Z_SW_STATUS != 0b11) //!disabled
- {
- if(ACS_TR_Z_SW_STATUS == 0b10) //oc fault and powered off
- ACS_TR_Z_ENABLE = 1;
- if(ACS_TR_Z_OC_FAULT || ACS_TR_Z_FAULT)
- {
- ACS_TR_Z_ENABLE = 0;
- ACS_TR_Z_FAULT_COUNTER++; //Temp name
- if(ACS_TR_Z_FAULT_COUNTER == 3)
- ACS_TR_Z_SW_STATUS = 0b11; //disabled
- //update in flash as default state at bootup
- else ACS_TR_Z_SW_STATUS = 0b10; //oc fault and powered off
- }
- else
- {
- ACS_TR_Z_SW_STATUS = 0b01; //powered on and working;
- //update in flash also
- ACS_TR_Z_FAULT_COUNTER = 0;
- }
- }
-
- //--------ACS_TR_XY--------//
- //Same as ACS_TR_Z, just replace Z with XY
- if(ACS_TR_XY_SW_STATUS != 0b11) //!disabled
- {
- if(ACS_TR_XY_SW_STATUS == 0b10) //oc fault and powered off
- ACS_TR_XY_ENABLE = 1;
- if(ACS_TR_XY_OC_FAULT || ACS_TR_XY_FAULT)
- {
- ACS_TR_XY_ENABLE = 0;
- ACS_TR_XY_FAULT_COUNTER++; //Temp name
- if(ACS_TR_XY_FAULT_COUNTER == 3)
- ACS_TR_XY_SW_STATUS = 0b11; //disabled
- //update in flash as default state at bootup
- else ACS_TR_XY_SW_STATUS = 0b10; //oc fault and powered off
- }
- else
- {
- ACS_TR_XY_SW_STATUS = 0b01; //powered on and working;
- //update in flash also
- ACS_TR_XY_FAULT_COUNTER = 0;
- }
- }
-
- //--------ACS_ATS1--------//
- //Same as ACS_ATS2
- //if(ACS_ATS1_SW_STATUS & 0b1100 != 0b1100) //!disabled
- if(ACS_ATS_STATUS&0xC0!=0xC0)
- {
- //if(ACS_ATS1_SW_STATUS & 0b1100 == 0b1000) //oc fault and powered off
- if(ACS_ATS_STATUS&0xC0!=0x80)
- ATS1_SW_ENABLE = 0; //Temp name
- if(ACS_ATS1_OC_FAULT)
- {
- ATS1_SW_ENABLE = 1;
- ACS_ATS1_FAULT_COUNTER++; //Temp name
- if(ACS_ATS1_FAULT_COUNTER == 3)
- //ACS_ATS1_SW_STATUS = ACS_ATS1_SW_STATUS | 0b1100; //disabled
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
- //update in flash as default state at bootup
- else
- {
- //ACS_ATS1_SW_STATUS = ACS_ATS1_SW_STATUS | 0b1000; //setting to 10xx
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x80;
- //ACS_ATS1_SW_STATUS = ACS_ATS1_SW_STATUS & 0b1011; //oc fault and powered off
- }
- }
- else
- {
- //if(ACS_ATS1_SW_STATUS & 0b1100 == 0b1000) //Device oc fault?
- if(ACS_ATS_STATUS&0xC0==0x80)
- ATS1_SW_ENABLE = 1;
- //ACS_ATS1_SW_STATUS = ACS_ATS1_SW_STATUS & 0b0011; //working but powered off
- ACS_ATS_STATUS = ACS_ATS_STATUS&0x3F;
- //Update in flash also
- ACS_ATS1_FAULT_COUNTER = 0;
- }
- }
-
- //--------ACS_ATS2--------//
- //if(ACS_ATS2_SW_STATUS & 0b1100 != 0b1100) //!disabled
- if(ACS_ATS_STATUS&0x0C!=0x0C)
- {
- //if(ACS_ATS2_SW_STATUS & 0b1100 == 0b1000) //oc fault and powered off
- if(ACS_ATS_STATUS&0x0C!=0x08)
- ATS2_SW_ENABLE = 0; //Temp name
- if(ACS_ATS2_OC_FAULT)
- {
- ATS2_SW_ENABLE = 1;
- ACS_ATS2_FAULT_COUNTER++; //Temp name
- if(ACS_ATS2_FAULT_COUNTER == 3)
- //ACS_ATS2_SW_STATUS = ACS_ATS2_SW_STATUS | 0b1100; //disabled
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
- //update in flash as default state at bootup
- else
- {
- //ACS_ATS2_SW_STATUS = ACS_ATS2_SW_STATUS | 0b1000; //setting to 10xx
- //ACS_ATS2_SW_STATUS = ACS_ATS2_SW_STATUS & 0b1011; //oc fault and powered off
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x08;
- }
- }
- else
- {
- //if(ACS_ATS2_SW_STATUS & 0b1100 == 0b1000) //Device oc fault?
- if(ACS_ATS_STATUS&0x0C==0x08)
- ATS2_SW_ENABLE = 1;
- //ACS_ATS2_SW_STATUS = ACS_ATS2_SW_STATUS & 0b0011; //working but powered off
- ACS_ATS_STATUS = ACS_ATS_STATUS&0xF3;
- //Update in flash also
- ACS_ATS2_FAULT_COUNTER = 0;
- }
- }
-
- //--------BCN_TX----------//
- if(BCN_TX_SW_STATUS != 0b11) //!disabled
- {
- if(BCN_TX_SW_STATUS == 0b10) //oc fault and powered off
- BCN_TX_ENABLE = 1; //Temp name
- if(BCN_TX_OC_FAULT)
- {
- BCN_TX_ENABLE = 0;
- BCN_TX_FAULT_COUNTER++; //Temp name
- if(BCN_TX_FAULT_COUNTER == 3)
- BCN_TX_SW_STATUS = 0b11; //disabled
- //update in flash as default state at bootup
- else BCN_TX_SW_STATUS = 0b10; //oc fault and powered off
-
- }
- else
- {
- BCN_TX_SW_STATUS = 0b01; //powered on and working;
- //update in flash also
- BCN_TX_FAULT_COUNTER = 0;
- }
- }
-
-}
-
-//----------------------------------------------------Power algo code--------------------------------------------------------------------//
-/*update the power modes*/
-
-void FCTN_EPS_POWERMODE(float soc) //dummy algo
-{
- if(soc >= 80)
- actual_data.power_mode = 4;
- else if(soc >= 70 & soc < 80)
- actual_data.power_mode = 3;
- else if(soc >= 60 & soc < 70)
- actual_data.power_mode = 2;
- else if(soc < 60)
- actual_data.power_mode = 1;
-}
-
-//...................................................HK...........................................//
-/*reading values*/
-
-void FCTN_HK_MAIN()
-{
- int Iteration=0;
-
- SelectLinea0=0;
- SelectLinea1=0;
- SelectLinea2=0;
- SelectLinea3=0;
-
- SelectLineb0=0;
- SelectLineb1=0;
- SelectLineb2=0;
- SelectLineb3=0;
-
- //collecting data
- for(Iteration=0; Iteration<16; Iteration++)
- {
- actual_data.voltage_actual[Iteration]=VoltageInput.read();
- actual_data.current_actual[Iteration]=CurrentInput.read();
-
- SelectLinea0=!(SelectLinea0);
- if(Iteration%2==1)
- SelectLinea1=!(SelectLinea1);
- if(Iteration%4==3)
- SelectLinea2=!(SelectLinea2);
- if(Iteration%8==7)
- SelectLinea3=!(SelectLinea3);
- int s0,s1,s2,s3;
- s0=SelectLineb0=SelectLinea0;
- s1=SelectLineb1=SelectLinea2;
- s2=SelectLineb2=SelectLinea2;
- s3=SelectLineb3=SelectLinea3;
-//// printf("\n\r %d %d %d %d", s0,s1,s2,s3);
-
- }
- for(Iteration=0; Iteration<16; Iteration++)
- {
- if(Iteration==14)
- actual_data.voltage_actual[Iteration]= (-90.7*3.3*actual_data.voltage_actual[Iteration])+190.1543;
- else
- actual_data.voltage_actual[Iteration]= actual_data.voltage_actual[Iteration]*3.3*5.63;
- }
-
- for(Iteration=0;Iteration<12;Iteration++){
- if(Iteration<8)
- actual_data.current_actual[Iteration]= actual_data.current_actual[Iteration]*3.3/(50*rsens);
- else
- actual_data.current_actual[Iteration]=actual_data.current_actual[Iteration]*3.3;
- int resistance;
-
- resistance=24000*actual_data.current_actual[Iteration]/(3.3-actual_data.current_actual[Iteration]);
- if(actual_data.current_actual[Iteration]>1.47)
- {
- actual_data.current_actual[Iteration]=3694/log(24.032242*resistance);
- }
- else{
-
- actual_data.current_actual[Iteration]=3365.4/log(7.60573*resistance);
- }
- }
- actual_data.BAE_temp_actual=(-90.7*3.3*actual_data.BAE_temp_actual)+190.1543;
-
- actual_data.Batt_voltage_actual=Batt_voltage.read()*3.3*5.63;
-
- //quantizing data
- for(Iteration=0; Iteration<16; Iteration++){
-
- if(Iteration==14)
- quant_data.voltage_quant[Iteration]=quantiz(tstart,tstep,actual_data.voltage_actual[Iteration]);
- else
- quant_data.voltage_quant[Iteration]=quantiz(vstart,vstep,actual_data.voltage_actual[Iteration]);
-
- }
- for(Iteration=0;Iteration<12;Iteration++){
- if(Iteration<8)
- quant_data.current_quant[Iteration]=quantiz(cstart,cstep,actual_data.current_actual[Iteration]);
- else
- quant_data.current_quant[Iteration]=quantiz(tstart_thermistor,tstep_thermistor,actual_data.current_actual[Iteration]);
- }
- for(Iteration=0;Iteration<2;Iteration++){
-
- quant_data.Batt_temp_quant[Iteration]=quantiz(tstart,tstep,actual_data.Batt_temp_actual[Iteration]);
- }
-
- quant_data.Batt_gauge_quant[0]=quantiz(vcell_start,vcell_step,actual_data.Batt_gauge_actual[0]);
- quant_data.Batt_gauge_quant[1]=quantiz(soc_start,soc_step,actual_data.Batt_gauge_actual[1]);
- quant_data.Batt_gauge_quant[2]=quantiz(crate_start,crate_step,actual_data.Batt_gauge_actual[2]);
- quant_data.Batt_gauge_alerts=actual_data.Batt_gauge_actual[3];
-
- quant_data.BAE_temp_quant=quantiz(tstart,tstep,actual_data.BAE_temp_actual);
-
-//// for(Iteration=0;Iteration<3;Iteration++){
-//// quant_data.AngularSpeed_quant[Iteration]=actual_data.AngularSpeed_actual[Iteration];
-//// printf("\n\r w value %f",quant_data.AngularSpeed_quant[Iteration]);
-//// }
-
-//// for(Iteration=0;Iteration<3;Iteration++){
-//// quant_data.Bvalue_quant[Iteration]=actual_data.Bvalue_actual[Iteration];
-//// printf("\n\r b value %f",quant_data.Bvalue_quant[Iteration]);
-//// }
-
- quant_data.Batt_voltage_quant=quantiz(vstart,vstep,actual_data.Batt_voltage_actual);
-
-
- arch_data.Batt_1_temp=quant_data.Batt_temp_quant[0];
- arch_data.Batt_2_temp=quant_data.Batt_temp_quant[1];
- arch_data.EPS_PCB_temp=quant_data.voltage_quant[14];
- arch_data.Batt_SOC=quant_data.Batt_gauge_quant[1];
- arch_data.power_mode=actual_data.power_mode;
- arch_data.faultPoll_status=actual_data.faultPoll_status;
- arch_data.faultIr_status=actual_data.faultIr_status;
- arch_data.Batt_voltage=quant_data.Batt_voltage_quant;
-//// printf("\n\r in hk");
-
-}
-
-void FCTN_APPEND_HKDATA()
-{
- BAE_HK_data[0] = 0x28;
- BAE_HK_data[1] = 0x00;
- BAE_HK_data[2] = 0x00;
- BAE_HK_data[3] = 0x00;
- BAE_HK_data[4] = ACS_ATS_STATUS;
- BAE_HK_data[5] = ACS_TR_XY_SW_STATUS;
- BAE_HK_data[5] = (BAE_HK_data[5]<<2)| ACS_TR_Z_SW_STATUS;
- BAE_HK_data[5] = (BAE_HK_data[5]<<1) | ACS_DETUMBLING_ALGO_TYPE;
- BAE_HK_data[5] = (BAE_HK_data[5]<<3) | ACS_STATE;
- BAE_HK_data[6] = BCN_TX_SW_STATUS;
- BAE_HK_data[6] = (BAE_HK_data[6]<<1) | BCN_SPND_TX;
- BAE_HK_data[6] = (BAE_HK_data[6]<<1) | BCN_FEN;
- BAE_HK_data[6] = (BAE_HK_data[6]<<1) | BCN_LONG_MSG_TYPE;
- BAE_HK_data[6] = (BAE_HK_data[6]<<1) | EPS_BTRY_HTR_AUTO;//EPS_BATTERY_HEATER_ENABLE
- //now two spares in BAE_HK_data[5]
- BAE_HK_data[7] = BAE_RESET_COUNTER;
- BAE_HK_data[8] = EPS_SOC_LEVEL_12;
- BAE_HK_data[9] = EPS_SOC_LEVEL_23;
- BAE_HK_data[10] = ACS_MAG_TIME_DELAY;
- BAE_HK_data[11] = ACS_DEMAG_TIME_DELAY;
- BAE_HK_data[12] = EPS_BAT_TEMP_LOW;
- BAE_HK_data[13] = EPS_BAT_TEMP_HIGH;
- BAE_HK_data[14] = EPS_BAT_TEMP_DEFAULT;
- BAE_HK_data[15] = ACS_MM_X_COMSN >> 8;
- BAE_HK_data[16] = ACS_MM_X_COMSN;
-
- BAE_HK_data[17] = ACS_MM_Y_COMSN >> 8;
- BAE_HK_data[18] = ACS_MM_Y_COMSN;
-
- BAE_HK_data[19] = ACS_MG_X_COMSN >> 8;
- BAE_HK_data[20] = ACS_MG_X_COMSN;
-
- BAE_HK_data[21] = ACS_MG_Y_COMSN >> 8;
- BAE_HK_data[22] = ACS_MG_Y_COMSN;
-
- BAE_HK_data[23] = ACS_MM_Z_COMSN >> 8;
- BAE_HK_data[24] = ACS_MM_Z_COMSN;
-
- BAE_HK_data[25] = ACS_MG_Z_COMSN >> 8;
- BAE_HK_data[26] = ACS_MG_Z_COMSN;
-
- BAE_HK_data[27] = ACS_Z_FIXED_MOMENT >> 8;
- BAE_HK_data[28] = ACS_Z_FIXED_MOMENT;
-
- //BAE RAM PARAMETER
- BAE_HK_data[29] = BAE_INIT_STATUS;
- BAE_HK_data[29] = (BAE_HK_data[29]<<1) | 0;//change it================================
- BAE_HK_data[29] = (BAE_HK_data[29]<<1) | BCN_INIT_STATUS;
- BAE_HK_data[29] = (BAE_HK_data[29]<<1) | BCN_TX_MAIN_STATUS;
- BAE_HK_data[29] = (BAE_HK_data[29]<<3) | BCN_TX_STATUS;
- BAE_HK_data[29] = (BAE_HK_data[29]<<3) | ACS_INIT_STATUS;
-
- BAE_HK_data[30] = ACS_DATA_ACQ_STATUS;
- BAE_HK_data[30] = (BAE_HK_data[30]<<1) | ACS_MAIN_STATUS;
- BAE_HK_data[30] = (BAE_HK_data[30]<<3) | ACS_STATUS;
- BAE_HK_data[30] = (BAE_HK_data[30]<<1) | EPS_INIT_STATUS;
- BAE_HK_data[30] = (BAE_HK_data[30]<<3) | EPS_BATTERY_GAUGE_STATUS;
-
- BAE_HK_data[31] = EPS_MAIN_STATUS;
- BAE_HK_data[31] = (BAE_HK_data[31]<<1) | EPS_BATTERY_TEMP_STATUS;
- BAE_HK_data[31] = (BAE_HK_data[31]<<3) | EPS_STATUS;
- BAE_HK_data[31] = (BAE_HK_data[31]<<2) | CDMS_SW_STATUS;
- //BAE_HK_data[31] = (BAE_HK_data[31]<<1) | EPS_BTRY_HTR_STATUS;//new to : implement===========
- //spare 1
- //spare 5
- BAE_HK_data[32] = BAE_STANDBY;
- // 6 next telemetries value to be given by registers
- BAE_HK_data[33] = ATS1_EVENT_STATUS_RGTR;
- BAE_HK_data[34] = ATS1_SENTRAL_STATUS_RGTR;
- BAE_HK_data[35] = ATS1_ERROR_RGTR;
- BAE_HK_data[36] = ATS2_EVENT_STATUS_RGTR;
- BAE_HK_data[37] = ATS2_SENTRAL_STATUS_RGTR;
- BAE_HK_data[38] = ATS2_ERROR_RGTR;
-
- BAE_HK_data[39] = BCN_FAIL_COUNT;
- BAE_HK_data[40] = actual_data.power_mode;
- BAE_HK_data[41] = HTR_CYCLE_COUNTER;//new to : implement
-
- BAE_HK_data[42] = BAE_I2C_COUNTER;
- BAE_HK_data[43] = BAE_I2C_COUNTER>>8;
- BAE_HK_data[44] = ACS_MAIN_COUNTER;
- BAE_HK_data[45] = ACS_MAIN_COUNTER>>8;
- BAE_HK_data[46] = BCN_TX_MAIN_COUNTER;
- BAE_HK_data[47] = BCN_TX_MAIN_COUNTER>>8;
- BAE_HK_data[48] = EPS_MAIN_COUNTER;
- BAE_HK_data[49] = EPS_MAIN_COUNTER>>8;
- BAE_HK_data[50] = actual_data.bit_data_acs_mm[0];
- BAE_HK_data[51] = actual_data.bit_data_acs_mm[0]>>8;
- BAE_HK_data[52] = actual_data.bit_data_acs_mm[1];
- BAE_HK_data[53] = actual_data.bit_data_acs_mm[1]>>8;
- BAE_HK_data[54] = actual_data.bit_data_acs_mm[2];
- BAE_HK_data[55] = actual_data.bit_data_acs_mm[2]>>8;
-
- BAE_HK_data[56] = actual_data.bit_data_acs_mg[0];
- BAE_HK_data[57] = actual_data.bit_data_acs_mg[0]>>8;
- BAE_HK_data[58] = actual_data.bit_data_acs_mg[1];
- BAE_HK_data[59] = actual_data.bit_data_acs_mg[1]>>8;
- BAE_HK_data[60] = actual_data.bit_data_acs_mg[2];
- BAE_HK_data[61] = actual_data.bit_data_acs_mg[2]>>8;
-
- BAE_HK_data[62] = BCN_TX_OC_FAULT;
- BAE_HK_data[62] = (BAE_HK_data[62]<<1) | ACS_TR_XY_ENABLE;
- BAE_HK_data[62] = (BAE_HK_data[62]<<1) | ACS_TR_Z_ENABLE;
- BAE_HK_data[62] = (BAE_HK_data[62]<<1) | ACS_TR_XY_OC_FAULT;
- BAE_HK_data[62] = (BAE_HK_data[62]<<1) | ACS_TR_Z_OC_FAULT;
- BAE_HK_data[62] = (BAE_HK_data[62]<<1) | ACS_TR_XY_FAULT;
- BAE_HK_data[62] = (BAE_HK_data[62]<<1) | EPS_CHARGER_FAULT;
- BAE_HK_data[62] = (BAE_HK_data[62]<<1) | EPS_CHARGER_STATUS;
-
- BAE_HK_data[63] = (BAE_HK_data[63]<<1) | EPS_BATTERY_GAUGE_ALERT;
- BAE_HK_data[63] = (BAE_HK_data[63]<<1) | CDMS_OC_FAULT;
- BAE_HK_data[63] = (BAE_HK_data[63]<<1) | ACS_ATS1_OC_FAULT;
- BAE_HK_data[63] = (BAE_HK_data[63]<<1) | ACS_ATS2_OC_FAULT;
- BAE_HK_data[63] = (BAE_HK_data[63]<<1) | ACS_TR_Z_FAULT;
- //3 spare
-
- BAE_HK_data[64] = ACS_TR_X_PWM;
- BAE_HK_data[65] = ACS_TR_Y_PWM;
- BAE_HK_data[66] = ACS_TR_Z_PWM;
- //spare byte
- //assigned it to counter HTR_CYCLE_COUNTER
-
- //assign it b_scz_angle
- BAE_HK_data[67] = 0x00;
- BAE_HK_data[67] = (BAE_HK_data[65]<<1) | alarmmode;
- BAE_HK_data[67] = (BAE_HK_data[65]<<1) | controlmode_mms;
- //2 bit spare
-
- for(int i=0;i<9;i++)
- {
- BAE_HK_data[68+i] = invjm_mms[i];
- BAE_HK_data[81+i] = jm_mms[i];
- }
-
- for(int i=0;i<3;i++)
- BAE_HK_data[77+i] = bb_mms[i];
-
- BAE_HK_data[80] = singularity_flag_mms;
-
- for(int i=0;i<16;i++)
- {
- BAE_HK_data[90+i] = quant_data.voltage_quant[i];
- BAE_HK_data[106+i] = quant_data.current_quant[i];
- }
-
- BAE_HK_data[122] = quant_data.Batt_voltage_quant;
- BAE_HK_data[123] = quant_data.BAE_temp_quant;
- BAE_HK_data[124] = (uint8_t)(actual_data.Batt_gauge_actual[1]);
- BAE_HK_data[125] = quant_data.Batt_temp_quant[0];
- BAE_HK_data[126] = quant_data.Batt_temp_quant[1];
- BAE_HK_data[127] = BCN_TMP;
- BAE_HK_data[128] = 0x00;
- BAE_HK_data[129] = 0x00;
- BAE_HK_data[130] = 0x00;
- BAE_HK_data[131] = 0x00;
- uint16_t crc = crc_hk_data();
- BAE_HK_data[132] = (uint8_t)(crc >> 8);
- BAE_HK_data[133] = crc;
-
-//===================================================
-/* can be retrived from the earlier code (function)*/
-}
-
-uint8_t quantiz(float start,float step,float x)
-{
- int y=(x-start)/step;
- if(y<=0)y=0;
- if(y>=255)y=255;
- return y;
-}
-
-bool firstCount=true; // goes to EPS init
-
-
-void saveMin(char x,char y){
- if(y<x){
- x=y;
- }
-
-}
-void saveMax(char x,char y){
- if (y>x)
- {
- x=y;
- }
-}
-
-
-void minMaxHkData(){
- if(firstCount==true){
- for (int i = 0; i < 16; ++i){
- bae_HK_minmax.voltage_min[i] = quant_data.voltage_quant[i];
- bae_HK_minmax.voltage_max[i] = quant_data.voltage_quant[i];
- }
- for (int i = 0; i < 12; ++i){
- bae_HK_minmax.current_min[i] = quant_data.current_quant[i];
- bae_HK_minmax.current_max[i] = quant_data.current_quant[i];
- }
-
- for (int i = 0; i < 2; ++i){
- bae_HK_minmax.Batt_temp_min[i] = quant_data.Batt_temp_quant[i];
- bae_HK_minmax.Batt_temp_max[i] = quant_data.Batt_temp_quant[i];
- }
- /*
- for (int i = 0; i < 3; ++i){
- bae_HK_minmax.Batt_gauge_min[i] = quant_data.Batt_gauge_quant[i];
- bae_HK_minmax.Batt_gauge_max[i] = quant_data.Batt_gauge_quant[i];
- }
- */
- bae_HK_minmax.Batt_SOC_min = quant_data.Batt_gauge_quant[1];
- bae_HK_minmax.Batt_SOC_max = quant_data.Batt_gauge_quant[1];
-
- bae_HK_minmax.BCN_TEMP_min = BCN_TMP;
- bae_HK_minmax.BCN_TEMP_min = BCN_TMP;
-
- for (int i = 0; i < 3; ++i){
- bae_HK_minmax.bit_data_acs_mg_min[i] = actual_data.bit_data_acs_mg[i];
- bae_HK_minmax.bit_data_acs_mg_max[i] = actual_data.bit_data_acs_mg[i];
- }
- for (int i = 0; i < 3; ++i){
- bae_HK_minmax.bit_data_acs_mm_min[i] = actual_data.bit_data_acs_mm[i];//Bvalue_quant earlier
- bae_HK_minmax.bit_data_acs_mm_max[i] = actual_data.bit_data_acs_mm[i];
- }
- bae_HK_minmax.BAE_temp_min=quant_data.BAE_temp_quant;
- bae_HK_minmax.BAE_temp_max=quant_data.BAE_temp_quant;
- bae_HK_minmax.Batt_voltage_min=quant_data.Batt_voltage_quant;
- bae_HK_minmax.Batt_voltage_max=quant_data.Batt_voltage_quant;
-
- }
- else {
- for (int i = 0; i < 16; ++i)
- {
- saveMin(bae_HK_minmax.voltage_min[i],quant_data.voltage_quant[i]);
- saveMax(bae_HK_minmax.voltage_max[i],quant_data.voltage_quant[i]);
- }
- for (int i = 0; i < 12; ++i)
- {
- saveMin(bae_HK_minmax.current_min[i],quant_data.current_quant[i]);
- saveMax(bae_HK_minmax.current_max[i],quant_data.current_quant[i]);
- }
-
- for (int i = 0; i < 2; ++i)
- {
- saveMin(bae_HK_minmax.Batt_temp_min[i],quant_data.Batt_temp_quant[i]);
- saveMax(bae_HK_minmax.Batt_temp_max[i],quant_data.Batt_temp_quant[i]);
- }
- /*
- for (int i = 0; i < 3; ++i)
- {
- saveMin(bae_HK_minmax.Batt_gauge_min[i], quant_data.Batt_gauge_quant[i]);
- saveMax(bae_HK_minmax.Batt_gauge_max[i], quant_data.Batt_gauge_quant[i]);
- }
- */
- saveMin(bae_HK_minmax.Batt_SOC_min, quant_data.Batt_gauge_quant[1]);
- saveMax(bae_HK_minmax.Batt_SOC_max, quant_data.Batt_gauge_quant[1]);
-
- saveMin(bae_HK_minmax.BCN_TEMP_min, BCN_TMP);
- saveMin(bae_HK_minmax.BCN_TEMP_max, BCN_TMP);
-
- for (int i = 0; i < 3; ++i)
- {
- saveMin(bae_HK_minmax.bit_data_acs_mg_min[i], actual_data.bit_data_acs_mg[i]);
- saveMax(bae_HK_minmax.bit_data_acs_mg_max[i], actual_data.bit_data_acs_mg[i]);
- }
- for (int i = 0; i < 3; ++i)
- {
- saveMin(bae_HK_minmax.bit_data_acs_mm_min[i], actual_data.bit_data_acs_mm[i]);
- saveMax(bae_HK_minmax.bit_data_acs_mm_max[i], actual_data.bit_data_acs_mm[i]);
- }
- saveMin(bae_HK_minmax.BAE_temp_min,quant_data.BAE_temp_quant);
- saveMax(bae_HK_minmax.BAE_temp_max,quant_data.BAE_temp_quant);
- saveMin(bae_HK_minmax.Batt_voltage_min,quant_data.Batt_voltage_quant);
- saveMin(bae_HK_minmax.Batt_voltage_max,quant_data.Batt_voltage_quant);
-
-
- }
- firstCount=false;
-}
-
-
-//............................................BATTERY GAUGE......................................//
-void FCTN_BATTERYGAUGE_INIT()
-{
- disable_sleep();
- disable_hibernate();
- socChangeAlertEnabled(true); //enabling alert on soc changing by 1%
- emptyAlertThreshold(32);//setting empty alert threshold to 32% soc
- vAlertMinMaxThreshold();//set min, max value of Valrt register
- vResetThresholdSet();//set threshold voltage for reset
- vResetAlertEnabled(true);//enable alert on reset for V < Vreset
-}
-
-void FCTN_BATTERYGAUGE_MAIN(float Battery_parameters[4])
-{
-//// printf("\n\r battery gauge \n");
-
- float temp=30; //=Battery_temp (from temp sensor on battery board) //value of battery temperature in C currently given a dummy value. Should be updated everytime.
- tempCompensation(temp);
-
-
- Battery_parameters[0]=vcell();
- Battery_parameters[1]=soc();
- Battery_parameters[2]=crate();
-
-//// printf("\nVcell=%f",vcell()); //remove this for final code
-//// printf("\nSOC=%f",soc()); //remove this for final code
-//// printf("\nC_rate=%f",crate()); //remove this for final code
-
-
- if (alerting()== true) //alert is on
- {
- Battery_parameters[3]=alertFlags();
- clearAlert();//clear alert
- clearAlertFlags();//clear all alert flags
- }
-
-}
-
-unsigned short read(char reg)
- {
-
- //Create a temporary buffer
- char buff[2];
-
- //Select the register
- m_I2C.write(m_ADDR, ®, 1, true);
-
- //Read the 16-bit register
- m_I2C.read(m_ADDR, buff, 2);
-
- //Return the combined 16-bit value
- return (buff[0] << 8) | buff[1];
- }
-
-
-
-
-
- void write(char reg, unsigned short data)
- {
- //Create a temporary buffer
- char buff[3];
-
- //Load the register address and 16-bit data
- buff[0] = reg;
- buff[1] = data >> 8;
- buff[2] = data;
-
- //Write the data
- m_I2C.write(m_ADDR, buff, 3);
- }
-
-
-
- // Command the MAX17049 to perform a power-on reset
- void reset()
- {
- //Write the POR command
- write(REG_CMD, 0x5400);
- }
-
- // Command the MAX17049 to perform a QuickStart
- void quickStart()
- {
- //Read the current 16-bit register value
- unsigned short value = read(REG_MODE);
-
- //Set the QuickStart bit
- value |= (1 << 14);
-
- //Write the value back out
- write(REG_MODE, value);
- }
-
-
- //disable sleep
- void disable_sleep()
- {
- unsigned short value = read(REG_MODE);
- value &= ~(1 << 13);
- write(REG_MODE, value);
- }
-
- //disable the hibernate of the MAX17049
- void disable_hibernate()
- {
- write(REG_HIBRT, 0x0000);
- }
-
-
- // Enable or disable the SOC 1% change alert on the MAX17049
- void socChangeAlertEnabled(bool enabled)
- {
- //Read the current 16-bit register value
- unsigned short value = read(REG_CONFIG);
-
- //Set or clear the ALSC bit
- if (enabled)
- value |= (1 << 6);
- else
- value &= ~(1 << 6);
-
- //Write the value back out
- write(REG_CONFIG, value);
-}
-
-
-void compensation(char rcomp)
-{
- //Read the current 16-bit register value
- unsigned short value = read(REG_CONFIG);
-
- //Update the register value
- value &= 0x00FF;
- value |= rcomp << 8;
-
- //Write the value back out
- write(REG_CONFIG, value);
-}
-
-
-void tempCompensation(float temp)
-{
- //Calculate the new RCOMP value
- char rcomp;
- if (temp > 20.0) {
- rcomp = RCOMP0 + (temp - 20.0) * -0.5;
- } else {
- rcomp = RCOMP0 + (temp - 20.0) * -5.0;
- }
-
- //Update the RCOMP value
- compensation(rcomp);
-}
-
- // Command the MAX17049 to de-assert the ALRT pin
- void clearAlert()
- {
- //Read the current 16-bit register value
- unsigned short value = read(REG_CONFIG);
-
- //Clear the ALRT bit
- value &= ~(1 << 5);
-
- //Write the value back out
- write(REG_CONFIG, value);
- }
-
-
- //Set the SOC empty alert threshold of the MAX17049
- void emptyAlertThreshold(char threshold)
- {
- //Read the current 16-bit register value
- unsigned short value = read(REG_CONFIG);
-
- //Update the register value
- value &= 0xFFE0;
- value |= 32 - threshold;
-
- //Write the 16-bit register
- write(REG_CONFIG, value);
- }
-
- // Set the low and high voltage alert threshold of the MAX17049
- void vAlertMinMaxThreshold()
- {
- //Read the current 16-bit register value
- unsigned short value = read(REG_VALRT);
-
- //Mask off the old value
-
- value = 0x96D2;
-
- //Write the 16-bit register
- write(REG_VALRT, value);
- }
-
-
- // Set the reset voltage threshold of the MAX17049
- void vResetThresholdSet()
- {
- //Read the current 16-bit register value
- unsigned short value = read(REG_VRESET_ID);
-
- //Mask off the old //value
- value &= 0x00FF;//Dis=0
-
- value |= 0x9400;//corresponding to 2.5 V
-
-
- //Write the 16-bit register
- write(REG_VRESET_ID, value);
- }
-
-
- // Enable or disable the voltage reset alert on the MAX17049
- void vResetAlertEnabled(bool enabled)
- {
- //Read the current 16-bit register value
- unsigned short value = read(REG_STATUS);
-
- //Set or clear the EnVR bit
- if (enabled)
- value |= (1 << 14);
- else
- value &= ~(1 << 14);
-
- //Write the value back out
- write(REG_STATUS, value);
- }
-
- //Get the current alert flags on the MAX17049
- //refer datasheet-status registers section to decode it.
- char alertFlags()
- {
- //Read the 16-bit register value
- unsigned short value = read(REG_STATUS);
-
- //Return only the flag bits
- return (value >> 8) & 0x3F;
- }
-
- // Clear all the alert flags on the MAX17049
- void clearAlertFlags()
- {
- //Read the current 16-bit register value
- unsigned short value = read(REG_STATUS);
-
- //Clear the specified flag bits
- value &= ~( 0x3F<< 8);
-
- //Write the value back out
- write(REG_STATUS, value);
- }
-
- // Get the current cell voltage measurement of the MAX17049
- float vcell()
- {
-
- //Read the 16-bit raw Vcell value
- unsigned short value = read(REG_VCELL);
-
- //Return Vcell in volts
- return value * 0.000078125*2;
- }
-
- // Get the current state of charge measurement of the MAX17049 as a float
- float soc()
- {
-
- //Create a temporary buffer
- char buff[2];
- int ack = 1;
- //Select the register
- char reg = REG_SOC; // cannot pass the hash defined values directly
- m_I2C.write(m_ADDR, ®, 1, true);
-
-
- //Read the 16-bit register
-
- ack = m_I2C.read(m_ADDR, buff, 2);
-
-//// printf("\n\r acknow %d", ack);
-
- //Return SOC in percent
- if(ack == 0)
- return ((buff[0] << 8) | buff[1]) * 0.00390625;
- else
- return 200;
- }
-
-
-
- // Get the current C rate measurement of the MAX17049
- float crate()
- {
- //Read the 16-bit raw C/Rate value
- short value = read(REG_CRATE);
-
- //Return C/Rate in %/hr
- return value * 0.208;
- }
-
- // Determine whether or not the MAX17049 is asserting the ALRT pin
- bool alerting()
- {
- //Read the 16-bit register value
- unsigned short value = read(REG_CONFIG);
-
- //Return the status of the ALRT bit
- if (value & (1 << 5))
- return true;
- else
- return false;
- }
-
-//.............................Battery board Temp sensor........................//
-void FCTN_BATTTEMP_INIT()
-{
- ssn1=1;ssn2=1;
- //PS=0;
- //HS=0;
- spi_bt.format(8,3);
- spi_bt.frequency(1000000);
- EPS_BATTERY_TEMP_STATUS = 1;
-}
-
-void FCTN_BATT_TEMP_SENSOR_MAIN(float temp[2])
-{
- uint8_t MSB, LSB;
- int16_t bit_data;
- float sensitivity=0.0078125; //1 LSB = sensitivity degree celcius
- wait_ms(320);
- //can we reduce it further ??? azad.......!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
- ssn1=0;
-
- spi_bt.write(0x80);//Reading digital data from Sensor 1
- LSB = spi_bt.write(0x00);//LSB first
- wait_ms(10);
- MSB = spi_bt.write(0x00);
- wait_ms(10);
-//// pc_eps.printf("%d %d\n",MSB,LSB);
- bit_data= ((uint16_t)MSB<<8)|LSB;
- wait_ms(10);
- temp[0]=(float)bit_data*sensitivity;//Converting into decimal value
- ssn1=1;
- wait_ms(10);
- ssn2=0;//Reading data from sensor 2
- spi_bt.write(0x80);
- LSB = spi_bt.write(0x00);
- wait_ms(10);
- MSB = spi_bt.write(0x00);
- wait_ms(10);
- bit_data= ((int16_t)MSB<<8)|LSB;
- wait_ms(10);
- temp[1]=(float)bit_data*sensitivity;
- ssn2=1;
-
-}
\ No newline at end of file
--- a/EPS.h Fri Jul 01 17:55:30 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,182 +0,0 @@
-#include <mbed.h>
-
-#define tstart -40
-#define tstep 8
-#define tstep_thermistor 8 //verify everything!!
-#define tstart_thermistor -40
-#define vstart 3.3
-#define vstep 0.84667
-#define cstart 0.0691
-#define cstep 0.09133
-#define vcell_start 5 //check
-#define vcell_step 0.0133 //check
-#define soc_start 0
-#define soc_step 0.3921
-#define crate_start 0 //check
-#define crate_step 1 //check
-#define AngularSpeed_start 0 //check
-#define AngularSpeed_step 0.1 //check
-#define rsens 0.095
-
-
-#define m_ADDR (0x6C) //slave address
-
- //I2C register addresses
-#define REG_VCELL 0x02
-#define REG_SOC 0x04
-#define REG_MODE 0x06
-#define REG_VERSION 0x08
-#define REG_HIBRT 0x0A
-#define REG_CONFIG 0x0C
-#define REG_VALRT 0x14
-#define REG_CRATE 0x16
-#define REG_VRESET_ID 0x18
-#define REG_STATUS 0x1A
-#define REG_TABLE 0x40
-#define REG_CMD 0xFE
-
-//.............Power switching..........
-#define TRXY_DR_EN PIN82 //torque rod driver enable
-#define TRZ_DR_EN PIN88
-#define ENBL3V3A PIN33
-//#define VBATT PIN20
-#define BATT_HEAT PIN96
-//#define BATT_HEAT_OUTPUT
-extern void FCTN_CONVERT_FLOAT(float input, uint8_t* output);
-//PARAMETER VALUE //change
-//#define EPS_BAT_TEMP_LOW 0
-//#define EPS_BAT_TEMP_HIGH 55
-//#define EPS_BAT_TEMP_DEFAULT 20
-
-void FCTN_EPS_INIT();
-
-void FCTN_EPS_POWERMODE(float soc) ;
-
-void FCTN_HK_MAIN();
-void FCTN_APPEND_HKDATA();
-uint8_t quantiz(float start,float step,float x);
-
-void saveMin();
-void saveMax();
-void minMaxHkData();
-
-void FCTN_BATTERYGAUGE_INIT();
-void FCTN_BATTERYGAUGE_MAIN(float*Battery_parameters);
-
-// unsigned short read_soc(char , bool );
-unsigned short read_soc(char,bool ack);
-unsigned short read(char reg);
-void write(char reg, unsigned short data);
-void reset(); //not used
-void quickStart(); //not used
-void disable_sleep();
-void disable_hibernate();
-void socChangeAlertEnabled(bool);
-void compensation(char rcomp);
-void tempCompensation(float temp);
-void clearAlert();
-void emptyAlertThreshold(char threshold);
-void vAlertMinMaxThreshold();
-void vResetThresholSet();
-void vResetAlertEnabled(bool enabled);
-char alertFlags();
-void clearAlertFlags();
-float vcell();
-float soc();
-float crate();
-bool alerting();
-void vResetThresholdSet();
-
-void FCTN_BATTTEMP_INIT();
-void FCTN_BATT_TEMP_SENSOR_MAIN(float*);
-
-typedef struct BAE_HK_actual{
- float voltage_actual[16];
- float current_actual[12];
- float Batt_temp_actual[2];
- float Batt_gauge_actual[4];
- float BAE_temp_actual;
- char power_mode;
- uint8_t faultPoll_status;
- uint8_t faultIr_status;
-
- //changed
- int16_t bit_data_acs_mm[3];
- int16_t bit_data_acs_mg[3];
-
- float AngularSpeed_actual[3];//required for algo
- float Bvalue_actual[3];//required for algo
- float Batt_voltage_actual;
- /*changed 1.0*/
- //uint16_t Batt_voltage_data;
-
-}BAE_HK_actual;
-
-typedef struct BAE_HK_quant{
- uint8_t voltage_quant[16]; //power_mode + faults should be appended to sd card
- uint8_t current_quant[12];
- uint8_t Batt_temp_quant[2];
- uint8_t Batt_gauge_quant[3]; // why only 3 here??
- float Batt_gauge_alerts; //why is this float??
- uint8_t BAE_temp_quant;
-
- float AngularSpeed_quant[3];
- float Bvalue_quant[3];
-
- uint8_t Batt_voltage_quant;
-}BAE_HK_quant;
-
-typedef struct BAE_HK_arch{ //power_mode + faults should be appended to sd card
-
- uint8_t Batt_1_temp; //verify if uint8_t is right
- uint8_t Batt_2_temp;
- uint8_t EPS_PCB_temp;
- uint8_t Batt_SOC;
- char power_mode;
- uint8_t faultPoll_status;
- uint8_t faultIr_status;
- uint8_t Batt_voltage;
-
- //char Batt_voltage2;
- // char Digital power bus voltage;
-
-
-}BAE_HK_arch;
-
-typedef struct BAE_HK_min_max{
- uint8_t voltage_max[16];
- uint8_t current_max[12];
- uint8_t Batt_voltage_max;
- uint8_t BAE_temp_max;
-
- //battery soc
- uint8_t Batt_SOC_max;
-
- uint8_t Batt_temp_max[2];//bcn temp also included?
- //uint8_t Batt_gauge_max[3];//what is the use.
-
- uint16_t bit_data_acs_mm_max[3];
- uint16_t bit_data_acs_mg_max[3];
- uint8_t BCN_TEMP_max;
- //below no longer valid
- /*uint16_t Bvalue_max[3];
- uint16_t AngularSpeed_max[3];
- */
-
- uint8_t voltage_min[16];
- uint8_t current_min[12];
- uint8_t Batt_voltage_min;
- uint8_t BAE_temp_min;
- uint8_t Batt_SOC_min;
-
- uint8_t Batt_temp_min[3];//includes bcn temp
- uint8_t BCN_TEMP_min;
-
- //uint8_t Batt_gauge_min[3];
-
- uint16_t bit_data_acs_mm_min[3];
- uint16_t bit_data_acs_mg_min[3];
-
-
-}BAE_HK_min_max;
-
--- a/FreescaleIAP.lib Fri Jul 01 17:55:30 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://developer.mbed.org/users/Sissors/code/FreescaleIAP/#474d231b2f35
--- a/TCTM.cpp Fri Jul 01 17:55:30 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,2009 +0,0 @@
-#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"
-#include "iostream"
-#include "stdint.h"
-#include "cassert"
-#include"math.h"
-
-//**********************************STATUS_PARAMETERS*****************************************************
-uint8_t BCN_TX_SW_ENABLE=0x00;
-
-//***********************************FOR STANDBY TIMER****************************************************
-extern void BAE_STANDBY_TIMER_RESET();
-
-uint8_t telemetry[135];
-extern uint8_t BAE_HK_data[134];
-
-//*****************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_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 DigitalOut TRXY_SW;
-//extern DigitalOut TRZ_SW_EN; //same as TRZ_SW
-extern uint32_t BAE_ENABLE;
-extern uint16_t BAE_I2C_COUNTER;
-//extern uint8_t BCN_FAIL_COUNT;
-
-
-//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 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
-
-//included after home
-//extern void FCTN_ACS_GENPWM_MAIN();
-
-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;
-}
-
-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];
- *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);
- 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 tm1[134];
-
-void FCTN_BAE_TM_TC (uint8_t* tc)
-{
- //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(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
- crc16 = CRC::crc16_gen(telemetry,11);
- telemetry[11] = (uint8_t)((crc16&0xFF00)>>8);
- telemetry[12] = (uint8_t)(crc16&0x00FF);
- printf("\n\rincorrect crc");
- for(int i=13;i<134;i++)
- {
- telemetry[i]=0x00;
- }
- }
- else
- {
- //check apid
- uint8_t apid_check=((tc[1]&0xC0)>>6);
- if(apid_check!=0x01)
- {
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];//tc psc defined for this case
- telemetry[2]=0x02;//ack code for this case
- for(int 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);
- printf("\n\rillegal TC packet APID don't match");
- for(int i=13;i<134;i++)
- {
- telemetry[i]=0x00;
- }
- }
- else
- {// all the possible cases of fms and mms
- uint8_t service_type=(tc[2]&0xF0);
- //check for fms first
- switch(service_type)
- {
- case 0x60:
- {
- printf("Memory Management Service\r\n");
- uint8_t service_subtype=(tc[2]&0x0F);
- switch(service_subtype)
- {
- case 0x02:
- {
- printf("\n\rRead from RAM");
- uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
- switch(MID)
- {
- case 0x0000://changed from 0001
- {
- printf("\n\rRead from MID 0000 \n");
- printf("\n\rReading flash parameters");
-
- /*taking some varible till we find some thing more useful*/
- //uint8_t ref_val=0x01;
- telemetry[0] = 1;
- telemetry[1] = tc[0];
- 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] = 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)]);
- ///
- for (int i=127; 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);
-
- break;
- }
- case 0x0001:
- {
- printf("\r\nhk data tm");
- telemetry[0] = 0x60;
- telemetry[1] = tc[0];
- telemetry[2] = ACK_CODE;
-
- for(int i;i<16;i++)
- {
- telemetry[i+3] = bae_HK_minmax.voltage_max[i];
- telemetry[i+19] = bae_HK_minmax.current_max[i];
- }
-
- telemetry[35] = bae_HK_minmax.Batt_voltage_max;;
- telemetry[36] = bae_HK_minmax.BAE_temp_max;
-
- telemetry[37] = bae_HK_minmax.Batt_SOC_max;
-
- telemetry[38] = bae_HK_minmax.Batt_temp_max[0];
- telemetry[39] = bae_HK_minmax.Batt_temp_max[1];
-
- /*BCN temp there*/
- telemetry[40] = bae_HK_minmax.BCN_TEMP_max;
-
- for(int i=0; i<3; i++)
- {
- 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+47] = bae_HK_minmax.voltage_min[i];
-
- for(int i;i<16;i++)
- telemetry[i+63] = bae_HK_minmax.current_min[i];
-
- telemetry[79] = bae_HK_minmax.Batt_voltage_min;
- telemetry[80] = bae_HK_minmax.BAE_temp_min;
-
- /*battery soc*/
- telemetry[81] = bae_HK_minmax.Batt_SOC_min;
-
- telemetry[82] = bae_HK_minmax.Batt_temp_min[0];
- telemetry[83] = bae_HK_minmax.Batt_temp_min[1];
- //huhu//
-
- /*BCN temp named as BCN_TS_BUFFER there*/
- telemetry[84] = bae_HK_minmax.BCN_TEMP_min;
-
- for(int i=0; i<3; 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;
- }
- crc16 = CRC::crc16_gen(telemetry,132);
- telemetry[132] = (uint8_t)((crc16&0xFF00)>>8);
- telemetry[133] = (uint8_t)(crc16&0x00FF);
- break;
- }
- default://invalid MID
- {
- //ACK_L234_telemetry
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- telemetry[2]=0x02;//for this case
- 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;
- }
- }
- break;
- }
- case 0x05:
- {
- printf("\n\rdata for mms 0x05 flash");
- /*changed*/
- printf("\n\rwrite on flash\n");
- 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 0x1100:
- {
- //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]);
- 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]);
- //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]);
- 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]);
- 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 0x0106:
- {
- //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 0x0108:
- {
- //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 0x0109:
- {
- //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;
- }
-
- default:
- {
- printf("Invalid MMS case 0x05 invalid MID\r\n");
- VALID_MID=0;
- //ACK_L234_telemetry
- break;
-
- }
- }
-
- if(VALID_MID==1)//valid MID
- {
- telemetry[0]=0xB0;//or 0x60? check
- telemetry[1]=tc[0];
- telemetry[2]=0xA0;// when valid
- }
- else if(VALID_MID==0)//invalid MID
- {
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- telemetry[2]=0x02;//for this case
- }
-
- 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\rWritten on Flash");
- break;
- }
- default://when invalid service subtype
- {
- printf("\n\r MMS invalid Service Subtype");
- //ACK_L234_telemetry
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- telemetry[2]=0x02;//for this case
- 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;
- }
- }
- break;
- }
- case 0x80:
- {
- //printf("Function Management Service\r\n");
- uint8_t service_subtype=(tc[2]&0x0F);
- switch(service_subtype)
- {
- case 0x01:
- {
- printf("\n\rFMS Activated");
- uint8_t fid=tc[3];//changed from pid to fid
- switch(fid)
- {
- case 0xE0:
- {
- 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
- 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
- {
- ACS_STATUS = 7;
- }
-
- // 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[133] = (uint8_t)((crc16&0xFF00)>>8);
- telemetry[134] = (uint8_t)(crc16&0x00FF);
- break;
- }
- case 0xE1:
- {
- printf("HARDWARE_COMSN\r\n");
- //ACK_L234_telemetry
-
- int init1=0;
- int init2=0;
- int data1=0;
- int data2=0;
-
- 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;
-
- 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;
-
- 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;
- }
-
- 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
-
- // to include commission TR as well
- for(uint8_t i=35;i<132;i++)
- {
- telemetry[i]=0x00;
- }
-
- crc16 = CRC::crc16_gen(telemetry,132);
- telemetry[133] = (uint8_t)((crc16&0xFF00)>>8);
- telemetry[134] = (uint8_t)(crc16&0x00FF);
- break;
- }
- case 0xE2:
- {
- uint8_t BCN_SPND_STATE;
- BCN_SPND_STATE=tc[4];
- if(BCN_SPND_STATE==0x00)
- {
- BCN_SPND_TX=0;
- //stop BCN_STANDBY_TIMER.stop();//create
- telemetry[2]=0xA0;
- }
- else if(BCN_SPND_STATE==0x01)
- {
- BCN_SPND_TX=1;
- //stop BCN_STANDBY_TIMER.start();//create
- if(BCN_TX_MAIN_STATUS==0)
- {
- telemetry[2]=0xA0;
- }
- else if(BCN_TX_MAIN_STATUS==1)
- {
- telemetry[2]=0xC0;
- }
- }
- else
- {
- telemetry[2]=0x02;
- }
- //ACK_L234_telemetry
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- //ack_code taken care above
- 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 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)
- {
- printf("\n\rRun P_EPS_INIT");
- FCTN_EPS_INIT();
- telemetry[2]=ACK_CODE;
- }
- else
- {
- printf("\n\runable to Run P_EPS_INIT as BAE_STATUS not 111 ");;
- telemetry[2]=0x87;
- }
-
- //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 0x02:
- {
- if(BAE_STANDBY==0x07)
- {
- printf("\n\rRun P_EPS_MAIN");
- F_EPS();
- telemetry[2]=ACK_CODE;
- }
- else
- {
- printf("\n\runable to Run P_EPS_MAIN as BAE_STATUS not 111 ");;
- telemetry[2]=0x87;
- }
-
- //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 0x03:
- {
- if(BAE_STANDBY==0x07)
- {
- printf("\n\rRun P_ACS_INIT");
- FCTN_ACS_INIT();
- telemetry[2]=ACK_CODE;
- }
- else
- {
- printf("\n\runable to Run P_ACS_INIT as BAE_STATUS not 111 ");;
- telemetry[2]=0x87;
- }
-
- //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 0x05:
- {
- if(BAE_STANDBY==0x07)
- {
- printf("\n\rRun P_ACS_MAIN");
- F_ACS();
- telemetry[2]=ACK_CODE;
- }
- else
- {
- printf("\n\runable to Run P_ACS_MAIN as BAE_STATUS not 111 ");;
- telemetry[2]=0x87;
- }
-
- //ACK_L234_TM
- 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 0x06:
- {
- if(BAE_STANDBY==0x07)
- {
- printf("\n\rRun P_BCN_INIT");
- F_BCN();
- telemetry[2]=ACK_CODE;
- }
- else
- {
- printf("\n\runable to Run P_BCN_INIT as BAE_STATUS not 111 ");;
- telemetry[2]=0x87;
- }
-
- //ACK_L234_TM
- 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 0x07:
- {
- if(BAE_STANDBY==0x07)
- {
- printf("\n\rRun P_BCN_TX_MAIN");
- FCTN_BCN_TX_MAIN();//correct function check once
- telemetry[2]=ACK_CODE;
- }
- else
- {
- printf("\n\runable to Run P_BCN_TX_MAIN as BAE_STATUS not 111 ");;
- telemetry[2]=0x87;
- }
-
- //ACK_L234_TM
- 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 0x11:
- {
- printf("\n\rSW_ON_ACS_ATS1_SW_ENABLE");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- //____________________________________________************************************
- /*
- 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 ;
-
- ATS1_SW_ENABLE = 0;
- ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F);
- 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;
- }
- break;
- }
- case 0x12:
- {
- printf("\n\rSW_ON_ACS_ATS2_SW_ENABLE");
- //ACK_L234_TM
- 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 ;
- ATS2_SW_ENABLE = 0;
- ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3);
- 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;
- }
- break;
- }
- case 0x13:
- {
- printf("\n\rSW_ON_ACS_TR_XY_ENABLE");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- TRXY_SW = 1;//1 SWITCH enable here
- ACS_TR_XY_SW_STATUS=0x01;
- 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;
- }
- break;
- }
- case 0x14:
- {
- printf("\n\rSW_ON_ACS_TR_Z_ENABLE");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- TRZ_SW = 1;
- ACS_TR_Z_SW_STATUS=0x01;
- 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;
- }
- break;
- }
- case 0x15:
- {
- printf("\n\rSW_ON_BCN_TX_SW_ENABLE");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- BCN_SW = 0;//here 0 is switch enable
- BCN_TX_SW_ENABLE=0x01;
- 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;
- }
- break;
- }
- case 0x21:
- {
- printf("\n\rSW_OFF_ACS_ATS1_SW_ENABLE");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- ATS1_SW_ENABLE = 1;
- ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F) | 0xC0 ;
- 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;
- }
- break;
- }
- case 0x22:
- {
- printf("\n\rSW_OFF_ACS_ATS2_SW_ENABLE");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- ATS2_SW_ENABLE = 1;
- ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3) | 0x0C ;
- 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;
- }
- break;
- }
- case 0x23:
- {
- printf("\n\rSW_OFF_ACS_TR_XY_ENABLE");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- TRXY_SW= 0;
- ACS_TR_XY_SW_STATUS=0x03;
- 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;
- }
- break;
- }
- case 0x24:
- {
- printf("\n\rSW_OFF_ACS_TR_Z_ENABLE");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- TRZ_SW = 0;
- ACS_TR_Z_SW_STATUS=0x03;
- 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;
- }
- break;
- }
- case 0x25:
- {
- printf("\n\rSW_OFF_BCN_TX_SW_ENABLE");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- BCN_SW = 1;
- BCN_TX_SW_ENABLE=0x03;
- 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;
- }
- break;
- }
- case 0x31:
- {
- printf("\n\rACS_ATS1_SW_RESET");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- ATS2_SW_ENABLE = 1;//as ats switched off
- ATS1_SW_ENABLE = 1;
- wait_ms(5);
- ATS1_SW_ENABLE = 0;
- 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;
- }
- break;
- }
- case 0x32:
- {
- printf("\n\rACS_ATS2_SW_RESET");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- ATS1_SW_ENABLE = 1;//as ats1 switched off
- ATS2_SW_ENABLE = 1;
- wait_ms(5);
- ATS2_SW_ENABLE = 0;
- 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;
- }
- break;
- }
- case 0x33:
- {
- printf("\n\rACS_TR_XY_SW_RESET");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- TRXY_SW= 0;
- wait_ms(5);
- TRXY_SW= 1;
- 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;
- }
- break;
- }
- case 0x34:
- {
- printf("\n\rACS_TR_Z_SW_RESET");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- TRZ_SW= 0;
- wait_ms(5);
- TRZ_SW= 1;
- 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;
- }
- break;
- }
- case 0x35:
- {
- printf("\n\rBCN_TX_SW_RESET");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- BCN_SW = 1;
- wait_ms(5);
- BCN_SW = 0;
- 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;
- }
- break;
- }
- case 0x36:
- {
- printf("\n\rBAE_INTERNAL_RESET TO be done ??");
- NVIC_SystemReset();
- break;
- }
- case 0x37:
- {
- printf("\n\rCDMS_RESET");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- CDMS_RESET = 0;
- wait_ms(5);
- CDMS_RESET = 1;
- 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;
- }
- break;
- }
- case 0x38:
- {
- printf("\n\rCDMS_SW_RESET pin yet to be decided");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- /*
- PIN to be DECIDED***************************************************************8
- */
- 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;
- }
- break;
- }
- case 0x40:
- {
- uint8_t STANDBY_DATA_TC;
- BAE_STANDBY=0x00;
- STANDBY_DATA_TC=tc[4];
- if(STANDBY_DATA_TC==0x01)
- {
- BAE_STANDBY |=0x04;
- BAE_STANDBY_TIMER_RESET();
- //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
- }
- STANDBY_DATA_TC=tc[5];
- if(STANDBY_DATA_TC==0x01)
- {
- BAE_STANDBY |=0x02;
- BAE_STANDBY_TIMER_RESET();
- //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
- }
- STANDBY_DATA_TC=tc[6];
- if(STANDBY_DATA_TC==0x01)
- {
- BAE_STANDBY |=0x01;
- BAE_STANDBY_TIMER_RESET();
- //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes
- }
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- telemetry[2]=0xC0;//ack_code for this case
- 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 0x41:
- {
- printf("\n\rexecutng BAE reset HK counter");
- firstCount=true;
- void minMaxHkData();
-
- //what to do here??*************************************************
- //TO BE DONE
-
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- telemetry[2]=0x02;
- 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;
- }
- default:
- {
- printf("\n\rInvalid TC for FMS no matching FID");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- telemetry[2]=0x02;
- 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;
- }
- }
- break;
- }
- default:
- {
- printf("\n\rInvalid TC, FMS service subtype mismacth");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- telemetry[2]=0x02;
- 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;
- }
- }
- break;
- }
- default:
- {
- printf("\n\rInvalid TC neither FMS nor MMS");
- //ACK_L234_TM
- telemetry[0]=0xB0;
- telemetry[1]=tc[0];
- telemetry[2]=0x02;
- 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;
- }
- }
- }
- }
-}
-
-
-int strt_add = flash_size() - (2*SECTOR_SIZE);
-uint32_t flasharray[8]; //256+(3*1024)
-/*corrected*/
-int *nativeflash = (int*)strt_add;
-
-/*Writing to the Flash*/
-void FCTN_BAE_WR_FLASH(uint16_t j,uint32_t fdata) //j-position to write address ; fdata - flash data to be written
-{
- for(int i=0;i<8;i++)
- {
- flasharray[i]=nativeflash[i];
- }
- flasharray[j]=fdata;
- erase_sector(strt_add);
- program_flash(strt_add, (char*)flasharray,32);
-}
-
-/*End*/
-
-/*Reading from Flash*/
-/*return choice parameter included so that i f we want the whole 32 packet data to be sent back we can do so*/
-uint32_t FCTN_BAE_RD_FLASH_ENTITY(uint16_t entity)
-{
- for(int i=0;i<8;i++)
- {
- flasharray[i]=nativeflash[i];
- }
- return flasharray[entity];
-}
-
-uint32_t* FCTN_BAE_RD_FLASH()
-{
- for(int i=0;i<8;i++)
- {
- flasharray[i]=nativeflash[i];
- }
- return flasharray;
-}
-
-/*End*/
-
-// Convert float to 4 uint8_t
-
-
-void FCTN_CONVERT_FLOAT(float input, uint8_t output[4])
-{
- assert(sizeof(float) == sizeof(uint32_t));
- uint32_t* temp = reinterpret_cast<uint32_t*>(&input);
-
- //float* output1 = reinterpret_cast<float*>(temp);
-
- //printf("\n\r %f ", input);
- //std::cout << "\n\r uint32"<<*temp << std::endl;
-
- output[0] =(uint8_t )(((*temp)>>24)&0xFF);
- output[1] =(uint8_t ) (((*temp)>>16)&0xFF);
- output[2] =(uint8_t ) (((*temp)>>8)&0xFF);
- output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic
-
- // printf("\n\rthe values generated are\n");
- /*printf("\n\r%x\n",output[0]);
- printf("\n\r%x\n",output[1]);
- printf("\n\r%x\n",output[2]);
- printf("\n\r%x\n",output[3]);
- to check the values generated
- */
- //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
--- a/TCTM.h Fri Jul 01 17:55:30 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,16 +0,0 @@ -//#define ACK_CODE 0x02; -/*ack code changed*/ -#define ACK_CODE 0xA0; -void FCTN_BAE_TM_TC(uint8_t*); -void FCTN_BAE_WR_FLASH(uint16_t ,uint32_t);/*CHANGED from FCTN_CDMS_WR_FLASH*/ -uint32_t FCTN_BAE_RD_FLASH_ENTITY(uint16_t );/*CHANGED from FCTN_CDMS_RD_FLASH*/ -uint32_t * FCTN_BAE_RD_FLASH(); -void FCTN_CONVERT_FLOAT(float input, uint8_t* output); - -#define B0 -23 -#define B1 -2 -#define B2 5 -#define W0 -5.65 -#define W1 0.45 -#define W2 -1.97 -
--- a/crc.h Fri Jul 01 17:55:30 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,90 +0,0 @@
-//EDITS
-//changed the initial remainder from 0x0000 to 0xffff according to the standards
-//made two seperate functions crc16_gen and crc8_gen
-
-#define TOPBIT16 (1 << 15)
-#define TOPBIT8 (1 << 7)
-#define POLYNOMIAL16 0x1021
-#define POLYNOMIAL8 0xEA
-
-namespace CRC
-{
- typedef uint16_t crctype16;
- crctype16 crc16_gen(const unsigned char message[], unsigned int nBytes)
- {
- crctype16 remainder = 0xffff;
- int byte;
- char bit;
-
- for( byte = 0 ; byte < nBytes ; byte++ )
- {
- /*
- Bring the data byte by byte
- each time only one byte is brought
- 0 xor x = x
- */
- remainder = remainder ^ ( message[byte] << 8 );
-
- for( bit = 8 ; bit > 0 ; bit--)
- {
- /*
- for each bit, xor the remainder with polynomial
- if the MSB is 1
- */
- if(remainder & TOPBIT16)
- {
- remainder = (remainder << 1) ^ POLYNOMIAL16;
- /*
- each time the remainder is xor-ed with polynomial, the MSB is made zero
- hence the first digit of the remainder is ignored in the loop
- */
- }
- else
- {
- remainder = (remainder << 1);
- }
- }
- }
-
- return remainder;
- }
-
-
- typedef uint8_t crctype8;
- crctype8 crc8_gen(const unsigned char message[], unsigned int nBytes)
- {
- crctype8 remainder = 0xff;
-
- for(int byte = 0 ; byte < nBytes ; byte++ )
- {
- /*
- Bring the data byte by byte
- each time only one byte is brought
- 0 xor x = x
- */
- remainder = remainder ^ ( message[byte] );
-
- for(int bit = 8 ; bit > 0 ; bit--)
- {
- /*
- for each bit, xor the remainder with polynomial
- if the MSB is 1
- */
- if(remainder & TOPBIT8)
- {
- remainder = (remainder << 1) ^ POLYNOMIAL8;
- /*
- each time the remainder is xor-ed with polynomial, the MSB is made zero
- hence the first digit of the remainder is ignored in the loop
- */
- }
- else
- {
- remainder = (remainder << 1);
- }
- }
- }
-
- return remainder;
- }
-}
\ No newline at end of file
--- a/main.cpp Fri Jul 01 17:55:30 2016 +0000
+++ b/main.cpp Fri Jul 01 17:59:04 2016 +0000
@@ -1,1144 +0,0 @@
-#include "mbed.h"
-#include "rtos.h"
-#include "pin_config.h"
-#include "ACS.h"
-#include "EPS.h"
-#include "BCN.h"
-#include "TCTM.h"
-#define tm_len 135
-#define tc_len 11
-#define batt_heat_low 20
-
-DigitalOut time_wdog(PIN68);// for determining the time between code
-
-//**********************************************GLOBAL RTOS TIMER*********************************************************//
-RtosTimer *BAE_STANDBY_STATUS_TIMER;
-
-
-//bcn heater rtostimers=============================================================================
-RtosTimer *HTR_OFF=NULL;
-RtosTimer *HTR_CYCLE=NULL;
-RtosTimer *HTR_DLY=NULL;
-
-extern DigitalOut EN_BTRY_HT;
-
-uint8_t HTR_CYCLE_COUNTS=0; //Count of heater cycles
-uint8_t HTR_CYCLE_START_DLY=0; //EPS_HTR_DLY_TIMER timer duration in minutes
-uint8_t HTR_ON_DURATION=0; //EPS_HTR_OFF timer duration in minutes
-uint16_t HTR_CYCLE_PERIOD =0; //EPS_HTR_CYCLE timer duration - MSB minutes, LSB seconds
-uint8_t HTR_CYCLE_COUNTER = 0 ;
-
-uint32_t HTR_CYCLE_PERIOD_DECODER() //To convert HTR_CYCLE_PERIOD to millisecs
-{
- uint8_t PERIOD_mins, PERIOD_secs;
- //pc.printf("\n\r FCTN_HTR_CYCLE_PERIOD");
- PERIOD_secs = HTR_CYCLE_PERIOD;
- PERIOD_mins = HTR_CYCLE_PERIOD >> 8;
- uint32_t period = 1000*((int)PERIOD_mins * 60 + (int)PERIOD_secs);
-
- return period;
-}
-
-void FCTN_EPS_HTR_CYCLE(void const *arg)
-{
- EN_BTRY_HT = 1;//assuming its active high check
- //pc.printf("\n\r on kar diya");
- //pc.printf("\n\r FCTN_EPS_HTR_CYCLE");
- //tim.reset();
- //tim.start();
- //HTR_OFF->start(((uint32_t)HTR_ON_DURATION)*1000);
- HTR_OFF->start((int)HTR_ON_DURATION*60*1000);
-}
-
-void FCTN_EPS_HTR_DLY(void const* arg)
-{
- EN_BTRY_HT = 1;
- HTR_CYCLE_COUNTER = 0;
- HTR_OFF->start(((int)HTR_ON_DURATION*60*1000));
- //pc.printf("\n\r on kar diya");
- //pc.printf("\n\r FCTN_EPS_HTR_DLY");
- //tim.start();
- HTR_CYCLE->start((uint32_t)HTR_CYCLE_PERIOD_DECODER);
-}
-
-void FCTN_EPS_HTR_OFF(void const *arg)
-{
- EN_BTRY_HT = 0;
- //tim.stop();
- //pc.printf("\n\r off kar diya");
- //pc.printf("\n\r the timer value is %f",tim.read());
- //tim.reset();
- if(HTR_CYCLE_COUNTS != 0xFF)
- {
- HTR_CYCLE_COUNTER++;
- if( HTR_CYCLE_COUNTER == (int)HTR_CYCLE_COUNTS)//HTR_CYCLE_COUNTS )
- HTR_CYCLE->stop();
- }
-}
-
-//====================================================================================================
-
-//fctn to pass the data to bcn long type
-uint8_t CDMS_HK_data[134];
-void FCTN_CDMS_HK_TC(uint8_t tc[])
-{
- for(int i=0;i<134;i++)
- CDMS_HK_data[i] = tc[i];
-}
-
-uint8_t BAE_STANDBY=0x00;// as Bcn Acs Eps last three bits BAE order , 1 refer to switch off.
-
-void BAE_STANDBY_STATUS_RESET(const void* arg)
-{
- printf("\n\rBAE standby reset to 0x00");
- BAE_STANDBY=0x00;
-}
-
-
-/*no problem in first stopping and then starting even when for the first time*/
-void BAE_STANDBY_TIMER_RESET()
-{
- BAE_STANDBY_STATUS_TIMER->stop();
- BAE_STANDBY_STATUS_TIMER->start(30000);//20 min ie 20*60=1200 sec or 1200,000 milisec but for testing setting it to 30sec
-}
-
-extern void gen_I_TM();
-
-//*************** FLAGS ****************************//
-uint32_t BAE_STATUS = 0x00000000;
-uint32_t BAE_ENABLE = 0xFFFFFFFF;
-
-//i2c//
-char data_send_flag = 'h';
-//uint8_t BAE_MNG_I2C_STATUS = 0;
-
-//BAE
-uint8_t BAE_INIT_STATUS=0;
-uint8_t BAE_MNG_I2C_STATUS=0;
-
-
-//ACS
-uint8_t ACS_INIT_STATUS = 0;
-uint8_t ACS_DATA_ACQ_STATUS = 0;
-uint8_t ACS_ATS_STATUS = 0x73;
-uint8_t ACS_MAIN_STATUS = 0;
-uint8_t ACS_STATUS = 0;
-uint8_t ACS_DETUMBLING_ALGO_TYPE = 0;
-uint8_t ACS_ATS_ENABLE = 1;
-uint8_t ACS_DATA_ACQ_ENABLE = 1;
-uint8_t ACS_STATE = 7;
-
-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;
-
-//BCN
-extern uint8_t BCN_FEN;
-extern void FCTN_BCN_FEN(void const *args);
-extern void LONG_BCN_DATA(uint8_t* );
-extern uint16_t gencrc16_for_me();//for calculating the crc of baehk data before sendingkk as including crc causes problem
-extern DigitalOut EN_BTRY_HT;
-
-
-Timeout timeout_bcn;
-/*long beacon data/sending*/
-/*be sure it is created in bcn.cpp*/
-
-//EPS
-uint8_t EPS_INIT_STATUS = 0;
-uint8_t EPS_BATTERY_GAUGE_STATUS = 0;
-uint8_t EPS_MAIN_STATUS = 0;
-uint8_t EPS_BATTERY_TEMP_STATUS = 0;
-uint8_t EPS_STATUS = 7; //invalid status
-
-uint8_t EPS_BTRY_HTR_AUTO = 1;
-extern uint8_t EPS_SOC_LEVEL_12;
-extern uint8_t EPS_SOC_LEVEL_23;
-extern uint8_t EPS_BAT_TEMP_LOW;
-extern uint8_t EPS_BAT_TEMP_HIGH;
-extern uint8_t EPS_BAT_TEMP_DEFAULT;
-
-
-//**********************GLOBAL DECLARATIONS********************************//
-
-//CDMS
-//eps cdms fault
-uint8_t CDMS_SW_STATUS;
-DigitalOut CDMS_OC_FAULT(PIN79);
-bool CDMS_SW_ENABLE;
-int CDMS_FAULT_COUNTER = 0;
-
-
-//BAE new hk structure- everything has to changed based on this
-char BAE_chardata[74];
-uint8_t BAE_HK_data[134];
-uint8_t BAE_RESET_COUNTER;// INITIALIZATION or do you want to initialize evertime the main is reset? or is it same
-uint16_t BAE_I2C_COUNTER = 0;//change/apply
-//uint8_t BAE_STANDBY=0x00;// as Bcn Acs Eps last three bits BAE order , 1 refer to switch off.
-
-//BCN
-//uint16_t BCN_TX_MAIN_COUNTER = 0;
-uint8_t BCN_LONG_MSG_TYPE = 1;
-
-DigitalOut BCN_TX_OC_FAULT(PIN80);
-int BCN_TX_FAULT_COUNTER;
-
-//ACS
-uint16_t ACS_MAIN_COUNTER = 0;
-
-//eps hw faults
-//uint8_t ACS_TR_Z_SW_STATUS;
-DigitalOut ACS_TR_Z_ENABLE(PIN40);
-DigitalOut ACS_TR_Z_OC_FAULT(PIN91);
-DigitalOut ACS_TR_Z_FAULT(PIN89); //Driver IC fault
-int ACS_TR_Z_FAULT_COUNTER = 0;
-
-//uint8_t ACS_TR_XY_SW_STATUS;
-DigitalOut ACS_TR_XY_ENABLE(PIN71);
-DigitalOut ACS_TR_XY_OC_FAULT(PIN72);
-DigitalOut ACS_TR_XY_FAULT(PIN83); //Driver IC fault
-int ACS_TR_XY_FAULT_COUNTER = 0;
-
-//bool ACS_ATS1_ENABLE;
-DigitalOut ACS_ATS1_OC_FAULT(PIN97);
-int ACS_ATS1_FAULT_COUNTER = 0;
-
-//bool ACS_ATS2_ENABLE;
-DigitalOut ACS_ATS2_OC_FAULT(PIN5);
-int ACS_ATS2_FAULT_COUNTER;
-
-//EPS
-int EPS_MAIN_COUNTER = 0;
-
-
-//GEN DEC FOR TESTING or to be DECIDED whre to ASSIGN HERE
-const int addr = 0x20; //slave address
-Timer t_rfsilence;
-Timer t_start;
-/*defined as of now to check execution time*/
-/*remember to remove them*/
-Timer t_acs;
-Timer t_eps;
-//Timer t_tc;
-Timer t_tm;
-Serial pc(USBTX, USBRX);
-int power_flag_dummy=2;
-float data[6];
-
-
-//*************EXTERN PARA********
-
-//BAE
-
-extern BAE_HK_actual actual_data;
-extern BAE_HK_quant quant_data;
-extern BAE_HK_min_max bae_HK_minmax;
-extern BAE_HK_arch arch_data;
-
-
-//BCN
-extern uint8_t BCN_FEN;
-extern uint8_t BCN_TX_SW_STATUS;
-extern uint8_t BCN_SPND_TX;
-
-
-//TCTM
-extern uint8_t telemetry[135];
-
-
-//ACS
-extern uint8_t ACS_TR_Z_SW_STATUS;
-extern uint8_t ACS_TR_XY_SW_STATUS;
-extern float gyro_data[3];
-extern float mag_data[3];
-extern float moment[3];
-extern float b_old[3]; // Unit: Tesla
-extern float db[3];
-extern uint8_t flag_firsttime;
-extern uint8_t ACS_MAG_TIME_DELAY;
-extern uint8_t ACS_DEMAG_TIME_DELAY;
-extern uint16_t ACS_Z_FIXED_MOMENT;
-
-int write_ack = 1;
-int read_ack = 1;
-char telecommand[tc_len];
-
-
-bool pf1check = 0;
-bool pf2check = 0;
-bool if1check = 0;
-bool if2check = 0;
-
-//ASSIGNING PINS//
-DigitalOut ATS1_SW_ENABLE(PTC0); // enable of att sens2 switch
-DigitalOut ATS2_SW_ENABLE(PTC16); // enable of att sens switch
-InterruptIn irpt_4m_mstr(PIN38); //I2c interrupt from CDMS
-DigitalOut irpt_2_mstr(PIN4); //I2C interrupt to CDMS
-I2CSlave slave (PIN1,PIN2);///pin1 pin2
-//DigitalOut batt_heat(PIN96);
-
-//ATS1_SW_ENABLE = 0;
-PwmOut PWM1(PIN93); //x //Functions used to generate PWM signal
-PwmOut PWM2(PIN94); //y
-PwmOut PWM3(PIN95); //z //PWM output comes from pins p6
-
-//........faults
-//Polled Faults
-DigitalIn pf1(PIN5);//Attitude Sensor 1 OC bar fault signal
-DigitalIn pf2(PIN97);//Attitude Sensor 2 OC bar fault signal
-DigitalIn pf3(PIN83);//Fault Bar for TRXY driver
-
-DigitalOut TRXY_SW(PIN71); //TR XY Switch
-DigitalOut DRV_Z_EN(PIN88); //Sleep pin of driver z
-DigitalOut TRZ_SW(PIN40); //TR Z Switch
-DigitalOut CDMS_RESET(PIN7,1); // CDMS RESET
-DigitalOut BCN_SW(PIN14); //Beacon switch
-DigitalOut DRV_XY_EN(PIN82);
-
-
-
-//================================================================================
-//default flash array some filler bits added (detail in MMS file)
-uint32_t ARR_INITIAL_VAL[8]={0x33574C00,0x505A4141,0x1A1A1400,0x00000000,0x00000000,0x00000000,0xCBA20000,0x00000000};//to be done
-
-void FLASH_INI()
-{
- uint32_t read[8];
- for(int i=0;i<8;i++)
- {
- read[i] = FCTN_BAE_RD_FLASH_ENTITY(i);
- printf("\n\r val is read after starting %x ",read[i]);
- }
-
- if(read[0] == -1)
- for(int j=0;j<8;j++)
- {
- FCTN_BAE_WR_FLASH(j,ARR_INITIAL_VAL[j]);
- }
- else
- {
- for(int j=0;j<8;j++)
- {
- read[j] = FCTN_BAE_RD_FLASH_ENTITY(j);
- }
- ACS_ATS_STATUS = read[0]>>24;
- ACS_TR_XY_SW_STATUS = ((uint8_t)(read[0]>>22))&0x03;
- ACS_TR_Z_SW_STATUS = (read[0]>>20)&0x03;
- ACS_DETUMBLING_ALGO_TYPE = (read[0]>>19)&0x01;
- ACS_STATE = (read[0]>>16)&0x07;
- BCN_TX_SW_STATUS = ((uint8_t)(read[0]>>14))&0x03;
- BCN_SPND_TX = ((uint8_t)(read[0]>>13))&0x01;
- BCN_FEN = ((uint8_t)(read[0]>>12))&0x01;
- BCN_LONG_MSG_TYPE = ((uint8_t)(read[0]>>11))&0x01;
- EPS_BTRY_HTR_AUTO = ((uint8_t)(read[0]>>10))&0x03;//EPS_BATTERY_HEATER_ENABLE
- //now two spares in telemetry[5]
-
- //updating the reset counter
- BAE_RESET_COUNTER = ((uint8_t)(read[0]))+1;
- read[0]=(read[0]&0xffffff00) | (uint32_t)BAE_RESET_COUNTER;
- FCTN_BAE_WR_FLASH(0,read[0]);
-
-
- EPS_SOC_LEVEL_12 = (uint8_t)(read[1]>>24);
- EPS_SOC_LEVEL_23 = (uint8_t)(read[1]>>16);
- ACS_MAG_TIME_DELAY = (uint8_t)(read[1]>>8);
- ACS_DEMAG_TIME_DELAY = (uint8_t)read[1];
-
- EPS_BAT_TEMP_LOW = (uint8_t)(read[2]>>24);
- EPS_BAT_TEMP_HIGH = (uint8_t)(read[2]>>16);
- EPS_BAT_TEMP_DEFAULT = (uint8_t)(read[2]>>8);
-
- ACS_MM_X_COMSN = (uint16_t)(read[3]>>16);
- ACS_MM_Y_COMSN = (uint16_t)read[3];
-
- ACS_MG_X_COMSN = (uint16_t)(read[4]>>16);
- ACS_MG_Y_COMSN = (uint16_t)read[4];
-
- ACS_MM_Z_COMSN = (uint16_t)(read[5]>>16);
- ACS_MG_Z_COMSN = (uint16_t)read[5];
-
- ACS_Z_FIXED_MOMENT = (uint16_t)(read[6]>>16);//assignvalues
-
- }
-
-}
-//================================================================================
-
-
-
-/*****************************************************************Threads USed***********************************************************************************/
-Thread *ptr_t_i2c;
-
-
-/*********************************************************FCTN HEADERS***********************************************************************************/
-void FCTN_ISR_I2C();
-void FCTN_TM();
-void F_ACS();
-void F_EPS();
-void F_BCN();
-
-//I2C
-uint32_t pdir_tc1,pdir_tc2,pdir_tm1,pdir_tm2,pdir_ss1,pdir_ss2;//variables used to verify i2c working
-uint16_t crc16_check;
-void I2C_busreset()
-{
- PORTE->PCR[1] &= 0xfffffffb;
- PORTE->PCR[0] &= 0xfffffffb;
- I2C1->C1 &= 0x7f;
- SIM->SCGC4 &= 0xffffff7f;
- SIM->SCGC4 |= 0x00000080;
- I2C1->C1 |= 0x80;
- PORTE->PCR[1] |= 0x00000004;
- PORTE->PCR[0] |= 0x00000004;
- Thread::wait(1);
-}
-
-//*******************************************ACS THREAD**************************************************//
-uint8_t iterP1;
-uint8_t iterP2;
-uint8_t iterI1;
-uint8_t iterI2;
-
-//FLOAT TO UINT_8 CONVERSION FUNCTION
-extern uint8_t float_to_uint8(float min,float max,float val);
-
-#define print 0
-
-void F_ACS()
-{
- ACS_MAIN_COUNTER++;
- //time_wdog = 1;
- pc.printf("Entered ACS.\n\r");
-
- ACS_MAIN_STATUS = 1; //set ACS_MAIN_STATUS flag
- FLAG();
-
- ACS_MAIN_COUNTER+=1;
-
- PWM1 = 0; //clear pwm pins
- PWM2 = 0; //clear pwm pins
- PWM3 = 0; //clear pwm pins
-
- wait_ms(ACS_DEMAG_TIME_DELAY);
- ACS_DATA_ACQ_STATUS = (uint8_t) FCTN_ATS_DATA_ACQ();
-
- #if print
- printing the angular speed and magnetic field values
- pc.printf("gyro values\n\r");
- for(int i=0; i<3; i++)
- {
- printf("%f\n\r",actual_data.AngularSpeed_actual[i]);
- }
-
- pc.printf("mag values\n\r");
- for(int i=0; i<3; i++)
- {
- pc.printf("%f\n\r",actual_data.Bvalue_actual[i]);
- }
- #endif
-
- for(int i=0;i<3;i++)
- {
- mag_data[i] = actual_data.Bvalue_actual[i]/1000000;
- gyro_data[i] = actual_data.AngularSpeed_actual[i]*3.14159/180;
- }
-
- if(ACS_STATE == 0) // check ACS_STATE = ACS_CONTROL_OFF?
- {
- #if print
- printf("\n\r acs control off\n");
- #endif
- ACS_STATUS = 0; // set ACS_STATUS = ACS_CONTROL_OFF
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if(actual_data.power_mode<=2)
- {
- #if print
- printf("\n\r Low Power \n\r");
- #endif
- DRV_Z_EN = 0;
- DRV_XY_EN = 0;
- ACS_STATUS = 1; // set ACS_STATUS = ACS_LOW_POWER
- ACS_MAIN_STATUS = 0;
- return;
-
- }
- else if(ACS_TR_Z_SW_STATUS != 1)
- {
- DRV_Z_EN = 0;
- DRV_XY_EN = 0;
- ACS_STATUS = 2; // set ACS_STAUS = ACS_TRZ_DISABLED
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if(ACS_TR_XY_SW_STATUS != 1)
- {
- DRV_Z_EN = 1;
- DRV_XY_EN = 0;
- ACS_STATUS = 3; // set ACS_STAUS = ACS_TRXY_DISABLED , Z axis only
- moment[0] = 0;
- moment[1] = 0;
- moment[2] = ACS_Z_FIXED_MOMENT; // is a dummy value
-
- FCTN_ACS_GENPWM_MAIN(moment) ;
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if((ACS_DATA_ACQ_STATUS == 0)||(ACS_DATA_ACQ_STATUS == 1))
- {
- DRV_Z_EN = 1;
- DRV_XY_EN = 0;
- ACS_STATUS = 3; // set Set ACS_STATUS = ACS_DATA_ACQN_FAILURE , Z axis only
-
- moment[0] = 0;
- moment[1] = 0;
- moment[2] = ACS_Z_FIXED_MOMENT; // is a dummy value
-
- FCTN_ACS_GENPWM_MAIN(moment) ;
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if(ACS_STATE == 5)
- {
- DRV_Z_EN = 1;
- DRV_XY_EN = 0;
- ACS_STATUS = 3; // set ACS_STAUS = ACS_TRXY_DISABLED by ACS_STATE i.e Z axis only
-
- moment[0] = 0;
- moment[1] = 0;
- moment[2] = ACS_Z_FIXED_MOMENT; // 1.3 is a dummy value
- FCTN_ACS_GENPWM_MAIN(moment) ;
-
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if(ACS_DATA_ACQ_STATUS == 2) // MM only is available
- {
- DRV_Z_EN = 1;
- DRV_XY_EN = 1;
-
- ACS_STATUS = 4; // set Set ACS_STATUS = ACS_BDOT_CONTROL
- ACS_DETUMBLING_ALGO_TYPE = 0x01;
- FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
- #if print
- printf("\n\r Moment values returned by control algo \n");
- #endif
- for(int i=0; i<3; i++)
- {
- printf("%f\t",moment[i]);
- }
- FCTN_ACS_GENPWM_MAIN(moment) ;
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if(ACS_STATE == 7) // Nominal mode
- {
- #if print
- printf("\n\r Nominal mode \n");
- #endif
- DRV_Z_EN = 1;
- DRV_XY_EN = 1;
- FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE);
- #if print
- printf("\n\r Moment values returned by control algo \n");
- #endif
- for(int i=0; i<3; i++)
- {
- printf("%f\t",moment[i]);
- }
- FCTN_ACS_GENPWM_MAIN(moment) ;
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if(ACS_STATE == 8) // Auto Control
- {
- #if print
- printf("\n\r Auto control mode \n");
- #endif
- DRV_Z_EN = 1;
- DRV_XY_EN = 1;
- FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE);
- #if print
- printf("\n\r Moment values returned by control algo \n");
- for(int i=0; i<3; i++)
- {
- printf("%f\t",moment[i]);
- }
- #endif
- FCTN_ACS_GENPWM_MAIN(moment) ;// set ACS_STATUS in function
- ACS_MAIN_STATUS = 0;
- return;
- }
- else if(ACS_STATE == 9) // Detumbling
- {
- DRV_Z_EN = 1;
- DRV_XY_EN = 1;
- FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
- FCTN_ACS_GENPWM_MAIN(moment) ;
- ACS_MAIN_STATUS = 0;
- return;
- }
- ACS_STATUS = 7; //INVALID_STATE
- DRV_Z_EN = 0;
- DRV_XY_EN = 0;
- ACS_MAIN_STATUS = 0; //clear ACS_MAIN_STATUS flag
-}
-
-
-//***************************************************EPS THREAD***********************************************//
-
-void F_EPS()
-{
- //time_wdog = 1;
- t_eps.start();
- EPS_MAIN_STATUS = 1; // Set EPS main status
- FLAG();
- EPS_MAIN_COUNTER+=1;
- #if print
- pc.printf("\n\rEntered EPS %f\n",t_start.read());
- #endif
-
-
- FCTN_BATT_TEMP_SENSOR_MAIN(actual_data.Batt_temp_actual);
- #if print
- pc.printf("\n\r Battery temperature %f %f" ,actual_data.Batt_temp_actual[0], actual_data.Batt_temp_actual[1]);
- #endif
- EPS_BATTERY_TEMP_STATUS = 1; //set EPS_BATTERY_TEMP_STATUS
- if(EPS_BTRY_HTR_AUTO == 1)
- {
- if((actual_data.Batt_temp_actual[0] < batt_heat_low) && (actual_data.Batt_temp_actual[1] < batt_heat_low)) // to confirm
- {
- EN_BTRY_HT = 1; //turn on battery heater //earlier named as batt_heat
- }
- else
- {
- EN_BTRY_HT = 0; //turn off battery heater
- }
- }
- else if(EPS_BTRY_HTR_AUTO == 0)
- {
- EPS_STATUS = 1;//EPS_STATUS = EPS_BATTERY_HEATER_DISABLED
- }
- FCTN_BATTERYGAUGE_MAIN(actual_data.Batt_gauge_actual);
- if (actual_data.Batt_gauge_actual[1] == 200) //data not received
- {
- actual_data.power_mode = 1;
- EPS_BATTERY_GAUGE_STATUS = 0; //clear EPS_BATTERY_GAUGE_STATUS
- }
- else
- {
- FCTN_EPS_POWERMODE(actual_data.Batt_gauge_actual[1]); //updating power level
- EPS_BATTERY_GAUGE_STATUS = 1; //set EPS_BATTERY_GAUGE_STATUS
- }
- FCTN_HK_MAIN();
- FCTN_APPEND_HKDATA();
- minMaxHkData();
- EPS_MAIN_STATUS = 0; // clear EPS main status
- t_eps.stop();
- #if print
- printf("\n\r the time for eps is %f",(float)t_eps.read_ms());
- #endif
- t_eps.reset();
-
-}
-
-
-//**************************************************BCN THREAD*******************************************************************//
-
-void F_BCN()
-{
- pc.printf("\n\rEntered BCN %f\n",t_start.read());
- //BCN_TX_MAIN_COUNTER=+1;
- FCTN_BCN_TX_MAIN();
-}
-
-//**************************************************TCTM THREAD*******************************************************************//
-
-void T_TC(void const * args)
-{
- while(1)
- {
- Thread::signal_wait(0x4);
- //wait_us(200);// se if it can be changed//@Lakshya:It can't be changed :)
- wait_us(300);
- BAE_MNG_I2C_STATUS =1 ;
- FLAG();
- //earlier BAE_I2CRX_STATUS=1;
- #if print
- printf("\n\rreached\n");
- #endif // can be between 38 to 15700
- if( slave.receive() == 0)
- {
- irpt_2_mstr = 0;
- data_send_flag = 'h';
- pc.printf("\n\rSlave not addressed");
- pc.printf("\n\rPTE->DIR = 0x%08X",pdir_ss1);
- slave.stop();
- pdir_ss1=PTE->PDIR; /////////edited
- if(((pdir_ss1 & 0x00000003)!=3)) //check if bus has freezed
- {
- I2C_busreset(); /////////edited
- }
- /*pdir_ss2=PTE->PDIR;
- #if PRINT2
- pc.printf("\n\rPTE->DIR = 0x%08X",pdir_ss2);
- #endif*/
- #if print
- printf("\n\rnot send\n");
- #endif
- }
- else if( slave.receive() == 1) // slave writes to master
- {
- BAE_I2C_COUNTER++; //////////edited
- if(data_send_flag = 'h') //to be renamed as BAE_I2C_STATUS
- {
- irpt_2_mstr =1; //wait till cdms code is changed
- FCTN_APPEND_HKDATA();
- uint8_t i2c_count =0;
- //crc is already being added
- write_ack=slave.write((char*)BAE_chardata,134);
- Thread::wait(1); //for correct values of register to be updated
- pdir_tm1=PTE->PDIR;
- irpt_2_mstr = 0;
- if(write_ack==0)// wait till cdms code is changed
- {
- while(((pdir_tm1 & 0x00000003)!=3)&& i2c_count<10)
- {
- Thread::wait(1);
- pdir_tm1=PTE->PDIR;
- i2c_count++;
- }
- if(((pdir_tm1 & 0x00000003)==3))
- {
- pc.printf("\n\rWrite HK success");
- data_send_flag = 'h';
- //shoulddn't it be here
- irpt_2_mstr = 0; //////////edited
- }
- else
- {
- wait_ms(20); //should be atleast 7ms for correct operation but fix this value as 20ms
- I2C_busreset();
- }
- #if print
- pc.printf("\n\rgot interrupt\n");
- #endif
- }
- else
- {
- I2C_busreset();
- }
- i2c_count=0;
- }
- else//data_send_flag = "t" //else if(telecommand[1]&0xC0 == 't')
- {
- uint8_t i2c_count =0;
- write_ack=slave.write((char*)telemetry,134); ////////edited(size)
- Thread::wait(1); //for correct values of register to be updated
- pdir_tm1=PTE->PDIR;
- irpt_2_mstr = 0;
- data_send_flag = 'h';
- if(write_ack==0)
- {
- while(((pdir_tm1 & 0x00000003)!=3)&& i2c_count<10)
- {
- Thread::wait(1);
- pdir_tm1=PTE->PDIR;
- i2c_count++;
- }
- if(((pdir_tm1 & 0x00000003)==3))
- {
- pc.printf("\n\rWrite TM success");
- }
- else
- {
- wait_ms(20); //should be atleast 7ms for correct operation but fix this value as 20ms
- I2C_busreset();
- }
- }
- else
- {
- I2C_busreset();
- }
- i2c_count=0;
- }
-
- }
- else if( slave.receive()==3 || slave.receive()==2) // slave read
- {
- BAE_I2C_COUNTER++;
- uint8_t i2c_count = 0;
- read_ack=slave.read((char *)telecommand,11); //read() function returns acknowledgement
- Thread::wait(1);
- pdir_tc1=PTE->PDIR;
- if(read_ack==0) //read() says it was successful
- {
- while(((pdir_tc1 & 0x00000003)!=3)&& i2c_count<10)//checking if SDA and SCL lines are logic 0 and not more than 10 times(10ms)
- {
- Thread::wait(1);
- pdir_tc1=PTE->PDIR;
- i2c_count++;
- }
- if(((pdir_tc1 & 0x00000003)==3))
- {
- pc.printf("\n\n\rRead TC success");
- //if(telecommand[1]&0xC0 == 0x00)
- if(telecommand[0] == 0x00) /////////////edited
- FCTN_CDMS_HK_TC((uint8_t*) telecommand); /////////////edited
- //FCTN_TC_DECODE((uint8_t*) telecommand);
- else
- {
- FCTN_BAE_TM_TC((uint8_t*) telecommand);
- //telemetry = (char*)temp;
- data_send_flag = 't';
- //pc.printf("\n\r Telemetry Generation \n");
- irpt_2_mstr = 1;
- }
- }
- else //either or both of SDA and SCL lines low
- {
- I2C_busreset();
- // data_send_flag = 'i';
- gen_I_TM();
- data_send_flag = 't';
- //shouldn't it be here
- irpt_2_mstr = 1; /////////////added
- }
- }
- else //read() says it was not successful
- {
- I2C_busreset();
- //data_send_flag = 'i'; //'i' = invalid
- gen_I_TM();
- data_send_flag = 't';
- irpt_2_mstr = 1; /////////////added
- }
- i2c_count = 0;
- #if print
- pc.printf("\n\r time taken %d",t_tc.read_us());
- for(int i = 0; i<134; i++)
- pc.printf("%c", telemetry[i]);
- #endif
- }
- //was commented here now in txt file main_i2ccomment on desktp
- BAE_MNG_I2C_STATUS=0;
- FLAG();
- }
-}
-void FCTN_TM()
-{
- data_send_flag = 't';
- pc.printf("\n\r Telemetry Generation \n");
- irpt_2_mstr = 1;
-}
-
-
-//******************************************************* I2C *******************************************************************//
-
-void FCTN_I2C_ISR()
-{
- ptr_t_i2c->signal_set(0x4);
-}
-
-
-//***********************************************************FAULTS***************************************************************//
-
-uint8_t iter2=0,iter4 = 0;
-
-void pollfault()
-{
- if (pf1==0) // OC_ATS1
- {
- pf1check=1;
- actual_data.faultPoll_status |=0x01 ;
- ATS1_SW_ENABLE = 1; // turn off ats1 // to be turned on next cycle in ACS
- }
- else
- actual_data.faultPoll_status &= 0xFE;
-
- if(pf2==0)
- {
- pf2check=1;
- actual_data.faultPoll_status |=0x02 ;
- ATS2_SW_ENABLE = 1; // turn off ats2 // turn on in ACS
- }
- else
- actual_data.faultPoll_status &= 0xFD;
-
- if (pf3==0)
- {
- actual_data.faultPoll_status |=0x04 ;
- DRV_XY_EN = 0;
- wait_us(1);
- DRV_XY_EN = 1;
- }
- else
- actual_data.faultPoll_status &= 0xFB;
-}
-
-//------------------------------------------------------------------------------------------------------------------------------------------------
-//SCHEDULER
-//------------------------------------------------------------------------------------------------------------------------------------------------
-
-uint8_t schedcount=1;
-void T_SC(void const *args)
-{
- #if print
- printf("\n\r in scheduler");
- #endif
- /*if keeping thish many cases creates a problem then make 3 seperate flagvariable i.e bae_standby_acs so on that will make it easy.!!!*/
- if(schedcount == 7) //to reset the counter
- schedcount = 1;
- if(schedcount%1==0)
- {
- if( BAE_STANDBY!=0x02 && BAE_STANDBY!=0x03 && BAE_STANDBY!=0x06 && BAE_STANDBY!=0x07)
- {
- pc.printf("\nSTATE IS !!!!!! = %x !!\n",ACS_STATE);
- F_ACS();
- //time_wdog = 0;
- }
-
- }
- if(schedcount%2==0)
- {
- if( BAE_STANDBY!=0x01 && BAE_STANDBY!=0x03 && BAE_STANDBY!=0x05 && BAE_STANDBY!=0x07)
- {
- //time_wdog = 1;
- F_EPS();
- }
- //time_wdog = 0;
- }
- if(schedcount%3==0)
- {
- //if(BAE_STANDBY!=0x04 && BAE_STANDBY!=0x05 && BAE_STANDBY!=0x06 && BAE_STANDBY!=0x07)
- // time_wdog = 0;
- // F_BCN();
- }
- schedcount++;
- #if print
- printf("\n\r exited scheduler h");
- printf("\n\r time taken %f",t_start.read());
- #endif
-}
-
-
-Timer t_flag;
-
-
-void FLAG()
-{
-
-//I2C
- //if(BAE_MNG_I2C_STATUS == 1)
- // BAE_STATUS = BAE_STATUS | 0x10000000;
- //else if(BAE_MNG_I2C_STATUS == 0)
- // BAE_STATUS &= 0xEFFFFFF;
-
-//.............bae..................//
- if(BAE_INIT_STATUS == 1)
- BAE_STATUS = BAE_STATUS | 0x00000001; //BAE_STATUS |= 0x00000001;
- else if(BAE_INIT_STATUS == 0)
- BAE_STATUS &= 0xFFFFFFFE;
-
- if(BAE_MNG_I2C_STATUS == 1)
- BAE_STATUS = BAE_STATUS | 0x00000002;
- if(BAE_MNG_I2C_STATUS == 0)
- BAE_STATUS &= 0xFFFFFFFD;
-
-//.............acs..................//
- if(ACS_INIT_STATUS == 1)
- BAE_STATUS = BAE_STATUS | 0x00000080; //set ACS_INIT_STATUS flag
- else if(ACS_INIT_STATUS == 0)
- BAE_STATUS &= 0xFFFFFF7F; //clear ACS_INIT_STATUS flag
-
- if(ACS_DATA_ACQ_STATUS == 1)
- BAE_STATUS =BAE_STATUS | 0x00000100; //set ACS_DATA_ACQ_STATUS flag
- else if(ACS_DATA_ACQ_STATUS == 0)
- BAE_STATUS &= 0xFFFFFEFF; //clear ACS_DATA_ACQ_STATUS flag
-
- if(ACS_ATS_ENABLE == 1)
- BAE_ENABLE |= 0x00000004;
- else if(ACS_ATS_ENABLE == 0)
- BAE_ENABLE = BAE_ENABLE &0xFFFFFFFB | 0x00000004;
-
- if(ACS_DATA_ACQ_STATUS == 'f')
- BAE_STATUS |= 0x00000200;
-
- if(ACS_MAIN_STATUS == 1)
- BAE_STATUS = (BAE_STATUS | 0x00001000); //set ACS_MAIN_STATUS flag
- else if(ACS_MAIN_STATUS == 0)
- BAE_STATUS &= 0xFFFFEFFF; //clear ACS_MAIN_STATUS flag
-
- if(ACS_STATUS == '0')
- BAE_STATUS = (BAE_STATUS & 0xFFFF1FFF); // set ACS_STATUS = ACS_CONTROL_OFF
- else if(ACS_STATUS == '1')
- BAE_STATUS =(BAE_STATUS & 0xFFFF1FFF) | 0x00002000; // set ACS_STATUS = ACS_LOW_POWER
- else if(ACS_STATUS == '2')
- BAE_STATUS = (BAE_STATUS & 0xFFFF1FFF)| 0x00004000; // set ACS_STATUS = ACS_ZAXIS_MOMENT_ONLY
- else if(ACS_STATUS == '3')
- BAE_STATUS = (BAE_STATUS & 0xFFFF1FFF) | 0x00006000; // set ACS_STATUS = ACS_DATA_ACQ_FAILURE
- else if(ACS_STATUS == '4')
- BAE_STATUS = (BAE_STATUS & 0xFFFF1FFF) | 0x00008000; // set ACS_STATUS = ACS_NOMINAL_ONLY
- else if(ACS_STATUS == '5')
- BAE_STATUS =(BAE_STATUS & 0xFFFF1FFF) | 0x0000A000; // set ACS_STATUS = ACS_AUTO_CONTROL
- else if(ACS_STATUS == '6')
- BAE_STATUS =(BAE_STATUS & 0xFFFF1FFF) | 0x0000C000; // set ACS_STATUS = ACS_DETUMBLING_ONLY
- else
- BAE_STATUS =(BAE_STATUS & 0xFFFF1FFF) | 0x0000E000; // set ACS_STATUS = INVALID STATE
-
- if(ACS_STATE == '0')
- BAE_ENABLE = (BAE_ENABLE & 0xFFFFFF8F); //ACS_STATE = ACS_CONTROL_OFF
- else if(ACS_STATE == '2')
- BAE_ENABLE = ((BAE_ENABLE & 0xFFFFFF8F)| 0x00000020); // ACS_STATE = ACS_ZAXIS_MOMENT_ONLY
- else if(ACS_STATE == '3')
- BAE_ENABLE = ((BAE_ENABLE & 0xFFFFFF8F)| 0x00000030); // set ACS_STATUS = ACS_DATA_ACQ_FAILURE
- else if(ACS_STATE == '4')
- BAE_ENABLE = ((BAE_ENABLE & 0xFFFFFF8F)| 0x00000040); // ACS_STATE = ACS_NOMINAL_ONLY
- else if(ACS_STATE == '5')
- BAE_ENABLE = ((BAE_ENABLE & 0xFFFFFF8F)| 0x00000050); // ACS_STATE = ACS_AUTO_CONTROL
- else if(ACS_STATE == '6')
- BAE_ENABLE = ((BAE_ENABLE & 0xFFFFFF8F)| 0x00000060); //ACS_STATE = ACS_DETUMBLING_CONTROL
-
-//...............eps......................//
- if (EPS_INIT_STATUS==1) // Set EPS_INIT_STATUS
- BAE_STATUS |= 0x00010000;
- else if(EPS_INIT_STATUS==0) // Clear
- BAE_STATUS &= 0xFFFEFFFF;
-
-
- if (EPS_MAIN_STATUS==1) // Set EPS_MAIIN_STATUS
- BAE_STATUS |= 0x00040000;
- else if(EPS_MAIN_STATUS==0) // Clear
- BAE_STATUS &= 0xFFFBFFFF;
-
-
- if (EPS_BATTERY_GAUGE_STATUS==1) // Set EPS_BATTERY_GAUGE_STATUS
- BAE_STATUS |= 0x00020000;
- else if(EPS_BATTERY_GAUGE_STATUS==0) // Clear
- BAE_STATUS &= 0xFFFDFFFF;
-
- if (EPS_BATTERY_TEMP_STATUS==1) // Set EPS_BATTERY_TEMP_STATUS
- BAE_STATUS |= 0x00080000;
- else if(EPS_BATTERY_TEMP_STATUS==0) // Clear
- BAE_STATUS &= 0xFFF7FFFF;
-
- if (EPS_STATUS==0)
- BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF); // Set EPS_ERR_BATTERY_TEMP
- else if (EPS_STATUS==1)
- BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF)|0x00010000; // Set EPS_BATTERY_HEATER_DISABLED
- else if (EPS_STATUS==2)
- BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF)|0x00020000; // Set EPS_ERR_HEATER_SWITCH_OFF
- else if (EPS_STATUS==3)
- BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF)|0x00030000; // Set EPS_ERR_HEATER_SWITCH_ON
- else if (EPS_STATUS==4)
- BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF)|0x00040000; // Set EPS_BATTERY_HEATER_OFF
- else if (EPS_STATUS==5)
- BAE_STATUS = (BAE_STATUS & 0xFF8FFFFF)|0x00050000; // Set EPS_BATTERY_HEATER_ON
-
- if(EPS_BTRY_HTR_AUTO == 1)
- BAE_ENABLE |= 0x00000080;
- else if(EPS_BTRY_HTR_AUTO == 0)
- BAE_ENABLE = BAE_ENABLE &0xFFFFFF7;
-
- #if print
- pc.printf("\n\r BAE status %x BAE ENABLE %x ",BAE_STATUS,BAE_ENABLE);
- #endif
-}
-
-
-void FCTN_BAE_INIT()
-{
- #if print
- printf("\n\r Initialising BAE _________________________________________________________________________________");
- #endif
- BAE_INIT_STATUS=1;
- FLAG();
-
- //..........intial status....//
- ACS_STATE = 4;
- ACS_ATS_ENABLE = 1;
- ACS_DATA_ACQ_ENABLE = 1;
- EPS_BTRY_HTR_AUTO = 1;
- actual_data.power_mode=3;
-
- //............intializing pins................//
- ATS1_SW_ENABLE = 0;
- ATS2_SW_ENABLE = 1;
-
- DRV_XY_EN = 1;
- DRV_Z_EN = 1;
- TRZ_SW = 1;
- TRXY_SW = 1;
-
- //time_wdog = 1;
-
- //...........order mentioned in flow chart.................//
- FCTN_ACS_INIT();
- FCTN_EPS_INIT();
- FCTN_BCN_INIT();
-
- uint32_t data_flash=FCTN_BAE_RD_FLASH_ENTITY(0);/*sending the 0 entity as in mms tc/tm bae_reset_counter is present in first 32 bits */
- uint32_t data_modify=data_flash & 0x000000FF;
- data_modify +=1;
- data_modify |=data_flash;
- FCTN_BAE_WR_FLASH(0,data_modify);
- #if print
- printf("\n\rthe number of reset %d",data_modify);
- #endif
- BAE_INIT_STATUS=0;
- FLAG();
-}
-
-
-int main()
-{
- //time_wdog = 1;
- pc.printf("\n\r BAE Activated. Testing Version 1.2 \n");
- //FLASH_INI();
- FCTN_BAE_INIT();
- //time_wdog = 0;
-
- slave.address(addr);
- irpt_2_mstr = 0;
-
- ptr_t_i2c = new Thread(T_TC);
- ptr_t_i2c->set_priority(osPriorityHigh);
-
- irpt_4m_mstr.enable_irq();
- irpt_4m_mstr.rise(&FCTN_I2C_ISR);
-
- RtosTimer t_sc_timer(T_SC,osTimerPeriodic); // Initiating the scheduler thread
- t_sc_timer.start(10000);
- t_start.start();
- #if print
- pc.printf("\n\rStarted scheduler %f\n\r",t_start.read());
- #endif
- /*if one defines it dynamically then one has to take care that the destuct function is calle everytime BAE resets otheriwse it will lead to memory leakage*/
- //BAE_STANDBY_STATUS_TIMER = new RtosTimer(BAE_STANDBY_STATUS_RESET,osTimerOnce);
- /*static allocation*/
- RtosTimer STANDBY_TIMER(BAE_STANDBY_STATUS_RESET,osTimerOnce);
- BAE_STANDBY_STATUS_TIMER=&STANDBY_TIMER;
-
- RtosTimer bcn_start_timer(FCTN_BCN_FEN,osTimerOnce);
- /*later change it to 30 min 1800 seconds*/
- bcn_start_timer.start(20000);
-
- //BCN HTR TIMERS
- RtosTimer EPS_HTR_OFF_TIMER(FCTN_EPS_HTR_OFF, osTimerOnce);
- HTR_OFF=&EPS_HTR_OFF_TIMER;
-
- RtosTimer EPS_HTR_CYCLE_TIMER(FCTN_EPS_HTR_CYCLE);
- HTR_CYCLE=&EPS_HTR_CYCLE_TIMER;
-
- RtosTimer EPS_HTR_DLY_TIMER(FCTN_EPS_HTR_DLY,osTimerOnce);
- HTR_DLY=&EPS_HTR_DLY_TIMER;
-
- FLASH_INI();
-
-while(1); //required to prevent main from terminating
-
-}
\ No newline at end of file
--- a/mbed-rtos.lib Fri Jul 01 17:55:30 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/mbed_official/code/mbed-rtos/#12552ef4e980
--- a/pin_config.h Fri Jul 01 17:55:30 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,101 +0,0 @@ -// 100 LQFP format pin assignment -#define PIN1 PTE0 -#define PIN2 PTE1 -#define PIN3 PTE2 -#define PIN4 PTE3 -#define PIN5 PTE4 -#define PIN6 PTE5 -#define PIN7 PTE6 -//#define 8 -//#define 9 -//#define 10 -//#define 11 -//#define 12 -//#define 13 -#define PIN14 PTE16 -#define PIN15 PTE17 -#define PIN16 PTE18 -#define PIN17 PTE19 -#define PIN18 PTE20 -#define PIN19 PTE21 -#define PIN20 PTE22 -#define PIN21 PTE23 -//#define 22 -//#define 23 -//#define 24 -//#define 25 -#define PIN26 PTE29 -#define PIN27 PTE30 -#define PIN28 PTE31 -//#define 29 -//#define 30 -#define PIN31 PTE24 -#define PIN32 PTE25 -#define PIN33 PTE26 -#define PIN34 PTA0 -#define PIN35 PTA1 -#define PIN36 PTA2 -#define PIN37 PTA3 -#define PIN38 PTA4 -#define PIN39 PTA5 -#define PIN40 PTA6 -#define PIN41 PTA7 -#define PIN42 PTA12 -#define PIN43 PTA13 -#define PIN44 PTA14 -#define PIN45 PTA15 -#define PIN46 PTA16 -#define PIN47 PTA17 -//#define 48 -//#define 49 -#define PIN50 PTA18 -#define PIN51 PTA19 -#define PIN52 PTA20 -#define PIN53 PTB0 -#define PIN54 PTB1 -#define PIN55 PTB2 -#define PIN56 PTB3 -#define PIN57 PTB7 -#define PIN58 PTB8 -#define PIN59 PTB9 -#define PIN60 PTB10 -#define PIN61 PTB11 -#define PIN62 PTB16 -#define PIN63 PTB17 -#define PIN64 PTB18 -#define PIN65 PTB19 -#define PIN66 PTB20 -#define PIN67 PTB21 -#define PIN68 PTB22 //use this temporarily for timing -#define PIN69 PTB23 -#define PIN70 PTC0 -#define PIN71 PTC1 -#define PIN72 PTC2 -#define PIN73 PTC3 -//#define 74 -//#define 75 -#define PIN76 PTC20 -#define PIN77 PTC21 -#define PIN78 PTC22 -#define PIN79 PTC23 -#define PIN80 PTC4 -#define PIN81 PTC5 -#define PIN82 PTC6 -#define PIN83 PTC7 -#define PIN84 PTC8 -#define PIN85 PTC9 -#define PIN86 PTC10 -#define PIN87 PTC11 -#define PIN88 PTC12 -#define PIN89 PTC13 -#define PIN90 PTC16 -#define PIN91 PTC17 -#define PIN92 PTC18 -#define PIN93 PTD0 -#define PIN94 PTD1 -#define PIN95 PTD2 -#define PIN96 PTD3 -#define PIN97 PTD4 -#define PIN98 PTD5 -#define PIN99 PTD6 -#define PIN100 PTD7 \ No newline at end of file
--- a/pni.h Fri Jul 01 17:55:30 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,41 +0,0 @@ -#define SLAVE_ADDR 0x50 -#define SLAVE_ADDR_READ 0x51 -#define SENTRALSTATUS 0x37 -#define RESETREQ 0x9B -#define ERROR 0x50 -#define MAGRATE 0x55 -#define SENSORSTATUS 0x36 -#define ACCERATE 0x56 -#define GYRORATE 0x57 -#define QRATE_DIV 0x32 -#define ALGO_CTRL 0x54 -#define ENB_EVT 0x33 -#define HOST_CTRL 0x34 -#define EVT_STATUS 0x35 -#define ALGO_STATUS 0x38 -#define GYRO_XOUT_H 0x22 -#define MAG_XOUT_H 0X12 -#define UPLOAD_ADDR 0x94 - -//Configaration bits -#define BIT_RESREQ 0x01 -#define BIT_EEP_DET 0x01 -#define BIT_EEP_UPDN 0x02 -#define BIT_EEP_UPERR 0x04 -#define BIT_EEP_IDLE 0x08 -#define BIT_EEP_NODET 0x10 -#define BIT_STBY 0x01 -#define BIT_RAW_ENB 0x02 -#define BIT_HPR_OUT 0x04 -#define BIT_CPU_RES 0x01 -#define BIT_ERR 0x02 -#define BIT_QRES 0x04 -#define BIT_MAG_RES 0x08 -#define BIT_ACC_RES 0x10 -#define BIT_GYRO_RES 0x20 -#define BIT_GYROODR 0x0F//0x0F -#define BIT_MAGODR 0x64//0x64 -#define BIT_RUN_ENB 0x01//0x01 -#define BIT_HOST_UPLD_ENB 0x02 -#define BIT_ALGO_RAW 0x02 -#define BIT_EVT_ENB 0X2A// 0x2A \ No newline at end of file