ArduPilot开源飞控之AP

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
 ├──> <primary != nullptr>
 │   └──> return
 │
 │  // perform any required parameter conversion
 ├──> convert_params()
 │
 │  // create camera instance
 ├──> <case CameraType::SERVO> <AP_CAMERA_SERVO_ENABLED>
 │   └──> backends[instance] = new AP_Camera_Servo(*this, _params[instance], instance)
 ├──> <case CameraType::RELAY> <AP_CAMERA_RELAY_ENABLED>
 │   └──> __backends[instance] = new AP_Camera_Relay(*this, _params[instance], instance)   
 ├──> <case CameraType::SOLOGIMBAL> <AP_CAMERA_SOLOGIMBAL_ENABLED>  // check for GoPro in Solo camera
 │   └──> _backends[instance] = new AP_Camera_SoloGimbal(*this, _params[instance], instance)
 ├──> <case CameraType::MOUNT> <AP_CAMERA_MOUNT_ENABLED> // check for Mount camera
 │   └──> _backends[instance] = new AP_Camera_Mount(*this, _params[instance], instance)
 ├──> <case CameraType::MAVLINK> <AP_CAMERA_MAVLINK_ENABLED> // check for MAVLink enabled camera driver
 │   └──> _backends[instance] = new AP_Camera_MAVLink(*this, _params[instance], instance)
 ├──> <case CameraType::MAVLINK_CAMV2> <AP_CAMERA_MAVLINKCAMV2_ENABLED>  // check for MAVLink Camv2 driver
 │   └──> _backends[instance] = new AP_Camera_MAVLinkCamV2(*this, _params[instance], instance)
 ├──> <case CameraType::SCRIPTING> <AP_CAMERA_SCRIPTING_ENABLED>  // check for Scripting driver
 │   └──> _backends[instance] = new AP_Camera_Scripting(*this, _params[instance], instance)
 │
 │  // set primary to first non-null instance
 ├──> <primary == nullptr>
 │   └──> primary = _backends[instance]
 │
 │  // init each instance, do it after all instances were created, so that they all know things
 └──> <for (uint8_t instance = 0 instance < AP_CAMERA_MAX_INSTANCES instance++)>
     └──> <_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
 └──> <for (uint8_t instance = 0 instance < AP_CAMERA_MAX_INSTANCES instance++)>
     └──> <_backends[instance] != nullptr>
         └──> _backends[instance]->update()

3.3 handle_message

GCSMavlink命令操作。

// handle incoming mavlink messages
AP_Camera::handle_message
 ├──> WITH_SEMAPHORE(_rsem);
 ├──> <msg.msgid == MAVLINK_MSG_ID_DIGICAM_CONTROL>  // 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;
 └──> <for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) > // 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;
    }
}