Skip to content

Commit

Permalink
AP_Volz_Protocol: move output to thread
Browse files Browse the repository at this point in the history
  • Loading branch information
@IamPete1
IamPete1 committed Jul 8, 2024
1 parent d380394 commit 453b7bd
Show file tree
Hide file tree
Showing 2 changed files with 91 additions and 56 deletions.
136 changes: 84 additions & 52 deletions libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@

#include <AP_SerialManager/AP_SerialManager.h>
#include <SRV_Channel/SRV_Channel.h>
#include <AP_BoardConfig/AP_BoardConfig.h>

#define SET_EXTENDED_POSITION_CMD 0xDC

Expand All @@ -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[] = {
Expand Down Expand Up @@ -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; i<NUM_SERVO_CHANNELS; i++) {
// check if current channel is needed for Volz protocol
if (last_used_bitmask & (1U<<i)) {
// Wait the expected amount of time for the send and receive to complete so we don't step on the response
hal.scheduler->delay_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; i<ARRAY_SIZE(servo_pwm); i++) {
// Send each channels in turn
const uint8_t index = (last_sent_index + 1 + i) % ARRAY_SIZE(servo_pwm);
if ((uint32_t(bitmask.get()) & (1U<<index)) == 0) {
// Not configured to send
continue;
}
uint16_t pwm = c->get_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
Expand All @@ -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; i<ARRAY_SIZE(servo_pwm); i++) {
const SRV_Channel *c = SRV_Channels::srv_channel(i);
if (c == nullptr) {
continue;
}
// 0 PMW should stop outputting, for example in "safe"
// There is no way to de-power, move to trim
const uint16_t pwm = c->get_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)
{
Expand All @@ -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
11 changes: 7 additions & 4 deletions libraries/AP_Volz_Protocol/AP_Volz_Protocol.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@

#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <SRV_Channel/SRV_Channel_config.h>

class AP_Volz_Protocol {
public:
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 453b7bd

Please sign in to comment.