mirror of https://github.com/ArduPilot/ardupilot
208 lines
7.0 KiB
C++
208 lines
7.0 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
|
|
|
#include <AP_Camera.h>
|
|
#include <AP_Relay.h>
|
|
#include <../RC_Channel/RC_Channel_aux.h>
|
|
|
|
extern int32_t wp_distance; // Note: unfortunately this variable is in meter for ArduPlane and cm for ArduCopter
|
|
extern AP_Relay relay;
|
|
|
|
// ------------------------------
|
|
#define CAM_DEBUG DISABLED
|
|
|
|
const AP_Param::GroupInfo AP_Camera::var_info[] PROGMEM = {
|
|
// @Param: TRIGG_TYPE
|
|
// @DisplayName: Camera shutter (trigger) type
|
|
// @Description: how to trigger the camera to take a picture
|
|
// @Values: 0:Servo,1:Relay,2:Servo and turn off throttle,3:Servo when 3m from waypoint,4:transistor
|
|
// @User: Standard
|
|
AP_GROUPINFO("TRIGG_TYPE", 0, AP_Camera, _trigger_type, AP_CAMERA_TRIGGER_DEFAULT_TRIGGER_TYPE),
|
|
|
|
// @Param: DURATION
|
|
// @DisplayName: Duration that shutter is held open
|
|
// @Description: How long the shutter will be held open in 10ths of a second (i.e. enter 10 for 1second, 50 for 5seconds)
|
|
// @Range: 0 50
|
|
// @User: Standard
|
|
AP_GROUPINFO("DURATION", 1, AP_Camera, _trigger_duration, AP_CAMERA_TRIGGER_DEFAULT_DURATION),
|
|
|
|
// @Param: SERVO_ON
|
|
// @DisplayName: Servo ON PWM value
|
|
// @Description: PWM value to move servo to when shutter is activated
|
|
// @Range: 1000 2000
|
|
// @User: Standard
|
|
AP_GROUPINFO("SERVO_ON", 2, AP_Camera, _servo_on_pwm, AP_CAMERA_SERVO_ON_PWM),
|
|
|
|
// @Param: SERVO_OFF
|
|
// @DisplayName: Servo OFF PWM value
|
|
// @Description: PWM value to move servo to when shutter is deactivated
|
|
// @Range: 1000 2000
|
|
// @User: Standard
|
|
AP_GROUPINFO("SERVO_OFF", 3, AP_Camera, _servo_off_pwm, AP_CAMERA_SERVO_OFF_PWM),
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
|
|
/// Servo operated camera
|
|
void
|
|
AP_Camera::servo_pic()
|
|
{
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_cam_trigger, _servo_on_pwm);
|
|
|
|
// leave a message that it should be active for this many loops (assumes 50hz loops)
|
|
_trigger_counter = constrain(_trigger_duration*5,0,255);
|
|
}
|
|
|
|
/// basic relay activation
|
|
void
|
|
AP_Camera::relay_pic()
|
|
{
|
|
relay.on();
|
|
|
|
// leave a message that it should be active for this many loops (assumes 50hz loops)
|
|
_trigger_counter = constrain(_trigger_duration*5,0,255);
|
|
}
|
|
|
|
/// pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle.
|
|
void
|
|
AP_Camera::throttle_pic()
|
|
{
|
|
// TODO find a way to do this without using the global parameter g
|
|
// g.channel_throttle.radio_out = g.throttle_min;
|
|
if (_thr_pic_counter == 10) {
|
|
servo_pic(); // triggering method
|
|
_thr_pic_counter = 0;
|
|
// g.channel_throttle.radio_out = g.throttle_cruise;
|
|
}
|
|
_thr_pic_counter++;
|
|
}
|
|
|
|
/// distance_pic - triggers picture when within 3m of waypoint
|
|
void
|
|
AP_Camera::distance_pic()
|
|
{
|
|
if (wp_distance < AP_CAMERA_WP_DISTANCE) {
|
|
servo_pic(); // triggering method
|
|
}
|
|
}
|
|
|
|
/// hacked the circuit to run a transistor? use this trigger to send output.
|
|
void
|
|
AP_Camera::transistor_pic()
|
|
{
|
|
// TODO: Assign pin spare pin for output
|
|
digitalWrite(AP_CAMERA_TRANSISTOR_PIN, HIGH);
|
|
|
|
// leave a message that it should be active for two event loop cycles
|
|
_trigger_counter = 1;
|
|
}
|
|
|
|
/// single entry point to take pictures
|
|
void
|
|
AP_Camera::trigger_pic()
|
|
{
|
|
switch (_trigger_type)
|
|
{
|
|
case AP_CAMERA_TRIGGER_TYPE_SERVO:
|
|
servo_pic(); // Servo operated camera
|
|
break;
|
|
case AP_CAMERA_TRIGGER_TYPE_RELAY:
|
|
relay_pic(); // basic relay activation
|
|
break;
|
|
case AP_CAMERA_TRIGGER_TYPE_THROTTLE_OFF_TIME:
|
|
throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle.
|
|
break;
|
|
case AP_CAMERA_TRIGGER_TYPE_WP_DISTANCE:
|
|
distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle.
|
|
break;
|
|
case AP_CAMERA_TRIGGER_TYPE_TRANSISTOR:
|
|
transistor_pic(); // hacked the circuit to run a transistor? use this trigger to send output.
|
|
break;
|
|
}
|
|
}
|
|
|
|
/// de-activate the trigger after some delay, but without using a delay() function
|
|
/// should be called at 50hz
|
|
void
|
|
AP_Camera::trigger_pic_cleanup()
|
|
{
|
|
if (_trigger_counter) {
|
|
_trigger_counter--;
|
|
} else {
|
|
switch (_trigger_type) {
|
|
case AP_CAMERA_TRIGGER_TYPE_SERVO:
|
|
case AP_CAMERA_TRIGGER_TYPE_THROTTLE_OFF_TIME:
|
|
case AP_CAMERA_TRIGGER_TYPE_WP_DISTANCE:
|
|
RC_Channel_aux::set_radio(RC_Channel_aux::k_cam_trigger, _servo_off_pwm);
|
|
break;
|
|
case AP_CAMERA_TRIGGER_TYPE_RELAY:
|
|
relay.off();
|
|
break;
|
|
case AP_CAMERA_TRIGGER_TYPE_TRANSISTOR:
|
|
digitalWrite(AP_CAMERA_TRANSISTOR_PIN, LOW);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// decode MavLink that configures camera
|
|
void
|
|
AP_Camera::configure_msg(mavlink_message_t* msg)
|
|
{
|
|
__mavlink_digicam_configure_t packet;
|
|
mavlink_msg_digicam_configure_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system, packet.target_component)) {
|
|
// not for us
|
|
return;
|
|
}
|
|
// TODO do something with these values
|
|
/*
|
|
* packet.aperture
|
|
* packet.command_id
|
|
* packet.engine_cut_off
|
|
* packet.exposure_type
|
|
* packet.extra_param
|
|
* packet.extra_value
|
|
* packet.iso
|
|
* packet.mode
|
|
* packet.shutter_speed
|
|
*/
|
|
// echo the message to the ArduCam OSD camera control board
|
|
// for more info see: http://code.google.com/p/arducam-osd/
|
|
// TODO is it connected to MAVLINK_COMM_3 ?
|
|
mavlink_msg_digicam_configure_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.mode, packet.shutter_speed, packet.aperture, packet.iso, packet.exposure_type, packet.command_id, packet.engine_cut_off, packet.extra_param, packet.extra_value);
|
|
}
|
|
|
|
/// decode MavLink that controls camera
|
|
void
|
|
AP_Camera::control_msg(mavlink_message_t* msg)
|
|
{
|
|
__mavlink_digicam_control_t packet;
|
|
mavlink_msg_digicam_control_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system, packet.target_component)) {
|
|
// not for us
|
|
return;
|
|
}
|
|
// TODO do something with these values
|
|
/*
|
|
* packet.command_id
|
|
* packet.extra_param
|
|
* packet.extra_value
|
|
* packet.focus_lock
|
|
* packet.session
|
|
* packet.shot
|
|
* packet.zoom_pos
|
|
* packet.zoom_step
|
|
*/
|
|
if (packet.shot)
|
|
{
|
|
trigger_pic();
|
|
}
|
|
// echo the message to the ArduCam OSD camera control board
|
|
// for more info see: http://code.google.com/p/arducam-osd/
|
|
// TODO is it connected to MAVLINK_COMM_3 ?
|
|
mavlink_msg_digicam_control_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.session, packet.zoom_pos, packet.zoom_step, packet.focus_lock, packet.shot, packet.command_id, packet.extra_param, packet.extra_value);
|
|
}
|
|
|
|
|