aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Working on HIL for sensors.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@873 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2010-11-19 21:20:49 +00:00
parent 391e4268c4
commit 0eef05b734
3 changed files with 28 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/APM_ADC/APM_ADC.cpp
View File

@ -165,12 +165,18 @@ int APM_ADC_HIL_Class::Ch(unsigned char ch_num)
int APM_ADC_HIL_Class::setHIL(float p, float q, float r, float gyroTemp,
float aX, float aY, float aZ, float diffPress)
{
static const float 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;
// TODO: map floats to raw
adc_value[0] = r;
adc_value[1] = p;
adc_value[2] = q;
adc_value[0] = r/(gyroGainX*deg2rad) + 1665;
adc_value[1] = p/(gyroGainY*deg2rad) + 1665;
adc_value[2] = q/(gyroGainZ*deg2rad) + 1665;
adc_value[4] = gyroTemp;
adc_value[5] = aX;
adc_value[6] = aY;
adc_value[7] = aZ;
adc_value[5] = -(aX*adcPerG)/1e3 + 2025;
adc_value[6] = -(aY*adcPerG)/1e3 + 2025;
adc_value[7] = -(aZ*adcPerG)/1e3 + 2025;
adc_value[8] = diffPress;
}

12
libraries/AP_GPS/AP_GPS_HIL.cpp
View File

@ -38,12 +38,12 @@ void AP_GPS_HIL::setHIL(long _time, long _latitude, long _longitude, long _altit
long _ground_speed, long _ground_course, long _speed_3d, uint8_t _num_sats)
{
time = _time;
latitude = _latitude;
longitude = _longitude;
altitude = _altitude;
ground_speed = _ground_speed;
ground_course = _ground_course;
speed_3d = _speed_3d;
latitude = _latitude*1e7;
longitude = _longitude*1e7;
altitude = _altitude*1e2;
ground_speed = _ground_speed*1e2;
ground_course = _ground_course*1e2;
speed_3d = _speed_3d*1e2;
num_sats = _num_sats;
new_data = true;
fix = true;

10
libraries/AP_GPS/AP_GPS_HIL.h
View File

@ -20,6 +20,16 @@ public:
void init(void);
void update(void);
int status(void);
/**
* Hardware in the loop set function
* @param latitude - latitude in deggrees
* @param longitude - longitude in degrees
* @param altitude - altitude in degrees
* @param ground_speed - ground speed in meters/second
* @param ground_course - ground course in degrees
* @param speed_3d - ground speed in meters/second
* @param altitude - altitude in meters
*/
void setHIL(long time, long latitude, long longitude, long altitude,
long ground_speed, long ground_course, long speed_3d, uint8_t num_sats);
};