Line data    Source code

       1             : #include "AP_Camera_Servo.h"
       2             : 
       3             : #if AP_CAMERA_SERVO_ENABLED
       4             : 
       5             : #include <SRV_Channel/SRV_Channel.h>
       6             : 
       7             : extern const AP_HAL::HAL& hal;
       8             : 
       9             : // initialize the AP_Camera_Servo driver
      10           5 : void AP_Camera_Servo::init()
      11             : {
      12             :     // call parent init to set default camera info (flags, focal length, etc.)
      13           5 :     AP_Camera_Backend::init();
      14             : 
      15             :     // set the zoom and focus to the trim point
      16           5 :     SRV_Channels::set_output_scaled(SRV_Channel::k_cam_zoom, 500);
      17           5 :     SRV_Channels::set_output_scaled(SRV_Channel::k_cam_focus, 500);
      18           5 : }
      19             : 
      20             : // update - should be called at 50hz
      21       35470 : void AP_Camera_Servo::update()
      22             : {
      23             :     // shutter counter
      24       35470 :     if (trigger_counter > 0) {
      25         245 :         trigger_counter--;
      26             :     } else {
      27       35225 :         SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _params.servo_off_pwm);
      28             :     }
      29             : 
      30             :     // iso counter
      31       35470 :     if (iso_counter > 0) {
      32           0 :         iso_counter--;
      33             :     } else {
      34       35470 :         SRV_Channels::set_output_pwm(SRV_Channel::k_cam_iso, _params.servo_off_pwm);
      35             :     }
      36       35470 :     float current_zoom = SRV_Channels::get_output_scaled(SRV_Channel::k_cam_zoom);
      37       35470 :     float new_zoom = constrain_float(current_zoom + zoom_current_rate, 0, 1000);
      38       35470 :     SRV_Channels::set_output_scaled(SRV_Channel::k_cam_zoom, new_zoom);
      39             : 
      40       35470 :     float current_focus = SRV_Channels::get_output_scaled(SRV_Channel::k_cam_focus);
      41       35470 :     float new_focus = constrain_float(current_focus + focus_current_rate, 0, 1000);
      42       35470 :     SRV_Channels::set_output_scaled(SRV_Channel::k_cam_focus, new_focus);
      43             : 
      44             :     // call parent update
      45       35470 :     AP_Camera_Backend::update();
      46       35470 : }
      47             : 
      48             : // entry point to actually take a picture.  returns true on success
      49          49 : bool AP_Camera_Servo::trigger_pic()
      50             : {
      51             :     // fail if have not completed previous picture
      52          49 :     if (trigger_counter > 0) {
      53           0 :         return false;
      54             :     }
      55             : 
      56          49 :     SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _params.servo_on_pwm);
      57             : 
      58             :     // set counter to move servo to off position after this many iterations of update (assumes 50hz update rate)
      59          49 :     trigger_counter = constrain_float(_params.trigger_duration * 50, 0, UINT16_MAX);
      60             : 
      61          49 :     return true;
      62             : }
      63             : 
      64             : 
      65           0 : bool AP_Camera_Servo::set_zoom(ZoomType zoom_type, float zoom_value)
      66             : {
      67           0 :     switch (zoom_type) {
      68           0 :         case ZoomType::RATE:
      69           0 :             zoom_current_rate = zoom_value;
      70           0 :             return true;
      71           0 :         case ZoomType::PCT:
      72             :             // expects to receive a value between 0 and 100
      73             :             // This maps it to our 0-1000 range
      74           0 :             SRV_Channels::set_output_scaled(SRV_Channel::k_cam_zoom, constrain_float(zoom_value * 10, 0, 1000));
      75           0 :             return true;
      76             :     }
      77           0 :     return false;
      78             : }
      79             : 
      80             : // set focus specified as rate
      81           0 : SetFocusResult AP_Camera_Servo::set_focus(FocusType focus_type, float focus_value)
      82             : {
      83           0 :     switch (focus_type) {
      84           0 :         case FocusType::RATE:
      85           0 :             focus_current_rate = focus_value;
      86           0 :             return SetFocusResult::ACCEPTED;
      87           0 :         case FocusType::PCT:
      88             :             // expects to receive a value between 0 and 100
      89             :             // This maps it to our 0-1000 range
      90           0 :             SRV_Channels::set_output_scaled(SRV_Channel::k_cam_focus, constrain_float(focus_value * 10, 0, 1000));
      91           0 :             return SetFocusResult::ACCEPTED;
      92           0 :         case FocusType::AUTO:
      93           0 :             return SetFocusResult::UNSUPPORTED;
      94             :     }
      95           0 :     return SetFocusResult::UNSUPPORTED;
      96             : }
      97             : 
      98             : // configure camera
      99           0 : void AP_Camera_Servo::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time)
     100             : {
     101             :     // designed to control Blackmagic Micro Cinema Camera (BMMCC) cameras
     102             :     // if the message contains non zero values then use them for the below functions
     103           0 :     if (ISO > 0) {
     104             :         // set a trigger for the iso function that is flip controlled
     105           0 :         iso_counter = constrain_float(_params.trigger_duration * 50, 0, UINT16_MAX);
     106           0 :         SRV_Channels::set_output_pwm(SRV_Channel::k_cam_iso, _params.servo_on_pwm);
     107             :     }
     108             : 
     109           0 :     if (aperture > 0) {
     110           0 :         SRV_Channels::set_output_pwm(SRV_Channel::k_cam_aperture, (uint16_t)aperture);
     111             :     }
     112             : 
     113           0 :     if (shutter_speed > 0) {
     114           0 :         SRV_Channels::set_output_pwm(SRV_Channel::k_cam_shutter_speed, (uint16_t)shutter_speed);
     115             :     }
     116             : 
     117             :     // Use the shooting mode PWM value for the BMMCC as the focus control - no need to modify or create a new MAVlink message type.
     118           0 :     if (shooting_mode > 0) {
     119           0 :         SRV_Channels::set_output_pwm(SRV_Channel::k_cam_focus, (uint16_t)shooting_mode);
     120             :     }
     121           0 : }
     122             : 
     123             : // send camera settings message to GCS
     124           0 : void AP_Camera_Servo::send_camera_settings(mavlink_channel_t chan) const
     125             : {
     126           0 :     mavlink_msg_camera_settings_send(
     127             :         chan,
     128             :         AP_HAL::millis(),   // time_boot_ms
     129             :         CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
     130           0 :         SRV_Channels::get_output_scaled(SRV_Channel::k_cam_zoom) / 10.0f,     // zoomLevel float, percentage from 0 to 100, 0 if unassigned
     131           0 :         SRV_Channels::get_output_scaled(SRV_Channel::k_cam_focus) / 10.0f);   // focusLevel float, percentage from 0 to 100, 0 if unassigned
     132           0 : }
     133             : 
     134             : #endif // AP_CAMERA_SERVO_ENABLED