The IOmcu's sampling schedule for the sensor bus: what is read on each 1ms tick, the 50ms windows made from it, the run's peaks and totals, and the servo move capture on the FAST channel. More...

#include <stdbool.h>
#include <stdint.h>
#include "as5600.h"
#include "ina228.h"
#include "ina3221.h"
#include "sense_bus.h"
#include "servo_move.h"

Data Structures
struct	sense_sched_io_t
	The wire, the clock and the bus recovery. More...
struct	sense_i3221_cfg_t
	The INA3221s: which are fitted, each channel's shunt, and FAST. More...
struct	sense_sched_cfg_t
	What is fitted and how; a part not enabled is never addressed. More...
struct	sense_window_t
	One complete window of one source. More...
struct	sense_acc_t
	A window being filled. More...
struct	sense_ring_win_t
	One FAST window as the ring keeps it. More...
struct	sense_run_t
	The run since sense_sched_arm(), from the INA228. More...
struct	sense_cap_arm_t
	A capture's levels, µA. More...
struct	sense_cap_pre_t
	A FAST sample held while armed, for the filter. More...
struct	sense_fast_t
	A FAST sample kept for the level before a command. More...
struct	sense_cap_t
	The state of a capture of a FAST move. More...
struct	sense_enc_t
	The output encoder as last read. More...
struct	sense_sched_t
	The sampling schedule of the sensor bus: its parts, windows and captures. More...

Macros
#define	SENSE_WINDOW_MS 50u
#define	SENSE_ROTATION 20u
	Slots in the rotation; one is read every 2ms, so a rotation is 40ms.
#define	SENSE_WIN_RING 4u
	FAST windows the ring keeps: 200ms.
#define	SENSE_CAP_FILTER_N 4u
	The capture's filter: a moving mean of 4 samples, 4ms.
#define	SENSE_ENC_STILL_TOL 12u
	The encoder's tolerance for holding still: 12 counts, 1.05 degrees.
#define	SENSE_ENC_MAG_EVERY 25u
	One slot in this many reads AGC and MAGNITUDE besides the angle.
#define	SENSE_CAP_SETTLE_N 10u
	Samples in a row at an end held harder than the servo moves: 10ms.
#define	SENSE_CAP_LAG_MS 5u
	The capture's lag, added to the window: the filter's 3ms, up to 1ms from a conversion to its read, and the 140µs conversion, rounded up.
#define	SENSE_CAP_T_PER_MS 10u
	The capture's time unit: 0.1ms.
#define	SENSE_CAP_EDGE_WAIT_MS SERVO_MOVE_TIMEOUT_MS
	An armed capture given no edge in this long ends lost: nothing renders the frame it waits for.
#define	SENSE_CAP_RISE_AUTO INT32_MIN
	sense_cap_arm_t.rise_ua: the level before the command taken from FAST's SENSE_WINDOW_MS before the edge.
#define	SENSE_FAST_HISTORY SENSE_WINDOW_MS
	FAST samples kept for that level: one window at 1kHz.
#define	SENSE_I3221_PARTS 4u
	INA3221 parts, at 0x40 to 0x43, and their channels.
#define	SENSE_CHANNELS 12u
#define	SENSE_CH(a, ch)
#define	SENSE_FAST_NONE 0xFFu
	sense_i3221_cfg_t.fast: no channel is read every tick.
#define	SENSE_SRC_INA228 SENSE_CHANNELS
	Where a window's readings come from: channel c (0 to 11) is source c, the INA228 (the ESC's path, electronic speed controller) the one after them.
#define	SENSE_SRC_COUNT (SENSE_CHANNELS + 1u)

Enumerations
enum	sense_cap_state_t { SENSE_CAP_IDLE = 0 , SENSE_CAP_ARMED , SENSE_CAP_WAITING , SENSE_CAP_MOVING , SENSE_CAP_ARRIVED , SENSE_CAP_SETTLED , SENSE_CAP_LATE , SENSE_CAP_UNSEEN , SENSE_CAP_LOST }
	The move capture's state, in the order a capture passes through; the numbers are link_cap_state_t's. More...

Functions
static unsigned	sense_ch (unsigned a, unsigned ch)
	Channel `ch` (1 to 3) of part `a` (0 to 3): 0 to 11.
void	sense_i3221_one (sense_i3221_cfg_t *m, bool en, uint8_t addr, uint32_t shunt_uohm, uint8_t channels)
	One INA3221 as the SENSE page sets it up before protocol 4.25: the part at `addr` fitted when `en` and `addr` lies in 0x40 to 0x43, each channel in `channels` (bit 0 CH1) on `shunt_uohm`, FAST its CH1 when CH1 is one of them.
void	sense_sched_init (sense_sched_t s, const sense_sched_io_t io, const sense_sched_cfg_t *cfg)
	A schedule over `io` for the parts `cfg` enables.
void	sense_sched_tick (sense_sched_t *s)
	One tick: every 1ms.
void	sense_sched_arm (sense_sched_t *s)
	The edge into driving: the run's peaks start again and the INA228's totals are cleared on the next tick.
bool	sense_sched_window (const sense_sched_t s, unsigned src, sense_window_t out)
	The last complete window of source `src` (a channel, or SENSE_SRC_INA228); false before one has closed.
bool	sense_sched_ring (const sense_sched_t s, unsigned back, sense_ring_win_t out)
	FAST's window numbered `back` before the newest complete one, `back` under SENSE_WIN_RING; false, and `out` not closed, for a number that was skipped, lies before the first window, or before one has closed.
bool	sense_sched_cap_arm (sense_sched_t s, const sense_cap_arm_t levels)
	Arm a capture on FAST with `levels`; rise_ua SENSE_CAP_RISE_AUTO takes the level before the command from FAST.
void	sense_sched_cap_edge (sense_sched_t *s, uint64_t edge_us)
	The PWM frame carrying the new pulse starts at `edge_us`, on the clock now_us() reads; it may lie ahead.
void	sense_sched_cap_disarm (sense_sched_t *s)
	Stop a capture; its count stays.
const ina3221_t *	sense_sched_fast_part (const sense_sched_t *s)
	FAST's INA3221, or NULL with no FAST channel.
uint16_t	sense_sched_enc_still_ms (const sense_sched_t *s)
	How long the encoder's angle has stayed within SENSE_ENC_STILL_TOL counts of its anchor, ms, saturating at 65535; 0 with no anchor.

Detailed Description

The IOmcu's sampling schedule for the sensor bus: what is read on each 1ms tick, the 50ms windows made from it, the run's peaks and totals, and the servo move capture on the FAST channel.

Pure logic. The wire is a sense_i2c_t and the time a callback, both given at init, so the host suite runs it against fake_ina.h; the IOmcu gives its I2C (Inter-Integrated Circuit) block and its 64-bit microsecond timer. Nothing here trips or disarms anything: the monitors measure only.

The parts. Up to SENSE_I3221_PARTS INA3221 at 0x40 to 0x43, part a at 0x40 + a, three channels each: channel c (0 to 11) is part c / 3's CH(c % 3 + 1), SENSE_CH(a, ch). Each channel has its own shunt, and a shunt of 0 leaves the channel off. One channel, FAST, is read every tick; its windows go into the ring and the move capture runs on it. Beside them one INA228 and the AS5600 output encoder.

Each call of sense_sched_tick(), every 1ms: a bus recovery when sense_bus says one is due (the io's recover callback: 9 clocks on SCL, a STOP, the block re-initialised); a part's probe when due (at the start, and SENSE_RETRY_MS after it was found absent, wrong or offline), one INA3221 probe a tick at most; the INA228's accumulators cleared when sense_sched_arm() owes it; then the reads, on a 400kHz bus:

Read Rate FAST channel current 1000Hz every other channel's current, one a tick 1000 / n Hz for n (90.9Hz with 11) INA228 CURRENT, VBUS, alternating 500Hz each every channel's bus voltage, one an even tick 41.7Hz each INA228 DIAG_ALRT, ENERGY, CHARGE 50Hz each INA228 DIETEMP, ADC_CONFIG; each INA3221's Mask/Enable and Configuration 25Hz each

A transaction of N bytes is 9 * (3 + N) + 3 clocks, 2.5µs each at 400kHz: a 2-byte read 120µs, the INA228's 3-byte CURRENT and VBUS 142.5µs, its 5-byte ENERGY and CHARGE 187.5µs. Every tick reads FAST, one other channel and one INA228 register; an even tick then one bus voltage and one item of the rotation (SENSE_ROTATION slots, 40ms: eight for the INA228, eight for the INA3221s' Mask/Enable and Configuration, four left empty); an odd tick the encoder. An even tick is at most 690µs of bus time, 742.5µs in the tick in which a set-up register is read a second time; an odd tick 600µs, 720µs in an encoder field slot. Bus time over a second, arithmetic only, with 4 parts and all 12 channels: 49.7% without the encoder, 60.8% with it; with one part reading CH1 and the INA228, 30.4%. Not counted: the controller's own time between transactions, not measured. The bus runs at 400kHz only: at 100kHz a read takes 480 to 750µs, and the tick does not fit. A probe is 10 transactions on the INA228, 1.1ms, and 4 on the INA3221, 455µs: the ticks it covers run late.

A part that reset itself. Both kinds answer every read after a reset of their own (supply under about 1.26V at the INA228, SLYS021A §7.4.2), on their power-on set-up: the INA228 at ADCRANGE 0, where a set-up at ADCRANGE 1 reads a quarter of the current; the INA3221 on all three channels with 1.1ms conversions, a result every 6.6ms. The rotation reads ADC_CONFIG and each Configuration back (ina228_verify(), ina3221_verify()). A part found on another value than the one written, twice in a row or once as FFFFh (the bus that no part drives), goes offline and is probed and set up again SENSE_RETRY_MS on; each time is counted modulo 256 (i228_resets, i3221_resets[a]); the windows being filled from it are emptied; the INA228's totals are no longer the run's; and a capture under way on that INA3221 ends lost. A reset is found within one rotation, 40ms: the samples read in that time are in the window before the one emptied when a window boundary lies between – the INA228's at the reset range, the INA3221's right in value and up to 6.6ms old. The readings are not checked for all ones: until the read-back finds a bus that no part drives, within the same 40ms, an all-ones reading is code -1, -390µA on a 200 µΩ INA228 shunt, -400µA on a 0.1Ω INA3221 shunt, and -8mV of INA3221 bus voltage.

A channel that is off, and a part that is not fitted, not online or on a stuck bus, is not read: its windows stay empty. A stuck bus takes every part offline within 3 reads of each. After a recovery the bus counts as stuck until a transaction goes through, and with every part offline the next one is a probe: the flag can outlast a freed bus by up to SENSE_RETRY_MS.

Windows. Fixed SENSE_WINDOW_MS windows on the IOmcu clock, counted in 64 bits from the first tick, each with its number. A read is stamped when it is done and counts in the window that time falls in, whatever ran before it in the tick. For each source – the 12 INA3221 channels and the INA228 – the mean, lowest and highest current and the mean and lowest bus voltage. A clipped current sample carries no value: it adds nothing to the figures and sets the window's clip flag for its end of the range. Reading a window ends nothing: the last complete window stays readable until the next one closes. A tick late by more than a window leaves the windows it missed out, so their numbers are skipped.

The ring. FAST's last SENSE_WIN_RING complete windows, newest first: entry k is the window numbered k before the newest, and reads not closed for a number that was skipped or lies before the first window. With no FAST channel every entry closes empty. A ring entry counts every FAST sample: a clipped one is the end of the range it read (ina3221_end_ua(): 1.638A or -1.6384A on the 0.1Ω shunt) in the mean, the highest and the lowest, and is counted in n_clip. The ring's figures and sense_sched_window()'s differ in a window with a clipped sample, and only there.

The run. sense_sched_arm() is the edge into driving: the run's lowest INA228 voltage and highest current and power start again, and the INA228's ENERGY and CHARGE are cleared (CONFIG.RSTACC) on the next tick. Power is a current sample times the voltage read in the INA228 slot just before it, 1ms earlier, and none when that read failed. A step in both within that 1ms pairs the new current with the old voltage. The totals are this run's while totals_ok holds: the clear went through, and no INA228 transaction has failed since. The first one that does ends them for the run, as does a part not online at the clear. The totals start at the clear, up to one tick after the arm, later when the clear's write fails and is repeated.

The move capture. sense_sched_cap_arm() gives the move's levels; sense_sched_cap_edge() gives the time of the PWM (pulse-width modulation) frame that carries the new pulse, which the caller reads off the slice's counter. FAST samples are then judged by servo_move.h's rules, times in 0.1ms from that edge and resolved to FAST's 1ms sample interval: the moving mean of SENSE_CAP_FILTER_N samples, seeded at the edge with the last of them taken while armed, settled over SENSE_CAP_SETTLE_N samples, and a window of SERVO_MOVE_TIMEOUT_MS plus the capture's lag, the deadline checked before a sample is judged: a sample at or past it is never the arrival. FAST samples are stamped when FAST's read is done. A one-sample excursion starts a move only when a quarter of it passes the threshold. A clipped FAST sample is at or past the full scale less a step: 1.6376A on the 0.1Ω shunt. FAST's INA3221 leaving online ends a capture as lost, and so does an armed capture given no edge within SENSE_CAP_EDGE_WAIT_MS of the arm. The capture states take link_cap_state_t's numbers.

