设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 电子技术分类讨论区 › 四轴/航模/飞行器DIY › 基于freeRTOS写的用stm32f407写的飞控程序
返回列表 发新帖
查看: 5991|回复: 2
打印 上一主题 下一主题
收起左侧

基于freeRTOS写的用stm32f407写的飞控程序

[复制链接]
跳转到指定楼层
楼主
ID:329868 发表于 2018-9-13 00:11 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
此代码主要用于学习，SAU-115

中断表：

taskStart   1
/* 此定时器用来计算程序运行时间 */
void TIME2_Cout_Init(void)      2
/* 此定时器用来计算imuupdate周期*/
void TIME5_Cout_Init(void)      3
taskbuhuo    4
/* 此定时器用于超声波捕获 */
void TIM4_IT_CH1_Init(u16 arr,u16 psc)  5

TIM3_CHx_IC   7
flytask       8
siyuanshutask 9
void TIM7_Count(u16 arr,u16 psc)  14 算yaw角
MS5611task   11
USART2 10
USART1 15

四元数函数失败保存

#include "siyuanshu.h"
#include "parameter.h"
#include "mymath.h"
#include "math.h"
#include "stdbool.h"

#define Kp 0.6f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.3f // 0.001  integral gain governs rate of convergence of gyroscope biases
/* 加速计均方根积分滤波常量 */
#define NORM_ACC_LPF_HZ 10
/* 误差积分滤波常量 */
#define REF_ERR_LPF_HZ  1
/* 角度转弧度比例 */
#define ANGLE_TO_RADIAN 0.017453293f
/* 2倍角度转弧度比例 */
#define IMU_INTEGRAL_LIM  0.034906585f

/* 加速度：由下向上方向的加速度在加速度计的分量 */
XYZfloat reference_v={0};

/* 磁力计均方根 */ /* 磁力计数据 除以 均方根 */
float mag_norm=0 ,mag_norm_xyz=0 ;

/* 匹配好方位的磁力计数据 */
XYZfloat mag_sim_3d={0};

/* 四元数的W X Y Z */
float ref_q[4] = {1,0,0,0};

/* 加速计均方根 *//* 四元数均方根 */
float norm_acc=0,norm_q=0;

/* 加速计均方根积分滤波 */
float norm_acc_lpf=0;

/* 数据处理过程量结构体 */
ref_t     ref={0};

/* 解锁判断标志
0未解锁 1已经解锁 */
extern u8 unlocked_to_fly;

/* 最终计算出的姿态 单位 角度 */
float IMU_Roll=0,IMU_Pitch=0,IMU_Yaw=0;

void simple_3d_trans(XYZfloat *ref, XYZfloat *in, XYZfloat *out) //小范围内正确。
{
    static s8 pn;
    static float h_tmp_x,h_tmp_y;

    h_tmp_x = my_sqrt(my_pow(ref->z) + my_pow(ref->y));
    h_tmp_y = my_sqrt(my_pow(ref->z) + my_pow(ref->x));

    pn = ref->z < 0? -1 : 1;
   
    out->x = ( h_tmp_x *in->x - pn *ref->x *in->z ) ;
    out->y = ( pn *h_tmp_y *in->y - ref->y *in->z ) ;   
    out->z = ref->x *in->x + ref->y *in->y + ref->z *in->z ;

}

