fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

add oneshot mode capability

Browse Source 
change fmu to task

increase fmu_servo task priority to max and enable true oneshot

use lowest FMU priority which minimizes jitter

constrain oneshot updates to control group 0 events
This commit is contained in:
Mark Whitehorn 2017-02-14 10:08:14 -07:00 committed by Lorenz Meier
parent eac72051b8
commit aa9fbbedd5
10 changed files with 717 additions and 552 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/drivers/drv_pwm_output.h
View File

@ -305,6 +305,9 @@ struct pwm_output_rc_config {
* This is the low-level API to the platform-specific PWM driver.
*/
__EXPORT extern void up_pwm_set_oneshot_mode(bool on);
__EXPORT extern bool up_pwm_get_oneshot_mode(void);
/**
* Intialise the PWM servo outputs using the specified configuration.
*
@ -360,6 +363,13 @@ __EXPORT extern uint32_t up_pwm_servo_get_rate_group(unsigned group);
*/
__EXPORT extern int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate);
/**
* Force update of all timer channels in group.
*
* @param group The rate group to update.
*/
__EXPORT extern void up_pwm_force_update(void);
/**
* Set the current output value for a channel.
*

1087
src/drivers/px4fmu/fmu.cpp
View File

File diff suppressed because it is too large Load Diff

6
src/drivers/px4io/px4io.cpp
View File

@ -951,7 +951,7 @@ PX4IO::task_main()
* primary PWM output or not.
*/
_t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0));
orb_set_interval(_t_actuator_controls_0, 20); /* default to 50Hz */
// orb_set_interval(_t_actuator_controls_0, 20); /* default to 50Hz */
_t_actuator_controls_1 = orb_subscribe(ORB_ID(actuator_controls_1));
orb_set_interval(_t_actuator_controls_1, 33); /* default to 30Hz */
_t_actuator_controls_2 = orb_subscribe(ORB_ID(actuator_controls_2));
@ -3527,6 +3527,8 @@ test(void)
}
}
usleep(250);
/* readback servo values */
for (unsigned i = 0; i < servo_count; i++) {
servo_position_t value;
@ -3536,7 +3538,7 @@ test(void)
}
if (value != servos[i]) {
errx(1, "servo %u readback error, got %hu expected %hu", i, value, servos[i]);
warnx("servo %u readback error, got %hu expected %hu", i, value, servos[i]);
}
}

74
src/drivers/stm32/drv_io_timer.c
View File

