/** * Computes gyro offsets */ void FreeIMU::zeroGyro() { const int totSamples = 3; int16_t raw[11]; float tmpOffsets[] = {0,0,0}; for (int i = 0; i < totSamples; i++){ getRawValues(raw); tmpOffsets[0] += raw[3]; tmpOffsets[1] += raw[4]; tmpOffsets[2] += raw[5]; } gyro_off_x = tmpOffsets[0] / totSamples; gyro_off_y = tmpOffsets[1] / totSamples; gyro_off_z = tmpOffsets[2] / totSamples; }
/* set gyro start angle -----------------------------------------------------*/ void setAngleFilterACC(vs32 * gyroAngle, vs32 * copterAngle) { vs32 accRawValues[3]; vs32 gyroRawValues[3]; vs32 accAngle[2]; getRawValues(gyroRawValues, accRawValues); getACCAnglesFilterACC(accAngle, accRawValues); copterAngle[X] = gyroAngle[X] = accAngle[X]; copterAngle[Y] = gyroAngle[Y] = accAngle[Y]; copterAngle[Z] = gyroAngle[Z] = targetAngle[Z] = 0; char x [80]; sprintf(x,"gyro start value:%d:%d:%d\r\n", gyroAngle[X], gyroAngle[Y], gyroAngle[Z]); print_uart1(x); }
/* mix Gyro and ACC for Copter-Angel ----------------------------------------*/ void getCopterAnglesFilterHH(vs32 * gyroAngle, vs32 * copterAngle) { vs32 accRawValues[3]; vs32 gyroRawValues[3]; getRawValues(gyroRawValues, accRawValues); //char x [80]; //sprintf(x,"raw value:%d:%d\r\n",gyroRawValues[X],accRawValues[X]); //print_uart1(x); // get basis angles from sensors getGyroAnglesFilterHH(gyroAngle, gyroRawValues); //getACCAnglesFilterHH(accAngle, accRawValues); copterAngle[X] = gyroAngle[X]; copterAngle[Y] = gyroAngle[Y]; copterAngle[Z] = gyroAngle[Z]; }
/* mix Gyro and ACC for Copter-Angel ----------------------------------------*/ void getCopterAnglesFilterACC(vs32 * gyroAngle, vs32 * accAngle, vs32 * copterAngle) { vs32 accRawValues[3]; vs32 gyroRawValues[3]; getRawValues(gyroRawValues, accRawValues); // we read compass only if HW_Bit is set if (getParameter(PARA_HW) & PARA_HW_COMP) { getCompassAngle(); } // get basis angles from sensors getGyroAnglesFilterACC(gyroAngle, gyroRawValues); getACCAnglesFilterACC(accAngle, accRawValues); s32 factorDegree = (abs(copterAngle[X] - 9000000) / 100000) + (abs(copterAngle[Y] - 9000000) / 100000)+ 10; factorDegree = factorDegree * factorDegree; u32 accForce = (parameter[PARA_ACC_FORCE] * 100 / factorDegree) + 1; u32 gyroCorr = (parameter[PARA_GYRO_CORR] * 100 / factorDegree) + 1; //needs about 50us (all 5) copterAngle[X] = weightingValues(accAngle[X], gyroAngle[X], accForce); copterAngle[Y] = weightingValues(accAngle[Y], gyroAngle[Y], accForce); copterAngle[Z] = gyroAngle[Z]; //char x [80]; //sprintf(x,"%d:%d:%d\r\n",factorDegree,abs(copterAngle[X] - 9000000),abs(copterAngle[Y] - 9000000)); //print_uart1(x); // trimm gyro to new Angle gyroAngle[X] = weightingValues(copterAngle[X], gyroAngle[X], gyroCorr); gyroAngle[Y] = weightingValues(copterAngle[Y], gyroAngle[Y], gyroCorr); }
int main() { //initialize pids init_pids(); //initialize pwm pwm1.period_ms(100); pwm2.period_ms(100); pwm3.period_ms(100); pwm4.period_ms(100); pwm1.write(0.00f); pwm2.write(0.00f); pwm3.write(0.00f); pwm4.write(0.00f); bool connection=0; setup();//Prepare Mpu6050 For Communication if(MPU6050.testConnection()) { pc.printf("MPU6050 connected...\r\n"); connection=1; // myledR=1; myledG=0;//connection led } else { pc.printf("MPU6050 not connected...\r\n"); connection=0; //myledR=1; myledG=0; } while (connection) { if(MPU6050.testConnection()) { getRawValues(); //transCeiver(); // ...add orientation data to the transmit buffer txData[0] =rx; txData[1] =ry; txData[2] =rz; txData[3] =heightF; // If the transmit buffer is full //pc.printf("Function executed \r\n"); my_nrf24l01p.disable(); my_nrf24l01p.setTransmitMode(); my_nrf24l01p.enable(); // Send the transmitbuffer via the nRF24L01+ //wait(0.2f); txDatacnt=my_nrf24l01p.write(NRF24L01P_PIPE_P0, txData, sizeof(txData)); my_nrf24l01p.disable(); my_nrf24l01p.setReceiveMode(); my_nrf24l01p.enable(); if (my_nrf24l01p.readable()) { // ...read the data into the receive buffer rxDataCnt = my_nrf24l01p.read(NRF24L01P_PIPE_P0, rxData, sizeof(rxData)); recvd=rxData[0]; // Display the receive buffer contents via the host serial link switch(recvd) { case 'F': myledR=1; setpP-=1; setpR=0; // pc.printf("moving forward... \r\n"); rxData[0]='0'; break; case 'B': myledR=1; setpP+=1; setpR=0; //pc.printf("moving Backward...\r\n"); rxData[0]='0'; break; case 'S': myledR=1; setpP=0; setpR=0; //pc.printf("stoping... \r\n"); rxData[0]='0'; break; case 'R': setpP=0; setpR-=1; //pc.printf("Rolling...right \r\n"); rxData[0]='0'; break; case 'L': myledR=1; setpP=0; setpR+=1; //pc.printf("Rolling...left \r\n"); rxData[0]='0'; break; //default : //pc.printf("Waiting... \r"); //pc.printf("Waiting... %c\r",recvd); //break; } // pc.printf("recvd %c\r\n",recvd); } //calculate orientation and determine motor power ACCEL_YANGLE=ry;ACCEL_XANGLE=rx; rollF=pid_out(&roll,ACCEL_YANGLE,setpR); pitchF=pid_out(&pitch,ACCEL_XANGLE,setpP); pwmsignals((float)heightF,(float)rollF,(float)yawF,(float)pitchF);//call function for esc // pc.printf("Motor1:%.2f\tMotor2:%.2f\tMotor3:%.2f\tMotor4:%.2f\r",motor1power,motor2power,motor3power,motor4power); //writing current orientation pc.printf("\rPitch %.4f\tRoll %.2f\tZ_angle %.4f\t\r",rx,ry,rz); } else { pc.printf("No connection..\r\n"); } } }