Free fall detection in firmware (stabilizer.c)

[Johannes]

Hello,

I am currently trying to implement a very basic free fall detection. I basically just check the current value of vSpeedAcc and after a certain threshold I set the actuatorThrust to the maximum.
Flashing the firmware works fine, but even though I log values above the threshold, the Crazyflie does not increase the thrust.

Code in stabilizerAltHoldUpdate:

Code: Select all

if (vSpeedAcc < -0.04) {
	  falling = true;
  }
  else if (vSpeedAcc > 0.00) {
	  falling = false;
  }

Code in stabilizerTask (before distributePower is called):

Code: Select all

if (!altHold || !imuHasBarometer() || !falling)
      {
        // Use thrust from controller if not in altitude hold mode
        commanderGetThrust(&actuatorThrust);
      }
      else
      {
        // Added so thrust can be set to 0 while in altitude hold mode after disconnect
        commanderWatchdog();
        if (falling) {
        	actuatorThrust = altHoldMaxThrust;
        }
      }

Any ideas on what might be the problem?

[fredgrat]

Hi,

(Just in case this could be helpful)

I implemented a free fall detection some time ago, you can see the patch here:
https://github.com/bitcraze/crazyflie-f ... 49dc85895f

To detect the free fall I did the following:

* Checked against the accWZ variable (detects free fall regardless of orientation in pitch, roll, yaw)
* Checked for multiple, consequtive measurements of accWZ indicating free fall (see the ffTrig parameter)
* accWZ approaches -1 during free fall, so I checked for multiple, consequtive measurements within a threshold (see ffaccWZ), set to -0.85
* Enable / Disable function (disabled by default, see ffEnable)
* Once free fall is detected, it simply turns on the AltHold feature

The following log variables and parameters are available in the cfclient, so you can fine tune the detection through the cfclient, as well as enable / disable the functionality, and log the actual detection (ffCount):

+// Log free fall detection
+LOG_GROUP_START(ff)
+LOG_ADD(LOG_UINT16, ffCount, &ffCount)
+LOG_ADD(LOG_UINT8, ffDetected, &ffDetected)
+LOG_GROUP_STOP(ff)
+
+// Params for free fall detection
+PARAM_GROUP_START(ff)
+PARAM_ADD(PARAM_UINT8, ffEnabled, &ffEnabled)
+PARAM_ADD(PARAM_UINT16, ffTrig, &ffTrig)
+PARAM_ADD(PARAM_FLOAT, ffaccWZ, &ffaccWZ)
+PARAM_GROUP_STOP(ff)

The patch is part of an effort to implement the following functionality:

* Flight take-off by throwing the crazyflie into the air
* Flight "landing" by simply holding your hand directly underneath (and close to) the crazyflie, to make the crazyflie simply stop when you grab it. This last part I am working on in this thread: viewtopic.php?f=6&t=1462, where I interface proximity sensors to the ADC.

The patch with free fall detection currently has one problem (which is why I have not submitted it yet):

* Once free fall has been detected, and AltHold enabled, the AltHold altitude slowly decreases. This is probably due to some part of the AltHold regulator that I am not resetting, but I have not looked into this properly yet. I will try to pick up this over the next couple of days to see if I can get this last problem out of the way.

[Johannes]

Thank you, using your code as a base I was able to play around a little bit with different ways to recover from a free fall.

[fredgrat]

Hi,

I'm glad the code could help. I have looked into the Free Fall and the "Take-Off-by-Throwing" feature over the weekend, and have just created a pull request for the bitcraze guys:

https://github.com/bitcraze/crazyflie-firmware/pull/59

The patch with free fall detection currently has one problem (which is why I have not submitted it yet):

* Once free fall has been detected, and AltHold enabled, the AltHold altitude slowly decreases. This is probably due to some part of the AltHold regulator that I am not resetting, but I have not looked into this properly yet. I will try to pick up this over the next couple of days to see if I can get this last problem out of the way.

This has now been fixed, in the pull request, see the commanderSetAltHold() function, it contains code and comments on the fix.

Additionally, I have made a much more robust 'Free Fall' detection, in the pull request see the sitAwFFTest() function and its comments. The new Free Fall detection is much less likely to be confused with other movements now, thanks to checking another variable as well.