void mpuCalibrate(void)
{
int i;
int count = 200;
mpuValues sum =
{ 0, 0, 0, 0, 0, 0 };
for (i = 0; i < count; i++)
{
mpuValues v;
int16_t r[6] =
{ 0 };
MPU6050_GetRawAccelGyro(r);
v.A_X = r[0];
v.A_Y = r[1];
v.A_Z = r[2];
v.G_X = r[3];
v.G_Y = r[4];
v.G_Z = r[5];
sum = add(sum, v);
chThdSleepMilliseconds(1);
}
calib = divMpu(sum, count);
chThdCreateStatic(mpuPositionUpdateWorkingArea, sizeof(mpuPositionUpdateWorkingArea), NORMALPRIO, mpuPositionUpdate, NULL);
}
void callback_MPU6050(void){
static uint32_t last = 0;
uint32_t current = HAL_GetTick10u();
timeDiffMPU = current - last;
last = current;
MPU6050_GetRawAccelGyro(acceltempgyroVals); // GET ACCLEx3 TEMP GYROx3
filterMain(); // FILTER MPU DATA
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_3);
HAL_NVIC_ClearPendingIRQ(EXTI3_IRQn);
}
// The sensor should be motionless on a horizontal surface
// while calibration is happening
void calibrate_sensors() {
int num_readings = 10;
float x_accel = 0;
float y_accel = 0;
float z_accel = 0;
float x_gyro = 0;
float y_gyro = 0;
float z_gyro = 0;
// Discard the first set of values read from the IMU
MPU6050_GetRawAccelGyro(AccelGyro);
// Read and average the raw values from the IMU
for (int i = 0; i < num_readings; i++) {
MPU6050_GetRawAccelGyro(AccelGyro);
x_accel += AccelGyro[ACC_X];
y_accel += AccelGyro[ACC_Y];
z_accel += AccelGyro[ACC_Z];
x_gyro += AccelGyro[GYRO_X];
y_gyro += AccelGyro[GYRO_Y];
z_gyro += AccelGyro[GYRO_Z];
Delay(10);
}
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;
//Serial.println("Finishing Calibration");
}
mpuValues readMotion(void)
{
mpuValues v;
int16_t r[6] =
{ 0 };
MPU6050_GetRawAccelGyro(r);
v.A_X = r[0];
v.A_Y = r[1];
v.A_Z = r[2];
v.G_X = r[3];
v.G_Y = r[4];
v.G_Z = r[5];
return sub(v, calib);
}
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);
}
}
int nmea_decode_test(void){
int tempOut=0;
s16 AccelGyro[7]={0};
s16 TempBuffer[7]={0};
nmeaINFO info; //GPS parsed info
nmeaPARSER parser; //struct used for decoding
uint8_t new_parse=0; //new or not, have history?
nmeaTIME beiJingTime;
nmea_property()->trace_func = &trace;
nmea_property()->error_func = &error;
//GPS initialization
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
while(1)
{
if(GPS_HalfTransferEnd) /* received half the buffer size*/
{
/* parse using nmea format */
nmea_parse(&parser, (const char*)&gps_rbuff[0], HALF_GPS_RBUFF_SIZE, &info);
GPS_HalfTransferEnd = 0; //Clear flag
new_parse = 1; //new info
}
else if(GPS_TransferEnd) /* receiving the other half */
{
nmea_parse(&parser, (const char*)&gps_rbuff[HALF_GPS_RBUFF_SIZE], HALF_GPS_RBUFF_SIZE, &info);
GPS_TransferEnd = 0;
new_parse =1;
}
if(new_parse ) //if have new info
{
//Converts time to GMT
GMTconvert(&info.utc,&beiJingTime,8,1);
/* Output data*/
printf("\r\n Time:%d,%d,%d,%d,%d,%d\r\n", beiJingTime.year+1900, beiJingTime.mon+1,beiJingTime.day,beiJingTime.hour,beiJingTime.min,beiJingTime.sec);
printf("\r\n Latitude:%f,Longtitude:%f\r\n",info.lat,info.lon);
printf("\r\n Numbers of Sat in use:%d, Numbers of Sat in view:%d",info.satinfo.inuse,info.satinfo.inview);
printf("\r\n Numbers of meters above horizon: %f", info.elv);
printf("\r\n Speed: %f km/h ", info.speed);
printf("\r\n Direction: %f degree", info.direction);
new_parse = 0;
}
//--------------------------actual loop------------------------
//------------------------this is imu------------------------
printf("\r\nMPU Readings:");
MPU6050_GetRawAccelGyro(AccelGyro);
printf("\r\nIMU[0]: %10d",AccelGyro[0]);
printf("\t IMU[1]: %10d",AccelGyro[1]);
printf("\t IMU[2]: %10d",AccelGyro[2]);
printf("\t IMU[3]: %10d",AccelGyro[3]);
printf("\t IMU[4]: %10d",AccelGyro[4]);
printf("\t IMU[5]: %10d",AccelGyro[5]);
//--------------------------temp loop--------------------------
//tempOut=USART3_getTemp();
//printf("\r\n temp is: %d\n",tempOut);
//-------------------------------------------------------------
delay_ms(1000);
}
}
/**
* @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);
}
}
