//--------------
//main loop
int main(void)
{
int i=0;
for(i=0;i<100000ul;i++);
rcc_config();
nvic_config();
gpio_config();
usart_config();
USART_puts(USART1, "USART BT initialization complete!\r\n"); // just send a message to indicate that it works
MPU6050_I2C_Init();
MPU6050_Initialize();
if( MPU6050_TestConnection() == 1){
// connection success
USART_puts(USART1, "I2C IMU connection initialization complete!\r\n");
}else{
// connection failed
USART_puts(USART1, "I2C initialization failed!\r\n");
}
//sysTick_Config_Mod(SysTick_CLKSource_HCLK_Div8, 10500000ul); // interruption every 1/2sec from systick
sysTick_Config_Mod(SysTick_CLKSource_HCLK_Div8, 840000ul); // interruption every 0.04sec from systick
while(1)
{
}
}
/** Power on and prepare for general usage.
* This will activate the device and take it out of sleep mode (which must be done
* after start-up). This function also sets both the accelerometer and the gyroscope
* to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
* the clock source to use the X Gyro for reference, which is slightly better than
* the default internal clock source.
*/
void MPU6050_Init()
{
DIS_SYSTICK();
MPU6050_I2C_Init();
if( !MPU6050_TestConnection() )
{
printf( "init error\r\n" );
while( 1 ) ;
}
MPU6050_SetClockSource( MPU6050_CLOCK_PLL_XGYRO );
MPU6050_SetFullScaleGyroRange( MPU6050_GYRO_FS_2000 );
MPU6050_SetFullScaleAccelRange( MPU6050_ACCEL_FS_4 );
MPU6050_SetSleepModeStatus( DISABLE );
EN_SYSTICK();
}
unsigned char mpu_test ( void ) {
usart_puts ( 1, "lib MPU6050 - test - enter!\r\n" );
usart_puts ( 1, "lib MPU6050 - test - i2c init!\r\n" );
MPU6050_I2C_Init();
usart_puts ( 1, "lib MPU6050 - test - mpu init!\r\n" );
MPU6050_Initialize();
usart_puts ( 1, "lib MPU6050 - test - test connection!\r\n" );
if ( MPU6050_TestConnection() == FALSE ) {
usart_puts ( 1, "Init MPU6050 - FAIL!\r\n" );
return ( 0 );
}
// connection success
usart_puts ( 1, "Init MPU6050 - success\r\n" );
// run a looping test
//
int16_t AccelGyro[6]={0};
unsigned char i;
char b [ 20 ];
while ( 1 ) {
MPU6050_GetRawAccelGyro ( AccelGyro );
usart_puts ( 1, "GyroAccel: " );
for ( i = 0; i <= 5; i++ ) {
lame_itoa ( AccelGyro [ i ], b );
usart_puts ( 1, b );
usart_puts ( 1, " , " );
}
usart_puts ( 1, "\r\n" );
lame_delay_ms ( 1000 );
} // while
return ( 1 );
}
int main(void)
{
int16_t buff[6];
uint8_t bin_buff[13];
comm_package imu_comm;
imu_comm.header = (uint8_t)'I';
init_led();
init_usart1();
MPU6050_I2C_Init();
MPU6050_Initialize();
init_tim2();
if( MPU6050_TestConnection() == TRUE)
{
puts("connection success\r\n");
}else {
puts("connection failed\r\n");
}
imu_calibration();
while (1) {
//puts("running now\r\n");
MPU6050_GetRawAccelGyro(buff);
imu_comm.acc_x = buff[0]-ACC_X_OFFSET;
imu_comm.acc_y = buff[1]-ACC_Y_OFFSET;
imu_comm.acc_z = buff[2]-ACC_Z_OFFSET;
imu_comm.gyro_x = buff[3]-GYRO_X_OFFSET;
imu_comm.gyro_y = buff[4]-GYRO_Y_OFFSET;
imu_comm.gyro_z = buff[5]-GYRO_Z_OFFSET;
generate_package( &imu_comm, &bin_buff[0]);
for (int i = 0 ; i<13 ; i++)
send_byte( bin_buff[i] );
gpio_toggle(GPIOA, GPIO_Pin_0);
gpio_toggle(GPIOA, GPIO_Pin_1);
delay_ms(10);
}
}
/**
* @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
}
}
