mirror of https://github.com/ArduPilot/ardupilot
AP_Volz_Protocol: add support for telem and logging
This commit is contained in:
parent
2524583dda
commit
441dba493f
|
@ -14,7 +14,7 @@
|
|||
#include <SRV_Channel/SRV_Channel.h>
|
||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
|
||||
#define SET_EXTENDED_POSITION_CMD 0xDC
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
// 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
|
||||
|
@ -77,12 +77,48 @@ void AP_Volz_Protocol::init(void)
|
|||
}
|
||||
}
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
// Request telem data, cycling through each servo and telem item
|
||||
void AP_Volz_Protocol::request_telem()
|
||||
{
|
||||
// Request the queued item, making sure the servo is enabled
|
||||
if ((uint32_t(bitmask.get()) & (1U<<telem.actuator_id)) != 0) {
|
||||
// Assemble command
|
||||
CMD cmd {};
|
||||
cmd.ID = telem.types[telem.request_type];
|
||||
cmd.actuator_id = telem.actuator_id + 1;
|
||||
send_command(cmd);
|
||||
|
||||
// Increment the request type
|
||||
telem.request_type++;
|
||||
if (telem.request_type < ARRAY_SIZE(telem.types)) {
|
||||
// Request the next telem type from the same actuators on the next call
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Requested all items from a id or invalid id.
|
||||
// start again with a new id
|
||||
telem.request_type = 0;
|
||||
|
||||
// Same logic as `send_position_cmd`
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(telem.data); i++) {
|
||||
const uint8_t index = (telem.actuator_id + 1 + i) % ARRAY_SIZE(telem.data);
|
||||
if ((uint32_t(bitmask.get()) & (1U<<index)) == 0) {
|
||||
continue;
|
||||
}
|
||||
telem.actuator_id = index;
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void AP_Volz_Protocol::loop()
|
||||
{
|
||||
const uint32_t baudrate = 115200;
|
||||
port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_RTS_DE);
|
||||
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)
|
||||
|
@ -93,8 +129,10 @@ void AP_Volz_Protocol::loop()
|
|||
// receive packet is same length as sent, double to allow some time for the servo respond
|
||||
const uint16_t receive_us = send_us * 2;
|
||||
|
||||
// This gives a total time of 1560ms, message rate of 641 Hz.
|
||||
// This gives a total time of 1560us, message rate of 641 Hz.
|
||||
// One servo at 641Hz, two at 320.5 each, three at 213.7 each ect...
|
||||
// Note that we send a telem request every time servo sending is complete. This is like a extra servo.
|
||||
// So for a single servo position commands are at 320.5Hz and telem at 320.5Hz.
|
||||
|
||||
while (port != nullptr) {
|
||||
|
||||
|
@ -106,6 +144,34 @@ void AP_Volz_Protocol::loop()
|
|||
hal.scheduler->delay_microseconds(100);
|
||||
}
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
// Send a command for each servo, then one telem request
|
||||
const uint8_t num_servos = __builtin_popcount(bitmask.get());
|
||||
if (sent_count < num_servos) {
|
||||
send_position_cmd();
|
||||
sent_count++;
|
||||
|
||||
} else {
|
||||
request_telem();
|
||||
sent_count = 0;
|
||||
}
|
||||
|
||||
{
|
||||
// Read in telem data if available
|
||||
WITH_SEMAPHORE(telem.sem);
|
||||
read_telem();
|
||||
}
|
||||
|
||||
#else // No logging, send only
|
||||
send_position_cmd();
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
// Send postion commands from PWM, cycle through each servo
|
||||
void AP_Volz_Protocol::send_position_cmd()
|
||||
{
|
||||
|
||||
// loop for all channels
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(servo_pwm); i++) {
|
||||
// Send each channels in turn
|
||||
|
@ -135,16 +201,154 @@ void AP_Volz_Protocol::loop()
|
|||
|
||||
// prepare Volz protocol data.
|
||||
CMD cmd;
|
||||
cmd.ID = SET_EXTENDED_POSITION_CMD;
|
||||
cmd.ID = CMD_ID::SET_EXTENDED_POSITION;
|
||||
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);
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
{
|
||||
// Update the commanded angle
|
||||
WITH_SEMAPHORE(telem.sem);
|
||||
static_assert(ARRAY_SIZE(servo_pwm) == ARRAY_SIZE(telem.data), "actuator index invalid for telem data array");
|
||||
telem.data[index].desired_angle = angle;
|
||||
}
|
||||
#endif
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
void AP_Volz_Protocol::process_response(const CMD &cmd)
|
||||
{
|
||||
// Convert to 0 indexed
|
||||
const uint8_t index = cmd.actuator_id - 1;
|
||||
if (index >= ARRAY_SIZE(telem.data)) {
|
||||
// Invalid ID
|
||||
return;
|
||||
}
|
||||
|
||||
switch (cmd.ID) {
|
||||
case CMD_ID::EXTENDED_POSITION_RESPONSE:
|
||||
// Map back to angle
|
||||
telem.data[index].angle = linear_interpolate(ANGLE_POSITION_MIN, ANGLE_POSITION_MAX, UINT16_VALUE(cmd.arg1, cmd.arg2), EXTENDED_POSITION_MIN, EXTENDED_POSITION_MAX);
|
||||
break;
|
||||
|
||||
case CMD_ID::CURRENT_RESPONSE:
|
||||
// Current is reported in 20mA increments (0.02A)
|
||||
telem.data[index].primary_current = cmd.arg1 * 0.02;
|
||||
telem.data[index].secondary_current = cmd.arg2 * 0.02;
|
||||
break;
|
||||
|
||||
case CMD_ID::VOLTAGE_RESPONSE:
|
||||
// Voltage is reported in 200mv increments (0.2v)
|
||||
telem.data[index].primary_voltage = cmd.arg1 * 0.2;
|
||||
telem.data[index].secondary_voltage = cmd.arg2 * 0.2;
|
||||
break;
|
||||
|
||||
case CMD_ID::TEMPERATURE_RESPONSE:
|
||||
// Temperature is reported relative to -50 deg C
|
||||
telem.data[index].motor_temp_deg = -50 + cmd.arg1;
|
||||
telem.data[index].pcb_temp_deg = -50 + cmd.arg2;
|
||||
break;
|
||||
|
||||
default:
|
||||
// This should never happen
|
||||
return;
|
||||
}
|
||||
|
||||
telem.data[index].last_response_ms = AP_HAL::millis();
|
||||
}
|
||||
|
||||
// Return true if the given ID is a valid response
|
||||
bool AP_Volz_Protocol::is_response(uint8_t ID) const
|
||||
{
|
||||
switch(ID) {
|
||||
case (uint8_t)CMD_ID::EXTENDED_POSITION_RESPONSE:
|
||||
case (uint8_t)CMD_ID::CURRENT_RESPONSE:
|
||||
case (uint8_t)CMD_ID::VOLTAGE_RESPONSE:
|
||||
case (uint8_t)CMD_ID::TEMPERATURE_RESPONSE:
|
||||
return true;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void AP_Volz_Protocol::read_telem()
|
||||
{
|
||||
// Try and read data a few times, this could be a while loop, using a for loop gives a upper bound to run time
|
||||
for (uint8_t attempts = 0; attempts < sizeof(telem.cmd_buffer) * 4; attempts++) {
|
||||
|
||||
uint32_t n = port->available();
|
||||
if (n == 0) {
|
||||
// No data available
|
||||
return;
|
||||
}
|
||||
if (telem.buffer_offset < sizeof(telem.cmd_buffer)) {
|
||||
// Read enough bytes to fill buffer
|
||||
ssize_t nread = port->read(&telem.cmd_buffer.data[telem.buffer_offset], MIN(n, unsigned(sizeof(telem.cmd_buffer)-telem.buffer_offset)));
|
||||
if (nread <= 0) {
|
||||
// Read failed
|
||||
return;
|
||||
}
|
||||
telem.buffer_offset += nread;
|
||||
}
|
||||
|
||||
// Check for valid response start byte
|
||||
if (!is_response(telem.cmd_buffer.data[0])) {
|
||||
|
||||
// Search for a valid response start byte
|
||||
uint8_t cmd_start;
|
||||
for (cmd_start = 1; cmd_start < telem.buffer_offset; cmd_start++) {
|
||||
if (is_response(telem.cmd_buffer.data[cmd_start])) {
|
||||
// Found one
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Shift buffer to put start on valid byte, or if no valid byte was found clear
|
||||
const uint8_t n_move = telem.buffer_offset - cmd_start;
|
||||
if (n_move > 0) {
|
||||
// No need to move 0 bytes
|
||||
memmove(&telem.cmd_buffer.data[0], &telem.cmd_buffer.data[cmd_start], n_move);
|
||||
}
|
||||
telem.buffer_offset = 0;
|
||||
|
||||
// Since the buffer is the same length as a full command, we can never get a valid packet after shifting
|
||||
// Always need to read in some more data
|
||||
continue;
|
||||
}
|
||||
|
||||
if (telem.buffer_offset < sizeof(telem.cmd_buffer)) {
|
||||
// Not enough data to make up packet
|
||||
continue;
|
||||
}
|
||||
|
||||
// Have valid ID and enough data, check crc
|
||||
if (UINT16_VALUE(telem.cmd_buffer.crc1, telem.cmd_buffer.crc2) != calculate_crc(telem.cmd_buffer)) {
|
||||
// Probably lost sync shift by one and try again
|
||||
memmove(&telem.cmd_buffer.data[0], &telem.cmd_buffer.data[1], telem.buffer_offset - 1);
|
||||
telem.buffer_offset -= 1;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Valid packet passed crc check
|
||||
process_response(telem.cmd_buffer);
|
||||
|
||||
// zero offset and continue
|
||||
telem.buffer_offset = 0;
|
||||
}
|
||||
|
||||
// Used up all attempts without running out of data.
|
||||
// Really should not end up here
|
||||
}
|
||||
#endif // HAL_LOGGING_ENABLED
|
||||
|
||||
// Called each time the servo outputs are sent
|
||||
void AP_Volz_Protocol::update()
|
||||
|
@ -171,10 +375,53 @@ void AP_Volz_Protocol::update()
|
|||
const uint16_t pwm = c->get_output_pwm();
|
||||
servo_pwm[i] = (pwm == 0) ? c->get_trim() : pwm;
|
||||
}
|
||||
|
||||
// take semaphore and log all channels
|
||||
#if HAL_LOGGING_ENABLED
|
||||
{
|
||||
WITH_SEMAPHORE(telem.sem);
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(telem.data); i++) {
|
||||
if ((telem.data[i].last_response_ms == 0) || ((now_ms - telem.data[i].last_response_ms) > 5000)) {
|
||||
// Never seen telem, or not had a response for more than 5 seconds
|
||||
continue;
|
||||
}
|
||||
|
||||
// @LoggerMessage: VOLZ
|
||||
// @Description: Volz servo data
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: I: Instance
|
||||
// @Field: Dang: desired angle
|
||||
// @Field: ang: reported angle
|
||||
// @Field: pc: primary supply current
|
||||
// @Field: sc: secondary supply current
|
||||
// @Field: pv: primary supply voltage
|
||||
// @Field: sv: secondary supply voltage
|
||||
// @Field: mt: motor temperature
|
||||
// @Field: pt: pcb temperature
|
||||
AP::logger().WriteStreaming("VOLZ",
|
||||
"TimeUS,I,Dang,ang,pc,sc,pv,sv,mt,pt",
|
||||
"s#ddAAvvOO",
|
||||
"F000000000",
|
||||
"QBffffffHH",
|
||||
AP_HAL::micros64(),
|
||||
i + 1, // convert to 1 indexed to match actuator IDs and SERVOx numbering
|
||||
telem.data[i].desired_angle,
|
||||
telem.data[i].angle,
|
||||
telem.data[i].primary_current,
|
||||
telem.data[i].secondary_current,
|
||||
telem.data[i].primary_voltage,
|
||||
telem.data[i].secondary_voltage,
|
||||
telem.data[i].motor_temp_deg,
|
||||
telem.data[i].pcb_temp_deg
|
||||
);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// calculate CRC for volz serial protocol and send the data.
|
||||
void AP_Volz_Protocol::send_command(CMD &cmd)
|
||||
// Return the crc for a given command packet
|
||||
uint16_t AP_Volz_Protocol::calculate_crc(const CMD &cmd) const
|
||||
{
|
||||
uint16_t crc = 0xFFFF;
|
||||
|
||||
|
@ -192,6 +439,14 @@ void AP_Volz_Protocol::send_command(CMD &cmd)
|
|||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
// calculate CRC for volz serial protocol and send the data.
|
||||
void AP_Volz_Protocol::send_command(CMD &cmd)
|
||||
{
|
||||
const uint16_t crc = calculate_crc(cmd);
|
||||
|
||||
// add CRC result to the message
|
||||
cmd.crc1 = HIGHBYTE(crc);
|
||||
cmd.crc2 = LOWBYTE(crc);
|
||||
|
|
|
@ -44,6 +44,8 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <SRV_Channel/SRV_Channel_config.h>
|
||||
#include <AP_Logger/AP_Logger_config.h>
|
||||
|
||||
|
||||
class AP_Volz_Protocol {
|
||||
public:
|
||||
|
@ -58,10 +60,22 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
// Command and response IDs
|
||||
enum class CMD_ID : uint8_t {
|
||||
SET_EXTENDED_POSITION = 0xDC,
|
||||
EXTENDED_POSITION_RESPONSE = 0x2C,
|
||||
READ_CURRENT = 0xB0,
|
||||
CURRENT_RESPONSE = 0x30,
|
||||
READ_VOLTAGE = 0xB1,
|
||||
VOLTAGE_RESPONSE = 0x31,
|
||||
READ_TEMPERATURE = 0xC0,
|
||||
TEMPERATURE_RESPONSE = 0x10,
|
||||
};
|
||||
|
||||
// Command frame
|
||||
union CMD {
|
||||
struct PACKED {
|
||||
uint8_t ID; // CMD ID
|
||||
CMD_ID 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
|
||||
|
@ -75,17 +89,63 @@ private:
|
|||
|
||||
// Loop in thread to output to uart
|
||||
void loop();
|
||||
uint8_t last_sent_index;
|
||||
uint8_t sent_count;
|
||||
|
||||
void init(void);
|
||||
|
||||
// Return the crc for a given command packet
|
||||
uint16_t calculate_crc(const CMD &cmd) const;
|
||||
|
||||
// calculate CRC for volz serial protocol and send the data.
|
||||
void send_command(CMD &cmd);
|
||||
|
||||
// Incoming PWM commands from the servo lib
|
||||
uint16_t servo_pwm[NUM_SERVO_CHANNELS];
|
||||
|
||||
// Send postion commands from PWM, cycle through each servo
|
||||
void send_position_cmd();
|
||||
uint8_t last_sent_index;
|
||||
|
||||
AP_Int32 bitmask;
|
||||
AP_Int16 range;
|
||||
bool initialised;
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
// Request telem data, cycling through each servo and telem item
|
||||
void request_telem();
|
||||
|
||||
// Return true if the given ID is a valid response
|
||||
bool is_response(uint8_t ID) const;
|
||||
|
||||
// Reading of telem packets
|
||||
void read_telem();
|
||||
void process_response(const CMD &cmd);
|
||||
|
||||
struct {
|
||||
CMD_ID types[3] {
|
||||
CMD_ID::READ_CURRENT,
|
||||
CMD_ID::READ_VOLTAGE,
|
||||
CMD_ID::READ_TEMPERATURE,
|
||||
};
|
||||
uint8_t actuator_id;
|
||||
uint8_t request_type;
|
||||
HAL_Semaphore sem;
|
||||
CMD cmd_buffer;
|
||||
uint8_t buffer_offset;
|
||||
struct {
|
||||
uint32_t last_response_ms;
|
||||
float desired_angle;
|
||||
float angle;
|
||||
float primary_current;
|
||||
float secondary_current;
|
||||
float primary_voltage;
|
||||
float secondary_voltage;
|
||||
uint16_t motor_temp_deg;
|
||||
uint16_t pcb_temp_deg;
|
||||
} data[NUM_SERVO_CHANNELS];
|
||||
} telem;
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
#endif // AP_VOLZ_PROTOCOL
|
||||
|
|
Loading…
Reference in New Issue