1、说明
在正点原子的源码中,高度融合两种数据,分别是z轴方向的加速度计计算预测值,气压计去除零偏之后的值作为测量值;两者做差得到一个纠偏值,将该差值乘上气压计权值,去更新预测值,最后得到最优估计值;此处我将tof的数据替换气压计的数据,最终将估计得到z轴速度和位置打印到匿名上位机中;
其中,重要的是如何调试出tof的修正权值,此处我将融合的数据波形实时打印到匿名上位机上,观察不同的权值下的融合效果,最后试出一个较为理想的值;
2、code
(1)融合估计
//速度预估和位置预估
static void updateEstimatedTopic(float dt)
{
//使用加速度预估位移和速度
posAndVelocityPredict(Z, dt, posEstimator.imu.accelNEU.z);
posAndVelocityPredict(Y, dt, posEstimator.imu.accelNEU.y);
posAndVelocityPredict(X, dt, posEstimator.imu.accelNEU.x);
//加速度偏置值
const bool updateAccBias = (W_ACC_BIAS > 0);
Axis3f accelBiasCorr = { { 0, 0, 0} };
//使用气压计高度误差修正预估的位移和速度(Z轴)
#if USE_TOF_NO_BARO
const float tofResidual = posEstimator.tof.alt - posEstimator.est.pos.z;
posAndVelocityCorrect(Z, dt, tofResidual, W_Z_TOF_P);
#else
const float baroResidual = posEstimator.baro.alt - posEstimator.est.pos.z;
posAndVelocityCorrect(Z, dt, baroResidual, W_Z_TOF_P);
#endif
if (updateAccBias)
{
#if USE_TOF_NO_BARO
accelBiasCorr.z -= tofResidual * sq(W_Z_TOF_P);
#else
accelBiasCorr.z -= baroResidual * sq(W_Z_BARO_P);
#endif
}
//修正加速度偏置值
if (updateAccBias)
{
const float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.x) + sq(accelBiasCorr.y) + sq(accelBiasCorr.z);
if (accelBiasCorrMagnitudeSq < sq(GRAVITY_CMSS * 0.25f))//偏置小于0.25G时可以修正
{
//将加速度偏置值由世界坐标系转换为机体坐标系
imuTransformVectorEarthToBody(&accelBiasCorr);
posEstimator.imu.accelBias.x += accelBiasCorr.x * W_ACC_BIAS * dt;
posEstimator.imu.accelBias.y += accelBiasCorr.y * W_ACC_BIAS * dt;
posEstimator.imu.accelBias.z += accelBiasCorr.z * W_ACC_BIAS * dt;
}
}
}
//预估器初始化
static void initializePositionEstimator(void)
{
posEstimator.imu.gravityCalibrationComplete = false;
for (int axis = 0; axis < 3; axis++)
{
posEstimator.imu.accelBias.axis[axis] = 0;
posEstimator.est.pos.axis[axis] = 0;
posEstimator.est.vel.axis[axis] = 0;
}
}
此处重要的就是这里的tof正值;实际在调试的过程中,需要不断调试这个值的大小;
我在调试的过程中,将融合的高度和速度,以及tof直接输出的数据都打印到上位机的波形显示界面上面;
根据实际融合的高度是否实时跟随tof直出的数据,不断尝试调整这个修正的值,最后得到一个相对理想的值;
(2)相关定义
#define W_Z_TOF_P 5.1f //tof修正权重
#define USE_TOF_NO_BARO 1
(3)上位机自定义输出
分别输出:
气压计高度、
tof原始高度、
姿态估计中z轴的高度、
期望的z轴的高度、
姿态估计中z轴的速度、
期望的z轴的速度、
油门调整值、
static void sendaboutalt(float bar_alt,float tof_alt,float state_pos_z,float setpoint_pos_z,float state_vel_z,float setpoint_vel_z,float ThrustAdj)
{
u8 _cnt=0;
atkp_t p;
p.msgID = 0xF1;
p.data[_cnt++]=BYTE3(bar_alt);
p.data[_cnt++]=BYTE2(bar_alt);
p.data[_cnt++]=BYTE1(bar_alt);
p.data[_cnt++]=BYTE0(bar_alt);
p.data[_cnt++]=BYTE3(tof_alt);
p.data[_cnt++]=BYTE2(tof_alt);
p.data[_cnt++]=BYTE1(tof_alt);
p.data[_cnt++]=BYTE0(tof_alt);
p.data[_cnt++]=BYTE3(state_pos_z);
p.data[_cnt++]=BYTE2(state_pos_z);
p.data[_cnt++]=BYTE1(state_pos_z);
p.data[_cnt++]=BYTE0(state_pos_z);
p.data[_cnt++]=BYTE3(setpoint_pos_z);
p.data[_cnt++]=BYTE2(setpoint_pos_z);
p.data[_cnt++]=BYTE1(setpoint_pos_z);
p.data[_cnt++]=BYTE0(setpoint_pos_z);
p.data[_cnt++]=BYTE3(state_vel_z);
p.data[_cnt++]=BYTE2(state_vel_z);
p.data[_cnt++]=BYTE1(state_vel_z);
p.data[_cnt++]=BYTE0(state_vel_z);
p.data[_cnt++]=BYTE3(setpoint_vel_z);
p.data[_cnt++]=BYTE2(setpoint_vel_z);
p.data[_cnt++]=BYTE1(setpoint_vel_z);
p.data[_cnt++]=BYTE0(setpoint_vel_z);
p.data[_cnt++]=BYTE3(ThrustAdj);
p.data[_cnt++]=BYTE2(ThrustAdj);
p.data[_cnt++]=BYTE1(ThrustAdj);
p.data[_cnt++]=BYTE0(ThrustAdj);
p.dataLen = _cnt;
atkpSendPacket(&p);
}
(4)打印队列添加
if(!(count_ms % PERIOD_MOTOR))
{
sendaboutalt(sensorData.baro.asl*1000,sensorData.tof.asl*1000,state.position.z*1000,setpoint.position.z*1000,state.velocity.z*1000,setpoint.velocity.z*1000,altThrustAdj*1000);
}
3、上位机设置
(1)高级收码设置
(2)用户数据波形
4、git地址
git@gitee.com:ggxxd/wukong_dronefly.git