Pixhawk(APM固件) ArduPilot的遥控器读取及油门转换

Pixhawk(APM固件) ArduPilot的遥控器读取及油门转换

1.遥控器硬件连接

Futaba的接收机通过Sbus连接到PX4IO芯片上，即STM32F10X的芯片上，PX4IO与PX4FMU(STM32F42X)通过串口相连，这样接收机数据就由PX4IO读取并送到PX4FMU中，本博客记录ArduPilot上层读取遥控器数据及油门转换的过程。

2.遥控器各通道数据读取

ArduCopter.cpp中定义了rc_loop这个scheduler，以100Hz的速度运行，rc_loop函数也定义在此文件中，其定义如下：

void Copter::rc_loop()
{
    // Read radio and 3-position switch on radio
    // -----------------------------------------
    read_radio();
    read_control_switch();
}

read_radio用来读取各个通道的PWM值，并进行角度或范围的转换。 read_control_switch根据上一函数读到的PWM值判断开关的位置，并作进一步处理。今天主要看read_radio.此函数定义在radio.cpp中，原型如下：

    static uint32_t last_update_ms = 0;
    uint32_t tnow_ms = millis();


    if (hal.rcin->new_input()) {
        last_update_ms = tnow_ms;
        ap.new_radio_frame = true;
        RC_Channel::set_pwm_all();


        set_throttle_and_failsafe(channel_throttle->radio_in);
        set_throttle_zero_flag(channel_throttle->control_in);


        // flag we must have an rc receiver attached
        if (!failsafe.rc_override_active) {
            ap.rc_receiver_present = true;
        }


        // update output on any aux channels, for manual passthru
        RC_Channel_aux::output_ch_all();
    }else{
        uint32_t elapsed = tnow_ms - last_update_ms;
        // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
        if (((!failsafe.rc_override_active && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (failsafe.rc_override_active && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) &&
            (g.failsafe_throttle && (ap.rc_receiver_present||motors.armed()) && !failsafe.radio)) {
            Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME);
            set_failsafe_radio(true);
        }
    }
}

第一步判断是否有新的输入，主要是通过判断orb_check出来的数据的时间戳，是否与我们上次读取的时间戳一致来完成的，源代码在libraries/AP_HAL_PX4/RCInput.cpp中，注意其中的_timer_tick函数是定时器(软件的)触发运行的。

有输入的话，就通过RC_Channel::set_pwm_all()读取并转换所有的通道，其定义如下：

void
RC_Channel::set_pwm_all(void)
{
    for (uint8_t i=0; i         if (rc_ch[i] != NULL) {
            rc_ch[i]->set_pwm(rc_ch[i]->read());
        }
    }
}

在这一函数中通过rc_ch[i]->read()读取该通道的PWM值，并调用set_pwm函数。

// read input from APM_RC - create a control_in value
void
RC_Channel::set_pwm(int16_t pwm)
{
    radio_in = pwm;


    if (_type == RC_CHANNEL_TYPE_RANGE) {
        control_in = pwm_to_range();
    } else {
        //RC_CHANNEL_TYPE_ANGLE, RC_CHANNEL_TYPE_ANGLE_RAW
        control_in = pwm_to_angle();
    }
}

这一函数将读到的PWM值记录到radio_in变量中，并根据此通道的type调用pwm_to_range或pwm_to_rangle，将值赋给control_in。type的赋值过程为，ArduCopter.cpp中的setup函数调用system.cpp中的init_ardupilot函数，此函数调用radio.cpp中的init_rc_in,在此函数中完成对主要通道的参数的修改。

油门通道的type为RC_CHANNEL_TYPE_RANGE，所以调用pwm_to_range，此函数直接调用pwm_to_range_dz，分析见下图。

用matlab画出其曲线，如下：

自此，便完成了遥控器数据的读取，及把油门通道的PWM值转换为0-1000的范围值，即0%到100%。

在control_stabilize中，通过get_pilot_desired_throttle对上述的油门做了进一步转换，主要目的是使油门在中间的时候，能够按照THR_MID定义的值输出，并同时对其他的油门scale。当把THR-MID设置为300，500时，可以得到下面曲线。