目錄
文章目錄
- 目錄
- 摘要
- 1.GPS資訊簡介
-
- 1. GPGGA(gps定位資訊)
- 2. GPGSA(目前衛星資訊)
- 3. GPGSV(可見衛星資訊)
- 4. GPRMC (推薦定位資訊)
- 5. GPVTG (地面速度資訊)
- 6. GPG LL(定位地理資訊)
- 7. GPZDA (目前時間資訊)
- 8.GPS的UBLOX協定
- 2.GPS傳感器初始化
-
- 1.執行 GPS的序列槽協定初始化：serial_manager.init();
- 2.GPS函數初始化
- 3.更新GPS資料
-
- 1. gps.update();
-
- 1.update_instance(i); //更新GPS執行個體的個數
-
- 1.執行個體化選擇配置不同的GPS
- 2. bool result = drivers[instance]->read(); //讀取資料
-
- 1.UBLOX資料讀取
- 2.NMEA資料讀取
- 3.UBLOX資料讀取
-
- 1.需要注意的函數_request_next_config();
- 2.需要注意的函數_parse_gps()
- 2. calc_blended_state();
摘要
本節主要學習ardupilot的GPS資料更新過程，歡迎批評指正。
1.GPS資訊簡介
GPS（Global Positioning System），即全球定位系統，利用24顆GPS的測距和測時功能進行全球定位，在許多系統中，如現代船載（或車載）導航系統、機場導航系統、江河流域的災害資訊管理和預測系統中，GPS得到了廣泛的應用。
GPS主要有3大組成部分，即空間星座部分、地面監控部分和使用者裝置部分。其中空間星座部分、地面監控部分均為美國所控制。GPS的使用者裝置主要有接收機硬體和處理軟體組成。使用者通過使用者裝置接收GPS衛星信号，經信号處理而獲得使用者位置、速度等資訊，最終實作GPS進行導航和定位的目的。
對于通用GPS應用軟體，需要一個統一格式的資料标準，以解決與任意一台GPS的接口問題。NMEA-0183資料标準就是解決這類問題的方案之一。NMEA協定是為了在不同的GPS導航裝置中建立統一的RTCM(海事無線電技術委員會)标準，它最初是由美國國家海洋電子協會(NMEA—The NationalMarine Electronics Association)制定的。NMEA協定有0180、0182和0183這3種，0183可以認為是前兩種的更新，也是目前使用最為廣泛的一種。
一般GPS子產品采用序列槽方式與外部子產品進行通信，輸出的GPS定位資料采用NMEA-0183協定，控制協定為為UBX協定。
注 1： 即協調世界時，相當于本初子午線(0 度經線)上的時間，中原標準時間比 UTC 早 8 個小時。
1. GPGGA(gps定位資訊)
2. GPGSA(目前衛星資訊)
3. GPGSV(可見衛星資訊)
4. GPRMC (推薦定位資訊)
5. GPVTG (地面速度資訊)
6. GPG LL(定位地理資訊)
7. GPZDA (目前時間資訊)
8.GPS的UBLOX協定
2.GPS傳感器初始化
void Copter::setup()
{
//加載var_info[]s中列出的變量的預設值---- Load the default values of variables listed in var_info[]s
AP_Param::setup_sketch_defaults();
//為直升機設定存儲布局-------------------setup storage layout for copter
StorageManager::set_layout_copter();
//驅動裝置初始化
init_ardupilot();
//初始化主循環計劃程式------------------ initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}
//執行init_ardupilot();
void Copter::init_ardupilot()
{
serial_manager.init();
}
1.執行 GPS的序列槽協定初始化：serial_manager.init();
void AP_SerialManager::init()
{
// initialise pointers to serial ports
state[1].uart = hal.uartC; // serial1, uartC, normally telem1
state[2].uart = hal.uartD; // serial2, uartD, normally telem2
state[3].uart = hal.uartB; // serial3, uartB, normally 1st GPS
state[4].uart = hal.uartE; // serial4, uartE, normally 2nd GPS
state[5].uart = hal.uartF; // serial5
state[6].uart = hal.uartG; // serial6
if (state[0].uart == nullptr)
{
init_console(); //沒有序列槽就直接進行終端初始化
}
//初始化串行端口------ initialise serial ports
for (uint8_t i=1; i<SERIALMANAGER_NUM_PORTS; i++)
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
if (i == 5 && state[i].protocol != SerialProtocol_None)
{
// tell nsh to exit to free up this uart
g_nsh_should_exit = true;
}
#endif
if (state[i].uart != nullptr)
{
switch (state[i].protocol)
{
case SerialProtocol_None:
break;
case SerialProtocol_Console: //0
case SerialProtocol_MAVLink: //1
case SerialProtocol_MAVLink2: //2
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_MAVLINK_BUFSIZE_RX,
AP_SERIALMANAGER_MAVLINK_BUFSIZE_TX);
break;
case SerialProtocol_FrSky_D:
// Note baudrate is hardcoded to 9600
state[i].baud = AP_SERIALMANAGER_FRSKY_D_BAUD/1000; // update baud param in case user looks at it
// begin is handled by AP_Frsky_telem library
break;
case SerialProtocol_FrSky_SPort:
case SerialProtocol_FrSky_SPort_Passthrough:
// Note baudrate is hardcoded to 57600
state[i].baud = AP_SERIALMANAGER_FRSKY_SPORT_BAUD/1000; // update baud param in case user looks at it
// begin is handled by AP_Frsky_telem library
break;
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;
case SerialProtocol_AlexMos:
// Note baudrate is hardcoded to 115200
state[i].baud = AP_SERIALMANAGER_ALEXMOS_BAUD / 1000; // update baud param in case user looks at it
state[i].uart->begin(AP_SERIALMANAGER_ALEXMOS_BAUD,
AP_SERIALMANAGER_ALEXMOS_BUFSIZE_RX,
AP_SERIALMANAGER_ALEXMOS_BUFSIZE_TX);
break;
case SerialProtocol_SToRM32:
// Note baudrate is hardcoded to 115200
state[i].baud = AP_SERIALMANAGER_SToRM32_BAUD / 1000; // update baud param in case user looks at it
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_SToRM32_BUFSIZE_RX,
AP_SERIALMANAGER_SToRM32_BUFSIZE_TX);
break;
case SerialProtocol_Aerotenna_uLanding:
state[i].protocol.set_and_save(SerialProtocol_Rangefinder);
break;
case SerialProtocol_Volz:
// Note baudrate is hardcoded to 115200
state[i].baud = AP_SERIALMANAGER_VOLZ_BAUD; // update baud param in case user looks at it
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_VOLZ_BUFSIZE_RX,
AP_SERIALMANAGER_VOLZ_BUFSIZE_TX);
state[i].uart->set_unbuffered_writes(true);
state[i].uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
break;
case SerialProtocol_Sbus1:
state[i].baud = AP_SERIALMANAGER_SBUS1_BAUD / 1000; // update baud param in case user looks at it
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_SBUS1_BUFSIZE_RX,
AP_SERIALMANAGER_SBUS1_BUFSIZE_TX);
state[i].uart->configure_parity(2); // enable even parity
state[i].uart->set_stop_bits(2);
state[i].uart->set_unbuffered_writes(true);
state[i].uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
break;
case SerialProtocol_ESCTelemetry:
// ESC telemetry protocol from BLHeli32 ESCs. Note that baudrate is hardcoded to 115200
state[i].baud = 115200;
state[i].uart->begin(map_baudrate(state[i].baud), 30, 30);
state[i].uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
break;
//配置NRA24資料
case SerialProtocol_NRA24:
// Note baudrate is hardcoded to 115200
state[i].baud = AP_SERIALMANAGER_ULANDING_BAUD / 1000; // update baud param in case user looks at it
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_ULANDING_BUFSIZE_RX,
AP_SERIALMANAGER_ULANDING_BUFSIZE_TX);
break;
case SerialProtocol_MQTT:
// Note baudrate is hardcoded to 115200
state[i].baud = AP_SERIALMANAGER_ULANDING_BAUD / 1000; // update baud param in case user looks at it
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_ULANDING_BUFSIZE_RX,
AP_SERIALMANAGER_ULANDING_BUFSIZE_TX);
break;
case SerialProtocol_HPS166U:
// Note baudrate is hardcoded to 115200
state[i].baud = AP_SERIALMANAGER_ULANDING_BAUD / 1000; // update baud param in case user looks at it
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_ULANDING_BUFSIZE_RX,
AP_SERIALMANAGER_ULANDING_BUFSIZE_TX);
break;
case SerialProtocol_HPS167UL:
// Note baudrate is hardcoded to 115200
state[i].baud = AP_SERIALMANAGER_ULANDING_BAUD / 1000; // update baud param in case user looks at it
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_ULANDING_BUFSIZE_RX,
AP_SERIALMANAGER_ULANDING_BUFSIZE_TX);
break;
case SerialProtocol_HL6_M30:
// Note baudrate is hardcoded to 19200
state[i].baud = AP_SERIALMANAGER6_MAVLINK_BAUD / 1000; // update baud param in case user looks at it
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_ULANDING_BUFSIZE_RX,
AP_SERIALMANAGER_ULANDING_BUFSIZE_TX);
break;
case SerialProtocol_Lidar360:
// Note baudrate is hardcoded to 115200
state[i].baud = AP_SERIALMANAGER_ULANDING_BAUD / 1000; // update baud param in case user looks at it
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_ULANDING_BUFSIZE_RX,
AP_SERIALMANAGER_ULANDING_BUFSIZE_TX);
break;
case SerialProtocol_MR72:
// Note baudrate is hardcoded to 115200
state[i].baud = AP_SERIALMANAGER_ULANDING_BAUD / 1000; // update baud param in case user looks at it
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_ULANDING_BUFSIZE_RX,
AP_SERIALMANAGER_ULANDING_BUFSIZE_TX);
break;
}
}
}
}
從代碼可以看出預設GPS1選擇的是serial3,GPS2選擇的是serial4,重點是配置實作相應的序列槽協定和波特率。
2.GPS函數初始化
gps.set_log_gps_bit(MASK_LOG_GPS); //設定記錄GPS log标志位
gps.init(serial_manager); //進行GPS函數初始化
需要看的函數是： gps.init(serial_manager);
void AP_GPS::init(const AP_SerialManager& serial_manager)
{
primary_instance = 0; //主要的執行個體
//尋找滿足GPS協定的串行端口協定------search for serial ports with gps protocol
_port[0] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 0); //預設0
_port[1] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 1); //預設1
_last_instance_swap_ms = 0; //最後執行個體交換ms
//初始化用于GPS融合的類變量---- 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
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++)
{
if (_rate_ms[i] <= 0 || _rate_ms[i] > GPS_MAX_RATE_MS) //設定最慢的更新頻率是5HZ
{
_rate_ms[i] = GPS_MAX_RATE_MS;
}
}
}
3.更新GPS資料
void Copter::update_GPS(void)
{
//上次GPS資訊資料
static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message
//gps是否更新标志
bool gps_updated = false;
//更新GPS資料
gps.update();
//log記錄每一個GPS資訊---- 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);
gps_updated = true;
break;
}
}
if (gps_updated) //gps資料更新了，記錄相機更新标志
{
#if CAMERA == ENABLED
camera.update();
#endif
}
}
1. gps.update();
void AP_GPS::update(void)
{
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++)
{
update_instance(i); //更新GPS執行個體的個數
}
//計算GPS執行個體的數量------ calculate number of instances
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++)
{
if (state[i].status != NO_GPS) //
{
num_instances = i+1;
}
}
//如果采用融合政策，嘗試計算每個GPS的權重-------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) //輸出資料是融合的
{
// 使用權重計算GPS融合狀态-----Use the weighting to calculate blended GPS states
calc_blended_state();
//設定虛拟執行個體的優先級-----set primary to the virtual instance
primary_instance = GPS_BLENDED_INSTANCE;
} else
{
//查找最好的GPS-----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];
}
//更新通知GPS狀态，我們總是根據優先級執行個體---------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;
}
1.update_instance(i); //更新GPS執行個體的個數
void AP_GPS::update_instance(uint8_t instance)
{
if (_type[instance] == GPS_TYPE_HIL) //判斷是不是仿真
{
// in HIL, leave info alone
return;
}
if (_type[instance] == GPS_TYPE_NONE) //沒有GPS類型
{
// not enabled
state[instance].status = NO_GPS;
state[instance].hdop = GPS_UNKNOWN_DOP;
state[instance].vdop = GPS_UNKNOWN_DOP;
return;
}
if (locked_ports & (1U<<instance))
{
// the port is locked by another driver
return;
}
if (drivers[instance] == nullptr || state[instance].status == NO_GPS)
{
// 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) //自動配置是否使能
{
send_blob_update(instance); //自動配置成UBLOX
}
//我們已經激活GPS驅動執行個體，這裡進行了資料的讀取----we have an active driver for this instance
bool result = drivers[instance]->read(); //讀取資料
const 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
bool data_should_be_logged = false;
if (!result)
{
if (tnow - timing[instance].last_message_time_ms > GPS_TIMEOUT_MS)
{
memset(&state[instance], 0, sizeof(state[instance]));
state[instance].instance = instance;
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;
// do not try to detect again if type is MAV
if (_type[instance] == GPS_TYPE_MAV) {
state[instance].status = NO_FIX;
} else {
// 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;
state[instance].status = NO_GPS;
}
// log this data as a "flag" that the GPS is no longer
// valid (see PR#8144)
data_should_be_logged = true;
}
} 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;
}
data_should_be_logged = true;
}
if (data_should_be_logged &&
should_df_log() &&
!AP::ahrs().have_ekf_logging()) {
DataFlash_Class::instance()->Log_Write_GPS(instance);
}
if (state[instance].status >= GPS_OK_FIX_3D)
{
const uint64_t now = time_epoch_usec(instance);
AP::rtc().set_utc_usec(now, AP_RTC::SOURCE_GPS);
}
}
1.執行個體化選擇配置不同的GPS
void AP_GPS::detect_instance(uint8_t instance)
{
AP_GPS_Backend *new_gps = nullptr;
struct detect_state *dstate = &detect_state[instance];
const 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]) {
// 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() == 0) {
return;
}
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i);
if (ap_uavcan == nullptr) {
continue;
}
uint8_t gps_node = ap_uavcan->find_gps_without_listener();
if (gps_node == UINT8_MAX) {
continue;
}
new_gps = new AP_GPS_UAVCAN(*this, state[instance], nullptr);
((AP_GPS_UAVCAN*) new_gps)->set_uavcan_manager(i);
if (ap_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 && new_gps == nullptr) //這裡選擇自動配置
{
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
}
if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) {
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();
}
}
如果是選擇自動配置就會選擇UBLOX
需要重點分析下面代碼
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 && new_gps == nullptr) //這裡選擇自動配置
{
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
}
if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr)
{
send_blob_update(instance);
}
//重點：send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));我們先看下_initialisation_blob
//初始化blob發送到gps以嘗試使其進入正确模式
const char AP_GPS::_initialisation_blob[] = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY;
下面是UBLOX的Class資訊
具體的UBLOX協定資訊可以去這裡下載下傳：UBLOX協定
void AP_GPS::send_blob_start(uint8_t instance, const char *_blob, uint16_t size)
{
initblob_state[instance].blob = _blob;
initblob_state[instance].remaining = size;
}
send_blob_update(instance);
void AP_GPS::send_blob_update(uint8_t instance)
{
//如果沒有UART，立即退出此執行個體---- exit immediately if no uart for this instance
if (_port[instance] == nullptr)
{
return;
}
//看看我們能不能再寫一些初始化blob---- see if we can write some more of the initialisation blob
if (initblob_state[instance].remaining > 0)
{
int16_t space = _port[instance]->txspace();
if (space > (int16_t)initblob_state[instance].remaining)
{
space = initblob_state[instance].remaining;
}
while (space > 0)
{
_port[instance]->write(*initblob_state[instance].blob);
initblob_state[instance].blob++;
space--;
initblob_state[instance].remaining--;
}
}
}
2. bool result = drivers[instance]->read(); //讀取資料
這裡我們主要選擇UBLOX進行GPS資料解析，當然你可以選擇NMEA等等。
1.UBLOX資料讀取
bool AP_GPS_UBLOX::read(void)
{
uint8_t data;
int16_t numc;
bool parsed = false;
uint32_t millis_now = AP_HAL::millis();
// walk through the gps configuration at 1 message per second
if (millis_now - _last_config_time >= _delay_time) {
_request_next_config();
_last_config_time = millis_now;
if (_unconfigured_messages) { // send the updates faster until fully configured
if (!havePvtMsg && (_unconfigured_messages & CONFIG_REQUIRED_INITIAL)) {
_delay_time = 300;
} else {
_delay_time = 750;
}
} else {
_delay_time = 2000;
}
}
if(!_unconfigured_messages && gps._save_config && !_cfg_saved &&
_num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 &&
!hal.util->get_soft_armed()) {
//save the configuration sent until now
if (gps._save_config == 1 ||
(gps._save_config == 2 && _cfg_needs_save)) {
_save_cfg();
}
}
numc = port->available();
for (int16_t i = 0; i < numc; i++) { // Process bytes received
//這裡就是序列槽讀取資料--read the next byte
data = port->read();
reset:
switch(_step) //按順序進行解析
{
// Message preamble detection
//
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// as data in some other message.
//
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
Debug("reset %u", __LINE__);
FALLTHROUGH;
case 0:
if(PREAMBLE1 == data)
_step++;
break;
// Message header processing
//
// We sniff the class and message ID to decide whether we
// are going to gather the message bytes or just discard
// them.
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
_class = data;
_ck_b = _ck_a = data; // reset the checksum accumulators
break;
case 3:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
break;
case 4:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t)(data<<8);
if (_payload_length > sizeof(_buffer)) {
Debug("large payload %u", (unsigned)_payload_length);
// assume any payload bigger then what we know about is noise
_payload_length = 0;
_step = 0;
goto reset;
}
_payload_counter = 0; // prepare to receive payload
break;
// Receive message data
//
case 6:
_ck_b += (_ck_a += data); // checksum byte
if (_payload_counter < sizeof(_buffer)) {
_buffer[_payload_counter] = data;
}
if (++_payload_counter == _payload_length)
_step++;
break;
// Checksum and message processing
//
case 7:
_step++;
if (_ck_a != data) {
Debug("bad cka %x should be %x", data, _ck_a);
_step = 0;
goto reset;
}
break;
case 8:
_step = 0;
if (_ck_b != data) {
Debug("bad ckb %x should be %x", data, _ck_b);
break; // bad checksum
}
if (_parse_gps()) {
parsed = true;
}
break;
}
}
return parsed;
}
2.NMEA資料讀取
bool AP_GPS_NMEA::read(void)
{
int16_t numc;
bool parsed = false;
numc = port->available();
// hal.console->printf("numc=%d\r\n ",numc);
while (numc--)
{
char c = port->read(); //擷取序列槽資料
// hal.console->printf("c=%c\r\n ",c);
#ifdef NMEA_LOG_PATH
static FILE *logf = nullptr;
if (logf == nullptr)
{
logf = fopen(NMEA_LOG_PATH, "wb");
}
if (logf != nullptr)
{
::fwrite(&c, 1, 1, logf);
}
#endif
if (_decode(c)) //進行資料解析
{
parsed = true;
}
}
return parsed;
}
進行GPS資料解析
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;
}
_term_complete(); //首先進行資料解析
****************************************************************************************************************/
bool AP_GPS_NMEA::_term_complete()
{
// hal.console->printf("_is_checksum_term4=%d\r\n ",_is_checksum_term);
// hal.console->printf("_sentence_type=%d\r\n ",_sentence_type);
// hal.console->printf("_gps_data_good=%d\r\n ",_gps_data_good);
//處理消息中的最後一個術語----- handle the last term in a message
if (_is_checksum_term) //目前是校驗資料
{
uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]);
// hal.console->printf("checksum=%d\r\n ",checksum);
// hal.console->printf("_parity=%d\r\n ",_parity);
// hal.console->printf("_gps_data_good=%d\r\n ",_gps_data_good);
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: //最終擷取的GPS資料就在這裡，這些資料将會參與EKF的運算
_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.
// hal.console->printf("HHHKKK\r\n ");
state.status = AP_GPS::NO_FIX;
}
}
// hal.console->printf("HHH\r\n ");
// see if we got a good message
return _have_new_message();
}
// we got a bad message, ignore it
return false;
}
// hal.console->printf("_term_number=%d\r\n ",_term_number);
//第一個詞決定句子類型---- the first term determines the sentence type
if (_term_number == 0)
{
// hal.console->printf("HHH123\r\n ");
/*NMEA術語的前兩個字母是talker身份證。最常見的是“GP，但還有很多其他的那是有效的。我們接受任何兩個字元。
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')
{
// hal.console->printf("HHH222\r\n ");
_sentence_type = _GPS_SENTENCE_OTHER;
return false;
}
const char *term_type = &_term[2];
if (strcmp(term_type, "RMC") == 0)
{
// hal.console->printf("HHH333\r\n ");
_sentence_type = _GPS_SENTENCE_RMC;
} else if (strcmp(term_type, "GGA") == 0)
{
// hal.console->printf("HHH444\r\n ");
_sentence_type = _GPS_SENTENCE_GGA;
} else if (strcmp(term_type, "VTG") == 0)
{
// hal.console->printf("HHH555\r\n ");
_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])
{
// hal.console->printf("HHH666\r\n ");
// hal.console->printf("_sentence_type00=%d\r\n ",_sentence_type + _term_number);
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)
// hal.console->printf("_term[0]=%d\r\n ",_term[0]);
_gps_data_good = _term[0] != 'N';
// hal.console->printf("_gps_data_good=%d\r\n ",_gps_data_good);
// _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;
}
3.UBLOX資料讀取
bool AP_GPS_UBLOX::read(void)
{
uint8_t data;
int16_t numc;
bool parsed = false;
uint32_t millis_now = AP_HAL::millis(); //擷取時間
//以每秒1條消息的速度浏覽GPS配置--------walk through the gps configuration at 1 message per second
if (millis_now - _last_config_time >= _delay_time)
{
_request_next_config(); //請求下一次配置
_last_config_time = millis_now;
if (_unconfigured_messages) // send the updates faster until fully configured
{
if (!havePvtMsg && (_unconfigured_messages & CONFIG_REQUIRED_INITIAL))
{
_delay_time = 300;
} else
{
_delay_time = 750;
}
} else
{
_delay_time = 2000;
}
}
if(!_unconfigured_messages && gps._save_config && !_cfg_saved &&
_num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 &&
!hal.util->get_soft_armed())
{
//save the configuration sent until now
if (gps._save_config == 1 ||
(gps._save_config == 2 && _cfg_needs_save))
{
_save_cfg(); //儲存所有的配置
}
}
numc = port->available(); //擷取序列槽有效資料
for (int16_t i = 0; i < numc; i++) //處理位元組資料接收--------Process bytes received
{
//讀取下一位元組資料-----read the next byte
data = port->read();
reset:
switch(_step)
{
// Message preamble detection
//
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// as data in some other message.
//
case 1:
if (PREAMBLE2 == data) //擷取幀頭2資料
{
_step++;
break;
}
_step = 0;
Debug("reset %u", __LINE__);
FALLTHROUGH;
case 0:
if(PREAMBLE1 == data) //擷取幀頭2資料
_step++;
break;
//消息幀頭處理----Message header processing
//
// We sniff the class and message ID to decide whether we
// are going to gather the message bytes or just discard
// them.
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
_class = data;
_ck_b = _ck_a = data; //重置校驗和累加器------reset the checksum accumulators
break;
case 3:
_step++;
_ck_b += (_ck_a += data); //校驗總數位元組----checksum byte
_msg_id = data;
break;
case 4:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t)(data<<8);
if (_payload_length > sizeof(_buffer))
{
Debug("large payload %u", (unsigned)_payload_length);
// assume any payload bigger then what we know about is noise
_payload_length = 0;
_step = 0;
goto reset;
}
_payload_counter = 0; // prepare to receive payload
break;
// 接收消息資料------Receive message data
//
case 6:
_ck_b += (_ck_a += data); //校驗總和------ checksum byte
if (_payload_counter < sizeof(_buffer))
{
_buffer[_payload_counter] = data;
}
if (++_payload_counter == _payload_length)
_step++;
break;
// Checksum and message processing
//
case 7:
_step++;
if (_ck_a != data)
{
Debug("bad cka %x should be %x", data, _ck_a);
_step = 0;
goto reset;
}
break;
case 8:
_step = 0;
if (_ck_b != data)
{
Debug("bad ckb %x should be %x", data, _ck_b);
break; // bad checksum
}
if (_parse_gps()) //解析需要的資料
{
parsed = true;
}
break;
}
}
return parsed; //傳回解析是否成功
}
1.需要注意的函數_request_next_config();
2.需要注意的函數_parse_gps()
bool AP_GPS_UBLOX::_parse_gps(void)
{
if (_class == CLASS_ACK) //已經應答
{
Debug("ACK %u", (unsigned)_msg_id);
if(_msg_id == MSG_ACK_ACK)
{
switch(_buffer.ack.clsID)
{
case CLASS_CFG:
switch(_buffer.ack.msgID)
{
case MSG_CFG_CFG:
_cfg_saved = true;
_cfg_needs_save = false;
break;
case MSG_CFG_GNSS:
_unconfigured_messages &= ~CONFIG_GNSS;
break;
case MSG_CFG_MSG:
// There is no way to know what MSG config was ack'ed, assume it was the last
// one requested. To verify it rerequest the last config we sent. If we miss
// the actual ack we will catch it next time through the poll loop, but that
// will be a good chunk of time later.
break;
case MSG_CFG_NAV_SETTINGS:
_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
break;
case MSG_CFG_RATE:
// The GPS will ACK a update rate that is invalid. in order to detect this
// only accept the rate as configured by reading the settings back and
// validating that they all match the target values
break;
case MSG_CFG_SBAS:
_unconfigured_messages &= ~CONFIG_SBAS;
break;
}
break;
case CLASS_MON:
switch(_buffer.ack.msgID)
{
case MSG_MON_HW:
_unconfigured_messages &= ~CONFIG_RATE_MON_HW;
break;
case MSG_MON_HW2:
_unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
break;
}
}
}
return false;
}
if (_class == CLASS_CFG)
{
switch(_msg_id)
{
case MSG_CFG_NAV_SETTINGS:
Debug("Got settings %u min_elev %d drLimit %u\n",
(unsigned)_buffer.nav_settings.dynModel,
(int)_buffer.nav_settings.minElev,
(unsigned)_buffer.nav_settings.drLimit);
_buffer.nav_settings.mask = 0;
if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
_buffer.nav_settings.dynModel != gps._navfilter) {
// we've received the current nav settings, change the engine
// settings and send them back
Debug("Changing engine setting from %u to %u\n",
(unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
_buffer.nav_settings.dynModel = gps._navfilter;
_buffer.nav_settings.mask |= 1;
}
if (gps._min_elevation != -100 &&
_buffer.nav_settings.minElev != gps._min_elevation) {
Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
_buffer.nav_settings.minElev = gps._min_elevation;
_buffer.nav_settings.mask |= 2;
}
if (_buffer.nav_settings.mask != 0) {
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
&_buffer.nav_settings,
sizeof(_buffer.nav_settings));
_unconfigured_messages |= CONFIG_NAV_SETTINGS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
}
return false;
#if UBLOX_GNSS_SETTINGS
case MSG_CFG_GNSS:
if (gps._gnss_mode[state.instance] != 0)
{
struct ubx_cfg_gnss start_gnss = _buffer.gnss;
uint8_t gnssCount = 0;
Debug("Got GNSS Settings %u %u %u %u:\n",
(unsigned)_buffer.gnss.msgVer,
(unsigned)_buffer.gnss.numTrkChHw,
(unsigned)_buffer.gnss.numTrkChUse,
(unsigned)_buffer.gnss.numConfigBlocks);
#if UBLOX_DEBUGGING
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
Debug(" %u %u %u 0x%08x\n",
(unsigned)_buffer.gnss.configBlock[i].gnssId,
(unsigned)_buffer.gnss.configBlock[i].resTrkCh,
(unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
(unsigned)_buffer.gnss.configBlock[i].flags);
}
#endif
for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
if((gps._gnss_mode[state.instance] & (1 << i)) && i != GNSS_SBAS) {
gnssCount++;
}
}
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
// Reserve an equal portion of channels for all enabled systems
if(gps._gnss_mode[state.instance] & (1 << _buffer.gnss.configBlock[i].gnssId)) {
if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId) {
_buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
_buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
} else {
_buffer.gnss.configBlock[i].resTrkCh = 1;
_buffer.gnss.configBlock[i].maxTrkCh = 3;
}
_buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
} else {
_buffer.gnss.configBlock[i].resTrkCh = 0;
_buffer.gnss.configBlock[i].maxTrkCh = 0;
_buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
}
}
if (memcmp(&start_gnss, &_buffer.gnss, sizeof(start_gnss))) {
_send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
_unconfigured_messages |= CONFIG_GNSS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_GNSS;
}
} else {
_unconfigured_messages &= ~CONFIG_GNSS;
}
return false;
#endif
case MSG_CFG_SBAS:
if (gps._sbas_mode != 2) {
Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n",
(unsigned)_buffer.sbas.mode,
(unsigned)_buffer.sbas.usage,
(unsigned)_buffer.sbas.maxSBAS,
(unsigned)_buffer.sbas.scanmode2,
(unsigned)_buffer.sbas.scanmode1);
if (_buffer.sbas.mode != gps._sbas_mode) {
_buffer.sbas.mode = gps._sbas_mode;
_send_message(CLASS_CFG, MSG_CFG_SBAS,
&_buffer.sbas,
sizeof(_buffer.sbas));
_unconfigured_messages |= CONFIG_SBAS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_SBAS;
}
} else {
_unconfigured_messages &= ~CONFIG_SBAS;
}
return false;
case MSG_CFG_MSG:
if(_payload_length == sizeof(ubx_cfg_msg_rate_6)) {
// can't verify the setting without knowing the port
// request the port again
if(_ublox_port >= UBLOX_MAX_PORTS) {
_request_port();
return false;
}
_verify_rate(_buffer.msg_rate_6.msg_class, _buffer.msg_rate_6.msg_id,
_buffer.msg_rate_6.rates[_ublox_port]);
} else {
_verify_rate(_buffer.msg_rate.msg_class, _buffer.msg_rate.msg_id,
_buffer.msg_rate.rate);
}
return false;
case MSG_CFG_PRT:
_ublox_port = _buffer.prt.portID;
return false;
case MSG_CFG_RATE:
if(_buffer.nav_rate.measure_rate_ms != gps._rate_ms[state.instance] ||
_buffer.nav_rate.nav_rate != 1 ||
_buffer.nav_rate.timeref != 0) {
_configure_rate();
_unconfigured_messages |= CONFIG_RATE_NAV;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_RATE_NAV;
}
return false;
}
}
if (_class == CLASS_MON) {
switch(_msg_id) {
case MSG_MON_HW:
if (_payload_length == 60 || _payload_length == 68) {
log_mon_hw();
}
break;
case MSG_MON_HW2:
if (_payload_length == 28) {
log_mon_hw2();
}
break;
case MSG_MON_VER:
_have_version = true;
strncpy(_version.hwVersion, _buffer.mon_ver.hwVersion, sizeof(_version.hwVersion));
strncpy(_version.swVersion, _buffer.mon_ver.swVersion, sizeof(_version.swVersion));
gcs().send_text(MAV_SEVERITY_INFO,
"u-blox %d HW: %s SW: %s",
state.instance + 1,
_version.hwVersion,
_version.swVersion);
break;
default:
unexpected_message();
}
return false;
}
#if UBLOX_RXM_RAW_LOGGING
if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
log_rxm_raw(_buffer.rxm_raw);
return false;
} else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
log_rxm_rawx(_buffer.rxm_rawx);
return false;
}
#endif // UBLOX_RXM_RAW_LOGGING
if (_class != CLASS_NAV) {
unexpected_message();
return false;
}
switch (_msg_id) {
case MSG_POSLLH:
Debug("MSG_POSLLH next_fix=%u", next_fix);
if (havePvtMsg) {
_unconfigured_messages |= CONFIG_RATE_POSLLH;
break;
}
_last_pos_time = _buffer.posllh.time;
state.location.lng = _buffer.posllh.longitude;
state.location.lat = _buffer.posllh.latitude;
state.location.alt = _buffer.posllh.altitude_msl / 10;
state.status = next_fix;
_new_position = true;
state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
#if UBLOX_FAKE_3DLOCK
state.location.lng = 1491652300L;
state.location.lat = -353632610L;
state.location.alt = 58400;
state.vertical_accuracy = 0;
state.horizontal_accuracy = 0;
#endif
break;
case MSG_STATUS:
Debug("MSG_STATUS fix_status=%u fix_type=%u",
_buffer.status.fix_status,
_buffer.status.fix_type);
if (havePvtMsg) {
_unconfigured_messages |= CONFIG_RATE_STATUS;
break;
}
if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
(_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
}else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
next_fix = AP_GPS::GPS_OK_FIX_2D;
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
#if UBLOX_FAKE_3DLOCK
state.status = AP_GPS::GPS_OK_FIX_3D;
next_fix = state.status;
#endif
break;
case MSG_DOP:
Debug("MSG_DOP");
noReceivedHdop = false;
state.hdop = _buffer.dop.hDOP;
state.vdop = _buffer.dop.vDOP;
#if UBLOX_FAKE_3DLOCK
state.hdop = 130;
state.hdop = 170;
#endif
break;
case MSG_SOL:
Debug("MSG_SOL fix_status=%u fix_type=%u",
_buffer.solution.fix_status,
_buffer.solution.fix_type);
if (havePvtMsg) {
state.time_week = _buffer.solution.week;
break;
}
if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
(_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
}else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
next_fix = AP_GPS::GPS_OK_FIX_2D;
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
if(noReceivedHdop) {
state.hdop = _buffer.solution.position_DOP;
}
state.num_sats = _buffer.solution.satellites;
if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
state.last_gps_time_ms = AP_HAL::millis();
state.time_week_ms = _buffer.solution.time;
state.time_week = _buffer.solution.week;
}
#if UBLOX_FAKE_3DLOCK
next_fix = state.status;
state.num_sats = 10;
state.time_week = 1721;
state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
state.last_gps_time_ms = AP_HAL::millis();
state.hdop = 130;
#endif
break;
case MSG_PVT:
Debug("MSG_PVT");
havePvtMsg = true;
// position
_last_pos_time = _buffer.pvt.itow;
state.location.lng = _buffer.pvt.lon;
state.location.lat = _buffer.pvt.lat;
state.location.alt = _buffer.pvt.h_msl / 10;
switch (_buffer.pvt.fix_type)
{
case 0:
state.status = AP_GPS::NO_FIX;
break;
case 1:
state.status = AP_GPS::NO_FIX;
break;
case 2:
state.status = AP_GPS::GPS_OK_FIX_2D;
break;
case 3:
state.status = AP_GPS::GPS_OK_FIX_3D;
if (_buffer.pvt.flags & 0b00000010) // diffsoln
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
if (_buffer.pvt.flags & 0b01000000) // carrsoln - float
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
if (_buffer.pvt.flags & 0b10000000) // carrsoln - fixed
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 4:
gcs().send_text(MAV_SEVERITY_INFO,
"Unexpected state %d", _buffer.pvt.flags);
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 5:
state.status = AP_GPS::NO_FIX;
break;
default:
state.status = AP_GPS::NO_FIX;
break;
}
next_fix = state.status;
_new_position = true;
state.horizontal_accuracy = _buffer.pvt.h_acc*1.0e-3f;
state.vertical_accuracy = _buffer.pvt.v_acc*1.0e-3f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
// SVs
state.num_sats = _buffer.pvt.num_sv;
// velocity
_last_vel_time = _buffer.pvt.itow;
state.ground_speed = _buffer.pvt.gspeed*0.001f; // m/s
state.ground_course = wrap_360(_buffer.pvt.head_mot * 1.0e-5f); // Heading 2D deg * 100000
state.have_vertical_velocity = true;
state.velocity.x = _buffer.pvt.velN * 0.001f;
state.velocity.y = _buffer.pvt.velE * 0.001f;
state.velocity.z = _buffer.pvt.velD * 0.001f;
state.have_speed_accuracy = true;
state.speed_accuracy = _buffer.pvt.s_acc*0.001f;
_new_speed = true;
// dop
if(noReceivedHdop) {
state.hdop = _buffer.pvt.p_dop;
state.vdop = _buffer.pvt.p_dop;
}
state.last_gps_time_ms = AP_HAL::millis();
// time
state.time_week_ms = _buffer.pvt.itow;
#if UBLOX_FAKE_3DLOCK
state.location.lng = 1491652300L;
state.location.lat = -353632610L;
state.location.alt = 58400;
state.vertical_accuracy = 0;
state.horizontal_accuracy = 0;
state.status = AP_GPS::GPS_OK_FIX_3D;
state.num_sats = 10;
state.time_week = 1721;
state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
state.last_gps_time_ms = AP_HAL::millis();
state.hdop = 130;
state.speed_accuracy = 0;
next_fix = state.status;
#endif
break;
case MSG_VELNED:
Debug("MSG_VELNED");
if (havePvtMsg) {
_unconfigured_messages |= CONFIG_RATE_VELNED;
break;
}
_last_vel_time = _buffer.velned.time;
state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s
state.ground_course = wrap_360(_buffer.velned.heading_2d * 1.0e-5f); // Heading 2D deg * 100000
state.have_vertical_velocity = true;
state.velocity.x = _buffer.velned.ned_north * 0.01f;
state.velocity.y = _buffer.velned.ned_east * 0.01f;
state.velocity.z = _buffer.velned.ned_down * 0.01f;
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
state.ground_speed = norm(state.velocity.y, state.velocity.x);
state.have_speed_accuracy = true;
state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
#if UBLOX_FAKE_3DLOCK
state.speed_accuracy = 0;
#endif
_new_speed = true;
break;
case MSG_NAV_SVINFO:
{
Debug("MSG_NAV_SVINFO\n");
static const uint8_t HardwareGenerationMask = 0x07;
_hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
switch (_hardware_generation) {
case UBLOX_5:
case UBLOX_6:
// only 7 and newer support CONFIG_GNSS
_unconfigured_messages &= ~CONFIG_GNSS;
break;
case UBLOX_7:
case UBLOX_M8:
#if UBLOX_SPEED_CHANGE
port->begin(4000000U);
Debug("Changed speed to 4Mhz for SPI-driven UBlox\n");
#endif
break;
default:
hal.console->printf("Wrong Ublox Hardware Version%u\n", _hardware_generation);
break;
};
_unconfigured_messages &= ~CONFIG_VERSION;
/* We don't need that anymore */
_configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
break;
}
default:
Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
if (++_disable_counter == 0) {
Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
_configure_message_rate(CLASS_NAV, _msg_id, 0);
}
return false;
}
// we only return true when we get new position and speed data
// this ensures we don't use stale data
if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
_new_speed = _new_position = false;
return true;
}
return false;
}
2. calc_blended_state();
void AP_GPS::calc_blended_state(void)
{
// initialise the blended states so we can accumulate the results using the weightings for each GPS receiver
state[GPS_BLENDED_INSTANCE].instance = GPS_BLENDED_INSTANCE;
state[GPS_BLENDED_INSTANCE].status = NO_FIX;
state[GPS_BLENDED_INSTANCE].time_week_ms = 0;
state[GPS_BLENDED_INSTANCE].time_week = 0;
state[GPS_BLENDED_INSTANCE].ground_speed = 0.0f;
state[GPS_BLENDED_INSTANCE].ground_course = 0.0f;
state[GPS_BLENDED_INSTANCE].hdop = GPS_UNKNOWN_DOP;
state[GPS_BLENDED_INSTANCE].vdop = GPS_UNKNOWN_DOP;
state[GPS_BLENDED_INSTANCE].num_sats = 0;
state[GPS_BLENDED_INSTANCE].velocity.zero();
state[GPS_BLENDED_INSTANCE].speed_accuracy = 1e6f;
state[GPS_BLENDED_INSTANCE].horizontal_accuracy = 1e6f;
state[GPS_BLENDED_INSTANCE].vertical_accuracy = 1e6f;
state[GPS_BLENDED_INSTANCE].have_vertical_velocity = false;
state[GPS_BLENDED_INSTANCE].have_speed_accuracy = false;
state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = false;
state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = false;
memset(&state[GPS_BLENDED_INSTANCE].location, 0, sizeof(state[GPS_BLENDED_INSTANCE].location));
_blended_antenna_offset.zero();
_blended_lag_sec = 0;
timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = 0;
timing[GPS_BLENDED_INSTANCE].last_message_time_ms = 0;
// combine the states into a blended solution
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++)
{
// use the highest status
if (state[i].status > state[GPS_BLENDED_INSTANCE].status)
{
state[GPS_BLENDED_INSTANCE].status = state[i].status;
}
// calculate a blended average velocity
state[GPS_BLENDED_INSTANCE].velocity += state[i].velocity * _blend_weights[i];
// report the best valid accuracies and DOP metrics
if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f && state[i].horizontal_accuracy < state[GPS_BLENDED_INSTANCE].horizontal_accuracy) {
state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = true;
state[GPS_BLENDED_INSTANCE].horizontal_accuracy = state[i].horizontal_accuracy;
}
if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f && state[i].vertical_accuracy < state[GPS_BLENDED_INSTANCE].vertical_accuracy) {
state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = true;
state[GPS_BLENDED_INSTANCE].vertical_accuracy = state[i].vertical_accuracy;
}
if (state[i].have_vertical_velocity) {
state[GPS_BLENDED_INSTANCE].have_vertical_velocity = true;
}
if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f && state[i].speed_accuracy < state[GPS_BLENDED_INSTANCE].speed_accuracy) {
state[GPS_BLENDED_INSTANCE].have_speed_accuracy = true;
state[GPS_BLENDED_INSTANCE].speed_accuracy = state[i].speed_accuracy;
}
if (state[i].hdop > 0 && state[i].hdop < state[GPS_BLENDED_INSTANCE].hdop) {
state[GPS_BLENDED_INSTANCE].hdop = state[i].hdop;
}
if (state[i].vdop > 0 && state[i].vdop < state[GPS_BLENDED_INSTANCE].vdop) {
state[GPS_BLENDED_INSTANCE].vdop = state[i].vdop;
}
if (state[i].num_sats > 0 && state[i].num_sats > state[GPS_BLENDED_INSTANCE].num_sats) {
state[GPS_BLENDED_INSTANCE].num_sats = state[i].num_sats;
}
// report a blended average GPS antenna position
Vector3f temp_antenna_offset = _antenna_offset[i];
temp_antenna_offset *= _blend_weights[i];
_blended_antenna_offset += temp_antenna_offset;
// blend the timing data
if (timing[i].last_fix_time_ms > timing[GPS_BLENDED_INSTANCE].last_fix_time_ms)
{
timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = timing[i].last_fix_time_ms;
}
if (timing[i].last_message_time_ms > timing[GPS_BLENDED_INSTANCE].last_message_time_ms)
{
timing[GPS_BLENDED_INSTANCE].last_message_time_ms = timing[i].last_message_time_ms;
}
}
/*
* Calculate an instantaneous weighted/blended average location from the available GPS instances and store in the _output_state.
* This will be statistically the most likely location, but will be not stable enough for direct use by the autopilot.
*/
// Use the GPS with the highest weighting as the reference position
float best_weight = 0.0f;
uint8_t best_index = 0;
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (_blend_weights[i] > best_weight) {
best_weight = _blend_weights[i];
best_index = i;
state[GPS_BLENDED_INSTANCE].location = state[i].location;
}
}
// Calculate the weighted sum of horizontal and vertical position offsets relative to the reference position
Vector2f blended_NE_offset_m;
float blended_alt_offset_cm = 0.0f;
blended_NE_offset_m.zero();
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (_blend_weights[i] > 0.0f && i != best_index) {
blended_NE_offset_m += location_diff(state[GPS_BLENDED_INSTANCE].location, state[i].location) * _blend_weights[i];
blended_alt_offset_cm += (float)(state[i].location.alt - state[GPS_BLENDED_INSTANCE].location.alt) * _blend_weights[i];
}
}
// Add the sum of weighted offsets to the reference location to obtain the blended location
location_offset(state[GPS_BLENDED_INSTANCE].location, blended_NE_offset_m.x, blended_NE_offset_m.y);
state[GPS_BLENDED_INSTANCE].location.alt += (int)blended_alt_offset_cm;
// Calculate ground speed and course from blended velocity vector
state[GPS_BLENDED_INSTANCE].ground_speed = norm(state[GPS_BLENDED_INSTANCE].velocity.x, state[GPS_BLENDED_INSTANCE].velocity.y);
state[GPS_BLENDED_INSTANCE].ground_course = wrap_360(degrees(atan2f(state[GPS_BLENDED_INSTANCE].velocity.y, state[GPS_BLENDED_INSTANCE].velocity.x)));
/*
* The blended location in _output_state.location is not stable enough to be used by the autopilot
* as it will move around as the relative accuracy changes. To mitigate this effect a low-pass filtered
* offset from each GPS location to the blended location is calculated and the filtered offset is applied
* to each receiver.
*/
// Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
// A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering
float alpha[GPS_MAX_RECEIVERS] = {};
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (state[i].last_gps_time_ms - _last_time_updated[i] > 0) {
float min_alpha = constrain_float(_omega_lpf * 0.001f * (float)(state[i].last_gps_time_ms - _last_time_updated[i]), 0.0f, 1.0f);
if (_blend_weights[i] > min_alpha) {
alpha[i] = min_alpha / _blend_weights[i];
} else {
alpha[i] = 1.0f;
}
_last_time_updated[i] = state[i].last_gps_time_ms;
}
}
// Calculate the offset from each GPS solution to the blended solution
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
_NE_pos_offset_m[i] = location_diff(state[i].location, state[GPS_BLENDED_INSTANCE].location) * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]);
_hgt_offset_cm[i] = (float)(state[GPS_BLENDED_INSTANCE].location.alt - state[i].location.alt) * alpha[i] + _hgt_offset_cm[i] * (1.0f - alpha[i]);
}
// Calculate a corrected location for each GPS
Location corrected_location[GPS_MAX_RECEIVERS];
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
corrected_location[i] = state[i].location;
location_offset(corrected_location[i], _NE_pos_offset_m[i].x, _NE_pos_offset_m[i].y);
corrected_location[i].alt += (int)(_hgt_offset_cm[i]);
}
// If the GPS week is the same then use a blended time_week_ms
// If week is different, then use time stamp from GPS with largest weighting
// detect inconsistent week data
uint8_t last_week_instance = 0;
bool weeks_consistent = true;
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (last_week_instance == 0 && _blend_weights[i] > 0) {
// this is our first valid sensor week data
last_week_instance = state[i].time_week;
} else if (last_week_instance != 0 && _blend_weights[i] > 0 && last_week_instance != state[i].time_week) {
// there is valid sensor week data that is inconsistent
weeks_consistent = false;
}
}
// calculate output
if (!weeks_consistent) {
// use data from highest weighted sensor
state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week;
state[GPS_BLENDED_INSTANCE].time_week_ms = state[best_index].time_week_ms;
} else {
// use week number from highest weighting GPS (they should all have the same week number)
state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week;
// calculate a blended value for the number of ms lapsed in the week
double temp_time_0 = 0.0;
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (_blend_weights[i] > 0.0f) {
temp_time_0 += (double)state[i].time_week_ms * (double)_blend_weights[i];
}
}
state[GPS_BLENDED_INSTANCE].time_week_ms = (uint32_t)temp_time_0;
}
// calculate a blended value for the timing data and lag
double temp_time_1 = 0.0;
double temp_time_2 = 0.0;
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (_blend_weights[i] > 0.0f) {
temp_time_1 += (double)timing[i].last_fix_time_ms * (double) _blend_weights[i];
temp_time_2 += (double)timing[i].last_message_time_ms * (double)_blend_weights[i];
float gps_lag_sec = 0;
get_lag(i, gps_lag_sec);
_blended_lag_sec += gps_lag_sec * _blend_weights[i];
}
}
timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = (uint32_t)temp_time_1;
timing[GPS_BLENDED_INSTANCE].last_message_time_ms = (uint32_t)temp_time_2;
}
具體上面的代碼思路可以看下圖