ArduPilot第5章 GPS解析

---

参考文献
Ardupilot串口代码解析（应用篇）
https://zhuanlan.zhihu.com/p/64437691

Ardupilot添加自定义参数Param
https://blog.csdn.net/LiaRonBob/article/details/129546952

APM_ArduCopter源码解析学习（五）——GPS
https://blog.csdn.net/zhangfengmei1987/article/details/110794867

gps协议解析_the android sdk location
https://ispacesoft.com/107429.html

APM-GPS数据解析之一
https://blog.csdn.net/qq_30623591/article/details/124378705

pixhawk当中关于NMEA类型的gps数据处理流程
https://blog.csdn.net/wangxiyanw/article/details/77873523

Ardupilot前后端基于NEMA协议GPS解包过程
https://blog.csdn.net/leoli95/article/details/93531952

APM-GPS数据解析(NMEA协议)
https://blog.csdn.net/qq_30623591/article/details/123872876

前言

本文介绍如何在Ardupilot框架下解析GPS程序。

一、GPS串口初始化

1.AP_Vehicle::setup()

此函数仅在启动时调用一次。用于初始化一些必要的任务。此函数由 HAL 中的 main() 函数调用。

setup()调用init_ardupilot()
setup()分别使用serial_manager.init_console()和serial_manager.init()来初始化串口。

\libraries\AP_Vehicle\AP_Vehicle.cpp

void AP_Vehicle::setup()
{
	...
	serial_manager.init_console();
	...
	init_ardupilot();
	...
	serial_manager.init();
	...
}

2.AP_Vehicle& vehicle = copter

\ArduCopter\ArduCopter.cpp

Copter copter;
AP_Vehicle& vehicle = copter;

3.Copter::init_ardupilot()

执行init_ardupilot()
\ArduCopter\system.cpp

void Copter::init_ardupilot()
{
	gps.set_log_gps_bit(MASK_LOG_GPS);
    gps.init(serial_manager);
}

4.AP_SerialManager::init()

执行AP_SerialManager::init()函数

在libraries/AP_SerialManager/AP_SerialManager.cpp中从swatch函数中可以看出，init函数重点是配置实现相应的串口协议和波特率。

\libraries\AP_SerialManager\AP_SerialManager.cpp

