Implemneted force feedback support

src/force_feedback_handler.cpp

@@ -107,7 +107,19 @@ std::ostream& operator<<(std::ostream& out, const struct ff_effect& effect)
   return out;
 }
 
+ForceFeedbackEffect::ForceFeedbackEffect()
+  : playing(false),
+    count(0),
+    weak_magnitude(0),
+    strong_magnitude(0)
+{
+}
+
 ForceFeedbackEffect::ForceFeedbackEffect(const struct ff_effect& effect)
+  : playing(false),
+    count(0),
+    weak_magnitude(0),
+    strong_magnitude(0)
 {
   delay  = effect.replay.delay;
   length = effect.replay.length;
@@ -155,10 +167,76 @@ ForceFeedbackEffect::ForceFeedbackEffect(const struct ff_effect& effect)
         // case FF_DAMPER
         // case FF_INERTIA:
         // case FF_CUSTOM:
-        assert(!"Unsupported effect");
+        std::cout << "Unsupported effect" << std::endl;
+        start_weak_magnitude   = 0;
+        start_strong_magnitude = 0;
+        end_weak_magnitude     = 0;
+        end_strong_magnitude   = 0;
         break;
     }
 }
+
+static int get_pos(int start, int end, int pos, int len)
+{
+  int rel = end - start;
+  return start + (rel * pos / len);
+}
+
+void
+ForceFeedbackEffect::update(int msec_delta)
+{
+  if (playing)
+    {
+      count += msec_delta;
+
+      if (count > delay)
+        {
+          int t = count - delay;
+          if (t < envelope.attack_length)
+            { // attack
+              strong_magnitude = get_pos(start_strong_magnitude, end_strong_magnitude, t, length);
+              weak_magnitude   = get_pos(start_weak_magnitude,   end_weak_magnitude,   t, length);
+              
+              // apply envelope
+              strong_magnitude = strong_magnitude * t / envelope.attack_length;
+              weak_magnitude   = weak_magnitude   * t / envelope.attack_length;
+            }
+          else if  (t < length - envelope.fade_length)
+            { // sustain
+              strong_magnitude = get_pos(start_strong_magnitude, end_strong_magnitude, t, length);
+              weak_magnitude   = get_pos(start_weak_magnitude,   end_weak_magnitude,   t, length);
+            }
+          else if (t < length)
+            { // fade
+              strong_magnitude = get_pos(start_strong_magnitude, end_strong_magnitude, t, length);
+              weak_magnitude   = get_pos(start_weak_magnitude,   end_weak_magnitude,   t, length);
+
+              // apply envelope
+              strong_magnitude = strong_magnitude * (envelope.fade_length - t) / envelope.fade_length;
+              weak_magnitude   = weak_magnitude   * (envelope.fade_length - t) / envelope.fade_length;
+            }
+          else
+            { // effect ended
+              stop();
+            }
+        }
+    }
+}
+
+void
+ForceFeedbackEffect::play()
+{
+  playing = true;
+}
+
+void
+ForceFeedbackEffect::stop()
+{
+  playing = false;
+  count = 0;
+  weak_magnitude   = 0;
+  strong_magnitude = 0;
+}
 
 ForceFeedbackHandler::ForceFeedbackHandler()
   : max_effects(16),
@@ -209,7 +287,7 @@ ForceFeedbackHandler::play(int id)
 
   std::map<int, ForceFeedbackEffect>::iterator i = effects.find(id);
   if (i != effects.end())
-    ; // play
+    i->second.play();
   else
     std::cout << "ForceFeedbackHandler::play: Unknown id " << id << std::endl;
 }
@@ -221,7 +299,7 @@ ForceFeedbackHandler::stop(int id)
 
   std::map<int, ForceFeedbackEffect>::iterator i = effects.find(id);
   if (i != effects.end())
-    ; // stop
+    i->second.stop();
   else
     std::cout << "ForceFeedbackHandler::play: Unknown id " << id << std::endl;
 }
@@ -229,9 +307,21 @@ ForceFeedbackHandler::stop(int id)
 void
 ForceFeedbackHandler::update(int msec_delta)
 {
-  for(Effects::iterator i = effects.begin(); i != effects.end(); ++i)
+  weak_magnitude   = 0;
+  strong_magnitude = 0;
+
+  if (!effects.empty())
     {
-      
+      for(Effects::iterator i = effects.begin(); i != effects.end(); ++i)
+        {
+          i->second.update(msec_delta);
+
+          weak_magnitude   += i->second.get_weak_magnitude();
+          strong_magnitude += i->second.get_strong_magnitude();
+        }
+
+      weak_magnitude   = std::min(weak_magnitude,   0x7fff);
+      strong_magnitude = std::min(strong_magnitude, 0x7fff);
     }
 }
 

