px4飞控位置估计lpe移植到vs

本文主要内容

px4飞控的位置估计有两种方式，一是inav，二是lpe，用到的传感器用加速度计，磁场传感器，gps，超声，激光，气压，视觉，动作捕捉。但是动作捕捉在目前固件的inav中还不支持。

本文将进行移植lpe中使用加速度计，磁场传感器和GPS进行位置估计的部分。

本文的主要内容用：用到的px4固件中的文件，移植过程，移植完成后主函数内容，效果展示。

用到的px4固件中的文件

/Firmware/src/lib/geo下所有文件。

/Firmware/src/lib/mathlib下所有文件。

/Firmware/src/lib/matrix下所有文件。

/Firmware/src/modules/local_position_estimator/sensors/gps.cpp

/Firmware/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

/Firmware/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

移植过程

首先按照px4源码中文件的组织结构在vs工程下添加这些文件，然后修正各头文件之间的包含路径错
误。

把/Firmware/src/lib/geo/geo.c改成geo.cpp。

把/Firmware/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp中类的基类去掉。然后改正因为没有基类而产生的部分成员变量没有定义错误，这些变量大部分是没有用到的，也可以直接删除。

去掉/Firmware/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp/Firmwar

e/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp中除GPS外其他传感器相关的成员变量和成员函数。

把代码中出现的__EXPORT全部去掉。

把部分未定义的宏在相应的.h文件中定义。

一部分包含文件需要c++11支持，需要vs2013以上版本。如果是vs2012以下版本，则一些头文件需要按照px4固件的写法重新写好放到程序源码文件夹下。

这是一个大概的流程，可能不一定要按照顺序来，但是每一条都至关重要。而且编译过程中还有其他更多的小问题，这就需要根据具体情况进行处理了。

移植完成后主函数内容

main.h中内容如下：

#include <iostream>#include<fstream>#include "LocalPositionEstimator/BlockLocalPositionEstimator.h"#include <windows.h> #include "gyro/stdafx.h"#include "gyro/Com.h"#include "gyro/JY901.h"using namespace std;DWORD WINAPI mavThreadRead(LPVOID lpParam);DWORD WINAPI mavThreadWrite(LPVOID lpParam);

main.cpp中内容如下：

/*作者：高伟晋时间：2017年7月1日功能：进行陀螺仪数据和GPS数据的融合，得到位置坐标。*/#include"main.h"//定时器和线程HANDLE hTimer = NULL;HANDLE hThreadRead = NULL;HANDLE hThreadWrite = NULL;LARGE_INTEGER liDueTime;int totalNum = 0;//串口char chrBuffer[2000];unsigned short usLength = 0;unsigned long ulBaund = 9600, ulComNo = 4;signed char cResult = 1;//写文件ofstream fout;//中断循环控制bool isrun = 0;bool isinit = 0;int main(){    hTimer = CreateWaitableTimer(NULL, TRUE, "TestWaitableTimer");    if (hTimer)    {        printf("定时器开启\r\n");    }    liDueTime.QuadPart = -10000000;     //1s    SetWaitableTimer(hTimer, &liDueTime, 0, NULL, NULL, 0);    hThreadRead = CreateThread(NULL, 0,        mavThreadRead, NULL, 0, NULL);    hThreadWrite = CreateThread(NULL, 0,        mavThreadWrite, NULL, 0, NULL);    while(1);    return 0;}DWORD WINAPI mavThreadRead(LPVOID lpParam){    while (cResult != 0)    {        cResult = OpenCOMDevice(ulComNo, ulBaund);    }    isinit =  1;    cout<<"陀螺仪初始化完成..."<<endl;    while (isrun)    {        usLength = CollectUARTData(ulComNo, chrBuffer);        if (usLength > 0)        {            JY901.CopeSerialData(chrBuffer, usLength);        }        Sleep(50);    }    return 0;}DWORD WINAPI mavThreadWrite(LPVOID lpParam){    while(!isinit);    //位置估计器    BlockLocalPositionEstimator lpe;       //构造函数中进行初始化    time_t rawtime;    char filenametemp[15];    cout<<"请输入要保存数据的文件名：";    cin>>filenametemp;    strcat(filenametemp,".txt");    char* filename = filenametemp;    fout.open(filename);           //不能有空格    isrun = 1;    while (isrun)    {        if (WaitForSingleObject(hTimer, INFINITE) != WAIT_OBJECT_0)        {            printf("1秒定时器出错了\r\n");        }        else        {            time(&rawtime);            liDueTime.QuadPart = -50000;     //0.005s                -            SetWaitableTimer(hTimer, &liDueTime, 0, NULL, NULL, 0);            totalNum++;            //cout << totalNum << endl;            if(totalNum<1000)            {                lpe.update();            }            //cout << "at "<<ctime(&rawtime) << endl;            float x,y,z;            x = lpe.getx();            y = lpe.gety();            z = lpe.getz();            //system("cls");            //printf("Pos:%.3f %.3f %.3f\r\n", x, y, z);            if(totalNum<1000)            {                fout<<x<<" "<<y<<" "<<z<<endl;            }            else if(totalNum ==1000)            {                fout<<flush;                fout.close();                printf("文件输出完毕\r\n");            }        }    }    return 0;}

