FlightTasks: hotfix guard against accessing no task

Safety guard for null pointer exception when accessing the current task
when none is running. This could only happen if the running task
was changed during the update because of command handling failing.

Note: This is fixed on master in a different, better way.

cmake/px4_add_common_flags.cmake

@@ -89,6 +89,7 @@ function(px4_add_common_flags)
 		-Wno-missing-field-initializers
 		-Wno-missing-include-dirs # TODO: fix and enable
 		-Wno-unused-parameter
+		-Wno-logical-op # to compile 1.11 release on GCC 10 see https://github.com/PX4/PX4-Matrix/pull/146
 		)
 
 	# compiler specific flags

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -41,11 +41,11 @@
 
 #include "LidarLiteI2C.h"
 
-LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
+LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t orientation, int bus_frequency,
 			   const int address) :
 	I2C(DRV_RNG_DEVTYPE_LL40LS, MODULE_NAME, bus, address, bus_frequency),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
-	_px4_rangefinder(get_device_id(), ORB_PRIO_DEFAULT, rotation)
+	_px4_rangefinder(get_device_id(), ORB_PRIO_DEFAULT, orientation)
 {
 	_px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE);
 	_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE);

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h

@@ -94,7 +94,7 @@ static constexpr uint32_t LL40LS_CONVERSION_TIMEOUT{100_ms};
 class LidarLiteI2C : public device::I2C, public I2CSPIDriver<LidarLiteI2C>
 {
 public:
-	LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
+	LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t orientation, int bus_frequency,
 		     const int address = LL40LS_BASEADDR);
 	virtual ~LidarLiteI2C();
 

src/drivers/distance_sensor/ll40ls/ll40ls.cpp

@@ -72,7 +72,7 @@ Setup/usage information: https://docs.px4.io/master/en/sensor/lidar_lite.html
 I2CSPIDriverBase *LidarLiteI2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 				      int runtime_instance)
 {
-	LidarLiteI2C* instance = new LidarLiteI2C(iterator.configuredBusOption(), iterator.bus(), cli.rotation, cli.bus_frequency);
+	LidarLiteI2C* instance = new LidarLiteI2C(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency);
 
 	if (instance == nullptr) {
 		PX4_ERR("alloc failed");
@@ -105,7 +105,7 @@ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[])
 	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
 		switch (ch) {
 		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optarg());
+			cli.orientation = (enum Rotation)atoi(cli.optarg());
 			break;
 		}
 	}

src/lib/flight_tasks/FlightTasks.hpp

@@ -150,14 +150,24 @@ public:
 	/**
 	 * Sets an external yaw handler. The active flight task can use the yaw handler to implement a different yaw control strategy.
 	 */
-	void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);}
+	void setYawHandler(WeatherVane *ext_yaw_handler)
+	{
+		if (isAnyTaskActive()) {
+			_current_task.task->setYawHandler(ext_yaw_handler);
+		}
+	}
 
 	/**
 	 *   This method will re-activate current task.
 	 */
 	void reActivate();
 
-	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp); }
+	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp)
+	{
+		if (isAnyTaskActive()) {
+			_current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp);
+		}
+	}
 
 private:
 

src/lib/rc/dsm.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -44,6 +44,7 @@
 #include <px4_platform_common/defines.h>
 
 #include <fcntl.h>
+#include <math.h>
 #include <unistd.h>
 #include <termios.h>
 #include <string.h>
@@ -53,6 +54,8 @@
 #include "common_rc.h"
 #include <drivers/drv_hrt.h>
 
+#include <include/containers/Bitset.hpp>
+
 #if defined(__PX4_NUTTX)
 #include <nuttx/arch.h>
 #define dsm_udelay(arg)    up_udelay(arg)
