Add ability to get motor telemetry data

[tvannoy]

This PR adds the ability to process data received after sending the MSP_MOTOR_TELEMETRY MSP code.

This implementation is based off of betaflight configurator's implementation: https://github.com/betaflight/betaflight-configurator/blob/dd16cb767989ecfefe76d6779969466a53d5bd63/src/js/msp/MSPHelper.js#L273

Example usage:

from yamspy import MSPy
from yamspy.msp_codes import MSPCodes

board = MSPy(device=<serial-port>)
board.connect()

# Set the four motors to 1500 throttle
board.send_RAW_MOTORS([1500, 1500, 1500, 1500, 0, 0, 0, 0])

# Read back the motor telemetry
board.send_RAW_msg(MSPCodes['MSP_MOTOR_TELEMETRY'])
dataHandler = board.receive_msg()
board.process_recv_data(dataHandler)

# Print out the telemetry values
print(board.MOTOR_TELEMETRY_DATA)

This was tested on a Happymodel Crux 3 running betaflight.

[tvannoy]

Testing this some more, I've found that the telemetry data tends to take a while to update. The telemetry data I get back will be for old throttle settings. After reading telemetry data serveral times, I will finally get the correct telemetry data. This seems like there is some sort of buffer that I don't know about.

I'm not very familiar with betaflight or MSP, so I'm not sure if this is expected or not.

Here's a log of the behavior in a python console, along with comments:

def get_telemetry(board):
    board.send_RAW_msg(MSPCodes['MSP_MOTOR_TELEMETRY'])
    dataHandler = board.receive_msg()
    board.process_recv_data(dataHandler)
    return board.MOTOR_TELEMETRY_DATA


