Ardupilot添加一个新模式全攻略,以Copter为例

无人机 同时被 3 个专栏收录
12 篇文章 2 订阅
13 篇文章 1 订阅
2 篇文章 0 订阅

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 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
	ACQIN     =    25,  // Special mode built by acqin
};

直接在最后添加一个新mode,ACQIN = 25,记住这个值!

3、写声明

mode的声明写在了mode.h中,我们同样写一个

class ModeACQIN : public Mode {

public:
    // inherit constructor
    using Mode::Mode;
    bool init(bool ignore_checks) override;
    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 "ACQIN"; }
    const char *name4() const override { return "ACQI"; }
    
private:
};

必须包含

bool init(bool ignore_checks) override;
    void run() override;

把ACQIN等改写成合适的代号。

const char *name() const override { return "ACQIN"; }
const char *name4() const override { return "ACQI"; }

4、新建.cpp文件

在ArduCopter目录下新建一个.cpp的文件,比如说mode_ACQIN.cpp,里面要定义init()run(),具体功能就看大家发挥了。本文只是仿写 mode_stabilize.cpp。

#include "Copter.h"
#include "mode.h"

#if MODE_ACQIN_ENABLED == ENABLED

bool ModeACQIN::init(bool ignore_checks)
{

    return true;
}
// circle_run - runs the circle flight mode
// should be called at 100hz or more
void ModeACQIN::run()
{
    gcs().send_text(MAV_SEVERITY_INFO, "hello111");

    // 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();
        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);
}


#endif

5、实例化mode类

修改Copter.h,将自己的mode添进去,进行实例化。

    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_ACQIN_ENABLED == ENABLED
    ModeACQIN mode_ACQIN;
#endif

此处多了一个MODE_ACQIN_ENABLED我们要在config.h中添加他的定义。

#ifndef MODE_ACQIN_ENABLED
# define MODE_ACQIN_ENABLED ENABLED
#endif

6、修改mode_from_mode_num()

此函数是传入mode名,返回一个对应的MODE类,我们需要在函数内添加我们自己新加的mode类。

#if MODE_ACQIN_ENABLED == ENABLED
        case ACQIN:
            ret = &mode_ACQIN;
            break;
#endif

&后面是你在上一步中实例化的mode类。

7、修改Parameters.cpp

在Parameters.cpp 中修改FLTMODE1 ~ FLTMODE6@Values ,这里的值对应control_mode_t{}:中mode值。

// @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:ACQIN
    // @User: Standard
    GSCALAR(flight_mode1, "FLTMODE1",               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:ACQIN
    // @User: Standard
    GSCALAR(flight_mode2, "FLTMODE2",               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:ACQIN
    // @User: Standard
    GSCALAR(flight_mode3, "FLTMODE3",               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:ACQIN
    // @User: Standard
    GSCALAR(flight_mode4, "FLTMODE4",               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:ACQIN
    // @User: Standard
    GSCALAR(flight_mode5, "FLTMODE5",               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:ACQIN
    // @User: Standard
    GSCALAR(flight_mode6, "FLTMODE6",               FLIGHT_MODE_6),

8、修改ardupilotmega.xml

在ardupilot/modules/mavlink/message_definitions/v1.0/ardupilotmega.xml修改COPTER_MODE,在最后仿照其他添加自己的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="22" name="COPTER_MODE_ACQIN"/>
    </enum>

运行setup.py完成修改
我们cd到ardupilot/modules/mavlink/pymavlink/,目录下有一个setup.py,这里setup 了mvproxy,需要运行他才可以在console中输入mode指令改变飞行器的飞行模式,终端中输入sudo python setup.py install

9、修改mavutil.py文件

记得第一步我们说mvproxy传来一个数值参数,我们当然也要修改mvproxy中的参数列表,打开ardupilot/modules/mavlink/pymavlink/mavutil.py,找到mode_mapping_acm = {},向最后添加对应的参数值。注意参数值和第一步数值相同。

mode_mapping_acm = {
    0 : 'STABILIZE',
    1 : 'ACRO',
    2 : 'ALT_HOLD',
    3 : 'AUTO',
    4 : 'GUIDED',
    5 : 'LOITER',
    6 : 'RTL',
    7 : 'CIRCLE',
    8 : 'POSITION',
    9 : 'LAND',
    10 : 'OF_LOITER',
    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 : 'ACQIN',
}

(同时你会发现项目中落下了一个模式,你可以帮他们补上-

10、完成添加

直接运行Tools/autotest/sim_vehicle.py -v ArduCopter --console --map完成编译加仿真(真方便)。
MAV中输入mode (新mode)切换mode,console显示切换成功。

在这里插入图片描述
我们发现新mode添加成功。

写在最后

接下来一段时间将会研究ArduCopter,小白独自摸索,希望各路大佬能互关,或者拉我进群(嘿嘿嘿),大家一起讨论ArduCopter代码分析以及二次开发

  • 6
    点赞
  • 2
    评论
  • 17
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

©️2021 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值