From f92e7323f500c7da8a0e2f510a369d0628f55505 Mon Sep 17 00:00:00 2001 From: "joeholdsworth@gmail.com" Date: Sun, 21 Aug 2011 21:14:02 +0000 Subject: [PATCH] Amilcar Lucas & Ritchie Wilson's camera code mixed in with ap_mount code, this needs to be separated. git-svn-id: https://arducopter.googlecode.com/svn/trunk@3145 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_Camera/Camera.cpp | 277 +++++++++++++++++++++++++++++++++ libraries/AP_Camera/Camera.h | 71 +++++++++ 2 files changed, 348 insertions(+) create mode 100644 libraries/AP_Camera/Camera.cpp create mode 100644 libraries/AP_Camera/Camera.h diff --git a/libraries/AP_Camera/Camera.cpp b/libraries/AP_Camera/Camera.cpp new file mode 100644 index 0000000000..2c9bb928fd --- /dev/null +++ b/libraries/AP_Camera/Camera.cpp @@ -0,0 +1,277 @@ +// -*- 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 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 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; + } + } +} diff --git a/libraries/AP_Camera/Camera.h b/libraries/AP_Camera/Camera.h new file mode 100644 index 0000000000..88dc792748 --- /dev/null +++ b/libraries/AP_Camera/Camera.h @@ -0,0 +1,71 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file Camera.h +/// @brief Photo or video camera manager, with EEPROM-backed storage of constants. + +#ifndef CAMERA_H +#define CAMERA_H + +#include + +/// @class Camera +/// @brief Object managing a Photo or video camera +class Camera{ +protected: + AP_Var_group _group; // must be before all vars to keep ctor init order correct + +public: + /// Constructor + /// + /// @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) : + _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), + 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) + {} + + // 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 + +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 + 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. + 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. + void NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. + +}; + +#endif /* CAMERA_H */