diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index 6667645a47..84091522e9 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(); + 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); - update_volz_bitmask(bitmask); + if (port == nullptr) { + // No port configured + return; + } + + // 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"); + } } -void AP_Volz_Protocol::update() +void AP_Volz_Protocol::loop() { - if (!initialised) { - initialised = true; - init(); - } - - if (port == nullptr) { - return; - } + 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); - if (last_used_bitmask != uint32_t(bitmask.get())) { - update_volz_bitmask(bitmask); - } + // 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; - uint32_t now = AP_HAL::micros(); - if (((now - last_volz_update_time) < volz_time_frame_micros) || - (port->txspace() < sizeof(CMD))) { - return; - } + // 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 1541955319..d1ccfbe841 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 @@ private: 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;