/**
* get throttle offset by acceleration
*
* @param
* void
*
* @return
* void
*/
float getThrottleOffsetByAcceleration(void) {
float output = 0.f;
setPidSp(&verticalAccelPidSettings, 0.f);
output = LIMIT_MIN_MAX_VALUE(pidCalculation(&verticalAccelPidSettings,
getVerticalAcceleration(),true,true,true), -maxThrottleOffset,
maxThrottleOffset);
//_DEBUG(DEBUG_NORMAL,"%s output =%f\n",__func__,output);
return output;
}
void Localization::ProcessMeasurementIMU(unsigned int time, const float* gyroscope, const float* accelerometer) {
float gyroCorrected[3];
float accelCorrected[3];
float deltaTime = (time - timeNow) / 1000000.0f;
deltaTime = std::min(deltaTime, 4.0f * this->deltaTime);
for (int i = 0; i < 3; ++i) {
gyroCorrected[i] = gyroscope[i];
accelCorrected[i] = accelerometer[i] * 9.81f;
}
se_compensate_imu(deltaTime, ahrsType, ahrsParameters, &imuState, gyroCorrected, accelCorrected, magLastMeas, hasMagMeas);
hasMagMeas = false;
se_kalman_predict(deltaTime, z, zCovar);
timeNow = time;
se_kalman_correct(z, zCovar, SE_STATE_A_Z, getVerticalAcceleration(imuState.q, accelCorrected), SE_ACC_VARIANCE);
}
