EKF: publish control limits for optical flow navigation

EKF/ekf.h

@@ -138,6 +138,13 @@ public:
 	// get the 1-sigma horizontal and vertical velocity uncertainty
 	void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning);
 
+	/*
+	Returns the following vehicle control limits required by the estimator.
+	vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required.
+	tilt_rate_max : maximum allowed tilt rate against the direction of travel (rad/sec). NaN when no limiting required.
+	*/
+	void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl);
+
 	void get_vel_var(Vector3f &vel_var);
 
 	void get_pos_var(Vector3f &pos_var);

EKF/ekf_helper.cpp

@@ -1000,6 +1000,36 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon
 	memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool));
 }
 
+/*
+Returns the following vehicle control limits required by the estimator.
+vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required.
+limit_hagl : Boolean true when height above ground needs to be controlled to remain between optical flow focus and rang efinder max range limits.
+*/
+void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl)
+{
+	float flow_gnd_spd_max;
+	bool flow_limit_hagl;
+
+	// If relying on optical flow for navigation we need to keep within flow and range sensor limits
+	bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos);
+	if (relying_on_optical_flow) {
+		// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
+		flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2));
+		flow_gnd_spd_max = fmaxf(flow_gnd_spd_max , 0.0f);
+
+		flow_limit_hagl = true;
+
+	} else {
+		flow_gnd_spd_max = NAN;
+		flow_limit_hagl = false;
+
+	}
+
+	memcpy(vxy_max, &flow_gnd_spd_max, sizeof(float));
+	memcpy(limit_hagl, &flow_limit_hagl, sizeof(bool));
+
+}
+
 // get EKF innovation consistency check status information comprising of:
 // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
 // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.

EKF/estimator_interface.h

@@ -148,6 +148,13 @@ public:
 	// get the 1-sigma horizontal and vertical velocity uncertainty
 	virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning) = 0;
 
+	/*
+	Returns the following vehicle control limits required by the estimator.
+	vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required.
+	limit_hagl : Boolean true when height above ground needs to be controlled to remain between optical flow focus and rang efinder max range limits.
+	*/
+	virtual void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) = 0;
+
 	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
 	virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; }