From 87b2375be436aa92d361e0131be9ff63ae43fe36 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Tue, 26 Aug 2014 14:34:14 +0200
Subject: Ignore single channels during PWM output

---
 src/modules/px4iofirmware/registers.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 43161aa70..0da778b6f 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -285,7 +285,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
 
 			/* XXX range-check value? */
-			r_page_servos[offset] = *values;
+			if (*values != PWM_IGNORE_THIS_CHANNEL) {
+				r_page_servos[offset] = *values;
+			}
 
 			offset++;
 			num_values--;
-- 
cgit v1.2.3


From 49846d476f77290093c24097e05d5a8d60d1a4f1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 27 Aug 2014 08:00:12 +0200
Subject: IO firmware supports termination failsafe

---
 src/modules/px4iofirmware/mixer.cpp   | 10 ++++++++++
 src/modules/px4iofirmware/protocol.h  |  3 ++-
 src/modules/px4iofirmware/registers.c | 18 ++++++++++++++++--
 3 files changed, 28 insertions(+), 3 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 606c639f9..1eacda97a 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -154,6 +154,16 @@ mixer_tick(void)
 		}
 	}
 
+	/*
+	 * Check if failsafe termination is set - if yes,
+	 * set the force failsafe flag once entering the first
+	 * failsafe condition.
+	 */
+	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
+		(source == MIX_FAILSAFE)) {
+		r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
+	}
+
 	/*
 	 * Check if we should force failsafe - and do it if we have to
 	 */
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 050783687..eae7f9567 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -180,6 +180,7 @@
 #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
 #define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */
 #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE	(1 << 8) /* If set, the system will always output the failsafe values */
+#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE	(1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 0da778b6f..7a5a5e484 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -190,7 +190,8 @@ volatile uint16_t	r_page_setup[] =
 					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
 					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
 					 PX4IO_P_SETUP_ARMING_LOCKDOWN | \
-					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
+					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
+					 PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -518,6 +519,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 			}
 
+			/*
+			 * If the failsafe termination flag is set, do not allow the autopilot to unset it
+			 */
+			value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
+
+			/*
+			 * If failsafe termination is enabled and force failsafe bit is set, do not allow
+			 * the autopilot to clear it.
+			 */
+			if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) {
+				value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
+			}
+
 			r_setup_arming = value;
 
 			break;
-- 
cgit v1.2.3


From 91d50301c61cf495e83cab59621ef83cff24da3a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 27 Aug 2014 10:46:10 +0200
Subject: Do not enter RC override if FMU is lost and termination failsafe mode
 requested

---
 src/modules/px4iofirmware/mixer.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 1eacda97a..0c65b7642 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -139,7 +139,9 @@ mixer_tick(void)
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
 		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
-		     !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
+		     !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
+		     /* do not enter manual override if we asked for termination failsafe and FMU is lost */
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
 
 		 	/* if allowed, mix from RC inputs directly */
 			source = MIX_OVERRIDE;
-- 
cgit v1.2.3


From c85d7aae06b49f43e32dde05c00bcee1310aee9f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 28 Aug 2014 20:36:59 +0200
Subject: Make sure we got to valid input at least once before kicking in
 failsafe

---
 src/modules/px4iofirmware/mixer.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 0c65b7642..17751bd6d 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -71,6 +71,7 @@ extern "C" {
 static bool mixer_servos_armed = false;
 static bool should_arm = false;
 static bool should_always_enable_pwm = false;
+static bool input_valid_initialized = false;		/* the input was valid at least once */
 static volatile bool in_mixer = false;
 
 /* selected control values and count for mixing */
@@ -162,7 +163,8 @@ mixer_tick(void)
 	 * failsafe condition.
 	 */
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
-		(source == MIX_FAILSAFE)) {
+		(source == MIX_FAILSAFE) &&
+		input_valid_initialized) {
 		r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
 	}
 
@@ -180,6 +182,9 @@ mixer_tick(void)
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
 	} else {
 		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
+
+		/* we got valid input, kick off the full failsafe checks */
+		input_valid_initialized = true;
 	}
 
 	/*
-- 
cgit v1.2.3


From 6773eb988017063366dce95bbfec8fae1f1913f0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 28 Aug 2014 21:39:33 +0200
Subject: Introduce FMU initialised status flag, only run failsafe trap if
 initialized once

---
 src/modules/px4iofirmware/mixer.cpp  | 67 +++++++++++++++++++-----------------
 src/modules/px4iofirmware/protocol.h |  1 +
 2 files changed, 36 insertions(+), 32 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 17751bd6d..3e19333d8 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -58,7 +58,7 @@ extern "C" {
 /*
  * Maximum interval in us before FMU signal is considered lost
  */
-#define FMU_INPUT_DROP_LIMIT_US		200000
+#define FMU_INPUT_DROP_LIMIT_US		500000
 
 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
 #define ROLL     0
@@ -71,7 +71,6 @@ extern "C" {
 static bool mixer_servos_armed = false;
 static bool should_arm = false;
 static bool should_always_enable_pwm = false;
-static bool input_valid_initialized = false;		/* the input was valid at least once */
 static volatile bool in_mixer = false;
 
 /* selected control values and count for mixing */
@@ -99,7 +98,8 @@ mixer_tick(void)
 {
 
 	/* check that we are receiving fresh data from the FMU */
-	if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
+	if ((system_state.fmu_data_received_time == 0) ||
+		hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
 
 		/* too long without FMU input, time to go to failsafe */
 		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
@@ -110,6 +110,9 @@ mixer_tick(void)
 
 	} else {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
+
+		/* this flag is never cleared once OK */
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
 	}
 
 	/* default to failsafe mixing - it will be forced below if flag is set */
@@ -157,14 +160,41 @@ mixer_tick(void)
 		}
 	}
 
+	/*
+	 * Decide whether the servos should be armed right now.
+	 *
+	 * We must be armed, and we must have a PWM source; either raw from
+	 * FMU or from the mixer.
+	 *
+	 */
+	should_arm = (
+		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+		/* and FMU is armed */ 		  && (
+							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
+		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+		/* or failsafe was set manually */	 || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
+						     )
+	);
+
+	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+
 	/*
 	 * Check if failsafe termination is set - if yes,
 	 * set the force failsafe flag once entering the first
 	 * failsafe condition.
 	 */
