应用介绍

此项目是常用姿态解算算法MATLAB仿真与C代码。

下面展示一部分代码：

//    This is part of UAVXArm.

//    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU

//    General Public License as published by the Free Software Foundation, either version 3 of the

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

//    UAVXArm 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 General Public License for more details.

//    You should have received a copy of the GNU General Public License along with this program.

#include "UAVXArm.h"

// Reference frame is positive X forward, Y left, Z down, Roll right, Pitch up, Yaw CW.

// CAUTION: Because of the coordinate frame the LR Acc sense must be negated for roll compensation.

void AdaptiveYawLPFreq(void);

void DoLegacyYawComp(uint8);

void NormaliseAccelerations(void);

void AttitudeTest(void);

void InitAttitude(void);

real32 AccMagnitude;

real32 EstAngle[3][MaxAttitudeScheme];

real32 EstRate[3][MaxAttitudeScheme];

real32 Correction[2];

real32 YawFilterLPFreq;

real32 dT, dTOn2, dTR, dTmS;

uint32 uSp;

uint8 AttitudeMethod = Wolferl; //Integrator, Wolferl MadgwickIMU PremerlaniDCM MadgwickAHRS, MultiWii;

void AdaptiveYawLPFreq(void) { // Filter LP freq is decreased with reduced yaw stick deflection

    YawFilterLPFreq = ( MAX_YAW_FREQ*abs(DesiredYaw) / RC_NEUTRAL );

    YawFilterLPFreq = Limit(YawFilterLPFreq, 0.5, MAX_YAW_FREQ);

} // AdaptiveYawFilterA

real32 HE;

void DoLegacyYawComp(uint8 S) {

#define COMPASS_MIDDLE          10                 // yaw stick neutral dead zone

#define DRIFT_COMP_YAW_RATE     QUARTERPI          // Radians/Sec

    static int16 Temp;

    // Yaw Angle here is meant to be interpreted as the Heading Error

    Rate[Yaw] = Gyro[Yaw];

    Temp = DesiredYaw - Trim[Yaw];

    if ( F.CompassValid )  // CW+

        if ( abs(Temp) > COMPASS_MIDDLE ) {

            DesiredHeading = Heading; // acquire new heading

            Angle[Yaw] = 0.0;

        } else {

            HE = MinimumTurn(DesiredHeading - Heading);

            HE = Limit1(HE, SIXTHPI); // 30 deg limit

            HE = HE*K[CompassKp];

            Rate[Yaw] = -Limit1(HE, DRIFT_COMP_YAW_RATE);

        }

    else {

        DesiredHeading = Heading;

        Angle[Yaw] = 0.0;

    }

    Angle[Yaw] += Rate[Yaw]*dT;

 //   Angle[Yaw] = Limit1(Angle[Yaw], K[YawIntLimit]);

} // DoLegacyYawComp

void NormaliseAccelerations(void) {

    const real32 MIN_ACC_MAGNITUDE = 0.7; // below this the accelerometers are deemed unreliable - falling?

    static real32 ReNorm;

    AccMagnitude = sqrt(Sqr(Acc[BF]) + Sqr(Acc[LR]) + Sqr(Acc[UD]));

    F.AccMagnitudeOK = AccMagnitude > MIN_ACC_MAGNITUDE;

    if ( F.AccMagnitudeOK ) {

        ReNorm = 1.0 / AccMagnitude;

        Acc[BF] *= ReNorm;

        Acc[LR] *= ReNorm;

        Acc[UD] *= ReNorm;

    } else {

        Acc[LR] = Acc[BF]  = 0.0;

        Acc[UD] = 1.0;

    }

} // NormaliseAccelerations

void GetAttitude(void) {

    static uint32 Now;

    static uint8 i;

    if ( GyroType == IRSensors )

        GetIRAttitude();

    else {

        GetGyroRates();

        GetAccelerations();

    }

    Now = uSClock();

    dT = ( Now - uSp)*0.000001;

    dTOn2 = 0.5 * dT;

    dTR = 1.0 / dT;

    uSp = Now;

 ....................想了解更多请下载附件。