Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_INA3221_ENABLED
       4             : 
       5             : #include "SIM_INA3221.h"
       6             : 
       7             : #include <SITL/SITL.h>
       8             : 
       9        2145 : SITL::INA3221::INA3221()
      10             : {
      11        2145 :     writable_registers.set(0);
      12        2145 :     writable_registers.set(7);
      13        2145 :     writable_registers.set(8);
      14        2145 :     writable_registers.set(9);
      15        2145 :     writable_registers.set(10);
      16        2145 :     writable_registers.set(11);
      17        2145 :     writable_registers.set(12);
      18        2145 :     writable_registers.set(14);
      19        2145 :     writable_registers.set(15);
      20        2145 :     writable_registers.set(16);
      21        2145 :     writable_registers.set(254);
      22        2145 :     writable_registers.set(255);
      23             : 
      24        2145 :     reset();
      25        2145 : }
      26             : 
      27        2147 : void SITL::INA3221::reset()
      28             : {
      29             :     // from page 24 of datasheet:
      30        2147 :     registers.byname.configuration.word = 0x7127;
      31        2147 :     registers.byname.Channel_1_Shunt_Voltage = 0x0;
      32        2147 :     registers.byname.Channel_1_Bus_Voltage = 0x0;
      33        2147 :     registers.byname.Channel_2_Shunt_Voltage = 0x0;
      34        2147 :     registers.byname.Channel_2_Bus_Voltage = 0x0;
      35        2147 :     registers.byname.Channel_3_Shunt_Voltage = 0x0;
      36        2147 :     registers.byname.Channel_3_Bus_Voltage = 0x0;
      37        2147 :     registers.byname.Channel_1_CriticalAlertLimit = 0x7FF8;
      38        2147 :     registers.byname.Channel_1_WarningAlertLimit = 0x7FF8;
      39        2147 :     registers.byname.Channel_2_CriticalAlertLimit = 0x7FF8;
      40        2147 :     registers.byname.Channel_2_WarningAlertLimit = 0x7FF8;
      41        2147 :     registers.byname.Channel_3_CriticalAlertLimit = 0x7FF8;
      42        2147 :     registers.byname.Channel_3_WarningAlertLimit = 0x7FF8;
      43        2147 :     registers.byname.Shunt_VoltageSum = 0x0;
      44        2147 :     registers.byname.Shunt_VoltageSumLimit = 0x7FFE;
      45        2147 :     registers.byname.Mask_Enable = 0x0002;
      46        2147 :     registers.byname.Power_ValidUpperLimit = 0x2710;
      47        2147 :     registers.byname.Power_ValidLowerLimit = 0x2328;
      48        2147 :     registers.byname.ManufacturerID = MANUFACTURER_ID;
      49        2147 :     registers.byname.Die_ID = DIE_ID;
      50        2147 : }
      51             : 
      52         562 : int SITL::INA3221::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
      53             : {
      54         562 :     if (data->nmsgs == 2) {
      55         560 :         if (data->msgs[0].flags != 0) {
      56           0 :             AP_HAL::panic("Unexpected flags");
      57             :         }
      58         560 :         if (data->msgs[1].flags != I2C_M_RD) {
      59           0 :             AP_HAL::panic("Unexpected flags");
      60             :         }
      61         560 :         const uint8_t reg_addr = data->msgs[0].buf[0];
      62         560 :         const uint16_t register_value = registers.word[reg_addr];
      63         560 :         data->msgs[1].buf[0] = register_value >> 8;
      64         560 :         data->msgs[1].buf[1] = register_value & 0xFF;
      65         560 :         data->msgs[1].len = 2;
      66         560 :         return 0;
      67             :     }
      68             : 
      69           2 :     if (data->nmsgs == 1) {
      70           2 :         if (data->msgs[0].flags != 0) {
      71           0 :             AP_HAL::panic("Unexpected flags");
      72             :         }
      73           2 :         const uint8_t reg_addr = data->msgs[0].buf[0];
      74           2 :         if (!writable_registers.get(reg_addr)) {
      75           0 :             AP_HAL::panic("Register 0x%02x is not writable!", reg_addr);
      76             :         }
      77           2 :         const uint16_t register_value = data->msgs[0].buf[2] << 8 | data->msgs[0].buf[1];
      78           2 :         registers.word[reg_addr] = register_value;
      79           2 :         return 0;
      80             :     }
      81             : 
      82           0 :     return -1;
      83             : };
      84             : 
      85   536279826 : static uint16_t convert_voltage(float voltage_v) {
      86             :     // 8mV per count, register value is x8 (3 lowest bits not used)
      87   536279826 :     float volt_counts = (voltage_v/8e-3)*8;
      88   536279826 :     if (volt_counts < INT16_MIN) {
      89           0 :         volt_counts = INT16_MIN;
      90   536279826 :     } else if (volt_counts > INT16_MAX) {
      91      734723 :         volt_counts = INT16_MAX;
      92             :     }
      93             : 
      94             :     // register value is signed
      95   536279826 :     return (uint16_t)((int16_t)volt_counts);
      96             : }
      97             : 
      98   536279826 : static uint16_t convert_current(float current_a) {
      99             :     // V = IR
     100   536279826 :     const float shunt_resistance_ohms = 0.001; // default value in driver
     101   536279826 :     const float shunt_voltage = current_a*shunt_resistance_ohms;
     102             : 
     103             :     // 40uV per count, register value is x8 (3 lowest bits not used)
     104   536279826 :     float shunt_counts = (shunt_voltage/40e-6)*8;
     105   536279826 :     if (shunt_counts < INT16_MIN) {
     106           0 :         shunt_counts = INT16_MIN;
     107   536279826 :     } else if (shunt_counts > INT16_MAX) {
     108      208970 :         shunt_counts = INT16_MAX;
     109             :     }
     110             : 
     111             :     // register value is signed
     112   536279826 :     return (uint16_t)((int16_t)shunt_counts);
     113             : }
     114             : 
     115             : 
     116   178759942 : void SITL::INA3221::update(const class Aircraft &aircraft)
     117             : {
     118   178759942 :     if (registers.byname.configuration.bits.reset != 0) {
     119           2 :         reset();
     120             :     }
     121             : 
     122             :     // update readings
     123   178759942 :     if (registers.byname.configuration.bits.mode == 0b000 ||
     124   178759942 :         registers.byname.configuration.bits.mode == 0b100) {
     125             :         // power-off
     126           0 :         return;
     127             :     }
     128             : 
     129   178759942 :     const bool update_shunt = registers.byname.configuration.bits.mode & 0b001;
     130   178759942 :     const bool update_bus = registers.byname.configuration.bits.mode & 0b010;
     131   178759942 :     if ((registers.byname.configuration.bits.mode & 0b100) == 0) {
     132             :         // single-shot only
     133           0 :         registers.byname.configuration.bits.mode &= ~0b011;
     134             :     }
     135             : 
     136             :     // channel 2 gets the first simulated battery's voltage and current, others are test values:
     137   178759942 :     if (registers.byname.configuration.bits.ch1_enable) {
     138             :         // values close to chip limits (assuming 1mOhm current shunt)
     139   178759942 :         if (update_bus) {
     140   178759942 :             registers.byname.Channel_1_Bus_Voltage = convert_voltage(25); // max of 26V
     141             :         }
     142   178759942 :         if (update_shunt) {
     143   178759942 :             registers.byname.Channel_1_Shunt_Voltage = convert_current(160); // max of 163.8A
     144             :         }
     145             :     }
     146             : 
     147   178759942 :     if (registers.byname.configuration.bits.ch2_enable) {
     148   178759942 :         if (update_bus) {
     149   178759942 :             registers.byname.Channel_2_Bus_Voltage = convert_voltage(AP::sitl()->state.battery_voltage);
     150             :         }
     151   178759942 :         if (update_shunt) {
     152   178759942 :             registers.byname.Channel_2_Shunt_Voltage = convert_current(AP::sitl()->state.battery_current);
     153             :         }
     154             :     }
     155             : 
     156   178759942 :     if (registers.byname.configuration.bits.ch3_enable) {
     157             :         // values different from above to test channel switching
     158   178759942 :         if (update_bus) {
     159   178759942 :             registers.byname.Channel_3_Bus_Voltage = convert_voltage(3.14159);
     160             :         }
     161   178759942 :         if (update_shunt) {
     162   178759942 :             registers.byname.Channel_3_Shunt_Voltage = convert_current(2.71828);
     163             :         }
     164             :     }
     165             : }
     166             : 
     167             : #endif  // AP_SIM_INA3221_ENABLED