mirror of https://github.com/ArduPilot/ardupilot
AP_Volz_Protocol: move output to thread
This commit is contained in:
parent
0c5160f05c
commit
8eceff2d7a
|
@ -12,6 +12,7 @@
|
||||||
|
|
||||||
#include <AP_SerialManager/AP_SerialManager.h>
|
#include <AP_SerialManager/AP_SerialManager.h>
|
||||||
#include <SRV_Channel/SRV_Channel.h>
|
#include <SRV_Channel/SRV_Channel.h>
|
||||||
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||||
|
|
||||||
#define SET_EXTENDED_POSITION_CMD 0xDC
|
#define SET_EXTENDED_POSITION_CMD 0xDC
|
||||||
|
|
||||||
|
@ -25,6 +26,9 @@
|
||||||
#define ANGLE_POSITION_MAX 100.0
|
#define ANGLE_POSITION_MAX 100.0
|
||||||
#define EXTENDED_POSITION_MAX 0x0F80
|
#define EXTENDED_POSITION_MAX 0x0F80
|
||||||
|
|
||||||
|
#define UART_BUFSIZE_RX 128
|
||||||
|
#define UART_BUFSIZE_TX 128
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
const AP_Param::GroupInfo AP_Volz_Protocol::var_info[] = {
|
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)
|
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);
|
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) {
|
const uint32_t baudrate = 115200;
|
||||||
initialised = true;
|
port->begin(baudrate, UART_BUFSIZE_RX, UART_BUFSIZE_TX);
|
||||||
init();
|
port->set_unbuffered_writes(true);
|
||||||
}
|
port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
|
||||||
|
|
||||||
if (port == nullptr) {
|
// Calculate the amount of time it should take to send a command
|
||||||
return;
|
// 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;
|
||||||
|
|
||||||
if (last_used_bitmask != uint32_t(bitmask.get())) {
|
// receive packet is same length as sent, double to allow some time for the servo respond
|
||||||
update_volz_bitmask(bitmask);
|
const uint16_t receive_us = send_us * 2;
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t now = AP_HAL::micros();
|
// This gives a total time of 1560ms, message rate of 641 Hz.
|
||||||
if (((now - last_volz_update_time) < volz_time_frame_micros) ||
|
// One servo at 641Hz, two at 320.5 each, three at 213.7 each ect...
|
||||||
(port->txspace() < sizeof(CMD))) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
last_volz_update_time = now;
|
while (port != nullptr) {
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
// loop for all channels
|
while (port->txspace() < sizeof(CMD)) {
|
||||||
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
// Wait until there is enough space to transmit a full command
|
||||||
// check if current channel is needed for Volz protocol
|
hal.scheduler->delay_microseconds(100);
|
||||||
if (last_used_bitmask & (1U<<i)) {
|
}
|
||||||
|
|
||||||
const SRV_Channel *c = SRV_Channels::srv_channel(i);
|
// loop for all channels
|
||||||
if (c == nullptr) {
|
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;
|
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) {
|
if (pwm == 0) {
|
||||||
// 0 PMW should stop outputting, for example in "safe"
|
// Never use zero PWM, the check in update should ensure this never happens
|
||||||
// There is no way to de-power, move to trim
|
// If we were to use zero the range extrapolation would result in a -100 deg angle request
|
||||||
pwm = c->get_trim();
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Map PWM to angle, this is a un-constrained interpolation
|
// Map PWM to angle, this is a un-constrained interpolation
|
||||||
|
@ -110,15 +136,43 @@ void AP_Volz_Protocol::update()
|
||||||
// prepare Volz protocol data.
|
// prepare Volz protocol data.
|
||||||
CMD cmd;
|
CMD cmd;
|
||||||
cmd.ID = SET_EXTENDED_POSITION_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.arg1 = HIGHBYTE(value);
|
||||||
cmd.arg2 = LOWBYTE(value);
|
cmd.arg2 = LOWBYTE(value);
|
||||||
|
|
||||||
send_command(cmd);
|
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.
|
// calculate CRC for volz serial protocol and send the data.
|
||||||
void AP_Volz_Protocol::send_command(CMD &cmd)
|
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));
|
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
|
#endif // AP_VOLZ_ENABLED
|
||||||
|
|
|
@ -43,6 +43,7 @@
|
||||||
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <AP_Param/AP_Param.h>
|
#include <AP_Param/AP_Param.h>
|
||||||
|
#include <SRV_Channel/SRV_Channel_config.h>
|
||||||
|
|
||||||
class AP_Volz_Protocol {
|
class AP_Volz_Protocol {
|
||||||
public:
|
public:
|
||||||
|
@ -72,13 +73,15 @@ private:
|
||||||
|
|
||||||
AP_HAL::UARTDriver *port;
|
AP_HAL::UARTDriver *port;
|
||||||
|
|
||||||
|
// Loop in thread to output to uart
|
||||||
|
void loop();
|
||||||
|
uint8_t last_sent_index;
|
||||||
|
|
||||||
void init(void);
|
void init(void);
|
||||||
void send_command(CMD &cmd);
|
void send_command(CMD &cmd);
|
||||||
void update_volz_bitmask(uint32_t new_bitmask);
|
|
||||||
|
|
||||||
uint32_t last_volz_update_time;
|
// Incoming PWM commands from the servo lib
|
||||||
uint32_t volz_time_frame_micros;
|
uint16_t servo_pwm[NUM_SERVO_CHANNELS];
|
||||||
uint32_t last_used_bitmask;
|
|
||||||
|
|
||||||
AP_Int32 bitmask;
|
AP_Int32 bitmask;
|
||||||
AP_Int16 range;
|
AP_Int16 range;
|
||||||
|
|
Loading…
Reference in New Issue