mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 23:43:58 -04:00
Again remove some more files from the APM_Camera branch
This commit is contained in:
Amilcar Lucas
parent
52dc0a915a
commit
b3d4bb1478
@ -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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,71 +0,0 @@
|
|||||||
// -*- 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 <AP_Common.h>
|
|
||||||
|
|
||||||
/// @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 */
|
|
||||||
@ -1,9 +0,0 @@
|
|||||||
AP_Mount KEYWORD1
|
|
||||||
SetGPSTarget KEYWORD2
|
|
||||||
SetAssisted KEYWORD2
|
|
||||||
SetAntenna KEYWORD2
|
|
||||||
SetMountFPV KEYWORD2
|
|
||||||
SetMountLanding KEYWORD2
|
|
||||||
UpDateMount KEYWORD2
|
|
||||||
SetMode KEYWORD2
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user