Poshold with optical flow works weird

Doolly · February 12, 2017, 11:19am

I’m using firmware ver-1.5.5 (LPE) with my pixhawk and try using optical flow
and I have same issue with

and a little difference is I sometimes can get in to poshold mode but my copter flies crazy and crash down
and one more question what is MPC_ALT_MODE?
what is the difference between ‘terrain following’ and ‘altitude following’ ??

Nicolas · February 13, 2017, 2:35am

Can you post a log? It’s hard to help otherwise.

Re MPC_ALT_MODE: “altitude following” means the vehicle will perform altitude hold (position control in the z axis) with respect to the estimated altitude, which is based on baro and/or GPS. in “terrain following” the vehicle will control its altitude such that the height above ground (based on range measurements) remains constant (unless you change the altitude setpoint of course).
If you are flying over completely flat ground, there is essentially no difference. If you are flying e.g. up a hill, “terrain following” will result in the vehicle climbing in order to keep its height above ground, altitude following" will result in the vehicle getting closer and closer to the ground.

Doolly · February 14, 2017, 9:21am

thank you for the reply and here are two logs
http://logs.uaventure.com/view/ZfYtmfYoFKbZaAZgreYU8J this is one tested outdoors and thrust ctrl had some problem, if I change in to alt_hold, copter just rises and if I get in to pos_hold than it seems to be rejected and even not rejected for few seconds, performance was not that reliable

http://logs.uaventure.com/view/U4o9Ddq46KhzikVLMmk8n5 this is one tested indoors and alt_hold was fine(no rising) but after switching to pos_hold my copter flew crazy and crashed…

Nicolas · February 14, 2017, 6:34pm

One problem that I see is that your range sensor is very noisy. Most of the time it is not actually fused into the HAGL estimate, which is fatal for flow.

You should see in the message logs if/when lidar and flow measurements begin/stop to be fused with messages like “[lpe] lidar init: mean X cm stddev Y cm”.
You can find the message log in the root directory of the SD card, the names are the timestamps of the boot-time.

You are using the sonar on the PX4FLOW board, correct? Do you have another range sensor (lidar lit/TeraRanger) at hand to try?

Doolly · February 17, 2017, 4:36am

2016_11_16_10_26_43: [lpe] Sonar detected with ID 0
2016_11_16_10_26_43: [lpe] land init
2016_11_16_10_26_43: [lpe] baro init -131 m std 28 cm
2016_11_16_10_26_44: [lpe] z resume
2016_11_16_10_26_44: [lpe] tz resume
2016_11_16_10_26_59: ARMED by RC
2016_11_16_10_26_59: [blackbox] /fs/microsd/log/2016-11-16
2016_11_16_10_26_59: [blackbox] recording: 10_26_59.px4log
2016_11_16_10_27_05: [lpe] sonar timeout
2016_11_16_10_27_06: DISARMED by RC
2016_11_16_10_27_07: [blackbox] stopped (333 drops)
2016_11_16_10_27_09: [lpe] sonar init std > min
2016_11_16_10_27_13: [lpe] flow init: quality 254 std 0
2016_11_16_10_27_13: [lpe] xy resume
2016_11_16_10_27_16: [lpe] flow timeout
2016_11_16_10_27_20: [lpe] sonar timeout
2016_11_16_10_27_24: [lpe] xy timeout
2016_11_16_10_28_20: PREFLIGHT FAIL: GPS RECEIVER MISSING
2016_11_16_10_28_20: MANUAL CONTROL LOST (at t=102134ms)
2016_11_16_10_28_20: MANUAL CONTROL REGAINED after 2075ms
2016_11_16_10_28_33: ARMED by RC
2016_11_16_10_28_33: [blackbox] /fs/microsd/log/2016-11-16
2016_11_16_10_28_33: [blackbox] recording: 10_28_33.px4log
2016_11_16_10_28_35: Takeoff detected
2016_11_16_10_28_39: [lpe] sonar init std > min
2016_11_16_10_28_40: [lpe] flow init: quality 250 std 10
2016_11_16_10_28_40: [lpe] xy resume
2016_11_16_10_28_41: [lpe] sonar init std > min
2016_11_16_10_28_42: no gps
2016_11_16_10_28_42: failsafe mode on
2016_11_16_10_28_42: LOW BATTERY, RETURN TO LAND ADVISED
2016_11_16_10_28_43: [lpe] sonar init std > min
2016_11_16_10_28_44: [lpe] sonar init std > min
2016_11_16_10_28_46: [lpe] sonar init std > min
2016_11_16_10_28_48: CRITICAL BATTERY, LANDING ADVISED!
2016_11_16_10_28_48: [lpe] sonar init std > min
2016_11_16_10_28_50: [lpe] sonar init std > min
2016_11_16_10_28_52: [lpe] sonar init std > min
2016_11_16_10_28_53: [lpe] sonar init std > min
2016_11_16_10_28_54: [lpe] sonar init std > min
2016_11_16_10_28_55: failsafe mode off
2016_11_16_10_28_58: [lpe] flow timeout
2016_11_16_10_28_58: [lpe] sonar init std > min
2016_11_16_10_29_05: DISARMED by RC
2016_11_16_10_29_05: [lpe] xy timeout
2016_11_16_10_29_05: [lpe] sonar timeout
2016_11_16_10_29_05: [lpe] land fault, beta 19.71
2016_11_16_10_29_06: [blackbox] stopped (0 drops)
2016_11_16_10_29_07: Landing detected

