created a task to calculate the thruster generated force based on the…

… thruster feedback

This task gets the RPM or PWM rotation feedback provided by the thruster
and calculates the generate force. It does the oppposite of the
ThrustersInput task, where the desired thruster force is converted to
the necessary thruster command in terms of RPM or PWM.

auv_control.orogen

@@ -352,6 +352,20 @@ task_context "ThrustersInput" do
     port_driven 'cmd_in'                   
 end
 
+# Task that converts thruster's feedback from rpm to force.
+task_context "ThrustersFeedback" do
+    needs_configuration
+    subclasses "ThrustersInput"
+
+    # Feedback of the thrusters' rotation speed
+    input_port "joint_samples_in", "base::commands::Joints"
+
+    # Resulting forces for each one of the thrusters
+    output_port "forces_out", "base::commands::Joints"
+
+    port_driven 'joint_samples_in'
+end
+
 # Task that works as switch betwen two commands
 task_context "CommandInjection" do
     subclasses "Base"

tasks/ThrustersFeedback.cpp

@@ -0,0 +1,98 @@
+/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */
+
+#include "ThrustersFeedback.hpp"
+#include "base/commands/Joints.hpp"
+
+using namespace auv_control;
+
+ThrustersFeedback::ThrustersFeedback(std::string const& name)
+    : ThrustersFeedbackBase(name)
+{
+}
+
+ThrustersFeedback::ThrustersFeedback(std::string const& name, RTT::ExecutionEngine* engine)
+    : ThrustersFeedbackBase(name, engine)
+{
+}
+
+ThrustersFeedback::~ThrustersFeedback()
+{
+}
+
+
+
+/// The following lines are template definitions for the various state machine
+// hooks defined by Orocos::RTT. See ThrustersFeedback.hpp for more detailed
+// documentation about them.
+
+bool ThrustersFeedback::configureHook()
+{
+    if (! ThrustersFeedbackBase::configureHook())
+        return false;
+    return true;
+}
+bool ThrustersFeedback::startHook()
+{
+    if (! ThrustersFeedbackBase::startHook())
+        return false;
+    return true;
+}
+void ThrustersFeedback::updateHook()
+{
+    base::commands::Joints joint_samples;
+    if(_joint_samples_in.read(joint_samples) == RTT::NewData)
+    {
+        if(ThrustersInput::checkControlInput(joint_samples, controlModes[0]))
+        {
+            base::commands::Joints output = calcOutput(joint_samples);
+            _forces_out.write(output);
+        }
+    }
+}
+void ThrustersFeedback::errorHook()
+{
+    ThrustersFeedbackBase::errorHook();
+}
+void ThrustersFeedback::stopHook()
+{
+    ThrustersFeedbackBase::stopHook();
+}
+void ThrustersFeedback::cleanupHook()
+{
+    ThrustersFeedbackBase::cleanupHook();
+}
+
+base::samples::Joints ThrustersFeedback::calcOutput(base::samples::Joints const &joint_samples) const
+{
+    base::samples::Joints forces;
+    forces.elements.resize(numberOfThrusters);
+    base::VectorXd feedback = base::VectorXd::Zero(numberOfThrusters);
+
+    for (uint i = 0; i < numberOfThrusters; i++)
+    {
+        // Force = Cv * Speed * |Speed|
+        if(controlModes[i] == base::JointState::SPEED)
+        {
+            feedback[i] = joint_samples.elements[i].speed;
+            if(joint_samples.elements[i].speed >= 0)
+                forces.elements[i].effort = coeffPos[i] * feedback[i] * fabs(feedback[i]);
+            else
+                forces.elements[i].effort = coeffNeg[i] * feedback[i] * fabs(feedback[i]);
+        }
+        // Force = Cv * V * |V|
+        // V = pwm * thrusterVoltage
+        else if(controlModes[i] == base::JointState::RAW)
+        {
+            feedback[i] = joint_samples.elements[i].raw * thrusterVoltage;
+            if(joint_samples.elements[i].speed >= 0)
+                forces.elements[i].effort = coeffPos[i] * feedback[i] * fabs(feedback[i]);
+            else
+                forces.elements[i].effort = coeffNeg[i] * feedback[i] * fabs(feedback[i]);
+        }
+    }
+
+    forces.time = joint_samples.time;
+    forces.names = joint_samples.names;
+
+    return forces;
+}

tasks/ThrustersFeedback.hpp

