Set timestamp with vehicle commands

The vehicle_command uORB messages had the timestamp unset at 0.

src/drivers/camera_trigger/camera_trigger.cpp

@@ -484,6 +484,7 @@ void
 CameraTrigger::test()
 {
 	struct vehicle_command_s cmd = {};
+	cmd.timestamp = hrt_absolute_time();
 	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
 	cmd.param5 = 1.0f;
 

src/drivers/px4io/px4io.cpp

@@ -821,6 +821,7 @@ PX4IO::init()
 		cmd.param5 = 0;
 		cmd.param6 = 0;
 		cmd.param7 = 0;
+		cmd.timestamp = hrt_absolute_time();
 		cmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
 
 		/* ask to confirm command */

src/drivers/vmount/output_mavlink.cpp

@@ -63,6 +63,7 @@ int OutputMavlink::update(const ControlData *control_data)
 		vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
 		vehicle_command.target_system = _config.mavlink_sys_id;
 		vehicle_command.target_component = _config.mavlink_comp_id;
+		vehicle_command.timestamp = hrt_absolute_time();
 
 		if (control_data->type == ControlData::Type::Neutral) {
 			vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
@@ -90,6 +91,7 @@ int OutputMavlink::update(const ControlData *control_data)
 	hrt_abstime t = hrt_absolute_time();
 	_calculate_output_angles(t);
 
+	vehicle_command.timestamp = t;
 	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
 	vehicle_command.target_system = _config.mavlink_sys_id;
 	vehicle_command.target_component = _config.mavlink_comp_id;

src/modules/commander/commander.cpp

@@ -495,6 +495,8 @@ int commander_main(int argc, char *argv[])
 				cmd.param6 = NAN;
 				cmd.param7 = NAN;
 
+				cmd.timestamp = hrt_absolute_time();
+
 				orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 				(void)orb_unadvertise(h);
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -452,6 +452,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 
 		memset(&vcmd, 0, sizeof(vcmd));
 
+		vcmd.timestamp = hrt_absolute_time();
+
 		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
 		vcmd.param1 = cmd_mavlink.param1;
 
@@ -559,6 +561,8 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 
 		memset(&vcmd, 0, sizeof(vcmd));
 
+		vcmd.timestamp = hrt_absolute_time();
+
 		/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
 		vcmd.param1 = cmd_mavlink.param1;
 
@@ -767,6 +771,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	vcmd.source_system = msg->sysid;
 	vcmd.source_component = msg->compid;
 	vcmd.confirmation = 1;
+	vcmd.timestamp = hrt_absolute_time();
 
 	if (_cmd_pub == nullptr) {
 		_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);

src/modules/navigator/mission_block.cpp

@@ -479,7 +479,9 @@ MissionBlock::issue_command(const struct mission_item_s *item)
 		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();
+		const hrt_abstime now = hrt_absolute_time();
+		_action_start = now;
+		cmd.timestamp = now;
 
 		_navigator->publish_vehicle_cmd(cmd);
 	}
@@ -715,6 +717,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 		struct vehicle_command_s cmd = {};
 		cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
 		cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+		cmd.timestamp = hrt_absolute_time();
 
 		_navigator->publish_vehicle_cmd(cmd);
 	}

src/modules/navigator/navigator_main.cpp

@@ -475,6 +475,7 @@ Navigator::task_main()
 					vcmd.param1 = land_start;
 					vcmd.param2 = 0;
 
+					vcmd.timestamp = hrt_absolute_time();
 					publish_vehicle_cmd(vcmd);
 
 				} else {

src/modules/sdlog2/sdlog2.c

@@ -389,6 +389,7 @@ int sdlog2_main(int argc, char *argv[])
 		cmd.param1 = -1;
 		cmd.param2 = -1;
 		cmd.param3 = 1;
+		cmd.timestamp = hrt_absolute_time();
 		orb_advertise(ORB_ID(vehicle_command), &cmd);
 		return 0;
 	}
@@ -399,6 +400,7 @@ int sdlog2_main(int argc, char *argv[])
 		cmd.param1 = -1;
 		cmd.param2 = -1;
 		cmd.param3 = 2;
+		cmd.timestamp = hrt_absolute_time();
 		orb_advertise(ORB_ID(vehicle_command), &cmd);
 		return 0;
 	}