void EVV_Init_Orientation()
{
AccAngleSmooth[axisROLL] = 0.0f;
AccAngleSmooth[axisPITCH] = 0.0f;
AccAngleSmooth[axisYAW] = 0.0f;
CameraOrient[axisROLL] = 0.0f;
CameraOrient[axisPITCH] = 0.0f;
CameraOrient[axisYAW] = 0.0f;
int init_loops = 150;
float AccAngle[NUMAXIS];
int i;
for (i = 0; i < init_loops; i++)
{
readACC(axisROLL);
readACC(axisPITCH);
readACC(axisYAW);
//AccAngle[axisROLL] = -(atan2(accADC[axisX], accADC[axisZ])); //Calculating pitch ACC angle
AccAngle[axisROLL] = -sgn(atan2(accADC[axisX], accADC[axisZ])) * fabs( atan2(accADC[axisX], sqrt(accADC[axisZ] * accADC[axisZ] + accADC[axisY] * accADC[axisY]))); //Calculating pitch ACC angle
AccAngle[axisPITCH] = +(atan2(accADC[axisY], accADC[axisZ])); //Calculating roll ACC angle
if (i == 0)
{
AccAngleSmooth[axisROLL] = AccAngle[axisROLL];
AccAngleSmooth[axisPITCH] = AccAngle[axisPITCH];
} else {
//AccAngleSmooth[axisROLL] = ((AccAngleSmooth[axisROLL] * (float)(init_loops - 1)) + AccAngle[axisROLL]) / (float)init_loops; //Averaging pitch ACC values
//AccAngleSmooth[axisPITCH] = ((AccAngleSmooth[axisPITCH] * (float)(init_loops - 1)) + AccAngle[axisPITCH]) / (float)init_loops; //Averaging roll ACC values
AccAngleSmooth[axisROLL] = ((AccAngleSmooth[axisROLL] * (float)(i - 1)) + AccAngle[axisROLL]) / (float)i; //Averaging pitch ACC values
AccAngleSmooth[axisPITCH] = ((AccAngleSmooth[axisPITCH] * (float)(i - 1)) + AccAngle[axisPITCH]) / (float)i; //Averaging roll ACC values
}
delay(1);
}
CameraOrient[axisPITCH] = AccAngleSmooth[axisPITCH];
CameraOrient[axisROLL] = AccAngleSmooth[axisROLL];
CameraOrient[axisYAW] = 0.0;
}
void ACCGYR(float *gyr_x, float *gyr_y, float *acc_x, float *acc_y)
{
float rate_gyr_y = 0.0; // [deg/s]
float rate_gyr_x = 0.0; // [deg/s]
float rate_gyr_z = 0.0; // [deg/s]
int accRaw[3];
int magRaw[3];
int gyrRaw[3];
float gyroXangle = 0.0;
float gyroYangle = 0.0;
float gyroZangle = 0.0;
float AccYangle = 0.0;
float AccXangle = 0.0;
float CFangleX = 0.0;
float CFangleY = 0.0;
//read ACC and GYR data
readACC(accRaw);
readGYR(gyrRaw);
//Convert Gyro raw to degrees per second
rate_gyr_x = (float) gyrRaw[0] * G_GAIN;
rate_gyr_y = (float) gyrRaw[1] * G_GAIN;
rate_gyr_z = (float) gyrRaw[2] * G_GAIN;
//Convert Accelerometer values to degrees
AccXangle = (float) (atan2(accRaw[1],accRaw[2])+M_PI)*RAD_TO_DEG;
AccYangle = (float) (atan2(accRaw[2],accRaw[0])+M_PI)*RAD_TO_DEG;
//Change the rotation value of the accelerometer to -/+ 180 and move the Y axis '0' point to up.
//Two different pieces of code are used depending on how your IMU is mounted.
//If IMU is upside down
/*
if (AccXangle >180)
AccXangle -= (float)360.0;
AccYangle-=90;
if (AccYangle >180)
AccYangle -= (float)360.0;
*/
//If IMU is up the correct way, use these lines
AccXangle -= (float)180.0;
if (AccYangle > 90)
AccYangle -= (float)270;
else
AccYangle += (float)90;
//return values;
*gyr_x = rate_gyr_x;
*gyr_y = rate_gyr_y;
*acc_x = AccXangle;
*acc_y = AccYangle;
}
//szenzor kiolvasas
static void prvSensorReadTask (void* pvParameters)
{
portTickType xLastWakeTime;
static uint8_t data[6];
static uint8_t newData;
AccAxesRaw_t AccAxes;
uint8_t status;
int16_t x_acc,y_acc,z_acc;
int16_t x_mag,y_mag,z_mag;
int16_t x_gyr,y_gyr,z_gyr;
xLastWakeTime=xTaskGetTickCount();
while(1)
{
GetSatusReg(&status);
if (status&0b0001000) //new data received
{
newData|=1;
// GetAccAxesRaw(&AccAxes);
readACC(data);
//allithato endiannes miatt olvasható így is
x_acc=((int16_t*)data)[0];
y_acc=((int16_t*)data)[1];
z_acc=((int16_t*)data)[2];
}
ReadStatusM(&status);
if (status&0x01)
{
readMag(data);
x_mag=((int16_t)data[0])<<8;
x_mag|=data[1];
y_mag=((int16_t)data[2])<<8;
y_mag|=data[3];
z_mag=((int16_t)data[4])<<8;
z_mag|=data[5];
}
L3G4200D_GetSatusReg(&status);
if (status&0b00001000)
{
newData|=2;
x_gyr=((int16_t*)data)[0];
y_gyr=((int16_t*)data)[1];
z_gyr=((int16_t*)data)[2];
}
if (newData==3)
{
newData=0;
}
vTaskDelayUntil(&xLastWakeTime,100);
}
}
void ACCGYR(int &accRaw, int &magRaw, int &gyrRaw, float *x, float *y)
{
float rate_gyr_y = 0.0; // [deg/s]
float rate_gyr_x = 0.0; // [deg/s]
float rate_gyr_z = 0.0; // [deg/s]
int accRaw = *accRaw; //pulling values from pointers
int magRaw = *magRaw;
int gyrRaw = *gyrRaw;
int accRaw[3];
int magRaw[3];
int gyrRaw[3];
float gyroXangle = 0.0;
float gyroYangle = 0.0;
float gyroZangle = 0.0;
float AccYangle = 0.0;
float AccXangle = 0.0;
float CFangleX = 0.0;
float CFangleY = 0.0;
//read ACC and GYR data
readACC(accRaw);
readGYR(gyrRaw);
//Convert Gyro raw to degrees per second
rate_gyr_x = (float) gyrRaw[0] * G_GAIN;
rate_gyr_y = (float) gyrRaw[1] * G_GAIN;
rate_gyr_z = (float) gyrRaw[2] * G_GAIN;
//Calculate the angles from the gyro
gyroXangle+=rate_gyr_x*DT;
gyroYangle+=rate_gyr_y*DT;
gyroZangle+=rate_gyr_z*DT;
//Convert Accelerometer values to degrees
AccXangle = (float) (atan2(accRaw[1],accRaw[2])+M_PI)*RAD_TO_DEG;
AccYangle = (float) (atan2(accRaw[2],accRaw[0])+M_PI)*RAD_TO_DEG;
//Change the rotation value of the accelerometer to -/+ 180 and move the Y axis '0' point to up.
//Two different pieces of code are used depending on how your IMU is mounted.
//If IMU is upside down
/*
if (AccXangle >180)
AccXangle -= (float)360.0;
AccYangle-=90;
if (AccYangle >180)
AccYangle -= (float)360.0;
*/
//If IMU is up the correct way, use these lines
AccXangle -= (float)180.0;
if (AccYangle > 90)
AccYangle -= (float)270;
else
AccYangle += (float)90;
//Complementary filter used to combine the accelerometer and gyro values.
CFangleX=AA*(CFangleX+rate_gyr_x*DT) +(1 - AA) * AccXangle;
CFangleY=AA*(CFangleY+rate_gyr_y*DT) +(1 - AA) * AccYangle;
*x = CFangleX;
*y = CFangleY;
}
void* getData(void* myParameter)
{
int *Pacc_raw;
int *Pmag_raw;
int acc_raw[3];
int mag_raw[3];
float magXmax,magXmin;
float magYmax,magYmin;
float magZmax,magZmin;
float compass;
int i = 0;
float AccXangle = 0.0;
float AccYangle = 0.0;
float MagX = 0.0;
float MagY = 0.0;
float MagZ = 0.0;
float Xbefore = 250.0;
float Ybefore = 250.0;
float Zbefore = 250.0;
float heading;
Pacc_raw=acc_raw;
Pmag_raw=mag_raw;
int gyroYbuff,gyroXbuff;
char filename[20];
sprintf(filename, "/dev/i2c-%d", 1);
file = open(filename, O_RDWR);
if (file<0) {
printf("Unable to open I2C bus! \n");
exit(1);
}
printf("I2C Openned \n");
enableACC();
enableMAG();
while(EnableSensor)
{
readACC(Pacc_raw);
readMAG(Pmag_raw);
AccXangle = (float) (atan2(*(acc_raw+1),*(acc_raw+2))+M_PI)*RAD_TO_DEG;
AccYangle = (float) (atan2(*(acc_raw+2),*acc_raw)+M_PI)*RAD_TO_DEG;
//MagX= (float) *(mag_raw)+482;
//MagZ= (float) *(mag_raw+1)+783;
//MagY= (float) *(mag_raw+2);
//if (MagX<magXmin){magXmin=MagX;}else if(MagX>magXmax){magXmax=MagX;}
//if (MagY<magYmin){magYmin=MagY;}else if(MagY>magYmax){magYmax=MagY;}
//if (MagZ<magZmin){magZmin=MagZ;}else if(MagZ>magZmax){magZmax=MagZ;}
//printf (" MagX \e[m %7.3f \t -- MagY %7.3f \t -- MagZ %7.3f \t \n",MagX,MagY,MagZ);
//printf (" MinX \e[m %7.3f \t -- MinY %7.3f \t -- MinZ %7.3f \t \n",magXmin,magYmin,magZmin);
//printf (" MaxX \e[m %7.3f \t -- MaxY %7.3f \t -- MaxZ %7.3f \t \n",magXmax,magYmax,magZmax);
if ((AccYangle < 185) && (AccYangle > 100)) {condition=2;}
else if ((AccYangle > 335)&&(AccYangle < 361)||((AccYangle >0)&& (AccYangle < 50))) {condition=1;}
else {condition=0;}
//printf (" AccXangle \e[m %7.3f \t -- AccYangle %7.3f \t -- AccZangle %7.3f \t \n",AccXangle,AccYangle,AccZangle);
if (i==10){virtualGyroX=0;virtualGyroY=0;i=0;}
virtualGyroX=virtualGyroX+(Xbefore-AccXangle);
virtualGyroY=virtualGyroY+(Ybefore-AccYangle);
Xbefore=AccXangle; Ybefore=AccYangle;
//conAction();
usleep(10000);
i++;
}
return 0;
}
int main(int argc, char *argv[])
{
char filename[20];
int magRaw[3];
int accRaw[3];
float accXnorm,accYnorm,pitch,roll,magXcomp,magYcomp;
//Open the i2c bus
sprintf(filename, "/dev/i2c-%d", 1);
file = open(filename, O_RDWR);
if (file<0) {
printf("Unable to open I2C bus!");
exit(1);
}
//Select the magnetomoter
if (ioctl(file, I2C_SLAVE, MAG_ADDRESS) < 0) {
printf("Error: Could not select magnetometer\n");
}
// Enable accelerometer.
writeAccReg(CTRL_REG1_XM, 0b01100111); // z,y,x axis enabled, continuos update, 100Hz data rate
writeAccReg(CTRL_REG2_XM, 0b00100000); // +/- 16G full scale
//Enable the magnetometer
writeMagReg( CTRL_REG5_XM, 0b11110000); // Temp enable, M data rate = 50Hz
writeMagReg( CTRL_REG6_XM, 0b01100000); // +/-12gauss
writeMagReg( CTRL_REG7_XM, 0b00000000); // Continuous-conversion mode
while(1)
{
readMAG(magRaw);
readACC(accRaw);
//If your IMU is upside down, comment out the two lines below which we correct the tilt calculation
//accRaw[0] = -accRaw[0];
//accRaw[1] = -accRaw[1];
//Compute heading
float heading = 180 * atan2(magRaw[1],magRaw[0])/M_PI;
//Convert heading to 0 - 360
if(heading < 0)
heading += 360;
printf("heading %7.3f \t ", heading);
//Normalize accelerometer raw values.
accXnorm = accRaw[0]/sqrt(accRaw[0] * accRaw[0] + accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]);
accYnorm = accRaw[1]/sqrt(accRaw[0] * accRaw[0] + accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]);
//Calculate pitch and roll
pitch = asin(accXnorm);
roll = -asin(accYnorm/cos(pitch));
//Calculate the new tilt compensated values
magXcomp = magRaw[0]*cos(pitch)+magRaw[02]*sin(pitch);
magYcomp = magRaw[0]*sin(roll)*sin(pitch)+magRaw[1]*cos(roll)-magRaw[2]*sin(roll)*cos(pitch);
//Calculate heading
heading = 180*atan2(magYcomp,magXcomp)/M_PI;
//Convert heading to 0 - 360
if(heading < 0)
heading += 360;
printf("Compensated Heading %7.3f \n", heading);
//Sleep for 25ms
usleep(25000);
}
}
int main(int argc, char** argv)
{ //
int select = 102;
int Tdelay = 10 ;
long Mdelay = 10000 ;
// Switch statement control variable
int cnt = 30000 ;
int dutcycl = 24000 ;
int mot = 5000 ;
int period = 3125 ;
int pos = 0 ;
int position = 16000 ;
int stat = 0 ; // return status
int valu = 0 ; //returned value
int catch = 500 ;
char IMU_buf[96]={0x55} ;
int IMU_val[6] ; // IMU 3 axis value
long int nanoseconds=0 ;
long int dutcyc = 0 ;
int PWM_Positions[10] = {16000,18000,20000,22000,24000,26000,28000,30000,32000,16000} ;
// Initialization begins here
system.init() ;
dateTime.init() ;
_TRISE6 = 0; // configure port as output
_RE6 = 1; // set the output (active high) Turn on 6Volt supply
// Init I21C2
// Init Servo motor
PWM9_init() ;
SET_PWM9_Period(period) ;
SET_PWM9_DutyCycle(dutcycl) ;
// Init A2D Channel 10- motor current
// A2D_c10_init() ;
Analog_init() ;
// i2c2_TalkToDevice(RTC_ADDR, RTC_SET_LENGTH, STG_SET_TIME, 0, 0) ;
i2c2_init(75) ; // I2C2_KHz
i2c2_reset_bus() ;
while (cnt>0)
{ switch (select)
{ case 0 :
while (mot>0)
{ pos=0 ;
while (pos <9)
{ position=PWM_Positions[pos];
SET_PWM9_DutyCycle(position) ;
delay(Mdelay) ;
pos++ ;
}
--mot ;
}
break ;
case 1 :
valu = Read_Switch_Align() ;
break ;
case 2 :
valu = Read_Switch_Home() ;
break ;
case 3 :
nanoseconds= RSP78_init () ;
break ;
case 4 :
dutcyc = RSP78_Read_Sig_Period() ;
break ;
case 5 :
valu = A2D_c10_read() ;
break ;
case 6 :
valu = A2D_c10_sample(5,1) ;
break ;
case 7 :
SET_PWM9_Period( period) ;
break ;
case 8 :
SET_PWM9_DutyCycle(dutcycl) ;
break ;
case 9 :
break ;
// *********************************************
// Camera 1 Testing
// ******************************************
case 10 :
initializeCam1() ;
break ;
case 11 :
setOutputCam1(0) ;
break ;
case 12 :
setOutputCam1(1) ;
break ;
case 13 :
setOffCam1() ;
break ;
case 14 :
setOnCam1() ;
break ;
// *********************************************
// Camera 2 Testing
// *****************************************
case 20 :
initializeCam2() ;
break ;
case 21 :
setOutputCam2(0) ;
break ;
case 22 :
setOutputCam2(1) ;
break ;
case 23 :
setOffCam2() ;
break ;
case 24 :
setOnCam2() ;
break ;
// *********************************************
// LED 1 Testing
// *********************************************
case 30 :
initializeLED1() ;
break ;
case 31 :
setOutputLED1(0) ;
break ;
case 32 :
setOutputLED1(1) ;
break ;
case 33 :
setOffLED1() ;
break ;
case 34 :
setOnLED1() ;
break ;
// *********************************************
// LED2 Testing
// *********************************************
case 40 :
initializeLED2() ;
break ;
case 41 :
setOutputLED2(0) ;
break ;
case 42 :
setOutputLED2(1) ;
break ;
case 43 :
setOffLED2() ;
break ;
case 44 :
setOnLED2() ;
break ;
// *********************************************
// Servo 1 Testing
// *********************************************
case 50 :
initializeServo() ;
break ;
case 51 :
setOutputServo(0) ;
break ;
case 52 :
setOutputServo(1) ;
break ;
case 53 :
setOffServo() ;
break ;
case 54 :
setOnServo() ;
break ;
// *********************************************
// PING I2C2 Device Testing
// *********************************************
case 60 :
i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; // Clear Diagn ostic Countrers
break ;
case 61 :
stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ;
break ;
case 62 :
setI2C2_RTCTime(RTC_I2C_TimeStampData) ; // RGB Gesture
select = 63 ;
break ;
case 63 :
getI2C2_RTCTime(RTC_I2C_TimeStampData) ; //Temp Humidity Pressure
break ;
case 64 :
stat=i2c2_TalkToDevice(0x6B, 0, NullPtr, 0, NullPtr) ;
break ;
case 65 :
stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ;
stat=i2c2_TalkToDevice(0x39, 0, NullPtr, 0, NullPtr) ;
stat=i2c2_TalkToDevice(0x77, 0, NullPtr, 0, NullPtr) ;
stat=i2c2_TalkToDevice(0x1D, 0, NullPtr, 0, NullPtr) ;
stat=i2c2_TalkToDevice(0x6B, 0, NullPtr, 0, NullPtr) ;
break ;
case 66 :
// dt=datetime.get ;
break ;
case 69 :
stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ; // Real Time Clock Set Harmon Time
stat=i2c2_TalkToDevice(0x39, 0, NullPtr, 0, NullPtr) ; // RGBC
stat=i2c2_TalkToDevice(0x77, 0, NullPtr, 0, NullPtr) ; // THP
stat=i2c2_TalkToDevice(0x1D, 0, NullPtr, 0, NullPtr) ; // Accel/Magne
stat=i2c2_TalkToDevice(0x6B, 0, NullPtr, 0, NullPtr) ; // Gyro
break ;
// *********************************************
// RTC I2C2 Test both Real Time Clocks
// *********************************************
case 70 :
// Clear Diagnostic Countrers
stat=i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ;
delay(1) ;
break ;
case 71 :
// Ping using talk to device
stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ;
delay(1) ;
break ;
case 72 :
stat=i2c2_TalkToDevice(RTC_ADDR, RTC_GET_LENGTH, RTC_SET_TIME, RTC_RSP_LENGTH, RTC_GET_TIME) ; // get I2C RTC BCD string
delay(1) ;
break ;
case 73 :
stat=i2c2_TalkToDevice(RTC_ADDR, RTC_SET_LENGTH, RTC_SET_TIME, 0, NullPtr) ; // set I2C RTC BCD string
delay(1) ;
break ;
case 74 :
stat=getI2C2_RTCTime(RTC_I2C_TimeStampData) ; // Get I2C RTC into TimeStamp STring
delay(1) ;
break ;
case 75 :
stat = setI2C2_RTCTime(RTC_I2C_TimeStampData) ; // Set I2c RTC from TimeStamp STring
delay(1) ;
break ;
case 76 :
getTimeStamp(RTC_I2C_TimeStampData) ; // get PIC RTC into timestamp string
delay(1) ;
break ;
case 77 :
setPIC_RTCTime(RTC_I2C_TimeStampData) ; // set PIC RTC from timestamp string
delay(1) ;
break ;
case 78 :
delay(1) ;
break ;
case 79 :
stat=getI2C2_RTCTime(RTC_I2C_TimeStampData) ;
setPIC_RTCTime(RTC_I2C_TimeStampData) ;
delay(1) ;
break ;
// *********************************************
// GYR I2C2 Decive Testing
// *********************************************
case 80 :
// Clear the Diagnostic buffer
stat=i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; // Clear Diagn ostic Countrers
delay (1) ;
break ;
case 81 :
stat=enableIMU() ;
delay (1) ;
break ;
case 82 :
stat= dumpIMU(IMU_buf) ;
delay (1) ;
break ;
case 83 :
stat= readACC(IMU_val) ;
delay (1) ;
break ;
case 84 :
stat=readMAG(IMU_val) ;
delay (1) ;
break ;
case 85 :
stat=readGYR(IMU_val) ;
delay (1) ;
break ;
case 86 :
stat=i2c2_TalkToDevice(ACC_Addr, 0, NullPtr, 0, NullPtr) ;
delay (1) ;
break ;
case 87 :
stat=gyro3_main(argc, argv) ;
delay (1) ;
break ;
case 88 :
stat=AMG5_main(argc, argv) ;
delay (1) ;
break ;
case 89 :
delay (1) ;
break ;
// *********************************************
// RGB I2C2 Decive Testing
// *********************************************
case 90 :
stat=i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; // Clear Diagn ostic Countrers
RGB_Configure1 () ;
select=94 ; // read back values
break ;
case 91 :
RGB_Configure2 () ;
break ;
case 92 :
RGB_Configure3 () ;
break ;
case 93 :
RGB_ReadStatus () ;
break ;
case 94 :
RGB_ReadValues () ;
break ;
case 95 :
RGB_ClearIrqs () ;
break ;
case 99 :
stat=RGB_Configure1 () ;
stat=RGB_ReadStatus () ;
stat=RGB_ReadValues () ;
stat=RGB_ReadStatus () ;
stat=RGB_ClearIrqs () ;
break ;
// *********************************************
// THP I2C2 Device Testing
// *********************************************
case 100 :
stat= i2c2_TalkToDevice(THP_Addr,0, NullPtr, 0, NullPtr) ; // ping device
break ;
case 101 :
// i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ;
stat = THP_Configure1 () ;
// select=105 ; // read back values
break ;
case 102 :
i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ;
stat = THP_Configure2 () ;
select=106 ; // read back values
break ;
case 103 :
stat = THP_Configure3 () ;
break ;
case 104 :
// stat = THP_ReadStatus () ; // part of read values
// select=104 ; // read back values
break ;
case 105 :
// stat = THP_Diag_RdIrqs () ;
THP_Init () ;
break ;
case 106 :
stat = THP_Read_Values () ;
break ;
case 107 :
// stat = THP_Diag_RdIrqs () ;
break ;
case 108 :
// stat = THP_Init () ;
// stat = THP_Diag_RdIrqs () ;
break ;
case 109 :
stat = THP_Configure1 () ;
// THP_ReadStatus () ;
stat = THP_Read_Values () ;
// stat = THP_ReadStatus () ;
// THP_Diag_RdIrqs () ;
break ;
default:
delay(10) ;
} // end of switch
// *********************************************
// SWITCH ENDS
// *********************************************
if(cnt % catch==0)
{ delay(10) ;
}
delay (Tdelay) ;
cnt-- ;
} // end of while
return (0);
}