The level before the command. A capture armed with rise_ua SENSE_CAP_RISE_AUTO takes it from FAST itself: the mean of the FAST samples stamped in the SENSE_WINDOW_MS before the edge, a clipped one counted at the end of the range as the filter takes it. The schedule keeps the last SENSE_WINDOW_MS FAST samples for it, whether a capture is armed or not. The level is taken when the first FAST sample at or past the edge arrives, so an edge that lies ahead counts the samples taken up to it. A capture with no FAST sample in those 50ms ends as lost: the INA3221 did not answer.

The output encoder. An AS5600 on the same bus (as5600.h), when enabled, is read on the ticks that read no rotation item – every second tick, 500Hz: STATUS (1 byte) and RAW ANGLE (2 bytes), two transactions, 217.5 µs of bus time. Every SENSE_ENC_MAG_EVERY-th slot (20Hz) reads RAW ANGLE, AGC (1 byte) and MAGNITUDE (2 bytes) instead, 337.5µs: the angle is read in every slot, STATUS in 24 of 25. The angle is kept as the 12-bit count; the schedule judges nothing from it but how long it has held still.

Still time. The angle holds an anchor, the count and the time of the last sample that lay more than SENSE_ENC_STILL_TOL counts from the previous anchor on the circle; that sample is the new anchor. sense_sched_enc_still_ms() is the time since the anchor, saturating at 65535ms: how long the output has stayed within SENSE_ENC_STILL_TOL counts of where it is. A move that settles gives the time its last anchor was set – the moment the angle came within the tolerance of its final value – as now minus the still time, to the sample interval of 2ms. The first sample after a set-up, and the first after the part has been offline, set the anchor. A sample whose STATUS has MD clear (no magnet, as5600.h) is no position: it clears the anchor, the still time reads 0, and the first sample with MD set is the new anchor. A field slot reads no STATUS and goes by the last one read, in the slot 2ms before.

Not known: the INA3221's noise at 140µs conversions, and so whether 4 samples of filter and 10 of settling suit it; the controller's time between transactions.

SPDX-License-Identifier: MIT

Macro Definition Documentation

◆ SENSE_CAP_EDGE_WAIT_MS

#define SENSE_CAP_EDGE_WAIT_MS SERVO_MOVE_TIMEOUT_MS

An armed capture given no edge in this long ends lost: nothing renders the frame it waits for.

The panel writes the command straight after the arm, and a frame at the slowest PWM rate, 40Hz, is 25ms; the bound is the move's own window.

◆ SENSE_CAP_FILTER_N

#define SENSE_CAP_FILTER_N 4u

The capture's filter: a moving mean of 4 samples, 4ms.

A step shows half after 1.5 samples and whole after 3: a fall from a moving current far above the band arrives 3ms late. Chosen, not measured: the noise is not known.

◆ SENSE_CH

#define SENSE_CH	(		a,
			ch )

Value:

sense_ch((unsigned)(a), (unsigned)(ch))

sense_ch

static unsigned sense_ch(unsigned a, unsigned ch)

Channel ch (1 to 3) of part a (0 to 3): 0 to 11.

Definition sense_sched.h:250

◆ SENSE_ROTATION

#define SENSE_ROTATION 20u

Slots in the rotation; one is read every 2ms, so a rotation is 40ms.

An item read at 50Hz has two of them.

◆ SENSE_WINDOW_MS

#define SENSE_WINDOW_MS 50u

one window

Enumeration Type Documentation

◆ sense_cap_state_t

enum sense_cap_state_t

The move capture's state, in the order a capture passes through; the numbers are link_cap_state_t's.

Enumerator
SENSE_CAP_ARMED	levels given, waiting for the edge
SENSE_CAP_WAITING	edge given, no movement yet
SENSE_CAP_SETTLED	arrived at an end held harder than it moves
SENSE_CAP_LATE	movement, no arrival in the window
SENSE_CAP_UNSEEN	no movement in the window
SENSE_CAP_LOST	the INA3221 stopped answering

Function Documentation

◆ sense_ch()

unsigned sense_ch	(	unsigned	a,
		unsigned	ch )

inlinestatic

Channel ch (1 to 3) of part a (0 to 3): 0 to 11.

Parameters

a	Part index, 0 to 3.
ch	Channel of the part, 1 to 3.

Returns: The channel index, 0 to 11.

◆ sense_i3221_one()

void sense_i3221_one	(	sense_i3221_cfg_t *	m,
		bool	en,
		uint8_t	addr,
		uint32_t	shunt_uohm,
		uint8_t	channels )

One INA3221 as the SENSE page sets it up before protocol 4.25: the part at addr fitted when en and addr lies in 0x40 to 0x43, each channel in channels (bit 0 CH1) on shunt_uohm, FAST its CH1 when CH1 is one of them.

