ardupilot GPS代码分析
目录
文章目录
- 目录
- 摘要
- 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
detect_instance(instance);
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;
}
具体上面的代码思路可以看下图
