diff --git a/libraries/APM_ADC/APM_ADC.cpp b/libraries/APM_ADC/APM_ADC.cpp index 069b21c94a..d9bf92720c 100644 --- a/libraries/APM_ADC/APM_ADC.cpp +++ b/libraries/APM_ADC/APM_ADC.cpp @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index d2a1505ecc..e21ee4ea21 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -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; diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h index db9cb5b550..ad31bb29b4 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.h +++ b/libraries/AP_GPS/AP_GPS_HIL.h @@ -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); };