void AccelGyro::calculate() { // Gyro data receiver int error; double dT; error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro); // Get the time of reading for rotation computations unsigned long t_now = millis(); // Convert gyro values to degrees/sec float FS_SEL = 131; float gyro_x = (accel_t_gyro.value.x_gyro - accel_t_gyro_global.base_x_gyro)/FS_SEL; float gyro_y = (accel_t_gyro.value.y_gyro - accel_t_gyro_global.base_y_gyro)/FS_SEL; float gyro_z = (accel_t_gyro.value.z_gyro - accel_t_gyro_global.base_z_gyro)/FS_SEL; // Get raw acceleration values //float G_CONVERT = 16384; float accel_x = accel_t_gyro.value.x_accel; float accel_y = accel_t_gyro.value.y_accel; float accel_z = accel_t_gyro.value.z_accel; // Get angle values from accelerometer float RADIANS_TO_DEGREES = 180/3.14159; float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; float accel_angle_z = 0; // Compute the (filtered) gyro angles float dt =(t_now - get_last_time())/1000.0; float gyro_angle_x = gyro_x*dt + get_last_x_angle(); float gyro_angle_y = gyro_y*dt + get_last_y_angle(); float gyro_angle_z = gyro_z*dt + get_last_z_angle(); // Compute the drifting gyro angles float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(); float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(); float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(); // Apply the complementary filter to figure out the change in angle - choice of alpha is // estimated now. Alpha depends on the sampling rate... float alpha = 0.96; float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x; float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y; float angle_z = gyro_angle_z; //Accelerometer doesn't give z-angle // Update the saved data with the latest values accel_t_gyro_global.x_angle = angle_x; accel_t_gyro_global.y_angle = angle_y; accel_t_gyro_global.z_angle = angle_z; accel_t_gyro_global.x_last_five[accel_t_gyro_global.position] = angle_x; accel_t_gyro_global.position = (accel_t_gyro_global.position + 1) % 5; set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z); }
// The sensor should be motionless on a horizontal surface // while calibration is happening void calibrate_sensors() { //int num_readings = 20; int num_readings = 1; float x_accel = 0; float y_accel = 0; float z_accel = 0; float x_gyro = 0; float y_gyro = 0; float z_gyro = 0; accel_t_gyro_union accel_t_gyro; Serial.println(F("")); Serial.println(F("Sensor Calibrating..")); // Discard the first set of values read from the IMU read_gyro_accel_vals((uint8_t *) &accel_t_gyro); // Read and average the raw values from the IMU for (int i = 0; i < num_readings; i++) { read_gyro_accel_vals((uint8_t *) &accel_t_gyro); x_accel += accel_t_gyro.value.x_accel; y_accel += accel_t_gyro.value.y_accel; z_accel += accel_t_gyro.value.z_accel; x_gyro += accel_t_gyro.value.x_gyro; y_gyro += accel_t_gyro.value.y_gyro; z_gyro += accel_t_gyro.value.z_gyro; delay(100); } x_accel /= num_readings; y_accel /= num_readings; z_accel /= num_readings; x_gyro /= num_readings; y_gyro /= num_readings; z_gyro /= num_readings; // Store the raw calibration values globally base_x_accel = x_accel; base_y_accel = y_accel; base_z_accel = z_accel; base_x_gyro = x_gyro; base_y_gyro = y_gyro; base_z_gyro = z_gyro; // hard code base_x_accel = 1164.40002; base_y_accel = -625.40002; base_z_accel = 17013.59960; base_x_gyro = -222.30000; base_y_gyro = -57.75000; base_z_gyro = -122.90000; set_last_read_angle_data(micros(), 0, 0, 0, 0, 0, 0); /* Serial.print(F("base_x_accel: ")); Serial.println(base_x_accel,5); Serial.print(F("base_y_accel: ")); Serial.println(base_y_accel,5); Serial.print(F("base_z_accel: ")); Serial.println(base_z_accel,5); Serial.print(F("base_x_gyro: ")); Serial.println(base_x_gyro,5); Serial.print(F("base_y_gyro: ")); Serial.println(base_y_gyro,5); Serial.print(F("base_z_gyro: ")); Serial.println(base_z_gyro,5); */ Serial.println(F("Finishing Calibration")); }
void complement_filter(unsigned long t_now){ int error; accel_t_gyro_union accel_t_gyro; // Read the raw values. error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro); //out_draw_data(accel_t_gyro); float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro)/FS_SEL; float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro)/FS_SEL; float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro)/FS_SEL; // Get raw acceleration values //float G_CONVERT = 16384; float accel_x = accel_t_gyro.value.x_accel; float accel_y = accel_t_gyro.value.y_accel; float accel_z = accel_t_gyro.value.z_accel; // Get angle values from accelerometer // float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2)); float unfiltered_accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RAD_TO_DEG; float unfiltered_accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RAD_TO_DEG; float unfiltered_accel_angle_z = 0; // Compute the (filtered) gyro angles float dt =(t_now - get_last_time())/MILLION; // dt is in micro second float gyro_angle_x = gyro_x*dt + get_last_x_angle(); float gyro_angle_y = gyro_y*dt + get_last_y_angle(); float gyro_angle_z = gyro_z*dt + get_last_z_angle(); // Compute the drifting gyro angles float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(); float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(); float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(); // Apply the complementary filter to figure out the change in angle - choice of alpha is // estimated now. Alpha depends on the sampling rate... angle[AXIS_X] = alpha*gyro_angle_x + (1.0 - alpha)*unfiltered_accel_angle_x; angle[AXIS_Y] = alpha*gyro_angle_y + (1.0 - alpha)*unfiltered_accel_angle_y; angle[AXIS_Z] = gyro_angle_z; //Accelerometer doesn't give z-angle // Update the saved data with the latest values set_last_read_angle_data(t_now, angle[AXIS_X], angle[AXIS_Y], angle[AXIS_Z], unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z); #if 0 // Send the data to the serial port Serial.print(F("DEL:")); //Delta T Serial.print(dt, DEC); Serial.print(F("#ACC:")); //Accelerometer angle Serial.print(unfiltered_accel_angle_x, 2); Serial.print(F(",")); Serial.print(unfiltered_accel_angle_y, 2); Serial.print(F(",")); Serial.print(unfiltered_accel_angle_z, 2); Serial.print(F("#GYR:")); Serial.print(unfiltered_gyro_angle_x, 2); //Gyroscope angle Serial.print(F(",")); Serial.print(unfiltered_gyro_angle_y, 2); Serial.print(F(",")); Serial.print(unfiltered_gyro_angle_z, 2); Serial.print(F("#FIL:")); //Filtered angle Serial.print(angle_x, 2); Serial.print(F(",")); Serial.print(angle_y, 2); Serial.print(F(",")); Serial.print(angle_z, 2); Serial.println(F("")); #endif }
/** * @brief Main program * @param None * @retval None */ int main(void) { /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup files (startup_stm32f40xx.s/startup_stm32f427x.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f4xx.c file */ /* USART configuration -----------------------------------------------------*/ USART_Config(); /* SysTick configuration ---------------------------------------------------*/ SysTickConfig(); /* LEDs configuration ------------------------------------------------------*/ STM_EVAL_LEDInit(LED3); STM_EVAL_LEDInit(LED4); STM_EVAL_LEDInit(LED5); STM_EVAL_LEDInit(LED6); STM_EVAL_LEDOn(LED3);//orange STM_EVAL_LEDOn(LED4);//verte STM_EVAL_LEDOn(LED5);//rouge STM_EVAL_LEDOn(LED6);//bleue //PWM config (motor control) TIM1_Config(); PWM1_Config(10000); /* Tamper Button Configuration ---------------------------------------------*/ STM_EVAL_PBInit(BUTTON_USER,BUTTON_MODE_GPIO); //Set motor speed PWM_SetDC(1, SPEED_100); //PE9 | PC6//ON 2ms PWM_SetDC(2, SPEED_100); //PE11 | PC 7 PWM_SetDC(3, SPEED_100); //PE13 PWM_SetDC(4, SPEED_100); //PE14 // /* Wait until Tamper Button is released */ while (STM_EVAL_PBGetState(BUTTON_USER)); PWM_SetDC(1, SPEED_0); //PE9 | PC6//ON 2ms PWM_SetDC(2, SPEED_0); //PE11 | PC 7 PWM_SetDC(3, SPEED_0); //PE13 PWM_SetDC(4, SPEED_0); //PE14 /* Initialization of the accelerometer -------------------------------------*/ MPU6050_I2C_Init(); MPU6050_Initialize(); if (MPU6050_TestConnection()) { // connection success STM_EVAL_LEDOff(LED3); }else{ STM_EVAL_LEDOff(LED4); } //Calibration process // Use the following global variables and access functions // to calibrate the acceleration sensor calibrate_sensors(); zeroError(); //Ready to receive message /* Enable DMA USART RX Stream */ DMA_Cmd(USARTx_RX_DMA_STREAM,ENABLE); /* Enable USART DMA RX Requsts */ USART_DMACmd(USARTx, USART_DMAReq_Rx, ENABLE); while(1){ //-------------------------------------------------------- //------ Used to configure the speed controller ---------- //-------------------------------------------------------- // press blue button to force motor at SPEED_100 if (STM_EVAL_PBGetState(BUTTON_USER)){ PWM_SetDC(1, SPEED_100); //PE9 | PC6//ON 2ms PWM_SetDC(2, SPEED_100); //PE11 | PC 7 PWM_SetDC(3, SPEED_100); //PE13 PWM_SetDC(4, SPEED_100); //PE14 // /* Wait until Tamper Button is released */ while (STM_EVAL_PBGetState(BUTTON_USER)); PWM_SetDC(1, SPEED_0); //PE9 | PC6//ON 2ms PWM_SetDC(2, SPEED_0); //PE11 | PC 7 PWM_SetDC(3, SPEED_0); //PE13 PWM_SetDC(4, SPEED_0); //PE14 Delay(100); } //-------------------------------------------------------- //------ Get gyro information ---------- //-------------------------------------------------------- // Read the raw values. MPU6050_GetRawAccelGyro(AccelGyro); // Get the time of reading for rotation computations unsigned long t_now = millis(); STM_EVAL_LEDToggle(LED5); // The temperature sensor is -40 to +85 degrees Celsius. // It is a signed integer. // According to the datasheet: // 340 per degrees Celsius, -512 at 35 degrees. // At 0 degrees: -512 – (340 * 35) = -12412 //dT = ( (double) AccelGyro[TEMP] + 12412.0) / 340.0; // Convert gyro values to degrees/sec gyro_x = (AccelGyro[GYRO_X] - base_x_gyro) / FSSEL; gyro_y = (AccelGyro[GYRO_Y] - base_y_gyro) / FSSEL; gyro_z = (AccelGyro[GYRO_Z] - base_z_gyro) / FSSEL; // Get raw acceleration values accel_x = AccelGyro[ACC_X]; accel_y = AccelGyro[ACC_Y]; accel_z = AccelGyro[ACC_Z]; // Get angle values from accelerometer //float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2)); float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS2DEGREES; float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS2DEGREES; //float accel_angle_z = 0; //// Compute the (filtered) gyro angles //Get the value in second, a tick is every 10ms dt = (t_now - last_read_time)/100.0; float gyro_angle_x = gyro_x*dt + lastAngle[X];//get_last_x_angle(); float gyro_angle_y = gyro_y*dt + lastAngle[Y];//(get_last_y_angle(); float gyro_angle_z = gyro_z*dt + lastAngle[Z];//get_last_z_angle(); // Compute the drifting gyro angles float unfiltered_gyro_angle_x = gyro_x*dt + lastGyroAngle[X];//get_last_gyro_x_angle(); float unfiltered_gyro_angle_y = gyro_y*dt + lastGyroAngle[Y];//get_last_gyro_y_angle(); float unfiltered_gyro_angle_z = gyro_z*dt + lastGyroAngle[Z];//get_last_gyro_z_angle(); // Apply the complementary filter to figure out the change in angle – choice of alpha is // estimated now. Alpha depends on the sampling rate… float alpha = 0.96; angle_x = alpha * gyro_angle_x + (1.0 - alpha) * accel_angle_x; angle_y = alpha * gyro_angle_y + (1.0 - alpha) * accel_angle_y; angle_z = gyro_angle_z; //Accelerometer doesn’t give z-angle //printf("%4.2f %4.2f %4.2f\r\n",angle_x,angle_y,angle_z); //// Update the saved data with the latest values set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z); //Stabilisation // Stable Mode angl = getAngleFromRC(rcBluetooth[ROLL]); levelRoll = (getAngleFromRC(rcBluetooth[ROLL]) - angle_x) * PID[LEVELROLL].P; levelPitch = (getAngleFromRC(rcBluetooth[PITCH]) - angle_y) * PID[LEVELPITCH].P; // Check if pilot commands are not in hover, don't auto trim // if ((abs(receiver.getTrimData(ROLL)) > levelOff) || (abs(receiver.getTrimData(PITCH)) > levelOff)) { // zeroIntegralError(); // } // else { PID[LEVELROLL].integratedError = constrain(PID[LEVELROLL].integratedError + (((getAngleFromRC(rcBluetooth[ROLL]) - angle_x) * dt) * PID[LEVELROLL].I), -LEVEL_LIMIT, LEVEL_LIMIT); PID[LEVELPITCH].integratedError = constrain(PID[LEVELPITCH].integratedError + (((getAngleFromRC(rcBluetooth[PITCH]) + angle_y) * dt) * PID[LEVELROLL].I), -LEVEL_LIMIT, LEVEL_LIMIT); // } //motors.setMotorAxisCommand(ROLL, motor[ROLL] = updatePID(rcBluetooth[ROLL] + levelRoll, gyro_x + 1500, &PID[LEVELGYROROLL],dt) + PID[LEVELROLL].integratedError;//); //motors.setMotorAxisCommand(PITCH, motor[PITCH] = updatePID(rcBluetooth[PITCH] + levelPitch, gyro_y + 1500, &PID[LEVELGYROPITCH],dt) + PID[LEVELPITCH].integratedError;//); getLastSpeedFromMsg(); PWM_SetDC(1, SPEED_0 + SPEED_RANGE*rcSpeed[1] + motor[ROLL] *0.10f); //PE9 | PC6//ON 2ms //Send data on UART *(float*)(aTxBuffer) = angle_x; *(float*)(aTxBuffer+4) = angle_y; *(float*)(aTxBuffer+8) = angle_z; *(float*)(aTxBuffer+12) = motor[ROLL]; *(float*)(aTxBuffer+16) = motor[PITCH]; sendTxDMA((uint32_t)aTxBuffer,20); //Wait a little bit Delay(3); //30 ms } }