diff --git a/ArduPlane/Cam_move.pde b/ArduPlane/Cam_move.pde deleted file mode 100644 index bc8ad0eac7..0000000000 --- a/ArduPlane/Cam_move.pde +++ /dev/null @@ -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 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 diff --git a/ArduPlane/Cam_trigger.pde b/ArduPlane/Cam_trigger.pde deleted file mode 100644 index 7b03a90c8b..0000000000 --- a/ArduPlane/Cam_trigger.pde +++ /dev/null @@ -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); -} diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp new file mode 100644 index 0000000000..df32a4adac --- /dev/null +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -0,0 +1,116 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include +#include + +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; + } + } +} diff --git a/libraries/AP_Camera/Camera.h b/libraries/AP_Camera/AP_Camera.h similarity index 63% rename from libraries/AP_Camera/Camera.h rename to libraries/AP_Camera/AP_Camera.h index 88dc792748..e1c520131a 100644 --- a/libraries/AP_Camera/Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -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 /// @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 */ diff --git a/libraries/AP_Camera/Camera.cpp b/libraries/AP_Camera/Camera.cpp deleted file mode 100644 index 2c9bb928fd..0000000000 --- a/libraries/AP_Camera/Camera.cpp +++ /dev/null @@ -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 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; - } - } -}