ardupilot & PX4 RTK配置指南

随着无人机对于高精度位置需求越来越强烈，同时也伴随着北斗三代导航系统正式服务全球，国产的实时载波相位差分（RTK）导航产品也正在以更优惠、更好用的形象进入无人航空器和无人车市场
NovAtel公司是精密全球导航卫星系统（GNSS）及其子系统领域中，处于领先地位的产品与技术供应商，可向客户提供丰厚的投资回报和出色的服务。作为一个通过ISO9001认证的公司，NovAtel 公司开发高质量的OEM产品，包括接收机、封装、天线和固件，这些产品都已集成到全世界高精度的定位应用中。这些应用有测绘、地理信息系统（GIS）、精密农业机械导航、港口自动化、采矿、授时和海事等行业领域。NovAtel公司的参考接收机也是某些国家的航空地面网的核心设备，如美国、日本、欧洲、中国和印度等。

目前诺瓦泰的导航报文协议在无人机行业中逐步扩展着占有率，国产厂商和芯星通、上海思南、北云科技等导航产品公司的产品，大多对该协议有很良好的支持。
下面就根据 ardupilot 源码中的数据解析代码来，看看如何配置我们的RTK导航板卡并接入ardupilot飞控

/*This program is free software: you can redistribute it and/or modifyit under the terms of the GNU General Public License as published bythe 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 ofMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See theGNU General Public License for more details.You should have received a copy of the GNU General Public Licensealong with this program.  If not, see .*///  Novatel/Tersus/ComNav GPS driver for ArduPilot.
//  Code by Michael Oborne
//  Derived from http://www.novatel.com/assets/Documents/Manuals/om-20000129.pdf#include "AP_GPS.h"
#include "AP_GPS_NOVA.h"
#include extern const AP_HAL::HAL& hal;#define NOVA_DEBUGGING 0#if NOVA_DEBUGGING
#include # define Debug(fmt, args ...)                  \
do {                                            \printf("%s:%d: " fmt "\n",     \__FUNCTION__, __LINE__, \## args);               \hal.scheduler->delay(1);                    \
} while(0)
#else# define Debug(fmt, args ...)
#endifAP_GPS_NOVA::AP_GPS_NOVA(AP_GPS &_gps, AP_GPS::GPS_State &_state,AP_HAL::UARTDriver *_port) :AP_GPS_Backend(_gps, _state, _port)
{nova_msg.nova_state = nova_msg_parser::PREAMBLE1;const char *init_str = _initialisation_blob[0];const char *init_str1 = _initialisation_blob[1];port->write((const uint8_t*)init_str, strlen(init_str));port->write((const uint8_t*)init_str1, strlen(init_str1));
}const char* const AP_GPS_NOVA::_initialisation_blob[6] {"\r\n\r\nunlogall\r\n", // cleanup enviroment"log bestposb ontime 0.2 0 nohold\r\n", // get bestpos"log bestvelb ontime 0.2 0 nohold\r\n", // get bestvel"log psrdopb onchanged\r\n", // tersus"log psrdopb ontime 0.2\r\n", // comnav"log psrdopb\r\n" // poll message, as dop only changes when a sat is dropped/added to the visible list
};// Process all bytes available from the stream
//
bool
AP_GPS_NOVA::read(void)
{uint32_t now = AP_HAL::millis();if (_init_blob_index < (sizeof(_initialisation_blob) / sizeof(_initialisation_blob[0]))) {const char *init_str = _initialisation_blob[_init_blob_index];if (now > _init_blob_time) {port->write((const uint8_t*)init_str, strlen(init_str));_init_blob_time = now + 200;_init_blob_index++;}}bool ret = false;while (port->available() > 0) {uint8_t temp = port->read();ret |= parse(temp);}return ret;
}bool
AP_GPS_NOVA::parse(uint8_t temp)
{switch (nova_msg.nova_state){default:case nova_msg_parser::PREAMBLE1:if (temp == NOVA_PREAMBLE1)nova_msg.nova_state = nova_msg_parser::PREAMBLE2;nova_msg.read = 0;break;case nova_msg_parser::PREAMBLE2:if (temp == NOVA_PREAMBLE2){nova_msg.nova_state = nova_msg_parser::PREAMBLE3;}else{nova_msg.nova_state = nova_msg_parser::PREAMBLE1;}break;case nova_msg_parser::PREAMBLE3:if (temp == NOVA_PREAMBLE3){nova_msg.nova_state = nova_msg_parser::HEADERLENGTH;}else{nova_msg.nova_state = nova_msg_parser::PREAMBLE1;}break;case nova_msg_parser::HEADERLENGTH:Debug("NOVA HEADERLENGTH\n");nova_msg.header.data[0] = NOVA_PREAMBLE1;nova_msg.header.data[1] = NOVA_PREAMBLE2;nova_msg.header.data[2] = NOVA_PREAMBLE3;nova_msg.header.data[3] = temp;nova_msg.header.nova_headeru.headerlength = temp;nova_msg.nova_state = nova_msg_parser::HEADERDATA;nova_msg.read = 4;break;case nova_msg_parser::HEADERDATA:if (nova_msg.read >= sizeof(nova_msg.header.data)) {Debug("parse header overflow length=%u\n", (unsigned)nova_msg.read);nova_msg.nova_state = nova_msg_parser::PREAMBLE1;break;}nova_msg.header.data[nova_msg.read] = temp;nova_msg.read++;if (nova_msg.read >= nova_msg.header.nova_headeru.headerlength){nova_msg.nova_state = nova_msg_parser::DATA;}break;case nova_msg_parser::DATA:if (nova_msg.read >= sizeof(nova_msg.data)) {Debug("parse data overflow length=%u msglength=%u\n", (unsigned)nova_msg.read,nova_msg.header.nova_headeru.messagelength);nova_msg.nova_state = nova_msg_parser::PREAMBLE1;break;}nova_msg.data.bytes[nova_msg.read - nova_msg.header.nova_headeru.headerlength] = temp;nova_msg.read++;if (nova_msg.read >= (nova_msg.header.nova_headeru.messagelength + nova_msg.header.nova_headeru.headerlength)){Debug("NOVA DATA exit\n");nova_msg.nova_state = nova_msg_parser::CRC1;}break;case nova_msg_parser::CRC1:nova_msg.crc = (uint32_t) (temp << 0);nova_msg.nova_state = nova_msg_parser::CRC2;break;case nova_msg_parser::CRC2:nova_msg.crc += (uint32_t) (temp << 8);nova_msg.nova_state = nova_msg_parser::CRC3;break;case nova_msg_parser::CRC3:nova_msg.crc += (uint32_t) (temp << 16);nova_msg.nova_state = nova_msg_parser::CRC4;break;case nova_msg_parser::CRC4:nova_msg.crc += (uint32_t) (temp << 24);nova_msg.nova_state = nova_msg_parser::PREAMBLE1;uint32_t crc = CalculateBlockCRC32((uint32_t)nova_msg.header.nova_headeru.headerlength, (uint8_t *)&nova_msg.header.data, (uint32_t)0);crc = CalculateBlockCRC32((uint32_t)nova_msg.header.nova_headeru.messagelength, (uint8_t *)&nova_msg.data, crc);if (nova_msg.crc == crc){return process_message();}else{Debug("crc failed");crc_error_counter++;}break;}return false;
}bool
AP_GPS_NOVA::process_message(void)
{uint16_t messageid = nova_msg.header.nova_headeru.messageid;Debug("NOVA process_message messid=%u\n",messageid);check_new_itow(nova_msg.header.nova_headeru.tow, nova_msg.header.nova_headeru.messagelength + nova_msg.header.nova_headeru.headerlength);if (messageid == 42) // bestpos{const bestpos &bestposu = nova_msg.data.bestposu;state.time_week = nova_msg.header.nova_headeru.week;state.time_week_ms = (uint32_t) nova_msg.header.nova_headeru.tow;state.last_gps_time_ms = AP_HAL::millis();state.location.lat = (int32_t) (bestposu.lat * (double)1e7);state.location.lng = (int32_t) (bestposu.lng * (double)1e7);state.location.alt = (int32_t) (bestposu.hgt * 100);state.num_sats = bestposu.svsused;state.horizontal_accuracy = (float) ((bestposu.latsdev + bestposu.lngsdev)/2);state.vertical_accuracy = (float) bestposu.hgtsdev;state.have_horizontal_accuracy = true;state.have_vertical_accuracy = true;state.rtk_age_ms = bestposu.diffage * 1000;state.rtk_num_sats = bestposu.svsused;if (bestposu.solstat == 0) // have a solution{switch (bestposu.postype){case 16:state.status = AP_GPS::GPS_OK_FIX_3D;break;case 17: // psrdiffcase 18: // waascase 20: // omnistarcase 68: // ppp_convergcase 69: // pppstate.status = AP_GPS::GPS_OK_FIX_3D_DGPS;break;case 32: // l1 floatcase 33: // iono floatcase 34: // narrow floatstate.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;break;case 48: // l1 intcase 50: // narrow intstate.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;break;case 0: // NONEcase 1: // FIXEDPOScase 2: // FIXEDHEIGHTdefault:state.status = AP_GPS::NO_FIX;break;}}else{state.status = AP_GPS::NO_FIX;}_new_position = true;}if (messageid == 99) // bestvel{const bestvel &bestvelu = nova_msg.data.bestvelu;state.ground_speed = (float) bestvelu.horspd;state.ground_course = (float) bestvelu.trkgnd;fill_3d_velocity();state.velocity.z = -(float) bestvelu.vertspd;state.have_vertical_velocity = true;_last_vel_time = (uint32_t) nova_msg.header.nova_headeru.tow;_new_speed = true;}if (messageid == 174) // psrdop{const psrdop &psrdopu = nova_msg.data.psrdopu;state.hdop = (uint16_t) (psrdopu.hdop*100);state.vdop = (uint16_t) (psrdopu.htdop*100);return false;}// ensure out position and velocity stay insyncif (_new_position && _new_speed && _last_vel_time == state.time_week_ms) {_new_speed = _new_position = false;return true;}return false;
}void
AP_GPS_NOVA::inject_data(const uint8_t *data, uint16_t len)
{if (port->txspace() > len) {last_injected_data_ms = AP_HAL::millis();port->write(data, len);} else {Debug("NOVA: Not enough TXSPACE");}
}#define CRC32_POLYNOMIAL 0xEDB88320L
uint32_t AP_GPS_NOVA::CRC32Value(uint32_t icrc)
{int i;uint32_t crc = icrc;for ( i = 8 ; i > 0; i-- ){if ( crc & 1 )crc = ( crc >> 1 ) ^ CRC32_POLYNOMIAL;elsecrc >>= 1;}return crc;
}uint32_t AP_GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc)
{while ( length-- != 0 ){crc = ((crc >> 8) & 0x00FFFFFFL) ^ (CRC32Value(((uint32_t) crc ^ *buffer++) & 0xff));}return( crc );
}

