diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 889abc4d9a..7ec314460d 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/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) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index d383146f99..f5f3dea761 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,7 +51,6 @@ #include #include #include -#include #include #include #include @@ -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,6 +78,7 @@ 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" @@ -88,7 +86,15 @@ int gpio_led_main(int argc, char *argv[]) "\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" + "\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); }