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 "control.ino"
#include "log.ino" #include "log.ino"
#include "cli.ino" #include "cli.ino"
#include "debug.ino"
#include "lpf.h"
using ignition::math::Vector3d; using ignition::math::Vector3d;
using ignition::math::Pose3d; using ignition::math::Pose3d;
@ -69,7 +71,7 @@ private:
event::ConnectionPtr updateConnection, resetConnection; event::ConnectionPtr updateConnection, resetConnection;
transport::NodePtr nodeHandle; transport::NodePtr nodeHandle;
transport::PublisherPtr motorPub[4]; transport::PublisherPtr motorPub[4];
ofstream log; LowPassFilter<Vector> accFilter = LowPassFilter<Vector>(0.1);
public: public:
void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
@ -112,7 +114,7 @@ public:
// read imu // read imu
rates = flu2frd(imu->AngularVelocity()); rates = flu2frd(imu->AngularVelocity());
acc = flu2frd(imu->LinearAcceleration()); acc = this->accFilter.update(flu2frd(imu->LinearAcceleration()));
// read rc // read rc
joystickGet(); joystickGet();
@ -220,11 +222,6 @@ public:
motorPub[i]->Publish(msg); 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) GZ_REGISTER_MODEL_PLUGIN(ModelFlix)