-	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
+	if (	/* if we have requested flight termination style failsafe (noreturn) */
+		(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
+		/* and we ended up in a failsafe condition */
 		(source == MIX_FAILSAFE) &&
-		input_valid_initialized) {
+		/* and we should be armed, so we intended to provide outputs */
+		should_arm &&
+		/* and FMU is initialized */
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) {
 		r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
 	}
 
@@ -182,35 +212,8 @@ mixer_tick(void)
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
 	} else {
 		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
-
-		/* we got valid input, kick off the full failsafe checks */
-		input_valid_initialized = true;
 	}
 
-	/*
-	 * Decide whether the servos should be armed right now.
-	 *
-	 * We must be armed, and we must have a PWM source; either raw from
-	 * FMU or from the mixer.
-	 *
-	 * XXX correct behaviour for failsafe may require an additional case
-	 * here.
-	 */
-	should_arm = (
-		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
-		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
-		/* and FMU is armed */ 		  && (
-							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
-		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
-		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
-		/* or failsafe was set manually */	 || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
-						     )
-	);
-
-	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
-						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
-						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
-
 	/*
 	 * Run the mixers.
 	 */
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index eae7f9567..4739f6e40 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -111,6 +111,7 @@
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED	(1 << 13) /* FMU was initialized and OK once */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
-- 
cgit v1.2.3


From 69109b622796dacd6f78ebafa92b10379ba0d7b4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 1 Sep 2014 00:45:41 +0200
Subject: Compile and link ST24 parser in IO firmware

---
 src/lib/rc/st24.c                    | 120 ++++++++++++++++++++++++++++++++++-
 src/lib/rc/st24.h                    |  46 ++++++++------
 src/modules/px4iofirmware/controls.c |  27 +++++++-
 src/modules/px4iofirmware/module.mk  |   3 +-
 4 files changed, 173 insertions(+), 23 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c
index addbcd899..956a72bf3 100644
--- a/src/lib/rc/st24.c
+++ b/src/lib/rc/st24.c
@@ -39,6 +39,23 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include <stdbool.h>
+#include "st24.h"
+
+enum ST24_DECODE_STATE {
+	ST24_DECODE_STATE_UNSYNCED,
+	ST24_DECODE_STATE_GOT_STX1,
+	ST24_DECODE_STATE_GOT_STX2,
+	ST24_DECODE_STATE_GOT_LEN,
+	ST24_DECODE_STATE_GOT_TYPE,
+	ST24_DECODE_STATE_GOT_DATA
+};
+
+static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
+static unsigned _rxlen;
+
+static ReceiverFcPacket _rxpacket;
+
 uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
 {
 	uint8_t i, crc ;
@@ -66,6 +83,105 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
 }
 
 
-uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) {
+uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count)
+{
+
+	bool ret = false;
+
+	switch (_decode_state) {
+		case ST24_DECODE_STATE_UNSYNCED:
+			if (byte == ST24_STX1) {
+				_decode_state = ST24_DECODE_STATE_GOT_STX1;
+			}
+			break;
+
+		case ST24_DECODE_STATE_GOT_STX1:
+			if (byte == ST24_STX2) {
+				_decode_state = ST24_DECODE_STATE_GOT_STX2;
+			} else {
+				_decode_state = ST24_DECODE_STATE_UNSYNCED;
+			}
+			break;
+
+		case ST24_DECODE_STATE_GOT_STX2:
+			_rxpacket.length = byte;
+			_rxlen = 0;
+			_decode_state = ST24_DECODE_STATE_GOT_LEN;
+			break;
+
+		case ST24_DECODE_STATE_GOT_LEN:
+			_rxpacket.type = byte;
+			_rxlen++;
+			_decode_state = ST24_DECODE_STATE_GOT_TYPE;
+			break;
+
+		case ST24_DECODE_STATE_GOT_TYPE:
+			if (_rxlen < (_rxpacket.length - 1)) {
+				_rxpacket.st24_data[_rxlen] = byte;
+				_rxlen++;
+			} else {
+				_decode_state = ST24_DECODE_STATE_GOT_DATA;
+			}
+			break;
+
+		case ST24_DECODE_STATE_GOT_DATA:
+			_rxpacket.crc8 = byte;
+			_rxlen++;
+
+			if (st24_common_crc8((uint8_t*)&(_rxpacket.length), sizeof(_rxpacket.length) +
+				sizeof(_rxpacket.st24_data) + sizeof(_rxpacket.type)) == _rxpacket.crc8) {
 
-}
\ No newline at end of file
+				ret = true;
+
+				/* decode the actual packet */
+
+				switch (_rxpacket.type) {
+
+					case ST24_PACKET_TYPE_CHANNELDATA12:
+						{
+							ChannelData12* d = (ChannelData12*)&_rxpacket;
+
+							*rssi = d->rssi;
+							*rx_count = d->packet_count;
+
+							for (unsigned i = 0; i < 1; i += 2) {
+								channels[i] = ((uint16_t)d->channel[i]) << 8;
+								channels[i] |= (0xF & d->channel[i+1]);
+
+								channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4;
+								channels[i+1] |= d->channel[i+2];
+							}
+						}
+						break;
+
+					case ST24_PACKET_TYPE_CHANNELDATA24:
+						{
+							ChannelData24* d = (ChannelData12*)&_rxpacket;
+
+							*rssi = d->rssi;
+							*rx_count = d->packet_count;
+
+							for (unsigned i = 0; i < 1; i += 2) {
+								channels[i] = ((uint16_t)d->channel[i]) << 8;
+								channels[i] |= (0xF & d->channel[i+1]);
+
+								channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4;
+								channels[i+1] |= d->channel[i+2];
+							}
+						}
+						break;
+
+					default:
+						ret = false;
+						break;
+				}
+
+			} else {
+				/* decoding failed */
+				_decode_state = ST24_DECODE_STATE_UNSYNCED;
+			}
+			break;
+	}
+
+	return ret;
+}
diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h
index 3621e8506..24dbf8e51 100644
--- a/src/lib/rc/st24.h
+++ b/src/lib/rc/st24.h
@@ -39,22 +39,30 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#pragma once
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
 #define ST24_DATA_LEN_MAX	64
+#define ST24_STX1		0x55
+#define ST24_STX2		0x55
 
