Ardupilot遥控器输入数据分析

目录

  • 目录
  • 摘要
  • 1.遥控器输入初始化
  • 2.遥控器数据更新
  • 3.遥控器代码调用逻辑

摘要


本节主要记录自己学习arducopter代码中遥控器输入命令处理代码,无人机遥控器的输入决定着无人机怎么飞行,是非常重要的。特别是作为目标输入控制量,理解这个值,对于我们对飞控固件中的控制有很大的帮助。这里选择以futaba为例,来说明,我用的遥控器是14通道的,以美国手为例来说明。
Ardupilot遥控器输入数据分析_第1张图片

需要注意的是遥控器的1,2,3,4通道不是控制单个电机,这点一定要分清楚,特别看ardupilot代码中一个伺服输出接口怎么实现很多功能,怎么映射遥控器到伺服输出(遥控器的单个通道怎么控制映射到伺服输出)?
ardupilot多旋翼代码的电机输出只有八个通道,最多只能实现8旋翼,还有六个外部通道输出,可以用来植保,等辅助功能键。



1.遥控器输入初始化



(1)void Copter::setup()

void Copter::setup() 
{
    cliSerial = hal.console;

    //加载初始化参数表到 in var_info[]s
    AP_Param::setup_sketch_defaults();

    // 初始化无人机的结构布局
    StorageManager::set_layout_copter();

    //注册传感器
    init_ardupilot();

    // 初始化 main loop 任务
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));

    // 初始化 main loop 任务
    perf_info_reset();
    fast_loopTimer = AP_HAL::micros();
}

(2)init_ardupilot();

void Copter::init_ardupilot()
{
   init_rc_in(); //遥控器初始化
   ......
   reset_control_switch();//遥控器上面的5通道,复位控制通道
   init_aux_switches();   //遥控器上的初始化外部开关,具有一定的功能
}

1》 init_rc_in();

void Copter::init_rc_in()
{
    channel_roll     = RC_Channels::rc_channel(rcmap.roll()-1);     //横滚通道
    channel_pitch    = RC_Channels::rc_channel(rcmap.pitch()-1);    //俯仰通道
    channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1); //油门通道
    channel_yaw      = RC_Channels::rc_channel(rcmap.yaw()-1);     //偏航通道

    // 设置四个通道的最大范围
    channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX); //最大4500
    channel_pitch->set_angle(ROLL_PITCH_YAW_INPUT_MAX);//最大4500
    channel_yaw->set_angle(ROLL_PITCH_YAW_INPUT_MAX);  //最大4500
    channel_throttle->set_range(1000);                 //1000

    //设置辅助伺服范围-----------------set auxiliary servo ranges
    RC_Channels::rc_channel(CH_5)->set_range(1000);
    RC_Channels::rc_channel(CH_6)->set_range(1000);
    RC_Channels::rc_channel(CH_7)->set_range(1000);
    RC_Channels::rc_channel(CH_8)->set_range(1000);

    //设置默认死区-------------------set default dead zones
    default_dead_zones();

    //初始化遥控通道死区范围-----------initialise throttle_zero flag
    ap.throttle_zero = true;
}

Ardupilot遥控器输入数据分析_第2张图片


2》reset_control_switch()


void Copter::reset_control_switch()
{
    control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1;//设置当前模式
    read_control_switch();//读取控制开关
}
   //用于检测飞行模式控制开关的变化的结构体
    struct 
    {
        int8_t debounced_switch_position;   //当前使用的开关位置-----------currently used switch position
        int8_t last_switch_position;        //上一次迭代中的开关位置--------switch position in previous iteration
        uint32_t last_edge_time_ms;         //切换位置上次更改的系统时间----system time that switch position was last changed
    } control_switch_state;