BlockLocalPositionEstimator.h中内容：

#pragma once#include "../geo/geo.h"#include "../matrix/math.hpp"using namespace matrix;static const float DELAY_MAX = 0.5f; // secondsstatic const float HIST_STEP = 0.05f; // 20 hzstatic const float BIAS_MAX = 1e-1f;static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP;static const size_t N_DIST_SUBS = 4;// for fault detection// chi squared distribution, false alarm probability 0.0001// see fault_table.py// note skip 0 index so we can use degree of freedom as indexstatic const float BETA_TABLE[7] = {0,                    8.82050518214f,                    12.094592431f,                    13.9876612368f,                    16.0875642296f,                    17.8797700658f,                    19.6465647819f,                   };class BlockLocalPositionEstimator       //: public control::SuperBlock{// dynamics:////  x(+) = A * x(-) + B * u(+)//  y_i = C_i*x//// kalman filter////  E[xx'] = P//  E[uu'] = W//  E[y_iy_i'] = R_i////  prediction//      x(+|-) = A*x(-|-) + B*u(+)//      P(+|-) = A*P(-|-)*A' + B*W*B'////  correction//      x(+|+) =  x(+|-) + K_i * (y_i - H_i * x(+|-) )////// input://  ax, ay, az (acceleration NED)//// states://  px, py, pz , ( position NED, m)//  vx, vy, vz ( vel NED, m/s),//  bx, by, bz ( accel bias, m/s^2)//  tz (terrain altitude, ASL, m)//// measurements:////  sonar: pz (measured d*cos(phi)*cos(theta))////  baro: pz////  flow: vx, vy (flow is in body x, y frame)////  gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)////  lidar: pz (actual measured d*cos(phi)*cos(theta))////  vision: px, py, pz, vx, vy, vz////  mocap: px, py, pz////  land (detects when landed)): pz (always measures agl = 0)//public:    // constants    enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz, X_tz, n_x};    enum {U_ax = 0, U_ay, U_az, n_u};    enum {Y_baro_z = 0, n_y_baro};    enum {Y_lidar_z = 0, n_y_lidar};    enum {Y_flow_vx = 0, Y_flow_vy, n_y_flow};    enum {Y_sonar_z = 0, n_y_sonar};    enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};    enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};    enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};    enum {Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land};    enum {POLL_FLOW = 0, POLL_SENSORS, POLL_PARAM, n_poll};    enum {        FUSE_GPS = 1 << 0,        FUSE_FLOW = 1 << 1,        FUSE_VIS_POS = 1 << 2,        FUSE_VIS_YAW = 1 << 3,        FUSE_LAND = 1 << 4,        FUSE_PUB_AGL_Z = 1 << 5,        FUSE_FLOW_GYRO_COMP = 1 << 6,        FUSE_BARO = 1 << 7,    };    enum sensor_t {        SENSOR_BARO = 1 << 0,        SENSOR_GPS = 1 << 1,        SENSOR_LIDAR = 1 << 2,        SENSOR_FLOW = 1 << 3,        SENSOR_SONAR = 1 << 4,        SENSOR_VISION = 1 << 5,        SENSOR_MOCAP = 1 << 6,        SENSOR_LAND = 1 << 7,    };    enum estimate_t {        EST_XY = 1 << 0,        EST_Z = 1 << 1,        EST_TZ = 1 << 2,    };    // public methods    BlockLocalPositionEstimator();    void update();    inline float getx(){return _x(0);}    inline float gety(){return _x(1);}    inline float getz(){return _x(2);}    virtual ~BlockLocalPositionEstimator();private:    // prevent copy and assignment    BlockLocalPositionEstimator(const BlockLocalPositionEstimator &);    BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &);    // methods    // ----------------------------    //    Vector<float, n_x> dynamics(        float t,        const Vector<float, n_x> &x,        const Vector<float, n_u> &u);    void initP();    void initSS();    void updateSSStates();    void updateSSParams();    // predict the next state    void predict();    void publishLocalPos();    // baro    int  baroMeasure(Vector<float, n_y_baro> &y);    void baroCorrect();    void baroInit();    // gps    int  gpsMeasure(Vector<double, n_y_gps> &y);    void gpsCorrect();    void gpsInit();    // misc    inline float agl() { return _x(X_tz) - _x(X_z); }    inline double getDt() { return 0.5; }    // map projection    struct map_projection_reference_s _map_ref;    // reference altitudes    float _altOrigin;    bool _altOriginInitialized;    float _baroAltOrigin;    float _gpsAltOrigin;    unsigned char _estimatorInitialized;    // state space    Vector<float, n_x>  _x; // state vector    Vector<float, n_u>  _u; // input vector    Matrix<float, n_x, n_x>  _P; // state covariance matrix    matrix::Dcm<float> _R_att;    Vector3f _eul;    Matrix<float, n_x, n_x>  _A; // dynamics matrix    Matrix<float, n_x, n_u>  _B; // input matrix    Matrix<float, n_u, n_u>  _R; // input covariance    Matrix<float, n_x, n_x>  _Q; // process noise covariance};

