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; i<gps.num_sensors(); i++) {
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
last_gps_reading[i] = gps.last_message_time_ms(i);
// log GPS message
if (should_log(MASK_LOG_GPS) && !ahrs.have_ekf_logging()) {
DataFlash.Log_Write_GPS(gps, i);
}
gps_updated = true;
}
}
if (gps_updated) {
// set system time if necessary
set_system_time_from_GPS();
#if CAMERA == ENABLED
camera.update();
#endif
}
}
gps.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<GPS_MAX_RECEIVERS; i++) {
update_instance(i);
}
// calculate number of instances
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (state[i].status != NO_GPS) {
num_instances = i+1;
}
}
// if blending is requested, attempt to calculate weighting for each GPS
if (_auto_switch == 2) {
_output_is_blended = calc_blend_weights();
// adjust blend health counter
if (!_output_is_blended) {
_blend_health_counter = MIN(_blend_health_counter+BLEND_COUNTER_FAILURE_INCREMENT, 100);
} else if (_blend_health_counter > 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<GPS_MAX_RECEIVERS; i++) {
// choose GPS with highest state or higher number of satellites
if ((state[i].status > 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<GPS_MAX_RECEIVERS; i++) {
if (i == primary_instance) {
continue;
}
if (state[i].status > 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<<instance)) {
//端口被另外一個驅動程序鎖定（後續介紹操作系統會有更深刻的理解）
return;
}
if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
//這不是我們定義的GPS類型或者是它因爲時間問題需要重新初始化
detect_instance(instance);
return;
}
if (_auto_config == GPS_AUTO_CONFIG_ENABLE) {
send_blob_update(instance);
}
// we have an active driver for this instance
bool result = drivers[instance]->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點。
原創不易，轉載請註明出處
歡迎加我
