Changes for multi-rate PWM output; default and alternate rates. ioctl protocol, PX4IO support.

3 files changed, 81 insertions, 27 deletions

diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 14d221b5b..8d8b7e941 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -76,7 +76,7 @@
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
 /* static configuration page */
-#define PX4IO_PAGE_CONFIG			0
+#define PX4IO_PAGE_CONFIG		0
 #define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
@@ -88,7 +88,7 @@
 #define PX4IO_P_CONFIG_RELAY_COUNT		8	/* harcoded # of relay outputs */
 
 /* dynamic status page */
-#define PX4IO_PAGE_STATUS			1
+#define PX4IO_PAGE_STATUS		1
 #define PX4IO_P_STATUS_FREEMEM			0
 #define PX4IO_P_STATUS_CPULOAD			1
 
@@ -112,28 +112,33 @@
 #define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* 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_VBATT			4	/* battery voltage in mV */
 #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 */
+#define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* array of PWM servo output values, microseconds */
-#define PX4IO_PAGE_SERVOS			3	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_SERVOS		3		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* array of raw RC input values, microseconds */
-#define PX4IO_PAGE_RAW_RC_INPUT			4
+#define PX4IO_PAGE_RAW_RC_INPUT		4
 #define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
 #define PX4IO_P_RAW_RC_BASE			1	/* CONFIG_RC_INPUT_COUNT channels from here */
 
 /* array of scaled RC input values, -10000..10000 */
-#define PX4IO_PAGE_RC_INPUT			5
+#define PX4IO_PAGE_RC_INPUT		5
 #define PX4IO_P_RC_VALID			0	/* bitmask of valid controls */
 #define PX4IO_P_RC_BASE				1	/* CONFIG_RC_INPUT_COUNT controls from here */
 
 /* array of raw ADC values */
-#define PX4IO_PAGE_RAW_ADC_INPUT		6	/* 0..CONFIG_ADC_INPUT_COUNT-1 */
+#define PX4IO_PAGE_RAW_ADC_INPUT	6		/* 0..CONFIG_ADC_INPUT_COUNT-1 */
+
+/* PWM servo information */
+#define PX4IO_PAGE_PWM_INFO		7
+#define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
 
 /* setup page */
 #define PX4IO_PAGE_SETUP			100
@@ -146,8 +151,8 @@
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 
 #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_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_IBATT_SCALE		7	/* battery current scaling factor (float) */
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 7b4b07c2c..202e9d9d9 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -94,8 +94,8 @@ extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
 #define r_setup_features	r_page_setup[PX4IO_P_SETUP_FEATURES]
 #define r_setup_arming		r_page_setup[PX4IO_P_SETUP_ARMING]
 #define r_setup_pwm_rates	r_page_setup[PX4IO_P_SETUP_PWM_RATES]
-#define r_setup_pwm_lowrate	r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE]
-#define r_setup_pwm_highrate	r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE]
+#define r_setup_pwm_defaultrate	r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
+#define r_setup_pwm_altrate	r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
 #define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
 
 #define r_control_values	(&r_page_controls[0])
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index 645c1565d..b09937300 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -48,6 +48,7 @@
 #include "protocol.h"
 
 static int	registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
+static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate);
 
 /**
  * PAGE 0
@@ -116,11 +117,12 @@ uint16_t		r_page_rc_input[] = {
 };
 
 /**
- * PAGE 6
+ * Scratch page; used for registers that are constructed as-read.
  *
- * Raw ADC input.
+ * PAGE 6 Raw ADC input.
+ * PAGE 7 PWM rate maps.
  */
-uint16_t		r_page_adc[ADC_CHANNEL_COUNT];
+uint16_t		r_page_scratch[32];
 
 /**
  * PAGE 100
@@ -132,8 +134,8 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_FEATURES]		= 0,
 	[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_PWM_DEFAULTRATE]		= 50,
+	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
 	[PX4IO_P_SETUP_RELAYS]			= 0,
 	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
 	[PX4IO_P_SETUP_IBATT_SCALE]		= 0,
@@ -321,26 +323,23 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 		case PX4IO_P_SETUP_PWM_RATES:
 			value &= PX4IO_P_SETUP_RATES_VALID;
-			r_setup_pwm_rates = value;
-			/* XXX re-configure timers */
+			pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate);
 			break;
 
-		case PX4IO_P_SETUP_PWM_LOWRATE:
+		case PX4IO_P_SETUP_PWM_DEFAULTRATE:
 			if (value < 50)
 				value = 50;
 			if (value > 400)
 				value = 400;
-			r_setup_pwm_lowrate = value;
-			/* XXX re-configure timers */
+			pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
 			break;
 
-		case PX4IO_P_SETUP_PWM_HIGHRATE:
+		case PX4IO_P_SETUP_PWM_ALTRATE:
 			if (value < 50)
 				value = 50;
 			if (value > 400)
 				value = 400;
-			r_setup_pwm_highrate = value;
-			/* XXX re-configure timers */
+			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
 			break;
 
 		case PX4IO_P_SETUP_RELAYS:
@@ -529,10 +528,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 		break;
 
 	case PX4IO_PAGE_RAW_ADC_INPUT:
-		r_page_adc[0] = adc_measure(ADC_VBATT);
-		r_page_adc[1] = adc_measure(ADC_IN5);
+		memset(r_page_scratch, 0, sizeof(r_page_scratch));
+		r_page_scratch[0] = adc_measure(ADC_VBATT);
+		r_page_scratch[1] = adc_measure(ADC_IN5);
 
-		SELECT_PAGE(r_page_adc);
+		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++)
+			r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
+
+		SELECT_PAGE(r_page_scratch);
 		break;
 
 	/*
@@ -593,3 +601,44 @@ last_offset = offset;
 
 	return 0;
 }
+
+/*
+ * Helper function to handle changes to the PWM rate control registers.
+ */
+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++) {
+
+			/* get the channel mask for this rate group */
+			uint32_t mask = up_pwm_servo_get_rate_group(group);
+			if (mask == 0)
+				continue;
+
+			/* all channels in the group must be either high or low-rate */
+			uint32_t high = map & mask;
+
+			if (pass == 0) {
+				/* preflight */
+				if ((high != 0) && (high != mask)) {
+					/* not a legal map, bail with an alarm */
+					r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+					return;
+				}
+			} else {
+				/* set it - errors here are unexpected */
+				if (high != 0) {
+					if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK)
+						r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+				} else {
+					if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK)
+						r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+				}
+			}
+		}
+	}
+	r_setup_pwm_rates = map;
+	r_setup_pwm_defaultrate = defaultrate;
+	r_setup_pwm_altrate = altrate;
+}