新唐的飞控调试软件AHRS软件

只看该作者 · 2017-3-1 16:54

请问新唐的飞控调试软件AHRS软件，请问怎么得到这个软件？

1万 · 2017-3-4 13:21

楼主买了那个飞控套件没？弄不好在套件的光盘里。

只看该作者 · 2017-3-8 21:08

那个软件应该是编译后的了，没法弄出了吧。

只看该作者 · 2017-3-8 21:11

matrix.h

复制

#ifndef _MATRIX_H

#define _MATRIX_H





extern void   MatrixAdd(float *a, float *b, float *c, unsigned char m, unsigned char n);

extern void   MatrixMinus(float *a, float *b, float *c, unsigned char m, unsigned char n);

extern void   MatrixMul(float *a, float *b, float *c, unsigned char m, unsigned char p, unsigned char n);

extern void   MatrixTrans(float *a, float *c, unsigned char m, unsigned char n);

extern float  MatrixDet1(float *a, unsigned char m, unsigned char n);

extern void   MatrixInv1(float *a, float *c, unsigned char m, unsigned char n);

extern unsigned char Gauss_Jordan(float *a, unsigned char n);

extern void   MatrixCal(float *a, float *b, float *c, unsigned char n);





#endif

只看该作者 · 2017-3-8 21:12

kalman.h

复制

#ifndef _KALMAN_H

#define _KALMAN_H





#define   LENGTH      1*1

#define   ORDER       1

#define   N           100

#define   SEED        1567



//================================================//

//==               最优值结构体                 ==//

//================================================//

typedef struct  _tOptimal

{

  float XNowOpt[LENGTH];

  float XPreOpt[LENGTH];

}tOptimal;



extern void   KalMan_PramInit(void);

extern float KalMan_Update(float *Z);





#endif

只看该作者 · 2017-3-8 21:13

德国的AHRS姿态解算源码，很有参考价值