// init - // init - initialise serial ports
void AP_SerialManager::init()
{
	...
 // initialise serial ports
    for (uint8_t i=1; i<SERIALMANAGER_NUM_PORTS; i++) {
    	...
    	                case SerialProtocol_GPS:
                case SerialProtocol_GPS2:
                    state[i].uart->begin(map_baudrate(state[i].baud), 
                                         AP_SERIALMANAGER_GPS_BUFSIZE_RX,
                                         AP_SERIALMANAGER_GPS_BUFSIZE_TX);
                    break;
 ...}

而对应的协议和波特率已经通过如下定义实现。

5.AP_SerialManager::var_info[]

(1)
在\libraries\AP_SerialManager\AP_SerialManager.h
在AP_SerialManager的类中执行调用初始化

class AP_SerialManager {
public:
AP_Param::setup_object_defaults(this, var_info);
...
}

(2)
执行AP_SerialManager::var_info[]

\libraries\AP_SerialManager\AP_SerialManager.cpp

const AP_Param::GroupInfo AP_SerialManager::var_info[] = {
...
#if SERIALMANAGER_NUM_PORTS > 3
    // @Param: 3_PROTOCOL
    // @DisplayName: Serial 3 (GPS) protocol selection
    // @Description: Control what protocol Serial 3 (GPS) should be used for. Note that the Frsky options require external converter hardware. See the wiki for details.
    // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:Crossfire, 30:Generator, 31:Winch, 32:MSP, 33:DJI FPV
    // @User: Standard
    // @RebootRequired: True
    AP_GROUPINFO("3_PROTOCOL",  5, AP_SerialManager, state[3].protocol, SERIAL3_PROTOCOL),

    // @Param: 3_BAUD
    // @DisplayName: Serial 3 (GPS) Baud Rate
    // @Description: The baud rate used for the Serial 3 (GPS). Most stm32-based boards can support rates of up to 1500. If you setup a rate you cannot support and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
    // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,230:230400,256:256000,460:460800,500:500000,921:921600,1500:1500000
    // @User: Standard
    AP_GROUPINFO("3_BAUD", 6, AP_SerialManager, state[3].baud, AP_SERIALMANAGER_GPS_BAUD/1000),
#endif
...
}

二、GPS函数初始化

1.gcs().init()

在void AP_Vehicle::setup()中，调用gcs().init()函数。
\libraries\AP_Vehicle\AP_Vehicle.cpp

void AP_Vehicle::setup()
{
	...
	gcs().init();
	...
}

2.AP_GPS gps

\libraries\AP_Vehicle\AP_Vehicle.h

class AP_Vehicle : public AP_HAL::HAL::Callbacks {
{
	...
	protected:
	...
	 AP_GPS gps;
	...
}

3.AP_Param::GroupInfo AP_GPS::var_info[]

AP_GPS，_type[0]，_type[1]选择参数

// table of user settable parameters
const AP_Param::GroupInfo AP_GPS::var_info[] = {
    // @Param: _TYPE
    // @DisplayName: 1st GPS type
    // @Description: GPS type of 1st GPS
    // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover
    // @RebootRequired: True
    // @User: Advanced
    AP_GROUPINFO("_TYPE",    0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT),

#if GPS_MAX_RECEIVERS > 1
    // @Param: _TYPE2
    // @DisplayName: 2nd GPS type
    // @Description: GPS type of 2nd GPS
   // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover
    // @RebootRequired: True
    // @User: Advanced
    AP_GROUPINFO("_TYPE2",   1, AP_GPS, _type[1], 0),
#endif
	...
}

4 AP_GPS::init

在class AP_Vehicle : public AP_HAL::HAL::Callbacks 中，有gcs().init()函数。
\libraries\AP_GPS\AP_GPS.cpp

/// Startup initialisation.
void AP_GPS::init(const AP_SerialManager& serial_manager)
{
    // Set new primary param based on old auto_switch use second option
    if ((_auto_switch.get() == 3) && !_primary.configured()) {
        _primary.set_and_save(1);
        _auto_switch.set_and_save(0);
    }

    // search for serial ports with gps protocol
    uint8_t uart_idx = 0;
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (needs_uart((GPS_Type)_type[i].get())) {
            _port[i] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, uart_idx);
            uart_idx++;
        }
    }
    _last_instance_swap_ms = 0;

    // Initialise class variables used to do GPS blending
    _omega_lpf = 1.0f / constrain_float(_blend_tc, 5.0f, 30.0f);

    // prep the state instance fields
    for (uint8_t i = 0; i < GPS_MAX_INSTANCES; i++) {
        state[i].instance = i;
    }

    // sanity check update rate
    // sanity check update rate
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (_rate_ms[i] <= 0 || _rate_ms[i] > GPS_MAX_RATE_MS) {
            _rate_ms[i] = GPS_MAX_RATE_MS;
        }
    }
}

三、GPS的update

1.SCHED_TASK_CLAS

在Task Copter::scheduler_tasks[] 中，建立SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200, 9)调度，调用GPS的updata函数

在ArduCopter/Copter.cpp中可知任务调度GPS更新采用的是50Hz

\ArduCopter\Copter.cpp

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
    SCHED_TASK(rc_loop,              100,    130,  3),
    SCHED_TASK(throttle_loop,         50,     75,  6),
    SSCHED_TASK_CLASS(AP_GPS,   &copter.gps,     update,    50, 200,   9),
...
}

2.AP_GPS::update

GPS更新函数中主要是完成update_instance和update_primary。
在这里对每一个GPS实例进行更新，第一个for循环对GPS进行遍历，进入函数update_instance()(AP_GPS.cpp)

\libraries\AP_GPS\AP_GPS.cpp

void AP_GPS::update(void)
{
    WITH_SEMAPHORE(rsem);

    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        update_instance(i);
    }

    // calculate number of instances
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (drivers[i] != nullptr) {
            num_instances = i+1;
        }
    }

#if GPS_MAX_RECEIVERS > 1
#if HAL_LOGGING_ENABLED
    const uint8_t old_primary = primary_instance;
#endif
    update_primary();
#if HAL_LOGGING_ENABLED
    if (primary_instance != old_primary) {
        AP::logger().Write_Event(LogEvent::GPS_PRIMARY_CHANGED);
    }
#endif  // HAL_LOGING_ENABLED
#endif  // GPS_MAX_RECEIVERS > 1

