void
WMFMediaDataDecoder::ProcessDecode(MediaRawData* aSample)
{
{
MonitorAutoLock mon(mMonitor);
if (mIsFlushing) {
// Skip sample, to be released by runnable.
return;
}
}
HRESULT hr = mMFTManager->Input(aSample);
if (FAILED(hr)) {
NS_WARNING("MFTManager rejected sample");
mCallback->Error();
if (!mRecordedError) {
SendTelemetry(hr);
mRecordedError = true;
}
return;
}
mLastStreamOffset = aSample->mOffset;
ProcessOutput();
}
void
WMFMediaDataDecoder::ProcessShutdown()
{
if (mMFTManager) {
mMFTManager->Shutdown();
mMFTManager = nullptr;
if (!mRecordedError && mHasSuccessfulOutput) {
SendTelemetry(S_OK);
}
}
}
void
WMFMediaDataDecoder::ProcessOutput()
{
RefPtr<MediaData> output;
HRESULT hr = S_OK;
while (SUCCEEDED(hr = mMFTManager->Output(mLastStreamOffset, output)) &&
output) {
mHasSuccessfulOutput = true;
mCallback->Output(output);
}
if (hr == MF_E_TRANSFORM_NEED_MORE_INPUT) {
if (mTaskQueue->IsEmpty()) {
mCallback->InputExhausted();
}
} else if (FAILED(hr)) {
NS_WARNING("WMFMediaDataDecoder failed to output data");
mCallback->Error();
if (!mRecordedError) {
SendTelemetry(hr);
mRecordedError = true;
}
}
}
void TIM7_IRQHandler(void) {
float pitchAcc, rollAcc;
float accMagnitude;
//float mean_detector = 0;
TIM7->SR &= ~TIM_SR_UIF;
// complementary_filter(calibrated_a, calibrated_ar, &roll, &pitch);
// message.accelRoll = 0;
// message.accelPitch = 0;
// message.complementaryPitch = 0;
// message.complementaryRoll = 0;
//
// pitch += calibrated_ar[1] * dt;
// pitchGyr += calibrated_ar[1] * dt;
// roll += calibrated_ar[0] * dt;
// rollGyr += calibrated_ar[0] * dt;
//
// message.gyroPitch = pitchGyr;
// message.gyroRoll = rollGyr;
//
// accMagnitude = sqrt(calibrated_a[0]*calibrated_a[0] + calibrated_a[1]*calibrated_a[1] + calibrated_a[2]*calibrated_a[2]);
// if (accMagnitude < REASONABLE_ACCEL_MAGNITUDE) {
// pitchAcc = atan2f(calibrated_a[0], calibrated_a[2]) * 180 / 3.14159;
// message.accelPitch = pitchAcc;
// pitch = pitch * 0.98 + pitchAcc * 0.02;
// message.complementaryPitch = pitch;
//
// rollAcc = atan2f(calibrated_a[1], calibrated_a[2]) * 180 / 3.14159;
// message.accelRoll = rollAcc;
// roll = roll * 0.98 + rollAcc * 0.02;
// message.complementaryRoll = roll;
// }
// calcAngleRate();
// calcAngle();
//
// Zroll[0] = roll;
// Zroll[1] = eulerAngleRate[0];
// Zpitch[0] = pitch;
// Zpitch[1] = eulerAngleRate[1];
//
// kalman2(A, Qroll, Rroll, Xroll, Zroll, Proll);
// kalman2(A, Qpitch, Rpitch, Xpitch, Zpitch, Ppitch);
//
// angle[0] = Xroll[0];
// angle[1] = Xpitch[0];
//
// researchIndex++;
// switch (research) {
// case IMPULSE_RESPONSE:
// if (researchIndex == 1) {
// F = 1.0;
// Motors_CalcPwm();
// Motors_Run();
// } else if (researchIndex == 50) {
// F = 0;
// Motors_CalcPwm();
// Motors_Stop();
// research = NO_RESEARCH;
// }
// break;
// case STEP_RESPONSE:
// if (researchIndex == 1) {
// F = 1.0;
// Motors_CalcPwm();
// Motors_Run();
// }
// break;
// case SINE_RESPONSE:
// F = researchAmplitude * sin(researchFrequency * researchIndex);
// Motors_CalcPwm();
// Motors_Run();
// break;
// case EXP_RESPONSE:
// F = researchAmplitude * exp(-researchFrequency * researchIndex);
// Motors_CalcPwm();
// Motors_Run();
// break;
// case PID_CONTROL:
// pid_control();
// break;
// default:
// researchIndex--;
// break;
// }
// arm_mean_f32(detector, ACCEL_DECIMATION, &mean_detector);
// message.detector = mean_detector;
message.ax = calibrated_a[0];
message.ay = calibrated_a[1];
message.az = calibrated_a[2];
message.angle = atan2f(calibrated_a[1], calibrated_a[2]);
SendTelemetry(&message);
}
