diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index d195b8ad61..a89669ba06 100644 --- a/ArduCopter/motors_octa.pde +++ b/ArduCopter/motors_octa.pde @@ -182,37 +182,75 @@ static void output_motors_disarmed() static void output_motor_test() { - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); - delay(1000); + 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_1, g.rc_3.radio_min + 100); + delay(1000); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); - delay(1000); + APM_RC.OutputCh(CH_1, 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_11, g.rc_3.radio_min + 100); - delay(1000); + APM_RC.OutputCh(CH_3, 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_4, g.rc_3.radio_min + 100); - delay(1000); + APM_RC.OutputCh(CH_11, 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_2, g.rc_3.radio_min + 100); - delay(1000); + APM_RC.OutputCh(CH_4, 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_8, g.rc_3.radio_min + 100); - delay(1000); + APM_RC.OutputCh(CH_2, 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_10, g.rc_3.radio_min + 100); - delay(1000); + 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_7, g.rc_3.radio_min + 100); - delay(1000); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + 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 diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde index 37398ed21f..ac40870ab0 100644 --- a/ArduCopter/motors_octa_quad.pde +++ b/ArduCopter/motors_octa_quad.pde @@ -149,7 +149,37 @@ static void output_motors_disarmed() 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 diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 3a8bbb504f..4ebb342fd1 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -1,5 +1,4 @@ - - // -*- 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 // their default values, place the appropriate #define statements here. @@ -22,4 +21,15 @@ #define HIL_PORT 0 #define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK #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 diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 6d8685b3e3..3786839dc6 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -304,19 +304,6 @@ //#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 // then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 9a0da7d2e8..3b62fe925a 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- 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 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 // ArduPilot Mega DCM Library #include // PID library #include // RC Channel Library +//#include // Auxiliary RC Channel Library (Channels 5..8) #include // Range finder library #include +#include // Photo or video camera +#include // Camera mount + #include // MAVLink GCS definitions #include @@ -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 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 @@ -563,6 +576,18 @@ static void fast_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 // ----------------------------------------- switch(medium_loopCounter) { @@ -710,6 +735,11 @@ static void slow_loop() // ---------------------------------- 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; case 2: diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 3305927fa4..07300a65b6 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -274,21 +274,16 @@ static void set_servos(void) } g.channel_throttle.radio_out = g.channel_throttle.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_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_out = g.rc_6.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; } else { if (g.mix_mode == 0) { g.channel_roll.calc_pwm(); g.channel_pitch.calc_pwm(); g.channel_rudder.calc_pwm(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) { - g.rc_5.servo_out = g.channel_roll.servo_out; - g.rc_5.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(); + if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) { + g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out; + g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm(); } }else{ @@ -320,8 +315,7 @@ static void set_servos(void) } 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_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.radio_out = g.rc_6.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; } else if (control_mode >= FLY_BY_WIRE_C) { if (g.airspeed_enabled == true) { flapSpeedSource = g.airspeed_cruise; @@ -329,14 +323,11 @@ static void set_servos(void) flapSpeedSource = g.throttle_cruise; } 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_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.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; } 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_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.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; } else { - if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.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; + 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; } } @@ -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_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_5, g.rc_5.radio_out); // send to Servos - APM_RC.OutputCh(CH_6, g.rc_6.radio_out); // send to Servos + // Route configurable aux. functions to their respective 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 } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 570c38800a..7b62d9a8fa 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // 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 // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -112,6 +112,13 @@ public: k_param_RTL_altitude, k_param_inverted_flight_ch, + // + // Camera parameters + // +#if CAMERA == ENABLED + k_param_camera, +#endif + // // 170: Radio settings // @@ -134,11 +141,6 @@ public: k_param_long_fs_action, k_param_gcs_heartbeat_fs_enabled, 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 @@ -322,19 +324,20 @@ public: AP_Int8 flap_2_percent; AP_Int8 flap_2_speed; + // Camera +#if CAMERA == ENABLED + AP_Camera camera; +#endif + // RC channels RC_Channel channel_roll; RC_Channel channel_pitch; RC_Channel channel_throttle; RC_Channel channel_rudder; - RC_Channel rc_5; - RC_Channel rc_6; - RC_Channel rc_7; - RC_Channel rc_8; - AP_Int8 rc_5_funct; - AP_Int8 rc_6_funct; - AP_Int8 rc_7_funct; - AP_Int8 rc_8_funct; + RC_Channel_aux rc_5; + RC_Channel_aux rc_6; + RC_Channel_aux rc_7; + RC_Channel_aux rc_8; // PID controllers // @@ -428,13 +431,13 @@ public: inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")), sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_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! +#if CAMERA == ENABLED + camera (k_param_camera, PSTR("CAM_")), +#endif + // RC channel group key name //---------------------------------------------------------------------- channel_roll (k_param_channel_roll, PSTR("RC1_")), diff --git a/ArduPlane/WP_activity.pde b/ArduPlane/WP_activity.pde new file mode 100644 index 0000000000..474644cb90 --- /dev/null +++ b/ArduPlane/WP_activity.pde @@ -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 diff --git a/ArduPlane/config.h b/ArduPlane/config.h index bc1056e55f..f5fac05daa 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -226,19 +226,6 @@ # define CH8_MAX 2000 #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 # define FLAP_1_PERCENT 0 diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b48deed2da..5c5b01b10f 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -41,39 +41,12 @@ #define GPS_PROTOCOL_MTK16 6 #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_PITCH CH_2 #define CH_THROTTLE CH_3 #define CH_RUDDER 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 #define HIL_PROTOCOL_XPLANE 1 #define HIL_PROTOCOL_MAVLINK 2 diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 748b7a9228..ed3d1782b2 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -23,24 +23,26 @@ static void init_rc_in() g.channel_throttle.dead_zone = 6; //set auxiliary ranges - if (g.rc_5_funct == RC_5_FUNCT_AILERON) { - g.rc_5.set_angle(SERVO_MAX); - } else if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO || g.rc_5_funct == RC_5_FUNCT_FLAPERON) { - g.rc_5.set_range(0,100); - } else { - g.rc_5.set_range(0,1000); // Insert proper init for camera mount, etc., here - } - - if (g.rc_6_funct == RC_6_FUNCT_AILERON) { - g.rc_6.set_angle(SERVO_MAX); - } else if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO || g.rc_6_funct == RC_6_FUNCT_FLAPERON) { - g.rc_6.set_range(0,100); - } else { - g.rc_6.set_range(0,1000); // Insert proper init for camera mount, etc., here - } - - g.rc_7.set_range(0,1000); // Insert proper init for camera mount, etc., here - g.rc_8.set_range(0,1000); + G_RC_AUX(k_flap)->set_range(0,100); + G_RC_AUX(k_flap_auto)->set_range(0,100); + G_RC_AUX(k_aileron)->set_angle(SERVO_MAX); + G_RC_AUX(k_flaperon)->set_range(0,100); +#if CAMERA == ENABLED + 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); + G_RC_AUX(k_mount_pitch)->set_range( + g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10, + g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10); + G_RC_AUX(k_mount_roll)->set_range( + g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10, + 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_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10); + 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() @@ -173,8 +175,7 @@ static void trim_control_surfaces() g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.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 - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.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 }else{ elevon1_trim = ch1_temp; @@ -191,8 +192,7 @@ static void trim_control_surfaces() g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom(); - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); } static void trim_radio() @@ -208,8 +208,7 @@ static void trim_radio() g.channel_pitch.radio_trim = g.channel_pitch.radio_in; //g.channel_throttle.radio_trim = g.channel_throttle.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 - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.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 } else { elevon1_trim = ch1_temp; @@ -225,7 +224,5 @@ static void trim_radio() g.channel_pitch.save_eeprom(); //g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom(); - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); } - diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index 236f85593d..bcac692abe 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -5,6 +5,17 @@ #define MIN_PULSEWIDTH 900 #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 class APM_RC_Class diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp new file mode 100644 index 0000000000..ddfbc2d8a3 --- /dev/null +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -0,0 +1,113 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include +#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; + } + } +} diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h new file mode 100644 index 0000000000..e1c520131a --- /dev/null +++ b/libraries/AP_Camera/AP_Camera.h @@ -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 + +/// @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 */ diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index 9a10eb03bc..8e8af7cb36 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -1,7 +1,7 @@ #ifndef 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 // convention and the AP_DCM should be the top // header file diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp new file mode 100644 index 0000000000..ef931eb58c --- /dev/null +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -0,0 +1,192 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include +#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; + } +} diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h new file mode 100644 index 0000000000..e80c3e294a --- /dev/null +++ b/libraries/AP_Mount/AP_Mount.h @@ -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 +#include +#include +#include + +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 \ No newline at end of file diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 24fae0f5ea..aa189dff15 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -120,7 +120,7 @@ RC_Channel::calc_pwm(void) { if(_type == RC_CHANNEL_RANGE){ 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){ pwm_out = (float)servo_out * .1; diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index aec2ebc998..132e9146f3 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -7,12 +7,11 @@ #define RC_Channel_h #include -#include /// @class RC_Channel /// @brief Object managing one RC channel class RC_Channel{ - private: + protected: AP_Var_group _group; // must be before all vars to keep ctor init order correct public: @@ -103,8 +102,7 @@ class RC_Channel{ int16_t _low; }; +// This is ugly, but it fixes compilation on arduino +#include "RC_Channel_aux.h" + #endif - - - - diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp new file mode 100644 index 0000000000..caa7affb61 --- /dev/null +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -0,0 +1,96 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include +#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 + (anglemax?0:3600); // add 360 degrees if on the "wrong side" + angle = err_minfunction.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; +} diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h new file mode 100644 index 0000000000..6d8c228472 --- /dev/null +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -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_ */