#ifndef HAL_BUILD_AP_PERIPH
    // update notify with gps status. We always base this on the primary_instance
    AP_Notify::flags.gps_status = state[primary_instance].status;
    AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats;
#endif
}

3.AP_GPS::update_instance

update_instance函数主要是更新GPS实例的个数。内部detect_instance实例化选择配置不同的GPS。
在这里对一个GPS实例进行更新，随之而来的4个if条件，分别对在硬件环仿真、GPS没有使能、端口被另外一个驱动程序锁定以及这不是我们定义的GPS类型或者是它因为时间问题需要重新初始化，这几种情况都会退出这个函数，回到原来的for循环，对下一个GPS实例进行更新，不同的是最后一个，我们至今还没有判断出是什么GPS类型，所以我们会进入该函数，detect_instance();

bool result = drivers[instance]->read(); //读取数据
这里可以选择UBLOX或NMEA进行GPS数据解析

\libraries\AP_GPS\AP_GPS.cpp

void AP_GPS::update_instance(uint8_t instance)
{
	...
    if (drivers[instance] == nullptr) {
        // we don't yet know the GPS type of this one, or it has timed
        // out and needs to be re-initialised
        detect_instance(instance);
        return;
    }

    if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
        send_blob_update(instance);
    }

    // we have an active driver for this instance
    bool result = drivers[instance]->read();
    uint32_t tnow = AP_HAL::millis();
    bool data_should_be_logged = false;

	if (!result) {
        if (tnow - timing[instance].last_message_time_ms > GPS_TIMEOUT_MS) {
            ...
            } 
       else {
       		}
               
	...
}

4.AP_GPS::detect_instance

以下是detect_instance函数。
检查GPS的代码，运行一个GPS实例的检测步骤，如果它找到了GPS，那么它将填写drivers[instance]和state[instance].status。
进入到识别gps的类型的方法中会先执行一段发送初始化的代码,这部分代码有意思的是它可以实现每隔1200ms改变波特率来发送初始配置gps的代码。
在之后的while循环，它遍历各个端口所读到的数据，对于其中一个实例来说，它将从缓存读到的数据包放在data（无符号八位整型），然后通过对不同类型GPS的detect()函数，来判断这个数据是属于哪个GPS的，从而确定这是什么类型的GPS，在这里进入NEMA的detect()函数。（AP_GPS_NEMA）。

\libraries\AP_GPS\AP_GPS.cpp

void AP_GPS::detect_instance(uint8_t instance)
{
	
	...
	//进入到识别gps的类型的方法中会先执行一段发送初始化的代码,这部分代码有意思的是它可以实现每隔1200ms改变波特率,来发送初始配置gps的代码
    if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) {
        // try the next baud rate
        // incrementing like this will skip the first element in array of bauds
        // this is okay, and relied upon
        dstate->current_baud++;
        if (dstate->current_baud == ARRAY_SIZE(_baudrates)) {
            dstate->current_baud = 0;
        }
        uint32_t baudrate = _baudrates[dstate->current_baud];
        _port[instance]->begin(baudrate);
        _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
        dstate->last_baud_change_ms = now;

        if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == nullptr) {
            if (_type[instance] == GPS_TYPE_UBLOX && (_driver_options & AP_GPS_Backend::DriverOptions::UBX_Use115200)) {
                static const char blob[] = UBLOX_SET_BINARY_115200;
                send_blob_start(instance, blob, sizeof(blob));
            } else if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE ||
                        _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) &&
                       ((_driver_options.get() & AP_GPS_Backend::DriverOptions::UBX_MBUseUart2) == 0)) {
                // we use 460800 when doing moving baseline as we need
                // more bandwidth. We don't do this if using UART2, as
                // in that case the RTCMv3 data doesn't go over the
                // link to the flight controller
                static const char blob[] = UBLOX_SET_BINARY_460800;
                send_blob_start(instance, blob, sizeof(blob));
#if AP_GPS_NMEA_ENABLED
            } else if (_type[instance] == GPS_TYPE_HEMI) {
                send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING));
#endif // AP_GPS_NMEA_ENABLED        
            } else {
                send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
            }
        }
    }

    if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == nullptr) {
        send_blob_update(instance);
    }

	

