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