FlightTaskAuto: Draft impülementation to command gimbal to neutral position when entering crawl speed at landing

Note: This would douplicate some commands issued by navigator in Land and RTL. We have to check what cases are really not covered.

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -37,6 +37,7 @@
 #include "FlightTaskAuto.hpp"
 #include <mathlib/mathlib.h>
 #include <float.h>
+#include <uORB/topics/gimbal_manager_set_attitude.h>
 
 using namespace matrix;
 
@@ -235,7 +236,12 @@ void FlightTaskAuto::_prepareLandSetpoints()
 	bool range_dist_available = PX4_ISFINITE(_dist_to_bottom);
 
 	if (range_dist_available && _dist_to_bottom <= _param_mpc_land_alt3.get()) {
-		vertical_speed = _param_mpc_land_crawl_speed.get();
+		commandGimbalNeutral();
+		_was_gimbal_commanded_neutral = true;
+		vertical_speed = _param_mpc_land_crwl.get();
+
+	} else {
+		_was_gimbal_commanded_neutral = false;
 	}
 
 	if (_type_previous != WaypointType::land) {
@@ -850,3 +856,44 @@ void FlightTaskAuto::updateParams()
 	// make sure that alt1 is above alt2
 	_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
 }
+
+void FlightTaskAuto::commandGimbalNeutral()
+{
+	vehicle_command_s vehicle_command{};
+	vehicle_command.source_system = _sub_vehicle_status.get().system_id;
+	vehicle_command.source_component = _sub_vehicle_status.get().component_id;
+	vehicle_command.target_system = _sub_vehicle_status.get().system_id;
+	vehicle_command.target_component = 0;
+	vehicle_command.confirmation = false;
+	vehicle_command.from_external = false;
+
+	// Take control
+	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
+	vehicle_command.param1 = _sub_vehicle_status.get().system_id; // Take primary control
+	vehicle_command.param2 = _sub_vehicle_status.get().component_id;
+	vehicle_command.param3 = -1.f; // Leave secondary control unchanged
+	vehicle_command.param4 = -1.f;
+	vehicle_command.timestamp = hrt_absolute_time();
+	_vehicle_command_pub.publish(vehicle_command);
+
+	// Command neutral position
+	vehicle_command = {};
+	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
+	vehicle_command.param1 = NAN; // Don't set any angles
+	vehicle_command.param2 = NAN;
+	vehicle_command.param3 = NAN; // Don't set any angular velocities
+	vehicle_command.param4 = NAN;
+	vehicle_command.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL;
+	vehicle_command.timestamp = hrt_absolute_time();
+	_vehicle_command_pub.publish(vehicle_command);
+
+	// Release control
+	vehicle_command = {};
+	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
+	vehicle_command.param1 = -3.f; // Remove primary control if it was taken
+	vehicle_command.param2 = -3.f;
+	vehicle_command.param3 = -1.f; // Leave secondary control unchanged
+	vehicle_command.param4 = -1.f;
+	vehicle_command.timestamp = hrt_absolute_time();
+	_vehicle_command_pub.publish(vehicle_command);
+}

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -132,6 +132,7 @@ protected:
 
 	uORB::SubscriptionData<home_position_s>			_sub_home_position{ORB_ID(home_position)};
 	uORB::SubscriptionData<vehicle_status_s>		_sub_vehicle_status{ORB_ID(vehicle_status)};
+	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 
 	State _current_state{State::none};
 	float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */
@@ -165,20 +166,16 @@ protected:
 					(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
 					(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
-					(ParamFloat<px4::params::MPC_LAND_CRWL>) _param_mpc_land_crawl_speed,
+					(ParamFloat<px4::params::MPC_LAND_CRWL>) _param_mpc_land_crwl,
 					(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
 					(ParamFloat<px4::params::MPC_LAND_RADIUS>) _param_mpc_land_radius,
-					(ParamFloat<px4::params::MPC_LAND_ALT1>)
-					_param_mpc_land_alt1, // altitude at which we start ramping down speed
-					(ParamFloat<px4::params::MPC_LAND_ALT2>)
-					_param_mpc_land_alt2, // altitude at which we descend at land speed
-					(ParamFloat<px4::params::MPC_LAND_ALT3>)
-					_param_mpc_land_alt3, // altitude where we switch to crawl speed, if LIDAR available
+					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1,
+					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
+					(ParamFloat<px4::params::MPC_LAND_ALT3>) _param_mpc_land_alt3,
 					(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,
 					(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
 					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
-					(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
-					_param_mpc_tko_ramp_t // time constant for smooth takeoff ramp
+					(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t
 				       );
 
 private:
@@ -211,4 +208,7 @@ private:
 	bool _evaluateGlobalReference(); /**< Check is global reference is available. */
 	State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */
 	void _set_heading_from_mode(); /**< @see  MPC_YAW_MODE */
+
+	void commandGimbalNeutral();
+	bool _was_gimbal_commanded_neutral{false};
 };

src/modules/mc_pos_control/multicopter_takeoff_land_params.c

@@ -89,9 +89,8 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.f);
 /**
  * Altitude for 3. step of slow landing
  *
- * Below this altitude descending velocity gets
- * limited to "MPC_LAND_CRWL", if LIDAR available.
- * No effect if LIDAR not available
+ * If a valid distance sensor measurement to the ground is available,
+ * limit descending velocity to "MPC_LAND_CRWL" below this altitude.
  *
  * @unit m
  * @min 0

src/modules/navigator/land.cpp

@@ -109,17 +109,16 @@ Land::on_active()
 	if (_navigator->abort_landing()) {
 
 		// send reposition cmd to get out of land mode (will loiter at current position and altitude)
-		vehicle_command_s vcmd = {};
-
-		vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
-		vcmd.param1 = -1;
-		vcmd.param2 = 1;
-		vcmd.param5 = _navigator->get_global_position()->lat;
-		vcmd.param6 = _navigator->get_global_position()->lon;
+		vehicle_command_s vehicle_command{};
+		vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
+		vehicle_command.param1 = -1.f;
+		vehicle_command.param2 = 1.f;
+		vehicle_command.param5 = _navigator->get_global_position()->lat;
+		vehicle_command.param6 = _navigator->get_global_position()->lon;
 		// as we don't know the landing point altitude assume the worst case (abort at 0m above ground),
 		// and thus always climb MIS_LND_ABRT_ALT
-		vcmd.param7 = _navigator->get_global_position()->alt + _navigator->get_landing_abort_min_alt();
+		vehicle_command.param7 = _navigator->get_global_position()->alt + _navigator->get_landing_abort_min_alt();
 
-		_navigator->publish_vehicle_cmd(&vcmd);
+		_navigator->publish_vehicle_cmd(vehicle_command);
 	}
 }

src/modules/navigator/mission_base.cpp

@@ -915,16 +915,14 @@ MissionBase::do_abort_landing()
 	}
 
 	// send reposition cmd to get out of mission
-	vehicle_command_s vcmd = {};
-
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
-	vcmd.param1 = -1;
-	vcmd.param2 = 1;
-	vcmd.param5 = _mission_item.lat;
-	vcmd.param6 = _mission_item.lon;
-	vcmd.param7 = alt_sp;
-
-	_navigator->publish_vehicle_cmd(&vcmd);
+	vehicle_command_s vehicle_command{};
+	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
+	vehicle_command.param1 = -1.f;
+	vehicle_command.param2 = 1.f;
+	vehicle_command.param5 = _mission_item.lat;
+	vehicle_command.param6 = _mission_item.lon;
+	vehicle_command.param7 = alt_sp;
+	_navigator->publish_vehicle_cmd(vehicle_command);
 }
 
 void MissionBase::publish_navigator_mission_item()

src/modules/navigator/mission_block.cpp

@@ -529,27 +529,27 @@ MissionBlock::issue_command(const mission_item_s &item)
 
 	// Mission item's NAV_CMD enums directly map to the according vehicle command
 	// So set the raw value directly (MAV_FRAME_MISSION mission item)
-	vehicle_command_s vcmd = {};
-	vcmd.command = item.nav_cmd;
-	vcmd.param1 = item.params[0];
-	vcmd.param2 = item.params[1];
-	vcmd.param3 = item.params[2];
-	vcmd.param4 = item.params[3];
-	vcmd.param5 = static_cast<double>(item.params[4]);
-	vcmd.param6 = static_cast<double>(item.params[5]);
-	vcmd.param7 = item.params[6];
+	vehicle_command_s vehicle_command{};
+	vehicle_command.command = item.nav_cmd;
+	vehicle_command.param1 = item.params[0];
+	vehicle_command.param2 = item.params[1];
+	vehicle_command.param3 = item.params[2];
+	vehicle_command.param4 = item.params[3];
+	vehicle_command.param5 = static_cast<double>(item.params[4]);
+	vehicle_command.param6 = static_cast<double>(item.params[5]);
+	vehicle_command.param7 = item.params[6];
 
 	if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) {
 		// We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision
-		vcmd.param5 = item.lat;
-		vcmd.param6 = item.lon;
+		vehicle_command.param5 = item.lat;
+		vehicle_command.param6 = item.lon;
 
 		if (item.altitude_is_relative) {
-			vcmd.param7 = item.altitude + _navigator->get_home_position()->alt;
+			vehicle_command.param7 = item.altitude + _navigator->get_home_position()->alt;
 		}
 	}
 
-	_navigator->publish_vehicle_cmd(&vcmd);
+	_navigator->publish_vehicle_cmd(vehicle_command);
 
 	if (item_has_timeout(item)) {
 		_timestamp_command_timeout = hrt_absolute_time();
@@ -781,11 +781,11 @@ MissionBlock::set_land_item(struct mission_item_s *item)
 	/* VTOL transition to RW before landing */
 	if (_navigator->force_vtol()) {
 
-		vehicle_command_s vcmd = {};
-		vcmd.command = NAV_CMD_DO_VTOL_TRANSITION;
-		vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
-		vcmd.param2 = 0.0f;
-		_navigator->publish_vehicle_cmd(&vcmd);
+		vehicle_command_s vehicle_command{};
+		vehicle_command.command = NAV_CMD_DO_VTOL_TRANSITION;
+		vehicle_command.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+		vehicle_command.param2 = 0.f;
+		_navigator->publish_vehicle_cmd(vehicle_command);
 	}
 
 	/* set the land item */

src/modules/navigator/navigator.h

@@ -132,12 +132,12 @@ public:
 	/**
 	 * @brief Publish a given specified vehicle command
 	 *
-	 * Sets the target_component of the vehicle command accordingly depending on the
-	 * vehicle command value (e.g. For Camera control, sets target system component id)
+	 * Fill in timestamp, source and target IDs.
+	 * target_component special handling (e.g. For Camera control, set camera ID)
 	 *
-	 * @param vcmd Vehicle command to execute
+	 * @param vcmd Vehicle command to publish
 	 */
-	void publish_vehicle_cmd(vehicle_command_s *vcmd);
+	void publish_vehicle_cmd(vehicle_command_s &vehicle_command);
 
 #if CONFIG_NAVIGATOR_ADSB
 	/**

src/modules/navigator/navigator_main.cpp

@@ -662,10 +662,10 @@ void Navigator::run()
 				uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED};
 
 				if (_mission.get_land_start_available()) {
-					vehicle_command_s vcmd = {};
-					vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
-					vcmd.param1 = _mission.get_land_start_index();
-					publish_vehicle_cmd(&vcmd);
+					vehicle_command_s vehicle_command{};
+					vehicle_command.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
+					vehicle_command.param1 = _mission.get_land_start_index();
+					publish_vehicle_cmd(vehicle_command);
 
 				} else {
 					PX4_WARN("planned mission landing not available");
@@ -864,10 +864,10 @@ void Navigator::run()
 		if (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND &&
 		    _vstatus.is_vtol && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
 		    force_vtol()) {
-			vehicle_command_s vcmd = {};
-			vcmd.command = NAV_CMD_DO_VTOL_TRANSITION;
-			vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
-			publish_vehicle_cmd(&vcmd);
+			vehicle_command_s vehicle_command{};
+			vehicle_command.command = NAV_CMD_DO_VTOL_TRANSITION;
+			vehicle_command.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+			publish_vehicle_cmd(vehicle_command);
 			mavlink_log_info(&_mavlink_log_pub, "Transition to hover mode and descend.\t");
 			events::send(events::ID("navigator_transition_descend"), events::Log::Critical,
 				     "Transition to hover mode and descend");
@@ -1220,28 +1220,28 @@ void Navigator::load_fence_from_file(const char *filename)
 void Navigator::take_traffic_conflict_action()
 {
 
-	vehicle_command_s vcmd = {};
+	vehicle_command_s vehicle_command{};
 
 	switch (_adsb_conflict._conflict_detection_params.traffic_avoidance_mode) {
 
 	case 2: {
 			_rtl.set_return_alt_min(true);
-			vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
-			publish_vehicle_cmd(&vcmd);
+			vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
+			publish_vehicle_cmd(vehicle_command);
 			break;
 		}
 
 	case 3: {
-			vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
-			publish_vehicle_cmd(&vcmd);
+			vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
+			publish_vehicle_cmd(vehicle_command);
 			break;
 
 		}
 
 	case 4: {
 
-			vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM;
-			publish_vehicle_cmd(&vcmd);
+			vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM;
+			publish_vehicle_cmd(vehicle_command);
 			break;
 
 		}
@@ -1392,34 +1392,32 @@ void Navigator::publish_navigator_status()
 	}
 }
 
-void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
+void Navigator::publish_vehicle_cmd(vehicle_command_s &vehicle_command)
 {
-	vcmd->timestamp = hrt_absolute_time();
-	vcmd->source_system = _vstatus.system_id;
-	vcmd->source_component = _vstatus.component_id;
-	vcmd->target_system = _vstatus.system_id;
-	vcmd->confirmation = false;
-	vcmd->from_external = false;
+	vehicle_command.timestamp = hrt_absolute_time();
+	vehicle_command.source_system = _vstatus.system_id;
+	vehicle_command.source_component = _vstatus.component_id;
+	vehicle_command.target_system = _vstatus.system_id;
+	vehicle_command.confirmation = false;
+	vehicle_command.from_external = false;
 
 	int target_camera_component_id;
 
 	// The camera commands are not processed on the autopilot but will be
 	// sent to the mavlink links to other components.
-	switch (vcmd->command) {
-
-
+	switch (vehicle_command.command) {
 	case NAV_CMD_IMAGE_START_CAPTURE:
 
-		if (static_cast<int>(vcmd->param3) == 1) {
+		if (static_cast<int>(vehicle_command.param3) == 1) {
 			// When sending a single capture we need to include the sequence number, thus camera_trigger needs to handle this cmd
-			vcmd->param1 = 0.0f;
-			vcmd->param2 = 0.0f;
-			vcmd->param3 = 0.0f;
-			vcmd->param4 = 0.0f;
-			vcmd->param5 = 1.0;
-			vcmd->param6 = 0.0;
-			vcmd->param7 = 0.0f;
-			vcmd->command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
+			vehicle_command.param1 = 0.f;
+			vehicle_command.param2 = 0.f;
+			vehicle_command.param3 = 0.f;
+			vehicle_command.param4 = 0.f;
+			vehicle_command.param5 = 1.;
+			vehicle_command.param6 = 0.;
+			vehicle_command.param7 = 0.f;
+			vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
 
 		} else {
 			// We are only capturing multiple if param3 is 0 or > 1.
@@ -1427,65 +1425,65 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
 			_is_capturing_images = true;
 		}
 
-		target_camera_component_id = static_cast<int>(vcmd->param1); // Target id from param 1
+		target_camera_component_id = static_cast<int>(vehicle_command.param1); // Target id from param 1
 
 		if (target_camera_component_id > 0 && target_camera_component_id < 256) {
-			vcmd->target_component = target_camera_component_id;
+			vehicle_command.target_component = target_camera_component_id;
 
 		} else {
-			vcmd->target_component = 100; // MAV_COMP_ID_CAMERA
+			vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA
 		}
 
 		break;
 
 	case NAV_CMD_IMAGE_STOP_CAPTURE:
 		_is_capturing_images = false;
-		target_camera_component_id = static_cast<int>(vcmd->param1); // Target id from param 1
+		target_camera_component_id = static_cast<int>(vehicle_command.param1); // Target id from param 1
 
 		if (target_camera_component_id > 0 && target_camera_component_id < 256) {
-			vcmd->target_component = target_camera_component_id;
+			vehicle_command.target_component = target_camera_component_id;
 
 		} else {
-			vcmd->target_component = 100; // MAV_COMP_ID_CAMERA
+			vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA
 		}
 
 		break;
 
 	case NAV_CMD_SET_CAMERA_MODE:
-		target_camera_component_id = static_cast<int>(vcmd->param1); // Target id from param 1
+		target_camera_component_id = static_cast<int>(vehicle_command.param1); // Target id from param 1
 
 		if (target_camera_component_id > 0 && target_camera_component_id < 256) {
-			vcmd->target_component = target_camera_component_id;
+			vehicle_command.target_component = target_camera_component_id;
 
 		} else {
-			vcmd->target_component = 100; // MAV_COMP_ID_CAMERA
+			vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA
 		}
 
 		break;
 
 	case NAV_CMD_SET_CAMERA_SOURCE:
-		target_camera_component_id = static_cast<int>(vcmd->param1); // Target id from param 1
+		target_camera_component_id = static_cast<int>(vehicle_command.param1); // Target id from param 1
 
 		if (target_camera_component_id > 0 && target_camera_component_id < 256) {
-			vcmd->target_component = target_camera_component_id;
+			vehicle_command.target_component = target_camera_component_id;
 
 		} else {
-			vcmd->target_component = 100; // MAV_COMP_ID_CAMERA
+			vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA
 		}
 
 		break;
 
 	case NAV_CMD_VIDEO_START_CAPTURE:
 	case NAV_CMD_VIDEO_STOP_CAPTURE:
-		vcmd->target_component = 100; // MAV_COMP_ID_CAMERA
+		vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA
 		break;
 
 	default:
-		vcmd->target_component = 0;
+		vehicle_command.target_component = 0;
 		break;
 	}
 
-	_vehicle_cmd_pub.publish(*vcmd);
+	_vehicle_cmd_pub.publish(vehicle_command);
 }
 
 void Navigator::publish_distance_sensor_mode_request()
@@ -1531,24 +1529,24 @@ void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_
 
 void Navigator::acquire_gimbal_control()
 {
-	vehicle_command_s vcmd = {};
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
-	vcmd.param1 = _vstatus.system_id;
-	vcmd.param2 = _vstatus.component_id;
-	vcmd.param3 = -1.0f; // Leave unchanged.
-	vcmd.param4 = -1.0f; // Leave unchanged.
-	publish_vehicle_cmd(&vcmd);
+	vehicle_command_s vehicle_command{};
+	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
+	vehicle_command.param1 = _vstatus.system_id; // Take primary control
+	vehicle_command.param2 = _vstatus.component_id;
+	vehicle_command.param3 = -1.f; // Leave secondary control unchanged
+	vehicle_command.param4 = -1.f;
+	publish_vehicle_cmd(vehicle_command);
 }
 
 void Navigator::release_gimbal_control()
 {
-	vehicle_command_s vcmd = {};
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
-	vcmd.param1 = -3.0f; // Remove control if it had it.
-	vcmd.param2 = -3.0f; // Remove control if it had it.
-	vcmd.param3 = -1.0f; // Leave unchanged.
-	vcmd.param4 = -1.0f; // Leave unchanged.
-	publish_vehicle_cmd(&vcmd);
+	vehicle_command_s vehicle_command{};
+	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
+	vehicle_command.param1 = -3.f; // Remove primary control if it was taken
+	vehicle_command.param2 = -3.f;
+	vehicle_command.param3 = -1.f; // Leave secondary control unchanged
+	vehicle_command.param4 = -1.f;
+	publish_vehicle_cmd(vehicle_command);
 }
 
 
@@ -1556,10 +1554,10 @@ void
 Navigator::stop_capturing_images()
 {
 	if (_is_capturing_images) {
-		vehicle_command_s vcmd = {};
-		vcmd.command = NAV_CMD_IMAGE_STOP_CAPTURE;
-		vcmd.param1 = 0.0f;
-		publish_vehicle_cmd(&vcmd);
+		vehicle_command_s vehicle_command{};
+		vehicle_command.command = NAV_CMD_IMAGE_STOP_CAPTURE;
+		vehicle_command.param1 = 0.f;
+		publish_vehicle_cmd(vehicle_command);
 
 		// _is_capturing_images is reset inside publish_vehicle_cmd.
 	}
@@ -1607,24 +1605,24 @@ void Navigator::mode_completed(uint8_t nav_state, uint8_t result)
 void Navigator::disable_camera_trigger()
 {
 	// Disable camera trigger
-	vehicle_command_s cmd {};
-	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
+	vehicle_command_s vehicle_command{};
+	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
 	// Pause trigger
-	cmd.param1 = -1.0f;
-	cmd.param3 = 1.0f;
-	publish_vehicle_cmd(&cmd);
+	vehicle_command.param1 = -1.f;
+	vehicle_command.param3 = 1.f;
+	publish_vehicle_cmd(vehicle_command);
 }
 
 void Navigator::set_gimbal_neutral()
 {
-	vehicle_command_s vcmd = {};
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
-	vcmd.param1 = NAN;
-	vcmd.param2 = NAN;
-	vcmd.param3 = NAN;
-	vcmd.param4 = NAN;
-	vcmd.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL;
-	publish_vehicle_cmd(&vcmd);
+	vehicle_command_s vehicle_command{};
+	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
+	vehicle_command.param1 = NAN; // Don't set any angles
+	vehicle_command.param2 = NAN;
+	vehicle_command.param3 = NAN; // Don't set any angular velocities
+	vehicle_command.param4 = NAN;
+	vehicle_command.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL;
+	publish_vehicle_cmd(vehicle_command);
 }
 
 void Navigator::sendWarningDescentStoppedDueToTerrain()