Merge pull request #728 from PX4/gpio_led_fmuv2

Gpio led fmuv2

makefiles/config_px4fmu-v2_default.mk

@@ -72,6 +72,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/mavlink_onboard
+MODULES		+= modules/gpio_led
 
 #
 # Estimation modules (EKF/ SO3 / other filters)

src/modules/gpio_led/gpio_led.c

@@ -51,7 +51,6 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_armed.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
 #include <modules/px4iofirmware/protocol.h>
@@ -63,8 +62,6 @@ struct gpio_led_s {
 	int pin;
 	struct vehicle_status_s status;
 	int vehicle_status_sub;
-	struct actuator_armed_s armed;
-	int actuator_armed_sub;
 	bool led_state;
 	int counter;
 };
@@ -81,14 +78,23 @@ void gpio_led_cycle(FAR void *arg);
 int gpio_led_main(int argc, char *argv[])
 {
 	if (argc < 2) {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
 		     "\t-p\tUse pin:\n"
 		     "\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
 		     "\t\t2\tPX4FMU GPIO_EXT2\n"
 		     "\t\ta1\tPX4IO ACC1\n"
 		     "\t\ta2\tPX4IO ACC2\n"
 		     "\t\tr1\tPX4IO RELAY1\n"
-		     "\t\tr2\tPX4IO RELAY2");
+		     "\t\tr2\tPX4IO RELAY2"
+		    );
+#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"
+		    );
+#endif
 
 	} else {
 
@@ -98,10 +104,14 @@ int gpio_led_main(int argc, char *argv[])
 			}
 
 			bool use_io = false;
-			int pin = GPIO_EXT_1;
+
+			/* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */
+			int pin = 1;
 
 			if (argc > 2) {
 				if (!strcmp(argv[2], "-p")) {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
 					if (!strcmp(argv[3], "1")) {
 						use_io = false;
 						pin = GPIO_EXT_1;
@@ -129,6 +139,20 @@ int gpio_led_main(int argc, char *argv[])
 					} else {
 						errx(1, "unsupported pin: %s", argv[3]);
 					}
+
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+					unsigned int n = strtoul(argv[3], NULL, 10);
+
+					if (n >= 1 && n <= 6) {
+						use_io = false;
+						pin = n;
+
+					} else {
+						errx(1, "unsupported pin: %s", argv[3]);
+					}
+
+#endif
 				}
 			}
 
@@ -144,6 +168,8 @@ int gpio_led_main(int argc, char *argv[])
 				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));
@@ -154,9 +180,13 @@ int gpio_led_main(int argc, char *argv[])
 
 				} 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);
 			}
 
@@ -186,6 +216,7 @@ void gpio_led_start(FAR void *arg)
 
 	if (priv->use_io) {
 		gpio_dev = PX4IO_DEVICE_PATH;
+
 	} else {
 		gpio_dev = PX4FMU_DEVICE_PATH;
 	}
@@ -204,8 +235,10 @@ void gpio_led_start(FAR void *arg)
 	/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
 	ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
 
-	/* subscribe to vehicle status topic */
+	/* initialize vehicle status structure */
 	memset(&priv->status, 0, sizeof(priv->status));
+
+	/* subscribe to vehicle status topic */
 	priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/* add worker to queue */
@@ -224,38 +257,32 @@ void gpio_led_cycle(FAR void *arg)
 	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
 
 	/* check for status updates*/
-	bool status_updated;
-	orb_check(priv->vehicle_status_sub, &status_updated);
+	bool updated;
+	orb_check(priv->vehicle_status_sub, &updated);
 
-	if (status_updated)
+	if (updated)
 		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
 
-	orb_check(priv->vehicle_status_sub, &status_updated);
-
-	if (status_updated)
-		orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
-
 	/* select pattern for current status */
 	int pattern = 0;
 
-	if (priv->armed.armed) {
-		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
+	if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		pattern = 0x2A;	// *_*_*_ fast blink (armed, error)
+
+	} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
+		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) {
 			pattern = 0x3f;	// ****** solid (armed)
 
 		} else {
-			pattern = 0x2A;	// *_*_*_ fast blink (armed, battery warning)
+			pattern = 0x3e;	// *****_ slow blink (armed, battery low or failsafe)
 		}
 
-	} else {
-		if (priv->armed.ready_to_arm) {
-			pattern = 0x00;	// ______ off (disarmed, preflight check)
+	} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
+		pattern = 0x38;	// ***___ slow blink (disarmed, ready)
 
-		} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
-			pattern = 0x38;	// ***___ slow blink (disarmed, ready)
+	} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
+		pattern = 0x28;	// *_*___ slow double blink (disarmed, error)
 
-		} else {
-			pattern = 0x28;	// *_*___ slow double blink (disarmed, not good to arm)
-		}
 	}
 
 	/* blink pattern */
@@ -266,6 +293,7 @@ void gpio_led_cycle(FAR void *arg)
 
 		if (led_state_new) {
 			ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
+
 		} else {
 			ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
 		}