Apply LPF to Gazebo's accel data to remove collision detector glitche…

…s, minor cleanups
okalachev · Dec 19, 2023 · fcd695f · fcd695f
1 parent 57e8304
commit fcd695f
Showing 1 changed file with 4 additions and 7 deletions.
diff --git a/gazebo/flix.cpp b/gazebo/flix.cpp
@@ -43,6 +43,8 @@
 #include "control.ino"
 #include "log.ino"
 #include "cli.ino"
+#include "debug.ino"
+#include "lpf.h"
 
 using ignition::math::Vector3d;
 using ignition::math::Pose3d;
@@ -69,7 +71,7 @@ class ModelFlix : public ModelPlugin
 	event::ConnectionPtr updateConnection, resetConnection;
 	transport::NodePtr nodeHandle;
 	transport::PublisherPtr motorPub[4];
-	ofstream log;
+	LowPassFilter<Vector> accFilter = LowPassFilter<Vector>(0.1);
 
 public:
 	void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
@@ -112,7 +114,7 @@ class ModelFlix : public ModelPlugin
 
 		// read imu
 		rates = flu2frd(imu->AngularVelocity());
-		acc = flu2frd(imu->LinearAcceleration());
+		acc = this->accFilter.update(flu2frd(imu->LinearAcceleration()));
 
 		// read rc
 		joystickGet();
@@ -220,11 +222,6 @@ class ModelFlix : public ModelPlugin
 			motorPub[i]->Publish(msg);
 		}
 	}
-
-	void logData() {
-		if (!log.is_open()) return;
-		log << this->model->GetWorld()->SimTime() << "\t" << rollRatePID.derivative << "\t" << pitchRatePID.derivative << "\n";
-	}
 };
 
 GZ_REGISTER_MODEL_PLUGIN(ModelFlix)