fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 01:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: pushing modularity

Browse Source
This commit is contained in:
Pedro-Roque
2025-02-05 18:28:08 +01:00
parent fa4189f669
commit 05e5700da6
2 changed files with 21 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/spacecraft/SpacecraftHandler.cpp
+19 -16
View File
@@ -80,26 +80,29 @@ void SpacecraftHandler::Run()
manual_control_setpoint_s manual_control_setpoint{}; manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
rover_differential_setpoint_s rover_differential_setpoint{}; _thrust_setpoint(0) = math::constrain((manual_control_setpoint.pitch * _manual_force_max), -1.f, 1.f);
rover_differential_setpoint.timestamp = timestamp; _thrust_setpoint(1) = math::constrain((manual_control_setpoint.roll * _manual_force_max), -1.f, 1.f);
rover_differential_setpoint.forward_speed_setpoint = NAN; _thrust_setpoint(2) = 0.0;
rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
rover_differential_setpoint.yaw_setpoint = NAN;
if (_max_yaw_rate > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { _torque_setpoint(0) = _torque_setpoint(1) = 0.0;
const float scaled_yaw_rate_input = math::interpolate<float>(manual_control_setpoint.roll, _torque_setpoint(2) = math::constrain((manual_control_setpoint.yaw * _manual_torque_max), -1.f, 1.f);
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); // publish thrust and torque setpoints
const float speed_diff = scaled_yaw_rate_input * _param_rd_wheel_track.get() / 2.f; vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
rover_differential_setpoint.speed_diff_setpoint_normalized = math::interpolate<float>(speed_diff, vehicle_torque_setpoint_s vehicle_torque_setpoint{};
-_param_rd_max_thr_yaw_r.get(), _param_rd_max_thr_yaw_r.get(), -1.f, 1.f);
} else { _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
rover_differential_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.roll; _torque_setpoint.copyTo(vehicle_torque_setpoint.xyz);
} vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
rover_differential_setpoint.yaw_rate_setpoint = NAN; vehicle_torque_setpoint.timestamp = hrt_absolute_time();
_rover_differential_setpoint_pub.publish(rover_differential_setpoint); vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
updateActuatorControlsStatus(vehicle_torque_setpoint, dt);
} }
} break; } break;
src/modules/spacecraft/SpacecraftHandler.hpp
+2
View File
@@ -99,6 +99,8 @@ private:
*/ */
void updateSubscriptions(); void updateSubscriptions();
void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt);
// uORB Subscriptions // uORB Subscriptions
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};