Fixed incorrect timestamps

This commit is contained in:
Timothy Scott 2019-07-03 13:30:46 +02:00 committed by Lorenz Meier
parent a2471fb539
commit dce1dda871
3 changed files with 3 additions and 1 deletions

View File

@ -234,6 +234,7 @@ ObstacleAvoidance::_publishAvoidanceDesiredWaypoint()
void ObstacleAvoidance::_publishVehicleCmdDoLoiter()
{
vehicle_command_s command{};
command.timestamp = hrt_absolute_time();
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = (float)1; // base mode
command.param3 = (float)0; // sub mode

View File

@ -568,7 +568,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
optical_flow_s f = {};
f.timestamp = _mavlink_timesync.sync_stamp(flow.time_usec);
f.timestamp = hrt_absolute_time();
f.time_since_last_sonar_update = flow.time_delta_distance_us;
f.integration_timespan = flow.integration_time_us;
f.pixel_flow_x_integral = flow.integrated_x;

View File

@ -979,6 +979,7 @@ void MulticopterPositionControl::check_failure(bool task_failure, uint8_t nav_st
void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state)
{
vehicle_command_s command{};
command.timestamp = hrt_absolute_time();
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = (float)1; // base mode
command.param3 = (float)0; // sub mode