• Main Page
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

/home/jgoppert/Projects/ap/libraries/AP_ADC/AP_ADC_HIL.h

Go to the documentation of this file.
00001 #ifndef AP_ADC_HIL_H
00002 #define AP_ADC_HIL_H
00003 
00004 /*
00005         AP_ADC_HIL.h
00006         Author: James Goppert
00007 
00008         License:
00009         This library is free software; you can redistribute it and/or
00010         modify it under the terms of the GNU Lesser General Public
00011         License as published by the Free Software Foundation; either
00012         version 2.1 of the License, or (at your option) any later version.
00013 */
00014 
00015 #include <inttypes.h>
00016 #include "AP_ADC.h"
00017 
00019 // A hardware in the loop model of the ADS7844 analog to digital converter
00020 // @author James Goppert DIYDrones.com
00021 class AP_ADC_HIL : public AP_ADC
00022 {
00023   public:
00024 
00026         // Constructor
00027         AP_ADC_HIL();  // Constructor
00028 
00030         // Initializes sensor, part of public AP_ADC interface
00031         void Init();
00032 
00034         // Read the sensor, part of public AP_ADC interface
00035         int Ch(unsigned char ch_num);     
00036 
00038         // Set the adc raw values given the current rotations rates,
00039         // temps, accels, and pressures
00040         int setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp,
00041         int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress);
00042 
00043   private:
00044 
00046         // The raw adc array
00047         uint16_t adcValue[8];
00048 
00050         // sensor constants
00051         // constants declared in cpp file
00052         // @see AP_ADC_HIL.cpp
00053     static const uint8_t sensors[6];            
00054     static const float gyroBias[3];             
00055     static const float gyroScale[3];
00056         static const float accelBias[3];                
00057     static const float accelScale[3];
00058     static const int8_t sensorSign[6];
00059 
00061         // gyro set function
00062         // @param val the value of the gyro in milli rad/s
00063         // @param index the axis for the gyro(0-x,1-y,2-z)
00064         inline void setGyro(uint8_t index, int16_t val) {
00065                 int16_t temp = val * gyroScale[index] / 1000 + gyroBias[index];
00066                 adcValue[sensors[index]] = (sensorSign[index] < 0) ? -temp : temp;
00067         }
00068 
00070         // accel set function
00071         // @param val the value of the accel in milli g's
00072         // @param index the axis for the accelerometer(0-x,1-y,2-z)
00073         inline void setAccel(uint8_t index, int16_t val) {
00074                 int16_t temp = val * accelScale[index] / 1000 + accelBias[index];
00075                 adcValue[sensors[index+3]] = (sensors[index+3] < 0) ? -temp : temp;
00076         }
00077 
00079         // Sets the differential pressure adc channel
00080         // TODO: implement
00081         void setPressure(int16_t val) {} 
00082 
00084         // Sets the gyro temp adc channel
00085         // TODO: implement
00086         void setGyroTemp(int16_t val) {}
00087 };
00088 
00089 #endif

Generated for ArduPilot Libraries by doxygen