diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris index 443ba292c0..a15ce0f2e0 100644 --- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris @@ -41,7 +41,6 @@ param set MPC_Z_VEL_P 0.4 # changes for LPE param set COM_RC_IN_MODE 1 -param set LPE_BETA_MAX 10000 rgbledsim start tone_alarm start @@ -57,6 +56,7 @@ commander start land_detector start multicopter navigator start attitude_estimator_q start +local_position_estimator start mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix @@ -73,7 +73,3 @@ mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 mavlink stream -r 20 -s MANUAL_CONTROL -u 14556 mavlink boot_complete sdlog2 start -r 100 -e -t -a - -# start LPE at end, when we know it is ok to init sensors -sleep 5 -local_position_estimator start diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index e562593bdc..352362bc52 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -1498,9 +1498,12 @@ void BlockLocalPositionEstimator::correctGps() mavlink_log_info(_mavlink_fd, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)), double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5))); - mavlink_log_info(_mavlink_fd, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", - double(r(0)), double(r(1)), double(r(2)), - double(r(3)), double(r(4)), double(r(5))); + warnx("[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + double(r(0)), double(r(1)), double(r(2)), + double(r(3)), double(r(4)), double(r(5))); + warnx("[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)), + double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5))); _gpsFault = FAULT_MINOR; }