void imuUpdate(float half_T,XYZfloat acc,XYZfloat gyro,XYZfloat Mag,FlyZITAI *ZITAI)
{
    //误差积分滤波比例
    float ref_err_lpf_hz=0;
    static float yaw_correct=0;
    static float yaw_mag=0;
   
    static char temp_i=0;
    static XYZfloat mag_tmp={0};
    float mag_norm_tmp=0;
   
    /* 积分滤波比例 */
    mag_norm_tmp=20*(6.28f*half_T);
    /* 磁力计均方根计算 */
    mag_norm_xyz=my_sqrt(Mag.x*Mag.x+Mag.y*Mag.y+Mag.z*Mag.z);
    /* 数据正常 */
    if(mag_norm_xyz!=0)
    {
        mag_tmp.x+=mag_norm_tmp*(Mag.x/(mag_norm_xyz)-mag_tmp.x);
        mag_tmp.y+=mag_norm_tmp*(Mag.y/(mag_norm_xyz)-mag_tmp.y);
        mag_tmp.z+=mag_norm_tmp*(Mag.z/(mag_norm_xyz)-mag_tmp.z);
    }
   
    /* 引用加速度计分量作为参考，磁力方位匹配计算 */
    simple_3d_trans(&reference_v,&mag_tmp,&mag_sim_3d);
    /* 匹配好方位的磁力计数据 均方根计算 */
    mag_norm = my_sqrt(mag_sim_3d.x * mag_sim_3d.x + mag_sim_3d.y *mag_sim_3d.y);
    /* 数据正常 */
    if( mag_sim_3d.x != 0 && mag_sim_3d.y != 0 && mag_sim_3d.z != 0 && mag_norm != 0)
    {    /* 计算Y航向角度 单位角度 */
        yaw_mag = fast_atan2( ( mag_sim_3d.x/mag_norm ) , ( mag_sim_3d.y/mag_norm) ) *57.324841f;
    }
    /* 加速度：由下向上方向的加速度在加速度计X分量 */
   
    reference_v.x = 2*(ref_q[1]*ref_q[3] - ref_q[0]*ref_q[2]);
    /* 加速度：由下向上方向的加速度在加速度计Y分量 */
    reference_v.y = 2*(ref_q[2]*ref_q[3] + ref_q[0]*ref_q[1]);
    /* 加速度：由下向上方向的加速度在加速度计Z分量 */
    reference_v.z = 1 - 2*(ref_q[1]*ref_q[1] + ref_q[2]*ref_q[2]);
   
    /* 加速计均方根 估值约4096 */
    norm_acc = my_sqrt(acc.x*acc.x + acc.y*acc.y + acc.z*acc.z);   
   
    if(ABS(acc.x)<4400 && ABS(acc.y)<4400 && ABS(acc.z)<4400 )
    {
        /* 加速度计数值 除以加速计均方根 */
        acc.x = acc.x / norm_acc;
        acc.y = acc.y / norm_acc;
        acc.z = acc.z / norm_acc;
        /* 数据正常 */
        if( 3800 < norm_acc && norm_acc < 4400 )
        {
            /* 叉乘得到误差 */
            ref.err_tmp.x = acc.y*reference_v.z - acc.z*reference_v.y;
            ref.err_tmp.y = acc.z*reference_v.x - acc.x*reference_v.z;
            //ref.err_tmp.z = acc.x*reference_v.y - acc.y*reference_v.x;
            /* 误差积分滤波比例 */
            ref_err_lpf_hz = REF_ERR_LPF_HZ *(6.28f *half_T);
            /* 误差积分滤波 */
            ref.err_lpf.x += ref_err_lpf_hz *( ref.err_tmp.x  - ref.err_lpf.x );
            ref.err_lpf.y += ref_err_lpf_hz *( ref.err_tmp.y  - ref.err_lpf.y );
            //ref.err_lpf.z += ref_err_lpf_hz *( ref.err_tmp.z  - ref.err_lpf.z);
            ref.err.x = ref.err_lpf.x;
            ref.err.y = ref.err_lpf.y;
            //ref.err.z = ref.err_lpf.z;
        }
    }
    /* 数据异常 */
    else
    {
        ref.err.x = 0 ;
        ref.err.y = 0 ;
    }
    /* 误差积分 */
    ref.err_Int.x += ref.err.x *Ki *2 *half_T ;
    ref.err_Int.y += ref.err.y *Ki *2 *half_T ;
    ref.err_Int.z += ref.err.z *Ki *2 *half_T ;
    /* 积分限幅 */
    ref.err_Int.x = LIMIT(ref.err_Int.x, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM );
    ref.err_Int.y = LIMIT(ref.err_Int.y, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM );
    ref.err_Int.z = LIMIT(ref.err_Int.z, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM );
//    //if( reference_v.z >= 0.0f )
    yaw_correct = Kp *0.1f *To_180_degrees(yaw_mag - IMU_Yaw);
////    else
////    {
////        yaw_mag+=180.0f;
////        yaw_correct = Kp *0.1f *To_180_degrees(yaw_mag - IMU_Yaw);
////    }
    /* 引用陀螺仪数据  */
    ref.g.x = (gyro.x - reference_v.x *yaw_correct) *ANGLE_TO_RADIAN + (  Kp*(ref.err.x + ref.err_Int.x) ) ;
    ref.g.y = (gyro.y - reference_v.y *yaw_correct) *ANGLE_TO_RADIAN + (  Kp*(ref.err.y + ref.err_Int.y) ) ;
    ref.g.z = (gyro.z - reference_v.z *yaw_correct) *ANGLE_TO_RADIAN;
   
    /* 更新四元数 */
    ref_q[0] = ref_q[0] +(-ref_q[1]*ref.g.x - ref_q[2]*ref.g.y - ref_q[3]*ref.g.z)*half_T;
    ref_q[1] = ref_q[1] + (ref_q[0]*ref.g.x + ref_q[2]*ref.g.z - ref_q[3]*ref.g.y)*half_T;
    ref_q[2] = ref_q[2] + (ref_q[0]*ref.g.y - ref_q[1]*ref.g.z + ref_q[3]*ref.g.x)*half_T;
    ref_q[3] = ref_q[3] + (ref_q[0]*ref.g.z + ref_q[1]*ref.g.y - ref_q[2]*ref.g.x)*half_T;

    /* 计算四元数均方根 */
    norm_q = my_sqrt(ref_q[0]*ref_q[0] + ref_q[1]*ref_q[1] + ref_q[2]*ref_q[2] + ref_q[3]*ref_q[3]);

    /* 四元数 除以四元数均方根，保证范围1以内 */
    ref_q[0] = ref_q[0] / norm_q;
    ref_q[1] = ref_q[1] / norm_q;
    ref_q[2] = ref_q[2] / norm_q;
    ref_q[3] = ref_q[3] / norm_q;

    /* 四元数转换到欧拉角 算法 */
    IMU_Roll = fast_atan2(2*(ref_q[0]*ref_q[1] + ref_q[2]*ref_q[3]),1 - 2*(ref_q[1]*ref_q[1] + ref_q[2]*ref_q[2])) *57.324841f ;
    IMU_Pitch = asin(2*(ref_q[1]*ref_q[3] - ref_q[0]*ref_q[2])) *57.324841f ;
    IMU_Yaw = fast_atan2(2*(-ref_q[1]*ref_q[2] - ref_q[0]*ref_q[3]),2*(ref_q[0]*ref_q[0] + ref_q[1]*ref_q[1]) - 1) *57.324841f ;
   
    ZITAI->pitch=IMU_Pitch;
    ZITAI->roll=IMU_Roll;
    ZITAI->yaw=IMU_Yaw;
}






