diff --git a/lib/comms/bus.cpp b/lib/comms/bus.cpp index 2e73042..8bedb88 100644 --- a/lib/comms/bus.cpp +++ b/lib/comms/bus.cpp @@ -1,5 +1,4 @@ #include "bus.h" -#include bool initUARTs(Config &config) { @@ -97,3 +96,139 @@ uint8_t _scanSupportedDevicesOnWire(TwoWire &w, int bus_num) return res; } + +uint8_t _write_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t value, + bool skipValue) +{ + wire.beginTransmission(addr); + size_t s = wire.write(reg); + if (s == 1 && !skipValue) + { + s = wire.write(value); + } + + size_t s1 = wire.endTransmission(); + if (s != 1 && s1 == 0) + { + return 1; // "data too long to fit in transmit buffer" + } + + return s1; +} + +uint8_t _write_registers(TwoWire &wire, uint8_t addr, uint8_t reg, size_t sz, + const uint8_t *value) +{ + wire.beginTransmission(addr); + size_t s = wire.write(reg); + if (s == 1 && sz > 0) + { + s = wire.write(value, sz); + } + + size_t s1 = wire.endTransmission(); + if (s != sz && s1 == 0) + { + return 1; // "data too long to fit in transmit buffer" + } + + return s1; +} + +int8_t I2Cdev::writeBytes(TwoWire &w, uint8_t addr, uint8_t reg, size_t sz, + const uint8_t *buf) +{ + return _write_registers(w, addr, reg, sz, buf); +} + +int8_t I2Cdev::writeWords(TwoWire &w, uint8_t addr, uint8_t reg, size_t sz, + const uint16_t *buf16) +{ + uint8_t *buf = (uint8_t *)malloc(sz * 2); + uint8_t *p = buf; + for (int i = 0; i < sz; i++) + { + *(p++) = buf16[i] >> 8; + *(p++) = (uint8_t)buf16[i]; + } + uint8_t r = writeBytes(w, addr, reg, sz * 2, buf); + free(buf); + return r; +} + +int8_t I2Cdev::writeBit(TwoWire &wireObj, uint8_t addr, uint8_t reg, size_t bitNum, + uint8_t data) +{ + uint8_t b; + int8_t r = readBytes(wireObj, addr, reg, 1, &b, I2Cdev::readTimeout); + if (r != 1) + { + return r; + } + + b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); + return writeBytes(wireObj, addr, reg, 1, &b); +} + +int8_t I2Cdev::writeBits(TwoWire &w, uint8_t addr, uint8_t reg, size_t bitn, size_t sz, + uint8_t v) +{ + // 010 value to write + // 76543210 bit numbers + // xxx args: bitn=4, sz=3 + // 00011100 mask byte + // 10101111 original value (sample) + // 10100011 original & ~mask + // 10101011 masked | value + uint8_t b; + int8_t r = readBytes(w, addr, reg, 1, &b, I2Cdev::readTimeout); + if (r != 1) + { + return r; + } + + uint8_t mask = ((1 << sz) - 1) << (bitn - sz + 1); + v <<= (bitn - sz + 1); // shift data into correct position + v &= mask; // zero all non-important bits in data + b &= ~(mask); // zero all important bits in existing byte + b |= v; // combine data with existing byte + return writeBytes(w, addr, reg, 1, &b); +} + +int8_t _read_registers(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t *v, size_t sz, + bool skipRegister) +{ + if (!skipRegister) + { + uint8_t s = _write_registers(wire, addr, reg, 0, NULL); + if (s != 0) + { + return s; + } + } + + uint8_t r = wire.requestFrom(addr, sz); + r = wire.readBytes(v, r); + + return r - sz; +} + +uint8_t _read_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t &v, + bool skipRegister) +{ + uint8_t r = _read_registers(wire, addr, reg, &v, 1, skipRegister); + if (r != 0) + { + return 1; + } + + return 0; +} + +int8_t I2Cdev::readBytes(TwoWire &wireObj, uint8_t addr, uint8_t reg, size_t length, + uint8_t *data, uint16_t timeout) +{ // timeout is unused right now + return _read_registers(wireObj, addr, reg, data, length); +} + +uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; diff --git a/lib/comms/bus.h b/lib/comms/bus.h index 35a0aae..bfe81fc 100644 --- a/lib/comms/bus.h +++ b/lib/comms/bus.h @@ -1,17 +1,20 @@ #pragma once +#include #include struct { String name; uint8_t address; -} known_i2c_devices[] = {{"HMC5883L", 0x1e}, {"QMC5883L", 0x0d}, {" last record ", 0}}; +} known_i2c_devices[] = { + {"HMC5883L", 0x1e}, {"QMC5883L", 0x0d}, {"MPU6050", 0x68}, {" last record ", 0}}; enum I2CDevices { // powers of 2 HMC5883L = 1, - QMC5883L = 2 + QMC5883L = 2, + MPU6050 = 4 }; extern uint8_t wireDevices; @@ -39,3 +42,30 @@ bool initSPIs(Config &config); bool initUARTs(Config &config); bool initWires(Config &config); + +#define I2CDEV_DEFAULT_READ_TIMEOUT 0 + +struct I2Cdev +{ + static uint16_t readTimeout; + + static int8_t writeBit(TwoWire &w, uint8_t addr, uint8_t reg, size_t bitn, uint8_t v); + static int8_t writeBits(TwoWire &w, uint8_t addr, uint8_t reg, size_t bitn, size_t sz, + uint8_t v); + static int8_t writeBytes(TwoWire &w, uint8_t addr, uint8_t reg, size_t sz, + const uint8_t *buf); + static int8_t writeWords(TwoWire &w, uint8_t addr, uint8_t reg, size_t sz, + const uint16_t *buf); + + static int8_t readBytes(TwoWire &wireObj, uint8_t addr, uint8_t reg, size_t length, + uint8_t *data, uint16_t timeout = I2Cdev::readTimeout); +}; + +uint8_t _read_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t &v, + bool skipRegister = false); +int8_t _read_registers(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t *v, size_t sz, + bool skipRegister = false); +uint8_t _write_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t value, + bool skipValue = false); +uint8_t _write_registers(TwoWire &wire, uint8_t addr, uint8_t reg, size_t sz, + const uint8_t *value); diff --git a/lib/comms/comms.cpp b/lib/comms/comms.cpp index c0215d0..ec5b066 100644 --- a/lib/comms/comms.cpp +++ b/lib/comms/comms.cpp @@ -432,12 +432,16 @@ String _wrap_str(String v) Message::~Message() { - if (type == SCAN_RESULT || type == SCAN_MAX_RESULT) + if (type == SCAN_RESULT || type == SCAN_MAX_RESULT || type == SCAN_HEADING_MAX) { if (payload.dump.sz > 0) { delete[] payload.dump.freqs_khz; delete[] payload.dump.rssis; + if (payload.dump.rssis2) + { + delete[] payload.dump.rssis2; + } payload.dump.sz = 0; } diff --git a/lib/comms/comms.h b/lib/comms/comms.h index 57fb90b..68370f6 100644 --- a/lib/comms/comms.h +++ b/lib/comms/comms.h @@ -20,6 +20,7 @@ enum MessageType SCAN, SCAN_RESULT, SCAN_MAX_RESULT, + SCAN_HEADING_MAX, CONFIG_TASK, HEADING, _MAX_MESSAGE_TYPE = HEADING @@ -53,6 +54,8 @@ struct ScanTaskResult int16_t *rssis; int16_t *rssis2; int16_t prssi; + int16_t heading_min; + int16_t heading_max; }; struct ConfigTask diff --git a/lib/comms/radio_comms.cpp b/lib/comms/radio_comms.cpp index a994cdb..026588d 100644 --- a/lib/comms/radio_comms.cpp +++ b/lib/comms/radio_comms.cpp @@ -144,6 +144,32 @@ uint8_t *_serialize_scan_max_result(Message &m, size_t &p, uint8_t *msg) return msg; } +uint8_t *_serialize_scan_heading_max(Message &m, size_t &p, uint8_t *msg) +{ + if (m.type != SCAN_HEADING_MAX) + { + return NULL; + } + + size_t written = 0; + written = _write(msg, p, 0, (uint8_t *)&m.payload.dump.heading_min, 2); + written = _write(msg, p, written, (uint8_t *)&m.payload.dump.heading_max, 2); + + uint8_t *sub_msg = msg + written; + p -= written; + m.type = SCAN_MAX_RESULT; + sub_msg = _serialize_scan_max_result(m, p, sub_msg); + p += written; + m.type = SCAN_HEADING_MAX; + + if (sub_msg == NULL) + { + msg = NULL; + } + + return msg; +} + uint8_t *_serialize_config_task(Message &m, size_t &p, uint8_t *msg) { if (m.type != CONFIG_TASK) @@ -201,6 +227,10 @@ int16_t RadioComms::send(Message &m) { msg = _serialize_scan_max_result(m, p, msg_buf); } + else if (m.type == MessageType::SCAN_HEADING_MAX) + { + msg = _serialize_scan_heading_max(m, p, msg_buf); + } else if (m.type == MessageType::CONFIG_TASK) { msg = _serialize_config_task(m, p, msg_buf); @@ -301,6 +331,7 @@ Message *_deserialize_scan_max_result(size_t len, size_t &p, uint8_t *packet) message->payload.dump.sz = dump_sz; message->payload.dump.freqs_khz = freqs; message->payload.dump.rssis = rssis; + message->payload.dump.rssis2 = NULL; uint32_t scale = 0; p = _read(packet, len, p, (uint8_t *)&scale); @@ -318,6 +349,24 @@ Message *_deserialize_scan_max_result(size_t len, size_t &p, uint8_t *packet) return message; } +Message *_deserialize_scan_heading_max(size_t len, size_t &p, uint8_t *packet) +{ + int16_t heading_min = -999; + int16_t heading_max = 0; + p = _read(packet, len, p, (uint8_t *)&heading_min, 2); + p = _read(packet, len, p, (uint8_t *)&heading_max, 2); + + Message *message = _deserialize_scan_max_result(len, p, packet); + + if (message != NULL) + { + message->type = SCAN_HEADING_MAX; + message->payload.dump.heading_min = heading_min; + message->payload.dump.heading_max = heading_max; + } + return message; +} + Message *_deserialize_config_task(size_t len, size_t &p, uint8_t *packet) { Message *message = new Message(); @@ -439,6 +488,10 @@ Message *RadioComms::receive(uint16_t timeout_ms) { message = _deserialize_scan_max_result(len, p, packet); } + else if (b == SCAN_HEADING_MAX) + { + message = _deserialize_scan_heading_max(len, p, packet); + } else if (b == CONFIG_TASK) { message = _deserialize_config_task(len, p, packet); diff --git a/lib/heading/Compass.cpp b/lib/heading/Compass.cpp index 8a86871..d8d0d70 100644 --- a/lib/heading/Compass.cpp +++ b/lib/heading/Compass.cpp @@ -1,4 +1,108 @@ #include "heading.h" +#include + +enum MountingOrientation +{ + XY, // X forward, Y right, Z down + X_Y, // X forward, Y left, Z up + XZ, // X forward, Z right, Y up + X_Z, // X forward, Z left, Y down + YX, // Y forward, X right, Z up + Y_X, // Y forward, X left, Z down + YZ, // Y forward, Z right, X down + Y_Z, // Y forward, Z left, X up + ZX, // Z forward, X right, Y down + Z_X, // Z forward, X left, Y up + ZY, // Z forward, Y right, X down + Z_Y // Z forward, Y left, X up +}; + +// Produces CompassXYZ in a canonical mounting orientation: X forward, Y left, Z up +CompassXYZ _orientation(MountingOrientation o, int16_t x, int16_t y, int16_t z) +{ + CompassXYZ res; + res.status = 0; + + switch (o) + { + case XY: + case X_Y: + case XZ: + case X_Z: + res.x = x; + break; + case YX: + case Y_X: + case YZ: + case Y_Z: + res.x = y; + break; + case ZY: + case Z_Y: + case ZX: + case Z_X: + res.x = z; + break; + } + + switch (o) + { + case X_Y: + case Z_Y: + res.y = y; + break; + case XY: + case ZY: + res.y = -y; + break; + case Y_X: + case Z_X: + res.y = x; + break; + case YX: + case ZX: + res.y = -x; + break; + case X_Z: + case Y_Z: + res.y = z; + break; + case XZ: + case YZ: + res.y = -z; + break; + } + + switch (o) + { + case X_Y: + case YX: + res.z = z; + break; + case XY: + case Y_X: + res.z = -z; + break; + case XZ: + case Z_X: + res.z = y; + break; + case X_Z: + case ZX: + res.z = -y; + break; + case Y_Z: + case Z_Y: + res.z = x; + break; + case YZ: + case ZY: + res.z = -x; + break; + } + + return res; +} /* * QMC5883L Registers: @@ -71,58 +175,6 @@ bool QMC5883LCompass::begin() return true; } -uint8_t _write_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t value, - bool skipValue = false) -{ - wire.beginTransmission(addr); - size_t s = wire.write(reg); - if (s == 1 && !skipValue) - { - s = wire.write(value); - } - - size_t s1 = wire.endTransmission(); - if (s != 1 && s1 == 0) - { - return 1; // "data too long to fit in transmit buffer" - } - - return s1; -} - -int8_t _read_registers(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t *v, size_t sz, - bool skipRegister = false) -{ - if (!skipRegister) - { - uint8_t s = _write_register(wire, addr, reg, 0, true); - if (s != 0) - { - return s; - } - } - - uint8_t r = wire.requestFrom(addr, sz); - for (int i = 0; i < r; i++, v++) - { - *v = wire.read(); - } - - return r - sz; -} - -uint8_t _read_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t &v, - bool skipRegister = false) -{ - uint8_t r = _read_registers(wire, addr, reg, &v, 1, skipRegister); - if (r != 0) - { - return 1; - } - - return 0; -} - int8_t _read_xyz(TwoWire &wire, CompassXYZ &xyz) { xyz.status = 0; @@ -147,7 +199,7 @@ int8_t _read_xyz(TwoWire &wire, CompassXYZ &xyz) int8_t r = _read_registers(wire, QMC5883_ADDR, 0, (uint8_t *)&mags, 6); xyz.x = mags[0]; xyz.y = mags[1]; - xyz.z = mags[1]; + xyz.z = mags[2]; return r; } @@ -262,7 +314,15 @@ int16_t Compass::heading() return -999; } + // heading for canonical mounting orientation: X forward, Y left, Z up float heading = atan2(xyz.y, xyz.x); + + // Once you have your heading, you must then add your 'Declination Angle', which + // is the 'Error' of the magnetic field in your location. Find yours here: + // http://www.magnetic-declination.com/ Mine is: -13* 2' W, which is ~13 Degrees, + // or (which we need) 0.22 radians If you cannot find your Declination, comment + // out these two lines, your compass will be slightly off. + float declinationAngle = 0.22; heading += declinationAngle; @@ -279,3 +339,157 @@ int16_t Compass::heading() return headingDegrees; } + +bool UninitializedCompass::begin() +{ + _lastErr = CompassStatus::COMPASS_UNINITIALIZED; + return false; +} + +String UninitializedCompass::selfTest() { return "No compass is attached"; } + +uint8_t UninitializedCompass::setMode(CompassMode m) { return 4; } + +int8_t UninitializedCompass::readXYZ() { return 4; } + +#ifdef COMPASS_ENABLED +Adafruit_HMC5883_Unified _mag = Adafruit_HMC5883_Unified(12345); +#else +UninitializedCompass _mag; +#endif + +bool HMC5883LCompass::begin() +{ + _lastErr = CompassStatus::COMPASS_UNINITIALIZED; + +#ifdef COMPASS_ENABLED + + if (!mag.begin()) + { + return false; + } + + String err = selfTest(); + if (!err.startsWith("OK\n")) + { + return false; + } + + _lastErr = CompassStatus::COMPASS_OK; + return true; +#else + return mag.begin(); +#endif +} + +String HMC5883LCompass::selfTest() +{ +#ifdef COMPASS_ENABLED + sensor_t sensor; + mag.getSensor(&sensor); + return "OK\nSensor: " + String(sensor.name) + + "\nDriver Ver: " + String(sensor.version) + + "\nUnique ID: " + String(sensor.sensor_id) + + "\nMax Value: " + String(sensor.max_value) + " uT" + + "\nMin Value: " + String(sensor.min_value) + " uT" + + "\nResolution: " + String(sensor.resolution) + " uT"; +#else + return mag.selfTest(); +#endif +} + +uint8_t HMC5883LCompass::setMode(CompassMode m) +{ +#ifdef COMPASS_ENABLED + _lastErr = m; + return 0; +#else + return 1; +#endif +} + +int8_t HMC5883LCompass::readXYZ() +{ +#ifdef COMPASS_ENABLED + if (calStart == 0) + { + calStart = millis(); + } + + /* Get a new sensor event */ + sensors_event_t event2; + mag.getEvent(&event2); + sensors_event_t event3; + mag.getEvent(&event3); + +#ifdef COMPASS_DEBUG + /* Display the results (magnetic vector values are in micro-Tesla (uT)) */ + Serial.print("X: "); + Serial.print(event2.magnetic.x); + Serial.print(" "); + Serial.print("Y: "); + Serial.print(event2.magnetic.y); + Serial.print(" "); + Serial.print("Z: "); + Serial.print(event2.magnetic.z); + Serial.print(" "); + Serial.println("uT"); +#endif + + // Hold the module so that Z is pointing 'up' and you can measure the heading with + // x&y Calculate heading when the magnetometer is level, then correct for signs of + // axis. float heading = atan2(event.magnetic.y, event.magnetic.x); Use Y as the + // forward axis float heading = atan2(event.magnetic.x, event.magnetic.y); + /// If Z-axis is forward and Y-axis points upward: + // float heading = atan2(event.magnetic.x, event.magnetic.y); + // If Z-axis is forward and X-axis points upward: + // float heading = atan2(event.magnetic.y, -event.magnetic.x); + + // heading based on the magnetic readings from the Z-axis (forward) and the X-axis + // (perpendicular to Z, horizontal). + // float heading = atan2(event.magnetic.z, event.magnetic.x); + + // Dynamicly Calibrated out + + // Read raw magnetometer data + float x = (event2.magnetic.x + event3.magnetic.x) / 2; + float y = (event2.magnetic.y + event3.magnetic.y) / 2; + float z = (event2.magnetic.z + event3.magnetic.z) / 2; + + // Doing calibration first 1 minute + if (millis() - calStart < 60000) + { + // Update min/max values dynamically + x_min = min(x_min, x); + x_max = max(x_max, x); + y_min = min(y_min, y); + y_max = max(y_max, y); + z_min = min(z_min, z); + z_max = max(z_max, z); + } + +#ifdef COMPASS_DEBUG + Serial.println("x_min:" + String(x_min) + " x_max: " + String(x_max) + + " y_min: " + String(y_min)); +#endif + + // Calculate offsets and scales in real-time + float x_offset = (x_max + x_min) / 2; + float y_offset = (y_max + y_min) / 2; + float z_offset = (z_max + z_min) / 2; + + float x_scale = (x_max - x_min) / 2; + float y_scale = (y_max - y_min) / 2; + float z_scale = (z_max - z_min) / 2; + + // Apply calibration to raw data + float calibrated_x = (x - x_offset) / x_scale; + float calibrated_y = (y - y_offset) / y_scale; + float calibrated_z = (z - z_offset) / z_scale; + + xyz = _orientation(ZX, calibrated_x, calibrated_y, calibrated_z); + return 0; +#else + return mag.readXYZ(); +#endif +} diff --git a/lib/heading/DroneHeading.cpp b/lib/heading/DroneHeading.cpp index 475184b..8d631a4 100644 --- a/lib/heading/DroneHeading.cpp +++ b/lib/heading/DroneHeading.cpp @@ -8,3 +8,34 @@ void DroneHeading::setHeading(int64_t now, int16_t h) int64_t DroneHeading::lastRead() { return _lastRead; } int16_t DroneHeading::heading() { return _heading; } + +int16_t meanHeading(int16_t h1, int16_t h2) +{ + while (h1 < 0) + { + h1 += 360; + } + + while (h2 < 0) + { + h2 += 360; + } + + h1 = h1 % 360; + h2 = h2 % 360; + + if (h1 > h2) + { + h1 += h2; + h2 = h1 - h2; + h1 -= h2; + } + // h1 is min, h2 is max + + if (h2 - h1 > 180) + { + h1 += 360; + } + + return (h1 + h2) / 2; +} diff --git a/lib/heading/MPU6050.cpp b/lib/heading/MPU6050.cpp new file mode 100644 index 0000000..82150a8 --- /dev/null +++ b/lib/heading/MPU6050.cpp @@ -0,0 +1,344 @@ +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#include "MPU6050.h" +#include "../comms/bus.h" + +#include + +/* ================================================================ * + | Default MotionApps v6.12 28-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 | + | | + | [ACC X][ACC Y][ACC Z][GYRO X][GYRO Y][GYRO Z] | + | 16 17 18 19 20 21 22 23 24 25 26 27 | + * ================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) + +// this divisor is pre configured into the above image and can't be modified at this time. +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR \ + 0x01 // The New instance of the Firmware has this as the default +#endif + +// this is the most basic initialization I can create. with the intent that we access the +// register bytes as few times as needed to get the job done. for detailed descriptins of +// all registers and there purpose google "MPU-6000/MPU-6050 Register Map and +// Descriptions" +uint8_t MPU6050::dmpInitialize() +{ // Lets get it over with fast Write everything once and set it up nicely + uint8_t val; + uint16_t ival; + // Reset procedure per instructions in the "MPU-6000/MPU-6050 Register Map and + // Descriptions" page 41 + I2Cdev::writeBit(wireObj, devAddr, 0x6B, 7, + (val = 1)); // PWR_MGMT_1: reset with 100ms delay + delay(100); + I2Cdev::writeBits(wireObj, devAddr, 0x6A, 2, 3, + (val = 0b111)); // full SIGNAL_PATH_RESET: with another 100ms delay + delay(100); + I2Cdev::writeBytes( + wireObj, devAddr, 0x6B, 1, + &(val = 0x01)); // 1000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro + I2Cdev::writeBytes(wireObj, devAddr, 0x38, 1, + &(val = 0x00)); // 0000 0000 INT_ENABLE: no Interrupt + I2Cdev::writeBytes( + wireObj, devAddr, 0x23, 1, + &(val = 0x00)); // 0000 0000 MPU FIFO_EN: (all off) Using DMP's FIFO instead + I2Cdev::writeBytes( + wireObj, devAddr, 0x1C, 1, + &(val = 0x00)); // 0000 0000 ACCEL_CONFIG: 0 = Accel Full Scale Select: 2g + I2Cdev::writeBytes( + wireObj, devAddr, 0x37, 1, + &(val = 0x80)); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is + // active low. and interrupt status bits are cleared on any read + I2Cdev::writeBytes( + wireObj, devAddr, 0x6B, 1, + &(val = 0x01)); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro + I2Cdev::writeBytes( + wireObj, devAddr, 0x19, 1, + &(val = 0x04)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( + // Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)) + I2Cdev::writeBytes(wireObj, devAddr, 0x1A, 1, + &(val = 0x01)); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) + // Configuration 188HZ + // //Im betting this will be the beat + if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) + return 1; // Loads the DMP image into the MPU6050 Memory // Should Never Fail + I2Cdev::writeWords(wireObj, devAddr, 0x70, 1, + &(ival = 0x0400)); // DMP Program Start Address + I2Cdev::writeBytes(wireObj, devAddr, 0x1B, 1, + &(val = 0x18)); // 0001 1000 GYRO_CONFIG: 3 = +2000 Deg/sec + I2Cdev::writeBytes(wireObj, devAddr, 0x6A, 1, + &(val = 0xC0)); // 1100 1100 USER_CTRL: Enable Fifo and Reset Fifo + I2Cdev::writeBytes(wireObj, devAddr, 0x38, 1, + &(val = 0x02)); // 0000 0010 INT_ENABLE: RAW_DMP_INT_EN on + I2Cdev::writeBit(wireObj, devAddr, 0x6A, 2, + 1); // Reset FIFO one last time just for kicks. (MPUi2cWrite reads + // 0x6A first and only alters 1 bit and then saves the byte) + + setDMPEnabled(false); // disable DMP for compatibility with the MPU6050 library + /* + dmpPacketSize += 16;//DMP_FEATURE_6X_LP_QUAT + dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_ACCEL + dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_GYRO + */ + dmpPacketSize = 28; + return 0; +} + +void MPU6050::setDMPEnabled(bool e) +{ + dmpEnabled = e; + I2Cdev::writeBit(wireObj, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, + e); +} + +/** Get raw 6-axis motion sensor readings (accel/gyro). + * Retrieves all currently available motion sensor values. + * @param ax 16-bit signed integer container for accelerometer X-axis value + * @param ay 16-bit signed integer container for accelerometer Y-axis value + * @param az 16-bit signed integer container for accelerometer Z-axis value + * @param gx 16-bit signed integer container for gyroscope X-axis value + * @param gy 16-bit signed integer container for gyroscope Y-axis value + * @param gz 16-bit signed integer container for gyroscope Z-axis value + * @see getAcceleration() + * @see getRotation() + * @see MPU6050_RA_ACCEL_XOUT_H + */ +void MPU6050::getMotion6(int16_t *ax, int16_t *ay, int16_t *az, int16_t *gx, int16_t *gy, + int16_t *gz) +{ + uint8_t buffer[14]; + + I2Cdev::readBytes(wireObj, devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer, + I2Cdev::readTimeout); + *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; + *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; + *az = (((int16_t)buffer[4]) << 8) | buffer[5]; + *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; + *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; + *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; +} + +void MPU6050::setMemoryBank(uint8_t bank) +{ + bank &= 0x1F; + I2Cdev::writeBytes(wireObj, devAddr, MPU6050_RA_BANK_SEL, 1, &bank); +} + +void MPU6050::setMemoryStartAddress(uint8_t address) +{ + I2Cdev::writeBytes(wireObj, devAddr, MPU6050_RA_MEM_START_ADDR, 1, &address); +} + +bool MPU6050::writeProgMemoryBlock(const unsigned char *dump, size_t sz, uint8_t bank, + uint8_t address, bool verify) +{ + setMemoryBank(bank); + setMemoryStartAddress(address); + uint8_t chunkSize; + uint8_t *verifyBuffer = 0; + uint8_t *progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); + uint16_t i; + uint8_t j; + if (verify) + verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); + + for (i = 0; i < sz;) + { + // determine correct chunk size according to bank position and data size + chunkSize = min(MPU6050_DMP_MEMORY_CHUNK_SIZE, min((int)sz - i, 256 - address)); + + // write the chunk of data as specified + for (j = 0; j < chunkSize; j++) + progBuffer[j] = pgm_read_byte(dump + i + j); + I2Cdev::writeBytes(wireObj, devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer); + + // verify data if needed + if (verify && verifyBuffer) + { + setMemoryBank(bank); + setMemoryStartAddress(address); + I2Cdev::readBytes(wireObj, devAddr, MPU6050_RA_MEM_R_W, chunkSize, + verifyBuffer, I2Cdev::readTimeout); + if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) + { + free(verifyBuffer); + free(progBuffer); + return false; // uh oh. + } + } + + // increase byte index by [chunkSize] + i += chunkSize; + + // uint8_t automatically wraps to 0 at 256 + address += chunkSize; + + // if we aren't done, update bank (if necessary) and address + if (i < sz) + { + if (address == 0) + bank++; + setMemoryBank(bank); + setMemoryStartAddress(address); + } + } + if (verify) + free(verifyBuffer); + free(progBuffer); + return true; +} + +/** Get timeout to get a packet from FIFO buffer. + * @return Current timeout to get a packet from FIFO buffer + * @see MPU6050_FIFO_DEFAULT_TIMEOUT + */ +uint32_t MPU6050::getFIFOTimeout() { return MPU6050_FIFO_DEFAULT_TIMEOUT; } + +int8_t MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) +{ + if (length > 0) + { + return I2Cdev::readBytes(wireObj, devAddr, MPU6050_RA_FIFO_R_W, length, data, + I2Cdev::readTimeout); + } + + *data = 0; + return 0; +} + +/** Get current FIFO buffer size. + * This value indicates the number of bytes stored in the FIFO buffer. This + * number is in turn the number of bytes that can be read from the FIFO buffer + * and it is directly proportional to the number of samples available given the + * set of sensor data bound to be stored in the FIFO (register 35 and 36). + * @return Current FIFO buffer size + */ +uint16_t MPU6050::getFIFOCount() +{ + uint16_t buffer; + I2Cdev::readBytes(wireObj, devAddr, MPU6050_RA_FIFO_COUNTH, 2, (uint8_t *)&buffer, + I2Cdev::readTimeout); + return (buffer << 8) | ((buffer >> 8) & 0xff); +} + +/** Reset the FIFO. + * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This + * bit automatically clears to 0 after the reset has been triggered. + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_FIFO_RESET_BIT + */ +void MPU6050::resetFIFO() +{ + I2Cdev::writeBit(wireObj, devAddr, MPU6050_RA_USER_CTRL, + MPU6050_USERCTRL_FIFO_RESET_BIT, true); +} + +/** Get latest byte from FIFO buffer no matter how much time has passed. + * === GetCurrentFIFOPacket === + * ================================================================ + * Returns 1) when nothing special was done + * 2) when recovering from overflow + * 0) when no valid data is available + * ================================================================ */ +uint8_t MPU6050::getCurrentFIFOPacket(MPU6050Reading &v) +{ + uint8_t length = dmpPacketSize; + int16_t fifoC; + // This section of code is for when we allowed more than 1 packet to be acquired + uint32_t BreakTimer = micros(); + bool packetReceived = false; + do + { + if ((fifoC = getFIFOCount()) > length) + { + + if (fifoC > 200) + { // if you waited to get the FIFO buffer to > 200 bytes it will take longer + // to get the last packet in the FIFO Buffer than it will take to reset the + // buffer and wait for the next to arrive + resetFIFO(); // Fixes any overflow corruption + fifoC = 0; + while (!(fifoC = getFIFOCount()) && + ((micros() - BreakTimer) <= (getFIFOTimeout()))) + ; // Get Next New Packet + } + else + { // We have more than 1 packet but less than 200 bytes of data in the FIFO + // Buffer + uint8_t Trash[I2CDEVLIB_WIRE_BUFFER_LENGTH]; + while ((fifoC = getFIFOCount()) > length) + { // Test each time just in case the MPU is writing to the FIFO Buffer + fifoC = fifoC - length; // Save the last packet + uint16_t RemoveBytes; + while (fifoC) + { // fifo count will reach zero so this is safe + RemoveBytes = + (fifoC < I2CDEVLIB_WIRE_BUFFER_LENGTH) + ? fifoC + : I2CDEVLIB_WIRE_BUFFER_LENGTH; // Buffer Length is + // different than the + // packet length this will + // efficiently clear the + // buffer + getFIFOBytes(Trash, (uint8_t)RemoveBytes); + fifoC -= RemoveBytes; + } + } + } + } + if (!fifoC) + return 0; // Called too early no data or we timed out after FIFO Reset + // We have 1 packet + packetReceived = fifoC == length; + if (!packetReceived && (micros() - BreakTimer) > (getFIFOTimeout())) + return 0; + } while (!packetReceived); + getFIFOBytes(fifoPacket, length); // Get 1 packet + + v.q.w = (((uint32_t)fifoPacket[0] << 24) | ((uint32_t)fifoPacket[1] << 16) | + ((uint32_t)fifoPacket[2] << 8) | fifoPacket[3]); + v.q.x = (((uint32_t)fifoPacket[4] << 24) | ((uint32_t)fifoPacket[5] << 16) | + ((uint32_t)fifoPacket[6] << 8) | fifoPacket[7]); + v.q.y = (((uint32_t)fifoPacket[8] << 24) | ((uint32_t)fifoPacket[9] << 16) | + ((uint32_t)fifoPacket[10] << 8) | fifoPacket[11]); + v.q.z = (((uint32_t)fifoPacket[12] << 24) | ((uint32_t)fifoPacket[13] << 16) | + ((uint32_t)fifoPacket[14] << 8) | fifoPacket[15]); + + v.a.x = (fifoPacket[16] << 8) | fifoPacket[17]; + v.a.y = (fifoPacket[18] << 8) | fifoPacket[19]; + v.a.z = (fifoPacket[20] << 8) | fifoPacket[21]; + + v.g.x = (fifoPacket[22] << 8) | fifoPacket[23]; + v.g.y = (fifoPacket[24] << 8) | fifoPacket[25]; + v.g.z = (fifoPacket[26] << 8) | fifoPacket[27]; + return 1; +} diff --git a/lib/heading/MPU6050.h b/lib/heading/MPU6050.h new file mode 100644 index 0000000..5ae8d75 --- /dev/null +++ b/lib/heading/MPU6050.h @@ -0,0 +1,448 @@ +#pragma once + +#include "heading.h" + +#define MPU6050_ADDRESS_AD0_LOW \ + 0x68 // address pin low (GND), default for InvenSense evaluation board +#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) +#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW + +#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN +#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN +#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN +#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS +#define MPU6050_RA_XA_OFFS_L_TC 0x07 +#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS +#define MPU6050_RA_YA_OFFS_L_TC 0x09 +#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS +#define MPU6050_RA_ZA_OFFS_L_TC 0x0B +#define MPU6050_RA_SELF_TEST_X 0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Y 0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Z 0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_A \ + 0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0] +#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR +#define MPU6050_RA_XG_OFFS_USRL 0x14 +#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR +#define MPU6050_RA_YG_OFFS_USRL 0x16 +#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR +#define MPU6050_RA_ZG_OFFS_USRL 0x18 +#define MPU6050_RA_SMPLRT_DIV 0x19 +#define MPU6050_RA_CONFIG 0x1A +#define MPU6050_RA_GYRO_CONFIG 0x1B +#define MPU6050_RA_ACCEL_CONFIG 0x1C +#define MPU6050_RA_FF_THR 0x1D +#define MPU6050_RA_FF_DUR 0x1E +#define MPU6050_RA_MOT_THR 0x1F +#define MPU6050_RA_MOT_DUR 0x20 +#define MPU6050_RA_ZRMOT_THR 0x21 +#define MPU6050_RA_ZRMOT_DUR 0x22 +#define MPU6050_RA_FIFO_EN 0x23 +#define MPU6050_RA_I2C_MST_CTRL 0x24 +#define MPU6050_RA_I2C_SLV0_ADDR 0x25 +#define MPU6050_RA_I2C_SLV0_REG 0x26 +#define MPU6050_RA_I2C_SLV0_CTRL 0x27 +#define MPU6050_RA_I2C_SLV1_ADDR 0x28 +#define MPU6050_RA_I2C_SLV1_REG 0x29 +#define MPU6050_RA_I2C_SLV1_CTRL 0x2A +#define MPU6050_RA_I2C_SLV2_ADDR 0x2B +#define MPU6050_RA_I2C_SLV2_REG 0x2C +#define MPU6050_RA_I2C_SLV2_CTRL 0x2D +#define MPU6050_RA_I2C_SLV3_ADDR 0x2E +#define MPU6050_RA_I2C_SLV3_REG 0x2F +#define MPU6050_RA_I2C_SLV3_CTRL 0x30 +#define MPU6050_RA_I2C_SLV4_ADDR 0x31 +#define MPU6050_RA_I2C_SLV4_REG 0x32 +#define MPU6050_RA_I2C_SLV4_DO 0x33 +#define MPU6050_RA_I2C_SLV4_CTRL 0x34 +#define MPU6050_RA_I2C_SLV4_DI 0x35 +#define MPU6050_RA_I2C_MST_STATUS 0x36 +#define MPU6050_RA_INT_PIN_CFG 0x37 +#define MPU6050_RA_INT_ENABLE 0x38 +#define MPU6050_RA_DMP_INT_STATUS 0x39 +#define MPU6050_RA_INT_STATUS 0x3A +#define MPU6050_RA_ACCEL_XOUT_H 0x3B +#define MPU6050_RA_ACCEL_XOUT_L 0x3C +#define MPU6050_RA_ACCEL_YOUT_H 0x3D +#define MPU6050_RA_ACCEL_YOUT_L 0x3E +#define MPU6050_RA_ACCEL_ZOUT_H 0x3F +#define MPU6050_RA_ACCEL_ZOUT_L 0x40 +#define MPU6050_RA_TEMP_OUT_H 0x41 +#define MPU6050_RA_TEMP_OUT_L 0x42 +#define MPU6050_RA_GYRO_XOUT_H 0x43 +#define MPU6050_RA_GYRO_XOUT_L 0x44 +#define MPU6050_RA_GYRO_YOUT_H 0x45 +#define MPU6050_RA_GYRO_YOUT_L 0x46 +#define MPU6050_RA_GYRO_ZOUT_H 0x47 +#define MPU6050_RA_GYRO_ZOUT_L 0x48 +#define MPU6050_RA_EXT_SENS_DATA_00 0x49 +#define MPU6050_RA_EXT_SENS_DATA_01 0x4A +#define MPU6050_RA_EXT_SENS_DATA_02 0x4B +#define MPU6050_RA_EXT_SENS_DATA_03 0x4C +#define MPU6050_RA_EXT_SENS_DATA_04 0x4D +#define MPU6050_RA_EXT_SENS_DATA_05 0x4E +#define MPU6050_RA_EXT_SENS_DATA_06 0x4F +#define MPU6050_RA_EXT_SENS_DATA_07 0x50 +#define MPU6050_RA_EXT_SENS_DATA_08 0x51 +#define MPU6050_RA_EXT_SENS_DATA_09 0x52 +#define MPU6050_RA_EXT_SENS_DATA_10 0x53 +#define MPU6050_RA_EXT_SENS_DATA_11 0x54 +#define MPU6050_RA_EXT_SENS_DATA_12 0x55 +#define MPU6050_RA_EXT_SENS_DATA_13 0x56 +#define MPU6050_RA_EXT_SENS_DATA_14 0x57 +#define MPU6050_RA_EXT_SENS_DATA_15 0x58 +#define MPU6050_RA_EXT_SENS_DATA_16 0x59 +#define MPU6050_RA_EXT_SENS_DATA_17 0x5A +#define MPU6050_RA_EXT_SENS_DATA_18 0x5B +#define MPU6050_RA_EXT_SENS_DATA_19 0x5C +#define MPU6050_RA_EXT_SENS_DATA_20 0x5D +#define MPU6050_RA_EXT_SENS_DATA_21 0x5E +#define MPU6050_RA_EXT_SENS_DATA_22 0x5F +#define MPU6050_RA_EXT_SENS_DATA_23 0x60 +#define MPU6050_RA_MOT_DETECT_STATUS 0x61 +#define MPU6050_RA_I2C_SLV0_DO 0x63 +#define MPU6050_RA_I2C_SLV1_DO 0x64 +#define MPU6050_RA_I2C_SLV2_DO 0x65 +#define MPU6050_RA_I2C_SLV3_DO 0x66 +#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 +#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 +#define MPU6050_RA_MOT_DETECT_CTRL 0x69 +#define MPU6050_RA_USER_CTRL 0x6A +#define MPU6050_RA_PWR_MGMT_1 0x6B +#define MPU6050_RA_PWR_MGMT_2 0x6C +#define MPU6050_RA_BANK_SEL 0x6D +#define MPU6050_RA_MEM_START_ADDR 0x6E +#define MPU6050_RA_MEM_R_W 0x6F +#define MPU6050_RA_DMP_CFG_1 0x70 +#define MPU6050_RA_DMP_CFG_2 0x71 +#define MPU6050_RA_FIFO_COUNTH 0x72 +#define MPU6050_RA_FIFO_COUNTL 0x73 +#define MPU6050_RA_FIFO_R_W 0x74 +#define MPU6050_RA_WHO_AM_I 0x75 + +#define MPU6050_SELF_TEST_XA_1_BIT 0x07 +#define MPU6050_SELF_TEST_XA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_XA_2_BIT 0x05 +#define MPU6050_SELF_TEST_XA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_YA_1_BIT 0x07 +#define MPU6050_SELF_TEST_YA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_YA_2_BIT 0x03 +#define MPU6050_SELF_TEST_YA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_ZA_1_BIT 0x07 +#define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_ZA_2_BIT 0x01 +#define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02 + +#define MPU6050_SELF_TEST_XG_1_BIT 0x04 +#define MPU6050_SELF_TEST_XG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_YG_1_BIT 0x04 +#define MPU6050_SELF_TEST_YG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_ZG_1_BIT 0x04 +#define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05 + +#define MPU6050_TC_PWR_MODE_BIT 7 +#define MPU6050_TC_OFFSET_BIT 6 +#define MPU6050_TC_OFFSET_LENGTH 6 +#define MPU6050_TC_OTP_BNK_VLD_BIT 0 + +#define MPU6050_VDDIO_LEVEL_VLOGIC 0 +#define MPU6050_VDDIO_LEVEL_VDD 1 + +#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 +#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 +#define MPU6050_CFG_DLPF_CFG_BIT 2 +#define MPU6050_CFG_DLPF_CFG_LENGTH 3 + +#define MPU6050_EXT_SYNC_DISABLED 0x0 +#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 +#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 +#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 +#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 +#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 +#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 +#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 + +#define MPU6050_DLPF_BW_256 0x00 +#define MPU6050_DLPF_BW_188 0x01 +#define MPU6050_DLPF_BW_98 0x02 +#define MPU6050_DLPF_BW_42 0x03 +#define MPU6050_DLPF_BW_20 0x04 +#define MPU6050_DLPF_BW_10 0x05 +#define MPU6050_DLPF_BW_5 0x06 + +#define MPU6050_GCONFIG_FS_SEL_BIT 4 +#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 + +#define MPU6050_GYRO_FS_250 0x00 +#define MPU6050_GYRO_FS_500 0x01 +#define MPU6050_GYRO_FS_1000 0x02 +#define MPU6050_GYRO_FS_2000 0x03 + +#define MPU6050_ACONFIG_XA_ST_BIT 7 +#define MPU6050_ACONFIG_YA_ST_BIT 6 +#define MPU6050_ACONFIG_ZA_ST_BIT 5 +#define MPU6050_ACONFIG_AFS_SEL_BIT 4 +#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 +#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 +#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 + +#define MPU6050_ACCEL_FS_2 0x00 +#define MPU6050_ACCEL_FS_4 0x01 +#define MPU6050_ACCEL_FS_8 0x02 +#define MPU6050_ACCEL_FS_16 0x03 + +#define MPU6050_DHPF_RESET 0x00 +#define MPU6050_DHPF_5 0x01 +#define MPU6050_DHPF_2P5 0x02 +#define MPU6050_DHPF_1P25 0x03 +#define MPU6050_DHPF_0P63 0x04 +#define MPU6050_DHPF_HOLD 0x07 + +#define MPU6050_TEMP_FIFO_EN_BIT 7 +#define MPU6050_XG_FIFO_EN_BIT 6 +#define MPU6050_YG_FIFO_EN_BIT 5 +#define MPU6050_ZG_FIFO_EN_BIT 4 +#define MPU6050_ACCEL_FIFO_EN_BIT 3 +#define MPU6050_SLV2_FIFO_EN_BIT 2 +#define MPU6050_SLV1_FIFO_EN_BIT 1 +#define MPU6050_SLV0_FIFO_EN_BIT 0 + +#define MPU6050_MULT_MST_EN_BIT 7 +#define MPU6050_WAIT_FOR_ES_BIT 6 +#define MPU6050_SLV_3_FIFO_EN_BIT 5 +#define MPU6050_I2C_MST_P_NSR_BIT 4 +#define MPU6050_I2C_MST_CLK_BIT 3 +#define MPU6050_I2C_MST_CLK_LENGTH 4 + +#define MPU6050_CLOCK_DIV_348 0x0 +#define MPU6050_CLOCK_DIV_333 0x1 +#define MPU6050_CLOCK_DIV_320 0x2 +#define MPU6050_CLOCK_DIV_308 0x3 +#define MPU6050_CLOCK_DIV_296 0x4 +#define MPU6050_CLOCK_DIV_286 0x5 +#define MPU6050_CLOCK_DIV_276 0x6 +#define MPU6050_CLOCK_DIV_267 0x7 +#define MPU6050_CLOCK_DIV_258 0x8 +#define MPU6050_CLOCK_DIV_500 0x9 +#define MPU6050_CLOCK_DIV_471 0xA +#define MPU6050_CLOCK_DIV_444 0xB +#define MPU6050_CLOCK_DIV_421 0xC +#define MPU6050_CLOCK_DIV_400 0xD +#define MPU6050_CLOCK_DIV_381 0xE +#define MPU6050_CLOCK_DIV_364 0xF + +#define MPU6050_I2C_SLV_RW_BIT 7 +#define MPU6050_I2C_SLV_ADDR_BIT 6 +#define MPU6050_I2C_SLV_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV_EN_BIT 7 +#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 +#define MPU6050_I2C_SLV_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV_GRP_BIT 4 +#define MPU6050_I2C_SLV_LEN_BIT 3 +#define MPU6050_I2C_SLV_LEN_LENGTH 4 + +#define MPU6050_I2C_SLV4_RW_BIT 7 +#define MPU6050_I2C_SLV4_ADDR_BIT 6 +#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV4_EN_BIT 7 +#define MPU6050_I2C_SLV4_INT_EN_BIT 6 +#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 +#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 + +#define MPU6050_MST_PASS_THROUGH_BIT 7 +#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 +#define MPU6050_MST_I2C_LOST_ARB_BIT 5 +#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 +#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 +#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 +#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 +#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 + +#define MPU6050_INTCFG_INT_LEVEL_BIT 7 +#define MPU6050_INTCFG_INT_OPEN_BIT 6 +#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 +#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 +#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 +#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 +#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 +#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 + +#define MPU6050_INTMODE_ACTIVEHIGH 0x00 +#define MPU6050_INTMODE_ACTIVELOW 0x01 + +#define MPU6050_INTDRV_PUSHPULL 0x00 +#define MPU6050_INTDRV_OPENDRAIN 0x01 + +#define MPU6050_INTLATCH_50USPULSE 0x00 +#define MPU6050_INTLATCH_WAITCLEAR 0x01 + +#define MPU6050_INTCLEAR_STATUSREAD 0x00 +#define MPU6050_INTCLEAR_ANYREAD 0x01 + +#define MPU6050_INTERRUPT_FF_BIT 7 +#define MPU6050_INTERRUPT_MOT_BIT 6 +#define MPU6050_INTERRUPT_ZMOT_BIT 5 +#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 +#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 +#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 +#define MPU6050_INTERRUPT_DMP_INT_BIT 1 +#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 + +#define MPU6050_DMPINT_5_BIT 5 +#define MPU6050_DMPINT_4_BIT 4 +#define MPU6050_DMPINT_3_BIT 3 +#define MPU6050_DMPINT_2_BIT 2 +#define MPU6050_DMPINT_1_BIT 1 +#define MPU6050_DMPINT_0_BIT 0 + +#define MPU6050_MOTION_MOT_XNEG_BIT 7 +#define MPU6050_MOTION_MOT_XPOS_BIT 6 +#define MPU6050_MOTION_MOT_YNEG_BIT 5 +#define MPU6050_MOTION_MOT_YPOS_BIT 4 +#define MPU6050_MOTION_MOT_ZNEG_BIT 3 +#define MPU6050_MOTION_MOT_ZPOS_BIT 2 +#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 + +#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 +#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 +#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 +#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 +#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 +#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 + +#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 +#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 +#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 + +#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 +#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 +#define MPU6050_DETECT_FF_COUNT_BIT 3 +#define MPU6050_DETECT_FF_COUNT_LENGTH 2 +#define MPU6050_DETECT_MOT_COUNT_BIT 1 +#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 + +#define MPU6050_DETECT_DECREMENT_RESET 0x0 +#define MPU6050_DETECT_DECREMENT_1 0x1 +#define MPU6050_DETECT_DECREMENT_2 0x2 +#define MPU6050_DETECT_DECREMENT_4 0x3 + +#define MPU6050_USERCTRL_DMP_EN_BIT 7 +#define MPU6050_USERCTRL_FIFO_EN_BIT 6 +#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 +#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 +#define MPU6050_USERCTRL_DMP_RESET_BIT 3 +#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 +#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 +#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 + +#define MPU6050_PWR1_DEVICE_RESET_BIT 7 +#define MPU6050_PWR1_SLEEP_BIT 6 +#define MPU6050_PWR1_CYCLE_BIT 5 +#define MPU6050_PWR1_TEMP_DIS_BIT 3 +#define MPU6050_PWR1_CLKSEL_BIT 2 +#define MPU6050_PWR1_CLKSEL_LENGTH 3 + +#define MPU6050_CLOCK_INTERNAL 0x00 +#define MPU6050_CLOCK_PLL_XGYRO 0x01 +#define MPU6050_CLOCK_PLL_YGYRO 0x02 +#define MPU6050_CLOCK_PLL_ZGYRO 0x03 +#define MPU6050_CLOCK_PLL_EXT32K 0x04 +#define MPU6050_CLOCK_PLL_EXT19M 0x05 +#define MPU6050_CLOCK_KEEP_RESET 0x07 + +#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 +#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 +#define MPU6050_PWR2_STBY_XA_BIT 5 +#define MPU6050_PWR2_STBY_YA_BIT 4 +#define MPU6050_PWR2_STBY_ZA_BIT 3 +#define MPU6050_PWR2_STBY_XG_BIT 2 +#define MPU6050_PWR2_STBY_YG_BIT 1 +#define MPU6050_PWR2_STBY_ZG_BIT 0 + +#define MPU6050_WAKE_FREQ_1P25 0x0 +#define MPU6050_WAKE_FREQ_2P5 0x1 +#define MPU6050_WAKE_FREQ_5 0x2 +#define MPU6050_WAKE_FREQ_10 0x3 +#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 +#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 +#define MPU6050_BANKSEL_MEM_SEL_BIT 4 +#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 + +#define MPU6050_WHO_AM_I_BIT 6 +#define MPU6050_WHO_AM_I_LENGTH 6 + +#define MPU6050_DMP_MEMORY_BANKS 8 +#define MPU6050_DMP_MEMORY_BANK_SIZE 256 +#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 + +#define MPU6050_FIFO_DEFAULT_TIMEOUT 11000 + +struct VectorI16 +{ + int16_t x; + int16_t y; + int16_t z; +}; + +struct Quaternion +{ + float w; + float x; + float y; + float z; +}; + +struct MPU6050Reading +{ + struct + { + int32_t w; + int32_t x; + int32_t y; + int32_t z; + } q; // quaternion + + VectorI16 a; // acceleration + VectorI16 g; // gyro +}; + +struct MPU6050 +{ + TwoWire wireObj; + uint8_t devAddr; + bool dmpEnabled; + uint8_t dmpPacketSize; + uint8_t *fifoPacket; + + MPU6050(TwoWire w) + : devAddr(0x68), dmpPacketSize(28), wireObj(w), dmpEnabled(false), + fifoPacket((uint8_t *)malloc(dmpPacketSize)) {}; + + uint8_t dmpInitialize(); + void setDMPEnabled(bool e); + // BANK_SEL register + void setMemoryBank(uint8_t bank); + + // MEM_START_ADDR register + void setMemoryStartAddress(uint8_t address); + + bool writeProgMemoryBlock(const unsigned char *dump, size_t sz, uint8_t bank = 0, + uint8_t address = 0, bool verify = true); + void getMotion6(int16_t *ax, int16_t *ay, int16_t *az, int16_t *gx, int16_t *gy, + int16_t *gz); + uint8_t getCurrentFIFOPacket(MPU6050Reading &v); + + uint32_t getFIFOTimeout(); + int8_t getFIFOBytes(uint8_t *data, uint8_t length); + uint16_t getFIFOCount(); + void resetFIFO(); +}; + +#define I2CDEVLIB_WIRE_BUFFER_LENGTH I2C_BUFFER_LENGTH +#define MPU6050_DMP_CODE_SIZE 3062 // dmpMemory[] + +extern const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE]; diff --git a/lib/heading/MPU6050_612_dump.cpp b/lib/heading/MPU6050_612_dump.cpp new file mode 100644 index 0000000..5061466 --- /dev/null +++ b/lib/heading/MPU6050_612_dump.cpp @@ -0,0 +1,211 @@ +// DON'T OPEN THIS FILE, OR VSCODE IS GOING TO MAKE A MESS OF IT +#include "MPU6050.h" + +// *** this is a capture of the DMP Firmware V6.1.2 after all the messy changes were made +// so we can just load it +const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { +/* bank # 0 */ +0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x03, 0x00, 0x00, +0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, +0x03, 0x0C, 0x30, 0xC3, 0x0A, 0x74, 0x56, 0x2D, 0x0D, 0x62, 0xDB, 0xC7, 0x16, 0xF4, 0xBA, 0x02, +0x38, 0x83, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83, 0x25, 0x8E, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83, +0xFF, 0xFF, 0xFF, 0xFF, 0x0C, 0xBD, 0xD8, 0x11, 0x24, 0x00, 0x04, 0x00, 0x1A, 0x82, 0x79, 0xA1, +0x00, 0x36, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6F, 0xA2, +0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, +0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, +0x1F, 0xA4, 0xE8, 0xE4, 0xFF, 0xF5, 0xDC, 0xB9, 0x00, 0x5B, 0x79, 0xCF, 0x1F, 0x3F, 0x78, 0x76, +0x00, 0x86, 0x7C, 0x5A, 0x00, 0x86, 0x23, 0x47, 0xFA, 0xB9, 0x86, 0x31, 0x00, 0x74, 0x87, 0x8A, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x43, 0x05, 0xFF, 0xFF, 0xE9, 0xA8, 0x00, 0x00, 0x21, 0x82, +0xFA, 0xB8, 0x4D, 0x46, 0xFF, 0xFA, 0xDF, 0x3D, 0xFF, 0xFF, 0xB2, 0xB3, 0x00, 0x00, 0x00, 0x00, +0x3F, 0xFF, 0xBA, 0x98, 0x00, 0x5D, 0xAC, 0x08, 0x00, 0x0A, 0x63, 0x78, 0x00, 0x01, 0x46, 0x21, +0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x42, 0xB5, 0x00, 0x06, 0x00, 0x64, 0x00, 0x64, 0x00, 0x06, +0x14, 0x06, 0x02, 0x9F, 0x0F, 0x47, 0x91, 0x32, 0xD9, 0x0E, 0x9F, 0xC9, 0x1D, 0xCF, 0x4C, 0x34, +0x3B, 0xB6, 0x7A, 0xE8, 0x00, 0x64, 0x00, 0x06, 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFE, +/* bank # 1 */ +0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x07, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00, +0x10, 0x00, 0x00, 0x00, 0x04, 0xD6, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, +0x00, 0x00, 0x00, 0x32, 0xF8, 0x98, 0x00, 0x00, 0xFF, 0x65, 0x00, 0x00, 0x83, 0x0F, 0x00, 0x00, +0x00, 0x06, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, +0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0D, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x02, 0x00, 0x00, +0x00, 0x01, 0xFB, 0x83, 0x00, 0x7C, 0x00, 0x00, 0xFB, 0x15, 0xFC, 0x00, 0x1F, 0xB4, 0xFF, 0x83, +0x00, 0x00, 0x00, 0x01, 0x00, 0x65, 0x00, 0x07, 0x00, 0x64, 0x03, 0xE8, 0x00, 0x64, 0x00, 0x28, +0x00, 0x00, 0xFF, 0xFD, 0x00, 0x00, 0x00, 0x00, 0x16, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x10, 0x00, 0x00, 0x2F, 0x00, 0x00, 0x00, 0x00, 0x01, 0xF4, 0x00, 0x00, 0x10, 0x00, +/* bank # 2 */ +0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0xBA, 0xC6, 0x00, 0x47, 0x78, 0xA2, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, +0x00, 0x00, 0x23, 0xBB, 0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49, +0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, +0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x64, 0x00, 0x07, 0x00, 0x08, 0x00, 0x06, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49, 0x00, 0x00, 0x00, 0x00, +0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00, +0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x0E, +0xFF, 0xFF, 0xFF, 0xCF, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xFF, 0xFF, 0xFF, 0x9C, +0x00, 0x00, 0x43, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, +0xFF, 0xE5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +/* bank # 3 */ +0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xD3, +0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3C, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x9E, 0x65, 0x5D, +0x0C, 0x0A, 0x4E, 0x68, 0xCD, 0xCF, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xC6, 0x19, 0xCE, 0x82, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xD7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, +0x00, 0x11, 0xDC, 0x47, 0x03, 0x00, 0x00, 0x00, 0xC7, 0x93, 0x8F, 0x9D, 0x1E, 0x1B, 0x1C, 0x19, +0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0E, 0xDF, 0xA4, 0x38, 0x1F, 0x9E, 0x65, 0x5D, +0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x3F, 0xFF, 0xFF, 0xFD, 0xFF, 0xFF, 0xF4, 0xC9, 0xFF, 0xFF, 0xBC, 0xF0, 0x00, 0x01, 0x0C, 0x0F, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0xF5, 0xB7, 0xBA, 0xB3, 0x67, 0x7D, 0xDF, 0x7E, 0x72, 0x90, 0x2E, 0x55, 0x4C, 0xF6, 0xE6, 0x88, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +/* bank # 4 */ +0xD8, 0xDC, 0xB4, 0xB8, 0xB0, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xB3, 0xB7, 0xBB, 0x8E, 0x9E, +0xAE, 0xF1, 0x32, 0xF5, 0x1B, 0xF1, 0xB4, 0xB8, 0xB0, 0x80, 0x97, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, +0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0xF1, 0xA9, +0x89, 0x26, 0x46, 0x66, 0xB2, 0x89, 0x99, 0xA9, 0x2D, 0x55, 0x7D, 0xB0, 0xB0, 0x8A, 0xA8, 0x96, +0x36, 0x56, 0x76, 0xF1, 0xBA, 0xA3, 0xB4, 0xB2, 0x80, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB2, 0x83, +0x98, 0xBA, 0xA3, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xB2, 0xB9, 0xB4, 0x98, 0x83, 0xF1, +0xA3, 0x29, 0x55, 0x7D, 0xBA, 0xB5, 0xB1, 0xA3, 0x83, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xB2, +0xB6, 0xAA, 0x83, 0x93, 0x28, 0x54, 0x7C, 0xF1, 0xB9, 0xA3, 0x82, 0x93, 0x61, 0xBA, 0xA2, 0xDA, +0xDE, 0xDF, 0xDB, 0x81, 0x9A, 0xB9, 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xF1, 0xDA, 0xBA, 0xA2, 0xDF, +0xD9, 0xBA, 0xA2, 0xFA, 0xB9, 0xA3, 0x82, 0x92, 0xDB, 0x31, 0xBA, 0xA2, 0xD9, 0xBA, 0xA2, 0xF8, +0xDF, 0x85, 0xA4, 0xD0, 0xC1, 0xBB, 0xAD, 0x83, 0xC2, 0xC5, 0xC7, 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, +0xBA, 0xA0, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xAA, 0xB3, 0x8D, 0xB4, 0x98, 0x0D, 0x35, +0x5D, 0xB2, 0xB6, 0xBA, 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, +0xB8, 0xAA, 0x87, 0x2C, 0x54, 0x7C, 0xBA, 0xA4, 0xB0, 0x8A, 0xB6, 0x91, 0x32, 0x56, 0x76, 0xB2, +0x84, 0x94, 0xA4, 0xC8, 0x08, 0xCD, 0xD8, 0xB8, 0xB4, 0xB0, 0xF1, 0x99, 0x82, 0xA8, 0x2D, 0x55, +0x7D, 0x98, 0xA8, 0x0E, 0x16, 0x1E, 0xA2, 0x2C, 0x54, 0x7C, 0x92, 0xA4, 0xF0, 0x2C, 0x50, 0x78, +/* bank # 5 */ +0xF1, 0x84, 0xA8, 0x98, 0xC4, 0xCD, 0xFC, 0xD8, 0x0D, 0xDB, 0xA8, 0xFC, 0x2D, 0xF3, 0xD9, 0xBA, +0xA6, 0xF8, 0xDA, 0xBA, 0xA6, 0xDE, 0xD8, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xF3, 0xC8, +0x41, 0xDA, 0xA6, 0xC8, 0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0x82, 0xA8, 0x92, 0xF5, 0x2C, 0x54, 0x88, +0x98, 0xF1, 0x35, 0xD9, 0xF4, 0x18, 0xD8, 0xF1, 0xA2, 0xD0, 0xF8, 0xF9, 0xA8, 0x84, 0xD9, 0xC7, +0xDF, 0xF8, 0xF8, 0x83, 0xC5, 0xDA, 0xDF, 0x69, 0xDF, 0x83, 0xC1, 0xD8, 0xF4, 0x01, 0x14, 0xF1, +0xA8, 0x82, 0x4E, 0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x28, 0x97, 0x88, 0xF1, +0x09, 0xF4, 0x1C, 0x1C, 0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x29, +0xF4, 0x0D, 0xD8, 0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC2, 0x03, 0xD8, 0xDE, 0xDF, 0x1A, +0xD8, 0xF1, 0xA2, 0xFA, 0xF9, 0xA8, 0x84, 0x98, 0xD9, 0xC7, 0xDF, 0xF8, 0xF8, 0xF8, 0x83, 0xC7, +0xDA, 0xDF, 0x69, 0xDF, 0xF8, 0x83, 0xC3, 0xD8, 0xF4, 0x01, 0x14, 0xF1, 0x98, 0xA8, 0x82, 0x2E, +0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x50, 0x97, 0x88, 0xF1, 0x09, 0xF4, 0x1C, +0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF8, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x49, 0xF4, 0x0D, 0xD8, +0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC4, 0x03, 0xD8, 0xDE, 0xDF, 0xD8, 0xF1, 0xAD, 0x88, +0x98, 0xCC, 0xA8, 0x09, 0xF9, 0xD9, 0x82, 0x92, 0xA8, 0xF5, 0x7C, 0xF1, 0x88, 0x3A, 0xCF, 0x94, +0x4A, 0x6E, 0x98, 0xDB, 0x69, 0x31, 0xDA, 0xAD, 0xF2, 0xDE, 0xF9, 0xD8, 0x87, 0x95, 0xA8, 0xF2, +0x21, 0xD1, 0xDA, 0xA5, 0xF9, 0xF4, 0x17, 0xD9, 0xF1, 0xAE, 0x8E, 0xD0, 0xC0, 0xC3, 0xAE, 0x82, +/* bank # 6 */ +0xC6, 0x84, 0xC3, 0xA8, 0x85, 0x95, 0xC8, 0xA5, 0x88, 0xF2, 0xC0, 0xF1, 0xF4, 0x01, 0x0E, 0xF1, +0x8E, 0x9E, 0xA8, 0xC6, 0x3E, 0x56, 0xF5, 0x54, 0xF1, 0x88, 0x72, 0xF4, 0x01, 0x15, 0xF1, 0x98, +0x45, 0x85, 0x6E, 0xF5, 0x8E, 0x9E, 0x04, 0x88, 0xF1, 0x42, 0x98, 0x5A, 0x8E, 0x9E, 0x06, 0x88, +0x69, 0xF4, 0x01, 0x1C, 0xF1, 0x98, 0x1E, 0x11, 0x08, 0xD0, 0xF5, 0x04, 0xF1, 0x1E, 0x97, 0x02, +0x02, 0x98, 0x36, 0x25, 0xDB, 0xF9, 0xD9, 0x85, 0xA5, 0xF3, 0xC1, 0xDA, 0x85, 0xA5, 0xF3, 0xDF, +0xD8, 0x85, 0x95, 0xA8, 0xF3, 0x09, 0xDA, 0xA5, 0xFA, 0xD8, 0x82, 0x92, 0xA8, 0xF5, 0x78, 0xF1, +0x88, 0x1A, 0x84, 0x9F, 0x26, 0x88, 0x98, 0x21, 0xDA, 0xF4, 0x1D, 0xF3, 0xD8, 0x87, 0x9F, 0x39, +0xD1, 0xAF, 0xD9, 0xDF, 0xDF, 0xFB, 0xF9, 0xF4, 0x0C, 0xF3, 0xD8, 0xFA, 0xD0, 0xF8, 0xDA, 0xF9, +0xF9, 0xD0, 0xDF, 0xD9, 0xF9, 0xD8, 0xF4, 0x0B, 0xD8, 0xF3, 0x87, 0x9F, 0x39, 0xD1, 0xAF, 0xD9, +0xDF, 0xDF, 0xF4, 0x1D, 0xF3, 0xD8, 0xFA, 0xFC, 0xA8, 0x69, 0xF9, 0xF9, 0xAF, 0xD0, 0xDA, 0xDE, +0xFA, 0xD9, 0xF8, 0x8F, 0x9F, 0xA8, 0xF1, 0xCC, 0xF3, 0x98, 0xDB, 0x45, 0xD9, 0xAF, 0xDF, 0xD0, +0xF8, 0xD8, 0xF1, 0x8F, 0x9F, 0xA8, 0xCA, 0xF3, 0x88, 0x09, 0xDA, 0xAF, 0x8F, 0xCB, 0xF8, 0xD8, +0xF2, 0xAD, 0x97, 0x8D, 0x0C, 0xD9, 0xA5, 0xDF, 0xF9, 0xBA, 0xA6, 0xF3, 0xFA, 0xF4, 0x12, 0xF2, +0xD8, 0x95, 0x0D, 0xD1, 0xD9, 0xBA, 0xA6, 0xF3, 0xFA, 0xDA, 0xA5, 0xF2, 0xC1, 0xBA, 0xA6, 0xF3, +0xDF, 0xD8, 0xF1, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xCA, 0xF3, 0x49, 0xDA, 0xA6, 0xCB, +0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0xD8, 0xAD, 0x84, 0xF2, 0xC0, 0xDF, 0xF1, 0x8F, 0xCB, 0xC3, 0xA8, +/* bank # 7 */ +0xB2, 0xB6, 0x86, 0x96, 0xC8, 0xC1, 0xCB, 0xC3, 0xF3, 0xB0, 0xB4, 0x88, 0x98, 0xA8, 0x21, 0xDB, +0x71, 0x8D, 0x9D, 0x71, 0x85, 0x95, 0x21, 0xD9, 0xAD, 0xF2, 0xFA, 0xD8, 0x85, 0x97, 0xA8, 0x28, +0xD9, 0xF4, 0x08, 0xD8, 0xF2, 0x8D, 0x29, 0xDA, 0xF4, 0x05, 0xD9, 0xF2, 0x85, 0xA4, 0xC2, 0xF2, +0xD8, 0xA8, 0x8D, 0x94, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xF2, 0xD8, 0x87, 0x21, 0xD8, 0xF4, 0x0A, +0xD8, 0xF2, 0x84, 0x98, 0xA8, 0xC8, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xD8, 0xF3, 0xA4, 0xC8, 0xBB, +0xAF, 0xD0, 0xF2, 0xDE, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xD8, 0xF1, 0xB8, 0xF6, +0xB5, 0xB9, 0xB0, 0x8A, 0x95, 0xA3, 0xDE, 0x3C, 0xA3, 0xD9, 0xF8, 0xD8, 0x5C, 0xA3, 0xD9, 0xF8, +0xD8, 0x7C, 0xA3, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA5, 0xD9, 0xDF, 0xDA, 0xFA, 0xD8, 0xB1, +0x85, 0x30, 0xF7, 0xD9, 0xDE, 0xD8, 0xF8, 0x30, 0xAD, 0xDA, 0xDE, 0xD8, 0xF2, 0xB4, 0x8C, 0x99, +0xA3, 0x2D, 0x55, 0x7D, 0xA0, 0x83, 0xDF, 0xDF, 0xDF, 0xB5, 0x91, 0xA0, 0xF6, 0x29, 0xD9, 0xFB, +0xD8, 0xA0, 0xFC, 0x29, 0xD9, 0xFA, 0xD8, 0xA0, 0xD0, 0x51, 0xD9, 0xF8, 0xD8, 0xFC, 0x51, 0xD9, +0xF9, 0xD8, 0x79, 0xD9, 0xFB, 0xD8, 0xA0, 0xD0, 0xFC, 0x79, 0xD9, 0xFA, 0xD8, 0xA1, 0xF9, 0xF9, +0xF9, 0xF9, 0xF9, 0xA0, 0xDA, 0xDF, 0xDF, 0xDF, 0xD8, 0xA1, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xAC, +0xDE, 0xF8, 0xAD, 0xDE, 0x83, 0x93, 0xAC, 0x2C, 0x54, 0x7C, 0xF1, 0xA8, 0xDF, 0xDF, 0xDF, 0xF6, +0x9D, 0x2C, 0xDA, 0xA0, 0xDF, 0xD9, 0xFA, 0xDB, 0x2D, 0xF8, 0xD8, 0xA8, 0x50, 0xDA, 0xA0, 0xD0, +0xDE, 0xD9, 0xD0, 0xF8, 0xF8, 0xF8, 0xDB, 0x55, 0xF8, 0xD8, 0xA8, 0x78, 0xDA, 0xA0, 0xD0, 0xDF, +/* bank # 8 */ +0xD9, 0xD0, 0xFA, 0xF8, 0xF8, 0xF8, 0xF8, 0xDB, 0x7D, 0xF8, 0xD8, 0x9C, 0xA8, 0x8C, 0xF5, 0x30, +0xDB, 0x38, 0xD9, 0xD0, 0xDE, 0xDF, 0xA0, 0xD0, 0xDE, 0xDF, 0xD8, 0xA8, 0x48, 0xDB, 0x58, 0xD9, +0xDF, 0xD0, 0xDE, 0xA0, 0xDF, 0xD0, 0xDE, 0xD8, 0xA8, 0x68, 0xDB, 0x70, 0xD9, 0xDF, 0xDF, 0xA0, +0xDF, 0xDF, 0xD8, 0xF1, 0xA8, 0x88, 0x90, 0x2C, 0x54, 0x7C, 0x98, 0xA8, 0xD0, 0x5C, 0x38, 0xD1, +0xDA, 0xF2, 0xAE, 0x8C, 0xDF, 0xF9, 0xD8, 0xB0, 0x87, 0xA8, 0xC1, 0xC1, 0xB1, 0x88, 0xA8, 0xC6, +0xF9, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, +0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xF7, 0x8D, 0x9D, 0xAD, 0xF8, 0x18, 0xDA, +0xF2, 0xAE, 0xDF, 0xD8, 0xF7, 0xAD, 0xFA, 0x30, 0xD9, 0xA4, 0xDE, 0xF9, 0xD8, 0xF2, 0xAE, 0xDE, +0xFA, 0xF9, 0x83, 0xA7, 0xD9, 0xC3, 0xC5, 0xC7, 0xF1, 0x88, 0x9B, 0xA7, 0x7A, 0xAD, 0xF7, 0xDE, +0xDF, 0xA4, 0xF8, 0x84, 0x94, 0x08, 0xA7, 0x97, 0xF3, 0x00, 0xAE, 0xF2, 0x98, 0x19, 0xA4, 0x88, +0xC6, 0xA3, 0x94, 0x88, 0xF6, 0x32, 0xDF, 0xF2, 0x83, 0x93, 0xDB, 0x09, 0xD9, 0xF2, 0xAA, 0xDF, +0xD8, 0xD8, 0xAE, 0xF8, 0xF9, 0xD1, 0xDA, 0xF3, 0xA4, 0xDE, 0xA7, 0xF1, 0x88, 0x9B, 0x7A, 0xD8, +0xF3, 0x84, 0x94, 0xAE, 0x19, 0xF9, 0xDA, 0xAA, 0xF1, 0xDF, 0xD8, 0xA8, 0x81, 0xC0, 0xC3, 0xC5, +0xC7, 0xA3, 0x92, 0x83, 0xF6, 0x28, 0xAD, 0xDE, 0xD9, 0xF8, 0xD8, 0xA3, 0x50, 0xAD, 0xD9, 0xF8, +0xD8, 0xA3, 0x78, 0xAD, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA1, 0xDA, 0xDE, 0xC3, 0xC5, 0xC7, +0xD8, 0xA1, 0x81, 0x94, 0xF8, 0x18, 0xF2, 0xB0, 0x89, 0xAC, 0xC3, 0xC5, 0xC7, 0xF1, 0xD8, 0xB8, +/* bank # 9 */ +0xB4, 0xB0, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, +0x0C, 0x20, 0x14, 0x40, 0xB0, 0xB4, 0xB8, 0xF0, 0xA8, 0x8A, 0x9A, 0x28, 0x50, 0x78, 0xB7, 0x9B, +0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xF1, 0xBB, 0xAB, +0x88, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0xB3, 0x8B, 0xB8, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0xB0, +0x88, 0xB4, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xBB, 0xAB, 0xB3, 0x8B, 0x02, 0x26, 0x46, 0x66, 0xB0, +0xB8, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 0x8A, 0x24, 0x70, 0x59, +0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 0x8A, 0x64, 0x48, 0x31, +0x8B, 0x30, 0x49, 0x60, 0x88, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 0x28, +0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 0xF0, +0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xA9, +0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 0x8C, +0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 0x7E, +0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB5, 0xB9, 0xA3, 0xDF, 0xDF, 0xDF, 0xAE, 0xD0, +0xDF, 0xAA, 0xD0, 0xDE, 0xF2, 0xAB, 0xF8, 0xF9, 0xD9, 0xB0, 0x87, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, +0xBB, 0xAF, 0xDF, 0xDF, 0xB9, 0xD8, 0xB1, 0xF1, 0xA3, 0x97, 0x8E, 0x60, 0xDF, 0xB0, 0x84, 0xF2, +0xC8, 0xF8, 0xF9, 0xD9, 0xDE, 0xD8, 0x93, 0x85, 0xF1, 0x4A, 0xB1, 0x83, 0xA3, 0x08, 0xB5, 0x83, +/* bank # 10 */ +0x9A, 0x08, 0x10, 0xB7, 0x9F, 0x10, 0xD8, 0xF1, 0xB0, 0xBA, 0xAE, 0xB0, 0x8A, 0xC2, 0xB2, 0xB6, +0x8E, 0x9E, 0xF1, 0xFB, 0xD9, 0xF4, 0x1D, 0xD8, 0xF9, 0xD9, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, +0x61, 0xD9, 0xAE, 0xFB, 0xD8, 0xF4, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, 0x19, 0xD9, 0xAE, 0xFB, +0xDF, 0xD8, 0xF4, 0x16, 0xF1, 0xD8, 0xF8, 0xAD, 0x8D, 0x61, 0xD9, 0xF4, 0xF4, 0xAC, 0xF5, 0x9C, +0x9C, 0x8D, 0xDF, 0x2B, 0xBA, 0xB6, 0xAE, 0xFA, 0xF8, 0xF4, 0x0B, 0xD8, 0xF1, 0xAE, 0xD0, 0xF8, +0xAD, 0x51, 0xDA, 0xAE, 0xFA, 0xF8, 0xF1, 0xD8, 0xB9, 0xB1, 0xB6, 0xA3, 0x83, 0x9C, 0x08, 0xB9, +0xB1, 0x83, 0x9A, 0xB5, 0xAA, 0xC0, 0xFD, 0x30, 0x83, 0xB7, 0x9F, 0x10, 0xB5, 0x8B, 0x93, 0xF2, +0x02, 0x02, 0xD1, 0xAB, 0xDA, 0xDE, 0xD8, 0xF1, 0xB0, 0x80, 0xBA, 0xAB, 0xC0, 0xC3, 0xB2, 0x84, +0xC1, 0xC3, 0xD8, 0xB1, 0xB9, 0xF3, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, +0x87, 0x9C, 0xB9, 0xA3, 0xDD, 0xF1, 0xB3, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0xB0, 0x87, 0x20, 0x28, +0x30, 0x38, 0xB2, 0x8B, 0xB6, 0x9B, 0xF2, 0xA3, 0xC0, 0xC8, 0xC2, 0xC4, 0xCC, 0xC6, 0xA3, 0xA3, +0xA3, 0xF1, 0xB0, 0x87, 0xB5, 0x9A, 0xD8, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 0xBA, 0xAC, 0xDF, 0xB9, //Reverted back as packet size changes causing isues... TODO:change 2742 from 0xD8 to 0x20 Including the DMP_FEATURE_TAP -- known issue in which if you do not enable DMP_FEATURE_TAP then the interrupts will be at 200Hz even if fifo rate +0xA3, 0xFE, 0xF2, 0xAB, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, 0xBB, 0xAF, 0xDF, 0xDF, 0xA3, 0xA3, 0xA3, +0xD8, 0xD8, 0xD8, 0xBB, 0xB3, 0xB7, 0xF1, 0xAA, 0xF9, 0xDA, 0xFF, 0xD9, 0x80, 0x9A, 0xAA, 0x28, +0xB4, 0x80, 0x98, 0xA7, 0x20, 0xB7, 0x97, 0x87, 0xA8, 0x66, 0x88, 0xF0, 0x79, 0x51, 0xF1, 0x90, +0x2C, 0x87, 0x0C, 0xA7, 0x81, 0x97, 0x62, 0x93, 0xF0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, +/* bank # 11 */ +0x51, 0x79, 0x90, 0xA5, 0xF1, 0x28, 0x4C, 0x6C, 0x87, 0x0C, 0x95, 0x18, 0x85, 0x78, 0xA3, 0x83, +0x90, 0x28, 0x4C, 0x6C, 0x88, 0x6C, 0xD8, 0xF3, 0xA2, 0x82, 0x00, 0xF2, 0x10, 0xA8, 0x92, 0x19, +0x80, 0xA2, 0xF2, 0xD9, 0x26, 0xD8, 0xF1, 0x88, 0xA8, 0x4D, 0xD9, 0x48, 0xD8, 0x96, 0xA8, 0x39, +0x80, 0xD9, 0x3C, 0xD8, 0x95, 0x80, 0xA8, 0x39, 0xA6, 0x86, 0x98, 0xD9, 0x2C, 0xDA, 0x87, 0xA7, +0x2C, 0xD8, 0xA8, 0x89, 0x95, 0x19, 0xA9, 0x80, 0xD9, 0x38, 0xD8, 0xA8, 0x89, 0x39, 0xA9, 0x80, +0xDA, 0x3C, 0xD8, 0xA8, 0x2E, 0xA8, 0x39, 0x90, 0xD9, 0x0C, 0xD8, 0xA8, 0x95, 0x31, 0x98, 0xD9, +0x0C, 0xD8, 0xA8, 0x09, 0xD9, 0xFF, 0xD8, 0x01, 0xDA, 0xFF, 0xD8, 0x95, 0x39, 0xA9, 0xDA, 0x26, +0xFF, 0xD8, 0x90, 0xA8, 0x0D, 0x89, 0x99, 0xA8, 0x10, 0x80, 0x98, 0x21, 0xDA, 0x2E, 0xD8, 0x89, +0x99, 0xA8, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x86, 0x96, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, +0x87, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x82, 0x92, 0xF3, 0x41, 0x80, 0xF1, 0xD9, 0x2E, 0xD8, +0xA8, 0x82, 0xF3, 0x19, 0x80, 0xF1, 0xD9, 0x2E, 0xD8, 0x82, 0xAC, 0xF3, 0xC0, 0xA2, 0x80, 0x22, +0xF1, 0xA6, 0x2E, 0xA7, 0x2E, 0xA9, 0x22, 0x98, 0xA8, 0x29, 0xDA, 0xAC, 0xDE, 0xFF, 0xD8, 0xA2, +0xF2, 0x2A, 0xF1, 0xA9, 0x2E, 0x82, 0x92, 0xA8, 0xF2, 0x31, 0x80, 0xA6, 0x96, 0xF1, 0xD9, 0x00, +0xAC, 0x8C, 0x9C, 0x0C, 0x30, 0xAC, 0xDE, 0xD0, 0xDE, 0xFF, 0xD8, 0x8C, 0x9C, 0xAC, 0xD0, 0x10, +0xAC, 0xDE, 0x80, 0x92, 0xA2, 0xF2, 0x4C, 0x82, 0xA8, 0xF1, 0xCA, 0xF2, 0x35, 0xF1, 0x96, 0x88, +0xA6, 0xD9, 0x00, 0xD8, 0xF1, 0xFF, +}; diff --git a/lib/heading/heading.h b/lib/heading/heading.h index 193faca..4afee0f 100644 --- a/lib/heading/heading.h +++ b/lib/heading/heading.h @@ -68,12 +68,50 @@ struct QMC5883LCompass : Compass int8_t readXYZ() override; }; +struct UninitializedCompass : Compass +{ + UninitializedCompass() : Compass() {} + bool begin() override; + String selfTest() override; + + uint8_t setMode(CompassMode m) override; + int8_t readXYZ() override; +}; + +#ifdef COMPASS_ENABLED +#include +#include +#define HMC5883Type Adafruit_HMC5883_Unified +#else +#define HMC5883Type UninitializedCompass +#endif + +extern HMC5883Type _mag; + +struct HMC5883LCompass : Compass +{ + HMC5883Type &mag; + long calStart = 0; + // Variables for dynamic calibration + float x_min = 1000, x_max = -1000; + float y_min = 1000, y_max = -1000; + float z_min = 1000, z_max = -1000; + + HMC5883LCompass() : Compass(), mag(_mag) {} + + bool begin() override; + String selfTest() override; + + uint8_t setMode(CompassMode m) override; + int8_t readXYZ() override; +}; + struct DroneHeading : HeadingSensor { int64_t _lastRead; int16_t _heading; - DroneHeading() : HeadingSensor(), _lastRead(-1) {} + DroneHeading() : HeadingSensor(), _lastRead(-1), _heading(-999) {} void setHeading(int64_t now, int16_t heading); @@ -81,6 +119,8 @@ struct DroneHeading : HeadingSensor int16_t heading() override; }; +int16_t meanHeading(int16_t m, int16_t mm); + #define QMC5883_ADDR 0xD #define QMC5883_X_LSB_REG 0 #define QMC5883_X_MSB_REG 1 diff --git a/src/main.cpp b/src/main.cpp index 396e18b..9330205 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -62,9 +62,9 @@ #include #include -#ifdef BT_MOBILE +bool bleDeviceConnected = false; -bool deviceConnected = false; +#ifdef BT_MOBILE #define SERVICE_UUID "00001234-0000-1000-8000-00805f9b34fb" #define CHARACTERISTIC_UUID "00001234-0000-1000-8000-00805f9b34ac" @@ -80,11 +80,11 @@ BLEAdvertising *pAdvertising = NULL; class MyServerCallbacks : public BLEServerCallbacks { - void onConnect(BLEServer *pServer) { deviceConnected = true; }; + void onConnect(BLEServer *pServer) { bleDeviceConnected = true; }; void onDisconnect(BLEServer *pServer) { - deviceConnected = false; + bleDeviceConnected = false; BLEDevice::startAdvertising(); // Restart advertising after disconnect } }; @@ -101,7 +101,7 @@ class BTServerCallbacks : public NimBLEServerCallbacks public: void onConnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo) override { - deviceConnected = true; + bleDeviceConnected = true; Serial.printf("Device Connected | Free Heap: %d kByte\n", ESP.getFreeHeap() / 1000); Serial.printf("Client address: %s\n", connInfo.getAddress().toString().c_str()); @@ -112,7 +112,7 @@ class BTServerCallbacks : public NimBLEServerCallbacks void onDisconnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo, int reason) override { - deviceConnected = false; + bleDeviceConnected = false; Serial.println("Device Disconnected"); NimBLEDevice::startAdvertising(); // Restart advertising } @@ -265,6 +265,8 @@ void initBT() #endif } +#endif + // Function to send RSSI and Heading Data void sendBTData(float heading, float rssi) { @@ -274,10 +276,45 @@ void sendBTData(float heading, float rssi) #ifdef COMPASS_DEBUG Serial.println("Sending data: " + data); #endif +#ifdef BT_MOBILE pCharacteristic->setValue(data.c_str()); // Set BLE characteristic value pCharacteristic->notify(); // Notify connected client +#endif } + +// Send Scan Result to BLE +void sendBTData(Message &msg) +{ + if (msg.type != SCAN_HEADING_MAX && msg.type != SCAN_MAX_RESULT && + msg.type != SCAN_RESULT) + { + Serial.println("Unsupported message type: " + String(msg.type)); + return; + } + + String data = "{\"SCAN_RESULT\":{\"Hmin\":" + String(msg.payload.dump.heading_min) + + ",\"Hmax\":" + String(msg.payload.dump.heading_max) + ",\"Spectrum\":["; + + for (int i = 0; i < msg.payload.dump.sz; i++) + { + data += String(i == 0 ? "" : ",") + + "{\"F\":" + String(msg.payload.dump.freqs_khz[i]) + + ",\"R\":" + String(msg.payload.dump.rssis[i]) + + (msg.payload.dump.rssis2 == NULL + ? "" + : ",\"R2\":" + String(msg.payload.dump.rssis2[i])) + + "}"; + } + + data += "]}}"; +#ifdef COMPASS_DEBUG + Serial.println("Sending data: " + data); #endif +#ifdef BT_MOBILE + pCharacteristic->setValue(data.c_str()); // Set BLE characteristic value + pCharacteristic->notify(); // Notify connected client +#endif +} #ifndef LILYGO #include @@ -304,6 +341,7 @@ void sendBTData(float heading, float rssi) DroneHeading droneHeading; Compass *compass = NULL; +HeadingSensor &headingSensor = droneHeading; RadioModule *radio2; @@ -390,41 +428,14 @@ DFRobot_OSD osd(OSD_CS); #include "global_config.h" #include "ui.h" -#ifdef COMPASS_ENABLED -#include -#include -/* Assign a unique ID to this sensor at the same time */ -Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345); void displaySensorDetails(void) { - sensor_t sensor; - mag.getSensor(&sensor); - Serial.println("------------------------------------"); - Serial.print("Sensor: "); - Serial.println(sensor.name); - Serial.print("Driver Ver: "); - Serial.println(sensor.version); - Serial.print("Unique ID: "); - Serial.println(sensor.sensor_id); - Serial.print("Max Value: "); - Serial.print(sensor.max_value); - Serial.println(" uT"); - Serial.print("Min Value: "); - Serial.print(sensor.min_value); - Serial.println(" uT"); - Serial.print("Resolution: "); - Serial.print(sensor.resolution); - Serial.println(" uT"); - Serial.println("------------------------------------"); - Serial.println(""); + if (compass == NULL) + return; + + Serial.println(compass->selfTest()); heltec_delay(500); } - -// Variables for dynamic calibration -float x_min = 1000, x_max = -1000; -float y_min = 1000, y_max = -1000; -float z_min = 1000, z_max = -1000; -#endif // ----------------------------------------------------------------- // CONFIGURATION OPTIONS // ----------------------------------------------------------------- @@ -1149,6 +1160,8 @@ void eventListenerForReport(void *arg, Event &e) if (e.epoch != frequency_scan_result.last_epoch) { frequency_scan_result.dump.sz = 0; + frequency_scan_result.dump.heading_min = -999; + frequency_scan_result.dump.heading_max = -999; } if (frequency_scan_result.dump.sz >= frequency_scan_result.readings_sz) @@ -1196,6 +1209,23 @@ void eventListenerForReport(void *arg, Event &e) frequency_scan_result.rssi = e.detected.rssi; } + int16_t heading = headingSensor.heading(); + if (heading > -999) + { + if (frequency_scan_result.dump.heading_min == -999) + { + frequency_scan_result.dump.heading_min = heading; + frequency_scan_result.dump.heading_max = heading; + } + else + { + frequency_scan_result.dump.heading_min = + min(frequency_scan_result.dump.heading_min, heading); + frequency_scan_result.dump.heading_max = + min(frequency_scan_result.dump.heading_max, heading); + } + } + return; } @@ -1850,32 +1880,14 @@ void setup(void) r.addEventListener(ALL_EVENTS, eventListenerForReport, NULL); -#ifdef COMPASS_ENABLED - - Serial.println("Compass Init Start"); - Wire1.end(); - Wire1.begin(46, 42); - - Serial.println("Compass BEGIN"); - Serial.println("HMC5883 Magnetometer Test"); - - /* Initialise the sensor */ - if (!mag.begin()) - { - /* There was a problem detecting the HMC5883 ... check your connections */ - Serial.println("Ooops, no HMC5883 detected ... Check your wiring!"); - } - - /* Display some basic information on this sensor */ - displaySensorDetails(); - Serial.println("Compass Success!!!"); - -#endif - if (wireDevices & QMC5883L || wire1Devices & QMC5883L) { compass = new QMC5883LCompass(wireDevices & QMC5883L ? Wire : Wire1); } + else if (wireDevices & HMC5883L) + { + compass = new HMC5883LCompass(); + } if (compass) { @@ -1888,6 +1900,7 @@ void setup(void) if (err.startsWith("OK\n")) { Serial.printf("Compass self-test passed: %s\n", err.c_str()); + headingSensor = *compass; } else { @@ -2072,7 +2085,8 @@ void routeMessage(RoutedMessage &m) } if (m.message->type == MessageType::SCAN_RESULT || - m.message->type == MessageType::SCAN_MAX_RESULT) + m.message->type == MessageType::SCAN_MAX_RESULT || + m.message->type == MessageType::SCAN_HEADING_MAX) { m.to.host = 1; return; @@ -2266,13 +2280,24 @@ void sendMessage(RoutedMessage &m) break; case SCAN_RESULT: case SCAN_MAX_RESULT: + case SCAN_HEADING_MAX: if (config.is_host) { + if (msg->type == SCAN_HEADING_MAX) + { + droneHeading.setHeading(millis(), + meanHeading(msg->payload.dump.heading_min, + msg->payload.dump.heading_max)); + } #ifdef DISPLAY_RAW_SCAN display_raw_scan(m.message->payload.dump); #else display_scan_result(m.message->payload.dump); #endif + if (bleDeviceConnected) + { + sendBTData(*msg); + } } break; case HEADING: @@ -2386,111 +2411,8 @@ int max_rssi_x = 999; void doScan(); void reportScan(); -long calStart = 0; - -#ifdef COMPASS_ENABLED -float getCompassHeading() -{ - if (calStart == 0) - { - calStart = millis(); - } - - /* Get a new sensor event */ - sensors_event_t event2; - mag.getEvent(&event2); - sensors_event_t event3; - mag.getEvent(&event3); -#ifdef COMPASS_DEBUG - /* Display the results (magnetic vector values are in micro-Tesla (uT)) */ - Serial.print("X: "); - Serial.print(event2.magnetic.x); - Serial.print(" "); - Serial.print("Y: "); - Serial.print(event2.magnetic.y); - Serial.print(" "); - Serial.print("Z: "); - Serial.print(event2.magnetic.z); - Serial.print(" "); - Serial.println("uT"); -#endif - - // Hold the module so that Z is pointing 'up' and you can measure the heading with - // x&y Calculate heading when the magnetometer is level, then correct for signs of - // axis. float heading = atan2(event.magnetic.y, event.magnetic.x); Use Y as the - // forward axis float heading = atan2(event.magnetic.x, event.magnetic.y); - /// If Z-axis is forward and Y-axis points upward: - // float heading = atan2(event.magnetic.x, event.magnetic.y); - // If Z-axis is forward and X-axis points upward: - // float heading = atan2(event.magnetic.y, -event.magnetic.x); - - // heading based on the magnetic readings from the Z-axis (forward) and the X-axis - // (perpendicular to Z, horizontal). - // float heading = atan2(event.magnetic.z, event.magnetic.x); - - // Dynamicly Calibrated out - - // Read raw magnetometer data - float x = (event2.magnetic.x + event3.magnetic.x) / 2; - float y = (event2.magnetic.y + event3.magnetic.y) / 2; - float z = (event2.magnetic.z + event3.magnetic.z) / 2; - - // Doing calibration first 1 minute - if (millis() - calStart < 60000) - { - // Update min/max values dynamically - x_min = min(x_min, x); - x_max = max(x_max, x); - y_min = min(y_min, y); - y_max = max(y_max, y); - z_min = min(z_min, z); - z_max = max(z_max, z); - } - -#ifdef COMPASS_DEBUG - Serial.println("x_min:" + String(x_min) + " x_max: " + String(x_max) + - " y_min: " + String(y_min)); -#endif - - // Calculate offsets and scales in real-time - float x_offset = (x_max + x_min) / 2; - float y_offset = (y_max + y_min) / 2; - float z_offset = (z_max + z_min) / 2; - - float x_scale = (x_max - x_min) / 2; - float y_scale = (y_max - y_min) / 2; - float z_scale = (z_max - z_min) / 2; - - // Apply calibration to raw data - float calibrated_x = (x - x_offset) / x_scale; - float calibrated_y = (y - y_offset) / y_scale; - float calibrated_z = (z - z_offset) / z_scale; - - // Calculate heading using Z-axis forward, X-axis horizontal - float heading = atan2(calibrated_z, calibrated_x); - - // Once you have your heading, you must then add your 'Declination Angle', which - // is the 'Error' of the magnetic field in your location. Find yours here: - // http://www.magnetic-declination.com/ Mine is: -13* 2' W, which is ~13 Degrees, - // or (which we need) 0.22 radians If you cannot find your Declination, comment - // out these two lines, your compass will be slightly off. - float declinationAngle = 0.22; - heading += declinationAngle; - - // Correct for when signs are reversed. - if (heading < 0) - heading += 2 * PI; - - // Check for wrap due to addition of declination. - if (heading > 2 * PI) - heading -= 2 * PI; - - // Convert radians to degrees for readability. - float headingDegrees = heading * 180 / M_PI; - return headingDegrees; -} -#endif +void processHeading(); float historicalCompassRssi[STEPS] = {999}; int compassCounter = 0; @@ -2569,8 +2491,14 @@ void loop(void) } #endif #endif + if (compass != NULL || droneHeading.lastRead() > -1) + { + processHeading(); + } +} -#ifdef COMPASS_ENABLED +void processHeading() +{ #if defined(COMPASS_FREQ) // delay(1000); display.clear(); @@ -2587,7 +2515,7 @@ void loop(void) { // ToDO: fix go to; compass: - float headingDegrees = getCompassHeading(); + float headingDegrees = headingSensor.heading(); // Serial.println("Heading (degrees): " + String(headingDegrees)); #ifndef COMPASS_FREQ t = 0; @@ -2625,7 +2553,7 @@ void loop(void) #else rssi = getRssi(false); #endif - float headingDegreesAfter = getCompassHeading(); + float headingDegreesAfter = headingSensor.heading(); float compassDiff = abs(headingDegreesAfter - headingDegrees); if (compassDiff >= 3) { @@ -2838,8 +2766,6 @@ void loop(void) #if defined(COMPASS_FREQ) display.clear(); #endif // COMPASS_FREQ - -#endif // end COMPASS_ENABLED } void doScan() @@ -3411,6 +3337,8 @@ void reportScan() Message m; m.type = SCAN_RESULT; m.payload.dump.sz = 0; + m.payload.dump.rssis2 = NULL; + m.payload.dump.heading_min = -999; if (config.detection_strategy.equalsIgnoreCase("RSSI")) { @@ -3428,6 +3356,13 @@ void reportScan() { m.type = SCAN_MAX_RESULT; + m.payload.dump.heading_min = frequency_scan_result.dump.heading_min; + m.payload.dump.heading_max = frequency_scan_result.dump.heading_max; + if (m.payload.dump.heading_min > -999) + { + m.type = SCAN_HEADING_MAX; + } + size_t sz = config.scan_ranges_sz; m.payload.dump.sz = sz; m.payload.dump.freqs_khz = new uint32_t[sz]; diff --git a/web_app/compass/index.html b/web_app/compass/index.html index 4f16b4d..10365a2 100644 --- a/web_app/compass/index.html +++ b/web_app/compass/index.html @@ -160,36 +160,77 @@

RSSI Radar

ctx.stroke(); } + // rotate everything relative to angleOffset, in radians + const angleOffset = -Math.round(currentPoint.angle / 5) * 5 * Math.PI / 180; + // Compass lines for (let angle = 0; angle < 360; angle += 45) { const rad = (angle * Math.PI) / 180; ctx.beginPath(); ctx.moveTo(centerX, centerY); - ctx.lineTo(centerX + radius * Math.cos(rad), centerY + radius * Math.sin(rad)); - ctx.strokeStyle = "#e0ffe0"; - ctx.lineWidth = 1; + ctx.lineTo(centerX + radius * Math.sin(rad + angleOffset), centerY - radius * Math.cos(rad + angleOffset)); + ctx.strokeStyle = angle % 90 == 0 ? "white" : "green"; + ctx.lineWidth = angle == 0 ? 3 : 1; ctx.stroke(); } let lineCoef = 2.5; + + var minf = -1, maxf = -1; + dataPoints.forEach(({ spectrum }) => { + if (!spectrum) return; + + spectrum.forEach(({ F }) => { + if (minf < 0 || minf > F) minf = F; + maxf = Math.max(maxf, F); + }); + }); + // Draw data points - dataPoints.forEach(({ angle, rssi }) => { + dataPoints.forEach(({ angle, rssi, t0, spectrum }) => { + if (!currentPoint.spectrum || !spectrum) return; + const rad = (angle * Math.PI) / 180; //const length = (120 + rssi) / (radius / 2) * radius; - const length = ((120 - 90) + (rssi + 90)) * lineCoef; // Scale RSSI to fit within radar - if (length > radius) { - length = radius; + if (minf == maxf) { + const length = Math.max(radius - 10, ((120 - 90) + (rssi + 90)) * lineCoef); // Scale RSSI to fit within radar + + //console.log("Length: " + length); + const x = centerX + length * Math.sin(rad + angleOffset); + const y = centerY - length * Math.cos(rad + angleOffset); + + ctx.beginPath(); + ctx.moveTo(centerX, centerY); + ctx.lineTo(x, y); + // calculating linear decay factor: + // for 5000 < dt < 45000 - linear decay from 1.0 to 0.5 + // for dt outside those - flat 1.0, or flat 0.5 + const colorDecay = 1 - Math.min(Math.max((currentPoint.t0 - t0 - 5000) / (2 * 40000), 0), 0.5); + ctx.strokeStyle = `rgba(${Math.trunc(255 * colorDecay)}, ${Math.trunc((1 - colorDecay) * 255)}, ${Math.trunc((1 - colorDecay) * 255)}, 1)`; + ctx.lineWidth = 2; + ctx.stroke(); + } else { + // now, here's radical new view: + // colour: the redder it is, the more recent the reading is + // intensity: the more intense it is, the higher the RSSI + // radius: the distance from centre represents frequency for which the reading was made + // angle: direction + + spectrum.forEach(({ F, R }) => { + const length = (F - minf) / (maxf - minf) * (radius - 20 - 10) + 20; + + ctx.beginPath(); + ctx.arc(centerX, centerY, length, rad + angleOffset - 0.5 * Math.PI / 180 - Math.PI / 2, rad + angleOffset + 0.5 * Math.PI / 180 - Math.PI / 2, false); + // calculating linear decay factor: + // for 5000 < dt < 45000 - linear decay from 1.0 to 0.5 + // for dt outside those - flat 1.0, or flat 0.5 + const colorDecay = 1 - Math.min(Math.max((currentPoint.t0 - t0 - 5000) / (2 * 40000), 0), 0.5); + const colorIntensity = 1 + Math.max(-1, Math.min(0, R + 20) / (70 - 20)); + ctx.strokeStyle = `rgba(${Math.trunc(255 * colorDecay)}, ${Math.trunc((1 - colorDecay) * 255)}, ${Math.trunc((1 - colorDecay) * 255)}, ${colorIntensity})`; + ctx.lineWidth = 5; + ctx.stroke(); + }); } - console.log("Length: " + length); - const x = centerX + length * Math.cos(rad); - const y = centerY + length * Math.sin(rad); - - ctx.beginPath(); - ctx.moveTo(centerX, centerY); - ctx.lineTo(x, y); - ctx.strokeStyle = "red"; - ctx.lineWidth = 2; - ctx.stroke(); }); const maxPoint = dataPoints.reduce((max, point) => (point.rssi > max.rssi ? point : max), { angle: 0, rssi: -120 }); @@ -198,8 +239,8 @@

RSSI Radar

headingDisplayMAX.textContent = `${maxPoint.angle.toFixed(1)}°`; rssiDisplayMAX.textContent = `${maxPoint.rssi.toFixed(1)} dBm`; - const maxX = centerX + maxLength * Math.cos(maxRad); - const maxY = centerY + maxLength * Math.sin(maxRad); + const maxX = centerX + maxLength * Math.sin(maxRad + angleOffset); + const maxY = centerY - maxLength * Math.cos(maxRad + angleOffset); ctx.beginPath(); ctx.moveTo(centerX, centerY); @@ -212,11 +253,11 @@

RSSI Radar

const arrowHeadLength = 10; const arrowAngle = Math.PI / 6; // 30 degrees for the arrowhead - const arrowX1 = maxX - arrowHeadLength * Math.cos(maxRad - arrowAngle); - const arrowY1 = maxY - arrowHeadLength * Math.sin(maxRad - arrowAngle); + const arrowX1 = maxX - arrowHeadLength * Math.sin(maxRad - arrowAngle + angleOffset); + const arrowY1 = maxY + arrowHeadLength * Math.cos(maxRad - arrowAngle + angleOffset); - const arrowX2 = maxX - arrowHeadLength * Math.cos(maxRad + arrowAngle); - const arrowY2 = maxY - arrowHeadLength * Math.sin(maxRad + arrowAngle); + const arrowX2 = maxX - arrowHeadLength * Math.sin(maxRad + arrowAngle + angleOffset); + const arrowY2 = maxY + arrowHeadLength * Math.cos(maxRad + arrowAngle + angleOffset); ctx.beginPath(); ctx.moveTo(maxX, maxY); @@ -231,8 +272,8 @@

RSSI Radar

const currentRad = (currentPoint.angle * Math.PI) / 180; const currentLength = ((120 + currentPoint.rssi) / (radius / 2)) * radius * 1.2; - const currentX = centerX + currentLength * Math.cos(currentRad); - const currentY = centerY + currentLength * Math.sin(currentRad); + const currentX = centerX + currentLength * Math.sin(currentRad + angleOffset); + const currentY = centerY - currentLength * Math.cos(currentRad + angleOffset); ctx.beginPath(); ctx.moveTo(centerX, centerY); @@ -240,6 +281,27 @@

RSSI Radar

ctx.strokeStyle = "yellow"; ctx.lineWidth = 3; ctx.stroke(); + + const headingString = `${currentPoint.angle.toFixed(1)}°`; + const rssiString = `${currentPoint.rssi.toFixed(1)} dBm`; + ctx.fillStyle = "white"; + ctx.font = "20px Arial"; + + const m = ctx.measureText(headingString); + ctx.fillText(headingString, centerX - m.width / 2, 30); + const m1 = ctx.measureText(rssiString); + ctx.fillText(rssiString, centerX - m1.width / 2, 60); + + const maxHeadingString = `${maxPoint.angle.toFixed(1)}°`; + const maxRssiString = `${maxPoint.rssi.toFixed(1)} dBm`; + ctx.fillStyle = "blue"; + ctx.font = "20px Arial"; + + const maxOffset = Math.max(m.width, m1.width) * ((currentPoint.angle - maxPoint.angle + 360) % 360 > 180 ? -1 : 1); + const m2 = ctx.measureText(maxHeadingString); + ctx.fillText(maxHeadingString, centerX - m2.width / 2 - maxOffset, 60); + const m3 = ctx.measureText(maxRssiString); + ctx.fillText(maxRssiString, centerX - m3.width / 2 - maxOffset, 90); } // Handle canvas hover for pointer change @@ -327,6 +389,42 @@

