Pmw3901 Optical flow sensor not working with latest master

Kyuhyong_You · November 28, 2018, 7:29am

So far I have been testing position hold using pmw3901 with firmware branch stable on both pixhawk and crazyflie board and worked just fine.

In an attempt to improve stability, decided to move on to master version of Firmware and apply same modifications.

I had to modify some code in ekf2 interface to work with pmw3901 driver as here

github.com

PX4/PX4-ECL/blob/10a0fef7b7db749f5baf3384e9c900d3aa0e8140/EKF/estimator_interface.cpp#L346

    
      
          		rangeSample range_sample_new;
          		range_sample_new.rng = data;
          		range_sample_new.time_us = time_usec - _params.range_delay_ms * 1000;
          		_time_last_range = time_usec;
          
          
		_range_buffer.push(range_sample_new);
          	}
          }
          
          
// set optical flow data
          void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow)
          {
          	if (!_initialised || _flow_buffer_fail) {
          		return;
          	}
          
          
	// Allocate the required buffer size if not previously done
          	// Do not retry if allocation has failed previously
          	if (_flow_buffer.get_length() < _obs_buffer_length) {
          		_flow_buffer_fail = !_flow_buffer.allocate(_obs_buffer_length);

I had to change this because pmw3901 has no gyro in it.

Checked pmw3901, distance sensor drivers are working.
In nsh, ekf2 has valid local position estimation
In QGC analyzer, local position is plotted.

Problem is, whenever I switch to position hold mode, the drone drift a lot and never stay on position.
Here is the log.
https://logs.px4.io/plot_app?log=8bd954fd-aa9e-4557-a991-d72b92070814

Kyuhyong_You · November 29, 2018, 2:35am

I found ecl has been changed since the last commit here https://github.com/PX4/Firmware/commit/6c02cc53deec917b58b4728cdf20b0f99d5a3848

in src/lib/ecl/EKF/common.h
flowSample has member Vector2f flowRadXYcomp

github.com

PX4/ecl/blob/3077e6ae77dc54694cd91a23a9f858005cb4beac/EKF/common.h#L142




struct airspeedSample {
	float       true_airspeed;	///< true airspeed measurement (m/sec)
	float 		eas2tas;	///< equivalent to true airspeed factor
	uint64_t    time_us;		///< timestamp of the measurement (uSec)
};


struct flowSample {
	uint8_t  quality;	///< quality indicator between 0 and 255
	Vector2f flowRadXY;	///< measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
	Vector2f flowRadXYcomp;	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
	Vector3f gyroXYZ;	///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
	float    dt;		///< amount of integration time (sec)
	uint64_t time_us;	///< timestamp of the integration period mid-point (uSec)
};


struct extVisionSample {
	Vector3f posNED;	///< measured NED position relative to the local origin (m)
	Quatf quat;		///< measured quaternion orientation defining rotation from NED to body frame
	float posErr;		///< 1-Sigma spherical position accuracy (m)
	float angErr;		///< 1-Sigma angular error (rad)

And this variable is updated in estimator_interface.cpp when new flow data is given

github.com

PX4/ecl/blob/3077e6ae77dc54694cd91a23a9f858005cb4beac/EKF/estimator_interface.cpp#L421


				// when on the ground with poor flow quality, assume zero ground relative velocity
				//optflow_sample_new.flowRadXY(0) = - flow->gyrodata(0);
				//optflow_sample_new.flowRadXY(1) = - flow->gyrodata(1);


				optflow_sample_new.flowRadXY(0) = - matching_gyro_sample(0);
				optflow_sample_new.flowRadXY(1) = - matching_gyro_sample(1);


			}


			// compensate for body motion to give a LOS rate
			optflow_sample_new.flowRadXYcomp(0) = (optflow_sample_new.flowRadXY(0) + optflow_sample_new.gyroXYZ(0)) * delta_time;
			optflow_sample_new.flowRadXYcomp(1) = (optflow_sample_new.flowRadXY(1) + optflow_sample_new.gyroXYZ(1)) * delta_time;


			optflow_sample_new.gyroXYZ(0) *= matching_imu_sample.delta_ang_dt;
			optflow_sample_new.gyroXYZ(1) *= matching_imu_sample.delta_ang_dt;


			// convert integration interval to seconds
			optflow_sample_new.dt = delta_time;
			_time_last_optflow = time_usec;


			_flow_buffer.push(optflow_sample_new);

But in the latest master I found this variable is moved to src/lib/ecl/EKF/ekf.h

github.com

PX4/ecl/blob/acde4ebcc38692213ee475c392f68ecb0b853818/EKF/ekf.h#L374




	// optical flow processing
	float _flow_innov[2] {};	///< flow measurement innovation (rad/sec)
	float _flow_innov_var[2] {};	///< flow innovation variance ((rad/sec)**2)
	Vector3f _flow_gyro_bias;	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
	Vector3f _imu_del_ang_of;	///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
	float _delta_time_of{0.0f};	///< time in sec that _imu_del_ang_of was accumulated over (sec)
	uint64_t _time_bad_motion_us{0};	///< last system time that on-ground motion exceeded limits (uSec)
	uint64_t _time_good_motion_us{0};	///< last system time that on-ground motion was within limits (uSec)
	bool _inhibit_flow_use{false};	///< true when use of optical flow and range finder is being inhibited
	Vector2f _flowRadXYcomp;	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive


	float _mag_declination{0.0f};	///< magnetic declination used by reset and fusion functions (rad)


	// output predictor states
	Vector3f _delta_angle_corr;	///< delta angle correction vector (rad)
	imuSample _imu_down_sampled{};	///< down sampled imu data (sensor rate -> filter update rate)
	Quatf _q_down_sampled;		///< down sampled quaternion (tracking delta angles between ekf update steps)
	Vector3f _vel_err_integ;	///< integral of velocity tracking error (m)
	Vector3f _pos_err_integ;	///< integral of position tracking error (m.s)
	float _output_tracking_error[3] {}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)

And it looks like flow rate compensation is done in control.h as shown here

github.com

PX4/ecl/blob/acde4ebcc38692213ee475c392f68ecb0b853818/EKF/control.cpp#L477




			bool do_reset = _time_last_imu - _time_last_of_fuse > _params.reset_timeout_max;


			if (do_reset) {
				resetVelocity();
				resetPosition();
			}
		}


		// Only fuse optical flow if valid body rate compensation data is available
		if (calcOptFlowBodyRateComp()) {


			bool flow_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);


			if (!flow_quality_good && !_control_status.flags.in_air) {
				// when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate
				_flowRadXYcomp.zero();
			} else {
				// compensate for body motion to give a LOS rate
				_flowRadXYcomp(0) = _flow_sample_delayed.flowRadXY(0) - _flow_sample_delayed.gyroXYZ(0);
				_flowRadXYcomp(1) = _flow_sample_delayed.flowRadXY(1) - _flow_sample_delayed.gyroXYZ(1);

Why flow rad compensation has changed and how to modify pwm3901 can work again?
Thanks,

Kyu