Merge pull request #2292 from PX4/l1_fix

L1 fix

src/modules/commander/commander.cpp

@@ -143,7 +143,9 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage
 #define DIFFPRESS_TIMEOUT		2000000
 
 #define PRINT_INTERVAL	5000000
-#define PRINT_MODE_REJECT_INTERVAL	2000000
+#define PRINT_MODE_REJECT_INTERVAL	10000000
+
+#define INAIR_RESTART_HOLDOFF_INTERVAL	2000000
 
 enum MAV_MODE_FLAG {
 	MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
@@ -169,6 +171,7 @@ static volatile bool thread_should_exit = false;	/**< daemon exit flag */
 static volatile bool thread_running = false;		/**< daemon status flag */
 static int daemon_task;					/**< Handle of daemon task / thread */
 static bool need_param_autosave = false;		/**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */
+static hrt_abstime commander_boot_timestamp = 0;
 
 static unsigned int leds_counter;
 /* To remember when last notification was sent */
@@ -208,7 +211,7 @@ void usage(const char *reason);
  */
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
 		    struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
-		    orb_advert_t *home_pub);
+		    struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub);
 
 /**
  * Mainloop of commander.
@@ -347,6 +350,7 @@ int commander_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "arm")) {
 		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
 		arm_disarm(true, mavlink_fd_local, "command line");
+		warnx("note: not updating home position on commandline arming!");
 		close(mavlink_fd_local);
 		exit(0);
 	}
@@ -450,7 +454,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 
 bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
 		    struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
-		    struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
+		    struct home_position_s *home, struct vehicle_global_position_s *global_pos,
+		    struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub)
 {
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
@@ -478,6 +483,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			// Transition the arming state
 			arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
+			/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
+			if (arming_ret == TRANSITION_CHANGED && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) {
+				commander_set_home_position(*home_pub, *home, *local_pos, *global_pos);
+			}
+
 			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
 				/* use autopilot-specific mode */
 				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
@@ -1161,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[])
 		set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	}
 
-	const hrt_abstime commander_boot_timestamp = hrt_absolute_time();
+	commander_boot_timestamp = hrt_absolute_time();
 
 	transition_result_t arming_ret;
 
@@ -1957,7 +1967,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
 			/* handle it */
-			if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &home_pub)) {
+			if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) {
 				status_changed = true;
 			}
 		}
@@ -2023,7 +2033,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
-		else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) {
+		else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) {
 			commander_set_home_position(home_pub, _home, local_position, global_position);
 		}
 

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -174,7 +174,7 @@ private:
 
     struct map_projection_reference_s   _pos_ref;
 
-    float                       _baro_ref_offset;   /**< offset between initial baro reference and GPS init baro altitude */
+    float                       _filter_ref_offset;   /**< offset between initial baro reference and GPS init baro altitude */
     float                       _baro_gps_offset;   /**< offset between baro altitude (at GPS init time) and GPS altitude */
     hrt_abstime                 _last_debug_print = 0;
 
@@ -193,7 +193,6 @@ private:
     bool            _gpsIsGood;               ///< True if the current GPS fix is good enough for us to use
     uint64_t        _previousGPSTimestamp;    ///< Timestamp of last good GPS fix we have received
     bool            _baro_init;
-    float           _baroAltRef;
     bool            _gps_initialized;
     hrt_abstime     _filter_start_time;
     hrt_abstime     _last_sensor_timestamp;
@@ -333,6 +332,12 @@ private:
     **/
     void initializeGPS();
 
+    /**
+     * Initialize the reference position for the local coordinate frame
+     */
+    void initReferencePosition(hrt_abstime timestamp,
+            double lat, double lon, float gps_alt, float baro_alt);
+
     /**
     * @brief
     *   Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -153,7 +153,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
     _sensor_combined {},
 
     _pos_ref{},
-    _baro_ref_offset(0.0f),
+    _filter_ref_offset(0.0f),
     _baro_gps_offset(0.0f),
 
     /* performance counters */
