mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
commit
2c27be980d
|
@ -181,6 +181,8 @@ static void output_motors_disarmed()
|
||||||
}
|
}
|
||||||
|
|
||||||
static void output_motor_test()
|
static void output_motor_test()
|
||||||
|
{
|
||||||
|
if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME )
|
||||||
{
|
{
|
||||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
|
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
|
||||||
|
@ -215,5 +217,41 @@ static void output_motor_test()
|
||||||
delay(1000);
|
delay(1000);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if( g.frame_orientation == V_FRAME )
|
||||||
|
{
|
||||||
|
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -149,8 +149,37 @@ static void output_motors_disarmed()
|
||||||
|
|
||||||
static void output_motor_test()
|
static void output_motor_test()
|
||||||
{
|
{
|
||||||
|
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||||
|
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
|
||||||
|
delay(1000);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -1,4 +1,3 @@
|
||||||
|
|
||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from
|
// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from
|
||||||
|
@ -23,3 +22,14 @@
|
||||||
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
|
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
|
||||||
#define GCS_PORT 3
|
#define GCS_PORT 3
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
// ----- Camera definitions ------
|
||||||
|
// ------------------------------
|
||||||
|
#define CAMERA ENABLED
|
||||||
|
#define CAM_DEBUG DISABLED
|
||||||
|
|
||||||
|
// - Options to reduce code size -
|
||||||
|
// -------------------------------
|
||||||
|
// Disable text based terminal configuration
|
||||||
|
#define CLI_ENABLED DISABLED
|
||||||
|
|
|
@ -304,19 +304,6 @@
|
||||||
//#define FLIGHT_MODE_6 MANUAL
|
//#define FLIGHT_MODE_6 MANUAL
|
||||||
//
|
//
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
|
||||||
// RC_5_FUNCT OPTIONAL
|
|
||||||
// RC_6_FUNCT OPTIONAL
|
|
||||||
// RC_7_FUNCT OPTIONAL
|
|
||||||
// RC_8_FUNCT OPTIONAL
|
|
||||||
//
|
|
||||||
// The channel 5 through 8 function assignments allow configuration of those
|
|
||||||
// channels for use with differential ailerons, flaps, flaperons, or camera
|
|
||||||
// or intrument mounts
|
|
||||||
//
|
|
||||||
//#define RC_5_FUNCT RC_5_FUNCT_NONE
|
|
||||||
//etc.
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed
|
// For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed
|
||||||
// then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed
|
// then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
#define THISFIRMWARE "ArduPlane V2.24"
|
#define THISFIRMWARE "ArduPilotMega V2.24"
|
||||||
/*
|
/*
|
||||||
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short
|
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short
|
||||||
Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi
|
Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi
|
||||||
|
@ -39,8 +39,12 @@ version 2.1 of the License, or (at your option) any later version.
|
||||||
#include <AP_DCM.h> // ArduPilot Mega DCM Library
|
#include <AP_DCM.h> // ArduPilot Mega DCM Library
|
||||||
#include <PID.h> // PID library
|
#include <PID.h> // PID library
|
||||||
#include <RC_Channel.h> // RC Channel Library
|
#include <RC_Channel.h> // RC Channel Library
|
||||||
|
//#include <RC_Channel/RC_Channel_aux.h> // Auxiliary RC Channel Library (Channels 5..8)
|
||||||
#include <AP_RangeFinder.h> // Range finder library
|
#include <AP_RangeFinder.h> // Range finder library
|
||||||
#include <ModeFilter.h>
|
#include <ModeFilter.h>
|
||||||
|
#include <AP_Camera.h> // Photo or video camera
|
||||||
|
#include <AP_Mount.h> // Camera mount
|
||||||
|
|
||||||
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
||||||
#include <memcheck.h>
|
#include <memcheck.h>
|
||||||
|
|
||||||
|
@ -415,6 +419,15 @@ static unsigned long nav_loopTimer; // used to track the elapsed time for GP
|
||||||
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
|
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
|
||||||
static float load; // % MCU cycles used
|
static float load; // % MCU cycles used
|
||||||
|
|
||||||
|
RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
|
||||||
|
|
||||||
|
//Camera tracking and stabilisation stuff
|
||||||
|
// --------------------------------------
|
||||||
|
#if CAMERA == ENABLED
|
||||||
|
AP_Mount camera_mount(g_gps, &dcm);
|
||||||
|
|
||||||
|
//pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82)
|
||||||
|
#endif
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
// Top-level logic
|
// Top-level logic
|
||||||
|
@ -563,6 +576,18 @@ static void fast_loop()
|
||||||
|
|
||||||
static void medium_loop()
|
static void medium_loop()
|
||||||
{
|
{
|
||||||
|
#if CAMERA == ENABLED
|
||||||
|
// TODO replace home with a POI coming from a MavLink message or command
|
||||||
|
//camera_mount.set_GPS_target(home);
|
||||||
|
|
||||||
|
// For now point the camera manually via the RC inputs (later remove these two lines)
|
||||||
|
// for simple dcm tests, replace k_manual with k_stabilise
|
||||||
|
camera_mount.set_mode(AP_Mount::k_stabilise);
|
||||||
|
camera_mount.update_mount();
|
||||||
|
|
||||||
|
g.camera.trigger_pic_cleanup();
|
||||||
|
#endif
|
||||||
|
|
||||||
// This is the start of the medium (10 Hz) loop pieces
|
// This is the start of the medium (10 Hz) loop pieces
|
||||||
// -----------------------------------------
|
// -----------------------------------------
|
||||||
switch(medium_loopCounter) {
|
switch(medium_loopCounter) {
|
||||||
|
@ -710,6 +735,11 @@ static void slow_loop()
|
||||||
// ----------------------------------
|
// ----------------------------------
|
||||||
update_servo_switches();
|
update_servo_switches();
|
||||||
|
|
||||||
|
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
|
||||||
|
|
||||||
|
#if CAMERA == ENABLED
|
||||||
|
camera_mount.update_mount_type();
|
||||||
|
#endif
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 2:
|
case 2:
|
||||||
|
|
|
@ -61,7 +61,7 @@ static void stabilize()
|
||||||
|
|
||||||
// Mix Stick input to allow users to override control surfaces
|
// Mix Stick input to allow users to override control surfaces
|
||||||
// -----------------------------------------------------------
|
// -----------------------------------------------------------
|
||||||
if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) {
|
if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B && failsafe == FAILSAFE_NONE)) {
|
||||||
|
|
||||||
|
|
||||||
// TODO: use RC_Channel control_mix function?
|
// TODO: use RC_Channel control_mix function?
|
||||||
|
@ -92,7 +92,7 @@ static void stabilize()
|
||||||
// stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
|
// stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
|
||||||
// important for steering on the ground during landing
|
// important for steering on the ground during landing
|
||||||
// -----------------------------------------------
|
// -----------------------------------------------
|
||||||
if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) {
|
if (control_mode <= FLY_BY_WIRE_B || (ENABLE_STICK_MIXING == 1 && failsafe == FAILSAFE_NONE)) {
|
||||||
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
|
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
|
||||||
ch4_inf = fabs(ch4_inf);
|
ch4_inf = fabs(ch4_inf);
|
||||||
ch4_inf = min(ch4_inf, 400.0);
|
ch4_inf = min(ch4_inf, 400.0);
|
||||||
|
@ -274,21 +274,16 @@ static void set_servos(void)
|
||||||
}
|
}
|
||||||
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
|
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
|
||||||
g.channel_rudder.radio_out = g.channel_rudder.radio_in;
|
g.channel_rudder.radio_out = g.channel_rudder.radio_in;
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_out = g.rc_5.radio_in;
|
if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_out = g_rc_function[RC_Channel_aux::k_aileron]->radio_in;
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_out = g.rc_6.radio_in;
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (g.mix_mode == 0) {
|
if (g.mix_mode == 0) {
|
||||||
g.channel_roll.calc_pwm();
|
g.channel_roll.calc_pwm();
|
||||||
g.channel_pitch.calc_pwm();
|
g.channel_pitch.calc_pwm();
|
||||||
g.channel_rudder.calc_pwm();
|
g.channel_rudder.calc_pwm();
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_AILERON) {
|
if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) {
|
||||||
g.rc_5.servo_out = g.channel_roll.servo_out;
|
g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out;
|
||||||
g.rc_5.calc_pwm();
|
g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm();
|
||||||
}
|
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_AILERON) {
|
|
||||||
g.rc_6.servo_out = g.channel_roll.servo_out;
|
|
||||||
g.rc_6.calc_pwm();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}else{
|
}else{
|
||||||
|
@ -320,8 +315,7 @@ static void set_servos(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
if(control_mode <= FLY_BY_WIRE_B) {
|
if(control_mode <= FLY_BY_WIRE_B) {
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.radio_out = g.rc_5.radio_in;
|
if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in;
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.radio_out = g.rc_6.radio_in;
|
|
||||||
} else if (control_mode >= FLY_BY_WIRE_C) {
|
} else if (control_mode >= FLY_BY_WIRE_C) {
|
||||||
if (g.airspeed_enabled == true) {
|
if (g.airspeed_enabled == true) {
|
||||||
flapSpeedSource = g.airspeed_cruise;
|
flapSpeedSource = g.airspeed_cruise;
|
||||||
|
@ -329,14 +323,11 @@ static void set_servos(void)
|
||||||
flapSpeedSource = g.throttle_cruise;
|
flapSpeedSource = g.throttle_cruise;
|
||||||
}
|
}
|
||||||
if ( flapSpeedSource > g.flap_1_speed) {
|
if ( flapSpeedSource > g.flap_1_speed) {
|
||||||
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = 0;
|
if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = 0;
|
||||||
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = 0;
|
|
||||||
} else if (flapSpeedSource > g.flap_2_speed) {
|
} else if (flapSpeedSource > g.flap_2_speed) {
|
||||||
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_1_percent;
|
if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_1_percent;
|
||||||
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_1_percent;
|
|
||||||
} else {
|
} else {
|
||||||
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_2_percent;
|
if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_2_percent;
|
||||||
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_2_percent;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -347,8 +338,11 @@ static void set_servos(void)
|
||||||
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
|
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
|
||||||
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
|
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
|
||||||
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos
|
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos
|
||||||
APM_RC.OutputCh(CH_5, g.rc_5.radio_out); // send to Servos
|
// Route configurable aux. functions to their respective servos
|
||||||
APM_RC.OutputCh(CH_6, g.rc_6.radio_out); // send to Servos
|
g.rc_5.output_ch(CH_5);
|
||||||
|
g.rc_6.output_ch(CH_6);
|
||||||
|
g.rc_7.output_ch(CH_7);
|
||||||
|
g.rc_8.output_ch(CH_8);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -17,7 +17,7 @@ public:
|
||||||
// The increment will prevent old parameters from being used incorrectly
|
// The increment will prevent old parameters from being used incorrectly
|
||||||
// by newer code.
|
// by newer code.
|
||||||
//
|
//
|
||||||
static const uint16_t k_format_version = 11;
|
static const uint16_t k_format_version = 12;
|
||||||
|
|
||||||
// The parameter software_type is set up solely for ground station use
|
// The parameter software_type is set up solely for ground station use
|
||||||
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
|
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
|
||||||
|
@ -112,6 +112,13 @@ public:
|
||||||
k_param_RTL_altitude,
|
k_param_RTL_altitude,
|
||||||
k_param_inverted_flight_ch,
|
k_param_inverted_flight_ch,
|
||||||
|
|
||||||
|
//
|
||||||
|
// Camera parameters
|
||||||
|
//
|
||||||
|
#if CAMERA == ENABLED
|
||||||
|
k_param_camera,
|
||||||
|
#endif
|
||||||
|
|
||||||
//
|
//
|
||||||
// 170: Radio settings
|
// 170: Radio settings
|
||||||
//
|
//
|
||||||
|
@ -135,11 +142,6 @@ public:
|
||||||
k_param_gcs_heartbeat_fs_enabled,
|
k_param_gcs_heartbeat_fs_enabled,
|
||||||
k_param_throttle_slewrate,
|
k_param_throttle_slewrate,
|
||||||
|
|
||||||
k_param_rc_5_funct,
|
|
||||||
k_param_rc_6_funct,
|
|
||||||
k_param_rc_7_funct,
|
|
||||||
k_param_rc_8_funct,
|
|
||||||
|
|
||||||
//
|
//
|
||||||
// 200: Feed-forward gains
|
// 200: Feed-forward gains
|
||||||
//
|
//
|
||||||
|
@ -322,19 +324,20 @@ public:
|
||||||
AP_Int8 flap_2_percent;
|
AP_Int8 flap_2_percent;
|
||||||
AP_Int8 flap_2_speed;
|
AP_Int8 flap_2_speed;
|
||||||
|
|
||||||
|
// Camera
|
||||||
|
#if CAMERA == ENABLED
|
||||||
|
AP_Camera camera;
|
||||||
|
#endif
|
||||||
|
|
||||||
// RC channels
|
// RC channels
|
||||||
RC_Channel channel_roll;
|
RC_Channel channel_roll;
|
||||||
RC_Channel channel_pitch;
|
RC_Channel channel_pitch;
|
||||||
RC_Channel channel_throttle;
|
RC_Channel channel_throttle;
|
||||||
RC_Channel channel_rudder;
|
RC_Channel channel_rudder;
|
||||||
RC_Channel rc_5;
|
RC_Channel_aux rc_5;
|
||||||
RC_Channel rc_6;
|
RC_Channel_aux rc_6;
|
||||||
RC_Channel rc_7;
|
RC_Channel_aux rc_7;
|
||||||
RC_Channel rc_8;
|
RC_Channel_aux rc_8;
|
||||||
AP_Int8 rc_5_funct;
|
|
||||||
AP_Int8 rc_6_funct;
|
|
||||||
AP_Int8 rc_7_funct;
|
|
||||||
AP_Int8 rc_8_funct;
|
|
||||||
|
|
||||||
// PID controllers
|
// PID controllers
|
||||||
//
|
//
|
||||||
|
@ -428,13 +431,13 @@ public:
|
||||||
inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")),
|
inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")),
|
||||||
sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")),
|
sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")),
|
||||||
airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")),
|
airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")),
|
||||||
rc_5_funct (RC_5_FUNCT, k_param_rc_5_funct, PSTR("RC5_FUNCT")),
|
|
||||||
rc_6_funct (RC_6_FUNCT, k_param_rc_6_funct, PSTR("RC6_FUNCT")),
|
|
||||||
rc_7_funct (RC_7_FUNCT, k_param_rc_7_funct, PSTR("RC7_FUNCT")),
|
|
||||||
rc_8_funct (RC_8_FUNCT, k_param_rc_8_funct, PSTR("RC8_FUNCT")),
|
|
||||||
|
|
||||||
// Note - total parameter name length must be less than 14 characters for MAVLink compatibility!
|
// Note - total parameter name length must be less than 14 characters for MAVLink compatibility!
|
||||||
|
|
||||||
|
#if CAMERA == ENABLED
|
||||||
|
camera (k_param_camera, PSTR("CAM_")),
|
||||||
|
#endif
|
||||||
|
|
||||||
// RC channel group key name
|
// RC channel group key name
|
||||||
//----------------------------------------------------------------------
|
//----------------------------------------------------------------------
|
||||||
channel_roll (k_param_channel_roll, PSTR("RC1_")),
|
channel_roll (k_param_channel_roll, PSTR("RC1_")),
|
||||||
|
|
|
@ -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
|
|
@ -226,19 +226,6 @@
|
||||||
# define CH8_MAX 2000
|
# define CH8_MAX 2000
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
|
||||||
#ifndef RC_5_FUNCT
|
|
||||||
# define RC_5_FUNCT RC_5_FUNCT_NONE
|
|
||||||
#endif
|
|
||||||
#ifndef RC_6_FUNCT
|
|
||||||
# define RC_6_FUNCT RC_6_FUNCT_NONE
|
|
||||||
#endif
|
|
||||||
#ifndef RC_7_FUNCT
|
|
||||||
# define RC_7_FUNCT RC_7_FUNCT_NONE
|
|
||||||
#endif
|
|
||||||
#ifndef RC_8_FUNCT
|
|
||||||
# define RC_8_FUNCT RC_8_FUNCT_NONE
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef FLAP_1_PERCENT
|
#ifndef FLAP_1_PERCENT
|
||||||
# define FLAP_1_PERCENT 0
|
# define FLAP_1_PERCENT 0
|
||||||
|
|
|
@ -41,39 +41,12 @@
|
||||||
#define GPS_PROTOCOL_MTK16 6
|
#define GPS_PROTOCOL_MTK16 6
|
||||||
#define GPS_PROTOCOL_AUTO 7
|
#define GPS_PROTOCOL_AUTO 7
|
||||||
|
|
||||||
// Radio channels
|
|
||||||
// Note channels are from 0!
|
|
||||||
//
|
|
||||||
// XXX these should be CH_n defines from RC.h at some point.
|
|
||||||
#define CH_1 0
|
|
||||||
#define CH_2 1
|
|
||||||
#define CH_3 2
|
|
||||||
#define CH_4 3
|
|
||||||
#define CH_5 4
|
|
||||||
#define CH_6 5
|
|
||||||
#define CH_7 6
|
|
||||||
#define CH_8 7
|
|
||||||
|
|
||||||
#define CH_ROLL CH_1
|
#define CH_ROLL CH_1
|
||||||
#define CH_PITCH CH_2
|
#define CH_PITCH CH_2
|
||||||
#define CH_THROTTLE CH_3
|
#define CH_THROTTLE CH_3
|
||||||
#define CH_RUDDER CH_4
|
#define CH_RUDDER CH_4
|
||||||
#define CH_YAW CH_4
|
#define CH_YAW CH_4
|
||||||
|
|
||||||
#define RC_5_FUNCT_NONE 0
|
|
||||||
#define RC_5_FUNCT_AILERON 1
|
|
||||||
#define RC_5_FUNCT_FLAP_AUTO 2
|
|
||||||
#define RC_5_FUNCT_FLAPERON 3
|
|
||||||
|
|
||||||
#define RC_6_FUNCT_NONE 0
|
|
||||||
#define RC_6_FUNCT_AILERON 1
|
|
||||||
#define RC_6_FUNCT_FLAP_AUTO 2
|
|
||||||
#define RC_6_FUNCT_FLAPERON 3
|
|
||||||
|
|
||||||
#define RC_7_FUNCT_NONE 0
|
|
||||||
|
|
||||||
#define RC_8_FUNCT_NONE 0
|
|
||||||
|
|
||||||
// HIL enumerations
|
// HIL enumerations
|
||||||
#define HIL_PROTOCOL_XPLANE 1
|
#define HIL_PROTOCOL_XPLANE 1
|
||||||
#define HIL_PROTOCOL_MAVLINK 2
|
#define HIL_PROTOCOL_MAVLINK 2
|
||||||
|
|
|
@ -43,12 +43,12 @@ static void failsafe_long_on_event()
|
||||||
case STABILIZE:
|
case STABILIZE:
|
||||||
case FLY_BY_WIRE_A: // middle position
|
case FLY_BY_WIRE_A: // middle position
|
||||||
case FLY_BY_WIRE_B: // middle position
|
case FLY_BY_WIRE_B: // middle position
|
||||||
|
case CIRCLE:
|
||||||
set_mode(RTL);
|
set_mode(RTL);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case AUTO:
|
case AUTO:
|
||||||
case LOITER:
|
case LOITER:
|
||||||
case CIRCLE:
|
|
||||||
if(g.long_fs_action == 1) {
|
if(g.long_fs_action == 1) {
|
||||||
set_mode(RTL);
|
set_mode(RTL);
|
||||||
}
|
}
|
||||||
|
|
|
@ -23,24 +23,26 @@ static void init_rc_in()
|
||||||
g.channel_throttle.dead_zone = 6;
|
g.channel_throttle.dead_zone = 6;
|
||||||
|
|
||||||
//set auxiliary ranges
|
//set auxiliary ranges
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_AILERON) {
|
G_RC_AUX(k_flap)->set_range(0,100);
|
||||||
g.rc_5.set_angle(SERVO_MAX);
|
G_RC_AUX(k_flap_auto)->set_range(0,100);
|
||||||
} else if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO || g.rc_5_funct == RC_5_FUNCT_FLAPERON) {
|
G_RC_AUX(k_aileron)->set_angle(SERVO_MAX);
|
||||||
g.rc_5.set_range(0,100);
|
G_RC_AUX(k_flaperon)->set_range(0,100);
|
||||||
} else {
|
#if CAMERA == ENABLED
|
||||||
g.rc_5.set_range(0,1000); // Insert proper init for camera mount, etc., here
|
G_RC_AUX(k_mount_yaw)->set_range(
|
||||||
}
|
g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10,
|
||||||
|
g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10);
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_AILERON) {
|
G_RC_AUX(k_mount_pitch)->set_range(
|
||||||
g.rc_6.set_angle(SERVO_MAX);
|
g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10,
|
||||||
} else if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO || g.rc_6_funct == RC_6_FUNCT_FLAPERON) {
|
g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10);
|
||||||
g.rc_6.set_range(0,100);
|
G_RC_AUX(k_mount_roll)->set_range(
|
||||||
} else {
|
g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10,
|
||||||
g.rc_6.set_range(0,1000); // Insert proper init for camera mount, etc., here
|
g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10);
|
||||||
}
|
G_RC_AUX(k_cam_trigger)->set_range(
|
||||||
|
g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10,
|
||||||
g.rc_7.set_range(0,1000); // Insert proper init for camera mount, etc., here
|
g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10);
|
||||||
g.rc_8.set_range(0,1000);
|
G_RC_AUX(k_cam_open)->set_range(0,100);
|
||||||
|
#endif
|
||||||
|
G_RC_AUX(k_egg_drop)->set_range(0,100);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void init_rc_out()
|
static void init_rc_out()
|
||||||
|
@ -173,8 +175,7 @@ static void trim_control_surfaces()
|
||||||
g.channel_roll.radio_trim = g.channel_roll.radio_in;
|
g.channel_roll.radio_trim = g.channel_roll.radio_in;
|
||||||
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
|
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
|
||||||
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
|
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel
|
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel
|
|
||||||
|
|
||||||
}else{
|
}else{
|
||||||
elevon1_trim = ch1_temp;
|
elevon1_trim = ch1_temp;
|
||||||
|
@ -191,8 +192,7 @@ static void trim_control_surfaces()
|
||||||
g.channel_pitch.save_eeprom();
|
g.channel_pitch.save_eeprom();
|
||||||
g.channel_throttle.save_eeprom();
|
g.channel_throttle.save_eeprom();
|
||||||
g.channel_rudder.save_eeprom();
|
g.channel_rudder.save_eeprom();
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom();
|
G_RC_AUX(k_aileron)->save_eeprom();
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static void trim_radio()
|
static void trim_radio()
|
||||||
|
@ -208,8 +208,7 @@ static void trim_radio()
|
||||||
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
|
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
|
||||||
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
|
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
|
||||||
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
|
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel
|
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
elevon1_trim = ch1_temp;
|
elevon1_trim = ch1_temp;
|
||||||
|
@ -225,7 +224,5 @@ static void trim_radio()
|
||||||
g.channel_pitch.save_eeprom();
|
g.channel_pitch.save_eeprom();
|
||||||
//g.channel_throttle.save_eeprom();
|
//g.channel_throttle.save_eeprom();
|
||||||
g.channel_rudder.save_eeprom();
|
g.channel_rudder.save_eeprom();
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom();
|
G_RC_AUX(k_aileron)->save_eeprom();
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -5,6 +5,17 @@
|
||||||
#define MIN_PULSEWIDTH 900
|
#define MIN_PULSEWIDTH 900
|
||||||
#define MAX_PULSEWIDTH 2100
|
#define MAX_PULSEWIDTH 2100
|
||||||
|
|
||||||
|
// Radio channels
|
||||||
|
// Note channels are from 0!
|
||||||
|
#define CH_1 0
|
||||||
|
#define CH_2 1
|
||||||
|
#define CH_3 2
|
||||||
|
#define CH_4 3
|
||||||
|
#define CH_5 4
|
||||||
|
#define CH_6 5
|
||||||
|
#define CH_7 6
|
||||||
|
#define CH_8 7
|
||||||
|
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
|
|
||||||
class APM_RC_Class
|
class APM_RC_Class
|
||||||
|
|
|
@ -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 @@
|
||||||
|
// -*- 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 */
|
|
@ -1,7 +1,7 @@
|
||||||
#ifndef AP_DCM_h
|
#ifndef AP_DCM_h
|
||||||
#define AP_DCM_h
|
#define AP_DCM_h
|
||||||
|
|
||||||
// teporarily include all other classes here
|
// temporarily include all other classes here
|
||||||
// since this naming is a bit off from the
|
// since this naming is a bit off from the
|
||||||
// convention and the AP_DCM should be the top
|
// convention and the AP_DCM should be the top
|
||||||
// header file
|
// header file
|
||||||
|
|
|
@ -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 @@
|
||||||
|
// -*- 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
|
|
@ -120,7 +120,7 @@ RC_Channel::calc_pwm(void)
|
||||||
{
|
{
|
||||||
if(_type == RC_CHANNEL_RANGE){
|
if(_type == RC_CHANNEL_RANGE){
|
||||||
pwm_out = range_to_pwm();
|
pwm_out = range_to_pwm();
|
||||||
radio_out = pwm_out + radio_min;
|
radio_out = (_reverse >=0 ? pwm_out + radio_min : radio_max - pwm_out);
|
||||||
|
|
||||||
}else if(_type == RC_CHANNEL_ANGLE_RAW){
|
}else if(_type == RC_CHANNEL_ANGLE_RAW){
|
||||||
pwm_out = (float)servo_out * .1;
|
pwm_out = (float)servo_out * .1;
|
||||||
|
|
|
@ -7,12 +7,11 @@
|
||||||
#define RC_Channel_h
|
#define RC_Channel_h
|
||||||
|
|
||||||
#include <AP_Common.h>
|
#include <AP_Common.h>
|
||||||
#include <stdint.h>
|
|
||||||
|
|
||||||
/// @class RC_Channel
|
/// @class RC_Channel
|
||||||
/// @brief Object managing one RC channel
|
/// @brief Object managing one RC channel
|
||||||
class RC_Channel{
|
class RC_Channel{
|
||||||
private:
|
protected:
|
||||||
AP_Var_group _group; // must be before all vars to keep ctor init order correct
|
AP_Var_group _group; // must be before all vars to keep ctor init order correct
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
@ -103,8 +102,7 @@ class RC_Channel{
|
||||||
int16_t _low;
|
int16_t _low;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// This is ugly, but it fixes compilation on arduino
|
||||||
|
#include "RC_Channel_aux.h"
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -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 @@
|
||||||
|
// -*- 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