From aeda61d252269a2009d34323821b82bd9f850ffb Mon Sep 17 00:00:00 2001
From: Oleg Semyonov <os-openpilot-org@os-propo.info>
Date: Thu, 16 Jun 2011 15:06:01 +0300
Subject: [PATCH] usart: serial telemetry can be disabled to free USART

It was tested being merged with OP-472_CorvusCorax_CopterControl-Guidance_v3
branch, Spektrum on USART3 and GPS on USART1 and seems to work.

Currently defaults mimic original behavior, that is, if USE_SPEKTRUM
is not defined - define USE_PWM and USE_GPS. Thsi should be refactored
later to make it configurable from the Makefile.

Also it was not ported to the OP MB: it currently does not support the
S.Bus hardware and still has original behavior with the patch. But this
is one more step to dynamic configuration of ports.
---
 flight/CopterControl/System/inc/pios_config.h | 5 ++++-
 flight/CopterControl/System/pios_board.c      | 6 ++++++
 flight/PiOS/Common/pios_com.c                 | 6 ++++--
 3 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h
index 2b3a8eb7a..fddef9c72 100644
--- a/flight/CopterControl/System/inc/pios_config.h
+++ b/flight/CopterControl/System/inc/pios_config.h
@@ -55,9 +55,11 @@
  * and optionally define PIOS_PORT_* to USART port numbers
  */
 
-/* Current defaults */
+/* Current defaults - mimic original behavior */
 #define USE_TELEMETRY
+#if !defined(USE_SPEKTRUM)
 #define USE_GPS
+#endif
 
 /* Serial telemetry: USART1 or USART3 */
 #if !defined(PIOS_PORT_TELEMETRY)
@@ -103,6 +105,7 @@
 #define PIOS_GPIO_TELEMETRY	USART_GPIO(PIOS_PORT_TELEMETRY)
 #define PIOS_RXIO_TELEMETRY	USART_RXIO(PIOS_PORT_TELEMETRY)
 #define PIOS_TXIO_TELEMETRY	USART_TXIO(PIOS_PORT_TELEMETRY)
+#define PIOS_INCLUDE_TELEMETRY_RF
 #endif
 
 #if defined(USE_GPS)
diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c
index b5aabc8b1..142bb2ba3 100644
--- a/flight/CopterControl/System/pios_board.c
+++ b/flight/CopterControl/System/pios_board.c
@@ -200,6 +200,7 @@ void PIOS_ADC_handler() {
 
 #include "pios_usart_priv.h"
 
+#if defined(PIOS_INCLUDE_TELEMETRY_RF)
 /*
  * Telemetry USART
  */
@@ -245,6 +246,7 @@ const struct pios_usart_cfg pios_usart_telem_cfg = {
     },
   },
 };
+#endif /* PIOS_INCLUDE_TELEMETRY_RF */
 
 #if defined(PIOS_INCLUDE_GPS)
 /*
@@ -472,11 +474,13 @@ void PIOS_SUPV_irq_handler() {
 }
 #endif	/* PIOS_INCLUDE_SBUS */
 
+#if defined(PIOS_INCLUDE_TELEMETRY_RF)
 static uint32_t pios_usart_telem_rf_id;
 void PIOS_USART_telem_irq_handler(void)
 {
 	PIOS_USART_IRQ_Handler(pios_usart_telem_rf_id);
 }
+#endif /* PIOS_INCLUDE_TELEMETRY_RF */
 
 #if defined(PIOS_INCLUDE_GPS)
 static uint32_t pios_usart_gps_id;
@@ -807,12 +811,14 @@ void PIOS_Board_Init(void) {
 
 	/* Initialize the PiOS library */
 #if defined(PIOS_INCLUDE_COM)
+#if defined(PIOS_INCLUDE_TELEMETRY_RF)
 	if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
 		PIOS_DEBUG_Assert(0);
 	}
 	if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) {
 		PIOS_DEBUG_Assert(0);
 	}
+#endif /* PIOS_INCLUDE_TELEMETRY_RF */
 #if defined(PIOS_INCLUDE_GPS)
 	if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
 		PIOS_DEBUG_Assert(0);
diff --git a/flight/PiOS/Common/pios_com.c b/flight/PiOS/Common/pios_com.c
index 0d2c21d30..4bd00c22b 100644
--- a/flight/PiOS/Common/pios_com.c
+++ b/flight/PiOS/Common/pios_com.c
@@ -38,7 +38,7 @@
 
 static bool PIOS_COM_validate(struct pios_com_dev * com_dev)
 {
-	return (com_dev->magic == PIOS_COM_DEV_MAGIC);
+	return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC));
 }
 
 #if defined(PIOS_INCLUDE_FREERTOS) && 0
@@ -298,7 +298,9 @@ int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id)
 
 	if (!PIOS_COM_validate(com_dev)) {
 		/* Undefined COM port for this board (see pios_board.c) */
-	PIOS_DEBUG_Assert(0);
+		/* This is commented out so com_id=NULL can be used to disable telemetry */
+		//PIOS_DEBUG_Assert(0);
+		return 0;
 	}
 
 	if (!com_dev->driver->rx_avail) {