mirror of https://github.com/ArduPilot/ardupilot
This is the real HEAD of the APM_Camera branch. Seams that lots of changes got lost in the SVN to GIT port
This commit is contained in:
Amilcar Lucas
parent
ec398505a6
commit
f40c85a601
|
|
@ -1,210 +0,0 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
|
||||
void init_camera()
|
||||
{
|
||||
g.rc_camera_pitch.set_angle(4500); // throw of servo?
|
||||
g.rc_camera_pitch.radio_min = 1000; // lowest radio input
|
||||
g.rc_camera_pitch.radio_trim = 1500; // middle radio input
|
||||
g.rc_camera_pitch.radio_max = 2000; // highest radio input
|
||||
|
||||
g.rc_camera_roll.set_angle(4500);
|
||||
g.rc_camera_roll.radio_min = 1000;
|
||||
g.rc_camera_roll.radio_trim = 1500;
|
||||
g.rc_camera_roll.radio_max = 2000;
|
||||
|
||||
|
||||
//use test target for now
|
||||
camera_target = home;
|
||||
|
||||
}
|
||||
|
||||
void camera()
|
||||
{
|
||||
//decide what happens to camera depending on camera mode
|
||||
switch(camera_mode)
|
||||
{
|
||||
case 0:
|
||||
//do nothing, i.e lock camera in place
|
||||
break;
|
||||
case 1:
|
||||
//stabilize
|
||||
target_vector.x=0; //east to west gives +tive value (i.e. longitude)
|
||||
target_vector.y=0; //south to north gives +tive value (i.e. latitude)
|
||||
target_vector.z=100; //downwards is +tive
|
||||
camera_move();
|
||||
break;
|
||||
case 2:
|
||||
//track target
|
||||
if(g_gps->fix)
|
||||
{
|
||||
target_vector=get_location_vector(¤t_loc,&camera_target);
|
||||
camera_move();
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void camera_move()
|
||||
{
|
||||
Matrix3f m = dcm.get_dcm_transposed();
|
||||
Vector3<float> targ = m*target_vector; //to do: find out notion of x y convention
|
||||
|
||||
switch(gimbal_mode)
|
||||
{
|
||||
case 0: // pitch & roll
|
||||
cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch
|
||||
cam_roll = degrees(atan2(targ.y, targ.z)); //roll
|
||||
// check_limits(pitch);
|
||||
// check_limits(roll);
|
||||
// camera_out();
|
||||
break;
|
||||
|
||||
case 1: // pitch and yaw
|
||||
cam_tilt = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1;
|
||||
cam_pan = 9000 + atan2(-targ.y, targ.x) * 5729.57795;
|
||||
if (cam_pan < 0) cam_pan += 36000;
|
||||
// check_limits(pitch);
|
||||
// check_limits(yaw);
|
||||
// camera_out();
|
||||
break;
|
||||
|
||||
/* case 2: // pitch, roll & yaw - not started
|
||||
float cam_ritch = 100;
|
||||
float cam_yoll = 100;
|
||||
float cam_paw = 100;
|
||||
break; */
|
||||
}
|
||||
}
|
||||
|
||||
/* void check_limits(axis,variable) // Use servo definitions to calculate for all servo throws - TO DO
|
||||
{
|
||||
// find limits of servo range in deg
|
||||
track_pan_right = PAN_CENTER + (PAN_RANGE/2);
|
||||
track_pan_left = track_pan_right + (360 - PAN_RANGE);
|
||||
if (track_pan_left > 360){
|
||||
track_pan_left = track_pan_left - 360;
|
||||
}
|
||||
// check track_bearing is "safe" - not outside pan servo limits
|
||||
// if the bearing lies in the servo dead zone change bearing to closet edge
|
||||
if (track_bearing < track_pan_left && track_bearing > track_pan_right){
|
||||
track_oor_l = abs(track_bearing - track_pan_left);
|
||||
track_oor_r = abs(track_bearing - track_pan_right);
|
||||
if (track_oor_r > track_oor_l){
|
||||
track_bearing = track_pan_right;
|
||||
}
|
||||
if (track_oor_l > track_oor_r){
|
||||
track_bearing = track_pan_left;
|
||||
}
|
||||
}
|
||||
// center bearing to cam_servo center
|
||||
track_pan_deg = track_bearing - PAN_CENTER;
|
||||
// make negative is left rotation
|
||||
if (track_pan_deg > 180){
|
||||
track_pan_deg = (180 - (track_pan_deg - 180)) * -1;
|
||||
}
|
||||
|
||||
} */
|
||||
|
||||
void camera_out()
|
||||
{
|
||||
switch(gimbal_mode)
|
||||
{
|
||||
case 0: // pitch & roll
|
||||
g.rc_camera_pitch.servo_out = cam_pitch;
|
||||
g.rc_camera_pitch.calc_pwm();
|
||||
g.rc_camera_roll.servo_out = cam_roll;
|
||||
g.rc_camera_roll.calc_pwm();
|
||||
break;
|
||||
|
||||
case 1: // pitch and yaw
|
||||
g.rc_camera_pitch.servo_out = cam_tilt;
|
||||
g.rc_camera_pitch.calc_pwm();
|
||||
g.rc_camera_roll.servo_out = cam_pan; // borrowing roll servo output for pan/yaw
|
||||
g.rc_camera_roll.calc_pwm();
|
||||
break;
|
||||
|
||||
/*case 2: // pitch, roll & yaw
|
||||
g.rc_camera_pitch.servo_out = cam_ritch;
|
||||
g.rc_camera_pitch.calc_pwm();
|
||||
|
||||
g.rc_camera_roll.servo_out = cam_yoll;
|
||||
g.rc_camera_roll.calc_pwm();
|
||||
|
||||
g.rc_camera_yaw.servo_out = cam_paw; // camera_yaw doesn't exist it should unless we use another channel
|
||||
g.rc_camera_yaw.calc_pwm();
|
||||
break; */
|
||||
}
|
||||
#if defined PITCH_SERVO
|
||||
APM_RC.OutputCh(PITCH_SERVO, g.rc_camera_pitch.radio_out);
|
||||
#endif
|
||||
#if defined ROLL_SERVO
|
||||
APM_RC.OutputCh(ROLL_SERVO, g.rc_camera_roll.radio_out);
|
||||
#endif
|
||||
/*#if defined YAW_SERVO
|
||||
APM_RC.OutputCh(YAW_SERVO, g.rc_camera_yaw.radio_out);
|
||||
#endif */
|
||||
|
||||
#if CAM_DEBUG == ENABLED
|
||||
//for debugging purposes
|
||||
Serial.println();
|
||||
Serial.print("current_loc: lat: ");
|
||||
Serial.print(current_loc.lat);
|
||||
Serial.print(", lng: ");
|
||||
Serial.print(current_loc.lng);
|
||||
Serial.print(", alt: ");
|
||||
Serial.print(current_loc.alt);
|
||||
Serial.println();
|
||||
Serial.print("target_loc: lat: ");
|
||||
Serial.print(camera_target.lat);
|
||||
Serial.print(", lng: ");
|
||||
Serial.print(camera_target.lng);
|
||||
Serial.print(", alt: ");
|
||||
Serial.print(camera_target.alt);
|
||||
Serial.print(", distance: ");
|
||||
Serial.print(get_distance(¤t_loc,&camera_target));
|
||||
Serial.print(", bearing: ");
|
||||
Serial.print(get_bearing(¤t_loc,&camera_target));
|
||||
Serial.println();
|
||||
Serial.print("dcm_angles: roll: ");
|
||||
Serial.print(degrees(dcm.roll));
|
||||
Serial.print(", pitch: ");
|
||||
Serial.print(degrees(dcm.pitch));
|
||||
Serial.print(", yaw: ");
|
||||
Serial.print(degrees(dcm.yaw));
|
||||
Serial.println();
|
||||
Serial.print("target_vector: x: ");
|
||||
Serial.print(target_vector.x,2);
|
||||
Serial.print(", y: ");
|
||||
Serial.print(target_vector.y,2);
|
||||
Serial.print(", z: ");
|
||||
Serial.print(target_vector.z,2);
|
||||
Serial.println();
|
||||
Serial.print("rotated_target_vector: x: ");
|
||||
Serial.print(targ.x,2);
|
||||
Serial.print(", y: ");
|
||||
Serial.print(targ.y,2);
|
||||
Serial.print(", z: ");
|
||||
Serial.print(targ.z,2);
|
||||
Serial.println();
|
||||
Serial.print("gimbal type 0: roll: ");
|
||||
Serial.print(roll);
|
||||
Serial.print(", pitch: ");
|
||||
Serial.print(pitch);
|
||||
Serial.println();
|
||||
/* Serial.print("gimbal type 1: pitch: ");
|
||||
Serial.print(pan);
|
||||
Serial.print(", roll: ");
|
||||
Serial.print(tilt);
|
||||
Serial.println(); */
|
||||
/* Serial.print("gimbal type 2: pitch: ");
|
||||
Serial.print(ritch);
|
||||
Serial.print(", roll: ");
|
||||
Serial.print(yoll);
|
||||
Serial.print(", yaw: ");
|
||||
Serial.print(paw);
|
||||
Serial.println(); */
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1,41 +0,0 @@
|
|||
void servo_pic() // Servo operated camera
|
||||
{
|
||||
APM_RC.OutputCh(CAM_SERVO,1500 + (333)); // Camera click, not enough - add more, wring way - put a minus before bracket number (-300)
|
||||
delay(250); // delay
|
||||
APM_RC.OutputCh(CAM_SERVO,1500); // Return servo to mid position
|
||||
}
|
||||
|
||||
void relay_picture() // basic relay activation
|
||||
{
|
||||
relay_on();
|
||||
delay(250); // 0.25 seconds delay
|
||||
relay_off();
|
||||
}
|
||||
|
||||
void 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.
|
||||
{
|
||||
g.channel_throttle.radio_out = g.throttle_min;
|
||||
if (thr_pic = 10){
|
||||
servo_pic(); // triggering method
|
||||
thr_pic = 0;
|
||||
g.channel_throttle.radio_out = g.throttle_cruise;
|
||||
}
|
||||
thr_pic++;
|
||||
}
|
||||
|
||||
void distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle.
|
||||
{
|
||||
g.channel_throttle.radio_out = g.throttle_min;
|
||||
if (wp_distance < 3){
|
||||
servo_pic(); // triggering method
|
||||
g.channel_throttle.radio_out = g.throttle_cruise;
|
||||
}
|
||||
}
|
||||
|
||||
void NPN_trigger() // hacked the circuit to run a transistor? use this trigger to send output.
|
||||
{
|
||||
// To Do: Assign pin spare pin for output
|
||||
digitalWrite(camtrig, HIGH);
|
||||
delay(50);
|
||||
digitalWrite(camtrig, LOW);
|
||||
}
|
||||
|
|
@ -0,0 +1,116 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
#include <AP_Camera.h>
|
||||
#include <RC_Channel.h>
|
||||
|
||||
extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
|
||||
extern long wp_distance;
|
||||
extern "C" {
|
||||
void relay_on();
|
||||
void relay_off();
|
||||
}
|
||||
|
||||
void
|
||||
AP_Camera::servo_pic() // Servo operated camera
|
||||
{
|
||||
if (g_rc_function[RC_Channel_aux::k_cam_trigger])
|
||||
{
|
||||
g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_max;
|
||||
keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
AP_Camera::relay_pic() // basic relay activation
|
||||
{
|
||||
relay_on();
|
||||
keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles
|
||||
}
|
||||
|
||||
void
|
||||
AP_Camera::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.
|
||||
{
|
||||
// TODO find a way to do this without using the global parameter g
|
||||
// g.channel_throttle.radio_out = g.throttle_min;
|
||||
if (thr_pic == 10){
|
||||
servo_pic(); // triggering method
|
||||
thr_pic = 0;
|
||||
// g.channel_throttle.radio_out = g.throttle_cruise;
|
||||
}
|
||||
thr_pic++;
|
||||
}
|
||||
|
||||
void
|
||||
AP_Camera::distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle.
|
||||
{
|
||||
// TODO find a way to do this without using the global parameter g
|
||||
// g.channel_throttle.radio_out = g.throttle_min;
|
||||
if (wp_distance < 3){
|
||||
servo_pic(); // triggering method
|
||||
// g.channel_throttle.radio_out = g.throttle_cruise;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
AP_Camera::NPN_pic() // hacked the circuit to run a transistor? use this trigger to send output.
|
||||
{
|
||||
// TODO: Assign pin spare pin for output
|
||||
digitalWrite(camtrig, HIGH);
|
||||
keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles
|
||||
}
|
||||
|
||||
// single entry point to take pictures
|
||||
void
|
||||
AP_Camera::trigger_pic()
|
||||
{
|
||||
switch (trigger_type)
|
||||
{
|
||||
case 0:
|
||||
servo_pic(); // Servo operated camera
|
||||
break;
|
||||
case 1:
|
||||
relay_pic(); // basic relay activation
|
||||
break;
|
||||
case 2:
|
||||
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 3:
|
||||
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 4:
|
||||
NPN_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
|
||||
void
|
||||
AP_Camera::trigger_pic_cleanup()
|
||||
{
|
||||
if (keep_cam_trigg_active_cycles)
|
||||
{
|
||||
keep_cam_trigg_active_cycles --;
|
||||
}
|
||||
else
|
||||
{
|
||||
switch (trigger_type)
|
||||
{
|
||||
case 0:
|
||||
case 2:
|
||||
case 3:
|
||||
if (g_rc_function[RC_Channel_aux::k_cam_trigger])
|
||||
{
|
||||
g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min;
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
// TODO for some strange reason the function call bellow gives a linker error
|
||||
//relay_off();
|
||||
PORTL &= ~B00000100; // hardcoded version of relay_off(). Replace with a proper function call later.
|
||||
break;
|
||||
case 4:
|
||||
digitalWrite(camtrig, LOW);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,16 +1,16 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/// @file Camera.h
|
||||
/// @file AP_Camera.h
|
||||
/// @brief Photo or video camera manager, with EEPROM-backed storage of constants.
|
||||
|
||||
#ifndef CAMERA_H
|
||||
#define CAMERA_H
|
||||
#ifndef AP_CAMERA_H
|
||||
#define AP_CAMERA_H
|
||||
|
||||
#include <AP_Common.h>
|
||||
|
||||
/// @class Camera
|
||||
/// @brief Object managing a Photo or video camera
|
||||
class Camera{
|
||||
class AP_Camera{
|
||||
protected:
|
||||
AP_Var_group _group; // must be before all vars to keep ctor init order correct
|
||||
|
||||
|
|
@ -20,45 +20,31 @@ public:
|
|||
/// @param key EEPROM storage key for the camera configuration parameters.
|
||||
/// @param name Optional name for the group.
|
||||
///
|
||||
Camera(AP_Var::Key key, const prog_char_t *name) :
|
||||
AP_Camera(AP_Var::Key key, const prog_char_t *name) :
|
||||
_group(key, name),
|
||||
mode (&_group, 0, 0, name ? PSTR("MODE") : 0), // suppress name if group has no name
|
||||
trigger_type(&_group, 1, 0, name ? PSTR("TRIGGER_MODE") : 0),
|
||||
trigger_type(&_group, 0, 0, name ? PSTR("TRIGG_TYPE") : 0),
|
||||
picture_time (0), // waypoint trigger variable
|
||||
thr_pic (0), // timer variable for throttle_pic
|
||||
camtrig (83), // PK6 chosen as it not near anything so safer for soldering
|
||||
// camera_target (home), // use test target for now
|
||||
gimbal_type (1),
|
||||
keep_cam_trigg_active_cycles (0)
|
||||
keep_cam_trigg_active_cycles (0),
|
||||
wp_distance_min (10)
|
||||
{}
|
||||
|
||||
// move the camera depending on the camera mode
|
||||
void move();
|
||||
|
||||
// single entry point to take pictures
|
||||
void trigger_pic();
|
||||
|
||||
// de-activate the trigger after some delay, but without using a delay() function
|
||||
void trigger_pic_cleanup();
|
||||
|
||||
// call this from time to time to make sure the correct gimbal type gets choosen
|
||||
void update_camera_gimbal_type();
|
||||
|
||||
// set camera orientation target
|
||||
void set_target(struct Location target);
|
||||
|
||||
int picture_time; // waypoint trigger variable
|
||||
long wp_distance_min; // take picture if distance to WP is smaller than this
|
||||
|
||||
private:
|
||||
uint8_t keep_cam_trigg_active_cycles; // event loop cycles to keep trigger active
|
||||
struct Location camera_target; // point of interest for the camera to track
|
||||
// struct Location GPS_mark; // GPS POI for position based triggering
|
||||
int thr_pic; // timer variable for throttle_pic
|
||||
int camtrig; // PK6 chosen as it not near anything so safer for soldering
|
||||
|
||||
AP_Int8 mode; // 0=do nothing, 1=stabilize, 2=track target, 3=manual, 4=simple stabilize test
|
||||
AP_Int8 trigger_type; // 0=Servo, 1=relay, 2=throttle_off time, 3=throttle_off waypoint 4=transistor
|
||||
uint8_t gimbal_type; // 0=pitch & roll (tilt & roll), 1=yaw & pitch(pan & tilt), 2=pitch, roll & yaw (to be added)
|
||||
|
||||
void servo_pic(); // Servo operated camera
|
||||
void relay_pic(); // basic relay activation
|
||||
|
|
@ -68,4 +54,4 @@ private:
|
|||
|
||||
};
|
||||
|
||||
#endif /* CAMERA_H */
|
||||
#endif /* AP_CAMERA_H */
|
||||
|
|
@ -1,277 +0,0 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
#include "Camera.h"
|
||||
#include "../RC_Channel/RC_Channel.h"
|
||||
|
||||
void
|
||||
Camera::move()
|
||||
{
|
||||
Vector3<float> target_vector(0,0,1); // x, y, z to target before rotating to planes axis, values are in meters
|
||||
|
||||
//decide what happens to camera depending on camera mode
|
||||
switch(mode)
|
||||
{
|
||||
case 0:
|
||||
//do nothing, i.e lock camera in place
|
||||
return;
|
||||
break;
|
||||
case 1:
|
||||
//stabilize
|
||||
target_vector.x=0; //east to west gives +tive value (i.e. longitude)
|
||||
target_vector.y=0; //south to north gives +tive value (i.e. latitude)
|
||||
target_vector.z=100; //downwards is +tive
|
||||
break;
|
||||
case 2:
|
||||
//track target
|
||||
if(g_gps->fix)
|
||||
{
|
||||
target_vector=get_location_vector(¤t_loc,&camera_target);
|
||||
}
|
||||
break;
|
||||
case 3: // radio manual control
|
||||
case 4: // test (level the camera and point north)
|
||||
break; // see code 25 lines bellow
|
||||
}
|
||||
|
||||
Matrix3f m = dcm.get_dcm_transposed();
|
||||
Vector3<float> targ = m*target_vector; //to do: find out notion of x y convention
|
||||
switch(gimbal_type)
|
||||
{
|
||||
case 0: // pitch & roll (tilt & roll)
|
||||
cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch
|
||||
cam_roll = degrees(atan2(targ.y, targ.z)); //roll
|
||||
break;
|
||||
|
||||
case 1: // yaw & pitch (pan & tilt)
|
||||
cam_pitch = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1;
|
||||
cam_yaw = 9000 + atan2(-targ.y, targ.x) * 5729.57795;
|
||||
break;
|
||||
|
||||
/* case 2: // pitch, roll & yaw - not started
|
||||
cam_ritch = 0;
|
||||
cam_yoll = 0;
|
||||
cam_paw = 0;
|
||||
break; */
|
||||
|
||||
}
|
||||
|
||||
//some camera modes overwrite the gimbal_type calculations
|
||||
switch(mode)
|
||||
{
|
||||
case 3: // radio manual control
|
||||
if (rc_function[CAM_PITCH])
|
||||
cam_pitch = map(rc_function[CAM_PITCH]->radio_in,
|
||||
rc_function[CAM_PITCH]->radio_min,
|
||||
rc_function[CAM_PITCH]->radio_max,
|
||||
rc_function[CAM_PITCH]->angle_min,
|
||||
rc_function[CAM_PITCH]->radio_max);
|
||||
if (rc_function[CAM_ROLL])
|
||||
cam_roll = map(rc_function[CAM_ROLL]->radio_in,
|
||||
rc_function[CAM_ROLL]->radio_min,
|
||||
rc_function[CAM_ROLL]->radio_max,
|
||||
rc_function[CAM_ROLL]->angle_min,
|
||||
rc_function[CAM_ROLL]->radio_max);
|
||||
if (rc_function[CAM_YAW])
|
||||
cam_yaw = map(rc_function[CAM_YAW]->radio_in,
|
||||
rc_function[CAM_YAW]->radio_min,
|
||||
rc_function[CAM_YAW]->radio_max,
|
||||
rc_function[CAM_YAW]->angle_min,
|
||||
rc_function[CAM_YAW]->radio_max);
|
||||
break;
|
||||
case 4: // test (level the camera and point north)
|
||||
cam_pitch = -dcm.pitch_sensor;
|
||||
cam_yaw = dcm.yaw_sensor; // do not invert because the servo is mounted upside-down on my system
|
||||
// TODO: the "trunk" code can invert using parameters, but this branch still can't
|
||||
cam_roll = -dcm.roll_sensor;
|
||||
break;
|
||||
}
|
||||
|
||||
#if CAM_DEBUG == ENABLED
|
||||
//for debugging purposes
|
||||
Serial.println();
|
||||
Serial.print("current_loc: lat: ");
|
||||
Serial.print(current_loc.lat);
|
||||
Serial.print(", lng: ");
|
||||
Serial.print(current_loc.lng);
|
||||
Serial.print(", alt: ");
|
||||
Serial.print(current_loc.alt);
|
||||
Serial.println();
|
||||
Serial.print("target_loc: lat: ");
|
||||
Serial.print(camera_target.lat);
|
||||
Serial.print(", lng: ");
|
||||
Serial.print(camera_target.lng);
|
||||
Serial.print(", alt: ");
|
||||
Serial.print(camera_target.alt);
|
||||
Serial.print(", distance: ");
|
||||
Serial.print(get_distance(¤t_loc,&camera_target));
|
||||
Serial.print(", bearing: ");
|
||||
Serial.print(get_bearing(¤t_loc,&camera_target));
|
||||
Serial.println();
|
||||
Serial.print("dcm_angles: roll: ");
|
||||
Serial.print(degrees(dcm.roll));
|
||||
Serial.print(", pitch: ");
|
||||
Serial.print(degrees(dcm.pitch));
|
||||
Serial.print(", yaw: ");
|
||||
Serial.print(degrees(dcm.yaw));
|
||||
Serial.println();
|
||||
Serial.print("target_vector: x: ");
|
||||
Serial.print(target_vector.x,2);
|
||||
Serial.print(", y: ");
|
||||
Serial.print(target_vector.y,2);
|
||||
Serial.print(", z: ");
|
||||
Serial.print(target_vector.z,2);
|
||||
Serial.println();
|
||||
Serial.print("rotated_target_vector: x: ");
|
||||
Serial.print(targ.x,2);
|
||||
Serial.print(", y: ");
|
||||
Serial.print(targ.y,2);
|
||||
Serial.print(", z: ");
|
||||
Serial.print(targ.z,2);
|
||||
Serial.println();
|
||||
Serial.print("gimbal type 0: roll: ");
|
||||
Serial.print(roll);
|
||||
Serial.print(", pitch: ");
|
||||
Serial.print(pitch);
|
||||
Serial.println();
|
||||
/* Serial.print("gimbal type 1: pitch: ");
|
||||
Serial.print(pan);
|
||||
Serial.print(", roll: ");
|
||||
Serial.print(tilt);
|
||||
Serial.println(); */
|
||||
/* Serial.print("gimbal type 2: pitch: ");
|
||||
Serial.print(ritch);
|
||||
Serial.print(", roll: ");
|
||||
Serial.print(yoll);
|
||||
Serial.print(", yaw: ");
|
||||
Serial.print(paw);
|
||||
Serial.println(); */
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Camera::set_target(struct Location target)
|
||||
{
|
||||
camera_target = target;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Camera::update_camera_gimbal_type()
|
||||
{
|
||||
|
||||
// Auto detect the camera gimbal type depending on the functions assigned to the servos
|
||||
if ((rc_function[CAM_YAW] == NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] != NULL))
|
||||
{
|
||||
gimbal_type = 0;
|
||||
}
|
||||
if ((rc_function[CAM_YAW] != NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] == NULL))
|
||||
{
|
||||
gimbal_type = 1;
|
||||
}
|
||||
if ((rc_function[CAM_YAW] != NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] != NULL))
|
||||
{
|
||||
gimbal_type = 2;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Camera::servo_pic() // Servo operated camera
|
||||
{
|
||||
if (rc_function[CAM_TRIGGER])
|
||||
{
|
||||
cam_trigger = rc_function[CAM_TRIGGER]->radio_max;
|
||||
keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Camera::relay_pic() // basic relay activation
|
||||
{
|
||||
relay_on();
|
||||
keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles
|
||||
}
|
||||
|
||||
void
|
||||
Camera::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.
|
||||
{
|
||||
g.channel_throttle.radio_out = g.throttle_min;
|
||||
if (thr_pic == 10){
|
||||
servo_pic(); // triggering method
|
||||
thr_pic = 0;
|
||||
g.channel_throttle.radio_out = g.throttle_cruise;
|
||||
}
|
||||
thr_pic++;
|
||||
}
|
||||
|
||||
void
|
||||
Camera::distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle.
|
||||
{
|
||||
g.channel_throttle.radio_out = g.throttle_min;
|
||||
if (wp_distance < 3){
|
||||
servo_pic(); // triggering method
|
||||
g.channel_throttle.radio_out = g.throttle_cruise;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Camera::NPN_pic() // hacked the circuit to run a transistor? use this trigger to send output.
|
||||
{
|
||||
// To Do: Assign pin spare pin for output
|
||||
digitalWrite(camtrig, HIGH);
|
||||
keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles
|
||||
}
|
||||
|
||||
// single entry point to take pictures
|
||||
void
|
||||
Camera::trigger_pic()
|
||||
{
|
||||
switch (trigger_type)
|
||||
{
|
||||
case 0:
|
||||
servo_pic(); // Servo operated camera
|
||||
break;
|
||||
case 1:
|
||||
relay_pic(); // basic relay activation
|
||||
break;
|
||||
case 2:
|
||||
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 3:
|
||||
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 4:
|
||||
NPN_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
|
||||
void
|
||||
Camera::trigger_pic_cleanup()
|
||||
{
|
||||
if (keep_cam_trigg_active_cycles)
|
||||
{
|
||||
keep_cam_trigg_active_cycles --;
|
||||
}
|
||||
else
|
||||
{
|
||||
switch (trigger_type)
|
||||
{
|
||||
case 0:
|
||||
case 2:
|
||||
case 3:
|
||||
if (rc_function[CAM_TRIGGER])
|
||||
{
|
||||
cam_trigger = rc_function[CAM_TRIGGER]->radio_min;
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
relay_off();
|
||||
break;
|
||||
case 4:
|
||||
digitalWrite(camtrig, LOW);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue