Tweak protocol register assignments and add new registers to accommodate differences in IOv2.

src/drivers/px4io/px4io.cpp

@@ -134,6 +134,7 @@ private:
 	PX4IO_interface		*_interface;
 
 	// XXX
+	unsigned		_hardware;
 	unsigned		_max_actuators;
 	unsigned		_max_controls;
 	unsigned		_max_rc_input;
@@ -318,6 +319,7 @@ PX4IO	*g_dev;
 PX4IO::PX4IO(PX4IO_interface *interface) :
 	CDev("px4io", "/dev/px4io"),
 	_interface(interface),
+	_hardware(0),
 	_max_actuators(0),
 	_max_controls(0),
 	_max_rc_input(0),
@@ -387,18 +389,25 @@ PX4IO::init()
 		return ret;
 
 	/* get some parameters */
+	unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+	if (protocol != PX4IO_PROTOCOL_VERSION) {
+		log("protocol/firmware mismatch");
+		mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
+		return -1;
+	}
+	_hardware      = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION);
 	_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
-	_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
+	_max_controls  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
 	_max_relays    = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
 	_max_transfer  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
 	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
 	if ((_max_actuators < 1) || (_max_actuators > 255) ||
-	    (_max_relays < 1)    || (_max_relays > 255)    ||
+	    (_max_relays > 32)   ||
 	    (_max_transfer < 16) || (_max_transfer > 255)  ||
 	    (_max_rc_input < 1)  || (_max_rc_input > 255)) {
 
-		log("failed getting parameters from PX4IO");
+		log("config read error");
-		mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort.");
+		mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort.");
 		return -1;
 	}
 	if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
@@ -1122,7 +1131,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
 
 	int ret =  _interface->set_reg(page, offset, values, num_values);
 	if (ret != OK)
-		debug("io_reg_set: error %d", ret);
+		debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno);
 	return ret;
 }
 
@@ -1143,7 +1152,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v
 
 	int ret =  _interface->get_reg(page, offset, values, num_values);
 	if (ret != OK)
-		debug("io_reg_get: data error %d", ret);
+		debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno);
 	return ret;
 }
 
@@ -1237,9 +1246,9 @@ void
 PX4IO::print_status()
 {
 	/* basic configuration */
-	printf("protocol %u software %u bootloader %u buffer %uB\n",
+	printf("protocol %u hardware %u bootloader %u buffer %uB\n",
 		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION),
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
 		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
 		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
 	printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
@@ -1268,7 +1277,7 @@ PX4IO::print_status()
 		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE)  ? " FAILSAFE" : ""));
 	uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
-	printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
+	printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n",
 		alarms,
 		((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW)     ? " VBATT_LOW" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE)   ? " TEMPERATURE" : ""),
@@ -1276,18 +1285,26 @@ PX4IO::print_status()
 		((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT)   ? " ACC_CURRENT" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST)      ? " FMU_LOST" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""));
+		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""),
+		((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT)  ? " VSERVO_FAULT" : ""));
 	/* now clear alarms */
 	io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
 
-	printf("vbatt %u ibatt %u vbatt scale %u\n",
+	if (_hardware == 1) {
-	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
+		printf("vbatt %u ibatt %u vbatt scale %u\n",
-	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
-	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
-	printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
+		       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
-	       (double)_battery_amp_per_volt,
+		printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
-	       (double)_battery_amp_bias,
+		       (double)_battery_amp_per_volt,
-	       (double)_battery_mamphour_total);
+		       (double)_battery_amp_bias,
+		       (double)_battery_mamphour_total);
+	} else if (_hardware == 2) {
+		printf("vservo %u vservo scale %u\n",
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO),
+		       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
+		printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI));
+	}
 	printf("actuators");
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));

src/modules/px4iofirmware/controls.c

@@ -66,7 +66,7 @@ controls_init(void)
 	sbus_init("/dev/ttyS2");
 
 	/* default to a 1:1 input map, all enabled */
-	for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
 		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
 
 		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = 0;
