vehicle_command topic: use uorb queuing with length 3

Just to make sure we don't lose any messages.

msg/vehicle_command.msg

@@ -83,6 +83,8 @@ uint32 VEHICLE_ROI_LOCATION = 3                     # Point toward fixed locatio
 uint32 VEHICLE_ROI_TARGET = 4                       # Point toward target
 uint32 VEHICLE_ROI_ENUM_END = 5
 
+uint32 ORB_QUEUE_LENGTH = 3
+
 float32 param1			# Parameter 1, as defined by MAVLink uint32 VEHICLE_CMD enum.
 float32 param2			# Parameter 2, as defined by MAVLink uint32 VEHICLE_CMD enum.
 float32 param3			# Parameter 3, as defined by MAVLink uint32 VEHICLE_CMD enum.

msg/vehicle_command_ack.msg

@@ -4,5 +4,7 @@ uint8 VEHICLE_RESULT_DENIED = 2
 uint8 VEHICLE_RESULT_UNSUPPORTED = 3
 uint8 VEHICLE_RESULT_FAILED = 4
 
+uint32 ORB_QUEUE_LENGTH = 3
+
 uint16 command
 uint8 result

src/drivers/camera_trigger/camera_trigger.cpp

@@ -395,7 +395,7 @@ CameraTrigger::test()
 	cmd.param5 = 1.0f;
 
 	orb_advert_t pub;
-	pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
+	pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 	(void)orb_unadvertise(pub);
 }
 

src/drivers/px4io/px4io.cpp

@@ -825,7 +825,7 @@ PX4IO::init()
 		cmd.confirmation =  1;
 
 		/* send command once */
-		orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
+		orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 
 		/* spin here until IO's state has propagated into the system */
 		do {

src/drivers/vmount/input_mavlink.cpp

@@ -302,9 +302,14 @@ void InputMavlinkCmdMount::_ack_vehicle_command(uint16_t command)
 	vehicle_command_ack.command = command;
 	vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
-	int instance;
+	if (_vehicle_command_ack_pub == nullptr) {
-	orb_publish_auto(ORB_ID(vehicle_command_ack), &_vehicle_command_ack_pub, &vehicle_command_ack,
+		_vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
-			 &instance, ORB_PRIO_DEFAULT);
+					   vehicle_command_ack_s::ORB_QUEUE_LENGTH);
+
+	} else {
+		orb_publish(ORB_ID(vehicle_command_ack), _vehicle_command_ack_pub, &vehicle_command_ack);
+	}
+
 }
 
 void InputMavlinkCmdMount::print_status()

src/drivers/vmount/output_mavlink.cpp

@@ -75,7 +75,8 @@ int OutputMavlink::update(const ControlData *control_data)
 			orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
 
 		} else {
-			_vehicle_command_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, 3);
+			_vehicle_command_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command,
+					       vehicle_command_s::ORB_QUEUE_LENGTH);
 		}
 
 	}

src/modules/commander/commander.cpp

@@ -424,7 +424,7 @@ int commander_main(int argc, char *argv[])
 				cmd.param6 = NAN;
 				cmd.param7 = NAN;
 
-				orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd);
+				orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 				(void)orb_unadvertise(h);
 
 			} else {
@@ -453,7 +453,7 @@ int commander_main(int argc, char *argv[])
 		cmd.param6 = NAN;
 		cmd.param7 = NAN;
 
-		orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd);
+		orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 		(void)orb_unadvertise(h);
 
 		return 0;
@@ -476,7 +476,7 @@ int commander_main(int argc, char *argv[])
 		cmd.param6 = NAN;
 		cmd.param7 = NAN;
 
-		orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd);
+		orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 		(void)orb_unadvertise(h);
 
 		return 0;
@@ -538,8 +538,8 @@ int commander_main(int argc, char *argv[])
 		/* if the comparison matches for off (== 0) set 0.0f, 2.0f (on) else */
 		cmd.param1 = strcmp(argv[2], "off") ? 2.0f : 0.0f; /* lockdown */
 
-		// XXX inspect use of publication handle
+		orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
-		(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+		(void)orb_unadvertise(h);
 
 		return 0;
 	}
@@ -3665,7 +3665,7 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result,
 		orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
 
 	} else {
-		command_ack_pub = orb_advertise(ORB_ID(vehicle_command_ack), &command_ack);
+		command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
 	}
 }
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -412,7 +412,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 			vcmd.confirmation =  cmd_mavlink.confirmation;
 
 			if (_cmd_pub == nullptr) {
-				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+				_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 
 			} else {
 				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
@@ -488,7 +488,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 			vcmd.source_component = msg->compid;
 
 			if (_cmd_pub == nullptr) {
-				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+				_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 
 			} else {
 				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
@@ -636,7 +636,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	vcmd.confirmation = 1;
 
 	if (_cmd_pub == nullptr) {
-		_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+		_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 
 	} else {
 		orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);

src/modules/navigator/mission_block.cpp

@@ -364,7 +364,7 @@ MissionBlock::issue_command(const struct mission_item_s *item)
 	}
 
 	if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
-		PX4_WARN("do_set_servo command");
+		PX4_INFO("do_set_servo command");
 		// XXX: we should issue a vehicle command and handle this somewhere else
 		memset(&actuators, 0, sizeof(actuators));
 		// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
@@ -380,7 +380,7 @@ MissionBlock::issue_command(const struct mission_item_s *item)
 		}
 
 	} else {
-		PX4_WARN("forwarding command %d\n", item->nav_cmd);
+		PX4_INFO("forwarding command %d", item->nav_cmd);
 		struct vehicle_command_s cmd = {};
 		mission_item_to_vehicle_command(item, &cmd);
 		_action_start = hrt_absolute_time();
@@ -389,7 +389,7 @@ MissionBlock::issue_command(const struct mission_item_s *item)
 			orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
 
 		} else {
-			_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
+			_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 		}
 	}
 }
@@ -616,7 +616,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 		if (_cmd_pub != nullptr) {
 			orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
 		} else {
-			_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
+			_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 		}
 	}