Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Volz_Protocol: added Volz protocol library

This commit is contained in:
Guy Tzoler 2017-11-03 13:27:29 +11:00 committed by Andrew Tridgell
parent 9720fcb8cc
commit 65032919e2
2 changed files with 236 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

155
libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp Normal file
View File

@ -0,0 +1,155 @@
/*
* AP_VOLZ_PROTOCOL.cpp
*
* Created on: Oct 31, 2017
* Author: guy
*/
#include <AP_HAL/AP_HAL.h>
#include <SRV_Channel/SRV_Channel.h>
#include "AP_Volz_Protocol.h"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_Volz_Protocol::var_info[] = {
// @Param: MASK
// @DisplayName: Channel Bitmask
// @Description: Enable of volz servo protocol to specific channels
// @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16
// @User: Standard
AP_GROUPINFO("MASK", 1, AP_Volz_Protocol, bitmask, 0),
AP_GROUPEND
};
// constructor
AP_Volz_Protocol::AP_Volz_Protocol(void)
{
// set defaults from the parameter table
AP_Param::setup_object_defaults(this, var_info);
}
void AP_Volz_Protocol::init(void)
{
AP_SerialManager *serial_manager = AP_SerialManager::get_instance();
if (!serial_manager) {
return;
}
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 (port == nullptr) {
return;
}
if (last_used_bitmask != uint32_t(bitmask.get())) {
update_volz_bitmask(bitmask);
}
uint32_t now = AP_HAL::micros();
if (now - last_volz_update_time < volz_time_frame_micros ||
port->txspace() < VOLZ_DATA_FRAME_SIZE) {
return;
}
last_volz_update_time = now;
uint8_t i;
uint16_t value;
// loop for all 16 channels
for (i=0; i<NUM_SERVO_CHANNELS; i++) {
// check if current channel is needed for Volz protocol
if (last_used_bitmask & (1U<<i)) {
SRV_Channel *ch = SRV_Channels::srv_channel(i);
if (ch == nullptr) {
continue;
}
// check if current channel PWM is within range
if (ch->get_output_pwm() < ch->get_output_min()) {
value = 0;
} else {
value = ch->get_output_pwm() - ch->get_output_min();
}
// scale the PWM value to Volz value
value = value + VOLZ_EXTENDED_POSITION_MIN;
value = value * VOLZ_SCALE_VALUE / (ch->get_output_max() - ch->get_output_min());
// make sure value stays in range
if (value > VOLZ_EXTENDED_POSITION_MAX) {
value = VOLZ_EXTENDED_POSITION_MAX;
}
// prepare Volz protocol data.
uint8_t data[VOLZ_DATA_FRAME_SIZE];
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);
send_command(data);
}
}
}
// calculate CRC for volz serial protocol and send the data.
void AP_Volz_Protocol::send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE])
{
uint8_t i,j;
uint16_t crc = 0xFFFF;
// calculate Volz CRC value according to protocol definition
for(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++) {
if (crc & 0x8000) {
crc = (crc << 1) ^ 0x8005;
} else {
crc = crc << 1;
}
}
}
// 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; i<NUM_SERVO_CHANNELS; i++) {
if (new_bitmask & (1U<<i)) {
count++;
}
}
// 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.
uint64_t channels_micros = count * 450;
// 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;
}

81
libraries/AP_Volz_Protocol/AP_Volz_Protocol.h Normal file
View File

@ -0,0 +1,81 @@
/*
* AP_VOLZ_PROTOCOL.h
*
* Created on: Oct 31, 2017
* Author: guy tzoler
*
* Baud-Rate: 115.200 bits per second
* Number of Data bits: 8
* Number of Stop bits: 1
* Parity: None
* Half/Full Duplex: Half Duplex
*
* Volz Command and Response are all 6 bytes
*
* Command
* byte | Communication Type
* 1 Command Code
* 2 Actuator ID
* 3 Argument 1
* 4 Argument 2
* 5 CRC High-byte
* 6 CRC Low-Byte
*
* byte | Communication Type
* 1 Response Code
* 2 Actuator ID
* 3 Argument 1
* 4 Argument 2
* 5 CRC High-byte
* 6 CRC Low-Byte
*
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Param/AP_Param.h>
//#include <GCS_MAVLink/GCS.h>
#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
class AP_Volz_Protocol {
public:
static const struct AP_Param::GroupInfo var_info[];
static AP_Volz_Protocol create() {
return AP_Volz_Protocol{};
}
constexpr AP_Volz_Protocol(AP_Volz_Protocol &&other) = default;
/* Do not allow copies */
AP_Volz_Protocol(const AP_Volz_Protocol &other) = delete;
AP_Volz_Protocol &operator=(const AP_Volz_Protocol&) = delete;
void update();
private:
AP_Volz_Protocol();
AP_HAL::UARTDriver *port;
void init(void);
void send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE]);
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;
AP_Int32 bitmask;
bool initialised;
};