@@ -117,7 +117,7 @@ controls_tick() {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
 	perf_end(c_gather_ppm);
 
-	ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
+	ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS);
 
 	/*
 	 * In some cases we may have received a frame, but input has still
@@ -190,7 +190,7 @@ controls_tick() {
 
 				/* and update the scaled/mapped version */
 				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-				ASSERT(mapped < MAX_CONTROL_CHANNELS);
+				ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
 
 				/* invert channel if pitch - pulling the lever down means pitching up by convention */
 				if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
@@ -202,7 +202,7 @@ controls_tick() {
 		}
 
 		/* set un-assigned controls to zero */
-		for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
 			if (!(assigned_channels & (1 << i)))
 				r_rc_values[i] = 0;
 		}
@@ -312,8 +312,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
 
 		/* PPM data exists, copy it */
 		*num_values = ppm_decoded_channels;
-		if (*num_values > MAX_CONTROL_CHANNELS)
+		if (*num_values > PX4IO_CONTROL_CHANNELS)
-			*num_values = MAX_CONTROL_CHANNELS;
+			*num_values = PX4IO_CONTROL_CHANNELS;
 
 		for (unsigned i = 0; i < *num_values; i++)
 			values[i] = ppm_buffer[i];

src/modules/px4iofirmware/mixer.cpp

@@ -154,7 +154,7 @@ mixer_tick(void)
 	if (source == MIX_FAILSAFE) {
 
 		/* copy failsafe values to the servo outputs */
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
 			r_page_servos[i] = r_page_servo_failsafe[i];
 
 			/* safe actuators for FMU feedback */
@@ -164,11 +164,11 @@ mixer_tick(void)
 
 	} else if (source != MIX_NONE) {
 
-		float	outputs[IO_SERVO_COUNT];
+		float	outputs[PX4IO_SERVO_COUNT];
 		unsigned mixed;
 
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 
 		/* scale to PWM and update the servo outputs as required */
 		for (unsigned i = 0; i < mixed; i++) {
@@ -180,7 +180,7 @@ mixer_tick(void)
 			r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
 
 		}
-		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 			r_page_servos[i] = 0;
 	}
 
@@ -215,7 +215,7 @@ mixer_tick(void)
 
 	if (mixer_servos_armed) {
 		/* update the servo outputs. */
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servos[i]);
 	}
 }
@@ -349,11 +349,11 @@ mixer_set_failsafe()
 		return;
 
 	/* set failsafe defaults to the values for all inputs = 0 */
-	float	outputs[IO_SERVO_COUNT];
+	float	outputs[PX4IO_SERVO_COUNT];
 	unsigned mixed;
 
 	/* mix */
-	mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+	mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 
 	/* scale to PWM and update the servo outputs as required */
 	for (unsigned i = 0; i < mixed; i++) {
@@ -364,7 +364,7 @@ mixer_set_failsafe()
 	}
 
 	/* disable the rest of the outputs */
-	for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 		r_page_servo_failsafe[i] = 0;
 
 }

src/modules/px4iofirmware/protocol.h

@@ -62,12 +62,11 @@
  * Note that the implementation of readable pages prefers registers within
  * readable pages to be densely packed. Page numbers do not need to be
  * packed.
+ *
+ * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, 
+ * [2] denotes definitions specific to the PX4IOv2 board.
  */
 
-#define PX4IO_CONTROL_CHANNELS			8
-#define PX4IO_INPUT_CHANNELS			12
-#define PX4IO_RELAY_CHANNELS			4
-
 /* Per C, this is safe for all 2's complement systems */
 #define REG_TO_SIGNED(_reg)	((int16_t)(_reg))
 #define SIGNED_TO_REG(_signed)	((uint16_t)(_signed))
@@ -75,10 +74,12 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
+#define PX4IO_PROTOCOL_VERSION		2
+
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
-#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* PX4IO_PROTOCOL_VERSION */
-#define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
+#define PX4IO_P_CONFIG_HARDWARE_VERSION		1	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
 #define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum I2C transfer size */
 #define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
@@ -107,16 +108,20 @@
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
 
 #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) /* VBatt is very close to regulator dropout */
+#define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
 #define PX4IO_P_STATUS_ALARMS_TEMPERATURE	(1 << 1) /* board temperature is high */
