From 6d0bea0298100d4ecf6630c239aa549bb338cc7b Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 18 Dec 2012 00:27:12 -0800 Subject: Fix the PWM servo ioctl base so it's not overlapping the GPIOs --- apps/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 97033f2cd..b2fee65ac 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -95,7 +95,7 @@ ORB_DECLARE(output_pwm); * Note that ioctls and ObjDev updates should not be mixed, as the * behaviour of the system in this case is not defined. */ -#define _PWM_SERVO_BASE 0x2700 +#define _PWM_SERVO_BASE 0x2a00 /** arm all servo outputs handle by this driver */ #define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0) -- cgit v1.2.3 From b8044d978672c3ee760189fb6e532b0514a09446 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 18 Dec 2012 00:29:22 -0800 Subject: use more consistently in the fmu driver. --- apps/drivers/px4fmu/fmu.cpp | 32 +++++++++++++++----------------- 1 file changed, 15 insertions(+), 17 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 61dd418f8..f96aab45a 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -60,6 +60,7 @@ #include #include +#include #include #include @@ -800,13 +801,14 @@ test(void) fd = open(PWM_OUTPUT_DEVICE_PATH, 0); - if (fd < 0) { - puts("open fail"); - exit(1); - } + if (fd < 0) + errx(1, "open fail"); - ioctl(fd, PWM_SERVO_ARM, 0); - ioctl(fd, PWM_SERVO_SET(0), 1000); + if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); + if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); + if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); + if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); close(fd); @@ -816,10 +818,8 @@ test(void) void fake(int argc, char *argv[]) { - if (argc < 5) { - puts("fmu fake (values -100 .. 100)"); - exit(1); - } + if (argc < 5) + errx(1, "fmu fake (values -100 .. 100)"); actuator_controls_s ac; @@ -833,10 +833,8 @@ fake(int argc, char *argv[]) orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - if (handle < 0) { - puts("advertise failed"); - exit(1); - } + if (handle < 0) + errx(1, "advertise failed"); exit(0); } @@ -891,11 +889,11 @@ fmu_main(int argc, char *argv[]) if (argc > i + 1) { pwm_update_rate_in_hz = atoi(argv[i + 1]); } else { - fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + errx(1, "missing argument for pwm update rate (-u)"); return 1; } } else { - fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n"); + errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio"); } } } @@ -915,5 +913,5 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command, try:\n"); fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); - return -EINVAL; + exit(1); } -- cgit v1.2.3 From 8d716dea45db086cdfd0acebd7d1ce9aa6169fa3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 18 Dec 2012 00:33:33 -0800 Subject: Teach 'fake' to set the arming state as well. Whitespace. --- apps/drivers/px4fmu/fmu.cpp | 55 ++++++++++++++++++++++++++++++++++----------- 1 file changed, 42 insertions(+), 13 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index f96aab45a..a995f6214 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -111,9 +111,9 @@ private: void task_main() __attribute__((noreturn)); static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); int pwm_ioctl(file *filp, int cmd, unsigned long arg); @@ -178,6 +178,7 @@ PX4FMU::~PX4FMU() _task_should_exit = true; unsigned i = 10; + do { /* wait 50ms - it should wake every 100ms or so worst-case */ usleep(50000); @@ -213,6 +214,7 @@ PX4FMU::init() /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (ret == OK) { log("default PWM output device"); _primary_pwm_device = true; @@ -246,7 +248,7 @@ PX4FMU::task_main_trampoline(int argc, char *argv[]) int PX4FMU::set_mode(Mode mode) { - /* + /* * Configure for PWM output. * * Note that regardless of the configured mode, the task is always @@ -280,6 +282,7 @@ PX4FMU::set_mode(Mode mode) default: return -EINVAL; } + _mode = mode; return OK; } @@ -332,8 +335,10 @@ PX4FMU::task_main() /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); + if (update_rate_in_ms < 2) update_rate_in_ms = 2; + orb_set_interval(_t_actuators, update_rate_in_ms); up_pwm_servo_set_rate(_update_rate); _current_update_rate = _update_rate; @@ -405,9 +410,9 @@ PX4FMU::task_main() int PX4FMU::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -425,15 +430,17 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); + if (ret != -ENOTTY) return ret; /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch(_mode) { + switch (_mode) { case MODE_2PWM: case MODE_4PWM: ret = pwm_ioctl(filp, cmd, arg); break; + default: debug("not in a PWM mode"); break; @@ -551,8 +558,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) } _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (_mixers == nullptr) { ret = -ENOMEM; + } else { debug("loading mixers from %s", path); @@ -583,7 +592,7 @@ void PX4FMU::gpio_reset(void) { /* - * Setup default GPIO config - all pins as GPIOs, GPIO driver chip + * Setup default GPIO config - all pins as GPIOs, GPIO driver chip * to input mode. */ for (unsigned i = 0; i < _ngpio; i++) @@ -610,17 +619,20 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) /* configure selected GPIOs as required */ for (unsigned i = 0; i < _ngpio; i++) { - if (gpios & (1<set_mode(servo_mode); + if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0)) g_fmu->set_pwm_rate(update_rate); @@ -805,9 +818,13 @@ test(void) errx(1, "open fail"); if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); + if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); + if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); + if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); close(fd); @@ -836,6 +853,16 @@ fake(int argc, char *argv[]) if (handle < 0) errx(1, "advertise failed"); + actuator_armed_s aa; + + aa.armed = true; + aa.lockdown = false; + + handle = orb_advertise(ORB_ID(actuator_armed), &aa); + + if (handle < 0) + errx(1, "advertise failed 2"); + exit(0); } @@ -888,15 +915,17 @@ fmu_main(int argc, char *argv[]) if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) { if (argc > i + 1) { pwm_update_rate_in_hz = atoi(argv[i + 1]); + } else { errx(1, "missing argument for pwm update rate (-u)"); return 1; } + } else { errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio"); } } - } + } /* was a new mode set? */ if (new_mode != PORT_MODE_UNSET) { -- cgit v1.2.3 From 76895af6eb7752615fef69516dea490e8ac998a2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 18 Dec 2012 00:35:28 -0800 Subject: Fix several aspects of the PWM output driver; enable auto-reload, use named constants for various control bits, and use a more polite mechanism at disarm time to avoid runt pulses. This may address an issue we've seen where we get occasional malformed PWM output pulses, possibly due to a race between compare updates and the timer. --- apps/drivers/stm32/drv_pwm_servo.c | 51 ++++++++++++++++++++------------------ 1 file changed, 27 insertions(+), 24 deletions(-) (limited to 'apps') diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index 50aa34d81..954b458f5 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -60,13 +60,13 @@ #include "drv_pwm_servo.h" -#include "chip.h" -#include "up_internal.h" -#include "up_arch.h" +#include +#include +#include -#include "stm32_internal.h" -#include "stm32_gpio.h" -#include "stm32_tim.h" +#include +#include +#include /* default rate (in Hz) of PWM updates */ @@ -143,27 +143,27 @@ pwm_channel_init(unsigned channel) /* configure the channel */ switch (pwm_channels[channel].timer_channel) { case 1: - rCCMR1(timer) |= (6 << 4); + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE; rCCR1(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 0); + rCCER(timer) |= GTIM_CCER_CC1E; break; case 2: - rCCMR1(timer) |= (6 << 12); + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE; rCCR2(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 4); + rCCER(timer) |= GTIM_CCER_CC2E; break; case 3: - rCCMR2(timer) |= (6 << 4); + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE; rCCR3(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 8); + rCCER(timer) |= GTIM_CCER_CC3E; break; case 4: - rCCMR2(timer) |= (6 << 12); + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE; rCCR4(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 12); + rCCER(timer) |= GTIM_CCER_CC4E; break; } } @@ -288,17 +288,20 @@ up_pwm_servo_set_rate(unsigned rate) void up_pwm_servo_arm(bool armed) { - /* - * XXX this is inelgant and in particular will either jam outputs at whatever level - * they happen to be at at the time the timers stop or generate runts. - * The right thing is almost certainly to kill auto-reload on the timers so that - * they just stop at the end of their count for disable, and to reset/restart them - * for enable. - */ - /* iterate timers and arm/disarm appropriately */ for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { - if (pwm_timers[i].base != 0) - rCR1(i) = armed ? GTIM_CR1_CEN : 0; + if (pwm_timers[i].base != 0) { + if (armed) { + /* force an update to preload all registers */ + rEGR(i) = GTIM_EGR_UG; + + /* arm requires the timer be enabled */ + rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + + } else { + /* on disarm, just stop auto-reload so we don't generate runts */ + rCR1(i) &= ~GTIM_CR1_ARPE; + } + } } } -- cgit v1.2.3