久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 6742|回復: 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, 下載次數: 137)




評分

參與人數 1黑幣 +50 收起 理由
admin + 50 共享資料的黑幣獎勵！

查看全部評分

分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏2 分享淘帖 頂頂 踩踩
回復

使用道具 舉報

沙發
ID:369039 發表于 2019-2-14 21:33 | 只看該作者
收藏一下，謝謝分享
回復

使用道具 舉報

板凳
ID:708075 發表于 2020-6-27 14:41 來自手機 | 只看該作者
沒用到氣壓計是嗎
回復

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規則

手機版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術交流QQ群281945664

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板： 国产成人91视频 | 青草视频在线 | 国产成人精品午夜视频免费 | 久草视频网站 | 欧美久久精品一级黑人c片 91免费在线视频 | 久久综合久久自在自线精品自 | 日韩一区av | 91精品久久久久久久久中文字幕 | 欧美天天视频 | 亚洲精品欧美一区二区三区 | 夜夜操天天干 | 亚洲高清在线 | 精品av久久久久电影 | 日韩视频在线免费观看 | 成人欧美一区二区三区黑人孕妇 | 淫片一级国产 | 亚洲一区二区 | 精品国产乱码久久久久久牛牛 | 国产一级特黄真人毛片 | 夜夜操av | 久久成人国产精品 | 欧美成人a | 九七午夜剧场福利写真 | 亚洲精品久久久一区二区三区 | 欧美日韩一区二区三区在线观看 | 成人精品 | 欧美亚洲激情 | 日本公妇乱淫xxxⅹ 国产在线不卡 | 日韩三 | 久久久久久久一区二区三区 | 亚洲一区二区中文字幕 | 久久精品色欧美aⅴ一区二区 | 国产成人免费在线 | 精品日韩一区二区 | 国产高清视频一区 | 天天干成人网 | 国产三级电影网站 | 天天摸天天干 | 国产精品久久久久免费 | 日韩一区二区三区在线观看 | 成人片免费看 |