Apm飞控学习笔记之添加我的飞行模式-Cxm
Posted CHENxiaomingming
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了Apm飞控学习笔记之添加我的飞行模式-Cxm相关的知识，希望对你有一定的参考价值。
目录
https://blog.csdn.net/chen_taifu/article/details/122115245?spm=1001.2014.3001.5502这次的章节记录一下是如何实现在APM飞控中添加自己的模式
这里还是用的4.07版本的飞控固件
由于网上的很多版本都是3.6版本固件的添加模式 但是在最新的版本上 APM的模式架构发生了很大的变化所以这个笔记只适用于新版本的固件
先放官方的链接
APM官方模式添加说明
https://ardupilot.org/dev/docs/apmcopter-adding-a-new-flight-mode.html
架构
官方建议直接去复制一个他原本的模式去修改 更加方便
ArduCopter\\mode.h //这里包含了所有模式 一共26种
class Mode
public:
// Auto Pilot Modes enumeration
enum class Number : uint8_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 to follow another vehicle or ground station
ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers
AUTOROTATE = 26, // Autonomous autorotation
;
ArduCopter\\mode_stabilize.cpp 我们先参考一下官方推荐的模式
#include "Copter.h"
/*
* Init and run calls for stabilize flight mode
*/
// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void ModeStabilize::run()
// apply simple mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!motors->armed())
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
else if (copter.ap.throttle_zero)
// Attempting to Land
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
else
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
switch (motors->get_spool_state())
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
break;
case AP_Motors::SpoolState::GROUND_IDLE:
// Landed
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms_smoothly();
break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower)
set_land_complete(false);
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
break;
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// output pilot's throttle
attitude_control->set_throttle_out(get_pilot_desired_throttle(),
true,
g.throttle_filt);
直接通过VS code 跳转到 他的ModeStabilize 类里 回到了 ArduCopter\\mode.h 中
class ModeStabilize : public Mode
public:
// inherit constructor
using Mode::Mode;
virtual void run() override;
bool requires_GPS() const override return false;
bool has_manual_throttle() const override return true;
bool allows_arming(bool from_gcs) const override return true; ;
bool is_autopilot() const override return false;
protected:
const char *name() const override return "STABILIZE";
const char *name4() const override return "STAB";
private:
;我们大致就知道了 整个模式的内容是再那里 需要在那里定义声明
我们直接发扬传统开始复制粘贴 然后修改一下
ArduCopter\\mode.h 新建我们的模式类 并继承父类 这里我们直接照搬stabilize模式
++
//newfly
class ModeNewFly : public Mode
public:
// inherit constructor
using Mode::Mode;
virtual void run() override;
bool requires_GPS() const override return false;
bool has_manual_throttle() const override return true;
bool allows_arming(bool from_gcs) const override return true; ;
bool is_autopilot() const override return false;
protected:
const char *name() const override return "NewFly";
const char *name4() const override return "NewFly";
private:
;
///++内容不用改 因为使用了多态我们只需要修改名字就可以 最后再添加自己的功能
ArduCopter\\mode_newfly.cpp 通过架构图我们可以知道 run就是存放模式功能的地方 并且是400Hz的速度执行
我们在这里 给一个地面站消息输出用于验证 让每次波动到这个模式的时候就发送 当前模式 mode fly
/*
2022/2/24
@CHENxiaomingming
e-mail:2210138207@qq.com
newfly
*/
#include "Copter.h"
#include <GCS_MAVLink/GCS.h> //地面站
void ModeNewFly::run()
// apply simple mode transform to pilot inputs
update_simple_mode();
gcs().send_text(MAV_SEVERITY_CRITICAL, //地面站消息发送
"mode fly");
到这里我们内容基本就添加完毕了 但是还没有真的添加进去
我们先看官方文档
ArduCopter\\mode.h 添加进模式里面 先将我们的模式名字 NEWFLY 和编号 写进去
class Mode
public:
// Auto Pilot Modes enumeration
enum class Number : uint8_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 to follow another vehicle or ground station
ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers
AUTOROTATE = 26, // Autonomous autorotation
NEWFLY = 27, //+
;ArduCopter\\Copter.h 搜索ModeAcro 照样子写一个就可以
Mode *flightmode;
#if MODE_ACRO_ENABLED == ENABLED
#if FRAME_CONFIG == HELI_FRAME
ModeAcro_Heli mode_acro;
#else
ModeAcro mode_acro;
#endif
#endif
ModeAltHold mode_althold;
#if MODE_AUTO_ENABLED == ENABLED
ModeAuto mode_auto;
#endif
#if AUTOTUNE_ENABLED == ENABLED
AutoTune autotune;
ModeAutoTune mode_autotune;
#endif
#if MODE_BRAKE_ENABLED == ENABLED
ModeBrake mode_brake;
#endif
#if MODE_CIRCLE_ENABLED == ENABLED
ModeCircle mode_circle;
#endif
#if MODE_DRIFT_ENABLED == ENABLED
ModeDrift mode_drift;
#endif
#if MODE_FLIP_ENABLED == ENABLED
ModeFlip mode_flip;
#endif
#if MODE_FOLLOW_ENABLED == ENABLED
ModeFollow mode_follow;
#endif
#if MODE_GUIDED_ENABLED == ENABLED
ModeGuided mode_guided;
#endif
ModeLand mode_land;
#if MODE_LOITER_ENABLED == ENABLED
ModeLoiter mode_loiter;
#endif
#if MODE_POSHOLD_ENABLED == ENABLED
ModePosHold mode_poshold;
#endif
#if MODE_RTL_ENABLED == ENABLED
ModeRTL mode_rtl;
#endif
#if FRAME_CONFIG == HELI_FRAME
ModeStabilize_Heli mode_stabilize;
#else
ModeStabilize mode_stabilize;
#endif
#if MODE_SPORT_ENABLED == ENABLED
ModeSport mode_sport;
#endif
#if MODE_SYSTEMID_ENABLED == ENABLED
ModeSystemId mode_systemid;
#endif
#if ADSB_ENABLED == ENABLED
ModeAvoidADSB mode_avoid_adsb;
#endif
#if MODE_THROW_ENABLED == ENABLED
ModeThrow mode_throw;
#endif
#if MODE_GUIDED_NOGPS_ENABLED == ENABLED
ModeGuidedNoGPS mode_guided_nogps;
#endif
#if MODE_SMARTRTL_ENABLED == ENABLED
ModeSmartRTL mode_smartrtl;
#endif
#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED
ModeFlowHold mode_flowhold;
#endif
#if MODE_ZIGZAG_ENABLED == ENABLED
ModeZigZag mode_zigzag;
#endif
#if MODE_AUTOROTATE_ENABLED == ENABLED
ModeAutorotate mode_autorotate;
#endif
//+
#if MODE_NEWFLY_ENABLED == ENABLED
ModeNewFly mode_newfly;
#endifArduCopter\\config.h 需要声明一下
//
// Position Hold - enable holding of global position
#ifndef MODE_POSHOLD_ENABLED
# define MODE_POSHOLD_ENABLED ENABLED
#endif
//
// RTL - Return To Launch
#ifndef MODE_RTL_ENABLED
# define MODE_RTL_ENABLED ENABLED
#endif
//
// SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home
#ifndef MODE_SMARTRTL_ENABLED
# define MODE_SMARTRTL_ENABLED ENABLED
#endif
//
// Sport - fly vehicle in rate-controlled (earth-frame) mode
#ifndef MODE_SPORT_ENABLED
# define MODE_SPORT_ENABLED !HAL_MINIMIZE_FEATURES
#endif
//
// System ID - conduct system identification tests on vehicle
#ifndef MODE_SYSTEMID_ENABLED
# define MODE_SYSTEMID_ENABLED !HAL_MINIMIZE_FEATURES
#endif
//
// Throw - fly vehicle after throwing it in the air
#ifndef MODE_THROW_ENABLED
# define MODE_THROW_ENABLED ENABLED
#endif
//
// ZigZag - allow vehicle to fly in a zigzag manner with predefined point A B
#ifndef MODE_ZIGZAG_ENABLED
# define MODE_ZIGZAG_ENABLED !HAL_MINIMIZE_FEATURES
#endif
//+new fly
#ifndef MODE_NEWFLY_ENABLED
# define MODE_NEWFLY_ENABLED ENABLED
#endifArduCopter\\mode.cpp 需要在里面添加编号和建立映射
mode_from_mode_num()里
Mode *Copter::mode_from_mode_num(const Mode::Number mode)
Mode *ret = nullptr;
switch (mode)
#if MODE_ACRO_ENABLED == ENABLED
case Mode::Number::ACRO:
ret = &mode_acro;
break;
#endif
case Mode::Number::STABILIZE:
ret = &mode_stabilize;
break;
case Mode::Number::ALT_HOLD:
ret = &mode_althold;
break;
#if MODE_AUTO_ENABLED == ENABLED
case Mode::Number::AUTO:
ret = &mode_auto;
break;
#endif
#if MODE_CIRCLE_ENABLED == ENABLED
case Mode::Number::CIRCLE:
ret = &mode_circle;
break;
#endif
#if MODE_LOITER_ENABLED == ENABLED
case Mode::Number::LOITER:
ret = &mode_loiter;
break;
#endif
#if MODE_GUIDED_ENABLED == ENABLED
case Mode::Number::GUIDED:
ret = &mode_guided;
break;
#endif
case Mode::Number::LAND:
ret = &mode_land;
break;
#if MODE_RTL_ENABLED == ENABLED
case Mode::Number::RTL:
ret = &mode_rtl;
break;
#endif
#if MODE_DRIFT_ENABLED == ENABLED
case Mode::Number::DRIFT:
ret = &mode_drift;
break;
#endif
#if MODE_SPORT_ENABLED == ENABLED
case Mode::Number::SPORT:
ret = &mode_sport;
break;
#endif
#if MODE_FLIP_ENABLED == ENABLED
case Mode::Number::FLIP:
ret = &mode_flip;
break;
#endif
#if AUTOTUNE_ENABLED == ENABLED
case Mode::Number::AUTOTUNE:
ret = &mode_autotune;
break;
#endif
#if MODE_POSHOLD_ENABLED == ENABLED
case Mode::Number::POSHOLD:
ret = &mode_poshold;
break;
#endif
#if MODE_BRAKE_ENABLED == ENABLED
case Mode::Number::BRAKE:
ret = &mode_brake;
break;
#endif
#if MODE_THROW_ENABLED == ENABLED
case Mode::Number::THROW:
ret = &mode_throw;
break;
#endif
#if ADSB_ENABLED == ENABLED
case Mode::Number::AVOID_ADSB:
ret = &mode_avoid_adsb;
break;
#endif
#if MODE_GUIDED_NOGPS_ENABLED == ENABLED
case Mode::Number::GUIDED_NOGPS:
ret = &mode_guided_nogps;
break;
#endif
#if MODE_SMARTRTL_ENABLED == ENABLED
case Mode::Number::SMART_RTL:
ret = &mode_smartrtl;
break;
#endif
#if OPTFLOW == ENABLED
case Mode::Number::FLOWHOLD:
ret = (Mode *)g2.mode_flowhold_ptr;
break;
#endif
#if MODE_FOLLOW_ENABLED == ENABLED
case Mode::Number::FOLLOW:
ret = &mode_follow;
break;
#endif
#if MODE_ZIGZAG_ENABLED == ENABLED
case Mode::Number::ZIGZAG:
ret = &mode_zigzag;
break;
#endif
#if MODE_SYSTEMID_ENABLED == ENABLED
case Mode::Number::SYSTEMID:
ret = (Mode *)g2.mode_systemid_ptr;
break;
#endif
///NEWFLY +
#if MODE_NEWFLY_ENABLED == ENABLED
case Mode::Number::NEWFLY:
ret = &mode_newfly;
break;
#endif
#if MODE_AUTOROTATE_ENABLED == ENABLED
case Mode::Number::AUTOROTATE:
ret = &mode_autorotate;
break;
#endif
default:
break;
return ret;
到这里其实就可以直接编译使用了 但是如果想让地面站有显示的化 还需要一些操作
modules\\mavlink\\message_definitions\\v1.0\\ardupilotmega.xml
在这里文件里面 搜索COPTER_MODE 添加进这个枚举里面就可以
<enum name="COPTER_MODE">
<description>A mapping of copter flight modes for custom_mode field of heartbeat.</description>
<entry value="0" name="COPTER_MODE_STABILIZE"/>
<entry value="1" name="COPTER_MODE_ACRO"/>
<entry value="2" name="COPTER_MODE_ALT_HOLD"/>
<entry value="3" name="COPTER_MODE_AUTO"/>
<entry value="4" name="COPTER_MODE_GUIDED"/>
<entry value="5" name="COPTER_MODE_LOITER"/>
<entry value="6" name="COPTER_MODE_RTL"/>
<entry value="7" name="COPTER_MODE_CIRCLE"/>
<entry value="9" name="COPTER_MODE_LAND"/>
<entry value="11" name="COPTER_MODE_DRIFT"/>
<entry value="13" name="COPTER_MODE_SPORT"/>
<entry value="14" name="COPTER_MODE_FLIP"/>
<entry value="15" name="COPTER_MODE_AUTOTUNE"/>
<entry value="16" name="COPTER_MODE_POSHOLD"/>
<entry value="17" name="COPTER_MODE_BRAKE"/>
<entry value="18" name="COPTER_MODE_THROW"/>
<entry value="19" name="COPTER_MODE_AVOID_ADSB"/>
<entry value="20" name="COPTER_MODE_GUIDED_NOGPS"/>
<entry value="21" name="COPTER_MODE_SMART_RTL"/>
<entry value="27" name="COPTER_MODE_NEWFLY"/>
</enum>到这里 官方的说明就结束了 现在我们编译 然后下载到飞控中
但是地面站并没有显示我们的模式 官方说有的地面站会填充 可能是版本问题 所以我们只能通过修改参数 来开启这个模式 在官方文档里面有说明
ArduCopter\\Parameters.cpp 这里面搜索FLTMODE1
这里面是和地面上(上面的图)对应的六个模式的初始化模式
// @Param: FLTMODE1
// @DisplayName: Flight Mode 1
// @Description: Flight mode when Channel 5 pwm is <= 1230
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
// @User: Standard
GSCALAR(flight_mode1, "FLTMODE1", (uint8_t)FLIGHT_MODE_1),
// @Param: FLTMODE2
// @DisplayName: Flight Mode 2
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
// @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", (uint8_t)FLIGHT_MODE_2),
// @Param: FLTMODE3
// @DisplayName: Flight Mode 3
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
// @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", (uint8_t)FLIGHT_MODE_3),
// @Param: FLTMODE4
// @DisplayName: Flight Mode 4
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
// @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", (uint8_t)FLIGHT_MODE_4),
// @Param: FLTMODE5
// @DisplayName: Flight Mode 5
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
// @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", (uint8_t)FLIGHT_MODE_5),
// @Param: FLTMODE6
// @DisplayName: Flight Mode 6
// @Description: Flight mode when Channel 5 pwm is >=1750
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
// @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", (uint8_t)FLIGHT_MODE_6),
刚好6个 直接跳转到 FLIGHT_MODE_4配置里面去
ArduCopter\\config.h 下面的 STABILZE 就是默认模式 如果想吧默认模式修改 直接改成 NEWFLY 就可以了 但是我不推荐这样的操作 因为地面站有参数配置
#ifndef FLIGHT_MODE_1
# define FLIGHT_MODE_1 Mode::Number::STABILIZE
#endif
#ifndef FLIGHT_MODE_2
# define FLIGHT_MODE_2 Mode::Number::STABILIZE
#endif
#ifndef FLIGHT_MODE_3
# define FLIGHT_MODE_3 Mode::Number::STABILIZE
#endif
#ifndef FLIGHT_MODE_4
# define FLIGHT_MODE_4 Mode::Number::STABILIZE
#endif
#ifndef FLIGHT_MODE_5
# define FLIGHT_MODE_5 Mode::Number::STABILIZE
#endif
#ifndef FLIGHT_MODE_6
# define FLIGHT_MODE_6 Mode::Number::STABILIZE
#endif这里我们直接通过地面站的参数配置来设置我们的模式
在全部设置里面 搜索MODE 里面就有上面的参数了 后面0-26就是可选模式 因为我们的模式是27 就给27就可以了 然后写入
最后来看效果 地面站上没有显示这个原因 我觉得是因为地面站
不印象实际的运行 如果有前辈知道这么解决 麻烦留言
切换到模式4 成功输出
模式之间可以顺利切换
END
以上是关于Apm飞控学习笔记之添加我的飞行模式-Cxm的主要内容，如果未能解决你的问题，请参考以下文章