Merge branch '#102-pwm-output-correctness'

3 files changed, 85 insertions, 55 deletions

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)
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index 61dd418f8..a995f6214 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -60,6 +60,7 @@
 #include <drivers/boards/px4fmu/px4fmu_internal.h>
 
 #include <systemlib/systemlib.h>
+#include <systemlib/err.h>
 #include <systemlib/mixer/mixer.h>
 #include <drivers/drv_mixer.h>
 
@@ -110,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);
 
@@ -177,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);
@@ -212,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;
@@ -245,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
@@ -279,6 +282,7 @@ PX4FMU::set_mode(Mode mode)
 	default:
 		return -EINVAL;
 	}
+
 	_mode = mode;
 	return OK;
 }
@@ -331,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;
@@ -404,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;
 
@@ -424,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;
@@ -550,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);
@@ -582,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++)
@@ -609,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<<i)) {
+		if (gpios & (1 << i)) {
 			switch (function) {
 			case GPIO_SET_INPUT:
 				stm32_configgpio(_gpio_tab[i].input);
 				break;
+
 			case GPIO_SET_OUTPUT:
 				stm32_configgpio(_gpio_tab[i].output);
 				break;
+
 			case GPIO_SET_ALT_1:
 				if (_gpio_tab[i].alt != 0)
 					stm32_configgpio(_gpio_tab[i].alt);
+
 				break;
 			}
 		}
@@ -636,7 +649,7 @@ PX4FMU::gpio_write(uint32_t gpios, int function)
 	int value = (function == GPIO_SET) ? 1 : 0;
 
 	for (unsigned i = 0; i < _ngpio; i++)
-		if (gpios & (1<<i))
+		if (gpios & (1 << i))
 			stm32_gpiowrite(_gpio_tab[i].output, value);
 }
 
@@ -660,7 +673,7 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
 	lock();
 
 	switch (cmd) {
-		
+
 	case GPIO_RESET:
 		gpio_reset();
 		break;
@@ -762,6 +775,7 @@ fmu_new_mode(PortMode new_mode, int update_rate)
 
 	/* (re)set the PWM output mode */
 	g_fmu->set_mode(servo_mode);
+
 	if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0))
 		g_fmu->set_pwm_rate(update_rate);
 
@@ -800,13 +814,18 @@ test(void)
 
 	fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
 
-	if (fd < 0) {
-		puts("open fail");
-		exit(1);
-	}
+	if (fd < 0)
+		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");
 
-	ioctl(fd, PWM_SERVO_ARM, 0);
-	ioctl(fd, PWM_SERVO_SET(0), 1000);
+	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 +835,8 @@ test(void)
 void
 fake(int argc, char *argv[])
 {
-	if (argc < 5) {
-		puts("fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
-		exit(1);
-	}
+	if (argc < 5)
+		errx(1, "fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
 
 	actuator_controls_s ac;
 
@@ -833,10 +850,18 @@ 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");
+
+	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);
 }
@@ -890,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 {
-					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");
 			}
 		}
-        }
+	}
 
 	/* was a new mode set? */
 	if (new_mode != PORT_MODE_UNSET) {
@@ -915,5 +942,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);
 }
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 <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
 
-#include "stm32_internal.h"
-#include "stm32_gpio.h"
-#include "stm32_tim.h"
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
 
 
 /* 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;
+			}
+		}
 	}
 }