AP_Volz_Protocol: add range parameter allowing smaller range for improved PWM resolution

libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp

@@ -13,6 +13,18 @@
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <SRV_Channel/SRV_Channel.h>
 
+#define SET_EXTENDED_POSITION_CMD      0xDC
+
+// Extended Position Data Format defines -100 as 0x0080 decimal 128, we map this to a PWM of 1000 (if range is default)
+#define PWM_POSITION_MIN               1000
+#define ANGLE_POSITION_MIN            -100.0
+#define EXTENDED_POSITION_MIN          0x0080
+
+// Extended Position Data Format defines +100 as 0x0F80 decimal 3968, we map this to a PWM of 2000 (if range is default)
+#define PWM_POSITION_MAX               2000
+#define ANGLE_POSITION_MAX             100.0
+#define EXTENDED_POSITION_MAX          0x0F80
+
 extern const AP_HAL::HAL& hal;
 
 const AP_Param::GroupInfo AP_Volz_Protocol::var_info[] = {
@@ -23,6 +35,12 @@ const AP_Param::GroupInfo AP_Volz_Protocol::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("MASK",  1, AP_Volz_Protocol, bitmask, 0),
 
+    // @Param: RANGE
+    // @DisplayName: Range of travel
+    // @Description: Range to map between 1000 and 2000 PWM. Default value of 200 gives full +-100 deg range of extended position command. This results in 0.2 deg movement per US change in PWM. If the full range is not needed it can be reduced to increase resolution. 40 deg range gives 0.04 deg movement per US change in PWM, this is higher resolution than possible with the VOLZ protocol so further reduction in range will not improve resolution. Reduced range does allow PWMs outside the 1000 to 2000 range, with 40 deg range 750 PWM results in a angle of -30 deg, 2250 would be +30 deg. This is still limited by the 200 deg maximum range of the actuator.
+    // @Units: deg
+    AP_GROUPINFO("RANGE", 2, AP_Volz_Protocol, range, 200),
+
     AP_GROUPEND
 };
 
@@ -63,34 +81,31 @@ void AP_Volz_Protocol::update()
 
     last_volz_update_time = now;
 
-    uint8_t i;
-    uint16_t value;
 
     // loop for all channels
-    for (i=0; i<NUM_SERVO_CHANNELS; i++) {
+    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
         // check if current channel is needed for Volz protocol
         if (last_used_bitmask & (1U<<i)) {
 
-            SRV_Channel *c = SRV_Channels::srv_channel(i);
+            const SRV_Channel *c = SRV_Channels::srv_channel(i);
             if (c == nullptr) {
                 continue;
             }
-            
+            uint16_t pwm = c->get_output_pwm();
-            // check if current channel PWM is within range
+            if (pwm == 0) {
-            if (c->get_output_pwm() < VOLZ_PWM_POSITION_MIN) {
+                // 0 PMW should stop outputting, for example in "safe"
-                value = 0;
+                // There is no way to de-power, move to trim
-            } else {
+                pwm = c->get_trim();
-                value = c->get_output_pwm() - VOLZ_PWM_POSITION_MIN;
             }
 
-            // scale the PWM value to Volz value
+            // Map PWM to angle, this is a un-constrained interpolation
-            value = value * VOLZ_SCALE_VALUE / (VOLZ_PWM_POSITION_MAX - VOLZ_PWM_POSITION_MIN);
+            // ratio = 0 at PWM_POSITION_MIN to 1 at PWM_POSITION_MAX
-            value = value + VOLZ_EXTENDED_POSITION_MIN;
+            const float ratio = (float(pwm) - PWM_POSITION_MIN) / (PWM_POSITION_MAX - PWM_POSITION_MIN);
+            // Convert ratio to +-0.5 and multiply by stroke
+            const float angle = (ratio - 0.5) * constrain_float(range, 0.0, 200.0);
 
-            // make sure value stays in range
+            // Map angle to command out of full range, add 0.5 so that float to int truncation rounds correctly
-            if (value > VOLZ_EXTENDED_POSITION_MAX) {
+            const uint16_t value = linear_interpolate(EXTENDED_POSITION_MIN, EXTENDED_POSITION_MAX, angle, ANGLE_POSITION_MIN, ANGLE_POSITION_MAX) + 0.5;
-                value = VOLZ_EXTENDED_POSITION_MAX;
-            }
 
             // prepare Volz protocol data.
             CMD cmd;

libraries/AP_Volz_Protocol/AP_Volz_Protocol.h

@@ -44,18 +44,6 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Param/AP_Param.h>
 
-#define VOLZ_SCALE_VALUE 					(uint16_t)(VOLZ_EXTENDED_POSITION_MAX - VOLZ_EXTENDED_POSITION_MIN)	// Extended Position Data Format defines 100 as 0x0F80, which results in 1920 steps for +100 deg and 1920 steps for -100 degs meaning if you take movement a scaled between -1 ... 1 and multiply by 1920 you get the travel from center
-#define VOLZ_SET_EXTENDED_POSITION_CMD 		0xDC
-#define VOLZ_SET_EXTENDED_POSITION_RSP 		0x2C
-#define VOLZ_DATA_FRAME_SIZE		 		6
-
-#define VOLZ_EXTENDED_POSITION_MIN 			0x0080	// Extended Position Data Format defines -100 as 0x0080 decimal 128
-#define VOLZ_EXTENDED_POSITION_CENTER 		0x0800	// Extended Position Data Format defines 0 as 0x0800 - decimal 2048
-#define VOLZ_EXTENDED_POSITION_MAX 			0x0F80	// Extended Position Data Format defines +100 as 0x0F80 decimal 3968 -> full range decimal 3840
-
-#define VOLZ_PWM_POSITION_MIN				1000
-#define VOLZ_PWM_POSITION_MAX				2000
-
 class AP_Volz_Protocol {
 public:
     AP_Volz_Protocol();
@@ -93,6 +81,7 @@ private:
     uint32_t last_used_bitmask;
 
     AP_Int32 bitmask;
+    AP_Int16 range;
     bool initialised;
 };