[技术问答] 新唐的飞控调试软件AHRS软件

[复制链接]
1685|9
 楼主| lonever 发表于 2017-3-1 16:54 | 显示全部楼层 |阅读模式
请问新唐的飞控调试软件AHRS软件,请问怎么得到这个软件?
zhuotuzi 发表于 2017-3-4 13:21 | 显示全部楼层
楼主买了那个飞控套件没?弄不好在套件的光盘里。
玛尼玛尼哄 发表于 2017-3-8 21:08 | 显示全部楼层
那个软件应该是编译后的了,没法弄出了吧。
玛尼玛尼哄 发表于 2017-3-8 21:11 | 显示全部楼层

matrix.h

  1. #ifndef _MATRIX_H
  2. #define _MATRIX_H


  3. extern void   MatrixAdd(float *a, float *b, float *c, unsigned char m, unsigned char n);
  4. extern void   MatrixMinus(float *a, float *b, float *c, unsigned char m, unsigned char n);
  5. extern void   MatrixMul(float *a, float *b, float *c, unsigned char m, unsigned char p, unsigned char n);
  6. extern void   MatrixTrans(float *a, float *c, unsigned char m, unsigned char n);
  7. extern float  MatrixDet1(float *a, unsigned char m, unsigned char n);
  8. extern void   MatrixInv1(float *a, float *c, unsigned char m, unsigned char n);
  9. extern unsigned char Gauss_Jordan(float *a, unsigned char n);
  10. extern void   MatrixCal(float *a, float *b, float *c, unsigned char n);


  11. #endif


玛尼玛尼哄 发表于 2017-3-8 21:12 | 显示全部楼层


kalman.h
  1. #ifndef _KALMAN_H
  2. #define _KALMAN_H


  3. #define   LENGTH      1*1
  4. #define   ORDER       1
  5. #define   N           100
  6. #define   SEED        1567

  7. //================================================//
  8. //==               最优值结构体                 ==//
  9. //================================================//
  10. typedef struct  _tOptimal
  11. {
  12.   float XNowOpt[LENGTH];
  13.   float XPreOpt[LENGTH];
  14. }tOptimal;

  15. extern void   KalMan_PramInit(void);
  16. extern float KalMan_Update(float *Z);


  17. #endif


