zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

This is the real HEAD of the APM_Camera branch. Seams that lots of changes got lost in the SVN to GIT port

mission-4.1.18
Amilcar Lucas 14 years ago
parent
ec398505a6
commit
f40c85a601
5 changed files with 126 additions and 552 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 210
      ArduPlane/Cam_move.pde
  2. 41
      ArduPlane/Cam_trigger.pde
  3. 116
      libraries/AP_Camera/AP_Camera.cpp
  4. 34
      libraries/AP_Camera/AP_Camera.h
  5. 277
      libraries/AP_Camera/Camera.cpp

210
ArduPlane/Cam_move.pde
Unescape View File

@ -1,210 +0,0 @@ @@ -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(&current_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(&current_loc,&camera_target));
Serial.print(", bearing: ");
Serial.print(get_bearing(&current_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

41
ArduPlane/Cam_trigger.pde
Unescape View File

@ -1,41 +0,0 @@ @@ -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);
}

116
libraries/AP_Camera/AP_Camera.cpp
Unescape View File

@ -0,0 +1,116 @@ @@ -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;
}
}
}

34
libraries/AP_Camera/Camera.h → libraries/AP_Camera/AP_Camera.h
Unescape View File

@ -1,16 +1,16 @@ @@ -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: @@ -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: @@ -68,4 +54,4 @@ private:
};
#endif /* CAMERA_H */
#endif /* AP_CAMERA_H */

277
libraries/AP_Camera/Camera.cpp
Unescape View File

@ -1,277 +0,0 @@ @@ -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(&current_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(&current_loc,&camera_target));
Serial.print(", bearing: ");
Serial.print(get_bearing(&current_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;
}
}
}
Write Preview
Loading…
Cancel
Save