PX4遥控器控制量和模式开关的实现

匿名 (未验证) 提交于 2019-12-03 00:22:01

    上一篇文章讲到了PX4FMU通过问答的方式从PX4IO获取所需的所有信息。在px4io.cpp文件中的task_main()的任务循环里,可以看到读取和发布IO芯片的函数有:

1、io_get_status()――从IO芯片获取状态信息,并发布safety消息;

2、io_publish_raw_rc()――从IO芯片获取遥控器所有输入通道的原始数据,并发布input_rc信息;

3、io_publish_pwm_outputs()――从IO芯片获取并发布各通道实际输出的PWM值。

    本篇将追寻input_rc消息的路线,确定遥控控制量是如何进入飞控的,并确定模式开关的实现原理。

    在代码中,直接获取input_rc消息的主要有两个任务。

    一个是mavlink_messages,暂且猜测其目的应该是将遥控器原始输入值发送到地面站来实现遥控器校准、通道映射的功能,经过地面站校准之后的信息有从mavlink协议发回给飞控,保存为遥控器相关的参数。

    另一个就是sensors,在sensors.cpp文件的主任务循环中调用了Sensors::rc_poll()来获取input_rc消息并处理。

    首先判断是否有遥控信号丢失,将判断结果保存在signal_lost中;

    然后利用参数中的遥控器校准信息对各通道数值进行修剪,在转化为-1~+1之间的数值传递给_rc.channels[]数组中的对应位置,发布rc_channels消息供调试用;

    最后,如果信号没有丢失的话,就将之前转化之后的遥控量和控制开关的位置写入到manual结构体中,并发布manual_control_setpoint消息。

其中,控制量有:

			/* limit controls */ 			manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); 			manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); 			manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); 			manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); 			manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); 			manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); 			manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); 			manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); 			manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); 			manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);

6段模式开关位置的计算:

			if (_parameters.rc_map_flightmode > 0) {  				/* the number of valid slots equals the index of the max marker minus one */ 				const int num_slots = manual_control_setpoint_s::MODE_SLOT_MAX;  				/* the half width of the range of a slot is the total range 				 * divided by the number of slots, again divided by two 				 */ 				const float slot_width_half = 2.0f / num_slots / 2.0f;  				/* min is -1, max is +1, range is 2. We offset below min and max */ 				const float slot_min = -1.0f - 0.05f; 				const float slot_max = 1.0f + 0.05f;  				/* the slot gets mapped by first normalizing into a 0..1 interval using min 				 * and max. Then the right slot is obtained by multiplying with the number of 				 * slots. And finally we add half a slot width to ensure that integer rounding 				 * will take us to the correct final index. 				 */ 				manual.mode_slot = (((((_rc.channels[_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) + slot_width_half) / 						     (slot_max - slot_min)) + (1.0f / num_slots));  				if (manual.mode_slot >= num_slots) { 					manual.mode_slot = num_slots - 1; 				} 			}

独立模式开关的处理为:

			/* mode switches */ 			manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, 					     _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); 			manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, 						  _parameters.rc_rattitude_th, 						  _parameters.rc_rattitude_inv); 			manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, 					       _parameters.rc_posctl_inv); 			manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, 					       _parameters.rc_return_inv); 			manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, 					       _parameters.rc_loiter_inv); 			manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, 					     _parameters.rc_acro_inv); 			manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, 						 _parameters.rc_offboard_th, _parameters.rc_offboard_inv); 			manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH, 					     _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv); 			manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,  						   _parameters.rc_trans_th, _parameters.rc_trans_inv);

发布消息:

			/* publish manual_control_setpoint topic */ 			if (_manual_control_pub != nullptr) { 				orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);   			} else { 				_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);  			}

    在commander.cpp的set_main_state_rc()函数中接收这个manual_control_setpoint消息,并进行模式切换操作。

逻辑框图就是按照官网的文档介绍来处理的,这个很多文章里头都讲了,可以自己去查看。



易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!