//5通道六段开关
void Copter::read_control_switch()
{
    uint32_t tnow_ms = millis();

    //计算飞行模式开关位置--------------------calculate position of flight mode switch
    int8_t switch_position;                //获取5通道遥控器的位置设定飞行模式
    uint16_t rc5_in = RC_Channels::rc_channel(CH_5)->get_radio_in();  //获取5通道数据

    if      (rc5_in < 1231) switch_position = 0; //可以设置姿态
    else if (rc5_in < 1361) switch_position = 1;
    else if (rc5_in < 1491) switch_position = 2;
    else if (rc5_in < 1621) switch_position = 3; //可以设置定高
    else if (rc5_in < 1750) switch_position = 4;
    else switch_position = 5; //可以设置定点

    //存储上次切换开关的时间----store time that switch last moved
    if (control_switch_state.last_switch_position != switch_position)
    {
        control_switch_state.last_edge_time_ms = tnow_ms;
    }

    //开关去抖动------debounce switch
    bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position;
    bool sufficient_time_elapsed = tnow_ms - control_switch_state.last_edge_time_ms > CONTROL_SWITCH_DEBOUNCE_TIME_MS;
    bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0;
 //三个条件同时满足,才进行,开关确实改变了,切换时间足够长,遥控器有数据,
    if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged)
    {
        //设定飞行模式和简单模式设置-------set flight mode and simple mode setting
        if (set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND))
        {
            // play a tone
            if (control_switch_state.debounced_switch_position != -1)
            {
                // alert user to mode change failure (except if autopilot is just starting up)
                if (ap.initialised) {
                    AP_Notify::events.user_mode_change = 1;
                }
            }

            if (!check_if_auxsw_mode_used(AUXSW_SIMPLE_MODE) && !check_if_auxsw_mode_used(AUXSW_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(g.super_simple, switch_position))
                {
                    set_simple_mode(2);
                } else
                {
                    set_simple_mode(BIT_IS_SET(g.simple_modes, switch_position));
                }
            }

        } else if (control_switch_state.last_switch_position != -1) {
            // alert user to mode change failure
            AP_Notify::events.user_mode_change_failed = 1;
        }

        // set the debounced switch position
        control_switch_state.debounced_switch_position = switch_position;
    }
  //否则,记录的还是当前模式,也就是没有改变飞行模式。
    control_switch_state.last_switch_position = switch_position;
}
bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
{
    // boolean to record if flight mode could be set
    bool success = false;
    bool ignore_checks = !motors->armed();   //如果没有解锁,允许任何模式ignore_checks=1,_flags.armed=0表示没有解锁,_flags.armed=1表示解锁

    // return immediately if we are already in the desired mode
    if (mode == control_mode)
    {
        prev_control_mode = control_mode;
        prev_control_mode_reason = control_mode_reason;

        control_mode_reason = reason;
        return true;
    }


#if FRAME_CONFIG == HELI_FRAME
    // do not allow helis to enter a non-manual throttle mode if the
    // rotor runup is not complete
    if (!ignore_checks && !mode_has_manual_throttle(mode) && !motors->rotor_runup_complete()){
        goto failed;
    }
#endif

    switch (mode)
    {
        case ACRO:
            #if FRAME_CONFIG == HELI_FRAME
                success = heli_acro_init(ignore_checks);
            #else
                success = acro_init(ignore_checks);
            #endif
            break;

        case STABILIZE:
            #if FRAME_CONFIG == HELI_FRAME
                success = heli_stabilize_init(ignore_checks);
            #else
                success = stabilize_init(ignore_checks);  //姿态自我稳定模式
            #endif
            break;

        case ALT_HOLD:
            success = althold_init(ignore_checks);        //高度控制初始化
            break;

        case AUTO:
            success = auto_init(ignore_checks);           //自动模式
            break;

        case CIRCLE:
            success = circle_init(ignore_checks);
            break;

        case LOITER:
            success = loiter_init(ignore_checks);         //定点悬停初始化
            break;

        case GUIDED:
            success = guided_init(ignore_checks);
            break;

        case LAND:
            success = land_init(ignore_checks);
            break;

        case RTL:
            success = rtl_init(ignore_checks);
            break;

        case DRIFT:
            success = drift_init(ignore_checks);
            break;

        case SPORT:
            success = sport_init(ignore_checks);
            break;

        case FLIP:
            success = flip_init(ignore_checks);
            break;

#if AUTOTUNE_ENABLED == ENABLED
        case AUTOTUNE:
            success = autotune_init(ignore_checks);
            break;
#endif

#if POSHOLD_ENABLED == ENABLED
        case POSHOLD:
            success = poshold_init(ignore_checks);     //定点控制
            break;
#endif

        case BRAKE:
            success = brake_init(ignore_checks);       //刹车模式初始化
            break;

        case THROW:
            success = throw_init(ignore_checks);
            break;

        case AVOID_ADSB:
            success = avoid_adsb_init(ignore_checks);  //避障模块
            break;

        case GUIDED_NOGPS:
            success = guided_nogps_init(ignore_checks);
            break;

        // add by weihli
        case ZIGZAG:
            success = zigzag_init(ignore_checks); //植保之字形
            break;

        default:
            success = false;
            break;
    }

#if FRAME_CONFIG == HELI_FRAME
failed:
#endif

    // update flight mode
    if (success) {
        // perform any cleanup required by previous flight mode
        exit_mode(control_mode, mode);

        prev_control_mode = control_mode;
        prev_control_mode_reason = control_mode_reason;

        control_mode = mode;
        control_mode_reason = reason;
        DataFlash.Log_Write_Mode(control_mode, control_mode_reason);

        adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED));

#if AC_FENCE == ENABLED
        // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
        // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
        // but it should be harmless to disable the fence temporarily in these situations as well
        fence.manual_recovery_start();
#endif

#if FRSKY_TELEM_ENABLED == ENABLED
        frsky_telemetry.update_control_mode(control_mode);
#endif

    } else {
        // Log error that we failed to enter desired flight mode
        Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
        gcs_send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
    }

    // update notify object
    if (success) {
        notify_flight_mode(control_mode);
    }

    // return success or failure
    return success;
}

3》void Copter::init_aux_switches()


void Copter::init_aux_switches()
{
    //设定CH7,CH8,CH10,CH11通道标志-----------------set the CH7 ~ CH12 flags
    aux_con.CH7_flag = read_3pos_switch(CH_7);
    aux_con.CH8_flag = read_3pos_switch(CH_8);
    aux_con.CH10_flag = read_3pos_switch(CH_10);
    aux_con.CH11_flag = read_3pos_switch(CH_11);

    //CH9,CH12仅在某些板上支撑----------------------ch9, ch12 only supported on some boards
    aux_con.CH9_flag = read_3pos_switch(CH_9);
    aux_con.CH12_flag = read_3pos_switch(CH_12);

    //初始化函数给交到开关-------------------------initialise functions assigned to switches
    init_aux_switch_function(g.ch7_option, aux_con.CH7_flag);
    init_aux_switch_function(g.ch8_option, aux_con.CH8_flag);
    init_aux_switch_function(g.ch10_option, aux_con.CH10_flag);
    init_aux_switch_function(g.ch11_option, aux_con.CH11_flag);

    //CH9,CH12仅在某些板上支撑-------------------ch9, ch12 only supported on some boards
    init_aux_switch_function(g.ch9_option, aux_con.CH9_flag);
    init_aux_switch_function(g.ch12_option, aux_con.CH12_flag);
}

首先看下init_aux_switch_function()函数,

void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
{    
    //初始化通道选项-----------init channel options
    switch(ch_option) 
    {
        case AUXSW_SIMPLE_MODE:
        case AUXSW_RANGEFINDER:
        case AUXSW_FENCE:
        case AUXSW_SUPERSIMPLE_MODE:
        case AUXSW_ACRO_TRAINER:
        case AUXSW_GRIPPER:
        case AUXSW_SPRAYER:
        case AUXSW_PARACHUTE_ENABLE:
                                         //我们相信车辆将被解除武装,所以即使开关在释放位置,溜槽不会释放
        case AUXSW_PARACHUTE_3POS:      // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release
        case AUXSW_RETRACT_MOUNT:
        case AUXSW_MISSION_RESET:
        case AUXSW_ATTCON_FEEDFWD:
        case AUXSW_ATTCON_ACCEL_LIM:
        case AUXSW_MOTOR_ESTOP:
        case AUXSW_MOTOR_INTERLOCK:
        case AUXSW_AVOID_ADSB:
        case AUXSW_PRECISION_LOITER:
        case AUXSW_AVOID_PROXIMITY:
            do_aux_switch_function(ch_option, ch_flag);
            break;
    }
}

其次分析:do_aux_switch_function(ch_option, ch_flag);//这里才是关键


(1)分析static RC_Channel *rc_channel(uint8_t chan)


  static RC_Channel *rc_channel(uint8_t chan)
{
        return (chan < NUM_RC_CHANNELS)?&channels[chan]:nullptr;
}
//这个函数chan < NUM_RC_CHANNELS恒成立,则得到&channels[chan],
//由于RC_Channel *RC_Channels::channels;则是RC_Channels类中的RC_Channel的对象成员

(2)分析rcmap.roll(),rcmap.pitch(),rcmap.throttle(),rcmap.yaw()获取通道号


Ardupilot遥控器输入数据分析_第3张图片
最后得到:rcmap.roll()-1=0;rcmap.pitch()-1=1;rcmap.throttle()-1=2;rcmap.yaw()-1=3

channel_roll     = RC_Channels::rc_channel(0);     //横滚通道
channel_pitch    = RC_Channels::rc_channel(1);    //俯仰通道
channel_throttle = RC_Channels::rc_channel(2);   //油门通道
channel_yaw      = RC_Channels::rc_channel(3);  //偏航通道

这里还是要找到channels[0],channels[1],channels[2],channels[3]

//找到这就找到了初始化的值
 const RC_Channel *channels[] = 
{
        copter.channel_roll, 
        copter.channel_pitch,
        copter.channel_throttle,
        copter.channel_yaw
};

Ardupilot遥控器输入数据分析_第4张图片

到这里我们只需要研究

copter.channel_roll
copter.channel_pitch
copter.channel_throttle
copter.channel_yaw


(3)分析set_angle()


    channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX); //最大4500
    channel_pitch->set_angle(ROLL_PITCH_YAW_INPUT_MAX);//最大4500
    channel_yaw->set_angle(ROLL_PITCH_YAW_INPUT_MAX);  //最大4500
    channel_throttle->set_range(1000);                 //1000

//ROLL_PITCH_YAW_INPUT_MAX=4500

void RC_Channel::set_angle(uint16_t angle) //
{
    type_in = RC_CHANNEL_TYPE_ANGLE;//#define RC_CHANNEL_TYPE_ANGLE  0,这里后面会用到选择范围或者角度
    high_in = angle;
}
    RC_Channels::rc_channel(CH_5)->set_range(1000); //CH_5=4
    RC_Channels::rc_channel(CH_6)->set_range(1000);  //CH_6=5
    RC_Channels::rc_channel(CH_7)->set_range(1000);  //CH_7=6
    RC_Channels::rc_channel(CH_8)->set_range(1000);  //CH_8=7

可以看出:
横滚通道:[-4500,4500] ;
俯仰通道:[-4500,4500] ;
偏航通道:[-4500,4500] ;
油门通道:[0,1000] ;
其他外部通道值:0-1000