-enum {
+enum ST24_PACKET_TYPE {
 	ST24_PACKET_TYPE_CHANNELDATA12 = 0,
 	ST24_PACKET_TYPE_CHANNELDATA24,
 	ST24_PACKET_TYPE_TRANSMITTERGPSDATA
-} ST24_PACKET_TYPE;
+};
 
 #pragma pack(push, 1)
 typedef struct {
-	uint8_t	header1;				///< 0x55 for a valid packet
-	uint8_t	header2;				///< 0x55 for a valid packet
+	uint8_t	header1;			///< 0x55 for a valid packet
+	uint8_t	header2;			///< 0x55 for a valid packet
 	uint8_t	length;				///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8)
-	uint8_t	type;					///< from enum ST24_PACKET_TYPE
+	uint8_t	type;				///< from enum ST24_PACKET_TYPE
 	uint8_t	st24_data[ST24_DATA_LEN_MAX];
-	uint8_t	crc8;					///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
+	uint8_t	crc8;				///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
 } ReceiverFcPacket;
 
 /**
@@ -63,8 +71,8 @@ typedef struct {
  * This is incoming from the ST24
  */
 typedef struct {
-	uint16_t	t;						///< packet counter or clock
-	uint8_t	rssi					///< signal strength
+	uint16_t t;			///< packet counter or clock
+	uint8_t	rssi;			///< signal strength
 	uint8_t	packet_count;		///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
 	uint8_t	channel[18];		///< channel data, 12 channels (12 bit numbers)
 } ChannelData12;
@@ -74,8 +82,8 @@ typedef struct {
  *
  */
 typedef struct {
-	uint16_t	t;						///< packet counter or clock
-	uint8_t	rssi					///< signal strength
+	uint16_t t;			///< packet counter or clock
+	uint8_t	rssi;			///< signal strength
 	uint8_t	packet_count;		///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
 	uint8_t	channel[36];		///< channel data, 24 channels (12 bit numbers)
 } ChannelData24;
@@ -114,17 +122,17 @@ typedef struct {
  *
  */
 typedef struct {
-	uint16_t t;						///< packet counter or clock
-	int32_t	lat;					///< lattitude (degrees)	+/- 90 deg
-	int32_t	lon;					///< longitude (degrees)	+/- 180 deg
-	int32_t	alt;					///< 0.01m resolution, altitude (meters)
+	uint16_t t;			///< packet counter or clock
+	int32_t	lat;			///< lattitude (degrees)	+/- 90 deg
+	int32_t	lon;			///< longitude (degrees)	+/- 180 deg
+	int32_t	alt;			///< 0.01m resolution, altitude (meters)
 	int16_t	vx, vy, vz; 		///< velocity 0.01m res, +/-320.00 North-East- Down
-	uint8_t	nsat;					///<number of satellites
-	uint8_t	voltage; 			///< 25.4V	voltage = 5 + 255*0.1 = 30.5V, min=5V
-	uint8_t	current; 			///< 0.5A resolution
+	uint8_t	nsat;			///<number of satellites
+	uint8_t	voltage; 		///< 25.4V	voltage = 5 + 255*0.1 = 30.5V, min=5V
+	uint8_t	current; 		///< 0.5A resolution
 	int16_t	roll, pitch, yaw;	///< 0.01 degree resolution
 	uint8_t	motorStatus;		///< 1 bit per motor for status 1=good, 0= fail
-	uint8_t	imuStatus;			///< inertial measurement unit status
+	uint8_t	imuStatus;		///< inertial measurement unit status
 	uint8_t	pressCompassStatus;	///< baro / compass status
 } TelemetryData;
 
@@ -150,3 +158,5 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len);
  * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error
  */
 __EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count);
+
+__END_DECLS
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 185cb20dd..3aa3d28ff 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -43,6 +43,7 @@
 #include <drivers/drv_hrt.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/ppm_decode.h>
+#include <rc/st24.h>
 
 #include "px4io.h"
 
@@ -51,11 +52,21 @@
 #define RC_CHANNEL_LOW_THRESH		-8000	/* 10% threshold */
 
 static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
+static bool	dsm_port_input(void);
 
 static perf_counter_t c_gather_dsm;
 static perf_counter_t c_gather_sbus;
 static perf_counter_t c_gather_ppm;
 
+static int _dsm_fd;
+
+bool dsm_port_input()
+{
+	/* get data from FD and attempt to parse with DSM and ST24 libs */
+
+	return false;
+}
+
 void
 controls_init(void)
 {
@@ -65,7 +76,7 @@ controls_init(void)
 	system_state.rc_channels_timestamp_valid = 0;
 
 	/* DSM input (USART1) */
-	dsm_init("/dev/ttyS0");
+	_dsm_fd = dsm_init("/dev/ttyS0");
 
 	/* S.bus input (USART3) */
 	sbus_init("/dev/ttyS2");
@@ -175,6 +186,18 @@ controls_tick() {
 	}
 	perf_end(c_gather_ppm);
 
+	uint8_t st24_rssi, rx_count;
+	uint8_t st24_maxchans = 18;
+	bool st24_updated = st24_decode(0, &st24_rssi, &rx_count, r_raw_rc_values, st24_maxchans);
+	if (st24_updated) {
+
+		rssi = st24_rssi;
+
+		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);
+	}
+
 	/* 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;
@@ -191,7 +214,7 @@ controls_tick() {
 	/*
 	 * If we received a new frame from any of the RC sources, process it.
 	 */
