From 6897bfdc6bbafe202c65656f00bf539125145ec6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 20 Apr 2015 08:05:29 +1000
Subject: [PATCH] autotest: fixes for crrcsim with heli model

---
 Tools/autotest/pysim/crrcsim.py | 51 ++++++++++++++-------------------
 1 file changed, 22 insertions(+), 29 deletions(-)

diff --git a/Tools/autotest/pysim/crrcsim.py b/Tools/autotest/pysim/crrcsim.py
index 836b1b528a..847f87af16 100644
--- a/Tools/autotest/pysim/crrcsim.py
+++ b/Tools/autotest/pysim/crrcsim.py
@@ -23,7 +23,6 @@ class CRRCSim(Aircraft):
             self.decode_servos = self.decode_servos_heli
         else:
             self.decode_servos = self.decode_servos_fixed_wing
-        self.rsc_setpoint = 0.8
 
     def decode_servos_heli(self, servos):
         '''decode servos for heli'''
@@ -32,42 +31,36 @@ class CRRCSim(Aircraft):
         swash3 = servos[2]
         tail_rotor = servos[3]
         rsc = servos[7]
-        rsc = util.constrain(rsc, 0, self.rsc_setpoint)
 
-        # get total thrust from 0 to 1
-        rsc_scaling = (rsc/self.rsc_setpoint)
-        self.thrust = rsc_scaling*(swash1+swash2+swash3)/3.0
+        col_pitch = (swash1+swash2+swash3)/6.0
+        roll_rate = (swash1 - swash2)/2
+        pitch_rate = -((swash1 + swash2)/2.0 - swash3)/2
+        yaw_rate = -(tail_rotor - 0.5)
+        rsc_speed = rsc
 
-        # very simplistic mapping to body euler rates
-        self.roll_rate = (swash1 - swash2)*rsc_scaling
-        self.pitch_rate = -((swash1 + swash2)/2.0 - swash3)*rsc_scaling
-        self.yaw_rate = -(tail_rotor - 0.5)*rsc_scaling
+        buf = struct.pack('<fffff',
+                          util.constrain(roll_rate, -0.5, 0.5),
+                          util.constrain(pitch_rate, -0.5, 0.5),
+                          util.constrain(rsc, 0, 1),
+                          util.constrain(yaw_rate, -0.5, 0.5),
+                          util.constrain(col_pitch, -0.5, 0.5))
+        self.sim.send(buf)
 
     def decode_servos_fixed_wing(self, servos):
         '''decode servos for fixed wing'''
-        self.roll_rate = 2*(servos[0]-0.5)
-        self.pitch_rate = 2*(servos[1]-0.5)
-        self.thrust = servos[2]
-        self.yaw_rate = 2*(servos[3]-0.5)
-        
-
-    def send_servos(self, servos):
-        '''send servo packet'''
-        self.decode_servos(servos)
-
-        self.roll_rate = util.constrain(self.roll_rate, -1, 1)
-        self.pitch_rate = util.constrain(self.pitch_rate, -1, 1)
-        self.thrust = util.constrain(self.thrust, 0, 1)
-        self.yaw_rate = util.constrain(self.yaw_rate, -1, 1)
+        roll_rate = util.constrain(servos[0]-0.5, -0.5, 0.5)
+        pitch_rate = util.constrain(servos[1]-0.5, -0.5, 0.5)
+        throttle = util.constrain(servos[2], 0, 1)
+        yaw_rate = util.constrain(servos[3]-0.5, -0.5, 0.5)
 
         buf = struct.pack('<fffff',
-                          self.roll_rate,
-                          self.pitch_rate,
-                          self.thrust,
-                          self.yaw_rate,
+                          roll_rate,
+                          pitch_rate,
+                          throttle,
+                          yaw_rate,
                           0)
         self.sim.send(buf)
-
+        
     def recv_fdm(self):
         '''receive FDM packet'''
         try:
@@ -99,6 +92,6 @@ class CRRCSim(Aircraft):
 
     def update(self, actuators):
         '''update model'''
-        self.send_servos(actuators)
+        self.decode_servos(actuators)
         self.recv_fdm()
         self.update_position()