ArduPilot第6章电机模块解析

axzg

已于 2024-05-21 16:19:42 修改

阅读量1.7k

点赞数 26

文章标签：算法

于 2024-02-24 19:07:09 首次发布

本文链接：https://blog.csdn.net/zhanggeng0101/article/details/136255215

版权

文章目录

前言
一、roll pitch yaw throtttle
二、rc_in()和rc_out()初始化
三、电机初始化
二、电机pwm计算
总结

参考文献
ardupilot 中电机输出逻辑及电机转轴状态分析
https://blog.csdn.net/lixiaoweimashixiao/article/details/126745495

APM（Ardupilot）——电机输出流程图
https://blog.csdn.net/yinxian5224/article/details/102743618

APM_ArduCopter源码解析学习（一）——ArduCopter主程序
https://blog.csdn.net/zhangfengmei1987/article/details/110432871

APM_ArduCopter源码解析学习（二）——电机库学习
https://blog.csdn.net/zhangfengmei1987/article/details/110448094?spm=1001.2014.3001.5502

PX4用户指南-飞行-基本操作
https://www.ncnynl.com/archives/201810/2666.html

Ardupilot动力分配-混控部分分析
https://blog.csdn.net/qq_15390133/article/details/123822392

ardupilot 中电机输出逻辑及电机转轴状态分析
https://download.csdn.net/blog/column/7327436/126745495

ardupilot 函数output_armed_stabilizing
https://download.csdn.net/blog/column/7327436/109166189

[飞控]从零开始建模(三)-控制分配浅析
https://zhuanlan.zhihu.com/p/46839430

RC输入通道映射(RCMAP)
https://doc.cuav.net/tutorial/copter/advanced-configuration/rc-input-channel-mapping.html

Apm飞控学习笔记-AC_PosControl位置控制-Cxm
https://blog.csdn.net/chen_taifu/article/details/124610904

ardupilot开发 — 位置控制与导航制导篇
https://blog.csdn.net/weixin_43321489/article/details/132362043

// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
rc_write(AP_MOTORS_MOT_1, pwm);

前言

本文介绍Ardupilot的电机模块相关知识。

一、roll pitch yaw throtttle

roll pitch yaw throtttle
请添加图片描述

二、rc_in()和rc_out()初始化

1.Copter::init_ardupilot()

在void Copter::init_ardupilot()函数中调用init_rc_in()和init_rc_out()函数

\ardupilot\ardupilot\ArduCopter\system.cpp

void Copter::init_ardupilot()
{
   
	...
#endif
#if FRAME_CONFIG == HELI_FRAME
    input_manager.set_loop_rate(scheduler.get_loop_rate_hz());
#endif

    init_rc_in();               // sets up rc channels from radio

    ...

    allocate_motors();

   ...
    rc().init();

    // sets up motors and output to escs
    init_rc_out();

    

	...
}

2.init_rc_in()

\ArduCopter\radio.cpp

 void Copter::init_rc_in()
 {
   
     channel_roll     = rc().channel(rcmap.roll()-1);
     channel_pitch    = rc().channel(rcmap.pitch()-1);
     channel_throttle = rc().channel(rcmap.throttle()-1);
     channel_yaw      = rc().channel(rcmap.yaw()-1);
 
     // set rc channel ranges
     设置rc通道范围
     channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
     channel_pitch->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
     channel_yaw->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
     channel_throttle->set_range(1000);
 
     // set default dead zones
     default_dead_zones();
 
     // initialise throttle_zero flag
     ap.throttle_zero = true;
 }

3.init_rc_out()

比例因子的给定在init_rc_out()函数中完成的初始化

\ArduCopter\radio.cpp

 // init_rc_out -- initialise motors
void Copter::init_rc_out()
{
   
    motors->set_loop_rate(scheduler.get_loop_rate_hz());
    motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());

    // enable aux servos to cope with multiple output channels per motor
    SRV_Channels::enable_aux_servos();

    // update rate must be set after motors->init() to allow for motor mapping
    motors->set_update_rate(g.rc_speed);

