Takeoff/freefall/landing randomly detected while on ground

[srees]

Using the latest master to run 'make posix gazebo' in an Ubuntu based SITL setup. I get very frequent warnings in commander: TAKEOFF DETECTED FREEFALL DETECTED LANDING DETECTED

If I issue a 'commander arm', the auto-disarm feature nearly immediately disarms the quad because of the takeoff/freefall/landing detection.

I can work around this by modifying the rcS_gazebo_iris file from param set COM_DISARM_LAND 3 to param set COM_DISARM_LAND 0 but the messages are still rather annoying.

Here is a sample of the commander output:

...
pxh> WARN  pwm_out_sim: No valid fds
INFO  LANDING DETECTED
EKF gps is good - setting origin
INFO  home: 47.3977420, 8.5455938, 488.06
INFO  TONE_SET_ALARM 15
INFO  TONE_SET_ALARM 0
INFO  TONE_SET_ALARM 4
INFO  TONE_SET_ALARM 0
INFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977414, 8.5455949, 488.03
INFO  LANDING DETECTED
INFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977411, 8.5455944, 488.07
INFO  LANDING DETECTED
INFO  simulator is slow. Delay added: 18981 us
INFO  simulator is slow. Delay added: 34423 us

Here I start trying to type 'commander arm' but you can see the INFO messages are coming in too fast...

comINFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977411, 8.5455939, 488.17
maINFO  LANDING DETECTED
ndINFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977411, 8.5455939, 488.23
INFO  LANDING DETECTED
er arm
pxh> INFO  home: 47.3977410, 8.5455941, 488.26
INFO  TONE_SET_ALARM 6
INFO  TONE_SET_ALARM 4
INFO  TONE_SET_ALARM 0
INFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977410, 8.5455940, 488.21
INFO  LANDING DETECTED
INFO  [blackbox] stopped (0 drops)
WARN  extended logging: OFF
WARN  time: gps: 0 seconds
WARN  not logging
INFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977410, 8.5455938, 488.16
INFO  LANDING DETECTED
INFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977411, 8.5455934, 486.83
INFO  LANDING DETECTED
INFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977412, 8.5455939, 486.51
INFO  LANDING DETECTED
INFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977413, 8.5455936, 486.08
INFO  LANDING DETECTED
INFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977412, 8.5455932, 486.48
INFO  LANDING DETECTED

It continues like that, but you get the point.

[srees]

I discovered I can also comment out the line that starts the land detector in the RC file, however I'm not sure what other consequences that may have. Are there other critical systems that depend on it?

I'm still learning what parameters I can set to modify it's behavior. I notice I can set them in the RC file, and the defaults are in the respective module's params file if not already in RC.

[srees]

Working on debugging this...freefall is nearly constantly true while the quad is idle. acc_norm is wildly swinging all over the place - I wasn't able to figure out how to print out the actual value (PX4_WARN doesn't seem to take variables), but it's at least swinging below 2 and above 18 constantly, which shouldn't be the case when it's just sitting there not armed.

[srees]

The solution that works for me is to set LNDMC_FFALL_THR 1.0 instead of 2.0 in the RC file. This still detects freefall (simulated by disarming in the air) without triggering constantly.

[RomanBapst]

@srees Hi, thanks for reporting! I assume the acceleration reported by gazebo on the ground is jumping around between values. I wasn't aware that this is a problem with the current SITL models we support. I'll have a look and see if I can reproduce this. I believe the effect on acceleration is caused by the way gazebo models the ground effects.

[srees]

Would the ground effect be an issue when the props aren't even running? Because these messages are happening before it's even armed.

[julianoes]

I see this effect on Snapdragon as well, sitting on the bench. Presumably this happens because the accel signals are noisier.

[marianobianchi]

I'm having this issue when PX4 SITL is in the air. I have the latest master code. I can reproduce this error following these steps: 1) make posix_sitl_default gazebo 2) roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557" 3*) rostopic pub -r 5 /mavros/actuator_control mavros_msgs/ActuatorControl "{header: {}, group_mix: 0, controls: [0,0,0,0,0,0,0,0]}" 4) rosrun mavros mavsys mode -c OFFBOARD 5) rosservice call /mavros/cmd/arming true 6) rostopic pub -r 10 /mavros/setpoint_velocity/cmd_vel geometry_msgs/TwistStamped "{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: ''}, twist: {linear: {x: 0.0, y: 0.0, z: 1.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}}"

After running all these commands, px4 SITL takeoff and goes up without stopping. After a random amount of time, mavros console and px4 command window show a message like "LANDING DETECTED" or "FREEFALL DETECTED" and the UAV falls to the ground.

I had the same problem using OFFBOARD mode and using "commander takeoff" command in px4 console. After a while, i get a "landing" message...