...
	//在while循环中判断挨个遍历端口读取到的数据
	while (initblob_state[instance].remaining == 0 && _port[instance]->available() > 0
           && new_gps == nullptr) {
        uint8_t data = _port[instance]->read();
		}
		...
	//在遍历到数据后根据不同gps的detect来识别该gps是哪种类型的gps
	//识别成功后会将gps的类型设置好
	#if AP_GPS_NMEA_ENABLED
        else if ((_type[instance] == GPS_TYPE_NMEA ||
                    _type[instance] == GPS_TYPE_HEMI ||
                    _type[instance] == GPS_TYPE_ALLYSTAR) &&
                   AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
            new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
        }
#endif //AP_GPS_NMEA_ENABLED
	...
}

5.AP_GPS_NMEA::_detect

检测NMEA全球定位系统。如果流匹配一个NMEA字符串，添加一个字节。这里主要是整个报文字符串的合法性，包头。

\libraries\AP_GPS\AP_GPS.cpp

bool
AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data)
{
	switch (state.step) {
	case 0:
		state.ck = 0;
		if ('$' == data) {
			state.step++;
		}
		break;
	case 1:
		if ('*' == data) {
			state.step++;
		} else {
			state.ck ^= data;
		}
		break;
	case 2:
		if (hexdigit(state.ck>>4) == data) {
			state.step++;
		} else {
			state.step = 0;
		}
		break;
	case 3:
		if (hexdigit(state.ck&0xF) == data) {
            state.step = 0;
			return true;
		}
		state.step = 0;
		break;
    }
    return false;
}
#endif

	
}

6.AP_GPS_Backend ※drivers

可以知道drivers[instance]是AP_GPS_Backend 实体对象。

\libraries\AP_GPS\AP_GPS.h

AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS];

7.virtual bool read() = 0

在AP_GPS/GPS_Backend.h中定义的AP_GPS_Backend类中可以看出read函数纯虚函数，所以会在子类中进行实现。所以找到AP_GPS\AP_GPS_NMEA.h中，AP_GPS_Backend的子类class AP_AP_GPS_NMEA。

\libraries\AP_GPS\AP_GPS.h

class AP_GPS_Backend
{
public:
    ...
    virtual bool read() = 0;
    ...
 }
    

8.class AP_GPS_NMEA

在AP_GPS\AP_GPS_NMEA.h中，找到AP_GPS_Backend的子类class AP_GPS_NMEA。

\libraries\AP_GPS\AP_GPS_NMEA.h

class AP_GPS_NMEA : public AP_GPS_Backend{
}
 

9.AP_GPS_NMEA::read

由上调用gps的read即AP_GPS_NMEA.cpp中的read()
在AP_GPS/AP_GPS_NMEA.cpp中，找到read函数的实现，NMEA数据读取

\libraries\AP_GPS\AP_GPS_NMEA.cpp

bool AP_GPS_NMEA::read(void)
{
    int16_t numc;
    bool parsed = false;
	//通过端口读取到端口的字符长度numc循环,每次读一个字符,然后跳转到decode(字符)
    numc = port->available();
    while (numc--) {
        char c = port->read();
        if (_decode(c)) {
            parsed = true;
        }
#if AP_GPS_DEBUG_LOGGING_ENABLED
        log_data((const uint8_t *)&c, 1);
#endif
    }
    return parsed;
}
 

10.AP_GPS_NMEA::_decode

NEMA报文有GPGGA，GPRMC，GNHDT等格式。每句报文以dollar号开始，以星号结束。星号后面的两位数字是校验和。其中校验和以dollar号和星号之间所有字符异或得到，不包含dollar和星号，直接按ascii码方式计算。

取数：
将串口读到的每个字符存入_term[15]数组。单字符项最大15个字符，多出部分抛弃。

取数逻辑：
判断当前字符。若为$，则为句首；若为’,‘，则为句子中某一项，计算校验和，再通过fallthrough直接进入_term_complete解析；若为‘\r’或’\n’，则判断是否句子解析完毕，若解析完毕，直接退出；若为*，则为句尾，将_is_checksum_term置为true，下一次进入_term_complete()函数判断校验和。
注意：每解析完一项(term)都将_term_offset置零，以便下一项能装入_term[15]数组。

