Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22720] pkg/trunk/stacks
|
From: <wg_...@us...> - 2009-08-24 19:07:51
|
Revision: 22720
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22720&view=rev
Author: wg_cmeyers
Date: 2009-08-24 19:07:45 +0000 (Mon, 24 Aug 2009)
Log Message:
-----------
Adding BatteryServer and moving BatteryState to PowerState.
BatteryServer is a message from the ocean_server containing battery information.
PowerState is a message stating the remaining power left in the system.
Modified Paths:
--------------
pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryState.msg
pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.cpp
pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.h
pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean_test.cpp
pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/server.cpp
Added Paths:
-----------
pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryServer.msg
pkg/trunk/stacks/pr2_common/pr2_msgs/msg/PowerState.msg
Added: pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryServer.msg
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryServer.msg (rev 0)
+++ pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryServer.msg 2009-08-24 19:07:45 UTC (rev 22720)
@@ -0,0 +1,19 @@
+Header header
+uint32 MAX_BAT_COUNT=4
+uint32 MAX_BAT_REG=48
+# Battery System Stats
+int32 lastTimeSystem #epoch time
+uint16 timeLeft # in minutes
+uint16 averageCharge # in percent
+string message
+# Battery Controller Flags
+int32 lastTimeController #epoch time
+uint16 present
+uint16 charging
+uint16 discharging
+uint16 reserved
+uint16 powerPresent
+uint16 powerNG
+uint16 inhibited
+# for each battery
+pr2_msgs/BatteryState[] battery
Modified: pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryState.msg
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryState.msg 2009-08-24 19:07:15 UTC (rev 22719)
+++ pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryState.msg 2009-08-24 19:07:45 UTC (rev 22720)
@@ -1,5 +1,5 @@
-Header header
-float64 power_consumption ## Watts
-int64 time_remaining ## minutes
-string prediction_method ## how time_remaining is being calculated
-int8 AC_present ## number of packs detecting AC power
+# Each batteries registers
+int32 lastTimeBattery #epoch time
+uint16[48] batReg
+uint16[48] batRegFlag
+int32[48] batRegTime
Copied: pkg/trunk/stacks/pr2_common/pr2_msgs/msg/PowerState.msg (from rev 22719, pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryState.msg)
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_msgs/msg/PowerState.msg (rev 0)
+++ pkg/trunk/stacks/pr2_common/pr2_msgs/msg/PowerState.msg 2009-08-24 19:07:45 UTC (rev 22720)
@@ -0,0 +1,5 @@
+Header header
+float64 power_consumption ## Watts
+int64 time_remaining ## minutes
+string prediction_method ## how time_remaining is being calculated
+int8 AC_present ## number of packs detecting AC power
Modified: pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.cpp
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.cpp 2009-08-24 19:07:15 UTC (rev 22719)
+++ pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.cpp 2009-08-24 19:07:45 UTC (rev 22720)
@@ -72,7 +72,7 @@
ocean::ocean ( int debug)
{
debuglevel = debug;
-
+ server.battery.resize(4);
}
ocean::~ocean ()
@@ -112,11 +112,11 @@
set_speed (19200);
- memset(batReg, 0, sizeof(batReg));
- memset(batRegFlag, 0, sizeof(batReg));
- lastTimeSystem = lastTimeController = 0;
- memset( lastTimeBattery, 0, sizeof(lastTimeBattery));
- memset( batRegTime, 0, sizeof(batRegTime));
+ //memset(batReg, 0, sizeof(batReg));
+ //memset(batRegFlag, 0, sizeof(batReg));
+ server.lastTimeSystem = server.lastTimeController = 0;
+ //memset( server.lastTimeBattery, 0, sizeof(server.lastTimeBattery));
+ //memset( batRegTime, 0, sizeof(batRegTime));
#if (FILE_LOGGING > 0)
char logname[128];
@@ -538,7 +538,7 @@
* 6 ASCII text message
*
*/
- time(&lastTimeSystem);
+ time((time_t*)&server.lastTimeSystem);
for( int index = 1; index < count; )
{
@@ -548,16 +548,17 @@
switch(tmp)
{
case 1:
- timeLeft = (unsigned short)strtol( field[index], 0, 16 );
- report (5, "timeLeft=%x\n", timeLeft);
+ server.timeLeft = (unsigned short)strtol( field[index], 0, 16 );
+ report (5, "timeLeft=%x\n", server.timeLeft);
break;
case 3:
- sscanf( field[index], "%s", message);
- report (5, "processSystem message=%s\n", message);
+ server.message.assign(field[index]);
+ //sscanf( field[index], "%s", server.message.c_str());
+ report (5, "processSystem message=%s\n", server.message.c_str());
break;
case 4:
- averageCharge = (unsigned short)strtol( field[index], 0, 16 );
- report (5, "averageCharge=%x\n", averageCharge);
+ server.averageCharge = (unsigned short)strtol( field[index], 0, 16 );
+ report (5, "averageCharge=%x\n", server.averageCharge);
break;
default:
;
@@ -590,7 +591,7 @@
*
*/
- time(&lastTimeController);
+ time((time_t*)&server.lastTimeController);
for( int index = 1; index < count; )
{
@@ -611,37 +612,37 @@
{
case 1:
{
- present = value;
+ server.present = value;
}
break;
case 2:
{
- charging = value;
+ server.charging = value;
}
break;
case 3:
{
- discharging = value;
+ server.discharging = value;
}
break;
case 4:
{
- reserved = value;
+ server.reserved = value;
}
break;
case 5:
{
- powerPresent = value;
+ server.powerPresent = value;
}
break;
case 6:
{
- powerNG = value;
+ server.powerNG = value;
}
break;
case 7:
{
- inhibited = value;
+ server.inhibited = value;
}
break;
}
@@ -672,7 +673,7 @@
report (5, "processBattery count=%d \n", count);
report (5, "currentBattery=%d \n", battery);
- time(&lastTimeBattery[battery]);
+ time((time_t*)&server.battery[battery].lastTimeBattery);
--count; //get past sentence type
unsigned int regNumber;
@@ -685,15 +686,15 @@
value = strtoul( field[xx], 0, 16);
++xx;
report (5, "reg[%u]=%x \n", regNumber, value);
- if(regNumber >= MAX_BAT_REG)
+ if(regNumber >= server.MAX_BAT_REG)
{
- report (2, "Register greater than expected: %x MAX_BAT_REG=%x\n", regNumber, MAX_BAT_REG);
+ report (2, "Register greater than expected: %x MAX_BAT_REG=%x\n", regNumber, server.MAX_BAT_REG);
}
else
{
- batReg[battery][regNumber] = value;
- batRegFlag[battery][regNumber] = 0x1;
- time(&batRegTime[battery][regNumber]);
+ server.battery[battery].batReg[regNumber] = value;
+ server.battery[battery].batRegFlag[regNumber] = 0x1;
+ time((time_t*)&server.battery[battery].batRegTime[regNumber]);
}
count -= 2;
Modified: pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.h
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.h 2009-08-24 19:07:15 UTC (rev 22719)
+++ pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.h 2009-08-24 19:07:45 UTC (rev 22720)
@@ -8,6 +8,7 @@
#include <time.h>
#include <list>
#include <utility>
+#include "pr2_msgs/BatteryServer.h"
namespace willowgarage
{
@@ -93,6 +94,7 @@
static const struct regPair regList[];
static const unsigned regListLength;
+#if 0
static const unsigned int MAX_BAT_COUNT = 4;
static const unsigned int MAX_BAT_REG = 0x30;
time_t lastTimeSystem, lastTimeController, lastTimeBattery[MAX_BAT_COUNT];
@@ -109,6 +111,8 @@
unsigned short batRegFlag[MAX_BAT_COUNT][MAX_BAT_REG];
time_t batRegTime[MAX_BAT_COUNT][MAX_BAT_REG];
char message[64];
+#endif
+ pr2_msgs::BatteryServer server;
};
}
}
Modified: pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean_test.cpp
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean_test.cpp 2009-08-24 19:07:15 UTC (rev 22719)
+++ pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean_test.cpp 2009-08-24 19:07:45 UTC (rev 22720)
@@ -17,11 +17,12 @@
os.initialize(argv[1]);
- int count = 10;
+ //int count = 10;
while(1)
{
os.run();
usleep(100);
+#if 0
--count;
if(count == 0)
{
@@ -53,5 +54,6 @@
}
cout << "------------------------------\n";
}
+#endif
}
}
Modified: pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/server.cpp
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/server.cpp 2009-08-24 19:07:15 UTC (rev 22719)
+++ pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/server.cpp 2009-08-24 19:07:45 UTC (rev 22720)
@@ -100,6 +100,7 @@
std::stringstream ss;
ros::Publisher pub = handle.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
+ ros::Publisher bs = handle.advertise<pr2_msgs::BatteryServer>("/battery/server", 1);
ros::Rate rate(100); //set the rate we scan the device for input
diagnostic_msgs::DiagnosticArray msg_out;
@@ -117,6 +118,8 @@
//ros::spinOnce();
currentTime = Time::now();
+ bs.publish(os.server);
+
if((os.run() > 0) && ((currentTime - lastTime) > MESSAGE_TIME))
{
@@ -130,15 +133,16 @@
stat.level = 0;
stat.message = "OK";
+#if 0
stat.add("Time Remaining (min)", os.timeLeft);
stat.add("Average charge (percent)", os.averageCharge );
//stat.add("Current (A)", 0);
//stat.add("Voltage (V)", 0);
- stat.add("Time since update (s)", (currentTime.sec - os.lastTimeSystem));
+ stat.add("Time since update (s)", (currentTime.sec - os.server.lastTimeSystem));
msg_out.status.push_back(stat);
- for(unsigned int xx = 0; xx < os.MAX_BAT_COUNT; ++xx)
+ for(unsigned int xx = 0; xx < os.server.MAX_BAT_COUNT; ++xx)
{
unsigned batmask = (1<<xx);
if(os.present & batmask)
@@ -171,7 +175,7 @@
}
}
- stat.add("Time since update (s)", (currentTime.sec - os.lastTimeBattery[xx]));
+ stat.add("Time since update (s)", (currentTime.sec - os.server.lastTimeBattery[xx]));
msg_out.status.push_back(stat);
@@ -183,6 +187,7 @@
#endif
}
}
+#endif
pub.publish(msg_out);
msg_out.status.clear();
@@ -243,8 +248,8 @@
for(int xx = 0; xx < max_ports; ++xx)
server_list[xx].start();
- //ros::spin(); //wait for ros to shut us down
-#if 1
+ ros::spin(); //wait for ros to shut us down
+#if 0
ros::Rate rate(1);
ros::Publisher pubBatteryState = handle.advertise<pr2_msgs::BatteryState>("battery_state", 1);
pr2_msgs::BatteryState batteryState;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
