trigger CommandInjection also in the injection port

Otherwise one won't be able to use the command injection to
salvage the vehicle if the 'regular' controllers stop producing
commands.

auv_control.orogen

@@ -412,4 +412,6 @@ task_context "CommandInjection", subclasses: 'Base' do
 
     # The output command
     output_port "cmd_out", "/base/LinearAngular6DCommand"
+
+    port_driven
 end

tasks/CommandInjection.cpp

@@ -80,10 +80,32 @@ bool CommandInjection::startHook()
 void CommandInjection::updateHook()
 {
     CommandInjectionBase::updateHook();
+    if (state() != CONTROLLING)
+        outputInjectedCommand();
 }
 void CommandInjection::errorHook()
 {
     CommandInjectionBase::errorHook();
+    if (state() != CONTROLLING)
+        outputInjectedCommand();
+}
+void CommandInjection::outputInjectedCommand()
+{
+    base::LinearAngular6DCommand cmd_injection;
+    if(receiveCommandInjection(cmd_injection))
+    {
+        const ExpectedInputs& expected_inputs = _expected_inputs.value();
+        for(unsigned i = 0; i < 3; i++)
+        {
+            if(!expected_inputs.linear[i])
+                cmd_injection.linear[i] = base::unknown<float>();
+            if(!expected_inputs.angular[i])
+                cmd_injection.angular[i] = base::unknown<float>();
+        }
+
+        if (verifyMissingData(_expected_inputs.get(), cmd_injection))
+            _cmd_out.write(cmd_injection);
+    }
 }
 void CommandInjection::stopHook()
 {

tasks/CommandInjection.hpp

@@ -29,6 +29,7 @@ namespace auv_control {
         base::Time newest_injection_sample;
         bool calcOutput(const LinearAngular6DCommandStatus &merged_command);
         bool receiveCommandInjection(base::LinearAngular6DCommand& cmd_injection);
+        void outputInjectedCommand();
 
     public:
         /** TaskContext constructor for CommandInjection

test/test_command_injection.rb

@@ -0,0 +1,32 @@
+require 'minitest/spec'
+require 'orocos/test/component'
+require 'minitest/autorun'
+
+describe 'auv_control::PIDController' do
+    include Orocos::Test::Component
+
+    start  'injection', 'auv_control::CommandInjection' => 'injection'
+    reader 'injection', 'cmd_out', attr_name: 'cmd_out'
+    writer 'injection', 'cmd_injection', attr_name: 'cmd_injection'
+
+    before do
+        injection.expected_inputs = Hash[linear: [true, true, false], angular: [false, false, false]]
+        injection.addCommandInput 'controller', Time.at(0)
+        injection.configure
+        injection.start
+    end
+
+    it "outputs the injected command if it matches the expectations regardless of the other input states" do
+        injected_cmd = Types.base.commands.LinearAngular6DCommand.new(
+            time: Time.now, linear: Eigen::Vector3.new(1, 2, 0), angular: Eigen::Vector3.Zero)
+
+        w = injection.cmd_injection.writer
+        w.write injected_cmd
+        sample = assert_has_one_new_sample cmd_out
+        assert_equal 1, sample.linear[0]
+        assert_equal 2, sample.linear[1]
+        assert Base.unknown?(sample.linear[2])
+        3.times { |i| assert Base.unknown?(sample.angular[i]) }
+    end
+end
+