VTOL: use as use less hrt_absolute_time() calls to reduce flash and CPU

Signed-off-by: Silvan Fuhrer <[email protected]>

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;

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;

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);
 }
 

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);
 		}
 	}