void MultitouchJugador::calcularVelocidad(float newPos)
{
// float newTime, deltaT;
// newTime = getTimeTick();
// deltaT = newTime - lastTime;
// lastTime = newTime;
float deltaT = getDeltaT();
if (deltaT == 0) return;
float deltaPos;
deltaPos = newPos - yPosition;
yPosition = newPos;
float carga, descarga;
carga = sensibilidadCarga * deltaPos / deltaT;
descarga = sensibilidadDescarga * deltaT;
float newVelocity = yVelocity + carga;
if (fabs(descarga) >= fabs(newVelocity)) {
yVelocity = 0;
} else {
descarga *= newVelocity >= 0 ? 1 : -1;
yVelocity = newVelocity - descarga;
}
}
//计算飞行器姿态
void ANO_IMU::getAttitude()
{
float deltaT;
Vector3d accTemp, gyroTemp;
//加速度数据一阶低通滤波
acc_lpf = LPF_1st(acc_lpf, acc, ano.factor.acc_lpf);
//计算实际测量的加速度和重力加速度的比值
accRatio = acc_lpf.length_squared() * 100 / (ACC_1G * ACC_1G);
deltaT = getDeltaT(GetSysTime_us());
}
void MultitouchJugador::ccTouchEnded(CCTouch *pTouch, CCEvent *pEvent)
{
isTouching = false;
CCPoint posicion = pTouch->getLocation();
calcularVelocidad(posicion.y);
// float ahora, power;
// ahora = getTimeTick();
float power;
getDeltaT(); // actualiza totaldt
power = potenciaMinima + (totaldt) * fuerza;
power = MIN(power, potenciaMaxima);
// printf("ccTouchEnded %i\t con velocidad: %f; y potencia: %f:\n", ID, yVelocity, power);
resultadoGolpe.spin = yVelocity;
resultadoGolpe.power = power;
callbackGolpe->execute();
}
//计算飞行器姿态
void IMU_GetAttitude()
{
float deltaT;
#ifdef ANO_IMU_USE_LPF_1st
//加速度数据一阶低通滤波
Acc_lpf = LowPassFilter_1st(Acc_lpf, Acc, ano.factor.acc_lpf);
#endif
#ifdef ANO_IMU_USE_LPF_2nd
//加速度数据二阶低通滤波
imu.Acc_lpf = LowPassFilter_2nd(&imu.Acc_lpf_2nd, imu.Acc);
#endif
deltaT = getDeltaT(GetSysTime_us());
#ifdef ANO_IMU_USE_DCM_CF
DCM_CF(imu.Gyro,imu.Acc_lpf,deltaT);
#endif
#ifdef ANO_IMU_USE_Quaternions_CF
Quaternion_CF(Gyro,Acc_lpf_1st,deltaT);
#endif
}