yes I’m using my sonar on px4flow
these are the log msgs and I ordered lidar today

2016_11_16_10_28_39: [lpe] sonar init std > min
2016_11_16_10_28_40: [lpe] flow init: quality 250 std 10
2016_11_16_10_29_05: [lpe] xy timeout

can you explain what this log means? and does ‘timeout’ means it just doesn’t work?

Nicolas · February 17, 2017, 6:13pm

You can see that the height above ground estimate is re-initialized several times due to poor/invalid range measurements.

“sonar timeout” is printed here and depends on the _time_last_sonar timestamp which only gets updated if the range measurement is valid:

github.com

PX4/Firmware/blob/c28cd76e5f9fd3c10b2e682312acddb42f44e576/src/modules/local_position_estimator/sensors/sonar.cpp#L144


		Matrix<float, n_x, n_y_sonar> K =
			_P * C.transpose() * S_I;
		Vector<float, n_x> dx = K * r;
		_x += dx;
		_P -= K * C * _P;
	}
}


void BlockLocalPositionEstimator::sonarCheckTimeout()
{
	if (_timeStamp - _time_last_sonar > SONAR_TIMEOUT) {
		if (!(_sensorTimeout & SENSOR_SONAR)) {
			_sensorTimeout |= SENSOR_SONAR;
			_sonarStats.reset();
			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar timeout ");
		}
	}
}

“flow timeout” can happen for a variety of reasons, including bad height above ground estimate.

Since you do not have a GPS connected you have no position estimation anymore once flow times out, causing the “xy timeout”, and the quad goes into failsafe mode.

You could try flying over other surfaces that might be more reflective to sonar, but ultimately you’ll be better off with a light based TOF range sensor.

Doolly · February 28, 2017, 8:34am

I got lidar and I did
https://pixhawk.org/peripherals/rangefinder?s[]=lidar
these installations but I couldn’t get my lidar work
I coudn’t get lpe lidar init message and my pixhawk still uses sonar for distance sensing

I gave power from RCIN pwm port
I did check 5v comming out from aux6
I uncommented //if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) { return; }
this line from lpe sensor sonar code

by the way how do I make direct link from github code like you did??

Nicolas · February 28, 2017, 9:57am

Is the lidar working in general? I.e. do you see range measurements with only the lidar attached, flow module detached?
Do you have a lidar v3? I start mine (on a PixFalcon) with ll40ls -X start (note the -X for expansion I2C bus option).

