Sensors.cpp

00001 #include "Sensors.h"
00002 
00003 Sensors::Sensors(Status& status, ConfigFileWrapper& configFileWrapper, PinName gpsPinTx, PinName gpsPinRx, PinName i2cSda, PinName i2cScl, PinName lidarInterrupt) : _status(status)
00004 {
00005     _gpsValues = Gps::Value();
00006     _position = Position();
00007     _startingPosition = Position();
00008     _altitude = Altitude();
00009     
00010     //Initialise IMU
00011     _imu = new Imu(configFileWrapper);
00012     _imu->zeroGyro();
00013     
00014     //Initialise GPS
00015     _gps = new Gps(gpsPinTx, gpsPinRx);
00016     
00017     //Initialise Lidar
00018     _lidar = new LidarLitePwm(lidarInterrupt);
00019     
00020     
00021     //Initialise kalman
00022     _altitudeKalmanFilter = new Kalman(1, 1, 1, 0);
00023     //_xPosKalmanFilter = new Kalman(0.1, 500, 1, 0);
00024     //_yPosKalmanFilter = new Kalman(0.1, 500, 1, 0);*/
00025     
00026     DEBUG("Sensors initialised\r\n");
00027 }
00028 
00029 Sensors::~Sensors(){}
00030 
00031 void Sensors::update()
00032 {
00033     //Update GPS
00034     updateGpsValues();
00035     
00036     //Update Altitude
00037     updateAltitude();
00038     
00039     //Update Position
00040     //updatePosition();
00041 }
00042 
00043 Imu::Rate Sensors::getRate()
00044 {
00045     return _rate;
00046 }
00047 
00048 Imu::Angle Sensors::getAngle()
00049 {
00050     return _angle;
00051 }
00052 
00053 Gps::Value Sensors::getGpsValues()
00054 {
00055     return _position.gpsPosition;
00056 }
00057 
00058 Sensors::Altitude Sensors::getAltitude()
00059 {
00060     return _altitude;   
00061 }
00062 
00063 Sensors::Position Sensors::getPosition()
00064 {
00065     return _position;   
00066 }
00067 
00068 Imu::Velocity Sensors::getImuVelocity()
00069 {
00070     return _imu->getVelocity();
00071 }
00072 
00073 Imu::Acceleration Sensors::getImuAcceleration()
00074 {
00075     return _imu->getAcceleration();
00076 }
00077 
00078 void Sensors::updateImu()
00079 {
00080     _angle = _imu->getAngle();
00081     _rate = _imu->getRate();
00082 }
00083 
00084 void Sensors::updateGpsValues()
00085 {
00086     _position.gpsPosition = _gps->getValues();
00087 }
00088 
00089 void Sensors::updateAltitude()
00090 {
00091     _altitude.lidar = getLidarAltitude();  //cm
00092     _altitude.barometer = _imu->getAltitude(); //cm
00093     _altitude.gps = _gpsValues.altitude; //m
00094     
00095     //Get IMU velocity
00096     double time = (1.0 / ((double)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // In seconds
00097     Imu::Velocity velocity = _imu->getVelocity(time);
00098     
00099     //Compute predicted altitude
00100     double predictedAltitude = _altitude.computed + velocity.z;
00101     
00102     //Compute predicted change in altitude
00103     double predictedChange = 1;
00104     if(_altitude.computed != 0) predictedChange = predictedAltitude / _altitude.computed;
00105     
00106     //Compute estimated altitude
00107     float altitude = 0;
00108     if(_altitude.computed < 20 * 100) altitude = _altitude.lidar;
00109     else altitude = _altitude.barometer;
00110     double estimatedAltitude = _altitudeKalmanFilter->update(predictedChange, altitude);
00111     
00112     //Compute estimated velocity
00113     velocity.z = estimatedAltitude - _altitude.computed;
00114     
00115     //Reset IMU velocity to estimated velocity
00116     _imu->setCurrentVelocity(velocity);
00117     
00118     //Save new altitude
00119     _altitude.computed = estimatedAltitude;
00120     
00121     //printf("%1.6f, %1.6f, %1.6f\r\n", predictedAltitude, _altitude.lidar, _altitude.computed);
00122 }
00123 
00124 
00125 void Sensors::updatePosition()
00126 {
00127     /*
00128     //Get IMU velocity
00129     Imu::Velocity velocity = _imu->getVelocity();
00130     
00131     //Compute predicted positions
00132     double predictedXPos = _position.computedX + velocity.x;
00133     double predictedYPos = _position.computedY + velocity.y;
00134     
00135     //Compute predicted change in positions
00136     double predictedXChange = 1;
00137     double predictedYChange = 1;
00138     if(_position.computedX != 0) predictedXChange = predictedXPos / _position.computedX;
00139     if(_position.computedY != 0) predictedYChange = predictedYPos / _position.computedY;
00140     
00141     //Calc difference between current and starting GPS location
00142     //Assume pointed north for now
00143     Gps::Difference difference = _gps->getDifference(_startingPosition.gpsPosition, _position.gpsPosition);
00144     
00145     //printf("x vel %f, x diff %f, ", velocity.x, difference.x);
00146     //printf("y vel %f, y diff %f, ", velocity.y, difference.y);
00147     
00148     //Compute estimated position
00149     double estimatedXPos = _xPosKalmanFilter->update(predictedXChange, difference.x);
00150     double estimatedYPos = _yPosKalmanFilter->update(predictedYChange, difference.y);
00151     
00152     //Compute estimated velocity
00153     velocity.x = estimatedXPos - _position.computedX;
00154     velocity.y = estimatedYPos - _position.computedY;
00155     
00156     //Reset IMU velocity to estimated velocity
00157     _imu->setCurrentVelocity(velocity);
00158     
00159     //Save new position
00160     _position.computedX = estimatedXPos;
00161     _position.computedY = estimatedYPos;
00162     
00163     printf("Pos X %1.6f Pos Y %1.6f\r\n", _position.computedX, _position.computedY);*/
00164 }
00165 
00166 void Sensors::zeroGyro()
00167 {
00168     _imu->zeroGyro();
00169 }
00170 
00171 void Sensors::zeroBarometer()
00172 {
00173     _imu->zeroBarometer();
00174 }
00175 
00176 void Sensors::zeroPos()
00177 {
00178     _startingPosition.gpsPosition.latitude = getGpsValues().latitude;
00179     _startingPosition.gpsPosition.longitude = getGpsValues().longitude;
00180     
00181     //Get IMU velocity
00182     Imu::Velocity velocity = _imu->getVelocity();
00183     
00184     //Set x y to 0
00185     velocity.x = 0;
00186     velocity.y = 0;
00187  
00188    //Reset IMU velocity to estimated velocity
00189     _imu->setCurrentVelocity(velocity);   
00190 }
00191 
00192 void Sensors::enable(bool enable)
00193 {
00194     _imu->enable(enable);
00195 }
00196 
00197 int Sensors::getLidarAltitude()
00198 {
00199     Imu::Angle angles = getAngle();
00200     int rawAltitude = _lidar->read();
00201     
00202     float correctedAltitude = rawAltitude * cos(toRadian(angles.roll)) * cos(toRadian(-angles.pitch));  
00203     /*
00204     double oppAng = 0;
00205     double multiplier = 0;  
00206     int pitchAltitude = 0;
00207     int rollAltitude = 0;
00208     
00209     //Calulate pitch altitude
00210     oppAng = 90 - abs(angles.pitch);
00211     multiplier = sin(oppAng*PI/180);
00212     pitchAltitude = multiplier * rawAltitude;
00213     
00214     //Calulate roll altitude
00215     oppAng = 90 - abs(angles.roll);
00216     multiplier = sin(oppAng*PI/180);
00217     rollAltitude = multiplier * rawAltitude;
00218     
00219     return (pitchAltitude + rollAltitude)/ 2;*/
00220     return correctedAltitude;
00221 }
00222 
00223 void Sensors::zeroAccel()
00224 {
00225     _imu->zeroAccel();
00226 }
00227 
00228 double Sensors::toRadian(double deg)
00229 {
00230     return (deg * PI / 180);
00231 }