Situation Awareness - Free Fall Prevention

[Konstantin]

Hey Crazyfliers!

Going through the firmware I discovered a module, which is disabled by default, but sounds really interesting! It's the SITAW.C module, which is meant to detect a free fall and/or a tumbled situation while the drone is in the air. As long as a free fall is detected the module triggers the alt-hold mode, if the drone is not in a tumbled situation. It can easily enabled by removing the comment-doubleslashes in line 49:

#define SITAW_ENABLED

The first test (i just threw the crazyflie in the air) revealed, that the modules can detect a free fall or a tumbled situation. Good Job so far. The very problem (which is pointed out in the file-comments): "as soon as althold is enabled, we are not freefalling anymore". That means, the motors are activated and deactivated within milliseconds (cause as soon as there is no free fall anymore, the motors shut off). The Crazyfie can't be stabilized and it falls down with 99% of the original speed. Most times the CF flips due to a slightly stronger push of one motor. I understood, why it's deactivated by default.


But what a great opportunity, if the modules leads to a smooth landing! Lost connection while the CF is high in the air? No broken motormounts anymore. A strong windblow wants to flip your CF? Alright, it can be stabilized by SITAW.

But how to get the althold mode activated for - well, lets say 2 seconds? Maybe a timer would be useful. Fortunately there is already a good timer implemented in commander.c (there it is used to detect a timeout). So i decided to implement it into sitaw.c. The result: As I threw the CF in the air, it hovered autonomous for 2 seconds before falling down! After a second of falling the module was activated again, stabilizing the drone for a second time (this time 2 meters closer to the ground).

By implementing the Tumbled-Situation-Detection into this timer, the thrust is cut off immediately if the pitch/role-angle is over 60°. Useful, if the drone is upside down (I learned that the hard way - upside down ground crawling with 100% thrust costs motors and propellers..)

So far so good. I went outside and threw the CF up in the air. BAD IDEA! As the module detected the free fall, it activated the althold - and made the CF rise up in the air with constant speed and no control! After 2 seconds, the CF fell off the sky again, but after a short delay the CF rose again, this time with lower, but significant speed. I was able to catch the CF and bounded it to a cord. I tried that a few times, sometimes it worked well, sometimes the CF rose up in the air.

Conclusion:

- The SITAW module actually works! Tumbled Situation (pitch/role-angles over 60° will cut the thrust
- The control of the althold mode needs improvements. I used a timer (a free fall will activate the althold for 2 seconds)
- ONLY USE INDOORS! Activating the modified SITAW outside can lead to a uncontrollable flight!
- (you need to connect the CF to a client once to activate the SITAW module)

This is the first time I tried some programming (and my first post in this forum), so this solution might be a bit clumsy. Tell me what you think about the solution (hopefully in the attachments) and throw in some ideas!

[PierreD]

Hi Konstantin,
What you did seems really interesting !

From this basis, it would be possible to implement other states than FreeFall or tumbled situation.
For example, it would be interesting to detect a connection lost and then to use the althold mode to automatically land the crazyflie, rather than just shutting down the motors.

But of course as you said, it's better to test it indoor, 'cause mistakes outdoor are far more stressful !
Few weeks ago, I was playing with the hover mode, and just like you the quad rose into the sky at full speed.. Fortunately, I was able to easily make it stop, but I panicked for a few seconds !

Moreover, your work interest me because I modified the client in order to activate the althold mode with the alt1 button. (And the cf firmware to be able to take-off).
My idea was to enable the althold mode by pressing and releasing the alt1 button, and then disable the althold mode by pressing alt1 again.
It worked pretty well, but in case of crash it was horrible since the motors were still active.

So it could be possible to prevent that thanks to situation awareness !

[Konstantin]

Hey Pierre,

I'm happy you are interested in this "mod",too
What you wrote about the loss of connection is exactly the point that interests me. I tested the situation awareness (indoors) after a disconnect from the client and it worked. So if the CF should be disconnected while up in the air, the SitAw will do its job. My own motivation is: I consider to pimp up my CF with a BigQuad, but then a crash would me a bit more expensive. So I wanna make sure to have a "Failsafe" like the SitAw installed.

Your work seems interesting to me, too. What you described sounds like a "take-off" button, which rises the CF to a certain altitude. Did i get that right? I think this would be a good thing! I didn't discover how to modify the CFclient yet, so I am very interested in your work and what exactly you have done. =)

Considering the SITAW-mod: At the moment I try to implement a trigger, that prevents the CF from rising up while the FreeFall timer is running. That would make it possible to test the Mod outside safely.

Feel free to tell me about your work on the CFclient, I would be highly interested! (Maybe in a PM, 'cause it's another topic)

[PierreD]

Hi Konstantin,
I'm a bit busy this week, but I'll answer to you this week-end or at the beginning of next week!

[Konstantin]

Hey Pierre,

nice to hear from you!
Update 4 the Sitaw-modul: Currently I'm working on the implementation of the estimated altitude from position_estimator_altitude.c to detect, if the CF is near the ground (which would lead to a cut of the thrust). As this works, the CF would be able to modulate its downward speed depending on the above-ground-level. Here is a short overview on how the code looks like atm: (Tu = tumbled Situation)

1. in the first seconds the CF is connected to the Client --> set groundlevel = estimatedZ(altitude)
2. if (FreeFallDetected and no TuDetected) -> start timer
3. While (timer is running and no TuDetected) ->
if ( 2 <(estimatedZ - groundlevel) < 3 meter) -> fast downwardflight
if ( 1 <(estimatedZ - groundlevel) < 2 meter) -> moderate downwardflight
if ( 0.2 <(estimatedZ - groundlevel) < 1 meter) -> slow downwardflight
if ((estimatedZ - groundlevel) < 0.2 meter) -> cut thrust and deactivate timer

On the coding site this seems to work. I am able to build the Code and flash it to the CF. But as soon as the sensor-calibration is finished, the CF restarts and the red led blinks 5 times. The Client console says: System restarted after watchdog timeout. I guess, this means, that there is a loop in my code or the Code is too complex (which is be the case, I think). As soon as I get rid of this nasty error I'll post the (hopefully) functional Code so you can try it out =)

[PierreD]

Quick reply :
Maybe you can try to build the firmware on debug mode. Apparently it can solve this type of issue if it's not because of your code.
Worth the try !

One question : do you "initialize" the ground altitude? Because the barometer gives you the altitude at sea level.
Maybe you'll face a problem with the altitude estimation when the CF is close to the ground. Because of the wind, the value can fluctuate a lot but I don't remember in which direction. The foam will reduce this effect I think.
Hopefully, the altitude will be lowered when close to the ground and you'll be able to land ! : D
I hope it will work!

EDIT : do you have a GitHub repo?

[Konstantin]

Unfortunately I don't have the debug adapter board, which is need for debug (i think). So i try to do small changes, build, flash to CF, repeat.
This goes slow, but it's the only option i have atm.

You are right, hovering close to the Ground has an influence on the estimated Altitude, but a very positive one! =) The motors, close to the ground, add a small amount of pressure to the sensors. Higher pressure = lower estimated Altitude. My code contains the command: if altitude < (Groundlevel + 0.2) --> cut Thrust. So the hover-effect triggers the code. In the End, the Thrust will be cut as the effect appears and this means, the CF is very close to the ground when the thrust is cut off. A nice effect in my opinion =)

To give you a detailed view on the code i created a Github repo named "Crazyflie-SitAw-Implementation", BUT i didn't figure out, how to paste the code into this github repository yet :/

EDIT: I added the important files to the GIthub repo, the interesting part is in sitaw.c in line 149
EDIT2: If you change line 126 in sitaw.c into "#if defined(xSITAW_ENABLED)" (so that the sitAwPostStateUpdateCallout is empty), there is no watchdog-timeout (#funfact)

[PierreD]

When you compile the firmware, use the "Make Debug" link. If I remember well, it should skip some testing.

I'm glad to ear that the ground effect is positive !

[Konstantin]

Hey Pierre and all those who might be interested in this,

finally I managed to make the module functional! In the current version (https://github.com/Koschu/Crazyflie-Sit ... ementation) the SitAw module detects a free-fall-situation and initiates a descent, depending on the current altitude. As the CF reaches the groundlevel, the thrust is cut and the CF "lands". Of course the estimated Altitude underlies some value fluctuations, so sometimes the Thrust is cut off 10cm over actual groundlevel, sometimes it bumps slightly over the ground.

If you want to try it out by yourself, here some (important) instructions:
1. After flashing the firmware, restart the CF, connect to the Client and put it on the floor. After some seconds the Groundlevel is set (to check this I added some Logs for the plotter, just activate that in settings-->logging configuration. High = measured Altitude, Start Altitude = Groundlevel, Difference = Distance to Ground)
2. (for safety) put a towel or something on the ground. pick up the CF, place it on your flat hand and throw it in the air with caution =) enjoy the more or less smooth landing =)
Timer is set to 4 seconds.


Annotation: This is tested inside! The estimated altitude underlies value fluctuations, which might be intensive outdoors. So have caution if testing outdoors and maybe set another timer-interval (4 seconds may be too short)

[PierreD]

Happy to ear that!
Could you explain (very quickly) what was the reason of the watchdog issue?

I'll try that as soon as I get my hands back on a crazyflie!

Unfortunately It won't be this week.