void Bucle_Principal() { int salida=0; int angulo = 0; int who_a_I_tmp = 0; //------------------------------------------------------------------------------------------// double accXangle = (atan2(get_ax(), -get_ay()) * RAD_TO_DEG ); double gyroXrate = (double) (get_gz() ) / 131.0; if (tipoFiltro==1){ angulo = (signed int) Complementary2(accXangle, gyroXrate,Tsample)-90; } else { angulo = (signed int) kalman(accXangle, gyroXrate,Tsample)-90; } if (act_motor==1){ salida = pid(setpoint, angulo,5, KP, KI, KD, 5000, -5000, 10000, -10000); SetPwm1(BIAS1 - salida); SetPwm2(BIAS2 + salida); who_a_I_tmp = get_who_I_AM(); if(who_a_I_tmp != 104) { Init_I2C(); reset(); } enviar_valor_NOCR("ax= ",get_ax()); enviar_valor_NOCR(" ay = ",get_ay()); enviar_valor_NOCR(" gz= ",get_gz()); enviar_valor(" who = ",get_who_I_AM()); } else { SetPwm1(0); SetPwm2(0); ErrorI=0; } plot4(accXangle-90,angulo,salida/10,setpoint); LED_ROJO=!LED_ROJO; }
void IMU::update(){ read(); now = millis(); int looptime = (now - lastAHRSTime); lastAHRSTime = now; if (state == IMU_RUN){ // ------ roll, pitch -------------- float forceMagnitudeApprox = abs(acc.x) + abs(acc.y) + abs(acc.z); //if (forceMagnitudeApprox < 1.2) { //Console.println(forceMagnitudeApprox); accPitch = atan2(-acc.x , sqrt(sq(acc.y) + sq(acc.z))); accRoll = atan2(acc.y , acc.z); accPitch = scalePIangles(accPitch, ypr.pitch); accRoll = scalePIangles(accRoll, ypr.roll); // complementary filter ypr.pitch = Kalman(accPitch, gyro.x, looptime, ypr.pitch); ypr.roll = Kalman(accRoll, gyro.y, looptime, ypr.roll); /*} else { //Console.print("too much acceleration "); //Console.println(forceMagnitudeApprox); ypr.pitch = ypr.pitch + gyro.y * ((float)(looptime))/1000.0; ypr.roll = ypr.roll + gyro.x * ((float)(looptime))/1000.0; }*/ ypr.pitch=scalePI(ypr.pitch); ypr.roll=scalePI(ypr.roll); // ------ yaw -------------- // tilt-compensated yaw comTilt.x = com.x * cos(ypr.pitch) + com.z * sin(ypr.pitch); comTilt.y = com.x * sin(ypr.roll) * sin(ypr.pitch) + com.y * cos(ypr.roll) - com.z * sin(ypr.roll) * cos(ypr.pitch); comTilt.z = -com.x * cos(ypr.roll) * sin(ypr.pitch) + com.y * sin(ypr.roll) + com.z * cos(ypr.roll) * cos(ypr.pitch); comYaw = scalePI( atan2(comTilt.y, comTilt.x) ); comYaw = scalePIangles(comYaw, ypr.yaw); //comYaw = atan2(com.y, com.x); // assume pitch, roll are 0 // complementary filter ypr.yaw = Complementary2(comYaw, -gyro.z, looptime, ypr.yaw); ypr.yaw = scalePI(ypr.yaw); } else if (state == IMU_CAL_COM) { calibComUpdate(); } }