-	if (dsm_updated || sbus_updated || ppm_updated) {
+	if (dsm_updated || sbus_updated || ppm_updated || st24_updated) {
 
 		/* record a bitmask of channels assigned */
 		unsigned assigned_channels = 0;
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 01869569f..eb99e8a96 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -14,7 +14,8 @@ SRCS		= adc.c \
 		  ../systemlib/mixer/mixer_group.cpp \
 		  ../systemlib/mixer/mixer_multirotor.cpp \
 		  ../systemlib/mixer/mixer_simple.cpp \
-		  ../systemlib/pwm_limit/pwm_limit.c
+		  ../systemlib/pwm_limit/pwm_limit.c \
+		  ../../lib/rc/st24.c
 
 ifeq ($(BOARD),px4io-v1)
 SRCS		+= i2c.c
-- 
cgit v1.2.3


From c2687a777444f68e7a7685ec628848b950f02571 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 7 Oct 2014 08:05:45 +0200
Subject: ST24 integration in IO firmware

---
 src/modules/px4iofirmware/controls.c | 73 ++++++++++++++++++++++--------------
 src/modules/px4iofirmware/dsm.c      | 10 ++++-
 src/modules/px4iofirmware/protocol.h |  1 +
 src/modules/px4iofirmware/px4io.h    |  2 +-
 4 files changed, 54 insertions(+), 32 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 3aa3d28ff..1b7be692a 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -52,7 +52,7 @@
 #define RC_CHANNEL_LOW_THRESH		-8000	/* 10% threshold */
 
 static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
-static bool	dsm_port_input(void);
+static bool	dsm_port_input(uint8_t *rssi);
 
 static perf_counter_t c_gather_dsm;
 static perf_counter_t c_gather_sbus;
@@ -60,9 +60,49 @@ static perf_counter_t c_gather_ppm;
 
 static int _dsm_fd;
 
-bool dsm_port_input()
+bool dsm_port_input(uint8_t *rssi)
 {
+	perf_begin(c_gather_dsm);
+	uint16_t temp_count = r_raw_rc_count;
+	uint8_t n_bytes = 0;
+	uint8_t *bytes;
+	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
+	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);
+
 	/* get data from FD and attempt to parse with DSM and ST24 libs */
+	uint8_t st24_rssi, rx_count;
+	uint16_t st24_channel_count;
+	uint8_t st24_maxchans = 18;
+
+	bool st24_updated = false;
+
+	for (unsigned i = 0; i < n_bytes; i++) {
+		/* set updated flag if one complete packet was parsed */
+		st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count,
+					&st24_channel_count, r_raw_rc_values, st24_maxchans);
+	}
+
+	if (st24_updated) {
+
+		*rssi = st24_rssi;
+		r_raw_rc_count = st24_channel_count;
+
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+	}
 
 	return false;
 }
@@ -127,20 +167,7 @@ controls_tick() {
 #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);
-
-	}
+	(void)dsm_port_input(&rssi);
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
@@ -186,18 +213,6 @@ controls_tick() {
 	}
 	perf_end(c_gather_ppm);
 
-	uint8_t st24_rssi, rx_count;
-	uint8_t st24_maxchans = 18;
-	bool st24_updated = st24_decode(0, &st24_rssi, &rx_count, r_raw_rc_values, st24_maxchans);
-	if (st24_updated) {
-
-		rssi = st24_rssi;
-
-		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);
-	}
-
 	/* 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;
@@ -402,7 +417,7 @@ controls_tick() {
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 
 			/* mix new RC input control values to servos */
-			if (dsm_updated || sbus_updated || ppm_updated)
+			if (dsm_updated || sbus_updated || ppm_updated || st24_updated)
 				mixer_tick();
 
 		} else {
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index d3f365822..6e57c9ec7 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -452,10 +452,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
  *
  * @param[out] values pointer to per channel array of decoded values
  * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
+ * @param[out] n_butes number of bytes read
+ * @param[out] bytes pointer to the buffer of read bytes
  * @return true=decoded raw channel values updated, false=no update
  */
 bool
-dsm_input(uint16_t *values, uint16_t *num_values)
+dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -478,8 +480,12 @@ dsm_input(uint16_t *values, uint16_t *num_values)
 	ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
 
 	/* if the read failed for any reason, just give up here */
-	if (ret < 1)
+	if (ret < 1) {
 		return false;
+	} else {
+		*n_bytes = ret;
+		*bytes = &dsm_frame[dsm_partial_frame_count];
+	}
 
 	dsm_last_rx_time = now;
 
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 4739f6e40..e907d2959 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -112,6 +112,7 @@
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
 #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED	(1 << 13) /* FMU was initialized and OK once */
+#define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 14) /* SBUS input is valid */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index b00a96717..e32fcc72b 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -215,7 +215,7 @@ extern uint16_t	adc_measure(unsigned channel);
 extern void	controls_init(void);
 extern void	controls_tick(void);
 extern int	dsm_init(const char *device);
-extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
+extern bool	dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
-- 
cgit v1.2.3


From 35caa8bd996392924ab1d92eb46c3d40e0a91b28 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 7 Oct 2014 09:28:36 +0200
Subject: PX4IO Controls: compile fixes

---
 src/modules/px4iofirmware/controls.c | 19 ++++++++++---------
 1 file changed, 10 insertions(+), 9 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 1b7be692a..8670e30c2 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -52,7 +52,7 @@
 #define RC_CHANNEL_LOW_THRESH		-8000	/* 10% threshold */
 
 static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
-static bool	dsm_port_input(uint8_t *rssi);
+static bool	dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated);
 
 static perf_counter_t c_gather_dsm;
 static perf_counter_t c_gather_sbus;
@@ -60,14 +60,14 @@ static perf_counter_t c_gather_ppm;
 
 static int _dsm_fd;
 
