Skip to content

Commit

Permalink
uORB::Publication automatically set timestamp
Browse files Browse the repository at this point in the history
  • Loading branch information
dagar committed Mar 12, 2020
1 parent 69b38c9 commit df1a945
Show file tree
Hide file tree
Showing 54 changed files with 37 additions and 134 deletions.
3 changes: 1 addition & 2 deletions boards/bitcraze/crazyflie/syslink/syslink_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -533,9 +533,8 @@ Syslink::handle_raw(syslink_message_t *sys)

crtp_commander *cmd = (crtp_commander *) &c->data[0];

input_rc_s rc = {};
input_rc_s rc{};

rc.timestamp = hrt_absolute_time();
rc.timestamp_last_signal = rc.timestamp;
rc.channel_count = 5;
rc.rc_failsafe = false;
Expand Down
1 change: 0 additions & 1 deletion boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,6 @@ void NavioSysRCInput::Run()
data.timestamp_last_signal = timestamp_sample;
data.channel_count = CHANNELS;
data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
data.timestamp = hrt_absolute_time();

_input_rc_pub.publish(data);
perf_count(_publish_interval_perf);
Expand Down
1 change: 0 additions & 1 deletion src/drivers/camera_capture/camera_capture.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -195,7 +195,6 @@ CameraCapture::Run()
// Acknowledge the command
vehicle_command_ack_s command_ack{};

