feat: 全量同步 254 个常用的 Arduino 扩展库文件

arduino-libs/arduino-cli/libraries/MPU9250_asukiaaa/src/MPU9250_asukiaaa.cpp

@@ -0,0 +1,230 @@
+#include "MPU9250_asukiaaa.h"
+#include <math.h>
+
+#define AK8963_ADDRESS  0x0C
+#define AK8963_RA_HXL   0x03
+#define AK8963_RA_CNTL1 0x0A
+#define AK8963_RA_ASAX  0x10
+
+#define MPU9250_ADDR_ACCELCONFIG  0x1C
+#define MPU9250_ADDR_INT_PIN_CFG  0x37
+#define MPU9250_ADDR_ACCEL_XOUT_H 0x3B
+#define MPU9250_ADDR_GYRO_XOUT_H  0x43
+#define MPU9250_ADDR_PWR_MGMT_1   0x6B
+#define MPU9250_ADDR_WHOAMI       0x75
+
+uint8_t MPU9250_asukiaaa::i2cRead(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data) {
+  myWire->beginTransmission(Address);
+  myWire->write(Register);
+  uint8_t result = myWire->endTransmission();
+  if (result != 0) {
+    return result;
+  }
+
+  myWire->requestFrom(Address, Nbytes);
+  uint8_t index = 0;
+  while (myWire->available()) {
+    uint8_t d = myWire->read();
+    if (index < Nbytes) {
+      Data[index++] = d;
+    }
+  }
+  return 0;
+}
+
+uint8_t MPU9250_asukiaaa::i2cWriteByte(uint8_t Address, uint8_t Register, uint8_t Data) {
+  myWire->beginTransmission(Address);
+  myWire->write(Register);
+  myWire->write(Data);
+  return myWire->endTransmission();
+}
+
+MPU9250_asukiaaa::MPU9250_asukiaaa(uint8_t address):
+  address(address) {
+  accelRange = 0;
+  gyroRange  = 0;
+  magXOffset = 0;
+  magYOffset = 0;
+  magZOffset = 0;
+  myWire = NULL;
+}
+
+void MPU9250_asukiaaa::setWire(TwoWire* wire) {
+  myWire = wire;
+}
+
+uint8_t MPU9250_asukiaaa::readId(uint8_t *id) {
+  beginWireIfNull();
+  return i2cRead(address, MPU9250_ADDR_WHOAMI, 1, id);
+}
+
+void MPU9250_asukiaaa::beginWireIfNull() {
+  if (myWire == NULL) {
+    myWire = &Wire;
+    myWire->begin();
+  }
+}
+
+void MPU9250_asukiaaa::beginAccel(uint8_t mode) {
+  beginWireIfNull();
+  switch(mode) {
+  case ACC_FULL_SCALE_2_G:
+    accelRange = 2.0;
+    break;
+  case ACC_FULL_SCALE_4_G:
+    accelRange = 4.0;
+    break;
+  case ACC_FULL_SCALE_8_G:
+    accelRange = 8.0;
+    break;
+  case ACC_FULL_SCALE_16_G:
+    accelRange = 16.0;
+    break;
+  default:
+    return; // Return without writing invalid mode
+  }
+  i2cWriteByte(address, MPU9250_ADDR_ACCELCONFIG, mode);
+  delay(10);
+}
+
+void MPU9250_asukiaaa::magReadAdjustValues() {
+  magSetMode(MAG_MODE_POWERDOWN);
+  magSetMode(MAG_MODE_FUSEROM);
+  uint8_t buff[3];
+  i2cRead(AK8963_ADDRESS, AK8963_RA_ASAX, 3, buff);
+  magXAdjust = buff[0];
+  magYAdjust = buff[1];
+  magZAdjust = buff[2];
+}
+
+void MPU9250_asukiaaa::beginMag(uint8_t mode) {
+  beginWireIfNull();
+  magWakeup();
+  magEnableSlaveMode();
+
+  magReadAdjustValues();
+  magSetMode(MAG_MODE_POWERDOWN);
+  magSetMode(mode);
+  delay(10);
+}
+
+void MPU9250_asukiaaa::magSetMode(uint8_t mode) {
+  i2cWriteByte(AK8963_ADDRESS, AK8963_RA_CNTL1, mode);
+  delay(10);
+}
+
+void MPU9250_asukiaaa::magWakeup() {
+  unsigned char bits;
+  i2cRead(address, MPU9250_ADDR_PWR_MGMT_1, 1, &bits);
+  bits &= ~B01110000; // Turn off SLEEP, STANDBY, CYCLE
+  i2cWriteByte(address, MPU9250_ADDR_PWR_MGMT_1, bits);
+  delay(10);
+}
+
+void MPU9250_asukiaaa::magEnableSlaveMode() {
+  unsigned char bits;
+  i2cRead(address, MPU9250_ADDR_INT_PIN_CFG, 1, &bits);
+  bits |= B00000010; // Activate BYPASS_EN
+  i2cWriteByte(address, MPU9250_ADDR_INT_PIN_CFG, bits);
+  delay(10);
+}
+
+const float Pi = 3.14159;
+
+float MPU9250_asukiaaa::magHorizDirection() {
+  return atan2(magX(), magY()) * 180 / Pi;
+}
+
+uint8_t MPU9250_asukiaaa::magUpdate() {
+  return i2cRead(AK8963_ADDRESS, AK8963_RA_HXL, 7, magBuff);
+}
+
+int16_t MPU9250_asukiaaa::magGet(uint8_t highIndex, uint8_t lowIndex) {
+  return (((int16_t) magBuff[highIndex]) << 8) | magBuff[lowIndex];
+}
+
+float adjustMagValue(int16_t value, uint8_t adjust) {
+  return ((float) value * (((((float) adjust - 128) * 0.5) / 128) + 1));
+}
+
+float MPU9250_asukiaaa::magX() {
+  return adjustMagValue(magGet(1, 0), magXAdjust) + magXOffset;
+}
+
+float MPU9250_asukiaaa::magY() {
+  return adjustMagValue(magGet(3, 2), magYAdjust) + magYOffset;
+}
+
+float MPU9250_asukiaaa::magZ() {
+  return adjustMagValue(magGet(5, 4), magZAdjust) + magZOffset;
+}
+
+uint8_t MPU9250_asukiaaa::accelUpdate() {
+  return i2cRead(address, MPU9250_ADDR_ACCEL_XOUT_H, 6, accelBuff);
+}
+
+float MPU9250_asukiaaa::accelGet(uint8_t highIndex, uint8_t lowIndex) {
+  int16_t v = ((int16_t) accelBuff[highIndex]) << 8 | accelBuff[lowIndex];
+  return ((float) -v) * accelRange / (float) 0x8000; // (float) 0x8000 == 32768.0
+}
+
+float MPU9250_asukiaaa::accelX() {
+  return accelGet(0, 1);
+}
+
+float MPU9250_asukiaaa::accelY() {
+  return accelGet(2, 3);
+}
+
+float MPU9250_asukiaaa::accelZ() {
+  return accelGet(4, 5);
+}
+
+float MPU9250_asukiaaa::accelSqrt() {
+  return sqrt(pow(accelGet(0, 1), 2) +
+              pow(accelGet(2, 3), 2) +
+              pow(accelGet(4, 5), 2));
+}
+
+void MPU9250_asukiaaa::beginGyro(uint8_t mode) {
+  beginWireIfNull();
+  switch (mode) {
+  case GYRO_FULL_SCALE_250_DPS:
+    gyroRange = 250.0;
+    break;
+  case GYRO_FULL_SCALE_500_DPS:
+    gyroRange = 500.0;
+    break;
+  case GYRO_FULL_SCALE_1000_DPS:
+    gyroRange = 1000.0;
+    break;
+  case GYRO_FULL_SCALE_2000_DPS:
+    gyroRange = 2000.0;
+    break;
+  default:
+    return; // Return without writing invalid mode
+  }
+  i2cWriteByte(address, 27, mode);
+  delay(10);
+}
+
+uint8_t MPU9250_asukiaaa::gyroUpdate() {
+  return i2cRead(address, MPU9250_ADDR_GYRO_XOUT_H, 6, gyroBuff);
+}
+
+float MPU9250_asukiaaa::gyroGet(uint8_t highIndex, uint8_t lowIndex) {
+  int16_t v = ((int16_t) gyroBuff[highIndex]) << 8 | gyroBuff[lowIndex];
+  return ((float) -v) * gyroRange / (float) 0x8000;
+}
+
+float MPU9250_asukiaaa::gyroX() {
+  return gyroGet(0, 1);
+}
+
+float MPU9250_asukiaaa::gyroY() {
+  return gyroGet(2, 3);
+}
+
+float MPU9250_asukiaaa::gyroZ() {
+  return gyroGet(4, 5);
+}