From f077ecd338ba367c049266054e94d66fb0764cad Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 25 Nov 2014 14:58:15 +0900
Subject: [PATCH] BattMon_PX4: class to read from PX4Firmware via orb

---
 .../AP_BattMonitor_SMBus_PX4.cpp              | 62 +++++++++++++++++++
 .../AP_BattMonitor/AP_BattMonitor_SMBus_PX4.h | 40 ++++++++++++
 2 files changed, 102 insertions(+)
 create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp
 create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.h

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp
new file mode 100644
index 0000000000..b75726de75
--- /dev/null
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp
@@ -0,0 +1,62 @@
+/*
+  Battery SMBus PX4 driver
+*/
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+
+#include "AP_BattMonitor_SMBus_PX4.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <drivers/drv_batt_smbus.h>
+#include <uORB/topics/battery_status.h>
+
+extern const AP_HAL::HAL& hal;
+
+// Constructor
+AP_BattMonitor_SMBus_PX4::AP_BattMonitor_SMBus_PX4(AP_BattMonitor &mon, uint8_t instance, AP_BattMonitor::BattMonitor_State &mon_state) :
+        AP_BattMonitor_SMBus(mon, instance, mon_state)
+{
+    // orb subscription for battery status
+    _batt_sub = orb_subscribe(ORB_ID(battery_status));
+}
+
+// read - read latest voltage and current
+void AP_BattMonitor_SMBus_PX4::read()
+{
+    bool updated = false;
+    struct battery_status_s batt_status;
+
+    // check if new info has arrived from the orb
+    orb_check(_batt_sub, &updated);
+
+    // retrieve latest info
+    if (updated) {
+        if (OK == orb_copy(ORB_ID(battery_status), _batt_sub, &batt_status)) {
+            _state.voltage = batt_status.voltage_v;
+            _state.current_amps = batt_status.current_a;
+            _state.last_time_micros = hal.scheduler->micros();
+        }
+    }
+}
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.h
new file mode 100644
index 0000000000..e59e0441a6
--- /dev/null
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.h
@@ -0,0 +1,40 @@
+/*
+  Battery SMBus PX4 driver
+*/
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef AP_BATTMONITOR_SMBUS_PX4_H
+#define AP_BATTMONITOR_SMBUS_PX4_H
+
+#include <AP_Common.h>
+#include <AP_Param.h>
+#include <AP_Math.h>
+#include "AP_BattMonitor_SMBus.h"
+
+class AP_BattMonitor_SMBus_PX4 : public AP_BattMonitor_SMBus
+{
+public:
+    // Constructor
+    AP_BattMonitor_SMBus_PX4(AP_BattMonitor &mon, uint8_t instance, AP_BattMonitor::BattMonitor_State &mon_state);
+
+    /// read - read the battery voltage and current
+    void read();
+
+private:
+    int         _batt_sub;          // orb subscription description
+    uint64_t    _last_timestamp;    // time of last update (used to avoid processing old reports)
+};
+
+#endif // AP_BATTMONITOR_SMBUS_PX4_H