Line data    Source code

       1             : #include "AP_Camera_MAVLink.h"
       2             : 
       3             : #if AP_CAMERA_MAVLINK_ENABLED
       4             : #include <GCS_MAVLink/GCS.h>
       5             : 
       6             : extern const AP_HAL::HAL& hal;
       7             : 
       8             : // entry point to actually take a picture.  returns true on success
       9           0 : bool AP_Camera_MAVLink::trigger_pic()
      10             : {
      11             :     // tell all of our components to take a picture:
      12           0 :     mavlink_command_long_t cmd_msg {};
      13           0 :     cmd_msg.command = MAV_CMD_DO_DIGICAM_CONTROL;
      14           0 :     cmd_msg.param5 = 1;
      15             : 
      16             :     // forward to all components
      17           0 :     GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&cmd_msg, sizeof(cmd_msg));
      18           0 :     return true;
      19             : }
      20             : 
      21             : // configure camera
      22           2 : void AP_Camera_MAVLink::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time)
      23             : {
      24             :     // convert to mavlink message and send to all components
      25           2 :     mavlink_command_long_t mav_cmd_long = {};
      26             : 
      27             :     // convert mission command to mavlink command_long
      28           2 :     mav_cmd_long.command = MAV_CMD_DO_DIGICAM_CONFIGURE;
      29           2 :     mav_cmd_long.param1 = shooting_mode;
      30           2 :     mav_cmd_long.param2 = shutter_speed;
      31           2 :     mav_cmd_long.param3 = aperture;
      32           2 :     mav_cmd_long.param4 = ISO;
      33           2 :     mav_cmd_long.param5 = float(exposure_type);
      34           2 :     mav_cmd_long.param6 = float(cmd_id);
      35           2 :     mav_cmd_long.param7 = engine_cutoff_time;
      36             : 
      37             :     // send to all components
      38           2 :     GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&mav_cmd_long, sizeof(mav_cmd_long));
      39           2 : }
      40             : 
      41             : // handle camera control message
      42           3 : void AP_Camera_MAVLink::control(float session, float zoom_pos, float zoom_step, float focus_lock, int32_t shooting_cmd, int32_t cmd_id)
      43             : {
      44             :     // take picture and ignore other arguments
      45           3 :     if (shooting_cmd == 1) {
      46           0 :         take_picture();
      47           0 :         return;
      48             :     }
      49             : 
      50             :     // convert command to mavlink command long
      51           3 :     mavlink_command_long_t mav_cmd_long = {};
      52           3 :     mav_cmd_long.command = MAV_CMD_DO_DIGICAM_CONTROL;
      53           3 :     mav_cmd_long.param1 = session;
      54           3 :     mav_cmd_long.param2 = zoom_pos;
      55           3 :     mav_cmd_long.param3 = zoom_step;
      56           3 :     mav_cmd_long.param4 = focus_lock;
      57           3 :     mav_cmd_long.param5 = float(shooting_cmd);
      58           3 :     mav_cmd_long.param6 = float(cmd_id);
      59             : 
      60             :     // send to all components
      61           3 :     GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&mav_cmd_long, sizeof(mav_cmd_long));
      62             : }
      63             : 
      64             : #endif // AP_CAMERA_MAVLINK_ENABLED