PX4 IO [14] serial
PX4 IO [14] serial
-------- 转载请注明出处
-------- 更多笔记请访问我的博客：merafour.blog.163.com
-------- 2014-12-31.冷月追风
-------- email：merafour@163.com
1.hrt_ppm_decode
我们已经不止一次接触到 fmu跟 IO通讯，之前我们看到的只是 fmu部分源码，那么 IO中又是怎么处理的呢？下面我们就来瞧瞧。
IO固件的 mk文件为： "./PX4Firmware/makefiles/config_px4io-v2_default.mk"，其内容如下：
#
# Makefile for the px4iov2_default configuration
#
# Board support modules
#
MODULES += drivers/stm32
MODULES += drivers/boards/px4io-v2
MODULES += modules/px4iofirmware
这是 IO固件所用到的一些文件，当然仅仅是 PX4Firmware目录下的。而现在我们要阅读的源码在 "PX4Firmware/src/modules/px4iofirmware/"目录，其内容如下：
radiolink@ubuntu:~/apm$ ls PX4Firmware/src/modules/px4iofirmware/
adc.c dsm.c mixer.cpp protocol.h px4io.h safety.c serial.c
controls.c i2c.c module.mk px4io.c registers.c sbus.c
radiolink@ubuntu:~/apm$
因为前面我们知道 fmu跟 IO是通过串口进行通讯的，所以我们现在要阅读的源码主要在 serial.c中。
前面我们看到，在 io_get_raw_rc_input函数中使用下面的代码来获取遥控器数据：
if (channel_count > 9) {
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9);
if (ret != OK)
return ret;
}
int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsignednum_values)
{
/* range check the transfer */
if (num_values > ((_max_transfer) / sizeof(*values))) {
debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
return -EINVAL;
}
int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
if (ret != (int)num_values) {
debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret);
return -1;
}
return OK;
}
因此我有理由相信在 IO中也使用了宏 PX4IO_PAGE_RAW_RC_INPUT。所以：
radiolink@ubuntu:~/apm$ grep -nr PX4IO_PAGE_RAW_RC_INPUT ./PX4Firmware/src/modules/px4iofirmware/
./PX4Firmware/src/modules/px4iofirmware/px4io.h:77:externuint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
./PX4Firmware/src/modules/px4iofirmware/protocol.h:140:#definePX4IO_PAGE_RAW_RC_INPUT 4
./PX4Firmware/src/modules/px4iofirmware/registers.c:866: casePX4IO_PAGE_RAW_RC_INPUT:
radiolink@ubuntu:~/apm$
int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned*num_values)
{
#define SELECT_PAGE(_page_name) \
do { \
*values = &_page_name[0]; \
*num_values = sizeof(_page_name) / sizeof(_page_name[0]); \
} while(0)
switch (page) {
/* ... */
/* status pages */
case PX4IO_PAGE_CONFIG:
SELECT_PAGE(r_page_config);
break;
case PX4IO_PAGE_ACTUATORS:
SELECT_PAGE(r_page_actuators);
break;
case PX4IO_PAGE_SERVOS:
SELECT_PAGE(r_page_servos);
break;
case PX4IO_PAGE_RAW_RC_INPUT:
SELECT_PAGE(r_page_raw_rc_input);
break;
在这段代码中我们并没有看到数据拷贝，所以数据拷贝应该是由调用 registers_get的函数来完成。
static void rx_handle_packet(void)
{
/* check packet CRC */
uint8_t crc = dma_packet.crc;
dma_packet.crc = 0;
if (crc != crc_packet(&dma_packet)) {
perf_count(pc_crcerr);
/* send a CRC error reply */
dma_packet.count_code = PKT_CODE_CORRUPT;
dma_packet.page = 0xff;
dma_packet.offset = 0xff;
return;
}
if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) {
/* it's a blind write - pass it on */
if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
perf_count(pc_regerr);
dma_packet.count_code = PKT_CODE_ERROR;
} else {
dma_packet.count_code = PKT_CODE_SUCCESS;
}
return;
}
if (PKT_CODE(dma_packet) == PKT_CODE_READ) {
/* it's a read - get register pointer for reply */
unsigned count;
uint16_t *registers;
if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) {
perf_count(pc_regerr);
dma_packet.count_code = PKT_CODE_ERROR;
} else {
/* constrain reply to requested size */
if (count > PKT_MAX_REGS)
count = PKT_MAX_REGS;
if (count > PKT_COUNT(dma_packet))
count = PKT_COUNT(dma_packet);
/* copy reply registers into DMA buffer */
memcpy((void *)&dma_packet.regs[0], registers, count * 2);
dma_packet.count_code = count | PKT_CODE_SUCCESS;
}
return;
}
/* send a bad-packet error reply */
dma_packet.count_code = PKT_CODE_CORRUPT;
dma_packet.page = 0xff;
dma_packet.offset = 0xfe;
}
static
void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
{
/* ... */
rx_handle_packet();
在 rx_handle_packet函数中我们看到 IO提供了 registers_get跟 registers_set函数分别用来获取和设置 IO数据。而 rx_dma_callback函数的调用我们很容易想到是在串口收到数据的时候。这个我们回头再来折腾，现在我们要去看另外一个东西：r_page_raw_rc_input。在 registers_get函数中我们要读取的数据是来自 r_page_raw_rc_input，它的数据肯定也是有人放进去的，而不是凭空产生的。所以：
radiolink@ubuntu:~/apm$ grep -nr r_page_raw_rc_input ./PX4Firmware/src/modules/px4iofirmware/
./PX4Firmware/src/modules/px4iofirmware/controls.c:216: bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
./PX4Firmware/src/modules/px4iofirmware/controls.c:230: r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
./PX4Firmware/src/modules/px4iofirmware/px4io.h:77:externuint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
./PX4Firmware/src/modules/px4iofirmware/px4io.h:97:#definer_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
./PX4Firmware/src/modules/px4iofirmware/px4io.h:98:#definer_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
./PX4Firmware/src/modules/px4iofirmware/px4io.h:99:#definer_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
./PX4Firmware/src/modules/px4iofirmware/registers.c:114:uint16_t r_page_raw_rc_input[] =
./PX4Firmware/src/modules/px4iofirmware/registers.c:867: SELECT_PAGE(r_page_raw_rc_input);
radiolink@ubuntu:~/apm$
经分析， controls.c是用来获取遥控器数据的。其实，从其命名上也是相当明显的。
/*
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
if (ppm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_ppm);
/* limit number of channels to allowable data size */
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
/* store RSSI */
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
{
bool result = false;
/* avoid racing with PPM updates */
irqstate_t state = irqsave();
/*
* If we have received a new PPM frame within the last 200ms, accept it
* and then invalidate it.
*/
if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) {
/* PPM data exists, copy it */
*num_values = ppm_decoded_channels;
if (*num_values > PX4IO_RC_INPUT_CHANNELS)
*num_values = PX4IO_RC_INPUT_CHANNELS;
for (unsigned i = 0; i < *num_values; i++)
values[i] = ppm_buffer[i];
/* clear validity */
ppm_last_valid_decode = 0;
/* store PPM frame length */
if (num_values)
*frame_len = ppm_frame_length;
/* good if we got any channels */
result = (*num_values > 0);
}
irqrestore(state);
return result;
}
从源码中我们看到，数组 r_page_raw_rc_input中所存放的并不仅仅是接收到的遥控器数据。遥控器数据仅仅是其中一部分而已。如果我们去看其定义将会更加清楚：
/**
* PAGE 0
*
* Static configuration parameters.
*/
static const uint16_t r_page_config[] = {
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION,
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
[PX4IO_P_CONFIG_HARDWARE_VERSION] = 2,
#else
[PX4IO_P_CONFIG_HARDWARE_VERSION] = 1,
#endif
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS,
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
};
当然我现在也不去研究这个数组中到底都放了些什么数据。我关心的是 ppm_buffer中的数据是怎么来的，直至数据的最源头。但是我们要知道 ppm_buffer跟 ppm_decoded_channels是分不开的。
radiolink@ubuntu:~/apm$ grep -nr ppm_buffer ./PX4Firmware/src/
./PX4Firmware/src/systemcmds/tests/test_hrt.c:90:extern uint16_t ppm_buffer[];
./PX4Firmware/src/systemcmds/tests/test_hrt.c:103: printf(" %u\n", ppm_buffer[i]);
./PX4Firmware/src/modules/systemlib/ppm_decode.c:91:uint16_t ppm_buffer[PPM_MAX_CHANNELS];
./PX4Firmware/src/modules/systemlib/ppm_decode.c:179: ppm_buffer[i] = ppm_temp_buffer[i];
./PX4Firmware/src/modules/systemlib/ppm_decode.h:59:__EXPORT externuint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */
./PX4Firmware/src/modules/px4iofirmware/controls.c:472: values[i] = ppm_buffer[i];
./PX4Firmware/src/drivers/stm32/drv_hrt.c:352:__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
./PX4Firmware/src/drivers/stm32/drv_hrt.c:503: ppm_buffer[i] = ppm_temp_buffer[i];
./PX4Firmware/src/drivers/px4fmu/fmu.cpp:722: rc_in.values[i] = ppm_buffer[i];
radiolink@ubuntu:~/apm$
在这里，我们可能会觉得是源文件 ppm_decode.c是我们要找的文件，但是：
void ppm_input_decode(bool reset, unsigned count)
{
uint16_t width;
uint16_t interval;
unsigned i;
/* if we missed an edge, we have to give up */
if (reset)
goto error;
/* how long since the last edge? */
width = count - ppm.last_edge;
if (count < ppm.last_edge)
width += ppm.count_max; /* handle wrapped count */
ppm.last_edge = count;
if (width >= PPM_MIN_START) {
if (ppm.next_channel != ppm_decoded_channels) {
/* ... */
} else {
/* frame channel count matches expected, let's use it */
if (ppm.next_channel > PPM_MIN_CHANNELS) {
for (i = 0; i < ppm.next_channel; i++)
ppm_buffer[i] = ppm_temp_buffer[i];
ppm_last_valid_decode = hrt_absolute_time();
}
}
/* reset for the next frame */
ppm.next_channel = 0;
/* next edge is the reference for the first channel */
ppm.phase = ARM;
return;
}
radiolink@ubuntu:~/apm$ grep -nr ppm_input_decode ./PX4Firmware/src/
./PX4Firmware/src/modules/systemlib/ppm_decode.c:123:ppm_input_decode(bool reset, unsigned count)
./PX4Firmware/src/modules/systemlib/ppm_decode.h:68: * ppm_input_decode, used to determine how to
./PX4Firmware/src/modules/systemlib/ppm_decode.h:84:__EXPORT void ppm_input_decode(bool reset, unsigned count);
radiolink@ubuntu:~/apm$
所以函数 ppm_input_decode根本就没有被调用。而且我们前面的 mk文件中根本就没有添加 systemlib目录。那么我们要找的源码就只能在源文件 drv_hrt.c中了。
/**
* Handle the PPM decoder state machine.
*/
static void hrt_ppm_decode(uint32_t status)
{
uint16_t count = rCCR_PPM;
uint16_t width;
uint16_t interval;
unsigned i;
/* if we missed an edge, we have to give up */
if (status & SR_OVF_PPM)
goto error;
/* how long since the last edge? - this handles counter wrapping implicitely. */
width = count - ppm.last_edge;
ppm_edge_history[ppm_edge_next++] = width;
if (ppm_edge_next >= 32)
ppm_edge_next = 0;
/*
* if this looks like a start pulse, then push the last set of values
* and reset the state machine
*/
if (width >= PPM_MIN_START) {
/*
* If the number of channels changes unexpectedly, we don't want
* to just immediately jump on the new count as it may be a result
* of noise or dropped edges. Instead, take a few frames to settle.
*/
if (ppm.next_channel != ppm_decoded_channels) {
static unsigned new_channel_count;
static unsigned new_channel_holdoff;
if (new_channel_count != ppm.next_channel) {
/* start the lock counter for the new channel count */
new_channel_count = ppm.next_channel;
new_channel_holdoff = PPM_CHANNEL_LOCK;
} else if (new_channel_holdoff > 0) {
/* this frame matched the last one, decrement the lock counter */
new_channel_holdoff--;
} else {
/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
ppm_decoded_channels = new_channel_count;
new_channel_count = 0;
}
} else {
/* frame channel count matches expected, let's use it */
if (ppm.next_channel > PPM_MIN_CHANNELS) {
for (i = 0; i < ppm.next_channel; i++)
ppm_buffer[i] = ppm_temp_buffer[i];
ppm_last_valid_decode = hrt_absolute_time();
}
}
/* reset for the next frame */
ppm.next_channel = 0;
/* next edge is the reference for the first channel */
ppm.phase = ARM;
ppm.last_edge = count;
return;
}
/**
* Handle the compare interupt by calling the callout dispatcher
* and then re-scheduling the next deadline.
*/
static int
hrt_tim_isr( int irq, void *context)
{
uint32_t status;
/* grab the timer for latency tracking purposes */
latency_actual = rCNT;
/* copy interrupt status */
status = rSR;
/* ack the interrupts we just read */
rSR = ~status;
# ifdef HRT_PPM_CHANNEL
/* was this a PPM edge? */
if (status & (SR_INT_PPM | SR_OVF_PPM)) {
/* if required, flip edge sensitivity */
# ifdef PPM_EDGE_FLIP
rCCER ^= CCER_PPM_FLIP;
# endif
hrt_ppm_decode(status);
}
# endif
这个时候我们已经跟踪到中断服务函数了，在往下那就是中断初始化了。从这里我们也看到，真正的 PPM数据最后是来自 ppm_temp_buffer数组，这是由 hrt_ppm_decode函数剩下的部分代码来完成的，负责 PPM解码工作。
switch (ppm.phase) {
case UNSYNCH:
/* we are waiting for a start pulse - nothing useful to do here */
break;
case ARM:
/* we expect a pulse giving us the first mark */
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too short or too long */
/* record the mark timing, expect an inactive edge */
ppm.last_mark = ppm.last_edge;
/* frame length is everything including the start gap */
ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start);
ppm.frame_start = ppm.last_edge;
ppm.phase = ACTIVE;
break;
case INACTIVE:
/* we expect a short pulse */
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too short or too long */
/* this edge is not interesting, but now we are ready for the next mark */
ppm.phase = ACTIVE;
break;
case ACTIVE:
/* determine the interval from the last mark */
interval = count - ppm.last_mark;
ppm.last_mark = count;
ppm_pulse_history[ppm_pulse_next++] = interval;
if (ppm_pulse_next >= 32)
ppm_pulse_next = 0;
/* if the mark-mark timing is out of bounds, abandon the frame */
if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
goto error;
/* if we have room to store the value, do so */
if (ppm.next_channel < PPM_MAX_CHANNELS)
ppm_temp_buffer[ppm.next_channel++] = interval;
ppm.phase = INACTIVE;
break;
}
ppm.last_edge = count;
return;
/* the state machine is corrupted; reset it */
error:
/* we don't like the state of the decoder, reset it and try again */
ppm.phase = UNSYNCH;
ppm_decoded_channels = 0;
}
实际上就是对脉宽进行测量。
关于脉宽测量，我们目前还没有必要去研究它，所以就不深入其细节了。
2.user_start
现在，我们就来看看 ppm_input函数是如何被调用的。
void
controls_tick() {
uint16_t rssi = 0;
#ifdef ADC_RSSI
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) {
/* use 1:1 scaling on 3.3V ADC input */
unsigned mV = counts * 3300 / 4096;
/* scale to 0..253 */
rssi = mV / 13;
}
}
#endif
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
if (dsm_updated) {
r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
rssi = 255;
if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
rssi = 100;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
}
if (sbus_failsafe) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
rssi = 0;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
perf_end(c_gather_sbus);
/*
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
if (ppm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_ppm);
/* limit number of channels to allowable data size */
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
/* store RSSI */
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
我以为 PX4仅仅支持 ppm跟 sbus，但从这里我们知道，其实 PX4还支持另外一种 dsm输入。当然，采用那种输入方式并不是我们现在所关心的。我们现在关系的是谁调用了 controls_tick函数。
radiolink@ubuntu:~/apm$ grep -nr controls_tick ./PX4Firmware/src/
./PX4Firmware/src/modules/px4iofirmware/controls.c:145:controls_tick() {
./PX4Firmware/src/modules/px4iofirmware/px4io.h:217:extern voidcontrols_tick(void);
./PX4Firmware/src/modules/px4iofirmware/px4io.c:354: controls_tick();
radiolink@ubuntu:~/apm$
int user_start(int argc, char *argv[])
{
/* ... */
for (;;) {
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
controls_tick();
user_start函数是 IO的 init进程，这个前面我们已经分析过了。所以，最后是由 IO的主循环对 controls_tick函数进行调用。
以上就是 IO中遥控器的输入。说完了输入，我们还得说说输出。这个我们稍后再来看。
