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 diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index fdd788b42972c..84091522e9ff4 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -12,6 +12,22 @@ #include #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 + +#define UART_BUFSIZE_RX 128 +#define UART_BUFSIZE_TX 128 extern const AP_HAL::HAL& hal; @@ -23,6 +39,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 }; @@ -35,89 +57,133 @@ 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() < VOLZ_DATA_FRAME_SIZE) { - 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; - last_volz_update_time = now; + // receive packet is same length as sent, double to allow some time for the servo respond + const uint16_t receive_us = send_us * 2; - uint8_t i; - uint16_t value; + // 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... - // loop for all channels - for (i=0; idelay_microseconds(send_us + receive_us); + + 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() < VOLZ_PWM_POSITION_MIN) { - value = 0; - } else { - value = c->get_output_pwm() - VOLZ_PWM_POSITION_MIN; + last_sent_index = index; + + // Get PWM from saved array + const uint16_t pwm = servo_pwm[index]; + if (pwm == 0) { + // 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; } - // 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. - uint8_t data[VOLZ_DATA_FRAME_SIZE]; + CMD cmd; + cmd.ID = SET_EXTENDED_POSITION_CMD; + 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; + } + } +} - 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); +// 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; + } - send_command(data); + // 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(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,37 +193,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); -} - -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; iwrite(cmd.data, sizeof(cmd)); } #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 c4f5705bd0db4..d1ccfbe8416f7 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -43,18 +43,7 @@ #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 +#include class AP_Volz_Protocol { public: @@ -64,21 +53,38 @@ 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; - + + // Loop in thread to output to uart + void loop(); + uint8_t last_sent_index; + void init(void); - void send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE]); - void update_volz_bitmask(uint32_t new_bitmask); + void send_command(CMD &cmd); - 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; bool initialised; };