From 07d7d908826b0cd650b72c31a4d45ba0b8407672 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Mon, 26 Nov 2012 21:50:39 -0500 Subject: [PATCH] ACM: TradHeli Bringing in Low Pass Filter for Rate Controllers --- ArduCopter/APM_Config.h | 2 +- ArduCopter/ArduCopter.pde | 7 ++++--- ArduCopter/Attitude.pde | 10 ++++------ ArduCopter/system.pde | 2 +- 4 files changed, 10 insertions(+), 11 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 386e9fe1e0..4e94f38145 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -17,7 +17,7 @@ //#define DMP_ENABLED ENABLED //#define SECONDARY_DMP_ENABLED ENABLED // allows running DMP in parallel with DCM for testing purposes -#define FRAME_CONFIG HELI_FRAME +//#define FRAME_CONFIG QUAD_FRAME /* * options: * QUAD_FRAME diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e5091cde67..287801ed7b 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -103,6 +103,7 @@ #include // Mode Filter from Filter library #include // Mode Filter from Filter library #include // GPS Lead filter +#include // Low Pass Filter library #include // APM relay #include // Photo or video camera #include // Camera/Antenna mount @@ -613,9 +614,9 @@ int32_t pitch_axis; 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 +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 diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 102c26f39c..e74934f929 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -259,9 +259,7 @@ 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() @@ -274,7 +272,7 @@ void init_rate_controllers() // 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 @@ -308,7 +306,7 @@ get_heli_rate_roll(int32_t target_rate) current_rate = (omega.x * DEGX100); // filter input - // current_rate = rate_roll_filter.apply(current_rate); + current_rate = rate_roll_filter.apply(current_rate); // call pid controller rate_error = target_rate - (omega.x * DEGX100); @@ -361,7 +359,7 @@ get_heli_rate_pitch(int32_t target_rate) current_rate = (omega.y * DEGX100); // filter input - // current_rate = rate_pitch_filter.apply(current_rate); + current_rate = rate_pitch_filter.apply(current_rate); // call pid controller rate_error = target_rate - (omega.y * DEGX100); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index a313b2469e..86e97b3723 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -291,7 +291,7 @@ static void init_ardupilot() #if FRAME_CONIG == HELI_FRAME // initialise controller filters -// init_rate_controllers(); +init_rate_controllers(); #endif // HELI_FRAME // initialize commands