fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 13:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

There was a bug in position_estimator_mc. Solved

Browse Source
This commit is contained in:
Hyon Lim (Retina)
2013-08-31 03:11:58 +09:00
parent 2b62497fb5
commit fff1856377
1 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_mc/position_estimator_mc_main.c
+6 -1
View File
@@ -497,7 +497,12 @@ int position_estimator_mc_thread_main(int argc, char *argv[])
local_pos_est.vz = x_z_aposteriori_k[1];
local_pos_est.timestamp = hrt_absolute_time();
if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){
orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
if(local_pos_est_pub > 0)
orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
else
local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
//char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]);
//mavlink_log_info(mavlink_fd, buf);
}
}
} /* end of poll return value check */