/components/power/ina219.cpp - Annotate - AMiRo-OS - Research for Cognitive Interaction

amiro-os / components / power / ina219.cpp @ b8b3a9c9

History | View | Annotate | Download (10.767 KB)

-fe0e0b
+#include <amiro/power/ina219.hpp>
 #include <ch.hpp>
 #include <chprintf.h>
-            f8cf404d
+#include <global.hpp>
-fe0e0b
+            Thomas Schöpping
 using namespace chibios_rt;
 using namespace amiro;
 using namespace INA219;
-            f8cf404d
+extern Global global;
-fe0e0b
+Driver::Driver(I2CDriver &i2c_driver, const uint8_t i2c_address) :
   BaseSensor<InitData,CalibData>(), i2c_driver(&i2c_driver), tx_params({i2c_address, NULL, 0, NULL, 0}), current_lsb_uA(0)
+{
   this->config.content.brng = Configuration::BRNG_DEFAULT;
   this->config.content.pg = Configuration::PGA_DEFAULT;
   this->config.content.badc = Configuration::ADC_DEFAULT;
   this->config.content.sadc = Configuration::ADC_DEFAULT;
   this->config.content.mode = Configuration::MODE_DEFAULT;
   return;
+}
 Driver::~Driver()
 {}
 msg_t
 Driver::init(InitData* initialization_data)
+{
   if(!initialization_data)
+  {
     return ERROR;
+  }
   // write configuration
   if (this->writeRegister(REG_CONFIGURATION, initialization_data->configuration.value & MASK_CONFIGURATION)) {
     return ERROR;
+  }
   this->config.value = initialization_data->configuration.value & MASK_CONFIGURATION;
   if (this->writeRegister(REG_CALIBRATION, initialization_data->calibration & MASK_CALIBRATION)) {
     return ERROR;
+  }
   this->current_lsb_uA = initialization_data->current_lsb_uA;
   return this->update();
+}
 msg_t
 Driver::update()
+{
   this->status.power = 0;
   msg_t res = this->readRegister(REG_BUS_VOLTAGE, this->status.bus_voltage.value);
   res |= this->readRegister(REG_POWER, this->status.power);
   // if the power register was not updated yet, try again
   while (!this->status.bus_voltage.content.conversion_ready || this->status.power == 0 || res != 0)
+  {
     BaseThread::sleep(MS2ST(10));
     res |= this->readRegister(REG_BUS_VOLTAGE, this->status.bus_voltage.value);
     res |= this->readRegister(REG_POWER, this->status.power);
+  }
   return res ? ERROR : SUCCESS;
+}
 msg_t
 Driver::wakeup()
+{
   if (this->writeRegister(REG_CONFIGURATION, this->config.value)) {
     return ERROR;
   } else {
     return this->update();
+  }
+}
 msg_t
 Driver::hibernate()
+{
   Configuration::Register tmp_config = this->config;
   tmp_config.content.mode = Configuration::MODE_PowerDown;
   if (this->writeRegister(REG_CONFIGURATION, tmp_config.value)) {
     return ERROR;
   } else {
     return SUCCESS;
+  }
+}
 #ifndef AMIRO_NCALIBRATION
 msg_t
 Driver::calibration(CalibData* calibration_data)
+{
   if (!calibration_data) {
     return ERROR;
+  }
   uint16_t current_lsb_uA = calibration_data->input.current_lsb_uA;
   if (current_lsb_uA < calibration_data->input.max_expected_current_A / 0.032767f) {
     current_lsb_uA = calibration_data->input.max_expected_current_A / 0.032767f;
   } else if (current_lsb_uA > calibration_data->input.max_expected_current_A / 0.004096f) {
     current_lsb_uA = calibration_data->input.max_expected_current_A / 0.004096f;
+  }
   const uint16_t calibration_value = uint16_t(40960 / (current_lsb_uA * calibration_data->input.shunt_resistance_O));
   float V_shunt_max;
   switch (calibration_data->input.configuration.content.pg)
+  {
     case Configuration::PGA_40mV:
       V_shunt_max = 0.04f;
       break;
     case Configuration::PGA_80mV:
       V_shunt_max = 0.08f;
       break;
     case Configuration::PGA_160mV:
       V_shunt_max = 0.16f;
       break;
     case Configuration::PGA_320mV:
       V_shunt_max = 0.32f;
       break;
+  }
   const float max_current_before_overflow = ( (current_lsb_uA * 0.032767f >= V_shunt_max) / (calibration_data->input.shunt_resistance_O) )?
                                             V_shunt_max / calibration_data->input.shunt_resistance_O :
                                             current_lsb_uA * 0.032767f;
   const float max_shuntvoltage_before_overflow = ( (max_current_before_overflow * calibration_data->input.shunt_resistance_O) >= V_shunt_max )?
                                                  V_shunt_max :
                                                  max_current_before_overflow * calibration_data->input.shunt_resistance_O;
   calibration_data->output.max_current_before_overflow_A = max_current_before_overflow;
   calibration_data->output.max_shuntvoltage_before_overflow_V = max_shuntvoltage_before_overflow;
   calibration_data->output.current_lsb_uA = current_lsb_uA;
   calibration_data->output.calibration_value = calibration_value;
   return SUCCESS;
+}
 #endif
 #ifndef AMIRO_NSELFTEST
 msg_t
 Driver::selftest()
