LPE SITL script updates.

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

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",
+			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(0)),  double(r(1)), double(r(2)),
-					 double(r(3)), double(r(4)), double(r(5)));
+			      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;
 		}