You may have a conflict when you have two range sensor devices attached, which will both publish on the same topic. The driver that is started first will be the one that LPE ends up using.

You should be able to fix that by ensuring that the lidar driver is started first, e.g. by inserting the start command in rcS, somwhere above this line:

github.com

PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/rcS#L767


#!/bin/sh
# Un comment and use set +e to ignore and set -e to enable 'exit on error control'
set +e
# Un comment the line below to help debug scripts by printing a trace of the script commands
#set -x
# PX4FMU startup script.
#
# NOTE: environment variable references:
#    If the dollar sign ('$') is followed by a left bracket ('{') then the
#    variable name is terminated with the right bracket character ('}').
#    Otherwise, the variable name goes to the end of the argument.
#
#
# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
#
#------------------------------------------------------------------------------


#
# Set default paramter values.
# Do not add intra word spaces
# it wastes flash
#
set AUX_MODE pwm
set DATAMAN_OPT ""
set FAILSAFE none
set FAILSAFE_AUX none
set FCONFIG /fs/microsd/etc/config.txt
set FEXTRAS /fs/microsd/etc/extras.txt
set FMU_ARGS ""
set FMU_MODE pwm
set FRC /fs/microsd/etc/rc.txt
set IO_PRESENT no
set IOFW "/etc/extras/px4_io-v2_default.bin"
set IO_PRESENT no
set LOG_FILE /fs/microsd/bootlog.txt
set LOGGER_ARGS ""
set LOGGER_BUF  14
set MAV_TYPE none
set MIXER none
set MIXER_AUX none
set MIXER_FILE none
set MK_MODE none
set MKBLCTRL_ARG ""
set OUTPUT_MODE none
set PARAM_FILE /fs/microsd/params
set PWM_AUX_DISARMED p:PWM_AUX_DISARMED
set PWM_AUX_MAX p:PWM_AUX_MAX
set PWM_AUX_MIN p:PWM_AUX_MIN
set PWM_AUX_OUT none
set PWM_AUX_RATE p:PWM_AUX_RATE
set PWM_DISARMED p:PWM_DISARMED
set PWM_MAX p:PWM_MAX
set PWM_MIN p:PWM_MIN
set PWM_OUT none
set PWM_RATE p:PWM_RATE
set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers
set USE_IO no
set VEHICLE_TYPE none


# Airframe parameter versioning: airframe maintainers can set this in the
# airframe startup script, and then increase it by one whenever an airframe
# parameter is updated - it will ensure that these parameters will be updated
# when the firmware is flashed.
set PARAM_DEFAULTS_VER 1


#
# Mount the procfs.
#
mount -t procfs /proc


#
# Start CDC/ACM serial driver.
#
sercon


#
# Print full system version.
#
ver all


#
# Start the ORB (first app to start)
# tone_alarm and tune_control
# is dependent.
#
uorb start


#
# Try to mount the microSD card.
#
# REBOOTWORK this needs to start after the flight control loop.
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
	if hardfault_log check
	then
		# Error tune.
		tune_control play -t 2
		if hardfault_log commit
		then
			hardfault_log reset
		fi
	else
		# Play the startup tune.
		tune_control play -t 1
	fi
else
	# tune SD_INIT
	tune_control play -t 16
	if mkfatfs /dev/mmcsd0
	then
		if mount -t vfat /dev/mmcsd0 /fs/microsd
		then
			echo "INFO [init] card formatted"
		else
			tune_control play -t 17
			echo "ERROR [init] format failed"
			set LOG_FILE /dev/null
		fi
	else
		set LOG_FILE /dev/null
	fi
fi


# AEROCORE2 shouldn't have an sd card and CF2 may optionally have an sd card.
if ! ver hwcmp BITCRAZE_CRAZYFLIE GUMSTIX_AEROCORE2
then
	# Run no SD alarm.
	if [ $LOG_FILE = /dev/null ]
	then
		# tune Make FS MBAGP
		tune_control play -t 2
	fi
