BeeSTEM.C

Go to the documentation of this file.
00001 
00002 /*!@file Devices/BeeSTEM.C Interface to Rand Voorhies' BeeSTEM controller*/
00003 
00004 // //////////////////////////////////////////////////////////////////// //
00005 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00006 // University of Southern California (USC) and the iLab at USC.         //
00007 // See http://iLab.usc.edu for information about this project.          //
00008 // //////////////////////////////////////////////////////////////////// //
00009 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00010 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00011 // in Visual Environments, and Applications'' by Christof Koch and      //
00012 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00013 // pending; application number 09/912,225 filed July 23, 2001; see      //
00014 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00015 // //////////////////////////////////////////////////////////////////// //
00016 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00017 //                                                                      //
00018 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00019 // redistribute it and/or modify it under the terms of the GNU General  //
00020 // Public License as published by the Free Software Foundation; either  //
00021 // version 2 of the License, or (at your option) any later version.     //
00022 //                                                                      //
00023 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00024 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00025 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00026 // PURPOSE.  See the GNU General Public License for more details.       //
00027 //                                                                      //
00028 // You should have received a copy of the GNU General Public License    //
00029 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00030 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00031 // Boston, MA 02111-1307 USA.                                           //
00032 // //////////////////////////////////////////////////////////////////// //
00033 //
00034 // Primary maintainer for this file: Rand Voorhies <voorhies@usc.edu>
00035 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Devices/BeeSTEM.C $
00036 //
00037 #include "Devices/BeeSTEM.H"
00038 #include "Component/OptionManager.H"
00039 #include "Util/Assert.H"
00040 #include "rutz/compat_snprintf.h"
00041 
00042 
00043 // ######################################################################
00044 BeeSTEMListener::~BeeSTEMListener()
00045 { }
00046 
00047 // ######################################################################
00048 void *BeeSTEM_run(void *c)
00049 {
00050         BeeSTEM *d = (BeeSTEM *)c;
00051         d->run();
00052         return NULL;
00053 }
00054 
00055 // ######################################################################
00056 //BeeSTEM::BeeSTEM(OptionManager& mgr, const std::string& descrName,
00057 //                 const std::string& tagName, const char* dev) :
00058 //  ModelComponent(mgr, descrName, tagName),
00059 //  itsSerial(new Serial(mgr, descrName+" Serial Port", tagName+"SerialPort"))
00060 
00061 
00062 
00063 
00064 BeeSTEM::BeeSTEM(OptionManager& mgr, const std::string& descrName,
00065                 const std::string& tagName, const char *dev) :
00066         ModelComponent(mgr, descrName, tagName),
00067         itsSerial(new Serial(mgr, descrName+" Serial Port", tagName+"SerialPort")),
00068         itsKeepgoing(true), itsListener()
00069 {
00070 
00071         //Initialize the serial port
00072         itsSerial->configure(dev, 38400, "8N1", false, false, 0);
00073         addSubComponent(itsSerial);
00074 
00075         //Allocate space for the data
00076         itsData = new byte[DATATOP+1];
00077 
00078         // Initialize all data to zero:
00079         for (uint i = 0; i <= DATATOP; i ++)
00080                 itsData[i]=0;
00081 
00082         for(uint i=0; i<5; i++)
00083                 itsCurrentMotorValues[i] = 0;
00084 
00085         itsListener.reset(NULL);
00086         pthread_mutex_init(&lock, NULL);
00087         pthread_mutex_init(&serlock, NULL);
00088 
00089 }
00090 
00091 // ######################################################################
00092 void BeeSTEM::start1()
00093 {
00094         // start our thread:
00095         itsKeepgoing = true;
00096         pthread_create(&runner, NULL, &BeeSTEM_run, (void *)this);
00097 }
00098 
00099 // ######################################################################
00100 void BeeSTEM::stop2()
00101 {
00102         itsKeepgoing = false;
00103         usleep(300000); // make sure thread has exited
00104 }
00105 
00106 // ######################################################################
00107 BeeSTEM::~BeeSTEM()
00108 {
00109         pthread_mutex_destroy(&lock);
00110         pthread_mutex_destroy(&serlock);
00111 }
00112 
00113 // ######################################################################
00114 void BeeSTEM::setListener(rutz::shared_ptr<BeeSTEMListener>& listener)
00115 { itsListener = listener; }
00116 
00117 // ######################################################################
00118 bool BeeSTEM::echoRequest()
00119 {
00120         // 11111000: Echo request (will send back an echo reply)
00121         return writeByte(BIN(11111010));
00122 }
00123 
00124 // ######################################################################
00125 bool BeeSTEM::debugMode(const bool on)
00126 {
00127         // 1111110x: Turn debug mode on/off (x)
00128         return writeByte(BIN(11111100) | (on?1:0));
00129 }
00130 
00131 // ######################################################################
00132 bool BeeSTEM::resetChip()
00133 {
00134         // 11111001: Reset the BeeSTEM
00135         return writeByte(BIN(11111011));
00136 }
00137 
00138 
00139 // ######################################################################
00140 bool BeeSTEM::lcdPrintf(const char *fmt, ...)
00141 {
00142 
00143         LERROR("Not Yet Implemented!");
00144         return false;
00145         // format and write out our message, truncating at our number of columns:
00146         /*  char txt[itsLCDcols.getVal() + 1];
00147                         va_list a; va_start(a, fmt);
00148                         vsnprintf(txt, itsLCDcols.getVal()+1, fmt, a);
00149                         va_end(a);
00150 
00151         // send it off to the LCD, char by char:
00152         bool ret = true;
00153         pthread_mutex_lock(&serlock);
00154         for (unsigned int i = 0; i < strlen(txt); i ++)
00155         ret &= lcdSendRaw(txt[i], true, false);
00156         pthread_mutex_unlock(&serlock);
00157 
00158         return ret;*/
00159 }
00160 
00161 // ######################################################################
00162 bool BeeSTEM::lcdClear()
00163 {
00164         return writeByte(BIN(11000100), false);
00165 }
00166 
00167 bool BeeSTEM::setMotor(const int motor, signed char speed)
00168 {
00169         if(motor < 0 || motor >4) {
00170                 LERROR("Invalid Motor Number (%d)", motor);
00171                 return false;
00172         }
00173         if (speed > 100) speed = 100;
00174         if (speed < -100) speed = -100;
00175 
00176         if (speed == 0) speed = 1; //hack
00177         LINFO("speed %i", speed);
00178 
00179         if (abs(itsCurrentMotorValues[motor] - speed) > 60 &&
00180                         itsCurrentMotorValues[motor]*speed < 0)
00181         {
00182                 pthread_mutex_lock(&serlock);
00183                 bool ret = writeByte( (0) & BIN(00111111), false);
00184                 if (ret >= 0) ret = writeByte(BIN(01000000) | (motor << 2) | (BIN(00000011) & ((0) >> 6)), false);
00185                 pthread_mutex_unlock(&serlock);
00186                 sleep(1);
00187 
00188         }
00189         itsCurrentMotorValues[motor] = speed;
00190 
00191         // attempt to set it:
00192         // 00xxxxxx: Memorize 6 LSB xxxxxx to later send to a motor
00193         // 010xxxyy: Combine 2 MSB yy to memorized and send to motor xxx
00194         pthread_mutex_lock(&serlock);
00195         bool ret = writeByte( (speed/2) & BIN(00111111), false);
00196         if (ret >= 0) ret = writeByte(BIN(01000000) | (motor << 2) | (BIN(00000011) & ((speed/2) >> 6)), false);
00197         pthread_mutex_unlock(&serlock);
00198 
00199         if (ret < 0) { LERROR("Set motor failed - keeping old. ret=%d", ret); return false; }
00200 
00201         return true;
00202 }
00203 
00204 bool BeeSTEM::toggleCalibrateHMR3300() {
00205         pthread_mutex_lock(&serlock);
00206         bool ret = writeByte(BIN(11111110), false);
00207         pthread_mutex_unlock(&serlock);
00208 
00209         return ret;
00210 }
00211 
00212 bool BeeSTEM::levelHMR3300() {
00213         pthread_mutex_lock(&serlock);
00214         bool ret = writeByte(BIN(11111111), false);
00215         pthread_mutex_unlock(&serlock);
00216 
00217         return ret;
00218 }
00219 
00220 // ######################################################################
00221 
00222 bool BeeSTEM::setReporting(const BeeSTEMReportingMask m, const bool on) {
00223         return writeByte(m | (on?1:0));
00224 }
00225 
00226 // ######################################################################
00227 //! Get Compass Heading
00228 byte BeeSTEM::getCompassHeading() {
00229         pthread_mutex_lock(&lock);
00230         byte val = itsData[COMPASS_HEADING_EVENT];
00231         pthread_mutex_unlock(&lock);
00232         return val;
00233 }
00234 
00235 //! Get Compass Pitch
00236 byte BeeSTEM::getCompassPitch() {
00237         pthread_mutex_lock(&lock);
00238         byte val = itsData[COMPASS_PITCH_EVENT];
00239         pthread_mutex_unlock(&lock);
00240         return val;
00241 }
00242 
00243 //! Get Compass Roll
00244 byte BeeSTEM::getCompassRoll() {
00245         pthread_mutex_lock(&lock);
00246         byte val = itsData[COMPASS_ROLL_EVENT];
00247         pthread_mutex_unlock(&lock);
00248         return val;
00249 }
00250 
00251 //! Get High Speed Accelerometer X
00252 byte BeeSTEM::getAccelX() {
00253         pthread_mutex_lock(&lock);
00254         byte val = itsData[ACCEL_X_EVENT];
00255         pthread_mutex_unlock(&lock);
00256         return val;
00257 }
00258 
00259 //! Get High Speed Accelerometer Y
00260 byte BeeSTEM::getAccelY() {
00261         pthread_mutex_lock(&lock);
00262         byte val = itsData[ACCEL_Y_EVENT];
00263         pthread_mutex_unlock(&lock);
00264         return val;
00265 }
00266 
00267 //! Get Internal Pressure
00268 byte BeeSTEM::getIntPress() {
00269         pthread_mutex_lock(&lock);
00270         byte val = itsData[INT_PRESS_EVENT];
00271         pthread_mutex_unlock(&lock);
00272         return val;
00273 }
00274 
00275 //! Get External Pressure
00276 byte BeeSTEM::getExtPress() {
00277         pthread_mutex_lock(&lock);
00278         byte val = itsData[EXT_PRESS_EVENT];
00279         pthread_mutex_unlock(&lock);
00280         return val;
00281 }
00282 
00283 //! Get Temperature Zone 1
00284 byte BeeSTEM::getTemp1() {
00285         pthread_mutex_lock(&lock);
00286         byte val = itsData[TEMP1_EVENT];
00287         pthread_mutex_unlock(&lock);
00288         return val;
00289 }
00290 
00291 //! Get Temperature Zone 2
00292 byte BeeSTEM::getTemp2() {
00293         pthread_mutex_lock(&lock);
00294         byte val = itsData[TEMP2_EVENT];
00295         pthread_mutex_unlock(&lock);
00296         return val;
00297 }
00298 
00299 //! Get Temperature Zone 3
00300 byte BeeSTEM::getTemp3() {
00301         pthread_mutex_lock(&lock);
00302         byte val = itsData[TEMP3_EVENT];
00303         pthread_mutex_unlock(&lock);
00304         return val;
00305 }
00306 
00307 //! Get current raw Analog value from the spare ADC pin
00308 byte BeeSTEM::getSpareADC() {
00309         pthread_mutex_lock(&lock);
00310         byte val = itsData[ADC_IN_EVENT];
00311         pthread_mutex_unlock(&lock);
00312         return val;
00313 }
00314 
00315 //! Get the current from a motor
00316 /*! 0 for A, ..., 4 for E*/
00317 byte BeeSTEM::getMotorCurr(byte whichone) {
00318         pthread_mutex_lock(&lock);
00319         byte val = itsData[MOTOR_A_CURR_EVENT+whichone];
00320         pthread_mutex_unlock(&lock);
00321         return val;
00322 }
00323 //! Get the value of a single digital input pin
00324 bool BeeSTEM::getDigitalIn(const int whichone)
00325 {
00326         ASSERT(whichone >= 0 && whichone < 8);
00327         return ((itsData[DIG_IN] & (1 << whichone)) != 0);
00328 }
00329 
00330 //! Get the whole digital input byte
00331 byte BeeSTEM::getDigitalIn()
00332 {
00333         return itsData[DIG_IN_EVENT];
00334 }
00335 
00336 // ######################################################################
00337 bool BeeSTEM::setDigitalOut(const int outnum, const bool on)
00338 {
00339         ASSERT(outnum >= 0 && outnum < 8);
00340         //1110yyyx:        Set digital output yyy to value x
00341         return writeByte(BIN(11100000) | (outnum << 1) | (on?1:0));
00342 }
00343 
00344 // ######################################################################
00345 bool BeeSTEM::writeByte(const byte val, const bool uselock)
00346 {
00347         if (MYLOGVERB >= LOG_DEBUG)
00348         {
00349                 char txt[9];   txt[8] = '\0';
00350                 for (int i = 0; i < 8; i ++) txt[i] = (val >> (7-i)) & 1 ? '1':'0';
00351                 LDEBUG("Sending: %s", txt);
00352         }
00353 
00354         if (uselock) pthread_mutex_lock(&serlock);
00355         bool ret = (itsSerial->write(&val, 1) <= 0);
00356         if (uselock) pthread_mutex_unlock(&serlock);
00357 
00358         if (ret) PLERROR("Write to BeeSTEM failed!! -- ret=%d", ret);
00359         return ret;
00360 }
00361 
00362 // ######################################################################
00363 void BeeSTEM::run()
00364 {
00365         //Because the BeeSTEM data format relies on two seperate bytes to transmit the data and type
00366         //information, we not only have to keep track of the last bit of data recieved, but also
00367         //the last command type.
00368         BeeSTEMEventType last_cmd = NO_EVENT;
00369         bool last_bit = 0;  //The LSB from the header is the MSB of the data
00370         bool cmd_found = false;
00371         byte c;
00372 
00373         while(itsKeepgoing)
00374         {
00375 
00376                 c = itsSerial->read();
00377                 if (MYLOGVERB >= LOG_DEBUG)
00378                 {
00379                         char txt[9]; txt[8] = '\0';
00380                         for (int i = 0; i < 8; i ++) txt[i] = (c >> (7-i)) & 1 ? '1':'0';
00381                         LDEBUG("Received: %s", txt);
00382                 }
00383 
00384                 /*
00385                          BeeSTEM to PC serial command protocol:
00386                          To avoid framing problems, the following BeeSTEM data is sent in two byte frames.
00387                          The first (header) byte has an MSB of 0 followed by a 6 bit datacode
00388                          followed by the MSB of the corresponding data.
00389                          The second (data) byte has an MSB of 1 followed by the 7 LSBs of the data
00390 
00391 0000000x:        Compass Heading
00392 0000001x:        Compass Pitch
00393 0000010x:        Compass Roll
00394 0000011x:        High Speed Accelerometer X
00395 0000100x:        High Speed Accelerometer Y
00396 0000101x:        Internal Pressure
00397 0000110x:        External Pressure
00398 0000111x:        Temperature Zone 1
00399 0001000x:        Temperature Zone 2
00400 0001001x:        Temperature Zone 3
00401 0001010x:        Digital Input
00402 0001011x:        Spare ADC Pin
00403 0010yyyx:        Motor yyy current (A=000,...,E=101)
00404 
00405 The following status frames are single byte only:
00406 01110000:        Echo Reply (in reply to an echo request)
00407 01110001:        Reset occured, data may have been lost
00408 01110010:        Input command serial buffer overflow, data was lost (SW Error)
00409 01110011:        Framing error on serial input, data was lost (HW Error)
00410 01110100:        OverRun error on serial input, data was lost (HW Error)
00411 01110101:        Lost communication with HMR3300
00412 01110110:        Lost communication with High Speed Accelerometer
00413 01110111:        Lost communication with Temp1
00414 01111000:        Lost communication with Temp2
00415 01111001:         HMR3300 Leveled
00416 01111010:        EStop has been pulled
00417 01111011:        Invalid / Unrecognized / Unimplemented command -- ignored
00418 01111100:        Bad Command Sequence Recieved -- ignored
00419 01111101:        Reset request acknowledged, attempting reset
00420 01111110:         HMR3300 Calibrating
00421 */
00422 
00423                 //First check to see if the incoming byte is a header or data
00424                 if(c >> 7 == 0) {//If the MSB is a 0, then it is a header
00425                         last_bit = ( c & (00000001) ); //Grab the LSB off the header, because it will be the MSB of the data
00426 
00427                         //Now, parse the command type
00428 
00429                         //Is it a motor current report?
00430                         if(c>>4 == BIN(0010)) {
00431                                 switch((c >> 1) & 00000111) {
00432                                         case BIN(000):
00433                                                 last_cmd = MOTOR_A_CURR_EVENT;
00434                                                 LDEBUG("Got Motor A Current Header");
00435                                                 break;
00436                                         case BIN(001):
00437                                                 last_cmd = MOTOR_B_CURR_EVENT;
00438                                                 LDEBUG("Got Motor B Current Header");
00439                                                 break;
00440                                         case BIN(010):
00441                                                 last_cmd = MOTOR_C_CURR_EVENT;
00442                                                 LDEBUG("Got Motor C Current Header");
00443                                                 break;
00444                                         case BIN(011):
00445                                                 last_cmd = MOTOR_D_CURR_EVENT;
00446                                                 LDEBUG("Got Motor D Current Header");
00447                                                 break;
00448                                         case BIN(100):
00449                                                 last_cmd = MOTOR_E_CURR_EVENT;
00450                                                 LDEBUG("Got Motor E Current Header");
00451                                                 break;
00452                                         default:
00453                                                 last_cmd = UNRECOGNIZED_EVENT;
00454                                                 LERROR("Unknown Motor Current Report From BeeSTEM (Motor %d) -- Ignored", (c >> 1) & 00000111);
00455                                 }
00456                         }
00457                         else { //If not, then see what it is?
00458                                 cmd_found = false;
00459 
00460                                 //First let's check the single byte types (Echo Reply through Invalid Command)
00461                                 switch(c) {
00462                                         case BIN(01110000): //Echo Reply
00463                                                 LDEBUG("Got Echo Reply");
00464                                                 cmd_found = true;
00465                                                 if (itsListener.get())
00466                                                         itsListener->event(ECHO_REPLY_EVENT, 0, 0);
00467                                                 break;
00468                                         case BIN(01110001): //Reset Occured
00469                                                 cmd_found = true;
00470                                                 LDEBUG("Got Reset");
00471                                                 if (itsListener.get())
00472                                                         itsListener->event(RESET_EVENT, 0, 0);
00473                                                 break;
00474                                         case BIN(01110010): //Soft Buffer Overflow
00475                                                 cmd_found = true;
00476                                                 LDEBUG("Got Soft Buffer Overflow");
00477                                                 if (itsListener.get())
00478                                                         itsListener->event(SW_OVERFLOW_EVENT, 0, 0);
00479                                                 break;
00480                                         case BIN(01110011): //Hard Framing Error
00481                                                 cmd_found = true;
00482                                                 LDEBUG("Got Hard Framing Error");
00483                                                 if (itsListener.get())
00484                                                         itsListener->event(FRAMING_ERR_EVENT, 0, 0);
00485                                                 break;
00486                                         case BIN(01110100): //Hard Buffer Overflow
00487                                                 cmd_found = true;
00488                                                 LDEBUG("Got Hard Buffer Overflow");
00489                                                 if (itsListener.get())
00490                                                         itsListener->event(OVR_ERR_EVENT, 0, 0);
00491                                                 break;
00492                                         case BIN(01110101): //Lost HMR3300
00493                                                 cmd_found = true;
00494                                                 LDEBUG("Got Lost HMR3300");
00495                                                 if (itsListener.get())
00496                                                         itsListener->event(HMR3300_LOST_EVENT, 0, 0);
00497                                                 break;
00498                                         case BIN(01110110): //Lost Accelerometer
00499                                                 cmd_found = true;
00500                                                 LDEBUG("Got Lost Accelerometer");
00501                                                 if (itsListener.get())
00502                                                         itsListener->event(ACCEL_LOST_EVENT, 0, 0);
00503                                                 break;
00504                                         case BIN(01110111): //Lost Temperature Sensor 1
00505                                                 cmd_found = true;
00506                                                 LDEBUG("Got Lost Temp1");
00507                                                 if (itsListener.get())
00508                                                         itsListener->event(TEMP1_LOST_EVENT, 0, 0);
00509                                                 break;
00510                                         case BIN(01111000): //Lost Temperature Sensor 2
00511                                                 cmd_found = true;
00512                                                 LDEBUG("Got Lost Temp2");
00513                                                 if (itsListener.get())
00514                                                         itsListener->event(TEMP2_LOST_EVENT, 0, 0);
00515                                                 break;
00516                                         case BIN(01111001): //The HMR3300 has been leveled
00517                                                 cmd_found = true;
00518                                                 LDEBUG("Got HMR3300 Leveling Complete");
00519                                                 if (itsListener.get())
00520                                                         itsListener->event(HMR_LEVELED_EVENT, 0, 0);
00521                                                 break;
00522                                         case BIN(01111010): //ESTOP Was Pulled
00523                                                 cmd_found = true;
00524                                                 LDEBUG("Got ESTOP");
00525                                                 if (itsListener.get())
00526                                                         itsListener->event(ESTOP_EVENT, 0, 0);
00527                                                 break;
00528                                         case BIN(01111011): //The command was unrecognized by the BeeSTEM
00529                                                 cmd_found = true;
00530                                                 LDEBUG("Got Unrecognized Command");
00531                                                 if (itsListener.get())
00532                                                         itsListener->event(UNRECOGNIZED_EVENT, 0, 0);
00533                                                 break;
00534                                         case BIN(01111100): //The BeeSTEM has recieved a bad command sequence
00535                                                 cmd_found = true;
00536                                                 LDEBUG("Got BeeSTEM Reporting Bad Command Sequence");
00537                                                 if(itsListener.get())
00538                                                         itsListener->event(BAD_OUT_CMD_SEQ_EVENT, 0, 0);
00539                                                 break;
00540                                         case BIN(01111101): //The BeeSTEM has acknowledged a reset request, and is attempting to reset
00541                                                 cmd_found = true;
00542                                                 LDEBUG("Got Reset Acknowledge");
00543                                                 if(itsListener.get())
00544                                                         itsListener->event(RESET_ACK_EVENT, 0, 0);
00545                                                 break;
00546                                         case BIN(01111110): //The HMR3300 has been put into calibration mode
00547                                                 cmd_found = true;
00548                                                 LDEBUG("Got HMR3300 Calibration Started");
00549                                                 if(itsListener.get())
00550                                                         itsListener->event(HMR3300_CAL_EVENT, 0, 0);
00551                                                 break;
00552 
00553 
00554                                 }
00555                                 //If it's not any of the single data bytes, then it must be the header to a data byte
00556                                 if(!cmd_found)
00557                                         switch(c >> 1)
00558                                         {
00559                                                 case BIN(0000000):
00560                                                         LDEBUG("Got Compass Heading Header");
00561                                                         last_cmd = COMPASS_HEADING_EVENT;
00562                                                         break;
00563                                                 case BIN(0000001):
00564                                                         LDEBUG("Got Compass Pitch Header");
00565                                                         last_cmd = COMPASS_PITCH_EVENT;
00566                                                         break;
00567                                                 case BIN(0000010):
00568                                                         LDEBUG("Got Compass Roll Header");
00569                                                         last_cmd = COMPASS_ROLL_EVENT;
00570                                                         break;
00571                                                 case BIN(0000011):
00572                                                         LDEBUG("Got Accelerometer X Header");
00573                                                         last_cmd = ACCEL_X_EVENT;
00574                                                         break;
00575                                                 case BIN(0000100):
00576                                                         LDEBUG("Got Accelerometer Y Header");
00577                                                         last_cmd = ACCEL_Y_EVENT;
00578                                                         break;
00579                                                 case BIN(0000101):
00580                                                         LDEBUG("Got Internal Pressure Header");
00581                                                         last_cmd = INT_PRESS_EVENT;
00582                                                         break;
00583                                                 case BIN(0000110):
00584                                                                 LDEBUG("Got External Pressure Header");
00585                                                                 last_cmd = EXT_PRESS_EVENT;
00586                                                         break;
00587                                                 case BIN(0000111):
00588                                                         LDEBUG("Got Temperature Zone 1 Header");
00589                                                         last_cmd = TEMP1_EVENT;
00590                                                         break;
00591                                                 case BIN(0001000):
00592                                                         LDEBUG("Got Temperature Zone 2 Header");
00593                                                         last_cmd = TEMP2_EVENT;
00594                                                         break;
00595                                                 case BIN(0001001):
00596                                                         LDEBUG("Got Temperature Zone 3 Header");
00597                                                         last_cmd = TEMP3_EVENT;
00598                                                         break;
00599                                                 case BIN(0001010):
00600                                                         LDEBUG("Got Digital Input Header");
00601                                                         last_cmd = DIG_IN_EVENT;
00602                                                         break;
00603                                                 case BIN(0001011):
00604                                                         LDEBUG("Got ADC Input Header");
00605                                                         last_cmd = ADC_IN_EVENT;
00606                                                         break;
00607                                                 default:
00608                                                         last_cmd = UNRECOGNIZED_EVENT;
00609                                                         char txt[9];   txt[8] = '\0';
00610                                                         for (int i = 0; i < 8; i ++) txt[i] = (c >> (7-i)) & 1 ? '1':'0';
00611                                                         LERROR("Unknown message %s from BeeSTEM -- IGNORED", txt);
00612                                                         break;
00613                                         }
00614                         }
00615                 }
00616                 else if(c >> 7 == 1) {
00617                         if(last_cmd >=COMPASS_HEADING_EVENT && last_cmd <=MOTOR_E_CURR_EVENT) {//If the MSB is a 1, then it is data
00618 
00619                                 byte dat =( (c & BIN(01111111)) | ((last_bit?1:0) << 7));
00620 
00621                                 char txt[9]; txt[8] = '\0';
00622                                 for (int i = 0; i < 8; i ++) txt[i] = (dat >> (7-i)) & 1 ? '1':'0';
00623                                 LDEBUG("Data Byte: %s", txt);
00624 
00625                                 //Put the data in it's proper place based on the previous command and try to call
00626                                 //the event listener.
00627                                 pthread_mutex_lock(&lock);
00628                                 //I know it's a bit weird to address the data table with event types, but it really cuts down
00629                                 //on code here, and so long as no one messes with the event/data list bindings, this should work great.
00630                                 itsData[last_cmd] = dat;
00631                                 pthread_mutex_unlock(&lock);
00632                                 if (itsListener.get())
00633                                         itsListener->event(last_cmd, dat, 0);
00634                                 last_cmd = DATA_EVENT;
00635                         }
00636                         else {
00637                                 LERROR("Bad Data Ordering From BeeSTEM");
00638                                 if (itsListener.get())
00639                                         itsListener->event(BAD_IN_CMD_SEQ_EVENT, 0, 0);
00640                                 last_cmd = NO_EVENT;
00641                         }
00642                 }
00643         }
00644 
00645         pthread_exit(0);
00646 }
00647 
00648 // ######################################################################
00649 /* So things look consistent in everyone's emacs... */
00650 /* Local Variables: */
00651 /* indent-tabs-mode: nil */
00652 /* End: */
Generated on Sun May 8 08:40:37 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3