RSSI Radar

document.body.appendChild(promptDiv); } + function angleDiff(a, b) { + a = (a + 360) % 360; + b = (b + 360) % 360; + if (b < a) { + a += b; + b = a - b; + a -= b; + } + + return b - a > 180 ? a + 360 - b : b - a; + } + + function simulateRssi(foxes, h_angle) { + const rssis = foxes.map(({ angle, f_mhz, rssi }, i) => { + const fox = angleDiff(angle, h_angle); + return { + f_mhz: f_mhz, + rssi: Math.max(-90, (fox > 35 ? 0 : Math.cos((Math.PI / 2) * fox / 35) * (rssi + 90)) - Math.random() * 3 - 90) + }; + }); + + rssis.sort((a, b) => a.f_mhz - b.f_mhz); + + return rssis.reduce((v, fox) => { + if (v.length == 0 || v[v.length - 1].f_mhz != fox.f_mhz) { + v.push(fox); + return v; + } + + const prev = v[v.length - 1]; + prev.rssi = Math.log(Math.exp(prev.rssi) + Math.exp(fox.rssi)); + + return v; + }, []); + } + // Parse Bluetooth data function parseBTData(data) { // Match the data format and extract the heading and RSSI values @@ -336,8 +434,33 @@

RSSI Radar

const heading = parseInt(match[1]); const rssi = parseFloat(match[2]); console.log("H:" + heading + " R:" + rssi); - dataPoints[parseInt(heading)] = { angle: parseInt(heading), rssi: rssi }; - currentPoint = { angle: parseInt(heading), rssi: rssi }; + data = '{"SCAN_RESULT":{"Hmin":' + match[1] + ',"Hmax":' + match[1] + ',"Spectrum":[{"F":0,"R":' + match[2] + '}]}}'; + } + + try { + data = JSON.parse(data); + } catch (e) { + console.log("Skipping broken JSON:", e, "in", data); + return; + } + + if (data["SCAN_RESULT"]) { + scanResult = data["SCAN_RESULT"]; + const spectrum = scanResult["Spectrum"]; + if (spectrum.length == 0) { + console.log("Skipping scan result with no spectrum:", data); + return; + } + + const headingMin = scanResult.Hmin; + const headingMax = scanResult.Hmax; + + const heading = ((headingMax + headingMin + 720 + (headingMax - headingMin > 180 ? 360 : 0)) / 2) % 360; + const rssi = spectrum[0]["R"]; + + const t0 = Date.now(); + dataPoints[Math.trunc(heading)] = { angle: heading, rssi: rssi, t0: t0, spectrum: spectrum }; + currentPoint = { angle: heading, rssi: rssi, t0: t0, spectrum: spectrum }; //if (dataPoints.length > 50) dataPoints.shift(); // Keep only the last 50 points headingDisplay.textContent = `${heading.toFixed(1)}°`; rssiDisplay.textContent = `${rssi.toFixed(1)} dBm`; @@ -385,18 +508,32 @@

RSSI Radar

isSimulating = true; simulateBtn.textContent = "Stop Simulation"; - (function simulate() { + dataPoints = []; + for (i = 0; i < 360; i++) { + dataPoints[i] = { angle: i, rssi: -120 }; + } + + const foxes = [ + { angle: Math.random() * 360, f_mhz: 240, rssi: -60 }, + { angle: Math.random() * 360, f_mhz: 320, rssi: -45 }, + { angle: Math.random() * 360, f_mhz: 120, rssi: -20 }, + { angle: Math.random() * 360, f_mhz: 120, rssi: -35 } + ]; + (function simulate(foxes, prevAngle, prevRssi) { if (!isSimulating) return; - const angle = Math.random() * 360; - const rssi = -70 + Math.random() * 30; - dataPoints.push({ angle, rssi }); - currentPoint = { angle, rssi }; - if (dataPoints.length > 360 * 5) dataPoints.shift(); - headingDisplay.textContent = `${angle.toFixed(1)}°`; - rssiDisplay.textContent = `${rssi.toFixed(1)} dBm`; - drawRadar(); - setTimeout(simulate, 100); - })(); + const angle = (prevAngle - 3 + Math.random() * 7.5) % 360; // bias slightly to scan on + const spectrum = simulateRssi(foxes, angle); + + const data = spectrum.length == 1 ? "RSSI_HEADING: '{H:" + angle + ",RSSI:" + spectrum[0].rssi + "}'" : JSON.stringify({ + SCAN_RESULT: { + Hmin: Math.min(prevAngle, angle), + Hmax: Math.max(prevAngle, angle), + Spectrum: spectrum.map(({ f_mhz, rssi }) => ({ F: f_mhz, R: rssi })) + } + }); + parseBTData(data); // test actual BT data processing + setTimeout(simulate, 100, foxes, angle, rssi); + })(foxes, Math.random() * 360, -70 + Math.random() * 30); } });