From 65032919e200f5a68cc9519b83af68094bd1144b Mon Sep 17 00:00:00 2001 From: Guy Tzoler Date: Fri, 3 Nov 2017 13:27:29 +1100 Subject: [PATCH] AP_Volz_Protocol: added Volz protocol library --- .../AP_Volz_Protocol/AP_Volz_Protocol.cpp | 155 ++++++++++++++++++ libraries/AP_Volz_Protocol/AP_Volz_Protocol.h | 81 +++++++++ 2 files changed, 236 insertions(+) create mode 100644 libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp create mode 100644 libraries/AP_Volz_Protocol/AP_Volz_Protocol.h diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp new file mode 100644 index 0000000000..ba5c652b72 --- /dev/null +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -0,0 +1,155 @@ +/* + * AP_VOLZ_PROTOCOL.cpp + * + * Created on: Oct 31, 2017 + * Author: guy + */ +#include +#include + +#include "AP_Volz_Protocol.h" + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_Volz_Protocol::var_info[] = { + // @Param: MASK + // @DisplayName: Channel Bitmask + // @Description: Enable of volz servo protocol to specific channels + // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 + // @User: Standard + AP_GROUPINFO("MASK", 1, AP_Volz_Protocol, bitmask, 0), + + AP_GROUPEND +}; + +// constructor +AP_Volz_Protocol::AP_Volz_Protocol(void) +{ + // set defaults from the parameter table + AP_Param::setup_object_defaults(this, var_info); +} + +void AP_Volz_Protocol::init(void) +{ + AP_SerialManager *serial_manager = AP_SerialManager::get_instance(); + if (!serial_manager) { + return; + } + 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 (port == nullptr) { + return; + } + + if (last_used_bitmask != uint32_t(bitmask.get())) { + update_volz_bitmask(bitmask); + } + + uint32_t now = AP_HAL::micros(); + if (now - last_volz_update_time < volz_time_frame_micros || + port->txspace() < VOLZ_DATA_FRAME_SIZE) { + return; + } + + last_volz_update_time = now; + + uint8_t i; + uint16_t value; + + // loop for all 16 channels + for (i=0; iget_output_pwm() < ch->get_output_min()) { + value = 0; + } else { + value = ch->get_output_pwm() - ch->get_output_min(); + } + + // scale the PWM value to Volz value + value = value + VOLZ_EXTENDED_POSITION_MIN; + value = value * VOLZ_SCALE_VALUE / (ch->get_output_max() - ch->get_output_min()); + + // make sure value stays in range + if (value > VOLZ_EXTENDED_POSITION_MAX) { + value = VOLZ_EXTENDED_POSITION_MAX; + } + + // prepare Volz protocol data. + uint8_t data[VOLZ_DATA_FRAME_SIZE]; + + 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); + } + } +} + +// calculate CRC for volz serial protocol and send the data. +void AP_Volz_Protocol::send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE]) +{ + uint8_t i,j; + uint16_t crc = 0xFFFF; + + // calculate Volz CRC value according to protocol definition + for(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++) { + + if (crc & 0x8000) { + crc = (crc << 1) ^ 0x8005; + } else { + crc = crc << 1; + } + } + } + + // add CRC result to the message + data[4] = HIGHBYTE(crc); + data[5] = LOWBYTE(crc); + port->write(data, VOLZ_DATA_FRAME_SIZE); +} + +void AP_Volz_Protocol::update_volz_bitmask(uint32_t new_bitmask) +{ + uint8_t count = 0; + last_used_bitmask = new_bitmask; + + for (uint8_t i=0; i +#include +#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 + +class AP_Volz_Protocol { +public: + static const struct AP_Param::GroupInfo var_info[]; + + static AP_Volz_Protocol create() { + return AP_Volz_Protocol{}; + } + + constexpr AP_Volz_Protocol(AP_Volz_Protocol &&other) = default; + + /* Do not allow copies */ + AP_Volz_Protocol(const AP_Volz_Protocol &other) = delete; + AP_Volz_Protocol &operator=(const AP_Volz_Protocol&) = delete; + + void update(); + +private: + AP_Volz_Protocol(); + AP_HAL::UARTDriver *port; + + void init(void); + void send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE]); + 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; + + AP_Int32 bitmask; + bool initialised; +};