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); }