/********************************************************************************************************** *函 数 名: AttitudeEstimate *功能说明: 姿态估计 *形 参: 角速度测量值 加速度测量值 磁场强度测量值 *返 回 值: 无 **********************************************************************************************************/ void AttitudeEstimate(Vector3f_t gyro, Vector3f_t acc, Vector3f_t mag) { Vector3f_t accCompensate; static uint64_t previousT; float deltaT = (GetSysTimeUs() - previousT) * 1e-6; deltaT = ConstrainFloat(deltaT, 0.0005, 0.002); previousT = GetSysTimeUs(); //姿态初始对准 if(!AttitudeInitAlignment(&kalmanRollPitch, &kalmanYaw, acc, mag)) return; //运动加速度补偿 accCompensate = AccSportCompensate(acc, GetSportAccEf(), ahrs.angle, ahrs.accBfOffset); //向心加速度误差补偿 accCompensate = Vector3f_Sub(accCompensate, ahrs.centripetalAccBf); //加速度零偏补偿 accCompensate = Vector3f_Sub(accCompensate, ahrs.accBfOffset); //姿态更新 AttitudeEstimateUpdate(&ahrs.angle, gyro, accCompensate, mag, deltaT); //计算导航系下的运动加速度 TransAccToEarthFrame(ahrs.angle, acc, &ahrs.accEf, &ahrs.accEfLpf, &ahrs.accBfOffset); //转换角速度至导航系 GyroEfUpdate(gyro, ahrs.angle, &ahrs.gyroEf); //计算导航系下的向心加速度 CentripetalAccUpdate(&ahrs.centripetalAcc, GetCopterVelocity(), ahrs.gyroEf.z); //转换向心加速度至机体系,用于姿态更新补偿 EarthFrameToBodyFrame(ahrs.angle, ahrs.centripetalAcc, &ahrs.centripetalAccBf); }
status_t TimedTextMatroskaSource::read( int64_t *startTimeUs, int64_t *endTimeUs, Parcel *parcel, const MediaSource::ReadOptions *options) { MediaBuffer *textBuffer = NULL; status_t err = mSource->read(&textBuffer, options); if (err != OK) { return err; } if (textBuffer == NULL) { /* let TextPlayer do read after post some time. */ return WOULD_BLOCK; } if (textBuffer->range_length() ==0) { /* let TextPlayer do read after post some time. */ textBuffer->release(); return WOULD_BLOCK; } int64_t curSysTimeUs = GetSysTimeUs(); if ((mPreGetFrmTimeUs >0) && abs(curSysTimeUs - mPreGetFrmTimeUs) <SEND_MKV_TIMED_TEXT_MIN_DELTA_US) { /* skip this frame */ textBuffer->release(); return WOULD_BLOCK; } if (mPreGetFrmTimeUs == -1) { mPreGetFrmTimeUs = curSysTimeUs; } int32_t durMs = 0; *startTimeUs = 0; *endTimeUs = 0; textBuffer->meta_data()->findInt64(kKeyTime, startTimeUs); textBuffer->meta_data()->findInt32(kKeySubtitleDuration, &durMs); *endTimeUs = *startTimeUs + durMs*1000; CHECK_GE(*startTimeUs, 0); if ((mTimedMkvSource.srcMimeType == MKV_TIMED_SRC_MIME_VOBSUB) && mObserver) { mObserver->notifyObserver(MKV_TIMED_MSG_VOBSUB_GET, textBuffer); } else { extractAndAppendLocalDescriptions(*startTimeUs, textBuffer, parcel); } ALOGV("read one mkv text frame, size: %d, timeUs: %lld, durMs: %d", textBuffer->range_length(), *startTimeUs, durMs); mPreGetFrmTimeUs = curSysTimeUs; textBuffer->release(); // endTimeUs is a dummy parameter for Matroska timed text format. // Set a negative value to it to mark it is unavailable. return OK; }
/********************************************************************************************************** *函 数 名: MagCalibration *功能说明: 磁力计校准 *形 参: 磁力计原始数据 *返 回 值: 无 **********************************************************************************************************/ void MagCalibration(void) { static Vector3f_t samples[6]; static uint32_t cnt_m=0; static float cali_rotate_angle = 0; static Vector3f_t new_offset; static Vector3f_t new_scale; Vector3f_t magRaw; static float earthMag = 0; //计算时间间隔,用于积分 static uint64_t previousT; float deltaT = (GetSysTimeUs() - previousT) * 1e-6; previousT = GetSysTimeUs(); if(mag.cali.should_cali) { //读取罗盘数据 MagSensorRead(&magRaw); //校准分两个阶段:1.水平旋转 2.机头朝上或朝下然后水平旋转 //两个阶段分别对飞机的z轴和x轴陀螺仪数据进行积分,记录旋转过的角度 if(mag.cali.step == 1) { cali_rotate_angle += GyroGetData().z * deltaT; } else if(mag.cali.step == 2) { cali_rotate_angle += GyroGetData().x * deltaT; } if(cnt_m == 0) { mag.cali.step = 1; cali_rotate_angle = 0; cnt_m++; //发送当前校准步骤 MessageSensorCaliFeedbackEnable(MAG, mag.cali.step, mag.cali.success); } else if(cnt_m == 1) { //初始化磁场强度模值 earthMag = Pythagorous3(magRaw.x, magRaw.y, magRaw.z); //初始化采样点 samples[MaxX] = samples[MinX] = magRaw; samples[MaxY] = samples[MinY] = magRaw; samples[MaxZ] = samples[MinZ] = magRaw; cnt_m++; } else { //实时计算磁场强度模值 earthMag = earthMag * 0.998f + Pythagorous3(magRaw.x, magRaw.y, magRaw.z) * 0.002f; //找到每个轴的最大最小值,并对采样值进行一阶低通滤波 if(Pythagorous3(magRaw.x, magRaw.y, magRaw.z) < earthMag * 1.5f) { //找到每个轴的最大最小值,并对采样值进行一阶低通滤波 if(magRaw.x > samples[MaxX].x) { LowPassFilter1st(&samples[MaxX], magRaw, 0.3); } if(magRaw.x < samples[MinX].x) { LowPassFilter1st(&samples[MinX], magRaw, 0.3); } if(magRaw.y > samples[MaxY].y) { LowPassFilter1st(&samples[MaxY], magRaw, 0.3); } if(magRaw.y < samples[MinY].y) { LowPassFilter1st(&samples[MinY], magRaw, 0.3); } if(magRaw.z > samples[MaxZ].z) { LowPassFilter1st(&samples[MaxZ], magRaw, 0.3); } if(magRaw.z < samples[MinZ].z) { LowPassFilter1st(&samples[MinZ], magRaw, 0.3); } } else { earthMag = earthMag; } //mavlink发送当前校准进度 if(mag.cali.step == 1) MavlinkSendNoticeProgress(abs(cali_rotate_angle) / 72); else if(mag.cali.step == 2) MavlinkSendNoticeProgress((abs(cali_rotate_angle) + 360) / 72); //水平旋转一圈 if(mag.cali.step == 1 && abs(cali_rotate_angle) > 360) { mag.cali.step = 2; cali_rotate_angle = 0; //bsklink发送当前校准步骤 MessageSensorCaliFeedbackEnable(MAG, mag.cali.step, mag.cali.success); //msklink发送当前校准步骤 MavlinkSendNoticeEnable(CAL_DOWN_DONE); } //竖直旋转一圈 if(mag.cali.step == 2 && abs(cali_rotate_angle ) > 360) { cnt_m = 0; mag.cali.should_cali = 0; mag.cali.step = 3; cali_rotate_angle = 0; earthMag = 0; //计算当地地磁场强度模值均值 for(u8 i=0; i<3; i++) { earthMag += Pythagorous3((samples[i*2].x - samples[i*2+1].x) * 0.5f, (samples[i*2].y - samples[i*2+1].y) * 0.5f, (samples[i*2].z - samples[i*2+1].z) * 0.5f); } earthMag /= 3; //计算方程解初值 float initBeta[6]; initBeta[0] = (samples[MaxX].x + samples[MinX].x) * 0.5f; initBeta[1] = (samples[MaxY].y + samples[MinY].y) * 0.5f; initBeta[2] = (samples[MaxZ].z + samples[MinZ].z) * 0.5f; initBeta[3] = 1 / earthMag; initBeta[4] = 1 / earthMag; initBeta[5] = 1 / earthMag; //LM法求解传感器误差方程最优解 LevenbergMarquardt(samples, &new_offset, &new_scale, initBeta, earthMag); //判断校准参数是否正常 if(isnan(new_scale.x) || isnan(new_scale.y) || isnan(new_scale.z)) { mag.cali.success = false; } else if(fabsf(new_scale.x-1.0f) > 0.8f || fabsf(new_scale.y-1.0f) > 0.8f || fabsf(new_scale.z-1.0f) > 0.8f) { mag.cali.success = false; } else if(fabsf(new_offset.x) > (earthMag * 2) || fabsf(new_offset.y) > (earthMag * 2) || fabsf(new_offset.z) > (earthMag * 2)) { mag.cali.success = false; } else { mag.cali.success = true; } if(mag.cali.success) { mag.cali.offset = new_offset; mag.cali.scale = new_scale; mag.earthMag = earthMag; //保存校准参数 ParamUpdateData(PARAM_MAG_OFFSET_X, &mag.cali.offset.x); ParamUpdateData(PARAM_MAG_OFFSET_Y, &mag.cali.offset.y); ParamUpdateData(PARAM_MAG_OFFSET_Z, &mag.cali.offset.z); ParamUpdateData(PARAM_MAG_SCALE_X, &mag.cali.scale.x); ParamUpdateData(PARAM_MAG_SCALE_Y, &mag.cali.scale.y); ParamUpdateData(PARAM_MAG_SCALE_Z, &mag.cali.scale.z); ParamUpdateData(PARAM_MAG_EARTH_MAG, &mag.earthMag); //更新mavlink参数 MavParamSetValue(CAL_MAG0_XOFF, mag.cali.offset.x); MavParamSetValue(CAL_MAG0_YOFF, mag.cali.offset.y); MavParamSetValue(CAL_MAG0_ZOFF, mag.cali.offset.z); MavParamSetValue(CAL_MAG0_XSCALE, mag.cali.scale.x); MavParamSetValue(CAL_MAG0_YSCALE, mag.cali.scale.y); MavParamSetValue(CAL_MAG0_XSCALE, mag.cali.scale.z); //mavlink发送校准结果 MavlinkSendNoticeEnable(CAL_DONE); } else { //mavlink发送校准结果 MavlinkSendNoticeEnable(CAL_FAILED); } //bsklink发送校准结果 MessageSensorCaliFeedbackEnable(MAG, mag.cali.step, mag.cali.success); mag.cali.step = 0; earthMag = 0; } } } }