-bool dsm_port_input(uint8_t *rssi)
+bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated)
 {
 	perf_begin(c_gather_dsm);
 	uint16_t temp_count = r_raw_rc_count;
 	uint8_t n_bytes = 0;
 	uint8_t *bytes;
-	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
-	if (dsm_updated) {
+	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
+	if (*dsm_updated) {
 		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 		r_raw_rc_count = temp_count & 0x7fff;
 		if (temp_count & 0x8000)
@@ -86,15 +86,15 @@ bool dsm_port_input(uint8_t *rssi)
 	uint16_t st24_channel_count;
 	uint8_t st24_maxchans = 18;
 
-	bool st24_updated = false;
+	*st24_updated = false;
 
 	for (unsigned i = 0; i < n_bytes; i++) {
 		/* set updated flag if one complete packet was parsed */
-		st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count,
+		*st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count,
 					&st24_channel_count, r_raw_rc_values, st24_maxchans);
 	}
 
-	if (st24_updated) {
+	if (*st24_updated) {
 
 		*rssi = st24_rssi;
 		r_raw_rc_count = st24_channel_count;
@@ -104,7 +104,7 @@ bool dsm_port_input(uint8_t *rssi)
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 	}
 
-	return false;
+	return (*dsm_updated | *st24_updated);
 }
 
 void
@@ -167,7 +167,8 @@ controls_tick() {
 #endif
 
 	perf_begin(c_gather_dsm);
-	(void)dsm_port_input(&rssi);
+	bool dsm_updated, st24_updated;
+	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
-- 
cgit v1.2.3


From c7e8570f8355cb96b4daf4f1d3283807ae0cbdc5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 7 Oct 2014 09:36:24 +0200
Subject: PX4IO firmware: Fix comment

---
 src/modules/px4iofirmware/protocol.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index e907d2959..89a470b44 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -112,7 +112,7 @@
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
 #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED	(1 << 13) /* FMU was initialized and OK once */
-#define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 14) /* SBUS input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 14) /* ST24 input is valid */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
-- 
cgit v1.2.3


From 6436db1a99888aff3802008c91f8a6c151f7da9c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 7 Oct 2014 22:24:32 +0200
Subject: Fix parser return type handling

---
 src/modules/px4iofirmware/controls.c | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 8670e30c2..622f20c7d 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -83,15 +83,14 @@ bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated)
 
 	/* get data from FD and attempt to parse with DSM and ST24 libs */
 	uint8_t st24_rssi, rx_count;
-	uint16_t st24_channel_count;
-	uint8_t st24_maxchans = 18;
+	uint16_t st24_channel_count = 0;
 
 	*st24_updated = false;
 
 	for (unsigned i = 0; i < n_bytes; i++) {
 		/* set updated flag if one complete packet was parsed */
-		*st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count,
-					&st24_channel_count, r_raw_rc_values, st24_maxchans);
+		*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
+					&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
 	}
 
 	if (*st24_updated) {
-- 
cgit v1.2.3


From 7bb9b3efa71107c8988f6933720365b42c8a4046 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 9 Oct 2014 09:27:25 +0200
Subject: IO firmware hot fix: Use right pointer type for RSSI value.

---
 src/modules/px4iofirmware/controls.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 622f20c7d..7b127759a 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -52,7 +52,7 @@
 #define RC_CHANNEL_LOW_THRESH		-8000	/* 10% threshold */
 
 static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
-static bool	dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated);
+static bool	dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated);
 
 static perf_counter_t c_gather_dsm;
 static perf_counter_t c_gather_sbus;
@@ -60,7 +60,7 @@ static perf_counter_t c_gather_ppm;
 
 static int _dsm_fd;
 
-bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated)
+bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 {
 	perf_begin(c_gather_dsm);
 	uint16_t temp_count = r_raw_rc_count;
-- 
cgit v1.2.3


From 0d917576d484d2e2dc0233c5545a16e36f6e2f41 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 15 Oct 2014 22:19:06 +0200
Subject: Enable flaps in manual override

---
 src/drivers/px4io/px4io.cpp          |  9 ++++++++-
 src/modules/px4iofirmware/controls.c | 14 +++++++++++---
 2 files changed, 19 insertions(+), 4 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index d212be766..73160b2d9 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1265,11 +1265,18 @@ PX4IO::io_set_rc_config()
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 3;
 
-	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
+	param_get(param_find("RC_MAP_FLAPS"), &ichan);
 
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 4;
 
+	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
+
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
+		/* use out of normal bounds index to indicate special channel */
+		input_map[ichan - 1] = 8;
+	}
+
 	/*
 	 * Iterate all possible RC inputs.
 	 */
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 7b127759a..0b0832d55 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -60,6 +60,8 @@ static perf_counter_t c_gather_ppm;
 
 static int _dsm_fd;
 
+static uint16_t rc_value_override = 0;
+
 bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 {
 	perf_begin(c_gather_dsm);
@@ -313,8 +315,14 @@ controls_tick() {
 						}
 					}
 
-					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-					assigned_channels |= (1 << mapped);
+					/* pick out override channel, indicated by mapping 8 (9 th channel, virtual) */
+					if (mapped == 8) {
+						rc_value_override = SIGNED_TO_REG(scaled);
+					} else {
+						/* normal channel */
+						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+						assigned_channels |= (1 << mapped);
+					}
 
 				}
 			}
@@ -409,7 +417,7 @@ controls_tick() {
 		 * requested override.
 		 *
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
 			override = true;
 
 		if (override) {
-- 
cgit v1.2.3


From f4903316329ca91b9e872327f701de5b6a2cc13c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 16 Oct 2014 22:57:27 +0200
Subject: Enable flaps, avoid mode switch position

---
 src/drivers/px4io/px4io.cpp           |  2 +-
 src/modules/px4iofirmware/controls.c  | 13 +++++--------
 src/modules/px4iofirmware/mixer.cpp   |  7 -------
 src/modules/px4iofirmware/protocol.h  |  1 +
 src/modules/px4iofirmware/registers.c |  3 ++-
 5 files changed, 9 insertions(+), 17 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 73160b2d9..3871b4a2c 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1274,7 +1274,7 @@ PX4IO::io_set_rc_config()
 
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
 		/* use out of normal bounds index to indicate special channel */
-		input_map[ichan - 1] = 8;
+		input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
 	}
 
 	/*
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 0b0832d55..ad60ee03e 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -315,15 +315,12 @@ controls_tick() {
 						}
 					}
 
-					/* pick out override channel, indicated by mapping 8 (9 th channel, virtual) */
-					if (mapped == 8) {
-						rc_value_override = SIGNED_TO_REG(scaled);
-					} else {
-						/* normal channel */
-						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-						assigned_channels |= (1 << mapped);
-					}
+					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+					assigned_channels |= (1 << mapped);
 
+				} else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) {
+					/* pick out override channel, indicated by special mapping */
+					rc_value_override = SIGNED_TO_REG(scaled);
 				}
 			}
 		}
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 3e19333d8..c0b8ac358 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -60,13 +60,6 @@ extern "C" {
  */
 #define FMU_INPUT_DROP_LIMIT_US		500000
 
-/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
-#define ROLL     0
-#define PITCH    1
-#define YAW      2
-#define THROTTLE 3
-#define OVERRIDE 4
-
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
 static bool should_arm = false;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 89a470b44..a3e8a58d3 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -246,6 +246,7 @@ enum {							/* DSM bind states */
 #define PX4IO_P_RC_CONFIG_MAX			2		/**< highest input value */
 #define PX4IO_P_RC_CONFIG_DEADZONE		3		/**< band around center that is ignored */
 #define PX4IO_P_RC_CONFIG_ASSIGNMENT		4		/**< mapped input value */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH	100		/**< magic value for mode switch */
 #define PX4IO_P_RC_CONFIG_OPTIONS		5		/**< channel options bitmask */
 #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED	(1 << 0)
 #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE	(1 << 1)
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 7a5a5e484..7f19e983f 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -686,7 +686,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
 					disabled = true;
-				} else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
+				} else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
+					   (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
 					count++;
 				}
 
