Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Debugging HIL classes.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@917 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2010-11-25 01:34:33 +00:00
parent 9331f8616c
commit 13d586dd01
1 changed files with 21 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/APM_ADC/APM_ADC.cpp
View File

@ -170,13 +170,25 @@ int APM_ADC_HIL_Class::setHIL(float p, float q, float r, float gyroTemp,
static const float gyroGainY = 0.41; static const float gyroGainY = 0.41;
static const float gyroGainZ = 0.41; static const float gyroGainZ = 0.41;
static const float deg2rad = 3.14159/180.0; static const float deg2rad = 3.14159/180.0;
// TODO: map floats to raw static const uint8_t sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware
adc_value[0] = r/(gyroGainX*deg2rad) + 1665; static const int SENSOR_SIGN[] = { 1, -1, -1,
adc_value[1] = p/(gyroGainY*deg2rad) + 1665; -1, 1, 1,
adc_value[2] = q/(gyroGainZ*deg2rad) + 1665; -1, -1, -1};
adc_value[4] = gyroTemp; // TODO: map temp and press to raw
adc_value[5] = -(aX*adcPerG)/1e3 + 2025;
adc_value[6] = -(aY*adcPerG)/1e3 + 2025; // gyros
adc_value[7] = -(aZ*adcPerG)/1e3 + 2025; /* 0 */ adc_value[sensors[0]] = SENSOR_SIGN[0]* p/(gyroGainX*deg2rad) + 1665; // note apm says 1,2,0 gyro order, but
adc_value[8] = diffPress; /* 1 */ adc_value[sensors[1]] = SENSOR_SIGN[1]* q/(gyroGainY*deg2rad) + 1665; // this says 0,1,2
/* 2 */ adc_value[sensors[2]] = SENSOR_SIGN[2]* r/(gyroGainZ*deg2rad) + 1665;
// gyro temp
/* 3 */ adc_value[3] = SENSOR_SIGN[3]* 0; //gyroTemp;
// accelerometers
/* 4 */ adc_value[sensors[3]] = SENSOR_SIGN[4]* (aX*adcPerG)/1.0e3 + 2025;
/* 5 */ adc_value[sensors[4]] = SENSOR_SIGN[5]* (aY*adcPerG)/1.0e3 + 2025;
/* 6 */ adc_value[sensors[5]] = SENSOR_SIGN[6]* (aZ*adcPerG)/1.0e3 + 2025;
// differential pressure
/* 7 */ adc_value[7] = SENSOR_SIGN[7]* 0; //diffPress;
} }