@@ -173,7 +173,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
     _gpsIsGood(false),
     _previousGPSTimestamp(0),
     _baro_init(false),
-    _baroAltRef(0.0f),
     _gps_initialized(false),
     _filter_start_time(0),
     _last_sensor_timestamp(0),
@@ -404,6 +403,15 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 
 		// Count the reset condition
 		perf_count(_perf_reset);
+		// GPS is in scaled integers, convert
+		double lat = _gps.lat / 1.0e7;
+		double lon = _gps.lon / 1.0e7;
+		float gps_alt = _gps.alt / 1e3f;
+
+		// Set up height correctly
+		orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+
+		initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude);
 
 	} else if (_ekf_logging) {
 		_ekf->GetFilterState(&ekf_report);
@@ -585,6 +593,7 @@ void AttitudePositionEstimatorEKF::task_main()
 
 				_baro_init = false;
 				_gps_initialized = false;
+
 				_last_sensor_timestamp = hrt_absolute_time();
 				_last_run = _last_sensor_timestamp;
 
@@ -643,12 +652,13 @@ void AttitudePositionEstimatorEKF::task_main()
 					_ekf->posNE[1] = 0.0f;
 
 					_local_pos.ref_alt = 0.0f;
-					_baro_ref_offset = 0.0f;
 					_baro_gps_offset = 0.0f;
 					_baro_alt_filt = _baro.altitude;
 
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 
+					warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset);
+
 				} else {
 
 					if (!_gps_initialized && _gpsIsGood) {
@@ -702,6 +712,32 @@ void AttitudePositionEstimatorEKF::task_main()
 	_exit(0);
 }
 
+void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
+	double lat, double lon, float gps_alt, float baro_alt)
+{
+	// Reference altitude
+	if (isfinite(_ekf->states[9])) {
+		_filter_ref_offset = _ekf->states[9];
+	} else if (isfinite(-_ekf->hgtMea)) {
+		_filter_ref_offset = -_ekf->hgtMea;
+	} else {
+		_filter_ref_offset = -_baro.altitude;
+	}
+
+	// init filtered gps and baro altitudes
+	_gps_alt_filt = gps_alt;
+	_baro_alt_filt = baro_alt;
+
+	// Initialize projection
+	_local_pos.ref_lat = lat;
+	_local_pos.ref_lon = lon;
+	_local_pos.ref_alt = gps_alt;
+	_local_pos.ref_timestamp = timestamp;
+
+	map_projection_init(&_pos_ref, lat, lon);
+	mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
+}
+
 void AttitudePositionEstimatorEKF::initializeGPS()
 {
 	// GPS is in scaled integers, convert
@@ -711,11 +747,6 @@ void AttitudePositionEstimatorEKF::initializeGPS()
 
 	// Set up height correctly
 	orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-	_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
-
-	// init filtered gps and baro altitudes
-	_gps_alt_filt = gps_alt;
-	_baro_alt_filt = _baro.altitude;
 
 	_ekf->baroHgt = _baro.altitude;
 	_ekf->hgtMea = _ekf->baroHgt;
@@ -737,20 +768,13 @@ void AttitudePositionEstimatorEKF::initializeGPS()
 
 	_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
 
-	// Initialize projection
-	_local_pos.ref_lat = lat;
-	_local_pos.ref_lon = lon;
-	_local_pos.ref_alt = gps_alt;
-	_local_pos.ref_timestamp = _gps.timestamp_position;
-
-	map_projection_init(&_pos_ref, lat, lon);
-	mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
+	initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude);
 
 #if 0
 	warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
 	      (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
 	warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref,
-	      (double)_baro_ref_offset);
+	      (double)_filter_ref_offset);
 	warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv,
 	      (double)math::degrees(declination));
 #endif
@@ -811,7 +835,8 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
 	_local_pos.y = _ekf->states[8];
 
 	// XXX need to announce change of Z reference somehow elegantly
-	_local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef;
+	_local_pos.z = _ekf->states[9] - _filter_ref_offset;
+	//_local_pos.z_stable = _ekf->states[9];
 
 	_local_pos.vx = _ekf->states[4];
 	_local_pos.vy = _ekf->states[5];