在进入到decode中后会先选择case’$’后将各种变量初始化
然后下一个传的是普通字符的话，是进入最下面的两个if，其中第一个if的作用，是将传来的普通字符进行存储，第二个if，其中条件里面的(!_is_checksum_term)，即还没到校验和的时候，会进入if，通过异或检验操，如果下一个字符是逗号，此时将会进入_term_complete()函数，以此类推。
\libraries\AP_GPS\AP_GPS_NMEA.cpp

bool AP_GPS_NMEA::_decode(char c)
{
    bool valid_sentence = false;

    _sentence_length++;
        
    switch (c) {
    case ',': // term terminators
        _parity ^= c;
        FALLTHROUGH;
    //而当遇到’,’  ’;’  ‘\r’ ‘\n’ ‘*’等就会执行valid_sentence = _term_complete()，
    case '\r':
    case '\n':
    case '*':
        if (_sentence_done) {
            return false;
        }
        if (_term_offset < sizeof(_term)) {
            _term[_term_offset] = 0;
            valid_sentence = _term_complete();
        }
        //执行_term_complete后会将_term_number加1, _term_offset置0，
        ++_term_number;
        _term_offset = 0;
        _is_checksum_term = c == '*';
        return valid_sentence;
	//在进入到decode中后会先选择case’$’后将各种变量初始化
    case '$': // sentence begin
        _term_number = _term_offset = 0;
        _parity = 0;
        _sentence_type = _GPS_SENTENCE_OTHER;
        _is_checksum_term = false;
        _gps_data_good = false;
        _sentence_length = 1;
        _sentence_done = false;
        return valid_sentence;
    }
	//如果是普通字符例如G等字符则执行:
    // ordinary characters
    if (_term_offset < sizeof(_term) - 1)
        _term[_term_offset++] = c;
    if (!_is_checksum_term)
        _parity ^= c;

    return valid_sentence;
}

11.AP_GPS_NMEA::_term_complete()

由于_is_checksum_term在$字符之后被置零，因此我们不会进入这个大的if，接着往下,这if(_term_number == 0)一个条件的判断，是来判别整个句子的类型，比如说这里属于GPGGA，strcmp_P函数是C/C++函数里，比较两个字符串，假设这两个字符串为str1，str2， 若str1==str2，则返回零； 若str不等于str2，则返回正数。
因此在if条件中，我们会进入GPGGA，在这里我们将GPGGA所代表的值赋给了_sentence_type，而GPGGA的值以及各个类型的值是在.h文件中枚举来的，容易找到GPGGA的值是64。

而其他报文内容则是依据_sentence_type + _term_number来判断该报文以特殊符号所分割的内容的涵义都是什么例如:GPGGA报文的第7个特殊符号的涵义是当前gps的星数.

第一次的遍历，由于我们的_term_number的值是0，因此没有适合我们的which，退出选择，返回到上层decode()函数，这个时候_term_number+1，还判断了一下是否结尾，当然我们还没有到，下一个循环开始，这里出时间数据了，找到了下一个“，”，然后我们进入_term_complete()函数，此时会跳过前面两个if，然后回到switch这里，这一次我们将会进入case _GPS_SENTENCE_GGA + 1，这个是报表的时间，我们跟踪进入函数_parse_decimal_100(_term)

\libraries\AP_GPS\AP_GPS_NMEA.cpp

