AP_WheelEncoder: add MAVLink backend and support up to four encoders

[Ali-barari]

Summary

Adds a wheel encoder backend fed by the WHEEL_DISTANCE message (WENCx_TYPE = 2), for vehicles whose encoders are read by an external board rather than by the autopilot's own pins. Also raises the instance limit to four, so that a vehicle can have one encoder per wheel rather than one per side.

Classification & Testing (check all that apply and add your own)

• Checked by a human programmer

• Non-functional change

• No-binary change

• Infrastructure change (e.g. unit tests, helper scripts)

• Automated test(s) verify changes (e.g. unit test, autotest)

• Tested manually, description below (e.g. SITL)

• Tested on hardware

• Logs attached

• Logs available on request

• New autotest Rover.MAVLinkWheelEncoders. It streams WHEEL_DISTANCE for four wheels at about 3–5Hz from a component on the vehicle, with all wheels at 0.5m/s and a 1.5s pause in the stream, and checks:

  • the vehicle reports the streamed distances;
  • WHEEL_DISTANCE from another system ID is ignored;
  • WENC logs all four distances;
  • from the replay log (RWOH), every sample sent to EKF3 is close to the true speed, no readings are lost, no sample spans the pause, and readings resume after it.

• The test fails without the fix. With the existing update_wheel_encoder() it fails on a fabricated sample (speed 0.000m/s (DA=0 DT=0.040)). With a variant that discards readings more than 100ms apart, no samples reach EKF3. Without the gap check it fails on a sample spanning the pause (DT=1.94).

• Existing tests pass. Rover.WheelEncoders still passes.

• Builds. Built for SITL, ARKV6X (4 instances) and MatekF405 (2 instances). Size change from size_compare_branches.py against the merge base, in bytes:

  Board	Rover	Copter
  ARKV6X	1064	32
  CubeOrange	832	32
  MatekF405	248	32

  The MAVLink backend and the extra instances are excluded on MatekF405 (1MB). The Copter change is the longer WENC format string.

• Real hardware: this branch on an ARKV6X in a four-wheel skid-steer rover. An external board reads the four encoders and streams WHEEL_DISTANCE at 25Hz over serial, using the autopilot's system ID. Over three drives:

  • the autopilot accepts and re-reports the board's distances (median difference 0.1mm);
  • from RWOH, each wheel is sent to EKF3 every 80ms (longest 122ms) with no spikes. About 1% of samples are zero, and only when the wheel is barely moving (under 0.25m/s either side), which is the encoder's resolution;
  • with wheel odometry selected (EKF3 source set with VELXY = 7, alongside external nav), XKFD velocity innovations are median 0.016m/s, 95th percentile 0.078m/s.

  An earlier version of this change was also run on ArduRover 4.6.3. Logs: wheelencoder_mavlink_hw_logs.zip, containing fc_log_140900.bin (drive with outputs and wheel feed) and fc_log_142610.bin (drive with wheel odometry fused).

Description

Rover already sends WHEEL_DISTANCE; it now also accepts it.

• AP_WheelEncoder: allow up to four instances. Defaults to 4 on boards with more than 1MB of flash and keeps 2 elsewhere. WHEELENCODER_MAX_INSTANCES can be overridden at build time. Vehicles that configure only the first two instances behave as before. AP_WheelRateControl still uses instances 0 and 1 (left and right).
• AP_Logger / AP_WheelEncoder: log the third and fourth encoders. WENC had fields for two encoders only. It gains Dist2/Qual2 and Dist3/Qual3, which are zero for encoders that are not present. The existing fields are unchanged, so graphs and tools that read Dist0/Dist1 keep working.
• AP_WheelEncoder: MAVLink backend.
  • Each instance takes the distance at its own index in the message (instance 1 reads distance[0], and so on).
  • Only messages from the vehicle's own system ID are accepted. Other vehicles on the same network stream their own WHEEL_DISTANCE, and these would otherwise be read as this vehicle's wheels.
  • The library stores counts, so the backend converts metres to counts using WENCx_CPR and WENCx_RADIUS. These cancel on the way back, so only the radius affects get_rate().
  • The backend is unhealthy if no reading has arrived for 500ms.
  • It is enabled by default on boards with more than 1MB of flash, and controlled by AP_WHEELENCODER_MAV_ENABLED, which is listed in build_options.py.
• Rover: route WHEEL_DISTANCE to the library.
• Rover: don't feed EKF3 stale or missing readings. update_wheel_encoder() assumed that an unchanged reading means the wheel has stopped. That holds for a locally decoded encoder but not for a streamed one. When the sender is slower than the 50Hz loop, EKF3 received a fabricated zero rotation, and the next real reading was divided by the short time since that send. For streamed sensors it now waits for a new reading, uses the time between readings, and discards readings that span a gap of more than 500ms. It also skips unhealthy instances, which includes an unconfigured instance below the highest configured one. Previously such an instance was fed to EKF3 as a stationary wheel. Locally decoded encoders are unchanged.

Notes for reviewers

• The AP_Logger and AP_WheelEncoder logging commits are split by subsystem and only build together.
• Wiki: 4.8:34815/ wheel-encoder: document MAVLink encoders and up to four encoders ardupilot_wiki#8186.
• get_distance() and get_signal_quality() do not bounds-check the instance, so Log_Write() only calls them for instances that exist.

[Ali-barari]

Hardware logs referenced in the description.
wheelencoder_mavlink_hw_logs.zip

[github-actions]

Global Size Summary (Total Flash delta in bytes)

Built from 2a442ea rebased onto dc78bc5

Board	Periph	tracker	blimp	bootloader	copter	heli	iofw	plane	rover	sub
DrotekP3Pro	_	_	_	*	_	_	_	_	_	_
Durandal	_	+36	+36	_	+36	+36	_	+36	+992	+36
Hitec-Airspeed	*	_	_	_	_	_	_	_	_	_
KakuteF7	_	+36	+36	_	+36	+36	_	+36	+248	+36
MambaH743v4	_	+36	+36	_	+36	+36	_	+36	+1016	+36
MatekF405-CAN	_	_	_	*	_	_	_	_	_	_
MatekF405	_	+36	+36	_	+36	+36	_	+36	+248	+36
MatekH743-bdshot	_	+36	+36	_	+36	+36	_	+36	+928	+36
Pixhawk1-1M	_	+36	+36	_	+36	+36	_	+36	+248	+36
disco	_	+32	+40	_	+40	+40	_	+40	+2280	+32
f103-GPS	*	_	_	_	_	_	_	_	_	_