/**
* 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");
}
}
}