#if FRAME_CONFIG != HELI_FRAME
    if (channel_throttle->configured_in_storage()) {
   
        // throttle inputs setup, use those to set motor PWM min and max if not already configured
        motors->convert_pwm_min_max_param(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
    } else {
   
        // throttle inputs default, force set motor PWM min and max to defaults so they will not be over-written by a future change in RC min / max
        motors->convert_pwm_min_max_param(1000, 2000);
    }
    motors->update_throttle_range();
#else
    // setup correct scaling for ESCs like the UAVCAN ESCs which
    // take a proportion of speed.
    hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
#endif

    // refresh auxiliary channel to function map
    SRV_Channels::update_aux_servo_function();

#if FRAME_CONFIG != HELI_FRAME
    /*
      setup a default safety ignore mask, so that servo gimbals can be active while safety is on
     */
    uint16_t safety_ignore_mask = (~copter.motors->get_motor_mask()) & 0x3FFF;
    BoardConfig.set_default_safety_ignore_mask(safety_ignore_mask);
#endif
}

三、电机初始化

1.MULTICOPTER_FRAME

\ArduCopter\config.h

// FRAME_CONFIG
//
#ifndef FRAME_CONFIG
 # define FRAME_CONFIG   MULTICOPTER_FRAME
#endif

2.AP_MotorsMulticopter

在Copter.h中定义的Copter类中，指明了其所使用的电机类型
\ArduCopter\Copter.h

#if FRAME_CONFIG == HELI_FRAME
 #define MOTOR_CLASS AP_MotorsHeli
#else
 #define MOTOR_CLASS AP_MotorsMulticopter
#endif

3.AP_MOTORS_MAX_NUM_MOTORS

\libraries\AP_Motors\AP_Motors_Class.h

#define AP_MOTORS_MAX_NUM_MOTORS 12

4.AP_MotorsMatrix

t\libraries\AP_Motors\AP_MotorsMatrix.h

class AP_MotorsMatrix : public AP_MotorsMulticopter {
   
}

ArduCopter的电机库配置位于libraries\AP_Motors路径下的AP_MotorsMulticopter.cpp/AP_MotorsMulticopter.h文件中。

5.scheduler_tasks

\ArduCopter\Copter.cpp

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
   
	...
	 SCHED_TASK(one_hz_loop,            1,    100,  81),
	...
}

6.void Copter::one_hz_loop()

\ArduCopter\Copter.cpp

