diff --git a/src/madflight.h b/src/madflight.h index d329e7c..882764a 100644 --- a/src/madflight.h +++ b/src/madflight.h @@ -28,6 +28,34 @@ SOFTWARE. #pragma once +//defaults for OUT +#ifndef HW_OUT_COUNT + #define HW_OUT_COUNT 0 + #define HW_PIN_OUT_LIST {} +#endif + +//defaults for RCIN +#ifndef RCIN_NUM_CHANNELS + #define RCIN_NUM_CHANNELS 0 +#endif + +//defaults for madflight_setup(); +#ifndef IMU_SAMPLE_RATE + #define IMU_SAMPLE_RATE 1000 +#endif +#ifndef BARO_SAMPLE_RATE + #define BARO_SAMPLE_RATE 100 +#endif +#ifndef IMU_GYR_LP_HZ + #define IMU_GYR_LP_HZ 70 +#endif +#ifndef IMU_ACC_LP_HZ + #define IMU_ACC_LP_HZ 60 +#endif +#ifndef IMU_MAG_LP_HZ + #define IMU_MAG_LP_HZ 1e10 +#endif + //include hardware specific code & default board pinout #if defined ARDUINO_ARCH_ESP32 #include @@ -149,6 +177,7 @@ SOFTWARE. hw_setup(); //hardware specific setup for spi and Wire (see hw_xxx.h) cfg.begin(); //read config from EEPROM cli.print_i2cScan(); //print i2c scan + rcin.setup(); //Initialize radio communication. //IMU: keep on trying until no error is returned (some sensors need a couple of tries...) diff --git a/src/madflight/bat/INA226/INA226.h b/src/madflight/bat/INA226/INA226.h index 7d20a2d..9b8fb01 100644 --- a/src/madflight/bat/INA226/INA226.h +++ b/src/madflight/bat/INA226/INA226.h @@ -102,69 +102,66 @@ typedef enum class INA226 { - public: + public: - bool begin(HW_WIRETYPE *i2c, uint8_t address = INA226_ADDRESS); - bool configure(ina226_averages_t avg = INA226_AVERAGES_1, ina226_busConvTime_t busConvTime = INA226_BUS_CONV_TIME_1100US, ina226_shuntConvTime_t shuntConvTime = INA226_SHUNT_CONV_TIME_1100US, ina226_mode_t mode = INA226_MODE_SHUNT_BUS_CONT); - bool calibrate(float rShuntValue = 0.1, float iMaxExcepted = 2); + bool begin(HW_WIRETYPE *i2c, uint8_t address = INA226_ADDRESS); + bool configure(ina226_averages_t avg = INA226_AVERAGES_1, ina226_busConvTime_t busConvTime = INA226_BUS_CONV_TIME_1100US, ina226_shuntConvTime_t shuntConvTime = INA226_SHUNT_CONV_TIME_1100US, ina226_mode_t mode = INA226_MODE_SHUNT_BUS_CONT); + bool calibrate(float rShuntValue, float iMaxExpected = 0); - ina226_averages_t getAverages(void); - ina226_busConvTime_t getBusConversionTime(void); - ina226_shuntConvTime_t getShuntConversionTime(void); - ina226_mode_t getMode(void); + bool isConversionReady(void); - bool isConversionReady(void); + float readBusVoltage(void); + float readShuntCurrent(void); + float readBusPower(void); + float readShuntVoltage(void); - void enableShuntOverLimitAlert(void); - void enableShuntUnderLimitAlert(void); - void enableBusOvertLimitAlert(void); - void enableBusUnderLimitAlert(void); - void enableOverPowerLimitAlert(void); - void enableConversionReadyAlert(void); + ina226_averages_t getAverages(void); + ina226_busConvTime_t getBusConversionTime(void); + ina226_shuntConvTime_t getShuntConversionTime(void); + ina226_mode_t getMode(void); - void disableAlerts(void); + void enableShuntOverLimitAlert(void); + void enableShuntUnderLimitAlert(void); + void enableBusOvertLimitAlert(void); + void enableBusUnderLimitAlert(void); + void enableOverPowerLimitAlert(void); + void enableConversionReadyAlert(void); - void setBusVoltageLimit(float voltage); - void setShuntVoltageLimit(float voltage); - void setPowerLimit(float watts); + void disableAlerts(void); - void setAlertInvertedPolarity(bool inverted); - void setAlertLatch(bool latch); + void setBusVoltageLimit(float voltage); + void setShuntVoltageLimit(float voltage); + void setPowerLimit(float watts); - bool isMathOverflow(void); - bool isAlert(void); + void setAlertInvertedPolarity(bool inverted); + void setAlertLatch(bool latch); - float readShuntCurrent(void); - float readShuntVoltage(void); - float readBusPower(void); - float readBusVoltage(void); - int16_t readRawShuntCurrent(void); + bool isMathOverflow(void); + bool isAlert(void); - float getMaxPossibleCurrent(void); - float getMaxCurrent(void); - float getMaxShuntVoltage(void); - float getMaxPower(void); - - uint16_t getMaskEnable(void); - - private: - - HW_WIRETYPE *_i2c; - int8_t inaAddress; - float currentLSB, powerLSB; - float vShuntMax, vBusMax, rShunt; - - void setMaskEnable(uint16_t mask); - - void writeRegister16(uint8_t reg, uint16_t val); - int16_t readRegister16(uint8_t reg); -}; + int16_t readRawShuntCurrent(void); + int16_t readRawShuntVoltage(void); + float getMaxPossibleCurrent(void); + float getMaxCurrent(void); + float getMaxShuntVoltage(void); + float getMaxPower(void); + uint16_t getMaskEnable(void); + private: + HW_WIRETYPE *_i2c; + int8_t inaAddress; + float currentLSB = 1, powerLSB = 25; + float vShuntMax = 0, vBusMax = 0, rShunt = 0; + float shuntLSB = 1; + void setMaskEnable(uint16_t mask); + void writeRegister16(uint8_t reg, uint16_t val); + int16_t readRegister16(uint8_t reg); +}; @@ -189,27 +186,27 @@ bool INA226::configure(ina226_averages_t avg, ina226_busConvTime_t busConvTime, return true; } +//NOTE: setting lower iMaxExpected than actual max scale does not improve resolution of the CURRENT register when averaging +//for example if CAL = 10*2048, then CURRENT register will always be a multiple of 10 bool INA226::calibrate(float rShuntValue, float iMaxExpected) { - uint16_t calibrationValue; rShunt = rShuntValue; + + if(iMaxExpected <= 0) iMaxExpected = vShuntMax / rShunt; //default to full range for rShunt - float minimumLSB; + shuntLSB = 2.5e-6 / rShunt; //I[A] = regShuntVoltage * shuntLSB - minimumLSB = iMaxExpected / 32767; - - currentLSB = (uint32_t)(minimumLSB * 100000000); - currentLSB /= 100000000; - currentLSB /= 0.0001; - currentLSB = ceil(currentLSB); - currentLSB *= 0.0001; + //calculate best fitting CAL value + float minimumLSB = iMaxExpected / 32768; + uint32_t calibrationValue = 0.00512 / (minimumLSB * rShunt) + 0.5; + if (calibrationValue < 1) calibrationValue = 1; + if (calibrationValue > 0xffff) calibrationValue = 0xffff; + writeRegister16(INA226_REG_CALIBRATION, (uint16_t)calibrationValue); + //calculate currentLSB based on used CAL value + currentLSB = 0.00512 / (calibrationValue * rShunt); powerLSB = currentLSB * 25; - calibrationValue = (uint16_t)((0.00512) / (currentLSB * rShunt)); - - writeRegister16(INA226_REG_CALIBRATION, calibrationValue); - return true; } @@ -256,7 +253,7 @@ float INA226::readBusPower(void) } float INA226::readShuntCurrent(void) -{ +{ return (readRegister16(INA226_REG_CURRENT) * currentLSB); } @@ -267,11 +264,12 @@ int16_t INA226::readRawShuntCurrent(void) float INA226::readShuntVoltage(void) { - float voltage; - - voltage = readRegister16(INA226_REG_SHUNTVOLTAGE); + return (readRawShuntVoltage() * 0.0000025); +} - return (voltage * 0.0000025); +int16_t INA226::readRawShuntVoltage(void) +{ + return readRegister16(INA226_REG_SHUNTVOLTAGE); } float INA226::readBusVoltage(void) diff --git a/src/madflight/bat/bat.h b/src/madflight/bat/bat.h index 73627fc..80b4c6e 100644 --- a/src/madflight/bat/bat.h +++ b/src/madflight/bat/bat.h @@ -61,7 +61,6 @@ class BatteryINA226: public Battery { public: void setup() { float Rshunt = cfg.BAT_CAL_I; //ohm - float iMaxExpected = 0.080 / Rshunt; // ampere (max 80mv shunt voltage) // Default INA226 address is 0x40 bat_ina226.begin(i2c); @@ -70,7 +69,7 @@ class BatteryINA226: public Battery { bat_ina226.configure(INA226_AVERAGES_128, INA226_BUS_CONV_TIME_140US, INA226_SHUNT_CONV_TIME_140US, INA226_MODE_SHUNT_BUS_CONT); // Calibrate INA226. - bat_ina226.calibrate(Rshunt, iMaxExpected); + bat_ina226.calibrate(Rshunt); } bool update() { static uint32_t ts = micros(); @@ -80,7 +79,8 @@ class BatteryINA226: public Battery { ts = now; i = bat_ina226.readShuntCurrent(); v = bat_ina226.readBusVoltage(); - w = bat_ina226.readBusPower(); //note w is not always equal to v * i, because w, v, and i are averaged values + //w = bat_ina226.readBusPower(); //note w is not always equal to v * i, because w, v, and i are averaged values + w = v * i; //This appears to be more accurate, specially for low values. mah += i * dt_h * 1000; wh += w * dt_h; return true; diff --git a/src/madflight/imu/imu.h b/src/madflight/imu/imu.h index d09d207..ce81053 100644 --- a/src/madflight/imu/imu.h +++ b/src/madflight/imu/imu.h @@ -17,7 +17,7 @@ configures gyro and accel with 1000 Hz sample rate (with on sensor 200 Hz low pa ========================================================================================================================*/ //Available sensors -//#define IMU_USE_NONE 0 //always need an IMU +#define IMU_USE_NONE 0 #define IMU_USE_SPI_BMI270 1 #define IMU_USE_SPI_MPU9250 2 #define IMU_USE_SPI_MPU6500 3 @@ -78,8 +78,13 @@ configures gyro and accel with 1000 Hz sample rate (with on sensor 200 Hz low pa //===================================================================================================================== // setup the imu_Sensor object //===================================================================================================================== +#if !defined IMU_USE + #define IMU_USE IMU_USE_NONE -#if IMU_USE == IMU_USE_SPI_BMI270 +#elif IMU_USE == IMU_USE_NONE + //do nothing + +#elif IMU_USE == IMU_USE_SPI_BMI270 #define IMU_TYPE "IMU_USE_SPI_BMI270" #define IMU_IS_I2C 0 #define IMU_HAS_MAG 0 @@ -156,21 +161,32 @@ configures gyro and accel with 1000 Hz sample rate (with on sensor 200 Hz low pa MPU_InterfaceI2C mpu_iface(i2c, IMU_I2C_ADR); MPUXXXX imu_Sensor(MPUXXXX::MPU6000, &mpu_iface); -// None or undefined -#elif IMU_USE == IMU_USE_NONE || !defined IMU_USE - #error "IMU_USE not defined" - // Invalid value #else #error "invalid IMU_USE value" #endif + + //========================================================================================================================// // Imu Class Implementation //========================================================================================================================// #include "../interface.h" //Imu class declaration +//global Imu class instance +Imu imu; + +#if IMU_USE == IMU_USE_NONE + //dummy class define when no imu is present + int Imu::setup(uint32_t sampleRate) {return 0;} + bool Imu::waitNewSample() {return false;} + bool Imu::hasMag() {return false;} + bool Imu::usesI2C() {return false;} + void Imu::statReset() {} + void Imu::_interrupt_handler() {} +#else //#if IMU_USE == IMU_USE_NONE + void _imu_ll_interrupt_setup(); //prototype volatile bool _imu_ll_interrupt_enabled = false; volatile bool _imu_ll_interrupt_busy = false; @@ -249,9 +265,6 @@ void Imu::_interrupt_handler() { stat_cnt++; } -//global Imu class instance -Imu imu; - //========================================================================================================================// // _IMU_LL_ IMU Low Level Interrrupt Handler //========================================================================================================================// @@ -325,3 +338,5 @@ void _imu_ll_interrupt_handler() { } } } + +#endif //#if IMU_USE == IMU_USE_NONE diff --git a/src/madflight/interface.h b/src/madflight/interface.h index e3f3e34..0a0c61c 100644 --- a/src/madflight/interface.h +++ b/src/madflight/interface.h @@ -268,9 +268,9 @@ extern BlackBox &bb; class Out { public: bool armed = false; - float command[HW_OUT_COUNT] = {0}; //last commanded outputs (values: 0.0 to 1.0) + float command[HW_OUT_COUNT] = {}; //last commanded outputs (values: 0.0 to 1.0) PWM pwm[HW_OUT_COUNT]; //ESC and Servo outputs (values: 0.0 to 1.0) - char type[HW_OUT_COUNT] = {'X'}; + char type[HW_OUT_COUNT] = {}; void setup(); bool setupMotor(uint8_t i, int pin, int freq_hz = 400, int pwm_min_us = 950, int pwm_max_us = 2000); diff --git a/src/madflight/rcin/rcin_calibrate.h b/src/madflight/rcin/rcin_calibrate.h index 2b8fbc6..2d61114 100644 --- a/src/madflight/rcin/rcin_calibrate.h +++ b/src/madflight/rcin/rcin_calibrate.h @@ -31,8 +31,8 @@ class RcinCalibrate { stick_t arm; //arm switch stick_t flt; //flightmode switch - bool ignore[RCIN_NUM_CHANNELS]={}; //channels to ignore - int16_t pc[RCIN_NUM_CHANNELS] = {0}; //temp value + bool ignore[RCIN_NUM_CHANNELS] = {}; //channels to ignore + int16_t pc[RCIN_NUM_CHANNELS] = {}; //temp value int16_t arm_last; //temp value void prompt(const char *msg) {