fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 05:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

PX4 System gpio_led:Code cleanup

Browse Source 
Use PX4 log and module documantation
   Fixed memory leaks
This commit is contained in:
David Sidrane
2017-09-10 13:37:23 -04:00
committed by Daniel Agar
parent bcb89f54e2
commit 1e0489f48b
1 changed files with 98 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gpio_led/gpio_led.c
+98 -35
View File
@@ -45,6 +45,8 @@
#include <string.h>
#include <fcntl.h>
#include <stdbool.h>
#include <px4_module.h>
#include <px4_log.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/systemlib.h>
@@ -58,9 +60,12 @@
#define PIN_NAME "AUX OUT 1"
/* Minimum pin number */
#define GPIO_MIN_SERVO_PIN 1
#define CYCLE_RATE_HZ 5
/* Maximum */
#if defined(GPIO_SERVO_16)
# define GPIO_MAX_SERVO_PIN 16
@@ -86,7 +91,6 @@
# define GPIO_MAX_SERVO_PIN 6
#else
# error "Board must define GPIO_SERVO_1 and GPIO_SERVO_n where n is 6-16"
#endif
struct gpio_led_s {
@@ -103,7 +107,12 @@ struct gpio_led_s {
static struct gpio_led_s *gpio_led_data;
static bool gpio_led_started = false;
static volatile enum {
Stopped = 0,
Running = 1,
Falied = 2,
Stopping = 3
} gpio_led_state = Stopped;
__EXPORT int gpio_led_main(int argc, char *argv[]);
@@ -111,18 +120,49 @@ void gpio_led_start(FAR void *arg);
void gpio_led_cycle(FAR void *arg);
static void print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This module is responsible for drving a single LED on one of the FMU AUX pins.
It listens on the vehicle_status and battery_status topics and provides visual annunciation on the LED.
### Implementation
The module runs on the work queue. It schedules at a fixed frequency or 5 Hz
### Examples
It is started with:
$ gpio_led start
To drive an LED connected AUX1 pin.
OR with any of the avaliabel AUX pins
$ gpio_led start -p 5
To drive an LED connected AUX5 pin.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("gpio_led", "driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "annunciation on AUX OUT pin");
PRINT_MODULE_USAGE_PARAM_FLAG('p', "Use specified AUX OUT pin number (default: 1)", true);
PRINT_MODULE_USAGE_COMMAND("stop");
}
int gpio_led_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: gpio_led {start|stop} [-p <n>]\n"
"\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
);
print_usage(NULL);
exit(1);
} else {
if (!strcmp(argv[1], "start")) {
if (gpio_led_started) {
errx(1, "already running");
if (gpio_led_state != Stopped) {
PX4_WARN("already running");
exit(1);
}
/* by default GPIO_SERVO_1 on FMUv2 */
@@ -139,42 +179,61 @@ int gpio_led_main(int argc, char *argv[])
if (n >= GPIO_MIN_SERVO_PIN && n <= GPIO_MAX_SERVO_PIN) {
pin = 1 << (n - 1);
snprintf(pin_name, sizeof(pin_name), "AUX OUT %d", n);
} else {
errx(1, "unsupported pin: %s", argv[3]);
PX4_ERR("unsupported pin: %s (valid values are %d-%d)", argv[3], GPIO_MIN_SERVO_PIN, GPIO_MAX_SERVO_PIN);
exit(1);
}
}
}
gpio_led_data = malloc(sizeof(struct gpio_led_s));
memset(gpio_led_data, 0, sizeof(struct gpio_led_s));
gpio_led_data->pin = pin;
int ret = work_queue(LPWORK, &(gpio_led_data->work), gpio_led_start, gpio_led_data, 0);
if (ret != 0) {
errx(1, "failed to queue work: %d", ret);
if (gpio_led_data == NULL) {
PX4_ERR("failed to allocate memory!");
exit(1);
} else {
gpio_led_started = true;
warnx("start, using pin: %s", pin_name);
exit(0);
memset(gpio_led_data, 0, sizeof(struct gpio_led_s));
gpio_led_data->pin = pin;
int ret = work_queue(LPWORK, &(gpio_led_data->work), gpio_led_start, gpio_led_data, 0);
if (ret != 0) {
PX4_ERR("failed to queue work: %d", ret);
goto out;
} else {
usleep(1000000/CYCLE_RATE_HZ);
if (gpio_led_state != Running) {
gpio_led_state = Stopped;
goto out;
}
PX4_INFO("start, using pin: %s", pin_name);
exit(0);
}
}
} else if (!strcmp(argv[1], "stop")) {
if (gpio_led_started) {
gpio_led_started = false;
warnx("stop");
if (gpio_led_state == Running) {
gpio_led_state = Stopping;
while(gpio_led_state != Stopped) {
usleep(1000000/CYCLE_RATE_HZ);
}
PX4_INFO("stopped");
free (gpio_led_data);
gpio_led_data = NULL;
exit(0);
} else {
errx(1, "not running");
PX4_WARN("not running");
exit(1);
}
} else {
errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
print_usage("unrecognized command");
exit(1);
}
}
out:
free (gpio_led_data);
gpio_led_data = NULL;
exit(1);
}
void gpio_led_start(FAR void *arg)
@@ -187,14 +246,13 @@ void gpio_led_start(FAR void *arg)
priv->gpio_fd = open(gpio_dev, 0);
if (priv->gpio_fd < 0) {
// TODO find way to print errors
//printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev);
gpio_led_started = false;
PX4_ERR("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev);
gpio_led_state = Falied;
return;
}
/* configure GPIO pin */
/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
/* initialize vehicle status structure */
@@ -213,11 +271,12 @@ void gpio_led_start(FAR void *arg)
int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
if (ret != 0) {
// TODO find way to print errors
//printf("gpio_led: failed to queue work: %d\n", ret);
gpio_led_started = false;
PX4_ERR("gpio_led: failed to queue work: %d\n", ret);
close(priv->gpio_fd);
gpio_led_state = Falied;
return;
}
gpio_led_state = Running;
}
void gpio_led_cycle(FAR void *arg)
@@ -282,11 +341,15 @@ void gpio_led_cycle(FAR void *arg)
}
/* repeat cycle at 5 Hz */
if (gpio_led_started) {
work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
if (gpio_led_state == Running) {
work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(1000000/CYCLE_RATE_HZ));
} else {
/* switch off LED on stop */
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
orb_unsubscribe(priv->vehicle_status_sub);
orb_unsubscribe(priv->battery_status_sub);
close(priv->gpio_fd);
gpio_led_state = Stopped;
}
}