Pixhawk I2C costum Sensor did not work :(

Hello together,

I designed a sensorboard with 3 HC-SR04 ultrasonic sensors connected to an AVR Atmega88. This device is connected via I2C to the Pixhawk.

The Sensorboard works fine. I tested it with another AVR. To read out the values I use this part of the code:

if(!(i2c_start(SLAVE_ADRESSE+I2C_WRITE))) //Slave bereit zum lesen?
i2c_write(0x00); //Buffer Startadresse zum Auslesen
i2c_rep_start(SLAVE_ADRESSE+I2C_READ); //Lesen beginnen

	  i2cdata[0]= i2c_readAck(); // Bytes lesen...
	  i2cdata[1]= i2c_readAck();
	  i2cdata[2]= i2c_readAck();
	  i2cdata[3]= i2c_readAck();
	  i2cdata[4]= i2c_readAck();
	  i2cdata[5]= i2c_readNak(); // letztes Byte lesen, darum kein ACK
	  i2c_stop();           // Zugriff beenden

In the ardupilot folder I created a new lib under libraries called: AP_DistanceUltrasonic

Here I implemented my class with an header file and an cpp… by the way it works without any errors. The scheduler calls my functions in the right way.

Here is the code of my cpp file to update the Sensordata:

#include <AP_HAL.h>
#include “DistanceUltrasonic.h”

extern const AP_HAL::HAL& hal;

distanceData.left = 0;
distanceData.center = 0;
distanceData.right = 0;
distanceData.healthy = 0;

// read - return last value measured by sensor
bool Distance_Ultrasonic::get_distance(uint16_t &reading_left_cm, uint16_t &reading_center_cm, uint16_t &reading_right_cm)
uint8_t buff[6];

// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();

// exit immediately if we can't take the semaphore
if (i2c_sem == NULL || !i2c_sem->take(1)) {
    return false;

// // write the adress of the reading register
// if (hal.i2c->writeRegister(DISTANCE_ULTRASONIC_ADDR,0x00,0x00) != 0) {
// i2c_sem->give();
// return false;
// }

// take range reading and read back results
if (hal.i2c->read(DISTANCE_ULTRASONIC_ADDR, 6, buff) != 0) {
    return false;

// combine results into distance
reading_left_cm     = ((uint16_t)buff[0]) << 8 | buff[1];
reading_center_cm   = ((uint16_t)buff[2]) << 8 | buff[3];
reading_right_cm    = ((uint16_t)buff[4]) << 8 | buff[5];
return true;


void Distance_Ultrasonic::update(void)
distanceData.healthy = get_distance(distanceData.left, distanceData.center, distanceData.right);

I tryed it out but the result is always 0x00 for eatch byte!!!

May anybody help me?



can anybody help? or give me a good tutorial to implement own sensors at the I2C???


I’m also having problems with connecting new sensor via i2c to Pixhawk.

Thumb up for a good explanation of how exactly the AP_HAL_PX4 i2c library works. At the moment there no even I2CDriver files in that folder, so does it use the AP_HAL_Empty library?
Can somebody explain for example which code is used for APM Firmware of digital Airspeed sensor (MS4525D0).
this one:
github.com/ilia-sheremet/ardupi … ed_I2C.cpp
or this?
github.com/diydrones/PX4Firmwar … rspeed.cpp

Thank you in advance!