@@ -826,6 +851,17 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
 	_local_pos.z_global = false;
 	_local_pos.yaw = _att.yaw;
 
+	if (!isfinite(_local_pos.x) ||
+		!isfinite(_local_pos.y) ||
+		!isfinite(_local_pos.z) ||
+		!isfinite(_local_pos.vx) ||
+		!isfinite(_local_pos.vy) ||
+		!isfinite(_local_pos.vz))
+	{
+		// bad data, abort publication
+		return;
+	}
+
 	/* lazily publish the local position only once available */
 	if (_local_pos_pub > 0) {
 		/* publish the attitude setpoint */
@@ -859,7 +895,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 	}
 
 	/* local pos alt is negative, change sign and add alt offsets */
-	_global_pos.alt = (-_local_pos.z) - _baro_gps_offset;
+	_global_pos.alt = (-_local_pos.z) - _filter_ref_offset -  _baro_gps_offset;
 
 	if (_local_pos.v_z_valid) {
 		_global_pos.vel_d = _local_pos.vz;
@@ -874,6 +910,17 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 	_global_pos.eph = _gps.eph;
 	_global_pos.epv = _gps.epv;
 
+	if (!isfinite(_global_pos.lat) ||
+		!isfinite(_global_pos.lon) ||
+		!isfinite(_global_pos.alt) ||
+		!isfinite(_global_pos.vel_n) ||
+		!isfinite(_global_pos.vel_e) ||
+		!isfinite(_global_pos.vel_d))
+	{
+		// bad data, abort publication
+		return;
+	}
+
 	/* lazily publish the global position only once available */
 	if (_global_pos_pub > 0) {
 		/* publish the global position */
@@ -1070,8 +1117,9 @@ void AttitudePositionEstimatorEKF::print_status()
 	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
 	printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec);
-	printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
-	printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset,
+	printf("alt RAW: baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)_ekf->gpsHgt);
+	printf("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)\n", (double)(_local_pos.z), (double)_global_pos.alt);
+	printf("filter ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_filter_ref_offset,
 	       (double)_baro_gps_offset);
 	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y,
 	       (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
@@ -1317,7 +1365,11 @@ void AttitudePositionEstimatorEKF::pollData()
 				_ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD));
 
 				// update LPF
-				_gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
+				float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
+
+				if (isfinite(filter_step)) {
+					_gps_alt_filt += filter_step;
+				}
 			}
 
 			//check if we had a GPS outage for a long time
@@ -1400,15 +1452,19 @@ void AttitudePositionEstimatorEKF::pollData()
 		}
 
 		baro_last = _baro.timestamp;
-		if(!_baro_init) {
+		if (!_baro_init) {
 			_baro_init = true;
-			_baroAltRef = _baro.altitude;
+			_baro_alt_filt = _baro.altitude;
 		}
 
 		_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
 
 		_ekf->baroHgt = _baro.altitude;
-		_baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
+		float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
+
+		if (isfinite(filter_step)) {
+			_baro_alt_filt += filter_step;
+		}
 
 		perf_count(_perf_baro);
 	}
@@ -1494,30 +1550,34 @@ int AttitudePositionEstimatorEKF::trip_nan()
 
 		float nan_val = 0.0f / 0.0f;
 
-		warnx("system not armed, tripping state vector with NaN values");
+		warnx("system not armed, tripping state vector with NaN");
 		_ekf->states[5] = nan_val;
 		usleep(100000);
 
-		warnx("tripping covariance #1 with NaN values");
+		warnx("tripping covariance #1 with NaN");
 		_ekf->KH[2][2] = nan_val; //  intermediate result used for covariance updates
 		usleep(100000);
 
-		warnx("tripping covariance #2 with NaN values");
+		warnx("tripping covariance #2 with NaN");
 		_ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates
 		usleep(100000);
 
-		warnx("tripping covariance #3 with NaN values");
+		warnx("tripping covariance #3 with NaN");
 		_ekf->P[3][3] = nan_val; // covariance matrix
 		usleep(100000);
 
