pixhawk RC信号传输流程代码版本pixhawk1.5.5

最新推荐文章于 2023-09-03 07:59:58 发布

weixin_42487702

最新推荐文章于 2023-09-03 07:59:58 发布

阅读量2.2k

点赞数 3

分类专栏： pixhawk

本文链接：https://blog.csdn.net/weixin_42487702/article/details/82862228

版权

pixhawk 专栏收录该内容

6 篇文章 0 订阅

订阅专栏

本篇主要介绍RC信号的传输流程。主要包括遥控器信号的接收，解码，发送至FMU，处理，发送至IO，映射至通道。本文以SBUS协议的遥控器为例，其他协议的RC信号除接收与解码步骤外其余均相同。

遥控器信号接收与解码：

遥控器信号接收的代码在 Firmware\src\lib\rc目录下sbus.cpp其中的sbus_input函数中。sbus_input中调用了read(sbus_fd, &buf[0], SBUS_FRAME_SIZE); 函数用来获取SBUS的信号帧，并在结尾调用了sbus_parse 函数对数据帧进行解码将其转化为PWM值。

bool
sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
	   uint16_t max_channels)
{
	int		ret = 1;
	hrt_abstime	now;

	/*
	 * The S.BUS protocol doesn't provide reliable framing,
	 * so we detect frame boundaries by the inter-frame delay.
	 *
	 * The minimum frame spacing is 7ms; with 25 bytes at 100000bps
	 * frame transmission time is ~2ms.
	 *
	 * We expect to only be called when bytes arrive for processing,
	 * and if an interval of more than 3ms passes between calls,
	 * the first byte we read will be the first byte of a frame.
	 *
	 * In the case where byte(s) are dropped from a frame, this also
	 * provides a degree of protection. Of course, it would be better
	 * if we didn't drop bytes...
	 */
	now = hrt_absolute_time();

	/*
	 * Fetch bytes, but no more than we would need to complete
	 * a complete frame.
	 */
	uint8_t buf[SBUS_FRAME_SIZE * 2];
	bool sbus_decoded = false;

	ret = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE);

	/* if the read failed for any reason, just give up here */
	if (ret < 1) {
		return false;
	}

	/*
	 * Try to decode something with what we got
	 */
	if (sbus_parse(now, &buf[0], ret, values, num_values, sbus_failsafe,
		       sbus_frame_drop, &sbus_frame_drops, max_channels)) {

		sbus_decoded = true;
	}

	return sbus_decoded;
}

我们进一步搜索发现sbus_input()函数在px4io.c的controls_tick()函数中被调用了。由此可以定位到px4io.c中。

遥控器信号传递至FMU并处理：

在px4io.c中我们发现这段代码原来是IO芯片中最重要的代码，其主要实现的功能有基础环境的初始化,包括PWM,串口,ADC 等资源的初始化,最后运行一个死循环,用于处理遥控器输入以及与PX4FMU 通信的内容即controls_tick()函数。

