amikhalev/uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Make compass io actually work somewhat

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-27 16:13:59 -08:00
parent e8ee6cd107
commit 67a1c98b34
1 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
main/ugv_io_mpu.cc
View File

@ -56,6 +56,8 @@ void MPU::Init() {
}
mpu_->setAccelFullScale(MPU_ACCEL_FS);
mpu_->setGyroFullScale(MPU_GYRO_FS);
// force magnetometer into continuous mode
mpu_->compassWriteByte(0x0A, 0x12);
xSemaphoreGive(i2c_mutex);
BaseType_t xRet =
@ -77,19 +79,31 @@ void MPU::DoTask() {
esp_err_t ret;
while (true) {
vTaskDelay(pdMS_TO_TICKS(50));
uint8_t mxh, mxl, myh, myl, mzh, mzl, n;
xSemaphoreTake(i2c_mutex, portMAX_DELAY);
ret = mpu_->motion(&accel_, &gyro_, &mag_);
ret = mpu_->motion(&accel_, &gyro_);
mpu_->compassReadByte(0x03, &mxl);
mpu_->compassReadByte(0x04, &mxh);
mpu_->compassReadByte(0x05, &myl);
mpu_->compassReadByte(0x06, &myh);
mpu_->compassReadByte(0x07, &mzl);
mpu_->compassReadByte(0x08, &mzh);
mpu_->compassReadByte(0x09, &n);
xSemaphoreGive(i2c_mutex);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "error reading MPU");
continue;
}
int16_t mx = (static_cast<int16_t>(mxh) << 8) | static_cast<int16_t>(mxl);
int16_t my = (static_cast<int16_t>(myh) << 8) | static_cast<int16_t>(myl);
int16_t mz = (static_cast<int16_t>(mzh) << 8) | static_cast<int16_t>(mzl);
ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz);
xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
mpu_data_.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS);
mpu_data_.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
mpu_data_.mag.x = ((float)mag_.x) * MPU_MAG_TO_FLUX;
mpu_data_.mag.y = ((float)mag_.y) * MPU_MAG_TO_FLUX;
mpu_data_.mag.z = ((float)mag_.z) * MPU_MAG_TO_FLUX;
mpu_data_.mag.x = ((float)mx) * MPU_MAG_TO_FLUX;
mpu_data_.mag.y = ((float)my) * MPU_MAG_TO_FLUX;
mpu_data_.mag.z = ((float)mz) * MPU_MAG_TO_FLUX;
xSemaphoreGive(mutex_);
}
vTaskDelete(NULL);