@@ -0,0 +1,111 @@
+/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */
+
+#ifndef AUV_CONTROL_THRUSTERSFEEDBACK_TASK_HPP
+#define AUV_CONTROL_THRUSTERSFEEDBACK_TASK_HPP
+
+#include "auv_control/ThrustersFeedbackBase.hpp"
+
+namespace auv_control{
+
+    /*! \class ThrustersFeedback
+     * \brief The task context provides and requires services. It uses an ExecutionEngine to perform its functions.
+     * Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification.
+     * In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow.
+     * Task that converts thruster's feedback from rpm to force.
+     * \details
+     * The name of a TaskContext is primarily defined via:
+     \verbatim
+     deployment 'deployment_name'
+         task('custom_task_name','auv_control::ThrustersFeedback')
+     end
+     \endverbatim
+     *  It can be dynamically adapted when the deployment is called with a prefix argument.
+     */
+    class ThrustersFeedback : public ThrustersFeedbackBase
+    {
+	friend class ThrustersFeedbackBase;
+    protected:
+
+	    base::samples::Joints calcOutput(base::samples::Joints const &joint_samples) const;
+	    bool checkControlInput(base::samples::Joints const &cmd_in);
+
+    public:
+        /** TaskContext constructor for ThrustersFeedback
+         * \param name Name of the task. This name needs to be unique to make it identifiable via nameservices.
+         * \param initial_state The initial TaskState of the TaskContext. Default is Stopped state.
+         */
+        ThrustersFeedback(std::string const& name = "auv_control::ThrustersFeedback");
+
+        /** TaskContext constructor for ThrustersFeedback
+         * \param name Name of the task. This name needs to be unique to make it identifiable for nameservices.
+         * \param engine The RTT Execution engine to be used for this task, which serialises the execution of all commands, programs, state machines and incoming events for a task.
+         * 
+         */
+        ThrustersFeedback(std::string const& name, RTT::ExecutionEngine* engine);
+
+        /** Default deconstructor of ThrustersFeedback
+         */
+	~ThrustersFeedback();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from PreOperational to Stopped. If it returns false, then the
+         * component will stay in PreOperational. Otherwise, it goes into
+         * Stopped.
+         *
+         * It is meaningful only if the #needs_configuration has been specified
+         * in the task context definition with (for example):
+         \verbatim
+         task_context "TaskName" do
+           needs_configuration
+           ...
+         end
+         \endverbatim
+         */
+        bool configureHook();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from Stopped to Running. If it returns false, then the component will
+         * stay in Stopped. Otherwise, it goes into Running and updateHook()
+         * will be called.
+         */
+        bool startHook();
+
+        /** This hook is called by Orocos when the component is in the Running
+         * state, at each activity step. Here, the activity gives the "ticks"
+         * when the hook should be called.
+         *
+         * The error(), exception() and fatal() calls, when called in this hook,
+         * allow to get into the associated RunTimeError, Exception and
+         * FatalError states.
+         *
+         * In the first case, updateHook() is still called, and recover() allows
+         * you to go back into the Running state.  In the second case, the
+         * errorHook() will be called instead of updateHook(). In Exception, the
+         * component is stopped and recover() needs to be called before starting
+         * it again. Finally, FatalError cannot be recovered.
+         */
+        void updateHook();
+
+        /** This hook is called by Orocos when the component is in the
+         * RunTimeError state, at each activity step. See the discussion in
+         * updateHook() about triggering options.
+         *
+         * Call recover() to go back in the Runtime state.
+         */
+        void errorHook();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from Running to Stopped after stop() has been called.
+         */
+        void stopHook();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from Stopped to PreOperational, requiring the call to configureHook()
+         * before calling start() again.
+         */
+        void cleanupHook();
+    };
+}
+
+#endif
+

test/auv_control::ThrustersFeedback.yml

@@ -0,0 +1,31 @@
+--- name:default
+# If left empty, uses RAW by default
+control_modes: [RAW, RAW]
+# Convert thruster signal into forces, in negative direction or CCW.
+# Should have a size equal to the number of actuators
+# Thruster[N] = Coeff * rotation * |rotation|
+thruster_coeff_neg:
+  data: [6.698667e-5, 6.698667e-5]
+# Convert thruster signal into forces, in positive direction or CW.
+# Should have a size equal to the number of actuators
+# Thruster[N] = Coeff * rotation * |rotation|
+thruster_coeff_pos:
+  data: [6.698667e-5, 6.698667e-5]
+# In case the control_modes is RAW (pwm), used to convert the signal into DC Voltage
+# Thruster[N] = Coeff * voltage * |voltage|
+thruster_voltage: 19
+
+--- name:control_mode_3_raw
+control_modes: [RAW, RAW, RAW]
+
+--- name:thruster_coeff_pos_size_3
+thruster_coeff_pos:
+  data: [10, 10, 10]
+
+--- name:control_modes_effort
+# If left empty, uses RAW by default
+control_modes: [EFFORT, EFFORT]
+
+--- name:control_modes_speed
+# If left empty, uses RAW by default
+control_modes: [SPEED, SPEED]