-- 
cgit v1.2.3


From a6c6ae023de74f87ee593e8c914d66e7a8be7011 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 29 Oct 2014 14:04:33 +0100
Subject: Autoformatting (reviewed) IO sbus handler. No functionality changes.

---
 src/modules/px4iofirmware/sbus.c | 55 ++++++++++++++++++++++++----------------
 1 file changed, 33 insertions(+), 22 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 0f0414148..6ead38d61 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -61,7 +61,7 @@
 /*
   Measured values with Futaba FX-30/R6108SB:
     -+100% on TX:  PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700  (100% ATV)
-    -+140% on TX:  PCM 0.930/1.520/2.112ms -> SBus raw values:  78/1024/1964  (140% ATV)  
+    -+140% on TX:  PCM 0.930/1.520/2.112ms -> SBus raw values:  78/1024/1964  (140% ATV)
     -+152% on TX:  PCM 0.884/1.520/2.160ms -> SBus raw values:   1/1024/2047  (140% ATV plus dirty tricks)
 */
 
@@ -87,13 +87,15 @@ static unsigned partial_frame_count;
 
 unsigned sbus_frame_drops;
 
-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe,
+			bool *sbus_frame_drop, uint16_t max_channels);
 
 int
 sbus_init(const char *device)
 {
-	if (sbus_fd < 0)
+	if (sbus_fd < 0) {
 		sbus_fd = open(device, O_RDWR | O_NONBLOCK);
+	}
 
 	if (sbus_fd >= 0) {
 		struct termios t;
@@ -113,6 +115,7 @@ sbus_init(const char *device)
 	} else {
 		debug("S.Bus: open failed");
 	}
+
 	return sbus_fd;
 }
 
@@ -167,8 +170,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
 	ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
 
 	/* if the read failed for any reason, just give up here */
-	if (ret < 1)
+	if (ret < 1) {
 		return false;
+	}
 
 	last_rx_time = now;
 
@@ -180,8 +184,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
 	/*
 	 * If we don't have a full frame, return
 	 */
-	if (partial_frame_count < SBUS_FRAME_SIZE)
+	if (partial_frame_count < SBUS_FRAME_SIZE) {
 		return false;
+	}
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
@@ -228,7 +233,8 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 };
 
 static bool
-sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
+	    uint16_t max_values)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
 	if ((frame[0] != 0x0f)) {
@@ -237,23 +243,27 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 	}
 
 	switch (frame[24]) {
-		case 0x00:
+	case 0x00:
 		/* this is S.BUS 1 */
 		break;
-		case 0x03:
+
+	case 0x03:
 		/* S.BUS 2 SLOT0: RX battery and external voltage */
 		break;
-		case 0x83:
+
+	case 0x83:
 		/* S.BUS 2 SLOT1 */
 		break;
-		case 0x43:
-		case 0xC3:
-		case 0x23:
-		case 0xA3:
-		case 0x63:
-		case 0xE3:
+
+	case 0x43:
+	case 0xC3:
+	case 0x23:
+	case 0xA3:
+	case 0x63:
+	case 0xE3:
 		break;
-		default:
+
+	default:
 		/* we expect one of the bits above, but there are some we don't know yet */
 		break;
 	}
@@ -283,7 +293,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 
 
 		/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
-		values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
+		values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
 	}
 
 	/* decode switch channels if data fields are wide enough */
@@ -304,16 +314,17 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 		/* report that we failed to read anything valid off the receiver */
 		*sbus_failsafe = true;
 		*sbus_frame_drop = true;
-	}
-	else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
+
+	} else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
 		/* set a special warning flag
-		 * 
-		 * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this 
-		 * condition as fail-safe greatly reduces the reliability and range of the radio link, 
+		 *
+		 * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
+		 * condition as fail-safe greatly reduces the reliability and range of the radio link,
 		 * e.g. by prematurely issueing return-to-launch!!! */
 
 		*sbus_failsafe = false;
 		*sbus_frame_drop = true;