(4)分析void Copter::default_dead_zones()

void Copter::default_dead_zones()
{
    channel_roll->set_default_dead_zone(20); //死区范围+-20
    channel_pitch->set_default_dead_zone(20);//死区范围+-20
#if FRAME_CONFIG == HELI_FRAME
    channel_throttle->set_default_dead_zone(10);
    channel_yaw->set_default_dead_zone(15);
    RC_Channels::rc_channel(CH_6)->set_default_dead_zone(10);
#else
    channel_throttle->set_default_dead_zone(30);//死区范围+-30
    channel_yaw->set_default_dead_zone(20);//死区范围+-20
#endif
    RC_Channels::rc_channel(CH_6)->set_default_dead_zone(0); //死区范围0
}

最后:ap.throttle_zero = true;//设置油门死区范围使能,到这里遥控器初始化基本讲解完成,细节问题,以后继续深挖。特别讲解遥控器底层驱动时,怎么转换到这里。



2.遥控器数据更新



SCHED_TASK(rc_loop, 100,  130), //遥控器数据读取函数,10ms,判断飞行模式是否发生改变

(1)void Copter::rc_loop()


void Copter::rc_loop()
{
    //读取遥控器的通道数据和三段开关的位置信息-------------- Read radio and 3-position switch on radio
    read_radio();
    read_control_switch(); //读取三段开关数据
}

(2)read_radio();//读取遥控器数据


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


void Copter::read_radio()
{
    uint32_t tnow_ms = millis(); //获取系统时间

    if (hal.rcin->new_input())//判断有没有遥控器新的输入数据到来,有返回1,执行if没有返回0,执行else
    {
        ap.new_radio_frame = true;
        RC_Channels::set_pwm_all(); //设置所有遥控器输入值

        set_throttle_and_failsafe(channel_throttle->get_radio_in()); //油门通道检查
        set_throttle_zero_flag(channel_throttle->get_control_in());  //获取控制通道

        //我们必须安装一个RC接收器标志---------flag we must have an rc receiver attached
        if (!failsafe.rc_override_active)
        {
            ap.rc_receiver_present = true;
        }

        // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
        //将控制着的输入传递到电机(用于允许在HELI、单、同轴直升机上解除武装时的摆动伺服)
        radio_passthrough_to_motors();

        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;//更新下一次使用
    }else//没有新的数据到来,执行故障保护
    {
        uint32_t elapsed = tnow_ms - last_radio_update_ms;
        //打开故障安全处理--如果遥控器在500ms-2000ms没有数据到来----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);//设置故障
        }
    }
}

1>>bool PX4RCInput::new_input()


bool PX4RCInput::new_input()
{
    pthread_mutex_lock(&rcin_mutex); //信号量,加锁,nuttx没研究后期在分析;在编程中,引入了对象互斥锁的概念,来保证共享数据操作的完整性。每个对象都对应于一个可称为" 互斥锁" 的标记,这个标记用来保证在任一时刻,只能有一个线程访问该对象。
    bool valid = _rcin.timestamp_last_signal != _last_read;
    if (_rcin.rc_failsafe)
    {
        // don't consider input valid if we are in RC failsafe.
        valid = false;
    }
    if (_override_valid)
    {
        // if we have RC overrides active, then always consider it valid
        valid = true;
    }
    _last_read = _rcin.timestamp_last_signal;
    _override_valid = false;
    pthread_mutex_unlock(&rcin_mutex); //信号量不上锁,互斥信号量
    return valid;
}

2>>void RC_Channels::set_pwm_all(void)


