dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
px4dev 2012-12-23 11:38:16 -08:00
parent 6b3f36020c 76895af6eb
commit 95b3828e41
3 changed files with 85 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
apps/drivers/drv_pwm_output.h
View File

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

87
apps/drivers/px4fmu/fmu.cpp
View File

@ -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");
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 +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);
}

51
apps/drivers/stm32/drv_pwm_servo.c
View File

@ -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;
}
}
}
}