New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
Sensors/Imu/Imu.cpp
- Committer:
- joe4465
- Date:
- 2015-04-01
- Revision:
- 2:969dfa4f2436
- Parent:
- 0:c6a85bb2a827
- Child:
- 3:4823d6750629
File content as of revision 2:969dfa4f2436:
#include "Imu.h" Imu::Imu(ConfigFileWrapper& configFileWrapper) :_configFileWrapper(configFileWrapper) { _rate = Rate(); _angle = Angle(); _accelZeroPitch = _configFileWrapper.getAccelZeroPitch(); _accelZeroRoll = _configFileWrapper.getAccelZeroRoll(); Thread::wait(500); _freeImu.init(true); Thread::wait(500); _barometerZeroFilter = new filter(100); _barometerFilter = new filter(5); _barometerZero = 0; zeroBarometer(); _velocity = 0; _kalmanFilter = new Kalman(0.1, 32, 1, 0); DEBUG("IMU initialised\r\n"); } Imu::~Imu(){} Imu::Rate Imu::getRate() { float rate[3]; _freeImu.getRate(rate); float yaw = rate[2]; float pitch = rate[0]; float roll = rate[1]; rate[0] = yaw; rate[1] = pitch; rate[2] = roll; _rate.yaw = rate[0]; _rate.pitch = rate[1]; _rate.roll = rate[2]; return _rate; } Imu::Angle Imu::getAngle(bool bias) { //Filter Z accel data float values[9]; _freeImu.getValues(values); _kalmanFilter->update(1, values[2]); //printf("%1.6f, %1.6f, %1.6f\n", values[0], values[1], values[2]); //Get angle float angle[3]; _freeImu.getYawPitchRoll(angle); float yaw = -angle[0]; float pitch = angle[2]; float roll = -angle[1]; angle[0] = yaw; angle[1] = pitch; angle[2] = roll; if(bias == true) { _angle.yaw = angle[0]; _angle.pitch = angle[1];//; - _accelZeroPitch; _angle.roll = angle[2];//; - _accelZeroRoll; } else { _angle.yaw = angle[0]; _angle.pitch = angle[1]; _angle.roll = angle[2]; } return _angle; } double Imu::getAltitude() { float altitude = _barometerFilter->process(_freeImu.getBaroAlt()); float normalAltitude = (altitude - _barometerZero); return (normalAltitude * 100); } float Imu::getVelocity(float time) { //Get quat float q[4]; _freeImu.getQ(q); //Extract accelerometer data float acc[3]; acc[0]= 0; //x acc[1]= 0; //y acc[2]= _kalmanFilter->getEstimated(); //Gravity compensate _freeImu.gravityCompensateAcc(acc, q); float zAcceleration = 0; if(acc[2] < -0.01 || acc[2] > 0.01) zAcceleration = acc[2] * 9.8 * 100; _velocity += zAcceleration * time; return _velocity; //cm/s/s } float Imu::getVelocity() { return _velocity; //cm/s/s } void Imu::zeroGyro() { _freeImu.zeroGyro(); } void Imu::setCurrentVelocity(float velocity) { _velocity = velocity; } void Imu::zeroBarometer() { for(int i = 0; i < 100; i++) { _barometerZero = _barometerFilter->process(_freeImu.getBaroAlt()); } DEBUG("Barometer zero %f\r\n", _barometerZero); } void Imu::enable(bool enable) { _freeImu.sample(enable); } void Imu::zeroAccel() { Imu::Angle angle = getAngle(false); _accelZeroPitch = angle.pitch; _accelZeroRoll = angle.roll; //DEBUG("Zero accel, pitch %f, roll %f\r\n", _accelZeroPitch, _accelZeroRoll); //_configFileWrapper.setAccelZeroPitch(_accelZeroPitch); //_configFileWrapper.setAccelZeroRoll(_accelZeroRoll); //_configFileWrapper.saveSettings(); }