/**********************************************************************************************************
*函 数 名: 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;
}
}
}
}
