Merge pull request #728 from PX4/gpio_led_fmuv2

Gpio led fmuv2
This commit is contained in:
Lorenz Meier 2014-03-12 20:37:19 +01:00
commit f5b21fbcde
2 changed files with 55 additions and 26 deletions

View File

@ -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)

View File

@ -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,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>]\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);
}