Navigator: Increase headroom in stack

- require being inside both fences, not either

README.md

@@ -59,7 +59,7 @@ This repository contains code supporting these boards:
   * STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery)
   * MindPX V2.8 [Tutorial] (http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
   * MindRacer V1.2 [Tutorial] (http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
-  
+
 ## Project Milestones
 
 The PX4 software and Pixhawk hardware (which has been designed for it) has been created in 2011 by Lorenz Meier.

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -19,8 +19,14 @@ fi
 # LPE
 if param compare SYS_MC_EST_GROUP 1
 then
-	attitude_estimator_q start
-	local_position_estimator start
+	# Try to start LPE. If it fails, start EKF2 as a default
+	# Unfortunately we do not build it on px4fmu-v2 duo to a limited flash.
+	if attitude_estimator_q start
+	then
+		local_position_estimator start
+	else
+		ekf2 start
+	fi
 fi
 
 # EKF

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -20,8 +20,14 @@ fi
 # LPE
 if param compare SYS_MC_EST_GROUP 1
 then
-	attitude_estimator_q start
-	local_position_estimator start
+	# Try to start LPE. If it fails, start EKF2 as a default
+	# Unfortunately we do not build it on px4fmu-v2 duo to a limited flash.
+	if attitude_estimator_q start
+	then
+		local_position_estimator start
+	else
+		ekf2 start
+	fi
 fi
 
 # EKF

cmake/configs/nuttx_px4fmu-v2_default.cmake

@@ -96,9 +96,9 @@ set(config_module_list
 	#
 	# Estimation modules
 	#
-	modules/attitude_estimator_q
+	#modules/attitude_estimator_q
 	#modules/position_estimator_inav
-	modules/local_position_estimator
+	#modules/local_position_estimator
 	modules/ekf2
 
 	#

src/drivers/device/device_nuttx.h

@@ -130,6 +130,20 @@ public:
 	 */
 	virtual int	ioctl(unsigned operation, unsigned &arg);
 
+	/**
+	 * Return the bus ID the device is connected to.
+	 *
+	 * @return The bus ID
+	 */
+	virtual uint8_t get_device_bus() {return _device_id.devid_s.bus;};
+
+	/**
+	 * Return the bus address of the device.
+	 *
+	 * @return The bus address
+	 */
+	virtual uint8_t get_device_address() {return _device_id.devid_s.address;};
+
 	/*
 	  device bus types for DEVID
 	 */

src/drivers/mpu6000/mpu6000.cpp

@@ -1696,8 +1696,13 @@ MPU6000::stop()
 	memset(_last_accel, 0, sizeof(_last_accel));
 
 	/* discard unread data in the buffers */
-	_accel_reports->flush();
-	_gyro_reports->flush();
+	if (_accel_reports != nullptr) {
+		_accel_reports->flush();
+	}
+
+	if (_gyro_reports != nullptr) {
+		_gyro_reports->flush();
+	}
 }
 
 #if defined(USE_I2C)

src/drivers/mpu9250/mpu9250.cpp

@@ -166,11 +166,17 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const
 	// disable debug() calls
 	_debug_enabled = false;
 
+	/* Set device parameters and make sure parameters of the bus device are adopted */
 	_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
+	_device_id.devid_s.bus = _interface->get_device_bus();;
+	_device_id.devid_s.address = _interface->get_device_address();;
 
 	/* Prime _gyro with parents devid. */
+	/* Set device parameters and make sure parameters of the bus device are adopted */
 	_gyro->_device_id.devid = _device_id.devid;
 	_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU9250;
+	_gyro->_device_id.devid_s.bus = _interface->get_device_bus();
+	_gyro->_device_id.devid_s.address = _interface->get_device_address();
 
 	/* Prime _mag with parents devid. */
 	_mag->_device_id.devid = _device_id.devid;

src/modules/commander/PreflightCheck.cpp

@@ -500,7 +500,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
 	}
 
 	// check accelerometer delta velocity bias estimates
-	param_get(param_find("COM_ARM_IMU_AB"), &test_limit);
+	param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
 	if (fabsf(status.states[13]) > test_limit ||  fabsf(status.states[14]) > test_limit || fabsf(status.states[15]) > test_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU ACCEL BIAS");
@@ -510,7 +510,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
 	}
 
 	// check gyro delta angle bias estimates
-	param_get(param_find("COM_ARM_IMU_GB"), &test_limit);
+	param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
 	if (fabsf(status.states[10]) > test_limit ||  fabsf(status.states[11]) > test_limit || fabsf(status.states[12]) > test_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU GYRO BIAS");

src/modules/commander/commander_params.c

@@ -527,11 +527,11 @@ PARAM_DEFINE_FLOAT(COM_ARM_EKF_YAW, 0.5f);
  * @group Commander
  * @unit m/s
  * @min 0.001
- * @max 0.004
+ * @max 0.01
  * @decimal 4
  * @increment 0.0005
  */
-PARAM_DEFINE_FLOAT(COM_ARM_EKF_AB, 2.0e-3f);
+PARAM_DEFINE_FLOAT(COM_ARM_EKF_AB, 5.0e-3f);
 
 /**
  * Maximum value of EKF gyro delta angle bias estimate that will allow arming
@@ -539,11 +539,11 @@ PARAM_DEFINE_FLOAT(COM_ARM_EKF_AB, 2.0e-3f);
  * @group Commander
  * @unit rad
  * @min 0.0001
- * @max 0.0007
+ * @max 0.0017
  * @decimal 5
- * @increment 0.00005
+ * @increment 0.0001
  */
-PARAM_DEFINE_FLOAT(COM_ARM_EKF_GB, 3.5e-4f);
+PARAM_DEFINE_FLOAT(COM_ARM_EKF_GB, 8.7e-4f);
 
 /**
  * Maximum accelerometer inconsistency between IMU units that will allow arming

src/modules/navigator/geofence.cpp

@@ -168,7 +168,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
 		}
 	}
 
-	inside_fence |= inside_polygon(lat, lon, altitude);
+	// to be inside the geofence both fences have to report being inside
+	// as they both report being inside when not enabled
+	inside_fence = inside_fence && inside_polygon(lat, lon, altitude);
 
 	if (inside_fence) {
 		_outside_counter = 0;

src/modules/navigator/navigator_main.cpp

@@ -673,7 +673,7 @@ Navigator::start()
 	_navigator_task = px4_task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT + 5,
-					 1300,
+					 1500,
 					 (px4_main_t)&Navigator::task_main_trampoline,
 					 nullptr);