DataFlash: factor out a Log_Write_Current_instance

libraries/DataFlash/DataFlash.h

@@ -282,6 +282,10 @@ private:
                                     uint64_t time_us,
                                     uint8_t mag_instance,
                                     enum LogMessages type);
+    void Log_Write_Current_instance(const AP_BattMonitor &battery,
+                                    uint64_t time_us,
+                                    uint8_t battery_instance,
+                                    enum LogMessages type);
 
     void backend_starting_new_log(const DataFlash_Backend *backend);
 

libraries/DataFlash/LogFile.cpp

@@ -1526,71 +1526,54 @@ void DataFlash_Class::Log_Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f
     WriteBlock(&pkt, sizeof(pkt));
 }
 
-// Write an Current data packet
-void DataFlash_Class::Log_Write_Current(const AP_BattMonitor &battery)
+void DataFlash_Class::Log_Write_Current_instance(const AP_BattMonitor &battery,
+                                                 const uint64_t time_us,
+                                                 const uint8_t battery_instance,
+                                                 const enum LogMessages type)
 {
-    if (battery.num_instances() >= 1) {
-        float temp;
-        bool has_temp = battery.get_temperature(temp, 0);
-        struct log_Current pkt = {
-            LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
-            time_us             : AP_HAL::micros64(),
-            voltage             : battery.voltage(0),
-            voltage_resting     : battery.voltage_resting_estimate(0),
-            current_amps        : battery.current_amps(0),
-            current_total       : battery.current_total_mah(0),
-            temperature         : (int16_t)(has_temp ? (temp * 100) : 0),
-            resistance          : battery.get_resistance(0)
+    float temp;
+    bool has_temp = battery.get_temperature(temp, battery_instance);
+    struct log_Current pkt = {
+        LOG_PACKET_HEADER_INIT(type),
+        time_us             : time_us,
+        voltage             : battery.voltage(battery_instance),
+        voltage_resting     : battery.voltage_resting_estimate(battery_instance),
+        current_amps        : battery.current_amps(battery_instance),
+        current_total       : battery.current_total_mah(battery_instance),
+        temperature         : (int16_t)(has_temp ? (temp * 100) : 0),
+        resistance          : battery.get_resistance(battery_instance)
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+
+    // individual cell voltages
+    if (battery.has_cell_voltages(battery_instance)) {
+        const AP_BattMonitor::cells &cells = battery.get_cell_voltages(battery_instance);
+        struct log_Current_Cells cell_pkt = {
+            LOG_PACKET_HEADER_INIT(LOG_CURRENT_CELLS_MSG),
+            time_us             : time_us,
+            voltage             : battery.voltage(battery_instance)
         };
-        WriteBlock(&pkt, sizeof(pkt));
+        for (uint8_t i = 0; i < ARRAY_SIZE(cells.cells); i++) {
+            cell_pkt.cell_voltages[i] = cells.cells[i] + 1;
+        }
+        WriteBlock(&cell_pkt, sizeof(cell_pkt));
 
-        // individual cell voltages
-        if (battery.has_cell_voltages(0)) {
-            const AP_BattMonitor::cells &cells = battery.get_cell_voltages(0);
-            struct log_Current_Cells cell_pkt = {
-                LOG_PACKET_HEADER_INIT(LOG_CURRENT_CELLS_MSG),
-                time_us             : AP_HAL::micros64(),
-                voltage             : battery.voltage(0)
-            };
-            for (uint8_t i = 0; i < ARRAY_SIZE(cells.cells); i++) {
-                cell_pkt.cell_voltages[i] = cells.cells[i] + 1;
-            }
-            WriteBlock(&cell_pkt, sizeof(cell_pkt));
+        // check battery structure can hold all cells
+        static_assert(ARRAY_SIZE(cells.cells) == (sizeof(cell_pkt.cell_voltages) / sizeof(cell_pkt.cell_voltages[0])),
+                      "Battery cell number doesn't match in library and log structure");
+    }
+}
 
-            // check battery structure can hold all cells
-            static_assert(ARRAY_SIZE(cells.cells) == (sizeof(cell_pkt.cell_voltages) / sizeof(cell_pkt.cell_voltages[0])),
-                          "Battery cell number doesn't match in library and log structure");
-        }
+// Write an Current data packet
+void DataFlash_Class::Log_Write_Current(const AP_BattMonitor &battery)
+{
+    const uint64_t time_us = AP_HAL::micros64();
+    if (battery.num_instances() >= 1) {
+        Log_Write_Current_instance(battery, time_us, 0, LOG_CURRENT_MSG);
     }
 
     if (battery.num_instances() >= 2) {
-        float temp;
-        bool has_temp = battery.get_temperature(temp, 1);
-        struct log_Current pkt = {
-            LOG_PACKET_HEADER_INIT(LOG_CURRENT2_MSG),
-            time_us             : AP_HAL::micros64(),
-            voltage             : battery.voltage(1),
-            voltage_resting     : battery.voltage_resting_estimate(1),
-            current_amps        : battery.current_amps(1),
-            current_total       : battery.current_total_mah(1),
-            temperature         : (int16_t)(has_temp ? (temp * 100) : 0),
-            resistance          : battery.get_resistance(1)
-        };
-        WriteBlock(&pkt, sizeof(pkt));
-
-        // individual cell voltages
-        if (battery.has_cell_voltages(1)) {
-            const AP_BattMonitor::cells &cells = battery.get_cell_voltages(1);
-            struct log_Current_Cells cell_pkt = {
-                LOG_PACKET_HEADER_INIT(LOG_CURRENT_CELLS_MSG),
-                time_us             : AP_HAL::micros64(),
-                voltage             : battery.voltage(1)
-            };
-            for (uint8_t i = 0; i < ARRAY_SIZE(cells.cells); i++) {
-                cell_pkt.cell_voltages[i] = cells.cells[i] + 1;
-            }
-            WriteBlock(&cell_pkt, sizeof(cell_pkt));
-        }
+        Log_Write_Current_instance(battery, time_us, 1, LOG_CURRENT2_MSG);
     }
 }