fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 07:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add stop*Fusion function

Browse Source
This commit is contained in:
kamilritz
2019-12-05 20:09:34 +11:00
committed by Paul Riseborough
parent d5dc6bb8ea
commit 5c038a3b43
3 changed files with 111 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+70
View File
@@ -1870,3 +1870,73 @@ float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) c
accumulator = (t - sum_previous) - y;
return t;
}
void Ekf::stopGpsFusion()
{
stopGpsPosFusion();
stopGpsVelFusion();
stopGpsYawFusion();
}
void Ekf::stopGpsPosFusion()
{
_control_status.flags.gps = false;
_control_status.flags.gps_hgt = false;
memset(_gps_vel_pos_innov+3,0.0f, sizeof(float)*3);
memset(_gps_vel_pos_innov_var+3,0.0f, sizeof(float)*3);
memset(_gps_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2);
}
void Ekf::stopGpsVelFusion()
{
memset(_gps_vel_pos_innov,0.0f, sizeof(float)*3);
memset(_gps_vel_pos_innov_var,0.0f, sizeof(float)*3);
memset(_gps_vel_pos_test_ratio,0.0f, sizeof(float)*2);
}
void Ekf::stopGpsYawFusion()
{
_control_status.flags.gps_yaw = false;
}
void Ekf::stopEvFusion()
{
stopEvPosFusion();
stopEvVelFusion();
stopEvYawFusion();
}
void Ekf::stopEvPosFusion()
{
_control_status.flags.ev_pos = false;
memset(_ev_vel_pos_innov+3,0.0f, sizeof(float)*3);
memset(_ev_vel_pos_innov_var+3,0.0f, sizeof(float)*3);
memset(_ev_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2);
}
void Ekf::stopEvVelFusion()
{
_control_status.flags.ev_vel = false;
memset(_ev_vel_pos_innov,0.0f, sizeof(float)*3);
memset(_ev_vel_pos_innov_var,0.0f, sizeof(float)*3);
memset(_ev_vel_pos_test_ratio,0.0f, sizeof(float)*2);}
void Ekf::stopEvYawFusion()
{
_control_status.flags.ev_yaw = false;
}
void Ekf::stopAuxVelFusion()
{
// TODO: Add proper handling of auxiliar velocity fusion
}
void Ekf::stopFlowFusion()
{
_control_status.flags.opt_flow = false;
memset(_flow_innov,0.0f,sizeof(_flow_innov));
memset(_flow_innov_var,0.0f,sizeof(_flow_innov_var));
memset(&_optflow_test_ratio,0.0f,sizeof(_optflow_test_ratio));
}