玛尼玛尼哄 发表于 2017-3-8 21:13 | 显示全部楼层
德国的AHRS姿态解算源码,很有参考价值
  1.     /*
  2.     * AHRS
  3.     * Copyright 2010 S.O.H. Madgwick
  4.     *
  5.     * This program is free software: you can redistribute it and/or
  6.     * modify it under the terms of the GNU Lesser Public License as
  7.     * published by the Free Software Foundation, either version 3 of the
  8.     * License, or (at your option) any later version.
  9.     *
  10.     * This program is distributed in the hope that it will be useful, but
  11.     * WITHOUT ANY WARRANTY; without even the implied warranty of
  12.     * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.     * Lesser Public License for more details.
  14.     *
  15.     * You should have received a copy of the GNU Lesser Public License
  16.     * along with this program.  If not, see
  17.     * <http://www.gnu.org/licenses/>.
  18.     */
  19.     // AHRS.c
  20.     // S.O.H. Madgwick
  21.     // 25th August 2010
  22.     //
  23.     // Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
  24.     // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
  25.     // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
  26.     // axis only.
  27.     //
  28.     // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
  29.     //
  30.     // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
  31.     // orientation.  See my report for an overview of the use of quaternions in this application.
  32.     //
  33.     // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
  34.     // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
  35.     // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
  36.     //

  37.                                                                                     
  38.     #include "stm32f10x.h"                 
  39.     #include "AHRS.h"
  40.     #include "Positioning.h"      
  41.     #include <math.h>                          
  42.     #include <stdio.h>



  43.     /* Private define ------------------------------------------------------------*/
  44.     #define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
  45.     #define Ki 0.005f                // integral gain governs rate of convergence of gyroscope biases
  46.     #define halfT 0.0025f                // half the sample period        : 0.005s/2=0.0025s

  47.     #define ACCEL_1G 1000 //the acceleration of gravity is: 1000 mg

  48.     /* Private variables ---------------------------------------------------------*/
  49.     static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
  50.     static float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error

  51.     /* Public variables ----------------------------------------------------------*/
  52.     EulerAngle_Type EulerAngle;        //unit: radian
  53.     u8 InitEulerAngle_Finished = 0;

  54.     float Magnetoresistor_mGauss_X = 0, Magnetoresistor_mGauss_Y = 0, Magnetoresistor_mGauss_Z = 0;//unit: milli-Gauss                                                                                                                                                                                                      
  55.     float Accelerate_mg_X, Accelerate_mg_Y, Accelerate_mg_Z;//unit: mg                                                               
  56.     float AngularRate_dps_X, AngularRate_dps_Y, AngularRate_dps_Z;//unit: dps: degree per second      

  57.     int16_t Magnetoresistor_X, Magnetoresistor_Y, Magnetoresistor_Z;                                                                                                                                                                                                      
  58.     uint16_t Accelerate_X = 0, Accelerate_Y = 0, Accelerate_Z = 0;                                                                                                                                                                                               
  59.     uint16_t AngularRate_X = 0, AngularRate_Y = 0, AngularRate_Z = 0;

  60.     u8 Quaternion_Calibration_ok = 0;

  61.     /* Private macro -------------------------------------------------------------*/
  62.     /* Private typedef -----------------------------------------------------------*/
  63.     /* Private function prototypes -----------------------------------------------*/

  64.     /*******************************************************************************
  65.     * Function Name  : AHRSupdate
  66.     * Description    : None
  67.     * Input          : None
  68.     * Output         : None
  69.     * Return         : None
  70.     *******************************************************************************/
  71.     void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
  72.             float norm;
  73.             float hx, hy, hz, bx, bz;
  74.             float vx, vy, vz, wx, wy, wz;
  75.             float ex, ey, ez;

  76.             // auxiliary variables to reduce number of repeated operations
  77.             float q0q0 = q0*q0;
  78.             float q0q1 = q0*q1;
  79.             float q0q2 = q0*q2;
  80.             float q0q3 = q0*q3;
  81.             float q1q1 = q1*q1;
  82.             float q1q2 = q1*q2;
  83.             float q1q3 = q1*q3;
  84.             float q2q2 = q2*q2;   
  85.             float q2q3 = q2*q3;
  86.             float q3q3 = q3*q3;         
  87.            
  88.             // normalise the measurements
  89.             norm = sqrt(ax*ax + ay*ay + az*az);      
  90.             ax = ax / norm;
  91.             ay = ay / norm;
  92.             az = az / norm;
  93.             norm = sqrt(mx*mx + my*my + mz*mz);         
  94.             mx = mx / norm;
  95.             my = my / norm;
  96.             mz = mz / norm;         
  97.            
  98.             // compute reference direction of flux
  99.             hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
  100.             hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
  101.             hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
  102.             bx = sqrt((hx*hx) + (hy*hy));
  103.             bz = hz;        
  104.            
  105.             // estimated direction of gravity and flux (v and w)
  106.             vx = 2*(q1q3 - q0q2);
  107.             vy = 2*(q0q1 + q2q3);
  108.             vz = q0q0 - q1q1 - q2q2 + q3q3;
  109.             wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
  110.             wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
  111.             wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
  112.            
  113.             // error is sum of cross product between reference direction of fields and direction measured by sensors
  114.             ex = (ay*vz - az*vy) + (my*wz - mz*wy);
  115.             ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
  116.             ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
  117.            
  118.             // integral error scaled integral gain
  119.             exInt = exInt + ex*Ki;
  120.             eyInt = eyInt + ey*Ki;
  121.             ezInt = ezInt + ez*Ki;
  122.            
  123.             // adjusted gyroscope measurements
  124.             gx = gx + Kp*ex + exInt;
  125.             gy = gy + Kp*ey + eyInt;
  126.             gz = gz + Kp*ez + ezInt;
  127.            
  128.             // integrate quaternion rate and normalise
  129.             q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  130.             q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  131.             q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  132.             q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
  133.            
  134.             // normalise quaternion
  135.             norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  136.             q0 = q0 / norm;
  137.             q1 = q1 / norm;
  138.             q2 = q2 / norm;
  139.             q3 = q3 / norm;
  140.     }

  141.     float clamp(float Value, float Min, float Max)
  142.     {
  143.             if(Value > Max)
  144.             {
  145.                     return Max;
  146.             }else if(Value < Min)
  147.             {
  148.                     return Min;
  149.             }else
  150.             {
  151.                     return Value;
  152.             }
  153.     }


