/*=====================================================================================================*/
int main(void)
{
SystemInit();
//USART_Configuration(38400);
Led_Config();
//BLDC_Config();
delay_ms(1000);
I2C_Configuration();
delay_ms(1000);
MPU6050_Initialize();//LSB gyro = 32.8 LSB acc = 2048
delay_ms(1000);
HMC5883L_Initialize();
delay_ms(1000);//delay to avoid hating
IMU_Get_Offset();//read MPU6050 to calib gyro
delay_ms(1000);//wait for MPU to stabilize
IMU_Get_Start();
delay_ms(1000);//delay to avoid hating
TIMBase_Config();
//Rx_Configuration();//Configuration interrupt to calculate dutycycle received from Rx
//PID_Init_Start();
//SysTick_Config(SystemCoreClock / 999);//start to read MPU each 1 ms
//start PWM to test
//BasicThr = 800;
//printf(" Quadcopter Project\r\n");
while (1)
{
}
/*=====================================================================================================*/
/*=====================================================================================================*/
}
//--------------
//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)
{
}
}
int main(){
usart2_init();
delay_init();
MPU6050_I2C_Init();
MPU6050_Initialize();
MPU6050_Exti_Config();
TIM3_Init(72,10000);
printf("start\n");
while(1)
{
}
return 0;
}
int main(void)
{
//int16_t AccelGyro[6]={0};
delay_init(72);
//USART1_Config();
MPU6050_Initialize();
GPS_Config();
printf("\r\n GPS Ready.\r\n");
nmea_decode_test();
//USART3_Config();
/*
//Usart1 setup, sending info using printf
int16_t AccelGyro[6]={0};
USART1_Config();
GPS_Config();
printf("\r\n GPS Ready.\r\n");
nmea_decode_test();
MPU6050_I2C_Init();
MPU6050_Initialize();
if( MPU6050_TestConnection()== TRUE){
// connection success
}else{
// connection failed
}
MPU6050_GetRawAccelGyro(AccelGyro);
*/
while(1){
/*
tempOut=USART3_getTemp();
printf("temp is: %d\n",tempOut);
delay(10000000);
*/
}
}
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 );
}
void Initial_MCU(void)
{
RCC_Configuration(); // Enable Clock
GPIO_Configuration(); // Define Indicator LED
/*usart*/
DMA1_Channel4_Configuration();
/*i2c*/
DMA1_Channel7_Configuration();
MPU6050_I2C_Init();
MPU6050_Initialize();
USART_Config(USART1, 921600) ; //for display on computer
SysTick_cfg();
NVIC_Configuration();
}
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
}
}
int main(void) {
RCC_APB2PeriphClockCmd(
RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC
| RCC_APB2Periph_GPIOD, ENABLE);
motor_init();
uart_init();
printf("Uart started\r\n");
delay_init();
MPU6050_I2C_Init();
MPU6050_Initialize();
printf("MPU started\r\n");
//TIMER CONFIG
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
TIM_TimeBaseInitTypeDef tim;
NVIC_InitTypeDef nvic;
TIM_TimeBaseStructInit(&tim);
tim.TIM_CounterMode = TIM_CounterMode_Up;
tim.TIM_Prescaler = 64000 - 1;
tim.TIM_Period = SAMPLERATE - 1;
TIM_TimeBaseInit(TIM3, &tim);
TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
TIM_Cmd(TIM3, ENABLE);
nvic.NVIC_IRQChannel = TIM3_IRQn;
nvic.NVIC_IRQChannelPreemptionPriority = 0;
nvic.NVIC_IRQChannelSubPriority = 0;
nvic.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&nvic);
GPIO_InitTypeDef gpio;
GPIO_StructInit(&gpio);
gpio.GPIO_Pin = GPIO_Pin_5;
gpio.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOA, &gpio);
float AccX=0;
float AccY=0;
float GyroX=0;
float GyroXprev=0;
float GyroXbias=0;
float GyroXsum=0;
float pAcc=1;//something different than 0
float pFil=0;
float pWheel=0;
float PID1out=0;
float PID2out=0;
float error1=0;
float error1prev=0;
float error2=0;
float error2prev=0;
printf("Calibration stage 1...\r\n");
while(pAcc>0.01||pAcc<-0.01){
MPU6050_GetRawAccelGyro(mpu);
AccX=mpu[1]*2.0f/32678.0f;
AccY=mpu[2]*2.0f/32678.0f;
pAcc=atan(AccX/AccY)*180/3.14;
}
printf("Calibration stage 2...\r\n");
GPIO_SetBits(GPIOA, GPIO_Pin_5);
delay_ms(500);
GPIO_ResetBits(GPIOA, GPIO_Pin_5);
for(int i=1;i<=10;i++){
MPU6050_GetRawAccelGyro(mpu);
GyroXsum+=mpu[3]*250.0f/32678.0f;
delay_ms(10);
}
GPIO_SetBits(GPIOA, GPIO_Pin_5);
GyroXbias=GyroXsum/10;
printf("%.2f",GyroXbias);
MPU6050_GetRawAccelGyro(mpu);
pAcc=atan(AccX/AccY)*180/3.14;
pFil=0;
printf("While started\r\n");
while (1){
while (!flag_freq);
flag_freq=0;
MPU6050_GetRawAccelGyro(mpu);
AccX=mpu[1]*2.0f/32678.0f;
AccY=mpu[2]*2.0f/32678.0f;
GyroX=mpu[3]*250.0f/32678.0f-GyroXbias;
pAcc=(pAcc*9+atan(AccX/AccY)*180/3.14)/10; //in degres
pFil=0.96*(pFil+((GyroX+GyroXprev)*SAMPLERATE/2000))+(0.04*pAcc);//complementray filter
GyroXprev=GyroX;
error1=0-pFil;
PID1out=PID(3,0,0.5,error1,error1prev);
error2=pWheel;
PID2out=PID(0,0,0,error2,error2prev);
error1prev=error1;
error2prev=error2;
//printf("%f\r\n",pAcc);
//printf("%f\r\n",pGyro);
PID2out=PID1out; //fix here
if(PID2out>PIDMAX)PID2out=PIDMAX;
else if(PID2out<-PIDMAX)PID2out=-PIDMAX;
//printf("%.2f %.2f %.2f \r\n",pFil,pAcc,PID2out);
pWheel+=PID2out;
if(PID2out>0)
forward(PID2out);
else
backward(-1*PID2out);
}
}
