|
|
本帖最后由 6091820503 于 2014-7-22 21:36 编辑
最近在看飞控的代码，尝试自己拿stm32f4实现姿态解算，移植了FreeRTOS操作系统，便于多任务的管理。
gyro accel更新频率1Khz，mag更新频率50Hz
//hmc885l任务
- void hmc5883Task()
- {
- portTickType xLastWakeTime = xTaskGetTickCount();
- uint8_t buf[6] = {0};
- uint8_t ID[3] = {0};
- float yaw = 0.0f;
- int16_t magX, magY, magZ;
- //延时100ms为了让mpu6050先初始化，不然mpu6050不好使
- vTaskDelayUntil( &xLastWakeTime, ( 100 / portTICK_RATE_MS ) );
- printf("hmc5883 initialising...\r\n");
- I2C_init();
- delay(2000);
- //important!!! I2C can visit HMC5883 only when mpu6050 works in bypass mode
- setI2CBypassEnabled(true);
- printf("%s %d\r\n", __FILE__, __LINE__);
- vTaskDelayUntil( &xLastWakeTime, ( 10 / portTICK_RATE_MS ) );
- printf("%s %d\r\n", __FILE__, __LINE__);
- HMC5883L_init();
- printf("%s %d\r\n", __FILE__, __LINE__);
- vTaskDelayUntil( &xLastWakeTime, ( 10 / portTICK_RATE_MS ) );
- printf("%s %d\r\n", __FILE__, __LINE__);
- ID[0] = getIDA();
- ID[1] = getIDB();
- ID[2] = getIDC();
- printf("ID: %c %c %c\r\n", ID[0], ID[1], ID[2]);
- printf("%s %d\r\n", __FILE__, __LINE__);
- while(1)
- {
- //HMC5883 内部寄存器地址会自增
- //HMC5883L_init();
- read_HMC5883L_reg_multi(HMC5883L_DATA_X_MSB_REG, buf, 6);
- magX = ((int16_t)buf[0] << 8) + buf[1];
- magZ = ((int16_t)buf[2] << 8) + buf[3];
- magY = ((int16_t)buf[4] << 8) + buf[5];
-
- mag[0] = magX*hmc5883l_scale[0];
- mag[1] = magY*hmc5883l_scale[1];
- mag[2] = magZ*hmc5883l_scale[2];
-
- //yaw = atan2(-mag[1], mag[0])*RAD2DEG;
- //printf("mag: %f %f %f %f\r\n", mag[0], mag[1], mag[2], yaw);
- //hmc5883L最大读取频率75Hz
- vTaskDelayUntil( &xLastWakeTime, ( 20 / portTICK_RATE_MS ) );
- }
- }
//mpu6050任务
- void mpu6050Task(){
-
- portTickType xLastWakeTime = xTaskGetTickCount();
- uint8_t res;
- uint32_t count = 0;
- float q[4] = {0.0f};
- float rpy[3] = {0.0f};
- bool first_loop = true;
- float theta = 0.0f;
- //delay(2000000);
- //不要使用delay函数,使用vTaskDelayUntil避免浪费时间
- vTaskDelayUntil( &xLastWakeTime, ( 10 / portTICK_RATE_MS ) );
-
- printf("mpu6050 initialising...\r\n");
- I2C_init();
- //important!!! I2C can visit HMC5883 only when mpu6050 works in bypass mode
- //setI2CBypassEnabled(false);
-
- //delay(2000000);
- vTaskDelayUntil( &xLastWakeTime, ( 10 / portTICK_RATE_MS ) );
- setup_mpu6050();
- printf("hello world, ready to blink\r\n");
- res = read_mpu6050_reg(MPU6050_REG_WHOAMI);
- printf("MPU6050: whoami: %hhd", res);
- res = read_mpu6050_reg(MPU6050_REG_PWR_MGMT_1);
- printf("MPU6050: pwr_mgmt_1: %hhd", res);
-
- while (1)
- {
- uint8_t buf[14];
- //printf("read Sensor\r\n");
- read_mpu6050_reg_multi(MPU6050_REG_ACCEL_XOUT_H, buf, 14);
-
- mpu6050_regs_to_float(buf, gyro, accel);
-
- count++;
-
- //gyro前几帧数据有读错的现象，进而导致标定不准，积分就更不准了
- if(count < 100)
- {
- //丢掉前100帧的数据，因为发现前面几帧数据非常不准
- }else if(count < 200)
- {
- //calibration
- gyro_bias[0] += gyro[0];
- gyro_bias[1] += gyro[1];
- gyro_bias[2] += gyro[2];
- printf("gyro_0: %f %f %f\r\n", gyro[0], gyro[1], gyro[2]);
- printf("accel: %f %f %f\r\n", accel[0], accel[1], accel[2]);
- //printf("%d ", count);
- }else if(count == 200)
- {
- gyro_bias[0] /= 100.0f;
- gyro_bias[1] /= 100.0f;
- gyro_bias[2] /= 100.0f;
- printf("gyro_bias: %f %f %f\r\n", gyro_bias[0], gyro_bias[1], gyro_bias[2]);
- }else
- {
- //printf("gyro*: %f %f %f\r\n", gyro[0], gyro[1], gyro[2]);
- gyro[0] -= gyro_bias[0];
- gyro[1] -= gyro_bias[1];
- gyro[2] -= gyro_bias[2];
- //printf("gyro : %f %f %f\r\n", gyro[0], gyro[1], gyro[2]);
- //MadgwickAHRSupdateIMU(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2]);
- //第一次更新姿态，需要对姿态进行初始化
- if(first_loop)
- {
- theta = atan2f(accel[0], -accel[2]);
- rpy[1] = theta * RAD2DEG;
- rpy[0] = atan2f(-accel[1], -accel[2] / cosf(theta)) * RAD2DEG;
- rpy[2] = atan2f(-mag[1], mag[0]) * RAD2DEG;
- RPY2Quaternion(rpy, q);//初始化q
- q0 = q[0];
- q1 = q[1];
- q2 = q[2];
- q3 = q[3];
- first_loop = false;
- }
-
-
- MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], mag[0], mag[1], mag[2]);
- q[0] = q0;
- q[1] = q1;
- q[2] = q2;
- q[3] = q3;
- //printf("q: %f %f %f %f\r\n", q0, q1, q2,q3);
-
- Quaternion2RPY(q, rpy);
- //printf("rpy: %f %f %f\r\n", rpy[0], rpy[1], rpy[1]);
- }
- //由于目前串口是阻塞发送，发送频率太高时，系统很容易阻塞
- if(count%50 == 0)
- {
- //printf("rpy: %f %f %f\r\n", rpy[0], rpy[1], rpy[1]);
- print_rpy_float(rpy);
-
- //print_motion_float(gyro, accel, accel);
- //print_motion((int16_t*)&buf[0], (int16_t*)&buf[8], (int16_t*)&buf[8]);
- //printf("count: %d\r\n", count);
- //printf("q: %f %f %f %f\r\n", q0, q1, q2,q3);
- //printf("rpy: %f %f %f\r\n", rpy[0], rpy[1], rpy[1]);
- }
-
- vTaskDelayUntil( &xLastWakeTime, ( 1 / portTICK_RATE_MS ) );
-
- }
- }
//main函数中只需启动各个任务即可
- int main(void){
-
- SystemInit();
- button_init();
-
- xTaskCreate( LEDTask, "LED", configMINIMAL_STACK_SIZE,
- NULL, mainTIME_TASK_PRIORITY, &hTimeTask );
-
- // xTaskCreate( ButtonTask, "button", configMINIMAL_STACK_SIZE,
- // NULL, mainTIME_TASK_PRIORITY, &hTimeTask );
-
- xTaskCreate( debugConsoleTask, "console", configMINIMAL_STACK_SIZE,
- NULL, mainTIME_TASK_PRIORITY, &hTimeTask );
-
- xTaskCreate( mpu6050Task, "mpu6050", configMINIMAL_STACK_SIZE,
- NULL, mainTIME_TASK_PRIORITY, &hTimeTask );
-
- xTaskCreate( hmc5883Task, "hmc5883", configMINIMAL_STACK_SIZE,
- NULL, mainTIME_TASK_PRIORITY, &hTimeTask );
-
- // xTaskCreate( ms5611Task, "ms5611", configMINIMAL_STACK_SIZE,
- // NULL, mainTIME_TASK_PRIORITY, &hTimeTask );
-
- vTaskStartScheduler();
- // Will only get here if there was insufficient memory to create
- // the idle task.
- while(1);
- }
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看，没有帐号？注册
x
阿莫论坛20周年了！感谢大家的支持与爱护！！
知道什么是神吗？其实神本来也是人，只不过神做了人做不到的事情 所以才成了神。 （头文字D, 杜汶泽）
|
[复制链接]
发表于 2014-7-22 08:48:40
楼主
great!
