Ardupilot 串口代码学习
目录
文章目录
- 目录
- 摘要
- 1.串口初始化
- 1.usb串口初始化
- 2.其他串口初始化
- 1.如何设置波特率和协议
- 2. GPS串口初始化
- 3. GPS数据更新
- 1. update_instance（）
摘要
本节主要学习Ardupilot的串口资源代码，欢迎批评指正！！！
1.串口初始化
1.usb串口初始化
void Copter::init_ardupilot()
{
//初始化USB------------------initialise serial port
serial_manager.init_console(); //usb终端初始化
}
void AP_SerialManager::init_console()
{
//初始化终端立即设定字节和波特率-----------initialise console immediately at default size and baud
state[0].uart = hal.uartA; // serial0, uartA, always console
state[0].uart->begin(AP_SERIALMANAGER_CONSOLE_BAUD, //波特率---115200
AP_SERIALMANAGER_CONSOLE_BUFSIZE_RX, //接收缓冲区---128
AP_SERIALMANAGER_CONSOLE_BUFSIZE_TX); //发送缓冲区---128
}
void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
thread_init(); //线程初始化
if (sdef.serial == nullptr)
{
return;
}
uint16_t min_tx_buffer = 4096;
uint16_t min_rx_buffer = 1024;
// on PX4 we have enough memory to have a larger transmit and
// receive buffer for all ports. This means we don't get delays
// while waiting to write GPS config packets
if (txS < min_tx_buffer)
{
txS = min_tx_buffer;
}
if (rxS < min_rx_buffer)
{
rxS = min_rx_buffer;
}
/*
allocate the read buffer
we allocate buffers before we successfully open the device as we
want to allocate in the early stages of boot, and cause minimum
thrashing of the heap once we are up. The ttyACM0 driver may not
connect for some time after boot
*/
while (_in_timer)
{
hal.scheduler->delay(1);
}
if (rxS != _readbuf.get_size())
{
_initialised = false;
_readbuf.set_size(rxS);
}
bool clear_buffers = false;
if (b != 0)
{
// clear buffers on baudrate change, but not on the console (which is usually USB)
if (_baudrate != b && hal.console != this)
{
clear_buffers = true;
}
_baudrate = b;
}
if (clear_buffers)
{
_readbuf.clear();
}
if (rx_bounce_buf == nullptr) {
rx_bounce_buf = (uint8_t *)hal.util->malloc_type(RX_BOUNCE_BUFSIZE, AP_HAL::Util::MEM_DMA_SAFE);
}
if (tx_bounce_buf == nullptr) {
tx_bounce_buf = (uint8_t *)hal.util->malloc_type(TX_BOUNCE_BUFSIZE, AP_HAL::Util::MEM_DMA_SAFE);
chVTObjectInit(&tx_timeout);
tx_bounce_buf_ready = true;
}
/*
allocate the write buffer
*/
while (_in_timer)
{
hal.scheduler->delay(1);
}
if (txS != _writebuf.get_size())
{
_initialised = false;
_writebuf.set_size(txS);
}
if (clear_buffers)
{
_writebuf.clear();
}
if (sdef.is_usb)
{
#ifdef HAVE_USB_SERIAL
/*
* Initializes a serial-over-USB CDC driver.
*/
if (!_device_initialised)
{
sduObjectInit((SerialUSBDriver*)sdef.serial);
sduStart((SerialUSBDriver*)sdef.serial, &serusbcfg);
/*
* Activates the USB driver and then the USB bus pull-up on D+.
* Note, a delay is inserted in order to not have to disconnect the cable
* after a reset.
*/
usbDisconnectBus(serusbcfg.usbp);
hal.scheduler->delay_microseconds(1500);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
_device_initialised = true;
}
#endif
} else
{
#if HAL_USE_SERIAL == TRUE
if (_baudrate != 0) {
bool was_initialised = _device_initialised;
//setup Rx DMA
if(!_device_initialised) {
if(sdef.dma_rx) {
rxdma = STM32_DMA_STREAM(sdef.dma_rx_stream_id);
chSysLock();
bool dma_allocated = dmaStreamAllocate(rxdma,
12, //IRQ Priority
(stm32_dmaisr_t)rxbuff_full_irq,
(void *)this);
osalDbgAssert(!dma_allocated, "stream already allocated");
chSysUnlock();
#if defined(STM32F7)
dmaStreamSetPeripheral(rxdma, &((SerialDriver*)sdef.serial)->usart->RDR);
#else
dmaStreamSetPeripheral(rxdma, &((SerialDriver*)sdef.serial)->usart->DR);
#endif // STM32F7
}
if (sdef.dma_tx) {
// we only allow for sharing of the TX DMA channel, not the RX
// DMA channel, as the RX side is active all the time, so
// cannot be shared
dma_handle = new Shared_DMA(sdef.dma_tx_stream_id,
SHARED_DMA_NONE,
FUNCTOR_BIND_MEMBER(&UARTDriver::dma_tx_allocate, void, Shared_DMA *),
FUNCTOR_BIND_MEMBER(&UARTDriver::dma_tx_deallocate, void, Shared_DMA *));
}
_device_initialised = true;
}
sercfg.speed = _baudrate;
if (!sdef.dma_tx && !sdef.dma_rx) {
sercfg.cr1 = 0;
sercfg.cr3 = 0;
} else {
if (sdef.dma_rx) {
sercfg.cr1 = USART_CR1_IDLEIE;
sercfg.cr3 = USART_CR3_DMAR;
}
if (sdef.dma_tx) {
sercfg.cr3 |= USART_CR3_DMAT;
}
}
sercfg.cr2 = USART_CR2_STOP1_BITS;
sercfg.irq_cb = rx_irq_cb;
sercfg.ctx = (void*)this;
sdStart((SerialDriver*)sdef.serial, &sercfg);
if(sdef.dma_rx) {
//Configure serial driver to skip handling RX packets
//because we will handle them via DMA
((SerialDriver*)sdef.serial)->usart->CR1 &= ~USART_CR1_RXNEIE;
//Start DMA
if(!was_initialised) {
uint32_t dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE;
dmamode |= STM32_DMA_CR_CHSEL(STM32_DMA_GETCHANNEL(sdef.dma_rx_stream_id,
sdef.dma_rx_channel_id));
dmamode |= STM32_DMA_CR_PL(0);
dmaStreamSetMemory0(rxdma, rx_bounce_buf);
dmaStreamSetTransactionSize(rxdma, RX_BOUNCE_BUFSIZE);
dmaStreamSetMode(rxdma, dmamode | STM32_DMA_CR_DIR_P2M |
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
dmaStreamEnable(rxdma);
}
}
}
#endif // HAL_USE_SERIAL
}
if (_writebuf.get_size() && _readbuf.get_size()) {
_initialised = true;
}
_uart_owner_thd = chThdGetSelfX();
// setup flow control
set_flow_control(_flow_control);
if (serial_num == 0 && _initialised) {
#ifndef HAL_STDOUT_SERIAL
// setup hal.console to take printf() output
vprintf_console_hook = hal_console_vprintf;
#endif
}
}
2.其他串口初始化
//初始化串行端口-----initialise serial ports
serial_manager.init();
void AP_SerialManager::init()
{
//初始化指针到串口----------------------initialise pointers to serial ports
state[1].uart = hal.uartC; // serial1, uartC, normally telem1------uart2
state[2].uart = hal.uartD; // serial2, uartD, normally telem2------uart3
state[3].uart = hal.uartB; // serial3, uartB, normally 1st GPS-----uart1
state[4].uart = hal.uartE; // serial4, uartE, normally 2nd GPS-----uart4
state[5].uart = hal.uartF; // serial5------------------------------uart6
state[6].uart = hal.uartG; // serial6------------------------------uart7
if (state[0].uart == nullptr) //如果usb串口初始化完成了，就不进行初始化了，否则进行初始化
{
init_console(); //初始化终端
}
//初始化串行端口--------------initialise serial ports
for (uint8_t i=1; ibegin(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;
}
}
}
}
1.如何设置波特率和协议
通过参数设定波特率，串口具体的功能
const AP_Param::GroupInfo AP_SerialManager::var_info[] = {
/***************************************************************************************************************/
//设定usb的波特率
// @参数----Param: 0_BAUD
// @名字----DisplayName: Serial0 baud rate
// @描述----Description: The baud rate used on the USB console. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @值-----Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,460:460800,500:500000,921:921600,1500:1500000
// @用户----User: Standard
AP_GROUPINFO("0_BAUD", 0, AP_SerialManager, state[0].baud, AP_SERIALMANAGER_CONSOLE_BAUD/1000), //设定usb串口波特率115200
//设定USB的协议
// @Param: 0_PROTOCOL
// @DisplayName: Console protocol selection
// @Description: Control what protocol to use on the console.
// @Values: 1:MAVlink1, 2:MAVLink2
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("0_PROTOCOL", 11, AP_SerialManager, state[0].protocol, SerialProtocol_MAVLink2),
/***************************************************************************************************************/
//设定数传1参数
// @Param: 1_PROTOCOL
// @DisplayName: Telem1 protocol selection
// @Description: Control what protocol to use on the Telem1 port. Note that the Frsky options require external converter hardware. See the wiki for details.
// @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("1_PROTOCOL", 1, AP_SerialManager, state[1].protocol, SerialProtocol_MAVLink),
//设定数传1波特率57600
// @Param: 1_BAUD
// @DisplayName: Telem1 Baud Rate
// @Description: The baud rate used on the Telem1 port. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard
AP_GROUPINFO("1_BAUD", 2, AP_SerialManager, state[1].baud, AP_SERIALMANAGER_MAVLINK_BAUD/1000), //设定数传的波特率是57600
/***************************************************************************************************************/
//设定数传2参数
// @Param: 2_PROTOCOL
// @DisplayName: Telemetry 2 protocol selection
// @Description: Control what protocol to use on the Telem2 port. Note that the Frsky options require external converter hardware. See the wiki for details.
// @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("2_PROTOCOL", 3, AP_SerialManager, state[2].protocol, SerialProtocol_MAVLink),
//设定数传2波特率57600
// @Param: 2_BAUD
// @DisplayName: Telemetry 2 Baud Rate
// @Description: The baud rate of the Telem2 port. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard
AP_GROUPINFO("2_BAUD", 4, AP_SerialManager, state[2].baud, AP_SERIALMANAGER_MAVLINK_BAUD/1000),
/***************************************************************************************************************/
//设定gps1协议
// @Param: 3_PROTOCOL
// @DisplayName: Serial 3 (GPS) protocol selection
// @Description: Control what protocol Serial 3 (GPS) should be used for. Note that the Frsky options require external converter hardware. See the wiki for details.
// @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("3_PROTOCOL", 5, AP_SerialManager, state[3].protocol, SerialProtocol_GPS),
//设定波特率是38400
// @Param: 3_BAUD
// @DisplayName: Serial 3 (GPS) Baud Rate
// @Description: The baud rate used for the Serial 3 (GPS). The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard
AP_GROUPINFO("3_BAUD", 6, AP_SerialManager, state[3].baud, AP_SERIALMANAGER_GPS_BAUD/1000),
/***************************************************************************************************************/
//设定gps2协议
// @Param: 4_PROTOCOL
// @DisplayName: Serial4 protocol selection
// @Description: Control what protocol Serial4 port should be used for. Note that the Frsky options require external converter hardware. See the wiki for details.
// @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("4_PROTOCOL", 7, AP_SerialManager, state[4].protocol, SerialProtocol_GPS),
//设定波特率是38400
// @Param: 4_BAUD
// @DisplayName: Serial 4 Baud Rate
// @Description: The baud rate used for Serial4. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard
AP_GROUPINFO("4_BAUD", 8, AP_SerialManager, state[4].baud, AP_SERIALMANAGER_GPS_BAUD/1000),
/***************************************************************************************************************/
// @Param: 5_PROTOCOL
// @DisplayName: Serial5 protocol selection
// @Description: Control what protocol Serial5 port should be used for. Note that the Frsky options require external converter hardware. See the wiki for details.
// @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("5_PROTOCOL", 9, AP_SerialManager, state[5].protocol, SERIAL5_PROTOCOL),
// @Param: 5_BAUD
// @DisplayName: Serial 5 Baud Rate
// @Description: The baud rate used for Serial5. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard
AP_GROUPINFO("5_BAUD", 10, AP_SerialManager, state[5].baud, SERIAL5_BAUD),
/***************************************************************************************************************/
// index 11 used by 0_PROTOCOL
// @Param: 6_PROTOCOL
// @DisplayName: Serial6 protocol selection
// @Description: Control what protocol Serial6 port should be used for. Note that the Frsky options require external converter hardware. See the wiki for details.
// @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("6_PROTOCOL", 12, AP_SerialManager, state[6].protocol, SERIAL6_PROTOCOL),
// @Param: 6_BAUD
// @DisplayName: Serial 6 Baud Rate
// @Description: The baud rate used for Serial6. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard
AP_GROUPINFO("6_BAUD", 13, AP_SerialManager, state[6].baud, SERIAL6_BAUD),
AP_GROUPEND
};
2. GPS串口初始化
void AP_SerialManager::init()
{
------------------------------------------------------------------------------------------------------------------------------------
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;
------------------------------------------------------------------------------------------------------------------------------------
}
uint32_t AP_SerialManager::map_baudrate(int32_t rate) const
{
if (rate <= 0)
{
rate = 57;
}
switch (rate)
{
case 1: return 1200;
case 2: return 2400;
case 4: return 4800;
case 9: return 9600;
case 19: return 19200;
case 38: return 38400;
case 57: return 57600;
case 100: return 100000;
case 111: return 111100;
case 115: return 115200;
case 230: return 230400;
case 460: return 460800;
case 500: return 500000;
case 921: return 921600;
case 1500: return 1500000;
}
if (rate > 2000)
{
// assume it is a direct baudrate. This allows for users to
// set an exact baudrate as long as it is over 2000 baud
return (uint32_t)rate;
}
// otherwise allow any other kbaud rate
return rate*1000;
}
3. GPS数据更新
SCHED_TASK(update_GPS, 50, 200), //GPS数据更新
void AP_GPS::update(void)
{
for (uint8_t i=0; i 0)
{
_blend_health_counter--;
}
// stop blending if unhealthy
if (_blend_health_counter >= 50)
{
_output_is_blended = false;
}
} else
{
_output_is_blended = false;
_blend_health_counter = 0;
}
if (_output_is_blended)
{
// Use the weighting to calculate blended GPS states
calc_blended_state();
// set primary to the virtual instance
primary_instance = GPS_BLENDED_INSTANCE;
} else
{
// use switch logic to find best GPS
uint32_t now = AP_HAL::millis();
if (_auto_switch >= 1)
{
// handling switching away from blended GPS
if (primary_instance == GPS_BLENDED_INSTANCE)
{
primary_instance = 0;
for (uint8_t i=1; i state[primary_instance].status) ||
((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats)))
{
primary_instance = i;
_last_instance_swap_ms = now;
}
}
} else {
// handle switch between real GPSs
for (uint8_t i=0; i state[primary_instance].status)
{
// we have a higher status lock, or primary is set to the blended GPS, change GPS
primary_instance = i;
_last_instance_swap_ms = now;
continue;
}
bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats)
{
bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
(another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000))
{
// this GPS has more satellites than the
// current primary, switch primary. Once we switch we will
// then tend to stick to the new GPS as primary. We don't
// want to switch too often as it will look like a
// position shift to the controllers.
primary_instance = i;
_last_instance_swap_ms = now;
}
}
}
}
} else
{
// AUTO_SWITCH is 0 so no switching of GPSs
primary_instance = 0;
}
// copy the primary instance to the blended instance in case it is enabled later
state[GPS_BLENDED_INSTANCE] = state[primary_instance];
_blended_antenna_offset = _antenna_offset[primary_instance];
}
// 通知LED来显示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（）
void AP_GPS::update(void)
{
for (uint8_t i=0; i 0)
{
_blend_health_counter--;
}
//如果有不健康的就阻止---- stop blending if unhealthy
if (_blend_health_counter >= 50)
{
_output_is_blended = false;
}
} else
{
_output_is_blended = false;
_blend_health_counter = 0;
}
if (_output_is_blended)
{
//使用权重计算混合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 state[primary_instance].status) ||
((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats)))
{
primary_instance = i;
_last_instance_swap_ms = now;
}
}
} else {
// handle switch between real GPSs
for (uint8_t i=0; i state[primary_instance].status)
{
// we have a higher status lock, or primary is set to the blended GPS, change GPS
primary_instance = i;
_last_instance_swap_ms = now;
continue;
}
bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats)
{
bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
(another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000))
{
// this GPS has more satellites than the
// current primary, switch primary. Once we switch we will
// then tend to stick to the new GPS as primary. We don't
// want to switch too often as it will look like a
// position shift to the controllers.
primary_instance = i;
_last_instance_swap_ms = now;
}
}
}
}
} else
{
// AUTO_SWITCH is 0 so no switching of GPSs
primary_instance = 0;
}
// copy the primary instance to the blended instance in case it is enabled later
state[GPS_BLENDED_INSTANCE] = state[primary_instance];
_blended_antenna_offset = _antenna_offset[primary_instance];
}
// 通知LED来显示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;
}