PX4飞控手动位置控制POSCTL模式的实现流程

第一步，POSCTL模式的进入

1、从遥控器模式开关进入。

首先在PX4IO的主循环中调用了io_publish_raw_rc()，从IO芯片获取遥控器的各通道输入，发布input_rc消息；

然后，在Sensors的主循环中调用rc_poll()函数，获取input_rc消息的内容，按照通道映射和校准信息将遥控器输入值重新计算并写入manual_control_setpoint消息中进行发布，发布内容包括；

         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);/* filter controls */manual.y = math::constrain(_filter_roll.apply(manual.y), -1.f, 1.f);manual.x = math::constrain(_filter_pitch.apply(manual.x), -1.f, 1.f);manual.r = math::constrain(_filter_yaw.apply(manual.r), -1.f, 1.f);manual.z = math::constrain(_filter_throttle.apply(manual.z), 0.f, 1.f);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;}}

最后，在commander的主循环中调用set_main_state_rc()函数，其中利用sp_man.mode_slot的值来进行模式切换，当开关切换到POSCTL对应的位置时，执行main_state_transition()函数将新模式写入到internal_state.main_state中，并记录之前的模式和模式切换时间。

2、从mavlink协议进入。

同样在commander的主循环中，当接收到vehicle_command消息时运行handle_command()函数。如果消息的内容是VEHICLE_CMD_DO_SET_MODE，就是根据参数的内容进行模式设定。当参数中的base_mode为CUMTOM且custom_main_mode是PX4_CUSTOM_MAIN_MODE_POSCTL，同样是调用了main_state_transition()函数将新模式写入。

 case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {uint8_t base_mode = (uint8_t)cmd->param1;uint8_t custom_main_mode = (uint8_t)cmd->param2;uint8_t custom_sub_mode = (uint8_t)cmd->param3;
.......if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {/* use autopilot-specific mode */if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {/* MANUAL */main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {/* ALTCTL */main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state);} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {/* POSCTL */main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);......}break;

第二步，导航模式和控制模式的选择。

上一步，在commander中除了切换模式之外，还进行了导航模式和控制模式的设定。

在set_nav_state()函数中设定status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;

在set_control_mode()函数中根据nva_state的值设定了控制相关的flag，包括：

 case vehicle_status_s::NAVIGATION_STATE_POSCTL:control_mode.flag_control_manual_enabled = true;control_mode.flag_control_auto_enabled = false;control_mode.flag_control_rates_enabled = true;control_mode.flag_control_attitude_enabled = true;control_mode.flag_control_rattitude_enabled = false;control_mode.flag_control_altitude_enabled = true;control_mode.flag_control_climb_rate_enabled = true;control_mode.flag_control_position_enabled = !status.in_transition_mode;control_mode.flag_control_velocity_enabled = !status.in_transition_mode;control_mode.flag_control_acceleration_enabled = false;control_mode.flag_control_termination_enabled = false;break;

并按照一定周期发布了vehicle_control_mode和vehicle_status两个消息。

然后，在navigator的主循环中，根据在commander中设定的导航模式，选择了对应模式的实例。

     case vehicle_status_s::NAVIGATION_STATE_MANUAL:case vehicle_status_s::NAVIGATION_STATE_ACRO:case vehicle_status_s::NAVIGATION_STATE_ALTCTL:case vehicle_status_s::NAVIGATION_STATE_POSCTL:case vehicle_status_s::NAVIGATION_STATE_TERMINATION:case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:case vehicle_status_s::NAVIGATION_STATE_STAB:default:_pos_sp_triplet_published_invalid_once = false;_navigation_mode = nullptr;_can_loiter_at_sp = false;break;

由于这些都是手控模式，因此_navigation_mode = nullptr。也就是不会调用任何导航实例。而是调用了如下代码

     /* if nothing is running, set position setpoint triplet invalid once */if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {_pos_sp_triplet_published_invalid_once = true;_pos_sp_triplet.previous.valid = false;_pos_sp_triplet.current.valid = false;_pos_sp_triplet.next.valid = false;_pos_sp_triplet_updated = true;}if (_pos_sp_triplet_updated) {_pos_sp_triplet.timestamp = hrt_absolute_time();publish_position_setpoint_triplet();_pos_sp_triplet_updated = false;}

实际上由于_pos_sp_triplet.current.valid = false; 函数 publish_position_setpoint_triplet(); 会立即返回，并不会发布提供位置控制期望的position_setpoint_triplet消息出去。

第三步，位置控制的实现。

在mc_pos_control的主循环中首先调用do_control()函数；由于在commander中设定了control_mode.flag_control_manual_enabled = true，进而进入control_manual()函数。对于非手控的模式则进入control_non_manual()函数。

在control_manual()函数中从manual_control_setpoint消息中获取纵横垂向的杆量，并根据参数进行限幅之后给中间变量。然后转换到NED坐标系中。

 math::Vector<2> req_vel_sp_xy;req_vel_sp_xy.zero();float req_vel_sp_z = 0.0f;if (_control_mode.flag_control_altitude_enabled) {/* set vertical velocity setpoint with throttle stick */req_vel_sp_z = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); // D/* reset alt setpoint to current altitude if needed */reset_alt_sp();}if (_control_mode.flag_control_position_enabled) {/* set horizontal velocity setpoint with roll/pitch stick */req_vel_sp_xy(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_xy_dz);req_vel_sp_xy(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_xy_dz);/* reset position setpoint to current position if needed */reset_pos_sp();}/* scale requested velocity setpoint to cruisespeed and rotate around yaw */math::Vector<3> vel_cruise(_params.vel_cruise(0),_params.vel_cruise(1),(req_vel_sp_z > 0.0f) ? _params.vel_max_down : _params.vel_max_up);math::Vector<3> req_vel_sp_scaled(req_vel_sp_xy(0), req_vel_sp_xy(1), req_vel_sp_z);/* scale velocity setpoint to cruise speed (m/s) and rotate around yaw to NED frame */math::Matrix<3, 3> R_input_fame;if (_control_mode.flag_control_fixed_hdg_enabled) {R_input_fame.from_euler(0.0f, 0.0f, _yaw_takeoff);} else {R_input_fame.from_euler(0.0f, 0.0f, _att_sp.yaw_body);}req_vel_sp_scaled = R_input_fame * req_vel_sp_scaled.emult(vel_cruise); // in NED and scaled to actual velocity;

根据control_mode设定的flag_control_altitude_enabled和flag_control_position_enabled确定是否在赶回中后保持高度和平面位置（也就是ALTCTL模式和POSCTL模式的作用了）。最后判断是否已经着陆，如果没有着陆就执行control_position()函数，从位置偏差计算速度期望值_vel_sp，并通过消息vehicle_global_velocity_setpoint发布出去，当然还有很多边边角角的复杂计算，需要自己看代码来理解。

回到主循环中，结束了do_control()函数之后，将位置期望和速度期望赋值给_local_pos_sp，并发布消息vehicle_local_position_setpoint。接下来调用generate_attitude_setpoint()函数将姿态角期望写入_att_sp结构体中，并发布vehicle_attitude_setpoint消息。

第四步，姿态控制的实现。

目前还没看完这部分代码，等看完了再更新。

第五步，控制信号的输出。

在PX4IO的主循环中，执行io_set_control_groups()将对应控制组的控制量输出给IO芯片来产生控制电调或者舵机的PWM信号。

PX4飞控手动位置控制POSCTL模式的实现流程相关推荐

PX4位置控制offboard模式说明
offboard模式的开发及应用一.px4固件的模式 px4固件支持10几种飞行模式,从代码结构上分析,分为基本模式.自定义模式和自定义子模式. 1.基本模式基本模式又分为,位置控制模式.自稳模式 ...
飞控简析-从入门到跑路第二章PX4的位置控制(1)
一.前言首先,我们要清楚的我们的需求,PX4的位置控制需要完成什么样的工作.位置控制需要完成的是,从期望位置得到期望姿态的一个过程,然后把期望姿态传递给姿态控制模块,所以位置控制的输入是期望位置,输 ...
飞控简析-从入门到跑路第二章PX4的位置控制(2)
1.control_auto() 说完control_manual,接下来我们在看看control_auto.control_auto是用来处理自动控制的函数,即把航线任务转换为期望位置.函数位于Mc ...
PX4代码学习系列博客(6)——offboard模式位置控制代码分析（之前转载过，这是第二次转载了）
我刚刚发现这篇文章去年八月份的时候转载过一次了 https://blog.csdn.net/sinat_16643223/article/details/107874349 转载自:https://b ...
PX4飞控之PWM输出控制
PX4飞控之PWM输出控制多旋翼电调如好盈XRotor,DJI通用电调等都支持PWM信号来传输控制信号.常用的400Hz电调信号对应周期2500us,一般使用高电平时间1000us~2000us为有 ...
汇川伺服电机位置控制模式参数配置
1. 基本控制参数设置 1)设置位置控制模式 2)绝对值位置线性模式 2.端子输入参数设置 1)将输入端子使能失效 3. 位置控制参数设置 1)将位置来源设置为2:多段位位置指令 4.通信参数设置 1 ...
伺服速度控制模式接线图_伺服参数设置很重要（位置控制模式、速度控制模式、力矩控制）...
伺服在自动化设备的组成中占有重要地位.伺服是在其额定转速范围内,属于恒力矩输出.且本身具有多种反馈调节,用来保证伺服的运行精度以及输出力矩的精度.全功能的伺服控制器拥有3多种控制模式,每种控制模式的控 ...
s120面板控制调速_SINAMICS S120 SERVO模式下位置控制与速度控制方式的切换
图 1 控制系统结构图从图 1 中看出,通过参数 P2550 和 P1142 的组合设置,可以在两种控制方式之间切换:通过 EPOS 和 P1155 可以对位置和速度进行设定.两种控制方式的设置组 ...
PX4多旋翼位置控制程序分析 mc_pos_control
PX4多旋翼位置控制程序分析 mc_pos_control 1. 概述在本文中,我们将分析PX4程序的基本流程和关键模块. 2. PX4程序流程图原图大小太大,无法上传(在我主页下载里面找) 3. ...

PX4飞控手动位置控制POSCTL模式的实现流程

PX4飞控手动位置控制POSCTL模式的实现流程相关推荐

最新文章

热门文章