Ardupilot飞控添加使用诺瓦泰GPS

 

Ardupilot飞控添加使用诺瓦泰双天线GPS航向角的设置

 

一、添加诺瓦泰GPS heading角数据包解析代码

1、打开libraries\AP_GPS\AP_GPS_NOVA.h，添加如下代码：

   struct PACKED heading

    {

        uint32_t solstat;

        uint32_t postype;

        float length;

        float heading;

        float pitch;

        float resv1;

        float hdg_std_dev;

        float ptch_std;

        uint8_t stnID[4];

        uint8_t svstracked;

        uint8_t svslnsolution;

        uint8_t obsmask;

        uint8_t multi;

        uint8_t resv2;

        uint8_t extsolstat;

        uint8_t resv3;

        uint8_t sigmask;

};

在 msgbuffer中作如下修改：

union PACKED msgbuffer {

        bestvel bestvelu;

        bestpos bestposu;

        psrdop psrdopu;

        heading headingu;

        uint8_t bytes[256];

    };

2、打开libraries\AP_GPS\AP_GPS_NOVA.cpp，找到bool AP_GPS_NOVA::process_message(void)函数，添加如下代码：

if (messageid == 971) // heading

    {

        const heading &headingu = nova_msg.data.headingu;

 

        state.heading = (float) (headingu.heading);

        state.last_gps_fixed_time_ms = AP_HAL::millis();

        return false;

    }

同时，在本函数的开始处找到bestpos数据解析，定位到相应的位置并作如下修改：

 if (bestposu.solstat == 0) // have a solution

        {

            switch (bestposu.postype)

            {

                case 16:

                    state.status = AP_GPS::GPS_OK_FIX_3D;

                    state.last_gps_fixed_time_ms = AP_HAL::millis();

                    break;

                case 17: // psrdiff

                case 18: // waas

                case 20: // omnistar

                case 68: // ppp_converg

                case 69: // ppp

                    state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;

                    state.last_gps_fixed_time_ms = AP_HAL::millis();

                    break;

                case 32: // l1 float

                case 33: // iono float

                case 34: // narrow float

                    state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;

                    state.last_gps_fixed_time_ms = AP_HAL::millis();

                    break;

                case 48: // l1 int

                case 50: // narrow int

                    state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;

                    state.last_gps_fixed_time_ms = AP_HAL::millis();

                    break;

                case 0: // NONE

                case 1: // FIXEDPOS

                case 2: // FIXEDHEIGHT

                default:

                    state.status = AP_GPS::NO_FIX;

                    break;

            }

        }

1. 打开libraries\AP_GPS\AP_GPS.h，找到结构体GPS_State，作如下添加：

struct GPS_State {

        uint8_t instance; // the instance number of this GPS

 

        // all the following fields must all be filled by the backend driver

        GPS_Status status;                  ///< driver fix status

        uint32_t time_week_ms;              ///< GPS time (milliseconds from start of GPS week)

        uint16_t time_week;                 ///< GPS week number

        Location location;                  ///< last fix location

        float ground_speed;                 ///< ground speed in m/sec

        float ground_course;                ///< ground course in degrees

        uint16_t hdop;                      ///< horizontal dilution of precision in cm

        uint16_t vdop;                      ///< vertical dilution of precision in cm

        uint8_t num_sats;                   ///< Number of visible satellites

        Vector3f velocity;                  ///< 3D velocity in m/s, in NED format

        float speed_accuracy;               ///< 3D velocity accuracy estimate in m/s

        float horizontal_accuracy;          ///< horizontal accuracy estimate in m

        float vertical_accuracy;            ///< vertical accuracy estimate in m

        bool have_vertical_velocity:1;      ///< does GPS give vertical velocity? Set to true only once available.

        bool have_speed_accuracy:1;         ///< does GPS give speed accuracy? Set to true only once available.

        bool have_horizontal_accuracy:1;    ///< does GPS give horizontal position accuracy? Set to true only once available.

        bool have_vertical_accuracy:1;      ///< does GPS give vertical position accuracy? Set to true only once available.

        uint32_t last_gps_time_ms;          ///< the system time we got the last GPS timestamp, milliseconds

 

        float heading; //RTKGPS heading

        uint32_t last_gps_fixed_time_ms;

};

并在AP_GPS类的public中添加如下函数：

    // GPS heading in degrees

    float gps_heading(uint8_t instance) const {

        return state[instance].heading;

    }

    int32_t gps_heading() const {

        return gps_heading(primary_instance)*100;

    }

    //get gps fixed time

    uint32_t gps_last_fixed_time(uint8_t instance) const {

        return state[instance].last_gps_fixed_time_ms;

    }

    uint32_t gps_last_fixed_time() const {

        return gps_last_fixed_time(primary_instance);

    }

 

