diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 08f4c63195..0effee7438 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -612,6 +612,11 @@ int32_t pitch_axis; // Filters AP_LeadFilter xLeadFilter; // Long GPS lag filter AP_LeadFilter yLeadFilter; // Lat GPS lag filter +#if FRAME_CONFIG == HELI_FRAME +// LowPassFilterFloat rate_roll_filter; // Rate Roll filter +// LowPassFilterFloat rate_pitch_filter; // Rate Pitch filter 598 LowPassFilterFloat rate_pitch_filter; // Rate Pitch filter +// LowPassFilterFloat rate_yaw_filter; // Rate Yaw filter 599 LowPassFilterFloat rate_yaw_filter; // Rate Yaw filter +#endif // HELI_FRAME // Barometer filter AverageFilterInt32_Size5 baro_filter; diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index d46dff540f..102c26f39c 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -259,6 +259,23 @@ update_rate_contoller_targets() } } +/////////////////////////////////////////////////////////////////////////////////// +// FILTERS DOES NOT BUILD -- NEEDS UPDATED LOWPASS FILTER LIBRARY +/* +#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_time_constant(0.01, 1.0); + rate_pitch_filter.set_time_constant(0.01, 1.0); + rate_yaw_filter.set_time_constant(0.01, 1.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 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 21334eaf1e..a313b2469e 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -289,6 +289,11 @@ static void init_ardupilot() init_sonar(); #endif +#if FRAME_CONIG == HELI_FRAME +// initialise controller filters +// init_rate_controllers(); +#endif // HELI_FRAME + // initialize commands // ------------------- init_commands();