Merge openplotter:pypilot

pypilot/arduino_servo/arduino_servo.cpp

@@ -126,7 +126,7 @@ int ArduinoServo::process_packet(uint8_t *in_buf)
         if(value == 65535)
             rudder = NAN;
         else
-            rudder = (uint16_t)value / 65472.0; // nominal range of 0 to 1
+            rudder = (uint16_t)value / 65472.0 - 0.5; // nominal range of -0.5 to 0.5
 
         return RUDDER;
     case FLAGS_CODE:
@@ -274,8 +274,8 @@ bool ArduinoServo::fault()
 void ArduinoServo::params(double _raw_max_current, double _rudder_min, double _rudder_max, double _max_current, double _max_controller_temp, double _max_motor_temp, double _rudder_range, double _rudder_offset, double _rudder_scale, double _rudder_nonlinearity, double _max_slew_speed, double _max_slew_slow, double _current_factor, double _current_offset, double _voltage_factor, double _voltage_offset, double _min_speed, double _max_speed, double _gain)
 {
     raw_max_current = fmin(60, fmax(0, _raw_max_current));
-    rudder_min = fmin(1, fmax(0, _rudder_min));
-    rudder_max = fmin(1, fmax(0, _rudder_max));
+    rudder_min = fmin(.5, fmax(-.5, _rudder_min));
+    rudder_max = fmin(.5, fmax(-.5, _rudder_max));
 
     max_current = fmin(60, fmax(0, _max_current));
     eeprom.set_max_current(max_current);
@@ -289,7 +289,7 @@ void ArduinoServo::params(double _raw_max_current, double _rudder_min, double _r
     rudder_range = fmin(120, fmax(0, _rudder_range));
     eeprom.set_rudder_range(rudder_range);
 
-    rudder_offset = fmin(1000, fmax(-1000, _rudder_offset));
+    rudder_offset = fmin(500, fmax(-500, _rudder_offset));
     eeprom.set_rudder_offset(rudder_offset);
 
     rudder_scale = fmin(4000, fmax(-4000, _rudder_scale));
@@ -374,10 +374,10 @@ void ArduinoServo::send_params()
                    ((int)round(rudder_max*255) & 0xff)); 
 */
         // instead use 16 bit rudder ranges
-        send_value(RUDDER_MIN_CODE, (int)round(rudder_min*65472));
+        send_value(RUDDER_MIN_CODE, (int)round((rudder_min+0.5)*65472));
         break;
     case 14:
-        send_value(RUDDER_MAX_CODE, (int)round(rudder_max*65472));
+        send_value(RUDDER_MAX_CODE, (int)round((rudder_max+0.5)*65472));
         break;
     case 18:
         send_value(MAX_SLEW_CODE,

pypilot/arduino_servo/arduino_servo_eeprom.cpp

@@ -93,15 +93,15 @@ void arduino_servo_eeprom::set_rudder_range(double rudder_range)
     local.rudder_range = round(rudder_range * 2); // from 0 to 120 in 0.5 increments
 }
 
-// store offset as s10.5 fixed point
+// store offset as s9.6 fixed point
 double arduino_servo_eeprom::get_rudder_offset()
 {
-    return frombase255s(arduino.rudder_offset)/32.0;
+    return frombase255s(arduino.rudder_offset)/64.0;
 }
 
 void arduino_servo_eeprom::set_rudder_offset(double rudder_offset)
 {
-    local.rudder_offset = tobase255s(round(rudder_offset * 32));
+    local.rudder_offset = tobase255s(round(rudder_offset * 64));
 }
 
 // store rudder scale s12.3 fixed point

pypilot/rudder.py

@@ -21,17 +21,18 @@ def __init__(self, server):
         self.speed = self.Register(SensorValue, 'speed', timestamp)
         self.last = 0
         self.last_time = time.time()
-        self.offset = self.Register(Value, 'offset', -100, persistent=True)
-        self.scale = self.Register(Value, 'scale', 220, persistent=True)
+        self.offset = self.Register(Value, 'offset', 0, persistent=True)
+        self.scale = self.Register(Value, 'scale', 100, persistent=True)
         self.nonlinearity = self.Register(Value, 'nonlinearity',  0, persistent=True)
         self.calibration_state = self.Register(EnumProperty, 'calibration_state', 'idle', ['idle', 'reset', 'centered', 'starboard range', 'port range', 'auto gain'])
         self.calibration_raw = {}
         self.range = self.Register(RangeProperty, 'range',  45, 10, 100, persistent=True)
         self.lastrange = 0
-        self.minmax = 0, 1
+        self.minmax = -.5, .5
         self.autogain_state = 'idle'
         self.raw = 0
 
+    # calculate minimum and maximum raw rudder value in the range -0.5 to 0.5
     def update_minmax(self):        
         scale = self.scale.value
         offset = self.offset.value
@@ -59,8 +60,8 @@ def update_minmax(self):
     def calibration(self, command):
         if command == 'reset':
             self.nonlinearity.update(0.0)
-            self.scale.update(220.0)
-            self.offset.update(-100.0)
+            self.scale.update(100.0)
+            self.offset.update(0.0)
             self.update_minmax()
             self.calibration_raw = {}
             return
@@ -78,6 +79,7 @@ def calibration(self, command):
         # raw range 0 to 1
         self.calibration_raw[command] = {'raw': self.raw,
                                          'rudder': true_angle}
+
         scale = self.scale.value
         offset = self.offset.value
         nonlinearity = self.nonlinearity.value
@@ -87,7 +89,7 @@ def calibration(self, command):
         for c in ['starboard range', 'centered', 'port range']:
             if c in self.calibration_raw:
                 p.append(self.calibration_raw[c])
-
+                
         l = len(p)
         # 1 point, estimate offset
         if l == 1:
@@ -113,8 +115,6 @@ def calibration(self, command):
                 scale = (rudder2 - rudder0)/(raw2 - raw0)
                 offset = rudder0 - scale*raw0
                 nonlinearity = (rudder1 - scale*raw1 - offset)/(raw0-raw1)/(raw2-raw1)
-                self.minmax = raw0, raw1
-                self.lastrange = 0 # force update minmax
             else:
                 print('bad rudder calibration', self.calibration_raw)