Skip to content
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

add support for RC in AP_Periph #23805

Merged
merged 9 commits into from
Aug 19, 2023

Conversation

peterbarker
Copy link
Contributor

@peterbarker peterbarker commented May 16, 2023

This currently only allows for instantiation of RC in periph.

We still need a DroneCAN packet format, and an implementation of that both for AP_Periph (sending side) and AP_RCProtocol_DroneCAN (receiving side).

  • SBUS test (OrangeRX, throttle-based failsafe only)
  • FPort test (FS-A8S)
  • DSM test
  • failover test
  • recovery test
  • no-pulses vs SBUS failsafe test

@peterbarker
Copy link
Contributor Author

Conversation from Discord:

11:55 AM]Enki: Could be something like this. I think this would be very flexible, while also simple to configure, and simple to read and write packets. You could use the GetSet, but that is a bit messy to code for, as I discovered; would prefer sending and receiving a custom config packet that has these fields.

/// Configuration for this receiver node. Can be read and set over CAN, or USB.
/// Not currently in DroneCAN DSDL; addition pending. 
pub struct Config {
    /// Includes common CAN config, like bitrate, and if FD mode (64-byte packets) is enabled.
    pub common: ConfigCommon,
    /// Divide the ELRS update rate transmitted over the radio link by this. So, 1 means transmit every
    /// RC packet received. 2 means transmit every other update, etc.
    pub broadcast_ratio: u8,
    /// Up to 3 grades of precision, labeled `a`, `b`, and `c`. These `precision` settings
    /// are in bit-depth, and can be up to 64-bits. Broadcast order is all a channels, all b channels,
    /// all c channels, with no padding between. The indices are deteremined entirely by precision,
    /// and number of channels.
    /// 
    /// An example: Use 4 a channels as high-precision for control data. Use b channels as low-precision
    /// for switch and button positions, eg for arming or setting flight mode.
    pub ch_type_a_precision: u8,
    pub ch_type_b_precision: u8,
    pub ch_type_c_precision: u8,
    /// Number of channels per precision level.
    pub num_a_channels: u8,
    pub num_b_channels: u8,
    pub num_c_channels: u8,
}

impl Default for Config {
    fn default() -> Self {
        Self {
            common: Default::default(),
            broadcast_ratio: 1,
            ch_type_a_precision: 12,
            ch_type_b_precision: 4,
            ch_type_c_precision: 1,
            num_a_channels: 4,
            num_b_channels: 10,
            num_c_channels: 0,
        }
    }
}
[11:55 AM]Enki: Maybe something like that?
[12:00 PM]Peter Barker: Huh.  Much more complicate than I'd imagined   I was thinking something like 8 11-bit values + a 7-bit RSSI + 2 bits of "bank" - and 7 bits for random other things, that sort of thing.
[12:01 PM]Enki: Haven't even considered the link stats yet...
[12:01 PM]Peter Barker: In your scenario, how do you keep the RC receiver configuration in-step with the user's RC_Channels (and, sadly, RCMap) configuration? (edited)
[12:02 PM]Peter Barker: Protip: "they have to configure it" is probably not an option for most scenarios 
[12:02 PM]Enki: I think the sensible-defaults would help there
[12:02 PM]Enki: Don't have a good answer on syncing with the FC firmware
[12:02 PM]Enki: The FC could negotiate over CAN, but that adds complication
[12:03 PM]Enki: Maybe the answer is lean heavily on the defaults, but have the node fundamentally capable of flexibility
[12:03 PM]Enki: Ie maybe that default config above (it's somewhat arbitrary as-is) would be used in 99% of cases
[12:04 PM]Peter Barker: What does a data packet look like in such a scheme?  Essentially "here's a whole bunch of bits, you know what's going on"?
[12:04 PM]Enki: Config packet, or channel data?
[12:05 PM]Peter Barker: channel data
[12:06 PM]Enki: Using the default I posted:
12 bits of channel 1
12 bits of channel 2
12 bits of channel 3
12 bits of channel 4
4 bits of channel 5
4 bits of channel 6
(8 more 4-bit channels)
[12:06 PM]Enki: No padding
[12:06 PM]Enki: (Not byte-aligned unfortunately)
[12:07 PM]Peter Barker: Right, but it's up to the user of the packet to know what's in it, so the only thing the DroneCAN packet can be is "lump of bits containing data packed according to node configuration".
[12:07 PM]Enki: Ways the user could know
- Datasheet that shows the default
- Reading the config over CAN
[12:07 PM]Enki: - Plugging the device into a PC and reading using the included PC software
[12:08 PM]Peter Barker: Yep.  Just making sure I know what's going on.  I think what I'm saying is right.
[12:08 PM]Enki: In case it's not clear, I'm making this up as I go!
[12:09 PM]Enki: Would love to get input from diff people with diff use cases
[12:09 PM]Enki: Will probably continue prototyping using CRSF initially
@Enki
Will probably continue prototyping using CRSF initially
[12:13 PM]Peter Barker: Yep.  I've been trying not to think about the bi-directional telemetry stuff when thinking about this - do you see a compelling reason to couple telemetry (including mavlink-passthrough commands) to receiving of RC signals?  I'm thinking separate data packets on the CANBus are the right thing to do there.  Perhaps interpret the CRSF telemetry (or SPort) on the periph node and translate into a common packet format on the CANBus.
[12:13 PM]Enki: I concur re decoupling
[12:13 PM]Enki: 2 unrelated concepts IMO
[12:13 PM]Enki: other than they are both RF data between ground station and aircraft
[12:15 PM]Enki: *Eh I guess it depends on what messages specifically are going to/from

@peterbarker
Copy link
Contributor Author

1:26 PM]Enki: My latest thought is still this for a config packet, with 3 classes of channels, where precision is user-defined:
/// Configuration for this receiver node. Can be read and set over CAN, or USB.
/// Not currently in DroneCAN DSDL; addition pending.
pub struct Config {
    /// Includes common CAN config, like bitrate, and if FD mode (64-byte packets) is enabled.
    pub common: ConfigCommon,
    /// Divide the ELRS update rate transmitted over the radio link by this. So, 1 means transmit every
    /// RC packet received. 2 means transmit every other update, etc.
    pub broadcast_ratio: u8,
    /// Up to 3 grades of precision, labeled `a`, `b`, and `c`. These `precision` settings
    /// are in bit-depth, and can be up to 64-bits. Broadcast order is all a channels, all b channels,
    /// all c channels, with no padding between. The indices are deteremined entirely by precision,
    /// and number of channels.
    ///
    /// An example: Use 4 a channels as high-precision for control data. Use b channels as low-precision
    /// for switch and button positions, eg for arming or setting flight mode.
    pub ch_type_a_precision: u8,
    pub ch_type_b_precision: u8,
    pub ch_type_c_precision: u8,
    /// Number of channels per precision level.
    pub num_a_channels: u8,
    pub num_b_channels: u8,
    pub num_c_channels: u8,
}
[1:26 PM]Enki: But, that may be too flexible
[1:36 PM]Enki: Another approach is fixed-size but generous (at the expensve of packet-size), like u16 high-precision channels
[1:42 PM]Peter Barker: Why the extra precision?  11 bits ought to be enough for anybody   I assume you think those extra 5 bits are worth-while?  That an operator can move their sticks in sub-0.1% increments?  And notice the difference? (edited)
NEW
[2:03 PM]Enki: I'm trying to think of the broader use case
[2:04 PM]Enki: The unknown unknowns
[2:04 PM]Enki: I see your point though. Need to keep this grounded in practical use cases
[2:05 PM]Enki: Maybe 12
[2:06 PM]Enki: Given your point about hand-adjusted channel data not needing more than 11 or so bits, what are your thoughts on handling switch, button etc channels?
[2:13 PM]Enki: Would you do something like:

