ardupilot 串口再认识
目录
文章目录
- 目录
- 摘要
- 1.串口顺序定义
- 2.串口打印函数
- 1.终端打印函数hal.console->printf()
摘要
本节继续深入研究ardupilot代码的串口代码实现过程，这里主要分析：如何实现串口顺序定义，串口打印函数。
1.串口顺序定义
进入最底层的AP_HAL_ChiBios/hwdef/fmuv5文件夹，你可以看到：
UART_ORDER OTG1 USART1 USART2 USART3 UART4 USART6 UART7
上面这段代码就是定义我们的串口顺序，这个顺序是对应哪里呢？看下面的就可以明白。
class AP_HAL::HAL {
public:
HAL(AP_HAL::UARTDriver* _uartA, // console
AP_HAL::UARTDriver* _uartB, // 1st GPS
AP_HAL::UARTDriver* _uartC, // telem1
AP_HAL::UARTDriver* _uartD, // telem2
AP_HAL::UARTDriver* _uartE, // 2nd GPS
AP_HAL::UARTDriver* _uartF, // extra1
AP_HAL::UARTDriver* _uartG, // extra2
AP_HAL::I2CDeviceManager* _i2c_mgr,
AP_HAL::SPIDeviceManager* _spi,
AP_HAL::AnalogIn* _analogin,
AP_HAL::Storage* _storage,
AP_HAL::UARTDriver* _console,
AP_HAL::GPIO* _gpio,
AP_HAL::RCInput* _rcin,
AP_HAL::RCOutput* _rcout,
AP_HAL::Scheduler* _scheduler,
AP_HAL::Util* _util,
AP_HAL::OpticalFlow *_opticalflow,
#if HAL_WITH_UAVCAN
AP_HAL::CANManager* _can_mgr[MAX_NUMBER_OF_CAN_DRIVERS])
#else
AP_HAL::CANManager** _can_mgr)
#endif
:
uartA(_uartA),
uartB(_uartB),
uartC(_uartC),
uartD(_uartD),
uartE(_uartE),
uartF(_uartF),
uartG(_uartG),
// uartH(_uartH),
i2c_mgr(_i2c_mgr),
spi(_spi),
analogin(_analogin),
storage(_storage),
console(_console),
gpio(_gpio),
rcin(_rcin),
rcout(_rcout),
scheduler(_scheduler),
util(_util),
opticalflow(_opticalflow)
{
#if HAL_WITH_UAVCAN
if (_can_mgr == nullptr) {
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++)
can_mgr[i] = nullptr;
} else {
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++)
can_mgr[i] = _can_mgr[i];
}
#endif
AP_HAL::init();
}
struct Callbacks {
virtual void setup() = 0;
virtual void loop() = 0;
};
struct FunCallbacks : public Callbacks {
FunCallbacks(void (*setup_fun)(void), void (*loop_fun)(void));
void setup() override { _setup(); }
void loop() override { _loop(); }
private:
void (*_setup)(void);
void (*_loop)(void);
};
virtual void run(int argc, char * const argv[], Callbacks* callbacks) const = 0;
AP_HAL::UARTDriver* uartA; //定义串口A
AP_HAL::UARTDriver* uartB;//定义串口B
AP_HAL::UARTDriver* uartC;//定义串口C
AP_HAL::UARTDriver* uartD;//定义串口D
AP_HAL::UARTDriver* uartE;//定义串口E
AP_HAL::UARTDriver* uartF;//定义串口F
AP_HAL::UARTDriver* uartG;//定义串口G
//增加串口
AP_HAL::I2CDeviceManager* i2c_mgr;
AP_HAL::SPIDeviceManager* spi;
AP_HAL::AnalogIn* analogin;
AP_HAL::Storage* storage;
AP_HAL::UARTDriver* console;
AP_HAL::GPIO* gpio;
AP_HAL::RCInput* rcin;
AP_HAL::RCOutput* rcout;
AP_HAL::Scheduler* scheduler;
AP_HAL::Util *util;
AP_HAL::OpticalFlow *opticalflow;
#if HAL_WITH_UAVCAN
AP_HAL::CANManager* can_mgr[MAX_NUMBER_OF_CAN_DRIVERS];
#else
AP_HAL::CANManager** can_mgr;
#endif
};
那么这些串口是怎么实现一一对应的呢？
uartA 、 uartB、 uartC、 uartD、 uartE、 uartF、 uartG
OTG1 USART1 USART2 USART3 UART4 USART6 UART7
上面这个图片是通过脚本生成的，也就是说串口实现对应的代码在脚本中，具体就是解析hwdef.h这个脚本对应的文件chibios_hwdef.py
2.串口打印函数
1.终端打印函数hal.console->printf()
AP_HAL::UARTDriver* console;
可以看出UARTDriver这个类是派生类，来自基类BetterStream
class AP_HAL::UARTDriver : public AP_HAL::BetterStream
所以printf()函数是调用了父类的实现过程。
virtual void printf(const char *, …) FMT_PRINTF(2, 3); //format (printf, 2, 3)告诉编译器，FMT_PRINTF相当于printf的format，而可变参数是从my_printf的第3个参数开始。
具体代码实现过程是：
void AP_HAL::BetterStream::printf(const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
vprintf(fmt, ap); //最终函数执行在这
va_end(ap);
}
void AP_HAL::BetterStream::vprintf(const char *fmt, va_list ap)
{
print_vprintf(this, fmt, ap);
}
void print_vprintf(AP_HAL::BetterStream *s, const char *fmt, va_list ap)
{
unsigned char c; /* holds a char from the format string */
uint16_t flags;
unsigned char width;
unsigned char prec;
unsigned char buf[23];
for (;;) {
/*
* Process non-format characters
*/
for (;;) {
c = *fmt++;
if (!c) {
return;
}
if (c == '%') {
c = *fmt++;
if (c != '%') {
break;
}
}
/* emit cr before lf to make most terminals happy */
if (c == '\n') {
s->write('\r');
}
s->write(c);
}
flags = 0;
width = 0;
prec = 0;
/*
* Process format adjustment characters, precision, width.
*/
do {
if (flags < FL_WIDTH) {
switch (c) {
case '0':
flags |= FL_ZFILL;
continue;
case '+':
flags |= FL_PLUS;
FALLTHROUGH;
case ' ':
flags |= FL_SPACE;
continue;
case '-':
flags |= FL_LPAD;
continue;
case '#':
flags |= FL_ALT;
continue;
}
}
if (flags < FL_LONG) {
if (c >= '0' && c <= '9') {
c -= '0';
if (flags & FL_PREC) {
prec = 10*prec + c;
continue;
}
width = 10*width + c;
flags |= FL_WIDTH;
continue;
}
if (c == '.') {
if (flags & FL_PREC) {
return;
}
flags |= FL_PREC;
continue;
}
if (c == 'l') {
flags |= FL_LONG;
continue;
}
if (c == 'h') {
continue;
}
} else if ((flags & FL_LONG) && c == 'l') {
flags |= FL_LONGLONG;
continue;
}
break;
} while ((c = *fmt++) != 0);
/*
* Handle floating-point formats E, F, G, e, f, g.
*/
if (c >= 'E' && c <= 'G') {
flags |= FL_FLTUPP;
c += 'e' - 'E';
goto flt_oper;
} else if (c >= 'e' && c <= 'g') {
int exp; /* exponent of master decimal digit */
int n;
unsigned char vtype; /* result of float value parse */
unsigned char sign; /* sign character (or 0) */
unsigned char ndigs;
flags &= ~FL_FLTUPP;
flt_oper:
float value = va_arg(ap,double);
if (!(flags & FL_PREC)) {
prec = 6;
}
flags &= ~(FL_FLTEXP | FL_FLTFIX);
if (c == 'e') {
flags |= FL_FLTEXP;
} else if (c == 'f') {
flags |= FL_FLTFIX;
} else if (prec > 0) {
prec -= 1;
}
if ((flags & FL_FLTFIX) && fabsf(value) > 9999999) {
flags = (flags & ~FL_FLTFIX) | FL_FLTEXP;
}
if (flags & FL_FLTFIX) {
vtype = 7; /* 'prec' arg for 'ftoa_engine' */
ndigs = prec < 60 ? prec + 1 : 60;
} else {
if (prec > 10) {
prec = 10;
}
vtype = prec;
ndigs = 0;
}
memset(buf, 0, sizeof(buf));
exp = ftoa_engine(value, (char *)buf, vtype, ndigs);
vtype = buf[0];
sign = 0;
if ((vtype & FTOA_MINUS) && !(vtype & FTOA_NAN))
sign = '-';
else if (flags & FL_PLUS)
sign = '+';
else if (flags & FL_SPACE)
sign = ' ';
if (vtype & (FTOA_NAN | FTOA_INF)) {
ndigs = sign ? 4 : 3;
if (width > ndigs) {
width -= ndigs;
if (!(flags & FL_LPAD)) {
do {
s->write(' ');
} while (--width);
}
} else {
width = 0;
}
if (sign) {
s->write(sign);
}
const char *p = "inf";
if (vtype & FTOA_NAN)
p = "nan";
while ((ndigs = *p) != 0) {
if (flags & FL_FLTUPP)
ndigs += 'I' - 'i';
s->write(ndigs);
p++;
}
goto tail;
}
/* Output format adjustment, number of decimal digits in buf[] */
if (flags & FL_FLTFIX) {
ndigs += exp;
if ((vtype & FTOA_CARRY) && buf[1] == '1') {
ndigs -= 1;
}
if ((signed char)ndigs < 1) {
ndigs = 1;
} else if (ndigs > 8) {
ndigs = 8;
}
} else if (!(flags & FL_FLTEXP)) { /* 'g(G)' format */
if (exp <= prec && exp >= -4) {
flags |= FL_FLTFIX;
}
while (prec && buf[1+prec] == '0') {
prec--;
}
if (flags & FL_FLTFIX) {
ndigs = prec + 1; /* number of digits in buf */
prec = prec > exp ? prec - exp : 0; /* fractional part length */
}
}
/* Conversion result length, width := free space length */
if (flags & FL_FLTFIX) {
n = (exp>0 ? exp+1 : 1);
} else {
n = 5; /* 1e+00 */
}
if (sign) {
n += 1;
}
if (prec) {
n += prec + 1;
}
width = width > n ? width - n : 0;
/* Output before first digit */
if (!(flags & (FL_LPAD | FL_ZFILL))) {
while (width) {
s->write(' ');
width--;
}
}
if (sign) {
s->write(sign);
}
if (!(flags & FL_LPAD)) {
while (width) {
s->write('0');
width--;
}
}
if (flags & FL_FLTFIX) { /* 'f' format */
n = exp > 0 ? exp : 0; /* exponent of left digit */
unsigned char v = 0;
do {
if (n == -1) {
s->write('.');
}
v = (n <= exp && n > exp - ndigs)
? buf[exp - n + 1] : '0';
if (--n < -prec || v == 0) {
break;
}
s->write(v);
} while (1);
if (n == exp
&& (buf[1] > '5'
|| (buf[1] == '5' && !(vtype & FTOA_CARRY)))) {
v = '1';
}
if (v) {
s->write(v);
}
} else { /* 'e(E)' format */
/* mantissa */
if (buf[1] != '1')
vtype &= ~FTOA_CARRY;
s->write(buf[1]);
if (prec) {
s->write('.');
sign = 2;
do {
s->write(buf[sign++]);
} while (--prec);
}
/* exponent */
s->write(flags & FL_FLTUPP ? 'E' : 'e');
ndigs = '+';
if (exp < 0 || (exp == 0 && (vtype & FTOA_CARRY) != 0)) {
exp = -exp;
ndigs = '-';
}
s->write(ndigs);
for (ndigs = '0'; exp >= 10; exp -= 10)
ndigs += 1;
s->write(ndigs);
s->write('0' + exp);
}
goto tail;
}
/*
* Handle string formats c, s, S.
*/
{
const char * pnt;
size_t size;
switch (c) {
case 'c':
buf[0] = va_arg (ap, int);
pnt = (char *)buf;
size = 1;
break;
case 's':
pnt = va_arg (ap, char *);
size = strnlen (pnt, (flags & FL_PREC) ? prec : ~0);
break;
default:
goto non_string;
}
if (!(flags & FL_LPAD)) {
while (size < width) {
s->write(' ');
width--;
}
}
while (size) {
s->write(*pnt++);
if (width) width -= 1;
size -= 1;
}
goto tail;
}
non_string:
/*
* Handle integer formats variations for d/i, u, o, p, x, X.
*/
if (c == 'd' || c == 'i') {
if (flags & FL_LONGLONG) {
int64_t x = va_arg(ap,long long);
flags &= ~(FL_NEGATIVE | FL_ALT);
if (x < 0) {
x = -x;
flags |= FL_NEGATIVE;
}
c = ulltoa_invert (x, (char *)buf, 10) - (char *)buf;
} else {
long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int);
flags &= ~(FL_NEGATIVE | FL_ALT);
if (x < 0) {
x = -x;
flags |= FL_NEGATIVE;
}
c = ultoa_invert (x, (char *)buf, 10) - (char *)buf;
}
} else {
int base;
if (c == 'u') {
flags &= ~FL_ALT;
base = 10;
goto ultoa;
}
flags &= ~(FL_PLUS | FL_SPACE);
switch (c) {
case 'o':
base = 8;
goto ultoa;
case 'p':
flags |= FL_ALT;
FALLTHROUGH;
case 'x':
if (flags & FL_ALT)
flags |= FL_ALTHEX;
base = 16;
goto ultoa;
case 'X':
if (flags & FL_ALT)
flags |= (FL_ALTHEX | FL_ALTUPP);
base = 16 | XTOA_UPPER;
ultoa:
if (flags & FL_LONGLONG) {
c = ulltoa_invert (va_arg(ap, unsigned long long),
(char *)buf, base) - (char *)buf;
} else {
c = ultoa_invert ((flags & FL_LONG)
? va_arg(ap, unsigned long)
: va_arg(ap, unsigned int),
(char *)buf, base) - (char *)buf;
}
flags &= ~FL_NEGATIVE;
break;
default:
return;
}
}
/*
* Format integers.
*/
{
unsigned char len;
len = c;
if (flags & FL_PREC) {
flags &= ~FL_ZFILL;
if (len < prec) {
len = prec;
if ((flags & FL_ALT) && !(flags & FL_ALTHEX)) {
flags &= ~FL_ALT;
}
}
}
if (flags & FL_ALT) {
if (buf[c-1] == '0') {
flags &= ~(FL_ALT | FL_ALTHEX | FL_ALTUPP);
} else {
len += 1;
if (flags & FL_ALTHEX) {
len += 1;
}
}
} else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {
len += 1;
}
if (!(flags & FL_LPAD)) {
if (flags & FL_ZFILL) {
prec = c;
if (len < width) {
prec += width - len;
len = width;
}
}
while (len < width) {
s->write(' ');
len++;
}
}
width = (len < width) ? width - len : 0;
if (flags & FL_ALT) {
s->write('0');
if (flags & FL_ALTHEX) {
s->write(flags & FL_ALTUPP ? 'X' : 'x');
}
} else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {
unsigned char z = ' ';
if (flags & FL_PLUS) {
z = '+';
}
if (flags & FL_NEGATIVE) {
z = '-';
}
s->write(z);
}
while (prec > c) {
s->write('0');
prec--;
}
do {
s->write(buf[--c]);
} while (c);
}
tail:
/* Tail is possible. */
while (width) {
s->write(' ');
width--;
}
} /* for (;;) */
}
因此如果你想使用其他的串口进行打印函数，也是可以的，但是要设备相应的串口不能是其他功能。
AP_HAL::UARTDriver* uartA;
AP_HAL::UARTDriver* uartB;
AP_HAL::UARTDriver* uartC;
AP_HAL::UARTDriver* uartD;
AP_HAL::UARTDriver* uartE;
AP_HAL::UARTDriver* uartF;
AP_HAL::UARTDriver* uartG;
hal.console->printf()
hal.uartA->printf()
hal.uartB->printf()
hal.uartC->printf()
hal.uartD->printf()
hal.uartE->printf()
hal.uartF->printf()
hal.uartG->printf()
设备相应的串口不能是其他功能
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
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; 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;
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;
}
}
}
}