Skip to content

Commit

Permalink
Merge pull request #87 from hippo5329/pr-mag
Browse files Browse the repository at this point in the history
Pr mag
  • Loading branch information
grassjelly authored Aug 9, 2024
2 parents d663bb0 + 00b586d commit 2fe0d17
Show file tree
Hide file tree
Showing 26 changed files with 2,009 additions and 4,343 deletions.
41 changes: 41 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,15 @@ Supported IMUs:
- **MPU6050**
- **MPU9150**
- **MPU9250**
- **QMI8658**

Supported MAGs:

- **HMC5883L**
- **AK8963**
- **AK8975**
- **AK09918**
- **QMC5883L**

### 4. Connection Diagram
Below are connection diagrams you can follow for each supported motor driver and IMU. For simplicity, only one motor connection is provided but the same diagram can be used to connect the rest of the motors. You are free to decide which microcontroller pin to use just ensure that the following are met:
Expand Down Expand Up @@ -163,6 +172,38 @@ Constants' Meaning:

- **USE_MPU9250_IMU** - MPU9250 IMUs.

- **USE_QMI8658_IMU** - QMI8658 IMUs.

- **USE_HMC5883L_MAG** - HMC5883L MAGs.

- **USE_AK8963_MAG** - AK8963 MAGs.

- **USE_AK8975_MAG** - AK8975 MAGs.

- **USE_AK09918_MAG** - AK09918 MAGs.

- **USE_QMC5883L_MAG** - QMC5883L MAGs.

- **MAG_BIAS** - Magnetometer calibration, eg { -352, -382, -10 }.

If you enable magnetometer device, you will need to enable Magdwick filter by adding "madgwick:=true" in robot bringup. The topic from imu will be named as imu/data_raw. The Madgwick filter will fuse imu/data_raw and imu/mag to imu/data.

ros2 launch linorobot2_bringup bringup.launch.py madgwick:=true

