ESA drivers

Add I²C sensors and actuators to ESA: each new device is a header–source pair written from a template.

How to read the templatesIn every template, MYSENS1 marks a value you change; the number points to its explanation below the template.

I²C sensor driver

A new I²C sensor is one header and one source file in src/sensing/I2C/. The header ends with a registration line; at program start the driver enters itself in the I²C registry (Table S2) under its bus–address pair, and any device-list row with that pair uses it. No other file of the library is edited.

Item	Convention	Example (shipped)
Location	`ESA/src/sensing/I2C/`	n/a
Files	`<DEV>.h` and `<DEV>.cpp`	`NAU7802.h`, `NAU7802.cpp`
Functions	`<DEV>__INIT_` and `<DEV>__READ_`: two underscores before, one after	`NAU7802__INIT_`, `NAU7802__READ_`
Registration	one `REGISTER_I2C_SENSOR` line per address and bus, in the header	`REGISTER_I2C_SENSOR(0x2A, MAIN, …)`
Vendor library	installed in `~/Arduino/libraries/`	`SparkFun_Qwiic_Scale_NAU7802…`

Template: header

src/sensing/I2C/MYSENS.h

#pragma once
#include "I2C_Manager.h"
#include "MySensorLib.h"¹                   // the chip's Arduino library

bool MYSENS²__INIT_(I2C_Sens_Ctx* sens);
bool MYSENS²__READ_(I2C_Sens_Ctx* sens);

//                 address     bus        init function     read function     info1, info2 (optional)
REGISTER_I2C_SENSOR(0x48³,  MAIN⁴,  MYSENS__INIT_,  MYSENS__READ_,  1.0, 0.0⁵);
REGISTER_I2C_SENSOR(0x48,  AUX1,  MYSENS__INIT_,  MYSENS__READ_,  1.0, 0.0);    // same chip on the second bus

1	header of the vendor library for the chip
2	device name `<DEV>`; use the same name for the file, both functions and in the .cpp
3	the chip's 7-bit I²C address; add one line per address the chip can take
4	`MAIN` (SDA 21 / SCL 22) or `AUX1` (SDA 19 / SCL 18); add a line per bus you wire it to
5	values of `info1`, `info2` handed to the driver for this bus and address (e.g. calibration constants)

Template: source

src/sensing/I2C/MYSENS.cpp

#include "MYSENS¹.h"

static constexpr uint8_t N_CH = 2²;                  // channels this device publishes

bool MYSENS__INIT_(I2C_Sens_Ctx* sens) {
    if (!sens) return false;
    TwoWire& wire = (sens->line == MAIN) ? Wire : Wire1;  // bus chosen in the device list

    MySensor* dev = new MySensor();³                   // one driver object per instance
    for (int i = 0; i <= _MAX_SENSOR_INIT_RETRIES; i++) {
        if (dev->begin(sens->addr, wire)⁴) {
            dev->setRate(100);⁵                         // optional chip configuration
            deviceList[sens->idx].data_per_device = N_CH;  // declare the channel count
            sensorIndices += N_CH;                         // reserve the slots in the message
            sens->sens_instance = (void*)dev;              // keep the object for READ_
            return true;
        }
        delay(5);
    }
    return false;                                          // device reported as not ready
}

bool MYSENS__READ_(I2C_Sens_Ctx* sens) {
    if (!sens || !sens->sens_instance) { sensorIndices += N_CH; return false; }
    MySensor* dev = (MySensor*)sens->sens_instance;

    if (!dev->dataReady()⁶) {                           // no new sample this cycle:
        sensorIndices += N_CH;                             // skip the slots, keep the previous values
        return false;
    }
    msg.intensities.data[sensorIndices++] = dev->readX()⁷ * sens->info1 + sens->info2;
    msg.intensities.data[sensorIndices++] = dev->readY()⁷;
    return true;                                           // exactly N_CH values, in a fixed order
}

1	include your own header
2	number of values the device publishes every cycle, the length of its slot range
3	the vendor driver class (or your own register-level code)
4	the call that starts the chip at `sens->addr` on `wire`; must return true on success
5	any one-time chip set-up (range, rate, gain); delete if not needed
6	the call that reports a new sample; if the chip always has one, use `true`
7	the reads; scale with `info1`/`info2` where calibration applies; one line per channel

Use it

Copy both files into ~/Arduino/libraries/ESA/src/sensing/I2C/ and install the vendor library.
Add a row to the sketch: { I2C, 0x48, "MYS", MAIN },.
Add N_CH names to the hub in sensors.yaml, at the position of the new row.
Compile and upload; ros2 topic echo <hub topic> --once shows the new range_max.

DRIVER RULES

Include the header only from its own .cpp; including it elsewhere registers the driver twice.
A bus–address pair belongs to one driver. The registry has a fixed capacity, MAX_I2C_SENS in I2C_Manager.h (8 in v1.0, all taken by the shipped drivers): delete registration lines for addresses you do not wire, or raise the constant.
READ_ runs every cycle: no delay(), no blocking waits, no Serial output.
Always advance sensorIndices by N_CH, even when you write nothing, so that every later channel stays in its slot.