@@ -106,7 +109,7 @@ static uint16_t dsm_chan_count = 0;         /**< DSM channel count */
  */
 static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, uint16_t &value)
 {
-	if (raw == 0xffff) {
+	if (raw == 0 || raw == 0xffff) {
 		return false;
 	}
 
@@ -118,8 +121,26 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
 		static constexpr uint16_t MASK_1024_SXPOS = 0x03FF;
 
 		channel = (raw & MASK_1024_CHANID) >> 10; // 6 bits
-		uint16_t servo_position = (raw & MASK_1024_SXPOS); // 10 bits
-		value = servo_position * 2; // scale to be equivalent to 2048 mode
+
+		const uint16_t servo_position = (raw & MASK_1024_SXPOS); // 10 bits
+
+		if (channel > DSM_MAX_CHANNEL_COUNT) {
+			PX4_DEBUG("invalid channel: %d\n", channel);
+			return false;
+		}
+
+		// PWM_OUT = (ServoPosition x 1.166μs) + Offset
+		static constexpr uint16_t offset = 903; // microseconds
+		value = roundf(servo_position * 1.166f) + offset;
+
+		// Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1)
+		//  ±100% travel is 1102µs to 1898 µs
+		if (value < 990 || value > 2010) {
+			// if the value is unrealistic, fail the parsing entirely
+			PX4_DEBUG("channel %d invalid range %d", channel, value);
+			return false;
+		}
+
 		return true;
 
 	} else if (shift == 11) {
@@ -127,32 +148,54 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
 		//  Bits 15    Servo Phase
 		//  Bits 14-11 Channel ID
 		//  Bits 10-0  Servo Position
-		static constexpr uint16_t MASK_2048_CHANID = 0x7800;
-		static constexpr uint16_t MASK_2048_SXPOS = 0x07FF;
+
+		uint16_t servo_position = 0;
 
 		// from Spektrum Remote Receiver Interfacing Rev G Page 6
-		uint8_t chan = (raw & MASK_2048_CHANID) >> 11;
-		uint16_t servo_position = 0;
+		const bool phase = raw & (2 >> 15); // the phase is part of the X-Plus address (bit 15)
+		uint8_t chan = (raw >> 11) & 0x0F;
 
 		if (chan < 12) {
 			// Normal channels
+			static constexpr uint16_t MASK_2048_SXPOS = 0x07FF;
 			servo_position = (raw & MASK_2048_SXPOS);
 
 		} else if (chan == 12) {
 			// XPlus channels
 			chan += ((raw >> 9) & 0x03);
 
-			const bool phase = raw & (2 >> 15); // the phase is part of the X-Plus address (bit 15)
-
 			if (phase) {
 				chan += 4;
 			}
 
+			if (chan > DSM_MAX_CHANNEL_COUNT) {
+				PX4_DEBUG("invalid channel: %d\n", chan);
+				return false;
+			}
+
 			servo_position = (raw & 0x01FF) << 2;
+
+			channel = chan;
+
+		} else {
+			PX4_DEBUG("invalid channel: %d\n", chan);
+			return false;
 		}
 
 		channel = chan;
-		value = servo_position;
+
+		// PWM_OUT = (ServoPosition x 0.583μs) + Offset
+		static constexpr uint16_t offset = 903; // microseconds
+		value = roundf(servo_position * 0.583f) + offset;
+
+		// Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1)
+		//  ±100% travel is 1102µs to 1898 µs
+		if (value < 990 || value > 2010) {
+			// if the value is unrealistic, fail the parsing entirely
+			PX4_DEBUG("channel %d invalid range %d", channel, value);
+			return false;
+		}
+
 		return true;
 	}
 
@@ -180,6 +223,12 @@ dsm_guess_format(bool reset)
 		return false;
 	}
 
+	px4::Bitset<DSM_MAX_CHANNEL_COUNT> channels_found_10;
+	px4::Bitset<DSM_MAX_CHANNEL_COUNT> channels_found_11;
+
+	bool cs10_frame_valid = true;
+	bool cs11_frame_valid = true;
+
 	/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
@@ -190,15 +239,43 @@ dsm_guess_format(bool reset)
 		uint16_t value = 0;
 
 		/* if the channel decodes, remember the assigned number */
-		if (dsm_decode_channel(raw, 10, channel, value) && (channel < 31)) {
-			cs10 |= (1 << channel);
+		if (dsm_decode_channel(raw, 10, channel, value)) {
+			// invalidate entire frame (for 1024) if channel already found, no duplicate channels per DSM frame
+			if (channels_found_10[channel]) {
+				cs10_frame_valid = false;
+
+			} else {
+				channels_found_10.set(channel);
+			}
 		}
 
-		if (dsm_decode_channel(raw, 11, channel, value) && (channel < 31)) {
-			cs11 |= (1 << channel);
-		}
+		if (dsm_decode_channel(raw, 11, channel, value)) {
+			// invalidate entire frame (for 2048) if channel already found, no duplicate channels per DSM frame
+			if (channels_found_11[channel]) {
+				cs11_frame_valid = false;
 
-		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
+			} else {
+				channels_found_11.set(channel);
+			}
+		}
+	}
+
+	// add valid cs10 channels
+	if (cs10_frame_valid) {
+		for (unsigned channel = 0; channel < DSM_FRAME_CHANNELS; channel++) {
+			if (channels_found_10[channel]) {
+				cs10 |= 1 << channel;
+			}
+		}
+	}
+
+	// add valid cs11 channels
+	if (cs11_frame_valid) {
+		for (unsigned channel = 0; channel < DSM_FRAME_CHANNELS; channel++) {
+			if (channels_found_11[channel]) {
+				cs11 |= 1 << channel;
+			}
+		}
 	}
 
 	samples++;
