From b91f1d5cf0944314c179ea11a6d0cd41ac484ceb Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 4 Apr 2024 14:05:19 +0100 Subject: [PATCH 1/5] AP_Volz_Protocol: add CMD union helper --- .../AP_Volz_Protocol/AP_Volz_Protocol.cpp | 33 +++++++++---------- libraries/AP_Volz_Protocol/AP_Volz_Protocol.h | 20 +++++++++-- 2 files changed, 32 insertions(+), 21 deletions(-) diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index fdd788b42972c..211128723e307 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -56,8 +56,8 @@ void AP_Volz_Protocol::update() } uint32_t now = AP_HAL::micros(); - if (now - last_volz_update_time < volz_time_frame_micros || - port->txspace() < VOLZ_DATA_FRAME_SIZE) { + if (((now - last_volz_update_time) < volz_time_frame_micros) || + (port->txspace() < sizeof(CMD))) { return; } @@ -93,31 +93,28 @@ void AP_Volz_Protocol::update() } // prepare Volz protocol data. - uint8_t data[VOLZ_DATA_FRAME_SIZE]; + CMD cmd; + cmd.ID = SET_EXTENDED_POSITION_CMD; + cmd.actuator_id = i + 1; // send actuator id as 1 based index so ch1 will have id 1, ch2 will have id 2 .... + cmd.arg1 = HIGHBYTE(value); + cmd.arg2 = LOWBYTE(value); - data[0] = VOLZ_SET_EXTENDED_POSITION_CMD; - data[1] = i + 1; // send actuator id as 1 based index so ch1 will have id 1, ch2 will have id 2 .... - data[2] = HIGHBYTE(value); - data[3] = LOWBYTE(value); - - send_command(data); + send_command(cmd); } } } // calculate CRC for volz serial protocol and send the data. -void AP_Volz_Protocol::send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE]) +void AP_Volz_Protocol::send_command(CMD &cmd) { - uint8_t i,j; uint16_t crc = 0xFFFF; // calculate Volz CRC value according to protocol definition - for(i=0; i<4; i++) { + for(uint8_t i=0; i<4; i++) { // take input data into message that will be transmitted. - crc = ((data[i] << 8) ^ crc); - - for(j=0; j<8; j++) { + crc = (cmd.data[i] << 8) ^ crc; + for(uint8_t j=0; j<8; j++) { if (crc & 0x8000) { crc = (crc << 1) ^ 0x8005; } else { @@ -127,9 +124,9 @@ void AP_Volz_Protocol::send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE]) } // add CRC result to the message - data[4] = HIGHBYTE(crc); - data[5] = LOWBYTE(crc); - port->write(data, VOLZ_DATA_FRAME_SIZE); + cmd.crc1 = HIGHBYTE(crc); + cmd.crc2 = LOWBYTE(crc); + port->write(cmd.data, sizeof(cmd)); } void AP_Volz_Protocol::update_volz_bitmask(uint32_t new_bitmask) diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index c4f5705bd0db4..5497357342294 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -64,14 +64,28 @@ class AP_Volz_Protocol { CLASS_NO_COPY(AP_Volz_Protocol); static const struct AP_Param::GroupInfo var_info[]; - + void update(); private: + + // Command frame + union CMD { + struct PACKED { + uint8_t ID; // CMD ID + uint8_t actuator_id; // actuator send to or receiving from + uint8_t arg1; // CMD dependant argument 1 + uint8_t arg2; // CMD dependant argument 2 + uint8_t crc1; + uint8_t crc2; + }; + uint8_t data[6]; + }; + AP_HAL::UARTDriver *port; - + void init(void); - void send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE]); + void send_command(CMD &cmd); void update_volz_bitmask(uint32_t new_bitmask); uint32_t last_volz_update_time; From fa0ce9c8777e7d8337d34c49fd08a1a90ce00519 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 4 Apr 2024 14:05:51 +0100 Subject: [PATCH 2/5] AP_Volz_Protocol: use popcount --- libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index 211128723e307..e5fdb3029aaad 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -131,15 +131,9 @@ void AP_Volz_Protocol::send_command(CMD &cmd) void AP_Volz_Protocol::update_volz_bitmask(uint32_t new_bitmask) { - uint8_t count = 0; + const uint8_t count = __builtin_popcount(new_bitmask); last_used_bitmask = new_bitmask; - for (uint8_t i=0; i Date: Thu, 4 Apr 2024 14:07:05 +0100 Subject: [PATCH 3/5] 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 e5fdb3029aaad..6667645a47438 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 5497357342294..1541955319532 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 @@ class AP_Volz_Protocol { uint32_t last_used_bitmask; AP_Int32 bitmask; + AP_Int16 range; bool initialised; }; From 453b7bd9c6a99b51578ab9360e6c5cd5befd3959 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 4 Apr 2024 17:00:42 +0100 Subject: [PATCH 4/5] AP_Volz_Protocol: move output to thread --- .../AP_Volz_Protocol/AP_Volz_Protocol.cpp | 136 +++++++++++------- libraries/AP_Volz_Protocol/AP_Volz_Protocol.h | 11 +- 2 files changed, 91 insertions(+), 56 deletions(-) diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index 6667645a47438..84091522e9ff4 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -12,6 +12,7 @@ #include #include +#include #define SET_EXTENDED_POSITION_CMD 0xDC @@ -25,6 +26,9 @@ #define ANGLE_POSITION_MAX 100.0 #define EXTENDED_POSITION_MAX 0x0F80 +#define UART_BUFSIZE_RX 128 +#define UART_BUFSIZE_TX 128 + extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_Volz_Protocol::var_info[] = { @@ -53,49 +57,71 @@ AP_Volz_Protocol::AP_Volz_Protocol(void) void AP_Volz_Protocol::init(void) { - AP_SerialManager &serial_manager = AP::serialmanager(); - port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Volz,0); - update_volz_bitmask(bitmask); -} - -void AP_Volz_Protocol::update() -{ - if (!initialised) { - initialised = true; - init(); + if (uint32_t(bitmask.get()) == 0) { + // No servos enabled + return; } - + + const AP_SerialManager &serial_manager = AP::serialmanager(); + port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Volz,0); if (port == nullptr) { + // No port configured return; } - if (last_used_bitmask != uint32_t(bitmask.get())) { - update_volz_bitmask(bitmask); + // Create thread to handle output + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Volz_Protocol::loop, void), + "Volz", + 1024, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) { + AP_BoardConfig::allocation_error("Volz thread"); } +} - uint32_t now = AP_HAL::micros(); - if (((now - last_volz_update_time) < volz_time_frame_micros) || - (port->txspace() < sizeof(CMD))) { - return; - } +void AP_Volz_Protocol::loop() +{ + const uint32_t baudrate = 115200; + port->begin(baudrate, UART_BUFSIZE_RX, UART_BUFSIZE_TX); + port->set_unbuffered_writes(true); + port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); + + // Calculate the amount of time it should take to send a command + // Multiply by 10 to convert from bit rate to byte rate (8 data bits + start and stop bits) + // B/s to s/B, 1000000 converts to microseconds, multiply by number of bytes + // 6 bytes at 11520 bytes per second takes 520 us + const uint16_t send_us = (sizeof(CMD) * 1000000 * 10) / baudrate; + + // receive packet is same length as sent, double to allow some time for the servo respond + const uint16_t receive_us = send_us * 2; - last_volz_update_time = now; + // This gives a total time of 1560ms, message rate of 641 Hz. + // One servo at 641Hz, two at 320.5 each, three at 213.7 each ect... + while (port != nullptr) { - // loop for all channels - for (uint8_t i=0; idelay_microseconds(send_us + receive_us); - const SRV_Channel *c = SRV_Channels::srv_channel(i); - if (c == nullptr) { + while (port->txspace() < sizeof(CMD)) { + // Wait until there is enough space to transmit a full command + hal.scheduler->delay_microseconds(100); + } + + // loop for all channels + for (uint8_t i=0; iget_output_pwm(); + last_sent_index = index; + + // Get PWM from saved array + const uint16_t pwm = servo_pwm[index]; 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(); + // Never use zero PWM, the check in update should ensure this never happens + // If we were to use zero the range extrapolation would result in a -100 deg angle request + continue; } // Map PWM to angle, this is a un-constrained interpolation @@ -110,15 +136,43 @@ void AP_Volz_Protocol::update() // prepare Volz protocol data. CMD cmd; cmd.ID = SET_EXTENDED_POSITION_CMD; - cmd.actuator_id = i + 1; // send actuator id as 1 based index so ch1 will have id 1, ch2 will have id 2 .... + cmd.actuator_id = index + 1; // send actuator id as 1 based index so ch1 will have id 1, ch2 will have id 2 .... cmd.arg1 = HIGHBYTE(value); cmd.arg2 = LOWBYTE(value); send_command(cmd); + break; } } } +// Called each time the servo outputs are sent +void AP_Volz_Protocol::update() +{ + if (!initialised) { + // One time setup + initialised = true; + init(); + } + + if (port == nullptr) { + // no point if we don't have a valid port + return; + } + + // take semaphore and loop for all channels + for (uint8_t i=0; iget_output_pwm(); + servo_pwm[i] = (pwm == 0) ? c->get_trim() : pwm; + } +} + // calculate CRC for volz serial protocol and send the data. void AP_Volz_Protocol::send_command(CMD &cmd) { @@ -144,26 +198,4 @@ void AP_Volz_Protocol::send_command(CMD &cmd) port->write(cmd.data, sizeof(cmd)); } -void AP_Volz_Protocol::update_volz_bitmask(uint32_t new_bitmask) -{ - const uint8_t count = __builtin_popcount(new_bitmask); - last_used_bitmask = new_bitmask; - - // have a safety margin of 20% to allow for not having full uart - // utilisation. We really don't want to start filling the uart - // buffer or we'll end up with servo lag - const float safety = 1.3; - - // each channel take about 425.347us to transmit so total time will be ~ number of channels * 450us - // rounded to 450 to make sure we don't go over the baud rate. - uint32_t channels_micros = count * 450 * safety; - - // limit the minimum to 2500 will result a max refresh frequency of 400hz. - if (channels_micros < 2500) { - channels_micros = 2500; - } - - volz_time_frame_micros = channels_micros; -} - #endif // AP_VOLZ_ENABLED diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index 1541955319532..d1ccfbe8416f7 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -43,6 +43,7 @@ #include #include +#include class AP_Volz_Protocol { public: @@ -72,13 +73,15 @@ class AP_Volz_Protocol { AP_HAL::UARTDriver *port; + // Loop in thread to output to uart + void loop(); + uint8_t last_sent_index; + void init(void); void send_command(CMD &cmd); - void update_volz_bitmask(uint32_t new_bitmask); - uint32_t last_volz_update_time; - uint32_t volz_time_frame_micros; - uint32_t last_used_bitmask; + // Incoming PWM commands from the servo lib + uint16_t servo_pwm[NUM_SERVO_CHANNELS]; AP_Int32 bitmask; AP_Int16 range; From 0bc0115f4c30ba8963d22e255a17c88578faaf43 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 4 Apr 2024 17:01:37 +0100 Subject: [PATCH 5/5] AP_SerialManager: remove Volz port config --- libraries/AP_SerialManager/AP_SerialManager.cpp | 11 +++-------- libraries/AP_SerialManager/AP_SerialManager_config.h | 2 -- 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index 9818738b3fb90..a42de342ab2e7 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -502,14 +502,9 @@ void AP_SerialManager::init() state[i].protocol.set_and_save(SerialProtocol_Rangefinder); break; case SerialProtocol_Volz: - // Note baudrate is hardcoded to 115200 - state[i].baud.set_and_default(AP_SERIALMANAGER_VOLZ_BAUD); // update baud param in case user looks at it - uart->begin(state[i].baudrate(), - AP_SERIALMANAGER_VOLZ_BUFSIZE_RX, - AP_SERIALMANAGER_VOLZ_BUFSIZE_TX); - uart->set_unbuffered_writes(true); - uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); - break; + // Note baudrate is hardcoded to 115200 + state[i].baud.set_and_default(AP_SERIALMANAGER_VOLZ_BAUD); // update baud param in case user looks at it + break; case SerialProtocol_Sbus1: state[i].baud.set_and_default(AP_SERIALMANAGER_SBUS1_BAUD / 1000); // update baud param in case user looks at it uart->begin(state[i].baudrate(), diff --git a/libraries/AP_SerialManager/AP_SerialManager_config.h b/libraries/AP_SerialManager/AP_SerialManager_config.h index 74ed01243bec2..7ed9e502d692f 100644 --- a/libraries/AP_SerialManager/AP_SerialManager_config.h +++ b/libraries/AP_SerialManager/AP_SerialManager_config.h @@ -114,8 +114,6 @@ #define AP_SERIALMANAGER_GIMBAL_BUFSIZE_TX 128 #define AP_SERIALMANAGER_VOLZ_BAUD 115 -#define AP_SERIALMANAGER_VOLZ_BUFSIZE_RX 128 -#define AP_SERIALMANAGER_VOLZ_BUFSIZE_TX 128 #define AP_SERIALMANAGER_ROBOTIS_BUFSIZE_RX 128 #define AP_SERIALMANAGER_ROBOTIS_BUFSIZE_TX 128