From 789d589aea099643dae3dd6d2b94a00bfdd3498a Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 7 Nov 2024 18:31:42 +0100 Subject: [PATCH] VTOL: use as use less hrt_absolute_time() calls to reduce flash and CPU Signed-off-by: Silvan Fuhrer --- src/modules/vtol_att_control/standard.cpp | 10 ++++++---- src/modules/vtol_att_control/tailsitter.cpp | 19 +++++++++++-------- src/modules/vtol_att_control/tiltrotor.cpp | 11 ++++++----- .../vtol_att_control_main.cpp | 10 +++++----- 4 files changed, 28 insertions(+), 22 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index d8d640790e75..e0e0a9ea5bd7 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -281,25 +281,27 @@ void Standard::update_fw_state() */ void Standard::fill_actuator_outputs() { - _torque_setpoint_0->timestamp = hrt_absolute_time(); + const hrt_abstime now = hrt_absolute_time(); + + _torque_setpoint_0->timestamp = now; _torque_setpoint_0->timestamp_sample = _vehicle_torque_setpoint_virtual_mc->timestamp_sample; _torque_setpoint_0->xyz[0] = 0.f; _torque_setpoint_0->xyz[1] = 0.f; _torque_setpoint_0->xyz[2] = 0.f; - _torque_setpoint_1->timestamp = hrt_absolute_time(); + _torque_setpoint_1->timestamp = now; _torque_setpoint_1->timestamp_sample = _vehicle_torque_setpoint_virtual_fw->timestamp_sample; _torque_setpoint_1->xyz[0] = 0.f; _torque_setpoint_1->xyz[1] = 0.f; _torque_setpoint_1->xyz[2] = 0.f; - _thrust_setpoint_0->timestamp = hrt_absolute_time(); + _thrust_setpoint_0->timestamp = now; _thrust_setpoint_0->timestamp_sample = _vehicle_thrust_setpoint_virtual_mc->timestamp_sample; _thrust_setpoint_0->xyz[0] = 0.f; _thrust_setpoint_0->xyz[1] = 0.f; _thrust_setpoint_0->xyz[2] = 0.f; - _thrust_setpoint_1->timestamp = hrt_absolute_time(); + _thrust_setpoint_1->timestamp = now; _thrust_setpoint_1->timestamp_sample = _vehicle_thrust_setpoint_virtual_fw->timestamp_sample; _thrust_setpoint_1->xyz[0] = 0.f; _thrust_setpoint_1->xyz[1] = 0.f; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index eaacbe68d611..5c207e4729e4 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -64,11 +64,12 @@ void Tailsitter::update_vtol_state() * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. */ + const hrt_abstime now = hrt_absolute_time(); if (_vtol_vehicle_status->fixed_wing_system_failure) { // Failsafe event, switch to MC mode immediately if (_vtol_mode != vtol_mode::MC_MODE) { - _transition_start_timestamp = hrt_absolute_time(); + _transition_start_timestamp = now; } _vtol_mode = vtol_mode::MC_MODE; @@ -116,7 +117,7 @@ void Tailsitter::update_vtol_state() if (isFrontTransitionCompleted()) { _vtol_mode = vtol_mode::FW_MODE; - _trans_finished_ts = hrt_absolute_time(); + _trans_finished_ts = now; } break; @@ -125,7 +126,7 @@ void Tailsitter::update_vtol_state() case vtol_mode::TRANSITION_BACK: // failsafe into fixed wing mode _vtol_mode = vtol_mode::FW_MODE; - _trans_finished_ts = hrt_absolute_time(); + _trans_finished_ts = now; break; } } @@ -237,7 +238,7 @@ void Tailsitter::update_transition_state() blendThrottleBeginningBackTransition(progress); } - _v_att_sp->timestamp = hrt_absolute_time(); + _v_att_sp->timestamp = now; const Eulerf euler_sp(_q_trans_sp); _q_trans_sp.copyTo(_v_att_sp->q_d); @@ -260,25 +261,27 @@ void Tailsitter::update_fw_state() */ void Tailsitter::fill_actuator_outputs() { - _torque_setpoint_0->timestamp = hrt_absolute_time(); + const hrt_abstime now = hrt_absolute_time(); + + _torque_setpoint_0->timestamp = now; _torque_setpoint_0->timestamp_sample = _vehicle_torque_setpoint_virtual_mc->timestamp_sample; _torque_setpoint_0->xyz[0] = 0.f; _torque_setpoint_0->xyz[1] = 0.f; _torque_setpoint_0->xyz[2] = 0.f; - _torque_setpoint_1->timestamp = hrt_absolute_time(); + _torque_setpoint_1->timestamp = now; _torque_setpoint_1->timestamp_sample = _vehicle_torque_setpoint_virtual_fw->timestamp_sample; _torque_setpoint_1->xyz[0] = 0.f; _torque_setpoint_1->xyz[1] = 0.f; _torque_setpoint_1->xyz[2] = 0.f; - _thrust_setpoint_0->timestamp = hrt_absolute_time(); + _thrust_setpoint_0->timestamp = now; _thrust_setpoint_0->timestamp_sample = _vehicle_thrust_setpoint_virtual_mc->timestamp_sample; _thrust_setpoint_0->xyz[0] = 0.f; _thrust_setpoint_0->xyz[1] = 0.f; _thrust_setpoint_0->xyz[2] = 0.f; - _thrust_setpoint_1->timestamp = hrt_absolute_time(); + _thrust_setpoint_1->timestamp = now; _thrust_setpoint_1->timestamp_sample = _vehicle_thrust_setpoint_virtual_fw->timestamp_sample; _thrust_setpoint_1->xyz[0] = 0.f; _thrust_setpoint_1->xyz[1] = 0.f; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 3a1904257993..10ae871a6a55 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -342,26 +342,27 @@ void Tiltrotor::waiting_on_tecs() void Tiltrotor::fill_actuator_outputs() { + const hrt_abstime now = hrt_absolute_time(); - _torque_setpoint_0->timestamp = hrt_absolute_time(); + _torque_setpoint_0->timestamp = now; _torque_setpoint_0->timestamp_sample = _vehicle_torque_setpoint_virtual_mc->timestamp_sample; _torque_setpoint_0->xyz[0] = 0.f; _torque_setpoint_0->xyz[1] = 0.f; _torque_setpoint_0->xyz[2] = 0.f; - _torque_setpoint_1->timestamp = hrt_absolute_time(); + _torque_setpoint_1->timestamp = now; _torque_setpoint_1->timestamp_sample = _vehicle_torque_setpoint_virtual_fw->timestamp_sample; _torque_setpoint_1->xyz[0] = 0.f; _torque_setpoint_1->xyz[1] = 0.f; _torque_setpoint_1->xyz[2] = 0.f; - _thrust_setpoint_0->timestamp = hrt_absolute_time(); + _thrust_setpoint_0->timestamp = now; _thrust_setpoint_0->timestamp_sample = _vehicle_thrust_setpoint_virtual_mc->timestamp_sample; _thrust_setpoint_0->xyz[0] = 0.f; _thrust_setpoint_0->xyz[1] = 0.f; _thrust_setpoint_0->xyz[2] = 0.f; - _thrust_setpoint_1->timestamp = hrt_absolute_time(); + _thrust_setpoint_1->timestamp = now; _thrust_setpoint_1->timestamp_sample = _vehicle_thrust_setpoint_virtual_fw->timestamp_sample; _thrust_setpoint_1->xyz[0] = 0.f; _thrust_setpoint_1->xyz[1] = 0.f; @@ -405,7 +406,7 @@ void Tiltrotor::fill_actuator_outputs() tiltrotor_extra_controls_s tiltrotor_extra_controls = {}; tiltrotor_extra_controls.collective_tilt_normalized_setpoint = _tilt_control; tiltrotor_extra_controls.collective_thrust_normalized_setpoint = collective_thrust_normalized_setpoint; - tiltrotor_extra_controls.timestamp = hrt_absolute_time(); + tiltrotor_extra_controls.timestamp = now; _tiltrotor_extra_controls_pub.publish(tiltrotor_extra_controls); } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index afcde463da34..4c5c1289444a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -284,10 +284,10 @@ VtolAttitudeControl::Run() return; } -#if !defined(ENABLE_LOCKSTEP_SCHEDULER) - const hrt_abstime now = hrt_absolute_time(); +#if !defined(ENABLE_LOCKSTEP_SCHEDULER) + // prevent excessive scheduling (> 500 Hz) if (now - _last_run_timestamp < 2_ms) { return; @@ -428,7 +428,7 @@ VtolAttitudeControl::Run() _vehicle_thrust_setpoint1_pub.publish(_thrust_setpoint_1); // Advertise/Publish vtol vehicle status - _vtol_vehicle_status.timestamp = hrt_absolute_time(); + _vtol_vehicle_status.timestamp = now; _vtol_vehicle_status_pub.publish(_vtol_vehicle_status); // Publish flaps/spoiler setpoint with configured deflection in Hover if in Auto. @@ -439,7 +439,7 @@ VtolAttitudeControl::Run() // flaps normalized_unsigned_setpoint_s flaps_setpoint; flaps_setpoint.normalized_setpoint = 0.f; // for now always set flaps to 0 in transitions and hover - flaps_setpoint.timestamp = hrt_absolute_time(); + flaps_setpoint.timestamp = now; _flaps_setpoint_pub.publish(flaps_setpoint); // spoilers @@ -452,7 +452,7 @@ VtolAttitudeControl::Run() normalized_unsigned_setpoint_s spoiler_setpoint; spoiler_setpoint.normalized_setpoint = spoiler_control; - spoiler_setpoint.timestamp = hrt_absolute_time(); + spoiler_setpoint.timestamp = now; _spoilers_setpoint_pub.publish(spoiler_setpoint); } }