Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 19:47:50 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

Fix incorrect in-flight acceleration measurements in Gazebo (#2128)

Browse Source 
* Fix Gazebo acceleration measurement

Gazebo's GetWorldLinearAccel() did not seem to take the ardrone's
velocity_decay into account, instead it measured the drone's
acceleration *without drag* which resulted in incorrect measurements
in-flight (the drone's acceleration in world frame did not go to
zero when the drone reached its steady-state velocity).

Fixed by manually deriving the drone's world velocity over time,
as in [Gazebo's own IMU implementation](https://bitbucket.org/osrf/gazebo/src/e26144434b932b4b6a760ddaa19cfcf9f1734748/gazebo/sensors/ImuSensor.cc?at=default&fileviewer=file-view-default#ImuSensor.cc-370).

Created an issue for Gazebo [here](https://bitbucket.org/osrf/gazebo/issues/2363/linear-velocity_decay-not-included-in-link).

* Remove unnecessary whitespace changes

* Make dt (nps_fdm_gazebo.cpp:338) double instead of float
This commit is contained in:
Tom van Dijk
2017-10-20 12:33:32 +00:00
committed by Kirk Scheper
parent bc1434ae39
commit a710548b6a
1 changed files with 12 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/nps/nps_fdm_gazebo.cpp
+12 -1
View File
@@ -316,10 +316,12 @@ static void init_gazebo(void)
*/
static void gazebo_read(void)
{
static gazebo::math::Vector3 vel_prev;
static double time_prev;
gazebo::physics::WorldPtr world = model->GetWorld();
gazebo::math::Pose pose = model->GetWorldPose(); // In LOCAL xyz frame
gazebo::math::Vector3 vel = model->GetWorldLinearVel();
gazebo::math::Vector3 accel = model->GetWorldLinearAccel();
gazebo::math::Vector3 ang_vel = model->GetWorldAngularVel();
gazebo::common::SphericalCoordinatesPtr sphere =
world->GetSphericalCoordinates();
@@ -329,6 +331,15 @@ static void gazebo_read(void)
/* Fill FDM struct */
fdm.time = world->GetSimTime().Double();
// Find world acceleration by differentiating velocity
// model->GetWorldLinearAccel() does not seem to take the velocity_decay into account!
// Derivation of the velocity also follows the IMU implementation of Gazebo itself:
// https://bitbucket.org/osrf/gazebo/src/e26144434b932b4b6a760ddaa19cfcf9f1734748/gazebo/sensors/ImuSensor.cc?at=default&fileviewer=file-view-default#ImuSensor.cc-370
double dt = fdm.time - time_prev;
gazebo::math::Vector3 accel = (vel - vel_prev) / dt;
vel_prev = vel;
time_prev = fdm.time;
// init_dt: unused
// curr_dt: unused
// on_ground: unused