• 修改yaw漂移修正函数

打开libraries\AP_AHRS\AP_AHRS_DCM.cpp文件，找到void AP_AHRS_DCM::drift_correction_yaw(void)函数，找到else if并将其作如下替换：

else if (have_gps()) {

        //we are using GPS heading for yaw

        if ( _gps.gps_last_fixed_time() != _gps_last_update) {

            yaw_deltat = (_gps.gps_last_fixed_time() - _gps_last_update) * 1.0e-3f;

            _gps_last_update = _gps.gps_last_fixed_time();

 

            new_value = true;

 

            float gps_heading_rad = ToRad(_gps.gps_heading()*0.01f);

            float yaw_error_rad = wrap_PI(gps_heading_rad - yaw);

            yaw_error = sinf(yaw_error_rad);

 

            if (!_flags.have_initial_yaw ||

                    yaw_deltat > 20 ||

                    fabsf(yaw_error_rad) >= 1.047f){

                // reset DCM matrix based on current yaw

                _dcm_matrix.from_euler(roll, pitch, gps_heading_rad);

                _omega_yaw_P.zero();

                _flags.have_initial_yaw = true;

                yaw_error = 0;

            }

        }

}

 

• 修改AP_AHRS_NavEKF.cpp文件:

打开libraries\AP_AHRS\AP_AHRS_NavEKF.cpp文件，找到 void AP_AHRS_NavEKF::update_EKF2(void)函数,并找到yaw=eulers.z;将其注释掉。//yaw=eulers.z

 

• 修改AP_NavEKF2_core.cpp文件：

打开libraries\AP_NavEKF2\AP_NavEKF2_core.cpp文件，找到void NavEKF2_core::UpdateFiler(bool predict)函数，定位到SelectMafFusion();在其下面一行添加：调用recordYawReset();函数。

 

• Ubuntu系统编译飞控代码

1. 安装git

sudo apt-get install git

2、下载和安装交叉编译工具

wgeth ttps://launchpadlibrarian.net/218827486/gcc-arm-none-eabi-4_9-2015q3-20150921-linux.tar.bz2

tar -xjvf gcc-arm-none-eabi-4_9-2015q3-20150921-linux.tar.bz2

sudo apt-get install lsb-core

1. 设置环境变量

   export PATH=/home/your user name/gcc-arm-none-eabi-4_9-2015q3/bin:$PATH

4、克隆飞控源码

    git clone https://github.com/ArduPilot/ardupilot.git

    cd ardupilot

git submodule update --init --recursive

5、运行unbuntu系统环境安装工具脚本

Tools/scripts/install-prereqs-ubuntu.sh -y

6、. ~/.profile

7、编译

   cd ArduCopter

   make px4-v2

8、清空编译文件

   make px4-clean

 

• 烧写固件后参数设置

通过MissonPlanner地面站将编译好的固件ArduCopter-v2.px4（在ArduCopter目录下）,烧写到pixhawk飞控板子，然后点击配置（CONFIG/TUNING）,选择全部参数树（Full Parameter Tree），点击最右边刷新参数按钮（Refresh Params）。

                              

 

1、使用诺瓦泰GPS的设置

在参数表中，找到GPS，点击“+”号打开，找到GPS_TYPE，将其值设为15（即：使用诺瓦泰GPS），然后点击右边写入参数按钮（Write Params）。

 

1. 禁用罗盘的设置

同样，在参数表中，找到COMPASS，点击“+”号打开，找到COMPASS_USE,COMPASS_USE2,

COMPASS_USE3,将它们全部设为0（不使用罗盘），然后点击右边写入参数按钮（Write Params）。

 

 

1. 使用EKF2

 

 

并将EKF3关掉：

 

• 诺瓦泰GPS输出配置

    1、初始化GPS，执行该命令后GPS波特率变为9600

freset

  2、设置GPS串口波特率为115200

com com2 115200 n 8 1 n off off       

com com1 115200 n 8 1 n off off

1. 打开双天线测向功能

dualantennaalign enable 5 1

1. 设置飞控需要的数据

位置信息：

log com1 bestposb ontime 0.2

速度信息：

log com1 bestvelb ontime 0.2

精度因子：

log com1 psrdopb ontime 0.2

航向角信息：

log com1 headingb onchanged

1. 设置差分格式，与基站保持对应：

INTERFACEMODE COM2 NOVATELX NONE OFF

PSRDIFFTIMEOUT 15

RTKTIMEOUT 10

1. 保存配置

       saveconfig

 

• 诺瓦泰GPS接线

GPS的TX接到飞控板子的RX；

GPS的GND接飞控的GND；

GPS的RX空置不用。