-		warnx("tripping Kalman gains with NaN values");
+		warnx("tripping Kalman gains with NaN");
 		_ekf->Kfusion[0] = nan_val; // Kalman gains
 		usleep(100000);
 
-		warnx("tripping stored states[0] with NaN values");
+		warnx("tripping stored states[0] with NaN");
 		_ekf->storedStates[0][0] = nan_val;
 		usleep(100000);
 
+		warnx("tripping states[9] with NaN");
+		_ekf->states[9] = nan_val;
+		usleep(100000);
+
 		warnx("\nDONE - FILTER STATE:");
 		print_status();
 	}

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -900,6 +900,8 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
 
 void FixedwingPositionControl::navigation_capabilities_publish()
 {
+	_nav_capabilities.timestamp = hrt_absolute_time();
+
 	if (_nav_capabilities_pub > 0) {
 		orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
 	} else {
@@ -1674,7 +1676,8 @@ FixedwingPositionControl::task_main()
 				float turn_distance = _l1_control.switch_distance(100.0f);
 
 				/* lazily publish navigation capabilities */
-				if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
+				if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON
+					&& turn_distance > 0)) {
 
 					/* set new turn distance */
 					_nav_capabilities.turn_distance = turn_distance;

src/modules/navigator/mission.cpp

@@ -586,7 +586,7 @@ Mission::altitude_sp_foh_update()
 	}
 
 	/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
-	if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) {
+	if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < 0.0f) {
 		return;
 	}
 
@@ -608,7 +608,7 @@ Mission::altitude_sp_foh_update()
 
 	/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
 	 * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
-	if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) {
+	if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
 		pos_sp_triplet->current.alt = _mission_item.altitude;
 	} else {
 		/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
@@ -617,7 +617,7 @@ Mission::altitude_sp_foh_update()
 		* radius around the current waypoint
 		**/
 		float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
-		float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius);
+		float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius));
 		float a = _mission_item_previous_alt - grad * _distance_current_previous;
 		pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
 

src/modules/navigator/mission_block.cpp

@@ -123,12 +123,12 @@ MissionBlock::is_mission_item_reached()
 			 * Therefore the item is marked as reached once the system reaches the loiter
 			 * radius (+ some margin). Time inside and turn count is handled elsewhere.
 			 */
-			if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) {
 				_waypoint_position_reached = true;
 			}
 		} else {
 			/* for normal mission items used their acceptance radius */
-			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
 				_waypoint_position_reached = true;
 			}
 		}

src/modules/navigator/navigator.h

@@ -144,7 +144,22 @@ public:
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
-	float		get_acceptance_radius() { return _param_acceptance_radius.get(); }
+
+	/**
+	 * Get the acceptance radius
+	 *
+	 * @return the distance at which the next waypoint should be used
+	 */
+	float		get_acceptance_radius();
+
+	/**
+	 * Get the acceptance radius given the mission item preset radius
+	 *
+	 * @param mission_item_radius the radius to use in case the controller-derived radius is smaller
+	 *
+	 * @return the distance at which the next waypoint should be used
+	 */
+	float		get_acceptance_radius(float mission_item_radius);
 	int		get_mavlink_fd() { return _mavlink_fd; }
 
 private:

src/modules/navigator/navigator_main.cpp

@@ -571,6 +571,26 @@ Navigator::publish_position_setpoint_triplet()
 	}
 }
 
+float
+Navigator::get_acceptance_radius()
+{
+	return get_acceptance_radius(_param_acceptance_radius.get());
+}
+
+float
+Navigator::get_acceptance_radius(float mission_item_radius)
+{
+	float radius = mission_item_radius;
+
+	if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) {
+		if (_nav_caps.turn_distance > radius) {
+			radius = _nav_caps.turn_distance;
+		}
+	}
+
+	return radius;
+}
+
 void Navigator::add_fence_point(int argc, char *argv[])
 {
 	_geofence.addPoint(argc, argv);