Navigator: Fix RTL backtransition for VTOL

A recent change removed the command forwarding required for VTOL transitions. This change brings this back.

Partially reverts https://github.com/PX4/Firmware/pull/7249

Tools/generate_listener.py

@@ -148,19 +148,20 @@ int listener_main(int argc, char *argv[]) {
 	int sub = -1;
 	orb_id_t ID;
 	if(argc < 2) {
-		printf("need at least two arguments: topic name. [optional number of messages to print]\\n");
+		printf("need at least two arguments: topic name. [optional number of messages to print] [optional instance]\\n");
 		return 1;
 	}
 """)
 print("\tunsigned num_msgs = (argc > 2) ? atoi(argv[2]) : 1;")
+print("\tunsigned topic_instance = (argc > 3) ? atoi(argv[3]) : 0;")
 print("\tif (strncmp(argv[1],\"%s\",50) == 0) {" % messages[0])
-print("\t\tsub = orb_subscribe(ORB_ID(%s));" % messages[0])
+print("\t\tsub = orb_subscribe_multi(ORB_ID(%s), topic_instance);" % messages[0])
 print("\t\tID = ORB_ID(%s);" % messages[0])
 print("\t\tstruct %s_s container;" % messages[0])
 print("\t\tmemset(&container, 0, sizeof(container));")
 for index,m in enumerate(messages[1:]):
 	print("\t} else if (strncmp(argv[1],\"%s\",50) == 0) {" % m)
-	print("\t\tsub = orb_subscribe(ORB_ID(%s));" % m)
+	print("\t\tsub = orb_subscribe_multi(ORB_ID(%s), topic_instance);" % m)
 	print("\t\tID = ORB_ID(%s);" % m)
 	print("\t\tstruct %s_s container;" % m)
 	print("\t\tmemset(&container, 0, sizeof(container));")
@@ -173,7 +174,7 @@ for index,m in enumerate(messages[1:]):
 	print("\t\t\tif (updated) {")
 	print("\t\t\tstart_time = hrt_absolute_time();")
 	print("\t\t\ti++;")
-	print("\t\t\tprintf(\"\\nTOPIC: %s #%%d\\n\", i);" % m)
+	print("\t\t\tprintf(\"\\nTOPIC: %s instance %%d #%%d\\n\", topic_instance, i);" % m)
 	print("\t\t\torb_copy(ID,sub,&container);")
 	print("\t\t\tprintf(\"timestamp: %\" PRIu64 \"\\n\", container.timestamp);")
 	for item in message_elements[index+1]:

nuttx-configs/px4fmu-v4/nsh/defconfig

@@ -761,7 +761,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1800
 # Stack and heap information
 #
 CONFIG_IDLETHREAD_STACKSIZE=500
-CONFIG_USERMAIN_STACKSIZE=2500
+CONFIG_USERMAIN_STACKSIZE=2600
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 # CONFIG_LIB_SYSCALL is not set

src/drivers/bmi055/bmi055_accel.cpp

@@ -800,6 +800,7 @@ BMI055_accel::measure()
 
 	arb.temperature_raw = report.temp;
 	arb.temperature = _last_temperature;
+	arb.device_id = _device_id.devid;
 
 	_accel_reports->force(&arb);
 

src/drivers/bmi055/bmi055_gyro.cpp

@@ -184,7 +184,7 @@ BMI055_gyro::probe()
 		return OK;
 	}
 
-	DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami);
+	PX4_ERR("unexpected whoami 0x%02x", _whoami);
 	return -EIO;
 }
 
@@ -768,6 +768,7 @@ BMI055_gyro::measure()
 
 	grb.temperature_raw = report.temp;
 	grb.temperature = _last_temperature;
+	grb.device_id = _device_id.devid;
 
 	_gyro_reports->force(&grb);
 

src/modules/commander/state_machine_helper.cpp

@@ -1044,7 +1044,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
 		}
 	}
 
-	if (battery->warning == battery_status_s::BATTERY_WARNING_LOW) {
+	if (battery->warning >= battery_status_s::BATTERY_WARNING_LOW) {
 		preflight_ok = false;
 
 		if (reportFailures) {

src/modules/mavlink/mavlink_messages.cpp

@@ -1452,9 +1452,9 @@ protected:
 				msg_cmd.target_component = MAV_COMP_ID_CAMERA;
 				msg_cmd.command = MAV_CMD_IMAGE_START_CAPTURE;
 				msg_cmd.confirmation = 0;
-				msg_cmd.param1 = 0; // duration between 2 consecutive images (seconds)
-				msg_cmd.param2 = 1; // take 1 picture
-				msg_cmd.param3 = -1; // resolution (use the highest possible)
+				msg_cmd.param1 = 0; // all cameras
+				msg_cmd.param2 = 0; // duration 0 because only taking one picture
+				msg_cmd.param3 = 1; // only take one
 				msg_cmd.param4 = NAN;
 				msg_cmd.param5 = NAN;
 				msg_cmd.param6 = NAN;

src/modules/navigator/rtl.cpp

@@ -300,6 +300,11 @@ RTL::set_rtl_item()
 
 	reset_mission_item_reached();
 
+	/* execute command if set. This is required for commands like VTOL transition */
+	if (!item_contains_position(&_mission_item)) {
+		issue_command(&_mission_item);
+	}
+
 	/* convert mission item to current position setpoint and make it valid */
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;

src/modules/sensors/parameters.cpp

@@ -166,9 +166,11 @@ int initialize_parameter_handles(ParameterHandles &parameter_handles)
 	(void)param_find("CAL_MAG0_ID");
 	(void)param_find("CAL_MAG1_ID");
 	(void)param_find("CAL_MAG2_ID");
+	(void)param_find("CAL_MAG3_ID");
 	(void)param_find("CAL_MAG0_ROT");
 	(void)param_find("CAL_MAG1_ROT");
 	(void)param_find("CAL_MAG2_ROT");
+	(void)param_find("CAL_MAG3_ROT");
 	(void)param_find("CAL_MAG_SIDES");
 
 	(void)param_find("CAL_MAG1_XOFF");
@@ -185,6 +187,13 @@ int initialize_parameter_handles(ParameterHandles &parameter_handles)
 	(void)param_find("CAL_MAG2_ZOFF");
 	(void)param_find("CAL_MAG2_ZSCALE");
 
+	(void)param_find("CAL_MAG3_XOFF");
+	(void)param_find("CAL_MAG3_XSCALE");
+	(void)param_find("CAL_MAG3_YOFF");
+	(void)param_find("CAL_MAG3_YSCALE");
+	(void)param_find("CAL_MAG3_ZOFF");
+	(void)param_find("CAL_MAG3_ZSCALE");
+
 	(void)param_find("CAL_GYRO1_XOFF");
 	(void)param_find("CAL_GYRO1_XSCALE");
 	(void)param_find("CAL_GYRO1_YOFF");

src/modules/sensors/voted_sensors_update.cpp

@@ -230,9 +230,9 @@ void VotedSensorsUpdate::parameters_update()
 	unsigned accel_cal_found_count = 0;
 
 	/* run through all gyro sensors */
-	for (unsigned s = 0; s < GYRO_COUNT_MAX; s++) {
+	for (unsigned driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) {
 
-		(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
+		(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, driver_index);
 
 		DevHandle h;
 		DevMgr::getHandle(str, h);
@@ -258,7 +258,7 @@ void VotedSensorsUpdate::parameters_update()
 				continue;
 			}
 
-			if (s == 0 && device_id > 0) {
+			if (driver_index == 0 && device_id > 0) {
 				gyro_cal_found_count++;
 			}
 
@@ -313,9 +313,9 @@ void VotedSensorsUpdate::parameters_update()
 	}
 
 	/* run through all accel sensors */
-	for (unsigned s = 0; s < ACCEL_COUNT_MAX; s++) {
+	for (unsigned driver_index = 0; driver_index < ACCEL_COUNT_MAX; driver_index++) {
 
-		(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
+		(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, driver_index);
 
 		DevHandle h;
 		DevMgr::getHandle(str, h);
@@ -341,7 +341,7 @@ void VotedSensorsUpdate::parameters_update()
 				continue;
 			}
 
-			if (s == 0 && device_id > 0) {
+			if (driver_index == 0 && device_id > 0) {
 				accel_cal_found_count++;
 			}
 
@@ -396,15 +396,9 @@ void VotedSensorsUpdate::parameters_update()
 	}
 
 	/* run through all mag sensors */
-	for (unsigned s = 0; s < MAG_COUNT_MAX; s++) {
+	for (unsigned driver_index = 0; driver_index < MAG_COUNT_MAX; driver_index++) {
 
-		/* set a valid default rotation (same as board).
-		 * if the mag is configured, this might be replaced
-		 * in the section below.
-		 */
-		_mag_rotation[s] = _board_rotation;
-
-		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
+		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, driver_index);
 
 		DevHandle h;
 		DevMgr::getHandle(str, h);
@@ -414,7 +408,7 @@ void VotedSensorsUpdate::parameters_update()
 			continue;
 		}
 
-		_mag_device_id[s] = h.ioctl(DEVIOCGDEVICEID, 0);
+		_mag_device_id[driver_index] = h.ioctl(DEVIOCGDEVICEID, 0);
 		bool config_ok = false;
 
 		/* run through all stored calibrations */
@@ -425,8 +419,6 @@ void VotedSensorsUpdate::parameters_update()
 			(void)sprintf(str, "CAL_MAG%u_ID", i);
 			int device_id;
 			failed = failed || (OK != param_get(param_find(str), &device_id));
-			(void)sprintf(str, "CAL_MAG%u_ROT", i);
-			(void)param_find(str);
 
 			if (failed) {
 				DevMgr::releaseHandle(h);
@@ -434,7 +426,7 @@ void VotedSensorsUpdate::parameters_update()
 			}
 
 			/* if the calibration is for this device, apply it */
-			if (device_id == _mag_device_id[s]) {
+			if (device_id == _mag_device_id[driver_index]) {
 				struct mag_calibration_s mscale = {};
 				(void)sprintf(str, "CAL_MAG%u_XOFF", i);
 				failed = failed || (OK != param_get(param_find(str), &mscale.x_offset));
@@ -452,9 +444,7 @@ void VotedSensorsUpdate::parameters_update()
 				(void)sprintf(str, "CAL_MAG%u_ROT", i);
 
 				if (h.ioctl(MAGIOCGEXTERNAL, 0) <= 0) {
-					/* mag is internal */
-					_mag_rotation[s] = _board_rotation;
-					/* reset param to -1 to indicate internal mag */
+					/* mag is internal - reset param to -1 to indicate internal mag */
 					int32_t minus_one;
 					param_get(param_find(str), &minus_one);
 
@@ -464,11 +454,11 @@ void VotedSensorsUpdate::parameters_update()
 					}
 
 				} else {
-
+					/* mag is external */
 					int32_t mag_rot;
 					param_get(param_find(str), &mag_rot);
 
-					/* check if this mag is still set as internal */
+					/* check if this mag is still set as internal, otherwise leave untouched */
 					if (mag_rot < 0) {
 						/* it was marked as internal, change to external with no rotation */
 						mag_rot = 0;
@@ -732,19 +722,33 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
 
 			// First publication with data
 			if (_mag.priority[uorb_index] == 0) {
+
+				// Parameters update to get offsets and scaling loaded (if not already loaded)
+				parameters_update();
+
+				// Set device priority for the voter
 				int32_t priority = 0;
 				orb_priority(_mag.subscription[uorb_index], &priority);
 				_mag.priority[uorb_index] = (uint8_t)priority;
 
-				// match rotation parameters to uORB topics first time we get data
+				// Match rotation parameters to uORB topics first time we get data
 				char str[30];
 				int32_t mag_rot;
 
 				for (int driver_index = 0; driver_index < MAG_COUNT_MAX; driver_index++) {
 					if (mag_report.device_id == _mag_device_id[driver_index]) {
+
 						(void)sprintf(str, "CAL_MAG%u_ROT", driver_index);
 						param_get(param_find(str), &mag_rot);
-						get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[uorb_index]);
+
+						if (mag_rot < 0) {
+							// Set internal magnetometers to use the board rotation
+							_mag_rotation[uorb_index] = _board_rotation;
+
+						} else {
+							// Set external magnetometers to use the parameter value
+							get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[uorb_index]);
+						}
 					}
 				}
 			}

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -334,7 +334,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 
 			} else {
 				yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
-					       roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale;
+					       roll_pitch_scale + (thrust - thrust_reduction) + boost)) / _rotors[i].yaw_scale;
 			}
 		}
 	}