From 8e708a0a32f01156c5ab01c2a380e6f108250f94 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 29 Nov 2019 10:04:33 +1100
Subject: [PATCH] AP_BattMonitor: add backend for generator supply

---
 libraries/AP_BattMonitor/AP_BattMonitor.cpp   |  4 ++
 libraries/AP_BattMonitor/AP_BattMonitor.h     |  2 +
 .../AP_BattMonitor_Generator.cpp              | 38 +++++++++++++++++++
 .../AP_BattMonitor/AP_BattMonitor_Generator.h | 22 +++++++++++
 .../AP_BattMonitor/AP_BattMonitor_Params.h    |  1 +
 5 files changed, 67 insertions(+)
 create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_Generator.cpp
 create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_Generator.h

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp
index 96037b4e06..3ee06f5de8 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp
@@ -11,6 +11,7 @@
 #include "AP_BattMonitor_Sum.h"
 #include "AP_BattMonitor_FuelFlow.h"
 #include "AP_BattMonitor_FuelLevel_PWM.h"
+#include "AP_BattMonitor_Generator.h"
 
 #include <AP_HAL/AP_HAL.h>
 
@@ -181,6 +182,9 @@ AP_BattMonitor::init()
                                                                  hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR,
                                                                                          100000, true, 20));
                 break;
+            case AP_BattMonitor_Params::BattMonitor_TYPE_Generator:
+                drivers[instance] = new AP_BattMonitor_Generator(*this, state[instance], _params[instance]);
+                break;
             case AP_BattMonitor_Params::BattMonitor_TYPE_NONE:
             default:
                 break;
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h
index 311784cb54..0378ccf469 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor.h
@@ -37,6 +37,7 @@ class AP_BattMonitor_SMBus_Solo;
 class AP_BattMonitor_SMBus_Generic;
 class AP_BattMonitor_SMBus_Maxell;
 class AP_BattMonitor_UAVCAN;
+class AP_BattMonitor_Generator;
 
 class AP_BattMonitor
 {
@@ -50,6 +51,7 @@ class AP_BattMonitor
     friend class AP_BattMonitor_Sum;
     friend class AP_BattMonitor_FuelFlow;
     friend class AP_BattMonitor_FuelLevel_PWM;
+    friend class AP_BattMonitor_Generator;
 
 public:
 
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Generator.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Generator.cpp
new file mode 100644
index 0000000000..2acb231759
--- /dev/null
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_Generator.cpp
@@ -0,0 +1,38 @@
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+#include "AP_BattMonitor.h"
+#include "AP_BattMonitor_Generator.h"
+
+extern const AP_HAL::HAL& hal;
+
+// read - read the voltage and current
+void AP_BattMonitor_Generator::read()
+{
+    _state.healthy = false;
+
+#if GENERATOR_ENABLED
+    AP_Generator_RichenPower *generator = AP::generator();
+
+    // healthy if we can find a generator
+    if (generator == nullptr) {
+        return;
+    }
+
+    // get voltage
+    if (!generator->voltage(_state.voltage)) {
+        return;
+    }
+
+    // get current
+    if (!generator->current(_state.current_amps)) {
+        return;
+    }
+
+    if (!generator->healthy()) {
+        return;
+    }
+
+    _state.healthy = true;
+#endif
+}
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Generator.h b/libraries/AP_BattMonitor/AP_BattMonitor_Generator.h
new file mode 100644
index 0000000000..989dc727c7
--- /dev/null
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_Generator.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "AP_BattMonitor.h"
+#include "AP_BattMonitor_Backend.h"
+
+#include <AP_Generator/AP_Generator_RichenPower.h>
+
+class AP_BattMonitor_Generator : public AP_BattMonitor_Backend
+{
+public:
+
+    using AP_BattMonitor_Backend::AP_BattMonitor_Backend;
+
+    /// Read the battery voltage and current.  Should be called at 10hz
+    void read() override;
+
+    /// returns true if battery monitor provides current info
+    bool has_current() const override { return true; }
+
+    void init(void) override {}
+
+};
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h
index aa08333be3..bbf6cac676 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h
@@ -29,6 +29,7 @@ public:
         BattMonitor_TYPE_SUI6                       = 14,
         BattMonitor_TYPE_NeoDesign                  = 15,
         BattMonitor_TYPE_MAXELL                     = 16,
+        BattMonitor_TYPE_Generator                  = 17,
     };
 
     // low voltage sources (used for BATT_LOW_TYPE parameter)