struct Config {
    /// Number of 12-bit channels, ie for control-channel-data
    num_high_precision_channels: u8,
    num_5_bit_channels: u8,
    num_4_bit_channels: u8,
    num_3_bit_channels: u8,
    num_2_bit_channels: u8,
    num_1_bit_channels: u8,
}


Where the packet is arranged, as before, with no padding, in the order prescribed by a config as above
[2:14 PM]Enki: Something like that might strike a nice balance

@David-OConnor
Copy link
Contributor

David-OConnor commented May 16, 2023

Update from more convo on Discord: Peter has perhaps talked me off the ledge of arbitrary channel precision, with the justification that control channels, for practical purposes, may not need more than 11 (or 12?) bits. I'm unable to come up with a counter-example.

My latest best-shot at what a config looks like may be something like this:

struct Config {
    /// Divide the update rate transmitted over the radio link by this. So, 1 means transmit every
    /// RC packet received. 2 means transmit every other update, etc. 0 means don't transmit.
    broadcast_ratio: u8,
    /// Number of 12-bit channels, ie for control-channel-data
    num_high_precision_channels: u8,
    /// Number of 5-bit channels, eg for selecting from a number of options
    num_5_bit_channels: u8,
    num_4_bit_channels: u8,
    /// Multi-position switches. For example, for setting flight mode
    num_3_bit_channels: u8,
    /// Eg 3-position switches
    num_2_bit_channels: u8,
    /// Eg 2-position switches
    num_1_bit_channels: u8,
}    

Data would be transmitted in the order listed, with no padding. So, if configured for 4 high-precision (say, 11-bit) channels and 2 1-bit channels, the first 44 bits would be control channel data, and the last 2 bits would be 1-bit channels.

Here's more background chat where Hydra and I proposed a more flexible (and complicated) protocol: dronecan/DSDL#21

@peterbarker
Copy link
Contributor Author

peterbarker commented May 17, 2023

This is an example of a minimalist RC input packet. Notionally we could send 5 11 bit values in a single packet with this same format but nobody uses only 5 channels.

edit: added num_valid_channels

#
# support for RC input
#

# quality metric, -1-100, -1 meaning unknown, 0 meaning no RC input
#  being received by this sensor

int7_t quality

# bank of input.  Find channel by multiplying this number by 6 and adding input number:
uint2 bank 

# number of channels valid in this packet:
uint4 num_valid_channels

# input1; if bank is zero this is channel 1.  Find value by adding 500
to this value.
uint11 input1

# more RC inputs:
uint11 input2
uint11 input3
uint11 input4
uint11 input5
uint11 input6
uint11 input7
uint11 input8
uint11 input9
uint11 input10


# 7 + 2 + 4 + (10*11) == 123 bits == 14bytes+7 bits == 2 packets  (5 bits spare)

# could go with 9 12-bits or 10 bits of precision.

@tridge
Copy link
Contributor

tridge commented May 17, 2023

how about this:

uint8 STATUS_RSSI_VALID = 1 # RSSI field is valid
uint8 STATUS_FAILSAFE = 2 # receiver has lost contact with transmitter
uint8 status

uint8 rssi
uint12[<=24] rcin

@David-OConnor
Copy link
Contributor

I think RSSI would be a better fit for a separate link stats packet type. It would include info like RSSI, % of recent packets received, signal strength, xmitter power, active antenna etc.

@peterbarker
Copy link
Contributor Author

So for 10 channels coming in:

(8 + 8 + 10*12)/8.0
17.0

... just a little too much?

@tridge
Copy link
Contributor

tridge commented May 17, 2023

So for 10 channels coming in:

it is just as likely to be 12 or 16 channels

@tridge
Copy link
Contributor

tridge commented May 17, 2023

maybe uint16 status, so we can support things like "STATUS_SUPPORTS_FRSKY" etc

@tridge tridge removed the DevCallEU label May 17, 2023
@cbf123
Copy link

cbf123 commented May 17, 2023

This is for an on-the-wire protocol, yes? Unless bandwidth is a major factor, why not just keep it simple and have an arbitrary number of 12-bit channels plus a header for random stuff?

