PX4 / PX4-Autopilot

PX4 Autopilot Software
https://px4.io
BSD 3-Clause "New" or "Revised" License
8.58k stars 13.56k forks source link

[Matek H743] Can't open dev/pwm_outputs0 #19562

Open matthewoots opened 2 years ago

matthewoots commented 2 years ago

Hi there,

I'll bring @taileron into this picture since he is working on H743-mini Using the Matek H743 target (be it slim or wing or mini), there seems to be no pwm outputs + there is neither any mains or aux parameters to tune, this is largely weird and in nsh, pwm status gives me the following Screenshot 2022-04-29 23:22:45 Any idea on what is the issue with the H7 boards for Matek so that I can use the pwm outputs? Am I doing the following steps wrongly

(I am using mine based on v1.13.0beta1 and I also tried @taileron fork) To replicate, get a Matek H7 board

taileron commented 2 years ago

Both main and aux outputs provide a signal here. However, the code has become a bit bigger, so more modules have to be taken out to still fit into the flash memory. There is one general thing about this board that causes not every airframe to fit right away. The timers for the outputs are grouped differently than on all other boards. PWM Out 1,2=timer3 then 3 ... 6=timer5 then 7,8,aux 1,2=timer4. Each timer can output only one kind of signal and frequency.
device: /dev/pwm_output0 channel 1: 999 us (default rate: 50 Hz failsafe: 0, disarmed: 999 us, min: 1000 us, max: 2000 us) channel 2: 999 us (default rate: 50 Hz failsafe: 0, disarmed: 999 us, min: 1000 us, max: 2000 us) channel 3: 999 us (default rate: 50 Hz failsafe: 0, disarmed: 999 us, min: 1000 us, max: 2000 us) channel 4: 999 us (default rate: 50 Hz failsafe: 0, disarmed: 999 us, min: 1000 us, max: 2000 us) channel 5: 1500 us (default rate: 50 Hz failsafe: 0, disarmed: 1500 us, min: 1000 us, max: 2000 us) channel 6: 1500 us (default rate: 50 Hz failsafe: 0, disarmed: 1500 us, min: 1000 us, max: 2000 us) channel 7: 1500 us (default rate: 50 Hz failsafe: 0, disarmed: 1500 us, min: 1000 us, max: 2000 us) channel 8: 1500 us (default rate: 50 Hz failsafe: 0, disarmed: 1500 us, min: 1000 us, max: 2000 us) channel group 0: channels 1 2 channel group 1: channels 3 4 5 6 channel group 2: channels 7 8 nsh> pwm info -d /dev/pwm_output1 device: /dev/pwm_output1 channel 1: 1500 us (alternative rate: 50 Hz failsafe: 1500, disarmed: 1500 us, min: 1000 us, max: 2000 us) channel 2: 1500 us (alternative rate: 50 Hz failsafe: 1500, disarmed: 1500 us, min: 1000 us, max: 2000 us)

matthewoots commented 2 years ago

Wow somehow the flash is fixed, but I remembered that it was below 100% but oh well at least now it is working. @taileron maybe you can let me know which airframe worked out for you, I tested standard plane 2100 and also the generic quad x but both did not work,

For me I have this is my code since its for the h743-wing, I have 12 pwm outputs time_config.cpp

constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
    initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}),
    initIOTimer(Timer::Timer5, DMA{DMA::Index1, DMA::Stream0, DMA::Channel6}),
    initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}),
    initIOTimer(Timer::Timer15),
};

constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
    initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}),
    initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
    initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
    initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}),
    initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}),
    initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}),
    initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel1}, {GPIO::PortD, GPIO::Pin12}),
    initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}),
    initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}),
    initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}),
    initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}),
    initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}),
};

board.config_h

/* PWM
 */
#define DIRECT_PWM_OUTPUT_CHANNELS   12
#define BOARD_NUM_IO_TIMERS  4
taileron commented 2 years ago

