Add support for battery current scaling. Add feedback for mixer load operations.

3 files changed, 79 insertions, 55 deletions

diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 178b0bb43..932aab930 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -70,6 +70,7 @@ static bool mixer_servos_armed = false;
 
 /* selected control values and count for mixing */
 enum mixer_source {
+	MIX_NONE,
 	MIX_FMU,
 	MIX_OVERRIDE,
 	MIX_FAILSAFE
@@ -81,7 +82,6 @@ static int	mixer_callback(uintptr_t handle,
 			       uint8_t control_index,
 			       float &control);
 
-static void mix();
 static MixerGroup mixer_group(mixer_callback, 0);
 
 void
@@ -97,30 +97,56 @@ mixer_tick(void)
 		debug("AP RX timeout");
 	}
 
+	source = MIX_FAILSAFE;
+
 	/*
 	 * Decide which set of controls we're using.
 	 */
-	if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && 
-		(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
-		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
-	 	/* this is for planes, where manual override makes sense */
-		source = MIX_OVERRIDE;
+	if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) {
 
-		/* mix from the override controls */
-		mix();
+		/* don't actually mix anything - we already have raw PWM values */
+		source = MIX_NONE;
 
-	} else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
+	} else {
+
+		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
-		if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) {
-			/* FMU has already provided PWM values */
-		} else {
 			/* mix from FMU controls */
 			source = MIX_FMU;
-			mix();
 		}
-	} else {
-		source = MIX_FAILSAFE;
-		/* XXX actually, have no idea what to do here... load hardcoded failsafe controls? */
+
+		if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+
+		 	/* if allowed, mix from RC inputs directly */
+			source = MIX_OVERRIDE;
+		}
+	}
+
+	/*
+	 * Run the mixers.
+	 */
+	if (source != MIX_NONE) {
+
+		float	outputs[IO_SERVO_COUNT];
+		unsigned mixed;
+
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+
+		/* scale to PWM and update the servo outputs as required */
+		for (unsigned i = 0; i < mixed; i++) {
+
+			/* save actuator values for FMU readback */
+			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
+
+			/* scale to servo output */
+			r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
+
+		}
+		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+			r_page_servos[i] = 0;
 	}
 
 #if 0
@@ -196,9 +222,12 @@ 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.
 	 */
-
-	bool should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED);
+	bool should_arm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+		(r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PPM | PX4IO_P_STATUS_FLAGS_MIXER_OK)));
 
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
@@ -212,33 +241,6 @@ mixer_tick(void)
 	}
 }
 
-static void
-mix()
-{
-	/*
-	 * Run the mixers.
-	 */
-	float	outputs[IO_SERVO_COUNT];
-	unsigned mixed;
-
-	/* mix */
-	mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
-
-	/* scale to PWM and update the servo outputs as required */
-	for (unsigned i = 0; i < mixed; i++) {
-
-		/* save actuator values for FMU readback */
-		r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
-
-		/* scale to servo output */
-		r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
-
-	}
-	for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
-		r_page_servos[i] = 0;
-
-}
-
 static int
 mixer_callback(uintptr_t handle,
 	       uint8_t control_group,
@@ -264,6 +266,7 @@ mixer_callback(uintptr_t handle,
 		return -1;
 
 	case MIX_FAILSAFE:
+	case MIX_NONE:
 		/* XXX we could allow for configuration of per-output failsafe values */
 		return -1;
 	}
@@ -292,6 +295,7 @@ mixer_handle_text(const void *buffer, size_t length)
 		debug("reset");
 		mixer_group.reset();
 		mixer_text_length = 0;
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
 
 		/* FALLTHROUGH */
 	case F2I_MIXER_ACTION_APPEND:
@@ -299,8 +303,10 @@ mixer_handle_text(const void *buffer, size_t length)
 
 		/* check for overflow - this is really fatal */
 		/* XXX could add just what will fit & try to parse, then repeat... */
-		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
+		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
 			return;
+		}
 
 		/* append mixer text and nul-terminate */
 		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
