File content as of revision 1:07e046bbcb84:

#include "IMUDepthControl.h"

IMUDepthControl::IMUDepthControl(PinName sda, PinName scl, float Kp, float Ki) :
    IMU(sda,scl),
    m_Kp(Kp),
    m_Ki(Ki),
    output(p25)
{
    IMU.MS5803Init();
    m_errorsum=0;
    m_set_point=0;
    m_delta_t=0.01;
    m_last_pos=0.0;
    t.start();
    feedback.attach(this, &IMUDepthControl::iterate, m_delta_t);
}

void IMUDepthControl::setPoint(float setpoint)
{
    m_set_point=setpoint;
}

void IMUDepthControl::iterate()
{
    IMU.Barometer_MS5803();
    float error=m_set_point-IMU.MS5803_Pressure(); //get the error
    
    m_errorsum+=(error*m_Ki); //integrate it
    float I=m_errorsum; //multiply it with the integral gain
    
    float P=error*m_Kp; //proportional control
    
    //account for the change in position because servos don't take velocities
    float newpos=(P+I)*m_delta_t+m_last_pos;
    if (newpos>1.0)
        newpos=1.0;
    else if (newpos<0.0)
        newpos=0.0;
    output.write(newpos);
    m_last_pos=newpos;
}