//        xQueueOverwrite(MpudataHandle1,&Getdata1.acc);
//        xQueueOverwrite(MpudataHandle2,&Getdata1.gyro);
        //printf("gx:%f gy:%f gz:%f\r\n",Getdata.gyro.x,Getdata.gyro.y,Getdata.gyro.z);
        //printf("   ax:%f ay:%f az:%f gx:%f gy:%f gz:%f\r\n",Getdata1.acc.x,Getdata1.acc.y,Getdata1.acc.z,Getdata1.gyro.x,Getdata1.gyro.y,Getdata1.gyro.z);
//        printf("magx:%f y:%f z:%f\r\n",myMag.x,myMag.y,myMag.z);


//    u8 i=0,T=1;
//    TIM1_OC_CH1234_Init(254,83);
//        if(T)
//            i++;
//        else
//            i--;
//        if(i==250)
//            T=0;
//        if(i==0)
//            T=1;
//        TIM1->CCR1=i;
//        TIM1->CCR2=i;
//        TIM1->CCR3=i;
//        TIM1->CCR4=i;


Magy= (((int16_t)ak8975_buffer[1])<<8 | ak8975_buffer[0]);
Magx= (((int16_t)ak8975_buffer[3])<<8 | ak8975_buffer[2]);
Magz=-(((int16_t)ak8975_buffer[5])<<8 | ak8975_buffer[4]);




单片机源程序如下:
  1. #include "sensor_to_fly.h"
  2. #include "go_fly.h"


  5. TaskHandle_t taskStartHandle;
  6. void taskStart(void *parameter);

  8. extern TaskHandle_t taskbuhuoHandle;
  9. extern TaskHandle_t taskMS5611Handle;
  10. extern TaskHandle_t siyuanshuHandle;
  11. extern TaskHandle_t flytaskHandle;

  13. int main(void)
  14. {
  15.         rxState = waitForStartByte1;
  16.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
  17.         delay_init(168);
  18.         uart_init(115200);
  19.         LED_Init();
  20.         I2c_Soft_Init();
  21.         TIME5_Cout_Init();
  22.         TIME2_Cout_Init();
  23.         IWDG_Init(4,500);
  24.         usart2_Init(115200);
  25.         TIM1_OC_CH1234_Init(8190,3);
  26.         TIM1->CCR1=0;
  27.         TIM1->CCR2=0;
  28.         TIM1->CCR3=0;
  29.         TIM1->CCR4=0;
  30.         Read_pid_Flash();
  31.         xTaskCreate(taskStart,"taskStart",256,NULL,1,&taskStartHandle);
  32.         vTaskStartScheduler();
  33. }
  34.                                        
  35. void taskStart(void *parameter)
  36. {
  37.         taskENTER_CRITICAL();
  38.         xTaskCreate(taskbuhuo,"taskbuhuo",400,NULL,4,&taskbuhuoHandle);
  39.         xTaskCreate(flytask,"flytask",500,NULL,8,&flytaskHandle);
  40.         xTaskCreate(siyuanshutask,"siyuanshutask",500,NULL,9,&siyuanshuHandle);
  41.         xTaskCreate(MS5611task,"MS5611task",256,NULL,11,&taskMS5611Handle);
  42.         vTaskDelete(taskStartHandle);
  43.         taskEXIT_CRITICAL();
  44. }

  46.         
复制代码

所有资料51hei提供下载:
TICK重生.rar (787.03 KB, 下载次数: 135)




评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励！

查看全部评分

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏2 分享淘帖 顶顶 踩踩
回复

使用道具 举报

沙发
ID:369039 发表于 2019-2-14 21:33 | 只看该作者
收藏一下，谢谢分享
回复

使用道具 举报

板凳
ID:708075 发表于 2020-6-27 14:41 来自手机 | 只看该作者
没用到气压计是吗
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表