Checkpoint - I2C protocol register decode

1 files changed, 334 insertions, 0 deletions

diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
new file mode 100644
index 000000000..1397dd2a4
--- /dev/null
+++ b/apps/px4io/registers.c
@@ -0,0 +1,334 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file registers.c
+ *
+ * Implementation of the PX4IO register space.
+ */
+
+#include "px4io.h"
+#include "protocol.h"
+
+static int	registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
+
+/**
+ * Setup registers
+ */
+uint16_t		r_page_setup[] =
+{
+	[PX4IO_P_SETUP_ARMING]			= 0,
+	[PX4IO_P_SETUP_PWM_RATES]		= 0,
+	[PX4IO_P_SETUP_PWM_LOWRATE]		= 50,
+	[PX4IO_P_SETUP_PWM_HIGHRATE]		= 200,
+	[PX4IO_P_SETUP_RELAYS]			= 0,
+};
+
+#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_ARM_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE)
+#define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
+#define PX4IO_P_SETUP_RELAYS_VALID	((1 << PXIO_RELAY_CHANNELS) - 1)
+
+/**
+ * Control values from the FMU.
+ */
+uint16_t		r_page_controls[PX4IO_CONTROL_CHANNELS];
+
+/**
+ * Static configuration parameters.
+ */
+static const uint16_t	r_page_config[] = {
+	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= 0,
+	[PX4IO_P_CONFIG_SOFTWARE_VERSION]	= 0,
+	[PX4IO_P_CONFIG_BOOTLOADER_VERSION]	= 0,
+	[PX4IO_P_CONFIG_MAX_TRANSFER]		= 64,
+	[PX4IO_P_CONFIG_CONTROL_COUNT]		= PX4IO_CONTROL_CHANNELS,
+	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= IO_SERVO_COUNT,
+	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= MAX_CONTROL_CHANNELS,
+	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= ADC_CHANNEL_COUNT,
+	[PX4IO_P_CONFIG_RELAY_COUNT]		= PX4IO_RELAY_CHANNELS,
+};
+
+/**
+ * Status values.
+ */
+uint16_t		r_page_status[] = {
+	[PX4IO_P_STATUS_FREEMEM]		= 0,
+	[PX4IO_P_STATUS_CPULOAD]		= 0,
+	[PX4IO_P_STATUS_FLAGS]			= 0,
+	[PX4IO_P_STATUS_ALARMS]			= 0,
+	[PX4IO_P_STATUS_VBATT]			= 0,
+	[PX4IO_P_STATUS_TEMPERATURE]		= 0
+};
+
+/**
+ * ADC input buffer.
+ */
+uint16_t		r_page_adc[ADC_CHANNEL_COUNT];
+
+/**
+ * Post-mixed actuator values.
+ */
+uint16_t 		r_page_actuators[IO_SERVO_COUNT];
+
+/**
+ * Servo PWM values
+ */
+uint16_t		r_page_servos[IO_SERVO_COUNT];
+
+/**
+ * Scaled/routed RC input
+ */
+uint16_t		r_page_rc_input[MAX_CONTROL_CHANNELS];
+
+/**
+ * Raw RC input
+ */
+uint16_t		r_page_raw_rc_input[MAX_CONTROL_CHANNELS];
+
+
+void
+registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
+{
+	switch (page) {
+
+		/* handle bulk controls input */
+	case PX4IO_PAGE_CONTROLS:
+
+		/* copy channel data */
+		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+			/* XXX scaling - should be -10000..10000 */
+			r_page_controls[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+
+		/* XXX we should cause a mixer tick ASAP */
+		system_state.mixer_fmu_available = true;
+		break;
+
+		/* handle text going to the mixer parser */
+	case PX4IO_PAGE_MIXERLOAD:
+		mixer_handle_text(values, num_values * sizeof(*values));
+		break;
+
+	default:
+		/* avoid offset wrap */
+		if ((offset + num_values) > 255)
+			num_values = 255 - offset;
+
+		/* iterate individual registers, set each in turn */
+		while (num_values--) {
+			if (registers_set_one(page, offset, *values))
+				break;
+			offset++;
+			values++;
+		}
+	}
+}
+
+static int
+registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
+{
+	switch (page) {
+
+	case PX4IO_PAGE_STATUS:
+		switch (offset) {
+		case PX4IO_P_STATUS_ALARMS:
+			/* clear bits being written */
+			r_page_status[PX4IO_P_STATUS_ALARMS] &= ~value;
+			break;
+
+		default:
+			/* just ignore writes to other registers in this page */
+			break;
+		}
+		break;
+
+	case PX4IO_PAGE_SETUP:
+		switch (offset) {
+		case PX4IO_P_SETUP_ARMING:
+
+			value &= PX4IO_P_SETUP_ARMING_VALID;
+			r_page_setup[PX4IO_P_SETUP_ARMING] = value;
+
+			/* update arming state - disarm if no longer OK */
+			if (system_state.armed && !(value & PX4IO_P_SETUP_ARMING_ARM_OK))
+				system_state.armed = false;
+
+			break;
+
+		case PX4IO_P_SETUP_PWM_RATES:
+			value &= PX4IO_P_SETUP_RATES_VALID;
+			r_page_setup[PX4IO_P_SETUP_PWM_RATES] = value;
+			/* XXX re-configure timers */
+			break;
+
+		case PX4IO_P_SETUP_PWM_LOWRATE:
+			if (value < 50)
+				value = 50;
+			if (value > 400)
+				value = 400;
+			r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] = value;
+			/* XXX re-configure timers */
+			break;
+
+		case PX4IO_P_SETUP_PWM_HIGHRATE:
+			if (value < 50)
+				value = 50;
+			if (value > 400)
+				value = 400;
+			r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] = value;
+			/* XXX re-configure timers */
+			break;
+
+		case PX4IO_P_SETUP_RELAYS:
+			value &= PX4IO_P_SETUP_RELAYS_VALID;
+			r_page_setup[PX4IO_P_SETUP_RELAYS] = value;
+			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
+			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
+			POWER_ACC1(value & (1 << 2) ? 1 : 0);
+			POWER_ACC2(value & (1 << 3) ? 1 : 0);
+			break;
+
+		default:
+			return -1;
+		}
+		break;
+
+	default:
+		return -1;
+	}
+	return 0;
+}
+
+int
+registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
+{
+
+	switch (page) {
+	case PX4IO_PAGE_CONFIG:
+		*values = r_page_config;
+		*num_values = sizeof(r_page_config) / sizeof(r_page_config[0]);
+		break;
+
+	case PX4IO_PAGE_STATUS:
+		{
+			struct mallinfo minfo = mallinfo();
+			r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks;
+		}
+		/* XXX PX4IO_P_STATUS_CPULOAD */
+		r_page_status[PX4IO_P_STATUS_FLAGS] = 
+			(system_state.armed ? PX4IO_P_STATUS_FLAGS_ARMED : 0) |
+			(system_state.manual_override_ok ? PX4IO_P_STATUS_FLAGS_OVERRIDE : 0) |
+			((system_state.rc_channels > 0) ? PX4IO_P_STATUS_FLAGS_RC_OK : 0))
+			/* XXX specific receiver status */
+
+		/* XXX PX4IO_P_STATUS_ALARMS] */
+
+		{
+			/*
+			 * Coefficients here derived by measurement of the 5-16V
+			 * range on one unit:
+			 *
+			 * V   counts
+			 *  5  1001
+			 *  6  1219
+			 *  7  1436
+			 *  8  1653
+			 *  9  1870
+			 * 10  2086
+			 * 11  2303
+			 * 12  2522
+			 * 13  2738
+			 * 14  2956
+			 * 15  3172
+			 * 16  3389
+			 *
+			 * slope = 0.0046067
+			 * intercept = 0.3863
+			 *
+			 * Intercept corrected for best results @ 12V.
+			 */
+			unsigned counts = adc_measure(ADC_VBATT);
+			r_page_status[PX4IO_P_STATUS_VBATT] = (4150 + (counts * 46)) / 10;
+		}
+		/* XXX PX4IO_P_STATUS_TEMPERATURE */
+
+		*values = r_page_status;
+		*num_values = sizeof(r_page_status) / sizeof(r_page_status[0]);
+		break;
+
+	case PX4IO_PAGE_ACTUATORS:
+		*values = r_page_actuators;
+		*num_values = sizeof(r_page_actuators) / sizeof(r_page_actuators[0]);
+		break;
+
+	case PX4IO_PAGE_SERVOS:
+		*values = system_state.servos;
+		*num_values = IO_SERVO_COUNT;
+		break;
+
+	case PX4IO_PAGE_RAW_RC_INPUT:
+		*values = r_page_raw_rc_input;
+		*num_values = sizeof(r_page_raw_rc_input) / sizeof(r_page_raw_rc_input[0]);
+		break;
+
+	case PX4IO_PAGE_RC_INPUT:
+		*values = system_state.rc_channel_data;
+		*num_values = system_state.rc_channels;
+		return -1;
+
+	case PX4IO_PAGE_RAW_ADC_INPUT:
+		r_page_adc[0] = adc_measure(ADC_VBATT);
+		r_page_adc[1] = adc_measure(ADC_IN5);
+		*values = r_page_adc;
+		*num_values = ADC_CHANNEL_COUNT;
+		break;
+
+	default:
+		return -1;
+	}
+
+	/* if the offset is beyond the end of the page, we have no data */
+	if (*num_values <= offset)
+		return -1;
+
+	/* adjust value count and pointer for the page offset */
+	*num_values -= offset;
+	*values += offset;
+
+	return 0;
+}