#pragma once#include "AP_GPS.h"
#include "GPS_Backend.h"class AP_GPS_NOVA : public AP_GPS_Backend
{
public:AP_GPS_NOVA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }// Methodsbool read() override;void inject_data(const uint8_t *data, uint16_t len) override;const char *name() const override { return "NOVA"; }private:bool parse(uint8_t temp);bool process_message();uint32_t CRC32Value(uint32_t icrc);uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);static const uint8_t NOVA_PREAMBLE1 = 0xaa;static const uint8_t NOVA_PREAMBLE2 = 0x44;static const uint8_t NOVA_PREAMBLE3 = 0x12;// do we have new position information?bool            _new_position:1;// do we have new speed information?bool            _new_speed:1;uint32_t        _last_vel_time;uint8_t _init_blob_index = 0;uint32_t _init_blob_time = 0;static const char* const _initialisation_blob[6];uint32_t crc_error_counter = 0;uint32_t last_injected_data_ms = 0;struct PACKED nova_header{// 0uint8_t preamble[3];// 3uint8_t headerlength;// 4uint16_t messageid;// 6uint8_t messagetype;//7uint8_t portaddr;//8uint16_t messagelength;//10uint16_t sequence;//12uint8_t idletime;//13uint8_t timestatus;//14uint16_t week;//16uint32_t tow;//20uint32_t recvstatus;// 24uint16_t resv;//26uint16_t recvswver;};    struct PACKED psrdop{float gdop;float pdop;float hdop;float htdop;float tdop;float cutoff;uint32_t svcount;// extra data for individual prns};struct PACKED bestpos{uint32_t solstat;      ///< Solution statusuint32_t postype;      ///< Position typedouble lat;            ///< latitude (deg)double lng;            ///< longitude (deg)double hgt;            ///< height above mean sea level (m)float undulation;      ///< relationship between the geoid and the ellipsoid (m)uint32_t datumid;      ///< datum id numberfloat latsdev;         ///< latitude standard deviation (m)float lngsdev;         ///< longitude standard deviation (m)float hgtsdev;         ///< height standard deviation (m)// 4 bytesuint8_t stnid[4];      ///< base station idfloat diffage;         ///< differential position age (sec)float sol_age;         ///< solution age (sec)uint8_t svstracked;    ///< number of satellites trackeduint8_t svsused;       ///< number of satellites used in solutionuint8_t svsl1;         ///< number of GPS plus GLONASS L1 satellites used in solutionuint8_t svsmultfreq;   ///< number of GPS plus GLONASS L2 satellites used in solutionuint8_t resv;          ///< reserveduint8_t extsolstat;    ///< extended solution status - OEMV and greater onlyuint8_t galbeisigmask;uint8_t gpsglosigmask;};struct PACKED bestvel{uint32_t solstat;uint32_t veltype;float latency;float age;double horspd;double trkgnd;// + updouble vertspd;float resv;};union PACKED msgbuffer {bestvel bestvelu;bestpos bestposu;psrdop psrdopu;uint8_t bytes[256];};union PACKED msgheader {nova_header nova_headeru;uint8_t data[28];};struct PACKED nova_msg_parser{enum{PREAMBLE1 = 0,PREAMBLE2,PREAMBLE3,HEADERLENGTH,HEADERDATA,DATA,CRC1,CRC2,CRC3,CRC4,} nova_state;msgbuffer data;uint32_t crc;msgheader header;uint16_t read;} nova_msg;
};

代码解读

const char* const AP_GPS_NOVA::_initialisation_blob[6] {"\r\n\r\nunlogall\r\n", // cleanup enviroment"log bestposb ontime 0.2 0 nohold\r\n", // get bestpos"log bestvelb ontime 0.2 0 nohold\r\n", // get bestvel"log psrdopb onchanged\r\n", // tersus"log psrdopb ontime 0.2\r\n", // comnav"log psrdopb\r\n" // poll message, as dop only changes when a sat is dropped/added to the visible list
};

从初始化配置代码中解析的卫星报文主要包括 BESTPOSB、PSRDOPB、BESTVELB 这三种。在mission planner 地面站中勾选GPS_TYPE为15并保存参数，飞控即可完成板卡的自动配置。
unlogall
log bestposb ontime 0.2 0 nohold
log bestvelb ontime 0.2 0 nohold
log psrdopb onchanged
log psrdopb ontime 0.2

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！