And I think we'd want to support diversity receivers, so two separate rssi values and an "active antenna" bit might make sense.

@David-OConnor
Copy link
Contributor

David-OConnor commented May 17, 2023

After some thinking, an arbitrary num of 12-bit channels as Tridge proposed would be fine. Would be a bit heavy if some of the channels are for 2-bit switches, but maybe that's OK, and better from a simplicity perspective. Or agree on a packing protocol between FC firmware and Rx nodes as a backup.

For a linkstats packet, thoughts on something like this? This is the CRSF one, which is a good baseline.

#[derive(Default)]
/// https://www.expresslrs.org/3.0/info/signal-health/
pub struct LinkStats {
    /// Timestamp these stats were recorded. (TBD format; processed locally; not part of packet from tx).
    pub timestamp: u32,
    /// Uplink - received signal strength antenna 1 (RSSI). RSSI dBm as reported by the RX. Values
    /// vary depending on mode, antenna quality, output power and distance. Ranges from -128 to 0.
    pub uplink_rssi_1: u8,
    /// Uplink - received signal strength antenna 2 (RSSI). Second antenna RSSI, used in diversity mode
    /// (Same range as rssi_1)
    pub uplink_rssi_2: u8,
    /// Uplink - link quality (valid packets). The number of successful packets out of the last
    /// 100 from TX → RX
    pub uplink_link_quality: u8,
    /// Uplink - signal-to-noise ratio. SNR reported by the RX. Value varies mostly by radio chip
    /// and gets lower with distance (once the agc hits its limit)
    pub uplink_snr: i8,
    /// Active antenna for diversity RX (0 - 1)
    pub active_antenna: u8,
    pub rf_mode: u8,
    /// Uplink - transmitting power. See the `ElrsTxPower` enum and its docs for details.
    pub uplink_tx_power: TxPower,
    /// Downlink - received signal strength (RSSI). RSSI dBm of telemetry packets received by TX.
    pub downlink_rssi: u8,
    /// Downlink - link quality (valid packets). An LQ indicator of telemetry packets received RX → TX
    /// (0 - 100)
    pub downlink_link_quality: u8,
    /// Downlink - signal-to-noise ratio. SNR reported by the TX for telemetry packets
    pub downlink_snr: i8,
}

@peterbarker
Copy link
Contributor Author

This is for an on-the-wire protocol, yes? Unless bandwidth is a major factor, why not just keep it simple and have an arbitrary number of 12-bit channels plus a header for random stuff?

And I think we'd want to support diversity receivers, so two separate rssi values and an "active antenna" bit might make sense.

This one does have a bunch of 12-bit values. Interesting point on the dual-rssi values - which RC protocols bare that information?

@peterbarker peterbarker force-pushed the pr/ap-periph-rcprotocol branch 7 times, most recently from e7d0ee8 to 2e306ac Compare May 18, 2023 13:47
@MallikarjunSE
Copy link
Contributor

Doesn't periph require sbus inverter? Very few periphs with H7 breakouts.

@cbf123
Copy link

cbf123 commented May 18, 2023

Interesting point on the dual-rssi values - which RC protocols bare that information?

Pretty sure CRSF (used by Crossfire and ELRS and maybe others) does.

ELRS recently merged ExpressLRS/ExpressLRS#2143

@peterbarker
Copy link
Contributor Author

I've flown this on a quadcopter today; MatekL431 node with a Turnigy iBus receiver hanging off it.

@@ -563,6 +571,43 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
GOBJECT(rpm_sensor, "RPM", AP_RPM),
#endif

#ifdef HAL_PERIPH_ENABLE_RCIN
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

put back dev call topic - I'd like to discuss creating sub-table in rc_in.cpp, which I think we should do for new features

@peterbarker peterbarker merged commit 9fb872c into ArduPilot:master Aug 19, 2023
81 checks passed
@peterbarker peterbarker deleted the pr/ap-periph-rcprotocol branch August 21, 2023 03:19
@Hwurzburg Hwurzburg added the WikiNeeded needs wiki update label Jan 14, 2024
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

8 participants