玛尼玛尼哄 发表于 2017-3-8 21:13 | 显示全部楼层
kalman.zip (4.89 KB, 下载次数: 15)



玛尼玛尼哄 发表于 2017-3-8 21:23 | 显示全部楼层
玛尼玛尼哄 发表于 2017-3-8 21:30 | 显示全部楼层
姿态解算和Matlab实时姿态显示
1:完成matlab的串口,并且实时通过波形显示数据
2:添加RTT查看CPU使用率的扩展功能,MPU6050读取数据的优化
3:四元素表示的坐标变化,四元素与欧拉角的关系和Madgwick的IMUupdate算法
4:飞控数据采集线程和数据处理线程的安排,类似于生产者与消费者的关系。
1:matlab串口初始化还是比较简单的,网上的资料也很多,这里就直接贴初始化代码了。
  1. % --- Executes on button press in pb_OpenSerialPort.
  2. function pb_OpenSerialPort_Callback(hObject, eventdata, handles)
  3. % hObject    handle to pb_OpenSerialPort (see GCBO)
  4. % eventdata  reserved - to be defined in a future version of MATLAB
  5. % handles    structure with handles and user data (see GUIDATA)
  6. %
  7. global o_SerialPort;
  8. %______________________________________________
  9. %GUI全局变量


  10. %---------------------串口初始化-----------------------
  11. %%%COM端口初始化
  12. int_Index_COM=get(handles.pop_SerialPort,'Value');
  13. string_COM=get(handles.pop_SerialPort,'String');
  14. string_Select_COM=string_COM{int_Index_COM};
  15. o_SerialPort=serial(string_Select_COM);
  16. %%%Baud初始化
  17. int_Index_Baud=get(handles.pop_BaudRate,'Value');
  18. string_Baud=get(handles.pop_BaudRate,'String');
  19. string_Select_Baud=string_Baud{int_Index_Baud};
  20. double_Baud=str2double(string_Select_Baud);
  21. set(o_SerialPort,'BaudRate',double_Baud);
  22. %%%设置数据长度
  23. int_Index_DataBit=get(handles.pop_DataBit,'Value');
  24. string_DataBit=get(handles.pop_DataBit,'String');
  25. string_Select_DataBit=string_DataBit(int_Index_DataBit);
  26. double_DataBit=str2double(string_Select_DataBit);
  27. set(o_SerialPort,'DataBits',double_DataBit);
  28. %%%设置停止位长度
  29. int_Index_StopBits=get(handles.pop_StopBits,'Value');
  30. string_StopBits=get(handles.pop_StopBits,'String');
  31. string_Select_StopBits=string_StopBits(int_Index_StopBits);
  32. double_StopBits=str2double(string_Select_StopBits);
  33. set(o_SerialPort,'StopBits',double_StopBits);
  34. %%%设置输入缓冲区大小为1M
  35. set(o_SerialPort,'InputBufferSize',1024000);
  36. %%%串口事件回调设置
  37.         
  38.         set(o_SerialPort,'BytesAvailableFcnMode','terminator');
  39.         set(o_SerialPort,'terminator','!');     %!标识结束符结束,方便处理和读取数据

  40.         o_SerialPort.BytesAvailableFcn={@EveBytesAvailableFcn,handles};
  41. % ----------------------打开串口-----------------------
  42. fopen(o_SerialPort);
matlab串口我们采用回调函数,类似于中断方式哈,但是mtalb的串口十分的不好用哈,没有多线程,而我们在中断里面需要进行波形显示,四元素旋转等各种数据操作,是需要花费点时间的,这就导致我们的数据平率不能很高。。当上传的速率达到100hz以后,就会出错了。。50hz也不稳定。。这个实在是有点。。。担心以后的系统辨识和惯性导航的数据处理了。。头疼。。。
      matlab采用符号‘!’为结束符,碰到这个符号matlab就会调用回调函数,中间的数据都是逗号隔开的,数据顺序一次为accex,accey,accez,temp,gyrox,gyroy,gyroz,cpu_major,q0,q1,q2,q3发送,数据通过sprintf进行格式化,然后通过rt_kprintf函数发送,
1   sprintf(buffer,"\n%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7d,%7.2f,%7.2f,%7.2f,%7.2f!", \2             mpu6050_data_tf->acce_x,mpu6050_data_tf->acce_y,mpu6050_data_tf->acce_z,\
3             mpu6050_data_tf->temp,mpu6050_data_tf->gyro_x,mpu6050_data_tf->gyro_y,\
4             mpu6050_data_tf->gyro_z,(s8)cpu_major,q0,q1,q2,q3);
5           rt_kprintf("%s",buffer);


玛尼玛尼哄 发表于 2017-3-8 21:32 | 显示全部楼层
temp是MPU6050读出的温度数,cpu_major是CPU使用率,q0,q1,q2,q3分别对应四元素的四个参数,q0是实数,其他分别对应i,j,k的参数。
  matlab数据处理:收到数据后,其实标准的处理方式是用matlab的regexp函数,用正则表达式将数据读取出来,我们没有用这个,上传数据格式我们自己可以控制,所以处理起来很简单,没必要用到复杂的正则表达式,而且正则表达式处理时间应该比我们自己简单的处理方法的时间要长,所以采用简单的处理方法。处理方法是先将数据中的空格去掉,然后去掉结束符感叹号,最后把数据中的间隔福逗号去掉,去掉后调用str2num函数将字符串转换为数字就行了。
1     StringIn(StringIn==' ')=[];   %先去掉空格
2     StringIn(StringIn=='!')=[];   %去掉感叹号
3     StringIn(StringIn==',')=' ';  %逗号编程空格   
4     SourceData=str2num(StringIn);
调用plot函数就可以绘制波形了。。这个比较简单,不过还是解释下四元素在这里的用处。
话说我们最开始的时候写了一个通过yaw pitch roll现实姿态的函数,写着写着我们发现了用方向余弦的方法,而现在我们直接使用四元素进行坐标变化,简单暴力,几行代码搞定。具体代码如下:
  1. q0=SourceData(9);
  2.     q1=SourceData(10);
  3.     q2=SourceData(11);
  4.     q3=SourceData(12);
  5.    
  6.     %创建四元素矩阵
  7.    q = [1-2*(q2^2+q3^2)        2*(q1*q2-q0*q3)        2*(q0*q2+q1*q3);
  8.         2*(q1*q2+q0*q3)        1-2*(q1^2+q3^2)        2*(q2*q3-q0*q1);
  9.         2*(q1*q3-q0*q2)        2*(q2*q3+q0*q1)        1-2*(q1^2+q2^2)];
  10.      
  11.     %c初始化三角形的三个坐标点
  12.     xd=[3 -1.2735;3 -1.2735];
  13.     yd=[0  1.3474;0  -1.3474];
  14.     zd=[0 0;0 0];
  15.    
  16.     %坐标变换
  17.     temp = [xd(1,1) yd(1,1) zd(1,1);
  18.             xd(1,2) yd(1,2) zd(1,2);
  19.             xd(2,2) yd(2,2) zd(2,2)];
  20.     temp = temp*q;
  21.     xd = [temp(1:2,1)';temp(1,1),temp(3,1)];
  22.     yd = [temp(1:2,2)';temp(1,2),temp(3,2)];
  23.     zd = [temp(1:2,3)';temp(1,3),temp(3,3)];
首先成功数据中提取四元素的四个参数,然后创建四元素的旋转矩阵,最后对三角形的三个坐标旋转下就可以了。。真是暴汗。。之前那程序写了一天。。。。。。
matlab界面如下,后期我们还需要添加控制四个电机的pwm数值现实和pid控制器中yaw pitch roll目标值的显示,这样就可以看到PID的控制效果和对齐进行调整了。
172150552806874.jpg
左边中间两个方框,左边那个33.76是mpu6050读出来的温度数值,右边的7是代表CPU使用率为7%。


您需要登录后才可以回帖 登录 | 注册

本版积分规则

4

主题

7

帖子

0

粉丝
快速回复 在线客服 返回列表 返回顶部