New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added anti-turtle reversemotors and beeper using dshot commands on blheli_s #3267

Merged
merged 1 commit into from Jun 15, 2017

Conversation

Projects
None yet
10 participants
@brycedjohnson
Contributor

brycedjohnson commented Jun 12, 2017

Needs my branch of blheli_s to use the commands.

https://github.com/brycedjohnson/blheli-multishot/tree/dshotcommands

Used new commands 20 and 21 because I'm unsure how kiss esc will act. I don't have one to test with commands 7 and 8 that are similar. Currently 21 reverses motors from the blheli programmed direction and 20 returns it to the programmed direction.

Reusing BOX3DDISABLESWITCH to switch motors. Only switch when disarmed, settings are not saved on the ESC.

I'm anticipating a few more changes so I'll squash commits when finished. Looking for input..

Please please test props off first to make sure motors are working as expected!

@mikeller

There are a few bugs in your command implementation. Also, I think the mode should be checked on the fly, but should not effect permanent config changes.

src/main/drivers/pwm_output.h Outdated
DSHOT_CMD_3D_MODE_OFF, //NOT USED
DSHOT_CMD_3D_MODE_ON, //NOT USED
DSHOT_CMD_SETTINGS_REQUEST, //NOT USED
DSHOT_CMD_SAVE_SETTINGS, //NOT USED

This comment has been minimized.

@mikeller

mikeller Jun 12, 2017

Member

They are all used except for DSHOT_CMD_SETTINGS_REQUEST.

This comment has been minimized.

@brycedjohnson

brycedjohnson Jun 13, 2017

Contributor

I meant that they weren't being used for the blheli_s implementation right now and that there wasn't betaflight commands to send them yet.

This comment has been minimized.

@mikeller

mikeller Jun 13, 2017

Member

Yes there is, in CLI, just like for the DSHOT_CMD_BEEPx commands.

This comment has been minimized.

@brycedjohnson

brycedjohnson Jun 13, 2017

Contributor

Should I have a comment that says that they aren't currently implemented in blheli_s or just remove them?

This comment has been minimized.

@mikeller

mikeller Jun 13, 2017

Member

Just leave them in, without comments about implementation status in non-Betaflight. The definition for DShot commands includes all of the commands (except for 20 / 21, so maybe add a comment with respect to that). I think the Betaflight source is the wrong place to keep track of the implementation status of the commands in KISS / blheli_s / ..., the comments will just outdate and be confusing more than helpful.

