aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 17:49:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Apply LPF to Gazebo's accel data to remove collision detector glitches, minor cleanups

Browse Source
This commit is contained in:
Oleg Kalachev 2023-12-19 04:49:36 +03:00
parent 57e83040b8
commit fcd695fdd7
1 changed files with 4 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gazebo/flix.cpp
View File

@ -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 @@ private:
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 @@ public:
// read imu
rates = flu2frd(imu->AngularVelocity());
acc = flu2frd(imu->LinearAcceleration());
acc = this->accFilter.update(flu2frd(imu->LinearAcceleration()));
// read rc
joystickGet();
@ -220,11 +222,6 @@ public:
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)