@ -103,13 +103,17 @@
#define CCMR_C1_PWMOUT_INIT (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE
#define CCMR_C1_ONESHOT_INIT (GTIM_CCMR_MODE_PWM2 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE
#define CCMR_C1_PWMIN_INIT 0 // TBD
// NotUsed PWMOut PWMIn Capture
io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT8_MAX, 0, 0, 0 };
// NotUsed PWMOut PWMIn Capture OneShot
io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT8_MAX, 0, 0, 0, 0 };
typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
static uint8_t timer_freq[MAX_IO_TIMERS] = { 1, 1, 1, 1 }; /* default 1 MHz counter frequency */
static io_timer_allocation_t once = 0;
typedef struct channel_stat_t {
@ -242,7 +246,7 @@ static uint32_t get_timer_channels(unsigned timer)
if (validate_timer_index(timer) == 0) {
const io_timers_t *tmr = &io_timers[timer];
/* Gather the channel bit that belong to the timer */
/* Gather the channel bits that belong to the timer */
for (unsigned chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) {
channels |= 1 << chan_index;
@ -376,7 +380,7 @@ static int timer_set_rate(unsigned timer, unsigned rate)
{
/* configure the timer to update at the desired rate */
rARR(timer) = 1000000 / rate;
rARR(timer) = timer_freq[timer] * 1000000 / rate;
/* generate an update event; reloads the counter and all registers */
rEGR(timer) = GTIM_EGR_UG;
@ -384,6 +388,31 @@ static int timer_set_rate(unsigned timer, unsigned rate)
return 0;
}
//#define FAKE_ONESHOT
void io_timer_set_oneshot_mode(unsigned timer)
{
timer_freq[timer] = 8;
#ifdef FAKE_ONESHOT
// to eliminate jitter with max additional latency of 500usec, set PWM rate to 2KHz.
// Note that the FMU mixer jitter is on the order of 2msec.
timer_set_rate(timer, 2000);
#else
// set rate to 125Hz (lowest possible at 8MHz is ~122Hz)
// io_timer_set_ccr() must be called at > 125Hz to minimize latency
timer_set_rate(timer, 125);
#endif
}
extern void io_timer_force_update(unsigned timer)
{
#ifdef FAKE_ONESHOT
// let's not and say we did
#else
// force update of channel compare register
rEGR(timer) |= GTIM_EGR_UG;
#endif
}
int io_timer_init_timer(unsigned timer)
{
@ -423,12 +452,19 @@ int io_timer_init_timer(unsigned timer)
rBDTR(timer) = ATIM_BDTR_MOE;
}
/* If the timer clock source provided as clock_freq is the STM32_APBx_TIMx_CLKIN
* then configure the timer to free-run at 1MHz.
* Otherwize, other frequencies are attainable by adjusting .clock_freq accordingly.
/* If in oneshot mode, configure the prescaler for 8MHz output.
* else set prescaler for 1MHz.
*/
rPSC(timer) = (io_timers[timer].clock_freq / 1000000) - 1;
if (timer_freq[timer] == 8) {
rPSC(timer) = (io_timers[timer].clock_freq / 8000000) - 1;
// can't find a way to trigger in this mode
// /* set one pulse mode */
// rCR1(timer) |= 1 << 3;
} else {
rPSC(timer) = (io_timers[timer].clock_freq / 1000000) - 1;
}
/*
* Note we do the Standard PWM Out init here
@ -455,7 +491,7 @@ int io_timer_init_timer(unsigned timer)
int io_timer_set_rate(unsigned timer, unsigned rate)
{
/* Gather the channel bit that belong to the timer */
/* Gather the channel bits that belong to the timer */
uint32_t channels = get_timer_channels(timer);
@ -484,6 +520,12 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
/* figure out the GPIO config first */
switch (mode) {
case IOTimerChanMode_OneShot:
io_timer_set_oneshot_mode(timer_io_channels[channel].timer_index);
// intentional fallthrough
case IOTimerChanMode_PWMOut:
ccer_setbits = 0;
dier_setbits = 0;
@ -514,7 +556,7 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
if (rv >= 0) {
/* Blindly try to initialize the time - it will only do it once */
/* Blindly try to initialize the timer - it will only do it once */
io_timer_init_timer(channels_timer(channel));
@ -593,6 +635,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
switch (mode) {
case IOTimerChanMode_NotUsed:
case IOTimerChanMode_PWMOut:
case IOTimerChanMode_OneShot:
dier_bit = 0;
break;
@ -635,7 +678,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
action_cache[timer].dier_clearbits |= GTIM_DIER_CC1IE << shifts;
action_cache[timer].dier_setbits |= dier_bit << shifts;
if (state && mode == IOTimerChanMode_PWMOut) {
if ((state && mode == IOTimerChanMode_PWMOut) || (state && mode == IOTimerChanMode_OneShot)) {
action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out;
}
}
@ -689,13 +732,16 @@ int io_timer_set_ccr(unsigned channel, uint16_t value)
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
if (io_timer_get_channel_mode(channel) != IOTimerChanMode_PWMOut) {
if ((io_timer_get_channel_mode(channel) != IOTimerChanMode_PWMOut) &&
(io_timer_get_channel_mode(channel) != IOTimerChanMode_OneShot)) {
rv = -EIO;
} else {
/* configure the channel */
// this hack won't work correctly in oneshot mode; would be OK if it just inverted the output
#ifdef BOARD_PWM_DRIVE_ACTIVE_LOW
unsigned period = rARR(channels_timer(channel));
value = period - value;
@ -705,6 +751,7 @@ int io_timer_set_ccr(unsigned channel, uint16_t value)
value--;
}
// write PWM value into CCR preload register
REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) = value;
}
}
@ -717,7 +764,8 @@ uint16_t io_channel_get_ccr(unsigned channel)
uint16_t value = 0;
if (io_timer_validate_channel_index(channel) == 0 &&
io_timer_get_channel_mode(channel) == IOTimerChanMode_PWMOut) {
((io_timer_get_channel_mode(channel) == IOTimerChanMode_PWMOut) ||
(io_timer_get_channel_mode(channel) == IOTimerChanMode_OneShot))) {
value = REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) + 1;
#ifdef BOARD_PWM_DRIVE_ACTIVE_LOW

17
src/drivers/stm32/drv_io_timer.h
View File

@ -58,12 +58,19 @@ typedef enum io_timer_channel_mode_t {
IOTimerChanMode_PWMOut = 1,
IOTimerChanMode_PWMIn = 2,
IOTimerChanMode_Capture = 3,
IOTimerChanMode_OneShot = 4,
IOTimerChanModeSize
} io_timer_channel_mode_t;
typedef uint8_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */
/* array of timers dedicated to PWM in and out and capture use */
/* array of timers dedicated to PWM in and out and capture use
*** Note that the clock_freq field must be set to the frequency (in Hz) of the selected clock.
*** Normal PWM timers set the prescaler to achieve a counter frequency of 1MHz
*** and OneShot PWM timers set the prescaler to achieve a counter frequency of 8MHz.
*** These counter frequencies are specified (in MHz) in array timer_freq[MAX_IO_TIMERS].
*** Beware that legacy code *assumes* that the counter frequency is 1MHz.
*/
typedef struct io_timers_t {
uint32_t base;
uint32_t clock_register;
@ -108,6 +115,7 @@ __EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mod
channel_handler_t channel_handler, void *context);
__EXPORT int io_timer_init_timer(unsigned timer);
__EXPORT void io_timer_set_oneshot_mode(unsigned timer);
__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
__EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode,
@ -121,4 +129,11 @@ __EXPORT int io_timer_is_channel_free(unsigned channel);
__EXPORT int io_timer_free_channel(unsigned channel);
__EXPORT int io_timer_get_channel_mode(unsigned channel);
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
/**
* Force update of all timer channels
*
* @param timer The timer to force.
*/
__EXPORT extern void io_timer_force_update(unsigned timer);
__END_DECLS

49
src/drivers/stm32/drv_pwm_servo.c
View File

@ -63,6 +63,22 @@
#include <stm32_tim.h>
/* pwm_oneshot_mode true implies all servo outputs are oneshot */
static bool pwm_oneshot_mode = false;
/* this is a bitfield: if bit N is set, that timer is used for oneshot only */
static uint8_t oneshot_timers;
void up_pwm_set_oneshot_mode(bool value)
{
pwm_oneshot_mode = value;
}
bool up_pwm_get_oneshot_mode()
{
return pwm_oneshot_mode;
}
int up_pwm_servo_set(unsigned channel, servo_position_t value)
{
return io_timer_set_ccr(channel, value);
@ -76,7 +92,13 @@ servo_position_t up_pwm_servo_get(unsigned channel)
int up_pwm_servo_init(uint32_t channel_mask)
{
/* Init channels */
uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut);
io_timer_channel_mode_t chmode = IOTimerChanMode_PWMOut;
if (pwm_oneshot_mode) {
chmode = IOTimerChanMode_OneShot;
}
uint32_t current = io_timer_get_mode_channels(chmode);
// First free the current set of PWMs
@ -87,6 +109,8 @@ int up_pwm_servo_init(uint32_t channel_mask)
}
}
oneshot_timers = 0;
// Now allocate the new set
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
@ -98,8 +122,13 @@ int up_pwm_servo_init(uint32_t channel_mask)
io_timer_free_channel(channel);
}
io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
io_timer_channel_init(channel, chmode, NULL, NULL);
channel_mask &= ~(1 << channel);
if (pwm_oneshot_mode) {
// update the oneshot_timers bitfield
oneshot_timers |= (1 << timer_io_channels[channel].timer_index);
}
}
}
@ -132,6 +161,15 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
return OK;
}
void up_pwm_force_update(void)
{
for (unsigned i = 0; i < 8; i++) {
if (oneshot_timers & (1 << i)) {
io_timer_force_update(i);
}
}
}
int up_pwm_servo_set_rate(unsigned rate)
{
for (unsigned i = 0; i < MAX_IO_TIMERS; i++) {
@ -149,5 +187,10 @@ uint32_t up_pwm_servo_get_rate_group(unsigned group)
void
up_pwm_servo_arm(bool armed)
{
io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
if (pwm_oneshot_mode) {
io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS);
} else {
io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
}
}

17
src/modules/px4iofirmware/mixer.cpp
View File

@ -75,6 +75,9 @@ static volatile bool should_arm_nothrottle = false;
static volatile bool should_always_enable_pwm = false;
static volatile bool in_mixer = false;
static bool new_fmu_data = false;
static uint64_t last_fmu_update = 0;
extern int _sbus_fd;
/* selected control values and count for mixing */
@ -133,6 +136,11 @@ mixer_tick(void)
/* this flag is never cleared once OK */
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
if (system_state.fmu_data_received_time > last_fmu_update) {
new_fmu_data = true;
last_fmu_update = system_state.fmu_data_received_time;
}
}
/* default to failsafe mixing - it will be forced below if flag is set */
@ -301,6 +309,15 @@ mixer_tick(void)
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
}
if (new_fmu_data) {
new_fmu_data = false;
if (up_pwm_get_oneshot_mode()) {
up_pwm_force_update();
}
}
}
/* set arming */

1
src/modules/px4iofirmware/px4io.c
View File

@ -235,6 +235,7 @@ int
user_start(int argc, char *argv[])
{
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_set_oneshot_mode(true);
up_pwm_servo_init(0xff);
#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)

4
src/modules/px4iofirmware/registers.c
View File

@ -325,6 +325,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_RAW_PWM;
if (up_pwm_get_oneshot_mode()) {
up_pwm_force_update();
}
break;
/* handle setup for servo failsafe values */

4
src/systemcmds/pwm/pwm.c
View File

@ -61,6 +61,7 @@
#include "systemlib/err.h"
#include "systemlib/param/param.h"
#include "drivers/drv_pwm_output.h"
#include "drivers/stm32/drv_io_timer.h"
static void usage(const char *reason);
__EXPORT int pwm_main(int argc, char *argv[]);
@ -617,6 +618,7 @@ pwm_main(int argc, char *argv[])
warnx("Press CTRL-C or 'c' to abort.");
while (1) {
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1 << i) {
ret = px4_ioctl(fd, PWM_SERVO_SET(i), pwm_value);
@ -654,7 +656,7 @@ pwm_main(int argc, char *argv[])
}
}
usleep(2000);
usleep(2500);
}
return 0;