Apm飞控学习笔记之-RC_Channel遥控器数据获取-Cxm

前言

这一篇分析APM的遥控器数据获取

---

ArduCopter\Copter.cpp中rc_loop遥控器线程

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
    SCHED_TASK(rc_loop,              100,    130),
    SCHED_TASK(throttle_loop,         50,     75),
    SCHED_TASK(update_GPS,            50,    200),

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

read_radio();

这里开始跳转ArduCopter\Copter.h

    // radio.cpp
    void default_dead_zones();
    void init_rc_in();
    void init_rc_out();
    void enable_motor_output();
    void read_radio();
    void set_throttle_and_failsafe(uint16_t throttle_pwm);
    void set_throttle_zero_flag(int16_t throttle_control);
    void radio_passthrough_to_motors();
    int16_t get_throttle_mid(void);

这一步不能直接跳转了 看了一下注释找一下radio.cpp文件在ArduCopter\radio.cpp中

void Copter::read_radio()
{
    const uint32_t tnow_ms = millis();

    if (rc().read_input()) {
        ap.new_radio_frame = true;

        set_throttle_and_failsafe(channel_throttle->get_radio_in());
        set_throttle_zero_flag(channel_throttle->get_control_in());

        // RC receiver must be attached if we've just got input
        ap.rc_receiver_present = true;

        // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
        radio_passthrough_to_motors();

        const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
        rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt);
        last_radio_update_ms = tnow_ms;
        return;
    }

    // No radio input this time
    if (failsafe.radio) {
        // already in failsafe!
        return;
    }

    const uint32_t elapsed = tnow_ms - last_radio_update_ms;
    // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
    const uint32_t timeout = RC_Channels::has_active_overrides() ? FS_RADIO_RC_OVERRIDE_TIMEOUT_MS : FS_RADIO_TIMEOUT_MS;
    if (elapsed < timeout) {
        // not timed out yet
        return;
    }
    if (!g.failsafe_throttle) {
        // throttle failsafe not enabled
        return;
    }
    if (!ap.rc_receiver_present && !motors->armed()) {
        // we only failsafe if we are armed OR we have ever seen an RC receiver
        return;
    }

    // Nobody ever talks to us.  Log an error and enter failsafe.
    AP::logger().Write_Error(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME);
    set_failsafe_radio(true);
}

这里重点关注read_input()这个是通道信号输入的刷新

bool RC_Channels::read_input(void)
{
    if (!hal.rcin->new_input() && !has_new_overrides) {
        return false;
    }

    has_new_overrides = false;

    bool success = false;
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        success |= channel(i)->update();
    }

    return success;
}

直接关注hal.rcin->new_input()

bool RC_Channels::read_input(void)
{
    if (!hal.rcin->new_input() && !has_new_overrides) {
        return false;
    }

    has_new_overrides = false;

    bool success = false;
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        success |= channel(i)->update();
    }

    return success;
}

 channel(i)

    RC_Channel_Copter *channel(const uint8_t chan) override {
        if (chan >= NUM_RC_CHANNELS) {
            return nullptr;
        }
        return &obj_channels[chan];
    }

 channel(i)->update()信号的读取

radio_in保存读取到的PWM值

bool RC_Channel::update(void)
{
    if (has_override() && !rc().ignore_overrides()) {
        radio_in = override_value;
    } else if (!rc().ignore_receiver()) {
        radio_in = hal.rcin->read(ch_in);
    } else {
        return false;
    }

    if (type_in == RC_CHANNEL_TYPE_RANGE) {
        control_in = pwm_to_range();
    } else {
        //RC_CHANNEL_TYPE_ANGLE
        control_in = pwm_to_angle();
    }

    return true;
}

 

    float  rcin[8];         // RC input 0..1

 

read_mode_switch()

此函数的作用是判断飞行器当前的模式和飞行模式的设置