* Needed to make OFFBOARD mode work

[AndreasAntener]

@LorenzMeier I don't know what is wrong but I have 2 different effects locally. Like this, the SITL tests can't work.

Mac: freefall/landed detections as described here

EKF gps is good - setting origin
[Wrn] [Publisher.cc:140] Queue limit reached for topic motor_speed/1, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:140] Queue limit reached for topic motor_speed/2, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:140] Queue limit reached for topic motor_speed/3, deleting message. This warning is printed only once.

pxh> 
pxh> commander takeoff
WARN  rejecting takeoff, no position lock yet. Please retry..
pxh> 
pxh> INFO  TAKEOFF DETECTED
INFO  home: 47.3977388, 8.5456007, 488.97
INFO  TONE_SET_ALARM 15
INFO  TONE_SET_ALARM 0
INFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977419, 8.5455934, 488.63
INFO  TAKEOFF DETECTED
INFO  TONE_SET_ALARM 4
INFO  TONE_SET_ALARM 0
INFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977417, 8.5455937, 488.14
INFO  TAKEOFF DETECTED
INFO  TAKEOFF DETECTED
INFO  FREEFALL DETECTED
INFO  home: 47.3977419, 8.5455934, 488.15
INFO  TAKEOFF DETECTED

Ubuntu VM:

EKF gps is good - setting origin
INFO  TONE_SET_ALARM 9
WARN  Not ready to fly: Sensors not set up correctly
INFO  home: 47.3977418, 8.5455941, 487.77
INFO  TONE_SET_ALARM 15
INFO  TONE_SET_ALARM 0

[LorenzMeier]

[LorenzMeier]

The core issue is the ground contact of the sim. @tfoote It would be really good to have some quality time here. This is really killing the accuracy of the sim.

[LorenzMeier]

Printing this within the Gazebo plugin actually shows that it exactly alternates:

lin acc: -0.198488 -1.933560 18.085363
lin acc: -0.277321 -0.594489 1.647138
lin acc: -0.015548 2.035408 18.108961
lin acc: -0.042065 0.761172 1.756472
lin acc: -0.192849 -1.954831 18.324433
lin acc: -0.101776 -0.797659 1.633916
lin acc: -0.046431 1.943193 18.089173
lin acc: -0.083871 0.780900 1.703190
lin acc: -0.027410 -1.725375 18.133799
lin acc: -0.118629 -0.723878 1.645476
lin acc: -0.094787 1.852111 17.997702
lin acc: 0.043736 0.778432 1.615919
lin acc: 0.051530 -1.923788 18.073524
lin acc: -0.188048 -0.621323 1.602220
lin acc: -0.013476 1.961118 18.026240
lin acc: -0.064393 0.700686 1.559141
lin acc: -0.001125 -1.812743 17.845782
lin acc: -0.020460 -0.772475 1.531991
lin acc: 0.016386 1.856952 18.068799
lin acc: -0.121395 0.701988 1.621191
lin acc: -0.249548 -1.875844 17.987069
lin acc: 0.096619 -0.793055 1.712656
lin acc: 0.033620 1.975753 18.092358
lin acc: -0.025552 0.933790 1.673404
lin acc: -0.084782 -1.920315 18.134187
lin acc: -0.038967 -0.688117 1.569538
lin acc: -0.094778 2.006493 18.021437
lin acc: 0.009335 0.773514 1.466172
lin acc: -0.171490 -1.893713 18.163118
lin acc: 0.037477 -0.610275 1.796728
lin acc: -0.027356 2.143462 18.063668
lin acc: -0.094987 0.785441 1.412388
lin acc: -0.088647 -1.726456 18.127161
lin acc: -0.114389 -0.691851 1.582884
lin acc: -0.228852 1.949600 17.974510
lin acc: 0.019595 0.810883 1.459718
lin acc: -0.167628 -1.766273 17.966068
lin acc: -0.086669 -0.618806 1.695940
lin acc: -0.062853 2.169208 17.989689
lin acc: -0.064365 0.707138 1.557123
lin acc: -0.009867 -1.904246 18.078387
lin acc: -0.046596 -0.673256 1.706346
lin acc: 0.018536 2.153671 18.068212
lin acc: -0.070220 0.889857 1.726171
lin acc: -0.085360 -1.693041 18.069774

[LorenzMeier]

@tfoote I pulled this from:

math::Vector3 acceleration_I = link_->GetRelativeLinearAccel() - C_W_I.RotateVectorReverse(gravity_W_);

  printf("lin acc: %f %f %f\n", acceleration_I.x, acceleration_I.y, acceleration_I.z);

[tfoote]

This looks similar to https://github.com/Dronecode/sitl_gazebo/issues/29 which @hsu was unable to reproduce.

