Frsky bidirectional telemetry implementation

Eric_Stockenstrom · November 13, 2019, 11:46am

Thanks Alex …

yaapu · November 13, 2019, 10:12pm

Hi Eric,
I prepared a debug version of the lua script.
This sends a mavlite message 22 (param request read) every 5 seconds and decodes all incoming mavlite messages

first packet structure:
1 byte for sequence
1 byte for payload length
1 byte for message id
3 bytes payload

all other packets
1 byte sequence
5 bytes paylod

at the end of the message a CRC i calculated which is not counter in the payload length

In this example the uplink message requires 4 sport packets (seq: 1 - 4) since the payload is just the string “SERIAL1_PROTOCOL” (parameters names are max 16 chars with dynamic length.

the value message requires 4 extra bytes to encode the parameter value as an IEEE 754 float.
The float is always encoded before the string since the latter has a variable length.

By looking at the code this should be much clearer

ardupilotlua_debug_ver_0.1.ZIP (23.5 KB)

Eric_Stockenstrom · November 14, 2019, 5:50am

Thank you Alex. This will be very useful. I got a chunk of work done yesterday, but long way to go.

How do you determine that you have a mavlite message, rather than a frsky sensor message?

yaapu · November 14, 2019, 7:58am

uplink = TX → RX (master)
single sensor i.e. responds to poll only for ID 0x0D, uses frame:0x30

downlink= RX → TX (slave)
responds to polls for IDs 0x1B, 0x67, 0x34. uses frame: 0x32

All downlink traffic is using frame 0x32, this is a special frame that OpenTX passes untouched to lua.

github.com

yaapu/ardupilot/blob/dea5854f7b7490fc83c015f913a7a26477252b28/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp#L1300-L1309


      
          void AP_Frsky_Telem::process_sport_tx_queue()
          {
             if (_sport_tx_queue.empty()) {
                  return;
              }
          
              send_uint32(SPORT_DOWNLINK_FRAME, _sport_tx_queue[0]->data, _sport_tx_queue[0]->value);
          
              _sport_tx_queue.remove(0);
          }

All uplink traffic is using frame 0x30.

github.com

yaapu/ardupilot/blob/dea5854f7b7490fc83c015f913a7a26477252b28/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp#L1254-L1272


      
          void AP_Frsky_Telem::process_sport_telemetry_packet(const uint8_t * packet)
          {
          
            if (!check_sport_packet(packet)) {
              return;
            }
          
            sport_packet_t sp;
          
            sp.sensor_id = packet[0];
            sp.frame_id = packet[1];
            sp.data = *(uint16_t *)(packet+2);
            sp.value = *(uint32_t*)(packet+4);
          
            // queue only Uplink packets
            if (sp.sensor_id == _sport_config.uplink_sensor_id && sp.frame_id == SPORT_UPLINK_FRAME) {
              _sport_rx_queue.push_force(sp);
            }
          }

Note: I will eventually switch to f.port convetions

where uplink (master traffic) is separated between read/write

Eric_Stockenstrom · November 14, 2019, 8:43am

Ok, got it. Thanks…

yaapu · November 14, 2019, 8:47am

you’re welcome,

…did a little editing of the previous answer

Eric_Stockenstrom · November 14, 2019, 9:05am

Sorry, one more thing. Where can I get your ardupilot side repo / firmware? Is it the one on your git?

yaapu · November 14, 2019, 9:08am

yes, here

Eric_Stockenstrom · November 14, 2019, 3:23pm

Starting .... C:\Users\Eric\Documents\GitHub\+esp32_wip\Mav2PT_TwoWay_v2.45c\Mav2PT_TwoWay_v2.45c
Target Board is ESP32 / Variant is Dev Module
Air Mode
Battery_mAh_Source = 3 - Define battery capacities in the LUA script
Using Serial_1 for S.Port
RSSI Automatic Select
Mavlink Serial In
Waiting for telemetry
ESP32 S.Port pins NOT inverted for Air or Relay Modes. Must use a converter
hb_count=1
hb_count=2
hb_count=3
mavgood=true
FrSky from S.Port: 0x0D:0x30:1000:52455314
FrSky from S.Port: 0x0D:0x30:4901:5F314C41
FrSky from S.Port: 0x0D:0x30:5002:4F544F52
FrSky from S.Port: 0x0D:0x30:4303:00EC4C4F
FrSky from S.Port: 0x0D:0x30:1000:52455314
FrSky from S.Port: 0x0D:0x30:4901:5F314C41
FrSky from S.Port: 0x0D:0x30:5002:4F544F52
FrSky from S.Port: 0x0D:0x30:4303:00EC4C4F
FrSky from S.Port: 0x0D:0x30:1000:52455314
FrSky from S.Port: 0x0D:0x30:4901:5F314C41
FrSky from S.Port: 0x0D:0x30:5002:4F544F52
FrSky from S.Port: 0x0D:0x30:4303:00EC4C4F
FrSky from S.Port: 0x0D:0x30:1000:52455314
FrSky from S.Port: 0x0D:0x30:4901:5F314C41
FrSky from S.Port: 0x0D:0x30:5002:4F544F52
FrSky from S.Port: 0x0D:0x30:4303:00EC4C4F
FrSky from S.Port: 0x0D:0x30:1000:52455314
FrSky from S.Port: 0x0D:0x30:4901:5F314C41
FrSky from S.Port: 0x0D:0x30:5002:4F544F52
FrSky from S.Port: 0x0D:0x30:4303:00EC4C4F
FrSky from S.Port: 0x0D:0x30:1000:52455314
FrSky from S.Port: 0x0D:0x30:4901:5F314C41
FrSky from S.Port: 0x0D:0x30:5002:4F544F52
FrSky from S.Port: 0x0D:0x30:4303:00EC4C4F
FrSky from S.Port: 0x0D:0x30:1000:52455314
FrSky from S.Port: 0x0D:0x30:4901:5F314C41
FrSky from S.Port: 0x0D:0x30:5002:4F544F52
FrSky from S.Port: 0x0D:0x30:4303:00EC4C4F

yaapu · November 14, 2019, 4:34pm

yesss, looking good!

Eric_Stockenstrom · November 18, 2019, 11:17am

Another small step. We could use the param index rather than the id string.

Did I mention we are off to Australia for a month, leaving tomorrow? I have to pause this project now, but will work from there.

S.Port Read raw: 0D 30 00 10 14 53 45 52 	0x0D:0x30:1000:52455314
S.Port Read raw: 0D 30 01 49 41 4C 31 5F 	0x0D:0x30:4901:5F314C41
S.Port Read raw: 0D 30 02 50 52 4F 54 4F 	0x0D:0x30:5002:4F544F52
S.Port Read raw: 0D 30 03 43 4F 4C EC 00 	0x0D:0x30:4303:00EC4C4F
Mavlite #20 Request_Param_Read :SERIAL1_PROTOCOL:
Mavlink from FC #22 Param_Value: param_id=SERIAL1_PROTOCOL  param_value=2.0000  param_count=802  param_index=65535

yaapu · November 18, 2019, 11:31am

great Eric the mavlite decoder is working!

[Q] are you doing your testing in air mode or ground mode?

yes I know but to get the index I’d have to query the param first and cache the response.
In a regular GCS I’d do it this way but in lua I’m already using memory to store paramenter names and I opted to trade some memory vs speed, but I’m open to change it, actually it would improve speed quite a bit but only after the first pass with full parameter names.

yes you did have a nice trip Eric!

Eric_Stockenstrom · November 18, 2019, 11:50am

Air mode right now. Thank you.

tridge · December 2, 2019, 7:25am

@yaapu I’ve done a PR for an implementation of FPort here:

This support FPort for RC input. I’m hoping you can give me some assistance with the 2nd part of this, which is to use FPort for the SPort pass-through output as well.
What I need to know is how to deal with the “prim” and “appid” fields that come in with a FPort control packet and how they relate to the sport IDs in the ArduPilot sport passthru code.

yaapu · December 2, 2019, 8:24am

Hi @tridge,
prim_id is the frame type, ardupilot as of current (no bidirectional) only used 0x10, so each packet sent down the link had 0x10 as prim_id.
app_id is used by regular frsky sensors to distinguish “instances”, but ardupilot used it to mark packets with prim_id 0x10 as passthrough by using for app_id a “special” range above 0x5000 known as the DIY packet range.
By using 0x10 as prim_id and 0x5000 ad app_id the payload is only 4 bytes, OpenTX passes to LUA all 0x10 packets with app_id in the DIY range.
downlink/control traffic (master → slave) should use prim_id 0x00/0x10(R/W), 0x30(RO), 0x31(WO)
uplink traffic (slave → master) should use 0x10 and 0x32 where 0x32 is special because OpenTX always passes 0x32 up to the lua stack irregardless of the app_id, in this case the payload is 6 bytes, app_id + data.

every time we receive 0x10 or 0x00 we have a chance to send passtrough telemetry data
every time we receive 0x30,0x31 we should invoke the bidirectional passthrough packet decoder by passing app_id and data (6 bytes) (we will eventually respond with prim_id 0x32)
we respond to 0x00/0x10 with 0x10, and to 0x30,0x31 with 0x32, so for 0x00,0x10,0x30,0x31 the passthrough packet scheduler should be invoked.

hope this makes sense, later tonight I will have the time to check your code,

great stuff

Edit: I got your branch working on a Pixhawk1 + X4R, I wired the X4R sport pint to the RCIN pin
To process incoming packets from LUA (master->slave) you provide the decode_downlink() method, what we need is a method to write to the bus an fport_frame, something like send_uplink()

tridge · December 2, 2019, 10:10pm

I hacked together something for the F765-Wing which works with yaapu telem over fport. It is pretty ugly at the moment, but I think I can clean it up.
One thing that is wrong is the uart pin is idling high, whereas it should idle low as the fport signal is inverted (using a X4R). I checked with inav on the same board and it idles low. Something wrong in the uart config.

yaapu · December 2, 2019, 10:32pm

great! If you think I can be of any help, let me know!

tridge · December 3, 2019, 3:59am

I’ve updated the PR with first working version. To use it you need to set some serial port with:
SERIALn_PROTOCOL=23
which sets the serial port as RCIN. You will also need:
SERIALn_OPTIONS=7
to enable half-duplex inverted. After that the passthru telem output should work.
I’m looking at the possibility of supporting the output using soft-serial so it can work with a uart. FPort input does work in the PR for input without a UART, but not yet for output.

tridge · December 3, 2019, 8:53pm

I’ve got a test patch that allows FPort including telemetry without a UART, using soft-serial to output the SPort data. It does work, but is not suitable for merging in it’s current form.
One of the key issues is that when we don’t use a UART the RC protocol decoder doesn’t get told about a new frame until after it is too late to send the reply. The problem is that the SoftSigReader and SoftSigReaderInt implementations gather level transitions in pairs. So it can sit waiting for the first transition for the next packet before it passes the data to the RC decoder. I put in a hack to work around this just to test the soft-serial send part, but we need to fix this properly if we want to use a non-UART implementation for sport.

Eric_Stockenstrom · December 4, 2019, 2:26am

Hi Alex and tridge

Very nice progress, by the sounds of it! Alex I enjoyed your explanation of the FrSky sensor protocols. You illuminated some of the dark corners that have eluded me.

Tridge, it’s a pleasure to hear/see your insights into the workings of the ardupilot eco system. Awesome.

Alex I have bi-directional mavlite working in my modest little protocol translator, but I need to tidy up before I give you something to test. I don’t have a Horus here with me, so I hacked a spare esp32 board to send and read test data similar to openTX. It might not represent real life. Also, after reading your treatise above, I want to relook at how I respond to ids.

The Dragonlink boards arrived today, so I also look forward to playing with those. In between my vacation duties, of course.