ArduPilot开源飞控之AP_Mount
ArduPilot开源飞控之AP_Mount
- 1. 源由
- 2. 框架设计
-
- 2.1 启动代码
- 2.2 任务代码 update
- 2.3 任务代码 update_fast
- 2.4 任务代码 handle_message
- 3. 重要例程
-
- 3.1 init
- 3.2 update
- 3.3 update_fast
- 3.4 handle_message
- 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
├──>
│ ├──>
│ │ ├──> _backends[instance] = new AP_Mount_Servo(*this, _params[instance], true, instance)
│ │ ├──> _num_instances++
│ │ └──> break
│ ├──>
│ │ ├──> _backends[instance] = new AP_Mount_SoloGimbal(*this, _params[instance], instance)
│ │ ├──> _num_instances++
│ │ └──> break
│ ├──>
│ │ ├──> _backends[instance] = new AP_Mount_Alexmos(*this, _params[instance], instance)
│ │ ├──> _num_instances++
│ │ └──> break
│ ├──> // check for SToRM32 mounts using MAVLink protocol
│ │ ├──> _backends[instance] = new AP_Mount_SToRM32(*this, _params[instance], instance)
│ │ ├──> _num_instances++
│ │ └──> break
│ ├──> // check for SToRM32 mounts using serial protocol
│ │ ├──> _backends[instance] = new AP_Mount_SToRM32_serial(*this, _params[instance], instance)
│ │ ├──> _num_instances++
│ │ └──> break
│ ├──> // check for Gremsy mounts
│ │ ├──> _backends[instance] = new AP_Mount_Gremsy(*this, _params[instance], instance)
│ │ ├──> _num_instances++
│ │ └──> break
│ ├──> // check for BrushlessPWM mounts (uses Servo backend)
│ │ ├──> _backends[instance] = new AP_Mount_Servo(*this, _params[instance], false, instance)
│ │ ├──> _num_instances++
│ │ └──> break
│ ├──> // check for Siyi gimbal
│ │ ├──> _backends[instance] = new AP_Mount_Siyi(*this, _params[instance], instance)
│ │ ├──> _num_instances++
│ │ └──> break
│ ├──> // check for Scripting gimbal
│ │ ├──> _backends[instance] = new AP_Mount_Scripting(*this, _params[instance], instance)
│ │ ├──> _num_instances++
│ │ └──> break
│ ├──> // check for Xacti gimbal
│ │ ├──> _backends[instance] = new AP_Mount_Xacti(*this, _params[instance], instance)
│ │ ├──> _num_instances++
│ │ └──> break
│ ├──> // check for Xacti gimbal
│ │ ├──> _backends[instance] = new AP_Mount_Viewpro(*this, _params[instance], instance)
│ │ ├──> _num_instances++
│ │ └──> break
│ │
│ │ // init new instance
│ └──> <_backends[instance] != nullptr>
│ ├──> _primary = instance
│ └──> primary_set = true
│
│ // init each instance, do it after all instances were created, so that they all know things
└──>
└──> <_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
└──>
└──> <_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
└──>
└──> <_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
├──>
│ ├──> handle_gimbal_report(chan, msg)
│ └──> break
├──>
│ ├──> handle_mount_configure(msg)
│ └──> break
├──>
│ ├──> handle_mount_control(msg)
│ └──> break
├──>
│ ├──> handle_global_position_int(msg)
│ └──> break
├──>
│ ├──> handle_gimbal_manager_set_attitude(msg)
│ └──> break
├──>
│ ├──> handle_command_gimbal_manager_set_pitchyaw(msg)
│ └──> break
├──>
│ ├──> handle_gimbal_device_information(msg)
│ └──> break
├──>
│ ├──> handle_gimbal_device_attitude_status(msg)
│ └──> break
└──>
├──> AP_HAL::panic("Unhandled mount case")
└──> break
4. 总结
根据AP_Mount的代码实现上，总体可以归纳以下几个方面：
- 硬件总线
- MAVLink命令分类
- 驱动协议格式
因硬件的差异，需要根据代码支持情况进行选择，以便Ardupilot对云台的兼容性支持。
5. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计