src/main/drivers/pwm_output.h Outdated
@@ -75,6 +95,8 @@ typedef enum {
#define PWM_BRUSHED_TIMER_MHZ 24
#endif

This comment has been minimized.

@mikeller

mikeller Jun 12, 2017

Member

Extra newlines.

src/main/drivers/pwm_output.c Outdated
void pwmWriteDshotCommandAll(uint8_t motorCount, uint8_t command)
{
if (command <= DSHOT_MAX_COMMAND) {

This comment has been minimized.

@mikeller

mikeller Jun 12, 2017

Member

Instead of reimplementing pwmWriteDshotCommand, just call it for all motors.

This comment has been minimized.

@brycedjohnson

brycedjohnson Jun 13, 2017

Contributor

I was getting missed beeps on the esc, thought this could have been the issue.. but I don't think it is now.

This comment has been minimized.

@mikeller

mikeller Jun 13, 2017

Member

It's more about maintainability, and trying to implement function only once, to avoid the situation where one code path gets updated / fixed, and another one is forgotten.

src/main/drivers/pwm_output.c Outdated
repeats = 1;
}
for (; repeats; repeats--) {
pwmWritePtr(index, command);

This comment has been minimized.

@mikeller

mikeller Jun 12, 2017

Member

You don't set motor->requestTelemetry. The commands sent will just be ignored.

src/main/fc/fc_core.c Outdated
@@ -205,6 +205,13 @@ void mwArm(void)
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
#ifdef USE_DSHOT
if (mixerConfig()->reverse_motors == false)
pwmWriteDshotCommandAll(getMotorCount(), DSHOT_CMD_ROTATE_NORMAL);

This comment has been minimized.

@mikeller

mikeller Jun 12, 2017

Member

Why are we doing this? If users want motors to be permanently reversed, they should just use DShot commands to permanently set the motor direction to be reversed.

This comment has been minimized.

@brycedjohnson

brycedjohnson Jun 13, 2017

Contributor

I was using this is just for the temporarily reversing the motors, a little bit of cruft left over from a previous idea. I'll fix that into a separate non-permanent variable.

src/main/fc/fc_rc.c Outdated
@@ -317,6 +318,16 @@ void updateRcCommands(void)
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (!ARMING_FLAG(ARMED)) {
if (IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !mixerConfig()->reverse_motors) {

This comment has been minimized.

@mikeller

mikeller Jun 12, 2017

Member

Having the switch permanently change the configuration is wrong.

This comment has been minimized.

@brycedjohnson

brycedjohnson Jun 13, 2017

Contributor

I'll fix that

src/main/io/beeper.c Outdated
#ifdef USE_DSHOT
//dshot beep hack to test
if (ARMING_FLAG(OK_TO_ARM)) { //Don't want to start sending these off until the esc is up and running
pwmWriteDshotCommandAll(getMotorCount(), DSHOT_CMD_BEEP3);

This comment has been minimized.

@mikeller

mikeller Jun 12, 2017

Member

This should be removed.

src/main/drivers/pwm_output.c Outdated
@@ -338,7 +339,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command)
motorDmaOutput_t *const motor = getMotorDmaOutput(index);
unsigned repeats;
if ((command >= 7 && command <= 10) || command == 12) {
if ((command >= 7 && command <= 10) || command == 12 || (command >= 20 && command <= 21) ) {

This comment has been minimized.

@borisbstyle

borisbstyle Jun 13, 2017

Collaborator

Maybe good idea to use enumerated commands

@mikeller mikeller added this to the Betaflight v3.2 milestone Jun 14, 2017

src/main/fc/cli.c Outdated
@@ -2211,8 +2211,8 @@ static void cliDshotProg(char *cmdline)
int command = atoi(pch);
if (command >= 0 && command < DSHOT_MIN_THROTTLE) {
if (escNumber == ALL_ESCS) {
for (unsigned i = 0; i < getMotorCount(); i++) {
pwmWriteDshotCommand(i, command);
for (uint8_t index = 0; index < getMotorCount(); index++) {

This comment has been minimized.

@mikeller

mikeller Jun 14, 2017

Member

Please use unsigned or int for things like loop counters, they are fastest on STM32.

This comment has been minimized.

@brycedjohnson

brycedjohnson Jun 14, 2017

Contributor

Will get that changed

This comment has been minimized.

@brycedjohnson

brycedjohnson Jun 14, 2017

Contributor

Thinking about it some more, I'm surprised the compiler doesn't pick up on that optimization. I got it changed, but more curious about it.

This comment has been minimized.

@mikeller

mikeller Jun 14, 2017

Member

Well the size of an integer is 32 bit on STM32. So if you use an uint8_t, the compiler will have to add range checks. In a simple case like the above, they can probably be optimised away, because there is no way there could be an overrun. But over time, simple cases often grow into complex cases, and at some point the checks can't be optimised away again. If you use unsigned for loops from the get go, you'll never get into this situation.

src/main/fc/fc_rc.c Outdated
@@ -317,6 +324,16 @@ void updateRcCommands(void)
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (!ARMING_FLAG(ARMED)) {
if (IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !reverseMotors) {

This comment has been minimized.

@mikeller

mikeller Jun 14, 2017

Member

There is work under way to extend the maximum number of possible modes. Once this is done, we'll probably want to assign this to a mode of its own. But for now this is good.

Also, what about moving this whole block up into mwArm()? That way, you make the code pretty robust against somebody accidentally changing it so that motors can be reversed for the mixer only, but not for blheli_s.

src/main/fc/fc_rc.h Outdated
@@ -24,3 +24,4 @@ float getThrottlePIDAttenuation(void);
void updateRcCommands(void);
void resetYawAxis(void);
void generateThrottleCurve(void);
uint8_t isMotorsReversed(void);

This comment has been minimized.

@mikeller

mikeller Jun 14, 2017

Member

Should be a bool really.

@@ -338,11 +340,20 @@ void beeperUpdate(timeUs_t currentTimeUs)
return;
}
#ifdef USE_DSHOT
if (!ARMING_FLAG(ARMED) && currentBeeperEntry->mode == BEEPER_RX_SET) {

This comment has been minimized.

@mikeller

mikeller Jun 14, 2017

Member

What does this do?

This comment has been minimized.

@brycedjohnson

brycedjohnson Jun 14, 2017

Contributor

This sends the dshot beep command when not armed and with the beeper mode switch flipped

This comment has been minimized.

@mikeller

mikeller Jun 14, 2017

Member

Good idea. I think a lot of pilots use that switch as a 'lost craft indicator', so beeping the ESCs in case the beeper was smashed in the crash is a good thing.

(Also, the beeper code is a mess.)

This comment has been minimized.

@brycedjohnson

brycedjohnson Jun 14, 2017

Contributor

Yeah I tried to use the dshot beep for everything, but that was a little too much. I could see in the future you could use the 5 different kinds of beeps to signal different things much like the beeper has different patterns.

Also this has already saved me a few minutes finding my crashed quad in some tall grass! I don't have actual beepers on any of my quads.

This comment has been minimized.

@mikeller

mikeller Jun 14, 2017

Member

Using it for everything is a hard ask, since things like low voltage alarms make only sense if they work while armed, and you don't really want to waste motor update bandwidth on beeping in that situation.

This comment has been minimized.

@brycedjohnson

brycedjohnson Jun 14, 2017

Contributor

The loss of transmitter, blackbox erase completed, gyro calibration, acc calibration, maybe some others might be good ones to add in the future with different beep sounds... but that can be a separate PR .

@mikeller

This comment has been minimized.

Member

mikeller commented Jun 14, 2017

Yes, this looks really good now, thanks.

@brycedjohnson

This comment has been minimized.

Contributor

brycedjohnson commented Jun 14, 2017

@mikeller Thanks for the help! Still learning how all the parts of betaflight go together!

I'll see if I can squash the commits now and give it one final test flight...

@brycedjohnson brycedjohnson force-pushed the brycedjohnson:dshotcommand branch 2 times, most recently Jun 14, 2017

@brycedjohnson brycedjohnson force-pushed the brycedjohnson:dshotcommand branch to 13e21c9 Jun 14, 2017

@mikeller mikeller merged commit 227a1f6 into betaflight:master Jun 15, 2017

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details

@brycedjohnson brycedjohnson deleted the brycedjohnson:dshotcommand branch Jun 16, 2017

DSHOT_CMD_SETTINGS_REQUEST,
DSHOT_CMD_SAVE_SETTINGS,
DSHOT_CMD_ROTATE_NORMAL = 20, //Blheli_S only command
DSHOT_CMD_ROTATE_REVERSE = 21, //Blheli_S only command

This comment has been minimized.

@robertlacroix

robertlacroix Jul 1, 2017

Contributor

@brycedjohnson It's unfortunate to have different commands for the same thing on different ESCs. The idea behind DSHOT is that it's a standard protocol that works the same across both blheli and kiss. Is the only reason for that because you can't test Kiss ESCs? I can help with that.

This comment has been minimized.

@brycedjohnson

brycedjohnson Jul 1, 2017

Contributor

@robertlacroix Does the "DSHOT_CMD_SPIN_OTHER_WAY" always make kiss escs spin the opposite way as they are configured (CW ->CCW and CCW->CW)? If so it would be easy to standardize, if not there is no way to tell what way the motor is spinning without telemetry or telling betaflight what each motor is doing ahead of time.

This comment has been minimized.

@robertlacroix

robertlacroix Jul 1, 2017

Contributor

I would assume it spins the other way than the configured direction. I might be able to try in the next couple of days.

This comment has been minimized.

@mikeller

mikeller Jul 1, 2017

Member

@robertlacroix: These commands do different things indeed: DSHOT_CMD_SPIN_DIRECTION_1 and DSHOT_CMD_SPIN_DIRECTION_2 set the absolute motor direction, which is then persisted in the ESCs configuration. DSHOT_CMD_SPIN_DIRECTION_NORMAL and DSHOT_CMD_SPIN_DIRECTION_REVERSED temporarily reverse the motor direction, relative to what was configured before (with DShot commands, or through an ESC configuration tool). This seems to be redundant to some extent, but in the case of blheli_s, without the flight controller having any means to query the current setting for the spin direction, using an absolute change to effect a temporary reversal is not a good option.

This comment has been minimized.

@robertlacroix

robertlacroix Jul 2, 2017

Contributor

@mkeller @brycedjohnson Makes sense! Can we try to work with the Kiss guys and have that added as 20 and 21 on their end as well?

This comment has been minimized.

@fedorcomander

fedorcomander Jul 9, 2017

Contributor

well... different versions have a bit different protocol... but it will get u going ;)

This comment has been minimized.

@mikeller

mikeller Jul 9, 2017

Member

So what other protocol versions are out there?

Also, does this new version of the version info frame have a checksum, or has this been dropped?

This comment has been minimized.

@ronlix

ronlix Jul 10, 2017

Contributor

Hi,
for now there are only two versions. the first one sends 15 bytes:
1-12: ESC SN
13: EEprom/version (1.01 == 101)
14, bit 8-6: ESC Type
14, bit 5-1: ESC sub version (a-z)
15: crc (same crc as is used for telemetry)

after we worked on the dshot settings, we decided to not use the dshot setting request command as it just adds load to the FC and GUI to have more requests. so i streched the ESC info response a bit. its now 21 bytes:

1-12: ESC SN
13: now idicates if the new response is used. so if its 255 it is the new version.
14: EEprom/version (1.01 == 101)
15: ESC type
16: ESC sub version letter
17: rotation direction reversed by dshot command or not (1:0)
18: 3D mode active or not (1:0)
19: unused for now.. maybe used for new settings
20: unused for now.. maybe used for new settings
21: crc (same crc as is used for telemetry)

for now the rotation direction reversed byte only tells if the rotation is reversed and saved by dshot command 8, not when temporary reversed (21)

regards

Felix

This comment has been minimized.

@mikeller

mikeller Jul 10, 2017

Member

Hi @ronlix.

Thanks for the clarification. I will add this later on. Can you clarify some details please:

  • is there a list of ESC types?
  • 'ESC sub version letter' is ASCII?

I think it is correct for the command to return the direction that is saved, and not taking the temporary reversal (21) into account. After all the flight controller sets the reversal, so it should not have to query it.

Since this info block now includes the ESC config, am I right to assume that command 11 (send ESC settings) is now unused, and will not be implemented?

Also you ok with blheli_32 using the same format to dump ESC info over telemetry? For me, from the flight controller side, doing so would be a huge advantage, since only then will it be possible to reliably get ESC config info without the user having to provide some information. If this is ok, can I propose that we use one of the unused bytes to encode firmware type?

This comment has been minimized.

@ronlix

ronlix Jul 10, 2017

Contributor

for now:
1 = KISS8A (KISSAIO)
2 = KISS16A (KISSCC)
3 = KISS24A RE
5 = KISS Ultralite (Tealdrones)

yes, the subversion is ASCII

command 11 ESC settings is unused.

im fine with BLheli32 using the same protocol, i would just say that blheli32 then should start at 0x7F with the ESC type, so we have some free for future ;)

regards

felix

@sskaug

This comment has been minimized.

sskaug commented Jul 13, 2017

As BLHeli_32 also has LEDs, I would propose this command set:
0 DSHOT_CMD_MOTOR_STOP,
1 DSHOT_CMD_BEEP1,
2 DSHOT_CMD_BEEP2,
3 DSHOT_CMD_BEEP3,
4 DSHOT_CMD_BEEP4,
5 DSHOT_CMD_BEEP5,
6 DSHOT_CMD_ESC_INFO,
7 DSHOT_CMD_SPIN_DIRECTION_1, // Need 10x
8 DSHOT_CMD_SPIN_DIRECTION_2, // Need 10x
9 DSHOT_CMD_3D_MODE_OFF, // Need 10x
10 DSHOT_CMD_3D_MODE_ON, // Need 10x
11 DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented
12 DSHOT_CMD_SAVE_SETTINGS, // Need 10x
20 DSHOT_CMD_SPIN_DIRECTION_NORMAL, // Need 10x
21 DSHOT_CMD_SPIN_DIRECTION_REVERSED, // Need 10x
22 DSHOT_CMD_LED0_ON,
23 DSHOT_CMD_LED1_ON,
24 DSHOT_CMD_LED2_ON,
25 DSHOT_CMD_LED3_ON,
26 DSHOT_CMD_LED0_OFF,
27 DSHOT_CMD_LED1_OFF,
28 DSHOT_CMD_LED2_OFF,
29 DSHOT_CMD_LED3_OFF,
DSHOT_CMD_MAX = 47

@sskaug

This comment has been minimized.

sskaug commented Jul 13, 2017

Also, LED status should be reflected in the ESC info block:
ESC_INFO layout 3 (BLHeli_32):
1-12: ESC SN
13: Indicates which response version is used. 254 is for BLHeli_32 version.
14: FW revision (32 = 32)
15: FW sub revision (10 = xx.1, 11 = xx.11)
16: Unused
17: Rotation direction reversed by dshot command or not (1:0)
18: 3D mode active or not (1:0)
19: Low voltage protection limit [0.1V] (255 = not capable, 0 = disabled)
20: Current protection limit [A] (255 = not capable, 0 = disabled)
21: LED0 on or not (1:0, 255 = LED0 not present)
22: LED1 on or not (1:0, 255 = LED1 not present)
23: LED2 on or not (1:0, 255 = LED2 not present)
24: LED3 on or not (1:0, 255 = LED3 not present)
25-31: Unused
32-63: ESC signature
64: CRC (same CRC as is used for telemetry)

Here I have also proposed the let byte 13 = 254 identify a BLHeli_32 ESC.
And with a BLHeli_32 specific layout for the subsequent bytes.
Using a number to identify BLHeli_32 ESC type will be complicated, as new types are added all the time. I would therefore propose to use the 32byte long ESC signature.

@sskaug

This comment has been minimized.

sskaug commented Jul 24, 2017

Test code for the Wraith32, that implements the above is now published here: https://github.com/bitdump/BLHeli/tree/master/BLHeli_32%20ARM

@fedorcomander

This comment has been minimized.

Contributor

fedorcomander commented Jul 24, 2017

Just 2 cents. Leds info can be packed nicely to 1 byte TBH.... No led capable esc can just return 0 and done.
0: led0_on
1: led0_present
2: led1_on
3: led1_present
4: led2_on
5: led2_present
6: led3_on
7: led3_present

Unless its planned to be RGB full color xmas tree ;) LOL

@FinalFrag

This comment has been minimized.

FinalFrag commented Jul 24, 2017

I've been doing this manually for some time (using the regular 3D mode options in betaflight), and it super awesome to see improvements to this. No more giving up 50% resolution for anti-turtle mode!

However, could this be improved so that in reversed mode, not all 4 propellers spin? Often I roll over by just doing an aileron roll while on the ground (because taking off upside down in fpv is not easy). But this stalls the motors that are in contact with the ground during the flip. In the past 2-3 weeks I've replaced 3 escs. Not saying this is the cause, but I hadn't replaced an esc in a year before using anti-turtle mode, so there may be some connection there.

I tried programming my taranis to also disable airmode while in the reverse mode, but that still spins up all motors. Ideally it would only spin up 2 out of 4 (eg the 2 left motors when trying to roll over to the right).

Is this something that can be altered easily?

@FinalFrag

This comment has been minimized.

FinalFrag commented Jul 24, 2017

Correction, it should spin up the right side motors (in reverse) when trying to roll to the right. Upside down stuff is confusing :)

@fedorcomander

This comment has been minimized.

Contributor

fedorcomander commented Jul 24, 2017

You can see how i did it on kiss... Sorry for offtopic. You CANNOT fly up side down... When turtle mode is activated, i use pitch/roll stick to activate only ONE side of the quad. Works quite good i must say. Of course, it depends on the quad model :))) Throttle is NOT used at all during turtle mode. I use pitch/roll stick as throttle. ;)

https://youtu.be/uyzxy5nQSGs

@FinalFrag

This comment has been minimized.

FinalFrag commented Jul 24, 2017

Exactly what I meant with my elaborate comment above :-)

Would this be possible in betaflight? Using the regular 3d options kills escs

@brycedjohnson

This comment has been minimized.

Contributor

brycedjohnson commented Jul 25, 2017

@fedorcomander @FinalFrag That isn't a bad idea. In all the turtle mode flips have I done, I never need to use any throttle. Only spinning the props that need to spin would be helpful as well. I'll have to see how complicated it would be to implement.

@sskaug

This comment has been minimized.

@DieHertz

This comment has been minimized.

Member

DieHertz commented Aug 10, 2017

Added to blheli-configurator for distribution.

@ronlix

This comment has been minimized.

Contributor

ronlix commented Nov 19, 2017

Hi,

Just want to reserve dshot command 30 and 31
30 will be audio Stream mode on/Off
31 silent Mode on/Off
In the next kiss32a release

Regards

Felix

@martinbudden

This comment has been minimized.

Contributor

martinbudden commented Nov 19, 2017

@ronlix , thanks for letting us know Felix.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment