I'm writing some Quad Copter software and beginning to implement an altitude hold mode.
To enable me to do this I need to get an accurate reading for vertical velocity. I plan to use a Kalman filter for this but first I need to ensure that I'm getting the correct velocity from each individual sensor.
I have done this but I'm not 100% sure its correct so I was hoping to get some confirmation on here.
My first sensor is a Lidar distance sensor, I calculated acceleration and velocity using the following code:
float LidarLitePwm::getDisplacement()
{
int currentAltitude = read();
float displacement = currentAltitude - _oldAltitude;
_oldAltitude = currentAltitude;
return displacement; //cm
}
//Time since last update
float time = (1.0 / ((float)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // 50Hz, 0.02s
float lidarDisplacement = _lidar->getDisplacement();
_currentLidarVelocity = lidarDisplacement / time;
The second sensor is an accelerometer. I calculated acceleration and velocity using the following code:
Imu::Acceleration Imu::getAcceleration()
{
//Get quaternion
float q[4];
_freeImu.getQ(q);
//Get raw data
float values[9];
_freeImu.getValues(values);
//Extract accelerometer data
float acc[3];
acc[0]= values[0]; //x
acc[1]= values[1]; //y
acc[2]= values[2]; //z
//Gravity compensate
_freeImu.gravityCompensateAcc(acc, q);
//Convert acceleration from G to cm/s/s
_acceleration.x = acc[0] * 9.8 * 100;
_acceleration.y = acc[1] * 9.8 * 100;
_acceleration.z = acc[1] * 9.8 * 100;
return _acceleration; //cm/s/s
}
//Time since last update
float time = (1.0 / ((float)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // 50Hz, 0.02s
//Get accel
Imu::Acceleration imuAcceleration = _imu->getAcceleration();
//Get velocity
currentZVelocity += imuAcceleration.z * time; //cm/s
It would be great if someone could confirm if this is correct (or not)
Thanks Joe