Магнитометрический выход для датчика LSM303DLHC на шине i2c является ошибочным. Проблема написания гобота пилота
Я использую датчик Adafruit LSM303DLHC. Он состоит из 2-х датчиков, акселерометра и магнитометра. В настоящее время я пишу драйвер для этого датчика для работы с пакетом gobot.io через интерфейс i2c на Raspberry Pi 2.
Проблема: акселерометр работает. Часть датчика магнитометра нет. Я могу читать регистры магнитного поля, но получаю бессмысленные значения. Эти значения обновляются между каждым циклом чтения, но они не сильно меняются и не имеют смысла.
Используемые устройства:
- Датчик LSM303DLHC - Лист данных
- Raspberry Pi 2
- Arduino Uno
Подробная информация о чтении выходного сигнала магнитометра:
LSM303DLHC выдает 6 байтов, представляющих 3 значения магнитного поля вдоль 3 оси. Каждое значение состоит из 2 байтов (16 бит) для каждой оси. Порядок вывода следующий:
- Х старший байт
- X младший байт
- Старший байт Z
- Z младший байт
- Старший байт Y
- Y младший байт
Для настройки датчика мы записываем следующие регистры в таком порядке:
- Сброс усиления магнитометра: запишите 0x00 в регистр CRB_REG_M (0x01)
- Установите усиление магнитометра: запишите 0x60 (+/- 2,5 гаусса) в регистр CRB_REG_M (0x01)
- Установите выходную скорость передачи данных: запишите 0x05 (30 Гц) в регистр CRA_REG_M (0x00)
- Включить непрерывный режим: запись 0x00 в регистр MR_REG_M
После настройки датчика мы можем прочитать вывод. Чтобы его прочитать, мы записываем в 1-й из 6-ти выходных регистров. Затем мы читаем вывод 6 регистров за один цикл, помещая 6 байтов в буфер.
Тест уже сделан
- Использование библиотеки Adafruit и платформы Arduino: ОК - вывод нормальный
- Пример использования Raspberry Pi 2 + python: ОК - вывод нормальный
- Использование Raspberry Pi 2 + gobot.io + lsm303DLHC драйвер: ошибка
- Использование io.ReadFull() вместо io.Read(), как предлагается в комментарии: Ошибка
Первые 2 теста (1 и 2) говорят мне, что датчик работает. Это не сломано. Скорость i2c здесь не виновата, потому что программа python (2) тоже работает.
Я подозреваю, что что-то не так в моем коде, когда речь идет о правильном формировании значений int16 из байтов.Моя часть кода драйвера, чтобы прочитать выходные данные датчика и сформировать полученные значения
Этот код находится в ~/go/src/gobot.io/x/gobot/drivers/i2c/lsm303DLHC.go (он же драйвер)
func (d *LSM303Driver) MagneticField() (x, z, y float32, err error) {
// Write to the first output register to start the reading procedure
if _, err = d.Magnetometer.connection.Write([]byte{lsm303RegisterMagOutXLSB}); err != nil {
return 0, 0, 0, err
}
// create a buffer to put the output bytes
measurements := make([]byte, 6)
// read the 6 output bytes
if _, err = d.Magnetometer.connection.Read(measurements); err != nil {
return 0, 0, 0, err
}
var rawXh uint8
var rawXl uint8
var rawZh uint8
var rawZl uint8
var rawYh uint8
var rawYl uint8
buf := bytes.NewBuffer(measurements)
binary.Read(buf, binary.BigEndian, &rawXh)
binary.Read(buf, binary.BigEndian, &rawXl)
binary.Read(buf, binary.BigEndian, &rawZh)
binary.Read(buf, binary.BigEndian, &rawZl)
binary.Read(buf, binary.BigEndian, &rawYh)
binary.Read(buf, binary.BigEndian, &rawYl)
rawX := int16((uint16(rawXh) << 8) | uint16(rawXl))
rawZ := int16((uint16(rawZh) << 8) | uint16(rawZl))
rawY := int16((uint16(rawYh) << 8) | uint16(rawYl))
// Gain is set to +/- 2.5 LSB/Gauss (Least Significant Byte)
// Datasheet page 38
// Unit convertion: gaussToMicroTesla = 100
gainXY, gainZ := d.getGainXYZ()
x = float32(rawX) / float32(gainXY) * float32(gaussToMicroTesla)
z = float32(rawZ) / float32(gainZ) * float32(gaussToMicroTesla)
y = float32(rawY) / float32(gainXY) * float32(gaussToMicroTesla)
fmt.Printf("DEBUG rawX %016b ---> %v \t\t|\t X %v\n", rawX, rawX, x)
fmt.Printf("DEBUG rawZ %016b ---> %v \t\t|\t Z %v\n", rawZ, rawZ, z)
fmt.Printf("DEBUG rawY %016b ---> %v \t\t|\t Y %v\n\n", rawY, rawY, y)
return x, z, y, nil
}
Вот вывод моей маленькой программы, которая использует эту функцию:
...
DEBUG rawX 0000001100101011 ---> 811 | X 121.04478
DEBUG rawZ 0000001011110111 ---> 759 | Z 126.5
DEBUG rawY 0000001100110000 ---> 816 | Y 121.79104
DEBUG rawX 0000001100101011 ---> 811 | X 121.04478
DEBUG rawZ 0000001011110111 ---> 759 | Z 126.5
DEBUG rawY 0000001100110000 ---> 816 | Y 121.79104
DEBUG rawX 0000001100100111 ---> 807 | X 120.44777
DEBUG rawZ 0000001011110110 ---> 758 | Z 126.33333
DEBUG rawY 0000001100101100 ---> 812 | Y 121.19403
...
В каждой строке вы можете увидеть двоичное и обычное представление rawX(YZ), а затем окончательное значение в микротесле. Во всех случаях эти значения далеко. Они не сильно меняются, даже когда я поворачиваю устройство во всех направлениях.
Я внимательно посмотрел на библиотеку Adafruits C++ для Arduino и не вижу каких-либо существенных отличий. Вот код Adafruit для считывания выходных данных магнитометра:
void Adafruit_LSM303_Mag_Unified::read()
{
// Read the magnetometer
Wire.beginTransmission((byte)LSM303_ADDRESS_MAG);
Wire.send(LSM303_REGISTER_MAG_OUT_X_H_M);
Wire.endTransmission();
Wire.requestFrom((byte)LSM303_ADDRESS_MAG, (byte)6);
// Wait around until enough data is available
while (Wire.available() < 6);
// Note high before low (different than accel)
uint8_t xhi = Wire.receive();
uint8_t xlo = Wire.receive();
uint8_t zhi = Wire.receive();
uint8_t zlo = Wire.receive();
uint8_t yhi = Wire.receive();
uint8_t ylo = Wire.receive();
// Shift values to create properly formed integer (low byte first)
raw.x = (int16_t)(xlo | ((int16_t)xhi << 8));
raw.y = (int16_t)(ylo | ((int16_t)yhi << 8));
raw.z = (int16_t)(zlo | ((int16_t)zhi << 8));
}
Я что-то упускаю огромное? (Я надеюсь, что это так...)
Честно говоря, я трачу сумасшедшее количество времени на эту проблему, и меня нигде нет. Я узнал много интересного о ядре linux и протоколе i2c, ioctl и многом другом... но я все еще не могу заставить магнитометр работать в golang с gobot.io, даже если акселерометр работает...
Заранее благодарю тех, кто потратит время, чтобы прочитать меня.