@@ -229,8 +306,10 @@ dsm_guess_format(bool reset)
 		0x1ff,	/* 9 channels (DX9, etc.) */
 		0x3ff,	/* 10 channels (DX10) */
 		0x1fff,	/* 13 channels (DX10t) */
-		0x3fff	/* 18 channels (DX10) */
+		0xffff,	/* 16 channels */
+		0x3ffff,/* 18 channels (DX10) */
 	};
+
 	unsigned votes10 = 0;
 	unsigned votes11 = 0;
 
@@ -470,91 +549,6 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 		}
 	}
 
-	/*
-	 * The second byte indicates the protocol and frame rate. We have a
-	 * guessing state machine, so we don't need to use this. At any rate,
-	 * these are the allowable values:
-	 *
-	 * 0x01 22MS 1024 DSM2
-	 * 0x12 11MS 2048 DSM2
-	 * 0xa2 22MS 2048 DSMX
-	 * 0xb2 11MS 2048 DSMX
-	 */
-	const uint8_t system = dsm_frame[1];
-
-	switch (system) {
-	case 0x00: // SURFACE DSM1
-		// unsupported
-		PX4_DEBUG("ERROR system: SURFACE DSM1 (%X) unsupported\n", system);
-		return false;
-
-	case 0x01: // DSM2 1024 22ms
-		if (dsm_channel_shift != 10) {
-			dsm_guess_format(true);
-			return false;
-		}
-
-		break;
-
-	case 0x02: // DSM2 1024 (MC24)
-		// unsupported
-		PX4_DEBUG("ERROR system: DSM2 1024 (MC24) (%X) unsupported\n", system);
-		return false;
-
-	case 0x12: // DSM2 2048 11ms
-		if (dsm_channel_shift != 11) {
-			dsm_guess_format(true);
-			return false;
-		}
-
-		break;
-
-	case 0x23: // SURFACE DSM2 16.5ms
-		// unsupported
-		PX4_DEBUG("ERROR system: DSM2 16.5ms (%X) unsupported\n", system);
-		return false;
-
-	case 0x50: // DSM MARINE
-		// unsupported
-		PX4_DEBUG("ERROR system: DSM MARINE (%X) unsupported\n", system);
-		return false;
-
-	case 0x92: // DSMJ
-		// unsupported
-		PX4_DEBUG("ERROR system: DSMJ (%X) unsupported\n", system);
-		return false;
-
-	case 0xA2: // DSMX 22ms OR DSMR 11ms or DSMR 22ms
-		if (dsm_channel_shift != 11) {
-			dsm_guess_format(true);
-			return false;
-		}
-
-		break;
-
-	case 0xA4: // DSMR 5.5ms
-		// unsupported
-		PX4_DEBUG("ERROR system: DSMR 5.5ms (%X) unsupported\n", system);
-		return false;
-
-	case 0xAE: // NOT_BOUND
-		PX4_DEBUG("ERROR system: NOT_BOUND (%X) unsupported\n", system);
-		return false;
-
-	case 0xB2: // DSMX 11ms
-		if (dsm_channel_shift != 11) {
-			dsm_guess_format(true);
-			return false;
-		}
-
-		break;
-
-	default:
-		// ERROR
-		PX4_DEBUG("ERROR system: %X unsupported\n", system);
-		return false;
-	}
-
 	/*
 	 * Each channel is a 16-bit unsigned value containing either a 10-
 	 * or 11-bit channel value and a 4-bit channel number, shifted
@@ -563,18 +557,42 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 	 * seven channels are being transmitted.
 	 */
 
