control: reduce default control update frequency (#37)

Auterion · May 16, 2024 · b11608e · b11608e
1 parent 5ec96ae
commit b11608e
Show file tree

Hide file tree

Showing 4 changed files with 7 additions and 4 deletions.
diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/direct_actuators.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/direct_actuators.hpp
@@ -31,7 +31,7 @@ class DirectActuatorsSetpointType : public SetpointBase
   ~DirectActuatorsSetpointType() override = default;
 
   Configuration getConfiguration() override;
-  float desiredUpdateRateHz() override {return 500.f;}
+  float desiredUpdateRateHz() override {return 200.f;}
 
   /**
    * Send servos setpoint

diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp
@@ -27,7 +27,7 @@ class AttitudeSetpointType : public SetpointBase
   ~AttitudeSetpointType() override = default;
 
   Configuration getConfiguration() override;
-  float desiredUpdateRateHz() override {return 200.f;}
+  float desiredUpdateRateHz() override {return 100.f;}
 
   void update(
     const Eigen::Quaternionf & attitude_setpoint,

diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp
@@ -27,7 +27,7 @@ class RatesSetpointType : public SetpointBase
   ~RatesSetpointType() override = default;
 
   Configuration getConfiguration() override;
-  float desiredUpdateRateHz() override {return 500.f;}
+  float desiredUpdateRateHz() override {return 200.f;}
 
   void update(
     const Eigen::Vector3f & rate_setpoints_ned_rad,

diff --git a/px4_ros2_cpp/src/components/mode.cpp b/px4_ros2_cpp/src/components/mode.cpp
@@ -228,7 +228,10 @@ bool ModeBase::onRegistered()
   // TODO: check setpoint types compatibility with current vehicle type
 
   activateSetpointType(*_setpoint_types[0]);
-  setSetpointUpdateRateFromSetpointTypes();
+  if (_setpoint_update_rate_hz < FLT_EPSILON) {
+    // Do not use default setpoint rate if rate was already set by user
+    setSetpointUpdateRateFromSetpointTypes();
+  }
 
   return true;
 }