From 4faa57074a7b992afe7dac0a68b8313def797891 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Jul 2016 14:47:47 +1000 Subject: [PATCH] SITL: removed vehicle specific ground handling --- libraries/SITL/SIM_Helicopter.cpp | 11 ----------- libraries/SITL/SIM_Multicopter.cpp | 9 --------- libraries/SITL/SIM_SingleCopter.cpp | 11 ----------- 3 files changed, 31 deletions(-) diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index 5de365b2b3..a11d53c6f1 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/libraries/SITL/SIM_Helicopter.cpp @@ -162,17 +162,6 @@ void Helicopter::update(const struct sitl_input &input) update_dynamics(rot_accel); - // constrain height to the ground - if (on_ground(position) && !was_on_ground) { - // zero roll/pitch, but keep yaw - float r, p, y; - dcm.to_euler(&r, &p, &y); - dcm.from_euler(0, 0, y); - - position.z = -(ground_level + frame_height - home.alt*0.01f + ground_height_difference); - velocity_ef.zero(); - } - // update lat/lon/altitude update_position(); diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 33990b8874..5b307a6e69 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -62,15 +62,6 @@ void MultiCopter::update(const struct sitl_input &input) update_dynamics(rot_accel); - if (on_ground(position)) { - // zero roll/pitch, but keep yaw - float r, p, y; - dcm.to_euler(&r, &p, &y); - dcm.from_euler(0, 0, y); - - position.z = -(ground_level + frame_height - home.alt*0.01f + ground_height_difference); - } - // update lat/lon/altitude update_position(); diff --git a/libraries/SITL/SIM_SingleCopter.cpp b/libraries/SITL/SIM_SingleCopter.cpp index 1a565adf91..52e6638351 100644 --- a/libraries/SITL/SIM_SingleCopter.cpp +++ b/libraries/SITL/SIM_SingleCopter.cpp @@ -102,17 +102,6 @@ void SingleCopter::update(const struct sitl_input &input) update_dynamics(rot_accel); - // constrain height to the ground - if (on_ground(position) && !was_on_ground) { - // zero roll/pitch, but keep yaw - float r, p, y; - dcm.to_euler(&r, &p, &y); - dcm.from_euler(0, 0, y); - - position.z = -(ground_level + frame_height - home.alt*0.01f + ground_height_difference); - velocity_ef.zero(); - } - // update lat/lon/altitude update_position();