The implementation of Timer 15 in PX4 is very fresh and untested. Unfortunately I don't have a board available right now where Timer 15 linked outputs are on it. Even in the OS this timer had not been thought for a long time. There could unfortunately still be a gross error in the code, which must still be found.

taileron commented 2 years ago

currently I´m using it in this way (without timer15 / pwm outs 9,10): pwm 11,12 will appear as aux 1,2 (but actually outputs with timer 15 should work at the moment, as there are no compiling errors anymore.) constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}), initIOTimer(Timer::Timer5, DMA{DMA::Index1, DMA::Stream0, DMA::Channel6}), initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}), // initIOTimer(Timer::Timer15), };

constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}), initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}), initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel1}, {GPIO::PortD, GPIO::Pin12}), initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), // initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}), // initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}), };

matthewoots commented 2 years ago

I managed to configure to make modules pwm and pwm_out register 12 outputs, there are some hard coded values and I guess this is because of the original pixhawks having 8 main and 2-4 aux in the io board, but I've changed the values to fit all into mains. Timer 15 is enabled in my setup but I think the crucial thing is starting pwm_out module which is missing in the rc.board_XXX

Here is several things to make it work:

rc.board_sensors (in the board_name/init)

# We need to start the pwm_output, and set the mixer to load the XXX mixer
pwm_out start
param set-default SYS_AUTOSTART XXX
mixer load /dev/pwm_output0 etc/mixers/XXX.main.mix

PWMOut.hpp

static constexpr int PWM_OUT_MAX_INSTANCES{(DIRECT_PWM_OUTPUT_CHANNELS > 12) ? 2 : 1};
// line 72 static constexpr int PWM_OUT_MAX_INSTANCES{(DIRECT_PWM_OUTPUT_CHANNELS > 8) ? 2 : 1};

static const int MAX_PER_INSTANCE{12}; 
// line 128 static const int MAX_PER_INSTANCE{8};

PWMOut.cpp

// line 862
#endif
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 8
    // Add get 8-11 which is missing out of the 8 servo mapping
    case PWM_SERVO_GET(11):
    case PWM_SERVO_GET(10):
    case PWM_SERVO_GET(9):
    case PWM_SERVO_GET(8):

    case PWM_SERVO_GET(7):
    case PWM_SERVO_GET(6):

Small code error in pwm.cpp not major at all just a printout error

// line 569
if (ret == OK) {
        printf("channel %u: %" PRIu16 " us", i, spos); // printf("channel %u: %" PRIu16 " us", i+1, spos); 

I've not tried testing by arming but this is to get all pwm to be viewed Screenshot 2022-05-02 23:00:59

Screenshot 2022-05-02 23:00:39

taileron commented 2 years ago

The usual distribution of PWM outputs is 1-8 main and everything above that is always aux 1-X depending on how many there are in total. This history only comes from the fact that most boards used to have an additional I/O unit with its own MCU providing its own 8 outputs. This is also the basis for most airframes.

spiderkeys commented 2 years ago

I know that it isn't completely equivalent to your case, since we use ControlAllocator/Dynamic Mixing and not the old static mixer approach, but we use 12 out of the 13 PWM channels on the H743-Wing in our PX4 application. We use S2 through S12 and LED as servo outputs (while using S1 as a GPIO input), with S3-S10 being a group of 8 motors on timers 4 & 5, and the remaining four channels (S2, S11, S12, LED) being auxiliary servo functions. We kept all 8 motors on timers 4/5 so that they could all use DSHOT without interfering with the timer requirements for the aux servos.

The config is as below:

board_config.h

/* PWM
 */
#define DIRECT_PWM_OUTPUT_CHANNELS   12
#define BOARD_NUM_IO_TIMERS 5

timer_config.cpp

constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
    initIOTimer(Timer::Timer5, DMA{DMA::Index1}),
    initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
    initIOTimer(Timer::Timer8, DMA{DMA::Index1}),
    initIOTimer(Timer::Timer15),
    initIOTimer(Timer::Timer1, DMA{DMA::Index1})
};

constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
    initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
    initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}),
    initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}),
    initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}),
    initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel1}, {GPIO::PortD, GPIO::Pin12}),
    initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}),
    initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}),
    initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}),
    initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel3}, {GPIO::PortB, GPIO::Pin1}),
    initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}),
    initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}),
    initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8})
};

When used with ControlAllocator (SYS_CTRL_ALLOC=1), all outputs show up on PWM_MAIN and there is no longer a PWM_AUX. Each channel can then be assigned a function, in the order in which the timer channels are specified in your board config.

It's important to note that using the Timer8 channel/pin shown above requires some additional modifications to io_timer.c to allow the use of the complementary channel 3N. My quick hacks to do so are included for reference below, but in some discussions with @davids5, he mentioned that he has some ideas for how to add better support for complementary channels without the need for my very specific ifdef's as I have done in this case.

See the sections below that reference TIM8_USE_COMPLEMENTARY if this might be of use:

int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks)
{

    struct action_cache_t {
        uint32_t ccer_clearbits;
        uint32_t ccer_setbits;
        uint32_t dier_setbits;
        uint32_t dier_clearbits;
        uint32_t base;
        uint32_t gpio[MAX_CHANNELS_PER_TIMER];
    } action_cache[MAX_IO_TIMERS];
    memset(action_cache, 0, sizeof(action_cache));

    uint32_t dier_bit = state ? GTIM_DIER_CC1IE : 0;
    uint32_t ccer_bit =  state ? CCER_C1_INIT : 0;
    uint32_t cr1_bit  = 0;

    switch (mode) {
    case IOTimerChanMode_NotUsed:
    case IOTimerChanMode_OneShot:
    case IOTimerChanMode_PWMOut:
    case IOTimerChanMode_Trigger:
        dier_bit = 0;
        cr1_bit  = GTIM_CR1_CEN | GTIM_CR1_ARPE;
        break;

    case IOTimerChanMode_Dshot:
        dier_bit = 0;
        cr1_bit  = state ? GTIM_CR1_CEN : 0;
        break;

    case IOTimerChanMode_PWMIn:
    case IOTimerChanMode_Capture:
        break;

    default:
        return -EINVAL;
    }

    /* Was the request for all channels in this mode ?*/

    if (masks == IO_TIMER_ALL_MODES_CHANNELS) {

        /* Yes - we provide them */

        masks = channel_allocations[mode];

    } else {

        /* No - caller provided mask */

        /* Only allow the channels in that mode to be affected */

        masks &= channel_allocations[mode];

    }

    /* Pre calculate all the changes */

    for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) {
        if (masks & (1 << chan_index)) {
            masks &= ~(1 << chan_index);
            uint32_t shifts = timer_io_channels[chan_index].timer_channel - 1;
            uint32_t timer = channels_timer(chan_index);
            action_cache[timer].base  = io_timers[timer].base;

            #ifdef TIM8_USE_COMPLEMENTARY
                if( action_cache[timer].base == STM32_TIM8_BASE )
                {
                    action_cache[timer].ccer_clearbits |= CCER_C1_INIT << (shifts * CCER_C1_NUM_BITS + 2 );
                    action_cache[timer].ccer_setbits   |= ccer_bit  << ( shifts * CCER_C1_NUM_BITS + 2 );
                }
                else
                {
                    action_cache[timer].ccer_clearbits |= CCER_C1_INIT << (shifts * CCER_C1_NUM_BITS);
                    action_cache[timer].ccer_setbits   |= ccer_bit  << (shifts * CCER_C1_NUM_BITS);
                }
            #else
                action_cache[timer].ccer_clearbits |= CCER_C1_INIT << (shifts * CCER_C1_NUM_BITS);
                action_cache[timer].ccer_setbits   |= ccer_bit  << (shifts * CCER_C1_NUM_BITS);
            #endif

            action_cache[timer].dier_clearbits |= GTIM_DIER_CC1IE  << shifts;
            action_cache[timer].dier_setbits   |= dier_bit << shifts;

            if ((state &&
                 (mode == IOTimerChanMode_PWMOut ||
                  mode == IOTimerChanMode_OneShot ||
                  mode == IOTimerChanMode_Dshot ||
                  mode == IOTimerChanMode_Trigger))) {
                action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out;
            }
        }
    }

    irqstate_t flags = px4_enter_critical_section();

    for (unsigned actions = 0; actions < arraySize(action_cache); actions++) {
        if (action_cache[actions].base != 0) {
            uint32_t rvalue = _REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET);
            rvalue &= ~action_cache[actions].ccer_clearbits;
            rvalue |= action_cache[actions].ccer_setbits;
            _REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET) = rvalue;

            uint32_t after;
            #ifdef TIM8_USE_COMPLEMENTARY
                if( action_cache[actions].base == STM32_TIM8_BASE )
                {
                    after = rvalue & (GTIM_CCER_CC1NE | GTIM_CCER_CC2NE | GTIM_CCER_CC3NE | GTIM_CCER_CC4E);
                }
                else
                {
                    after = rvalue & (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E);
                }
            #else
                after = rvalue & (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E);
            #endif

            rvalue = _REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET);
            rvalue &= ~action_cache[actions].dier_clearbits;
            rvalue |= action_cache[actions].dier_setbits;
            _REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET) = rvalue;

            /* Any On ?*/

            if (after != 0) {

                /* force an update to preload all registers */
                rEGR(actions) = GTIM_EGR_UG;

                for (unsigned chan = 0; chan < arraySize(action_cache[actions].gpio); chan++) {
                    if (action_cache[actions].gpio[chan]) {
                        px4_arch_configgpio(action_cache[actions].gpio[chan]);
                        action_cache[actions].gpio[chan] = 0;
                    }
                }

                /* arm requires the timer be enabled */
                rCR1(actions) |= cr1_bit;

            } else  {

                rCR1(actions) = 0;
            }
        }
    }

    px4_leave_critical_section(flags);

    return 0;
}
matthewoots commented 2 years ago

