Blimp: Fix filter initialisation

ArduPilot · Apr 30, 2024 · c4533b0 · c4533b0
1 parent 8bcef96
commit c4533b0
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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)) {