From 2a5c6d3eec337d2bf1c6da11b5f4efe030120846 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 15 Sep 2023 21:51:20 +0200 Subject: [PATCH] [modules] Approach moving target filter (#3067) --- conf/airframes/tudelft/nederdrone8.xml | 1 + conf/modules/approach_moving_target.xml | 5 +- .../modules/ctrl/approach_moving_target.c | 62 ++++++++++++++++++- .../modules/ctrl/approach_moving_target.h | 2 + 4 files changed, 68 insertions(+), 2 deletions(-) diff --git a/conf/airframes/tudelft/nederdrone8.xml b/conf/airframes/tudelft/nederdrone8.xml index b28d3d3b94..9b5f74ef15 100644 --- a/conf/airframes/tudelft/nederdrone8.xml +++ b/conf/airframes/tudelft/nederdrone8.xml @@ -90,6 +90,7 @@ + diff --git a/conf/modules/approach_moving_target.xml b/conf/modules/approach_moving_target.xml index f801b933f8..7f3e968e48 100644 --- a/conf/modules/approach_moving_target.xml +++ b/conf/modules/approach_moving_target.xml @@ -5,6 +5,8 @@ Approach a moving target (e.g. ship) along a diagonal. + + @@ -13,10 +15,11 @@ - + + diff --git a/sw/airborne/modules/ctrl/approach_moving_target.c b/sw/airborne/modules/ctrl/approach_moving_target.c index 871783f577..62895ce227 100644 --- a/sw/airborne/modules/ctrl/approach_moving_target.c +++ b/sw/airborne/modules/ctrl/approach_moving_target.c @@ -27,11 +27,29 @@ #include "generated/modules.h" #include "modules/core/abi.h" +#include "filters/low_pass_filter.h" + + +#ifndef AMT_ERR_SLOWDOWN_GAIN +#define AMT_ERR_SLOWDOWN_GAIN 0.25 +#endif + +// Filter value in Hz +#ifndef CUTOFF_FREQ_FILTERS_HZ +#define CUTOFF_FREQ_FILTERS_HZ 0.5 +#endif + float amt_err_slowdown_gain = AMT_ERR_SLOWDOWN_GAIN; float approach_moving_target_angle_deg; +float cutoff_freq_filters_hz = CUTOFF_FREQ_FILTERS_HZ; + +Butterworth2LowPass target_pos_filt[3]; +Butterworth2LowPass target_vel_filt[3]; +Butterworth2LowPass target_heading_filt; + #define DEBUG_AMT TRUE #include @@ -75,11 +93,25 @@ static void send_approach_moving_target(struct transport_tx *trans, struct link_ void approach_moving_target_init(void) { + approach_moving_target_set_low_pass_freq(cutoff_freq_filters_hz); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_APPROACH_MOVING_TARGET, send_approach_moving_target); #endif } +void approach_moving_target_set_low_pass_freq(float filter_freq) { + cutoff_freq_filters_hz = filter_freq; + // tau = 1/(2*pi*Fc) + float tau = 1.0 / (2.0 * M_PI * cutoff_freq_filters_hz); + float sample_time = 1.0 / FOLLOW_DIAGONAL_APPROACH_FREQ; + for (uint8_t i = 0; i < 3; i++) { + init_butterworth_2_low_pass(&target_pos_filt[i], tau, sample_time, 0.0); + init_butterworth_2_low_pass(&target_vel_filt[i], tau, sample_time, 0.0); + } + init_butterworth_2_low_pass(&target_heading_filt, tau, sample_time, 0.0); + +} + // Update a waypoint such that you can see on the GCS where the drone wants to go void update_waypoint(uint8_t wp_id, struct FloatVect3 * target_ned) { @@ -127,11 +159,39 @@ void follow_diagonal_approach(void) { return; } target_get_vel(&target_vel); + + // filter target values + update_butterworth_2_low_pass(&target_pos_filt[0], target_pos.x); + update_butterworth_2_low_pass(&target_pos_filt[1], target_pos.y); + update_butterworth_2_low_pass(&target_pos_filt[2], target_pos.z); + + update_butterworth_2_low_pass(&target_vel_filt[0], target_vel.x); + update_butterworth_2_low_pass(&target_vel_filt[1], target_vel.y); + update_butterworth_2_low_pass(&target_vel_filt[2], target_vel.z); + + // just filtering the heading will go wrong if it wraps, instead do: + // take the diff with current heading_filtered, wrap that, add to current, insert in filter and wrap again? + // example: target_heading - target_heading_filt (170 - -170 = 340), -> -20 -> -190 -> into filter -> wrap + float heading_diff = target_heading - target_heading_filt.o[0]; + FLOAT_ANGLE_NORMALIZE(heading_diff); + float target_heading_input = target_heading_filt.o[0] + heading_diff; + update_butterworth_2_low_pass(&target_heading_filt, target_heading_input); + float target_heading_filt_wrapped = target_heading_filt.o[0]; + FLOAT_ANGLE_NORMALIZE(target_heading_filt_wrapped); + + target_pos.x = target_pos_filt[0].o[0]; + target_pos.y = target_pos_filt[1].o[0]; + target_pos.z = target_pos_filt[2].o[0]; + + target_vel.x = target_vel_filt[0].o[0]; + target_vel.y = target_vel_filt[1].o[0]; + target_vel.z = target_vel_filt[2].o[0]; + VECT3_SMUL(target_vel, target_vel, amt.speed_gain); // Reference model float gamma_ref = RadOfDeg(amt.slope_ref); - float psi_ref = RadOfDeg(target_heading + amt.psi_ref); + float psi_ref = RadOfDeg(target_heading_filt_wrapped + amt.psi_ref); amt.rel_unit_vec.x = cosf(gamma_ref) * cosf(psi_ref); amt.rel_unit_vec.y = cosf(gamma_ref) * sinf(psi_ref); diff --git a/sw/airborne/modules/ctrl/approach_moving_target.h b/sw/airborne/modules/ctrl/approach_moving_target.h index 0c7097a064..f48be866ac 100644 --- a/sw/airborne/modules/ctrl/approach_moving_target.h +++ b/sw/airborne/modules/ctrl/approach_moving_target.h @@ -47,9 +47,11 @@ struct Amt { extern struct Amt amt; extern float amt_err_slowdown_gain; +extern float cutoff_freq_filters_hz; extern void approach_moving_target_init(void); extern void follow_diagonal_approach(void); extern void approach_moving_target_enable(uint8_t wp_id); +extern void approach_moving_target_set_low_pass_freq(float filter_freq); #endif // APPROACH_MOVING_TARGET_H