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 reporting pos_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 settings pitchTrim = 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 immediately if (!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: Advanced AP_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: Advanced AP_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: Advanced AP_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: Advanced AP_GROUPINFO("REVERSED", 4, RC_Channel, reversed, 0),