+{
   struct RegisterContent {
     Configuration::Register configuration;
     uint16_t shunt_voltage = 0;
     BusVoltage bus_voltage;
     uint16_t power = 0;
     uint16_t current = 0;
     uint16_t calibration = 0;
   };
   // backup the current status
   RegisterContent backup;
   msg_t res = this->readRegister(REG_CONFIGURATION, backup.configuration.value);
   res |= this->readRegister(REG_SHUNT_VOLTAGE, backup.shunt_voltage);
   res |= this->readRegister(REG_BUS_VOLTAGE, backup.bus_voltage.value);
   res |= this->readRegister(REG_POWER, backup.power);
   res |= this->readRegister(REG_CURRENT, backup.current);
   res |= this->readRegister(REG_CALIBRATION, backup.calibration);
   if (res ||
       !(backup.configuration.value ||
         backup.shunt_voltage ||
         backup.bus_voltage.value ||
         backup.power ||
         backup.current ||
         backup.calibration))
+  {
     return ST_FAIL_BACKUP;
+  }
   // reset the INA219
   if (this->reset())
+  {
     return ST_FAIL_RESET;
+  }
   // read the configuration
   Configuration::Register config = this->readConfiguration();
   // check for the default configuration
   Configuration::Register config_default;
   config_default.content.brng = Configuration::BRNG_DEFAULT;
   config_default.content.pg = Configuration::PGA_DEFAULT;
   config_default.content.badc = Configuration::ADC_DEFAULT;
   config_default.content.sadc = Configuration::ADC_DEFAULT;
   config_default.content.mode = Configuration::MODE_DEFAULT;
   if (config.value != config_default.value)
+  {
     return ST_FAIL_IS_DEFAULT;
+  }
   // revert to the previous configuration
   if (this->writeRegister(REG_CONFIGURATION, backup.configuration.value))
+  {
     return ST_FAIL_WRITE_CONFIG;
+  }
   // revert to the previous calibration
   if (this->writeRegister(REG_CALIBRATION, backup.calibration))
+  {
     return ST_FAIL_WRITE_CALIB;
+  }
   // read the current configuration
   if (this->readConfiguration().value != backup.configuration.value)
+  {
     return ST_FAIL_CHECK_CONFIG;
+  }
   // read the current calibration
   if (this->readCalibration() != backup.calibration)
+  {
     return ST_FAIL_CHECK_CALIB;
+  }
   // read and print the current status
   INA219::BusVoltage bus_voltage = this->readBusVoltage();
   uint16_t power;
   this->readRegister(REG_POWER, power);
   // wait until the bus voltage and the power register are valid
   while (!bus_voltage.conversion_ready || power == 0)
+  {
     BaseThread::sleep(MS2ST(10));
     bus_voltage = this->readBusVoltage();
     this->readRegister(REG_POWER, power);
+  }
-            f8cf404d
+  chprintf((BaseSequentialStream*) &global.sercanmux1, "shunt voltage : %fV\n", this->readShuntVoltage_uV() / 1000000.f);
   chprintf((BaseSequentialStream*) &global.sercanmux1, "bus voltage   : %fV\n", bus_voltage.voltage_uV / 1000000.f);
   chprintf((BaseSequentialStream*) &global.sercanmux1, "power         : %fW\n", this->readPower_uW() / 1000000.f);
   chprintf((BaseSequentialStream*) &global.sercanmux1, "current       : %fA\n", this->readCurrent_uA() / 1000000.f);
-fe0e0b
+            Thomas Schöpping
   return ST_OK;
+}
 #endif
 Configuration::Register
 Driver::readConfiguration()
+{
   Configuration::Register reg;
   this->readRegister(REG_CONFIGURATION, reg.value);
   return reg;
+}
 int32_t
 Driver::readShuntVoltage_uV()
