Commit 1784c13d authored by David Johnson's avatar David Johnson

This commit adds software reset of the brainstem modules. First, the Moto

is reset, then the GP (it seems that the GP acts as the I2C router... so
moto has to be reset first -- this may not be true, not sure yet).  Reset
can be toggled in pilot by sending a USR2 to the pilot process.  If reset
is sent while robot is executing any sort of move, the only way to recover
is to kill pilot.  Reset can be toggled whenever the robot is not moving
without consequence (obviously, brainstem state (including wheel odometry)
is lost).  There is also a separate reset command (brainstem-reset) that
works independently.  This command is now called in the pilot-wrapper.sh
script before pilot is (re)started.

  * GNUmakefile.in: new target `brainstem-reset'.

  * garcia-pilot.cc: not much.

  * garciaUtil.cc: the brainstem_reset function.

  * garciaUtil.hh: prototypes...

  * pilot-wrapper.sh: call brainstem-reset (NOTE: this always fails the
    first time, and I'm not sure why.  It always succeeds subsequently...
    so it makes no difference at the moment).

  * brainstem-reset.cc: new command to reset the brainstem modules.
parent ff922f2c
......@@ -14,7 +14,7 @@ include $(OBJDIR)/Makeconf
ifeq ($(BRAINSTEM_DIR),)
PROGS =
else
PROGS = garcia-pilot
PROGS = garcia-pilot brainstem-reset
endif
all client: $(PROGS)
......@@ -89,6 +89,8 @@ PILOT_OBJS = \
pilotClient.o \
wheelManager.o
BSTEM_OBJS = garciaUtil.o
garcia-pilot-version.c: $(PILOT_OBJS)
garcia-pilot-debug: garcia-pilot-version.c $(PILOT_OBJS)
......@@ -97,6 +99,9 @@ garcia-pilot-debug: garcia-pilot-version.c $(PILOT_OBJS)
garcia-pilot: garcia-pilot-debug
$(STRIP) -o $@ $<
brainstem-reset: brainstem-reset.cc $(BSTEM_OBJS)
$(CXX) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) $(LIBS)
flash-user-led: flash-user-led.o ledManager.o garciaUtil.o
$(CXX) $(CXXFLAGS) -o $@ $^ $(LDFLAGS)
......
/*
* EMULAB-COPYRIGHT
* Copyright (c) 2005 University of Utah and the Flux Group.
* All rights reserved.
*/
/**
* @file garcia-pilot.cc
*
* The main body of the daemon that runs on the garcia robot.
*/
#include "config.h"
#include <errno.h>
#include <stdio.h>
#include <fcntl.h>
#include <assert.h>
#include <signal.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <ucontext.h>
#include <sys/stat.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <list>
#include <algorithm>
#include "acpGarcia.h"
#include "acpValue.h"
/* necessary to reset brainstem mods */
#include "aStem.h"
#include "garcia-pilot.hh"
#include "pilotClient.hh"
#include "dashboard.hh"
#include "wheelManager.hh"
#include "pilotButtonCallback.hh"
#include "garciaUtil.hh"
/**
* Prints the usage message for this daemon to stderr.
*/
static void usage(char *prog)
{
fprintf(stderr,
"usage: %s [module_list]\n"
" (default modules: 4 (Moto), then 2 (GP))\n"
"Optional:\n"
" module_list An ordered list (separated by spaces) of\n"
" addresses of brainstem modules you wish to reset.\n",
prog);
}
int main(int argc, char *argv[])
{
int retval;
aIOLib ioRef;
aErr err;
acpGarcia garcia;
unsigned char default_modules[2] = { 4,2 };
unsigned char *modules = default_modules;
int modules_len = 2;
int i;
/* grab the modules */
if (argc == 2 && strcmp(argv[1],"-h") == 0) {
usage(argv[0]);
exit(0);
}
else if (argc > 1) {
modules = NULL;
modules_len = 0;
for (i = 0; i < argc-1; ++i) {
modules = (unsigned char *)realloc(modules,sizeof(char)*i);
modules[i] = (unsigned char)atoi(argv[i+1]);
++modules_len;
}
}
aIO_GetLibRef(&ioRef, &err);
if (!wait_for_brainstem_link(ioRef, garcia)) {
fprintf(stderr,
"error: could not connect to robot %d\n",
garcia.getNamedValue("status")->getIntVal());
exit(-1);
}
retval = brainstem_reset(ioRef,modules,modules_len);
aIO_ReleaseLibRef(ioRef, &err);
if (modules != default_modules) {
free(modules);
}
return retval;
}
......@@ -31,12 +31,15 @@
#include "acpGarcia.h"
#include "acpValue.h"
/* necessary to reset brainstem mods */
#include "aStem.h"
#include "garcia-pilot.hh"
#include "pilotClient.hh"
#include "dashboard.hh"
#include "wheelManager.hh"
#include "pilotButtonCallback.hh"
#include "garciaUtil.hh"
/**
* The default port to listen for client connections.
......@@ -63,6 +66,12 @@ static const char *DEFAULT_SFILE = "sdata.txt";
*/
static volatile int looping = 1;
/**
* If set, the top of the main loop will reset the brainstem modules.
* Is set from the sigusr2 handler.
*/
static volatile int brainstemResetRequested = 0;
/**
* Signal handler for SIGINT, SIGTERM, and SIGQUIT signals. Sets looping to
* zero and aborts() if it is called more than three times in case the program
......@@ -140,6 +149,14 @@ static void usage(void)
BATTERY_LOG_PATH);
}
/**
* sighandler for USR2 -- reboots the stem modules
*/
static void sigusr2(int sig) {
brainstemResetRequested = 1;
}
class pilotFaultCallback : public faultCallback
{
......@@ -150,6 +167,9 @@ public:
void faultDetected(unsigned long faults)
{
this->pfc_wheel_manager.stop();
/* may wish to call the above function in here, or something */
};
private:
......@@ -158,8 +178,6 @@ private:
};
int main(int argc, char *argv[])
{
int c, port = PILOT_PORT, serv_sock, on_off = 1, ol_demo = 0;
......@@ -291,6 +309,8 @@ int main(int argc, char *argv[])
signal(SIGBUS, sigpanic);
signal(SIGPIPE, SIG_IGN);
signal(SIGUSR2, sigusr2);
aIO_GetLibRef(&ioRef, &err);
......@@ -508,6 +528,106 @@ int main(int argc, char *argv[])
struct timeval tv_zero = { 0, 5000 };
int rc;
/* see if we want to reset the brainstem modules */
if (brainstemResetRequested) {
unsigned char modules[2] = { 4,2 };
// acpValue *v;
// acpValue sv(0.2f);
// v = garcia.getNamedValue("distance-left");
// if (v != NULL) {
// fprintf(stderr,
// "left odometry: %f\n",
// v->getFloatVal());
// }
// v = garcia.getNamedValue("acceleration");
// if (v != NULL) {
// fprintf(stderr,
// "acceleration: %f\n",
// v->getFloatVal());
// }
// v = garcia.getNamedValue("distance-units-string");
// if (v != NULL) {
// fprintf(stderr,
// "dist units: %s\n",
// v->getStringVal());
// }
// garcia.setNamedValue("damped-speed-left", &sv);
// v = garcia.getNamedValue("damped-speed-left");
// if (v != NULL) {
// fprintf(stderr,
// "damped-speed-left: %s\n",
// v->getFloatVal());
// }
/* can't do this in the usr2 handler cause it sleeps */
brainstem_reset(ioRef,modules,2);
brainstemResetRequested = 0;
// v = garcia.getNamedValue("distance-left");
// if (v != NULL) {
// fprintf(stderr,
// "left odometry: %f\n",
// v->getFloatVal());
// }
// v = garcia.getNamedValue("heartbeat-status");
// if (v != NULL) {
// fprintf(stderr,
// "heartbeat-status: %d\n",
// v->getBoolVal());
// }
// {
// acpObject *b;
// acpValue av;
// if (!wait_for_brainstem_link(ioRef, garcia)) {
// fprintf(stderr,
// "error: could not connect to robot %d\n",
// garcia.getNamedValue("status")->getIntVal());
// exit(1);
// }
// garcia.flushQueuedBehaviors();
// av.set("radians");
// garcia.setNamedValue("angle-units-string", &av);
// av.set("meters");
// garcia.setNamedValue("distance-units-string", &av);
// /* turn off fall sensors */
// /* WHY?? -- Dan */
// av.set(0);
// garcia.setNamedValue("down-ranger-enable", &av);
// /* set the stall threshhold high */
// av.set(12.0f);
// garcia.setNamedValue("stall-threshhold", &av);
// av.set(0);
// garcia.setNamedValue("distance-left", &av);
// garcia.setNamedValue("distance-right", &av);
// av.set(aGARCIA_ERRFLAG_ABORT);
// garcia.setNamedValue("status", &av);
// b = garcia.createNamedBehavior("move", NULL);
// av.set(0.2f);
// b->setNamedValue("distance", &av);
// garcia.queueBehavior(b);
// }
}
/* Poll the file descriptors, don't block */
rc = select(FD_SETSIZE,
&rreadyfds,
......
......@@ -14,9 +14,15 @@
#include <stdio.h>
#include <assert.h>
#include <unistd.h>
#include "garciaUtil.hh"
//#include "acpGarcia.h"
//#include "acpValue.h"
/* necessary to reset brainstem mods */
#include "aStem.h"
int debug = 0;
int find_range_start(unsigned long *ranges, unsigned long index)
......@@ -55,3 +61,146 @@ bool wait_for_brainstem_link(aIOLib ioRef, acpGarcia &garcia)
return garcia.getNamedValue("active")->getBoolVal();
}
/**
* reset code for the brainstem GP/Moto modules. Will work IF the router
* hasn't gotten slammed. This could be called whenever the garcia won't
* accept commands, or if it gets into a bad state that we can identify...
*/
int brainstem_reset(aIOLib &ioRef,unsigned char *modules, int modules_len) {
aStemLib stem;
aErr err;
aPacketRef pkt;
aPacketRef recvPkt;
//aIOLib ioRef;
aStreamRef stream;
char code[2];
int retval = 0;
/**
* theoretically packets can't be longer than 8 bytes... but we're careful
* cause the aStem api is woefully underspecified!
*/
char recvData[16];
unsigned char recvLength;
unsigned char recvAddr;
int i;
/**
* doing this cause the prototype from acroname ain't clear as to whether
* the length param to aPacket_Create is actually used wrt the data ptr;
* they don't say if the data ptr has to be a null term byte seq.
*/
code[0] = CMD_RESET_CODE;
code[1] = '\0';
if (aStem_GetLibRef(&stem,&err) == 0) {
fprintf(stderr,"BReset: connected to brainstem.\n");
/* grab the io conn */
//aIO_GetLibRef(&ioRef, &err);
/* associate a stream with the stem connection */
if (aStream_CreateSerial(ioRef,"tts/1",38400,&stream,&err) != 0) {
fprintf(stderr,"BReset: get stream failed (%d)!\n",err);
aStem_ReleaseLibRef(stem,&err);
return -1;
}
/**
* the other possibility would be a kStemRelayStream (if you use
* the aStem_CreateRelayStream... which apparently is the wrong choice
* here...)
*/
if (aStem_SetStream(stem,stream,kStemModuleStream,&err) != 0) {
fprintf(stderr,
"BReset: could not associate stream with stem (%d)!\n",
err);
aStem_ReleaseLibRef(stem,&err);
return -1;
}
/* reset the modules in order given... */
for (i = 0; i < modules_len; ++i) {
if (aPacket_Create(stem,modules[i],1,code,&pkt,&err) != 0) {
fprintf(stderr,
"BReset: pkt create failed for module %d (%d)\n",
modules[i],
err);
retval = modules[i];
break;
}
if (aStem_SendPacket(stem,pkt,&err) != 0) {
fprintf(stderr,
"BReset: send failed for module %d.\n",
modules[i]);
retval = modules[i];
break;
}
if (aStem_GetPacket(stem,NULL,NULL,1000,&recvPkt,&err) != 0) {
fprintf(stderr,"BReset: did not get packet!\n");
retval = modules[i];
break;
}
if (aPacket_GetData(stem,
recvPkt,
&recvAddr,
&recvLength,
recvData,
&err) == 0) {
if (recvLength == 2
&& 1
&& recvData[0] == 128
&& recvData[1] == 4) {
fprintf(stderr,
"BReset: succeeded for module %d\n",
modules[i]);
}
else {
fprintf(stderr,
"BReset: packet: module address = %d, "
"length = %d; ",
recvAddr,
recvLength);
for (i = 0; i < recvLength; ++i) {
fprintf(stderr,"d[%d]=%d ",i,recvData[i]);
}
fprintf(stderr,"\n");
;
}
}
else {
fprintf(stderr,"could not get data!\n");
retval = modules[i];
break;
}
aPacket_Destroy(stem,recvPkt,&err);
if ((i+1) != modules_len) {
fprintf(stderr,"BReset: inter-module sleep\n");
sleep(1);
}
}
aStem_ReleaseLibRef(stem,&err);
fprintf(stderr,"BReset: done.\n");
}
else {
/* need to send somebody mail saying we couldn't reset the stem */
fprintf(stderr,
"BReset: could not connect to brainstem for reset!!!\n");
retval = -1;
}
return retval;
}
......@@ -68,4 +68,33 @@ public:
};
/**
* got this info from
* http://www.acroname.com/brainstem/ref/ref.html#Hardware/stemnetwork.html ;
* if you string multiple GPs or Motos, then I'm sure it's wrong. If anybody
* ever changes the bus addrs for our GP/Motos, THIS CODE WILL NOT WORK!!!
*/
#define BUS_ADDR_GP 2
#define BUS_ADDR_MOTO 4
/** this is valid for both the GP and Moto. */
#define CMD_RESET_CODE 24
/** this resets first module 4 (the moto), then module 2 (the gp) */
#define BRESET_DEFAULT_MOD "42"
#define BRESET_DEFAULT_MOD_LEN 2
/**
* reset code for the brainstem GP/Moto modules. Will work IF the router
* hasn't gotten slammed. This could be called whenever the garcia won't
* accept commands, or if it gets into a bad state that we can identify...
*
* @param ioRef The garcia IO lib reference.
* @param modules A char array of module addresses to be reset.
* @param modules_length Length of modules array.
* @return 0 if all modules successfully reset; -1 if could not connect to
* the brainstem; otherwise number of first failed module.
*/
int brainstem_reset(aIOLib &ioRef,unsigned char *modules,int modules_length);
#endif
......@@ -89,6 +89,13 @@ while (1) {
if ($childpid < 0);
if ($childpid == 0) {
$retval = system("$BINDIR/brainstem-reset");
if ($retval != 0) {
print "WARNING: brainstem reset failed ($retval)!!!\n";
}
sleep(1);
exec("$BINDIR/garcia-pilot -d -l /var/emulab/logs/pilot.log");
die("*** $0:\n".
" Could not exec garcia-pilot!\n");
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment