版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/lixiaoweimashixiao/article/details/81667594
目录
摘要
本节主要记录自己学习arducopter代码中遥控器输入命令处理代码，无人机遥控器的输入决定着无人机怎么飞行，是非常重要的。特别是作为目标输入控制量，理解这个值，对于我们对飞控固件中的控制有很大的帮助。这里选择以futaba为例，来说明，我用的遥控器是14通道的，以美国手为例来说明。
需要注意的是遥控器的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;
}
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()获取通道号
最后得到：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
};
到这里我们只需要研究
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; 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是多少呢，在参数中有说明。也可以看下图。或者地面站数据
}
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】
到这里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 <class 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固件作为控制切换无人机的飞行模式。
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;
}
}