/****************************************************************************/ /* Copyright (c) 2000 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : Crossbow.cc */ /* Author : */ /* Project : */ /* Version : 1.0 */ /* Created : 02/07/2000 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ /*-----------------------------------------------------------------------* CLASS: Crossbow DESCRIPTION: Device interface for Crossbow DMU-AHRS. $Id: m3dmgx1.cc,v 1.3 2010/03/11 17:07:04 hthomas Exp $ *-----------------------------------------------------------------------*/ /*-----------------------------------------------------------------------* A NOTE ON COORDINATE SYSTEMS: This driver has been programmed to be consistent with the following right-hand coordinate system: +X toward the nose of the AUV +Y to the starboard +Z down In this coord. system: Positive pitch is nose up Positive roll is toward the starboard (starboard down) Positive yaw is toward the starboard *-----------------------------------------------------------------------*/ #include "SemaphoreP.h" #include "MathP.h" #include "m3dmgx1.h" #include "BooleanAttribute.h" #include "IntegerAttribute.h" #include "AttributeParser.h" #include "VehicleConfigurationIF.h" #include "SerialDevice.h" #include "Syslog.h" #include "AHRSOutput.h" #include "SharedData.h" #define CrossbowConfigFileName "crossbow.cfg" SerialDevice::lineFormat CrossbowLineFormat = { 38400, 8, 1, "NONE" }; //========================================================================= Crossbow::Crossbow( SerialDevice *device ) : PeriodicTask(CrossbowTaskName), _output( SharedData::Write ), _command( SharedData::ReadWrite ) { Boolean debug = True; VehicleConfigurationIF::Port port; short priority = 10; _device = NULL; _log = NULL; // Default values _crossbowPeriod = 200; _crossbowDebugMode = False; _dumpHex = False; // Load value from the configuration file loadConfig( CrossbowConfigFileName ); // Clear out the output buffer _output.clear(); _output.data.status = DeviceIF::Initializing; _output.write(); _device = device; // Create the log _log = new CrossbowLog( this, DataLog::BinaryFormat ); // Initialize the device itself initialize(); // Add the periodic callback addPeriodicCallback( _crossbowPeriod, (CallbackMethod)Crossbow::crossbowCallback ); } //========================================================================= Crossbow::~Crossbow() { if( _device ) delete _device; if( _log ) delete _log; } //========================================================================== void Crossbow::loadConfig( const char *configFile ) { char fullConfigFile[256]; Attributes attributes( "CrossbowConstants" ); strcpy( fullConfigFile, System::configurationFile(configFile)); attributes.add( new IntegerAttribute("crossbowPeriod", "Period of crossbow queries (ms)", &_crossbowPeriod, 200 ) ); attributes.add( new BooleanAttribute("debug", "Extra debugging output", &_crossbowDebugMode, False ) ); attributes.add( new BooleanAttribute("dumpHex", "Provides a hex dump of Crossbow input", &_dumpHex, False ) ); try { AttributeParser::parse(fullConfigFile, &attributes); } catch(...) { Syslog::write("VehicleConstants -- Caught exception while " "loading vehicle constants"); return; } } //========================================================================= int Crossbow::initialize( void ) { // _device->commsDebugMode( SerialDevice::DebugOn ); if( _crossbowDebugMode ) Syslog::write( "Crossbow::initialize : Initializing Crossbow driver"); _device->setLineFormat( &CrossbowLineFormat ); _device->raw(); _device->blocking(); // setPolledMode(); // setAngleMode(); _device->clearPort(); if( _crossbowDebugMode ) Syslog::write( "Crossbow::initialize : \t ...done!"); // _device->blocking(); getSerialNumber(); // setTFactor( defaultTFactor ); return OK; } //========================================================================= void Crossbow::log(int millisec) { } //========================================================================= void Crossbow::crossbowDebugMode( Boolean newMode ) { _crossbowDebugMode = newMode; } //========================================================================= Boolean Crossbow::setHexDump( Boolean newVal ) { return _dumpHex = newVal; } //========================================================================= void Crossbow::status(char *str) { } //========================================================================= void Crossbow::currentData(char *str) { } //========================================================================= void Crossbow::crossbowCallback() { _device->clearPort(); // Place a data request with the Crossbow requestData(); readData(); // Write the current data out _output.write(); // Log the new data _log->write(); // Check for a command, then process _command.read(); if( _command.data.Request ) { switch( _command.data.Request ) { case CrossbowCommand::SerialNumber: getSerialNumber(); break; case CrossbowCommand::FirmwareVersion: break; case CrossbowCommand::SetZero: setZero( _command.data.ZeroSamples ); break; case CrossbowCommand::SetTFactor: setTFactor( _command.data.TFactor ); break; } _command.clear( ); _command.write( ); // Post the newly acquired data _output.write(); } } //========================================================================= int Crossbow::readData( void ) { rawAnglePacket dataIn; stabilizedVectorPacket vectorData; char xbowBuf[23], *start; int nbytes; // Assume error until proven otherwise _output.data.status = DeviceIF::Error; nbytes = ::dev_read(_device->getFd(), xbowBuf, 23, 23, 0, 20, 0, 0); if (nbytes != 23) Syslog::write("m3dmgx1::nbytes is %d %d\n", nbytes, errno); if (nbytes != 23) return ERROR; //swap to correct byte order swab(xbowBuf+1, xbowBuf+1, 22); memcpy(&dataIn, xbowBuf, sizeof(rawAnglePacket)); //read in gyro-stabilized vector data nbytes = ::dev_read(_device->getFd(), xbowBuf, 23, 23, 0, 10, 0, 0); if (nbytes != 23) { printf("nbytes in gyro stabilized read is %d %d\n", nbytes, errno); return ERROR; } swab(xbowBuf+1, xbowBuf+1, 22); memcpy(&vectorData, xbowBuf, sizeof(stabilizedVectorPacket)); // Generate real numbers from the binary in scaleRawAnglePacket( &dataIn, &vectorData); // At this point, we got good data. _output.data.status = DeviceIF::Ok; if( _crossbowDebugMode ) { if( _dumpHex ) Syslog::dumpHex( "Crossbow Data", start, anglePacketLen ); Syslog::write( "R = %7.2lf : P = %7.2lf : Y = %7.2lf", Math::radToDeg(_output.data.roll), Math::radToDeg(_output.data.pitch), Math::radToDeg(_output.data.yaw) ); } return OK; } //========================================================================= int Crossbow::setPolledMode( void ) { char cmd[3]; cmd[0] = 0x10; cmd[1] = 0x00; cmd[2] = 0x00; Syslog::write("Crossbow.cc: Setting polled mode"); _device->write(cmd, 3); DataMode = POLLED; _device->flushReceiveBuf(); return OK; } //========================================================================= int Crossbow::setContinuousMode( void ) { // Don't depend on the state variable DataMode. // Just maintain it. Syslog::write("Crossbow.cc: Setting continuous mode"); _device->flushReceiveBuf(); _device->write("C", 1); DataMode = CONTINUOUS; return OK; } //========================================================================= int Crossbow::setAngleMode( void ) { _device->write("a",1); // Needs to check for "A" response return OK; } //========================================================================= int Crossbow::requestData( void ) { char cmd[1] = {0x31}; //request gyro-stabilized euler angles _device->write(cmd,1); cmd[0] = 0x02; //request gyro-stabilized vectors _device->write(cmd,1); return OK; } //========================================================================= int Crossbow::getSerialNumber( void ) { unsigned char xbowBuf[8]; unsigned char cmd[1]; char *start; unsigned long serialNumber; int result = ERROR; CrossbowDataMode PrevMode; PrevMode = DataMode; // setPolledMode(); Syslog::write("Crossbow.cc: In getSerialNumber()"); cmd[0] = 0xF1; _device->clearPort(); _device->write((char *)cmd, 1); System::milliSleep(1000); int errn; if( (errn = _device->readNChars( (char *)xbowBuf, 5, 1500 )) == ERROR ) { Syslog::write("Crossbow.cc: _device->read failed with error" ); Syslog::write("Crossbow.cc: error is %s", strerror(errn) ); perror("Crossbow.cc: "); return ERROR; } Syslog::write("Crossbow.cc: device->read succeeded"); serialNumber = xbowBuf[1]<<8 | xbowBuf[2]; Syslog::write("m3dmgx1: Serial Number is %d\n", serialNumber); return result; } //========================================================================= int Crossbow::setTFactor( int newFactor ) { char response[3]; sprintf( response, "%c", (char)(newFactor & 0xFF ) ); _device->write("T",1); _device->write( response, 1 ); return OK; } //========================================================================= int Crossbow::setZero( int samples ) { int timeout; char convertedSamples[2]; char response[3]; samples /= 10; sprintf( convertedSamples, "%c", (char)(samples & 0xFF) ); _device->write("z",1); _device->write(convertedSamples,1); timeout = samples * 50; // Estimate 30-40 ms per 10 samples if( _device->readNChars( response, 1, timeout ) == ERROR ) { Syslog::write("Crossbow.cc: _device->read failed with error"); return ERROR; } return OK; } //========================================================================= void Crossbow::clearCommand( void ) { _command.data.Request = CrossbowCommand::None; _command.write( ); } //========================================================================= int Crossbow::checkChecksum( struct rawAnglePacket *testPacket ) { unsigned int i, sum=0; unsigned char *point = (unsigned char *)testPacket; for( i = 1; i < anglePacketLen-1; i++ ) sum += point[i]; sum &= 0xFF; if( _crossbowDebugMode ) Syslog::write("Checksum = %x", sum ); return (sum == testPacket->Checksum) ? OK : ERROR; } //========================================================================= int Crossbow::reverseByteOrder( struct rawAnglePacket *work ) { /* The Crossbow packet comes in MSB, LSB, which must be reversed on the ol' x86. Pairs to be reversed are 1-2, 3-4, ...27-28 See Crossbow.h for packet format */ swab( (char *)work+1, (char *)work+1, anglePacketLen-2 ); return OK; } //========================================================================= // angle (deg) = data * (SCALE)/2^15 // // for roll, SCALE = 180 deg // pitch, SCALE = 90 deg // yaw, SCALE = 180 deg // // The manual reports the pitch scale as 90deg, but results come in // correct with a scale of 180deg. ??? // const double scaleRoll = 0.00549316406; const double scalePitch = 0.00549316406; const double scaleYaw = 0.00549316406; // // Angular Rate (deg/sec) = data * (AR*1.5)/2^15 // // where AR = angular rate sensor limit // (+/- 100 deg/sec typical = use 100) const double scaleAngRate = (double)8500/(double)32768000; // // Acceleration (G) = data * (GR * 1.5)/2^15 // // where GR = accelerometer limit (+/-2G typical = use 2) const double scaleAccel = (double)7000/(double)32768000; // // where MR = 1.25 for DMU_AHRS // const double scaleMag = (double)2000/(double)32768000; // // temperature (degC) = 44.4 * ((data * 5/4096) - 1.375) // int Crossbow::scaleRawAnglePacket( struct rawAnglePacket *in , struct stabilizedVectorPacket *vin) { _output.data.roll = Math::degToRad( scaleRoll * in->Roll ); _output.data.pitch = Math::degToRad( scalePitch * in->Pitch ); _output.data.yaw = Math::degToRad( scaleYaw * in->Yaw ); _output.data.rollRate = scaleAngRate * in->RollRate; _output.data.pitchRate = scaleAngRate * in->PitchRate; _output.data.yawRate = scaleAngRate * in->YawRate; _output.data.accelX = scaleAccel * in->AccelX; _output.data.accelY = scaleAccel * in->AccelY; _output.data.accelZ = scaleAccel * in->AccelZ; _output.data.magX = scaleMag * vin->MagField_X; _output.data.magY = scaleMag * vin->MagField_Y; _output.data.magZ = scaleMag * vin->MagField_Z; _output.data.temperature = 0; return OK; }