复制

    /*

    * AHRS

    * Copyright 2010 S.O.H. Madgwick

    *

    * This program is free software: you can redistribute it and/or

    * modify it under the terms of the GNU Lesser Public License as

    * published by the Free Software Foundation, either version 3 of the

    * License, or (at your option) any later version.

    *

    * This program is distributed in the hope that it will be useful, but

    * WITHOUT ANY WARRANTY; without even the implied warranty of

    * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

    * Lesser Public License for more details.

    *

    * You should have received a copy of the GNU Lesser Public License

    * along with this program.  If not, see

    * <http://www.gnu.org/licenses/>.

    */

    // AHRS.c

    // S.O.H. Madgwick

    // 25th August 2010

    //

    // Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion

    // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference

    // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw

    // axis only.

    //

    // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.

    //

    // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated

    // orientation.  See my report for an overview of the use of quaternions in this application.

    //

    // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),

    // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are

    // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.

    //



                                                                                     

    #include "stm32f10x.h"                 

    #include "AHRS.h"

    #include "Positioning.h"       

    #include <math.h>                          

    #include <stdio.h>







    /* Private define ------------------------------------------------------------*/

    #define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer

    #define Ki 0.005f                // integral gain governs rate of convergence of gyroscope biases

    #define halfT 0.0025f                // half the sample period        : 0.005s/2=0.0025s



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



    /* Private variables ---------------------------------------------------------*/

    static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation

    static float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error



    /* Public variables ----------------------------------------------------------*/

    EulerAngle_Type EulerAngle;        //unit: radian

    u8 InitEulerAngle_Finished = 0;



    float Magnetoresistor_mGauss_X = 0, Magnetoresistor_mGauss_Y = 0, Magnetoresistor_mGauss_Z = 0;//unit: milli-Gauss                                                                                                                                                                                                       

    float Accelerate_mg_X, Accelerate_mg_Y, Accelerate_mg_Z;//unit: mg                                                               

    float AngularRate_dps_X, AngularRate_dps_Y, AngularRate_dps_Z;//unit: dps: degree per second       



    int16_t Magnetoresistor_X, Magnetoresistor_Y, Magnetoresistor_Z;                                                                                                                                                                                                       

    uint16_t Accelerate_X = 0, Accelerate_Y = 0, Accelerate_Z = 0;                                                                                                                                                                                               

    uint16_t AngularRate_X = 0, AngularRate_Y = 0, AngularRate_Z = 0;



    u8 Quaternion_Calibration_ok = 0;



    /* Private macro -------------------------------------------------------------*/

    /* Private typedef -----------------------------------------------------------*/

    /* Private function prototypes -----------------------------------------------*/



    /*******************************************************************************

    * Function Name  : AHRSupdate

    * Description    : None

    * Input          : None

    * Output         : None

    * Return         : None

    *******************************************************************************/

    void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {

            float norm;

            float hx, hy, hz, bx, bz;

            float vx, vy, vz, wx, wy, wz;

            float ex, ey, ez;



            // auxiliary variables to reduce number of repeated operations

            float q0q0 = q0*q0;

            float q0q1 = q0*q1;

            float q0q2 = q0*q2;

            float q0q3 = q0*q3;

            float q1q1 = q1*q1;

            float q1q2 = q1*q2;

            float q1q3 = q1*q3;

            float q2q2 = q2*q2;   

            float q2q3 = q2*q3;

            float q3q3 = q3*q3;         

           

            // normalise the measurements

            norm = sqrt(ax*ax + ay*ay + az*az);      

            ax = ax / norm;

            ay = ay / norm;

            az = az / norm;

            norm = sqrt(mx*mx + my*my + mz*mz);         

            mx = mx / norm;

            my = my / norm;

            mz = mz / norm;         

           

            // compute reference direction of flux

            hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);

            hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);

            hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         

            bx = sqrt((hx*hx) + (hy*hy));

            bz = hz;        

           

            // estimated direction of gravity and flux (v and w)

            vx = 2*(q1q3 - q0q2);

            vy = 2*(q0q1 + q2q3);

            vz = q0q0 - q1q1 - q2q2 + q3q3;

            wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);

            wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);

            wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  

           

            // error is sum of cross product between reference direction of fields and direction measured by sensors

            ex = (ay*vz - az*vy) + (my*wz - mz*wy);

            ey = (az*vx - ax*vz) + (mz*wx - mx*wz);

            ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

           

            // integral error scaled integral gain

            exInt = exInt + ex*Ki;

            eyInt = eyInt + ey*Ki;

            ezInt = ezInt + ez*Ki;

           

            // adjusted gyroscope measurements

            gx = gx + Kp*ex + exInt;

            gy = gy + Kp*ey + eyInt;

            gz = gz + Kp*ez + ezInt;

           

            // integrate quaternion rate and normalise

            q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;

            q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;

            q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;

            q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  

           

            // normalise quaternion

            norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

            q0 = q0 / norm;

            q1 = q1 / norm;

            q2 = q2 / norm;

            q3 = q3 / norm;

    }



    float clamp(float Value, float Min, float Max)

    {

            if(Value > Max)

            {

                    return Max;

            }else if(Value < Min)

            {

                    return Min;

            }else

            {

                    return Value;

            }

    }

只看该作者 · 2017-3-8 21:13

kalman.zip (4.89 KB)

只看该作者 · 2017-3-8 21:23

https://bbs.21ic.com/icview-1696652-1-1.html

只看该作者 · 2017-3-8 21:30

姿态解算和Matlab实时姿态显示
1：完成matlab的串口，并且实时通过波形显示数据
2：添加RTT查看CPU使用率的扩展功能，MPU6050读取数据的优化
3：四元素表示的坐标变化，四元素与欧拉角的关系和Madgwick的IMUupdate算法
4：飞控数据采集线程和数据处理线程的安排，类似于生产者与消费者的关系。
1：matlab串口初始化还是比较简单的，网上的资料也很多，这里就直接贴初始化代码了。

复制

% --- Executes on button press in pb_OpenSerialPort.

function pb_OpenSerialPort_Callback(hObject, eventdata, handles)

% hObject    handle to pb_OpenSerialPort (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

%

global o_SerialPort;

%______________________________________________

%GUI全局变量





%---------------------串口初始化-----------------------

%%%COM端口初始化

int_Index_COM=get(handles.pop_SerialPort,'Value');

string_COM=get(handles.pop_SerialPort,'String');

string_Select_COM=string_COM{int_Index_COM};

o_SerialPort=serial(string_Select_COM);

%%%Baud初始化

int_Index_Baud=get(handles.pop_BaudRate,'Value');

string_Baud=get(handles.pop_BaudRate,'String');

string_Select_Baud=string_Baud{int_Index_Baud};

double_Baud=str2double(string_Select_Baud);

set(o_SerialPort,'BaudRate',double_Baud);

%%%设置数据长度

int_Index_DataBit=get(handles.pop_DataBit,'Value');

string_DataBit=get(handles.pop_DataBit,'String');

string_Select_DataBit=string_DataBit(int_Index_DataBit);

double_DataBit=str2double(string_Select_DataBit);

set(o_SerialPort,'DataBits',double_DataBit);

%%%设置停止位长度

int_Index_StopBits=get(handles.pop_StopBits,'Value');

string_StopBits=get(handles.pop_StopBits,'String');

string_Select_StopBits=string_StopBits(int_Index_StopBits);

double_StopBits=str2double(string_Select_StopBits);

set(o_SerialPort,'StopBits',double_StopBits);

%%%设置输入缓冲区大小为1M

set(o_SerialPort,'InputBufferSize',1024000);

%%%串口事件回调设置

        

        set(o_SerialPort,'BytesAvailableFcnMode','terminator');

        set(o_SerialPort,'terminator','!');     %!标识结束符结束，方便处理和读取数据



        o_SerialPort.BytesAvailableFcn={@EveBytesAvailableFcn,handles};

% ----------------------打开串口-----------------------

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现实姿态的函数，写着写着我们发现了用方向余弦的方法，而现在我们直接使用四元素进行坐标变化，简单暴力，几行代码搞定。具体代码如下：

复制

q0=SourceData(9);

    q1=SourceData(10);

    q2=SourceData(11);

    q3=SourceData(12);

    

    %创建四元素矩阵

   q = [1-2*(q2^2+q3^2)        2*(q1*q2-q0*q3)        2*(q0*q2+q1*q3);

        2*(q1*q2+q0*q3)        1-2*(q1^2+q3^2)        2*(q2*q3-q0*q1); 

        2*(q1*q3-q0*q2)        2*(q2*q3+q0*q1)        1-2*(q1^2+q2^2)];

     

    %c初始化三角形的三个坐标点

    xd=[3 -1.2735;3 -1.2735]; 

    yd=[0  1.3474;0  -1.3474];

    zd=[0 0;0 0];

    

    %坐标变换

    temp = [xd(1,1) yd(1,1) zd(1,1);

            xd(1,2) yd(1,2) zd(1,2);

            xd(2,2) yd(2,2) zd(2,2)];

    temp = temp*q;

    xd = [temp(1:2,1)';temp(1,1),temp(3,1)];

    yd = [temp(1:2,2)';temp(1,2),temp(3,2)];

    zd = [temp(1:2,3)';temp(1,3),temp(3,3)];

首先成功数据中提取四元素的四个参数，然后创建四元素的旋转矩阵，最后对三角形的三个坐标旋转下就可以了。。真是暴汗。。之前那程序写了一天。。。。。。
matlab界面如下，后期我们还需要添加控制四个电机的pwm数值现实和pid控制器中yaw pitch roll目标值的显示，这样就可以看到PID的控制效果和对齐进行调整了。

左边中间两个方框，左边那个33.76是mpu6050读出来的温度数值，右边的7是代表CPU使用率为7%。