fi


#
# Look for an init script on the microSD card.
# Disable autostart if the script found.
#
if [ -f $FRC ]
then
	sh $FRC
else


	#
	# Set the parameter file if mtd starts successfully.
	#
	if mtd start
	then
		set PARAM_FILE /fs/mtd_params
	fi


	#
	# Load parameters.
	#
	param select $PARAM_FILE
	if ! param load
	then
		param reset
	fi


	#
	# Set AUTOCNF flag to use it in AUTOSTART scripts.
	#
	if param compare SYS_AUTOCONFIG 1
	then
		# Wipe out params except RC*, flight modes, total flight time, accel cal, gyro cal
		param reset_nostart RC* COM_FLTMODE* LND_FLIGHT_T_* TC_* CAL_ACC* CAL_GYRO*
		set AUTOCNF yes
	else
		if param compare SYS_AUTOCONFIG 2
		then
			set AUTOCNF yes
		else
			set AUTOCNF no
		fi
	fi


	#
	# Optional board defaults: rc.board_defaults
	#
	set BOARD_RC_DEFAULTS /etc/init.d/rc.board_defaults
	if [ -f $BOARD_RC_DEFAULTS ]
	then
		echo "Board defaults: ${BOARD_RC_DEFAULTS}"
		sh $BOARD_RC_DEFAULTS
	fi
	unset BOARD_RC_DEFAULTS


	#
	# Waypoint storage.
	# REBOOTWORK this needs to start in parallel.
	#
	dataman start $DATAMAN_OPT


	#
	# Start the socket communication send_event handler.
	#
	send_event start


	#
	# Start the resource load monitor.
	#
	load_mon start


	#
	# Start system state indicator.
	#
	rgbled start
	rgbled_ncp5623c start
	rgbled_pwm start


	if blinkm start
	then
		blinkm systemstate
	fi


	#
	# Start the tone_alarm driver.
	# Needs to be started after the parameters are loaded (for CBRK_BUZZER).
	# Note that this will still play the already published startup tone.
	#
	tone_alarm start


	if param compare SYS_FMU_TASK 1
	then
		set FMU_ARGS "-t"
	fi


	#
	# Set parameters and env variables for selected AUTOSTART.
	#
	if ! param compare SYS_AUTOSTART 0
	then
		sh /etc/init.d/rc.autostart
	fi


	#
	# Override parameters from user configuration file.
	#
	if [ -f $FCONFIG ]
	then
		echo "Custom: ${FCONFIG}"
		sh $FCONFIG
	fi


	#
	# If autoconfig parameter was set, reset it and save parameters.
	#
	if [ $AUTOCNF = yes ]
	then
		param set SYS_AUTOCONFIG 0
	fi


	#
	# Check if PX4IO present and update firmware if needed.
	# Assumption IOFW set to firmware file and IO_PRESENT = no
	#


	if [ -f $IOFW ]
	then
		# Check for the mini using build with px4io fw file
		# but not a px4IO
		if ver hwtypecmp V540
		then
			param set SYS_USE_IO 0
		else
			if px4io checkcrc ${IOFW}
			then
				set IO_PRESENT yes
			else
				# tune Program PX4IO
				tune_control play -t 18


				if px4io start
				then
					# Try to safety px4 io so motor outputs don't go crazy.
					if ! px4io safety_on
					then
						# px4io did not respond to the safety command.
						px4io stop
					fi
				fi


				if px4io forceupdate 14662 ${IOFW}
				then
					usleep 10000
					tune_control stop
					if px4io checkcrc ${IOFW}
					then
						echo "PX4IO CRC OK after updating" >> $LOG_FILE
						#tune MLL8CDE Program PX4IO success
						tune_control play -t 19
						set IO_PRESENT yes
					fi
				fi


				if [ $IO_PRESENT = no ]
				then
					echo "PX4IO update failed" >> $LOG_FILE
					# Error tune.
					tune_control play -t 20
				fi
			fi
		fi
	fi


	#
	# Set USE_IO flag.
	#
	if param compare SYS_USE_IO 1
	then
		set USE_IO yes
	fi


	if [ $USE_IO = yes -a $IO_PRESENT = no ]
	then
		echo "PX4IO not found" >> $LOG_FILE
		# Error tune.
		tune_control play -t 2
	fi


	if [ $IO_PRESENT = no -o $USE_IO = no ]
	then
		rc_input start
	fi


	#
	# Sensors System (start before Commander so Preflight checks are properly run).
	# Commander needs to be this early for in-air-restarts.
	#
	if param greater SYS_HITL 0
	then
		set OUTPUT_MODE hil
		sensors start -h
		commander start --hil
		# disable GPS
		param set GPS_1_CONFIG 0


		# start the simulator in hardware if needed
		if param compare SYS_HITL 2
		then
			sih start
		fi


	else
		#
		# board sensors: rc.sensors
		#
		set BOARD_RC_SENSORS /etc/init.d/rc.board_sensors
		if [ -f $BOARD_RC_SENSORS ]
		then
			echo "Board sensors: ${BOARD_RC_SENSORS}"
			sh $BOARD_RC_SENSORS
		fi
		unset BOARD_RC_SENSORS




		sh /etc/init.d/rc.sensors
		commander start
	fi


	# Sensors on the PWM interface bank.
	if param compare SENS_EN_LL40LS 1
	then
		# Clear pins 5 and 6.
		set FMU_MODE pwm4
		set AUX_MODE pwm4
	fi


	if param greater TRIG_MODE 0
	then
		# We ONLY support trigger on pins 5 and 6 when simultanously using AUX for actuator output.
		if param compare TRIG_PINS 56
		then
			# clear pins 5 and 6
			set FMU_MODE pwm4
			set AUX_MODE pwm4
		else
			set FMU_MODE none
			set AUX_MODE none
		fi


		camera_trigger start
		camera_feedback start
	fi


	#
	# Check if UAVCAN is enabled, default to it for ESCs.
	#
	if param greater UAVCAN_ENABLE 0
	then
		# Start core UAVCAN module.
		if uavcan start
		then
			if param greater UAVCAN_ENABLE 1
			then
				# Start UAVCAN firmware update server and dynamic node ID allocation server.
				uavcan start fw


				if param greater UAVCAN_ENABLE 2
				then
					set OUTPUT_MODE uavcan_esc
				fi
			fi
		else
			# Error tune.
			tune_control play -t 2
		fi
	fi


	#
	# Start mavlink streams that are not configurable (e.g. on USB).
	#
	sh /etc/init.d/rc.mavlink


	#
	# Start UART/Serial device drivers.
	# Note: rc.serial is auto-generated from Tools/serial/generate_config.py
	#
	sh /etc/init.d/rc.serial


	#
	# Configure vehicle type specific parameters.
	# Note: rc.vehicle_setup is the entry point for rc.interface,
	#       rc.fw_apps, rc.mc_apps, rc.ugv_apps, and rc.vtol_apps.
	#
	sh /etc/init.d/rc.vehicle_setup


	# Camera capture driver
	if param greater CAM_CAP_FBACK 0
	then
		if camera_capture start
		then
			camera_capture on
		fi
	fi


	#
	# Start the navigator.
	#
	navigator start


	#
	# Start the standalone wind estimator.
	#
	if param compare WEST_EN 1
	then
		wind_estimator start
	fi


	#
	# Start a thermal calibration if required.
	#
	sh /etc/init.d/rc.thermal_cal


	#
	# Start vmount to control mounts such as gimbals, disabled by default.
	#
	if ! param compare MNT_MODE_IN -1
	then
		vmount start
	fi


	# Check for flow sensor, launched as a background task to scan
	px4flow start &


	#
	# Optional board supplied extras: rc.board_extras
	#
	set BOARD_RC_EXTRAS /etc/init.d/rc.board_extras
	if [ -f $BOARD_RC_EXTRAS ]
	then
		echo "Board extras: ${BOARD_RC_EXTRAS}"
		sh $BOARD_RC_EXTRAS
	fi
	unset BOARD_RC_EXTRAS


	#
	# Start any custom addons from the sdcard.
	#
	if [ -f $FEXTRAS ]
	then
		echo "Addons script: ${FEXTRAS}"
		sh $FEXTRAS
	fi


	#
	# Start the logger.
	#
	sh /etc/init.d/rc.logging




	if ! param compare SYS_PARAM_VER ${PARAM_DEFAULTS_VER}
	then
		echo "Switched to different parameter version. Resetting parameters."
		param set SYS_PARAM_VER ${PARAM_DEFAULTS_VER}
		param set SYS_AUTOCONFIG 2
		param save
		reboot
	fi


