![](/media/cache/group/nine_tails_kurama_by_zubair43-d4zyf5s.jpg.50x50_q85.jpg)
FINAL ACS TO BE USED FOR TESTING. COMMISSIONING, ACS MAIN, DATA ACQ ALL DONE.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of ACS_FULL_Flowchart_BAE by
ACS.cpp
- Committer:
- Bragadeesh153
- Date:
- 2016-06-28
- Revision:
- 19:403cb36e22ed
- Parent:
- 18:21740620c65e
File content as of revision 19:403cb36e22ed:
/*------------------------------------------------------------------------------------------------------------------------------------------------------ -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/ #include <mbed.h> #include <math.h> #include "pni.h" //pni header file #include "pin_config.h" #include "ACS.h" #include "EPS.h" //********************************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 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; Serial pc_acs(USBTX,USBRX); //for usb communication //CONTROL_ALGO float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla uint8_t flag_firsttime=1, alarmmode=0; 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 db[3]; 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)) { 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) { 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 } } } } else { 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],int &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; } 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; int 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=1; } if (singularity_flag==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); 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); if (singularity_flag==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==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(); int CONFIG_UPLOAD(); int count =0; // Time for which the BAE uC is running (in seconds) //DEFINING VARIABLES char cmd[2]; char raw_gyro[6]; char raw_mag[6]; char reg_data[24]; char store,status; int16_t bit_data; uint16_t time_data; float gyro_data[3], mag_data[3]; 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(600); //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(100); 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(100); 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(100); return 0; } int SENSOR_INIT() { pc_acs.printf("Entered sensor init\n \r"); cmd[0]=RESETREQ; cmd[1]=BIT_RESREQ; wait(1); ack = i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up 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(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); //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(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(600); 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; } 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() { 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; 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 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) { ack = i2c.read(SLAVE_ADDR_READ,&status,1); if(ack!=0) return 0; } error = (int)status; 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 1; } 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) { wait_ms(100); 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); wait_ms(100); 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 bit_data= ((int16_t)reg_data[16+2*i+1]<<8)|(int16_t)reg_data[16+2*i]; gyro_data[i]=(float)bit_data; 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 bit_data= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i]; mag_data[i]=(float)bit_data; 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)) //ATS1 status is 01XX to check if ATS1 is ON { acq = SENSOR_DATA_ACQ(); //ATS1 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&0x0F)|0x70; 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)) //Only mag or only gyro { 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)) //ATS2 status is 01XX to check if ATS2 is ON { 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 pc_acs.printf("\r\n 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 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 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 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 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 pc_acs.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 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 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 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 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 pc_acs.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 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 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 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); //-----------------------------------------exiting the function-----------------------------------// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function }