From f08cea8044c7075d04419698896006f5b4ecd414 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Tue, 13 Sep 2011 01:54:47 +0200 Subject: [PATCH] These changes were meant to be inside commit a14c06adc06b. I'm sorry but reverting stuff is not that easy --- ArduPlane/APM_Config.h | 3 +-- ArduPlane/APM_Config.h.reference | 13 ------------- ArduPlane/ArduPlane.pde | 3 +++ ArduPlane/Parameters.h | 23 +++++------------------ ArduPlane/config.h | 13 ------------- ArduPlane/defines.h | 27 --------------------------- libraries/APM_RC/APM_RC.h | 11 +++++++++++ libraries/AP_DCM/AP_DCM.h | 2 +- libraries/RC_Channel/RC_Channel.cpp | 2 +- libraries/RC_Channel/RC_Channel.h | 10 ++++------ 10 files changed, 26 insertions(+), 81 deletions(-) diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 3a8bbb504f..40fcc180a9 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. diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 52b59655a0..4c560ef062 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..1c71b6b5be 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -415,6 +415,7 @@ 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 //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -710,6 +711,8 @@ static void slow_loop() // ---------------------------------- update_servo_switches(); + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); + break; case 2: diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 570c38800a..70976ce887 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) @@ -134,11 +134,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 @@ -327,14 +322,10 @@ public: 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,10 +419,6 @@ 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! diff --git a/ArduPlane/config.h b/ArduPlane/config.h index e6261d3560..d48b4cb73d 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/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_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/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 - - - -