-#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* servo current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* [1] servo current limit was exceeded */
-#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* accessory current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* [1] accessory current limit was exceeded */
 #define PX4IO_P_STATUS_ALARMS_FMU_LOST		(1 << 4) /* timed out waiting for controls from FMU */
 #define PX4IO_P_STATUS_ALARMS_RC_LOST		(1 << 5) /* timed out waiting for RC input */
 #define PX4IO_P_STATUS_ALARMS_PWM_ERROR		(1 << 6) /* PWM configuration or output was bad */
+#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT	(1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */
 
-#define PX4IO_P_STATUS_VBATT			4	/* battery voltage in mV */
+#define PX4IO_P_STATUS_VBATT			4	/* [1] battery voltage in mV */
-#define PX4IO_P_STATUS_IBATT			5	/* battery current (raw ADC) */
+#define PX4IO_P_STATUS_IBATT			5	/* [1] battery current (raw ADC) */
+#define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
+#define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
+#define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -142,7 +147,7 @@
 #define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
 
 /* setup page */
-#define PX4IO_PAGE_SETUP			100
+#define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
@@ -155,18 +160,27 @@
 #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 */
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
+
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
-#define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_RELAYS_POWER1		(1<<0)	/* [1] power relay 1 */
+#define PX4IO_P_SETUP_RELAYS_POWER2		(1<<1)	/* [1] power relay 2 */
+#define PX4IO_P_SETUP_RELAYS_ACC1		(1<<2)	/* [1] accessory power 1 */
+#define PX4IO_P_SETUP_RELAYS_ACC2		(1<<3)	/* [1] accessory power 2 */
+
+#define PX4IO_P_SETUP_VBATT_SCALE		6	/* [1] battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_VSERVO_SCALE		6	/* [2] servo voltage correction factor (float) */
+ 					     /*	7 */
+ 					     /*	8 */
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS			101	/* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS		51		/* 0..CONFIG_CONTROL_COUNT */
 
 /* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD			102
+#define PX4IO_PAGE_MIXERLOAD		52
 
 /* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG			103	/* R/C input configuration */
+#define PX4IO_PAGE_RC_CONFIG		53		/* R/C input configuration */
 #define PX4IO_P_RC_CONFIG_MIN			0	/* lowest input value */
 #define PX4IO_P_RC_CONFIG_CENTER		1	/* center input value */
 #define PX4IO_P_RC_CONFIG_MAX			2	/* highest input value */
@@ -178,13 +192,13 @@
 #define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
 
 /* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM			104	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM		54		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM			105	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM		55		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* Debug and test page - not used in normal operation */
-#define PX4IO_PAGE_TEST				127
+#define PX4IO_PAGE_TEST			127
 #define PX4IO_P_TEST_LED			0	/* set the amber LED on/off */
 
 /**

src/modules/px4iofirmware/px4io.h

@@ -54,8 +54,9 @@
 /*
  * Constants and limits.
  */
-#define MAX_CONTROL_CHANNELS	12
+#define PX4IO_SERVO_COUNT		8
-#define IO_SERVO_COUNT		8
+#define PX4IO_CONTROL_CHANNELS		8
+#define PX4IO_INPUT_CHANNELS		12
 
 /*
  * Debug logging
@@ -124,34 +125,41 @@ extern struct sys_state_s system_state;
 /*
  * GPIO handling.
  */
-#define LED_BLUE(_s)		stm32_gpiowrite(GPIO_LED1, !(_s))
+#define LED_BLUE(_s)			stm32_gpiowrite(GPIO_LED1, !(_s))
-#define LED_AMBER(_s)		stm32_gpiowrite(GPIO_LED2, !(_s))
+#define LED_AMBER(_s)			stm32_gpiowrite(GPIO_LED2, !(_s))
-#define LED_SAFETY(_s)		stm32_gpiowrite(GPIO_LED3, !(_s))
+#define LED_SAFETY(_s)			stm32_gpiowrite(GPIO_LED3, !(_s))
+
+#ifdef CONFIG_ARCH_BOARD_PX4IOV2
+
+# define PX4IO_RELAY_CHANNELS		0
+# define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+
+# define VDD_SERVO_FAULT		(!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT	2
+# define ADC_VSERVO			4
+# define ADC_RSSI			5
+
+#else /* CONFIG_ARCH_BOARD_PX4IOV1 */
+
+# define PX4IO_RELAY_CHANNELS		4
+# define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
+# define POWER_ACC1(_s)			stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+# define POWER_ACC2(_s)			stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+# define POWER_RELAY1(_s)		stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+# define POWER_RELAY2(_s)		stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+# define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
+# define OVERCURRENT_SERVO		(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT	2
+# define ADC_VBATT			4
+# define ADC_IN5			5
 
