Merge pull request #124 from PX4/pr-removeVehicleArmStatus

Remove reliance on vehicle arm status

EKF/common.h

@@ -371,11 +371,10 @@ union filter_control_status_u {
 		uint16_t mag_3D      : 1; // 5 - true if 3-axis magnetometer measurement are being fused
 		uint16_t mag_dec     : 1; // 6 - true if synthetic magnetic declination measurements are being fused
 		uint16_t in_air      : 1; // 7 - true when the vehicle is airborne
-		uint16_t armed       : 1; // 8 - true when the vehicle motors are armed
-		uint16_t wind        : 1; // 9 - true when wind velocity is being estimated
-		uint16_t baro_hgt    : 1; // 10 - true when baro height is being fused as a primary height reference
-		uint16_t rng_hgt     : 1; // 11 - true when range finder height is being fused as a primary height reference
-		uint16_t gps_hgt     : 1; // 12 - true when range finder height is being fused as a primary height reference
+		uint16_t wind        : 1; // 8 - true when wind velocity is being estimated
+		uint16_t baro_hgt    : 1; // 9 - true when baro height is being fused as a primary height reference
+		uint16_t rng_hgt     : 1; // 10 - true when range finder height is being fused as a primary height reference
+		uint16_t gps_hgt     : 1; // 11 - true when range finder height is being fused as a primary height reference
 	} flags;
 	uint16_t value;
 };

EKF/control.cpp

