From c4533b0e362412ed8139bcd45375178d22c17b8f Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Tue, 30 Apr 2024 19:07:04 +1000 Subject: [PATCH] Blimp: Fix filter initialisation --- Blimp/Blimp.cpp | 20 ++++++++++++++++---- Blimp/Blimp.h | 7 ++++--- Blimp/system.cpp | 9 +++++---- 3 files changed, 25 insertions(+), 11 deletions(-) diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 4379fd34e69028..0798e678d5c37d 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -213,6 +213,14 @@ void Blimp::one_hz_loop() AP_Notify::flags.flying = !ap.land_complete; + loiter->pid_vel_x.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + loiter->pid_vel_y.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + loiter->pid_vel_z.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + loiter->pid_vel_yaw.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + loiter->pid_pos_x.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + loiter->pid_pos_y.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + loiter->pid_pos_z.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + loiter->pid_pos_yaw.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); } void Blimp::read_AHRS(void) @@ -222,11 +230,15 @@ void Blimp::read_AHRS(void) IGNORE_RETURN(ahrs.get_velocity_NED(vel_ned)); IGNORE_RETURN(ahrs.get_relative_position_NED_origin(pos_ned)); - vel_yaw = ahrs.get_yaw_rate_earth(); - // Vector2f vel_xy_filtd = vel_xy_filter.apply({vel_ned.x, vel_ned.y}); - vel_ned_filtd = vel_ned; //{vel_xy_filtd.x, vel_xy_filtd.y, vel_z_filter.apply(vel_ned.z)}; - vel_yaw_filtd = vel_yaw; //vel_yaw_filter.apply(vel_yaw); + + if (g2.frame_class.get() == Fins::MOTOR_FRAME_FISHBLIMP) { + vel_ned_filtd = {vel_x_filter.apply(vel_ned.x), vel_y_filter.apply(vel_ned.y), vel_z_filter.apply(vel_ned.z)}; + vel_yaw_filtd = vel_yaw_filter.apply(vel_yaw); + } else { + vel_ned_filtd = vel_ned; + vel_yaw_filtd = vel_yaw; + } #if HAL_LOGGING_ENABLED AP::logger().WriteStreaming("VNF", "TimeUS,X,XF,Y,YF,Z,ZF,Yaw,YawF,PX,PY,PZ,PYaw", "Qffffffffffff", diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index fc94dc179e6eba..f4c894f8355d48 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -217,9 +217,10 @@ class Blimp : public AP_Vehicle Vector3f pos_ned; float vel_yaw; float vel_yaw_filtd; - // NotchFilterVector2f vel_xy_filter; - // NotchFilterFloat vel_z_filter; - // NotchFilterFloat vel_yaw_filter; + NotchFilterFloat vel_x_filter; + NotchFilterFloat vel_y_filter; + NotchFilterFloat vel_z_filter; + NotchFilterFloat vel_yaw_filter; // Inertial Navigation AP_InertialNav inertial_nav; diff --git a/Blimp/system.cpp b/Blimp/system.cpp index 987716d5ecc0df..202a21be17129c 100644 --- a/Blimp/system.cpp +++ b/Blimp/system.cpp @@ -94,10 +94,11 @@ void Blimp::init_ardupilot() enable_motor_output(); } - //Initialise fin filters - // vel_xy_filter.init(scheduler.get_loop_rate_hz(), motors->freq_hz, 0.5f, 15.0f); - // vel_z_filter.init(scheduler.get_loop_rate_hz(), motors->freq_hz, 1.0f, 15.0f); - // vel_yaw_filter.init(scheduler.get_loop_rate_hz(),motors->freq_hz, 5.0f, 15.0f); + //Initialise velocity filters + vel_x_filter.init(scheduler.get_loop_rate_hz(), motors->freq_hz, 0.5f, 15.0f); + vel_y_filter.init(scheduler.get_loop_rate_hz(), motors->freq_hz, 0.5f, 15.0f); + vel_z_filter.init(scheduler.get_loop_rate_hz(), motors->freq_hz, 1.0f, 15.0f); + vel_yaw_filter.init(scheduler.get_loop_rate_hz(), motors->freq_hz, 5.0f, 15.0f); // attempt to switch to MANUAL, if this fails then switch to Land if (!set_mode((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED)) {