Line data    Source code

       1             : #include "AP_BattMonitor_config.h"
       2             : #include <AP_Logger/AP_Logger_config.h>
       3             : 
       4             : #if AP_BATTERY_ENABLED && HAL_LOGGING_ENABLED
       5             : 
       6             : #include "AP_BattMonitor_Backend.h"
       7             : #include <AP_Logger/AP_Logger.h>
       8             : 
       9             : extern const AP_HAL::HAL& hal;
      10             : 
      11             : // Write BAT data packet(s)
      12     1214233 : void AP_BattMonitor_Backend::Log_Write_BAT(const uint8_t instance, const uint64_t time_us) const
      13             : {
      14     1214233 :     bool has_curr = has_current();
      15     1214233 :     uint8_t percent = -1;
      16     1214233 :     IGNORE_RETURN(capacity_remaining_pct(percent));
      17             : 
      18             :     float temperature;
      19     1214233 :     int16_t temperature_cd = 0;
      20     1214233 :     if (get_temperature(temperature)) {
      21         300 :         temperature_cd = temperature * 100.0;
      22             :     }
      23             : 
      24     1214233 :     uint8_t soh_pct = 0;
      25     1214233 :     IGNORE_RETURN(get_state_of_health_pct(soh_pct));
      26             : 
      27     1214233 :     const struct log_BAT pkt{
      28             :         LOG_PACKET_HEADER_INIT(LOG_BAT_MSG),
      29             :         time_us             : time_us,
      30             :         instance            : instance,
      31     1214233 :         voltage             : _state.voltage,
      32     1214233 :         voltage_resting     : _state.voltage_resting_estimate,
      33     1214233 :         current_amps        : has_curr ? _state.current_amps : AP_Logger::quiet_nanf(),
      34     1214233 :         current_total       : has_curr ? _state.consumed_mah : AP_Logger::quiet_nanf(),
      35     1214233 :         consumed_wh         : has_curr ? _state.consumed_wh : AP_Logger::quiet_nanf(),
      36             :         temperature         : temperature_cd,
      37     1214233 :         resistance          : _state.resistance,
      38             :         rem_percent         : percent,
      39     1214233 :         health              : _state.healthy,
      40             :         state_of_health_pct : soh_pct
      41     3642699 :     };
      42     1214233 :     AP::logger().WriteBlock(&pkt, sizeof(pkt));
      43     1214233 : }
      44             : 
      45             : // Write BCL data packet if has_cell_voltages
      46     1214233 : void AP_BattMonitor_Backend::Log_Write_BCL(const uint8_t instance, const uint64_t time_us) const
      47             : {
      48     1214233 :     if (!has_cell_voltages()) {
      49     1213933 :         return;
      50             :     }
      51             : 
      52         300 :     struct log_BCL cell_pkt{
      53             :         LOG_PACKET_HEADER_INIT(LOG_BCL_MSG),
      54             :         time_us             : time_us,
      55             :         instance            : instance,
      56         300 :         voltage             : _state.voltage
      57         300 :     };
      58             : 
      59             :     // we pack the entire BCL message - we must have at least that
      60             :     // many supported cells or the loop below will over-read
      61             :     static_assert(ARRAY_SIZE(_state.cell_voltages.cells) >= ARRAY_SIZE(cell_pkt.cell_voltages), "must have at least ARRAY_SIZE(log_BCL.cell_voltages) cells");
      62             : 
      63        3900 :     for (uint8_t i = 0; i < ARRAY_SIZE(cell_pkt.cell_voltages); i++) {
      64        3600 :         cell_pkt.cell_voltages[i] = _state.cell_voltages.cells[i] + 1; // add 1mv
      65             :     }
      66         300 :     AP::logger().WriteBlock(&cell_pkt, sizeof(cell_pkt));
      67             : 
      68             : #if AP_BATT_MONITOR_CELLS_MAX > 12
      69         300 :     if (_state.cell_voltages.cells[12] != UINT16_MAX || _state.cell_voltages.cells[13] != UINT16_MAX)
      70             :     {
      71             : // @LoggerMessage: BCL2
      72             : // @Description: Battery cell voltage information
      73             : // @Field: TimeUS: Time since system startup
      74             : // @Field: Instance: battery instance number
      75             : // @Field: V13: thirteenth cell voltage
      76             : // @Field: V14: fourteenth cell voltage
      77         284 :         AP::logger().WriteStreaming(
      78             :             "BCL2",
      79             :             "TimeUS,Instance,V13,V14",
      80             :             "s#vv",
      81             :             "F-CC",
      82             :             "QBHH",
      83             :             time_us,
      84             :             instance,
      85         284 :             _state.cell_voltages.cells[ARRAY_SIZE(cell_pkt.cell_voltages)+0] + 1, // add 1mv
      86         284 :             _state.cell_voltages.cells[ARRAY_SIZE(cell_pkt.cell_voltages)+1] + 1  // add 1mv
      87             :             );
      88             :     }
      89             : #endif
      90             : }
      91             : 
      92             : #endif  // AP_BATTERY_ENABLED && HAL_LOGGING_ENABLED