@@ -45,8 +45,6 @@ void Ekf::controlFusionModes()
 {
 	// Store the status to enable change detection
 	_control_status_prev.value = _control_status.value;
-	// Determine the vehicle status
-	calculateVehicleStatus();
 
 	// Get the magnetic declination
 	calcMagDeclination();
@@ -109,7 +107,7 @@ void Ekf::controlFusionModes()
 				// reset the horizontal velocity variance using the optical flow noise variance
 				P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
 
-				if (!_in_air) {
+				if (!_control_status.flags.in_air) {
 					// we are likely starting OF for the first time so reset the horizontal position and vertical velocity states
 					_state.pos(0) = 0.0f;
 					_state.pos(1) = 0.0f;
@@ -370,29 +368,23 @@ void Ekf::controlFusionModes()
 	// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
 	// or the more accurate 3-axis fusion
 	if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
-		if (!_control_status.flags.armed) {
-			// use heading fusion for initial startup
-			_control_status.flags.mag_hdg = true;
-			_control_status.flags.mag_3D = false;
-
-		} else {
-			if (_control_status.flags.in_air) {
-				// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
-				if (!_control_status.flags.mag_3D) {
-					_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
-				}
-
-				// use 3D mag fusion when airborne
-				_control_status.flags.mag_hdg = false;
-				_control_status.flags.mag_3D = true;
 
-			} else {
-				// use heading fusion when on the ground
-				_control_status.flags.mag_hdg = true;
-				_control_status.flags.mag_3D = false;
-			}
+	if (_control_status.flags.in_air) {
+		// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
+		if (!_control_status.flags.mag_3D) {
+			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
 		}
 
+		// use 3D mag fusion when airborne
+		_control_status.flags.mag_hdg = false;
+		_control_status.flags.mag_3D = true;
+
+	} else {
+		// use heading fusion when on the ground
+		_control_status.flags.mag_hdg = true;
+		_control_status.flags.mag_3D = false;
+	}
+
 	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
 		// always use heading fusion
 		_control_status.flags.mag_hdg = true;
@@ -448,26 +440,3 @@ void Ekf::controlFusionModes()
 	}
 
 }
-
-void Ekf::calculateVehicleStatus()
-{
-	// determine if the vehicle is armed
-	_control_status.flags.armed = _vehicle_armed;
-
-	// record vertical position whilst disarmed to use as a height change reference
-	if (!_control_status.flags.armed) {
-		_last_disarmed_posD = _state.pos(2);
-	}
-
-	// Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming
-	bool in_air = _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f;
-
-	if (!_control_status.flags.in_air && in_air) {
-		_control_status.flags.in_air = true;
-	}
-
-	// Transition to on-ground occurs when disarmed or if the land detector indicated landed state
-	if (_control_status.flags.in_air && (!_control_status.flags.armed || !_in_air)) {
-		_control_status.flags.in_air = false;
-	}
-}

EKF/ekf.cpp

@@ -253,7 +253,7 @@ bool Ekf::update()
 		} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
 			// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
 			// and are on the ground, then synthesise a measurement at the expected on ground value
-			if (!_in_air) {
+			if (!_control_status.flags.in_air) {
 				_range_sample_delayed.rng = _params.rng_gnd_clearance;
 				_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
 

EKF/ekf.h

@@ -332,9 +332,6 @@ private:
 	// Control the filter fusion modes
 	void controlFusionModes();
 
-	// Determine if we are airborne or motors are armed
-	void calculateVehicleStatus();
-
 	// return the square of two floating point numbers - used in auto coded sections
 	inline float sq(float var)
 	{

EKF/estimator_interface.cpp

@@ -52,8 +52,6 @@ EstimatorInterface::EstimatorInterface():
 	_imu_ticks(0),
 	_imu_updated(false),
 	_initialised(false),
-	_vehicle_armed(false),
-	_in_air(false),
 	_NED_origin_initialised(false),
 	_gps_speed_valid(false),
 	_gps_origin_eph(0.0f),

EKF/estimator_interface.h

@@ -149,11 +149,8 @@ public:
 	// in order to give access to the application
 	parameters *getParamHandle() {return &_params;}
 
-	// set vehicle arm status data
-	void set_arm_status(bool data) { _vehicle_armed = data; }
-
 	// set vehicle landed status data
-	void set_in_air_status(bool in_air) {_in_air = in_air;}
+	void set_in_air_status(bool in_air) {_control_status.flags.in_air = in_air;}
 
 	// return true if the global position estimate is valid
 	virtual bool global_position_is_valid() = 0;
@@ -258,8 +255,6 @@ protected:
 
 	bool _imu_updated;      // true if the ekf should update (completed downsampling process)
 	bool _initialised;      // true if the ekf interface instance (data buffering) is initialized
-	bool _vehicle_armed;    // vehicle arm status used to turn off functionality used on the ground
-	bool _in_air;           // we assume vehicle is in the air, set by the given landing detector
 
 	bool _NED_origin_initialised = false;
 	bool _gps_speed_valid = false;

EKF/gps_checks.cpp

@@ -162,7 +162,7 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 
 	// Calculate the horizontal drift speed and fail if too high
 	// This check can only be used if the vehicle is stationary during alignment
-	if (!_control_status.flags.armed) {
+	if (!_control_status.flags.in_air) {
 		float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE);
 		_gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift);
 
@@ -186,7 +186,7 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 
 	// Fail if the vertical drift speed is too high
 	// This check can only be used if the vehicle is stationary during alignment
-	if (!_control_status.flags.armed) {
+	if (!_control_status.flags.in_air) {
 		_gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift);
 
 	} else {
@@ -195,7 +195,7 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 
 	// Check the magnitude of the filtered horizontal GPS velocity
 	// This check can only be used if the vehicle is stationary during alignment
-	if (!_control_status.flags.armed) {
+	if (!_control_status.flags.in_air) {
 		vel_limit = 10.0f * _params.req_hdrift;
 		float gps_velN = fminf(fmaxf(gps->vel_ned[0], -vel_limit), vel_limit);
 		float gps_velE = fminf(fmaxf(gps->vel_ned[1], -vel_limit), vel_limit);

EKF/terrain_estimator.cpp

@@ -55,7 +55,7 @@ bool Ekf::initHagl()
 		// success
 		return true;
 
-	} else if (!_in_air) {
+	} else if (!_control_status.flags.in_air) {
 		// if on ground we assume a ground clearance
 		_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
 		// Use the ground clearance value as our uncertainty