Anything else in m is cleared.

Parameters

m	The map entry to fill.
en	true when the part is fitted.
addr	I2C address of the part.
shunt_uohm	Shunt resistance, in µΩ, for each channel it has.
channels	Channels to read, bit 0 CH1.

◆ sense_sched_arm()

void sense_sched_arm ( sense_sched_t * s )

The edge into driving: the run's peaks start again and the INA228's totals are cleared on the next tick.

Parameters

s	The schedule.

◆ sense_sched_cap_arm()

bool sense_sched_cap_arm	(	sense_sched_t *	s,
		const sense_cap_arm_t *	levels )

Arm a capture on FAST with levels; rise_ua SENSE_CAP_RISE_AUTO takes the level before the command from FAST.

Refused, and nothing changed, with no FAST channel or while its INA3221 is not online.

Parameters

s	The schedule.
levels	The capture levels.

Returns: true when the capture is armed; false when refused.

◆ sense_sched_cap_disarm()

void sense_sched_cap_disarm ( sense_sched_t * s )

Stop a capture; its count stays.

Parameters

s	The schedule.

◆ sense_sched_cap_edge()

void sense_sched_cap_edge	(	sense_sched_t *	s,
		uint64_t	edge_us )

The PWM frame carrying the new pulse starts at edge_us, on the clock now_us() reads; it may lie ahead.

The samples taken while armed seed the filter. Nothing unless a capture is armed.

Parameters

s	The schedule.
edge_us	Start of the PWM frame carrying the new pulse, in µs.

◆ sense_sched_enc_still_ms()

uint16_t sense_sched_enc_still_ms ( const sense_sched_t * s )

How long the encoder's angle has stayed within SENSE_ENC_STILL_TOL counts of its anchor, ms, saturating at 65535; 0 with no anchor.

Parameters

s	The schedule.

Returns: Time since the encoder angle was last anchored, in ms; 0 before an anchor, 65535 at most.

◆ sense_sched_fast_part()

const ina3221_t * sense_sched_fast_part ( const sense_sched_t * s )

FAST's INA3221, or NULL with no FAST channel.

Parameters

s	The schedule.

Returns: FAST's INA3221, or NULL with no FAST channel.

◆ sense_sched_init()

void sense_sched_init	(	sense_sched_t *	s,
		const sense_sched_io_t *	io,
		const sense_sched_cfg_t *	cfg )

A schedule over io for the parts cfg enables.

A part whose set-up the driver refuses is left unset, never addressed, and its error kept in i228_setup or i3221_setup[a]; a FAST that names a channel off is taken as none. The bus is the caller's to open at 400kHz.

Parameters

s	The schedule to set up.
io	The callbacks the schedule calls.
cfg	The parts to enable.

◆ sense_sched_ring()

bool sense_sched_ring	(	const sense_sched_t *	s,
		unsigned	back,
		sense_ring_win_t *	out )

FAST's window numbered back before the newest complete one, back under SENSE_WIN_RING; false, and out not closed, for a number that was skipped, lies before the first window, or before one has closed.

Parameters

s	The schedule.
back	How many windows before the newest complete one.
out	Receives the window.

Returns: true with a window; false for a number that was skipped, lies before the first window, or before one has closed.

◆ sense_sched_tick()

void sense_sched_tick ( sense_sched_t * s )

One tick: every 1ms.

Parameters

s	The schedule.

◆ sense_sched_window()

bool sense_sched_window	(	const sense_sched_t *	s,
		unsigned	src,
		sense_window_t *	out )

The last complete window of source src (a channel, or SENSE_SRC_INA228); false before one has closed.

Parameters

s	The schedule.
src	The source: a channel, or SENSE_SRC_INA228.
out	Receives the window.

Returns: true with a window; false before one has closed.

Data Structures

Macros

Enumerations

Functions

Detailed Description

Macro Definition Documentation

◆ SENSE_CAP_EDGE_WAIT_MS

◆ SENSE_CAP_FILTER_N

◆ SENSE_CH

◆ SENSE_ROTATION

◆ SENSE_WINDOW_MS

Enumeration Type Documentation

◆ sense_cap_state_t

Function Documentation

◆ sense_ch()

◆ sense_i3221_one()

◆ sense_sched_arm()

◆ sense_sched_cap_arm()

◆ sense_sched_cap_disarm()

◆ sense_sched_cap_edge()

◆ sense_sched_enc_still_ms()

◆ sense_sched_fast_part()

◆ sense_sched_init()

◆ sense_sched_ring()

◆ sense_sched_tick()

◆ sense_sched_window()