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

Posted 2023-02-05 CHENxiaomingming

前言

这一篇分析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,
;

后续更新