From @hsu

I loaded the plane model and rqt_plot /plane/imu/linear_acceleration/z, this is what I see:

​ Similarly for the tail sitter: ​ Maybe I could try to excite the model with the controllers somehow?

full disclosure, I am running gazebo default-ish branch (with some plotting customization for my own use), rotors_simulator and sitl_gazebo are both on master branch, gazebo_ros_pkgs on jade-devel branch.

I'm not familiar with what's going on under the hood but this looks to me like some sort of discritization issue.

[marianobianchi]

It is working correctly in the latest release but i have another bug. When running "make posix_sitl_default gazebo" i can't use the commander "takeoff" command. All i get when using it is "WARN rejecting takeoff, no position lock yet. Please retry.."

[dagar]

@marianobianchi wait until you see this message

EKF gps is good - setting origin
INFO  home: 47.3977416, 8.5455940, 487.92

[marianobianchi]

Yeah, i wait it for a long time but it didn't show up...

[pickledgator]

More debugging details from within the imu plugin, but not quite sure how gazebo is running this internally. Clear oscillation in the accel measurement.

printf("z: %0.3f\n", link_->GetRelativeLinearAccel()[2]);

z: 8.235
z: -8.237
z: 8.236
z: -8.234
z: 8.235
z: -8.237
z: 8.236
z: -8.234
z: 8.235
z: -8.237
z: 8.236

Looks like there is also oscillatory noise in the relative pose of the origin of the imu sensor, though the GetWorldPose().pos[2] shows that imu origin starts at 0.105m, so how this is resulting in the acceleration flipping signs is still beyond me. I thought initially that the origin of the imu may be oscillating around the ground plane, but this isn't necessarily true since the world origin is at 0.105.

auto pose = link_->GetRelativePose();
printf("pose: %0.3f %0.3f %0.15f\n", pose.pos[0], pose.pos[1], pose.pos[2]);

pose: -0.000 -0.000 -0.000000003915947
pose: -0.000 -0.000 0.000000001952448
pose: -0.000 0.000 -0.000000003016830
pose: -0.000 0.000 0.000000000997452
pose: -0.000 -0.000 -0.000000003915946
pose: -0.000 -0.000 0.000000001952449
pose: -0.000 0.000 -0.000000003016829
pose: -0.000 0.000 0.000000000997453

[hsu]

here's a patch to get better contact by assigning a small margin and capping impulse for interpenetration recovery (use position teleporting instead):

diff --git a/models/rotors_description/urdf/multirotor_base.xacro b/models/rotors_description/urdf/multirotor_base.xacro
index 40be30f..ca74bf2 100644
--- a/models/rotors_description/urdf/multirotor_base.xacro
+++ b/models/rotors_description/urdf/multirotor_base.xacro
@@ -63,6 +63,21 @@
         <rotorVelocitySlowdownSim>${rotor_velocity_slowdown_sim}</rotorVelocitySlowdownSim>
       </plugin>
     </gazebo>
+    <gazebo reference="base_link">
+      <collision name='base_link_inertia_collision'>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0.0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+    </gazebo>

     <gazebo reference="base_link">
       <material>Gazebo/${color}</material>
diff --git a/src/gazebo_imu_plugin.cpp b/src/gazebo_imu_plugin.cpp
index 5a08562..b3ebd8c 100644
--- a/src/gazebo_imu_plugin.cpp
+++ b/src/gazebo_imu_plugin.cpp
@@ -274,6 +274,8 @@ void GazeboImuPlugin::OnUpdate(const common::UpdateInfo& _info) {
   math::Vector3 acceleration_I = link_->GetRelativeLinearAccel() - C_W_I.RotateVectorReverse(gravity_W_);
 #endif

+  gzerr << acceleration_I << "\n";
+
   math::Vector3 angular_vel_I = link_->GetRelativeAngularVel();

   Eigen::Vector3d linear_acceleration_I(acceleration_I.x,

[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002032 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002031 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002031 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002031 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002031 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002031 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002031 9.8066
[Err] [gazebo_imu_plugin.cpp:278] -0.006716 -0.002031 9.8066

For reference, I am compiling against gazebo7, but with the imu sensor patch:

$ hg sum
parent: 30608:ef2622588113 
 merge gazebo7
branch: issue_1959_gazebo7
commit: 1641 unknown (clean)
update: (current)
mq:     (empty queue)

next step is to figure out why I can't get a location lock, and maybe if we should try and switch to using a gazebo ImuSensor.

[julianoes]

@hsu: can you make a PR with this change against the actual repo?

[LorenzMeier]

I've applied this to the SITL repo, testing.

[LorenzMeier]

Ok, this fixes it. @hsu Now on to the plane model!