ArduPilot开源飞控之AP_Camera
ArduPilot开源飞控之AP_Camera
- 1. 源由
- 2. 框架设计
-
- 2.1 启动代码
- 2.2 任务代码 update
- 2.3 任务代码 handle_message
- 2.4 任务代码 handle_command_long
- 3. 重要例程
-
- 3.1 init
- 3.2 update
- 3.3 handle_message
- 3.4 handle_command_long
- 4 总结
- 5. 参考资料
1. 源由
通常民用无人机上有两种类型的摄像头：
- FPV摄像头
- 云台摄像头
这里主要对象是云台摄像头的操控。
2. 框架设计
2.1 启动代码
对设备的初始化流程。
Copter::init_ardupilot
└──> AP_Camera::init
2.2 任务代码 update
SCHED_TASK_CLASS(AP_Camera, &copter.camera, update, 50, 75, 111)
└──> AP_Camera::update
2.3 任务代码 handle_message
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180, 102)
└──> GCS::update_receive
└──> GCS_MAVLINK::update_receive
└──> GCS_MAVLINK::packetReceived
└──> GCS_MAVLINK_Copter::handleMessage
└──> GCS_MAVLINK::handle_common_message
└──> AP_Camera::handle_message
2.4 任务代码 handle_command_long
目前最新代码没有找到相关的调用关系，高概率废弃。
???? //Dead End
└──> GCS_MAVLINK_Copter::handle_command_long_packet/GCS_MAVLINK::handle_command_long_packet
└──> GCS_MAVLINK::handle_command_camera
└──> AP_Camera::handle_command_long
3. 重要例程
3.1 init
根据配置情况，选择正确的驱动方式进行驱动初始化。
- AP_Camera_Servo
- AP_Camera_Relay
- AP_Camera_SoloGimbal
- AP_Camera_Mount
- AP_Camera_MAVLink
- AP_Camera_MAVLinkCamV2
- AP_Camera_Scripting
// detect and initialise backends
AP_Camera::init
│
│ // check init has not been called before
├──>
│ └──> return
│
│ // perform any required parameter conversion
├──> convert_params()
│
│ // create camera instance
├──>
│ └──> backends[instance] = new AP_Camera_Servo(*this, _params[instance], instance)
├──>
│ └──> __backends[instance] = new AP_Camera_Relay(*this, _params[instance], instance)
├──> // check for GoPro in Solo camera
│ └──> _backends[instance] = new AP_Camera_SoloGimbal(*this, _params[instance], instance)
├──> // check for Mount camera
│ └──> _backends[instance] = new AP_Camera_Mount(*this, _params[instance], instance)
├──> // check for MAVLink enabled camera driver
│ └──> _backends[instance] = new AP_Camera_MAVLink(*this, _params[instance], instance)
├──> // check for MAVLink Camv2 driver
│ └──> _backends[instance] = new AP_Camera_MAVLinkCamV2(*this, _params[instance], instance)
├──> // check for Scripting driver
│ └──> _backends[instance] = new AP_Camera_Scripting(*this, _params[instance], instance)
│
│ // set primary to first non-null instance
├──>
│ └──> primary = _backends[instance]
│
│ // init each instance, do it after all instances were created, so that they all know things
└──>
└──> <_backends[instance] != nullptr>
└──> _backends[instance]->init()
3.2 update
定时状态更新。
/*
update triggers by distance moved and camera trigger
*/
AP_Camera::update
├──> WITH_SEMAPHORE(_rsem)
│
│ // call each instance
└──>
└──> <_backends[instance] != nullptr>
└──> _backends[instance]->update()
3.3 handle_message
GCSMavlink命令操作。
// handle incoming mavlink messages
AP_Camera::handle_message
├──> WITH_SEMAPHORE(_rsem);
├──> // decode deprecated MavLink message that controls camera.
│ ├──> __mavlink_digicam_control_t packet;
│ ├──> mavlink_msg_digicam_control_decode(&msg, &packet);
│ ├──> control(packet.session, packet.zoom_pos, packet.zoom_step, packet.focus_lock, packet.shot, packet.command_id);
│ └──> return;
└──> // call each instance
└──> <_backends[instance] != nullptr> _backends[instance]->handle_message(chan, msg);
3.4 handle_command_long
注：高概率废弃代码，笔者没有找到相应的调用关系，如所述有误或者发现，也请联系我，谢谢！
// handle command_long mavlink messages
MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet)
{
switch (packet.command) {
case MAV_CMD_DO_DIGICAM_CONFIGURE:
configure(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6, packet.param7);
return MAV_RESULT_ACCEPTED;
case MAV_CMD_DO_DIGICAM_CONTROL:
control(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6);
return MAV_RESULT_ACCEPTED;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
set_trigger_distance(packet.param1);
if (is_equal(packet.param3, 1.0f)) {
take_picture();
}
return MAV_RESULT_ACCEPTED;
case MAV_CMD_SET_CAMERA_ZOOM:
if (is_equal(packet.param1, (float)ZOOM_TYPE_CONTINUOUS) &&
set_zoom(ZoomType::RATE, packet.param2)) {
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param1, (float)ZOOM_TYPE_RANGE) &&
set_zoom(ZoomType::PCT, packet.param2)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_UNSUPPORTED;
case MAV_CMD_SET_CAMERA_FOCUS:
// accept any of the auto focus types
switch ((SET_FOCUS_TYPE)packet.param1) {
case FOCUS_TYPE_AUTO:
case FOCUS_TYPE_AUTO_SINGLE:
case FOCUS_TYPE_AUTO_CONTINUOUS:
return (MAV_RESULT)set_focus(FocusType::AUTO, 0);
case FOCUS_TYPE_CONTINUOUS:
// accept continuous manual focus
return (MAV_RESULT)set_focus(FocusType::RATE, packet.param2);
// accept focus as percentage
case FOCUS_TYPE_RANGE:
return (MAV_RESULT)set_focus(FocusType::PCT, packet.param2);
case SET_FOCUS_TYPE_ENUM_END:
case FOCUS_TYPE_STEP:
case FOCUS_TYPE_METERS:
// unsupported focus (bad parameter)
break;
}
return MAV_RESULT_DENIED;
case MAV_CMD_IMAGE_START_CAPTURE:
// param1 : camera id
// param2 : interval (in seconds)
// param3 : total num images
// sanity check instance
if (is_negative(packet.param1)) {
return MAV_RESULT_UNSUPPORTED;
}
// check if this is a single picture request (e.g. total images is 1 or interval and total images are zero)
if (is_equal(packet.param3, 1.0f) ||
(is_zero(packet.param2) && is_zero(packet.param3))) {
if (is_zero(packet.param1)) {
// take pictures for every backend
return take_picture() ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
}
// take picture for specified instance
return take_picture(packet.param1-1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
} else if (is_zero(packet.param3)) {
// multiple picture request, take pictures forever
if (is_zero(packet.param1)) {
// take pictures for every backend
return take_multiple_pictures(packet.param2*1000, -1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
}
return take_multiple_pictures(packet.param1-1, packet.param2*1000, -1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
} else {
// take multiple pictures equal to the number specified in param3
if (is_zero(packet.param1)) {
// take pictures for every backend
return take_multiple_pictures(packet.param2*1000, packet.param3) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
}
return take_multiple_pictures(packet.param1-1, packet.param2*1000, packet.param3) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
}
case MAV_CMD_IMAGE_STOP_CAPTURE:
// param1 : camera id
if (is_negative(packet.param1)) {
return MAV_RESULT_UNSUPPORTED;
}
if (is_zero(packet.param1)) {
// stop capture for every backend
stop_capture();
return MAV_RESULT_ACCEPTED;
}
if (stop_capture(packet.param1-1)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_UNSUPPORTED;
case MAV_CMD_CAMERA_TRACK_POINT:
if (set_tracking(TrackingType::TRK_POINT, Vector2f{packet.param1, packet.param2}, Vector2f{})) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_UNSUPPORTED;
case MAV_CMD_CAMERA_TRACK_RECTANGLE:
if (set_tracking(TrackingType::TRK_RECTANGLE, Vector2f{packet.param1, packet.param2}, Vector2f{packet.param3, packet.param4})) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_UNSUPPORTED;
case MAV_CMD_CAMERA_STOP_TRACKING:
if (set_tracking(TrackingType::TRK_NONE, Vector2f{}, Vector2f{})) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_UNSUPPORTED;
case MAV_CMD_VIDEO_START_CAPTURE:
case MAV_CMD_VIDEO_STOP_CAPTURE:
{
bool success = false;
const bool start_recording = (packet.command == MAV_CMD_VIDEO_START_CAPTURE);
const uint8_t stream_id = packet.param1; // Stream ID
if (stream_id == 0) {
// stream id of 0 interpreted as primary camera
success = record_video(start_recording);
} else {
// convert stream id to instance id
success = record_video(stream_id - 1, start_recording);
}
if (success) {
return MAV_RESULT_ACCEPTED;
} else {
return MAV_RESULT_FAILED;
}
}
default:
return MAV_RESULT_UNSUPPORTED;
}
}
4 总结
在理解以下设计思路的基础上，看这个代码框架和逻辑思路，对于云台控制的方式还是比较容易理解的。
- ArduPilot之开源代码Library&Sketches设计
- ArduPilot之开源代码Sensor Drivers设计
说的更加直接一点，就是解析MAVLink命令，通过驱动执行相应的动作。
5. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计