ArAnalogGyro.h

/*
ActivMedia Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics, LLC


     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 
ActivMedia Robotics for information about a commercial version of ARIA at 
robots@activmedia.com or 
ActivMedia Robotics, 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:
  ArAnalogGyro(ArRobot *robot);
  virtual ~ArAnalogGyro();
  bool isActive(void) { return myIsActive; }
  void activate(void);
  void deactivate(void);
  bool haveGottenData(void) { return myHaveGottenData; }
  double getHeading(void) const { return myHeading; }
  int getTemperature(void) const { return myTemperature; }

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

  int getPacCount(void) { return myPacCount; }

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

  bool handleGyroPacket(ArRobotPacket *packet);
  double encoderCorrect(ArPoseWithTime deltaPose);
  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;
};

#endif // ARANALOGGYRO_H

---