Custom I2C driver

1

Hi I’m currently reading some sensor values from Arduino and sending them to Pixhawk. I have created a driver code similar to that of sf1xx i2c driver . I have modified the required parameters so as to get 6 set of float values from arduino. To send data i have used the UORB topic debug_array as mentioned in https://discuss.px4.io/t/custom-sensor-integration-and-monitoring/17951. I’m able to send and receive data from arduino without any problem if i don’t connect the Holybro digital airspeed sensor. But when both of them are connected, data is not getting received properly from both the sensors either it stops sending/receiving data or incorrect data is being received.

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
#include <drivers/device/i2c.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/debug_array.h>
#include <drivers/rangefinder/PX4Rangefinder.hpp>

/* Configuration Constants */
#define ARDUINOI2C_BASEADDR 0x08

class ARDUINOI2C : public device::I2C, public I2CSPIDriver
{
public:
ARDUINOI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
int address = ARDUINOI2C_BASEADDR);

~ARDUINOI2C() override;

static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();

int init() override;

/**

  • Diagnostics - print some basic information about the driver.
    */
    void print_status() override;

/**

  • Perform a poll cycle; collect from the previous measurement
  • and start a new one.
    */
    void RunImpl();

private:
int probe() override;

/**

  • Test whether the device supported by the driver is present at a
  • specific address.
  • @param address The I2C bus address to probe.
  • @return True if the device is present.
    */
    int probe_address(uint8_t address);

/**

  • Initialise the automatic measurement state machine and start it.
  • @note This function is called at open and error time. It might make sense
  •   to make it more aggressive about resetting the bus in case of errors.

*/
void start();

int measure();
int collect();
//PX4Rangefinder _px4_rangefinder;

bool _sensor_ok{false};

int _conversion_interval{-1};
int _measure_interval{0};

perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
};

ARDUINOI2C::ARDUINOI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) :
I2C(DRV_DEVTYPE_UNUSED, MODULE_NAME, bus, address, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus)
{
}

ARDUINOI2C::~ARDUINOI2C()
{
/* free perf counters */
perf_free(_sample_perf);
perf_free(_comms_errors);
}

int ARDUINOI2C::init()
{
int ret = PX4_ERROR;
int32_t hw_model = 0;
param_get(param_find(“SENS_EN_ARDI2C”), &hw_model);

switch (hw_model) {
case 0:
PX4_WARN(“disabled.”);
return ret;

case 1: /* SF10/a (25m 32Hz) */
//_px4_rangefinder.set_min_distance(0.01f);
//_px4_rangefinder.set_max_distance(25.0f);
_conversion_interval = 1000000;//31250;
break;

default:
PX4_ERR(“invalid HW model %d.”, hw_model);
return ret;
}

/* do I2C init (and probe) first */
if (I2C::init() != OK) {
return ret;
}

// Select altitude register
int ret2 = measure();

if (ret2 == 0) {
ret = OK;
_sensor_ok = true;
}

return ret;
}

int ARDUINOI2C::probe()
{
return measure();
}

int ARDUINOI2C::measure()
{
/*

  • Send the command ‘0’ – read altitude
    */
    //uint8_t cmd = 0;
    int ret = OK;//transfer(&cmd, 1, nullptr, 0);

if (OK != ret) {
perf_count(_comms_errors);
PX4_DEBUG(“i2c::transfer returned %d”, ret);
return ret;
}

ret = OK;

return ret;
}

int ARDUINOI2C::collect()
{

/* read from the sensor */
perf_begin(_sample_perf);
union u_tag{
float fval;
uint8_t val[4] {};
}u[6];
const hrt_abstime timestamp_sample = hrt_absolute_time();

  if (transfer(nullptr, 0, &u[0].val[0], 24) < 0) {
  	perf_count(_comms_errors);
  	perf_end(_sample_perf);
  	return PX4_ERROR;
  }

perf_end(_sample_perf);

//uint16_t distance_cm = val[0] << 8 | val[1];
//float distance_m = float(distance_cm) * 1e-2f;

/* advertise debug array */
struct debug_array_s dbg_array;
dbg_array.id = 1;
strncpy(dbg_array.name, “dbg_array”, 10);

orb_advert_t pub_dbg_array = orb_advertise(ORB_ID(debug_array), &dbg_array);

  /* send one array */

for (size_t i = 0; i < 6; i++) { // for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) {
dbg_array.data[i] = (float)u[i].fval;
}
dbg_array.timestamp = timestamp_sample;
orb_publish(ORB_ID(debug_array), pub_dbg_array, &dbg_array);

return PX4_OK;
}

void ARDUINOI2C::start()
{
if (_measure_interval == 0) {
_measure_interval = _conversion_interval;
}
PX4_INFO(“started”);
/* set register to ‘0’ */
measure();

/* schedule a cycle to start things */
ScheduleDelayed(_conversion_interval);
}

void ARDUINOI2C::RunImpl()
{
/* Collect results /
if (OK != collect()) {
PX4_DEBUG(“collection error”);
/ if error restart the measurement state machine */
start();
return;
}

/* schedule a fresh cycle call when the measurement is done */
ScheduleDelayed(_conversion_interval);
}

void ARDUINOI2C::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
}

void ARDUINOI2C::print_usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(

Description

I2C bus driver for Lightware SFxx series LIDAR rangefinders: SF10/a, SF10/b, SF10/c, SF11/c, SF/LW20.

Setup/usage information: Redirecting to latest version of document (main)
)DESCR_STR");

PRINT_MODULE_USAGE_NAME(“arduinoi2c”, “driver”);
PRINT_MODULE_USAGE_SUBCATEGORY(“power_sensor”);
PRINT_MODULE_USAGE_COMMAND(“start”);
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAM_INT(‘R’, 25, 1, 25, “Sensor rotation - downward facing by default”, true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}

I2CSPIDriverBase ARDUINOI2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
ARDUINOI2C instance = new ARDUINOI2C(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency);

if (instance == nullptr) {
PX4_ERR(“alloc failed”);
return nullptr;
}

if (instance->init() != PX4_OK) {
delete instance;
return nullptr;
}

instance->start();
return instance;
}

extern “C” __EXPORT int arduinoi2c_main(int argc, char *argv)
{
int ch;
using ThisDriver = ARDUINOI2C;
BusCLIArguments cli{true, false};
cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
cli.default_i2c_frequency = 100000;

while ((ch = cli.getopt(argc, argv, “R:”)) != EOF) {
switch (ch) {
case ‘R’:
cli.orientation = atoi(cli.optarg());
break;
}
}

const char *verb = cli.optarg();

if (!verb) {
ThisDriver::print_usage();
return -1;
}

BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DEVTYPE_UNUSED);

if (!strcmp(verb, “start”)) {
return ThisDriver::module_start(cli, iterator);
}

if (!strcmp(verb, “stop”)) {
return ThisDriver::module_stop(iterator);
}

if (!strcmp(verb, “status”)) {
return ThisDriver::module_status(iterator);
}

ThisDriver::print_usage();
return -1;
}

Note: I have not completely modified the driver, minimal changes have been made to the code inorder to receive data via i2c.
The gif shows the airspeed sensor behaving abruptly when the arduino is connected.
airspeed

What might be the cause? @dagar can u please help me on this.

`