From ea539031da96df3d3eb9faadd24eb1cc71813e7f Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 1 Nov 2012 23:42:36 -0700 Subject: Cleanup and refactor of the PX4IO firmware and board support. Builds, not tested yet. --- apps/px4io/Makefile | 11 +++++------ apps/px4io/comms.c | 2 +- apps/px4io/mixer.c | 51 ++++++++++++++++----------------------------------- apps/px4io/px4io.c | 27 +++++++++------------------ apps/px4io/px4io.h | 40 +++++++++++++++++++++++----------------- apps/px4io/safety.c | 3 --- 6 files changed, 54 insertions(+), 80 deletions(-) (limited to 'apps/px4io') diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile index 9b63d3ac8..801695f84 100644 --- a/apps/px4io/Makefile +++ b/apps/px4io/Makefile @@ -39,11 +39,10 @@ # Note that we pull a couple of specific files from the systemlib, since # we can't support it all. # -CSRCS = comms.c \ - mixer.c \ - px4io.c \ - safety.c \ - ../systemlib/hx_stream.c \ - ../systemlib/perf_counter.c +CSRCS = $(wildcard *.c) \ + ../systemlib/hx_stream.c \ + ../systemlib/perf_counter.c + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common include $(APPDIR)/mk/app.mk diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index f9106d830..507350442 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -93,7 +93,7 @@ comms_check(void) last_report_time = now; /* populate the report */ - for (unsigned i = 0; i < system_state.rc_channels; i++) + for (int i = 0; i < system_state.rc_channels; i++) report.rc_channel[i] = system_state.rc_channel_data[i]; report.channel_count = system_state.rc_channels; report.armed = system_state.armed; diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index a91aad3ca..cbc03e2f8 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -45,17 +45,12 @@ #include #include -#include -#include +#include #include -#include "px4io.h" - -#ifdef CONFIG_DISABLE_MQUEUE -# error Mixer requires message queues - set CONFIG_DISABLE_MQUEUE=n and try again -#endif +#include -static mqd_t input_queue; +#include "px4io.h" /* * Count of periodic calls in which we have no data. @@ -89,9 +84,6 @@ static void mixer_get_rc_input(void); */ static void mixer_update(int mixer, uint16_t *inputs, int input_count); -/* servo driver handle */ -int mixer_servo_fd; - /* current servo arm/disarm state */ bool mixer_servos_armed; @@ -106,14 +98,6 @@ struct mixer { int mixer_init(const char *mq_name) { - /* open the control input queue; this should always exist */ - input_queue = mq_open(mq_name, O_RDONLY | O_NONBLOCK); - ASSERTCODE((input_queue >= 0), A_INPUTQ_OPEN_FAIL); - - /* open the servo driver */ - mixer_servo_fd = open("/dev/pwm_servo", O_WRONLY); - ASSERTCODE((mixer_servo_fd >= 0), A_SERVO_OPEN_FAIL); - /* look for control data at 50Hz */ hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL); @@ -176,9 +160,8 @@ mixer_tick(void *arg) * If we are armed, update the servo output. */ if (system_state.armed) - ioctl(mixer_servo_fd, PWM_SERVO_SET(i), mixers[i].current_value); + up_pwm_servo_set(i, mixers[i].current_value); } - } /* @@ -187,12 +170,12 @@ mixer_tick(void *arg) should_arm = system_state.armed && (control_count > 0); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ - ioctl(mixer_servo_fd, PWM_SERVO_ARM, 0); + up_pwm_servo_arm(true); mixer_servos_armed = true; } else if (!should_arm && mixer_servos_armed) { - /* armed but need to disarm*/ - ioctl(mixer_servo_fd, PWM_SERVO_DISARM, 0); + /* armed but need to disarm */ + up_pwm_servo_arm(false); mixer_servos_armed = false; } } @@ -200,22 +183,20 @@ mixer_tick(void *arg) static void mixer_get_rc_input(void) { - ssize_t len; - /* - * Pull channel data from the message queue into the system state structure. + * XXX currently only supporting PPM * + * XXX check timestamp to ensure current */ - len = mq_receive(input_queue, &system_state.rc_channel_data, sizeof(system_state.rc_channel_data), NULL); - - /* - * If we have data, update the count and status. - */ - if (len > 0) { - system_state.rc_channels = len / sizeof(system_state.rc_channel_data[0]); + if (ppm_decoded_channels > 0) { mixer_input_drops = 0; - system_state.fmu_report_due = true; + + /* copy channel data */ + system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) + system_state.rc_channel_data[i] = ppm_buffer[i]; + } else { /* * No data; count the 'frame drops' and once we hit the limit diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index a67db9a7e..33c28fc6c 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -47,9 +47,7 @@ #include -#include -#include -#include +#include #include #include "px4io.h" @@ -73,8 +71,8 @@ int user_start(int argc, char *argv[]) bool heartbeat = false; bool failsafe = false; - /* Do board init */ - (void)up_boardinitialize(); + /* configure the PWM outputs */ + up_pwm_servo_init(0xff); /* print some startup info */ lib_lowprintf("\nPX4IO: starting\n"); @@ -84,21 +82,14 @@ int user_start(int argc, char *argv[]) /* start the software timer service */ hrt_call_every(&timer_tick_call, 1000, 1000, timer_tick, NULL); - /* Open the GPIO driver so we can do LEDs and the like. */ - gpio_fd = open("/dev/gpio", 0); - ASSERTCODE((gpio_fd >= 0), A_GPIO_OPEN_FAIL); - /* default all the LEDs to off while we start */ - LED_AMBER(heartbeat); - LED_BLUE(failsafe); + LED_AMBER(false); + LED_BLUE(false); LED_SAFETY(false); /* turn on servo power */ POWER_SERVO(true); - /* configure the PPM input driver */ - ppm_input_init(rc_input_mq_name); - /* start the mixer */ mixer_init(rc_input_mq_name); @@ -123,18 +114,19 @@ int user_start(int argc, char *argv[]) /* blink the heartbeat LED */ if (timers[TIMER_BLINK_AMBER] == 0) { timers[TIMER_BLINK_AMBER] = 250; - LED_AMBER((heartbeat = !heartbeat)); + LED_AMBER(heartbeat = !heartbeat); } /* blink the failsafe LED if we don't have FMU input */ if (!system_state.mixer_use_fmu) { if (timers[TIMER_BLINK_BLUE] == 0) { timers[TIMER_BLINK_BLUE] = 125; - LED_BLUE((failsafe = !failsafe)); + failsafe = !failsafe; } } else { - LED_BLUE((failsafe = false)); + failsafe = false; } + LED_BLUE(failsafe); /* print some simple status */ if (timers[TIMER_STATUS_PRINT] == 0) { @@ -147,7 +139,6 @@ int user_start(int argc, char *argv[]) system_state.rc_channels ); } - } /* Should never reach here */ diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index d90f7e36b..4889036b6 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -32,10 +32,18 @@ ****************************************************************************/ /** - * @file General defines and structures for the PX4IO module firmware. + * @file px4io.h + * + * General defines and structures for the PX4IO module firmware. */ -#include +#include + +#include +#include + +#include + #include "protocol.h" /* @@ -102,21 +110,19 @@ extern volatile int timers[TIMER_NUM_TIMERS]; /* * GPIO handling. */ -extern int gpio_fd; - -#define POWER_SERVO(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_POWER), (_s)) -#define POWER_ACC1(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_ACC1), (_s)) -#define POWER_ACC2(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_ACC2), (_s)) -#define POWER_RELAY1(_s) ioctl(gpio_fd, GPIO_SET(GPIO_RELAY1, (_s)) -#define POWER_RELAY2(_s) ioctl(gpio_fd, GPIO_SET(GPIO_RELAY2, (_s)) - -#define LED_AMBER(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_AMBER), !(_s)) -#define LED_BLUE(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_BLUE), !(_s)) -#define LED_SAFETY(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_SAFETY), !(_s)) - -#define OVERCURRENT_ACC ioctl(gpio_fd, GPIO_GET(GPIO_ACC_OVERCURRENT), 0) -#define OVERCURRENT_SERVO ioctl(gpio_fd, GPIO_GET(GPIO_SERVO_OVERCURRENT), 0) -#define BUTTON_SAFETY ioctl(gpio_fd, GPIO_GET(GPIO_SAFETY_BUTTON), 0) +#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) +#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) +#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) + +#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_SERVO_ACC1_EN, (_s)) +#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_SERVO_ACC2_EN, (_s)) +#define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +#define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) + +#define OVERCURRENT_ACC stm32_gpioread(GPIO_ACC_OC_DETECT) +#define OVERCURRENT_SERVO stm32_gpioread(GPIO_SERVO_OC_DETECT +#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) /* * Mixer diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 6cb85798f..f27432664 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -46,9 +46,6 @@ #include -#include -#include -#include #include #include "px4io.h" -- cgit v1.2.3