Ardupilot添加一个新MODE全攻略,以Copter为例
Ardupilot添加一个新mode在官网有比较详细的教程,此外网络上各路大佬也写过不少这类教程,鉴于项目不停的更新,以往教程已经不适合新版本。本博客参考官网,完成添加一个新mode,并能够在MVPROXY发送指令中使飞行器进入新mode。
“切换模式”代码框架
官网提供给我们一个比较完整的mode调用时的代码框架。
调试方法
我们知道c语言中printf()是一个非常耐打的调试工具,此处也不例外,本文使用了强大的 gcs().send_text(MAV_SEVERITY_INFO, "hello");
就是可以在console中输出一段话(hello)。
2、添加control_mode_t
首先我们看Copter接受到MAVLINK切换mode后的处理函数:
bool GCS_MAVLINK_Copter::set_mode(const uint8_t mode)
{
#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
if (copter.failsafe.radio) {
// don't allow mode changes while in radio failsafe
return false;
}
#endif
return copter.set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND);
}
此处mvlink传来的参数是uint8_t格式,使用 gcs().send_text()
我们可以发现这是一个数字,最后在return
中使用(control_mode_t)
转化成了mode名,我们打开control_mode_t{}
:
// Auto Pilot Modes enumeration
enum control_mode_t {
STABILIZE = 0, // manual airframe angle with manual throttle
ACRO = 1, // manual body-frame angular rate with manual throttle
ALT_HOLD = 2, // manual airframe angle with automatic throttle
AUTO = 3, // fully automatic waypoint control using mission commands
GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
LOITER = 5, // automatic horizontal acceleration with automatic throttle
RTL = 6, // automatic return to launching point
CIRCLE = 7, // automatic circular flight with automatic throttle
LAND = 9, // automatic landing with horizontal position control
DRIFT = 11, // semi-automous position, yaw and throttle control
SPORT = 13, // manual earth-frame angular rate control with manual throttle
FLIP = 14, // automatically flip the vehicle on the roll axis
AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
BRAKE = 17, // full-brake using inertial/GPS system, no pilot input
THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input
AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude
SMART_RTL = 21, // SMART_RTL returns to home by retracing its steps
FLOWHOLD = 22, // FLOWHOLD holds position with optical flow without rangefinder
FOLLOW = 23, // follow attempts