void RC_Channels::set_pwm_all(void)
{
    for (uint8_t i=0; i//NUM_RC_CHANNELS=16
    {
        channels[i].set_pwm(channels[i].read()); //关键分析点:channels[i].read()将会传入:Roll,,pitch,throttle,yaw,aux,初始化可以看出
    }
}
///////////////////分析上面代码/////////////////////////////
channels[i].read()//channels[i]这个初始化已经说得很详细了,应该看明白了

uint16_t RC_Channel::read() const
{
    return hal.rcin->read(ch_in);//这里就是获取遥控器数据的函数,往下先不分析,后期看底层驱动讲解,记住这里就是从底层驱动获取遥控器数据的地方,很重要,
}
///////////////////分析上面代码/////////////////////////////

// type_in = RC_CHANNEL_TYPE_ANGLE;//#define RC_CHANNEL_TYPE_ANGLE  0,这里后面会用到选择范围或者角度
void RC_Channel::set_pwm(int16_t pwm)
{
    radio_in = pwm;

    if (type_in == RC_CHANNEL_TYPE_RANGE)
    {
        control_in = pwm_to_range();//PWM到范围
    } else
    {
        //RC_CHANNEL_TYPE_ANGLE
        control_in = pwm_to_angle(); //pwm到角度,选择使用这一种方式,但是我们还是分析一下if语句
    }
}

我们重点看下pwm_to_range()和pwm_to_angle()函数


《1》pwm_to_range()


//将脉宽调制值转换为配置范围内的值

int16_t RC_Channel::pwm_to_range()//将脉宽调制值转换为配置范围内的值

{
    return pwm_to_range_dz(dead_zone);//分析这个函数
}
//调用pwm_to_range_dz(dead_zone)函数
//将脉宽调制值转换为配置中的值范围,使用指定的死区
int16_t RC_Channel::pwm_to_range_dz(uint16_t _dead_zone)
{
    int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get());//遥控器数入限制到一定的范围i值内

    if (reversed) //通道值是否翻转
    {
        r_in = radio_max.get() - (r_in - radio_min.get()); //
    }

    int16_t radio_trim_low  = radio_min + _dead_zone; //获取最小值

    if (r_in > radio_trim_low) //判断遥控器值是否大于最小设定值
    {
        return (((int32_t)(high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low));//比如:1000*(x-20)/(1000-20)
    }
    return 0;
}

《2》pwm_to_angle()

从当前遥控器输入值返回一个角度:“单位是角度的100倍,也就是cm角度”(通常为-4500到4500)

int16_t RC_Channel::pwm_to_angle()
{
    return pwm_to_angle_dz(dead_zone);
}
//从当前遥控器输入值返回一个角度:“单位是角度的100倍,也就是cm角度”(通常为-4500到4500),加入了死区限制
int16_t RC_Channel::pwm_to_angle_dz(uint16_t _dead_zone)
{
    return pwm_to_angle_dz_trim(_dead_zone, radio_trim);//这个radio_trim是多少呢,在参数中有说明。也可以看下图。或者地面站数据
}

Ardupilot遥控器输入数据分析_第5张图片
Ardupilot遥控器输入数据分析_第6张图片

int16_t RC_Channel::pwm_to_angle_dz_trim(uint16_t _dead_zone, uint16_t _trim)
{
    int16_t radio_trim_high = _trim + _dead_zone; //计算遥控器的最大值radio_trim, 1500
    int16_t radio_trim_low  = _trim - _dead_zone; //计算最小值radio_trim, 1500
    // prevent div by 0
    if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0)
        return 0;

    int16_t reverse_mul = (reversed?-1:1);
    if (radio_in > radio_trim_high) 
    {
        return reverse_mul * ((int32_t)high_in * (int32_t)(radio_in - radio_trim_high)) / (int32_t)(radio_max  - radio_trim_high);
    } else if (radio_in < radio_trim_low) 
    {
        return reverse_mul * ((int32_t)high_in * (int32_t)(radio_in - radio_trim_low)) / (int32_t)(radio_trim_low - radio_min);
    } else  
    {
        return 0;
    }
}

上面代码什么意思呢?
我们细细分析:如下图,以第一通道为例计算:

high_in =4500,这个初始化部分有,可以看这个函数:

channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
计算这个:reverse_mul * ((int32_t)high_in * (int32_t)(radio_in - radio_trim_high)) / (int32_t)(radio_max - radio_trim_high)
reverse_mul ((int32_t)4500(int32_t)(x-1506)/((int32_t)(1927-1506)))
这样是不是就是【-4500,+4500】,显然是的,横滚俯仰,偏航的都是这个范围,但是油门的不是,可以自己计算下,油门值范围:【0-1000】
Ardupilot遥控器输入数据分析_第7张图片


到这里RC_Channels::set_pwm_all();这个函数的作用就很清晰了:处理遥控器的输入数据,到规定的控制范围数据内,最多支持16通道数据。


3>>set_throttle_and_failsafe(channel_throttle->get_radio_in())设置故障保护


void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm)
{
    //如果故障安全未启用,则通过油门和退出这个函数------ if failsafe not enabled pass through throttle and exit
    if(g.failsafe_throttle == FS_THR_DISABLED) //通过参数设置
    {
        channel_throttle->set_pwm(throttle_pwm);
        return;
    }

    //检查低油门值----------check for low throttle value
    if (throttle_pwm < (uint16_t)g.failsafe_throttle_value) //g.failsafe_throttle_value通过地面站设置,FS_THR_VALUE=975
    {

        //如果我们已经在遥控器故障或电机没有解锁,油门直接通过和退出---- if we are already in failsafe or motors not armed pass through throttle and exit
        if (failsafe.radio || !(ap.rc_receiver_present || motors->armed())) 
        {
            channel_throttle->set_pwm(throttle_pwm);
            return;
        }

        //检查3个低油门值----- check for 3 low throttle values
        //注意:我们不通过低油门,直到接收到3个低油门值。--- Note: we do not pass through the low throttle until 3 low throttle values are received
        failsafe.radio_counter++;
        if( failsafe.radio_counter >= FS_COUNTER ) 
        {
            failsafe.radio_counter = FS_COUNTER;      //检查以确保我们不会溢出计数器---- check to ensure we don't overflow the counter
            set_failsafe_radio(true);
            channel_throttle->set_pwm(throttle_pwm); //通过故障安全---- pass through failsafe throttle
        }
    }else
    {
        //我们有一个好油门值,所以减少故障安全计数器----we have a good throttle so reduce failsafe counter
        failsafe.radio_counter--;
        if( failsafe.radio_counter <= 0 ) 
        {
            failsafe.radio_counter = 0;   //检查以确保我们不会溢出计数器---- check to ensure we don't underflow the counter

            //再三获取(几乎)连续有效油门值后,断开故障保险--- disengage failsafe after three (nearly) consecutive valid throttle values
            if (failsafe.radio) 
            {
                set_failsafe_radio(false);
            }
        }
        //通过油门值------pass through throttle
        channel_throttle->set_pwm(throttle_pwm);
    }
}

