/**********************************************************************************************************
*函 数 名: Sbus_Decode
*功能说明: sbus协议解析
*形 参: 输入数据
*返 回 值: 无
**********************************************************************************************************/
static void Sbus_Decode(uint8_t data)
{
static uint32_t lastTime;
static uint32_t dataCnt = 0;
static uint8_t initFlag = 0;
if(GetSysTimeMs() < 2000)
return;
uint32_t deltaT = GetSysTimeMs() - lastTime;
lastTime = GetSysTimeMs();
//数据间隔大于3ms则可以认为新的一帧开始了
if(deltaT > 3)
{
dataCnt = 0;
}
//接收数据
sbus.raw[dataCnt++] = data;
//每帧数据长度为25
if(dataCnt == 25)
{
//判断帧头帧尾是否正确
if(sbus.raw[0] != 0x0F || sbus.raw[24] != 0)
return;
//每个通道数据占据11个字节，这里使用了字节对齐的方式来进行解析
//转换摇杆数据量程为[1000:2000]
sbusData.roll = sbus.msg.chan1 * 0.625f + 880;
sbusData.pitch = sbus.msg.chan2 * 0.625f + 880;
sbusData.throttle = sbus.msg.chan3 * 0.625f + 880;
sbusData.yaw = sbus.msg.chan4 * 0.625f + 880;
sbusData.aux1 = sbus.msg.chan5 * 0.625f + 880;
sbusData.aux2 = sbus.msg.chan6 * 0.625f + 880;
sbusData.aux3 = sbus.msg.chan7 * 0.625f + 880;
sbusData.aux4 = sbus.msg.chan8 * 0.625f + 880;
sbusData.aux5 = sbus.msg.chan9 * 0.625f + 880;
sbusData.aux6 = sbus.msg.chan10 * 0.625f + 880;
sbusData.aux7 = sbus.msg.chan11 * 0.625f + 880;
sbusData.aux8 = sbus.msg.chan12 * 0.625f + 880;
sbusData.aux8 = sbus.msg.chan13 * 0.625f + 880;
sbusData.aux10 = sbus.msg.chan14 * 0.625f + 880;
sbusData.aux11 = sbus.msg.chan15 * 0.625f + 880;
sbusData.aux12 = sbus.msg.chan16 * 0.625f + 880;
//一帧数据解析完成
if(rcDataCallbackFunc != 0)
(*rcDataCallbackFunc)(sbusData);
if(!initFlag)
{
//禁用PPM输入
PPM_Disable();
initFlag = 1;
}
}
}
void delayline(unsigned int Time, unsigned int k)
{
// extern global var
extern unsigned char g_Speed;
unsigned long T1 = 0;
unsigned char maxIndex = 0;
unsigned int Speed2 = 0;
Speed2=g_Speed-10;Time=Time*10;
T1 = GetSysTimeMs();
while ( GetSysTimeMs()-T1<Time )
{
getAd();
lineslow1(8*k,7*k,5*k,4*k);
}
SetBuzzer(_TRACE_1_, 1);
SetMotor_Code(1, 1, 100);
SetMotor_Code(2, 1, 100);
}
void GoLineTimeLR(unsigned int Time, unsigned int LR)
{
// extern global var
extern unsigned char g_Speed;
unsigned long T1 = 0;
unsigned char maxIndex = 0;
unsigned int Speed2 = 0;
unsigned int n = 1;
Speed2=g_Speed-10;Time=Time*10;
T1 = GetSysTimeMs();
while ( GetSysTimeMs()-T1<=Time )
{
getAd();
slowLineLR(60, 50, 40, 30, LR);
}
SetBuzzer(_TRACE_1_, 1);
SetMotor_Code(1, 1, 100);
SetMotor_Code(2, 1, 100);
}
/**********************************************************************************************************
*函 数 名: TransAccToEarthFrame
*功能说明: 转换加速度到地理坐标系，并去除重力加速度
*形 参: 当前飞机姿态 加速度 地理系加速度 地理系加速度低通滤波 加速度零偏
*返 回 值: 无
**********************************************************************************************************/
static void TransAccToEarthFrame(Vector3f_t angle, Vector3f_t acc, Vector3f_t* accEf, Vector3f_t* accEfLpf, Vector3f_t* accBfOffset)
{
static uint16_t offset_cnt = 5000; //计算零偏的次数
static Vector3f_t accAngle; //用于计算初始零偏
Vector3f_t gravityBf;
//即使经过校准并对传感器做了恒温处理，加速度的零偏误差还是存在不稳定性，即相隔一定时间后再上电加速度零偏会发生变化
//由于加速度零偏对导航积分计算影响较大，因此每次上电工作都需要计算零偏并补偿
//计算初始零偏时，直接使用加速度值计算角度，因为飞控初始化时角度估计值存在误差，导致计算出来的零偏有误差
if(GetInitStatus() == INIT_FINISH)
{
//计算重力加速度在机体坐标系的投影
gravityBf.x = 0;
gravityBf.y = 0;
gravityBf.z = 1;
EarthFrameToBodyFrame(angle, gravityBf, &gravityBf);
//减去重力加速度和加速度零偏
acc.x -= (gravityBf.x + accBfOffset->x);
acc.y -= (gravityBf.y + accBfOffset->y);
acc.z -= (gravityBf.z + accBfOffset->z);
//转化加速度到地理坐标系
BodyFrameToEarthFrame(angle, acc, accEf);
//向心加速度误差补偿
accEf->x -= ahrs.centripetalAcc.x;
accEf->y -= ahrs.centripetalAcc.y;
//地理系加速度低通滤波
accEfLpf->x = accEfLpf->x * 0.998f + accEf->x * 0.002f;
accEfLpf->y = accEfLpf->y * 0.998f + accEf->y * 0.002f;
accEfLpf->z = accEfLpf->z * 0.998f + accEf->z * 0.002f;
}
//系统初始化时，计算加速度零偏
if(GetSysTimeMs() > 5000 && GetInitStatus() == HEAT_FINISH)
{
//飞机静止时才进行零偏计算
if(GetPlaceStatus() == STATIC)
{
//直接使用加速度数据计算姿态角
AccVectorToRollPitchAngle(&accAngle, acc);
accAngle.x = Degrees(accAngle.x);
accAngle.y = Degrees(accAngle.y);
//转换重力加速度到机体坐标系并计算零偏误差
gravityBf.x = 0;
gravityBf.y = 0;
gravityBf.z = 1;
EarthFrameToBodyFrame(accAngle, gravityBf, &gravityBf);
accBfOffset->x = accBfOffset->x * 0.993f + (acc.x - gravityBf.x) * 0.007f;
accBfOffset->y = accBfOffset->y * 0.993f + (acc.y - gravityBf.y) * 0.007f;
accBfOffset->z = accBfOffset->z * 0.993f + (acc.z - gravityBf.z) * 0.007f;
offset_cnt--;
}
else
{
//计算过程中如果出现晃动，则重新开始
offset_cnt = 5000;
accBfOffset->x = 0;
accBfOffset->y = 0;
accBfOffset->z = 0;
}
//完成零偏计算，系统初始化结束
if(offset_cnt == 0)
{
SetInitStatus(INIT_FINISH);
FaultDetectResetWarnning(SYSTEM_INITIALIZING);
}
}
}
/**********************************************************************************************************
*函 数 名: AttitudeEstimateUpdate
*功能说明: 姿态更新
*形 参: 姿态角指针 角速度 加速度测量值 磁力计测量值 时间间隔
*返 回 值: 无
**********************************************************************************************************/
static void AttitudeEstimateUpdate(Vector3f_t* angle, Vector3f_t gyro, Vector3f_t acc, Vector3f_t mag, float deltaT)
{
Vector3f_t deltaAngle;
static Vector3f_t gError;
static Vector3f_t gErrorIntRate = {0.2f, 0.2f, 0.05f};
static Vector3f_t gyro_bias = {0, 0, 0}; //陀螺仪零偏
static Vector3f_t input = {0, 0, 0};
float dcMat[9];
Vector3f_t mVectorEf;
static uint32_t count = 0;
//修正陀螺仪零偏
gyro.x -= gyro_bias.x;
gyro.y -= gyro_bias.y;
gyro.z -= gyro_bias.z;
//一阶积分计算角度变化量，单位为弧度
deltaAngle.x = Radians(gyro.x * deltaT);
deltaAngle.y = Radians(gyro.y * deltaT);
deltaAngle.z = Radians(gyro.z * deltaT);
//角度变化量转换为方向余弦矩阵
EulerAngleToDCM(deltaAngle, dcMat);
//更新卡尔曼状态转移矩阵
KalmanStateTransMatSet(&kalmanRollPitch, dcMat);
KalmanStateTransMatSet(&kalmanYaw, dcMat);
//卡尔曼滤波器更新
//磁强数据更新频率要低于陀螺仪，因此磁强数据未更新时只进行状态预估计
KalmanUpdate(&kalmanRollPitch, input, acc, true);
KalmanUpdate(&kalmanYaw, input, mag, count++ % 10 == 0?true:false);
//计算俯仰与横滚角
AccVectorToRollPitchAngle(angle, kalmanRollPitch.state);
angle->x = Degrees(angle->x);
angle->y = Degrees(angle->y);
//计算偏航角，并修正磁偏角误差
//磁偏角东偏为正，西偏为负，中国除新疆外大部分地区为西偏，比如深圳地区为-2°左右
BodyFrameToEarthFrame(*angle, kalmanYaw.state, &mVectorEf);
MagVectorToYawAngle(angle, mVectorEf);
angle->z = WrapDegree360(Degrees(angle->z) + GetMagDeclination());
//向量观测值与估计值进行叉积运算得到旋转误差矢量
gError = VectorCrossProduct(acc, kalmanRollPitch.state);
BodyFrameToEarthFrame(*angle, gError, &gError);
//计算偏航误差
float magAngle;
BodyFrameToEarthFrame(*angle, mag, &mVectorEf);
magAngle = WrapDegree360(Degrees(atan2f(-mVectorEf.y, mVectorEf.x)) + GetMagDeclination());
if(abs(angle->z - magAngle) < 10 && abs(angle->x) < 10 && abs(angle->y)< 10)
{
gError.z = Radians(angle->z - magAngle);
}
else
{
gError.z = 0;
}
/********************************************陀螺仪零偏估计******************************************/
//系统初始化结束后的一段时间内，加快零偏估计速度
if(GetInitStatus() == INIT_FINISH && (GetSysTimeMs() - GetInitFinishTime()) < 15000)
{
gErrorIntRate.x = 1.0f;
gErrorIntRate.y = 1.0f;
gErrorIntRate.z = 0.2f;
}
else
{
gErrorIntRate.x = 0.2f;
gErrorIntRate.y = 0.2f;
gErrorIntRate.z = 0.05f;
}
//加热完成前传感器零偏变化较大，因此不进行零偏估计
if(GetInitStatus() >= HEAT_FINISH)
{
gyro_bias.x += (gError.x * deltaT) * gErrorIntRate.x;
gyro_bias.y += (gError.y * deltaT) * gErrorIntRate.y;
gyro_bias.z += (gError.z * deltaT) * gErrorIntRate.z;
}
//陀螺仪零偏限幅
gyro_bias.x = ConstrainFloat(gyro_bias.x, -1.0f, 1.0f);
gyro_bias.y = ConstrainFloat(gyro_bias.y, -1.0f, 1.0f);
gyro_bias.z = ConstrainFloat(gyro_bias.z, -1.0f, 1.0f);
/************************************近似计算姿态角误差，用于观察和调试**********************************/
AccVectorToRollPitchAngle(&ahrs.angleMeasure, acc);
ahrs.angleMeasure.x = Degrees(ahrs.angleMeasure.x);
ahrs.angleMeasure.y = Degrees(ahrs.angleMeasure.y);
ahrs.angleError.x = ahrs.angleError.x * 0.999f + (angle->x - ahrs.angleMeasure.x) * 0.001f;
ahrs.angleError.y = ahrs.angleError.y * 0.999f + (angle->y - ahrs.angleMeasure.y) * 0.001f;
Vector3f_t magEf;
BodyFrameToEarthFrame(*angle, mag, &magEf);
ahrs.angleMeasure.z = WrapDegree360(Degrees(atan2f(-magEf.y, magEf.x)) + GetMagDeclination());
ahrs.angleError.z = ahrs.angleError.z * 0.999f + (angle->z - ahrs.angleMeasure.z) * 0.001f;
/********************************************************************************************************/
}
