/** \file * * Contains the AHRS_M2 class declaration. * * AHRS_M2.h should only be included by AHRS_M2.cpp * Other classes should include AHRS_M2IF.h * * Copyright (c) 2007,2008,2009 MBARI * MBARI Proprietary Information. All Rights Reserved */ #ifndef AHRS_M2_H #define AHRS_M2_H #include "component/SyncComponent.h" #include "io/LoadControl.h" #include "io/UartStream.h" #include "logger/Logger.h" #include "data/Matrix3x3.h" #include "data/Point3D.h" /** * Provides software interface to the Sparton AHRS-M2. * Can also use SimSlate to provide simulated AHRS values when * they are available. * * AHRS_M2.h should only be included by AHRS_M2.cpp * Other classes should include AHRS_M2IF.h * *** Exception is classCalibrateSparton, which is in the same module. * * \ingroup modules_sensor */ // Definitions for Sparton Asynchronous Packet Protocol (SAPP) //************************************************************************* // SOH starts a packet // ETX ends a packet // ACK acknowledges a packet // NAK negatively acknowledges a packet, i.e. non SYN received // with mal formed packet // SYN used for clock sync // All chars above prohibited from the body of the packet between SOH and ETX // If the char does occur the sequence DLE,(char OR 0x80) is sent instead. // This includes the DLE char itself, which gets sent as DLE, DLE|0x80 // On receive all chars up to SOH are discarded. All chars from SOH to ETX // are received. Each time DLE occurs in the data the DLE is discarded and // the next character is restored as (char AND 0x7f). // If a packet is received well formed (may or may not include a correct CRC // as an option). The ACK is sent. If a partial packet is received the NAK is // sent. Retries on packets are up to the application layer. // Fixed messages can be canned for this application. // See RFSProtocolSuite-M2.pdf for full description. class UniversalDataReader; class UniversalDataWriter; class AHRS_M2 : public SyncSensorComponent { public: AHRS_M2( const Module* module ); virtual ~AHRS_M2(); virtual void run(); /// Do what needs to be done to run /// Similar to initialize, in old init/run/uninit sequence virtual RunState start(); /// Might follow a STOP...START sequence virtual RunState starting(); /// Pause for a short period (indicated by pauseTime) virtual RunState pause(); /// Should eventually follow a PAUSE request: should set continueTime virtual RunState paused(); /// Resume from PAUSE virtual RunState resume(); /// Might follow a PAUSE...RESUME sequence virtual RunState resuming(); /// Should eventually follow a START request or RESETTING virtual RunState runnable(); /// Might occur in case of Error virtual RunState resetting() { return stop(); } /// Initial state -- can be later followed by START virtual RunState stop(); virtual RunState stopping(); /// Initial state -- can be later followed by START virtual RunState stopped(); virtual void uninitialize(); typedef enum { CAL_OFF, // Normal Operation, no calibration underway CAL_AUTO, // Auto calibration mode } CalibrateSpartonMode; /// Sets the state and commands the Sparton /// Retuns true if successful static bool SetCalMode( CalibrateSpartonMode calMode ); /// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc virtual ConfigURI getConfigURI( ConfigOption configOption ) const; private: // Note that the copy constructor below is private and not given a body. // Any attempt to call it will return a compiler error. AHRS_M2( const AHRS_M2& old ); // disallow copy constructor /// Pointer to the singleton-like instance of this component static AHRS_M2* Instance_; /// Sparton parsing constants for SAPP static const unsigned char NUL = 0x00; /* null */ static const unsigned char SOH = 0x01; /* start of header */ static const unsigned char STX = 0X02; /* start of text */ static const unsigned char ETX = 0x03; /* end of text */ static const unsigned char EOT = 0x04; /* end of transmission */ static const unsigned char ENQ = 0x05; /* enquiry */ static const unsigned char ACK = 0x06; /* acknowledge */ static const unsigned char BEL = 0x07; /* bell or alarm */ static const unsigned char BS = 0x08; /* backspace */ static const unsigned char HT = 0x09; /* horizontal tab */ static const unsigned char LF = 0x0A; /* line feed */ static const unsigned char VT = 0x0B; /* vertical tab */ static const unsigned char FF = 0x0C; /* form feed */ static const unsigned char CR = 0x0D; /* carriage return */ static const unsigned char SO = 0x0E; /* shift out */ static const unsigned char SI = 0x0F; /* shift in */ static const unsigned char DLE = 0x10; /* data link escape */ static const unsigned char DC1 = 0x11; /* device control 1 CTRL_Q */ static const unsigned char DC2 = 0x12; /* device control 2 */ static const unsigned char DC3 = 0x13; /* device control 3 CTRL_S */ static const unsigned char DC4 = 0x14; /* device control 4 */ static const unsigned char NAK = 0x15; /* negative acknowledge */ static const unsigned char SYN = 0x16; /* synchronous idle */ static const unsigned char MASK_UP = 0x80; static const unsigned char MASK_OFF = 0x7f; static const unsigned int MAX_DEVICE_RESPONSE = 255; static const unsigned int MIN_DEVICE_RESPONSE = 61; // SAPP ByteCount (1), SAPP Error/Proto (1), RFS Channel (1), Pitch (4), Roll (4), Yaw (4), MagX (4), MagY (4), MagZ (4), AccelX (4), AccelY (4), AccelZ (4), GyroX (4), GyroY (4), GyroZ (4), YawErrEst (4), Temp (4), CRC (2) #pragma pack(1) /// AHRS-M2 Bit Stream Binary data format definition typedef struct { // SAPP packet headers char byteCount_; char errorNum_; char channelRFS_; // SAPP packet payload float pitch_; float roll_; float yaw_; float magX_, magY_, magZ_; float accelX_, accelY_, accelZ_; float gyroX_, gyroY_, gyroZ_; float yawErrEst_; float temperature_; int numPointsCal_; short crc_; } AhrsM2Data, *pAhrsM2Data; #pragma pack() /// Stores the current response from the device char deviceResponse_[MAX_DEVICE_RESPONSE + 1]; /// Max number of packets queued up in the buffer static const unsigned short MAX_DEVICE_MSG_QUEUE_SIZE; /// Measurement limits static const float HEADING_LIMIT[]; static const float PITCH_LIMIT[]; static const float ROLL_LIMIT[]; /// Instance of load controller LoadControl loadControl_; /// Holds the communications device UartStream uart_; /// Slate inputs UniversalDataReader* latitudeReader_; UniversalDataReader* longitudeReader_; UniversalDataReader* depthReader_; /// Universal Slate outputs UniversalBlobWriter* rotationMatrixWriter_; UniversalDataWriter* magneticHeadingWriter_; UniversalDataWriter* pitchWriter_; UniversalDataWriter* rollWriter_; UniversalDataWriter* trueHeadingWriter_; /// Slate outputs DataWriter* compassCalStateWriter_; DataWriter* compassHeadingWriter_; DataWriter* compassHeadingErrWriter_; DataWriter* compassTemperatureWriter_; DataWriter* numPointCalWriter_; BlobWriter* accelWriter_; BlobWriter* gyroWriter_; BlobWriter* magWriter_; /// Configuration inputs ConfigReader* boresightMatrixCfgReader_; ConfigReader* magDeviationCfgReader_; ConfigReader* numPointsCalCfgReader_; ConfigReader* readAccelerationsCfgReader_; ConfigReader* readAngularVelocitiesCfgReader_; ConfigReader* readMagneticsCfgReader_; ConfigReader* verbosityCfgReader_; /// Returns true if data is requested from one of the readers bool isDataRequested(); /// Writes Sparton data to slate void writeData( void ); /// Sets slate writers to fail state while handeling calibration requests void setWritersInvalid( bool invalid ); /// Calibration mode status indicators CalibrateSpartonMode calMode_; CalibrateSpartonMode calModeRequest_; typedef enum { NUM_POINT_CAL, CLEAR_POINT_CAL, INIT_POINT_CAL, ACTIVATE_CAL, REPORT_NUM_POINT_CAL, REPORT_ERR_CAL, DEACTIVATE_CAL, } CalibrationSequance; CalibrationSequance calSequance_; /// If true, bail out of calibration mode and restart bool calibrationFailure_; /// AHRS-M2 datastream format configuration typedef enum { BORESIGHT, // Sets data Trigger to CompassData FORMAT, // Formats datastream (mode 2 = BitStreamBinary) TRIGGER, // Sets data Trigger to CompassData CLEAR, // Clears any previous data selections for channel PITCH, // Subscribes to Pitch data stream ROLL, // Subscribes to Roll data stream YAW, // Subscribes to Yaw data stream MAG, // Subscribes to Magnetometer data stream ACCEL, // Subscribes to Accelerometer data stream GYRO, // Subscribes to Gyro data stream YAWERR, // Subscribes to YawErrEst data stream TEMPERATURE,// Subscribes to Temperature data stream NUMPOINTCAL,// Subscribes to MagBufferActiveIndex data stream SETUPDONE, // All configured } DataSetupSequence; DataSetupSequence dataSetup_; bool dataStreamConfigured_; // True if data stream is configured /// True if data stream is enabled (i.e., device is streaming data) bool dataStreamActive_; float pitch_, roll_, compHeading_, magHeading_, trueHeading_; Point3D accel_, gyro_, mag_; float yawErrEst_; float temperature_; int numPointsCal_; Matrix3x3 rotationFromVehicleToNavigationFrame_; Matrix3x3 rotationFromDeviceToVehicleFrame_; /// Deviation of this compass in the vehicle. /// compass orientation +/- deviation due to vehicle = Magnetic orientation /// magnetic orientation +/- variation from map = True orientation float magDeviation_; float magVariation_; Timespan poTimeout_; Timespan timeout_; Timestamp startTime_, poTime_, dataTimestamp_; Str boresightMatrixCfg_; int readAccelerationsCfg_; int readAngularVelocitiesCfg_; int readMagneticsCfg_; int numPointsCalCfg_; /// Debugging outputs bool debug_; int verbosity_; /// Interacts with uart bool tryUartComms( const char *textToSend, const char *errorPrefix, Syslog::Severity severity = Syslog::ERROR ); /// Scans configuraiton settings bool readConfig( void ); /// Configures the Sparton data-stream to output desired variables /// Returns true once configuration is done bool configureDataStream( void ); /// Configures the AHRS-M2 to match LRAUV mounting orientation bool setBoresightMatrix( void ); /// Returns true if data-stream is enabled bool startDataStream( void ); /// Returns true if data-stream is disabled bool stopDataStream( void ); /// Gets the magnetic variation void getMagneticVariation( void ); /// Sets request for calibration state change /// Retuns true if successful bool setCalMode( CalibrateSpartonMode calMode ); /// Handles calibration requests from cal behavior (CalibrateAHRS_M2) /// Returns true if handling a request bool handleCalRequest( void ); /// Parses RFS data packet /// Returns true if successful bool receiveRFSData( void ); /// Reads in SAAP data packets from Sparton and reformats to RFS /// Returns true if successful bool readRFSPacket( unsigned char *response ); /// Processes data prior to logging void processData( void ); /// Convert a SAPP buffer into a non-SAPP buffer and return length of stripped msg unsigned int removeSAPPFrame( unsigned char *output, const char *input, unsigned int len ); /// Returns true when encounters a SAPP char unsigned char isControl( unsigned char value ); /// Calculates message CRC unsigned short getCRC( const unsigned char *data, unsigned int len ); /// Converts 2 bytes to short unsigned short extractShort( unsigned char *convertFloat, bool flipEndian = true ); /// Prints binary messages to syslog in human(e) readable foarmat void printBufferHex( const char *prefix, const char *buffer, unsigned int length, Syslog::Severity severity = Syslog::ERROR ); }; #endif /*AHRS_M2_H*/