4>>set_throttle_zero_flag(channel_throttle->get_control_in());设置油门0标志


void Copter::set_throttle_zero_flag(int16_t throttle_control)
{
    static uint32_t last_nonzero_throttle_ms = 0;
    uint32_t tnow_ms = millis();

    // if not using throttle interlock and non-zero throttle and not E-stopped,
    // or using motor interlock and it's enabled, then motors are running, 
    // and we are flying. Immediately set as non-zero
    if ((!ap.using_interlock && (throttle_control > 0) && !ap.motor_emergency_stop) || (ap.using_interlock && motors->get_interlock())) 
    {
        last_nonzero_throttle_ms = tnow_ms;
        ap.throttle_zero = false;
    } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) 
    {
        ap.throttle_zero = true;
    }
}

5>>radio_passthrough_to_motors();控制输入传递到电机库


void Copter::radio_passthrough_to_motors()
{
    motors->set_radio_passthrough(channel_roll->norm_input(), //范围【-1,+1】
                                  channel_pitch->norm_input(),//范围【-1,+1】
                                  channel_throttle->get_control_in_zero_dz()*0.001,//范围【0,+1】
                                  channel_yaw->norm_input());//范围【-1,+1】
}

我们先看下set_radio_passthrough()这个函数的实现过程。

void AP_Motors::set_radio_passthrough(float roll_input, float pitch_input, float throttle_input, float yaw_input)
{
    _roll_radio_passthrough = roll_input;//_roll_radio_passthrough 横滚受保护的值【-1,+1】
    _pitch_radio_passthrough = pitch_input;//_pitch_radio_passthrough俯仰受保护的值【-1,+1】 
    _throttle_radio_passthrough = throttle_input;//_throttle_radio_passthrough 油门受保护的值【0,+1】
    _yaw_radio_passthrough = yaw_input;//_yaw_radio_passthrough 偏航受保护的值【-1,+1】
}

上面函数就是传递数据到电机库,参与PID运算,进而控制无人机
继续分析float RC_Channel::norm_input()这个函数是处理输入到规定的范围值

float RC_Channel::norm_input()//这个函数不用解释,跟上面的计算【-4500,+4500】类似
{
    float ret;
    int16_t reverse_mul = (reversed?-1:1);
    if (radio_in < radio_trim) 
    {
        if (radio_min >= radio_trim) 
        {
            return 0.0f;
        }
        ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min);
    } else 
    {
        if (radio_max <= radio_trim) 
        {
            return 0.0f;
        }
        ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_max  - radio_trim);
    }
    return constrain_float(ret, -1.0f, 1.0f);
}

6>> rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt)
//不讲解这个滤波,可以看下网上的低通滤波推导过程

template T>
T LowPassFilter<T>::apply(T sample, float dt) 
{
    return _filter.apply(sample, _cutoff_freq, dt);
}

template
T DigitalLPF::apply(const T &sample)
{
_output += (sample - _output) * alpha;
return _output;
}




(3)void Copter::read_control_switch()//读取三段开关,切换飞行模式
上面的函数主要处理1,2,3,4,6,7,8通道数据,但是没有处理外部开关5通道数据,那是因为5通道数据,默认被apm固件作为控制切换无人机的飞行模式。
Ardupilot遥控器输入数据分析_第8张图片
Ardupilot遥控器输入数据分析_第9张图片

void Copter::read_control_switch()
{
    uint32_t tnow_ms = millis();//获取系统时间

    //计算飞行模式开关位置--------------------calculate position of flight mode switch
    int8_t switch_position;                //获取5通道遥控器的位置设定飞行模式
    uint16_t rc5_in = RC_Channels::rc_channel(CH_5)->get_radio_in();  //获取5通道数据
    //判断5通道PWM值得大小在哪个范围
    if      (rc5_in < 1231) switch_position = 0;
    else if (rc5_in < 1361) switch_position = 1;
    else if (rc5_in < 1491) switch_position = 2;
    else if (rc5_in < 1621) switch_position = 3;
    else if (rc5_in < 1750) switch_position = 4;
    else switch_position = 5;

    //存储上次切换的时间---- store time that switch last moved
    if (control_switch_state.last_switch_position != switch_position)
    {
        control_switch_state.last_edge_time_ms = tnow_ms;
    }

    //开关去抖动处理----debounce switch
    bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position;
    bool sufficient_time_elapsed = tnow_ms - control_switch_state.last_edge_time_ms > CONTROL_SWITCH_DEBOUNCE_TIME_MS;
    bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0;

    if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged)
    {
        //设置飞行模式和简单模式设置---- set flight mode and simple mode setting
        if (set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND))//核心函数在这里
        {
            // play a tone
            if (control_switch_state.debounced_switch_position != -1)
            {
                // alert user to mode change failure (except if autopilot is just starting up)
                if (ap.initialised) 
                {
                    AP_Notify::events.user_mode_change = 1;
                }
            }

            if (!check_if_auxsw_mode_used(AUXSW_SIMPLE_MODE) && !check_if_auxsw_mode_used(AUXSW_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
                //遥控器模式设置:如果没有AUX开关设置为简单或超级简单模式,那么使用EEPROM存储参数设置简单模式
                if (BIT_IS_SET(g.super_simple, switch_position))
                {
                    set_simple_mode(2);
                } else
                {
                    set_simple_mode(BIT_IS_SET(g.simple_modes, switch_position));
                }
            }

        } else if (control_switch_state.last_switch_position != -1) 
        {
            //警告用户更改模式失败--- alert user to mode change failure
            AP_Notify::events.user_mode_change_failed = 1;
        }

        //设置去抖动开关位置---set the debounced switch position
        control_switch_state.debounced_switch_position = switch_position;
    }

    control_switch_state.last_switch_position = switch_position;
}


