position_estimator_inav: better handling of lost GPS, minor fixes

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -254,7 +254,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					/* initialize coordinates */
 					map_projection_init(lat_current, lon_current);
 					warnx("init GPS: lat = %.10f,  lon = %.10f", lat_current, lon_current);
-					mavlink_log_info(mavlink_fd, "[inav] init GPS: lat = %.10f,  lon = %.10f", lat_current, lon_current);
+					mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current);
 				}
 			}
 		}
@@ -361,10 +361,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				}
 			}
 
-			if (params.use_gps) {
+			if (params.use_gps && fds[4].revents & POLLIN) {
 				/* vehicle GPS position */
-				if (fds[4].revents & POLLIN) {
-					/* new GPS value */
 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 				if (gps.fix_type >= 3) {
 					/* project GPS lat lon to plane */
@@ -379,16 +377,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						gps_corr[0][1] = 0.0f;
 						gps_corr[1][1] = 0.0f;
 					}
-						local_pos.valid = true;
 					gps_updates++;
 				} else {
-						local_pos.valid = false;
+					memset(gps_corr, 0, sizeof(gps_corr));
-					}
 				}
-			} else {
-				local_pos.valid = true;
 			}
-
 		}
 
 		/* end of poll return value check */
@@ -403,19 +396,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		inertial_filter_correct(baro_corr * dt, z_est, 0, params.w_alt_baro);
 		inertial_filter_correct(accel_corr[2] * dt, z_est, 2, params.w_alt_acc);
 
-		if (params.use_gps) {
+		/* dont't try to estimate position when no any position source available */
+		bool can_estimate_pos = params.use_gps && gps.fix_type >= 3;
+		if (can_estimate_pos) {
 			/* inertial filter prediction for position */
 			inertial_filter_predict(dt, x_est);
 			inertial_filter_predict(dt, y_est);
 
 			/* inertial filter correction for position */
+			inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc);
+			inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc);
+
+			if (params.use_gps && gps.fix_type >= 3) {
 				inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p);
-			inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v);
 				inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p);
+				if (gps.vel_ned_valid) {
+					inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v);
 					inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v);
-
+				}
-			inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc);
+			}
-			inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc);
 		}
 
 		if (verbose_mode) {
@@ -439,6 +438,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		if (t - pub_last > pub_interval) {
 			pub_last = t;
 			local_pos.timestamp = t;
+			local_pos.valid = can_estimate_pos;
 			local_pos.x = x_est[0];
 			local_pos.vx = x_est[1];
 			local_pos.y = y_est[0];