+	px4::Bitset<DSM_MAX_CHANNEL_COUNT> channels_found;
+
+	unsigned channel_decode_failures = 0;
+
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
 		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 
+		// ignore
+		if (raw == 0 || raw == 0xffff) {
+			continue;
+		}
+
 		uint8_t channel = 0;
 		uint16_t value = 0;
 
 		if (!dsm_decode_channel(raw, dsm_channel_shift, channel, value)) {
+			channel_decode_failures++;
 			continue;
 		}
 
+		// discard entire frame if at least half of it (4 channels) failed to decode
+		if (channel_decode_failures >= 4) {
+			return false;
+		}
+
+		// abort if channel already found, no duplicate channels per DSM frame
+		if (channels_found[channel]) {
+			PX4_DEBUG("duplicate channel %d\n\n", channel);
+			return false;
+
+		} else {
+			channels_found.set(channel);
+		}
+
 		/* reset bit guessing state machine if the channel index is out of bounds */
 		if (channel > DSM_MAX_CHANNEL_COUNT) {
 			dsm_guess_format(true);
@@ -614,11 +632,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 			break;
 		}
 
-		// Scaling
-		//  See Specification for Spektrum Remote Receiver Interfacing Rev G 2019 January 22
-		//  2048 Mode Scaling: PWM_OUT = (ServoPosition x 0.583μs) + Offset (903μs)
-		//  scaled integer for decent accuracy while staying efficient (0.583us ~= 1194/2048)
-		values[channel] = (value * 1194) / 2048 + 903;
+		values[channel] = value;
 	}
 
 	/*

src/lib/rc/rc_tests/RCTest.cpp

@@ -32,7 +32,7 @@ private:
 	bool crsfTest();
 	bool dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0);
 	bool dsmTest10Ch();
-	bool dsmTest12Ch();
+	bool dsmTest16Ch();
 	bool sbus2Test();
 	bool st24Test();
 	bool sumdTest();
@@ -42,7 +42,7 @@ bool RCTest::run_tests()
 {
 	ut_run_test(crsfTest);
 	ut_run_test(dsmTest10Ch);
-	ut_run_test(dsmTest12Ch);
+	ut_run_test(dsmTest16Ch);
 	ut_run_test(sbus2Test);
 	ut_run_test(st24Test);
 	ut_run_test(sumdTest);
@@ -138,12 +138,12 @@ bool RCTest::crsfTest()
 
 bool RCTest::dsmTest10Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 64, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500);
 }
 
-bool RCTest::dsmTest12Ch()
+bool RCTest::dsmTest16Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 12, 454, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 16, 3, 1500);
 }
 
 bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0)
@@ -172,7 +172,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 	unsigned dsm_frame_drops = 0;
 	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
 
-	int rate_limiter = 0;
+	int count = 0;
 	unsigned last_drop = 0;
 
 	dsm_proto_init();
@@ -192,7 +192,9 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 					&dsm_11_bit, &dsm_frame_drops, nullptr, max_channels);
 
 		if (result) {
-			ut_compare("num_values == expected_chancount", num_values, expected_chancount);
+			if (count > (16 * 10)) { // need to process enough data to have full channel count
+				ut_compare("num_values == expected_chancount", num_values, expected_chancount);
+			}
 
 			ut_test(abs((int)chan0 - (int)rc_values[0]) < 30);
 
@@ -208,13 +210,13 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 			last_drop = dsm_frame_drops;
 		}
 
-		rate_limiter++;
+		count++;
 	}
 
 	fclose(fp);
 
 	ut_test(ret == EOF);
-	PX4_INFO("drop: %d", (int)last_drop);
+	//PX4_INFO("drop: %d", (int)last_drop);
 	ut_compare("last_drop == expected_dropcount", last_drop, expected_dropcount);
 
 	return true;

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -164,7 +164,12 @@ void PositionControl::_velocityControl(const float dt)
 	// Get allowed horizontal thrust after prioritizing vertical control
 	const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
 	const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
-	float thrust_max_xy = sqrtf(thrust_max_squared - thrust_z_squared);
+	const float thrust_max_xy_squared = thrust_max_squared - thrust_z_squared;
+	float thrust_max_xy = 0;
+
+	if (thrust_max_xy_squared > 0) {
+		thrust_max_xy = sqrtf(thrust_max_xy_squared);
+	}
 
 	// Saturate thrust in horizontal direction
 	const Vector2f thrust_sp_xy(_thr_sp);