Ardusub源码解析学习（五）—— 从manual model开始

manual_init()
manual_run()

从本篇开始，将会陆续对Ardusub中各种模式进行介绍，stabilize model在系列第一篇博文已经介绍过了（如果有时间考虑再重新写一篇？），这边就直接从手动模式开始介绍吧（因为算是最简单的了...）。

manual model：手动控制模式。Pixhawk烧写进Ardusub固件之后默认都是从这个模式进行启动。顾名思义，该模式下ROV的各种控制直接通过手柄实现，系统将不会自动帮你稳定机器人姿态。

manual_init()

// manual_init - initialise manual controller
bool Sub::manual_init()
{// set target altitude to zero for reportingpos_control.set_alt_target(0);// attitude hold inputs become thrust inputs in manual mode// set to neutral to prevent chaotic behavior (esp. roll/pitch)set_neutral_controls();return true;
}

一般来说，模式控制的程序分为init()和run()两部分，manual模式下的init()函数比较简单，跟stabilize 一样，首先是将期望高度归零。然后将姿态保持输入转变为推力输入，并设置为空挡以防止不必要的震荡和摆动。

如下所示为set_neutral_controls()内部程序。

void Sub::set_neutral_controls()
{uint32_t tnow = AP_HAL::millis();for (uint8_t i = 0; i < 6; i++) {RC_Channels::set_override(i, 1500, tnow);}// Clear pitch/roll trim settingspitchTrim = 0;rollTrim  = 0;
}

manual_run()

// manual_run - runs the manual (passthrough) controller
// should be called at 100hz or more
void Sub::manual_run()
{// if not armed set throttle to zero and exit immediatelyif (!motors.armed()) {motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);attitude_control.set_throttle_out(0,true,g.throttle_filt);attitude_control.relax_attitude_controllers();return;}motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);motors.set_roll(channel_roll->norm_input());motors.set_pitch(channel_pitch->norm_input());motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P);motors.set_throttle(channel_throttle->norm_input());motors.set_forward(channel_forward->norm_input());motors.set_lateral(channel_lateral->norm_input());
}

前面部分和stabilize model一致（具体看这里），来说说后面的set_xxx()部分好了。注意该形式函数是对输入通道进行设置，分别为横滚、俯仰、偏航、沉浮、前后平移和左右平移6个通道，注意每个通道可以对应多个电机（具体看这里）。括号内输入的参数要求范围在-1~1之间。

那么重点来看一下内部的norm_input()。这个函数位于库中的RC_Channel.cpp中（注意RC_Channel.cpp用于控制单个通道，而RC_Channels.cpp用于批量控制多个通道）。

简单来说，这段程序首先判断是否将通道输入反向，并根据结果将值赋给reverse_mul 这个变量。然后对通道输入的值进行限幅和计算，并将输入值radio_in转换为-1~1之间的输出量，输出量偏离0值的大小表示希望尽快向某一方向进行运动。

float RC_Channel::norm_input() const
{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);
}

在同一文件中的变量表中可以找到对应值

    // @Param: MIN// @DisplayName: RC min PWM// @Description: RC minimum PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.// @Units: PWM// @Range: 800 2200// @Increment: 1// @User: AdvancedAP_GROUPINFO("MIN",  1, RC_Channel, radio_min, 1100),// @Param: TRIM// @DisplayName: RC trim PWM// @Description: RC trim (neutral) PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.// @Units: PWM// @Range: 800 2200// @Increment: 1// @User: AdvancedAP_GROUPINFO("TRIM", 2, RC_Channel, radio_trim, 1500),// @Param: MAX// @DisplayName: RC max PWM// @Description: RC maximum PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.// @Units: PWM// @Range: 800 2200// @Increment: 1// @User: AdvancedAP_GROUPINFO("MAX",  3, RC_Channel, radio_max, 1900),// @Param: REVERSED// @DisplayName: RC reversed// @Description: Reverse channel input. Set to 0 for normal operation. Set to 1 to reverse this input channel.// @Values: 0:Normal,1:Reversed// @User: AdvancedAP_GROUPINFO("REVERSED",  4, RC_Channel, reversed, 0),

Ardusub源码解析学习（五）——从manual model开始相关推荐

Ardusub源码解析学习（三）——车辆类型
APM_Sub源码解析学习(三)--车辆类型一.前言二.class AP_HAL::HAL 三.class AP_Vehicle 3.1 .h 3.2 .cpp 四.class Sub 4.1 . ...
Ardusub源码解析学习（一）——Ardusub主程序
APM_Sub源码解析学习(一)--Ardusub主程序前言一.准备工作二.Ardusub.cpp解析 2.1 scheduler table 2.2 scheduler get_schedul ...
APM_ArduCopter源码解析学习（三）——无人机类型
APM_ArduCopter源码解析学习(三)--无人机类型一.前言二.class AP_HAL::HAL 三.class AP_Vehicle 3.1 .h 3.2 .cpp 四.class C ...
ElasticSearch源码解析（五）：排序（评分公式）
ElasticSearch源码解析(五):排序(评分公式) 转载自:http://blog.csdn.net/molong1208/article/details/50623948 一.目的一个 ...
okhttp源码解析（五）：代理和DNS
前言之前我们分析了okhttp的重试机制,发现在获取可用地址的时候,都需要遍历一个路由选择器,里面保存了可用的地址,那么这些地址是从哪来的呢?这就是本篇分析的重点. 首先我们简单理解一下代理和DNS ...
#gStore-weekly | gstore源码解析（五）：安全机制之日志追踪
gstore源码解析(三):安全机制之日志追踪上一章我们介绍了安全机制的黑白名单配置,接下来将对安全机制中日志追踪的源码进行解析. 1.1 简介在gstore安全机制中的日志追踪分为三个模 ...
element-ui之el-collapse-transition（折叠展开动画）源码解析学习
2019独角兽企业重金招聘Python工程师标准>>> 项目中发现同事使用了element-ui的el-collapse-transition来做折叠展开效果,打开源码看了下发现挺有 ...
Spring源码解析（五）——自定义标签解析
2019独角兽企业重金招聘Python工程师标准>>> 前言作为标签解析的第二分支,也正是因为自定义标签的存在,才让Spring框架的诸多功能在短短的几行配置代码后,就生效了. 源 ...
BLAM源码解析（五）—— 回环检测
上一节介绍了BLAM的帧间匹配和帧图匹配,代码简洁明了. 本节介绍BLAM的回环检测模块.具体代码块如下: // Check for new loop closures.bool new_keyfra ...
Autoware的MPC源码解析（五）mpc_follower解析：calculateMPC()函数解析
版权声明:本文为博主原创文章,未经博主允许不得转载. 源码地址:https://gitlab.com/autowarefoundation/autoware.ai/autoware/wikis/Sou ...

Ardusub源码解析学习（五）——从manual model开始

Ardusub源码解析学习（五）—— 从manual model开始

manual_init()

manual_run()

Ardusub源码解析学习（五）——从manual model开始相关推荐

最新文章

热门文章