├── LICENSE ├── README.md ├── deltat.py ├── fusion.py ├── fusion_async.py ├── fusionlcd.py ├── fusiontest.py ├── fusiontest6.py ├── fusiontest_as.py ├── fusiontest_as6.py ├── orientate.py └── remote ├── README.md ├── capture.py ├── fusion_r_asyn.py ├── fusion_r_syn.py └── mpudata /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Peter Hinch 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Introduction: micropython-fusion 2 | 3 | Sensor fusion calculates heading, pitch and roll from the outputs of motion 4 | tracking devices. This uses the Madgwick algorithm, widely used in multicopter 5 | designs for its speed and quality. An update takes under 2mS on the Pyboard. 6 | The original Madgwick study indicated that an update rate of 10-50Hz was 7 | adequate for accurate results, suggesting that the performance of this 8 | implementation is fast enough. 9 | 10 | Two implementations are provided: one for synchronous code and one for 11 | asynchronous applications based on `asyncio`. The latter provides for 12 | continuous background updates of the angle data enabling access with minimal 13 | latency. 14 | 15 | ## Platforms 16 | 17 | This document describes the case where sensor data is acquired, and fusion 18 | is performed, on a single platform running MicroPython. 19 | 20 | Other modes are supported: 21 | 22 | * Fusion and data acquisition run on a common device under standard Python. 23 | * Fusion and data acquisition run on separate devices linked by some form of 24 | communications link. In this mode the data acquisition device may run any type 25 | of code and return data in any format, with the user application reading and 26 | converting the data to a form acceptable to the library. 27 | 28 | These modes are discussed [here](./remote/README.md). 29 | 30 | ## MicroPython issues 31 | 32 | The code is intended to be independent of the sensor device: testing was done 33 | with the InvenSense MPU-9150. 34 | 35 | The algorithm makes extensive use of floating point maths. Under MicroPython 36 | this implies RAM allocation. While the code is designed to be platform agnostic 37 | problems may be experienced on platforms with small amounts of RAM. Options 38 | are to use frozen bytecode and to periodically run a garbage collection; the 39 | latter is advisable even on the Pyboard. See the `fusionlcd.py` test program 40 | for an example of this. 41 | 42 | ## MicroPython firmware dependency 43 | 44 | Some modules in this library use asynchronous programming. This uses the 45 | `asyncio` library under CPython, `uasyncio` under MicroPython. The MicroPython 46 | version is much improved after a complete rewrite and is at version 3.0. It is 47 | syntax compatible with CPython 3.8 `asyncio`. All code has been updated to use 48 | this syntax, which is unsupported by older versions. 49 | 50 | To run asynchronous modules MicroPython targets should use a daily build of 51 | firmware, or a release build after V1.12: such firmware incorporates `uasyncio` 52 | V3. 53 | 54 | Where asynchronous code is run under CPython, this must be V3.8 or later. 55 | 56 | ## Terminology and units of measurement 57 | 58 | I should point out that I'm unfamiliar with aircraft conventions and would 59 | appreciate feedback if the following observations about coordinate conventions 60 | are incorrect. 61 | 62 | ### Angles 63 | 64 | Inertial measurement units (IMU's) exist with and without magnetometers. Those 65 | with them are known as 9DOF, and those without as 6DOF sensors where DOF stands 66 | for "degrees of freedom". 6DOF sensors cannot provide heading information as 67 | this is derived with reference to the Earth's magnetic field. 68 | 69 | The object of sensor fusion is to determine a vehicle's attitude with respect 70 | to Earth. This is expressed in the following three angles: 71 | 72 | 1. `heading` Angle relative to North. Note some sources use the term "yaw". 73 | As this is also used to mean the angle of an aircraft's fuselage relative to 74 | its direction of motion, I have avoided it. 75 | 2. `pitch` Angle of aircraft nose relative to ground (conventionally +ve is 76 | towards ground). Also known as "elevation". 77 | 3. `roll` Angle of aircraft wings to ground, also known as "bank". 78 | 79 | In this implementation these are measured in degrees. 80 | 81 | ### Sensors 82 | 83 | The units of measurement provided by the sensor driver are important only in 84 | the case of the gyroscope. This must provide a measurement in degrees per 85 | second. Values from the accelerometer (typically g) and the magnetometer 86 | (typically Microtesla) are normalised in the algorithm. This uses the fact that 87 | the magnitude of these vectors is locally constant; consequently the units of 88 | measurement are irrelevant. 89 | 90 | The issue of the orientation of the sensor is discussed in 91 | [section 4](./README.md#4-notes-for-constructors). 92 | 93 | # Contents 94 | 95 | 1. [Modules](./README.md#1-modules) 96 | 2. [Fusion module](./README.md#2-fusion-module) 97 | 2.1 [Fusion class](./README.md#21-fusion-class) 98 | 2.1.1 [Methods](./README.md#211-methods) 99 | 2.1.2 [Bound variables](./README.md#212-bound-variables) 100 | 3. [Asynchronous version](./README.md#3-asynchronous-version) 101 | 3.1 [Fusion class](./README.md#31-fusion-class) 102 | 3.1.1 [Methods](./README.md#311-methods) 103 | 3.1.2 [Variables](./README.md#312-variables) 104 | 4. [Notes for constructors](./README.md#4-notes-for-constructors) 105 | 5. [Background notes](./README.md#5-background-notes) 106 | 5.1 [Heading Pitch and Roll](./README.md#51-heading-pitch-and-roll) 107 | 5.2 [Beta](./README.md#52-beta) 108 | 6. [References](./README.md#6-references) 109 | 110 | # 1. Modules 111 | 112 | 1. `fusion.py` The standard synchronous fusion library. 113 | 2. `fusion_async.py` Version of the library using uasyncio for nonblocking 114 | access to pitch, heading and roll. 115 | 3. `deltat.py` Controls timing for above. 116 | 4. `orientate.py` A utility for adjusting orientation of an IMU for sensor 117 | fusion. 118 | 119 | Test/demo programs: 120 | 121 | 1. `fusiontest.py` A simple test program for synchronous library. 122 | 2. `fusiontest6.py` Variant of above for 6DOF sensors. 123 | 3. `fusiontest_as.py` Simple test for the asynchronous library. 124 | 4. `fusiontest_as6.py` Variant of above for 6DOF sensors. 125 | 5. `fusionlcd.py` Tests the async library with a Hitachi HD44780 2-row LCD 126 | text display to continuously display angle values. 127 | 128 | If using InvenSense MPU9150, MPU6050 or MPU9250 IMU's, drivers may be found 129 | [here](https://github.com/micropython-IMU/micropython-mpu9x50). 130 | 131 | The directory `remote` contains files and information specific to 132 | [remote mode](./remote/README.md) and to running fusion on standard Python. 133 | 134 | ###### [Jump to Contents](./README.md#contents) 135 | 136 | # 2. Fusion module 137 | 138 | ## 2.1 Fusion class 139 | 140 | The module supports this one class. A Fusion object needs to be periodically 141 | updated with data from the sensor. It provides heading, pitch and roll values 142 | (in degrees) as properties. Note that if you use a 6DOF sensor, heading will be 143 | invalid. 144 | 145 | ### 2.1.1 Methods 146 | 147 | `update(accel, gyro, mag)` 148 | 149 | For 9DOF sensors. Positional arguments: 150 | 1. `accel` A 3-tuple (x, y, z) of accelerometer data. 151 | 2. `gyro` A 3-tuple (x, y, z) of gyro data. 152 | 3. `mag` A 3-tuple (x, y, z) of magnetometer data. 153 | 154 | This method should be called periodically at a frequency depending on the 155 | required response speed. 156 | 157 | `update_nomag(accel, gyro)` 158 | 159 | For 6DOF sensors. Positional arguments: 160 | 1. `accel` A 3-tuple (x, y, z) of accelerometer data. 161 | 2. `gyro` A 3-tuple (x, y, z) of gyro data. 162 | 163 | This should be called periodically, depending on the required response speed. 164 | 165 | `calibrate(getxyz, stopfunc, wait=0)` 166 | 167 | Positional arguments: 168 | 1. `getxyz` A function returning a 3-tuple of magnetic x,y,z values. 169 | 2. `stopfunc` A function returning `True` when calibration is deemed 170 | complete: this could be a timer or an input from the user. 171 | 3. `wait` A delay in ms. Some hardware may require a delay between 172 | magnetometer readings. Alternatively a function which returns after a delay 173 | may be passed. 174 | 175 | Calibration updates the `magbias` bound variable. It is performed by rotating 176 | the unit slowly around each orthogonal axis while the routine runs, the aim 177 | being to compensate for offsets caused by static local magnetic fields. 178 | 179 | ### 2.1.2 Bound variables 180 | 181 | Three bound variables provide access to the Euler angles in degrees: 182 | 183 | 1. `heading` 184 | 2. `pitch` 185 | 3. `roll` 186 | 187 | Quaternion data may be accesed via the `q` bound variable: 188 | 189 | 1. `q` Contains `[w, x, y, z]` representing the normalised (unit) quaternion 190 | `w + xi + yj + zk`. Quaternion data is dimensionless. 191 | 192 | See [my notes on quaternions](https://github.com/peterhinch/micropython-samples/blob/master/README.md#412-quaternions) 193 | for code enabling them to be used to perform 3D rotation with minimal 194 | mathematics. They are easier to use for this purpose than Euler angles. 195 | 196 | A bound variable `beta` controls algorithm performance. The default value may 197 | be altered after instantiation. See [section 5.2](./README.md#52-beta). 198 | 199 | A class variable `declination`, defaulting to 0, enables the heading to be 200 | offset in order to provide readings relative to true North rather than magnetic 201 | North. A positive value adds to heading. 202 | 203 | ###### [Jump to Contents](./README.md#contents) 204 | 205 | # 3. Asynchronous version 206 | 207 | This uses the `uasyncio` library and is intended for applications based on 208 | asynchronous programming. Updates are performed by a continuously running 209 | coroutine. The `heading`, `pitch`, `roll` and `q` values are bound variables 210 | which may be accessed at any time with effectively zero latency. The test 211 | program `fusionlcd.py` illustrates its use showing realtime data on a text 212 | LCD display, `fusiontest_as.py` prints it at the REPL. 213 | 214 | ## 3.1 Fusion class 215 | 216 | The module supports this one class. The constructor is passed a user-supplied 217 | coro which returns the accelerometer, gyro, and (in the case of 9DOF sensors) 218 | magnetometer data. A Fusion instance has a continuously running coroutine which 219 | maintains the heading, pitch and roll bound variables. 220 | 221 | Typical constructor call: 222 | 223 | ```python 224 | imu = MPU9150('X') # Instantiate IMU (default orientation) 225 | 226 | async def read_coro(): 227 | imu.mag_trigger() # Hardware dependent: trigger a nonblocking read 228 | await asyncio.sleep_ms(20) # Wait for mag to be ready 229 | return imu.accel.xyz, imu.gyro.xyz, imu.mag_nonblocking.xyz 230 | # Returned (ax, ay, az), (gx, gy, gz), (mx, my, mz) 231 | 232 | fuse = Fusion(read_coro) 233 | ``` 234 | 235 | The update method is started as follows (usually, in the case of 9DOF sensors, 236 | after a calibration phase): 237 | 238 | ```python 239 | await fuse.start() 240 | ``` 241 | 242 | This starts a continuously running update task. It calls the coro supplied to 243 | the constructor to determine (from the returned data) whether the sensor is a 244 | 6DOF or 9DOF variety. It then launches the appropriate task. From this point 245 | the application accesses the `heading`, `pitch` and `roll` bound 246 | variables as required. 247 | 248 | ### 3.1.1 Methods 249 | 250 | Constructor: 251 | 252 | This takes a single argument which is a coroutine. This returns three (x, y, z) 253 | 3-tuples for accelerometer, gyro, and magnetometer data respectively. In the 254 | case of 6DOF sensors it returns two 3-tuples for accelerometer and gyro only. 255 | The coroutine must include at least one `await asyncio.sleep_ms` statement to 256 | conform to Python syntax rules. A nonzero delay may be required by the IMU 257 | hardware; it may also be employed to limit the update rate, thereby controlling 258 | the CPU resources used by this task. 259 | 260 | `async def start(slow_platform=False)` 261 | This launches the update task, returning immediately. 262 | 263 | Optional argument: 264 | 1. `slow_platform` Boolean. Adds a yield to the scheduler in the middle of 265 | the computation. This may improve application performance on slow platforms 266 | such as the ESP8266. 267 | 268 | `async def calibrate(stopfunc)` 269 | For 9DOF sensors only. 270 | 271 | Argument: 272 | 1. `stopfunc` Function returning `True` when calibration is deemed 273 | complete: this could be a timer or an input from the user. 274 | 275 | Calibration updates the `magbias` bound variable. It is performed by rotating 276 | the unit slowly around each orthogonal axis while the routine runs, the aim 277 | being to compensate for offsets caused by static local magnetic fields. 278 | 279 | ### 3.1.2 Variables 280 | 281 | Three bound variables provide the angles with negligible latency. Units are 282 | degrees. 283 | 284 | 1. `heading` 285 | 2. `pitch` 286 | 3. `roll` 287 | 288 | Quaternion data may be accesed via the `q` bound variable: 289 | 290 | 1. `q` Contains `[w, x, y, z]` representing the normalised (unit) quaternion 291 | `w + xi + yj + zk`. Quaternion data is dimensionless. 292 | 293 | A bound variable `beta` controls algorithm performance. The default value may 294 | be altered after instantiation. See [section 5.2](./README.md#52-beta). 295 | 296 | A class variable `declination`, defaulting to 0, enables the heading to be 297 | offset in order to provide readings relative to true North rather than magnetic 298 | North. A positive value adds to heading. 299 | 300 | ###### [Jump to Contents](./README.md#contents) 301 | 302 | # 4. Notes for constructors 303 | 304 | If you're developing a machine using a motion sensing device consider the 305 | effects of vibration. This can be surprisingly high (use the sensor to measure 306 | it). No amount of digital filtering will remove it because it is likely to 307 | contain frequencies above the sampling rate of the sensor: these will be 308 | aliased down into the filter passband and affect the results. It's normally 309 | necessary to isolate the sensor with a mechanical filter, typically a mass 310 | supported on very soft rubber mounts. 311 | 312 | If using a magnetometer consider the fact that the Earth's magnetic field is 313 | small: the field detected may be influenced by ferrous metals in the machine 314 | being controlled or by currents in nearby wires. If the latter are variable 315 | there is little chance of compensating for them, but constant magnetic offsets 316 | may be addressed by calibration. This involves rotating the machine around each 317 | of three orthogonal axes while running the fusion object's `calibrate` 318 | method. 319 | 320 | The local coordinate system for the sensor is usually defined as follows, 321 | assuming the vehicle is on the ground: 322 | z Vertical axis, vector points towards ground 323 | x Axis towards the front of the vehicle, vector points in normal direction of 324 | travel 325 | y Vector points left from pilot's point of view (I think) 326 | orientate.py has some simple code to correct for sensors mounted in ways which 327 | don't conform to this convention. 328 | 329 | You may want to take control of garbage collection (GC). In systems with 330 | continuously running control loops there is a case for doing an explicit GC on 331 | each iteration: this tends to make the GC time shorter and ensures it occurs at 332 | a predictable time. See the MicroPython `gc` module. 333 | 334 | ###### [Jump to Contents](./README.md#contents) 335 | 336 | # 5. Background notes 337 | 338 | These are blatantly plagiarised as this isn't my field. I have quoted sources. 339 | 340 | ## 5.1 Heading Pitch and Roll 341 | 342 | Perhaps better titled heading, elevation and bank: there seems to be ambiguity 343 | about the concept of yaw, whether this is measured relative to the aircraft's 344 | local coordinate system or that of the Earth: the original Madgwick study uses 345 | the term "heading", a convention I have retained as the angles emitted by the 346 | Madgwick algorithm (Tait-Bryan angles) are earth-relative. 347 | See [Wikipedia article](http://en.wikipedia.org/wiki/Euler_angles#Tait.E2.80.93Bryan_angles) 348 | 349 | The following adapted from https://github.com/kriswiner/MPU-9250.git 350 | These are Tait-Bryan angles, commonly used in aircraft orientation (DIN9300). 351 | In this coordinate system the positive z-axis is down toward Earth. Yaw is the 352 | angle between Sensor x-axis and Earth magnetic North (or true North if 353 | corrected for local declination). Looking down on the sensor positive yaw is 354 | counter-clockwise. Pitch is angle between sensor x-axis and Earth ground plane, 355 | aircraft nose down toward the Earth is positive, up toward the sky is negative. 356 | Roll is angle between sensor y-axis and Earth ground plane, y-axis up is 357 | positive roll. These arise from the definition of the homogeneous rotation 358 | matrix constructed from quaternions. Tait-Bryan angles as well as Euler angles 359 | are non-commutative; that is, the get the correct orientation the rotations 360 | must be applied in the correct order which for this configuration is yaw, 361 | pitch, and then roll. For more see 362 | [Wikipedia article](http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles) 363 | which has additional links. 364 | 365 | I have seen sources which contradict the above directions for yaw (heading) and 366 | roll (bank). 367 | 368 | ## 5.2 Beta 369 | 370 | The Madgwick algorithm has a "magic number" Beta which determines the tradeoff 371 | between accuracy and response speed. 372 | 373 | [Source](https://github.com/kriswiner/MPU-9250.git) of comments below. 374 | There is a tradeoff in the beta parameter between accuracy and response speed. 375 | In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError 376 | of 2.7 degrees/s) was found to give optimal accuracy. However, with this value, 377 | the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. 378 | Subsequent changes also require a longish lag time to a stable output, not fast 379 | enough for a quadcopter or robot car! By increasing beta (GyroMeasError) by 380 | about a factor of fifteen, the response time constant is reduced to ~2 sec. I 381 | haven't noticed any reduction in solution accuracy. This is essentially the I 382 | coefficient in a PID control sense; the bigger the feedback coefficient, the 383 | faster the solution converges, usually at the expense of accuracy. In any case, 384 | this is the free parameter in the Madgwick filtering and fusion scheme. 385 | 386 | ###### [Jump to Contents](./README.md#contents) 387 | 388 | # 6. References 389 | 390 | [Original Madgwick study](http://sharenet-wii-motion-trac.googlecode.com/files/An_efficient_orientation_filter_for_inertial_and_inertialmagnetic_sensor_arrays.pdf) 391 | [Euler and Tait Bryan angles](http://en.wikipedia.org/wiki/Euler_angles#Tait.E2.80.93Bryan_angles) 392 | [Quaternions to Euler angles](http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles) 393 | [Beta](https://github.com/kriswiner/MPU-9250.git) 394 | -------------------------------------------------------------------------------- /deltat.py: -------------------------------------------------------------------------------- 1 | # deltat.py time difference calculation for sensor fusion 2 | # Released under the MIT License (MIT) 3 | # Copyright (c) 2018 Peter Hinch 4 | 5 | # Provides TimeDiff function and DeltaT class. 6 | # The following notes cover special cases. Where the device performing fusion 7 | # is linked to the IMU and is running MicroPython no special treatment is 8 | # needed. 9 | # The special cases are: 10 | # 1. Device connected to the IMU is linked to a separate platform doing fusion. 11 | # 2. Either or both are not running MicroPython. 12 | 13 | # If the device providing the vectors is not running on MicroPython the user 14 | # must supply timestamps and a function capable of differencing these. The 15 | # function is passed to the Fusion constructor and the timestamp is provided 16 | # along with the vector, being the time when the vector was acquired. 17 | 18 | # If the device providing the vectors is running MicroPython but fusion is 19 | # being performed on a device which is not, the user must provide their own 20 | # implementation of ticks_diff which accounts for MicroPython rollover and 21 | # must supply the returned ticks_us() values as a timestamp. 22 | 23 | # Under MicroPython TimeDiff(start, end) uses time.ticks_diff. 24 | 25 | # A DeltaT instance, called with function call syntax, returns a time 26 | # difference from the previous call as a float value. Units seconds. 27 | 28 | # If running under MicroPython and no time differencing function is supplied 29 | # to the Fusion constructor it uses time.ticks_us as its time source and a 30 | # default timediff function using time.ticks_diff() with a division by 1e6. 31 | # If time differencing function is supplied a timestamp must be passsed as an 32 | # arg to instance calls of Fusion.update() or Fusion.update_nomag(). In the 33 | # async version the user supplied read_coro() must return a timestamp with the 34 | # vector. 35 | 36 | # On 1st pass dt evidently can't be computed. A notional value of 100μs is 37 | # returned. The Madgwick algorithm takes seconds to stabilise. 38 | 39 | try: 40 | import utime as time 41 | except ImportError: 42 | import time 43 | 44 | is_micropython = hasattr(time, 'ticks_diff') 45 | 46 | class DeltaT(): 47 | def __init__(self, timediff): 48 | if timediff is None: 49 | self.expect_ts = False 50 | if is_micropython: 51 | self.timediff = lambda start, end : time.ticks_diff(start, end)/1000000 52 | else: 53 | raise ValueError('You must define a timediff function') 54 | else: 55 | self.expect_ts = True 56 | self.timediff = timediff 57 | self.start_time = None 58 | 59 | def __call__(self, ts): 60 | if self.expect_ts: 61 | if ts is None: 62 | raise ValueError('Timestamp expected but not supplied.') 63 | else: 64 | if is_micropython: 65 | ts = time.ticks_us() 66 | else: 67 | raise RuntimeError('Not MicroPython: provide timestamps and a timediff function') 68 | # ts is now valid 69 | if self.start_time is None: # 1st call: self.start_time is invalid 70 | self.start_time = ts 71 | return 0.0001 # 100μs notional delay. 1st reading is invalid in any case 72 | 73 | dt = self.timediff(ts, self.start_time) 74 | self.start_time = ts 75 | return dt 76 | -------------------------------------------------------------------------------- /fusion.py: -------------------------------------------------------------------------------- 1 | # Sensor fusion for the micropython board. 25th June 2015 2 | # Ported to MicroPython by Peter Hinch. 3 | # Released under the MIT License (MIT) 4 | # Copyright (c) 2017, 2018 Peter Hinch 5 | 6 | # V0.9 Time calculations devolved to deltat.py 7 | # V0.8 Calibrate wait argument can be a function or an integer in ms. 8 | # V0.7 Yaw replaced with heading 9 | # V0.65 waitfunc now optional 10 | 11 | # Supports 6 and 9 degrees of freedom sensors. Tested with InvenSense MPU-9150 9DOF sensor. 12 | # Source https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU.git 13 | # also https://github.com/kriswiner/MPU-9250.git 14 | # Ported to Python. Integrator timing adapted for pyboard. 15 | # See README.md for documentation. 16 | 17 | # Portability: the only assumption is presence of time.sleep() used in mag 18 | # calibration. 19 | try: 20 | import utime as time 21 | except ImportError: 22 | import time 23 | 24 | from math import sqrt, atan2, asin, degrees, radians 25 | from deltat import DeltaT 26 | 27 | class Fusion(object): 28 | ''' 29 | Class provides sensor fusion allowing heading, pitch and roll to be extracted. This uses the Madgwick algorithm. 30 | The update method must be called peiodically. The calculations take 1.6mS on the Pyboard. 31 | ''' 32 | declination = 0 # Optional offset for true north. A +ve value adds to heading 33 | def __init__(self, timediff=None): 34 | self.magbias = (0, 0, 0) # local magnetic bias factors: set from calibration 35 | self.deltat = DeltaT(timediff) # Time between updates 36 | self.q = [1.0, 0.0, 0.0, 0.0] # vector to hold quaternion 37 | GyroMeasError = radians(40) # Original code indicates this leads to a 2 sec response time 38 | self.beta = sqrt(3.0 / 4.0) * GyroMeasError # compute beta (see README) 39 | self.pitch = 0 40 | self.heading = 0 41 | self.roll = 0 42 | 43 | def calibrate(self, getxyz, stopfunc, wait=0): 44 | magmax = list(getxyz()) # Initialise max and min lists with current values 45 | magmin = magmax[:] 46 | while not stopfunc(): 47 | if wait != 0: 48 | if callable(wait): 49 | wait() 50 | else: 51 | time.sleep(wait/1000) # Portable 52 | magxyz = tuple(getxyz()) 53 | for x in range(3): 54 | magmax[x] = max(magmax[x], magxyz[x]) 55 | magmin[x] = min(magmin[x], magxyz[x]) 56 | self.magbias = tuple(map(lambda a, b: (a +b)/2, magmin, magmax)) 57 | 58 | def update_nomag(self, accel, gyro, ts=None): # 3-tuples (x, y, z) for accel, gyro 59 | ax, ay, az = accel # Units G (but later normalised) 60 | gx, gy, gz = (radians(x) for x in gyro) # Units deg/s 61 | q1, q2, q3, q4 = (self.q[x] for x in range(4)) # short name local variable for readability 62 | # Auxiliary variables to avoid repeated arithmetic 63 | _2q1 = 2 * q1 64 | _2q2 = 2 * q2 65 | _2q3 = 2 * q3 66 | _2q4 = 2 * q4 67 | _4q1 = 4 * q1 68 | _4q2 = 4 * q2 69 | _4q3 = 4 * q3 70 | _8q2 = 8 * q2 71 | _8q3 = 8 * q3 72 | q1q1 = q1 * q1 73 | q2q2 = q2 * q2 74 | q3q3 = q3 * q3 75 | q4q4 = q4 * q4 76 | 77 | # Normalise accelerometer measurement 78 | norm = sqrt(ax * ax + ay * ay + az * az) 79 | if (norm == 0): 80 | return # handle NaN 81 | norm = 1 / norm # use reciprocal for division 82 | ax *= norm 83 | ay *= norm 84 | az *= norm 85 | 86 | # Gradient decent algorithm corrective step 87 | s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay 88 | s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az 89 | s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az 90 | s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay 91 | norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4) # normalise step magnitude 92 | s1 *= norm 93 | s2 *= norm 94 | s3 *= norm 95 | s4 *= norm 96 | 97 | # Compute rate of change of quaternion 98 | qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1 99 | qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2 100 | qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3 101 | qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4 102 | 103 | # Integrate to yield quaternion 104 | deltat = self.deltat(ts) 105 | q1 += qDot1 * deltat 106 | q2 += qDot2 * deltat 107 | q3 += qDot3 * deltat 108 | q4 += qDot4 * deltat 109 | norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) # normalise quaternion 110 | self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm 111 | self.heading = 0 112 | self.pitch = degrees(-asin(2.0 * (self.q[1] * self.q[3] - self.q[0] * self.q[2]))) 113 | self.roll = degrees(atan2(2.0 * (self.q[0] * self.q[1] + self.q[2] * self.q[3]), 114 | self.q[0] * self.q[0] - self.q[1] * self.q[1] - self.q[2] * self.q[2] + self.q[3] * self.q[3])) 115 | 116 | def update(self, accel, gyro, mag, ts=None): # 3-tuples (x, y, z) for accel, gyro and mag data 117 | mx, my, mz = (mag[x] - self.magbias[x] for x in range(3)) # Units irrelevant (normalised) 118 | ax, ay, az = accel # Units irrelevant (normalised) 119 | gx, gy, gz = (radians(x) for x in gyro) # Units deg/s 120 | q1, q2, q3, q4 = (self.q[x] for x in range(4)) # short name local variable for readability 121 | # Auxiliary variables to avoid repeated arithmetic 122 | _2q1 = 2 * q1 123 | _2q2 = 2 * q2 124 | _2q3 = 2 * q3 125 | _2q4 = 2 * q4 126 | _2q1q3 = 2 * q1 * q3 127 | _2q3q4 = 2 * q3 * q4 128 | q1q1 = q1 * q1 129 | q1q2 = q1 * q2 130 | q1q3 = q1 * q3 131 | q1q4 = q1 * q4 132 | q2q2 = q2 * q2 133 | q2q3 = q2 * q3 134 | q2q4 = q2 * q4 135 | q3q3 = q3 * q3 136 | q3q4 = q3 * q4 137 | q4q4 = q4 * q4 138 | 139 | # Normalise accelerometer measurement 140 | norm = sqrt(ax * ax + ay * ay + az * az) 141 | if (norm == 0): 142 | return # handle NaN 143 | norm = 1 / norm # use reciprocal for division 144 | ax *= norm 145 | ay *= norm 146 | az *= norm 147 | 148 | # Normalise magnetometer measurement 149 | norm = sqrt(mx * mx + my * my + mz * mz) 150 | if (norm == 0): 151 | return # handle NaN 152 | norm = 1 / norm # use reciprocal for division 153 | mx *= norm 154 | my *= norm 155 | mz *= norm 156 | 157 | # Reference direction of Earth's magnetic field 158 | _2q1mx = 2 * q1 * mx 159 | _2q1my = 2 * q1 * my 160 | _2q1mz = 2 * q1 * mz 161 | _2q2mx = 2 * q2 * mx 162 | hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4 163 | hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4 164 | _2bx = sqrt(hx * hx + hy * hy) 165 | _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4 166 | _4bx = 2 * _2bx 167 | _4bz = 2 * _2bz 168 | 169 | # Gradient descent algorithm corrective step 170 | s1 = (-_2q3 * (2 * q2q4 - _2q1q3 - ax) + _2q2 * (2 * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5 - q3q3 - q4q4) 171 | + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) 172 | + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)) 173 | 174 | s2 = (_2q4 * (2 * q2q4 - _2q1q3 - ax) + _2q1 * (2 * q1q2 + _2q3q4 - ay) - 4 * q2 * (1 - 2 * q2q2 - 2 * q3q3 - az) 175 | + _2bz * q4 * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) 176 | + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)) 177 | 178 | s3 = (-_2q1 * (2 * q2q4 - _2q1q3 - ax) + _2q4 * (2 * q1q2 + _2q3q4 - ay) - 4 * q3 * (1 - 2 * q2q2 - 2 * q3q3 - az) 179 | + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) 180 | + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) 181 | + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)) 182 | 183 | s4 = (_2q2 * (2 * q2q4 - _2q1q3 - ax) + _2q3 * (2 * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5 - q3q3 - q4q4) 184 | + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) 185 | + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)) 186 | 187 | norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4) # normalise step magnitude 188 | s1 *= norm 189 | s2 *= norm 190 | s3 *= norm 191 | s4 *= norm 192 | 193 | # Compute rate of change of quaternion 194 | qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1 195 | qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2 196 | qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3 197 | qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4 198 | 199 | # Integrate to yield quaternion 200 | deltat = self.deltat(ts) 201 | q1 += qDot1 * deltat 202 | q2 += qDot2 * deltat 203 | q3 += qDot3 * deltat 204 | q4 += qDot4 * deltat 205 | norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) # normalise quaternion 206 | self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm 207 | self.heading = self.declination + degrees(atan2(2.0 * (self.q[1] * self.q[2] + self.q[0] * self.q[3]), 208 | self.q[0] * self.q[0] + self.q[1] * self.q[1] - self.q[2] * self.q[2] - self.q[3] * self.q[3])) 209 | self.pitch = degrees(-asin(2.0 * (self.q[1] * self.q[3] - self.q[0] * self.q[2]))) 210 | self.roll = degrees(atan2(2.0 * (self.q[0] * self.q[1] + self.q[2] * self.q[3]), 211 | self.q[0] * self.q[0] - self.q[1] * self.q[1] - self.q[2] * self.q[2] + self.q[3] * self.q[3])) 212 | -------------------------------------------------------------------------------- /fusion_async.py: -------------------------------------------------------------------------------- 1 | # fusion_async.py Asynchronous sensor fusion for micropython targets. 2 | # Ported to MicroPython by Peter Hinch, May 2017. 3 | # Released under the MIT License (MIT) See LICENSE 4 | # Copyright (c) 2017-2020 Peter Hinch 5 | 6 | # Requires: 7 | # uasyncio V3 (Included in daily builds and release builds later than V1.12). 8 | # Uses the uasyncio library to enable updating to run as a background coroutine. 9 | 10 | # Supports 6 and 9 degrees of freedom sensors. Tested with InvenSense MPU-9150 9DOF sensor. 11 | # Source https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU.git 12 | # also https://github.com/kriswiner/MPU-9250.git 13 | # Ported to Python. Integrator timing adapted for pyboard. 14 | # See README.md for documentation. 15 | 16 | # V0.9 Time calculations devolved to deltat.py 17 | 18 | try: 19 | import uasyncio as asyncio 20 | except ImportError: 21 | import asyncio 22 | from math import sqrt, atan2, asin, degrees, radians 23 | from deltat import DeltaT 24 | 25 | class Fusion(object): 26 | ''' 27 | Class provides sensor fusion allowing heading, pitch and roll to be extracted. This uses the Madgwick algorithm. 28 | The update method runs as a coroutine. Its calculations take 1.6mS on the Pyboard. 29 | ''' 30 | declination = 0 # Optional offset for true north. A +ve value adds to heading 31 | def __init__(self, read_coro, timediff=None): 32 | self.read_coro = read_coro 33 | self.magbias = (0, 0, 0) # local magnetic bias factors: set from calibration 34 | self.expect_ts = timediff is not None 35 | self.deltat = DeltaT(timediff) # Time between updates 36 | self.q = [1.0, 0.0, 0.0, 0.0] # vector to hold quaternion 37 | GyroMeasError = radians(40) # Original code indicates this leads to a 2 sec response time 38 | self.beta = sqrt(3.0 / 4.0) * GyroMeasError # compute beta (see README) 39 | self.pitch = 0 40 | self.heading = 0 41 | self.roll = 0 42 | 43 | async def calibrate(self, stopfunc): 44 | res = await self.read_coro() 45 | mag = res[2] 46 | magmax = list(mag) # Initialise max and min lists with current values 47 | magmin = magmax[:] 48 | while not stopfunc(): 49 | res = await self.read_coro() 50 | magxyz = res[2] 51 | for x in range(3): 52 | magmax[x] = max(magmax[x], magxyz[x]) 53 | magmin[x] = min(magmin[x], magxyz[x]) 54 | self.magbias = tuple(map(lambda a, b: (a +b)/2, magmin, magmax)) 55 | 56 | async def start(self, slow_platform=False): 57 | data = await self.read_coro() 58 | if len(data) == 2 or (self.expect_ts and len(data) == 3): 59 | asyncio.create_task(self._update_nomag(slow_platform)) 60 | else: 61 | asyncio.create_task(self._update_mag(slow_platform)) 62 | 63 | async def _update_nomag(self, slow_platform): 64 | while True: 65 | if self.expect_ts: 66 | accel, gyro, ts = await self.read_coro() 67 | else: 68 | accel, gyro = await self.read_coro() 69 | ts = None 70 | ax, ay, az = accel # Units G (but later normalised) 71 | gx, gy, gz = (radians(x) for x in gyro) # Units deg/s 72 | q1, q2, q3, q4 = (self.q[x] for x in range(4)) # short name local variable for readability 73 | # Auxiliary variables to avoid repeated arithmetic 74 | _2q1 = 2 * q1 75 | _2q2 = 2 * q2 76 | _2q3 = 2 * q3 77 | _2q4 = 2 * q4 78 | _4q1 = 4 * q1 79 | _4q2 = 4 * q2 80 | _4q3 = 4 * q3 81 | _8q2 = 8 * q2 82 | _8q3 = 8 * q3 83 | q1q1 = q1 * q1 84 | q2q2 = q2 * q2 85 | q3q3 = q3 * q3 86 | q4q4 = q4 * q4 87 | 88 | # Normalise accelerometer measurement 89 | norm = sqrt(ax * ax + ay * ay + az * az) 90 | if (norm == 0): 91 | return # handle NaN 92 | norm = 1 / norm # use reciprocal for division 93 | ax *= norm 94 | ay *= norm 95 | az *= norm 96 | 97 | # Gradient decent algorithm corrective step 98 | s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay 99 | s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az 100 | s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az 101 | s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay 102 | norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4) # normalise step magnitude 103 | s1 *= norm 104 | s2 *= norm 105 | s3 *= norm 106 | s4 *= norm 107 | 108 | # Compute rate of change of quaternion 109 | qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1 110 | qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2 111 | qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3 112 | qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4 113 | 114 | if slow_platform: 115 | await asyncio.sleep_ms(0) 116 | 117 | # Integrate to yield quaternion 118 | deltat = self.deltat(ts) 119 | q1 += qDot1 * deltat 120 | q2 += qDot2 * deltat 121 | q3 += qDot3 * deltat 122 | q4 += qDot4 * deltat 123 | norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) # normalise quaternion 124 | self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm 125 | self.heading = 0 # Meaningless without a magnetometer 126 | self.pitch = degrees(-asin(2.0 * (self.q[1] * self.q[3] - self.q[0] * self.q[2]))) 127 | self.roll = degrees(atan2(2.0 * (self.q[0] * self.q[1] + self.q[2] * self.q[3]), 128 | self.q[0] * self.q[0] - self.q[1] * self.q[1] - self.q[2] * self.q[2] + self.q[3] * self.q[3])) 129 | 130 | async def _update_mag(self, slow_platform): 131 | while True: 132 | if self.expect_ts: 133 | accel, gyro, mag, ts = await self.read_coro() 134 | else: 135 | accel, gyro, mag = await self.read_coro() 136 | ts = None 137 | mx, my, mz = (mag[x] - self.magbias[x] for x in range(3)) # Units irrelevant (normalised) 138 | ax, ay, az = accel # Units irrelevant (normalised) 139 | gx, gy, gz = (radians(x) for x in gyro) # Units deg/s 140 | q1, q2, q3, q4 = (self.q[x] for x in range(4)) # short name local variable for readability 141 | # Auxiliary variables to avoid repeated arithmetic 142 | _2q1 = 2 * q1 143 | _2q2 = 2 * q2 144 | _2q3 = 2 * q3 145 | _2q4 = 2 * q4 146 | _2q1q3 = 2 * q1 * q3 147 | _2q3q4 = 2 * q3 * q4 148 | q1q1 = q1 * q1 149 | q1q2 = q1 * q2 150 | q1q3 = q1 * q3 151 | q1q4 = q1 * q4 152 | q2q2 = q2 * q2 153 | q2q3 = q2 * q3 154 | q2q4 = q2 * q4 155 | q3q3 = q3 * q3 156 | q3q4 = q3 * q4 157 | q4q4 = q4 * q4 158 | 159 | # Normalise accelerometer measurement 160 | norm = sqrt(ax * ax + ay * ay + az * az) 161 | if (norm == 0): 162 | return # handle NaN 163 | norm = 1 / norm # use reciprocal for division 164 | ax *= norm 165 | ay *= norm 166 | az *= norm 167 | 168 | # Normalise magnetometer measurement 169 | norm = sqrt(mx * mx + my * my + mz * mz) 170 | if (norm == 0): 171 | return # handle NaN 172 | norm = 1 / norm # use reciprocal for division 173 | mx *= norm 174 | my *= norm 175 | mz *= norm 176 | 177 | # Reference direction of Earth's magnetic field 178 | _2q1mx = 2 * q1 * mx 179 | _2q1my = 2 * q1 * my 180 | _2q1mz = 2 * q1 * mz 181 | _2q2mx = 2 * q2 * mx 182 | hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4 183 | hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4 184 | _2bx = sqrt(hx * hx + hy * hy) 185 | _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4 186 | _4bx = 2 * _2bx 187 | _4bz = 2 * _2bz 188 | 189 | # Gradient descent algorithm corrective step 190 | s1 = (-_2q3 * (2 * q2q4 - _2q1q3 - ax) + _2q2 * (2 * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5 - q3q3 - q4q4) 191 | + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) 192 | + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)) 193 | 194 | s2 = (_2q4 * (2 * q2q4 - _2q1q3 - ax) + _2q1 * (2 * q1q2 + _2q3q4 - ay) - 4 * q2 * (1 - 2 * q2q2 - 2 * q3q3 - az) 195 | + _2bz * q4 * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) 196 | + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)) 197 | 198 | if slow_platform: 199 | await asyncio.sleep_ms(0) 200 | 201 | s3 = (-_2q1 * (2 * q2q4 - _2q1q3 - ax) + _2q4 * (2 * q1q2 + _2q3q4 - ay) - 4 * q3 * (1 - 2 * q2q2 - 2 * q3q3 - az) 202 | + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) 203 | + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) 204 | + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)) 205 | 206 | s4 = (_2q2 * (2 * q2q4 - _2q1q3 - ax) + _2q3 * (2 * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5 - q3q3 - q4q4) 207 | + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) 208 | + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)) 209 | 210 | norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4) # normalise step magnitude 211 | s1 *= norm 212 | s2 *= norm 213 | s3 *= norm 214 | s4 *= norm 215 | 216 | # Compute rate of change of quaternion 217 | qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1 218 | qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2 219 | qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3 220 | qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4 221 | 222 | # Integrate to yield quaternion 223 | deltat = self.deltat(ts) 224 | q1 += qDot1 * deltat 225 | q2 += qDot2 * deltat 226 | q3 += qDot3 * deltat 227 | q4 += qDot4 * deltat 228 | norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) # normalise quaternion 229 | self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm 230 | self.heading = self.declination + degrees(atan2(2.0 * (self.q[1] * self.q[2] + self.q[0] * self.q[3]), 231 | self.q[0] * self.q[0] + self.q[1] * self.q[1] - self.q[2] * self.q[2] - self.q[3] * self.q[3])) 232 | self.pitch = degrees(-asin(2.0 * (self.q[1] * self.q[3] - self.q[0] * self.q[2]))) 233 | self.roll = degrees(atan2(2.0 * (self.q[0] * self.q[1] + self.q[2] * self.q[3]), 234 | self.q[0] * self.q[0] - self.q[1] * self.q[1] - self.q[2] * self.q[2] + self.q[3] * self.q[3])) 235 | -------------------------------------------------------------------------------- /fusionlcd.py: -------------------------------------------------------------------------------- 1 | # fusionlcd.py Test for asynchronous sensor fusion on Pyboard. Uses LCD display and uasyncio. 2 | # Author: Peter Hinch 3 | # Released under the MIT License (MIT) See LICENSE 4 | # Copyright (c) 2017-2020 Peter Hinch 5 | 6 | # Requires: 7 | # uasyncio V3 (Included in daily builds and release builds later than V1.12). 8 | # MPU9150 on X position 9 | # Normally open pushbutton connected between pin Y7 and ground 10 | # LCD driver alcd.py uasyncio V3 version from 11 | # https://github.com/peterhinch/micropython-async/blob/master/v3/as_drivers/hd44780/alcd.py 12 | # From https://github.com/micropython-IMU/micropython-mpu9x50: 13 | # imu.py, mpu9150.py, vector3d.py 14 | # From this repo: deltat.py fusion_async.py 15 | 16 | # Hitachi HD44780 2 row LCD display wired using 4 bit data bus as follows: 17 | 18 | # Name LCD connector Board 19 | # Rs 4 1 red Y1 20 | # E 6 2 Y2 21 | # D7 14 3 Y3 22 | # D6 13 4 Y4 23 | # D5 12 5 Y5 24 | # D4 11 6 Y6 25 | 26 | from machine import Pin 27 | import uasyncio as asyncio 28 | import gc 29 | from mpu9150 import MPU9150 30 | from fusion_async import Fusion # Using async version 31 | from alcd import LCD, PINLIST # Library supporting Hitachi LCD module 32 | 33 | switch = Pin('Y7', Pin.IN, pull=Pin.PULL_UP) # Switch to ground on Y7 34 | 35 | imu = MPU9150('X') # Attached to 'X' bus, 1 device, disable interruots 36 | 37 | lcd = LCD(PINLIST, cols = 24) # Should work with 16 column LCD 38 | 39 | # User coro returns data and determines update rate. 40 | # For 9DOF sensors returns three 3-tuples (x, y, z) for accel, gyro and mag 41 | # For 6DOF sensors two 3-tuples (x, y, z) for accel and gyro 42 | async def read_coro(): 43 | imu.mag_trigger() 44 | await asyncio.sleep_ms(20) # Plenty of time for mag to be ready 45 | return imu.accel.xyz, imu.gyro.xyz, imu.mag_nonblocking.xyz 46 | 47 | fuse = Fusion(read_coro) 48 | 49 | async def mem_manage(): # Necessary for long term stability 50 | while True: 51 | await asyncio.sleep_ms(100) 52 | gc.collect() 53 | gc.threshold(gc.mem_free() // 4 + gc.mem_alloc()) 54 | 55 | async def display(): 56 | lcd[0] = "{:5s}{:5s} {:5s}".format("Yaw","Pitch","Roll") 57 | while True: 58 | lcd[1] = "{:4.0f} {:4.0f} {:4.0f}".format(fuse.heading, fuse.pitch, fuse.roll) 59 | await asyncio.sleep_ms(500) 60 | 61 | async def lcd_task(): 62 | print('Running test...') 63 | if switch.value() == 1: 64 | lcd[0] = "Calibrate. Push switch" 65 | lcd[1] = "when done" 66 | await asyncio.sleep_ms(100) # Let LCD coro run 67 | await fuse.calibrate(lambda : not switch.value()) 68 | print(fuse.magbias) 69 | await fuse.start() # Start the update task 70 | loop = asyncio.get_event_loop() 71 | loop.create_task(display()) 72 | 73 | loop = asyncio.get_event_loop() 74 | loop.create_task(mem_manage()) 75 | loop.create_task(lcd_task()) 76 | loop.run_forever() 77 | -------------------------------------------------------------------------------- /fusiontest.py: -------------------------------------------------------------------------------- 1 | # fusiontest.py Simple test program for sensor fusion on Pyboard 2 | # Author Peter Hinch 3 | # Released under the MIT License (MIT) 4 | # Copyright (c) 2017 Peter Hinch 5 | # V0.8 14th May 2017 Option for external switch for cal test. Make platform independent. 6 | # V0.7 25th June 2015 Adapted for new MPU9x50 interface 7 | 8 | from machine import Pin 9 | import pyb 10 | import utime as time 11 | from mpu9150 import MPU9150 12 | from fusion import Fusion 13 | 14 | imu = MPU9150('X') 15 | 16 | fuse = Fusion() 17 | 18 | # Code for external switch 19 | switch = Pin('Y7', Pin.IN, pull=Pin.PULL_UP) # Switch to ground on Y7 20 | def sw(): 21 | return not switch.value() 22 | # Code for Pyboard switch 23 | #sw = pyb.Switch() 24 | 25 | # Choose test to run 26 | Calibrate = True 27 | Timing = True 28 | 29 | def getmag(): # Return (x, y, z) tuple (blocking read) 30 | return imu.mag.xyz 31 | 32 | if Calibrate: 33 | print("Calibrating. Press switch when done.") 34 | fuse.calibrate(getmag, sw, lambda : pyb.delay(100)) 35 | print(fuse.magbias) 36 | 37 | if Timing: 38 | mag = imu.mag.xyz # Don't include blocking read in time 39 | accel = imu.accel.xyz # or i2c 40 | gyro = imu.gyro.xyz 41 | start = time.ticks_us() # Measure computation time only 42 | fuse.update(accel, gyro, mag) # 1.97mS on Pyboard 43 | t = time.ticks_diff(time.ticks_us(), start) 44 | print("Update time (uS):", t) 45 | 46 | count = 0 47 | while True: 48 | fuse.update(imu.accel.xyz, imu.gyro.xyz, imu.mag.xyz) # Note blocking mag read 49 | if count % 50 == 0: 50 | print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(fuse.heading, fuse.pitch, fuse.roll)) 51 | time.sleep_ms(20) 52 | count += 1 53 | -------------------------------------------------------------------------------- /fusiontest6.py: -------------------------------------------------------------------------------- 1 | # fusiontest6.py Simple test program for 6DOF sensor fusion on Pyboard 2 | # Author Peter Hinch 3 | # Released under the MIT License (MIT) 4 | # Copyright (c) 2017 Peter Hinch 5 | # V0.8 14th May 2017 Option for external switch for cal test. Make platform independent. 6 | # V0.7 25th June 2015 Adapted for new MPU9x50 interface 7 | 8 | from machine import Pin 9 | import utime as time 10 | from mpu9150 import MPU9150 11 | from fusion import Fusion 12 | 13 | imu = MPU9150('X') 14 | 15 | fuse = Fusion() 16 | 17 | # Choose test to run 18 | Timing = True 19 | 20 | if Timing: 21 | accel = imu.accel.xyz 22 | gyro = imu.gyro.xyz 23 | start = time.ticks_us() # Measure computation time only 24 | fuse.update_nomag(accel, gyro) # 979μs on Pyboard 25 | t = time.ticks_diff(time.ticks_us(), start) 26 | print("Update time (uS):", t) 27 | 28 | count = 0 29 | while True: 30 | fuse.update_nomag(imu.accel.xyz, imu.gyro.xyz) 31 | if count % 50 == 0: 32 | print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(fuse.heading, fuse.pitch, fuse.roll)) 33 | time.sleep_ms(20) 34 | count += 1 35 | -------------------------------------------------------------------------------- /fusiontest_as.py: -------------------------------------------------------------------------------- 1 | # fusiontest_as.py Test for asynchronous sensor fusion on Pyboard. 2 | # Author: Peter Hinch 3 | # Released under the MIT License (MIT) See LICENSE 4 | # Copyright (c) 2017-2020 Peter Hinch 5 | 6 | # Requires: 7 | # uasyncio V3 (Included in daily builds and release builds later than V1.12). 8 | # From https://github.com/micropython-IMU/micropython-mpu9x50: 9 | # imu.py, mpu9150.py, vector3d.py 10 | # From this repo: deltat.py fusion_async.py 11 | 12 | # MPU9150 on X position 13 | # Normally open pushbutton connected between pin Y7 and ground 14 | 15 | from machine import Pin 16 | import uasyncio as asyncio 17 | import gc 18 | from mpu9150 import MPU9150 19 | from fusion_async import Fusion # Using async version 20 | 21 | switch = Pin('Y7', Pin.IN, pull=Pin.PULL_UP) # Switch to ground on Y7 22 | 23 | imu = MPU9150('X') # Attached to 'X' bus, 1 device, disable interrupts 24 | 25 | # User coro returns data and determines update rate. 26 | # For 9DOF sensors returns three 3-tuples (x, y, z) for accel, gyro and mag 27 | async def read_coro(): 28 | imu.mag_trigger() 29 | await asyncio.sleep_ms(20) # Plenty of time for mag to be ready 30 | return imu.accel.xyz, imu.gyro.xyz, imu.mag_nonblocking.xyz 31 | 32 | fuse = Fusion(read_coro) 33 | 34 | async def mem_manage(): # Necessary for long term stability 35 | while True: 36 | await asyncio.sleep_ms(100) 37 | gc.collect() 38 | gc.threshold(gc.mem_free() // 4 + gc.mem_alloc()) 39 | 40 | async def display(): 41 | fs = 'Heading: {:4.0f} Pitch: {:4.0f} Roll: {:4.0f}' 42 | while True: 43 | print(fs.format(fuse.heading, fuse.pitch, fuse.roll)) 44 | await asyncio.sleep_ms(500) 45 | 46 | async def test_task(): 47 | if switch.value() == 1: 48 | print('Calibrate. Push switch when done.') 49 | await fuse.calibrate(lambda : not switch.value()) 50 | print('Mag bias vector: ', fuse.magbias) 51 | await fuse.start() # Start the update task 52 | await display() 53 | 54 | 55 | asyncio.create_task(mem_manage()) 56 | asyncio.run(test_task()) 57 | -------------------------------------------------------------------------------- /fusiontest_as6.py: -------------------------------------------------------------------------------- 1 | # fusiontest_as6.py Test for 6DOF asynchronous sensor fusion on Pyboard. 2 | # Author: Peter Hinch 3 | # Released under the MIT License (MIT) See LICENSE 4 | # Copyright (c) 2017-2020 Peter Hinch 5 | 6 | # Requires: 7 | # uasyncio V3 (Included in daily builds and release builds later than V1.12). 8 | # From https://github.com/micropython-IMU/micropython-mpu9x50: 9 | # imu.py, mpu6050.py, vector3d.py 10 | # From this repo: deltat.py fusion_async.py 11 | # MPU9150 on X position 12 | 13 | from machine import Pin 14 | import uasyncio as asyncio 15 | import gc 16 | from imu import MPU6050 17 | from fusion_async import Fusion # Using async version 18 | 19 | imu = MPU6050('X') # Attached to 'X' bus, 1 device, disable interrupts 20 | 21 | # User coro returns data and determines update rate. 22 | # For 6DOF sensors two 3-tuples (x, y, z) for accel and gyro 23 | async def read_coro(): 24 | await asyncio.sleep_ms(20) # Plenty of time for mag to be ready 25 | return imu.accel.xyz, imu.gyro.xyz 26 | 27 | fuse = Fusion(read_coro) 28 | 29 | async def mem_manage(): # Necessary for long term stability 30 | while True: 31 | await asyncio.sleep_ms(100) 32 | gc.collect() 33 | gc.threshold(gc.mem_free() // 4 + gc.mem_alloc()) 34 | 35 | async def display(): 36 | fs = 'Heading: {:4.0f} Pitch: {:4.0f} Roll: {:4.0f}' 37 | while True: 38 | print(fs.format(fuse.heading, fuse.pitch, fuse.roll)) 39 | await asyncio.sleep_ms(500) 40 | 41 | async def test_task(): 42 | await fuse.start() # Start the update task 43 | loop = asyncio.get_event_loop() 44 | await display() 45 | 46 | 47 | asyncio.create_task(mem_manage()) 48 | asyncio.run(test_task()) 49 | -------------------------------------------------------------------------------- /orientate.py: -------------------------------------------------------------------------------- 1 | # orientate.py Utility for adjusting orientation of an IMU for sensor fusion 2 | # Author Peter Hinch 3 | # Released under the MIT License (MIT) 4 | # Copyright (c) 2015 Peter Hinch 5 | 6 | # Orientation. 7 | # The convention for vehicle axes is 8 | # z Vertical axis, vector points towards ground 9 | # x Axis towards the front of the vehicle, vector points in normal direction of travel 10 | # y Vector points left from pilot's point of view (I think) 11 | # Assuming the sensor is aligned such that it has one axis Vertical and another forward, the 12 | # transpose function enables axes to be swapped to achieve the conventional orientation. 13 | # The invert() function enables the sign of each axis to be corrected. 14 | 15 | # Note that it is the responsibilty of the driver to esnure that accelerometer, gyro and 16 | # magnetometer share the same coordinate system. In particular the MPU-9150's magnetometer is 17 | # rotated with respect to the other two sensors. The MicroPython driver transposes the magnetometer 18 | # axes to conform with those of the other two sensors. 19 | 20 | 21 | def invert(axes, vectors): # Invert one or more axes on a list of vectors 22 | res = [] 23 | for vector in vectors: 24 | res.append(tuple(map(lambda x, y: -y if x else y, axes, vector))) 25 | return res 26 | 27 | def transpose(idx, vectors):# transpose axes in a list of vectors 28 | res = [] # e.g. swap x and y transpose((1, 0, 2), (accel, gyro, mag)) 29 | for vector in vectors: 30 | res.append([vector[axis] for axis in idx]) 31 | return res 32 | 33 | # t: tuple of vectors to transpose (0, 1, 2) indicates do nothing 34 | # i: tuple of booleans indicating any vectors to be inverted. 35 | # folowed by accel, gyro and magnetometer vectors (x, y, z). 36 | # Note inversion operates on conventional vectors i.e. after transposition. 37 | # e.g. (False, False, True) invert vertical axis. 38 | # Returns a list of vectors. Typical invocation: 39 | # fuse.update(*orientate(T, I, imu.get_accel(), imu.get_gyro(), imu.get_mag())) 40 | 41 | def orientate(t, i, *vecs): 42 | return invert(i, transpose(t, vecs)) 43 | -------------------------------------------------------------------------------- /remote/README.md: -------------------------------------------------------------------------------- 1 | # 1. Sensor Fusion on generic devices 2 | 3 | This library was written for MicroPython but can be used on any device capable 4 | of running Python V3.4 or above (V3.5 or above for the asynchronous version). 5 | In its default mode as described in the [main README](../README.md) it uses 6 | specific MicroPython calls to handle precision time values. 7 | 8 | On other platforms the application must construct a `Fusion` instance in a way 9 | to prevent it doing this. The application must provide a precision time value 10 | representing the instant that the vectors were acquired. It must also provide a 11 | function capable of differencing a pair of these; this is typically merely 12 | subtraction and scaling. 13 | 14 | [Main README](../README.md) 15 | 16 | # 2. Files 17 | 18 | 1. `mpudata` A set of data captured from a Pyboard performing calibration 19 | followed by movement around each axis in turn. 20 | 2. `capture` The program used to create the above dataset. 21 | 3. `fusion_r_syn` Synchronous test program using the dataset. 22 | 4. `fusion_r_asyn` Asynchronous test program using the dataset. 23 | 24 | The test programs perform a calibration phase during which the device was fully 25 | rotated around each orthogonal axis. They then display the data as the device 26 | was partially rotated around each axis in turn. 27 | 28 | Test programs include comments indicating how synchronous and asynchronous 29 | applications might be developed. 30 | 31 | # 3. Remote Mode 32 | 33 | This caters for the case where a device acquires IMU data and returns that data 34 | to another platform performing the fusion. The remote device may run any OS (or 35 | none), run any programming language and return data in any format. It must 36 | return two or three vectors (depending on whether it is a 6DOF or 9DOF device) 37 | and a timestamp indicating when the vectors were acquired. 38 | 39 | The platform performing the fusion may run any Python version as described in 40 | [section 1](./README.md#1-sensor-fusion-on-generic-devices). 41 | 42 | The remote device must return a timestamp along with the IMU data, representing 43 | the time when the IMU data was acquired. This avoids inaccuracy in the fusion 44 | calculation caused by latency in the communications link. The format of the 45 | timestamp is arbitrary, but the user program must provide a function for 46 | calculating the difference between two timestamps. 47 | 48 | User code on the device performing the fusion must convert the data received 49 | from the remote to the format acceptable to the fusion module, namely 50 | 51 | ```python 52 | [[ax, ay, az], [gx, gy, gz], [mx, my, mz], timestamp] 53 | ``` 54 | 55 | where an, gn, mn are accelerometer, gyro, and magnetometer vectors. The axes 56 | of these readings must match. 57 | 58 | ## 3.1 Remote program design 59 | 60 | This needs to supply data at a rate dependent on the application. To track fast 61 | moving platforms the repetition rate may need to be as fast as 10ms. 62 | 63 | Format is user-dependent. If you have a choice consider a JSON encoded list as 64 | detailed above: since this matches the call signature of the fusion code it is 65 | trivial to convert. 66 | 67 | # 4. Fusion application design 68 | 69 | ## 4.1 Timestamps 70 | 71 | The library requires a function which accepts as arguments two timestamps and 72 | returns a time difference in seconds as a floating point value. In the case 73 | where the acquisition and fusion devices both run MicroPython, the acquisition 74 | device can transmit values from `utime.ticks_us()` and the differnencing 75 | function passed to the Fusion constructor would be 76 | 77 | ```python 78 | lambda start, end: utime.ticks_diff(start, end)/1000000 79 | ``` 80 | 81 | In other cases if timestamps roll over modulo N, the differencing function must 82 | accommodate this. In more usual case where times are floating point values 83 | without rollover it may simply be a matter of scaling: 84 | 85 | ```python 86 | def TimeDiff(start, end): # Timestamps here are in μs 87 | return (start - end)/1000000 # Scale to seconds 88 | ``` 89 | 90 | If standard Python's `time.time()` provides the timestamps no scaling is 91 | required. The function can reduce to: 92 | 93 | ```python 94 | lambda start, end: start-end 95 | ``` 96 | 97 | ## 4.2 Calibration 98 | 99 | If magnetometer calibration is to be used the fusion program needs some form of 100 | input to tell it that manual rotation of the target about each axis is 101 | complete. 102 | 103 | In the case of remote applications this might be sourced by user input to the 104 | remote or to the fusion device. In the test data a button on the remote was 105 | pressed and this is stored in the file, simulating communication with the host. 106 | 107 | ## 4.3 Synchronous applications 108 | 109 | The `Fusion` constructor should be instantiated as follows: 110 | 111 | ```python 112 | from fusion import Fusion 113 | fuse = Fusion(TimeDiff) 114 | ``` 115 | 116 | The arg is the user supplied time differencing function. 117 | 118 | For details of the method of updating the fusion data and retrieving the angles 119 | see the [main document section 2](../README.md#2-fusion-module). 120 | 121 | ### 4.3.1 Test program fusion_r_syn.py 122 | 123 | This encapsulates data acquisition from the target in the generator function 124 | `gdata`. The resultant generator `get_data` reads a line of data from the 125 | target, decodes the JSON string, and yields a list of form 126 | 127 | ``` 128 | [[ax, ay, az], [gx, gy, gz], [mx, my, mz], timestamp] 129 | ``` 130 | 131 | Code is included to handle the calibration mode: in the test file completion is 132 | flagged by a special record created when a button on the device was pressed. 133 | The handling of the `StopIteration` exception is required because the data 134 | source is a file of finite length. 135 | 136 | ## 4.4 Asynchronous applications 137 | 138 | The `Fusion` constructor should be instantiated as follows: 139 | 140 | ```python 141 | from fusion_async import Fusion 142 | fuse = Fusion(read_coro, TimeDiff) 143 | ``` 144 | 145 | The first argument is a coroutine. For 9DOF sensors this must be designed to 146 | return three (x, y, z) 3-tuples for accelerometer, gyro, and magnetometer data 147 | respectively followed by a timestamp. In the case of 6DOF sensors it returns 148 | two 3-tuples for accelerometer and gyro followed by a timestamp. The coroutine 149 | must include at least one `await` statement to conform to Python syntax rules. 150 | 151 | The second arg is the user supplied time differencing function. 152 | 153 | For details of the method of updating the fusion data and retrieving the angles 154 | see the [main document section 3](../README.md#3-asynchronous-version). 155 | 156 | ### 4.4.1 Test program fusion_r_asyn.py 157 | 158 | This encapsulates data acquisition from the target in the class `GetData`. In 159 | principle the constructor initiates communications with the (local or remote) 160 | IMU. The asynchronous `read` method acquires a record from the IMU, converts it 161 | to the correct format, and returns a list of form 162 | 163 | ``` 164 | [[ax, ay, az], [gx, gy, gz], [mx, my, mz], timestamp] 165 | ``` 166 | 167 | Code is included to handle the calibration mode: in the test file completion is 168 | flagged by a special record created when a button on the device was pressed. 169 | Further code handles the fact that the test fileis of finite length. 170 | 171 | [Main README](../README.md) 172 | -------------------------------------------------------------------------------- /remote/capture.py: -------------------------------------------------------------------------------- 1 | # capture.py Data capture for remote operation. Uses LCD display and uasyncio. 2 | # Author: Peter Hinch 3 | # Released under the MIT License (MIT) See LICENSE 4 | # Copyright (c) 2017-2020 Peter Hinch 5 | 6 | # Requires: 7 | # uasyncio V3 (Included in daily builds and release builds later than V1.12). 8 | # MPU9150 on X position 9 | # Normally open pushbutton connected between pin Y7 and ground 10 | # LCD driver alcd.py uasyncio V3 version from 11 | # https://github.com/peterhinch/micropython-async/blob/master/v3/as_drivers/hd44780/alcd.py 12 | # Hitachi HD44780 2 row LCD display wired using 4 bit data bus as follows: 13 | 14 | # Name LCD connector Board 15 | # Rs 4 1 red Y1 16 | # E 6 2 Y2 17 | # D7 14 3 Y3 18 | # D6 13 4 Y4 19 | # D5 12 5 Y5 20 | # D4 11 6 Y6 21 | 22 | from machine import Pin 23 | import uasyncio as asyncio 24 | import ujson 25 | import utime as time 26 | import gc 27 | from mpu9150 import MPU9150 28 | from fusion_async import Fusion # Using async version 29 | from alcd import LCD, PINLIST # Library supporting Hitachi LCD module 30 | 31 | switch = Pin('Y7', Pin.IN, pull=Pin.PULL_UP) # Switch to ground on Y7 32 | 33 | imu = MPU9150('X') # Attached to 'X' bus, 1 device, disable interruots 34 | 35 | lcd = LCD(PINLIST, cols = 24) # Should work with 16 column LCD 36 | 37 | f = open('/sd/mpudata', 'w') 38 | 39 | async def read_coro(): 40 | imu.mag_trigger() 41 | await asyncio.sleep_ms(20) # Plenty of time for mag to be ready 42 | f.write(ujson.dumps([imu.accel.xyz, imu.gyro.xyz, imu.mag_nonblocking.xyz, time.ticks_us()])) 43 | f.write('\n') 44 | return imu.accel.xyz, imu.gyro.xyz, imu.mag_nonblocking.xyz 45 | 46 | fuse = Fusion(read_coro) 47 | 48 | async def mem_manage(): # Necessary for long term stability 49 | while True: 50 | await asyncio.sleep_ms(100) 51 | gc.collect() 52 | gc.threshold(gc.mem_free() // 4 + gc.mem_alloc()) 53 | 54 | async def display(): 55 | lcd[0] = "{:5s}{:5s} {:5s}".format("Yaw","Pitch","Roll") 56 | while not switch.value(): 57 | lcd[1] = "{:4.0f} {:4.0f} {:4.0f}".format(fuse.heading, fuse.pitch, fuse.roll) 58 | await asyncio.sleep_ms(500) 59 | f.close() 60 | return 61 | 62 | async def lcd_task(): 63 | print('Running test...') 64 | if switch.value() == 1: 65 | lcd[0] = "Calibrate. Push switch" 66 | lcd[1] = "when done" 67 | await asyncio.sleep_ms(100) # Let LCD coro run 68 | await fuse.calibrate(lambda : not switch.value()) 69 | f.write('cal_end\n') 70 | print(fuse.magbias) 71 | print('Turn switch off to close the file and terminate.') 72 | await fuse.start() # Start the update task 73 | await display() 74 | 75 | asyncio.create_task(mem_manage()) 76 | asyncio.run(lcd_task()) 77 | -------------------------------------------------------------------------------- /remote/fusion_r_asyn.py: -------------------------------------------------------------------------------- 1 | # fusion_r_asyn.py Test for sensor fusion remote device 2 | # simulated by captured data mpudata 3 | # Author Peter Hinch 4 | # Released under the MIT License (MIT) See LICENSE 5 | # Copyright (c) 2017-2020 Peter Hinch 6 | 7 | # Requires: 8 | # uasyncio V3 (Included in daily builds and release builds later than V1.12). 9 | # Run under MicroPython on Unix or other target, or CPython 3.8 or later 10 | 11 | try: 12 | import utime as time 13 | except ImportError: 14 | import time 15 | try: 16 | import ujson as json 17 | except ImportError: 18 | import json 19 | try: 20 | import uasyncio as asyncio 21 | except ImportError: 22 | import asyncio 23 | 24 | from deltat import is_micropython 25 | from fusion_async import Fusion 26 | 27 | intro = ''' 28 | This demo reads data from a file created by recording IMU data from a Pyboard 29 | equipped with an IMU device. The data (in JSON format) contains the 3 IMU 30 | vectors followed by a timestamp in μs. 31 | 32 | Initially the board was rotated around each orthogonal axis to calibrate the 33 | magnetometer. A special line was inserted into the file to signal the end of 34 | cal. 35 | 36 | The device was then partially rotated in heading, then pitch, then roll: this 37 | data is displayed when calibration is complete. 38 | 39 | Calibration takes 15 seconds. 40 | ''' 41 | # GetData produces synthetic remote data from a file. 42 | # In an application this would asynchronously read data from a remote device. 43 | # The change in calibration mode might be returned from the remote as here or 44 | # handled at the fusion device by user input. 45 | 46 | # Initially we're in cal mode 47 | calibrate = True 48 | class GetData: 49 | def __init__(self): 50 | self.f = open('mpudata', 'r') # Initialise the comms link 51 | self.data = None 52 | # Because we're reading from a finite file we need a shutdown mechanism. 53 | # Would probably not apply to a comms link. 54 | self.running = True 55 | 56 | # Return [[ax, ay, az], [gx, gy, gz], [mx, my, mz], timestamp] 57 | # from whatever the device supplies (in this case JSON) 58 | async def read(self): 59 | global calibrate 60 | if not self.running: 61 | await asyncio.sleep(0.02) 62 | return self.data 63 | await asyncio.sleep(0.02) # Simulate async read data from remote 64 | line = self.f.readline() 65 | if line: # The file is finite 66 | if line.strip() == 'cal_end': # Remote has signalled end of cal 67 | calibrate = False 68 | await asyncio.sleep(0.02) # Read data from remote 69 | line = self.f.readline() 70 | 71 | self.data = json.loads(line) # Convert from foreign format 72 | else: 73 | self.running = False 74 | self.f.close() 75 | return self.data # Return old data if running has just been cleared 76 | 77 | get_data = GetData() 78 | 79 | # Test of supplying a timediff 80 | if is_micropython: 81 | def TimeDiff(start, end): 82 | return time.ticks_diff(start, end)/1000000 83 | else: # Cpython cheat: test data does not roll over 84 | def TimeDiff(start, end): 85 | return (start - end)/1000000 86 | 87 | fuse = Fusion(get_data.read, TimeDiff) 88 | 89 | async def display(): 90 | print('Heading Pitch Roll') 91 | while get_data.running: 92 | print("{:8.3f} {:8.3f} {:8.3f}".format(fuse.heading, fuse.pitch, fuse.roll)) 93 | await asyncio.sleep(0.5) 94 | 95 | async def main_task(): 96 | print(intro) 97 | await fuse.calibrate(lambda : not calibrate) 98 | print('Cal done. Magnetometer bias vector:', fuse.magbias) 99 | await fuse.start() # Start the update task 100 | await display() 101 | 102 | asyncio.run(main_task()) 103 | -------------------------------------------------------------------------------- /remote/fusion_r_syn.py: -------------------------------------------------------------------------------- 1 | # fusion_r_syn.py Test for sensor fusion remote device 2 | # simulated by captured data mpudata 3 | # Author Peter Hinch 4 | # Released under the MIT License (MIT) 5 | # Copyright (c) 2018 Peter Hinch 6 | # Run under MicroPython on Unix or other target, or CPython 3.4 or later 7 | 8 | try: 9 | import utime as time 10 | except ImportError: 11 | import time 12 | try: 13 | import ujson as json 14 | except ImportError: 15 | import json 16 | from fusion import Fusion 17 | from deltat import is_micropython 18 | 19 | intro = ''' 20 | This demo reads data from a file created by recording IMU data from a Pyboard 21 | equipped with an IMU device. The data (in JSON format) contains the 3 IMU 22 | vectors followed by a timestamp in μs. 23 | 24 | Initially the board was rotated around each orthogonal axis to calibrate the 25 | magnetometer. A special line was inserted into the file to signal the end of 26 | cal. 27 | 28 | The device was then partially rotated in heading, then pitch, then roll: this 29 | data is displayed when calibration is complete. 30 | 31 | Calibration takes 8 seconds. 32 | ''' 33 | 34 | # gdata produces synthetic remote data from a file. 35 | # In an application this would do a blocking data read from a remote device. 36 | # The change in calibration mode might be returned from the remote as here or 37 | # handled at the fusion device by user input. 38 | 39 | # Initially we're in cal mode 40 | calibrate = True 41 | def gdata(): 42 | global calibrate 43 | # Return [[ax, ay, az], [gx, gy, gz], [mx, my, mz], timestamp] 44 | # from whatever the device supplies (in this case JSON) 45 | with open('mpudata', 'r') as f: 46 | line = f.readline() # An app would do a blocking read of remote data 47 | while line: 48 | if line.strip() == 'cal_end': 49 | calibrate = False 50 | else: 51 | yield json.loads(line) # Convert foreign data format. 52 | line = f.readline() # Blocking read. 53 | 54 | get_data = gdata() 55 | 56 | # Test of supplying a timediff 57 | if is_micropython: 58 | def TimeDiff(start, end): 59 | return time.ticks_diff(start, end)/1000000 60 | else: # Cpython cheat: test data does not roll over 61 | def TimeDiff(start, end): 62 | return (start - end)/1000000 63 | 64 | # Expect a timestamp. Use supplied differencing function. 65 | fuse = Fusion(TimeDiff) 66 | 67 | def getmag(): # Return (x, y, z) magnetometer vector. 68 | imudata = next(get_data) 69 | return imudata[2] 70 | 71 | print(intro) 72 | fuse.calibrate(getmag, lambda : not calibrate, lambda : time.sleep(0.01)) 73 | print('Cal done. Magnetometer bias vector:', fuse.magbias) 74 | print('Heading Pitch Roll') 75 | 76 | count = 0 77 | while True: 78 | try: 79 | data = next(get_data) 80 | except StopIteration: # A file is finite. 81 | break # A real app would probably run forever. 82 | fuse.update(*data) 83 | if count % 25 == 0: 84 | print("{:8.3f} {:8.3f} {:8.3f}".format(fuse.heading, fuse.pitch, fuse.roll)) 85 | time.sleep(0.02) 86 | count += 1 87 | --------------------------------------------------------------------------------