ArduPilot开源飞控之AP_Mount

lida2003

已于 2024-07-12 08:16:24 修改

阅读量230

点赞数

分类专栏： ArduPilot 文章标签：开源 Ardupilot

于 2023-10-19 09:11:45 首次发布

本文链接：https://blog.csdn.net/lida2003/article/details/133919049

版权

ArduPilot 专栏收录该内容

147 篇文章 93 订阅

订阅专栏

ArduPilot开源飞控之AP_Mount

1. 源由
2. 框架设计
3. 重要例程
4. 总结
5. 参考资料

1. 源由

AP_Mount主要是挂的云台设备，配合相机使用。

当前云台也有各种硬件总线连接方式，并且软件协议也不尽相同，Ardupilot对这方面的支持还是不错的。

为了更好的选择云台，有必要从代码层面了解下目前支持的情况。

2. 框架设计

2.1 启动代码

传统Ardupilot启动方式。

Copter::init_ardupilot
 └──> AP_Mount::init

2.2 任务代码 update

命令状态更新。

SCHED_TASK_CLASS(AP_Mount,             &copter.camera_mount,        update,          50,  75, 108)
 └──> AP_Mount::update

2.3 任务代码 update_fast

云台姿态更新。

FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),
 └──> AP_Mount::update_fast

2.4 任务代码 handle_message

MMAVLink命令处理管道。

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102)
 └──> GCS::update_receive
     └──> GCS_MAVLINK::update_receive
         └──> GCS_MAVLINK_Copter::packetReceived
             └──> GCS_MAVLINK::packetReceived
                 └──> GCS_MAVLINK_Copter::handle_mount_message
                     └──> GCS_MAVLINK::handle_mount_message
                         └──> AP_Mount::handle_message

3. 重要例程

3.1 init

支持以下类型的云台：

AP_Mount_Servo
AP_Mount_SoloGimbal
AP_Mount_Alexmos
AP_Mount_SToRM32
AP_Mount_SToRM32_serial
AP_Mount_Gremsy
AP_Mount_Siyi
AP_Mount_Scripting
AP_Mount_Xacti
AP_Mount_Viewpro

// init - detect and initialise all mounts
void AP_Mount::init()
 │  // check init has not been called before
 ├──> <_num_instances != 0>
 │   └──> return
 │
 │  // perform any required parameter conversion
 ├──> convert_params()
 │
 │  // primary is reset to the first instantiated mount
 ├──> bool primary_set = false
 │
 │  // create mount instance
 ├──> <for (uint8_t instance=0 instance<AP_MOUNT_MAX_INSTANCES instance++)>
 │   ├──> <case Type::Servo> <HAL_MOUNT_SERVO_ENABLED>
 │   │   ├──> _backends[instance] = new AP_Mount_Servo(*this, _params[instance], true, instance)
 │   │   ├──> _num_instances++
 │   │   └──> break
 │   ├──> <case Type::SoloGimbal> <HAL_SOLO_GIMBAL_ENABLED>
 │   │   ├──> _backends[instance] = new AP_Mount_SoloGimbal(*this, _params[instance], instance)
 │   │   ├──> _num_instances++
 │   │   └──> break
 │   ├──> <case Type::Alexmos> <HAL_MOUNT_ALEXMOS_ENABLED>
 │   │   ├──> _backends[instance] = new AP_Mount_Alexmos(*this, _params[instance], instance)
 │   │   ├──> _num_instances++
 │   │   └──> break
 │   ├──> <case Type::SToRM32> <HAL_MOUNT_STORM32MAVLINK_ENABLED>  // check for SToRM32 mounts using MAVLink protocol
 │   │   ├──> _backends[instance] = new AP_Mount_SToRM32(*this, _params[instance], instance)
 │   │   ├──> _num_instances++
 │   │   └──> break
 │   ├──> <case Type::SToRM32_serial> <HAL_MOUNT_STORM32SERIAL_ENABLED>  // check for SToRM32 mounts using serial protocol
 │   │   ├──> _backends[instance] = new AP_Mount_SToRM32_serial(*this, _params[instance], instance)
 │   │   ├──> _num_instances++
 │   │   └──> break
 │   ├──> <case Type::Gremsy> <HAL_MOUNT_GREMSY_ENABLED> // check for Gremsy mounts
 │   │   ├──> _backends[instance] = new AP_Mount_Gremsy(*this, _params[instance], instance)
 │   │   ├──> _num_instances++
 │   │   └──> break
 │   ├──> <case Type::BrushlessPWM> <HAL_MOUNT_SERVO_ENABLED> // check for BrushlessPWM mounts (uses Servo backend)
 │   │   ├──> _backends[instance] = new AP_Mount_Servo(*this, _params[instance], false, instance)
 │   │   ├──> _num_instances++
 │   │   └──> break
 │   ├──> <case Type::Siyi> <HAL_MOUNT_SIYI_ENABLED> // check for Siyi gimbal
 │   │   ├──> _backends[instance] = new AP_Mount_Siyi(*this, _params[instance], instance)
 │   │   ├──> _num_instances++
 │   │   └──> break
 │   ├──> <case Type::Scripting> <HAL_MOUNT_SCRIPTING_ENABLED> // check for Scripting gimbal
 │   │   ├──> _backends[instance] = new AP_Mount_Scripting(*this, _params[instance], instance)
 │   │   ├──> _num_instances++
 │   │   └──> break
 │   ├──> <case Type::Xacti> <HAL_MOUNT_XACTI_ENABLED> // check for Xacti gimbal
 │   │   ├──> _backends[instance] = new AP_Mount_Xacti(*this, _params[instance], instance)
 │   │   ├──> _num_instances++
 │   │   └──> break
 │   ├──> <case Type::Viewpro> <HAL_MOUNT_VIEWPRO_ENABLED> // check for Xacti gimbal
 │   │   ├──> _backends[instance] = new AP_Mount_Viewpro(*this, _params[instance], instance)
 │   │   ├──> _num_instances++
 │   │   └──> break
 │   │
 │   │  // init new instance
 │   └──> <_backends[instance] != nullptr> <!primary_set>
 │       ├──> _primary = instance
 │       └──> primary_set = true
 │
 │  // init each instance, do it after all instances were created, so that they all know things
 └──> <for (uint8_t instance=0 instance<AP_MOUNT_MAX_INSTANCES instance++)>
     └──> <_backends[instance] != nullptr>
         ├──> _backends[instance]->init()
         └──> set_mode_to_default(instance)

