PI depth control test
Dependencies: MS5803 mbed Servo
Diff: IMUDepthControl.cpp
- Revision:
- 0:df16f9bfc07b
- Child:
- 1:07e046bbcb84
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMUDepthControl.cpp Wed Aug 06 20:17:02 2014 +0000 @@ -0,0 +1,28 @@ +#include "IMUDepthControl.h" + +IMUDepthControl::IMUDepthControl(PinName sda, PinName scl, float Kp, float Ki) : + IMU(sda,scl), + m_Kp(Kp), + m_Ki(Ki) +{ + IMU.MS5803Init(); + m_errorsum=0; +} + +void IMUDepthControl::setPoint(float setpoint) +{ + m_set_point=setpoint; +} + +float IMUDepthControl::iterate() +{ + IMU.Barometer_MS5803(); + float error=m_set_point-IMU.MS5803_Pressure(); //get the error + + m_errorsum+=error; //integrate it + float I=m_errorsum*m_Ki; //multiply it with the integral gain + + float P=error*m_Kp; //proportional control + + return P+I; +} \ No newline at end of file