From 0ce791b21f3bb0557034f144c26a8e6aa86f508c Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Sun, 14 Jul 2019 21:56:05 +0200
Subject: [PATCH] wt_node: battery capacity

---
 scripts/wt_node.py | 34 +++++++++++++++++++++++-----------
 1 file changed, 23 insertions(+), 11 deletions(-)

diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index dfa69b0..ee0aa41 100755
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -18,6 +18,8 @@ from sensor_msgs.msg import Imu, Range, BatteryState
 from wild_thumper.cfg import WildThumperConfig
 
 WHEEL_DIST = 0.248
+BATTERY_CAPACITY_FULL = 2.750 # Ah, NiMH=2.750, LiFePO4=3.100
+UPDATE_RATE = 20.0
 
 class WTBase:
 	def __init__(self):
@@ -42,23 +44,26 @@ class WTBase:
 		self.bMotorManual = False
 		self.set_speed(0, 0)
 		self.volt_last_warn = rospy.Time.now()
-		rospy.loginfo("Init done")
 		i2c_write_reg(0x50, 0x90, struct.pack("BB", 1, 1)) # switch direction
 		rospy.Subscriber("cmd_vel_out", Twist, self.cmdVelReceived)
 		rospy.Subscriber("imu", Imu, self.imuReceived)
 		self.bDocked = False
 		self.bDocked_last = False
+		self.battery_capacity = BATTERY_CAPACITY_FULL*3600 # As
+		rospy.loginfo("Init done")
 		self.run()
 	
 	def run(self):
-		rate = rospy.Rate(20.0)
+		rate = rospy.Rate(UPDATE_RATE)
 		sleep(3) # wait 3s for ros to register and establish all subscriber connections before sending reset diag
 		reset_val = self.get_reset()
 		rospy.loginfo("Reset Status: 0x%x" % reset_val)
 		ir_count = 0
 		sonar_count = 0
+		i2c_write_reg(0x50, 0xA4, struct.pack("B", 1)) # enable Watchdog
 		while not rospy.is_shutdown():
 			rospy.logdebug("Loop alive")
+			#print "Watchdog", struct.unpack(">B", i2c_read_reg(0x50, 0xA4, 1))[0]
 			#print struct.unpack(">B", i2c_read_reg(0x50, 0xA2, 1))[0] # count test
 			self.get_motor_err()
 			self.get_odom()
@@ -188,26 +193,33 @@ class WTBase:
 		msg.status.append(stat)
 		self.pub_diag.publish(msg)
 
+		if volt < 7 and (rospy.Time.now() - self.volt_last_warn).to_sec() > 10:
+			rospy.logerr("Voltage critical: %.2fV" % (volt))
+			self.volt_last_warn = rospy.Time.now()
+
+		self.bDocked = volt > 10.1
+		self.update_capacity(volt, current)
+
 		if self.pub_battery.get_num_connections() > 0:
 			batmsg = BatteryState()
 			batmsg.header.stamp = rospy.Time.now()
 			batmsg.voltage = volt
 			batmsg.current = current
-			batmsg.charge = float('nan')
+			batmsg.charge = self.battery_capacity/3600.0
 			batmsg.capacity = float('nan')
-			batmsg.design_capacity = 5.0
-			batmsg.percentage = float('nan')
+			batmsg.design_capacity = BATTERY_CAPACITY_FULL
+			batmsg.percentage = batmsg.charge/batmsg.design_capacity
 			batmsg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
 			batmsg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN
 			batmsg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_NIMH
+			batmsg.present = True
 			self.pub_battery.publish(batmsg)
 
-		if volt < 7 and (rospy.Time.now() - self.volt_last_warn).to_sec() > 10:
-			rospy.logerr("Voltage critical: %.2fV" % (volt))
-			self.volt_last_warn = rospy.Time.now()
-
-		self.bDocked = volt > 10.1
-
+	def update_capacity(self, volt, current):
+		if self.bDocked:
+			self.battery_capacity = BATTERY_CAPACITY_FULL*3600
+		else:
+			self.battery_capacity -= current/UPDATE_RATE
 
 	def get_odom(self):
 		speed_trans, speed_rot, posx, posy, angle = struct.unpack(">fffff", i2c_read_reg(0x50, 0x38, 20))
-- 
2.39.5