# throttle was set lower than 1080 before this
>>> board.send_RAW_MOTORS([1080,1080,1080,1080,0,0,0,0])
22
>>> get_telemetry(board)
# get telemetry prints rpm values from previous throttle setting
motor count 4
{'rpm': [516, 1616, 500, 1583, 0, 0, 0, 0], 'invalidPercent': [0, 0, 0, 0, 0, 0, 0, 0], 'temperature': [0, 0, 0, 0, 0, 0, 0, 0], 'voltage': [0, 0, 0, 0, 0, 0, 0, 0], 'current': [0, 0, 0, 0, 0, 0, 0, 0], 'consumption': [0, 0, 0, 0, 0, 0, 0, 0]}
>>> get_telemetry(board)
motor count 4
{'rpm': [516, 1650, 483, 1516, 0, 0, 0, 0], 'invalidPercent': [0, 0, 0, 0, 0, 0, 0, 0], 'temperature': [0, 0, 0, 0, 0, 0, 0, 0], 'voltage': [0, 0, 0, 0, 0, 0, 0, 0], 'current': [0, 0, 0, 0, 0, 0, 0, 0], 'consumption': [0, 0, 0, 0, 0, 0, 0, 0]}
>>> board.send_RAW_MOTORS([1020,1020,1020,1020,0,0,0,0])
22
>>> get_telemetry(board)
motor count 4
{'rpm': [500, 1650, 483, 1516, 0, 0, 0, 0], 'invalidPercent': [0, 0, 0, 0, 0, 0, 0, 0], 'temperature': [0, 0, 0, 0, 0, 0, 0, 0], 'voltage': [0, 0, 0, 0, 0, 0, 0, 0], 'current': [0, 0, 0, 0, 0, 0, 0, 0], 'consumption': [0, 0, 0, 0, 0, 0, 0, 0]}
>>> get_telemetry(board)
{'rpm': [500, 1650, 483, 1516, 0, 0, 0, 0], 'invalidPercent': [0, 0, 0, 0, 0, 0, 0, 0], 'temperature': [0, 0, 0, 0, 0, 0, 0, 0], 'voltage': [0, 0, 0, 0, 0, 0, 0, 0], 'current': [0, 0, 0, 0, 0, 0, 0, 0], 'consumption': [0, 0, 0, 0, 0, 0, 0, 0]}
>>> get_telemetry(board)
# telemetry finally prints the rpm values associated with 1080 throttle, even though the throttle is currently set to 1020
motor count 4
{'rpm': [2783, 2766, 2816, 2683, 0, 0, 0, 0], 'invalidPercent': [0, 0, 0, 0, 0, 0, 0, 0], 'temperature': [0, 0, 0, 0, 0, 0, 0, 0], 'voltage': [0, 0, 0, 0, 0, 0, 0, 0], 'current': [0, 0, 0, 0, 0, 0, 0, 0], 'consumption': [0, 0, 0, 0, 0, 0, 0, 0]}
>>> get_telemetry(board)
motor count 4
{'rpm': [2733, 2733, 2700, 2733, 0, 0, 0, 0], 'invalidPercent': [0, 0, 0, 0, 0, 0, 0, 0], 'temperature': [0, 0, 0, 0, 0, 0, 0, 0], 'voltage': [0, 0, 0, 0, 0, 0, 0, 0], 'current': [0, 0, 0, 0, 0, 0, 0, 0], 'consumption': [0, 0, 0, 0, 0, 0, 0, 0]}
>>> get_telemetry(board)
{'rpm': [2733, 2733, 2700, 2733, 0, 0, 0, 0], 'invalidPercent': [0, 0, 0, 0, 0, 0, 0, 0], 'temperature': [0, 0, 0, 0, 0, 0, 0, 0], 'voltage': [0, 0, 0, 0, 0, 0, 0, 0], 'current': [0, 0, 0, 0, 0, 0, 0, 0], 'consumption': [0, 0, 0, 0, 0, 0, 0, 0]}
>>> get_telemetry(board)
# telemetry prints the rpm associated with 1020 throttle
motor count 4
{'rpm': [500, 500, 500, 450, 0, 0, 0, 0], 'invalidPercent': [0, 0, 0, 0, 0, 0, 0, 0], 'temperature': [0, 0, 0, 0, 0, 0, 0, 0], 'voltage': [0, 0, 0, 0, 0, 0, 0, 0], 'current': [0, 0, 0, 0, 0, 0, 0, 0], 'consumption': [0, 0, 0, 0, 0, 0, 0, 0]}
>>> get_telemetry(board)
motor count 4
{'rpm': [433, 433, 450, 366, 0, 0, 0, 0], 'invalidPercent': [0, 0, 0, 0, 0, 0, 0, 0], 'temperature': [0, 0, 0, 0, 0, 0, 0, 0], 'voltage': [0, 0, 0, 0, 0, 0, 0, 0], 'current': [0, 0, 0, 0, 0, 0, 0, 0], 'consumption': [0, 0, 0, 0, 0, 0, 0, 0]}
>>> get_telemetry(board)
motor count 4
{'rpm': [483, 433, 416, 366, 0, 0, 0, 0], 'invalidPercent': [0, 0, 0, 0, 0, 0, 0, 0], 'temperature': [0, 0, 0, 0, 0, 0, 0, 0], 'voltage': [0, 0, 0, 0, 0, 0, 0, 0], 'current': [0, 0, 0, 0, 0, 0, 0, 0], 'consumption': [0, 0, 0, 0, 0, 0, 0, 0]}
>>>

Is this behavior expected? Or is something incorrect in my MSP_MOTOR_TELEMETRY implementation?

[ricardodeazambuja]

Have you tried it with the yb/stable branch?

[tvannoy]

Good suggestion. I just tried my changed based on the yp/stable branch, and got the same behavior.

This behavior isn't a deal breaker for what I'm planning on doing, as I can just read the telemetry data 10 or 20 times until the current RPM shows up.

Perhaps there is something going on in the betaflight firmware that is causing this to happen.

[tvannoy]

If you're interested in adding this motor telemetry functionality, I'm happy to make a pull request again yp/stable if that's what you'd prefer.