+{
   uint16_t val = 0;
   this->readRegister(REG_SHUNT_VOLTAGE, val);
   /*
    * Depending on the configuration either one, two, three or four most significant bits are used for coding the sign (no two's complement!)
    * -> Masking bits in order to separate sign bits and value bits.
    * Furthermore, the least significant bit represents 10uV.
    * -> Multiplication by 10 or -10 respectively.
    */
   switch (this->config.content.pg)
+  {
     case Configuration::PGA_320mV:
       return ( uint32_t(val & 0x7FFFu) * ((val & 0x8000u)? -10 : 10) );
     case Configuration::PGA_160mV:
       return ( uint32_t(val & 0x3FFFu) * ((val & 0xC000u)? -10 : 10) );
     case Configuration::PGA_80mV:
       return ( uint32_t(val & 0x1FFFu) * ((val & 0xE000u)? -10 : 10) );
     case Configuration::PGA_40mV:
       return ( uint32_t(val & 0x0FFFu) * ((val & 0xF000u)? -10 : 10) );
+  }
+}
 INA219::BusVoltage
 Driver::readBusVoltage()
+{
   Driver::BusVoltage reg;
   this->readRegister(REG_BUS_VOLTAGE, reg.value);
   return this->busVoltageReg2uV(reg);
+}
 uint32_t
 Driver::readPower_uW()
+{
   uint16_t val = 0;
   this->readRegister(REG_POWER, val);
   return this->powerReg2uW(val);
+}
 int32_t
 Driver::readCurrent_uA()
+{
   uint16_t val = 0;
   this->readRegister(REG_CURRENT, val);
   /*
    * Reinterpret register value as a signed integer (two's complement).
    * Multiply with the value of the least significant bit.
    */
   return int32_t(*reinterpret_cast<int16_t*>(&val)) * this->current_lsb_uA;
+}
 uint16_t
 Driver::readCalibration()
+{
   uint16_t val = 0;
   this->readRegister(REG_CALIBRATION, val);
   return (val & MASK_CALIBRATION);
+}
 uint8_t
 Driver::reset()
+{
   const msg_t res = this->writeRegister(REG_CONFIGURATION, MASK_RESET);
   return (res? ERROR : SUCCESS);
+}
 msg_t
 Driver::main(void)
+{
   while (!this->shouldTerminate())
+  {
     this->update();
     //this->eventSource.broadcastFlags(0);
     this->sleep(MS2ST(1000));
     //this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(1000));
+  }
   return RDY_OK;
+}
 msg_t
 Driver::readRegister(const RegisterAddress reg, uint16_t& dst)
+{
   uint8_t buffer[2];
   this->tx_params.txbuf = reinterpret_cast<const uint8_t*>(&reg);
   this->tx_params.txbytes = 1;
   this->tx_params.rxbuf = buffer;
   this->tx_params.rxbytes = 2;
   this->i2c_driver->acquireBus();
   const msg_t res = this->i2c_driver->masterTransmit(&this->tx_params);
   this->i2c_driver->releaseBus();
   if (!res) {
     dst = (buffer[0] << 8) | buffer[1];
     if (reg == REG_CALIBRATION)
       dst &= MASK_CALIBRATION;
 #ifndef NDEBUG
   } else {
-            f8cf404d
+    chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: i2c transmit failed (%d | 0x%08X)\n", __FILE__ , __LINE__ , res, this->i2c_driver->getErrors());
-fe0e0b
+#endif
+  }
   return res;
+}
 msg_t
 Driver::writeRegister(const RegisterAddress reg, const uint16_t& val)
+{
   const uint8_t buffer[3] = {reg,
                              static_cast<uint8_t>((val & 0xFF00u) >> 8),
                              static_cast<uint8_t>(val & 0x00FFu)};
   this->tx_params.txbuf = buffer;
   this->tx_params.txbytes = 3;
   this->tx_params.rxbytes = 0;
   this->i2c_driver->acquireBus();
   const msg_t res = this->i2c_driver->masterTransmit(&this->tx_params);
   this->i2c_driver->releaseBus();
 #ifndef NDEBUG
   if (res) {
-            f8cf404d
+    chprintf((BaseSequentialStream*) &global.sercanmux1, "%s(%d): ERROR: i2c transmit failed (%d | 0x%08X)\n", __FILE__ , __LINE__ , res, this->i2c_driver->getErrors());
-fe0e0b
+            Thomas Schöpping
 #endif
   return res;
+}