diff --git a/docs/calculations.ods b/docs/calculations.ods index a3878c4a..0e8a7f15 100644 Binary files a/docs/calculations.ods and b/docs/calculations.ods differ diff --git a/lib/EspWire/src/EspWire.h b/lib/EspWire/src/EspWire.h index ef84d050..6e908b68 100644 --- a/lib/EspWire/src/EspWire.h +++ b/lib/EspWire/src/EspWire.h @@ -29,7 +29,7 @@ #include #endif -#define ESPWIRE_BUFFER_LENGTH 32 +#define ESPWIRE_BUFFER_LENGTH 64 class EspTwoWire : public Stream { diff --git a/lib/EspWire/src/esp_twi.cpp b/lib/EspWire/src/esp_twi.cpp index c55ff835..0d2157b0 100644 --- a/lib/EspWire/src/esp_twi.cpp +++ b/lib/EspWire/src/esp_twi.cpp @@ -37,7 +37,8 @@ static uint32_t esp_twi_clockStretchLimit; #define SCL_HIGH() (GPEC = (1 << esp_twi_scl)) #define SCL_READ() ((GPI & (1 << esp_twi_scl)) != 0) #elif defined(ESP32) - #define SDA_LOW() (GPIO.enable_w1ts = (1 << esp_twi_sda)) //Enable SDA (becomes output and since GPO is 0 for the pin, it will pull the line low) + //#define SDA_LOW() (GPIO.enable_w1ts = (1 << esp_twi_sda)) //Enable SDA (becomes output and since GPO is 0 for the pin, it will pull the line low) + static inline void SDA_LOW() { GPIO.enable_w1ts = 1 << esp_twi_sda; GPIO.out_w1tc = 1 << esp_twi_sda; } #define SDA_HIGH() (GPIO.enable_w1tc = (1 << esp_twi_sda)) //Disable SDA (becomes input and since it has pullup it will go high) #define SDA_READ() ((GPIO.in & (1 << esp_twi_sda)) != 0) #define SCL_LOW() (GPIO.enable_w1ts = (1 << esp_twi_scl)) @@ -97,10 +98,9 @@ void esp_twi_setClock(unsigned int freq){ else if(freq < 400000) esp_twi_dcount = 1;//about 400KHz else esp_twi_dcount = 0;//about 400KHz #elif F_CPU == FCPU240 - if(freq <= 60000) esp_twi_dcount = 280; //about 50kHz - else if(freq < 80000) esp_twi_dcount = 180; //about 80kHz - else if(freq < 100000) esp_twi_dcount = 140; //about 100kHz - else if(freq < 150000) esp_twi_dcount = 90; //about 150kHz + if(freq < 50000) esp_twi_dcount = 300; //about 50kHz + else if(freq < 100000) esp_twi_dcount = 190; //about 100kHz + else if(freq < 150000) esp_twi_dcount = 133; //about 150kHz else if(freq < 200000) esp_twi_dcount = 65; //about 200kHz else if(freq < 250000) esp_twi_dcount = 50; //about 250kHz else if(freq < 300000) esp_twi_dcount = 39; //about 300kHz @@ -110,8 +110,8 @@ void esp_twi_setClock(unsigned int freq){ else if(freq < 700000) esp_twi_dcount = 11; //about 700kHz else if(freq < 800000) esp_twi_dcount = 8; //about 800kHz else if(freq < 900000) esp_twi_dcount = 7; //about 900kHz - else if(freq < 1000000) esp_twi_dcount = 4; //about 1.0MHz - else if(freq < 1100000) esp_twi_dcount = 3; //about 1.1MHz + else if(freq < 1000000) esp_twi_dcount = 5; //about 1.0MHz + else if(freq < 1100000) esp_twi_dcount = 4; //about 1.1MHz else if(freq < 1200000) esp_twi_dcount = 2; //about 1.2MHz else esp_twi_dcount = 1; //above 1.2MHz #elif F_CPU == FCPU133 // 133 mhz diff --git a/lib/Espfc/src/Device/BusDevice.h b/lib/Espfc/src/Device/BusDevice.h index 1d2b8251..bb54711c 100644 --- a/lib/Espfc/src/Device/BusDevice.h +++ b/lib/Espfc/src/Device/BusDevice.h @@ -21,17 +21,29 @@ namespace Device { class BusDevice { public: + BusDevice(): _timeout(ESPFC_BUS_TIMEOUT) {} + virtual BusType getType() const = 0; - virtual int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) = 0; + virtual int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) = 0; + + virtual int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) = 0; - virtual int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) = 0; + virtual bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) = 0; + + bool isSPI() const + { + return getType() == BUS_SPI; + } - virtual bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) = 0; + void setTimeout(uint32_t t) + { + _timeout = t; + } - int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) + int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data) { - return read(devAddr, regAddr, 1, data, timeout); + return read(devAddr, regAddr, 1, data); } bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) @@ -39,18 +51,18 @@ class BusDevice return write(devAddr, regAddr, 1, &data); } - int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) + int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data) { uint8_t b; - uint8_t count = readByte(devAddr, regAddr, &b, timeout); + uint8_t count = readByte(devAddr, regAddr, &b); *data = b & (1 << bitNum); return count; } - int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) + int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) { uint8_t count, b; - if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) + if ((count = readByte(devAddr, regAddr, &b)) != 0) { uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); b &= mask; @@ -111,6 +123,9 @@ class BusDevice } std::function onError; + + protected: + uint32_t _timeout; }; } diff --git a/lib/Espfc/src/Device/BusI2C.h b/lib/Espfc/src/Device/BusI2C.h index 31f32c10..1cc372b7 100644 --- a/lib/Espfc/src/Device/BusI2C.h +++ b/lib/Espfc/src/Device/BusI2C.h @@ -19,15 +19,16 @@ class BusI2C: public BusDevice if(sda == -1 || scl == -1) return 0; targetI2CInit(_dev, sda, scl, speed); + return 1; } - int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) override + int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override { - return read(devAddr, regAddr, length, data, timeout); + return read(devAddr, regAddr, length, data); } - int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) override + int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override { int8_t count = 0; uint32_t t1 = millis(); @@ -39,21 +40,21 @@ class BusI2C: public BusDevice _dev.endTransmission(); _dev.requestFrom(devAddr, length); - for (; _dev.available() && (timeout == 0 || millis() - t1 < timeout); count++) + for (; _dev.available() && (_timeout == 0 || millis() - t1 < _timeout); count++) { data[count] = _dev.read(); //D("i2c:r1", count, data[count]); } //D("i2c:r3", length, count); - if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout + if (_timeout > 0 && millis() - t1 >= _timeout && count < length) count = -1; // timeout if(onError && count != length) onError(); return count; } - bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) override + bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override { //Serial.print("I2C W "); Serial.print(devAddr, HEX); Serial.print(' '); Serial.print(regAddr, HEX); Serial.print(' '); Serial.println(length); diff --git a/lib/Espfc/src/Device/BusSPI.h b/lib/Espfc/src/Device/BusSPI.h index 8e14c620..8dbc9f92 100644 --- a/lib/Espfc/src/Device/BusSPI.h +++ b/lib/Espfc/src/Device/BusSPI.h @@ -29,21 +29,21 @@ class BusSPI: public BusDevice return 1; } - int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) override + int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override { //D("spi:r", regAddr, length); transfer(devAddr, regAddr | SPI_READ, length, NULL, data, SPI_SPEED_NORMAL); return length; } - int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout = ESPFC_BUS_TIMEOUT) override + int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override { //D("spi:r", regAddr, length); transfer(devAddr, regAddr | SPI_READ, length, NULL, data, SPI_SPEED_FAST); return length; } - bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) override + bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override { //D("spi:w", regAddr, length, *data); transfer(devAddr, regAddr & SPI_WRITE, length, data, NULL, SPI_SPEED_NORMAL); @@ -51,7 +51,7 @@ class BusSPI: public BusDevice } private: - void transfer(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *in, uint8_t *out, uint32_t speed) + void transfer(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t *in, uint8_t *out, uint32_t speed) { _dev.beginTransaction(SPISettings(speed, MSBFIRST, SPI_MODE0)); digitalWrite(devAddr, LOW); diff --git a/lib/Espfc/src/Device/GyroMPU9250.h b/lib/Espfc/src/Device/GyroMPU9250.h index 9faa7909..b94c6f81 100644 --- a/lib/Espfc/src/Device/GyroMPU9250.h +++ b/lib/Espfc/src/Device/GyroMPU9250.h @@ -55,7 +55,7 @@ class GyroMPU9250: public GyroMPU6050 delay(100); // enable I2C master mode, and disable I2C if SPI - if(_bus->getType() == BUS_SPI) + if(_bus->isSPI()) { _bus->writeByte(_addr, MPU9250_USER_CTRL, MPU9250_I2C_MST_EN | MPU9250_I2C_IF_DIS); } diff --git a/lib/Espfc/src/Math/Utils.h b/lib/Espfc/src/Math/Utils.h index 6852e417..34e3ab43 100644 --- a/lib/Espfc/src/Math/Utils.h +++ b/lib/Espfc/src/Math/Utils.h @@ -51,6 +51,17 @@ class Peak return value; } + int alignToClock(uint32_t clock, uint32_t maxFreq) + { + uint32_t result = clock; + uint32_t div = 1; + while(result > maxFreq) + { + result = clock / ++div; + } + return result; + } + constexpr float pi() { return 3.14159265358979f; diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index e622b43a..14b99017 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -265,16 +265,18 @@ class Model void sanitize() { - state.gyroRate = 1000; - int loopSyncMax = 1; - if(config.magDev != MAG_NONE || config.baroDev != BARO_NONE) loopSyncMax /= 2; - // for spi gyro allow full speed mode - if (state.gyroDev && state.gyroDev->getBus()->getType() == BUS_SPI) + if (state.gyroDev && state.gyroDev->getBus()->isSPI()) { - loopSyncMax = ESPFC_GYRO_DENOM_MAX; // max 8kHz - state.gyroRate = state.gyroClock; + state.gyroRate = Math::alignToClock(state.gyroClock, ESPFC_GYRO_SPI_RATE_MAX); } + else + { + state.gyroRate = Math::alignToClock(state.gyroClock, ESPFC_GYRO_I2C_RATE_MAX); + } + + int loopSyncMax = 1; + //if(config.magDev != MAG_NONE || config.baroDev != BARO_NONE) loopSyncMax /= 2; config.loopSync = std::max((int)config.loopSync, loopSyncMax); state.loopRate = state.gyroRate / config.loopSync; diff --git a/lib/Espfc/src/Target/TargetESP32.h b/lib/Espfc/src/Target/TargetESP32.h index 1dd9e205..a4e5c2a6 100644 --- a/lib/Espfc/src/Target/TargetESP32.h +++ b/lib/Espfc/src/Target/TargetESP32.h @@ -63,7 +63,7 @@ #define ESPFC_I2C_0 #define ESPFC_I2C_0_SCL 22 #define ESPFC_I2C_0_SDA 21 -//#define ESPFC_I2C_0_SOFT +#define ESPFC_I2C_0_SOFT #define ESPFC_BUZZER #define ESPFC_BUZZER_PIN 0 @@ -77,7 +77,10 @@ #define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER) #define ESPFC_GUARD 0 -#define ESPFC_GYRO_DENOM_MAX 1 + +#define ESPFC_GYRO_I2C_RATE_MAX 2000 +#define ESPFC_GYRO_SPI_RATE_MAX 8000 + //#define ESPFC_LOGGER_FS // doesn't compile on ESP32 #define ESPFC_FREE_RTOS @@ -146,6 +149,7 @@ template inline int targetI2CInit(T& dev, int8_t sda, int8_t scl, uint32_t speed) { dev.begin(sda, scl, speed); + dev.setTimeout(50); return 1; } diff --git a/lib/Espfc/src/Target/TargetESP8266.h b/lib/Espfc/src/Target/TargetESP8266.h index 46ec1da4..1935bd12 100644 --- a/lib/Espfc/src/Target/TargetESP8266.h +++ b/lib/Espfc/src/Target/TargetESP8266.h @@ -50,7 +50,10 @@ #define ESPFC_FEATURE_MASK (FEATURE_RX_PPM | FEATURE_DYNAMIC_FILTER) #define ESPFC_GUARD 1 -#define ESPFC_GYRO_DENOM_MAX 4 + +#define ESPFC_GYRO_I2C_RATE_MAX 1000 +#define ESPFC_GYRO_SPI_RATE_MAX 1000 + #define ESPFC_WIFI_ALT //#define ESPFC_LOGGER_FS // deprecated diff --git a/lib/Espfc/src/Target/TargetRP2040.h b/lib/Espfc/src/Target/TargetRP2040.h index e6adc835..24eb2dd5 100644 --- a/lib/Espfc/src/Target/TargetRP2040.h +++ b/lib/Espfc/src/Target/TargetRP2040.h @@ -66,7 +66,9 @@ #define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER) #define ESPFC_GUARD 0 -#define ESPFC_GYRO_DENOM_MAX 1 + +#define ESPFC_GYRO_I2C_RATE_MAX 2000 +#define ESPFC_GYRO_SPI_RATE_MAX 2000 #define ESPFC_MULTI_CORE #define ESPFC_MULTI_CORE_RP2040 diff --git a/lib/Espfc/src/Target/TargetUnitTest.h b/lib/Espfc/src/Target/TargetUnitTest.h index afba906c..77a0584a 100644 --- a/lib/Espfc/src/Target/TargetUnitTest.h +++ b/lib/Espfc/src/Target/TargetUnitTest.h @@ -13,4 +13,6 @@ #define ESPFC_FEATURE_MASK (0) #define ESPFC_GUARD 1 -#define ESPFC_GYRO_DENOM_MAX 1 \ No newline at end of file + +#define ESPFC_GYRO_I2C_RATE_MAX 2000 +#define ESPFC_GYRO_SPI_RATE_MAX 8000 diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index d9624354..021e6ea9 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -110,11 +110,11 @@ void test_model_gyro_init_1k_256dlpf() model.begin(); TEST_ASSERT_EQUAL_INT32(8000, model.state.gyroClock); - TEST_ASSERT_EQUAL_INT32(1000, model.state.gyroRate); - TEST_ASSERT_EQUAL_INT32(1000, model.state.gyroTimer.rate); - TEST_ASSERT_EQUAL_INT32(1000, model.state.loopRate); - TEST_ASSERT_EQUAL_INT32(1000, model.state.loopTimer.rate); - TEST_ASSERT_EQUAL_INT32(1000, model.state.mixerTimer.rate); + TEST_ASSERT_EQUAL_INT32(2000, model.state.gyroRate); + TEST_ASSERT_EQUAL_INT32(2000, model.state.gyroTimer.rate); + TEST_ASSERT_EQUAL_INT32(2000, model.state.loopRate); + TEST_ASSERT_EQUAL_INT32(2000, model.state.loopTimer.rate); + TEST_ASSERT_EQUAL_INT32(2000, model.state.mixerTimer.rate); } void test_model_gyro_init_1k_188dlpf() @@ -177,13 +177,13 @@ void test_model_outer_pid_init() model.config.pid[PID_LEVEL] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; model.begin(); - TEST_ASSERT_FLOAT_WITHIN( 0.1f, 1000.0f, model.state.outerPid[PID_ROLL].rate); + TEST_ASSERT_FLOAT_WITHIN( 0.1f, 2000.0f, model.state.outerPid[PID_ROLL].rate); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[PID_ROLL].Kp); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[PID_ROLL].Ki); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, model.state.outerPid[PID_ROLL].Kd); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, model.state.outerPid[PID_ROLL].Kf); - TEST_ASSERT_FLOAT_WITHIN( 0.1f, 1000.0f, model.state.outerPid[PID_PITCH].rate); + TEST_ASSERT_FLOAT_WITHIN( 0.1f, 2000.0f, model.state.outerPid[PID_PITCH].rate); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[PID_PITCH].Kp); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[PID_PITCH].Ki); TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, model.state.outerPid[PID_PITCH].Kd);