diff options
| author | Hyon Lim (Retina) <limhyon@gmail.com> | 2013-08-31 03:11:58 +0900 |
|---|---|---|
| committer | Hyon Lim (Retina) <limhyon@gmail.com> | 2013-08-31 03:11:58 +0900 |
| commit | fff1856377b687bc290f4a828363de0d416b401c (patch) | |
| tree | 8f3eb43513efe53f2885cd608f323dc12b3f401d /src/modules/position_estimator_mc | |
| parent | 2b62497fb5dd83db17b1d2851f686049841faa7e (diff) | |
| download | px4-firmware-fff1856377b687bc290f4a828363de0d416b401c.tar.gz px4-firmware-fff1856377b687bc290f4a828363de0d416b401c.tar.bz2 px4-firmware-fff1856377b687bc290f4a828363de0d416b401c.zip | |
There was a bug in position_estimator_mc. Solved
Diffstat (limited to 'src/modules/position_estimator_mc')
| -rwxr-xr-x | src/modules/position_estimator_mc/position_estimator_mc_main.c | 7 |
1 files changed, 6 insertions, 1 deletions
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index 984bd1329..363961819 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -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 */ |