Ardupilot前后端基于NEMA协议GPS解包过程
Ardupilot前后端基于NEMA协议GPS解包过程
我是李锡龙，在生日的这天给大家带来ardupilot使用NEMA类型GPS，从应用层前端调用，到后端的GPS解包协议过程。
这里我们先从ArduCopter任务调度器开始：
前端的调用：
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(rc_loop, 100, 130),
SCHED_TASK(throttle_loop, 50, 75),
SCHED_TASK(update_GPS, 50, 200),
#if OPTFLOW == ENABLED
SCHED_TASK(update_optical_flow, 200, 160),
#endif
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK(read_aux_switches, 10, 50),
SCHED_TASK(arm_motors_check, 10, 50),
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
SCHED_TASK(read_rangefinder, 20, 100),
SCHED_TASK(update_proximity, 100, 50),
SCHED_TASK(update_beacon, 400, 50),
SCHED_TASK(update_visual_odom, 400, 50),
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_throttle_hover,100, 90),
SCHED_TASK(smart_rtl_save_position, 3, 100),
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK(compass_accumulate, 100, 100),
SCHED_TASK(barometer_accumulate, 50, 90),
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 400, 50),
#endif
#if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75),
#endif
SCHED_TASK(fourhundred_hz_logging,400, 50),
SCHED_TASK(update_notify, 50, 90),
SCHED_TASK(update_my_led, 2, 90),
SCHED_TASK(update_my_telem2, 10, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(gpsglitch_check, 10, 50),
SCHED_TASK(landinggear_update, 10, 75),
SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK(gcs_check_input, 400, 180),
SCHED_TASK(gcs_send_heartbeat, 1, 110),
SCHED_TASK(gcs_send_deferred, 50, 550),
SCHED_TASK(gcs_data_stream_send, 50, 550),
SCHED_TASK(update_mount, 50, 75),
SCHED_TASK(update_trigger, 50, 75),
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(twentyfive_hz_logging, 25, 110),
SCHED_TASK(dataflash_periodic, 400, 300),
SCHED_TASK(perf_update, 0.1, 75),
SCHED_TASK(read_receiver_rssi, 10, 75),
SCHED_TASK(rpm_update, 10, 200),
SCHED_TASK(compass_cal_update, 100, 100),
SCHED_TASK(accel_cal_update, 10, 100),
#if ADSB_ENABLED == ENABLED
SCHED_TASK(avoidance_adsb_update, 10, 100),
#endif
#if ADVANCED_FAILSAFE == ENABLED
SCHED_TASK(afs_fs_check, 10, 100),
#endif
SCHED_TASK(terrain_update, 10, 100),
#if GRIPPER_ENABLED == ENABLED
SCHED_TASK(gripper_update, 10, 75),
#endif
#ifdef USERHOOK_FASTLOOP
SCHED_TASK(userhook_FastLoop, 100, 75),
#endif
#ifdef USERHOOK_50HZLOOP
SCHED_TASK(userhook_50Hz, 50, 75),
#endif
#ifdef USERHOOK_MEDIUMLOOP
SCHED_TASK(userhook_MediumLoop, 10, 75),
#endif
#ifdef USERHOOK_SLOWLOOP
SCHED_TASK(userhook_SlowLoop, 3.3, 75),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
SCHED_TASK(userhook_SuperSlowLoop, 1, 75),
#endif
SCHED_TASK(button_update, 5, 100),
SCHED_TASK(stats_update, 1, 100),
};
上面任务调度器源自ArduCopter.cpp，关于任务调度器我将会在另外一篇博文中给大家带来介绍，在这里我们先简单的了解下，第一项是这个任务的内容，比如在OPTFLOW使能后，update_optical_flow代表更新光流，可进一步追踪，第二项数字代表每多少HZ会被调用一次，第三项数字代表这个任务所能使用的最大时间，单位是ms。
此时再将目光移到咱的update_GPS，可以看到它是每50HZ调用一次进行GPS更新的，接着追踪
// called at 50hz
void Copter::update_GPS(void)
{
static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message
bool gps_updated = false;
gps.update();
// log after every gps message
for (uint8_t i=0; igps.update实际上是AP_GPS::update()的前端调用
如何进入后端：
在最近写自己的飞控才真的体会到这样一种前后端的魅力所在，尤其是现在各项任务的分工更加明确，每个团队负责的地方不同，接口的统一显得更加重要，我们以后想添加自己的传感器，只需要在后端添加相应的驱动程序，然后按照接口导入数据，就可以不用修改前端的代码，这样一来大大提高了开发项目的效率，以及代码的可用性，说远了，言归正传进入libraries中的AP_GPS，进入其中的update函数如下(AP_GPS.cpp):
/*
update all GPS instances
*/
void AP_GPS::update(void)
{
for (uint8_t i=0; i 0) {
_blend_health_counter--;
}
// stop blending if unhealthy
if (_blend_health_counter >= 50) {
_output_is_blended = false;
}
} else {
_output_is_blended = false;
_blend_health_counter = 0;
}
if (_output_is_blended) {
// Use the weighting to calculate blended GPS states
calc_blended_state();
// set primary to the virtual instance
primary_instance = GPS_BLENDED_INSTANCE;
} else {
// use switch logic to find best GPS
uint32_t now = AP_HAL::millis();
if (_auto_switch >= 1) {
// handling switching away from blended GPS
if (primary_instance == GPS_BLENDED_INSTANCE) {
primary_instance = 0;
for (uint8_t i=1; i state[primary_instance].status) ||
((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) {
primary_instance = i;
_last_instance_swap_ms = now;
}
}
} else {
// handle switch between real GPSs
for (uint8_t i=0; i state[primary_instance].status) {
// we have a higher status lock, or primary is set to the blended GPS, change GPS
primary_instance = i;
_last_instance_swap_ms = now;
continue;
}
bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
(another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000)) {
// this GPS has more satellites than the
// current primary, switch primary. Once we switch we will
// then tend to stick to the new GPS as primary. We don't
// want to switch too often as it will look like a
// position shift to the controllers.
primary_instance = i;
_last_instance_swap_ms = now;
}
}
}
}
} else {
// AUTO_SWITCH is 0 so no switching of GPSs
primary_instance = 0;
}
// copy the primary instance to the blended instance in case it is enabled later
state[GPS_BLENDED_INSTANCE] = state[primary_instance];
_blended_antenna_offset = _antenna_offset[primary_instance];
}
// 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;
}
在这里对每一个GPS实例进行更新，第一个for循环对GPS进行遍历，进入函数update_instance()(AP_GPS.cpp)
/*
更新一个GPS实例
*/
void AP_GPS::update_instance(uint8_t instance)
{
if (_type[instance] == GPS_TYPE_HIL) {
//硬件环仿真
return;
}
if (_type[instance] == GPS_TYPE_NONE) {
// 没用使能
state[instance].status = NO_GPS;
state[instance].hdop = GPS_UNKNOWN_DOP;
state[instance].vdop = GPS_UNKNOWN_DOP;
return;
}
if (locked_ports & (1U<read();
uint32_t tnow = AP_HAL::millis();
// if we did not get a message, and the idle timer of 2 seconds
// has expired, re-initialise the GPS. This will cause GPS
// detection to run again
if (!result) {
if (tnow - timing[instance].last_message_time_ms > GPS_TIMEOUT_MS) {
// free the driver before we run the next detection, so we
// don't end up with two allocated at any time
delete drivers[instance];
drivers[instance] = nullptr;
memset(&state[instance], 0, sizeof(state[instance]));
state[instance].status = NO_GPS;
state[instance].hdop = GPS_UNKNOWN_DOP;
state[instance].vdop = GPS_UNKNOWN_DOP;
timing[instance].last_message_time_ms = tnow;
timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
}
} else {
// delta will only be correct after parsing two messages
timing[instance].delta_time_ms = tnow - timing[instance].last_message_time_ms;
timing[instance].last_message_time_ms = tnow;
if (state[instance].status >= GPS_OK_FIX_2D) {
timing[instance].last_fix_time_ms = tnow;
}
}
}
在这里对一个GPS实例进行更新，随之而来的4个if条件，分别对在硬件环仿真、GPS没有使能、端口被另外一个驱动程序锁定以及这不是我们定义的GPS类型或者是它因为时间问题需要重新初始化，这几种情况都会退出这个函数，回到原来的for循环，对下一个GPS实例进行更新，不同的是最后一个，我们至今还没有判断出是什么GPS类型，所以我们会进入该函数，detect_instance();
void AP_GPS::detect_instance(uint8_t instance)
{
AP_GPS_Backend *new_gps = nullptr;
struct detect_state *dstate = &detect_state[instance];
uint32_t now = AP_HAL::millis();
state[instance].status = NO_GPS;
state[instance].hdop = GPS_UNKNOWN_DOP;
state[instance].vdop = GPS_UNKNOWN_DOP;
switch (_type[instance]) {
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
case GPS_TYPE_QURT:
dstate->auto_detected_baud = false; // specified, not detected
new_gps = new AP_GPS_QURT(*this, state[instance], _port[instance]);
goto found_gps;
break;
#endif
// user has to explicitly set the MAV type, do not use AUTO
// do not try to detect the MAV type, assume it's there
case GPS_TYPE_MAV:
dstate->auto_detected_baud = false; // specified, not detected
new_gps = new AP_GPS_MAV(*this, state[instance], nullptr);
goto found_gps;
break;
#if HAL_WITH_UAVCAN
// user has to explicitly set the UAVCAN type, do not use AUTO
case GPS_TYPE_UAVCAN:
dstate->auto_detected_baud = false; // specified, not detected
if (AP_BoardConfig_CAN::get_can_num_ifaces() >= 1) {
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr) {
AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN();
if (uavcan != nullptr) {
uint8_t gps_node = uavcan->find_gps_without_listener();
if (gps_node != UINT8_MAX) {
new_gps = new AP_GPS_UAVCAN(*this, state[instance], nullptr);
((AP_GPS_UAVCAN*) new_gps)->set_uavcan_manager(i);
if (uavcan->register_gps_listener_to_node(new_gps, gps_node)) {
if (AP_BoardConfig_CAN::get_can_debug() >= 2) {
printf("AP_GPS_UAVCAN registered\n\r");
}
goto found_gps;
} else {
delete new_gps;
}
}
}
}
}
}
return;
#endif
default:
break;
}
if (_port[instance] == nullptr) {
// UART not available
return;
}
// all remaining drivers automatically cycle through baud rates to detect
// the correct baud rate, and should have the selected baud broadcast
dstate->auto_detected_baud = true;
switch (_type[instance]) {
// by default the sbf/trimble gps outputs no data on its port, until configured.
case GPS_TYPE_SBF:
new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]);
break;
case GPS_TYPE_GSOF:
new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]);
break;
case GPS_TYPE_NOVA:
new_gps = new AP_GPS_NOVA(*this, state[instance], _port[instance]);
break;
default:
break;
}
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) {
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
}
if (_auto_config == GPS_AUTO_CONFIG_ENABLE) {
send_blob_update(instance);
}
while (initblob_state[instance].remaining == 0 && _port[instance]->available() > 0
&& new_gps == nullptr) {
uint8_t data = _port[instance]->read();
/*
running a uBlox at less than 38400 will lead to packet
corruption, as we can't receive the packets in the 200ms
window for 5Hz fixes. The NMEA startup message should force
the uBlox into 115200 no matter what rate it is configured
for.
*/
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) &&
((!_auto_config && _baudrates[dstate->current_baud] >= 38400) ||
_baudrates[dstate->current_baud] == 115200) &&
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]);
}
#if !HAL_MINIMIZE_FEATURES
// we drop the MTK drivers when building a small build as they are so rarely used
// and are surprisingly large
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) &&
AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) {
new_gps = new AP_GPS_MTK19(*this, state[instance], _port[instance]);
} else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) &&
AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) {
new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]);
}
#endif
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) {
new_gps = new AP_GPS_SBP2(*this, state[instance], _port[instance]);
}
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]);
}
#if !HAL_MINIMIZE_FEATURES
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) &&
AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]);
}
#endif
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) &&
AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) {
new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]);
} else if (_type[instance] == GPS_TYPE_NMEA &&
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
}
}
found_gps:
if (new_gps != nullptr) {
state[instance].status = NO_FIX;
drivers[instance] = new_gps;
timing[instance].last_message_time_ms = now;
timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
new_gps->broadcast_gps_type();
}
}
把整个检查GPS的代码都贴了上来,代码比较多，运行一个GPS实例的检测步骤，如果它找到了GPS，那么它将填写驱动程序[实例]和更改状态[实例].status从NO_GPS到NO_FIX。.进入到识别gps的类型的方法中会先执行一段发送初始化的代码,这部分代码有意思的是它可以实现每隔1200ms改变波特率,来发送初识配置gps的代码，在后面我们会提到。这里我们注意到while循环，它遍历各个端口所读到的数据，对于其中一个实例来说，它将从缓存读到的数据包放在data（无符号八位整型），然后通过对不同类型GPS的detect()函数，来判断这个数据是属于哪个GPS的，从而确定这是什么类型的GPS，在这里我们进入NEMA的detect()函数（AP_GPS_NEMA）
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;
}
它检测NMEA全球定位系统。如果流匹配一个NMEA字符串，添加一个字节。这里主要是整个报文字符串的合法性，包头
NEMA协议介绍
图注:全球时区的划分
每个时区跨15°经度。以0°经线为界向东向西各划出7.5°经度，作为0时区。即0时区的经度范围是7.5°W——7.5°E。从7.5°E与7.5°W分别向东、向西每15°经度划分为一个时区，直到东11区和西11区。东11区最东部的经度是172.5°E，由172.5°E——180°之间就是东12区。西11区最西部的经度是172.5°W，由172.5°W——180°之间就是西12区。东、西12区各占经度7.5°，合成一个完整的时区，即全球总共划分为24个时区。东、西12区钟点相同，日期相差1天，因此180°称为理论上的国际日期变更线。
由于地球的自转运动，不同地区有不同的地方时间，为了解决时间混乱的问题，采取了划分时区的办法。每个时区中央经线所在地的地方时间就是这个时区共用的时间，称为区时。在实际应用中各国不完全按照区时来定时间，许多国家制定一个法定时，作为该国统一使用的时间，例如我国使用120°E的地方时间，称为北京时间。
字段1：UTC 时间，hhmmss.sss，时分秒格式
字段2：纬度ddmm.mmmm，度分格式（前导位数不足则补0）
字段3：纬度N（北纬）或S（南纬）
字段4：经度dddmm.mmmm，度分格式（前导位数不足则补0）
字段5：经度E（东经）或W（西经）
字段6：GPS状态，0=未定位，1=非差分定位，2=差分定位，3=无效PPS，6=正在估算
字段7：正在使用的卫星数量（00 ~ 12）（前导位数不足则补0）
字段8：HDOP水平精度因子（0.5 ~ 99.9）
字段9：海拔高度（-9999.9~99999.9）
字段10：地球椭球面相对大地水准面的高度
字段11：差分时间（从最近一次接收到差分信号开始的秒数，如果不是差分定位将为空）
字段12：差分站ID号0000 ~ 1023（前导位数不足则补0，如果不是差分定位将为空）
字段13：校验值
代码解析NEMA
bool AP_GPS_NMEA::_decode(char c)
{
bool valid_sentence = false;
switch (c) {
case ',': // term terminators
_parity ^= c;
FALLTHROUGH;
case '\r':
case '\n':
case '*':
if (_term_offset < sizeof(_term)) {
_term[_term_offset] = 0;
valid_sentence = _term_complete();
}
++_term_number;
_term_offset = 0;
_is_checksum_term = c == '*';
return valid_sentence;
case '$': // sentence begin
_term_number = _term_offset = 0;
_parity = 0;
_sentence_type = _GPS_SENTENCE_OTHER;
_is_checksum_term = false;
_gps_data_good = false;
return valid_sentence;
}
// ordinary characters
if (_term_offset < sizeof(_term) - 1)
_term[_term_offset++] = c;
if (!_is_checksum_term)
_parity ^= c;
return valid_sentence;
}
因此我们先进入case里的，结束符∗，再取低四位匹配，和取高四位匹配，都OK，则说明是NEMAGPS，则返回true，确定是NEMA类型的GPS。¨K16K¨K14K每个时区跨15°经度。以0°经线为界向东向西各划出7.5°经度，作为0时区。即0时区的经度范围是7.5°W——7.5°E。从7.5°E与7.5°W分别向东、向西每15°经度划分为一个时区，直到东11区和西11区。东11区最东部的经度是172.5°E，由172.5°E——180°之间就是东12区。西11区最西部的经度是172.5°W，由172.5°W——180°之间就是西12区。东、西12区各占经度7.5°，合成一个完整的时区，即全球总共划分为24个时区。东、西12区钟点相同，日期相差1天，因此180°称为理论上的国际日期变更线。由于地球的自转运动，不同地区有不同的地方时间，为了解决时间混乱的问题，采取了划分时区的办法。每个时区中央经线所在地的地方时间就是这个时区共用的时间，称为区时。在实际应用中各国不完全按照区时来定时间，许多国家制定一个法定时，作为该国统一使用的时间，例如我国使用120°E的地方时间，称为北京时间。字段1：UTC 时间，hhmmss.sss，时分秒格式 字段2：纬度ddmm.mmmm，度分格式（前导位数不足则补0） 字段3：纬度N（北纬）或S（南纬） 字段4：经度dddmm.mmmm，度分格式（前导位数不足则补0） 字段5：经度E（东经）或W（西经） 字段6：GPS状态，0=未定位，1=非差分定位，2=差分定位，3=无效PPS，6=正在估算 字段7：正在使用的卫星数量（00 12）（前导位数不足则补0） 字段8：HDOP水平精度因子（0.5 99.9） 字段9：海拔高度（−9999.9 99999.9） 字段10：地球椭球面相对大地水准面的高度 字段11：差分时间（从最近一次接收到差分信号开始的秒数，如果不是差分定位将为空） 字段12：差分站ID号0000 1023（前导位数不足则补0，如果不是差分定位将为空） 字段13：校验值¨K15K¨G6G因此我们先进入case里的,这里对各种标志位，变量进行初始化，然后下一个传的是普通字符的话，是进入最下面的两个if，其中第一个if的作用，是将传来的普通字符进行存储，第二个if，其中条件里面的(!_is_checksum_term)，在中进行了false，它的含义是当前术语是校验和，现在还没到校验和的时候，所以他是false，因此会进入if，通过异或检验操作后的语句，看着格式我们现在收到解码了GPGGA，下一个字符是逗号，此时将会进入_term_complete()函数
bool AP_GPS_NMEA::_term_complete()
{
// handle the last term in a message
if (_is_checksum_term) {
uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]);
if (checksum == _parity) {
if (_gps_data_good) {
uint32_t now = AP_HAL::millis();
switch (_sentence_type) {
case _GPS_SENTENCE_RMC:
_last_RMC_ms = now;
//time = _new_time;
//date = _new_date;
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
state.ground_speed = _new_speed*0.01f;
state.ground_course = wrap_360(_new_course*0.01f);
make_gps_time(_new_date, _new_time * 10);
state.last_gps_time_ms = now;
fill_3d_velocity();
break;
case _GPS_SENTENCE_GGA:
_last_GGA_ms = now;
state.location.alt = _new_altitude;
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
state.num_sats = _new_satellite_count;
state.hdop = _new_hdop;
switch(_new_quality_indicator) {
case 0: // Fix not available or invalid
state.status = AP_GPS::NO_FIX;
break;
case 1: // GPS SPS Mode, fix valid
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 2: // Differential GPS, SPS Mode, fix valid
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 3: // GPS PPS Mode, fix valid
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 4: // Real Time Kinematic. System used in RTK mode with fixed integers
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 5: // Float RTK. Satellite system used in RTK mode, floating integers
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
break;
case 6: // Estimated (dead reckoning) Mode
state.status = AP_GPS::NO_FIX;
break;
default://to maintain compatibility with MAV_GPS_INPUT and others
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
}
break;
case _GPS_SENTENCE_VTG:
_last_VTG_ms = now;
state.ground_speed = _new_speed*0.01f;
state.ground_course = wrap_360(_new_course*0.01f);
fill_3d_velocity();
// VTG has no fix indicator, can't change fix status
break;
}
} else {
switch (_sentence_type) {
case _GPS_SENTENCE_RMC:
case _GPS_SENTENCE_GGA:
// Only these sentences give us information about
// fix status.
state.status = AP_GPS::NO_FIX;
}
}
// see if we got a good message
return _have_new_message();
}
// we got a bad message, ignore it
return false;
}
// the first term determines the sentence type
if (_term_number == 0) {
/*
The first two letters of the NMEA term are the talker
ID. The most common is 'GP' but there are a bunch of others
that are valid. We accept any two characters here.
*/
if (_term[0] < 'A' || _term[0] > 'Z' ||
_term[1] < 'A' || _term[1] > 'Z') {
_sentence_type = _GPS_SENTENCE_OTHER;
return false;
}
const char *term_type = &_term[2];
if (strcmp(term_type, "RMC") == 0) {
_sentence_type = _GPS_SENTENCE_RMC;
} else if (strcmp(term_type, "GGA") == 0) {
_sentence_type = _GPS_SENTENCE_GGA;
} else if (strcmp(term_type, "VTG") == 0) {
_sentence_type = _GPS_SENTENCE_VTG;
// VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise.
_gps_data_good = true;
} else {
_sentence_type = _GPS_SENTENCE_OTHER;
}
return false;
}
// 32 = RMC, 64 = GGA, 96 = VTG
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
switch (_sentence_type + _term_number) {
// operational status
//
case _GPS_SENTENCE_RMC + 2: // validity (RMC)
_gps_data_good = _term[0] == 'A';
break;
case _GPS_SENTENCE_GGA + 6: // Fix data (GGA)
_gps_data_good = _term[0] > '0';
_new_quality_indicator = _term[0] - '0';
break;
case _GPS_SENTENCE_VTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N';
break;
case _GPS_SENTENCE_GGA + 7: // satellite count (GGA)
_new_satellite_count = atol(_term);
break;
case _GPS_SENTENCE_GGA + 8: // HDOP (GGA)
_new_hdop = (uint16_t)_parse_decimal_100(_term);
break;
// time and date
//
case _GPS_SENTENCE_RMC + 1: // Time (RMC)
case _GPS_SENTENCE_GGA + 1: // Time (GGA)
_new_time = _parse_decimal_100(_term);
break;
case _GPS_SENTENCE_RMC + 9: // Date (GPRMC)
_new_date = atol(_term);
break;
// location
//
case _GPS_SENTENCE_RMC + 3: // Latitude
case _GPS_SENTENCE_GGA + 2:
_new_latitude = _parse_degrees();
break;
case _GPS_SENTENCE_RMC + 4: // N/S
case _GPS_SENTENCE_GGA + 3:
if (_term[0] == 'S')
_new_latitude = -_new_latitude;
break;
case _GPS_SENTENCE_RMC + 5: // Longitude
case _GPS_SENTENCE_GGA + 4:
_new_longitude = _parse_degrees();
break;
case _GPS_SENTENCE_RMC + 6: // E/W
case _GPS_SENTENCE_GGA + 5:
if (_term[0] == 'W')
_new_longitude = -_new_longitude;
break;
case _GPS_SENTENCE_GGA + 9: // Altitude (GPGGA)
_new_altitude = _parse_decimal_100(_term);
break;
// course and speed
//
case _GPS_SENTENCE_RMC + 7: // Speed (GPRMC)
case _GPS_SENTENCE_VTG + 5: // Speed (VTG)
_new_speed = (_parse_decimal_100(_term) * 514) / 1000; // knots-> m/sec, approximiates * 0.514
break;
case _GPS_SENTENCE_RMC + 8: // Course (GPRMC)
case _GPS_SENTENCE_VTG + 1: // Course (VTG)
_new_course = _parse_decimal_100(_term);
break;
}
}
return false;
}
由于我们的标志位_is_checksum_term在$字符后是置零的，因此我们不会进入这个大的if，接着往下,这if(_term_number == 0)一个条件的判断，是来判别整个句子的类型，比如说这里属于GPGGA，strcmp_P函数是C/C++函数里，比较两个字符串，假设这两个字符串为str1，str2， 若str1==str2，则返回零； 若str1str2，则返回正数。
因此在内部if条件中，我们会进入GPGGA，在这里我们将GPGGA所代表的值赋给了_sentence_type，而GPGGA的值以及各个类型的值是在.h文件中枚举来的，容易找到GPGGA的值是64，然后接着往下
第一次的遍历，由于我们的_term_number的值是0，因此没有适合我们的which，退出选择，回到decode()函数，这个时候_term_number+1，还判断了一下是否结尾，当然我们还没有到，下一个循环开始，这里出时间数据了，找到了下一个“，”，然后我们进入_term_complete()函数，此时会跳过前面两个if，然后回到switch这里，这一次我们将会进入case _GPS_SENTENCE_GGA + 1，这个是报表的时间，我们跟踪进入函数_parse_decimal_100(_term)
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;
}
_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()
uint32_t AP_GPS_NMEA::_parse_degrees()
{
char *p, *q;
uint8_t deg = 0, min = 0;
double 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;
double 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;
}
这个函数的功能是解析一个NMEA的纬度/经度值，然后返回的结果是角度1e7，GPGGA解析这一格式的时候是度分格式，举个例子，此时的纬度报文为4250.5589。 进行一定的初始化以后，我们将进入第一个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=deg10=410=40,然后deg+= DIGIT_TO_VAL(q++)=40+2=42,即deg=42然后q++,q指向’5’，进行while判断，（p-q）=2，此时不大于2了，退出第一个while循环，现在回首来看这第一个循环其实是进行了度的转换。
进入第二个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=min10=50,然后执行min+= DIGIT_TO_VAL(q++)=50+0=50,即min=50，然后*q++,q指向’,’，这个时候到了p的位置，两个指针相遇了，退出第二个while循环，现在回首来看这第二个循环其实是进行了小数点左边分的转换。
接下来我们要开始处理小数部分的分了，进入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+5*0.01=0.55,然后q++，往右移一格，此时q=’8’,缩减因子rac_scale *= 0.1f=0.001,，再进入while循环，此时frac_min+= DIGIT_TO_VAL(*q) * frac_scale=0.55+8*0.001=0.558，然后q++，往右移一格，此时q=‘9’，缩减因子rac_scale *= 0.1f=0.0001,再进入while循环，此时frac_min+= DIGIT_TO_VAL(*q) * frac_scale=0.558+9*0.0001=0.5589，然后q++，往右移一格，此时q=‘,’,缩减因子rac_scale *= 0.1f=0.00001，退出循环。现在回首来看这部分的函数，是专门用来处理小数点后面的分数部分，在这里我们可以发现，当我们传的数据是GPGGA long格式的时候，小数点后面会多三位，在这个程序里仍然是执行正确的，也就是说当执行GPGGA Long的时候，储存进入的值仍是正确的，接着往下看。
ret = (deg * (int32_t)10000000UL);
ret += (min * (int32_t)10000000UL / 60);
ret += (int32_t) (frac_min * (1.0e7f / 60.0f));
return ret;
ret=(deg* (int32_t)10000000UL)+ (min * (int32_t)10000000UL / 60)+ (int32_t) (frac_min * (1.0e7f / 60.0f))，我们把举的例子来算一算
ret=deg10^7+(min10^7)/60+(frac_min*10^7)/60
=420000000+8333333+93150
=428426483
也就是说即使我们这里使用的是GPGGA long型的包，这个程序也是正确解码的，通过虚拟浮点数保证了后七位经度，然后再传回_new_latitude进行后续使用。
追踪state.location.lat，画出调用关系图
图注:调用关系图
最后解包完就放在了AP_common供各层代码调用了，比如说home点，origin点。
原创不易，转载请注明出处
欢迎加我