PX4 IO [15] mixer [转载]

PX4 IO [15] mixer

PX4 IO [15] mixer
                                                                             -------- 转载请注明出处 
                                                                             -------- 更多笔记请访问我的博客:merafour.blog.163.com 

                                                                             -------- 2015-1-5.冷月追风

                                                                             -------- email:merafour@163.com 



    看过了 IO的输入,我们现在来看看 IO中是怎么把信号输出的。 
    这个我们当然得先知道在 fmu中是怎么往 IO发这些输出数据的。 
    回过头去看第十一篇笔记,我们会看到 " AP_MotorsMatrix::output_test"函数的源码如下: 

// output_test - spin a motor at the pwm value specified
//  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsMatrix::output_test(uint8_t motor_seq, int16_t pwm)
{
    // exit immediately if not armed
    if (!_flags.armed) {
        return;
    }

    // loop through all the possible orders spinning any motors that match that description
    for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i] && _test_order[i] == motor_seq) {
            // turn on this motor
            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), pwm);
        }
    }
}


也就是说通过 " hal.rcout->write"接口下发数据。" rcout"类型为 "PX4RCOutput"。从前面的分析中我们知道 "PX4RCOutput"其实是通过设备文件对 px4io进行调用,最终调用的是 "PX4IO::write"函数,源码如下: 

ssize_t PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
/* Make it obvious that file * isn't used here */
{
    unsigned count = len / 2;

    if (count > _max_actuators)
        count = _max_actuators;

    if (count > 0) {

        perf_begin(_perf_write);
        int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
        perf_end(_perf_write);

        if (ret != OK)
            return ret;
    }

    return count * 2;
}


px4io在我们的 V2版本中通过串口跟 io板通信。所以两者通信的关键在于 "PX4IO_PAGE_DIRECT_PWM"这个宏, 

radiolink@ubuntu:~/apm$ grep -nr PX4IO_PAGE_DIRECT_PWM ./PX4Firmware/src/
./PX4Firmware/src/modules/px4iofirmware/protocol.h:258:#definePX4IO_PAGE_DIRECT_PWM                    54              /**< 0..CONFIG_ACTUATOR_COUNT-1 */
./PX4Firmware/src/modules/px4iofirmware/registers.c:284:        casePX4IO_PAGE_DIRECT_PWM:
./PX4Firmware/src/modules/px4iofirmware/registers.c:883:        casePX4IO_PAGE_DIRECT_PWM:
./PX4Firmware/src/drivers/px4io/px4io.cpp:2366:                         ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
./PX4Firmware/src/drivers/px4io/px4io.cpp:2649:         int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
radiolink@ubuntu:~/apm$

因为我们现在关注的是 io固件,所以我们现在要找的源码在 "registers.c"源文件中。可能我们觉得这里值应该出现一个 "case"语句,但这里却出现了两个,为什么?如果你去看源码你会看到实际上有一个 "registers_set"和一个 "registers_get"函数。当然 get函数我们没有使用,就不去关心。下面我们就来看看 set函数。

int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsignednum_values)
{
    switch (page) {
  /* ... */
        /* handle raw PWM input */
    case PX4IO_PAGE_DIRECT_PWM:
        /* copy channel data */
        while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
            /* XXX range-check value? */
            r_page_servos[offset] = *values;
            offset++;
            num_values--;
            values++;
        }
        system_state.fmu_data_received_time = hrt_absolute_time();
        r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
        break;

所以我们看到,在这里 set主要是数据拷贝动作。而我们现在要解决的是在把数据放到 "r_page_servos"数组中之后数据是怎样到大寄存器的。

radiolink@ubuntu:~/apm$ grep -nr r_page_servos ./PX4Firmware/src/modules/px4iofirmware/
./PX4Firmware/src/modules/px4iofirmware/px4io.h:76:externuint16_t                      r_page_servos[];        /* PX4IO_PAGE_SERVOS */
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:217:                  r_page_servos[i] = r_page_servo_failsafe[i];
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:220:                  r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:237:          pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:240:                  r_page_servos[i] = 0;
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:276:                  up_pwm_servo_set(i, r_page_servos[i]);
./PX4Firmware/src/modules/px4iofirmware/registers.c:107:uint16_t                r_page_servos[PX4IO_SERVO_COUNT];
./PX4Firmware/src/modules/px4iofirmware/registers.c:221: * PAGE 104 uses r_page_servos.
./PX4Firmware/src/modules/px4iofirmware/registers.c:291:                                r_page_servos[offset] = *values;
./PX4Firmware/src/modules/px4iofirmware/registers.c:864:                SELECT_PAGE(r_page_servos);
./PX4Firmware/src/modules/px4iofirmware/registers.c:884:                SELECT_PAGE(r_page_servos);
radiolink@ubuntu:~/apm$

去阅读源码我们就会发现, mixer.cpp中的结果均来自同一个函数: mixer_tick。该函数同样是在 Px4io.c中由 user_start调用。代码如下:

int user_start(int argc, char *argv[])
{
    /* ... */
    for (;;) {
        /* track the rate at which the loop is running */
        perf_count(loop_perf);
        /* kick the mixer */
        perf_begin(mixer_perf);
        mixer_tick();
        perf_end(mixer_perf);
        /* kick the control inputs */
        perf_begin(controls_perf);
        controls_tick();
        perf_end(controls_perf);

这样我们就只需要关心 mixer_tick函数即可。而该函数从头到尾超过 150行代码,我们在看源码的时候要将它拆成几段。

void mixer_tick(void)
{
    /* check that we are receiving fresh data from the FMU */
    if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
        /* too long without FMU input, time to go to failsafe */
        if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
            isr_debug(1, "AP RX timeout");
        }
        r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
        r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
    } else {
        r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
    }
#define FMU_INPUT_DROP_LIMIT_US        200000

这段代码并不难理解,是用来检测超时的。超时时间为 200ms。可能我们都会奇怪, user_start中的主循环执行一次会消耗 200ms吗?关于这点,我只能说目前我还没有去研究它是怎么控制循环的,暂时不予讨论。

    /* default to failsafe mixing */
    source = MIX_FAILSAFE;
    /*
     * Decide which set of controls we're using.
     */
    /* do not mix if RAW_PWM mode is on and FMU is good */
    if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
            (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
        /* don't actually mix anything - we already have raw PWM values */
        source = MIX_NONE;
    } else {
        if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
            /* mix from FMU controls */
            source = MIX_FMU;
        }
        if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
             !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
             !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
             /* if allowed, mix from RC inputs directly */
            source = MIX_OVERRIDE;
        } else     if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
             !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
             (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
            /* if allowed, mix from RC inputs directly up to available rc channels */
            source = MIX_OVERRIDE_FMU_OK;
        }
    }
    /*
     * Set failsafe status flag depending on mixing source
     */
    if (source == MIX_FAILSAFE) {
        r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
    } else {
        r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
    }

