Cleaned up AP_ADC_HIL
git-svn-id: https://arducopter.googlecode.com/svn/trunk@961 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert
parent
458dfde81d
commit
40f1d184a1
@ -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);
|
||||
}
|
||||
|
||||
@ -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 <inttypes.h>
|
||||
#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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user