EV height reset (#379)

* fix typo

* EKF: use baro if it was reset to baro from ev

* EKF: set vert_pos_reset if resetting to ev hgt

otherwise the position controller will not reset the setpoint -> leading to unwanted altitude changes

EKF/control.cpp

@@ -229,7 +229,7 @@ void Ekf::controlExternalVisionFusion()
 
 		// determine if we should start using the height observations
 		if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
-			// don't start using EV data unless daa is arriving frequently
+			// don't start using EV data unless data is arriving frequently
 			if (!_control_status.flags.ev_hgt && (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL)) {
 				setControlEVHeight();
 				resetHeight();
@@ -933,6 +933,21 @@ void Ekf::controlHeightFusion()
 		}
 	}
 
+	// Determine if we rely on EV height but switched to baro
+	if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
+		if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
+			// switch to baro if there was a reset to baro
+			_fuse_height = true;
+			_in_range_aid_mode = false;
+
+			// we have just switched to using baro height, we don't need to set a height sensor offset
+			// since we track a separate _baro_hgt_offset
+			if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
+				_hgt_sensor_offset = 0.0f;
+			}
+		}
+	}
+
 	// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
 	if (!_control_status.flags.baro_hgt && _baro_data_ready) {
 		float local_time_step = 1e-6f * _delta_time_baro_us;

EKF/ekf_helper.cpp

@@ -240,6 +240,8 @@ void Ekf::resetHeight()
 		int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
 		int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
 
+		vert_pos_reset = true;
+
 		if (std::abs(dt_newest) < std::abs(dt_delayed)) {
 			_state.pos(2) = ev_newest.posNED(2);
 
@@ -1592,4 +1594,3 @@ void Ekf::get_ekf2ev_quaternion(float *quat)
 		quat[i] = quat_ekf2ev(i);
 	}
 }
-