Disable LPE in px4fmu-v2_default

With GCC 4.9 the binary is to large for the flash memory.
This is why we disabled LPE on that platform.

.gitignore

@@ -77,3 +77,7 @@ vectorcontrol/
 
 # CLion ignores
 .idea
+
+# gcov code coverage
+coverage-html/
+coverage.info

Makefile

@@ -257,6 +257,13 @@ run_tests_posix: posix_sitl_default
 
 tests: check_unittest run_tests_posix
 
+tests_coverage:
+	@(PX4_CODE_COVERAGE=1 CCACHE_DISABLE=1 ${MAKE} tests)
+	@(lcov --directory . --capture --quiet --output-file coverage.info)
+	@(lcov --remove coverage.info '/usr/*' --quiet --output-file coverage.info)
+	#@(lcov --list coverage.info)
+	@(genhtml coverage.info --quiet --output-directory coverage-html)
+
 # QGroundControl flashable firmware (currently built by travis-ci)
 qgc_firmware: \
 	check_px4fmu-v1_default \

ROMFS/px4fmu_common/init.d/13010_claire

@@ -17,6 +17,7 @@ param set VT_TILT_TRANS 0.5
 param set VT_TILT_FW 0.9
 
 param set VT_MOT_COUNT 4
+param set VT_FW_MOT_OFFID 13
 param set VT_IDLE_PWM_MC 1080
 param set VT_TYPE 1
 fi

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

ROMFS/px4fmu_common/mixers/claire.aux.mix

@@ -1,4 +1,4 @@
-# mixer for the CruiseAder Claire tilt mechansim servo, aileron and elevator
+# mixer for the CruiseAder Claire tilt mechansim servo and elevons
 
 =======================================================================
 
@@ -15,24 +15,17 @@ S: 1 4  10000  10000      0 -10000  10000
 
 
 
-Aileron mixers
+Elevon mixers
 
 -------------
 
-M: 1
+M: 2
 
 O:      10000  10000      0 -10000  10000
+S: 1 0  -7500  -7500      0 -10000  10000
+S: 1 1   7500   7500      0 -10000  10000
 
-S: 1 0  10000  10000      0 -10000  10000
-
-
-
-Elevator mixers
-
--------------
-
-M: 1
-
-O:      10000  10000      0 -10000  10000
-
-S: 1 1 -10000 -10000      0 -10000  10000
+M: 2
+O:       10000   10000    0 -10000  10000
+S: 1 0   -7500   -7500    0 -10000  10000
+S: 1 1   -7500   -7500    0 -10000  10000

Tools/upload_log.py

@@ -0,0 +1,109 @@
+#!/usr/bin/env python
+
+"""
+Upload an ULog file to the logs.px4.io web server.
+
+@author: Beat Kueng (beat-kueng@gmx.net)
+"""
+
+
+from __future__ import print_function
+
+from argparse import ArgumentParser
+import subprocess
+import sys
+
+
+try:
+    import requests
+except:
+    print("Failed to import requests.")
+    print("You may need to install it with 'pip install requests'")
+    print("")
+    raise
+
+
+SERVER = 'http://logs.px4.io'
+#SERVER = 'http://localhost:5006' # for testing locally
+UPLOAD_URL = SERVER+'/upload'
+
+quiet = False
+
+def ask_value(text, default=None):
+    """ ask the user to provide a certain value """
+    if quiet:
+        return ""
+
+    ask_string = 'Enter ' + text
+    if default != None:
+        ask_string += ' (Press ENTER to use ' + default + ')'
+    ask_string += ': '
+
+    if sys.version_info[0] < 3:
+        ret = raw_input(ask_string)
+    else:
+        ret = input(ask_string)
+
+    if ret == "" and default != None:
+        return default
+    return ret
+
+def get_git_email():
+    """ get (globally) configured git email """
+    output = subprocess.check_output(["git", "config", "--global", "user.email"])
+    return output.decode("utf-8").replace('\n', '')
+
+
+def main():
+    global quiet
+    parser = ArgumentParser(description=__doc__)
+    parser.add_argument('--quiet', '-q', dest='quiet', action='store_true', default=False,
+            help='Quiet mode: do not ask for values which were not provided as parameters')
+    parser.add_argument("--description", dest="description", type=str,
+                      help="Log description", default=None)
+    parser.add_argument("--feedback", dest="feedback", type=str,
+                      help="Additional feedback", default=None)
+    parser.add_argument("--source", dest="source", type=str,
+                      help="Log source (Eg. CI)", default="webui")
+    parser.add_argument("--email", dest="email", type=str,
+                      help="Your e-mail (to send the upload link)", default=None)
+    parser.add_argument("FILE", help="ULog file(s)", nargs="+")
+    args = parser.parse_args()
+
+    # arguments
+    quiet = args.quiet
+    if args.description == None:
+        description = ask_value('Log Description')
+    else:
+        description = args.description
+
+    if args.feedback == None:
+        feedback = ask_value('Additional Feedback')
+    else:
+        feedback = args.feedback
+
+    if args.email == None:
+        default_email = get_git_email()
+        email = ask_value('Your e-mail', default_email)
+    else:
+        email = args.email
+
+    payload = {'type': 'personal', 'description': description,
+            'feedback': feedback, 'email': email}
+
+    for file_name in args.FILE:
+        print('Uploading '+file_name+'...')
+        with open(file_name, 'rb') as f:
+            r = requests.post(UPLOAD_URL, data=payload, files={'filearg': f},
+                    allow_redirects=False)
+            if r.status_code == 302: # redirect
+                if 'Location'  in r.headers:
+                    plot_url = r.headers['Location']
+                    if len(plot_url) > 0 and plot_url[0] == '/':
+                        plot_url = SERVER + plot_url
+                    print('URL: '+plot_url)
+
+
+if __name__ == '__main__':
+    main()
+

