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
ACS.cpp
- Committer:
- lakshya
- Date:
- 2016-07-05
- Revision:
- 39:670133e7ffd8
- Parent:
- 20:949d13045431
- Child:
- 40:c2538d97e78b
- Child:
- 45:b5bd48ffbb67
- Child:
- 49:61c9f28332ba
File content as of revision 39:670133e7ffd8:
/*------------------------------------------------------------------------------------------------------------------------------------------------------
-------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
#include <mbed.h>
#include <math.h>
#include "pni.h" //pni header file
#include "pin_config.h"
#include "configuration.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 DigitalInOut ATS1_SW_ENABLE; // enable of att sens2 switch
extern DigitalInOut 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;
DigitalInOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
DigitalInOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
DigitalInOut 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 B_SCZ_ANGLE = 0x00;
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()
{
uint8_t value;
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]=SENTRALSTATUS;
i2c.write(SLAVE_ADDR,cmd,1);
i2c.read(SLAVE_ADDR_READ,&store,1);
value = (uint8_t)store;
if(value & 0x02)
{
printf("Sentral already has eeprom firmware loaded.\n");
}
/* Write value 0x01 to the ResetReq register, address 0x9B. This will result
in a hard reset of the Sentral. This is unnecessary if the prior event was
a Reset. */
if(!(value & 0x08))
{
printf("CPU is not in standby, issuing a shutdown request.\n");
//i2c_write(I2C_SLAVE_ADDR, 0x34, data, 1);
cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to shut down
cmd[1]=0x00;
i2c.write(SLAVE_ADDR,cmd,2);
int cnt=0;
do {
cmd[0]=SENTRALSTATUS;
i2c.write(SLAVE_ADDR,cmd,1);
i2c.read(SLAVE_ADDR_READ,&store,1);
value = (uint8_t)store;
wait_ms(100);
cnt++;
} while((!(value & 0x08))&&(cnt<4));
if(cnt==4)
{
return 0;
}
}
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; //0x0000 is written in RAM register to enable upload
cmd[1]=0x0000;
i2c.write(SLAVE_ADDR,cmd,3);
wait_ms(100);
printf("Uploading data...\n");
#define TRASACTION_SIZE 3
for(int i = 0; i < EEPROMTextLength; i += TRASACTION_SIZE * 4)
{
char* data = new char[TRASACTION_SIZE * 4];
data[0]=0x96;
for(int j = 0; j < TRASACTION_SIZE; j++)
{
data[j * 4 + 1] = configdata[i + j * 4 + 3];
data[j * 4 + 2] = configdata[i + j * 4 + 2];
data[j * 4 + 3] = configdata[i + j * 4 + 1];
data[j * 4 + 4] = configdata[i + j * 4 + 0];
}
if(EEPROMTextLength < (i + (TRASACTION_SIZE * 4)))
{
uint32_t bytes = EEPROMTextLength - i;
i2c.write(SLAVE_ADDR,data,bytes+1);
}
else
{
/* Write the Configuration File to Sentral’s program RAM. The file is sent
one byte at a time, using the UploadData register, register address 0x96. */
i2c.write(SLAVE_ADDR,data,13);
}
delete data;
}
char crc[4];
cmd[0]=0x97;
i2c.write(SLAVE_ADDR,cmd,1);
i2c.read(SLAVE_ADDR_READ,crc,4);
value = (uint8_t)store;
uint32_t actualCRC = ((uint32_t)crc[0] << 0) | ((uint32_t)crc[1] << 8) | ((uint32_t)crc[2] << 16) | ((uint32_t)crc[3] << 24);
if(actualCRC != EEPROMTextCRC)
{
pc_acs.printf("Program crc (0x%.8X) does not match CRC reported by Sentral (0x%0.8X)\n", EEPROMTextCRC, actualCRC);
return 0;
}
else
{
pc_acs.printf("Firmware Upload Complete.\n");
return 1;
}
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
}