分析重点函数:

set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND)

bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)有两个参数传入
(1)control_mode_t mode, //控制模式
(2)mode_reason_t reason//遥控器类型

enum control_mode_t 
{
    STABILIZE =     0,  //手动模式--- manual airframe angle with manual throttle
    ACRO =          1,  //速率模式--- manual body-frame angular rate with manual throttle
    ALT_HOLD =      2,  // manual airframe angle with automatic throttle
    AUTO =          3,  // fully automatic waypoint control using mission commands
    GUIDED =        4,  // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
    LOITER =        5,  // automatic horizontal acceleration with automatic throttle
    RTL =           6,  // automatic return to launching point
    CIRCLE =        7,  // automatic circular flight with automatic throttle
    LAND =          9,  // automatic landing with horizontal position control
    DRIFT =        11,  // semi-automous position, yaw and throttle control
    SPORT =        13,  // manual earth-frame angular rate control with manual throttle
    FLIP =         14,  // automatically flip the vehicle on the roll axis
    AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
    POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
    BRAKE =        17,  // full-brake using inertial/GPS system, no pilot input
    THROW =        18,  // throw to launch mode using inertial/GPS system, no pilot input
    AVOID_ADSB =   19,  // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
    GUIDED_NOGPS = 20,  // guided mode but only accepts attitude and altitude
    POSITION =  21,  // poshold, but sticks control speed in corresponding directions
};

//模式类型
enum mode_reason_t 
{
    MODE_REASON_UNKNOWN=0,
    MODE_REASON_TX_COMMAND,
    MODE_REASON_GCS_COMMAND,
    MODE_REASON_RADIO_FAILSAFE,
    MODE_REASON_BATTERY_FAILSAFE,
    MODE_REASON_GCS_FAILSAFE,
    MODE_REASON_EKF_FAILSAFE,
    MODE_REASON_GPS_GLITCH,
    MODE_REASON_MISSION_END,
    MODE_REASON_THROTTLE_LAND_ESCAPE,
    MODE_REASON_FENCE_BREACH,
    MODE_REASON_TERRAIN_FAILSAFE,
    MODE_REASON_BRAKE_TIMEOUT,
    MODE_REASON_FLIP_COMPLETE,
    MODE_REASON_AVOIDANCE,
    MODE_REASON_AVOIDANCE_RECOVERY,
    MODE_REASON_THROW_COMPLETE,
};
bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
{
    //如果飞行模式可以设置,则记录布尔值。---- boolean to record if flight mode could be set
    bool success = false;
    //如果没有解锁,允许开关在任何模式,我们依赖解锁检查执行
    bool ignore_checks = !motors->armed();   // allow switching to any mode if disarmed.  We rely on the arming check to perform

    //如果我们已经处于期望的模式,立即返回---- return immediately if we are already in the desired mode
    if (mode == control_mode)//设置好模式,就不用往下运行,没有设置好,继续设置
    {
        prev_control_mode = control_mode;
        prev_control_mode_reason = control_mode_reason;

        control_mode_reason = reason;
        return true;
    }


#if FRAME_CONFIG == HELI_FRAME
    // do not allow helis to enter a non-manual throttle mode if the
    // rotor runup is not complete
    if (!ignore_checks && !mode_has_manual_throttle(mode) && !motors->rotor_runup_complete())
    {
        goto failed;
    }
#endif

    switch (mode) 
    {
        case ACRO: //速率模式
            #if FRAME_CONFIG == HELI_FRAME
                success = heli_acro_init(ignore_checks);
            #else
                success = acro_init(ignore_checks);
            #endif
            break;

        case STABILIZE://自稳模式
            #if FRAME_CONFIG == HELI_FRAME
                success = heli_stabilize_init(ignore_checks);
            #else
                success = stabilize_init(ignore_checks);
            #endif
            break;

        case ALT_HOLD://定高模式
            success = althold_init(ignore_checks);
            break;

        case AUTO:  //自动模式
            success = auto_init(ignore_checks);
            break;

        case CIRCLE://绕圈模式
            success = circle_init(ignore_checks);
            break;

        case LOITER://留待模式
            success = loiter_init(ignore_checks);
            break;

        case GUIDED://指点模式
            success = guided_init(ignore_checks);
            break;

        case LAND://降落模式
            success = land_init(ignore_checks);
            break;

        case RTL: //返航模式
            success = rtl_init(ignore_checks);
            break;

        case DRIFT://漂移模式
            success = drift_init(ignore_checks);
            break;

        case SPORT://运动模式
            success = sport_init(ignore_checks);
            break;

        case FLIP://斜坡模式
            success = flip_init(ignore_checks);
            break;

#if AUTOTUNE_ENABLED == ENABLED
        case AUTOTUNE://自动调参模式
            success = autotune_init(ignore_checks);
            break;
#endif

#if POSHOLD_ENABLED == ENABLED
        case POSHOLD://位置控制模式
            success = poshold_init(ignore_checks);
            break;
#endif

        case BRAKE://刹车模式
            success = brake_init(ignore_checks);
            break;

        case THROW://抛投模式
            success = throw_init(ignore_checks);
            break;

        case AVOID_ADSB://自动广播模式
            success = avoid_adsb_init(ignore_checks);
            break;

        case GUIDED_NOGPS://指点模式没有GPS
            success = guided_nogps_init(ignore_checks);
            break;


        case POSITION://位置
            success = position_init(ignore_checks);
            break;

        default:
            success = false;
            break;
    }

#if FRAME_CONFIG == HELI_FRAME
failed:
#endif

    // update flight mode
    if (success) //更新飞行模式,如果设置成功
    {
        //执行先前飞行模式需要清理--- perform any cleanup required by previous flight mode
        exit_mode(control_mode, mode);

        prev_control_mode = control_mode;
        prev_control_mode_reason = control_mode_reason;

        control_mode = mode;
        control_mode_reason = reason;
        DataFlash.Log_Write_Mode(control_mode, control_mode_reason);

        adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED));

