Turtle mode optimization

[kernel-machine]

Sorry for the multiple PRs for the turtle mode.

With this PRs i've did:

• With the previous PR (Turtle mode issue fix #6845) i've fixed the issue that if the switch of turtle mode is accidentally triggered during the flight, the turtle mode will enter, motors are stopped and the quad will crash, but with the previous PR the turtle mode enters only if the quad is leveled., so useless :)

• Some users (included @DzikuVx) said me that the stick response it too much sensitive (they are right), this was caused by an error in the motor output calculation, now the response is more "soft" and i have tuned the power factor in order to have the right power for my quad to avoid double flip.
  The original code has two dead band one on the stick and another one on the motor output (they are borrowed from BF), i've removed that on the motors, i think that the one of the stick is enough and with both of them there was to much dead band.

• I've renamed all the code functions and variables related to the turtle mode from flipOverAfterCrashMode to turtleMode, we call this feature in this way in all the parts of iNav so i think that it has not to much sense to call it in another way in the code.

Now the response is something like this, so at the maximum stick deflection the max power is guaranteed, the power factor changes only the curve response and not the maximum power.

video_2021-04-21_16-46-45.mp4

[Jetrell]

@kernel-machine With the current #6845 fix.
After using Turtle mode. And disarming, without first switching out of Turtle mode, The quads motors will spin-up to full speed for 0.5 seconds, then disarm.
I am aware that its best to switch out of Turtle mode before disarming. But disarm should have priority.

Does this new PR have a fix for this ?

Thanks

[kernel-machine]

@Jetrell i've had not notice this issue in #6845, but in this PR it not happens, as you can see in the attached video, starting from the left, the first switch is the turtle mode, the second is for arming and when i've turned off the arming switchthe turtle mode was still actives, all was worked fine.
In this PR the code that interact with arming is almost identical as the original turtle mode, while in #6845 it was moved in a wrong position

[Jetrell]

@kernel-machine Thanks for the reply. I looked into it more.
It has a similar issue to what Dshot beeper has a present.
If the quad doesn't have a GPS lock. Then it disarms as yours does in the video.
But if the quad has a GPS lock and its disarmed with the Turtle mode switch still enabled. Then motor 3 will burst into life for around 0.5 seconds.

[kernel-machine]

Ok @Jetrell, i will do some tests in the afternoon

[kernel-machine]

With this branch the problem said by @Jetrell doesn't appears

[Odin263]

Wouldn't it make sense to activate the turtle mode with a small time delay ~ 0.5s? This would create a possibility to put arming and turtle mode on one switch. Each switch has 2 contacts for on and off but also a zero point, even if this has no grid position (-1, 0, 1 or 1000, 1500, 2000). If arm mode and turtle mode are set to this zero point and to on position is set only the arm mode , nothing would change for arming in the case of a delayed turtle mode, but turtle mode can simply be activated using the same switch is held in the middle until the quad is correctly positioned again. It already works like this, but the switch has to be toggled very quickly so that the turtle mode is not also active in the switch on position.

[Odin263]

I'm not sure if I was the only one who had the following problem, but almost every time the turtle mode was used, the directions of some motors were wrong. Sometimes one, sometimes several or all of them. The error may have been elsewhere, but I was able to fix the error as follows.

maybe that will help someone too

[kernel-machine]

Hey man @Odin263, this thing happen only if you disable first the turtle mode and then disarm the motors or even if you first disarm and then disable the turtle mode?

[Odin263]

I've tried differently. It's not always like that, but mostly. I couldn't find a connection. The engine selection also appears to be random.

[Odin263]

Hi @kernel-machine,

apparently that wasn't the solution after all. The error now only occurs very rarely, but it is not completely gone. I do not understand why the rotation change always works when activating the turtle mode, but after exiting the mode it can sometimes fail? Before activating the turtle mode, there are never any problems with the direction of rotation.
Do you have a suggested solution for me or an idea where exactly I could look?

[kernel-machine]

@Odin263 I don't know if you know, when the firmware sends a DShot command to the ESC, it will be send 10 times (it's required by the dshot protocol).
I'm quickly analyzing the main differences between the dhsot commands implementations in iNav and BetaFlight.
BetaFlight seems that waits for 1000us between the sent commands (the commands are sent 10 times) while iNav didn't (i had not implemented these delays in iNav because it also did work without them).

You could try to add these delays also if it's strange that the motors always reverse the direction and the problem is always when they go back to the original direction.
If the problem depends by these delays maybe also a slower dshot procotol could "fix" the problem

[Odin263]

@kernel-machine thank you very much for your support! I had already tried the slower dshot protocol before, but without any visible success (dshot300 and dshot150). I will try to copy this delay from BetaFlight. Maybe I have this problem because my ESC only supports BLHeli_S and not BLHeli_32. If I manage to integrate this delay into the repetitions and get the problem completely solved with that or with other means, I will give you feedback here.

[Odin263]

@kernel-machine I am not sure if I positioned the delay correctly because I am not familiar with programming, but it apparently worked. After about 100 attempts the error did not appear.

Thank you again!

[kernel-machine]

@Odin263 In these flight controller firmware you never should put an instruction that blocks the execution flow, for example that line blocks the whole iNav firmware for 10 ms while you should block only the dshot commands.
In the original implementation when i have initially implemented the delays i have did something like this:

if (isTherePendingCommands &&  micros() - lastCommandSent > DSHOT_COMMAND_INTERVAL_US){
...
lastCommandSent = micros();
}

In this way a new command will be put in currentExecutingCommand only if at least DSHOT_COMMAND_INTERVAL_US are elapsed otherwise it will exit from the function, doing so the execution flow will be never blocked.
When i did the implementation a maintainer didn't like these intervals #6685 (comment), but maybe for the BLHeli_S support is a nice compromise.

Another comment: you have write 10 000 us while betaflight uses 1 000 us.
With this improvements you can also create a new pull request, unfortunately i have not time now for iNav development.

[Odin263]

@kernel-machine,
I mainly chose the original location because I also wanted to include queues between the individual ESCs. The important thing for me was to find out whether the error could be eliminated at all due to the delay, hence the significantly higher time than with BetaFlight. After numerous attempts I was able to find out that the delay in the pending commands is sufficient, but for me it has to be higher than 1000µs, otherwise errors will occur again. I still have to determine the limit value. I tried to implement it according to your suggestion, whereby the FW for me still largely consists of hieroglyphics and I still haven't fully understood what micros () are. I also looked for an if function that only includes BLHeli_S, but which I did not find in the FW.
If everything works continuously, I can also try to create a new pull request. At first it works as shown below and when DSHOT_COMMAND_INTERVAL_US = 5000.

[kernel-machine]

micors() returns the amount of microseconds since the flight controller was started

[Odin263]

@kernel-machine
I've now created a new pull request (if I've done everything right). If you find the time and want to get involved in some way, you can find it here:
#7724