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函数计算得到的。
而其中具体每一个参数是做什么的，这个就要搞懂了流程以后详细去分析了。