cmake/common/px4_base.cmake

@@ -827,6 +827,7 @@ function(px4_add_common_flags)
 		)
 
 	set(added_link_dirs) # none used currently
+	set(added_exe_linker_flags)
 
 	string(TOUPPER ${BOARD} board_upper)
 	string(REPLACE "-" "_" board_config ${board_upper})
@@ -848,6 +849,20 @@ function(px4_add_common_flags)
 			)
 	endif()
 
+	# code coverage
+	if ($ENV{PX4_CODE_COVERAGE} MATCHES "1")
+		message(STATUS "Code coverage build flags enabled")
+		list(APPEND added_cxx_flags
+			-fprofile-arcs -ftest-coverage --coverage -g3 -O0 -fno-elide-constructors -Wno-invalid-offsetof -fno-default-inline -fno-inline
+		)
+		list(APPEND added_c_flags
+			-fprofile-arcs -ftest-coverage --coverage -g3 -O0 -fno-default-inline -fno-inline
+		)
+		list(APPEND added_exe_linker_flags
+			-ftest-coverage --coverage -lgcov
+		)
+	endif()
+
 	# output
 	foreach(var ${inout_vars})
 		string(TOLOWER ${var} lower_var)

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/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/mc_pos_control/mc_pos_control_main.cpp

@@ -2160,11 +2160,11 @@ MulticopterPositionControl::task_main()
 					z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
 
 					// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
-					// to calculate the new desired roll and pitch angles
-					// R_tilt can be written as a function of the new desired roll and pitch
-					// angles. we get three equations and have to solve for 2 unknowns
-					_att_sp.pitch_body = asinf(z_roll_pitch_sp(0));
-					_att_sp.roll_body = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2));
+					// R_tilt is computed from_euler; only true if cos(roll) not equal zero
+					// -> valid if roll is not +-pi/2;
+					_att_sp.roll_body = -asinf(z_roll_pitch_sp(1));
+					_att_sp.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2));
+
 				}
 
 				/* copy quaternion setpoint to attitude setpoint topic */

src/modules/navigator/loiter.cpp

@@ -102,15 +102,25 @@ Loiter::on_active()
 void
 Loiter::set_loiter_position()
 {
-	// not setting loiter position until armed
-	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED ||
-	    _loiter_pos_set) {
+	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED &&
+	    _navigator->get_land_detected()->landed) {
+
+		// Not setting loiter position if disarmed and landed, instead mark the current
+		// setpoint as invalid and idle (both, just to be sure).
+
+		_navigator->set_can_loiter_at_sp(false);
+		_navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
+		_navigator->set_position_setpoint_triplet_updated();
+		_loiter_pos_set = false;
 		return;
 
-	} else {
-		_loiter_pos_set = true;
+	} else if (_loiter_pos_set) {
+		// Already set, nothing to do.
+		return;
 	}
 
+	_loiter_pos_set = true;
+
 	// set current mission item to loiter
 	set_loiter_item(&_mission_item, _param_min_alt.get());
 

src/modules/vtol_att_control/standard.cpp

@@ -358,13 +358,13 @@ void Standard::update_mc_state()
 
 	// calculate the desired pitch seen in the heading frame
 	// this value corresponds to the amount the vehicle would try to pitch forward
-	float pitch_forward = asinf(body_z_sp(0));
+	float pitch_forward = atan2f(body_z_sp(0), body_z_sp(2));
 
 	// only allow pitching forward up to threshold, the rest of the desired
 	// forward acceleration will be compensated by the pusher
 	if (pitch_forward < -_params_standard.down_pitch_max) {
 		// desired roll angle in heading frame stays the same
-		float roll_new = -atan2f(body_z_sp(1), body_z_sp(2));
+		float roll_new = -asinf(body_z_sp(1));
 
 		_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max))
 				   * _v_att_sp->thrust * _params_standard.forward_thrust_scale;
@@ -383,9 +383,9 @@ void Standard::update_mc_state()
 		tilt_new = R_yaw_correction * tilt_new;
 
 		// now extract roll and pitch setpoints
-		float pitch = asinf(tilt_new(0));
-		float roll = -atan2f(tilt_new(1), tilt_new(2));
-		R_sp = matrix::Eulerf(roll, pitch, euler_sp(2));
+		_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
+		_v_att_sp->roll_body = -asinf(tilt_new(1));
+		R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2));
 		matrix::Quatf q_sp(R_sp);
 		memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d));
 	}

unittests/CMakeLists.txt

@@ -13,11 +13,17 @@ enable_testing()
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Werror -std=gnu99 -g")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -std=gnu++0x -g -fno-exceptions -fno-rtti -fno-threadsafe-statics -DCONFIG_WCHAR_BUILTIN -D__CUSTOM_FILE_IO__")
 
+# code coverage
+if ($ENV{PX4_CODE_COVERAGE} MATCHES "1")
+	message(STATUS "Code coverage build flags enabled")
+	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fprofile-arcs -ftest-coverage --coverage")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fprofile-arcs -ftest-coverage --coverage")
+endif()
+
 if (NOT PX4_SOURCE_DIR)
 	set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/..")
 endif()
 
-
 set(GTEST_DIR ${PX4_SOURCE_DIR}/unittests/googletest)
 add_subdirectory(${GTEST_DIR})
 include_directories(${GTEST_DIR}/include)