Magnetometer calibration should be taken on board with all hardware installed, inlcuding all connectors, battery and motors. The calibration package will rotate the robot slowly for 60 sec. And compute the hard iron bias. More info [here](https://github.com/mikeferguson/robot_calibration#the-magnetometer_calibration-node).

sudo apt-get install ros-humble-robot-calibration -y
rm -rf /tmp/magnetometer_calibration.bag
ros2 run robot_calibration magnetometer_calibration
...
mag_bias_x: -7.94713e-06
mag_bias_y: 3.49388e-05
mag_bias_z: -5.40286e-05

Set the MAG_BIAS to these values in robot configuration file.

#define MAG_BIAS { -7.94713e-06, 3.49388e-05, -5.40286e-05 }

Next, fill in the robot settings accordingly:

#define K_P 0.6
Expand Down
11 changes: 10 additions & 1 deletion config/custom/esp32_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@

//uncomment the IMU you're using
// #define USE_GY85_IMU
#define USE_MPU6050_IMU
// #define USE_MPU6050_IMU
// #define USE_MPU9150_IMU
// #define USE_MPU9250_IMU
// #define USE_QMI8658_IMU
Expand All @@ -40,6 +40,15 @@
// #define USE_AK09918_MAG
// #define USE_QMC5883L_MAG
// #define MAG_BIAS { 0, 0, 0 }
// #define IMU_TWEAK {}
// #define MAG_TWEAK {}

#define ACCEL_COV { 0.01, 0.01, 0.01 }
#define GYRO_COV { 0.001, 0.001, 0.001 }
#define ORI_COV { 0.01, 0.01, 0.01 }
#define MAG_COV { 1e-12, 1e-12, 1e-12 }
#define POSE_COV { 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 }
#define TWIST_COV { 0.001, 0.001, 0.001, 0.003, 0.003, 0.003 }

#define K_P 0.6 // P constant
#define K_I 0.8 // I constant
Expand Down
11 changes: 10 additions & 1 deletion config/custom/esp32s2_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@

//uncomment the IMU you're using
// #define USE_GY85_IMU
#define USE_MPU6050_IMU
// #define USE_MPU6050_IMU
// #define USE_MPU9150_IMU
// #define USE_MPU9250_IMU
// #define USE_QMI8658_IMU
Expand All @@ -40,6 +40,15 @@
// #define USE_AK09918_MAG
// #define USE_QMC5883L_MAG
// #define MAG_BIAS { 0, 0, 0 }
// #define IMU_TWEAK {}
// #define MAG_TWEAK {}

#define ACCEL_COV { 0.01, 0.01, 0.01 }
#define GYRO_COV { 0.001, 0.001, 0.001 }
#define ORI_COV { 0.01, 0.01, 0.01 }
#define MAG_COV { 1e-12, 1e-12, 1e-12 }
#define POSE_COV { 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 }
#define TWIST_COV { 0.001, 0.001, 0.001, 0.003, 0.003, 0.003 }

#define K_P 0.6 // P constant
#define K_I 0.8 // I constant
Expand Down
11 changes: 10 additions & 1 deletion config/custom/esp32s3_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@

//uncomment the IMU you're using
// #define USE_GY85_IMU
#define USE_MPU6050_IMU
// #define USE_MPU6050_IMU
// #define USE_MPU9150_IMU
// #define USE_MPU9250_IMU
// #define USE_QMI8658_IMU
Expand All @@ -40,6 +40,15 @@
// #define USE_AK09918_MAG
// #define USE_QMC5883L_MAG
// #define MAG_BIAS { 0, 0, 0 }
// #define IMU_TWEAK {}
// #define MAG_TWEAK {}

#define ACCEL_COV { 0.01, 0.01, 0.01 }
#define GYRO_COV { 0.001, 0.001, 0.001 }
#define ORI_COV { 0.01, 0.01, 0.01 }
#define MAG_COV { 1e-12, 1e-12, 1e-12 }
#define POSE_COV { 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 }
#define TWIST_COV { 0.001, 0.001, 0.001, 0.003, 0.003, 0.003 }

#define K_P 0.6 // P constant
#define K_I 0.8 // I constant
Expand Down
11 changes: 10 additions & 1 deletion config/custom/pico_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@

//uncomment the IMU you're using
// #define USE_GY85_IMU
#define USE_MPU6050_IMU
// #define USE_MPU6050_IMU
// #define USE_MPU9150_IMU
// #define USE_MPU9250_IMU
// #define USE_QMI8658_IMU
Expand All @@ -40,6 +40,15 @@
// #define USE_AK09918_MAG
// #define USE_QMC5883L_MAG
// #define MAG_BIAS { 0, 0, 0 }
// #define IMU_TWEAK {}
// #define MAG_TWEAK {}

#define ACCEL_COV { 0.01, 0.01, 0.01 }
#define GYRO_COV { 0.001, 0.001, 0.001 }
#define ORI_COV { 0.01, 0.01, 0.01 }
#define MAG_COV { 1e-12, 1e-12, 1e-12 }
#define POSE_COV { 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 }
#define TWIST_COV { 0.001, 0.001, 0.001, 0.003, 0.003, 0.003 }

#define K_P 0.6 // P constant
#define K_I 0.8 // I constant
Expand Down
7 changes: 7 additions & 0 deletions config/lino_base_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,13 @@
// #define USE_MPU6050_IMU
// #define USE_MPU9150_IMU
// #define USE_MPU9250_IMU
// #define USE_QMI8658_IMU
// #define USE_HMC5883L_MAG
// #define USE_AK8963_MAG
// #define USE_AK8975_MAG
// #define USE_AK09918_MAG
// #define USE_QMC5883L_MAG
// #define MAG_BIAS { 0, 0, 0 }

#define K_P 0.6 // P constant
#define K_I 0.8 // I constant
Expand Down
59 changes: 59 additions & 0 deletions firmware/lib/imu/AK09918.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
/*
AK09918.cpp
A library for Grove - IMU 9DOF(ICM20600 + AK09918)
Copyright (c) 2018 seeed technology inc.
Website : www.seeed.cc
Author : Jerry Yip
Create Time: 2018-06
Version : 0.1
Change Log :
The MIT License (MIT)
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/


#include "AK09918.h"

AK09918::AK09918() {
_addr = AK09918_I2C_ADDR;
}

AK09918_err_type_t AK09918::initialize(AK09918_mode_type_t mode) {
if (!I2Cdev::writeByte(_addr, AK09918_CNTL2, mode)) {
return AK09918_ERR_WRITE_FAILED;
}
return AK09918_ERR_OK;
}

bool AK09918::testConnection() {
if (I2Cdev::readByte(_addr, AK09918_WIA1, _buffer) == 1) {
return (_buffer[0] == 0x48);
}
return false;
}

void AK09918::getHeading(int16_t* x, int16_t* y, int16_t* z) {
I2Cdev::readBytes(_addr, AK09918_HXL, 8, _buffer);
*x = (int16_t)(_buffer[1] << 8 | _buffer[0]);
*y = (int16_t)(_buffer[3] << 8 | _buffer[2]);
*z = (int16_t)(_buffer[5] << 8 | _buffer[4]);
}
115 changes: 115 additions & 0 deletions firmware/lib/imu/AK09918.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,115 @@
/*
AK09918.h
A library for Grove - IMU 9DOF(ICM20600 + AK09918)
Copyright (c) 2018 seeed technology inc.
Website : www.seeed.cc
Author : Jerry Yip
Create Time: 2018-06
Version : 0.1
Change Log :
The MIT License (MIT)
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/




#ifndef __IMU_9DOF_AK09918_H__
#define __IMU_9DOF_AK09918_H__

#include "I2Cdev.h"

/***************************************************************
AK09918 I2C Register
***************************************************************/
#define AK09918_I2C_ADDR 0x0c // I2C address (Can't be changed)
#define AK09918_WIA1 0x00 // Company ID
#define AK09918_WIA2 0x01 // Device ID
#define AK09918_RSV1 0x02 // Reserved 1
#define AK09918_RSV2 0x03 // Reserved 2
#define AK09918_ST1 0x10 // DataStatus 1
#define AK09918_HXL 0x11 // X-axis data
#define AK09918_HXH 0x12
#define AK09918_HYL 0x13 // Y-axis data
#define AK09918_HYH 0x14
#define AK09918_HZL 0x15 // Z-axis data
#define AK09918_HZH 0x16
#define AK09918_TMPS 0x17 // Dummy
#define AK09918_ST2 0x18 // Datastatus 2
#define AK09918_CNTL1 0x30 // Dummy
#define AK09918_CNTL2 0x31 // Control settings
#define AK09918_CNTL3 0x32 // Control settings

#define AK09918_SRST_BIT 0x01 // Soft Reset
#define AK09918_HOFL_BIT 0x08 // Sensor Over Flow
#define AK09918_DOR_BIT 0x02 // Data Over Run
#define AK09918_DRDY_BIT 0x01 // Data Ready

// #define AK09918_MEASURE_PERIOD 9 // Must not be changed
// AK09918 has following seven operation modes:
// (1) Power-down mode: AK09918 doesn't measure
// (2) Single measurement mode: measure when you call any getData() function
// (3) Continuous measurement mode 1: 10Hz, measure 10 times per second,
// (4) Continuous measurement mode 2: 20Hz, measure 20 times per second,
// (5) Continuous measurement mode 3: 50Hz, measure 50 times per second,
// (6) Continuous measurement mode 4: 100Hz, measure 100 times per second,
// (7) Self-test mode
enum AK09918_mode_type_t {
AK09918_POWER_DOWN = 0x00,
AK09918_NORMAL = 0x01,
AK09918_CONTINUOUS_10HZ = 0x02,
AK09918_CONTINUOUS_20HZ = 0x04,
AK09918_CONTINUOUS_50HZ = 0x06,
AK09918_CONTINUOUS_100HZ = 0x08,
AK09918_SELF_TEST = 0x10, // ignored by switchMode() and initialize(), call selfTest() to use this mode
};

enum AK09918_err_type_t {
AK09918_ERR_OK = 0, // ok
AK09918_ERR_DOR = 1, // data skipped
AK09918_ERR_NOT_RDY = 2, // not ready
AK09918_ERR_TIMEOUT = 3, // read/write timeout
AK09918_ERR_SELFTEST_FAILED = 4, // self test failed
AK09918_ERR_OVERFLOW = 5, // sensor overflow, means |x|+|y|+|z| >= 4912uT
AK09918_ERR_WRITE_FAILED = 6, // fail to write
AK09918_ERR_READ_FAILED = 7, // fail to read

};

class AK09918 {
public:
AK09918();

// default to AK09918_CONTINUOUS_100HZ mode
AK09918_err_type_t initialize(AK09918_mode_type_t mode = AK09918_CONTINUOUS_100HZ);
bool testConnection();
// getHeading in I2Cdevlib
void getHeading(int16_t *x, int16_t *y, int16_t *z);

private:
uint8_t _addr;
uint8_t _buffer[16];

};


#endif // __IMU_9DOF_AK09918_H__
Loading

0 comments on commit 2fe0d17

Please sign in to comment.