Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_BattMonitor_config.h"
       4             : 
       5             : #if AP_BATTERY_ENABLED
       6             : 
       7             : #include <AP_Common/AP_Common.h>
       8             : #include <AP_Param/AP_Param.h>
       9             : #include <AP_Math/AP_Math.h>
      10             : #include <AP_TemperatureSensor/AP_TemperatureSensor_config.h>
      11             : #include <GCS_MAVLink/GCS_MAVLink.h>
      12             : #include "AP_BattMonitor_Params.h"
      13             : 
      14             : // first monitor is always the primary monitor
      15             : #define AP_BATT_PRIMARY_INSTANCE            0
      16             : 
      17             : #define AP_BATT_SERIAL_NUMBER_DEFAULT       -1
      18             : 
      19             : #define AP_BATT_MONITOR_TIMEOUT             5000
      20             : 
      21             : #define AP_BATT_MONITOR_RES_EST_TC_1        0.5f
      22             : #define AP_BATT_MONITOR_RES_EST_TC_2        0.1f
      23             : 
      24             : #if HAL_PROGRAM_SIZE_LIMIT_KB > 1024
      25             : #define AP_BATT_MONITOR_CELLS_MAX           14
      26             : #else
      27             : #define AP_BATT_MONITOR_CELLS_MAX           12
      28             : #endif
      29             : 
      30             : // declare backend class
      31             : class AP_BattMonitor_Backend;
      32             : class AP_BattMonitor_Analog;
      33             : class AP_BattMonitor_SMBus;
      34             : class AP_BattMonitor_SMBus_Solo;
      35             : class AP_BattMonitor_SMBus_Generic;
      36             : class AP_BattMonitor_SMBus_Maxell;
      37             : class AP_BattMonitor_SMBus_Rotoye;
      38             : class AP_BattMonitor_DroneCAN;
      39             : class AP_BattMonitor_Generator;
      40             : class AP_BattMonitor_INA2XX;
      41             : class AP_BattMonitor_INA239;
      42             : class AP_BattMonitor_LTC2946;
      43             : class AP_BattMonitor_Torqeedo;
      44             : class AP_BattMonitor_FuelLevel_Analog;
      45             : class AP_BattMonitor_EFI;
      46             : class AP_BattMonitor_Scripting;
      47             : 
      48             : 
      49             : class AP_BattMonitor
      50             : {
      51             :     friend class AP_BattMonitor_Backend;
      52             :     friend class AP_BattMonitor_Analog;
      53             :     friend class AP_BattMonitor_SMBus;
      54             :     friend class AP_BattMonitor_SMBus_Solo;
      55             :     friend class AP_BattMonitor_SMBus_Generic;
      56             :     friend class AP_BattMonitor_SMBus_Maxell;
      57             :     friend class AP_BattMonitor_SMBus_Rotoye;
      58             :     friend class AP_BattMonitor_DroneCAN;
      59             :     friend class AP_BattMonitor_Sum;
      60             :     friend class AP_BattMonitor_FuelFlow;
      61             :     friend class AP_BattMonitor_FuelLevel_PWM;
      62             :     friend class AP_BattMonitor_Generator;
      63             :     friend class AP_BattMonitor_EFI;
      64             :     friend class AP_BattMonitor_INA2XX;
      65             :     friend class AP_BattMonitor_INA239;
      66             :     friend class AP_BattMonitor_LTC2946;
      67             :     friend class AP_BattMonitor_AD7091R5;
      68             :     friend class AP_BattMonitor_INA3221;
      69             : 
      70             :     friend class AP_BattMonitor_Torqeedo;
      71             :     friend class AP_BattMonitor_FuelLevel_Analog;
      72             :     friend class AP_BattMonitor_Synthetic_Current;
      73             :     friend class AP_BattMonitor_Scripting;
      74             : 
      75             : public:
      76             : 
      77             :     // battery failsafes must be defined in levels of severity so that vehicles wont fall backwards
      78             :     enum class Failsafe : uint8_t {
      79             :         None = 0,
      80             :         Unhealthy,
      81             :         Low,
      82             :         Critical
      83             :     };
      84             : 
      85             :     // power states
      86             :     enum class ChargingState : uint8_t {
      87             :         UNKNOWN = 0,
      88             :         IDLE,
      89             :         CHARGING,
      90             :         DISCHARGING
      91             :     };
      92             : 
      93             :     // Battery monitor driver types
      94             :     using Type = AP_BattMonitor_Params::Type;
      95             : 
      96             :     FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);
      97             : 
      98             :     AP_BattMonitor(uint32_t log_battery_bit, battery_failsafe_handler_fn_t battery_failsafe_handler_fn, const int8_t *failsafe_priorities);
      99             : 
     100             :     /* Do not allow copies */
     101             :     CLASS_NO_COPY(AP_BattMonitor);
     102             : 
     103    65052479 :     static AP_BattMonitor *get_singleton() {
     104    65052479 :         return _singleton;
     105             :     }
     106             : 
     107             :     // cell voltages in millivolts
     108             :     struct cells {
     109             :         uint16_t cells[AP_BATT_MONITOR_CELLS_MAX];
     110             :     };
     111             : 
     112             :     // The BattMonitor_State structure is filled in by the backend driver
     113             :     struct BattMonitor_State {
     114             :         cells       cell_voltages;             // battery cell voltages in millivolts, 10 cells matches the MAVLink spec
     115             :         float       voltage;                   // voltage in volts
     116             :         float       current_amps;              // current in amperes
     117             :         float       consumed_mah;              // total current draw in milliamp hours since start-up
     118             :         float       consumed_wh;               // total energy consumed in Wh since start-up
     119             :         uint32_t    last_time_micros;          // time when voltage and current was last read in microseconds
     120             :         uint32_t    low_voltage_start_ms;      // time when voltage dropped below the minimum in milliseconds
     121             :         uint32_t    critical_voltage_start_ms; // critical voltage failsafe start timer in milliseconds
     122             :         float       temperature;               // battery temperature in degrees Celsius
     123             : #if AP_TEMPERATURE_SENSOR_ENABLED
     124             :         bool        temperature_external_use;
     125             :         float       temperature_external;      // battery temperature set by an external source in degrees Celsius
     126             : #endif
     127             :         uint32_t    temperature_time;          // timestamp of the last received temperature message
     128             :         float       voltage_resting_estimate;  // voltage with sag removed based on current and resistance estimate in Volt
     129             :         float       resistance;                // resistance, in Ohms, calculated by comparing resting voltage vs in flight voltage
     130             :         Failsafe failsafe;                     // stage failsafe the battery is in
     131             :         bool        healthy;                   // battery monitor is communicating correctly
     132             :         uint32_t    last_healthy_ms;           // Time when monitor was last healthy
     133             :         bool        is_powering_off;           // true when power button commands power off
     134             :         bool        powerOffNotified;          // only send powering off notification once
     135             :         uint32_t    time_remaining;            // remaining battery time
     136             :         bool        has_time_remaining;        // time_remaining is only valid if this is true
     137             :         uint8_t     state_of_health_pct;       // state of health (SOH) in percent
     138             :         bool        has_state_of_health_pct;   // state_of_health_pct is only valid if this is true
     139             :         ChargingState charging_state;          // Charging state (unknown, idle, charging, discharging)
     140             :         uint8_t     instance;                  // instance number of this backend
     141             :         Type        type;                      // allocated instance type
     142             :         const struct AP_Param::GroupInfo *var_info;
     143             :     };
     144             : 
     145             :     static const struct AP_Param::GroupInfo *backend_var_info[AP_BATT_MONITOR_MAX_INSTANCES];
     146             : 
     147             :     // Return the number of battery monitor instances
     148     1121382 :     uint8_t num_instances(void) const { return _num_instances; }
     149             : 
     150             :     // detect and initialise any available battery monitors
     151             :     __INITFUNC__ void init();
     152             : 
     153             :     /// Read the battery voltage and current for all batteries.  Should be called at 10hz
     154             :     void read();
     155             : 
     156             :     // healthy - returns true if monitor is functioning
     157             :     bool healthy(uint8_t instance) const;
     158             : 
     159             :     // return true if all configured battery monitors are healthy
     160             :     bool healthy() const;
     161             : 
     162             :     /// voltage - returns battery voltage in volts
     163             :     float voltage(uint8_t instance) const;
     164     1388611 :     float voltage() const { return voltage(AP_BATT_PRIMARY_INSTANCE); }
     165             : 
     166             :     // voltage for a GCS, may be resistance compensated
     167             :     float gcs_voltage(uint8_t instance) const;
     168     1116947 :     float gcs_voltage(void) const { return gcs_voltage(AP_BATT_PRIMARY_INSTANCE); }
     169             : 
     170             :     /// get voltage with sag removed (based on battery current draw and resistance)
     171             :     /// this will always be greater than or equal to the raw voltage
     172             :     float voltage_resting_estimate(uint8_t instance) const;
     173             :     float voltage_resting_estimate() const { return voltage_resting_estimate(AP_BATT_PRIMARY_INSTANCE); }
     174             : 
     175             :     /// current_amps - returns the instantaneous current draw in amperes
     176             :     bool current_amps(float &current, const uint8_t instance = AP_BATT_PRIMARY_INSTANCE) const WARN_IF_UNUSED;
     177             : 
     178             :     /// consumed_mah - returns total current drawn since start-up in milliampere.hours
     179             :     bool consumed_mah(float &mah, const uint8_t instance = AP_BATT_PRIMARY_INSTANCE) const WARN_IF_UNUSED;
     180             : 
     181             :     /// consumed_wh - returns total energy drawn since start-up in watt.hours
     182             :     bool consumed_wh(float&wh, const uint8_t instance = AP_BATT_PRIMARY_INSTANCE) const WARN_IF_UNUSED;
     183             : 
     184             :     /// capacity_remaining_pct - returns true if the percentage is valid and writes to percentage argument
     185             :     virtual bool capacity_remaining_pct(uint8_t &percentage, uint8_t instance) const WARN_IF_UNUSED;
     186         671 :     bool capacity_remaining_pct(uint8_t &percentage) const WARN_IF_UNUSED { return capacity_remaining_pct(percentage, AP_BATT_PRIMARY_INSTANCE); }
     187             : 
     188             :     /// time_remaining - returns remaining battery time
     189             :     bool time_remaining(uint32_t &seconds, const uint8_t instance = AP_BATT_PRIMARY_INSTANCE) const WARN_IF_UNUSED;
     190             : 
     191             :     /// pack_capacity_mah - returns the capacity of the battery pack in mAh when the pack is full
     192             :     int32_t pack_capacity_mah(uint8_t instance) const;
     193         126 :     int32_t pack_capacity_mah() const { return pack_capacity_mah(AP_BATT_PRIMARY_INSTANCE); }
     194             :  
     195             :     /// returns true if a battery failsafe has ever been triggered
     196     1359585 :     bool has_failsafed(void) const { return _has_triggered_failsafe; };
     197             : 
     198             :     /// returns the highest failsafe action that has been triggered
     199           1 :     int8_t get_highest_failsafe_priority(void) const { return _highest_failsafe_priority; };
     200             : 
     201             :     /// configured_type - returns battery monitor type as configured in parameters
     202    17324368 :     Type configured_type(uint8_t instance) const {
     203    17324368 :         return (Type)_params[instance]._type.get();
     204             :     }
     205             :     /// allocated_type - returns battery monitor type as allocated
     206     4018470 :     Type allocated_type(uint8_t instance) const {
     207     4018470 :         return state[instance].type;
     208             :     }
     209             : 
     210             :     /// get_serial_number - returns battery serial number
     211             :     int32_t get_serial_number() const { return get_serial_number(AP_BATT_PRIMARY_INSTANCE); }
     212           0 :     int32_t get_serial_number(uint8_t instance) const {
     213           0 :         return _params[instance]._serial_number;
     214             :     }
     215             : 
     216             :     /// true when (voltage * current) > watt_max
     217             :     bool overpower_detected() const;
     218             :     bool overpower_detected(uint8_t instance) const;
     219             : 
     220             : #if AP_BATTERY_WATT_MAX_ENABLED
     221             :     /// get_watt_max - returns maximum power in watts
     222      628798 :     float get_watt_max() const { return get_watt_max(AP_BATT_PRIMARY_INSTANCE); }
     223      628798 :     float get_watt_max(uint8_t instance) const {
     224      628798 :         return _params[instance]._watt_max;
     225             :     }
     226             : #endif // AP_BATTERY_WATT_MAX_ENABLED
     227             : 
     228             :     // cell voltages in millivolts
     229           0 :     bool has_cell_voltages() const { return has_cell_voltages(AP_BATT_PRIMARY_INSTANCE); }
     230             :     bool has_cell_voltages(const uint8_t instance) const;
     231           0 :     const cells &get_cell_voltages() const { return get_cell_voltages(AP_BATT_PRIMARY_INSTANCE); }
     232             :     const cells &get_cell_voltages(const uint8_t instance) const;
     233             : 
     234             :     // get once cell voltage (for scripting)
     235             :     bool get_cell_voltage(uint8_t instance, uint8_t cell, float &voltage) const;
     236             : 
     237             :     // temperature
     238             :     bool get_temperature(float &temperature) const { return get_temperature(temperature, AP_BATT_PRIMARY_INSTANCE); }
     239             :     bool get_temperature(float &temperature, const uint8_t instance) const;
     240             : #if AP_TEMPERATURE_SENSOR_ENABLED
     241             :     bool set_temperature(const float temperature, const uint8_t instance);
     242             :     bool set_temperature_by_serial_number(const float temperature, const int32_t serial_number);
     243             : #endif
     244             : 
     245             :     // Set powered state (Solar Panels, BMS)
     246             :     void set_powered_state_to_all(const bool power_on);
     247             :     void set_powered_state(const uint8_t instance, const bool power_on);
     248             : 
     249             :     bool option_is_set(uint8_t instance, AP_BattMonitor_Params::Options option) const;
     250             : 
     251             :     // cycle count
     252             :     bool get_cycle_count(uint8_t instance, uint16_t &cycles) const;
     253             : 
     254             :     // get battery resistance estimate in ohms
     255             :     float get_resistance() const { return get_resistance(AP_BATT_PRIMARY_INSTANCE); }
     256       94857 :     float get_resistance(uint8_t instance) const { return state[instance].resistance; }
     257             : 
     258             :     // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
     259             :     bool arming_checks(size_t buflen, char *buffer) const;
     260             : 
     261             :     // sends powering off mavlink broadcasts and sets notify flag
     262             :     void checkPoweringOff(void);
     263             : 
     264             :     // reset battery remaining percentage
     265             :     bool reset_remaining_mask(uint16_t battery_mask, float percentage);
     266           0 :     bool reset_remaining(uint8_t instance, float percentage) { return reset_remaining_mask(1U<<instance, percentage);}
     267             : 
     268             :     // Returns mavlink charge state
     269             :     MAV_BATTERY_CHARGE_STATE get_mavlink_charge_state(const uint8_t instance) const;
     270             : 
     271             :     // Returns mavlink fault state
     272             :     uint32_t get_mavlink_fault_bitmask(const uint8_t instance) const;
     273             : 
     274             :     // return true if state of health (as a percentage) can be provided and fills in soh_pct argument
     275             :     bool get_state_of_health_pct(uint8_t instance, uint8_t &soh_pct) const;
     276             : 
     277             :     // get charging state (idle, charging, discharging)
     278             :     ChargingState get_charging_state() const { return get_charging_state(AP_BATT_PRIMARY_INSTANCE); }
     279             :     ChargingState get_charging_state(uint8_t instance) const { return state[instance].charging_state; }
     280             : 
     281             :     static const struct AP_Param::GroupInfo var_info[];
     282             : 
     283             : #if AP_BATTERY_SCRIPTING_ENABLED
     284             :     bool handle_scripting(uint8_t idx, const struct BattMonitorScript_State &state);
     285             : #endif
     286             : 
     287             :     // set battery BMS sleep timeout in seconds
     288             :     // set to zero to disable sleep
     289             :     void set_sleep_timeout(uint16_t timeout_sec) { return set_sleep_timeout(AP_BATT_PRIMARY_INSTANCE, timeout_sec); }
     290             :     void set_sleep_timeout(uint8_t instance, uint16_t timeout_sec);
     291             : 
     292             : protected:
     293             : 
     294             :     /// parameters
     295             :     AP_BattMonitor_Params _params[AP_BATT_MONITOR_MAX_INSTANCES];
     296             : 
     297             : private:
     298             :     static AP_BattMonitor *_singleton;
     299             : 
     300             :     BattMonitor_State state[AP_BATT_MONITOR_MAX_INSTANCES];
     301             :     AP_BattMonitor_Backend *drivers[AP_BATT_MONITOR_MAX_INSTANCES];
     302             :     uint32_t    _log_battery_bit;
     303             :     uint8_t     _num_instances;                                     /// number of monitors
     304             : 
     305             :     /// returns the failsafe state of the battery
     306             :     Failsafe check_failsafe(const uint8_t instance);
     307             :     void check_failsafes(void); // checks all batteries failsafes
     308             : 
     309             :     battery_failsafe_handler_fn_t _battery_failsafe_handler_fn;
     310             :     const int8_t *_failsafe_priorities; // array of failsafe priorities, sorted highest to lowest priority, -1 indicates no more entries
     311             : 
     312             :     int8_t      _highest_failsafe_priority; // highest selected failsafe action level (used to restrict what actions we move into)
     313             :     bool        _has_triggered_failsafe;  // true after a battery failsafe has been triggered for the first time
     314             : 
     315             : };
     316             : 
     317             : namespace AP {
     318             :     AP_BattMonitor &battery();
     319             : };
     320             : 
     321             : #endif  // AP_BATTERY_ENABLED