-#ifdef GPIO_SERVO_PWR_EN
-# define POWER_SERVO(_s)	stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
-#endif
-#ifdef GPIO_ACC1_PWR_EN
-# define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
-#endif
-#ifdef GPIO_ACC2_PWR_EN
-# define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
-#endif
-#ifdef GPIO_RELAY1_EN
-# define POWER_RELAY1(_s)	stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
-#endif
-#ifdef GPIO_RELAY2_EN
-# define POWER_RELAY2(_s)	stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
 #endif
 
-#define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
-#define OVERCURRENT_SERVO	(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
 #define BUTTON_SAFETY		stm32_gpioread(GPIO_BTN_SAFETY)
 
-#define ADC_VBATT		4
-#define ADC_IN5			5
-#define ADC_CHANNEL_COUNT	2
-
 /*
  * Mixer
  */

src/modules/px4iofirmware/registers.c

@@ -58,14 +58,18 @@ static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt
  * Static configuration parameters.
  */
 static const uint16_t	r_page_config[] = {
-	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= 1,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= PX4IO_PROTOCOL_VERSION,
-	[PX4IO_P_CONFIG_SOFTWARE_VERSION]	= 1,	/* XXX hardcoded magic number */
+#ifdef CONFIG_ARCH_BOARD_PX4IOV2
+	[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]		= IO_SERVO_COUNT,
+	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= PX4IO_SERVO_COUNT,
-	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= MAX_CONTROL_CHANNELS,
+	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= PX4IO_CONTROL_CHANNELS,
-	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= ADC_CHANNEL_COUNT,
+	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= PX4IO_ADC_CHANNEL_COUNT,
 	[PX4IO_P_CONFIG_RELAY_COUNT]		= PX4IO_RELAY_CHANNELS,
 };
 
@@ -80,7 +84,10 @@ uint16_t		r_page_status[] = {
 	[PX4IO_P_STATUS_FLAGS]			= 0,
 	[PX4IO_P_STATUS_ALARMS]			= 0,
 	[PX4IO_P_STATUS_VBATT]			= 0,
-	[PX4IO_P_STATUS_IBATT]			= 0
+	[PX4IO_P_STATUS_IBATT]			= 0,
+	[PX4IO_P_STATUS_VSERVO]			= 0,
+	[PX4IO_P_STATUS_VRSSI]			= 0,
+	[PX4IO_P_STATUS_PRSSI]			= 0
 };
 
 /**
@@ -88,14 +95,14 @@ uint16_t		r_page_status[] = {
  *
  * Post-mixed actuator values.
  */
-uint16_t 		r_page_actuators[IO_SERVO_COUNT];
+uint16_t 		r_page_actuators[PX4IO_SERVO_COUNT];
 
 /**
  * PAGE 3
  *
  * Servo PWM values
  */
-uint16_t		r_page_servos[IO_SERVO_COUNT];
+uint16_t		r_page_servos[PX4IO_SERVO_COUNT];
 
 /**
  * PAGE 4
@@ -105,7 +112,7 @@ uint16_t		r_page_servos[IO_SERVO_COUNT];
 uint16_t		r_page_raw_rc_input[] =
 {
 	[PX4IO_P_RAW_RC_COUNT]			= 0,
-	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
 };
 
 /**
@@ -115,7 +122,7 @@ uint16_t		r_page_raw_rc_input[] =
  */
 uint16_t		r_page_rc_input[] = {
 	[PX4IO_P_RC_VALID]			= 0,
-	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
 };
 
 /**
@@ -148,7 +155,7 @@ volatile uint16_t	r_page_setup[] =
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
 					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
 					 PX4IO_P_SETUP_ARMING_IO_ARM_OK)
-#define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
+#define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
 /**
@@ -167,7 +174,7 @@ volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
  *
  * R/C channel input configuration.
  */
-uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+uint16_t		r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
 
 /* valid options */
 #define PX4IO_P_RC_CONFIG_OPTIONS_VALID	(PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
@@ -183,7 +190,7 @@ uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
  * 
  * Disable pulses as default.
  */
-uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
+uint16_t		r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
 
 int
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
@@ -234,7 +241,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 	case PX4IO_PAGE_FAILSAFE_PWM:
 
 		/* copy channel data */
-		while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
 
 			/* XXX range-check value? */
 			r_page_servo_failsafe[offset] = *values;
@@ -366,6 +373,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 #endif
 			break;
 
+		case PX4IO_P_SETUP_VBATT_SCALE:
+			r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
+			break;
+
 		case PX4IO_P_SETUP_SET_DEBUG:
 			r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value;
 			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
@@ -388,7 +399,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
 		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
 
-		if (channel >= MAX_CONTROL_CHANNELS)
+		if (channel >= PX4IO_CONTROL_CHANNELS)
 			return -1;
 
 		/* disable the channel until we have a chance to sanity-check it */
@@ -433,7 +444,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
 					count++;
 				}
-				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
 					count++;
 				}
 
@@ -497,6 +508,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 
 		/* PX4IO_P_STATUS_ALARMS maintained externally */
 
+#ifdef ADC_VBATT
 		/* PX4IO_P_STATUS_VBATT */
 		{
 			/*
@@ -530,7 +542,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 				r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
 			}
 		}
-
+#endif
+#ifdef ADC_IBATT
 		/* PX4IO_P_STATUS_IBATT */
 		{
 			/*
@@ -540,26 +553,74 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 			  FMU sort it out, with user selectable
 			  configuration for their sensor
 			 */
-			unsigned counts = adc_measure(ADC_IN5);
+			unsigned counts = adc_measure(ADC_IBATT);
 			if (counts != 0xffff) {
 				r_page_status[PX4IO_P_STATUS_IBATT] = counts;
 			}
 		}
+#endif
+#ifdef ADC_VSERVO
+		/* PX4IO_P_STATUS_VSERVO */
+		{
+			/*
+			 * 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_VSERVO);
+			if (counts != 0xffff) {
+				unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+				unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
+
+				r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
+			}
+		}
+#endif
+		/* XXX PX4IO_P_STATUS_VRSSI */
+		/* XXX PX4IO_P_STATUS_PRSSI */
 
 		SELECT_PAGE(r_page_status);
 		break;
 
 	case PX4IO_PAGE_RAW_ADC_INPUT:
 		memset(r_page_scratch, 0, sizeof(r_page_scratch));
+#ifdef ADC_VBATT
 		r_page_scratch[0] = adc_measure(ADC_VBATT);
-		r_page_scratch[1] = adc_measure(ADC_IN5);
+#endif
+#ifdef ADC_IBATT
+		r_page_scratch[1] = adc_measure(ADC_IBATT);
+#endif
 
+#ifdef ADC_VSERVO
+		r_page_scratch[0] = adc_measure(ADC_VSERVO);
+#endif
+#ifdef ADC_RSSI
+		r_page_scratch[1] = adc_measure(ADC_RSSI);
+#endif
 		SELECT_PAGE(r_page_scratch);
 		break;
 
 	case PX4IO_PAGE_PWM_INFO:
 		memset(r_page_scratch, 0, sizeof(r_page_scratch));
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
 
 		SELECT_PAGE(r_page_scratch);
@@ -631,7 +692,7 @@ static void
 pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
 {
 	for (unsigned pass = 0; pass < 2; pass++) {
-		for (unsigned group = 0; group < IO_SERVO_COUNT; group++) {
+		for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) {
 
 			/* get the channel mask for this rate group */
 			uint32_t mask = up_pwm_servo_get_rate_group(group);

src/systemcmds/preflight_check/preflight_check.c

@@ -210,7 +210,7 @@ int preflight_check_main(int argc, char *argv[])
 		}
 
 		/* XXX needs inspection of all the _MAP params */
-		// if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+		// if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
 		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
 		// 	/* give system time to flush error message in case there are more */
 		// 	usleep(100000);