Thank you @spiderkeys! Amazing work! I think since you have made a working build of the H743-Wing with the control allocation, then this is the closest to what I'm trying to do with the standard mixer module. TIM8_CH2N and TIM8_CH3N applies to the rest of the H743 models which I'm wondering why its working for @taileron and maybe anyone who have tried and used Timer3, Timer::Channel3 and Timer3, Timer::Channel4

For now I'll test out with the TIM8_USE_COMPLEMENTARY def first and your timer_config.cpp to test the individual servos first. Cheers!

taileron commented 2 years ago

In the H743 mini board, the first 4 outputs are on a socket for a 4:1 ESC and 1,2 already seem to be pins B0,B1. I will measure this again. Now only the support of the 3 different board variants with in each case different IMU at both positions must be integrated. I already have 2 versions of this board. Probably a separate H743-Mini target will be necessary. V3 has also a different pwm out pin structure. V1 and V1.5 I have tested and added support for the different IMU incl. rotation, which run with timer 3 on out 1,2 will retest that with the 2 different boards I have. http://www.mateksys.com/?portfolio=h743-mini Currently I don´t know how to integrate support also for V3 that has more changes than the previous.

taileron commented 2 years ago

V1 and V1.5 imu are working and merged now. I have measured the first 4 pwm outs are definitely on the 4:1 esc socket with different io timer on out 1,2 and out 3,4. 3,4 and 5,6 use the same io timer so a proper 1...4 pwm rate or dshot is only possible when the mc default in romfs is changed from pwm out 1234 to 123546

matthewoots commented 2 years ago

Sorry for the delay, @taileron that's great news, I've also managed to get the H743-Wing V2 to work in a usable way and here are some supp and test

I'll send out a pull request for H743-Wing V2 and include a markdown step-by-step for flashing the H743 series, thanks for the help @taileron (Why this issue thread even came about is my mistake, it's because of my stupid mistake in removing the mfd and mtd modules causing dev/pwm_outputs0 to not load and probably the whole pwm rail is unresponsive, but now its working well and dmesg shows a good load with all the rc.board_XXX loading properly)