From 0c5160f05c042e629ee4f30330507d6e234dcbf5 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 4 Apr 2024 14:07:05 +0100 Subject: [PATCH] AP_Volz_Protocol: add range parameter allowing smaller range for improved PWM resolution --- .../AP_Volz_Protocol/AP_Volz_Protocol.cpp | 49 ++++++++++++------- libraries/AP_Volz_Protocol/AP_Volz_Protocol.h | 13 +---- 2 files changed, 33 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index e5fdb3029a..6667645a47 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -13,6 +13,18 @@ #include #include +#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; iget_output_pwm() < VOLZ_PWM_POSITION_MIN) { - value = 0; - } else { - value = c->get_output_pwm() - VOLZ_PWM_POSITION_MIN; + uint16_t pwm = c->get_output_pwm(); + if (pwm == 0) { + // 0 PMW should stop outputting, for example in "safe" + // There is no way to de-power, move to trim + pwm = c->get_trim(); } - // scale the PWM value to Volz value - value = value * VOLZ_SCALE_VALUE / (VOLZ_PWM_POSITION_MAX - VOLZ_PWM_POSITION_MIN); - value = value + VOLZ_EXTENDED_POSITION_MIN; + // Map PWM to angle, this is a un-constrained interpolation + // ratio = 0 at PWM_POSITION_MIN to 1 at PWM_POSITION_MAX + 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 - if (value > VOLZ_EXTENDED_POSITION_MAX) { - value = VOLZ_EXTENDED_POSITION_MAX; - } + // Map angle to command out of full range, add 0.5 so that float to int truncation rounds correctly + const uint16_t value = linear_interpolate(EXTENDED_POSITION_MIN, EXTENDED_POSITION_MAX, angle, ANGLE_POSITION_MIN, ANGLE_POSITION_MAX) + 0.5; // prepare Volz protocol data. CMD cmd; diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index 5497357342..1541955319 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -44,18 +44,6 @@ #include #include -#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; };