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 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. 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 General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. */ // 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 <AP_Logger/AP_Logger.h> extern const AP_HAL::HAL& hal; #define NOVA_DEBUGGING 0 #if NOVA_DEBUGGING #include <cstdio> # define Debug(fmt, args ...) \ do { \ printf("%s:%d: " fmt "\n", \ __FUNCTION__, __LINE__, \ ## args); \ hal.scheduler->delay(1); \ } while(0) #else # define Debug(fmt, args ...) #endif AP_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: // psrdiff case 18: // waas case 20: // omnistar case 68: // ppp_converg case 69: // ppp state.status = AP_GPS::GPS_OK_FIX_3D_DGPS; break; case 32: // l1 float case 33: // iono float case 34: // narrow float state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT; break; case 48: // l1 int case 50: // narrow int state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; break; case 0: // NONE case 1: // FIXEDPOS case 2: // FIXEDHEIGHT default: 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 insync if (_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; else crc >>= 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; } // Methods bool 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 { // 0 uint8_t preamble[3]; // 3 uint8_t headerlength; // 4 uint16_t messageid; // 6 uint8_t messagetype; //7 uint8_t portaddr; //8 uint16_t messagelength; //10 uint16_t sequence; //12 uint8_t idletime; //13 uint8_t timestatus; //14 uint16_t week; //16 uint32_t tow; //20 uint32_t recvstatus; // 24 uint16_t resv; //26 uint16_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 status uint32_t postype; ///< Position type double 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 number float latsdev; ///< latitude standard deviation (m) float lngsdev; ///< longitude standard deviation (m) float hgtsdev; ///< height standard deviation (m) // 4 bytes uint8_t stnid[4]; ///< base station id float diffage; ///< differential position age (sec) float sol_age; ///< solution age (sec) uint8_t svstracked; ///< number of satellites tracked uint8_t svsused; ///< number of satellites used in solution uint8_t svsl1; ///< number of GPS plus GLONASS L1 satellites used in solution uint8_t svsmultfreq; ///< number of GPS plus GLONASS L2 satellites used in solution uint8_t resv; ///< reserved uint8_t extsolstat; ///< extended solution status - OEMV and greater only uint8_t galbeisigmask; uint8_t gpsglosigmask; }; struct PACKED bestvel { uint32_t solstat; uint32_t veltype; float latency; float age; double horspd; double trkgnd; // + up double 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