diff --git a/README.md b/README.md index fa05acd..7db157f 100644 --- a/README.md +++ b/README.md @@ -2,25 +2,67 @@ # Reefwing AHRS - The Reefwing flight controller for quadcopters is based around the Arduino Nano 33 BLE board. This board includes the LSM9DS1 chip which we use as an Inertial Measurement Unit (IMU). The IMU determines the current orientation of the drone. +The Reefwing AHRS library provides an Attitude and Heading Reference System (AHRS) class for use with Arduino compatible boards. The library has been tested with the Arduino Nano, Nano 33 BLE, Nano 33 BLE SENSE (REV 1 and REV 2), Nano 33 IoT, MKR Vidor 4000, Portenta H7 and the Seeed XIAO nRF52840 Sense boards. - The Reefwing AHRS library provides an Attitude and Heading Reference System (AHRS) class for use with the Arduino Nano 33 BLE, the Nano 33 BLE SENSE, and Nano 33 BLE SENSE Rev. 2 boards. - - The AHRS will convert the gyroscope rate and accelerometer force data to a roll and pitch angle. The yaw angle is then calculated using the pitch, roll and magnetometer data. +An Attitude and Heading Reference System (AHRS) takes information from the Inertial Measurement Unit (IMU) and processes it to provide reliable roll, pitch and yaw angles. Our library can be downloaded using the Arduino IDE Library Manager, or directly from the [Reefwing GitHub Repository](https://github.com/Reefwing-Software/Reefwing-AHRS). Version 1.1.0 of the library added the NONE option for Sensor Fusion. This option is used if you want Euler angles calculated but no sensor fusion filter applied. This release also fixed a bug in the Classic complementary filter calculation. - Version 2.2.0 added support for the Nano 33 BLE Sense Rev. 2, which uses two IMU sensors to replace the 9-axis IMU used in previous versions. It also incorporates the latest version of ```Fusion v1.0.7``` (commit [b287a9c](https://github.com/xioTechnologies/Fusion/commit/b287a9c1e11070625b4ee625ee6325fbbbadf1e3) which added ENU and NED support). + Version 2.2.0 added support for the Nano 33 BLE Sense Rev. 2. Version 2 of the Nano 33 BLE Sense, replaces the LSM9DS1 9 axis IMU with a combination of two IMUs, the BMI270, a 6 axis gyro & accelerometer and the BMM150, a 3 axis magnetometer. In order to support the new hardware, it makes sense to separate the sensor processing from the sensor fusion algorithms. This library version also added the Kalman Filter Fusion option. + + A complete description of this library is available in our Medium article: [Reefwing AHRS Arduino Library for Drones](https://reefwing.medium.com/reefwing-ahrs-arduino-library-for-drones-part-1-6d6457231764). + +## Reefwing IMU Types + +[Reefwing IMU Types](https://github.com/Reefwing-Software/Reefwing-imuTypes) is a common library header which is used to prevent duplicate definition of similar types, classes and enums. It also ensures that changes will flow through to all of the Reefwing libraries that use it. + +The libraries that use this definition header are: + +- [ReefwingAHRS](https://github.com/Reefwing-Software/Reefwing-AHRS) +- [ReefwingLSM9DS1](https://github.com/Reefwing-Software/Reefwing-LSM9DS1) +- [Reefwing_xIMU3](https://github.com/Reefwing-Software/Reefwing-xIMU3) +- [ReefwingMPU6x00](https://github.com/Reefwing-Software/MPU6x00) +- [ReefwingMPU6050](https://github.com/Reefwing-Software/Reefwing-MPU6050) + +The Structs Defined in the IMU Types Library are: + +- `EulerAngles` +- `InertialMessage` +- `RawData` +- `ScaledData` +- `TempData` +- `SensorData` +- `VectorData` + +The Class Defined in the IMU Types Library is: + +- `Quaternion` + +We have also included definitions of the I²C addresses for the following devices: + +- `LSM9DS1AG_ADDRESS` - Address of the LSM9DS1 accelerometer & gyroscope +- `LSM9DS1M_ADDRESS` - Address of the LSM9DS1 magnetometer +- `HTS221_ADDRESS` - Nano 33 BLE Sense Rev 1 Sensor - temp/humidity +- `HS3003_ADDRESS` - Nano 33 BLE Sense Rev 2 Sensor - temp/humidity +- `LSM6DS3_ADDRESS` - Seeed Studios xiao Sense gyro/accelerometer +- `MPU6000_ADDRESS` - TDK InvenSense MPU6x00 gyro/accelerometer +- `MPU6050_ADDRESS` - TDK InvenSense MPU6050 gyro/accelerometer + +## Library Dependencies + +To use the Reefwing AHRS Library, you need to also install the Reefwing_imuTypes library. Both of these are able to be installed from the Arduino IDE Library Manager. ## Sensor Fusion & Free Parameters Sensor fusion is the process of combining sensory data or data derived from disparate sources such that the resulting information has less uncertainty than would be possible when these sources were used individually. With the gyroscope and accelerometer, we have two angle sensors which should be providing the same data but with different errors. The concept is to combine or fuse the data in such a way as to eliminate the errors and produce an accurate angle that we can use. + In the original release of our AHRS Library, all the sensor fusion algorithms were contained within the IMU class. In version 2.2, these have been moved to the new ReefwingAHRS class. Now data from any IMU can be used as inputs to our sensor fusion algorithms. + The Reefwing AHRS provides the following Sensor Fusion options: - Complementary Filter - Madgwick Filter - - Fusion (Madgwick Filter v2) + - Kalman Filter - Mahony Filter - Classic Complementary Filter: Euler Angles are updated using trigonometry - NONE: Euler angles are calculated but no sensor fusion filter is used. @@ -31,7 +73,6 @@ - Alpha (𝛂) for the **Complementary filter**. Alpha is known as the filter co-efficient, smoothing factor or gain. It determines the cutoff frequency for the high pass filter, which we pass the gyro rate through. `𝛂 = 𝜏 / (𝜏 + ∆t)` where `𝜏 = filter time constant` and `∆t = imu sampling rate`. A typical value for `𝛂` is 0.95 to 0.98 but it depends on the sampling rate and application. A lot more detail is provided in our article [How to Write your own Flight Controller Software - Part 7](). - Kp and Ki for the **Mahony filter**. These are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, and Ki is for integral. The Madgwick and Mahony filters differ with regards to the resolution of sensor biases. Mahony uses a proportional and integral controller to correct the gyroscope bias, whereas Madgwick uses only a proportional controller. The Mahony filter takes into consideration the disparity between the orientation from the gyroscope and the estimation from the magnetometer and accelerometer and weighs them according to its gains. The changes made to the gyroscope are given by: `Kp ∗ em + Ki ∗ ei`, where `em` is the sensor error of the gyroscope, and `ei` is the integral error, which is calculated by: `em = 1 / sampling frequency`. The filter default values are: `Kp = 10.0f` and `Ki = 0.0f`. - - Gain for the **Fusion filter**. Equivalent to beta in the original Madgwick filter. A low gain will decrease the influence of the accelerometer and magnetometer so that the algorithm will better reject disturbances causes by translational motion and temporary magnetic distortions. However, a low gain will also increase the risk of drift due to gyroscope calibration errors. The `default value is 0.5`. - Beta (𝛃) for the **Madgwick filter**. The gain `𝛽` represents all mean zero gyroscope measurement errors, expressed as the magnitude of a quaternion derivative. It is defined using the angular velocity. Beta is based on our estimate of gyroscope measurement error (e.g., `GyroMeasError = 40` degrees per second is used as a default). We calculate beta using the code shown below. ```c++ @@ -42,53 +83,23 @@ For the Madgwick filter there is a tradeoff in the beta parameter between accura Increasing beta (`GyroMeasError`) by about a factor of fifteen, the response time is reduced to ~2 seconds. We haven't noticed any reduction in solution accuracy with this reduced lag. This is essentially the I coefficient in a PID control sense; the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. In any case, beta is the free parameter in the Madgwick filtering and fusion scheme. -The Fusion Filter (Madgwick v2) operates similarly. The default gain is 0.5 but this results in a fair lag until the angle settles, increasing this to 7.5 provides significantly faster response. - -The default free parameters are set in the LSM9DS1 `begin()` method, so custom values need to be assigned after this and before the `update()` method is called in `loop()`. The available `LSM9DS1` class set free parameter methods are: - -```c++ -void setAlpha(float a); // Complementary Filter - value between 0 and 1. -void setBeta(float b); // Madgwick Filter -void setGyroMeasError(float gme); // Madgwick Filter - value in degrees per second -void setKp(float p); // Mahony Filter - proportional term -void setKi(float i); // Mahony Filter - integral term -void setFusionGain(float g); // Fusion Filter - similar to beta -``` - For the Madgwick Filter use either `setBeta()` or `setGyroMeasError()`, **but not both**, since they both effect beta. -## Aircraft Reference Frame - -Any control system needs to use a defined reference frame. Being a drone application, we will use the convention for aircraft. This is based on three body co-ordinates and three attitude angles roll, pitch and yaw. +## Inertial Coordinate System and NED -The Aircraft Reference Frame (ARF) is relative to the drone itself and the origin is at the centre of gravity. Aircraft movement in the positive X, Y and Z directions translate to forward, right and down. In other words the positive X-Axis points towards the front of the drone. -Rotation around the X, Y and Z-Axes are referred to as roll, pitch and yaw. The direction of positive rotation is defined by the co-ordinate right hand rule. +The inertial coordinate system is closely related to the NED (North-East-Down) coordinate system, which is a commonly used local-level coordinate system in navigation and aerospace applications. -Roll is defined as the angle between the Y Axis and the horizontal plane. When rotating the drone around the X Axis with the Y Axis moving downwards, roll is positive and increasing. +The inertial coordinate system, also known as the Earth-Centered Earth-Fixed (ECEF) coordinate system, is a global reference frame with its origin located at the center of the Earth. It is fixed with respect to the Earth and does not rotate with respect to the Earth's rotation. The x-axis typically points towards the intersection of the equator and prime meridian (0 degrees latitude, 0 degrees longitude). The y-axis points eastward along the equator, and the z-axis points along the Earth's rotational axis, typically aligned with the North Pole. -Pitch is defined as the angle between the X Axis and the horizontal plane. When rotating the drone around the Y Axis with the X Axis moving upwards, pitch is positive and increasing. +The NED coordinate system, on the other hand, is a local-level coordinate system that is commonly used for navigation purposes. It is defined with respect to a specific location or observer on the Earth's surface. The origin of the NED coordinate system is typically set at the observer's position, and the NED axes are aligned such that: -Yaw is defined as the angle between the X Axis and magnetic north on the horizontal plane measured in a clockwise direction when viewing from the top of the drone. +- The North axis (N) points towards true North. +- The East axis (E) points towards true East. +- The Down axis (D) points vertically downward, perpendicular to the Earth's surface. -The aircraft reference frame is useful in a piloted flight control context, since we want command inputs to be in reference to the current drone position and attitude. +The NED coordinate system is relative to the observer's position and orientation, while the inertial coordinate system is fixed with respect to the Earth. However, the relationship between the two coordinate systems can be established through appropriate coordinate transformations. -## World Reference Frame - -Another possible reference frame is the world or ground. In this frame, the origin is defined as a location on Earth (e.g., the drone take off longitude, latitude and altitude) and the X, Y and Z-Axes correspond to North, East and Down. You may see this described as NED. - -Heading is similar to yaw and defined as the angle between the X Axis and magnetic north on the horizontal plane measured in a clockwise direction when viewing from the top of the drone. Thus, a heading angle of 0° will point towards the North, and +90° towards the East. - -The World Reference Frame is useful in flight planning or return to home failsafe scenarios where we need to know the location of the drone and where it came from. For any sort of accuracy you will need to complement the IMU with a GPS for this type of mission. Some filter libraries (e.g., the Madgewick fusion algorithm which we are using) also expect co-ordinates in the NED format. - -## Inertial Navigation - -Without a GPS, a drone relies on inertial navigation or dead reckoning to determine its position, velocity and orientation. It does this using an IMU to provide data to the Attitude and Heading Reference System (AHRS), which is part of the flight controller. The AHRS includes some sort of filtering or sensor fusion to deliver the most accurate roll, pitch and yaw data possible. Precision gyroscopes (e.g., ring lasers), are too expensive and bulky for drone applications and so less accurate MEMS (Micro Electrical Mechanical System) devices like the LSM9DS1 are used. - -A gyroscope is a device that can measure the orientation and angular velocity of an object. Gyroscopes are more advanced than accelerometers, in that they can measure the tilt and lateral orientation, whereas an accelerometer can only measure its linear motion. Gyroscope sensors are also called “Angular Rate Sensors” or “Angular Velocity Sensors”. Angular velocity (measured in degrees per second) is the change in the rotational angle of the object per unit of time. - -An accelerometer is an electromechanical device used to measure acceleration forces. Such forces may be static, like the continuous force of gravity or, as with a drone accelerating, dynamic to sense movement or vibrations. Changes in the acceleration force measured in the 3-axis directions can be used to determine the orientation of a drone. Accelerometer sensors are insensitive to rotation about the earth’s gravitational field vector. - -A magnetometer is an instrument used for measuring magnetic forces, and in our context, the earth’s magnetism. The Earth’s magnetic field is a 3-dimensional vector that, like gravity, can be used to determine long-term orientation. The typical magnitude of the Earth’s magnetic field is between 20 µT and 70 µT. +To convert between NED and inertial coordinates, one typically needs to consider the observer's position and the rotation of the Earth. This transformation involves accounting for the observer's latitude, longitude, and altitude, as well as considering the Earth's rotation rate. For these reasons, we will stick with the inertial reference frame. ## The LSM9DS1 IMU @@ -109,204 +120,257 @@ In Rev. 2 of the Nano 33 BLE SENSE, the LSM9DS1 IMU is replaced with the [BMI270 The BMI270 is a 3-axis magnetometer. It has a measurement range of ±1300 µt (x-, y- axis) and ±2500µT (z-axis). +## IMU Orientation + +IMU sensors don't have a standardised axis layout. Even if they did, it is the way that they are mounted on your device that is important. This matters most when we are combining data from different sensors like in the LSM9DS1 or the BMI270 and BMM150 on the Nano 33 BLE Rev 2. + +In this situation we want to ensure that all the sensors are using the same reference frame. To this end, we have provided the `setData(SensorData d, bool axisAlign)` method. If `axisAlign = true` (the default), then when setting the sensor data, the following modifications are done based on IMU type: + +- `LSM9DS1` - magnetometer x-axis value is reversed. +- `BMI270_BMM150` - magnetometer y-axis value is reversed. + +Depending on the sensor combination you are using and their mounting orientation, you may need to do something similar in your sketch. If `axisAlign = false`, then no modification is made to the loaded sensor data. + +Even though the Nano 33 BLE Sense Rev 2 mounts the two sensors to line up the x-axis, the magnetometer y-axis is still reversed according to the data sheet. + +The convention that we use, consistent with the inertial frame euler angles, is that roll is associated with the x-axis, pitch is associated with the y-axis and yaw is associated with the z-axis. + ## How to Use the Reefwing AHRS Library -The Reefwing AHRS Library includes the following classes: +In order to get the correct angles out of the AHRS, we need to take the following steps: - - Quaternion: converts Euler Angles to a Quaternion and includes open source complementary, madgwick and mahoney sensor fusion quaternion update methods. - - LSM9DS1: for testing, calibrating and reading the IMU data from the sensor. - - LPS22HB: to read the barometer pressure data if available (fitted to the Nano 33 BLE SENSE version only). +1. Calibrate the IMU (if available) and assign the appropriate offsets. This is normally possible in the IMU library. -Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be applied in the correct order which for this configuration is yaw, pitch, and then roll. +2. In `setup()`, instantiate the `ReefwingAHRS` class and call `begin()` to detect the board type, and assign the default free parameters, fusion algorithm (Madgwick) and declination (Sydney, Australia). If you want to use something other than the defaults, then change those now. -The Quaternion class is used by the LSM9DS1 class to provide fast updates to the roll, pitch and yaw Euler Angles derived from the IMU. The quaternion is a mathematical representation of the rotation matrix which describes a unique location in 3D space for our sensor. +3. On each iteration of `loop()`: -At its simplest, a sketch will include: +- If required adjust the sensor data to the correct format. Accelerometer - g, Gyroscope - DPS (degrees per second), and Magnetometer - can be anything (e.g., gauss or uT). The other thing to check is that the axis orientation is consistent for the three sensors. On the LSM9DS1, the magnetometer x-axis is inverted. +- Pass the latest formatted sensor data from the IMU to the AHRS class. -```c++ -#include +4. Update the AHRS model (call `update()`). -LSM9DS1 imu; -EulerAngles angles; +5. If the selected AHRS model uses quaternions (i.e., `COMPLEMENTARY`, `MADGWICK`, and `MAHONY`), convert the quaternion to a Euler Angle. -void setup() { - imu.begin(); // Initialise the LSM9DS1 IMU - imu.start(); // Start processing sensor data -} +The best way to illustrate this process is with the provided examples. -void loop() { - angles = imu.update(); // Check for new IMU data and update angles -} -``` +## Examples - boardType.ino + +This simple example attempts to detect the board type using the methodology explained in [our article on the nRF52840 boards](https://reefwing.medium.com/arduino-nano-33-ble-sense-rev-2-and-the-seeed-nrf52840-sense-3e16c5e25d95). -Unless you are using the Fusion filter, it is strongly recommended that you run the test and calibrate sketch first to obtain the gyro, acc and mag biases for your board, see the `serialRollPitchYaw.ino` example sketch for how these are applied. +The getBoardTypeString() method will return one of the following: -The Fusion filter has a gyroscope bias correction algorithm which provides run-time calibration of the gyroscope bias. The algorithm will detect when the gyroscope is stationary for a set period of time (`fusionThreshold`) and then begin to sample gyroscope measurements to calculate the bias as an average. +- Nano +- Nano 33 BLE +- Nano 33 BLE Sense +- Nano 33 BLE Sense Rev 2 +- Seeed XIAO nRF52840 Sense +- Portenta H7 +- Not Defined Board Type -The EulerAngles data structure returned by the `imu.update()` method is defined as: +The related method 'getBoardType()', will return one of the BoardType enums. ```c++ -struct EulerAngles { - // Tait-Bryan Euler Angles, commonly used for aircraft orientation. - float roll, pitch, yaw, heading; - float rollRadians, pitchRadians, yawRadians; +enum class BoardType { + NANO = 0, + NANO33BLE, + NANO33BLE_SENSE_R1, + NANO33BLE_SENSE_R2, + XIAO_SENSE, + PORTENTA_H7, + VIDOR_4000, + NANO33IOT, + NOT_DEFINED }; ``` -You can also access the raw sensor data from the accelerometer, gyroscope and magnetometer using the `rawData()` method. For example, +This is useful if you are using different hardware with common firmware and need it to conditionally compile using different header files (e.g., IMU libraries). -```c++ -SensorData data = imu.rawData(); -``` +## Examples - nanoMPU6050.ino + +The vanilla Arduino Nano (i.e., ATMega328P version), could have any IMU connected to it. This example illustrates how to define the IMU to the AHRS class, so that it handles the calculations correctly. We are using the MPU-6050 IMU, so in `setup()` we assign: -The SensorData structure contains: ```c++ -struct SensorData { - float ax, ay, az; - float gx, gy, gz; - float mx, my, mz; -}; +ahrs.setDOF(DOF::DOF_6); +ahrs.setImuType(ImuType::MPU6050); ``` -The LSM9DS1 class provides access to the quaternion which contains the latest fused sensor values with the `getQuaternion()` method. For example: +The most important setting is the `DOF`, so it doesn't matter if the IMU you use isn't one of the defined types. With IMUs that only have a gyroscope and accelerometer (6 DOF), you can only use the `CLASSIC`, `KALMAN` & `NONE` Sensor Fusion options. + +Set the algorithm that you wish to use with: ```c++ -Quaternion quaternion = imu.getQuaternion(); +ahrs.setFusionAlgorithm(SensorFusion::CLASSIC); ``` -The Quaternion class object includes the Euler Parameters: +The other Sensor Fusion algorithms require a 9 DOF IMU (i.e., a magnetometer as well). + +## Examples - nano33BLErev1.ino + +This example will print roll, pitch, yaw and heading angles to the Arduino IDE Serial Monitor. It is targeted at the LSM9DS1 IMU on the Nano 33 BLE or Nano 33 BLE Sense Revision 1 (rev1). If you run this sketch on the rev2 Sense (or any board that doesn't use the LSM9DS1), you will receive the message: `LSM9DS1 IMU Not Detected`. + +This sketch is configured to work with the `MADGWICK`, `MAHONY`, `CLASSIC`, `COMPLEMENTARY` and `NONE` Sensor Fusion options: + +- **Complementary Filter** - uses quaternions to find the weighted average of angles produced by the sensors. +- **Madgwick Filter** - uses quaternions and combines a complementary filter approach with adaptive gain to fuse the sensor data. +- **Mahony Filter** - uses quaternions, another riff on complementary fusion and includes an error correction step to compensate for gyroscope bias. +- **Classic Complementary Filter** - Euler Angles are updated using trigonometry, apart from this it is similar to the Quaternion version. +- **Kalman Filter** - can be used to minimise the impact of noise in sensors. +- **NONE** - Euler angles are calculated but no sensor fusion is used. This is useful for demonstrating why you need sensor fusion! + +The loop frequency will vary with the board and fusion algorithm. The `COMPLEMENTARY` filter is a bit slower than the `MADGWICK` and `MAHONY`. We found that we experienced significant gyroscopic drift with the classic complementary filter. The Madgwick and Mahony filters fixes this issue but take a bit longer to settle on an angle. Of the two, Mahony is a bit faster than Madgwick, but the best filter and associated free parameter settings will depend on the application. The `NONE` option is also fast but has crazy gyroscopic drift. + +## Examples - nano33BLErev2.ino + +This example prints roll, pitch, yaw and heading angles on the Arduino Serial Monitor using the BMI270/BMM150 IMUs found on the Nano 33 BLE Sense Revison 2 (rev2). This example is similar to the last one, but we use the standard Arduino IMU library for the BMI270 and BMM150. ```c++ -float q0, q1, q2, q3; // Euler Parameters +#include +#include ``` -The Quaternion class has a method for returning the associated Euler Angles: +To detect the presense of the BMI270/BMM150 IMUs, we initially used: ```c++ -EulerAngles toEulerAngles(float declination = 0.0); +if (IMU.begin()) {…} ``` -The declination is an optional parameter in this method. The yaw angle is relative to magnetic north, including the magnetic declination for your area will correct this to a heading which indicates the angle to true north. - - ## Examples +However, this condition is always true if you use the `Arduino_BMI270_BMM150.h` library (The `begin()` method always returns 1, it doesn't check for a sensor connection). To fix this, we instead used: - ### 1. IMU Configuration +```c++ +if (IMU.begin() && ahrs.getBoardType() == BoardType::NANO33BLE_SENSE_R2) {…} +``` -The `imuConfig.ino` sketch will test if the gyroscope, magnetometer and accelerometer are connected via I2C. It does this by reading the value of the `WHO AM I` register and comparing this with the expected value. The expected values are defined as follows for the gyro/accelerometer, magnetometer and barometer. Note that a barometer is only fitted to the Nano 33 BLE SENSE board not the base Nano 33 BLE. +The IMU data is stored in a SensorData struct, to allow us to easily load these values into the AHRS for angle updates. ```c++ -#define LSM9DS1XG_WHO_AM_I_VALUE 0x68 -#define LSM9DS1M_WHO_AM_I_VALUE 0x3D -#define LPS22HB_WHO_AM_I_VALUE 0xB1 +struct SensorData { + float ax, ay, az; + float gx, gy, gz; + float mx, my, mz; + uint32_t gTimeStamp, aTimeStamp, mTimeStamp; +}; ``` -If the LSM9DS1 IMU is connected, the sketch will print out the Gyro chip temperature and default sensitivities for the three IMU sensors. The linear acceleration sensitivity is given in units of mg/LSB. This means milli-G's (1G = 9.8ms^-2) per Least Significant Bit. This is the smallest value that the sensor can return. Similarly, magnetic sensitivity is given in mGauss/LSB and angular rate sensitivity is mdps (milli degrees per second)/LSB. +The loop frequency for the combination of the Nano 33 BLE Sense Rev2 and the BMI270/BMM150 IMUs, is quite a bit slower than the earlier generation boards. We are not sure why this is, the hardware is operating at the same clock speed, so it is probably IMU library related, -The sensitivity of the sensors depends on the full scale value selected (ref: Table 3 in the [LSM9DS1 Data Sheet](https://www.st.com/resource/en/datasheet/lsm9ds1.pdf)). +It could be the 100 Hz sample rate for the gyro and accelerometer. The standard library doesn't provide a method to change this rate but the `interrupts_subclassing.ino` example shows how it can be done (gyro maximum ODR = 6.4 kHz and accelerometer maximum ODR = 1.6 kHz). -An angular rate gyroscope is device that produces a positive-going digital output for counterclockwise rotation around the axis considered. Sensitivity describes the gain of the sensor. This value changes very little over temperature and time. Magnetic sensor sensitivity also describes the gain of the sensor. +For comparison, the default sample rate for the gyro/accelerometer in the Reefwing LSM9DS1 Library is 119 Hz (and can be set to a maximum of 952 Hz). -### 2. Self Test and Calibration +As for the previous example, you can adjust the sensor fusion method using: -Before you use an IMU for the first time, you need to calibrate it. The results wont be accurate if you don't do this. At the start of the self test, the Nano 33 BLE should be **level and not moving**. +```c++ +ahrs.setFusionAlgorithm(SensorFusion::MAHONY); +``` -#### 2.1 Gyroscope and Accelerometer +## Examples - xiaoSense.ino -The LSM9DS1 includes a self test function that is not particularly well documented. To start the Gyro self-test, control register 10, bit ST_G needs to be set to 1. For the accelerometer, control register 10, bit ST_XL is set. The Reefwing AHRS library looks after this in the `selfTest` method. It calculates the average of the IMU at-rest readings and then loads these resulting offsets into the accelerometer and gyroscope bias registers. +There are no processor specific calls in our AHRS library, so theoretically it could be used with any Arduino compatible board and IMU. To demonstrate this, we have provided an example for the Seeed Studio xiao Sense. This board is tiny! -An accelerometer in a steady state on a horizontal surface should measure 0 g on both the X-axis and Y-axis, whereas the Z-axis should measure 1 g. Ideally, the output is in the middle of the dynamic range of the sensor. A deviation from the ideal value is called a zero-g offset. Similarly, the gyro zero-rate level describes the actual output signal if there is no angular rate present and zero-gauss level offset describes the deviation of an actual output signal from the ideal output if no magnetic field is present. +The xiao Sense uses the LSM6DS3 IMU (6 DOF). This IMU is referered to as 6-axis device because it only has a gyroscope and accelerometer (no magnetometer). Before using the AHRS example, make sure that you can run the `HighLevelExample.ino` sketch, found in the LSM6DS3 library examples folder. -Offset is to some extent a result of stress to MEMS sensor and therefore the offset can slightly change after mounting the sensor onto a printed circuit board or exposing it to extensive mechanical stress (like a drone crash). Offset changes little with temperature variation. +To do this, delete the `Arduino_LSM6DS3` library if you have it installed, and replace it with the Seeed Arduino LSM6DS3 library (version 2.0.3 or later). Also install the Seeed nrf52 mbed-enabled Boards package, version 2.7.2, using the Arduino Boards Manager. -From the [LSM9DS1 Data Sheet](https://www.st.com/resource/en/datasheet/lsm9ds1.pdf), table 3, typical bias offsets are: +The Seeed library is a fork of the SparkFun version. Note that the default constructor for this library is I²C mode, with address `0x6B`. This is not what we want, so we need to include the I²C address when we instantiate the IMU class: -``` -- Gyroscope (G_TyOff): ± 30 dps -- Accelerometer (LA_TyOff): ± 90 mg -- Magnetometer (M_TyOff): ± 1 gauss +```c++ +LSM6DS3 imu(I2C_MODE, 0x6A); ``` -This highlights the need for calibration. An uncorrected gyro offset of 30 degrees per second is material! - -The results of the `selfTest()` function are loaded into a data structure, `SelfTestResults`, which is defined as: +The library API provides the following methods to read the IMU values. You have to call `imu.begin()`, before using these methods. ```c++ -struct SelfTestResults { - float gyrodx; - float gyrody; - float gyrodz; - float accdx; - float accdy; - float accdz; -}; +imu.readFloatAccelX(); +imu.readFloatAccelY(); +imu.readFloatAccelZ(); +imu.readFloatGyroX(); +imu.readFloatGyroY(); +imu.readFloatGyroZ(); +imu.readTempC(); ``` -A sensor will `PASS` the self test if these results are within the expected range: +However, it isn't obvious from the documentation what units are being returned (e.g., degrees per second, radians per second, g's, etc). To work out what is going on we need to have a look at the library code. From this we establish: + +- The default sample rate (ODR) for the gyro and accelerometer is 416 Hz +- Gyro Range = 2000 degrees/second (DPS) +- Accelerometer range = 16 g's +- Gyro output = register * 4.375 * (gyroRangeDivisor) / 1000 (i.e., DPS) +- Accel output = register * 0.061 * (accelRange >> 1) / 1000 (i.e., g's) -- Gyroscope x, y and z axis expected range: 20 - 250 dps. -- Accelerometer x, y, and z axis expected range: 60 - 1700 mg. +As we have no magnetometer, we can only use the NONE, KALMAN, and CLASSIC sensor fusion algorithms. These all have a loop frequency of around 240 Hz. We can't calculate the tilt compensated yaw without a magnetometer, so the yaw results are useless. We had high hopes for the xiao Sense as a flight controller because of its small size and light weight. In practise, the results returned by the AHRS are pretty inaccurate and could only be used for applications like a pedometer. -#### 2.2 Magnetometer +The axis orientation of the LSM6DS3 IMU has the y-axis reversed compared to the LSM9DS1. This reversal is corrected by the library, when `setData()` is called. -The magnetometer included in the LSM9DS1 chip is the LIS3MDL. Its calibration is a bit different to the accelerometer and gyroscope. At the start of the magnetometer calibration, you will be asked to wave the Nano 33 BLE in a vertical figure of 8. +## Examples - mkrMPU6500.ino -Since the magenetic field of the earth is relatively constant, the magnitude of the field measured by the magnetometer should also be constant, regardless of the orientation of the sensor. If one were to rotate the sensor and plot 𝑚𝑥, 𝑚𝑦, and 𝑚𝑧 in 3D, the paths should plot out the surface of a sphere with a constant radius. +This example will print roll, pitch, yaw and heading angles to the Serial Monitor, using the MPU6500 IMU and an Arduino MKR board (e.g., Portenta H7 or Vidor 4000), connected via SPI. -In reality, due to hard and soft iron distortion, a 3D plot of magnetic field strength ends up as a deformed sphere. Calibration is used to adjust each of the three axis readings, so that the magnitude is constant regardless of orientation. +The Arduino Portenta H7 is a beast of a board. Our initial plan was to port BetaFlight to this board, but that [didn't work out](https://reefwing.medium.com/the-new-arduino-portenta-h7-board-76040c3405f). It has two cores, and the one that we will be using is a Cortex M7 running at 480 MHz. -Performing the figure of 8 pattern 'traces out' part of our deformed sphere. From the coordinates obtained, the deformation of the sphere can be estimated, and the calibration coefficients calculated. A good pattern is one that traces through the greatest range of orientations and therefore estimates the greatest deviation from the true constant magnitude. +One downside of the Portenta is that it doesn't have an IMU. To address this, we created our own shield which includes the [MPU6500](https://invensense.tdk.com/download-pdf/mpu-6500-datasheet/). To maximise data transfer rates, the MPU6500 is connected to the Portenta using SPI (with a maximum clock speed of 20 MHz, schematic). -### 3. Roll, Pitch and Yaw Angles on Serial Port +Like the LSM6DS3, the MPU6500 is a 6-axis IMU (i.e., no magnetometer) and has a similar axis orientation. You can use the the `CLASSIC`, `KALMAN` & `NONE` Sensor Fusion options in our AHRS library. -This sketch is an example of a simple AHRS. It prints out the roll, pitch, yaw and heading angles to the Serial port every second. It is a good way to explore the capabilities of the library and for deciding which quaternion update method is best suited to your application. Running on a Nano 33 BLE SENSE, the loop frequency was just over 300 Hz for Mahony and Madgwick filters, and just under 300 Hz for the Complementary filter. +The Portenta H7 uses the Arduino MKR board layout, and we have tested this library with the MPU6500 mounted on the Portenta H7 and the MKR Vidor 4000. We are using the Reefwing MPU6500 IMU library in our example. Substitute the appropriate library and setup/calibration for your IMU. -### 4. Complementary Filter +## Examples - complementaryFilter.ino -Largely the same as the previous sketch (roll, pitch and yaw on the serial port), it demonstrates how and where to set the filter free parameters. In this case, you can adjust `𝛂` and watch the effect on angle stability. +This example is very similar to the `nano33BLErev1.ino` example but is targeted at the complementary filter sensor fusion algorithm. You can adjust the filter mix by setting alpha (𝛂). ```c++ -imu.setAlpha(0.95); +ahrs.setAlpha(0.95); ``` -For the complementary filter, with `𝛂 = 0.0` the angle result will be 100% from the accelerometer and magnetometer. With `𝛂 = 1.0`, the result will be 100% from the gyroscope. A large `α` implies that the output will decay very slowly but will also be strongly influenced by even small changes in input. The output of this filter will decay towards zero for an unchanging input. +Alpha (𝛂) in the Complementary filter is known as the filter co-efficient, smoothing factor or gain. It determines the cutoff frequency for the high pass filter, which we pass the gyro rate through.  -### 5. Beta Optimisation for the Madgwick Filter -The filter gain β represents all mean zero gyroscope measurement errors, expressed as the magnitude of a quaternion derivative. This error represents the gyroscope bias. +`𝛂 = 𝜏 / (𝜏 + ∆t) ` -What this means is that beta is a parameter that you can tweak to optimise the filter results for your Arduino. In Madgwick's paper, he tests different values of 𝛃 (0 to 0.5) and graphs the results. A copy of this paper may be found in the theory folder of the Reefwing AHRS library. The y-axis on Madgwick's graph, [Effect of β on filter performance](https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf), represents the RMS error in degrees. +where 𝜏 = filter time constant and ∆t = imu sampling rate.  -`There is a clear optimal value of β for each filter implementation; high enough to minimises errors due to integral drift but sufficiently low enough that unnecessary noise is not introduced by large steps of gradient descent iterations.` +A typical value for 𝛂 is 0.95 to 0.98 but it depends on the sampling rate and application. A large α implies that the output will decay very slowly but will also be strongly influenced by even small changes in input. The output of this filter will decay towards zero for an unchanging input. -RMS is often used to compare a theoretical prediction against an actual result. The root mean square (RMS) error is defined as the arithmetic mean of the squares of a set of numbers. If we take n angle measurements {𝜽1, 𝜽2, …, 𝜽n}, the RMS is: +In its simplest form, the complementary filter calculation is: -`𝜽rms = √[(𝜽1² + 𝜽2² + … + 𝜽n²)/n]` +`Complementary Filter Result = 𝛂 × Input_A + (1 - 𝛂) × Input_B` -This sketch assists with beta optimisation of your Madgwick filter. It will print to the serial port the value for beta between 0.0 and 0.5 and its associated static RMS error in CSV format. You can graph these results. The sketch will record the beta with the minimum error in a static situation for pitch and roll. This sketch also demonstrates how you can adjust the filter gain in the Reefwing AHRS library. The optimum beta and minimum error is printed out at the end of the sketch. +The complementary filter formula for the roll angle looks like: -``` -Optimum Beta = 0.17, with an RMS error of 0.00 degrees. -Default Beta = 0.60 -``` +`Roll 𝜽(t + ∆t) = 0.98 × Gyro Angle + 0.02 × Acc Angle` + +So a simple way to think about alpha is it adjusts the contribution from different sensors towards the final angle. In the formula above 98% of the angle comes from the gyroscope (fast acting sensor) and 2% is from the accelerometer (slow but lower noise sensor). + +## Examples - betaOptimisation.ino + +This example sketch attempts to determine the optimum beta for the Madgwick filter. The Nano 33 BLE should be placed flat and not moved for the duration of the test. -You need to run the test a few times to get a feel for where the true optimum is. On our test board, the optimum was around 0.2. +The default sample rate for this library is 238 Hz for the gyro/accelerometer and 10 Hz for the magnetometer. Thus a new gyro/acc reading should be available every 4.2 ms. We delay for 10 ms between readings. -However, for the Madgwick filter there is a tradeoff in the beta parameter between accuracy and response speed. In the original Madgwick study, a beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. With this value, the LSM9SD1 response time is about 10 seconds to a stable initial quaternion. Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter! +Beta (𝛃) is a free parameter used in the Madgwick filter. -Increasing beta (GyroMeasError) by about a factor of fifteen (beta = 0.6), the response time is reduced to ~2 seconds. We haven't noticed any reduction in solution accuracy with this reduced lag. This is essentially the I coefficient in a PID control sense; the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. +The gain 𝛽, represents all mean zero gyroscope measurement errors, +expressed as the magnitude of a quaternion derivative. It is defined +using the angular velocity. Beta is based on our estimate of gyroscope +measurement error (e.g., GyroMeasError = 40 degrees per second is used +as a default). -### 6. Fusion AHRS -[Fusion](https://github.com/xioTechnologies/Fusion) is the updated version of the Madgwick filter and was specifically developed for use with embedded systems and has been optimised for execution speed. +We calculate beta using the code shown below. ```c++ - imu.setFusionAlgorithm(SensorFusion::FUSION); - imu.setFusionPeriod(0.01f); // Estimated sample period = 0.01 s = 100 Hz - imu.setFusionGain(0.5); // Default Fusion Filter Gain +float beta = sqrt(3.0f / 4.0f) * gyroMeasError; ``` -The fusion AHRS sketch is similar to the other examples, apart from the Fusion specific configuration shown above. This is what we will focus on. +`The other free parameter for the Madgwick filter is the +filter gain ζ which represents the rate of convergence to remove gyroscope +measurement errors which are not mean zero, also expressed as the +magnitude of a quaternion derivative. ` + +There is a clear optimal value of β for each filter implementation; high enough to minimises errors due to integral drift but sufficiently low enough that unnecessary noise is not introduced by large steps of gradient descent iterations. + +RMS is often used to compare a theoretical prediction against an actual result. The root mean square (RMS) error is defined as the arithmetic mean of the squares of a set of numbers. If we take n angle measurements {𝜽1, 𝜽2, …, 𝜽n}, the RMS is: + +`𝜽rms = √[(𝜽1² + 𝜽2² + … + 𝜽n²)/n]` -- `imu.setFusionPeriod(0.01f)`: One difference with this filter is that you need to know the frequency that the model will be updated in advance. This is normally either the sensor sample rate or the loop frequency of the Arduino board (whichever is slower). In this example we tweaked the delay amount in `loop()` to get 100 Hz (i.e., a period of 0.01 seconds). -- `imu.setFusionGain(0.5)`: The fusion algorithm is governed by a gain. A low gain will decrease the influence of the accelerometer and magnetometer so that the algorithm will better reject disturbances causes by translational motion and temporary magnetic distortions. However, a low gain will also increase the risk of drift due to gyroscope calibration errors. Madgwick suggests a gain value of 0.5, but this results in a lagging angle resolution. For a drone we think a gain of around 7.5 is better. This provides a much faster angle response without appearing to sacrifice too much accuracy. +This sketch in the examples folder can be used to assist with beta optimisation of your Madgwick filter. It will print to the serial port the value for beta between 0.0 and 0.5 and its associated static RMS error in CSV format. You can graph these results (using Excel for example). The sketch will record the beta with the minimum error in a static situation for pitch and roll. This sketch also demonstrates how you can adjust the filter gain in the Reefwing AHRS library. -For more details of how this algorithm operates, refer to the [Fusion Readme](theory/FUSION_README.md) in the theory folder of this repository. diff --git a/examples/betaOptimisation/betaOptimisation.ino b/examples/betaOptimisation/betaOptimisation.ino index 6824f18..08dc27f 100644 --- a/examples/betaOptimisation/betaOptimisation.ino +++ b/examples/betaOptimisation/betaOptimisation.ino @@ -41,7 +41,7 @@ ReefwingAHRS ahrs; float beta = MIN_BETA; // Madgwick filter gain, Beta float sampleArray[SAMPLES_PER_BETA]; -float error, optimumBeta, minError = INFINITY; +float err, optimumBeta, minError = INFINITY; int ctr = 0; // Track samples collected for each value of beta float rms(float arr[]) { @@ -116,13 +116,13 @@ void setup() { delay(10); // Wait for new sample } ctr = 0; - error = rms(sampleArray); - if (error < minError) { - minError = error; + err = rms(sampleArray); + if (err < minError) { + minError = err; optimumBeta = beta; } - Serial.print(beta); Serial.print("\t"); Serial.println(error); + Serial.print(beta); Serial.print("\t"); Serial.println(err); beta += BETA_INC; } diff --git a/keywords.txt b/keywords.txt index 9c47bff..c97aefc 100644 --- a/keywords.txt +++ b/keywords.txt @@ -7,67 +7,46 @@ ####################################### ReefwingAHRS KEYWORD1 -LSM9DS1 KEYWORD1 -LPS22HB KEYWORD1 +BoardType KEYWORD1 +DOF KEYWORD1 +ImuType KEYWORD1 Quaternion KEYWORD1 SensorFusion KEYWORD1 SelfTestResults KEYWORD1 BiasOffsets KEYWORD1 EulerAngles KEYWORD1 SensorData KEYWORD1 -Ascale KEYWORD1 -Gscale KEYWORD1 -Mscale KEYWORD1 +KalmanFilter KEYWORD1 ####################################### # Methods and Functions (KEYWORD2) ####################################### begin KEYWORD2 -start KEYWORD2 -reset KEYWORD2 update KEYWORD2 -rawData KEYWORD2 -selfTest KEYWORD2 -connected KEYWORD2 -filterFormat KEYWORD2 -getQuaternion KEYWORD2 -accelAvailable KEYWORD2 -gyroAvailable KEYWORD2 -magAvailable KEYWORD2 -resetQuaternion KEYWORD2 -readGyroTemp KEYWORD2 -readTemperature KEYWORD2 -readPressure KEYWORD2 -readPressureUI KEYWORD2 -readPressureRAW KEYWORD2 +classicUpdate KEYWORD2 madgwickUpdate KEYWORD2 mahoneyUpdate KEYWORD2 complementaryUpdate KEYWORD2 -toEulerAngles KEYWORD2 -setAccResolution KEYWORD2 -setGyroResolution KEYWORD2 -setMagResolution KEYWORD2 -getAccResolution KEYWORD2 -getGyroResolution KEYWORD2 -getMagResolution KEYWORD2 -calibrateAccGyro KEYWORD2 -calibrateMag KEYWORD2 -setFusionPeriod KEYWORD2 -setFusionThreshold KEYWORD2 +kalmanUpdate KEYWORD2 +tiltCompensatedYaw KEYWORD2 setFusionAlgorithm KEYWORD2 setAlpha KEYWORD2 setBeta KEYWORD2 setGyroMeasError KEYWORD2 setKp KEYWORD2 setKi KEYWORD2 -setFusionGain KEYWORD2 setDeclination KEYWORD2 -loadAccBias KEYWORD2 -loadGyroBias KEYWORD2 -loadMagBias KEYWORD2 -getBiasOffsets KEYWORD2 +setData KEYWORD2 +setDOF KEYWORD2 +setImuType KEYWORD2 +setBoardType KEYWORD2 +updateEulerAngles KEYWORD2 getQuaternion KEYWORD2 +gyroToRadians KEYWORD2 +getBoardType KEYWORD2 +getBoardTypeString KEYWORD2 +formatAnglesForConfigurator KEYWORD2 ####################################### # Constants (LITERAL1) @@ -79,4 +58,6 @@ COMPLEMENTARY LITERAL1 FUSION LITERAL1 CLASSIC LITERAL1 KALMAN LITERAL1 -NONE LITERAL1 \ No newline at end of file +NONE LITERAL1 +DEG_TO_RAD LITERAL1 +RAD_TO_DEG LITERAL1 \ No newline at end of file