3.2 update

直接调用驱动更新云台状态。

// update - give mount opportunity to update servos.  should be called at 10hz or higher
AP_Mount::update
 │  // update each instance
 └──> <for (uint8_t instance=0 instance<AP_MOUNT_MAX_INSTANCES instance++)>
     └──> <_backends[instance] != nullptr>
         └──> _backends[instance]->update()

3.3 update_fast

直接调用驱动更新云台姿态。

// used for gimbals that need to read INS data at full rate
void AP_Mount::update_fast()
 │  // update each instance
 └──> <for (uint8_t instance=0 instance<AP_MOUNT_MAX_INSTANCES instance++)>
     └──> <_backends[instance] != nullptr>
         └──> _backends[instance]->update_fast()

3.4 handle_message

支持MAVLink命令：

MAVLINK_MSG_ID_GIMBAL_REPORT
MAVLINK_MSG_ID_MOUNT_CONFIGURE
MAVLINK_MSG_ID_MOUNT_CONTROL
MAVLINK_MSG_ID_GLOBAL_POSITION_INT
MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE
MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW
MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION
MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS

AP_Mount::handle_message
 ├──> <case MAVLINK_MSG_ID_GIMBAL_REPORT>
 │   ├──> handle_gimbal_report(chan, msg)
 │   └──> break
 ├──> <case MAVLINK_MSG_ID_MOUNT_CONFIGURE> <AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED>
 │   ├──> handle_mount_configure(msg)
 │   └──> break
 ├──> <case MAVLINK_MSG_ID_MOUNT_CONTROL> <AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED>
 │   ├──> handle_mount_control(msg)
 │   └──> break
 ├──> <case MAVLINK_MSG_ID_GLOBAL_POSITION_INT>
 │   ├──> handle_global_position_int(msg)
 │   └──> break
 ├──> <case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE>
 │   ├──> handle_gimbal_manager_set_attitude(msg)
 │   └──> break
 ├──> <case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW>
 │   ├──> handle_command_gimbal_manager_set_pitchyaw(msg)
 │   └──> break
 ├──> <case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION>
 │   ├──> handle_gimbal_device_information(msg)
 │   └──> break
 ├──> <case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS>
 │   ├──> handle_gimbal_device_attitude_status(msg)
 │   └──> break
 └──> <default> <HAL_BOARD_SITL>
     ├──> AP_HAL::panic("Unhandled mount case")
     └──> break