New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
Sensors/Sensors.cpp
- Committer:
- joe4465
- Date:
- 2015-04-24
- Revision:
- 3:4823d6750629
- Parent:
- 2:969dfa4f2436
- Child:
- 4:9ffbf9101992
File content as of revision 3:4823d6750629:
#include "Sensors.h" Sensors::Sensors(Status& status, ConfigFileWrapper& configFileWrapper, PinName gpsPinTx, PinName gpsPinRx, PinName i2cSda, PinName i2cScl, PinName lidarInterrupt) : _status(status) { _gpsValues = Gps::Value(); _position = Position(); _startingPosition = Position(); _altitude = Altitude(); //Initialise IMU _imu = new Imu(configFileWrapper); _imu->zeroGyro(); //Initialise GPS _gps = new Gps(gpsPinTx, gpsPinRx); //Initialise Lidar _lidar = new LidarLitePwm(lidarInterrupt); //Initialise kalman _altitudeKalmanFilter = new Kalman(1, 1, 1, 0); //_xPosKalmanFilter = new Kalman(0.1, 500, 1, 0); //_yPosKalmanFilter = new Kalman(0.1, 500, 1, 0);*/ DEBUG("Sensors initialised\r\n"); } Sensors::~Sensors(){} void Sensors::update() { //Update GPS updateGpsValues(); //Update Altitude updateAltitude(); //Update Position //updatePosition(); } Imu::Rate Sensors::getRate() { return _rate; } Imu::Angle Sensors::getAngle() { return _angle; } Gps::Value Sensors::getGpsValues() { return _position.gpsPosition; } Sensors::Altitude Sensors::getAltitude() { return _altitude; } Sensors::Position Sensors::getPosition() { return _position; } Imu::Velocity Sensors::getImuVelocity() { return _imu->getVelocity(); } void Sensors::updateImu() { _angle = _imu->getAngle(); _rate = _imu->getRate(); } void Sensors::updateGpsValues() { _position.gpsPosition = _gps->getValues(); } void Sensors::updateAltitude() { _altitude.lidar = getLidarAltitude(); //cm _altitude.barometer = _imu->getAltitude(); //cm _altitude.gps = _gpsValues.altitude; //m //Get IMU velocity double time = (1.0 / ((double)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // In seconds Imu::Velocity velocity = _imu->getVelocity(time); //Compute predicted altitude double predictedAltitude = _altitude.computed + velocity.z; //Compute predicted change in altitude double predictedChange = 1; if(_altitude.computed != 0) predictedChange = predictedAltitude / _altitude.computed; //Compute estimated altitude float altitude = 0; if(altitude.computed < 20 * 100) altitude = _altitude.lidar; else altitude = _altitude.barometer; double estimatedAltitude = _altitudeKalmanFilter->update(predictedChange, altitude); //Compute estimated velocity velocity.z = estimatedAltitude - _altitude.computed; //Reset IMU velocity to estimated velocity _imu->setCurrentVelocity(velocity); //Save new altitude _altitude.computed = estimatedAltitude; //printf("%1.6f, %1.6f, %1.6f\r\n", predictedAltitude, _altitude.lidar, _altitude.computed); } void Sensors::updatePosition() { /* //Get IMU velocity Imu::Velocity velocity = _imu->getVelocity(); //Compute predicted positions double predictedXPos = _position.computedX + velocity.x; double predictedYPos = _position.computedY + velocity.y; //Compute predicted change in positions double predictedXChange = 1; double predictedYChange = 1; if(_position.computedX != 0) predictedXChange = predictedXPos / _position.computedX; if(_position.computedY != 0) predictedYChange = predictedYPos / _position.computedY; //Calc difference between current and starting GPS location //Assume pointed north for now Gps::Difference difference = _gps->getDifference(_startingPosition.gpsPosition, _position.gpsPosition); //printf("x vel %f, x diff %f, ", velocity.x, difference.x); //printf("y vel %f, y diff %f, ", velocity.y, difference.y); //Compute estimated position double estimatedXPos = _xPosKalmanFilter->update(predictedXChange, difference.x); double estimatedYPos = _yPosKalmanFilter->update(predictedYChange, difference.y); //Compute estimated velocity velocity.x = estimatedXPos - _position.computedX; velocity.y = estimatedYPos - _position.computedY; //Reset IMU velocity to estimated velocity _imu->setCurrentVelocity(velocity); //Save new position _position.computedX = estimatedXPos; _position.computedY = estimatedYPos; printf("Pos X %1.6f Pos Y %1.6f\r\n", _position.computedX, _position.computedY);*/ } void Sensors::zeroGyro() { _imu->zeroGyro(); } void Sensors::zeroBarometer() { _imu->zeroBarometer(); } void Sensors::zeroPos() { _startingPosition.gpsPosition.latitude = getGpsValues().latitude; _startingPosition.gpsPosition.longitude = getGpsValues().longitude; //Get IMU velocity Imu::Velocity velocity = _imu->getVelocity(); //Set x y to 0 velocity.x = 0; velocity.y = 0; //Reset IMU velocity to estimated velocity _imu->setCurrentVelocity(velocity); } void Sensors::enable(bool enable) { _imu->enable(enable); } int Sensors::getLidarAltitude() { Imu::Angle angles = getAngle(); int rawAltitude = _lidar->read(); float correctedAltitude = rawAltitude * cos(toRadian(angles.roll)) * cos(toRadian(-angles.pitch)); /* double oppAng = 0; double multiplier = 0; int pitchAltitude = 0; int rollAltitude = 0; //Calulate pitch altitude oppAng = 90 - abs(angles.pitch); multiplier = sin(oppAng*PI/180); pitchAltitude = multiplier * rawAltitude; //Calulate roll altitude oppAng = 90 - abs(angles.roll); multiplier = sin(oppAng*PI/180); rollAltitude = multiplier * rawAltitude; return (pitchAltitude + rollAltitude)/ 2;*/ return correctedAltitude; } void Sensors::zeroAccel() { _imu->zeroAccel(); } double Sensors::toRadian(double deg) { return (deg * PI / 180); }