3.5RC_Channel 和 SRV_channel
来源:互联网 发布:java 响应式编程 编辑:程序博客网 时间:2024/06/05 10:09
前言
这部分是之前在折腾ROVER的时候梳理了一遍,但是没有形成记录,现在从新从对象的角度来分析一遍,包括映射、转换、数据结构...等方面进行梳理;
数据结构
首先分析的入口是从Rover::read_radio()的函数进行,我们就从这里为入口并作为本次分析的主线来进行阐述:
void Rover::read_radio(){//1如果没有新的RC信号,执行失控保护 if (!hal.rcin->new_input()) { control_failsafe(channel_throttle->get_radio_in()); return; } failsafe.last_valid_rc_ms = AP_HAL::millis(); //获取所有RC通道,并根据初始设定的方式[angle/range]按设置范围归一化保存至control_in中 RC_Channels::set_pwm_all(); control_failsafe(channel_throttle->get_radio_in()); //2处理伺服辅助功能 SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,channel_throttle->get_control_in()); // Check if the throttle value is above 50% and we need to nudge // Make sure its above 50% in the direction we are travelling .... ... .. //3开启舵机平滑 if (g.skid_steer_in) { // convert the two radio_in values from skid steering values float motor1 = channel_steer->norm_input(); float motor2 = channel_throttle->norm_input(); float steering_scaled = motor1 - motor2; float throttle_scaled = 0.5f*(motor1 + motor2); int16_t steer = channel_steer->get_radio_trim(); int16_t thr = channel_throttle->get_radio_trim(); channel_steer->set_pwm(steer); channel_throttle->set_pwm(thr); .... ... .. //4舵机解锁检测 rudder_arm_disarm_check();}
这里涉及到的几个对象单独罗列出来进行分析:
RC_Channel
RC_Channels
SRV_Channel
SRV_Channels
RC_Channels
首先是RC_Channels,从下面的定义来看内部定义了很多静态函数,并定义了定义了一组RC_Channel的数组以及指针,RC_Channels::set_pwm_all(void)的主要作用直接读取hal.rcin的值按range/angle转换并保存至control_in中;这里先不深入研究。
/* class RC_Channels. Hold the full set of RC_Channel objects*/class RC_Channels {public: friend class SRV_Channels; #申明SRV_Channels是其友元类 // constructor RC_Channels(void); #构造函数,并对channels[i].ch_in赋值通道号 static const struct AP_Param::GroupInfo var_info[]; static RC_Channel *rc_channel(uint8_t chan) { #返回RC_Channles[chan]指针 return (chan < NUM_RC_CHANNELS)?&channels[chan]:nullptr; } static void set_pwm_all(void);private: // this static arrangement is to avoid static pointers in AP_Param tables static RC_Channel *channels; #指向obj_channels[NUM_RC_CHANNELS]的首地址 RC_Channel obj_channels[NUM_RC_CHANNELS];};
总的来说RC_Channels就是一个定义了RC_Channel数组长度[16]且具备操作所有通道的输出的类;
RC_Channel
接下来就是RC_Channel,主要是抽象出了单个通道的数据结构,下面先从成员变量进行分析;
/// @class RC_Channel/// @brief Object managing one RC channelclass RC_Channel {public: friend class SRV_Channels; friend class RC_Channels; // Constructor RC_Channel(void); // used to get min/max/trim limit value based on _reverse enum LimitValue { RC_CHANNEL_LIMIT_TRIM, RC_CHANNEL_LIMIT_MIN, RC_CHANNEL_LIMIT_MAX }; // startup void load_eeprom(void); void save_eeprom(void); void save_trim(void); // setup the control preferences,设置获取通道数值范围/反相/死区的方法 void set_range(uint16_t high); void set_angle(uint16_t angle); bool get_reverse(void) const; void set_default_dead_zone(int16_t dzone); uint16_t get_dead_zone(void) const { return dead_zone; } // get the center stick position expressed as a control_in value int16_t get_control_mid() const; // read input from hal.rcin - create a control_in value void set_pwm(int16_t pwm); void set_pwm_no_deadzone(int16_t pwm); // calculate an angle given dead_zone and trim. This is used by the quadplane code // for hover throttle int16_t pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t trim); /* return a normalised input for a channel, in range -1 to 1, centered around the channel trim. Ignore deadzone. 归一化通道的数值为[-1,1] */ float norm_input(); /* return a normalised input for a channel, in range -1 to 1, centered around the channel trim. Take into account the deadzone 归一化通道的数值为[-1,1],增加死区 */ float norm_input_dz(); uint8_t percent_input(); int16_t pwm_to_range(); int16_t pwm_to_range_dz(uint16_t dead_zone); // read the input value from hal.rcin for this channel,从硬件抽象层中获取RC信号 uint16_t read() const; // read input from hal.rcin and set as pwm input for channel void input(); static const struct AP_Param::GroupInfo var_info[]; // return true if input is within deadzone of trim bool in_trim_dz(); int16_t get_radio_in() const { return radio_in;} void set_radio_in(int16_t val) {radio_in = val;} int16_t get_control_in() const { return control_in;} void set_control_in(int16_t val) { control_in = val;} // get control input with zero deadzone int16_t get_control_in_zero_dz(void); int16_t get_radio_min() const {return radio_min.get();} void set_radio_min(int16_t val) { radio_min = val;} int16_t get_radio_max() const {return radio_max.get();} void set_radio_max(int16_t val) {radio_max = val;} int16_t get_radio_trim() const { return radio_trim.get();} void set_radio_trim(int16_t val) { radio_trim.set(val);} void save_radio_trim() { radio_trim.save();} bool min_max_configured() const { return radio_min.configured() && radio_max.configured(); }private: // pwm is stored here // RC通道读出的原始值的保存位置 int16_t radio_in; // value generated from PWM normalised to configured scale // RC通道原始值经过归一化和配置后的保存位置 int16_t control_in; AP_Int16 radio_min; #RC通道原始值的最小值 AP_Int16 radio_trim; #RC通道原始值的中位值 AP_Int16 radio_max; #RC通道原始值的最大值 AP_Int8 reversed; #RC通道反相标志 AP_Int16 dead_zone; #RC通道死区 uint8_t type_in; #RC输入值类型是角度还是范围 int16_t high_in; // the input channel this corresponds to uint8_t ch_in; #RC通道号 int16_t pwm_to_angle(); int16_t pwm_to_angle_dz(uint16_t dead_zone);};
所有从上面的数据结构就可以看出,RC_Channel抽象的RC通道如上面注释所描述,其成员函数主要是围绕变量的设置...
在初始的通道号分配时,还涉及到RCMapper rcmap,这个主要是通道映射,在其构造函数中,通过const AP_Param::GroupInfo RCMapper::var_info[]对其成员变量进行赋值,根本上还是对通道进行映射;
RCMapper
class RCMapper{public: /// Constructor /// RCMapper(); /// roll - return input channel number for roll / aileron input uint8_t roll() const { return _ch_roll; } /// pitch - return input channel number for pitch / elevator input uint8_t pitch() const { return _ch_pitch; } /// throttle - return input channel number for throttle input uint8_t throttle() const { return _ch_throttle; } /// yaw - return input channel number for yaw / rudder input uint8_t yaw() const { return _ch_yaw; } /// forward - return input channel number for forward input uint8_t forward() const { return _ch_forward; } /// lateral - return input channel number for lateral input uint8_t lateral() const { return _ch_lateral; } static const struct AP_Param::GroupInfo var_info[];private: // channel mappings AP_Int8 _ch_roll; AP_Int8 _ch_pitch; AP_Int8 _ch_yaw; AP_Int8 _ch_throttle; AP_Int8 _ch_forward; AP_Int8 _ch_lateral;};const AP_Param::GroupInfo RCMapper::var_info[] = { // @Param: ROLL // @DisplayName: Roll channel // @Description: Roll channel number. // @Range: 1 8 // @Increment: 1 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("ROLL", 0, RCMapper, _ch_roll, 1), AP_GROUPINFO("PITCH", 1, RCMapper, _ch_pitch, 2), AP_GROUPINFO("THROTTLE", 2, RCMapper, _ch_throttle, 3), AP_GROUPINFO("YAW", 3, RCMapper, _ch_yaw, 4), AP_GROUPINFO_FRAME("FORWARD", 4, RCMapper, _ch_forward, 6, AP_PARAM_FRAME_SUB), AP_GROUPINFO_FRAME("LATERAL", 5, RCMapper, _ch_lateral, 7, AP_PARAM_FRAME_SUB),};
最终在使用的时候:
1.只需要定义一个RC_Channel的指针;
2.然后通过RC_Channels::rc_channel()返回一个实际通道指针;
3.实际通道指针的映射存在于RCMapper::var_info[];
关于使用上面三点了解了也就基本能使用了,接下来深入一点就是这个PWM的值究竟是怎么来的,从RC_Channel类中不难发现:
voidRC_Channel::input(){ radio_in = hal.rcin->read(ch_in);}uint16_tRC_Channel::read() const{ return hal.rcin->read(ch_in);}
上述两个方法的中调用都涉及到了hal.rcin,从RCInput可以找到相关实现,最终可以找到是通过orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin);来获取的到外部输入的RC 信号的;
void PX4RCInput::_timer_tick(void){ perf_begin(_perf_rcin); bool rc_updated = false; if (orb_check(_rc_sub, &rc_updated) == 0 && rc_updated) { pthread_mutex_lock(&rcin_mutex); orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin); pthread_mutex_unlock(&rcin_mutex); } // note, we rely on the vehicle code checking new_input() // and a timeout for the last valid input to handle failsafe perf_end(_perf_rcin);}
通过全局搜索我们可以在PX4Firmware/src/driver/px4_fmu/fmu.cpp中找到下面的函数,传输的_rc_in则是通过void
PX4FMU::fill_rc_in()来进行填充的,也就是我们拿到的原始包含pwm的值;
voidPX4FMU::cycle_trampoline(void *arg){PX4FMU *dev = reinterpret_cast<PX4FMU *>(arg);dev->cycle();}
而cycle中就有通过UORB发布input_rc的话题,同时调用了work_queue进行循环,仔细一看就会发现这个函数里面还有对安全开关的操作;
// slave safety from IO...../* check arming state */.....if (rc_updated) {/* lazily advertise on first publication */if (_to_input_rc == nullptr) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in);} else { orb_publish(ORB_ID(input_rc), _to_input_rc, &_rc_in);}}
work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this,
USEC2TICK(SCHEDULE_INTERVAL - main_out_latency));
到这里基本上能明白的是px4_fmu也是公共继承了CDev的类和之前的I2C、SPI一样都是属于字符型驱动;相同情况px4_io也是同样的是公共继承了CDev的字符类驱动;从nsh的终端上“ls dev”也可以看出,px对fmu和io的抽象,按照官网说明那样fmu和io之间是通过串口进行通信的,而在fmu的work_queue中还没有发现相关源码。这里需要在整理;
SRV_Channel
SRV_Channel是对输出通道的抽象出的数据结构,枚举了所有PWM通道;实现了单个SRV_Channel的从信号的输入到输出的计算,并且附加了对通道数值的类型和限幅操作;
/* class SRV_Channel. The class SRV_Channels contains an array of SRV_Channel objects. This is done to fit within the AP_Param limit of 64 parameters per object.*/class SRV_Channel {public: friend class SRV_Channels; // constructor SRV_Channel(void); static const struct AP_Param::GroupInfo var_info[]; typedef enum { k_none = 0, ///< disabled k_manual = 1, ///< manual, just pass-thru the RC in signal k_flap = 2, ///< flap k_flap_auto = 3, ///< flap automated k_aileron = 4, ///< aileron k_unused1 = 5, ///< unused function k_mount_pan = 6, ///< mount yaw (pan) k_mount_tilt = 7, ///< mount pitch (tilt) k_mount_roll = 8, ///< mount roll k_mount_open = 9, ///< mount open (deploy) / close (retract) k_cam_trigger = 10, ///< camera trigger k_egg_drop = 11, ///< egg drop k_mount2_pan = 12, ///< mount2 yaw (pan) k_mount2_tilt = 13, ///< mount2 pitch (tilt) k_mount2_roll = 14, ///< mount2 roll k_mount2_open = 15, ///< mount2 open (deploy) / close (retract) k_dspoiler1 = 16, ///< differential spoiler 1 (left wing) k_dspoiler2 = 17, ///< differential spoiler 2 (right wing) k_aileron_with_input = 18, ///< aileron, with rc input k_elevator = 19, ///< elevator k_elevator_with_input = 20, ///< elevator, with rc input k_rudder = 21, ///< secondary rudder channel k_sprayer_pump = 22, ///< crop sprayer pump channel k_sprayer_spinner = 23, ///< crop sprayer spinner channel k_flaperon1 = 24, ///< flaperon, left wing k_flaperon2 = 25, ///< flaperon, right wing k_steering = 26, ///< ground steering, used to separate from rudder k_parachute_release = 27, ///< parachute release k_gripper = 28, ///< gripper k_landing_gear_control = 29, ///< landing gear controller k_engine_run_enable = 30, ///< engine kill switch, used for gas airplanes and helicopters k_heli_rsc = 31, ///< helicopter RSC output k_heli_tail_rsc = 32, ///< helicopter tail RSC output k_motor1 = 33, ///< these allow remapping of copter motors k_motor2 = 34, k_motor3 = 35, k_motor4 = 36, k_motor5 = 37, k_motor6 = 38, k_motor7 = 39, k_motor8 = 40, k_motor_tilt = 41, ///< tiltrotor motor tilt control k_rcin1 = 51, ///< these are for pass-thru from arbitrary rc inputs k_rcin2 = 52, k_rcin3 = 53, k_rcin4 = 54, k_rcin5 = 55, k_rcin6 = 56, k_rcin7 = 57, k_rcin8 = 58, k_rcin9 = 59, k_rcin10 = 60, k_rcin11 = 61, k_rcin12 = 62, k_rcin13 = 63, k_rcin14 = 64, k_rcin15 = 65, k_rcin16 = 66, k_ignition = 67, k_choke = 68, k_starter = 69, k_throttle = 70, k_tracker_yaw = 71, ///< antennatracker yaw k_tracker_pitch = 72, ///< antennatracker pitch k_throttleLeft = 73, k_throttleRight = 74, k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; // used to get min/max/trim limit value based on reverse enum LimitValue { SRV_CHANNEL_LIMIT_TRIM, SRV_CHANNEL_LIMIT_MIN, SRV_CHANNEL_LIMIT_MAX, SRV_CHANNEL_LIMIT_ZERO_PWM }; // set the output value as a pwm value 设置当前通道的待输出的PWM值 void set_output_pwm(uint16_t pwm); // get the output value as a pwm value 获取当前通道输出的PWM值 uint16_t get_output_pwm(void) const { return output_pwm; } // set angular range of scaled output 设置在转换为PWM信号时,最大输入角度 void set_angle(int16_t angle); // set range of scaled output. Low is always zero 设置在转换为PWM信号时,最大输入范围 void set_range(uint16_t high); // return true if the channel is reversed 返回通道是否反相 bool get_reversed(void) const { return reversed?true:false; } // set MIN/MAX parameters 设置通道的PWM最大最小值 void set_output_min(uint16_t pwm) { servo_min.set(pwm); } void set_output_max(uint16_t pwm) { servo_max.set(pwm); } // get MIN/MAX/TRIM parameters 获取通道的PWM最大最小值 uint16_t get_output_min(void) const { return servo_min; } uint16_t get_output_max(void) const { return servo_max; } uint16_t get_trim(void) const { return servo_trim; }private: AP_Int16 servo_min; AP_Int16 servo_max; AP_Int16 servo_trim; // reversal, following convention that 1 means reversed, 0 means normal AP_Int8 reversed; AP_Int8 function; // a pending output value as PWM uint16_t output_pwm; // true for angle output type bool type_angle:1; // set_range() or set_angle() has been called bool type_setup:1; // the hal channel number uint8_t ch_num; // high point of angle or range output uint16_t high_out; // convert a 0..range_max to a pwm 将PWM转换为范围 uint16_t pwm_from_range(int16_t scaled_value) const; // convert a -angle_max..angle_max to a pwm 将PWM转换为角度 uint16_t pwm_from_angle(int16_t scaled_value) const; // convert a scaled output to a pwm value 输入output_scaled[范围、角度]转换为PWM并set_output_pwm(); void calc_pwm(int16_t output_scaled); // output value based on function, 调用hal.rcout直接输出PWM void output_ch(void); // setup output type and range based on function void aux_servo_function_setup(void); // return PWM for a given limit value uint16_t get_limit_pwm(LimitValue limit) const; // get normalised output from -1 to 1 float get_output_norm(void); // a bitmask type wide enough for NUM_SERVO_CHANNELS typedef uint16_t servo_mask_t; // mask of channels where we have a output_pwm value. Cleared when a // scaled value is written. static servo_mask_t have_pwm_mask;};
SRV_Channels
与SRV_Channel互为友元类,在成员变量上定义了SRV_Channels的类数组,方法上更对是对SRV_Channel的补充,例如设置中位、设置失控保护值....
下面分析一个简单的例子:
首先指定通道function(类中枚举的值)和待写入的value,这个值会被保存在functions[function].output_scaled中;void SRV_Channels::set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value); | VSCHED_TASK(set_servos, 50, 1500),//全部PWM输出这里会周期执行,里面会计算来自RC信号/其他填充的信号 | V先调用SRV_Channels::calc_pwm();再执行SRV_Channels::output_ch_all(); | Vchannels[i].output_ch();//最终回归到SRV_Channel 定义的channels[i]数组 | Vhal.rcout->write(ch_num, output_pwm);//调用AP_HAL执行输出;/// map a function to a servo channel and output itvoid SRV_Channel::output_ch(void){ int8_t passthrough_from = -1; // take care of special function cases,根据通道的编号进行不同处理,1,[51,66]k_rcin1~k_rcin16均为直通信号 switch(function) { case k_manual: // manual passthrough_from = ch_num; break; case k_rcin1 ... k_rcin16: // rc pass-thru,转换后的通道编号为0~15 passthrough_from = int8_t(function - k_rcin1); break; case k_motor1 ... k_motor8: // handled by AP_Motors::rc_write() return; } if (passthrough_from != -1) { // we are doing passthrough from input to output for this channel RC_Channel *rc = RC_Channels::rc_channel(passthrough_from); if (rc) { if (SRV_Channels::passthrough_disabled()) { output_pwm = rc->get_radio_trim(); } else { output_pwm = rc->get_radio_in(); } } } hal.rcout->write(ch_num, output_pwm);//直接通过抽象层输出,这里继续往下就要看回AP_HAL_PX下的RCOutput.cpp}
总结
RC_Channel、RC_Channels、SRV_Channel、SRV_Channels这4个类都是基于hal.rcin和hal.rcout与硬件沟通的;
- 3.5RC_Channel 和 SRV_channel
- . 和 ->
- || 和&&
- .和::
- “?”和“!”
- #和##
- && 和 ||
- 、和
- #和##
- .//和..//
- 和
- ?和\?
- ./ 和 ../
- #和##
- #、和##
- #和##
- *和&
- <和>
- spoj4487(splay)
- SPARK STREAMING之1:编程指南(翻译v1.4.1)
- 一例并发导致网站cpu 占用100% 的处理
- mysql 查询表 字段
- 通过ApplicationContextAware加载Spring上下文环境
- 3.5RC_Channel 和 SRV_channel
- TXC分布式事务简介
- 学生信息系统优化总结
- IMU原理
- 打开window桌面文件夹路径的方法
- SSH
- 数据结构与算法专题之树——二叉排序树与哈夫曼树
- 如何管理Xcode的COdeSnippets
- 连续子数组的最大和及其拓展