BlockLocalPositionEstimator.cpp中内容：

#include <iostream>#include "BlockLocalPositionEstimator.h"#include "../gyro/JY901.h"#pragma warning(disable:4244)using namespace std;// required standard deviation of estimate for estimator to publish datastatic const int        EST_STDDEV_XY_VALID = 2.0; // 2.0 mstatic const int        EST_STDDEV_Z_VALID = 2.0; // 2.0 mstatic const int        EST_STDDEV_TZ_VALID = 2.0; // 2.0 mstatic const float P_MAX = 1.0e6f; // max allowed value in state covariancestatic const float LAND_RATE = 10.0f; // rate of land detector correctionBlockLocalPositionEstimator::BlockLocalPositionEstimator() :    // reference altitudes    _altOrigin(0),    _altOriginInitialized(false),    _baroAltOrigin(0),    _gpsAltOrigin(0),    _estimatorInitialized(0),    // kf matrices    _x(), _u(), _P(), _R_att(), _eul(){    // initialize A, B,  P, x, u    _x.setZero();    _u.setZero();    initSS();    _estimatorInitialized = true;    baroInit();    gpsInit();    cout << "lpe初始化完成..." << endl;}BlockLocalPositionEstimator::~BlockLocalPositionEstimator(){}BlockLocalPositionEstimator BlockLocalPositionEstimator::operator=(const BlockLocalPositionEstimator &){    return BlockLocalPositionEstimator();}Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(    float t,    const Vector<float, BlockLocalPositionEstimator::n_x> &x,    const Vector<float, BlockLocalPositionEstimator::n_u> &u){    return _A * x + _B * u;}void BlockLocalPositionEstimator::update(){    bool gpsUpdated = 1;    bool baroUpdated = 0;    // do prediction    predict();    // sensor corrections/ initializations    if (gpsUpdated)     {        gpsCorrect();    }    if (baroUpdated)     {        baroCorrect();    }    publishLocalPos();    cout << "lpe updata!"<<endl;}void BlockLocalPositionEstimator::initP(){    _P.setZero();    // initialize to twice valid condition    _P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;    _P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;    _P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;    //=======================================================    //_P(X_vx, X_vx) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();    //_P(X_vy, X_vy) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();    //// use vxy thresh for vz init as well    //_P(X_vz, X_vz) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();    //=======================================================    _P(X_vx, X_vx) = 2 * 0.3f * 0.3f;    _P(X_vy, X_vy) = 2 * 0.3f * 0.3f;    // use vxy thresh for vz init as well    _P(X_vz, X_vz) = 2 * 0.3f * 0.3f;    // initialize bias uncertainty to small values to keep them stable    _P(X_bx, X_bx) = 1e-6f;    _P(X_by, X_by) = 1e-6f;    _P(X_bz, X_bz) = 1e-6f;    _P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID;}void BlockLocalPositionEstimator::initSS(){    initP();    // dynamics matrix    _A.setZero();    // derivative of position is velocity    _A(X_x, X_vx) = 1;    _A(X_y, X_vy) = 1;    _A(X_z, X_vz) = 1;    // input matrix    _B.setZero();    _B(X_vx, U_ax) = 1;    _B(X_vy, U_ay) = 1;    _B(X_vz, U_az) = 1;    // update components that depend on current state    updateSSStates();    updateSSParams();}void BlockLocalPositionEstimator::updateSSStates(){    // derivative of velocity is accelerometer acceleration    // (in input matrix) - bias (in body frame)    _A(X_vx, X_bx) = -_R_att(0, 0);    _A(X_vx, X_by) = -_R_att(0, 1);    _A(X_vx, X_bz) = -_R_att(0, 2);    _A(X_vy, X_bx) = -_R_att(1, 0);    _A(X_vy, X_by) = -_R_att(1, 1);    _A(X_vy, X_bz) = -_R_att(1, 2);    _A(X_vz, X_bx) = -_R_att(2, 0);    _A(X_vz, X_by) = -_R_att(2, 1);    _A(X_vz, X_bz) = -_R_att(2, 2);}void BlockLocalPositionEstimator::updateSSParams(){    // input noise covariance matrix    _R.setZero();    //_R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();   LPE_ACC_XY    _R(U_ax, U_ax) = 0.012f * 0.012f;    //_R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();   LPE_ACC_XY    _R(U_ay, U_ay) = 0.012f * 0.012f;    //_R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();     LPE_ACC_Z    _R(U_az, U_az) = 0.02f * 0.02f;    // process noise power matrix    _Q.setZero();    //float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();    float pn_p_sq = 0.1f * 0.1f;    //float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();    float pn_v_sq = 0.1f * 0.1f;    _Q(X_x, X_x) = pn_p_sq;    _Q(X_y, X_y) = pn_p_sq;    _Q(X_z, X_z) = pn_p_sq;    _Q(X_vx, X_vx) = pn_v_sq;    _Q(X_vy, X_vy) = pn_v_sq;    _Q(X_vz, X_vz) = pn_v_sq;    // technically, the noise is in the body frame,    // but the components are all the same, so    // ignoring for now    //float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();    float pn_b_sq = 0.001f * 0.001f;    _Q(X_bx, X_bx) = pn_b_sq;    _Q(X_by, X_by) = pn_b_sq;    _Q(X_bz, X_bz) = pn_b_sq;    // terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity    /*float pn_t_noise_density =        _pn_t_noise_density.get() + (_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));*/    float pn_t_noise_density = 0.001f + (1.0 / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));    _Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;}void BlockLocalPositionEstimator::predict(){    // get acceleration    float acc[3],angle[3];    angle[0] = JY901.stcAngle.Angle[0] / 32768 * 180;    angle[1] = JY901.stcAngle.Angle[1] / 32768 * 180;    angle[2] = JY901.stcAngle.Angle[2] / 32768 * 180;    matrix::Euler<float> eul(angle[0],angle[1],angle[2]);    matrix::Quaternion<float> q(eul);    //q.from_euler(JY901.stcAngle.Angle[0] / 32768 * 180,JY901.stcAngle.Angle[1] / 32768 * 180,JY901.stcAngle.Angle[2] / 32768 * 180);    //from_euler(JY901.stcAngle.Angle[0] / 32768 * 180,JY901.stcAngle.Angle[1] / 32768 * 180,JY901.stcAngle.Angle[2] / 32768 * 180);              //当前四元数     ================================    _eul = matrix::Euler<float>(q);    _R_att = matrix::Dcm<float>(q);    acc[0] = (float)JY901.stcAcc.a[0]/32768*16*9.81;    acc[1] = (float)JY901.stcAcc.a[1]/32768*16*9.81;    acc[2] = (float)JY901.stcAcc.a[2]/32768*16*9.81;    Vector3f a(acc[0], acc[1], acc[2]);                                  //当前加速度     ================================    // note, bias is removed in dynamics function    _u = _R_att * a;    //_u(U_az) += 9.81f; // add g    // update state space based on new states    updateSSStates();    // continuous time kalman filter prediction    // integrate runge kutta 4th order    // TODO move rk4 algorithm to matrixlib    // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods    float h = getDt();    Vector<float, n_x> k1, k2, k3, k4;    k1 = dynamics(0, _x, _u);    k2 = dynamics(h / 2, _x + k1 * h / 2, _u);    k3 = dynamics(h / 2, _x + k2 * h / 2, _u);    k4 = dynamics(h, _x + k3 * h, _u);    Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);    //// don't integrate position if no valid xy data    //if (!(_estimatorInitialized & EST_XY))      //{    //  dx(X_x) = 0;    //  dx(X_vx) = 0;    //  dx(X_y) = 0;    //  dx(X_vy) = 0;    //}    //// don't integrate z if no valid z data    //if (!(_estimatorInitialized & EST_Z))      //{    //  dx(X_z) = 0;    //}    //// don't integrate tz if no valid tz data    //if (!(_estimatorInitialized & EST_TZ))      //{    //  dx(X_tz) = 0;    //}    // saturate bias    float bx = dx(X_bx) + _x(X_bx);    float by = dx(X_by) + _x(X_by);    float bz = dx(X_bz) + _x(X_bz);    if (std::abs(bx) > BIAS_MAX)     {        bx = BIAS_MAX * bx / std::abs(bx);        dx(X_bx) = bx - _x(X_bx);    }    if (std::abs(by) > BIAS_MAX) {        by = BIAS_MAX * by / std::abs(by);        dx(X_by) = by - _x(X_by);    }    if (std::abs(bz) > BIAS_MAX) {        bz = BIAS_MAX * bz / std::abs(bz);        dx(X_bz) = bz - _x(X_bz);    }    // propagate    _x += dx;    Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +                      _B * _R * _B.transpose() + _Q) * getDt();    // covariance propagation logic    for (int i = 0; i < n_x; i++)     {        if (_P(i, i) > P_MAX)         {            // if diagonal element greater than max, stop propagating            dP(i, i) = 0;            for (int j = 0; j < n_x; j++)             {                dP(i, j) = 0;                dP(j, i) = 0;            }        }    }    _P += dP;    //_xLowPass.update(_x);    //_aglLowPass.update(agl());}void BlockLocalPositionEstimator::publishLocalPos(){    //const Vector<float, n_x> &xLP = _xLowPass.getState();    //_pub_lpos.get().x = xLP(X_x);     // north    //_pub_lpos.get().y = xLP(X_y);     // east    //_pub_lpos.get().z = xLP(X_z);     // down    //_pub_lpos.get().vx = xLP(X_vx); // north    //_pub_lpos.get().vy = xLP(X_vy); // east    //_pub_lpos.get().vz = xLP(X_vz); // down}

最终效果展示