diff --git a/libraries/APM_ADC/APM_ADC.cpp b/libraries/APM_ADC/APM_ADC.cpp index 28e35de03f..069b21c94a 100644 --- a/libraries/APM_ADC/APM_ADC.cpp +++ b/libraries/APM_ADC/APM_ADC.cpp @@ -145,5 +145,32 @@ int APM_ADC_Class::Ch(unsigned char ch_num) return(result); } -// make one instance for the user to use -APM_ADC_Class APM_ADC; \ No newline at end of file +// Constructors //////////////////////////////////////////////////////////////// +APM_ADC_HIL_Class::APM_ADC_HIL_Class() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void APM_ADC_HIL_Class::Init(void) +{ +} + +// Read one channel value +int APM_ADC_HIL_Class::Ch(unsigned char ch_num) +{ + return adc_value[ch_num]; +} + +// Set one channel value +int APM_ADC_HIL_Class::setHIL(float p, float q, float r, float gyroTemp, + float aX, float aY, float aZ, float diffPress) +{ + // TODO: map floats to raw + adc_value[0] = r; + adc_value[1] = p; + adc_value[2] = q; + adc_value[4] = gyroTemp; + adc_value[5] = aX; + adc_value[6] = aY; + adc_value[7] = aZ; +} diff --git a/libraries/APM_ADC/APM_ADC.h b/libraries/APM_ADC/APM_ADC.h index a37d3a97a5..b1a9280df0 100644 --- a/libraries/APM_ADC/APM_ADC.h +++ b/libraries/APM_ADC/APM_ADC.h @@ -19,6 +19,15 @@ class APM_ADC_Class int Ch(unsigned char ch_num); }; -extern APM_ADC_Class APM_ADC; +class APM_ADC_HIL_Class +{ + private: + public: + APM_ADC_HIL_Class(); // Constructor + void Init(); + int Ch(unsigned char ch_num); + int setHIL(float p, float q, float r, float gyroTemp, + float aX, float aY, float aZ, float diffPress); +}; -#endif \ No newline at end of file +#endif diff --git a/libraries/APM_ADC/examples/APM_ADC_test/APM_ADC_test.pde b/libraries/APM_ADC/examples/APM_ADC_test/APM_ADC_test.pde index e6ad9c7036..7d88312c75 100644 --- a/libraries/APM_ADC/examples/APM_ADC_test/APM_ADC_test.pde +++ b/libraries/APM_ADC/examples/APM_ADC_test/APM_ADC_test.pde @@ -7,6 +7,8 @@ unsigned long timer; +APM_ADC_Class APM_ADC; + void setup() { APM_ADC.Init(); // APM ADC initialization @@ -30,4 +32,4 @@ void loop() } Serial.println(); } -} \ No newline at end of file +} diff --git a/libraries/APM_BMP085/APM_BMP085.cpp b/libraries/APM_BMP085/APM_BMP085.cpp index 98cd791025..0e7000ad5a 100644 --- a/libraries/APM_BMP085/APM_BMP085.cpp +++ b/libraries/APM_BMP085/APM_BMP085.cpp @@ -238,6 +238,47 @@ void APM_BMP085_Class::Calculate() Press = p + ((x1 + x2 + 3791) >> 4); } +// Constructors //////////////////////////////////////////////////////////////// +APM_BMP085_HIL_Class::APM_BMP085_HIL_Class() +{ +} -// make one instance for the user to use -APM_BMP085_Class APM_BMP085; \ No newline at end of file +// Public Methods ////////////////////////////////////////////////////////////// +void APM_BMP085_HIL_Class::Init(void) +{ + BMP085_State=1; +} + + +// Read the sensor. This is a state machine +// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5) +uint8_t APM_BMP085_HIL_Class::Read() +{ + uint8_t result=0; + + if (BMP085_State==1) + { + BMP085_State++; + } + else + { + if (BMP085_State==5) + { + BMP085_State = 1; // Start again from state=1 + result=1; // New pressure reading + } + else + { + BMP085_State++; + result=1; // New pressure reading + } + } + return(result); +} + +void APM_BMP085_HIL_Class::setHIL(float _Temp, float _Press) +{ + // TODO: map floats to raw + Temp = _Temp; + Press = _Press; +} diff --git a/libraries/APM_BMP085/APM_BMP085.h b/libraries/APM_BMP085/APM_BMP085.h index 906b74d4cb..b1d2093bb8 100644 --- a/libraries/APM_BMP085/APM_BMP085.h +++ b/libraries/APM_BMP085/APM_BMP085.h @@ -29,6 +29,21 @@ class APM_BMP085_Class uint8_t Read(); }; -extern APM_BMP085_Class APM_BMP085; +class APM_BMP085_HIL_Class +{ + private: + uint8_t BMP085_State; + public: + int32_t RawPress; + int32_t RawTemp; + int16_t Temp; + int32_t Press; + //int Altitude; + uint8_t oss; + APM_BMP085_HIL_Class(); // Constructor + void Init(); + uint8_t Read(); + void setHIL(float Temp, float Press); +}; -#endif \ No newline at end of file +#endif diff --git a/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde b/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde index d79eb56a40..29b10b3f65 100644 --- a/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde +++ b/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde @@ -6,6 +6,8 @@ #include #include // ArduPilot Mega BMP085 Library +APM_BMP085_Class APM_BMP085; + unsigned long timer; void setup() @@ -38,4 +40,4 @@ void loop() Serial.print(Altitude); Serial.println(); } -} \ No newline at end of file +} diff --git a/libraries/APM_Compass/APM_Compass.cpp b/libraries/APM_Compass/APM_Compass.cpp index 9db760a8e4..3114c1ce6e 100644 --- a/libraries/APM_Compass/APM_Compass.cpp +++ b/libraries/APM_Compass/APM_Compass.cpp @@ -231,5 +231,128 @@ void APM_Compass_Class::SetDeclination(float radians) declination = radians; } -// make one instance for the user to use -APM_Compass_Class APM_Compass; + +// Constructors //////////////////////////////////////////////////////////////// +APM_Compass_HIL_Class::APM_Compass_HIL_Class() : orientation(0), declination(0.0) +{ + // mag x y z offset initialisation + offset[0] = 0; + offset[1] = 0; + offset[2] = 0; + + // initialise orientation matrix + orientationMatrix = ROTATION_NONE; +} + +// Public Methods ////////////////////////////////////////////////////////////// +bool APM_Compass_HIL_Class::Init(void) +{ + unsigned long currentTime = millis(); // record current time + int numAttempts = 0; + int success = 0; + + // calibration initialisation + calibration[0] = 1.0; + calibration[1] = 1.0; + calibration[2] = 1.0; + + while( success == 0 && numAttempts < 5 ) + { + // record number of attempts at initialisation + numAttempts++; + + // read values from the compass + Read(); + delay(10); + + // calibrate + if( abs(Mag_X) > 500 && abs(Mag_X) < 1000 && abs(Mag_Y) > 500 && abs(Mag_Y) < 1000 && abs(Mag_Z) > 500 && abs(Mag_Z) < 1000) + { + calibration[0] = abs(715.0 / Mag_X); + calibration[1] = abs(715.0 / Mag_Y); + calibration[2] = abs(715.0 / Mag_Z); + + // mark success + success = 1; + } + } + return(success); +} + +// Read Sensor data +void APM_Compass_HIL_Class::Read() +{ + // values set by setHIL function +} + +void APM_Compass_HIL_Class::Calculate(float roll, float pitch) +{ + float Head_X; + float Head_Y; + float cos_roll; + float sin_roll; + float cos_pitch; + float sin_pitch; + Vector3f rotMagVec; + + cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM? + sin_roll = sin(roll); + cos_pitch = cos(pitch); + sin_pitch = sin(pitch); + + // rotate the magnetometer values depending upon orientation + if( orientation == 0 ) + rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); + else + rotMagVec = orientationMatrix*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); + + // Tilt compensated Magnetic field X component: + Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch; + // Tilt compensated Magnetic field Y component: + Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll; + // Magnetic Heading + Heading = atan2(-Head_Y,Head_X); + + // Declination correction (if supplied) + if( declination != 0.0 ) + { + Heading = Heading + declination; + if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg) + Heading -= (2.0 * M_PI); + else if (Heading < -M_PI) + Heading += (2.0 * M_PI); + } + + // Optimization for external DCM use. Calculate normalized components + Heading_X = cos(Heading); + Heading_Y = sin(Heading); +} + +void APM_Compass_HIL_Class::SetOrientation(const Matrix3f &rotationMatrix) +{ + orientationMatrix = rotationMatrix; + if( orientationMatrix == ROTATION_NONE ) + orientation = 0; + else + orientation = 1; +} + +void APM_Compass_HIL_Class::SetOffsets(int x, int y, int z) +{ + offset[0] = x; + offset[1] = y; + offset[2] = z; +} + +void APM_Compass_HIL_Class::SetDeclination(float radians) +{ + declination = radians; +} + +void APM_Compass_HIL_Class::setHIL(float _Mag_X, float _Mag_Y, float _Mag_Z) +{ + // TODO: map floats to raw + Mag_X = _Mag_X; + Mag_Y = _Mag_Y; + Mag_Z = _Mag_Z; +} diff --git a/libraries/APM_Compass/APM_Compass.h b/libraries/APM_Compass/APM_Compass.h index da91e388aa..a29e87ac15 100644 --- a/libraries/APM_Compass/APM_Compass.h +++ b/libraries/APM_Compass/APM_Compass.h @@ -83,6 +83,31 @@ class APM_Compass_Class void SetDeclination(float radians); }; -extern APM_Compass_Class APM_Compass; +class APM_Compass_HIL_Class +{ + private: + int orientation; + Matrix3f orientationMatrix; + float calibration[3]; + int offset[3]; + float declination; + public: + int Mag_X; + int Mag_Y; + int Mag_Z; + float Heading; + float Heading_X; + float Heading_Y; + unsigned long lastUpdate; + + APM_Compass_HIL_Class(); // Constructor + bool Init(); + void Read(); + void Calculate(float roll, float pitch); + void SetOrientation(const Matrix3f &rotationMatrix); + void SetOffsets(int x, int y, int z); + void SetDeclination(float radians); + void setHIL(float Mag_X, float Mag_Y, float Mag_Z); +}; #endif diff --git a/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde b/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde index d309de353c..50ab6bde73 100644 --- a/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde +++ b/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde @@ -16,6 +16,7 @@ #define ToDeg(x) (x*57.2957795131) // *180/pi unsigned long timer; +APM_Compass_Class APM_Compass; void setup() { diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 384d7daccc..91ef957483 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -11,3 +11,4 @@ #include "AP_GPS_IMU.h" #include "AP_GPS_None.h" #include "AP_GPS_Auto.h" +#include "AP_GPS_HIL.h" diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp new file mode 100644 index 0000000000..d2a1505ecc --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -0,0 +1,52 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// Hardware in the loop gps class. +// Code by James Goppert +// +// 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. +// +// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" +// + +#include "AP_GPS_HIL.h" +#include "WProgram.h" + +// Constructors //////////////////////////////////////////////////////////////// +AP_GPS_HIL::AP_GPS_HIL(Stream *s) : GPS(s) +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void AP_GPS_HIL::init(void) +{ +} + +void AP_GPS_HIL::update(void) +{ +} + +int AP_GPS_HIL::status(void) +{ + return 2; // gps locked + // TODO: show as locked after first packet received +} + +void AP_GPS_HIL::setHIL(long _time, long _latitude, long _longitude, long _altitude, + 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; + num_sats = _num_sats; + new_data = true; + fix = true; + valid_read = true; +} + diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h new file mode 100644 index 0000000000..db9cb5b550 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_HIL.h @@ -0,0 +1,27 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// Hardware in the loop gps class. +// Code by James Goppert +// +// 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. +// +// +#ifndef AP_GPS_HIL_h +#define AP_GPS_HIL_h + +#include + +class AP_GPS_HIL : public GPS { +public: + AP_GPS_HIL(Stream *s); + void init(void); + void update(void); + int status(void); + void setHIL(long time, long latitude, long longitude, long altitude, + long ground_speed, long ground_course, long speed_3d, uint8_t num_sats); +}; + +#endif // AP_GPS_HIL_H