// one_hz_loop - runs at 1Hz
void Copter::one_hz_loop()
{
   
    
	...
    if (!motors->armed()) {
   
       ...

        // check the user hasn't updated the frame class or type
        motors->set_frame_class_and_type((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
		...

    }

    ...
}

7. set_frame_class_and_type

\libraries\AP_Motors\AP_MotorsMatrix.cpp

// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
void AP_MotorsMatrix::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
{
   
    // exit immediately if armed or no change
    if (armed() || (frame_class == _active_frame_class && _active_frame_type == frame_type)) {
   
        return;
    }
    _active_frame_class = frame_class;
    _active_frame_type = frame_type;

    init(frame_class, frame_type);

}

8.AP_MotorsMatrix::init

配置电机初始化。

\libraries\AP_Motors\AP_MotorsMatrix.cpp

// init
void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
   
    // record requested frame class and type
    _active_frame_class = frame_class;
    _active_frame_type = frame_type;

    if (frame_class == MOTOR_FRAME_SCRIPTING_MATRIX) {
   
        // if Scripting frame class, do nothing scripting must call its own dedicated init function
        return;
    }

    // setup the motors
    setup_motors(frame_class, frame_type);

    // enable fast channels or instant pwm
    set_update_rate(_speed_hz);
}

9.setup_motors

这个函数最主要的内容就是配置电机，包括每个电机对于不同运动的影响程度（推力分配）。

(1)
函数刚开始，首先就是把最初的所有电机配置全部移除，方便后续进行更改。这里的AP_MOTORS_MAX_NUM_MOTORS为最大的电机数，于AP_Motors_Class.h中定义为12。

(2)
然后进入一个switch()函数中进行具体的配置内容，首先判断是属于哪一种frame_class的架构，ArduCopter这边给出了14种配置结构，定义于AP_Motors_Class.h中的枚举类型里，如下所示

(3)
以X型四旋翼进行说明：
add_motor()这个函数的作用就是配置每一个电机在某一运动功能上的影响因子，AP_MOTORS_MOT_1从右上角开始，逆时针进行编号增加。

\libraries\AP_Motors\AP_MotorsMatrix.cpp

void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
{
   
    // remove existing motors
    for (int8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
   
        remove_motor(i);
    }
    ...
    switch (frame_class) {
   
    ...
    	
    	case MOTOR_FRAME_QUAD:
    	switch (frame_type) {
   
    		...
   				case MOTOR_FRAME_TYPE_X: {
   
                    _frame_type_string = "X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
   
                        {
      45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        {
    -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
                        {
     -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
                        {
     135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
         ...
		}
    }
    // normalise factors to magnitude 0.5
    normalise_rpy_factors();
}

ArduCopter这边给出了14种配置结构，定义于AP_Motors_Class.h中的枚举类型里，如下所示

\libraries\AP_Motors\AP_Motors_Class.h

class AP_Motors {
   
public:

    enum motor_frame_class {
   
        MOTOR_FRAME_UNDEFINED = 0,
        MOTOR_FRAME_QUAD = 1,
        MOTOR_FRAME_HEXA = 2,
        MOTOR_FRAME_OCTA = 3,
        MOTOR_FRAME_OCTAQUAD = 4,
        MOTOR_FRAME_Y6 = 5,
        MOTOR_FRAME_HELI = 6,
        MOTOR_FRAME_TRI = 7,
        MOTOR_FRAME_SINGLE = 8,
        MOTOR_FRAME_COAX = 9,
        MOTOR_FRAME_TAILSITTER = 10,
        MOTOR_FRAME_HELI_DUAL = 11,
        MOTOR_FRAME_DODECAHEXA = 12,
        MOTOR_FRAME_HELI_QUAD = 13,
        MOTOR_FRAME_DECA = 14,
        MOTOR_FRAME_SCRIPTING_MATRIX = 15,
        MOTOR_FRAME_6DOF_SCRIPTING = 16,
        MOTOR_FRAME_DYNAMIC_SCRIPTING_MATRIX = 17,
    };
    ...
 }

根据不同的机身结构，frame_type定义了不同的机型类别。

\libraries\AP_Motors\AP_Motors_Class.h

class AP_Motors {
   
public:
	...
    	enum motor_frame_type {
   
        MOTOR_FRAME_TYPE_PLUS = 0,
        MOTOR_FRAME_TYPE_X = 1,
        MOTOR_FRAME_TYPE_V = 2,
        MOTOR_FRAME_TYPE_H = 3,
        MOTOR_FRAME_TYPE_VTAIL = 4,
        MOTOR_FRAME_TYPE_ATAIL = 5,
        MOTOR_FRAME_TYPE_PLUSREV = 6, // plus with reversed motor direction
        MOTOR_FRAME_TYPE_Y6B = 10,
        MOTOR_FRAME_TYPE_Y6F = 11, // for FireFlyY6
        MOTOR_FRAME_TYPE_BF_X = 12, // X frame, betaflight ordering
        MOTOR_FRAME_TYPE_DJI_X = 13, // X frame, DJI ordering
        MOTOR_FRAME_TYPE_CW_X = 14, // X frame, clockwise ordering
        MOTOR_FRAME_TYPE_I = 15, // (sideways H) octo only
        MOTOR_FRAME_TYPE_NYT_PLUS = 16, // plus frame, no differential torque for yaw
        MOTOR_FRAME_TYPE_NYT_X = 17, // X fr