Actuator driver

A new actuator is one header and one source file in src/actuation/. The header registers the driver under an identifier, added once to the actuator list in actuation.h; the sketch selects the driver by that identifier in setupActuation().

Item	Convention	Example (shipped)
Location	`ESA/src/actuation/`	n/a
Files	`ACT_<DEV>.h` and `ACT_<DEV>.cpp`	`ACT_FESTO_VEAB_L26.h/.cpp`
Functions	`<DEV>__INIT_` and `<DEV>__ACTUATE_`	`FESTO_VEAB_L26__INIT_`, `FESTO_VEAB_L26__ACTUATE_`
Identifier	`ACT_<DEV>`: added to the `ActuatorType` list in `actuation.h`, used in `setupActuation()`	`ACT_FESTO_VEAB_L26`

Template: identifier

src/actuation/actuation.h (excerpt)

// UPDATE BELOW ENUM FOR NEW ACTUATOR
typedef enum {
    NO_ACT = 0,
    ACT_FESTO_VEAB_L26,
    ACT_SG90_SERVO,
    LED_TEST,
    ACT_MYACT¹,                     // new identifier, always before ACTUATOR_COUNT
    ACTUATOR_COUNT
} ActuatorType;

1	identifier of the new actuator; the same word is used in its registration line and in the sketch

Template: header

src/actuation/ACT_MYACT.h

#pragma once
#include "actuation.h"

void MYACT¹__INIT_(ActInfo* sub);
void MYACT¹__ACTUATE_(ActInfo* sub);

REGISTER_ACTUATOR(ACT_MYACT², MYACT__INIT_, MYACT__ACTUATE_)

1	driver name `<DEV>`, used for both functions
2	the identifier added to `actuation.h`; use it in `setupActuation()`

Template: source

src/actuation/ACT_MYACT.cpp

#include "ACT_MYACT¹.h"

void MYACT__INIT_(ActInfo* sub) {                 // once, during bootSystem()
    if (!sub) return;
    pinMode(sub->out_gpio_i2caddr, OUTPUT);²       // 'out' argument of setupActuation()
    if (sub->extra_info1 > 0) pinMode(sub->extra_info1, OUTPUT);³   // 'x1', optional
}

void MYACT__ACTUATE_(ActInfo* sub) {              // every loop, from actuate()
    if (!sub) return;
    float cmd = sub->val;                             // latest command (0 before the first message)
    int duty = constrain((int)(cmd * 2.55f), 0, 255);⁴   // convert the physical set-point
    analogWrite(sub->out_gpio_i2caddr, duty);⁵          // drive the hardware
}

1	include your own header
2	one-time set-up of the output (pin mode, I²C driver object, PWM frequency …)
3	use `extra_info1` / `extra_info2` (the x1, x2 arguments) for any extra pins or settings; omitted arguments arrive as 0
4	the conversion from the commanded physical quantity (kPa, degrees, %) to the hardware unit
5	the hardware call (`analogWrite`, `digitalWrite`, an I²C driver call …)

ActInfo field	Type	Comes from
`val`	float	latest `Float32` on the command topic
`out_gpio_i2caddr`	int8	`out` argument of `setupActuation()`
`extra_info1`, `extra_info2`	int8	`x1`, `x2` arguments (−128…127; 0 if omitted)
`topicName`	const char*	command topic

Optional: smooth an analog feedback signal

A driver can filter a feedback input with the library's software filters, as the pneumatic valve driver does with its pressure sensor. Register the pin in INIT_, read it in ACTUATE_:

feedback filter

// in MYACT__INIT_ :
act_fb_filter_config[_act_fb_idx].filter_type = 1¹;      // 1 = moving average (filter number = extra_info1 - 8)
act_fb_filter_config[_act_fb_idx].param1      = 10²;     // window
act_fb_filter_config[_act_fb_idx].gpio        = sub->extra_info2;
_act_fb_idx++;

// in MYACT__ACTUATE_ :
float fb_volts = _act_fb_filt.analog_filter(sub->extra_info2);

1	filter: 1 moving average, 2 median, 3 exponential, 4–7 second-order Butterworth LPF/HPF/BPF/notch
2	first filter parameter (window, α or cut-off Hz); `param2` holds Q for Butterworth

Use it

Add ACT_MYACT, to the ActuatorType list in ~/Arduino/libraries/ESA/src/actuation/actuation.h, before ACTUATOR_COUNT.
Copy both files into ~/Arduino/libraries/ESA/src/actuation/.
In the actuation sketch: exo.setupActuation("/my/cmd", ACT_MYACT, out, x1, x2);
Set the same topic as output_topic of a planner in mplanner.yaml.
Compile and upload; test with ros2 topic pub --once /my/cmd std_msgs/msg/Float32 "{data: 50.0}".

DRIVER RULES

Include the header only from its own .cpp.
ACTUATE_ runs every loop for every actuator: keep it short and non-blocking.
Variables declared at file scope are shared by all instances of the driver. If a board may run several, keep per-instance state in a small table keyed by sub->out_gpio_i2caddr.
Choose the zero command to be a safe state of the hardware: the driver receives 0 until the first message arrives.