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;
Distance_Ultrasonic::Distance_Ultrasonic()
{
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) {
i2c_sem->give();
return false;
}
i2c_sem->give();
// 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?
Regads
Tobias