bool AP_GPS_NMEA::_term_complete()
{
    //term收到是* 此时_is_checksum_term  表示这一帧数据结束
    // handle the last term in a message
    if (_is_checksum_term) {
    ...
	}
	...
	 //当执行_term_complete()的时候就会依据_term_number == 0来判断该报文是哪种报文
	 // the first term determines the sentence type
    if (_term_number == 0) {
    	...
        else if (strcmp(term_type, "GGA") == 0) {
            _sentence_type = _GPS_SENTENCE_GGA;
        } 
   // 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT, 160 = THS
    if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
        switch (_sentence_type + _term_number) {
        ...
        case _GPS_SENTENCE_GGA + 1: // Time (GGA)
            _new_time = _parse_decimal_100(_term);
            break;
       ...
       case _GPS_SENTENCE_GGA + 2:
            _new_latitude = _parse_degrees();
            break;
       ...

}

12.AP_GPS_NMEA::_parse_decimal_100

_term在当前句子中对当前句柄进行缓冲，是把_term字符串数组的首地址赋给了p，p是一个指针，atol()会扫描参数nptr字符串，跳过前面的空格字符(就是忽略掉字符串左空格的意思)，直到遇上数字或正负符号才开始做转换，而再遇到非数字或字符串结束时(‘\0’)才结束转换，并将结果返回。比如说此时p=12.63，将1263存入了ret，此时进入while循环后，一个一个开始移，直到遇到“.”，跳出while循环。对于p[0]=’1’,p[1]=’2’,p[2]=,p[3]=’6’，p[4]=’3’然后将p[1]转化成数字乘以10与ret相加，ret=1263+210=1283，注意这个地方是把数据的传递需要时间也考虑了进去，然后返回ret，我们报文的时间就解码完毕，然后进入下一个循环。
第二次的遍历，结束了一个适合我们的which时间，退出选择，回到decode()函数，这个时候_term_number+1，还判断了一下是否结尾，当然我们还没有到，下一个循环开始，这里出时间数据了，找到了下一个“，”，然后我们进入_term_complete()函数，此时会跳过前面两个if，然后回到switch这里,这一次我们将会进入case _GPS_SENTENCE_GGA + 1，这个是报表的纬度，我们跟踪进入函数_parse_degrees()

\libraries\AP_GPS\AP_GPS_NMEA.cpp

int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p)
{
    char *endptr = nullptr;
    long ret = 100 * strtol(p, &endptr, 10);
    int sign = ret < 0 ? -1 : 1;

    if (ret >= (long)INT32_MAX) {
        return INT32_MAX;
    }
    if (ret <= (long)INT32_MIN) {
        return INT32_MIN;
    }
    if (endptr == nullptr || *endptr != '.') {
        return ret;
    }

    if (isdigit(endptr[1])) {
        ret += sign * 10 * DIGIT_TO_VAL(endptr[1]);
        if (isdigit(endptr[2])) {
            ret += sign * DIGIT_TO_VAL(endptr[2]);
            if (isdigit(endptr[3])) {
                ret += sign * (DIGIT_TO_VAL(endptr[3]) >= 5);
            }
        }
    }
    return ret;
}

 

13.AP_GPS_NMEA::_parse_degrees()

这个函数的功能是解析一个NMEA的纬度/经度值，然后返回的结果是角度1e7，GPGGA解析这一格式的时候是度分格式，举个例子，此时的纬度报文为4250.5589。
(1)度的转换
进行一定的初始化以后，我们将进入第一个for循环，它的功效是发现小数点和结束标志的，换句话说当字符p不是十进制字符时（isdigit()主要用于检查其参数是否为十进制数字字符），就会退出这个for循环，对于我们的例子来说，p指到了’.’的位置，而q指的是’4’这个位置，此时(p-q)=4>2，并且q为真，这个while的作用是q指向的地址有值并且没有越界。我们进入if循环，此时deg为0，不进入内部，执行deg+=DIGIT_TO_VAL(q++)=4，然后q++，q指向’2’，进行while判断，(p-q)=3>2，继续进入，此时deg=4为真，因此进入if内部，deg=degx10=40,然后deg+= DIGIT_TO_VAL(q++)=40+2=42，即deg=42然后q++，q指向’5’，进行while判断，（p-q）=2，此时不大于2了，退出第一个while循环，现在回首来看这第一个循环其实是进行了度的转换。
(2)分的转换
进入第二个while循环，p-q=2,即p>q，且q指向的地址有值并且没有越界，我们进入循环，由于min=0，因此我们不进入if内部，执行min+= DIGIT_TO_VAL(q++)=5，然后q++，q指向’0’，进行while判断，p-q=1>0,继续进入，此时min=5为真，所以min=minx10=50，然后执行min+= DIGIT_TO_VAL(q++)=50+0=50，即min=50，然后*q++，q指向’,’，这个时候到了p的位置，两个指针相遇了，退出第二个while循环，现在回首来看这第二个循环其实是进行了小数点左边分的转换。
(3)小数部分的分的转换
接下来我们要开始处理小数部分的分了，进入if条件内部，把q按照p的位置往右移动一格，此时q=p+1=’5’，进入while循环，观看条件（q && isdigit(q)），前面的还是看q指向的地址是否有值，isdigit看它是否为一个十进制字符，然后进入循环，frac_min += DIGIT_TO_VAL(q) * frac_scale=0.5，然后q++，往右移一格，此时q=’5’，缩减因子frac_scale = 0.1f=0.01。
再进入while循环，此时frac_min+= DIGIT_TO_VAL(q) * frac_scale=0.5+50.01=0.55,然后q++，往右移一格，此时q=’8’,缩减因子rac_scale *= 0.1f=0.001。
再进入while循环，此时frac_min+= DIGIT_TO_VAL(q) * frac_scale=0.55+80.001=0.558，然后q++，往右移一格，此时q=‘9’，缩减因子rac_scale *= 0.1f=0.0001。
再进入while循环，此时frac_min+= DIGIT_TO_VAL(q) * frac_scale=0.558+90.0001=0.5589，然后q++，往右移一格，此时q=‘,’,缩减因子rac_scale *= 0.1f=0.00001，退出循环。现在回首来看这部分的函数，是专门用来处理小数点后面的分数部分，在这里我们可以发现，当我们传的数据是GPGGA long格式的时候，小数点后面会多三位，在这个程序里仍然是执行正确的，也就是说当执行GPGGA Long的时候，储存进入的值仍是正确的，接着往下看。

ret=(deg* (int32_t)10000000UL)+ (min * (int32_t)10000000UL / 60)+ (int32_t) (frac_min * (1.0e7f / 60.0f))，我们把举的例子来算一算
ret=deg10^7+(min107)/60+(frac_min*10^7)/60
=420000000+8333333+93150
=428426483
也就是说即使我们这里使用的是GPGGA long型的包，这个程序也是正确解码的，通过虚拟浮点数保证了后七位经度，然后再传回_new_latitude进行后续使用。

最后解包完就放在了AP_common供各层代码调用了，比如说home点，origin点。

\libraries\AP_GPS\AP_GPS_NMEA.cpp

uint32_t AP_GPS_NMEA::_parse_degrees()
{
    char *p, *q;
    uint8_t deg = 0, min = 0;
    float frac_min = 0;
    int32_t ret = 0;

    // scan for decimal point or end of field
    for (p = _term; *p && isdigit(*p); p++)
        ;
    q = _term;

    // convert degrees
    while ((p - q) > 2 && *q) {
        if (deg)
            deg *= 10;
        deg += DIGIT_TO_VAL(*q++);
    }

    // convert minutes
    while (p > q && *q) {
        if (min)
            min *= 10;
        min += DIGIT_TO_VAL(*q++);
    }

    // convert fractional minutes
    if (*p == '.') {
        q = p + 1;
        float frac_scale = 0.1f;
        while (*q && isdigit(*q)) {
            frac_min += DIGIT_TO_VAL(*q) * frac_scale;
            q++;
            frac_scale *= 0.1f;
        }
    }
    ret = (deg * (int32_t)10000000UL);
    ret += (min * (int32_t)10000000UL / 60);
    ret += (int32_t) (frac_min * (1.0e7f / 60.0f));
    return ret;
}
 

14.update_primary(void)

\libraries\AP_GPS\AP_GPS.cpp

void AP_GPS::update_primary(void)
{
	...
}

15.AP_GPS::is_healthy

判定GPS健康的状态主要是通过数据丢帧计数和是否接受到有效数据两部分组成。
对drivers[instance]->is_healthy();这个函数表示串口接收正常。

\libraries\AP_GPS\AP_GPS.cpp

bool AP_GPS::is_healthy(uint8_t instance) const
{
    if (instance >= GPS_MAX_INSTANCES) {
        return false;
    }

    if (get_type(_primary.get()) == GPS_TYPE_NONE) {
        return false;
    }

    
    const uint8_t delay_threshold = 2;
    const float delay_avg_max = ((_type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) || (_type[instance] == GPS_TYPE_UAVCAN_RTK_ROVER))?245:215;
    const GPS_timing &t = timing[instance];
    bool delay_ok = (t.delayed_count < delay_threshold) &&
        t.average_delta_ms < delay_avg_max &&
        state[instance].lagged_sample_count < 5;

#if defined(GPS_BLENDED_INSTANCE)
    if (instance == GPS_BLENDED_INSTANCE) {
        return delay_ok && blend_health_check();
    }
#endif
	return delay_ok && drivers[instance] != nullptr &&
           drivers[instance]->is_healthy();
}

16.location(primary_instance)

\libraries\AP_GPS\AP_GPS.h

const Location &location() const {
        return location(primary_instance);
    }

总结

以上就是今天要讲的内容，本文介绍Ardupilot的GPS解析相关内容。