ArAnalogGyro.h

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/

#ifndef ARANALOGGYRO_H
#define ARANALOGGYRO_H

#include "ariaTypedefs.h"
#include "ariaUtil.h"
#include "ArFunctor.h"

class ArRobot;
class ArRobotPacket;


class ArAnalogGyro
{
public:
  AREXPORT ArAnalogGyro(ArRobot *robot);
  AREXPORT virtual ~ArAnalogGyro();
  AREXPORT bool isActive(void) { return myIsActive; }
  AREXPORT void activate(void);
  AREXPORT void deactivate(void);
  bool hasGyroOnlyMode(void) { return myHasGyroOnlyMode; }
  bool isGyroOnlyActive(void) { return myIsGyroOnlyActive; }
  AREXPORT void activateGyroOnly(void);
  bool hasNoInternalData(void) { return myHasNoData; }
  AREXPORT bool haveGottenData(void) { return myHaveGottenData; }
  AREXPORT double getHeading(void) const { return myHeading; }
  AREXPORT int getTemperature(void) const { return myTemperature; }

  AREXPORT void setFilterModel(double gyroSigma, double inertialVar, 
                       double rotVar, double transVar)
    { myGyroSigma = gyroSigma; myInertialVarianceModel = inertialVar;
      myRotVarianceModel = rotVar; myTransVarianceModel = transVar; };

  AREXPORT int getPacCount(void) { return myPacCount; }

  AREXPORT double getAverage(void) const { return myLastAverage; }
  AREXPORT ArTime getAverageTaken(void) const { return myLastAverageTaken; }
  AREXPORT double getScalingFactor(void) const { return myScalingFactor; }
  AREXPORT void setScalingFactor(double factor) { myScalingFactor = factor; }

  AREXPORT bool handleGyroPacket(ArRobotPacket *packet);
  AREXPORT double encoderCorrect(ArPoseWithTime deltaPose);
  AREXPORT void stabilizingCallback(void);
  void setLogAnomalies(bool logAnomalies) { myLogAnomalies = logAnomalies; }
protected:
  // whether we're correcting readings or not
  bool myIsActive;
  // whether we're really getting readings or not
  bool myHaveGottenData;
  // our double for our scaling factor
  double myScalingFactor;
  // if we've gotten a reading this cycle
  bool myReadingThisCycle;
  // whether we're logging anomalies or not
  bool myLogAnomalies;
  // counting data
  int myPacCount;
  int myPacCurrentCount;
  time_t myTimeLastPacket;

  // data for averaging
  ArTime myAverageStarted;
  double myLastAverage;
  ArTime myLastAverageTaken;
  double myAverageTotal;
  int myAverageCount;


  // constants for kalman filtering
  double myGyroSigma;
  double myInertialVarianceModel;
  double myRotVarianceModel; // deg2/deg
  double myTransVarianceModel; // deg2/meter

  // for if our gyro packets aren't aligned with our sips how much
  // we've changed since we last got to correct one
  double myAccumulatedDelta;

  double myHeading;
  int myTemperature;
  double myLastHeading;
  ArRobot *myRobot;
  ArRetFunctor1C<bool, ArAnalogGyro, ArRobotPacket *> myHandleGyroPacketCB;
  ArRetFunctor1C<double, ArAnalogGyro, ArPoseWithTime> myEncoderCorrectCB;
  ArFunctorC<ArAnalogGyro> myStabilizingCB;

  enum GyroType
  {
    GYRO_NONE, 
    GYRO_ANALOG_COMPUTER, 
    GYRO_ANALOG_CONTROLLER 
  };
  GyroType myGyroType;
  bool myHasNoData;
  bool myHasGyroOnlyMode;
  bool myIsGyroOnlyActive;
};

#endif // ARANALOGGYRO_H

---