int
user_start(int argc, char *argv[])
{
	/* configure the first 8 PWM outputs (i.e. all of them) */
	up_pwm_servo_init(0xff);

#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)

	/* run C++ ctors before we go any further */

	up_cxxinitialize();

#	if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE)
#  		error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE.
#	endif

#else
#  error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined.
#endif

	/* reset all to zero */
	memset(&system_state, 0, sizeof(system_state));

	/* configure the high-resolution time/callout interface */
	hrt_init();

	/* calculate our fw CRC so FMU can decide if we need to update */
	calculate_fw_crc();

	/*
	 * Poll at 1ms intervals for received bytes that have not triggered
	 * a DMA event.
	 */
#ifdef CONFIG_ARCH_DMA
	hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif

	/* print some startup info */
	syslog(LOG_INFO, "\nPX4IO: starting\n");

	/* default all the LEDs to off while we start */
	LED_AMBER(false);
	LED_BLUE(false);
	LED_SAFETY(false);
#ifdef GPIO_LED4
	LED_RING(false);
#endif

	/* turn on servo power (if supported) */
#ifdef POWER_SERVO
	POWER_SERVO(true);
#endif

	/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
	ENABLE_SBUS_OUT(false);
#endif

	/* start the safety switch handler */
	safety_init();

	/* initialise the control inputs */
	controls_init();

	/* set up the ADC */
	adc_init();

	/* start the FMU interface */
	interface_init();

	/* add a performance counter for mixing */
	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");

	/* add a performance counter for controls */
	perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");

	/* and one for measuring the loop rate */
	perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");

	struct mallinfo minfo = mallinfo();
	r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk;
	syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);

	/* initialize PWM limit lib */
	pwm_limit_init(&pwm_limit);

	/*
	 *    P O L I C E    L I G H T S
	 *
	 * Not enough memory, lock down.
	 *
	 * We might need to allocate mixers later, and this will
	 * ensure that a developer doing a change will notice
	 * that he just burned the remaining RAM with static
	 * allocations. We don't want him to be able to
	 * get past that point. This needs to be clearly
	 * documented in the dev guide.
	 *
	 */
	if (minfo.mxordblk < 600) {

		syslog(LOG_ERR, "ERR: not enough MEM");
		bool phase = false;

		while (true) {

			if (phase) {
				LED_AMBER(true);
				LED_BLUE(false);

			} else {
				LED_AMBER(false);
				LED_BLUE(true);
			}

			up_udelay(250000);

			phase = !phase;
		}
	}

	/* Start the failsafe led init */
	failsafe_led_init();

	/*
	 * Run everything in a tight loop.
	 */

	uint64_t last_debug_time = 0;
	uint64_t last_heartbeat_time = 0;
	uint64_t last_loop_time = 0;

	for (;;) {
		dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f;
		last_loop_time = hrt_absolute_time();

		if (dt < 0.0001f) {
			dt = 0.0001f;

		} else if (dt > 0.02f) {
			dt = 0.02f;
		}

		/* 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();
		perf_end(controls_perf);

		/* some boards such as Pixhawk 2.1 made
		   the unfortunate choice to combine the blue led channel with
		   the IMU heater. We need a software hack to fix the hardware hack
		   by allowing to disable the LED / heater.
		 */
		if (r_page_setup[PX4IO_P_SETUP_THERMAL] == PX4IO_THERMAL_IGNORE) {
			/*
			  blink blue LED at 4Hz in normal operation. When in
			  override blink 4x faster so the user can clearly see
			  that override is happening. This helps when
			  pre-flight testing the override system
			 */
			uint32_t heartbeat_period_us = 250 * 1000UL;

			if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {
				heartbeat_period_us /= 4;
			}

			if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) {
				last_heartbeat_time = hrt_absolute_time();
				heartbeat_blink();
			}

		} else if (r_page_setup[PX4IO_P_SETUP_THERMAL] < PX4IO_THERMAL_FULL) {
			/* switch resistive heater off */
			LED_BLUE(false);

		} else {
			/* switch resistive heater hard on */
			LED_BLUE(true);
		}

		update_mem_usage();

		ring_blink();

		check_reboot();

		/* check for debug activity (default: none) */
		show_debug_messages();

		/* post debug state at ~1Hz - this is via an auxiliary serial port
		 * DEFAULTS TO OFF!
		 */
		if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {

			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
				  (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
				  (unsigned)r_status_flags,
				  (unsigned)r_setup_arming,
				  (unsigned)r_setup_features,
				  (unsigned)mallinfo().mxordblk);
			last_debug_time = hrt_absolute_time();
		}
	}
}

在pixhawk中IO芯片与fmu芯片是利用高速串口进行通讯。在px4io.c中这段代码中找到了一个中断初始化函数interface_init(); 在中断初始化函数中找到了中断服务函数，serial_interrupt()；这个中断服务函数便是fmu与io通讯的地方。进入中断服务函数我们发现IO侧向fmu侧发送数据属于被动发送，只有接收到fmu向io发送的数据，io才会向fmu返回数据。

static int
serial_interrupt(int irq, void *context, FAR void *arg)
{
	static bool abort_on_idle = false;

	uint32_t sr = rSR;	/* get UART status register */
	(void)rDR;		/* required to clear any of the interrupt status that brought us here */

	if (sr & (USART_SR_ORE |	/* overrun error - packet was too big for DMA or DMA was too slow */
		  USART_SR_NE |		/* noise error - we have lost a byte due to noise */
		  USART_SR_FE)) {		/* framing error - start/stop bit lost or line break */

		perf_count(pc_errors);

		if (sr & USART_SR_ORE) {
			perf_count(pc_ore);
		}

		if (sr & USART_SR_NE) {
			perf_count(pc_ne);
		}

		if (sr & USART_SR_FE) {
			perf_count(pc_fe);
		}

		/* send a line break - this will abort transmission/reception on the other end */
		rCR1 |= USART_CR1_SBK;

		/* when the line goes idle, abort rather than look at the packet */
		abort_on_idle = true;
	}

	if (sr & USART_SR_IDLE) {

		/*
		 * If we saw an error, don't bother looking at the packet - it should have
		 * been aborted by the sender and will definitely be bad. Get the DMA reconfigured
		 * ready for their retry.
		 */
		if (abort_on_idle) {

			abort_on_idle = false;
			dma_reset();
			return 0;
		}

		/*
		 * The sender has stopped sending - this is probably the end of a packet.
		 * Check the received length against the length in the header to see if
		 * we have something that looks like a packet.
		 */
		unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma);

		if ((length < 1) || (length < PKT_SIZE(dma_packet))) {

			/* it was too short - possibly truncated */
			perf_count(pc_badidle);
			dma_reset();
			return 0;
		}

		/*
		 * Looks like we received a packet. Stop the DMA and go process the
		 * packet.
		 */
		perf_count(pc_idle);
		stm32_dmastop(rx_dma);
		rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL);
	}

	return 0;
}

serial_interrupt()接收数据、清除中断后，再调用rx_dma_callback()进行DMA数据的处理和发送IO侧的数据至fmu。

static void
rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
{
	/*
	 * We are here because DMA completed, or UART reception stopped and
	 * we think we have a packet in the buffer.
	 */
	perf_begin(pc_txns);

	/* disable UART DMA */
	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);

	/* handle the received packet */
	rx_handle_packet();

	/* re-set DMA for reception first, so we are ready to receive before we start sending */
	dma_reset();

	/* send the reply to the just-processed request */
	dma_packet.crc = 0;
	dma_packet.crc = crc_packet(&dma_packet);
	stm32_dmasetup(
		tx_dma,
		(uint32_t)&rDR,
		(uint32_t)&dma_packet,
		PKT_SIZE(dma_packet),
		DMA_CCR_DIR		|
		DMA_CCR_MINC		|
		DMA_CCR_PSIZE_8BITS	|
		DMA_CCR_MSIZE_8BITS);
	stm32_dmastart(tx_dma, NULL, NULL, false); //发送io侧数据至fmu
	rCR3 |= USART_CR3_DMAT;

	perf_end(pc_txns);
}

在fmu侧px4io.cpp文件中io_get_raw_rc_input()函数完成了遥控信号的获取并将其通过orb广播出去，fmu的其他文件便可通过orb对数据进行处理。

发送处理后的数据至IO并将其映射至通道中：

其他文件将rc信号处理完成后通过ORB ORB_ID(actuator_controls_0) 广播之， px4io.cpp通过io_set_control_state()函数将广播的数据进行订阅复制，并通过不断调用最终在px4io.cpp的task_main函数中将其送回fmu中。IO测的中断处理函数接收到数据后将其解码为PWM值,然后调用mixer_tick();函数对其进行通道映射输出真正的PWM值。

本文大部分均为博主原创且讲的相对比较浅显，更详细深层次的内容请参考下面的博客。本人才识浅薄，并且刚接触pixhawk不久，大部分为博主自己推测和百度的结果，有错误之处还请指出，博主将进行更正。谢谢

参考文章：

https://blog.csdn.net/SIR_wkp/article/details/80548262

https://blog.csdn.net/czyv587/article/details/51445308