Jason Short
14 years ago
22 changed files with 886 additions and 164 deletions
@ -0,0 +1,56 @@
@@ -0,0 +1,56 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
||||
|
||||
#if CAMERA == ENABLED |
||||
void waypoint_check() |
||||
{ |
||||
if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want |
||||
if(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you |
||||
g.camera.trigger_pic(); |
||||
} // DO SOMETHIMNG |
||||
} |
||||
if(g.waypoint_index == 20){ // When here do whats underneath |
||||
g.camera.trigger_pic(); |
||||
} |
||||
} |
||||
|
||||
void picture_time_check() |
||||
{ |
||||
if (g.camera.picture_time == 1){ |
||||
if (wp_distance < g.camera.wp_distance_min){ |
||||
g.camera.trigger_pic(); // or any camera activation command |
||||
} |
||||
} |
||||
} |
||||
#endif |
||||
|
||||
void egg_waypoint() |
||||
{ |
||||
if (g_rc_function[RC_Channel_aux::k_egg_drop]) |
||||
{ |
||||
float temp = (float)(current_loc.alt - home.alt) * .01; |
||||
float egg_dist = sqrt(temp / 4.903) * (float)g_gps->ground_speed *.01; |
||||
|
||||
if(g.waypoint_index == 3){ |
||||
if(wp_distance < egg_dist){ |
||||
g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 100; |
||||
} |
||||
}else{ |
||||
g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 0; |
||||
} |
||||
} |
||||
} |
||||
|
||||
#if CAMERA == ENABLED |
||||
void johann_check() // if you aren't Johann it doesn't really matter :D |
||||
{ |
||||
APM_RC.OutputCh(CH_7,1500 + (350)); |
||||
if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want |
||||
if(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you |
||||
g.camera.trigger_pic(); |
||||
} |
||||
} |
||||
if(g.waypoint_index == 20){ // When here do whats underneath |
||||
g.camera.trigger_pic(); |
||||
} |
||||
} |
||||
#endif |
||||
@ -0,0 +1,113 @@
@@ -0,0 +1,113 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
#include <AP_Camera.h> |
||||
#include <../RC_Channel/RC_Channel_aux.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: |
||||
G_RC_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; |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,57 @@
@@ -0,0 +1,57 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/// @file AP_Camera.h
|
||||
/// @brief Photo or video camera manager, with EEPROM-backed storage of constants.
|
||||
|
||||
#ifndef AP_CAMERA_H |
||||
#define AP_CAMERA_H |
||||
|
||||
#include <AP_Common.h> |
||||
|
||||
/// @class Camera
|
||||
/// @brief Object managing a Photo or video camera
|
||||
class AP_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.
|
||||
///
|
||||
AP_Camera(AP_Var::Key key, const prog_char_t *name) : |
||||
_group(key, name), |
||||
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
|
||||
keep_cam_trigg_active_cycles (0), |
||||
wp_distance_min (10) |
||||
{} |
||||
|
||||
// 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(); |
||||
|
||||
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
|
||||
int thr_pic; // timer variable for throttle_pic
|
||||
int camtrig; // PK6 chosen as it not near anything so safer for soldering
|
||||
|
||||
AP_Int8 trigger_type; // 0=Servo, 1=relay, 2=throttle_off time, 3=throttle_off waypoint 4=transistor
|
||||
|
||||
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 /* AP_CAMERA_H */ |
||||
@ -0,0 +1,192 @@
@@ -0,0 +1,192 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
#include <AP_Mount.h> |
||||
#include <../RC_Channel/RC_Channel_aux.h> |
||||
|
||||
extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
|
||||
|
||||
AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm) |
||||
{ |
||||
_dcm=dcm; |
||||
_gps=gps; |
||||
} |
||||
|
||||
void AP_Mount::set_pitch_yaw(int pitchCh, int yawCh) |
||||
{ |
||||
} |
||||
|
||||
void AP_Mount::set_GPS_target(Location targetGPSLocation) |
||||
{ |
||||
_target_GPS_location=targetGPSLocation; |
||||
|
||||
//set mode
|
||||
_mount_mode=k_gps_target; |
||||
|
||||
//update mount position
|
||||
update_mount(); |
||||
} |
||||
|
||||
void AP_Mount::set_assisted(int roll, int pitch, int yaw) |
||||
{ |
||||
_assist_angles.x = roll; |
||||
_assist_angles.y = pitch; |
||||
_assist_angles.z = yaw; |
||||
|
||||
//set mode
|
||||
_mount_mode=k_assisted; |
||||
|
||||
//update mount position
|
||||
update_mount(); |
||||
} |
||||
|
||||
//sets the servo angles for FPV, note angles are * 100
|
||||
void AP_Mount::set_mount_free_roam(int roll, int pitch, int yaw) |
||||
{ |
||||
_roam_angles.x=roll; |
||||
_roam_angles.y=pitch; |
||||
_roam_angles.z=yaw; |
||||
|
||||
//set mode
|
||||
_mount_mode=k_roam; |
||||
|
||||
//now update mount position
|
||||
update_mount(); |
||||
} |
||||
|
||||
//sets the servo angles for landing, note angles are * 100
|
||||
void AP_Mount::set_mount_landing(int roll, int pitch, int yaw) |
||||
{ |
||||
_landing_angles.x=roll; |
||||
_landing_angles.y=pitch; |
||||
_landing_angles.z=yaw; |
||||
|
||||
//set mode
|
||||
_mount_mode=k_landing; |
||||
|
||||
//now update mount position
|
||||
update_mount(); |
||||
} |
||||
|
||||
void AP_Mount::set_none() |
||||
{ |
||||
//set mode
|
||||
_mount_mode=k_none; |
||||
|
||||
//now update mount position
|
||||
update_mount(); |
||||
} |
||||
|
||||
void AP_Mount::update_mount() |
||||
{ |
||||
Matrix3f m; //holds 3 x 3 matrix, var is used as temp in calcs
|
||||
Vector3f targ; //holds target vector, var is used as temp in calcs
|
||||
|
||||
switch(_mount_mode) |
||||
{ |
||||
case k_gps_target: |
||||
{ |
||||
if(_gps->fix)
|
||||
{ |
||||
calc_GPS_target_vector(&_target_GPS_location); |
||||
} |
||||
m = _dcm->get_dcm_transposed(); |
||||
targ = m*_GPS_vector; |
||||
roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll
|
||||
pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch
|
||||
break; |
||||
} |
||||
case k_stabilise: |
||||
{ |
||||
// TODO replace this simplified implementation with a proper one
|
||||
roll_angle = -_dcm->roll_sensor; |
||||
pitch_angle = -_dcm->pitch_sensor; |
||||
yaw_angle = -_dcm->yaw_sensor; |
||||
break; |
||||
} |
||||
case k_roam: |
||||
{ |
||||
roll_angle=100*_roam_angles.x; |
||||
pitch_angle=100*_roam_angles.y; |
||||
yaw_angle=100*_roam_angles.z; |
||||
break; |
||||
} |
||||
case k_assisted: |
||||
{ |
||||
m = _dcm->get_dcm_transposed();
|
||||
//rotate vector
|
||||
targ = m*_assist_vector; |
||||
roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll
|
||||
pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch
|
||||
break; |
||||
} |
||||
case k_landing: |
||||
{ |
||||
roll_angle=100*_roam_angles.x; |
||||
pitch_angle=100*_roam_angles.y; |
||||
yaw_angle=100*_roam_angles.z; |
||||
break; |
||||
} |
||||
case k_manual: // radio manual control
|
||||
if (g_rc_function[RC_Channel_aux::k_mount_roll]) |
||||
roll_angle = map(g_rc_function[RC_Channel_aux::k_mount_roll]->radio_in, |
||||
g_rc_function[RC_Channel_aux::k_mount_roll]->radio_min, |
||||
g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max, |
||||
g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min, |
||||
g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max); |
||||
if (g_rc_function[RC_Channel_aux::k_mount_pitch]) |
||||
pitch_angle = map(g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_in, |
||||
g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_min, |
||||
g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max, |
||||
g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min, |
||||
g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max); |
||||
if (g_rc_function[RC_Channel_aux::k_mount_yaw]) |
||||
yaw_angle = map(g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_in, |
||||
g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_min, |
||||
g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max, |
||||
g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min, |
||||
g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max); |
||||
break; |
||||
case k_none: |
||||
default: |
||||
{ |
||||
//do nothing
|
||||
break; |
||||
}
|
||||
} |
||||
|
||||
// write the results to the servos
|
||||
// Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic
|
||||
G_RC_AUX(k_mount_roll)->closest_limit(roll_angle/10); |
||||
G_RC_AUX(k_mount_pitch)->closest_limit(pitch_angle/10); |
||||
G_RC_AUX(k_mount_yaw)->closest_limit(yaw_angle/10); |
||||
}
|
||||
|
||||
void AP_Mount::set_mode(MountMode mode) |
||||
{ |
||||
_mount_mode=mode; |
||||
} |
||||
|
||||
void AP_Mount::calc_GPS_target_vector(struct Location *target) |
||||
{ |
||||
_GPS_vector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195; |
||||
_GPS_vector.y = (target->lat-_gps->latitude)*.01113195; |
||||
_GPS_vector.z = (_gps->altitude-target->alt); |
||||
} |
||||
|
||||
void |
||||
AP_Mount::update_mount_type() |
||||
{ |
||||
// Auto-detect the mount gimbal type depending on the functions assigned to the servos
|
||||
if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) |
||||
{ |
||||
_mount_type = k_pan_tilt; |
||||
} |
||||
if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == NULL)) |
||||
{ |
||||
_mount_type = k_tilt_roll; |
||||
} |
||||
if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) |
||||
{ |
||||
_mount_type = k_pan_tilt_roll; |
||||
} |
||||
} |
||||
@ -0,0 +1,90 @@
@@ -0,0 +1,90 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/************************************************************
|
||||
* AP_mount -- library to control a 2 or 3 axis mount. *
|
||||
* *
|
||||
* Author: Joe Holdsworth; * |
||||
* Ritchie Wilson; * |
||||
* Amilcar Lucas; * |
||||
* *
|
||||
* Purpose: Move a 2 or 3 axis mount attached to vehicle, * |
||||
* Used for mount to track targets or stabilise * |
||||
* camera plus other modes. *
|
||||
* *
|
||||
* Usage: Use in main code to control mounts attached to * |
||||
* vehicle. * |
||||
* * |
||||
*Comments: All angles in degrees * 100, distances in meters* |
||||
* unless otherwise stated. * |
||||
************************************************************/
|
||||
#ifndef AP_Mount_H |
||||
#define AP_Mount_H |
||||
|
||||
//#include <AP_Common.h>
|
||||
#include <AP_Math.h> |
||||
#include <AP_GPS.h> |
||||
#include <AP_DCM.h> |
||||
|
||||
class AP_Mount |
||||
{ |
||||
public: |
||||
//Constructors
|
||||
AP_Mount(GPS *gps, AP_DCM *dcm); |
||||
|
||||
//enums
|
||||
enum MountMode{ |
||||
k_gps_target = 0, |
||||
k_stabilise = 1, //note the correct English spelling :)
|
||||
k_roam = 2, |
||||
k_assisted = 3, |
||||
k_landing = 4, |
||||
k_none = 5, |
||||
k_manual = 6 |
||||
}; |
||||
|
||||
enum MountType{ |
||||
k_pan_tilt = 0, //yaw-pitch
|
||||
k_tilt_roll = 1, //pitch-roll
|
||||
k_pan_tilt_roll = 2, //yaw-pitch-roll
|
||||
}; |
||||
|
||||
//Accessors
|
||||
void set_pitch_yaw(int pitchCh, int yawCh); |
||||
void set_pitch_roll(int pitchCh, int rollCh); |
||||
void set_pitch_roll_yaw(int pitchCh, int rollCh, int yawCh); |
||||
|
||||
void set_GPS_target(Location targetGPSLocation); //used to tell the mount to track GPS location
|
||||
void set_assisted(int roll, int pitch, int yaw); |
||||
void set_mount_free_roam(int roll, int pitch, int yaw);//used in the FPV for example,
|
||||
void set_mount_landing(int roll, int pitch, int yaw); //set mount landing position
|
||||
void set_none(); |
||||
|
||||
//methods
|
||||
void update_mount(); |
||||
void update_mount_type(); //Auto-detect the mount gimbal type depending on the functions assigned to the servos
|
||||
void set_mode(MountMode mode);
|
||||
|
||||
int pitch_angle; //degrees*100
|
||||
int roll_angle; //degrees*100
|
||||
int yaw_angle; //degrees*100
|
||||
protected: |
||||
//methods
|
||||
void calc_GPS_target_vector(struct Location *target); |
||||
//void CalculateDCM(int roll, int pitch, int yaw);
|
||||
//members
|
||||
AP_DCM *_dcm; |
||||
GPS *_gps; |
||||
|
||||
MountMode _mount_mode; |
||||
MountType _mount_type; |
||||
|
||||
struct Location _target_GPS_location; |
||||
Vector3f _GPS_vector; //target vector calculated stored in meters
|
||||
|
||||
Vector3i _roam_angles; //used for roam mode vector.x = roll vector.y = pitch, vector.z=yaw
|
||||
Vector3i _landing_angles; //landing position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
|
||||
|
||||
Vector3i _assist_angles; //used to keep angles that user has supplied from assisted targeting
|
||||
Vector3f _assist_vector; //used to keep vector calculated from _AssistAngles
|
||||
}; |
||||
#endif |
||||
@ -0,0 +1,96 @@
@@ -0,0 +1,96 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
#include <APM_RC.h> |
||||
#include "RC_Channel_aux.h" |
||||
|
||||
extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
|
||||
|
||||
int16_t |
||||
RC_Channel_aux::closest_limit(int16_t angle) |
||||
{ |
||||
// Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic
|
||||
int16_t min = angle_min / 10; |
||||
int16_t max = angle_max / 10; |
||||
|
||||
// Make sure the angle lies in the interval [-180 .. 180[ degrees
|
||||
while (angle < -1800) angle += 3600; |
||||
while (angle >= 1800) angle -= 3600; |
||||
|
||||
// Make sure the angle limits lie in the interval [-180 .. 180[ degrees
|
||||
while (min < -1800) min += 3600; |
||||
while (min >= 1800) min -= 3600; |
||||
while (max < -1800) max += 3600; |
||||
while (max >= 1800) max -= 3600; |
||||
set_range(min, max); |
||||
|
||||
// If the angle is outside servo limits, saturate the angle to the closest limit
|
||||
// On a circle the closest angular position must be carefully calculated to account for wrap-around
|
||||
if ((angle < min) && (angle > max)){ |
||||
// angle error if min limit is used
|
||||
int16_t err_min = min - angle + (angle<min?0:3600); // add 360 degrees if on the "wrong side"
|
||||
// angle error if max limit is used
|
||||
int16_t err_max = angle - max + (angle>max?0:3600); // add 360 degrees if on the "wrong side"
|
||||
angle = err_min<err_max?min:max; |
||||
} |
||||
|
||||
servo_out = angle; |
||||
// convert angle to PWM using a linear transformation (ignores trimming because the camera limits might not be symmetric)
|
||||
calc_pwm(); |
||||
|
||||
return angle; |
||||
} |
||||
|
||||
// map a function to a servo channel and output it
|
||||
void |
||||
RC_Channel_aux::output_ch(unsigned char ch_nr) |
||||
{ |
||||
switch(function) |
||||
{ |
||||
case k_none: // disabled
|
||||
return; |
||||
break; |
||||
case k_manual: // manual
|
||||
radio_out = radio_in; |
||||
break; |
||||
case k_flap: // flaps
|
||||
case k_flap_auto: // flaps automated
|
||||
case k_aileron: // aileron
|
||||
case k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing)
|
||||
case k_mount_yaw: // mount yaw (pan)
|
||||
case k_mount_pitch: // mount pitch (tilt)
|
||||
case k_mount_roll: // mount roll
|
||||
case k_cam_trigger: // camera trigger
|
||||
case k_cam_open: // camera open
|
||||
case k_egg_drop: // egg drop
|
||||
case k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning
|
||||
default: |
||||
break; |
||||
} |
||||
|
||||
APM_RC.OutputCh(ch_nr, radio_out); |
||||
} |
||||
|
||||
// update the g_rc_function array from pointers to rc_x channels
|
||||
// This should be done periodically because the user might change the configuration and
|
||||
// expects the changes to take effect instantly
|
||||
void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8) |
||||
{ |
||||
// positions 0..3 of this array never get used, but this is a stack array, so the entire array gets freed at the end of the function
|
||||
RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos
|
||||
aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)rc_5->function.get(); |
||||
aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)rc_6->function.get(); |
||||
aux_servo_function[CH_7] = (RC_Channel_aux::Aux_servo_function_t)rc_7->function.get(); |
||||
aux_servo_function[CH_8] = (RC_Channel_aux::Aux_servo_function_t)rc_8->function.get(); |
||||
|
||||
// Assume that no auxiliary function is used
|
||||
for (int i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++) |
||||
{ |
||||
g_rc_function[i] = NULL; |
||||
} |
||||
|
||||
// assign the RC channel to each function
|
||||
g_rc_function[aux_servo_function[CH_5]] = rc_5; |
||||
g_rc_function[aux_servo_function[CH_6]] = rc_6; |
||||
g_rc_function[aux_servo_function[CH_7]] = rc_7; |
||||
g_rc_function[aux_servo_function[CH_8]] = rc_8; |
||||
} |
||||
@ -0,0 +1,61 @@
@@ -0,0 +1,61 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/// @file RC_Channel_aux.h
|
||||
/// @brief RC_Channel manager for Channels 5..8, with EEPROM-backed storage of constants.
|
||||
/// @author Amilcar Lucas
|
||||
|
||||
#ifndef RC_CHANNEL_AUX_H_ |
||||
#define RC_CHANNEL_AUX_H_ |
||||
|
||||
#include "RC_Channel.h" |
||||
|
||||
// Macro to simplify accessing the auxiliary servos
|
||||
#define G_RC_AUX(_t) if (g_rc_function[RC_Channel_aux::_t]) g_rc_function[RC_Channel_aux::_t] |
||||
|
||||
/// @class RC_Channel_aux
|
||||
/// @brief Object managing one aux. RC channel (CH5-8), with information about its function
|
||||
/// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements
|
||||
class RC_Channel_aux : public RC_Channel{ |
||||
public: |
||||
/// Constructor
|
||||
///
|
||||
/// @param key EEPROM storage key for the channel trim parameters.
|
||||
/// @param name Optional name for the group.
|
||||
///
|
||||
RC_Channel_aux(AP_Var::Key key, const prog_char_t *name) : |
||||
RC_Channel(key, name), |
||||
function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0), // suppress name if group has no name
|
||||
angle_min (&_group, 5, -4500, name ? PSTR("ANGLE_MIN") : 0), // assume -45 degrees min deflection
|
||||
angle_max (&_group, 6, 4500, name ? PSTR("ANGLE_MAX") : 0) // assume 45 degrees max deflection
|
||||
{} |
||||
|
||||
typedef enum |
||||
{ |
||||
k_none = 0, // disabled
|
||||
k_manual = 1, // manual, just pass-thru the RC in signal
|
||||
k_flap = 2, // flap
|
||||
k_flap_auto = 3, // flap automated
|
||||
k_aileron = 4, // aileron
|
||||
k_flaperon = 5, // flaperon (flaps and aileron combined, needs two independent servos one for each wing)
|
||||
k_mount_yaw = 6, // mount yaw (pan)
|
||||
k_mount_pitch = 7, // mount pitch (tilt)
|
||||
k_mount_roll = 8, // mount roll
|
||||
k_cam_trigger = 9, // camera trigger
|
||||
k_cam_open = 10, // camera open
|
||||
k_egg_drop = 11, // egg drop
|
||||
k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one)
|
||||
} Aux_servo_function_t; |
||||
|
||||
AP_Int8 function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop
|
||||
AP_Int16 angle_min; // min angle limit of actuated surface in 0.01 degree units
|
||||
AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units
|
||||
|
||||
int16_t closest_limit(int16_t angle); // saturate to the closest angle limit if outside of min max angle interval
|
||||
|
||||
void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it
|
||||
|
||||
}; |
||||
|
||||
void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8); |
||||
|
||||
#endif /* RC_CHANNEL_AUX_H_ */ |
||||
Loading…
Reference in new issue