#
# End of autostart.
#
fi


#
# Unset all script parameters to free RAM.
#
unset AUTOCNF
unset AUX_MODE
unset DATAMAN_OPT
unset FAILSAFE
unset FAILSAFE_AUX
unset FCONFIG
unset FEXTRAS
unset FMU_ARGS
unset FMU_MODE
unset FRC
unset IO_PRESENT
unset IOFW
unset LOG_FILE
unset LOGGER_ARGS
unset LOGGER_BUF
unset MAV_TYPE
unset MIXER
unset MIXER_AUX
unset MIXER_FILE
unset MK_MODE
unset MKBLCTRL_ARG
unset OUTPUT_MODE
unset PARAM_DEFAULTS_VER
unset PARAM_FILE
unset PWM_AUX_DISARMED
unset PWM_AUX_MAX
unset PWM_AUX_MIN
unset PWM_AUX_OUT
unset PWM_AUX_RATE
unset PWM_DISARMED
unset PWM_MAX
unset PWM_MIN
unset PWM_OUT
unset PWM_RATE
unset SDCARD_MIXERS_PATH
unset USE_IO
unset VEHICLE_TYPE


#
# Boot is complete, inform MAVLink app(s) that the system is now fully up and running.
#
mavlink boot_complete

This file has been truncated. show original