#if AC_FENCE == ENABLED
        // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
        // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
        // but it should be harmless to disable the fence temporarily in these situations as well
        fence.manual_recovery_start();
#endif

#if FRSKY_TELEM_ENABLED == ENABLED
        frsky_telemetry.update_control_mode(control_mode);
#endif

    } else {
        // Log error that we failed to enter desired flight mode
        Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
        gcs_send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
    }

    //发送更新模式到OreoLED---update notify object
    if (success) 
    {
        notify_flight_mode(control_mode);
    }

    //返回成功或者失败------return success or failure
    return success;
}

通知OreoLED处理函数:notify_flight_mode(control_mode);(OreoLED这个不是RGBled不知道是什么?)


void Copter::notify_flight_mode(control_mode_t mode)
{
    AP_Notify::flags.flight_mode = mode;

    switch (mode) 
    {
        case AUTO:
        case GUIDED:
        case RTL:
        case CIRCLE:
        case AVOID_ADSB:
        case GUIDED_NOGPS:
        case LAND:
            // autopilot modes
            AP_Notify::flags.autopilot_mode = true;
            break;
        default:
            // all other are manual flight modes
            AP_Notify::flags.autopilot_mode = false;
            break;
    }

    // set flight mode string
    switch (mode) {
        case STABILIZE:
            notify.set_flight_mode_str("STAB");
            break;
        case ACRO:
            notify.set_flight_mode_str("ACRO");
            break;
        case ALT_HOLD:
            notify.set_flight_mode_str("ALTH");
            break;
        case AUTO:
            notify.set_flight_mode_str("AUTO");
            break;
        case GUIDED:
            notify.set_flight_mode_str("GUID");
            break;
        case LOITER:
            notify.set_flight_mode_str("LOIT");
            break;
        case RTL:
            notify.set_flight_mode_str("RTL ");
            break;
        case CIRCLE:
            notify.set_flight_mode_str("CIRC");
            break;
        case LAND:
            notify.set_flight_mode_str("LAND");
            break;
        case DRIFT:
            notify.set_flight_mode_str("DRIF");
            break;
        case SPORT:
            notify.set_flight_mode_str("SPRT");
            break;
        case FLIP:
            notify.set_flight_mode_str("FLIP");
            break;
        case AUTOTUNE:
            notify.set_flight_mode_str("ATUN");
            break;
        case POSHOLD:
            notify.set_flight_mode_str("PHLD");
            break;
        case BRAKE:
            notify.set_flight_mode_str("BRAK");
            break;
        case THROW:
            notify.set_flight_mode_str("THRW");
            break;
        case AVOID_ADSB:
            notify.set_flight_mode_str("AVOI");
            break;
        case GUIDED_NOGPS:
            notify.set_flight_mode_str("GNGP");
            break;
        case POSITION:
            notify.set_flight_mode_str("POS");
        default:
            notify.set_flight_mode_str("----");
            break;
    }
}


3.遥控器代码调用逻辑

Ardupilot遥控器输入数据分析_第10张图片

你可能感兴趣的:(ardupilot学习)