command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
command_ack.target_system = cmd.source_system;
Expand Down
2 changes: 0 additions & 2 deletions src/drivers/camera_trigger/camera_trigger.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -494,7 +494,6 @@ void
CameraTrigger::test()
{
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.param5 = 1.0;
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;

Expand Down Expand Up @@ -701,7 +700,6 @@ CameraTrigger::Run()
if (updated && need_ack) {
vehicle_command_ack_s command_ack{};

command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)cmd_result;
command_ack.target_system = cmd.source_system;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -204,7 +204,6 @@ MEASAirspeed::collect()

differential_pressure_s report{};

report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
Expand Down
1 change: 0 additions & 1 deletion src/drivers/gps/gps.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -657,7 +657,6 @@ GPS::run()
while (!should_exit()) {

if (_fake_gps) {
_report_gps_pos.timestamp = hrt_absolute_time();
_report_gps_pos.lat = (int32_t)47.378301e7f;
_report_gps_pos.lon = (int32_t)8.538777e7f;
_report_gps_pos.alt = (int32_t)1200e3f;
Expand Down
1 change: 0 additions & 1 deletion src/drivers/irlock/irlock.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -366,7 +366,6 @@ int IRLOCK::read_device()
// publish over uORB
if (report.num_targets > 0) {
irlock_report_s orb_report{};
orb_report.timestamp = report.timestamp;
orb_report.signature = report.targets[0].signature;
orb_report.pos_x = report.targets[0].pos_x;
orb_report.pos_y = report.targets[0].pos_y;
Expand Down
1 change: 0 additions & 1 deletion src/drivers/optical_flow/px4flow/px4flow.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -303,7 +303,6 @@ PX4FLOW::collect()

optical_flow_s report{};

report.timestamp = hrt_absolute_time();
report.pixel_flow_x_integral = static_cast<float>(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
report.frame_count_since_last_readout = _frame_integral.frame_count_since_last_readout;
Expand Down
12 changes: 1 addition & 11 deletions src/drivers/px4io/px4io.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -751,7 +751,6 @@ PX4IO::init()
if (reg & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
mavlink_log_emergency(&_mavlink_log_pub, "IO is in failsafe, force failsafe");
/* send command to terminate flight via command API */
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = 1.0f; /* request flight termination */
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;

Expand Down Expand Up @@ -783,7 +782,6 @@ PX4IO::init()
}

/* send command to arm system via command API */
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = 1.0f; /* request arming */
vcmd.param3 = 1234.f; /* mark the command coming from IO (for in-air restoring) */
vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
Expand Down Expand Up @@ -1691,7 +1689,6 @@ PX4IO::io_handle_status(uint16_t status)
* Get and handle the safety status
*/
safety_s safety{};
safety.timestamp = hrt_absolute_time();
safety.safety_switch_available = true;
safety.safety_off = (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? true : false;
safety.override_available = _override_available;
Expand Down Expand Up @@ -1738,8 +1735,6 @@ PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
{
servorail_status_s servorail_status{};

servorail_status.timestamp = hrt_absolute_time();

/* voltage is scaled to mV */
servorail_status.voltage_v = vservo * 0.001f;
servorail_status.rssi_v = vrssi * 0.001f;
Expand Down Expand Up @@ -1818,8 +1813,6 @@ PX4IO::io_get_raw_rc_input(input_rc_s &input_rc)

_rc_chan_count = channel_count;

input_rc.timestamp = hrt_absolute_time();

input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];

if (!_analog_rc_rssi_stable) {
Expand Down Expand Up @@ -1888,9 +1881,8 @@ PX4IO::io_get_raw_rc_input(input_rc_s &input_rc)
int
PX4IO::io_publish_raw_rc()
{

/* fetch values from IO */
input_rc_s rc_val;
input_rc_s rc_val{};

/* set the RC status flag ORDER MATTERS! */
rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK);
Expand Down Expand Up @@ -1945,7 +1937,6 @@ PX4IO::io_publish_pwm_outputs()
}

actuator_outputs_s outputs = {};
outputs.timestamp = hrt_absolute_time();
outputs.noutputs = _max_actuators;

/* convert from register format to float */
Expand All @@ -1966,7 +1957,6 @@ PX4IO::io_publish_pwm_outputs()
/* publish mixer status */
if (saturation_status.flags.valid) {
multirotor_motor_limits_s motor_limits{};
motor_limits.timestamp = hrt_absolute_time();
motor_limits.saturation_status = saturation_status.value;

_to_mixer_status.publish(motor_limits);
Expand Down
1 change: 0 additions & 1 deletion src/drivers/qshell/posix/qshell.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,6 @@ int QShell::_send_cmd(std::vector<std::string> &argList)
strcpy((char *)qshell_req.cmd, cmd.c_str());
qshell_req.request_sequence = _current_sequence;

qshell_req.timestamp = hrt_absolute_time();
_qshell_req_pub.publish(qshell_req);

return 0;
Expand Down
1 change: 0 additions & 1 deletion src/drivers/qshell/qurt/qshell.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,6 @@ int QShell::main()
PX4_INFO("Ok executing command: %s", m_qshell_req.cmd);
}

retval.timestamp = hrt_absolute_time();
_qshell_retval_pub.publish(retval);

} else if (pret == 0) {
Expand Down
3 changes: 0 additions & 3 deletions src/drivers/safety_button/SafetyButton.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -146,14 +146,12 @@ SafetyButton::CheckPairingRequest(bool button_pressed)
led_control.color = led_control_s::COLOR_GREEN;
led_control.num_blinks = 1;
led_control.priority = 0;
led_control.timestamp = hrt_absolute_time();
_to_led_control.publish(led_control);

tune_control_s tune_control{};
tune_control.tune_id = TONE_NOTIFY_POSITIVE_TUNE;
tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
tune_control.tune_override = 0;
tune_control.timestamp = hrt_absolute_time();
_to_tune_control.publish(tune_control);

// reset state
Expand Down Expand Up @@ -214,7 +212,6 @@ SafetyButton::Run()
FlashButton();

safety_s safety{};
safety.timestamp = hrt_absolute_time();
safety.safety_switch_available = true;
safety.safety_off = _safety_btn_off || _safety_disabled;

Expand Down
3 changes: 0 additions & 3 deletions src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1107,8 +1107,6 @@ void IridiumSBD::publish_iridium_status()
_status.rx_session_pending = _rx_session_pending;
}

_status.timestamp = hrt_absolute_time();

// publish the status if it changed
if (need_to_publish) {
_iridiumsbd_status_pub.publish(_status);
Expand All @@ -1122,7 +1120,6 @@ void IridiumSBD::publish_subsystem_status()
const bool ok = _status.last_heartbeat > 0; // maybe at some point here an additional check should be made

if ((_info.present != present) || (_info.enabled != enabled) || (_info.ok != ok)) {
_info.timestamp = hrt_absolute_time();
_info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_SATCOM;
_info.present = present;
_info.enabled = enabled;
Expand Down
1 change: 0 additions & 1 deletion src/lib/avoidance/ObstacleAvoidanceTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,6 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy)
TestObstacleAvoidance oa;

vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint;
message.timestamp = hrt_absolute_time();
message.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0] = 2.6f;
message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1] = 2.4f;
Expand Down
6 changes: 0 additions & 6 deletions src/lib/drivers/accelerometer/PX4Accelerometer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -155,7 +155,6 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
report.x = val_calibrated(0);
report.y = val_calibrated(1);
report.z = val_calibrated(2);
report.timestamp = hrt_absolute_time();

_sensor_pub.publish(report);
}
Expand All @@ -178,7 +177,6 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
report.dt = integral_dt;
report.samples = _integrator_samples;
report.clip_count = _integrator_clipping;
report.timestamp = hrt_absolute_time();

_sensor_integrated_pub.publish(report);

Expand Down Expand Up @@ -219,7 +217,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
report.x = val_calibrated(0);
report.y = val_calibrated(1);
report.z = val_calibrated(2);
report.timestamp = hrt_absolute_time();

_sensor_pub.publish(report);
}
Expand Down Expand Up @@ -282,7 +279,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
report.samples = _integrator_fifo_samples;
report.clip_count = _integrator_clipping;

report.timestamp = hrt_absolute_time();
_sensor_integrated_pub.publish(report);

// update vibration metrics
Expand All @@ -308,7 +304,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * N);

fifo.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(fifo);


Expand All @@ -331,7 +326,6 @@ void PX4Accelerometer::PublishStatus()
status.clipping[0] = _clipping[0];
status.clipping[1] = _clipping[1];
status.clipping[2] = _clipping[2];
status.timestamp = hrt_absolute_time();
_sensor_status_pub.publish(status);

_status_last_publish = status.timestamp;
Expand Down
1 change: 0 additions & 1 deletion src/lib/flight_tasks/FlightTasks.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -167,7 +167,6 @@ void FlightTasks::_updateCommand()

// send back acknowledgment
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = command.command;
command_ack.result = cmd_result;
command_ack.result_param1 = static_cast<int>(switch_result);
Expand Down
1 change: 0 additions & 1 deletion src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,6 @@ const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
{
/* fill position setpoint message */
vehicle_local_position_setpoint_s vehicle_local_position_setpoint{};
vehicle_local_position_setpoint.timestamp = hrt_absolute_time();

vehicle_local_position_setpoint.x = _position_setpoint(0);
vehicle_local_position_setpoint.y = _position_setpoint(1);
Expand Down
1 change: 0 additions & 1 deletion src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,6 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
bool FlightTaskOrbit::sendTelemetry()
{
orbit_status_s orbit_status{};
orbit_status.timestamp = hrt_absolute_time();
orbit_status.radius = math::signNoZero(_v) * _r;
orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,6 @@ static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = {
static void arm_auth_request_msg_send()
{
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST;
vcmd.target_system = arm_parameters.struct_value.authorizer_system_id;

Expand Down
16 changes: 0 additions & 16 deletions src/modules/commander/Commander.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -178,8 +178,6 @@ static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2
vcmd.source_component = vehicle_status_sub.get().component_id;
vcmd.target_component = vehicle_status_sub.get().component_id;

vcmd.timestamp = hrt_absolute_time();

uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};

return vcmd_pub.publish(vcmd);
Expand Down Expand Up @@ -797,7 +795,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
if (local_pos.xy_global && local_pos.z_global) {
/* use specified position */
home_position_s home{};
home.timestamp = hrt_absolute_time();

home.lat = lat;
home.lon = lon;
Expand Down Expand Up @@ -1083,7 +1080,6 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd)
}

test_motor_s test_motor{};
test_motor.timestamp = hrt_absolute_time();
test_motor.motor_number = (int)(cmd.param1 + 0.5f) - 1;
int throttle_type = (int)(cmd.param2 + 0.5f);

Expand Down Expand Up @@ -1138,8 +1134,6 @@ Commander::set_home_position()
// Set home position
home_position_s home{};

home.timestamp = hrt_absolute_time();

home.lat = gpos.lat;
home.lon = gpos.lon;
home.valid_hpos = true;
Expand Down Expand Up @@ -1181,8 +1175,6 @@ Commander::set_home_position_alt_only()
home.alt = lpos.ref_alt;
home.valid_alt = true;

home.timestamp = hrt_absolute_time();

return _home_pub.update(home);
}

Expand Down Expand Up @@ -2209,7 +2201,6 @@ Commander::run()

update_control_mode();

status.timestamp = hrt_absolute_time();
_status_pub.publish(status);

switch ((PrearmedMode)_param_com_prearm_mode.get()) {
Expand Down Expand Up @@ -2243,15 +2234,12 @@ Commander::run()
break;
}

armed.timestamp = hrt_absolute_time();
_armed_pub.publish(armed);

/* publish internal state for logging purposes */
_internal_state.timestamp = hrt_absolute_time();
_commander_state_pub.publish(_internal_state);

/* publish vehicle_status_flags */
status_flags.timestamp = hrt_absolute_time();
_vehicle_status_flags_pub.publish(status_flags);
}

Expand Down Expand Up @@ -3012,8 +3000,6 @@ Commander::update_control_mode()
{
vehicle_control_mode_s control_mode{};

control_mode.timestamp = hrt_absolute_time();

/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = armed.armed;
control_mode.flag_external_manual_override_ok = (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
Expand Down Expand Up @@ -3241,13 +3227,11 @@ void answer_command(const vehicle_command_s &cmd, unsigned result,
/* publish ACK */
vehicle_command_ack_s command_ack{};

command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)result;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;


command_ack_pub.publish(command_ack);
}

Expand Down
1 change: 0 additions & 1 deletion src/modules/events/send_event.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,6 @@ void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
{
/* publish ACK */
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)result;
command_ack.target_system = cmd.source_system;
Expand Down
2 changes: 0 additions & 2 deletions src/modules/events/status_display.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -95,8 +95,6 @@ void StatusDisplay::process()

void StatusDisplay::publish()
{
_led_control.timestamp = hrt_absolute_time();

_led_control_pub.publish(_led_control);
}

Expand Down
Loading

0 comments on commit df1a945

Please sign in to comment.