void RC_Channel::read_mode_switch()
{
    // calculate position of flight mode switch
    const uint16_t pulsewidth = get_radio_in();
    if (pulsewidth <= 900 || pulsewidth >= 2200) {
        return;  // This is an error condition
    }

    modeswitch_pos_t position;
    if      (pulsewidth < 1231) position = 0;
    else if (pulsewidth < 1361) position = 1;
    else if (pulsewidth < 1491) position = 2;
    else if (pulsewidth < 1621) position = 3;
    else if (pulsewidth < 1750) position = 4;
    else position = 5;

    if (!debounce_completed(position)) {
        return;
    }

    // set flight mode and simple mode setting
    //设置飞行模式和简单模式设置
    mode_switch_changed(position);
}

这里的重点是void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos) 函数

ArduCopter\RC_Channel.cpp

void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos)
{
    if (new_pos < 0 || (uint8_t)new_pos > copter.num_flight_modes) {
        // should not have been called
        return;
    }

    if (!copter.set_mode((Mode::Number)copter.flight_modes[new_pos].get(), ModeReason::RC_COMMAND)) {
        // alert user to mode change failure
        if (copter.ap.initialised) {
            AP_Notify::events.user_mode_change_failed = 1;
        }
        return;
    }

    // play a tone
    // alert user to mode change (except if autopilot is just starting up)
    if (copter.ap.initialised) {
        AP_Notify::events.user_mode_change = 1;
    }

    if (!rc().find_channel_for_option(AUX_FUNC::SIMPLE_MODE) &&
        !rc().find_channel_for_option(AUX_FUNC::SUPERSIMPLE_MODE)) {
        // if none of the Aux Switches are set to Simple or Super Simple Mode then
        // set Simple Mode using stored parameters from EEPROM
        if (BIT_IS_SET(copter.g.super_simple, new_pos)) {
            copter.set_simple_mode(2);
        } else {
            copter.set_simple_mode(BIT_IS_SET(copter.g.simple_modes, new_pos));
        }
    }
}

 

判断一下是否有模式输入   

if (new_pos < 0 || (uint8_t)new_pos > copter.num_flight_modes) {

        // should not have been called

        //    const uint8_t num_flight_modes = 6;

        return;

    }

更改模式和判断模式是否更改成功

    if (!copter.set_mode((Mode::Number)copter.flight_modes[new_pos].get(), ModeReason::RC_COMMAND)) {

        // alert user to mode change failure

        if (copter.ap.initialised) {

            AP_Notify::events.user_mode_change_failed = 1;

        }

        return;

    }

其中set_mode有两个重载函数

 第一个传参很好理解就是设置成当前的模式而第二个传参modereason 则是接口相当于你模式的来源,我是这么理解的 

// interface to set the vehicles mode
enum class ModeReason : uint8_t {
  UNKNOWN,
  RC_COMMAND,
  GCS_COMMAND,
  RADIO_FAILSAFE,
  BATTERY_FAILSAFE,
  GCS_FAILSAFE,
  EKF_FAILSAFE,
  GPS_GLITCH,
  MISSION_END,
  THROTTLE_LAND_ESCAPE,
  FENCE_BREACHED,
  TERRAIN_FAILSAFE,
  BRAKE_TIMEOUT,
  FLIP_COMPLETE,
  AVOIDANCE,
  AVOIDANCE_RECOVERY,
  THROW_COMPLETE,
  TERMINATE,
  TOY_MODE,
  CRASH_FAILSAFE,
  SOARING_FBW_B_WITH_MOTOR_RUNNING,
  SOARING_THERMAL_DETECTED,
  SOARING_THERMAL_ESTIMATE_DETERIORATED,
  VTOL_FAILED_TRANSITION,
  VTOL_FAILED_TAKEOFF,
  FAILSAFE, // general failsafes, prefer specific failsafes over this as much as possible
  INITIALISED,
  SURFACE_COMPLETE,
  BAD_DEPTH,
  LEAK_FAILSAFE,
  SERVOTEST,
  STARTUP,
  SCRIPTING,
  UNAVAILABLE,
  AUTOROTATION_START,
  AUTOROTATION_BAILOUT,
};

后续更新