+
 	} else {
 		*sbus_failsafe = false;
 		*sbus_frame_drop = false;
-- 
cgit v1.2.3


From dd23d0acbcce9f4c79e120ec782a522164a25d83 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 9 Oct 2014 15:35:18 +1100
Subject: drivers: allow forcing the safety switch on

This allows forcing the safety switch to the on position from software
which stops the pwm outputs
---
 src/drivers/drv_pwm_output.h          | 3 +++
 src/drivers/px4fmu/fmu.cpp            | 1 +
 src/drivers/px4io/px4io.cpp           | 5 +++++
 src/modules/px4iofirmware/protocol.h  | 1 +
 src/modules/px4iofirmware/registers.c | 6 ++++++
 5 files changed, 16 insertions(+)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index f66ec7c95..6873f24b6 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -213,6 +213,9 @@ ORB_DECLARE(output_pwm);
 /** make failsafe non-recoverable (termination) if it occurs */
 #define PWM_SERVO_SET_TERMINATION_FAILSAFE	_IOC(_PWM_SERVO_BASE, 25)
 
+/** force safety switch on (to enable use of safety switch) */
+#define PWM_SERVO_SET_FORCE_SAFETY_ON  _IOC(_PWM_SERVO_BASE, 26)
+
 /*
  *
  *
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 122a3cd17..3d3e1b0eb 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -829,6 +829,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
 	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
+	case PWM_SERVO_SET_FORCE_SAFETY_ON:
 		// these are no-ops, as no safety switch
 		break;
 
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 3871b4a2c..fd9eb4170 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -2278,6 +2278,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
 		break;
 
+	case PWM_SERVO_SET_FORCE_SAFETY_ON:
+		/* force safety switch on */
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
+		break;
+
 	case PWM_SERVO_SET_FORCE_FAILSAFE:
 		/* force failsafe mode instantly */
 		if (arg == 0) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index a3e8a58d3..9b2e047cb 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -221,6 +221,7 @@ enum {							/* DSM bind states */
                                                            hence index 12 can safely be used. */
 #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */
 
+#define PX4IO_P_SETUP_FORCE_SAFETY_ON		14	/* force safety switch into 'disarmed' (PWM disabled state) */
 #define PX4IO_FORCE_SAFETY_MAGIC		22027	/* required argument for force safety (random) */
 
 /* autopilot control values, -10000..10000 */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 7f19e983f..49c2a9f56 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -603,6 +603,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			dsm_bind(value & 0x0f, (value >> 4) & 0xF);
 			break;
 
+		case PX4IO_P_SETUP_FORCE_SAFETY_ON:
+			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
+				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+			}
+			break;
+
 		case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
 			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
-- 
cgit v1.2.3


From 2cadd45f307750bb68c3f926d7d9830f680bd94b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 2 Nov 2014 10:13:11 +0100
Subject: Configure led ring and enable heartbeat on it

---
 src/drivers/boards/px4io-v2/board_config.h | 1 +
 src/drivers/boards/px4io-v2/px4iov2_init.c | 1 +
 src/modules/px4iofirmware/px4io.c          | 2 ++
 src/modules/px4iofirmware/px4io.h          | 1 +
 4 files changed, 5 insertions(+)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
index ef9bb5cad..10a93be0b 100644
--- a/src/drivers/boards/px4io-v2/board_config.h
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -77,6 +77,7 @@
 #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
 #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
 #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
 
 /* Safety switch button *******************************************************/
 
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
index 9f8c0eeb2..5c3343ccc 100644
--- a/src/drivers/boards/px4io-v2/px4iov2_init.c
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -108,6 +108,7 @@ __EXPORT void stm32_boardinitialize(void)
 	stm32_configgpio(GPIO_LED1);
 	stm32_configgpio(GPIO_LED2);
 	stm32_configgpio(GPIO_LED3);
+	stm32_configgpio(GPIO_LED4);
 
 	stm32_configgpio(GPIO_BTN_SAFETY);
 
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index cd134ccb4..f978ff406 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -124,6 +124,7 @@ heartbeat_blink(void)
 {
 	static bool heartbeat = false;
 	LED_BLUE(heartbeat = !heartbeat);
+	LED_RING(heartbeat);
 }
 
 static uint64_t reboot_time;
@@ -191,6 +192,7 @@ user_start(int argc, char *argv[])
 	LED_AMBER(false);
 	LED_BLUE(false);
 	LED_SAFETY(false);
+	LED_RING(false);
 
 	/* turn on servo power (if supported) */
 #ifdef POWER_SERVO
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index e32fcc72b..8186e4c78 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -140,6 +140,7 @@ extern pwm_limit_t pwm_limit;
 #define LED_BLUE(_s)			stm32_gpiowrite(GPIO_LED1, !(_s))
 #define LED_AMBER(_s)			stm32_gpiowrite(GPIO_LED2, !(_s))
 #define LED_SAFETY(_s)			stm32_gpiowrite(GPIO_LED3, !(_s))
+#define LED_RING(_s)			stm32_gpiowrite(GPIO_LED4, !(_s))
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 
-- 
cgit v1.2.3


From 75d0ffe4e5b7659e20bbbdcc1e840e06dfe7a138 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 2 Nov 2014 12:34:33 +0100
Subject: Build fix

---
 src/modules/px4iofirmware/px4io.c | 4 ++++
 1 file changed, 4 insertions(+)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index f978ff406..30f32b38e 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -124,7 +124,9 @@ heartbeat_blink(void)
 {
 	static bool heartbeat = false;
 	LED_BLUE(heartbeat = !heartbeat);
+#ifdef GPIO_LED4
 	LED_RING(heartbeat);
+#endif
 }
 
 static uint64_t reboot_time;
@@ -192,7 +194,9 @@ user_start(int argc, char *argv[])
 	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
-- 
cgit v1.2.3


From 02d31522cde1de13ab397f4fbd77c0cf9b7f712d Mon Sep 17 00:00:00 2001
From: Daniel Shiels <diyd5@tofubar.com>
Date: Wed, 29 Oct 2014 22:00:30 +1100
Subject: First attempt at sbus1 output.

---
 src/modules/px4iofirmware/sbus.c | 37 ++++++++++++++++++++++++++++++++++---
 1 file changed, 34 insertions(+), 3 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 6ead38d61..23dae7898 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -80,8 +80,10 @@ static int sbus_fd = -1;
 
 static hrt_abstime last_rx_time;
 static hrt_abstime last_frame_time;
+static hrt_abstime last_txframe_time=0;
 
 static uint8_t	frame[SBUS_FRAME_SIZE];
+static uint8_t	oframe[SBUS_FRAME_SIZE];
 
 static unsigned partial_frame_count;
 
@@ -122,10 +124,39 @@ sbus_init(const char *device)
 void
 sbus1_output(uint16_t *values, uint16_t num_values)
 {
-	char a = 'A';
-	write(sbus_fd, &a, 1);
-}
+	//int. first byte of data is offset 1 in sbus
+	uint8_t byteindex=1;
+	uint8_t offset=0;
+	uint16_t value;
+	hrt_abstime	now;
+	//oframe[0]=0xf0;
+	oframe[0]=0x0f;
 
+        now = hrt_absolute_time();
+        if ((now - last_txframe_time) > 14000) {
+		last_txframe_time = now;
+
+		for (uint16_t i=1;i<SBUS_FRAME_SIZE;++i) {
+			oframe[i]=0;
+		}
+		// 16 is sbus number of servos/channels minus 2 single bit channels.
+		// currently ignoring single bit channels.
+		for (uint16_t i=0;(i<num_values)&&(i<16);++i) {
+			value=(uint16_t)(((values[i]-SBUS_SCALE_OFFSET+.5f)/SBUS_SCALE_FACTOR) +.5f);
+			//protect from out of bounds values and limit to 11 bits;
+			value&=0x07ff;
+			while (offset>=8) {
+				++byteindex;
+				offset-=8;
+			}
+			oframe[byteindex] |= (value<<(offset))&0xff;
+			oframe[byteindex+1]|=(value>>(8-offset))&0xff;
+			oframe[byteindex+2]|=(value>>(16-offset))&0xff;
+			offset+=11;
+		}	
+		write(sbus_fd, oframe, SBUS_FRAME_SIZE);
+	}
+}
 void
 sbus2_output(uint16_t *values, uint16_t num_values)
 {
-- 
cgit v1.2.3


From 60ecd8868db3539e552bad5f51473d1baf5d6ecd Mon Sep 17 00:00:00 2001
From: Daniel Shiels <diyd5@tofubar.com>
Date: Sat, 8 Nov 2014 22:36:45 +1100
Subject: sbus1 output. Cleaned up. Safer bounds checking.

---
 src/modules/px4iofirmware/sbus.c | 45 +++++++++++++++++++++++-----------------
 1 file changed, 26 insertions(+), 19 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 23dae7898..2f234234e 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -57,6 +57,7 @@
 #define SBUS_FLAGS_BYTE		23
 #define SBUS_FAILSAFE_BIT	3
 #define SBUS_FRAMELOST_BIT	2
+#define SBUS1_FRAME_DELAY	14000
 
 /*
   Measured values with Futaba FX-30/R6108SB:
@@ -80,7 +81,7 @@ static int sbus_fd = -1;
 
 static hrt_abstime last_rx_time;
 static hrt_abstime last_frame_time;
-static hrt_abstime last_txframe_time=0;
+static hrt_abstime last_txframe_time = 0;
 
 static uint8_t	frame[SBUS_FRAME_SIZE];
 static uint8_t	oframe[SBUS_FRAME_SIZE];
@@ -125,35 +126,41 @@ void
 sbus1_output(uint16_t *values, uint16_t num_values)
 {
 	//int. first byte of data is offset 1 in sbus
-	uint8_t byteindex=1;
-	uint8_t offset=0;
+	uint8_t byteindex = 1;
+	uint8_t offset = 0;
 	uint16_t value;
 	hrt_abstime	now;
-	//oframe[0]=0xf0;
-	oframe[0]=0x0f;
+	oframe[0] = 0x0f;
 
-        now = hrt_absolute_time();
-        if ((now - last_txframe_time) > 14000) {
+	now = hrt_absolute_time();
+
+	if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
 		last_txframe_time = now;
 
-		for (uint16_t i=1;i<SBUS_FRAME_SIZE;++i) {
-			oframe[i]=0;
+		for (uint16_t i = 1; i < SBUS_FRAME_SIZE; ++i) {
+			oframe[i] = 0;
 		}
+
 		// 16 is sbus number of servos/channels minus 2 single bit channels.
 		// currently ignoring single bit channels.
-		for (uint16_t i=0;(i<num_values)&&(i<16);++i) {
-			value=(uint16_t)(((values[i]-SBUS_SCALE_OFFSET+.5f)/SBUS_SCALE_FACTOR) +.5f);
+		for (uint16_t i = 0; (i < num_values) && (i < 16); ++i) {
+			value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
 			//protect from out of bounds values and limit to 11 bits;
-			value&=0x07ff;
-			while (offset>=8) {
+			if (value > 0x07ff ) {
+				value = 0x07ff;
+			}
+
+			while (offset >= 8) {
 				++byteindex;
-				offset-=8;
+				offset -= 8;
 			}
-			oframe[byteindex] |= (value<<(offset))&0xff;
-			oframe[byteindex+1]|=(value>>(8-offset))&0xff;
-			oframe[byteindex+2]|=(value>>(16-offset))&0xff;
-			offset+=11;
-		}	
+
+			oframe[byteindex] |= (value << (offset)) & 0xff;
+			oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff;
+			oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff;
+			offset += 11;
+		}
+
 		write(sbus_fd, oframe, SBUS_FRAME_SIZE);
 	}
 }
-- 
cgit v1.2.3


From 34e9d9efce10ce487b4c5af0cf93a442ab3b8594 Mon Sep 17 00:00:00 2001
From: Daniel Shiels <diyd5@tofubar.com>
Date: Mon, 10 Nov 2014 15:54:28 +1100
Subject: Code style cleanup.

---
 src/modules/px4iofirmware/sbus.c | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 2f234234e..90c0e0503 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -125,8 +125,7 @@ sbus_init(const char *device)
 void
 sbus1_output(uint16_t *values, uint16_t num_values)
 {
-	//int. first byte of data is offset 1 in sbus
-	uint8_t byteindex = 1;
+	uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
 	uint8_t offset = 0;
 	uint16_t value;
 	hrt_abstime	now;
@@ -141,11 +140,13 @@ sbus1_output(uint16_t *values, uint16_t num_values)
 			oframe[i] = 0;
 		}
 
-		// 16 is sbus number of servos/channels minus 2 single bit channels.
-		// currently ignoring single bit channels.
-		for (uint16_t i = 0; (i < num_values) && (i < 16); ++i) {
+		/* 16 is sbus number of servos/channels minus 2 single bit channels.
+		* currently ignoring single bit channels.  */
+
+		for (unsigned i = 0; (i < num_values) && (i < 16); ++i) {
 			value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
-			//protect from out of bounds values and limit to 11 bits;
+
+			/*protect from out of bounds values and limit to 11 bits*/
 			if (value > 0x07ff ) {
 				value = 0x07ff;
 			}
-- 
cgit v1.2.3


From 3eeec2cce05c8f32a5c2d70b956786d41114df3c Mon Sep 17 00:00:00 2001
From: Daniel Shiels <diyd5@tofubar.com>
Date: Mon, 10 Nov 2014 16:34:28 +1100
Subject: Cleaned up sbus output frame initialisation.

---
 src/modules/px4iofirmware/sbus.c | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

(limited to 'src/modules/px4iofirmware')

diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 90c0e0503..d76ec55f0 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -84,7 +84,6 @@ static hrt_abstime last_frame_time;
 static hrt_abstime last_txframe_time = 0;
 
 static uint8_t	frame[SBUS_FRAME_SIZE];
-static uint8_t	oframe[SBUS_FRAME_SIZE];
 
 static unsigned partial_frame_count;
 
@@ -128,17 +127,13 @@ sbus1_output(uint16_t *values, uint16_t num_values)
 	uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
 	uint8_t offset = 0;
 	uint16_t value;
-	hrt_abstime	now;
-	oframe[0] = 0x0f;
+	hrt_abstime now;
 
 	now = hrt_absolute_time();
 
 	if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
 		last_txframe_time = now;
-
-		for (uint16_t i = 1; i < SBUS_FRAME_SIZE; ++i) {
-			oframe[i] = 0;
-		}
+		uint8_t	oframe[SBUS_FRAME_SIZE] = { 0x0f };
 
 		/* 16 is sbus number of servos/channels minus 2 single bit channels.
 		* currently ignoring single bit channels.  */
-- 
cgit v1.2.3