src/force_feedback_handler.hpp

@@ -25,7 +25,7 @@
 class ForceFeedbackEffect
 {
 public:
-  ForceFeedbackEffect() {}
+  ForceFeedbackEffect();
   ForceFeedbackEffect(const struct ff_effect& e);
 
   // Delay before the effect start
@@ -66,6 +66,18 @@ public:
     int fade_length;
     int fade_level;
   } envelope;
+
+  bool playing;
+  int  count;
+  int  weak_magnitude;
+  int  strong_magnitude;
+
+  int  get_weak_magnitude()   const { return weak_magnitude; }
+  int  get_strong_magnitude() const { return strong_magnitude; }
+
+  void update(int msec_delta);
+  void play();
+  void stop();
 };
 
 /** */

src/linux_uinput.cpp

@@ -16,6 +16,7 @@
 **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
+#include <boost/format.hpp>
 #include <assert.h>
 #include <iostream>
 #include <sstream>
@@ -165,6 +166,12 @@ LinuxUinput::add_ff(uint16_t code)
     }  
 }
 
+void
+LinuxUinput::set_ff_callback(const boost::function<void (uint8_t, uint8_t)>& callback)
+{
+  ff_callback = callback;
+}
+
 void
 LinuxUinput::finish()
 {
@@ -214,6 +221,20 @@ LinuxUinput::update(int msec_delta)
   if (ff_bit)
     {
       assert(ff_handler);
+
+      ff_handler->update(msec_delta);
+
+      if (0)
+        std::cout << boost::format("%5d %5d") 
+          % ff_handler->get_weak_magnitude() 
+          % ff_handler->get_strong_magnitude() << std::endl;
+
+      if (ff_callback)
+        {
+          ff_callback(ff_handler->get_weak_magnitude()   / 128,
+                      ff_handler->get_strong_magnitude() / 128);
+        }
+      
       struct input_event ev;
 
       int ret = read(fd, &ev, sizeof(ev));

src/linux_uinput.hpp

@@ -19,6 +19,7 @@
 #ifndef HEADER_LINUX_UINPUT_HPP
 #define HEADER_LINUX_UINPUT_HPP
 
+#include <boost/function.hpp>
 #include <linux/uinput.h>
 #include <string>
 #include <stdint.h>
@@ -46,6 +47,7 @@ private:
   bool ff_lst[FF_CNT];
 
   ForceFeedbackHandler* ff_handler;
+  boost::function<void (uint8_t, uint8_t)> ff_callback;
 
 public:
   LinuxUinput(const std::string& name, uint16_t vendor, uint16_t product);
@@ -62,7 +64,9 @@ public:
   void add_rel(uint16_t code);
 
   void add_ff(uint16_t code);
-  
+
+  void set_ff_callback(const boost::function<void (uint8_t, uint8_t)>& callback);
+
   /** Finish*/
   void finish();
   /*@}*/

src/uinput.cpp

@@ -911,4 +911,10 @@ uInput::get_joystick_uinput() const
   return joystick_uinput_dev.get();
 }
 
+void
+uInput::set_ff_callback(const boost::function<void (uint8_t, uint8_t)>& callback)
+{
+  get_joystick_uinput()->set_ff_callback(callback);
+}
+
 /* EOF */

src/uinput.hpp

@@ -154,6 +154,8 @@ public:
 
   void update(int msec_delta);
 
+  void set_ff_callback(const boost::function<void (uint8_t, uint8_t)>& callback);
+
   LinuxUinput* get_mouse_uinput() const;
   LinuxUinput* get_keyboard_uinput() const;
   LinuxUinput* get_joystick_uinput() const;

src/xboxdrv.cpp

@@ -1336,6 +1336,7 @@ void run_main(CommandLineOptions& opts)
             {
               std::cout << "\nStarting with uinput... " << std::flush;
               uinput = new uInput(dev_type, opts.uinput_config);
+              uinput->set_ff_callback(boost::bind(&XboxGenericController::set_rumble, controller, _1, _2));
               std::cout << "done" << std::endl;
             }
           else