mirror of https://github.com/ArduPilot/ardupilot
72 lines
2.2 KiB
C++
72 lines
2.2 KiB
C++
#include "AP_Camera_Servo.h"
|
|
|
|
#if AP_CAMERA_SERVO_ENABLED
|
|
|
|
#include <SRV_Channel/SRV_Channel.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
// update - should be called at 50hz
|
|
void AP_Camera_Servo::update()
|
|
{
|
|
// shutter counter
|
|
if (trigger_counter > 0) {
|
|
trigger_counter--;
|
|
} else {
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _params.servo_off_pwm);
|
|
}
|
|
|
|
// iso counter
|
|
if (iso_counter > 0) {
|
|
iso_counter--;
|
|
} else {
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_iso, _params.servo_off_pwm);
|
|
}
|
|
|
|
// call parent update
|
|
AP_Camera_Backend::update();
|
|
}
|
|
|
|
// entry point to actually take a picture. returns true on success
|
|
bool AP_Camera_Servo::trigger_pic()
|
|
{
|
|
// fail if have not completed previous picture
|
|
if (trigger_counter > 0) {
|
|
return false;
|
|
}
|
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _params.servo_on_pwm);
|
|
|
|
// set counter to move servo to off position after this many iterations of update (assumes 50hz update rate)
|
|
trigger_counter = constrain_float(_params.trigger_duration * 50, 0, UINT16_MAX);
|
|
|
|
return true;
|
|
}
|
|
|
|
// configure camera
|
|
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)
|
|
{
|
|
// designed to control Blackmagic Micro Cinema Camera (BMMCC) cameras
|
|
// if the message contains non zero values then use them for the below functions
|
|
if (ISO > 0) {
|
|
// set a trigger for the iso function that is flip controlled
|
|
iso_counter = constrain_float(_params.trigger_duration * 50, 0, UINT16_MAX);
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_iso, _params.servo_on_pwm);
|
|
}
|
|
|
|
if (aperture > 0) {
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_aperture, (uint16_t)aperture);
|
|
}
|
|
|
|
if (shutter_speed > 0) {
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_shutter_speed, (uint16_t)shutter_speed);
|
|
}
|
|
|
|
// Use the shooting mode PWM value for the BMMCC as the focus control - no need to modify or create a new MAVlink message type.
|
|
if (shooting_mode > 0) {
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_focus, (uint16_t)shooting_mode);
|
|
}
|
|
}
|
|
|
|
#endif // AP_CAMERA_SERVO_ENABLED
|