diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 3027948af2..ffb095c44c 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -10,8 +10,6 @@ // developers have these boards. //#define APM2_BETA_HARDWARE -// GPS is auto-selected - //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD //#define HIL_MODE HIL_MODE_ATTITUDE //#define DMP_ENABLED ENABLED @@ -39,27 +37,18 @@ //#define CH7_OPTION CH7_SAVE_WP /* * CH7_DO_NOTHING - * CH7_SET_HOVER // deprecated * CH7_FLIP * CH7_SIMPLE_MODE * CH7_RTL * CH7_SAVE_TRIM - * CH7_ADC_FILTER // deprecated * CH7_SAVE_WP - * CH7_MULTI_MODE // deprecated * CH7_CAMERA_TRIGGER */ -//#define TOY_EDF ENABLED -//#define TOY_MIXER TOY_LOOKUP_TABLE - // Inertia based contollers //#define INERTIAL_NAV_XY ENABLED #define INERTIAL_NAV_Z ENABLED - -//#define RATE_ROLL_I 0.18 -//#define RATE_PITCH_I 0.18 //#define MOTORS_JD880 //#define MOTORS_JD850 @@ -80,20 +69,3 @@ //#define LOGGING_ENABLED DISABLED -// Used to set the max roll and pitch angles in Degrees * 100 -//#define MAX_INPUT_ROLL_ANGLE 8000 -//#define MAX_INPUT_PITCH_ANGLE 8000 - -///////////////////////////////////////////////////////////////////////////////// -// Bulk defines for TradHeli. Cleans up defines.h and config.h to put these here -#if FRAME_CONFIG == HELI_FRAME - # define RC_FAST_SPEED 125 - # define RTL_YAW YAW_LOOK_AT_HOME - # define TILT_COMPENSATION 5 - # define RATE_INTEGRATOR_LEAK_RATE 0.02 - # define RATE_ROLL_D 0 - # define RATE_PITCH_D 0 - # define HELI_PITCH_FF 0 - # define HELI_ROLL_FF 0 - # define HELI_YAW_FF 0 - #endif diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 3f966f7002..290f044df9 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -259,21 +259,6 @@ update_rate_contoller_targets() } } - -#if FRAME_CONFIG == HELI_FRAME -// init_rate_controllers - set-up filters for rate controller inputs -void init_rate_controllers() -{ - // initalise low pass filters on rate controller inputs - // 1st parameter is time_step, 2nd parameter is time_constant - rate_roll_filter.set_cutoff_frequency(0.01, 2.0); - rate_pitch_filter.set_cutoff_frequency(0.01, 2.0); - // rate_yaw_filter.set_cutoff_frequency(0.01, 2.0); - // other option for initialisation is rate_roll_filter.set_cutoff_frequency(,); -} -#endif // HELI_FRAME - - // run roll, pitch and yaw rate controllers and send output to motors // targets for these controllers comes from stabilize controllers void @@ -299,6 +284,17 @@ run_rate_controllers() } #if FRAME_CONFIG == HELI_FRAME +// init_rate_controllers - set-up filters for rate controller inputs +void init_rate_controllers() +{ + // initalise low pass filters on rate controller inputs + // 1st parameter is time_step, 2nd parameter is time_constant + rate_roll_filter.set_cutoff_frequency(0.01, 2.0); + rate_pitch_filter.set_cutoff_frequency(0.01, 2.0); + // rate_yaw_filter.set_cutoff_frequency(0.01, 2.0); + // other option for initialisation is rate_roll_filter.set_cutoff_frequency(,); +} + static int16_t get_heli_rate_roll(int32_t target_rate) { diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 5814ae4cd8..ccebaf3b72 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -83,6 +83,22 @@ # define TOY_MIXER TOY_LINEAR_MIXER #endif +///////////////////////////////////////////////////////////////////////////////// +// Bulk defines for TradHeli +#if FRAME_CONFIG == HELI_FRAME + # define RC_FAST_SPEED 125 + # define RTL_YAW YAW_LOOK_AT_HOME + # define TILT_COMPENSATION 5 + # define RATE_INTEGRATOR_LEAK_RATE 0.02 + # define RATE_ROLL_D 0 + # define RATE_PITCH_D 0 + # define HELI_PITCH_FF 0 + # define HELI_ROLL_FF 0 + # define HELI_YAW_FF 0 + # define RC_FAST_SPEED 125 + #endif + + // optical flow doesn't work in SITL yet #ifdef DESKTOP_BUILD # define OPTFLOW DISABLED @@ -111,11 +127,7 @@ // PWM control // default RC speed in Hz #ifndef RC_FAST_SPEED - # if FRAME_CONFIG == HELI_FRAME - # define RC_FAST_SPEED 125 - # else - # define RC_FAST_SPEED 490 - # endif + # define RC_FAST_SPEED 490 #endif //////////////////////////////////////////////////////// @@ -841,11 +853,7 @@ #endif #ifndef TILT_COMPENSATION - # if FRAME_CONFIG == HELI_FRAME - # define TILT_COMPENSATION 5 - # else # define TILT_COMPENSATION 54 - # endif #endif