diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index bdd8a7cd4a..ee3898db1a 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/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; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cd2010e640..98054b894c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/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 */ diff --git a/src/drivers/vmount/output_mavlink.cpp b/src/drivers/vmount/output_mavlink.cpp index 956e1aa5be..3baa6dc110 100644 --- a/src/drivers/vmount/output_mavlink.cpp +++ b/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; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 877db429db..10dd63d447 100644 --- a/src/modules/commander/commander.cpp +++ b/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); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6ed6cbf041..8071a5b4e1 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4d113698a6..7a4d83bc2c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/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); } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ff64c52b39..f124ccfdca 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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 { diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2a1fceb69f..24d55c1b11 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/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; }