Merge branch 'russkel:master' into master

Greenroom-Robotics · Nov 15, 2023 · 0b506c0 · 0b506c0
2 parents 2002ef3 + dd149e5
commit 0b506c0
Show file tree

Hide file tree

Showing 5 changed files with 19 additions and 6 deletions.
diff --git a/README.md b/README.md
@@ -49,11 +49,12 @@ pwm_driver:
 
 | Parameter     | Type     | Description                                                                                                                     |
 |---------------|----------|---------------------------------------------------------------------------------------------------------------------------------|
-| `device_path` | `string` | Path to the PWM device. e.g. `/sys/class/pwm/pwmchip1`.       |
-| `channel`     | `int`    | The PWM channel that will be used for this output.                                                                         |
+| `device_path` | `string` | Path to the PWM device. e.g. `/sys/class/pwm/pwmchip1`.                                                                         |
+| `channel`     | `int`    | The PWM channel that will be used for this output.                                                                              |
 | `period`      | `int`    | The period of the PWM signal in nanoseconds. For RC servos and motor controllers this is 24000000.                              |
 | `type`        | `string` | Options are: `ppm` or `pwm`. The 'type' of PWM signal to output. RC servos and motor controllers needs this to be set to `ppm`. |
-| `range`   | `int[]`  | Format is either [`minimum`, `zero-point`, `maximum`] or [`minimum`, `maximum`].                                                |
+| `range`       | `int[]`  | Format is either [`minimum`, `zero-point`, `maximum`] or [`minimum`, `maximum`].                                                |
+| `trim`        | `float`  | Trim value for the scaled duty output. This value is added to the normalised duty value.                                        |
 
 ### PPM Range
 

diff --git a/include/pwmdriver/pwmlib.hpp b/include/pwmdriver/pwmlib.hpp
@@ -15,6 +15,8 @@ class PWMPort {
     void deactivate();
     void configure_ppm(int32_t min_point, int32_t zero_point, int32_t max_point);
     void configure_pwm();
+    void set_trim(float trim) { duty_trim = trim; }
+    float apply_trim(float duty);
 
     PWMPort(std::string_view pwm_device, int16_t channel, int32_t period);
     ~PWMPort();
@@ -30,4 +32,5 @@ class PWMPort {
     int32_t channel_ppm_min = 0;
     int32_t channel_ppm_max = 0;
     int32_t channel_ppm_zero = 0;
+    float duty_trim = 0.0;
 };
diff --git a/src/param_pwmport.cpp b/src/param_pwmport.cpp
@@ -9,13 +9,16 @@ ParamConfiguredPWMPort::ParamConfiguredPWMPort(rclcpp::Node* node, std::string p
     int period = node->declare_parameter(fmt::format("{}.period", port_name), 0);
     std::string port_type = node->declare_parameter(fmt::format("{}.type", port_name), "pwm");
     std::vector<int64_t> ppm_range = node->declare_parameter(fmt::format("{}.range", port_name), std::vector<int64_t> {});
+    float trim = node->declare_parameter(fmt::format("{}.trim", port_name), 0.0f);
 
     RCLCPP_DEBUG(node->get_logger(), "Setting up output: %s [%s channel %d]", port_name.c_str(), pwm_device.c_str(), channel);
 
     port = std::make_shared<PWMPort>(pwm_device, channel, period);
 
     if (port_type == "ppm")
         configure_ppm_range(ppm_range);
+
+    port->set_trim(trim);
 }
 
 void ParamConfiguredPWMPort::configure_ppm_range(std::vector<int64_t> ppm_range)

diff --git a/src/pwm_driver.cpp b/src/pwm_driver.cpp
@@ -27,7 +27,7 @@ class PWMDriver : public rclcpp::Node {
                 RCLCPP_INFO(this->get_logger(), "Configured output: %s [%s channel %d] on %s", port_name.c_str(), port->pwm_device.c_str(), port->channel, topic.c_str());
 
                 pwm_outputs[port_name] = port;
-                pwm_outputs_subs[port_name] = this->create_subscription<std_msgs::msg::Float32>(topic, rclcpp::SystemDefaultsQoS(),
+                pwm_outputs_subs[port_name] = this->create_subscription<std_msgs::msg::Float32>(topic, rclcpp::QoS(rclcpp::KeepLast(1)),
                     [this, port_name](const std_msgs::msg::Float32::SharedPtr msg) {
                         handle_set_duty(msg, port_name);});
 
@@ -63,6 +63,7 @@ void activate_outputs() {
             RCLCPP_ERROR(this->get_logger(), "Error occured while activating output (%s): %s", output.first.c_str(), e.what());
         }
     }
+    RCLCPP_DEBUG(this->get_logger(), "Activated outputs");
 }
 
 void deactivate_outputs() {

diff --git a/src/pwmlib.cpp b/src/pwmlib.cpp
@@ -5,6 +5,7 @@
 #include <thread>
 #include <chrono>
 #include <cmath>
+#include <algorithm>
 
 #include <fmt/core.h>
 
@@ -46,9 +47,13 @@ void PWMPort::set_duty_direct(int32_t duty) {
     duty_fs.close();
 }
 
+float PWMPort::apply_trim(float duty) {
+    duty += duty_trim;
+    return std::clamp(duty, -1.0f, 1.0f);
+}
+
 void PWMPort::set_duty_scaled(float duty) {
-    if (-1.0f > duty || duty > 1.0f)
-        throw std::runtime_error("Invalid duty provided. Duty must be between -1.0 and 1.0");
+    duty = apply_trim(duty);
 
     int32_t duty_value;
     if (channel_ppm) {