christianrauch / msp

Implementation of the MultiWii Serial Protocol (MSP) for MultiWii and Cleanflight flight controller
http://www.multiwii.com/wiki/index.php?title=Multiwii_Serial_Protocol
GNU Lesser General Public License v3.0
73 stars 26 forks source link

SetRc() only works for half a second with Betaflight MSP_OVERRIDE mode #62

Closed defrag-bambino closed 4 months ago

defrag-bambino commented 4 months ago

Hi,

I am trying to send RC commands to a SpeedyBee F405 V3 board using a usb-c cable. The board runs Betaflight 4.5.0-RC2.

When running your fcu_motor_test it works fine. Though this is not using SetRc() of course. But this works fine, even for longer durations. However, when I change the fcu_motor_test to use SetRc() instead of SetMotors(), like this:

...
    while(true)
    {
        fcu.setRc({{1500, 1500, 1100, 1500, 0, 0, 0, 0}});
    }
...

it only works (sometimes not at all) for about 1/2 seconds. After that, the motors turn off and cannot be turned on. They do not stop abruptly though, like sending command "1000" would do, they rather seem to not be getting any signal at all anymore.

To reproduce: I am using a regular I-BUS Transmitter (Serial RX, not MSP in Receiver tab). But to be able to send MSP messages, I have configured the MSP_OVERRIDE mode to be enabled via a switch. I arm the drone with one switch, then turn on the MSP_OVERRIDE mode with another switch, and then run the modified fcu_motor_test. Then, if the motors spin at all, they only do so for about half a second.

Debug output

``` sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 write complete: 22 vs 22 sending message - ID 214 sending: 214 | dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 packed: 24 4d 3c 10 d6 dc 5 dc 5 4c 4 dc 5 0 0 0 0 0 0 0 0 57 ```

Any ideas what might be going on here?

christianrauch commented 4 months ago

Using RC commands involves a failsafe for cases where the RC communication is lost. Sending values straight to the motors skips over this.

Is the flight controller going into some failsafe? This could happen when the messages do not arrive in time.

Can you read the motor and RC values back with MSP_MOTOR and MSP_RC and check if the correct values are sent back?

defrag-bambino commented 4 months ago

I do not think it is going into failsafe. But how would I tell? Which message shows me this?

I have printed MOTOR and RC states after sending the commands and it seems to FC receives the RC commands but does set the Motors back to 1000 (During ARM state, before turning into MSP_OVERRIDE mode, they are ~1050).

Outputs

``` #Rc channels (18) : 1500 1500 1500 1100 1500 1500 1500 1500 1000 2000 1500 1500 1500 1990 1500 1500 1500 1500 #Motor: 1000 1000 1000 1000 0 0 0 0 Arming Disable flags: 26 (THROTTLE ) Mode: 1048578 #Rc channels (18) : 1500 1500 1500 1100 1500 1500 1500 1500 1000 2000 1500 1500 1500 1990 1500 1500 1500 1500 #Status: Cycle time: 312 us I2C errors: 0 Sensors: Accelerometer: ON Barometer: ON Magnetometer: OFF GPS: OFF Sonar: OFF Active Boxes (by ID): 1 20 #Motor: 1000 1000 1000 1000 0 0 0 0 #Rc channels (18) : 1500 1500 1500 1100 1500 1500 1500 1500 1000 2000 1500 1500 1500 1990 1500 1500 1500 1500 #Motor: 1000 1000 1000 1000 0 0 0 0 #Rc channels (18) : 1500 1500 1500 1100 1500 1500 1500 1500 1000 2000 1500 1500 1500 1990 1500 1500 1500 1500 #Motor: 1000 1000 1000 1000 0 0 0 0 #Rc channels (18) : 1500 1500 1500 1100 1500 1500 1500 1500 1000 2000 1500 1500 1500 1990 1500 1500 1500 1500 #Motor: 1000 1000 1000 1000 0 0 0 0 ```

May this be a Betaflight issue?

christianrauch commented 4 months ago

I do not think it is going into failsafe. But how would I tell? Which message shows me this?

That depends on the hardware and your settings. Usually, there is some LED on the board that flashes or something.

I have printed MOTOR and RC states after sending the commands and it seems to FC receives the RC commands but does set the Motors back to 1000 (During ARM state, before turning into MSP_OVERRIDE mode, they are ~1050).

Then the communication is working as expected, I think. Are you actually arming the FC with the RC commands? The motors will not turn before you arm the FC via the RC stick combinations.

defrag-bambino commented 4 months ago

That depends on the hardware and your settings. Usually, there is some LED on the board that flashes or something.

Indeed the LED goes from solid blue (when armed) to blinking blue once the MSP_OVERRIDE mode is enabled. But I am not sure what this means or how I can prevent it.

Then the communication is working as expected, I think. Are you actually arming the FC with the RC commands? The motors will not turn before you arm the FC via the RC stick combinations.

No, I am not arming via the computer. I am arming via my remote control and only then enabling the MSP_OVERRIDE mode. The motors turn when I arm, but once I flip the mode switch into MSP_OVERRIDE they stop as described above.

defrag-bambino commented 4 months ago

One thing is that once enabling MSP_OVERRIDE mode (mind you already having armed before) the arming disabled flag "THROTTLE" appears. :thinking:

defrag-bambino commented 4 months ago

solved in betaflight/betaflight#13416 I had the bitmap for MSP_OVERRIDE set wrong... Thanks!