From 40f1d184a12ad3f9b7b3382ce719549273214d97 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Sun, 28 Nov 2010 00:28:01 +0000 Subject: [PATCH] Cleaned up AP_ADC_HIL git-svn-id: https://arducopter.googlecode.com/svn/trunk@961 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_ADC/AP_ADC_HIL.cpp | 48 +++++++++++++-------- libraries/AP_ADC/AP_ADC_HIL.h | 75 +++++++++++++++++++++++++++++---- 2 files changed, 97 insertions(+), 26 deletions(-) diff --git a/libraries/AP_ADC/AP_ADC_HIL.cpp b/libraries/AP_ADC/AP_ADC_HIL.cpp index b41c3277c8..46c0bfebd1 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.cpp +++ b/libraries/AP_ADC/AP_ADC_HIL.cpp @@ -1,10 +1,26 @@ #include "AP_ADC_HIL.h" +#include "WProgram.h" + +/* + AP_ADC_HIL.cpp + Author: James Goppert + + License: + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. +*/ const uint8_t AP_ADC_HIL::sensors[6] = {1,2,0,4,5,6}; -const int AP_ADC_HIL::sensorSign[9] = { 1, -1, -1, - -1, 1, 1, - -1, -1, -1}; -AP_ADC_HIL::AP_ADC_HIL() : adc_value() +const int8_t AP_ADC_HIL::sensorSign[6] = { 1, -1, -1,-1, 1, 1}; +const float AP_ADC_HIL::gyroBias[3] = {1665,1665,1665}; +const float AP_ADC_HIL::accelBias[3] = {2025,2025,2025}; +// gyroScale = 1/[GyroGain*pi/180] GyroGains (0.4,0.41,0.41) +const float AP_ADC_HIL::gyroScale[3] = {142.239, 139.746, 139.746}; +const float AP_ADC_HIL::accelScale[3] = {418,418,418}; // adcPerG + +AP_ADC_HIL::AP_ADC_HIL() : adcValue() { } @@ -15,28 +31,26 @@ void AP_ADC_HIL::Init(void) // Read one channel value int AP_ADC_HIL::Ch(unsigned char ch_num) { - return adc_value[ch_num]; + return adcValue[ch_num]; } // Set one channel value int AP_ADC_HIL::setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp, int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress) { - // TODO: map temp and press to raw - // gyros - /* 0 */ adc_value[sensors[0]] = sensorSign[0]* p/(gyroGainX*deg2rad*1000) + 1665; // note apm says 1,2,0 gyro order, but - /* 1 */ adc_value[sensors[1]] = sensorSign[1]* q/(gyroGainY*deg2rad*1000) + 1665; // this says 0,1,2 - /* 2 */ adc_value[sensors[2]] = sensorSign[2]* r/(gyroGainZ*deg2rad*1000) + 1665; + setGyro(0,p); + setGyro(1,q); + setGyro(2,r); - // gyro temp - /* 3 */ adc_value[3] = sensorSign[3]* gyroTemp; //gyroTemp XXX: fix scaling; + // temp + setGyroTemp(gyroTemp); - // accelerometers - /* 4 */ adc_value[sensors[3]] = sensorSign[4]* (aX*adcPerG)/1.0e3 + 2025; - /* 5 */ adc_value[sensors[4]] = sensorSign[5]* (aY*adcPerG)/1.0e3 + 2025; - /* 6 */ adc_value[sensors[5]] = sensorSign[6]* (aZ*adcPerG)/1.0e3 + 2025; + // accel + setAccel(0,aX); + setAccel(0,aY); + setAccel(0,aZ); // differential pressure - /* 7 */ adc_value[7] = sensorSign[7]* diffPress; //diffPress XXX: fix scaling; + setPressure(diffPress); } diff --git a/libraries/AP_ADC/AP_ADC_HIL.h b/libraries/AP_ADC/AP_ADC_HIL.h index a285f57fee..72df67eb59 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.h +++ b/libraries/AP_ADC/AP_ADC_HIL.h @@ -2,31 +2,88 @@ #define AP_ADC_HIL_H /* - AP_ADC_HIL.cpp - HIL model of ADC ADS7844 for Ardupilot Mega - Code by James Goppert. DIYDrones.com + AP_ADC_HIL.h + Author: James Goppert + + License: + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. */ #include #include "AP_ADC.h" +/// +// A hardware in the loop model of the ADS7844 analog to digital converter +// @author James Goppert DIYDrones.com class AP_ADC_HIL : public AP_ADC { public: + + /// + // Constructor AP_ADC_HIL(); // Constructor + + /// + // Initializes sensor, part of public AP_ADC interface void Init(); + + /// + // Read the sensor, part of public AP_ADC interface int Ch(unsigned char ch_num); + + /// + // Set the adc raw values given the current rotations rates, + // temps, accels, and pressures int setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp, int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress); private: - static const uint16_t adcPerG = 418; - static const float gyroGainX = 0.4; - static const float gyroGainY = 0.41; - static const float gyroGainZ = 0.41; - static const float deg2rad = 3.14159/180.0; + + /// + // The raw adc array + uint16_t adcValue[8]; + + /// + // sensor constants + // constants declared in cpp file + // @see AP_ADC_HIL.cpp static const uint8_t sensors[6]; - static const int sensorSign[9]; - long adc_value[8]; + static const float gyroBias[3]; + static const float gyroScale[3]; + static const float accelBias[3]; + static const float accelScale[3]; + static const int8_t sensorSign[6]; + + /// + // gyro set function + // @param val the value of the gyro in milli rad/s + // @param index the axis for the gyro(0-x,1-y,2-z) + inline void setGyro(uint8_t index, int16_t val) { + int16_t temp = val * gyroScale[index] / 1000 + gyroBias[index]; + adcValue[index] = (sensorSign[index] < 0) ? -temp : temp; + } + + /// + // accel set function + // @param val the value of the accel in milli g's + // @param index the axis for the accelerometer(0-x,1-y,2-z) + inline void setAccel(uint8_t index, int16_t val) { + int16_t temp = val * accelScale[index] / 1000 + accelBias[index]; + adcValue[index+3] = (sensorSign[index+3] < 0) ? -temp : temp; + } + + /// + // Sets the differential pressure adc channel + // TODO: implement + void setPressure(int16_t val) {} + + /// + // Sets the gyro temp adc channel + // TODO: implement + void setGyroTemp(int16_t val) {} }; #endif