float quad(int a, int b, int c, int sign)
{
float s;
if((b*b)-(4*a*c)>0)
{
s=sqrt1(b*b-4*a*c);
if(s>=0.0)
{
if(sign)
{
return (((float)(-1*b + s))/2*a);
}
else
return (((float)(-1*b - s))/2*a);
}
else
return (float)-1;
}
else
return (float)-1;
}
void MPU6050Task(void) {
char uart_out[32];
uint8_t controller_command = 0;
uint8_t pre_command = 0;
uint8_t count = 0;
MPU6050_Task_Suspend();
vTaskDelayUntil(&xLastWakeTime, xFrequency); // wait while sensor is ready
initKalman(&kalmanX);
initKalman(&kalmanY);
MPU6050_ReadAccelerometer();
accX = MPU6050_Data.Accelerometer_X;
accY = MPU6050_Data.Accelerometer_Y;
accZ = MPU6050_Data.Accelerometer_Z;
float roll = atan2(-accY, -accZ) * RAD_TO_DEG;
float pitch = atan(-accX / sqrt1(Square(accY) + Square(accZ))) * RAD_TO_DEG;
setAngle(&kalmanX, roll); // Set starting angle
setAngle(&kalmanY, pitch);
while (1) {
/* Read all data from sensor */
MPU6050_ReadAccGyo();
accX = MPU6050_Data.Accelerometer_X;
accY = MPU6050_Data.Accelerometer_Y;
accZ = MPU6050_Data.Accelerometer_Z;
gyroX = MPU6050_Data.Gyroscope_X;
gyroY = MPU6050_Data.Gyroscope_Y;
gyroZ = MPU6050_Data.Gyroscope_Z;
float roll = atan2(-accY, -accZ) * RAD_TO_DEG;
float pitch = atan(-accX / sqrt1(Square(accY) + Square(accZ))) * RAD_TO_DEG;
float gyroXrate = gyroX * MPU6050_Data.Gyro_Mult; // Convert to deg/s
float gyroYrate = gyroY * MPU6050_Data.Gyro_Mult; // Convert to deg/s
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
setAngle(&kalmanX, roll);
} else {
kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
}
if (Abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt);
#ifdef DEBUG
USART1_puts("\r\n");
shell_float2str(accZ, uart_out);
USART1_puts(uart_out);
#else
if (accY < -6300) {
controller_command = 1;
} else if (accY > 6300) {
controller_command = 2;
} else if (accZ < 0 && kalAngleY > 47) {
controller_command = 3;
} else if (accZ < 0 && kalAngleY < -30) {
controller_command = 4;
} else if (accZ < 0 && kalAngleY > 25 && kalAngleY < 47) {
controller_command = 5;
} else {
controller_command = 6;
}
// check hand gesture for a while
if (count == 5 && pre_command == controller_command) {
switch (controller_command) {
case 1:
USART1_puts("\r\nmove right");
break;
case 2:
USART1_puts("\r\nmove left");
break;
case 3:
USART1_puts("\r\nforward");
break;
case 4:
USART1_puts("\r\nDOWN");
break;
case 5:
USART1_puts("\r\nUP");
break;
case 6:
USART1_puts("\r\nsuspend");
break;
}
} else if (pre_command == controller_command) {
count++;
} else {
pre_command = controller_command;
count = 0;
}
#endif
vTaskDelayUntil(&xLastWakeTime, xFrequency);
}
}