@@ -314,6 +320,10 @@ mixer_handle_text(const void *buffer, size_t length)
 
 		/* if anything was parsed */
 		if (resid != mixer_text_length) {
+
+			/* ideally, this should test resid == 0 ? */
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
+
 			debug("used %u", mixer_text_length - resid);
 
 			/* copy any leftover text to the base of the buffer for re-use */
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index a2be4bfc4..39ee4c0b1 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -101,6 +101,7 @@
 #define PX4IO_P_STATUS_FLAGS_RC_SBUS		(1 << 5) /* SBUS input is valid */
 #define PX4IO_P_STATUS_FLAGS_FMU_OK		(1 << 6) /* controls from FMU are valid */
 #define PX4IO_P_STATUS_FLAGS_RAW_PPM		(1 << 7) /* raw PPM from FMU is bypassing the mixer */
+#define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
 
 #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 */
@@ -111,7 +112,7 @@
 #define PX4IO_P_STATUS_ALARMS_RC_LOST		(1 << 5)
 
 #define PX4IO_P_STATUS_VBATT			4	/* battery voltage in mV */
-#define PX4IO_P_STATUS_TEMPERATURE		5	/* temperature in (units tbd) */
+#define PX4IO_P_STATUS_IBATT			5	/* battery current in cA */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -143,11 +144,12 @@
 #define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK	(1 << 3)
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
-
 #define PX4IO_P_SETUP_PWM_LOWRATE		3	/* 'low' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_PWM_HIGHRATE		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_IBATT_SCALE		7	/* battery current scaling factor (float) */
+#define PX4IO_P_SETUP_IBATT_BIAS		8	/* battery current bias value */
 
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS		101		/* 0..CONFIG_CONTROL_COUNT */
@@ -187,5 +189,3 @@ struct px4io_mixdata {
 	char		text[0];	/* actual text size may vary */
 };
 
-/* maximum size is limited by the link frame size XXX use config values to set this */
-#define F2I_MIXER_MAX_TEXT	(64 - sizeof(struct px4io_mixdata))
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index 2f411eebf..0206e0db0 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -60,6 +60,9 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_PWM_LOWRATE]		= 50,
 	[PX4IO_P_SETUP_PWM_HIGHRATE]		= 200,
 	[PX4IO_P_SETUP_RELAYS]			= 0,
+	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
+	[PX4IO_P_SETUP_IBATT_SCALE]		= 0,
+	[PX4IO_P_SETUP_IBATT_BIAS]		= 0
 };
 
 #define PX4IO_P_SETUP_FEATURES_VALID	(PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK)
@@ -80,7 +83,7 @@ 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_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_RC_INPUT_COUNT]		= MAX_CONTROL_CHANNELS,
@@ -97,7 +100,7 @@ uint16_t		r_page_status[] = {
 	[PX4IO_P_STATUS_FLAGS]			= 0,
 	[PX4IO_P_STATUS_ALARMS]			= 0,
 	[PX4IO_P_STATUS_VBATT]			= 0,
-	[PX4IO_P_STATUS_TEMPERATURE]		= 0
+	[PX4IO_P_STATUS_IBATT]			= 0
 };
 
 /**
@@ -396,10 +399,21 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 			 * Intercept corrected for best results @ 12V.
 			 */
 			unsigned counts = adc_measure(ADC_VBATT);
-			r_page_status[PX4IO_P_STATUS_VBATT] = (4150 + (counts * 46)) / 10;
+			unsigned mV = (4150 + (counts * 46)) / 10;
+			unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
+
+			r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
 		}
 
-		/* XXX PX4IO_P_STATUS_TEMPERATURE */
+		/* PX4IO_P_STATUS_IBATT */
+		{
+			unsigned counts = adc_measure(ADC_VBATT);
+			unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000;
+			int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]);
+			if (corrected < 0)
+				corrected = 0;
+			r_page_status[PX4IO_P_STATUS_IBATT] = corrected;
+		}
 
 		SELECT_PAGE(r_page_status);
 		break;