r_status_flags就是前面我们在 set函数中设置的标志,如果set函数被正常调用,那么到这里 source的值为 MIX_NONE。关于失控保护,正常情况下是可以忽略的。

    /*
     * Decide whether the servos should be armed right now.
     *
     * We must be armed, and we must have a PWM source; either raw from
     * FMU or from the mixer.
     *
     * XXX correct behaviour for failsafe may require an additional case
     * here.
     */
    should_arm = (
        /* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
        /* and IO is armed */           && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
        /* and FMU is armed */           && (
                                ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
        /* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
        /* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
        /* or failsafe was set manually */     || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
                             )
    );
    should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
                        && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
                        && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);

这段代码注释说的比较清楚,是进行解锁检查的。说到解锁,我们都还没看 fmu是怎么解锁的,待会还真得看看。飞控通常都是要解锁之后才能飞的,这是出于安全考虑。我想 fmu解锁了应该也会通过串口发送一个信息给 io并最终调用 set函。

    /*
     * Run the mixers.
     */
    if (source == MIX_FAILSAFE) {
        /* copy failsafe values to the servo outputs */
        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
            r_page_servos[i] = r_page_servo_failsafe[i];
            /* safe actuators for FMU feedback */
            r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
        }
    } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
        float    outputs[PX4IO_SERVO_COUNT];
        unsigned mixed;
        /* mix */
        /* poor mans mutex */
        in_mixer = true;
        mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
        in_mixer = false;
        pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
        for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
            r_page_servos[i] = 0;
        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
            r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
        }
    }

从前面的代码中我们知道 source的值为 MIX_NONE,所以这一段不会执行。这段代码只是给 r_page_servos一个特定的值,前面是失控保护,而后面应该是关闭电机。其实如果 fmu发过来的数据是 1ms的脉宽,电机也是关闭的。

    /* set arming */
    bool needs_to_arm = (should_arm || should_always_enable_pwm);
    /* check any conditions that prevent arming */
    if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
        needs_to_arm = false;
    }
    if (!should_arm && !should_always_enable_pwm) {
        needs_to_arm = false;
    }
    if (needs_to_arm && !mixer_servos_armed) {
        /* need to arm, but not armed */
        up_pwm_servo_arm(true);
        mixer_servos_armed = true;
        r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
        isr_debug(5, "> PWM enabled");
    } else if (!needs_to_arm && mixer_servos_armed) {
        /* armed but need to disarm */
        up_pwm_servo_arm(false);
        mixer_servos_armed = false;
        r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
        isr_debug(5, "> PWM disabled");
    }
    if (mixer_servos_armed && should_arm) {
        /* update the servo outputs. */
        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
            up_pwm_servo_set(i, r_page_servos[i]);
    } else if (mixer_servos_armed && should_always_enable_pwm) {
        /* set the disarmed servo outputs. */
        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
            up_pwm_servo_set(i, r_page_servo_disarmed[i]);
    }
int up_pwm_servo_set(unsigned channel, servo_position_t value)
{
    if (channel >= PWM_SERVO_MAX_CHANNELS)
        return -1;

    unsigned timer = pwm_channels[channel].timer_index;

    /* test timer for validity */
    if ((pwm_timers[timer].base == 0) ||
        (pwm_channels[channel].gpio == 0))
        return -1;

    /* configure the channel */
    if (value > 0)
        value--;

    switch (pwm_channels[channel].timer_channel) {
    case 1:
        rCCR1(timer) = value;
        break;

    case 2:
        rCCR2(timer) = value;
        break;

    case 3:
        rCCR3(timer) = value;
        break;

    case 4:
        rCCR4(timer) = value;
        break;

    default:
        return -1;
    }

    return 0;
}

所以最终是通过 up_pwm_servo_set函数最终将 PWM信号输出的。其中 r_page_servos这组数据是 fmu发送过来的,而 r_page_servo_disarmed这组数据是前面 pwm_limit_calc函数计算得到的。

    而其中具体每一个参数是做什么的,这个就要搞懂了流程以后详细去分析了。

上一篇:python的re.py分析


下一篇:Day11 - 使用正则表达式