Re github link: Simply click on the line number you want to reference, this will add the line number to the URL. Paste it into discuss with a blank line above and below, discuss will then expand it.

Doolly · February 28, 2017, 10:17am

thank you so much for the reply

I think my lidar does not work in general I can’t see distance value through QGC analyze when I detach px4flow
what could be gone wrong?
yes I’m using v3 though PWM aux pin

github.com

PX4/Firmware/blob/94ce6e12c196a9c50e487945d33f40f58a600790/src/modules/local_position_estimator/sensors/sonar.cpp#L83


}


void BlockLocalPositionEstimator::sonarCorrect()
{
	// measure
	Vector<float, n_y_sonar> y;


	if (sonarMeasure(y) != OK) { return; }


	// do not use sonar if lidar is active
	//if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) { return; }


	// calculate covariance
	float cov = _sub_sonar->get().covariance;


	if (cov < 1.0e-3f) {
		// use sensor value if reasoanble
		cov = _sonar_z_stddev.get() * _sonar_z_stddev.get();
	}


	// sonar measurement matrix and noise matrix

and this is the line I uncommented to override sonar, is this not enough? or is this the line should be commented?

Nicolas · February 28, 2017, 10:24am

I have no experience with the lidar on PWM aux pin, so I can’t offer any advice.

I don’t think you need to uncomment that line. It appears to be a relic. Neither _lidarInitialized nor _lidarFault appear anywhere else in the code. LPE does not actually differentiate between different range measurement sources, it just uses whatever is on the distance_sensor topic, which is then fused in sonar.cpp.

Doolly · February 28, 2017, 12:50pm

ok I will try other way thank you Nicolas !