Merge pull request #729 from PX4/gpio_led_fmuv2

gpio_led: some more fixes

1 files changed, 21 insertions, 26 deletions

diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index f5f3dea76..6dfd22fdf 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -91,8 +91,7 @@ int gpio_led_main(int argc, char *argv[])
 #endif
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 		errx(1, "usage: gpio_led {start|stop} [-p <n>]\n"
-		     "\t-p\tUse pin:\n"
-		     "\t\tn\tAUX OUT pin number (default: 1)\n"
+		     "\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
 		    );
 #endif
 
@@ -108,6 +107,14 @@ int gpio_led_main(int argc, char *argv[])
 			/* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */
 			int pin = 1;
 
+			/* pin name to display */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+			char *pin_name = "PX4FMU GPIO_EXT1";
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+			char pin_name[] = "AUX OUT 1";
+#endif
+
 			if (argc > 2) {
 				if (!strcmp(argv[2], "-p")) {
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
@@ -115,26 +122,32 @@ int gpio_led_main(int argc, char *argv[])
 					if (!strcmp(argv[3], "1")) {
 						use_io = false;
 						pin = GPIO_EXT_1;
+						pin_name = "PX4FMU GPIO_EXT1";
 
 					} else if (!strcmp(argv[3], "2")) {
 						use_io = false;
 						pin = GPIO_EXT_2;
+						pin_name = "PX4FMU GPIO_EXT2";
 
 					} else if (!strcmp(argv[3], "a1")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_ACC1;
+						pin_name = "PX4IO ACC1";
 
 					} else if (!strcmp(argv[3], "a2")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_ACC2;
+						pin_name = "PX4IO ACC2";
 
 					} else if (!strcmp(argv[3], "r1")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_POWER1;
+						pin_name = "PX4IO RELAY1";
 
 					} else if (!strcmp(argv[3], "r2")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_POWER2;
+						pin_name = "PX4IO RELAY2";
 
 					} else {
 						errx(1, "unsupported pin: %s", argv[3]);
@@ -146,7 +159,8 @@ int gpio_led_main(int argc, char *argv[])
 
 					if (n >= 1 && n <= 6) {
 						use_io = false;
-						pin = n;
+						pin = 1 << (n - 1);
+						snprintf(pin_name, sizeof(pin_name), "AUX OUT %d", n);
 
 					} else {
 						errx(1, "unsupported pin: %s", argv[3]);
@@ -166,27 +180,6 @@ int gpio_led_main(int argc, char *argv[])
 
 			} else {
 				gpio_led_started = true;
-				char pin_name[24];
-
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-
-				if (use_io) {
-					if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) {
-						sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
-
-					} else {
-						sprintf(pin_name, "PX4IO RELAY%i", pin);
-					}
-
-				} else {
-					sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
-				}
-
-#endif
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
-				sprintf(pin_name, "AUX OUT %i", pin);
-#endif
-
 				warnx("start, using pin: %s", pin_name);
 			}
 
@@ -260,8 +253,9 @@ void gpio_led_cycle(FAR void *arg)
 	bool updated;
 	orb_check(priv->vehicle_status_sub, &updated);
 
-	if (updated)
+	if (updated) {
 		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
+	}
 
 	/* select pattern for current status */
 	int pattern = 0;
@@ -301,8 +295,9 @@ void gpio_led_cycle(FAR void *arg)
 
 	priv->counter++;
 
-	if (priv->counter > 5)
+	if (priv->counter > 5) {
 		priv->counter = 0;
+	}
 
 	/* repeat cycle at 5 Hz */
 	if (gpio_led_started) {