File content as of revision 2:969dfa4f2436:

#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();
    _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(0.75, 2, 1, 0);
    
    DEBUG("Sensors initialised\r\n");
}

Sensors::~Sensors(){}

void Sensors::update()
{
    //Update GPS
    updateGpsValues();
    
    //Update Altitude
    updateAltitude();
    
    //updatePosition();
}

Imu::Rate Sensors::getRate()
{
    return _rate;
}

Imu::Angle Sensors::getAngle()
{
    return _angle;
}

Gps::Value Sensors::getGpsValues()
{
    return _gpsValues;
}

Sensors::Altitude Sensors::getAltitude()
{
    return _altitude;   
}

Sensors::Position Sensors::getPosition()
{
    return _position;   
}

double Sensors::getZVelocity()
{
    return _imu->getVelocity();
}

void Sensors::updateImu()
{
    _angle = _imu->getAngle();
    _rate = _imu->getRate();
    
    return;
}

void Sensors::updateGpsValues()
{
    _gpsValues = _gps->getValues();
}

void Sensors::updateAltitude()
{
    _altitude.lidar = getLidarAltitude();  //cm
    _altitude.barometer = _imu->getAltitude(); //cm
    _altitude.gps = _gpsValues.altitude; //m
    
    //Update IMU velocity
    double time = (1.0 / ((double)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // In seconds
    double velocity = _imu->getVelocity(time);
    
    //Compute predicted altitude
    double predictedAltitude = _altitude.computed + velocity;
    
    //Compute predicted change in altitude
    double predictedChange = 1;
    if(_altitude.computed != 0) predictedChange = predictedAltitude / _altitude.computed;
    
    //Compute estimated altitude
    double estimatedAltitude = _altitudeKalmanFilter->update(predictedChange, _altitude.lidar);
    
    //Compute estimated velocity
    double estimatedVelocity = estimatedAltitude - _altitude.computed;
    
    //Reset IMU velocity to estimated velocity
    _imu->setCurrentVelocity(estimatedVelocity);
    
    //Save new altitude
    _altitude.computed = estimatedAltitude;
    
    //printf("%1.6f, %1.6f, %1.6f\r\n", predictedAltitude, _altitude.lidar, _altitude.computed);
}


void Sensors::updatePosition()
{
    
}

void Sensors::zeroGyro()
{
    _imu->zeroGyro();
}

void Sensors::zeroBarometer()
{
    _imu->zeroBarometer();
}

void Sensors::enable(bool enable)
{
    _imu->enable(enable);
}

int Sensors::getLidarAltitude()
{
    Imu::Angle angles = getAngle();
    int rawAltitude = _lidar->read();
    
    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;
}

void Sensors::zeroAccel()
{
    _imu->zeroAccel();
}