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

AP_PiccoloCAN: Add support for CBS servo protocol 

- Adds protocol files for the servo protocol
- Generated using Protogen tool
This commit is contained in:
Oliver Walters 2020-11-10 13:51:03 +11:00 committed by Andrew Tridgell
parent 9494a439ec
commit 43b7b4eb3b
19 changed files with 4692 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

182
libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp
View File

@ -12,7 +12,7 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
@ -35,9 +35,14 @@
#include <stdio.h>
// Protocol files for the Velocity ESC
#include <AP_PiccoloCAN/piccolo_protocol/ESCVelocityProtocol.h>
#include <AP_PiccoloCAN/piccolo_protocol/ESCPackets.h>
// Protocol files for the CBS servo
#include <AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h>
#include <AP_PiccoloCAN/piccolo_protocol/ServoPackets.h>
extern const AP_HAL::HAL& hal;
#if HAL_CANMANAGER_ENABLED
@ -265,8 +270,106 @@ void AP_PiccoloCAN::update()
}
}
}
AP_Logger *logger = AP_Logger::get_singleton();
// Push received telemtry data into the logging system
if (logger && logger->logging_enabled()) {
WITH_SEMAPHORE(_telem_sem);
for (uint8_t i = 0; i < PICCOLO_CAN_MAX_NUM_ESC; i++) {
PiccoloESC_Info_t &esc = _esc_info[i];
if (esc.newTelemetry) {
logger->Write_ESC(i, timestamp,
(int32_t) esc.rpm * 100,
esc.voltage,
esc.current,
(int16_t) esc.fetTemperature,
0, // TODO - Accumulated current
(int16_t) esc.motorTemperature);
esc.newTelemetry = false;
}
}
}
}
// send ESC telemetry messages over MAVLink
void AP_PiccoloCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
{
// Arrays to store ESC telemetry data
uint8_t temperature[4] {};
uint16_t voltage[4] {};
uint16_t rpm[4] {};
uint16_t count[4] {};
uint16_t current[4] {};
uint16_t totalcurrent[4] {};
bool dataAvailable = false;
uint8_t idx = 0;
WITH_SEMAPHORE(_telem_sem);
for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_ESC; ii++) {
// Calculate index within storage array
idx = (ii % 4);
VelocityESC_Info_t &esc = _esc_info[idx];
// Has the ESC been heard from recently?
if (is_esc_present(ii)) {
dataAvailable = true;
temperature[idx] = esc.fetTemperature;
voltage[idx] = esc.voltage;
current[idx] = esc.current;
totalcurrent[idx] = 0;
rpm[idx] = esc.rpm;
count[idx] = 0;
} else {
temperature[idx] = 0;
voltage[idx] = 0;
current[idx] = 0;
totalcurrent[idx] = 0;
rpm[idx] = 0;
count[idx] = 0;
}
// Send ESC telemetry in groups of 4
if ((ii % 4) == 3) {
if (dataAvailable) {
if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t) mav_chan, ESC_TELEMETRY_1_TO_4)) {
continue;
}
switch (ii) {
case 3:
mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count);
break;
case 7:
mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count);
break;
case 11:
mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count);
break;
default:
break;
}
}
dataAvailable = false;
}
}
}
// send ESC messages over CAN
void AP_PiccoloCAN::send_esc_messages(void)
{
@ -373,7 +476,7 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame)
return false;
}
PiccoloESC_Info_t &esc = _esc_info[addr];
VelocityESC_Info_t &esc = _esc_info[addr];
/*
* The STATUS_A packet has slight variations between Gen-1 and Gen-2 ESCs.
@ -481,7 +584,7 @@ bool AP_PiccoloCAN::is_esc_present(uint8_t chan, uint64_t timeout_ms) const
return false;
}
const PiccoloESC_Info_t &esc = _esc_info[chan];
VelocityESC_Info_t &esc = _esc_info[chan];
// No messages received from this ESC
if (esc.last_rx_msg_timestamp == 0) {
@ -514,7 +617,7 @@ bool AP_PiccoloCAN::is_esc_enabled(uint8_t chan)
return false;
}
PiccoloESC_Info_t &esc = _esc_info[chan];
VelocityESC_Info_t &esc = _esc_info[chan];
if (esc.status.hwInhibit || esc.status.swInhibit) {
return false;
@ -539,7 +642,7 @@ bool AP_PiccoloCAN::pre_arm_check(char* reason, uint8_t reason_len)
return false;
}
PiccoloESC_Info_t &esc = _esc_info[ii];
VelocityESC_Info_t &esc = _esc_info[ii];
if (esc.status.hwInhibit) {
snprintf(reason, reason_len, "ESC %u is hardware inhibited", (ii + 1));
@ -551,7 +654,6 @@ bool AP_PiccoloCAN::pre_arm_check(char* reason, uint8_t reason_len)
return true;
}
/* Piccolo Glue Logic
* The following functions are required by the auto-generated protogen code.
*/
@ -620,6 +722,74 @@ uint32_t getESCVelocityPacketID(const void* pkt)
return (uint32_t) ((frame->id >> 16) & 0xFF);
}
/* Piccolo Glue Logic
* The following functions are required by the auto-generated protogen code.
*/
//! \return the packet data pointer from the packet
uint8_t* getServoPacketData(void* pkt)
{
AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
return (uint8_t*) frame->data;
}
//! \return the packet data pointer from the packet, const
const uint8_t* getServoPacketDataConst(const void* pkt)
{
AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
return (const uint8_t*) frame->data;
}
//! Complete a packet after the data have been encoded
void finishServoPacket(void* pkt, int size, uint32_t packetID)
{
AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
if (size > AP_HAL::CANFrame::MaxDataLen) {
size = AP_HAL::CANFrame::MaxDataLen;
}
frame->dlc = size;
/* Encode the CAN ID
* 0x07mm20dd
* - 07 = ACTUATOR group ID
* - mm = Message ID
* - 00 = Servo actuator type
* - dd = Device ID
*
* Note: The Device ID (lower 8 bits of the frame ID) will have to be inserted later
*/
uint32_t id = (((uint8_t) AP_PiccoloCAN::MessageGroup::ACTUATOR) << 24) | // CAN Group ID
((packetID & 0xFF) << 16) | // Message ID
(((uint8_t) AP_PiccoloCAN::ActuatorType::SERVO) << 8); // Actuator type
// Extended frame format
id |= AP_HAL::CANFrame::FlagEFF;
frame->id = id;
}
//! \return the size of a packet from the packet header
int getServoPacketSize(const void* pkt)
{
AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
return (int) frame->dlc;
}
//! \return the ID of a packet from the packet header
uint32_t getServoPacketID(const void* pkt)
{
AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
// Extract the message ID field from the 29-bit ID
return (uint32_t) ((frame->id >> 16) & 0xFF);
}
#endif // HAL_PICCOLO_CAN_ENABLE

24
libraries/AP_PiccoloCAN/AP_PiccoloCAN.h
View File

@ -26,10 +26,15 @@
#include "piccolo_protocol/ESCPackets.h"
#include "piccolo_protocol/LegacyESCPackets.h"
#include "piccolo_protocol/ServoPackets.h"
// maximum number of ESC allowed on CAN bus simultaneously
#define PICCOLO_CAN_MAX_NUM_ESC 12
#define PICCOLO_CAN_MAX_GROUP_ESC (PICCOLO_CAN_MAX_NUM_ESC / 4)
#define PICCOLO_CAN_MAX_NUM_SERVO 12
#define PICCOLO_CAN_MAX_GROUP_SERVO (PICCOLO_CAN_MAX_NUM_SERVO / 4)
#ifndef HAL_PICCOLO_CAN_ENABLE
#define HAL_PICCOLO_CAN_ENABLE (HAL_NUM_CAN_IFACES && !HAL_MINIMIZE_FEATURES)
#endif
@ -114,7 +119,24 @@ private:
AP_HAL::CANIface* _can_iface;
HAL_EventHandle _event_handle;
struct PiccoloESC_Info_t {
// Data structure for representing the state of a CBS servo
struct CBSServo_Info_t {
/* Telemetry data provided across multiple packets */
Servo_StatusA_t statusA;
Servo_StatusB_t statusB;
/* Servo configuration information */
Servo_Firmware_t firmware;
Servo_Address_t address;
Servo_SettingsInfo_t settings;
Servo_SystemInfo_t systemInfo;
Servo_TelemetrySettings_t telemetry;
} _servo_info[PICCOLO_CAN_MAX_NUM_SERVO];
// Data structure for representing the state of a Velocity ESC
struct VelocityESC_Info_t {
/* Telemetry data provided in the PKT_ESC_STATUS_A packet */
uint8_t mode; //! ESC operational mode

492
libraries/AP_PiccoloCAN/piccolo_protocol/ServoCommands.c Normal file
View File

@ -0,0 +1,492 @@
// ServoCommands.c was generated by ProtoGen version 3.2.a
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#include "ServoCommands.h"
#include "fielddecode.h"
#include "fieldencode.h"
#include "scaleddecode.h"
#include "scaledencode.h"
/*!
* \brief Create the Servo_RequestHighFrequencyData packet
*
* Select packets for high-frequency transmission. Selected packets will be
* transmitted at 500Hz for one second. Sending this command again resets the
* timer, allowing continuous data as long as this packet is received
* \param _pg_pkt points to the packet which will be created by this function
* \param packets is Select which telemetry packets are transmitted at high-speed by the servo
*/
void encodeServo_RequestHighFrequencyDataPacket(void* _pg_pkt, const Servo_TelemetryPackets_t* packets)
{
uint8_t* _pg_data = getServoPacketData(_pg_pkt);
int _pg_byteindex = 0;
uint8ToBytes((uint8_t)(CMD_SERVO_REQUEST_HF_DATA), _pg_data, &_pg_byteindex);
// Select which telemetry packets are transmitted at high-speed by the servo
encodeServo_TelemetryPackets_t(_pg_data, &_pg_byteindex, packets);
// complete the process of creating the packet
finishServoPacket(_pg_pkt, _pg_byteindex, getServo_RequestHighFrequencyDataPacketID());
}// encodeServo_RequestHighFrequencyDataPacket
/*!
* \brief Decode the Servo_RequestHighFrequencyData packet
*
* Select packets for high-frequency transmission. Selected packets will be
* transmitted at 500Hz for one second. Sending this command again resets the
* timer, allowing continuous data as long as this packet is received
* \param _pg_pkt points to the packet being decoded by this function
* \param packets receives Select which telemetry packets are transmitted at high-speed by the servo
* \return 0 is returned if the packet ID or size is wrong, else 1
*/
int decodeServo_RequestHighFrequencyDataPacket(const void* _pg_pkt, Servo_TelemetryPackets_t* packets)
{
int _pg_byteindex = 0;
const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
int _pg_numbytes = getServoPacketSize(_pg_pkt);
// Verify the packet identifier
if(getServoPacketID(_pg_pkt) != getServo_RequestHighFrequencyDataPacketID())
return 0;
if(_pg_numbytes < getServo_RequestHighFrequencyDataMinDataLength())
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_REQUEST_HF_DATA)
return 0;
// Select which telemetry packets are transmitted at high-speed by the servo
if(decodeServo_TelemetryPackets_t(_pg_data, &_pg_byteindex, packets) == 0)
return 0;
return 1;
}// decodeServo_RequestHighFrequencyDataPacket
/*!
* \brief Create the Servo_SetNodeID packet
*
* Set the Node ID (CAN Address) for the servo
* \param _pg_pkt points to the packet which will be created by this function
* \param serialNumber is The serial number must match that of the servo for the command to be accepted
* \param nodeID is Set the CAN Node ID of the target servo
*/
void encodeServo_SetNodeIDPacket(void* _pg_pkt, uint32_t serialNumber, uint8_t nodeID)
{
uint8_t* _pg_data = getServoPacketData(_pg_pkt);
int _pg_byteindex = 0;
uint8ToBytes((uint8_t)(CMD_SERVO_SET_NODE_ID), _pg_data, &_pg_byteindex);
// The serial number must match that of the servo for the command to be accepted
// Range of serialNumber is 0 to 4294967295.
uint32ToBeBytes(serialNumber, _pg_data, &_pg_byteindex);
// Set the CAN Node ID of the target servo
// Range of nodeID is 0 to 255.
uint8ToBytes(nodeID, _pg_data, &_pg_byteindex);
// complete the process of creating the packet
finishServoPacket(_pg_pkt, _pg_byteindex, getServo_SetNodeIDPacketID());
}// encodeServo_SetNodeIDPacket
/*!
* \brief Decode the Servo_SetNodeID packet
*
* Set the Node ID (CAN Address) for the servo
* \param _pg_pkt points to the packet being decoded by this function
* \param serialNumber receives The serial number must match that of the servo for the command to be accepted
* \param nodeID receives Set the CAN Node ID of the target servo
* \return 0 is returned if the packet ID or size is wrong, else 1
*/
int decodeServo_SetNodeIDPacket(const void* _pg_pkt, uint32_t* serialNumber, uint8_t* nodeID)
{
int _pg_byteindex = 0;
const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
int _pg_numbytes = getServoPacketSize(_pg_pkt);
// Verify the packet identifier
if(getServoPacketID(_pg_pkt) != getServo_SetNodeIDPacketID())
return 0;
if(_pg_numbytes < getServo_SetNodeIDMinDataLength())
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_SET_NODE_ID)
return 0;
// The serial number must match that of the servo for the command to be accepted
// Range of serialNumber is 0 to 4294967295.
(*serialNumber) = uint32FromBeBytes(_pg_data, &_pg_byteindex);
// Set the CAN Node ID of the target servo
// Range of nodeID is 0 to 255.
(*nodeID) = uint8FromBytes(_pg_data, &_pg_byteindex);
return 1;
}// decodeServo_SetNodeIDPacket
/*!
* \brief Create the Servo_SetUserIdA packet
*
* Set user programmable ID value
* \param _pg_pkt points to the packet which will be created by this function
* \param id is User configurable value, 16-bit
*/
void encodeServo_SetUserIdAPacket(void* _pg_pkt, uint16_t id)
{
uint8_t* _pg_data = getServoPacketData(_pg_pkt);
int _pg_byteindex = 0;
uint8ToBytes((uint8_t)(CMD_SERVO_SET_USER_ID_A), _pg_data, &_pg_byteindex);
// User configurable value, 16-bit
// Range of id is 0 to 65535.
uint16ToBeBytes(id, _pg_data, &_pg_byteindex);
// complete the process of creating the packet
finishServoPacket(_pg_pkt, _pg_byteindex, getServo_SetUserIdAPacketID());
}// encodeServo_SetUserIdAPacket
/*!
* \brief Decode the Servo_SetUserIdA packet
*
* Set user programmable ID value
* \param _pg_pkt points to the packet being decoded by this function
* \param id receives User configurable value, 16-bit
* \return 0 is returned if the packet ID or size is wrong, else 1
*/
int decodeServo_SetUserIdAPacket(const void* _pg_pkt, uint16_t* id)
{
int _pg_byteindex = 0;
const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
int _pg_numbytes = getServoPacketSize(_pg_pkt);
// Verify the packet identifier
if(getServoPacketID(_pg_pkt) != getServo_SetUserIdAPacketID())
return 0;
if(_pg_numbytes < getServo_SetUserIdAMinDataLength())
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_SET_USER_ID_A)
return 0;
// User configurable value, 16-bit
// Range of id is 0 to 65535.
(*id) = uint16FromBeBytes(_pg_data, &_pg_byteindex);
return 1;
}// decodeServo_SetUserIdAPacket
/*!
* \brief Create the Servo_SetUserIdB packet
*
* Set user programmable ID value
* \param _pg_pkt points to the packet which will be created by this function
* \param id is User configurable value, 16-bit
*/
void encodeServo_SetUserIdBPacket(void* _pg_pkt, uint16_t id)
{
uint8_t* _pg_data = getServoPacketData(_pg_pkt);
int _pg_byteindex = 0;
uint8ToBytes((uint8_t)(CMD_SERVO_SET_USER_ID_B), _pg_data, &_pg_byteindex);
// User configurable value, 16-bit
// Range of id is 0 to 65535.
uint16ToBeBytes(id, _pg_data, &_pg_byteindex);
// complete the process of creating the packet
finishServoPacket(_pg_pkt, _pg_byteindex, getServo_SetUserIdBPacketID());
}// encodeServo_SetUserIdBPacket
/*!
* \brief Decode the Servo_SetUserIdB packet
*
* Set user programmable ID value
* \param _pg_pkt points to the packet being decoded by this function
* \param id receives User configurable value, 16-bit
* \return 0 is returned if the packet ID or size is wrong, else 1
*/
int decodeServo_SetUserIdBPacket(const void* _pg_pkt, uint16_t* id)
{
int _pg_byteindex = 0;
const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
int _pg_numbytes = getServoPacketSize(_pg_pkt);
// Verify the packet identifier
if(getServoPacketID(_pg_pkt) != getServo_SetUserIdBPacketID())
return 0;
if(_pg_numbytes < getServo_SetUserIdBMinDataLength())
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_SET_USER_ID_B)
return 0;
// User configurable value, 16-bit
// Range of id is 0 to 65535.
(*id) = uint16FromBeBytes(_pg_data, &_pg_byteindex);
return 1;
}// decodeServo_SetUserIdBPacket
/*!
* \brief Create the Servo_ResetDefaultSettings packet
*
* Reset servo settings to default values
* \param _pg_pkt points to the packet which will be created by this function
*/
void encodeServo_ResetDefaultSettingsPacket(void* _pg_pkt)
{
uint8_t* _pg_data = getServoPacketData(_pg_pkt);
int _pg_byteindex = 0;
uint8ToBytes((uint8_t)(CMD_SERVO_RESET_DEFAULT_SETTINGS), _pg_data, &_pg_byteindex);
uint8ToBytes((uint8_t)(0xAA), _pg_data, &_pg_byteindex);
uint8ToBytes((uint8_t)(0x55), _pg_data, &_pg_byteindex);
// complete the process of creating the packet
finishServoPacket(_pg_pkt, _pg_byteindex, getServo_ResetDefaultSettingsPacketID());
}// encodeServo_ResetDefaultSettingsPacket
/*!
* \brief Decode the Servo_ResetDefaultSettings packet
*
* Reset servo settings to default values
* \param _pg_pkt points to the packet being decoded by this function
* \return 0 is returned if the packet ID or size is wrong, else 1
*/
int decodeServo_ResetDefaultSettingsPacket(const void* _pg_pkt)
{
int _pg_byteindex = 0;
const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
int _pg_numbytes = getServoPacketSize(_pg_pkt);
// Verify the packet identifier
if(getServoPacketID(_pg_pkt) != getServo_ResetDefaultSettingsPacketID())
return 0;
if(_pg_numbytes < getServo_ResetDefaultSettingsMinDataLength())
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_RESET_DEFAULT_SETTINGS)
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0xAA)
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0x55)
return 0;
return 1;
}// decodeServo_ResetDefaultSettingsPacket
/*!
* \brief Create the Servo_UnlockSettings packet
*
* Servo system command
* \param _pg_pkt points to the packet which will be created by this function
*/
void encodeServo_UnlockSettingsPacket(void* _pg_pkt)
{
uint8_t* _pg_data = getServoPacketData(_pg_pkt);
int _pg_byteindex = 0;
uint8ToBytes((uint8_t)(CMD_SERVO_UNLOCK_SETTINGS), _pg_data, &_pg_byteindex);
uint8ToBytes((uint8_t)(0xA0), _pg_data, &_pg_byteindex);
uint8ToBytes((uint8_t)(0xB0), _pg_data, &_pg_byteindex);
uint8ToBytes((uint8_t)(0xC0), _pg_data, &_pg_byteindex);
// complete the process of creating the packet
finishServoPacket(_pg_pkt, _pg_byteindex, getServo_UnlockSettingsPacketID());
}// encodeServo_UnlockSettingsPacket
/*!
* \brief Decode the Servo_UnlockSettings packet
*
* Servo system command
* \param _pg_pkt points to the packet being decoded by this function
* \return 0 is returned if the packet ID or size is wrong, else 1
*/
int decodeServo_UnlockSettingsPacket(const void* _pg_pkt)
{
int _pg_byteindex = 0;
const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
int _pg_numbytes = getServoPacketSize(_pg_pkt);
// Verify the packet identifier
if(getServoPacketID(_pg_pkt) != getServo_UnlockSettingsPacketID())
return 0;
if(_pg_numbytes < getServo_UnlockSettingsMinDataLength())
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_UNLOCK_SETTINGS)
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0xA0)
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0xB0)
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0xC0)
return 0;
return 1;
}// decodeServo_UnlockSettingsPacket
/*!
* \brief Create the Servo_LockSettings packet
*
* Servo system command
* \param _pg_pkt points to the packet which will be created by this function
*/
void encodeServo_LockSettingsPacket(void* _pg_pkt)
{
uint8_t* _pg_data = getServoPacketData(_pg_pkt);
int _pg_byteindex = 0;
uint8ToBytes((uint8_t)(CMD_SERVO_LOCK_SETTINGS), _pg_data, &_pg_byteindex);
uint8ToBytes((uint8_t)(0x0A), _pg_data, &_pg_byteindex);
uint8ToBytes((uint8_t)(0x0B), _pg_data, &_pg_byteindex);
uint8ToBytes((uint8_t)(0x0C), _pg_data, &_pg_byteindex);
// complete the process of creating the packet
finishServoPacket(_pg_pkt, _pg_byteindex, getServo_LockSettingsPacketID());
}// encodeServo_LockSettingsPacket
/*!
* \brief Decode the Servo_LockSettings packet
*
* Servo system command
* \param _pg_pkt points to the packet being decoded by this function
* \return 0 is returned if the packet ID or size is wrong, else 1
*/
int decodeServo_LockSettingsPacket(const void* _pg_pkt)
{
int _pg_byteindex = 0;
const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
int _pg_numbytes = getServoPacketSize(_pg_pkt);
// Verify the packet identifier
if(getServoPacketID(_pg_pkt) != getServo_LockSettingsPacketID())
return 0;
if(_pg_numbytes < getServo_LockSettingsMinDataLength())
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_LOCK_SETTINGS)
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0x0A)
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0x0B)
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0x0C)
return 0;
return 1;
}// decodeServo_LockSettingsPacket
/*!
* \brief Create the Servo_EnterBootloader packet
*
* Servo system command
* \param _pg_pkt points to the packet which will be created by this function
*/
void encodeServo_EnterBootloaderPacket(void* _pg_pkt)
{
uint8_t* _pg_data = getServoPacketData(_pg_pkt);
int _pg_byteindex = 0;
uint8ToBytes((uint8_t)(CMD_SERVO_ENTER_BOOTLOADER), _pg_data, &_pg_byteindex);
// This byte is required for the command to be accepted
uint8ToBytes((uint8_t)(0xAA), _pg_data, &_pg_byteindex);
// This byte is required for the command to be accepted
uint8ToBytes((uint8_t)(0x55), _pg_data, &_pg_byteindex);
// complete the process of creating the packet
finishServoPacket(_pg_pkt, _pg_byteindex, getServo_EnterBootloaderPacketID());
}// encodeServo_EnterBootloaderPacket
/*!
* \brief Decode the Servo_EnterBootloader packet
*
* Servo system command
* \param _pg_pkt points to the packet being decoded by this function
* \return 0 is returned if the packet ID or size is wrong, else 1
*/
int decodeServo_EnterBootloaderPacket(const void* _pg_pkt)
{
int _pg_byteindex = 0;
const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
int _pg_numbytes = getServoPacketSize(_pg_pkt);
// Verify the packet identifier
if(getServoPacketID(_pg_pkt) != getServo_EnterBootloaderPacketID())
return 0;
if(_pg_numbytes < getServo_EnterBootloaderMinDataLength())
return 0;
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_ENTER_BOOTLOADER)
return 0;
// This byte is required for the command to be accepted
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0xAA)
return 0;
// This byte is required for the command to be accepted
if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0x55)
return 0;
return 1;
}// decodeServo_EnterBootloaderPacket
// end of ServoCommands.c

194
libraries/AP_PiccoloCAN/piccolo_protocol/ServoCommands.h Normal file
View File

@ -0,0 +1,194 @@
// ServoCommands.h was generated by ProtoGen version 3.2.a
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#ifndef _SERVOCOMMANDS_H
#define _SERVOCOMMANDS_H
// Language target is C, C++ compilers: don't mangle us
#ifdef __cplusplus
extern "C" {
#endif
/*!
* \file
*/
#include <stdbool.h>
#include "ServoProtocol.h"
#include "ServoDefines.h"
/*!
* Select packets for high-frequency transmission. Selected packets will be
* transmitted at 500Hz for one second. Sending this command again resets the
* timer, allowing continuous data as long as this packet is received
*/
typedef struct
{
Servo_TelemetryPackets_t packets; //!< Select which telemetry packets are transmitted at high-speed by the servo
}Servo_RequestHighFrequencyData_t;
//! Create the Servo_RequestHighFrequencyData packet from parameters
void encodeServo_RequestHighFrequencyDataPacket(void* pkt, const Servo_TelemetryPackets_t* packets);
//! Decode the Servo_RequestHighFrequencyData packet to parameters
int decodeServo_RequestHighFrequencyDataPacket(const void* pkt, Servo_TelemetryPackets_t* packets);
//! return the packet ID for the Servo_RequestHighFrequencyData packet
#define getServo_RequestHighFrequencyDataPacketID() (PKT_SERVO_SYSTEM_COMMAND)
//! return the minimum encoded length for the Servo_RequestHighFrequencyData packet
#define getServo_RequestHighFrequencyDataMinDataLength() (2)
//! return the maximum encoded length for the Servo_RequestHighFrequencyData packet
#define getServo_RequestHighFrequencyDataMaxDataLength() (2)
/*!
* Set the Node ID (CAN Address) for the servo
*/
typedef struct
{
uint32_t serialNumber; //!< The serial number must match that of the servo for the command to be accepted
uint8_t nodeID; //!< Set the CAN Node ID of the target servo
}Servo_SetNodeID_t;
//! Create the Servo_SetNodeID packet from parameters
void encodeServo_SetNodeIDPacket(void* pkt, uint32_t serialNumber, uint8_t nodeID);
//! Decode the Servo_SetNodeID packet to parameters
int decodeServo_SetNodeIDPacket(const void* pkt, uint32_t* serialNumber, uint8_t* nodeID);
//! return the packet ID for the Servo_SetNodeID packet
#define getServo_SetNodeIDPacketID() (PKT_SERVO_SYSTEM_COMMAND)
//! return the minimum encoded length for the Servo_SetNodeID packet
#define getServo_SetNodeIDMinDataLength() (6)
//! return the maximum encoded length for the Servo_SetNodeID packet
#define getServo_SetNodeIDMaxDataLength() (6)
/*!
* Set user programmable ID value
*/
typedef struct
{
uint16_t id; //!< User configurable value, 16-bit
}Servo_SetUserIdA_t;
//! Create the Servo_SetUserIdA packet from parameters
void encodeServo_SetUserIdAPacket(void* pkt, uint16_t id);
//! Decode the Servo_SetUserIdA packet to parameters
int decodeServo_SetUserIdAPacket(const void* pkt, uint16_t* id);
//! return the packet ID for the Servo_SetUserIdA packet
#define getServo_SetUserIdAPacketID() (PKT_SERVO_SYSTEM_COMMAND)
//! return the minimum encoded length for the Servo_SetUserIdA packet
#define getServo_SetUserIdAMinDataLength() (3)
//! return the maximum encoded length for the Servo_SetUserIdA packet
#define getServo_SetUserIdAMaxDataLength() (3)
/*!
* Set user programmable ID value
*/
typedef struct
{
uint16_t id; //!< User configurable value, 16-bit
}Servo_SetUserIdB_t;
//! Create the Servo_SetUserIdB packet from parameters
void encodeServo_SetUserIdBPacket(void* pkt, uint16_t id);
//! Decode the Servo_SetUserIdB packet to parameters
int decodeServo_SetUserIdBPacket(const void* pkt, uint16_t* id);
//! return the packet ID for the Servo_SetUserIdB packet
#define getServo_SetUserIdBPacketID() (PKT_SERVO_SYSTEM_COMMAND)
//! return the minimum encoded length for the Servo_SetUserIdB packet
#define getServo_SetUserIdBMinDataLength() (3)
//! return the maximum encoded length for the Servo_SetUserIdB packet
#define getServo_SetUserIdBMaxDataLength() (3)
//! Create the Servo_ResetDefaultSettings packet from parameters
void encodeServo_ResetDefaultSettingsPacket(void* pkt);
//! Decode the Servo_ResetDefaultSettings packet to parameters
int decodeServo_ResetDefaultSettingsPacket(const void* pkt);
//! return the packet ID for the Servo_ResetDefaultSettings packet
#define getServo_ResetDefaultSettingsPacketID() (PKT_SERVO_SYSTEM_COMMAND)
//! return the minimum encoded length for the Servo_ResetDefaultSettings packet
#define getServo_ResetDefaultSettingsMinDataLength() (3)
//! return the maximum encoded length for the Servo_ResetDefaultSettings packet
#define getServo_ResetDefaultSettingsMaxDataLength() (3)
//! Create the Servo_UnlockSettings packet from parameters
void encodeServo_UnlockSettingsPacket(void* pkt);
//! Decode the Servo_UnlockSettings packet to parameters
int decodeServo_UnlockSettingsPacket(const void* pkt);
//! return the packet ID for the Servo_UnlockSettings packet
#define getServo_UnlockSettingsPacketID() (PKT_SERVO_SYSTEM_COMMAND)
//! return the minimum encoded length for the Servo_UnlockSettings packet
#define getServo_UnlockSettingsMinDataLength() (4)
//! return the maximum encoded length for the Servo_UnlockSettings packet
#define getServo_UnlockSettingsMaxDataLength() (4)
//! Create the Servo_LockSettings packet from parameters
void encodeServo_LockSettingsPacket(void* pkt);
//! Decode the Servo_LockSettings packet to parameters
int decodeServo_LockSettingsPacket(const void* pkt);
//! return the packet ID for the Servo_LockSettings packet
#define getServo_LockSettingsPacketID() (PKT_SERVO_SYSTEM_COMMAND)
//! return the minimum encoded length for the Servo_LockSettings packet
#define getServo_LockSettingsMinDataLength() (4)
//! return the maximum encoded length for the Servo_LockSettings packet
#define getServo_LockSettingsMaxDataLength() (4)
//! Create the Servo_EnterBootloader packet from parameters
void encodeServo_EnterBootloaderPacket(void* pkt);
//! Decode the Servo_EnterBootloader packet to parameters
int decodeServo_EnterBootloaderPacket(const void* pkt);
//! return the packet ID for the Servo_EnterBootloader packet
#define getServo_EnterBootloaderPacketID() (PKT_SERVO_SYSTEM_COMMAND)
//! return the minimum encoded length for the Servo_EnterBootloader packet
#define getServo_EnterBootloaderMinDataLength() (3)
//! return the maximum encoded length for the Servo_EnterBootloader packet
#define getServo_EnterBootloaderMaxDataLength() (3)
#ifdef __cplusplus
}
#endif
#endif // _SERVOCOMMANDS_H

514
libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.c Normal file
View File

@ -0,0 +1,514 @@
// ServoDefines.c was generated by ProtoGen version 3.2.a
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#include "ServoDefines.h"
#include "fielddecode.h"
#include "fieldencode.h"
#include "scaleddecode.h"
#include "scaledencode.h"
/*!
* \brief Encode a Servo_StatusBits_t into a byte array
*
* Servo status bits
* \param _pg_data points to the byte array to add encoded data to
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of encoded bytes.
* \param _pg_user is the data to encode in the byte array
*/
void encodeServo_StatusBits_t(uint8_t* _pg_data, int* _pg_bytecount, const Servo_StatusBits_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// Servo enabled status
_pg_data[_pg_byteindex] = (uint8_t)_pg_user->enabled << 7;
// Servo mode
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->mode << 4;
// Set if the servo has received a valid position command within the configured timeout period
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->cmdReceived << 3;
// Reserved for future use
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->reservedA << 2;
// Reserved for future use
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->reservedB << 1;
// Reserved for future use
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->reservedC;
_pg_byteindex += 1; // close bit field
*_pg_bytecount = _pg_byteindex;
}// encodeServo_StatusBits_t
/*!
* \brief Decode a Servo_StatusBits_t from a byte array
*
* Servo status bits
* \param _pg_data points to the byte array to decoded data from
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of bytes decoded
* \param _pg_user is the data to decode from the byte array
* \return 1 if the data are decoded, else 0.
*/
int decodeServo_StatusBits_t(const uint8_t* _pg_data, int* _pg_bytecount, Servo_StatusBits_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// Servo enabled status
_pg_user->enabled = (_pg_data[_pg_byteindex] >> 7);
// Servo mode
_pg_user->mode = (ServoModes)((_pg_data[_pg_byteindex] >> 4) & 0x7);
// Set if the servo has received a valid position command within the configured timeout period
_pg_user->cmdReceived = ((_pg_data[_pg_byteindex] >> 3) & 0x1);
// Reserved for future use
_pg_user->reservedA = ((_pg_data[_pg_byteindex] >> 2) & 0x1);
// Reserved for future use
_pg_user->reservedB = ((_pg_data[_pg_byteindex] >> 1) & 0x1);
// Reserved for future use
_pg_user->reservedC = ((_pg_data[_pg_byteindex]) & 0x1);
_pg_byteindex += 1; // close bit field
*_pg_bytecount = _pg_byteindex;
return 1;
}// decodeServo_StatusBits_t
/*!
* \brief Encode a Servo_WarningBits_t into a byte array
*
* Servo warning bits
* \param _pg_data points to the byte array to add encoded data to
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of encoded bytes.
* \param _pg_user is the data to encode in the byte array
*/
void encodeServo_WarningBits_t(uint8_t* _pg_data, int* _pg_bytecount, const Servo_WarningBits_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// Servo current exceeds warning threshold
_pg_data[_pg_byteindex] = (uint8_t)_pg_user->overCurrent << 7;
// Servo temperature exceeds warning threshold
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->overTemperature << 6;
// Servo accelerometer reading exceeds warning threshold
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->overAcceleration << 5;
// Most recent servo position command was outside valid range
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->invalidInput << 4;
// Servo position is outside calibrated range
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->position << 3;
// Servo potentiometer is not correctly calibrated
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->potCalibration << 2;
// Servo voltage is below operational threshold
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->underVoltage << 1;
// Servo voltage is above operational threshold
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->overVoltage;
// Reserved for future use
// Range of reservedB is 0 to 255.
_pg_data[_pg_byteindex + 1] = (uint8_t)_pg_user->reservedB;
_pg_byteindex += 2; // close bit field
*_pg_bytecount = _pg_byteindex;
}// encodeServo_WarningBits_t
/*!
* \brief Decode a Servo_WarningBits_t from a byte array
*
* Servo warning bits
* \param _pg_data points to the byte array to decoded data from
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of bytes decoded
* \param _pg_user is the data to decode from the byte array
* \return 1 if the data are decoded, else 0.
*/
int decodeServo_WarningBits_t(const uint8_t* _pg_data, int* _pg_bytecount, Servo_WarningBits_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// Servo current exceeds warning threshold
_pg_user->overCurrent = (_pg_data[_pg_byteindex] >> 7);
// Servo temperature exceeds warning threshold
_pg_user->overTemperature = ((_pg_data[_pg_byteindex] >> 6) & 0x1);
// Servo accelerometer reading exceeds warning threshold
_pg_user->overAcceleration = ((_pg_data[_pg_byteindex] >> 5) & 0x1);
// Most recent servo position command was outside valid range
_pg_user->invalidInput = ((_pg_data[_pg_byteindex] >> 4) & 0x1);
// Servo position is outside calibrated range
_pg_user->position = ((_pg_data[_pg_byteindex] >> 3) & 0x1);
// Servo potentiometer is not correctly calibrated
_pg_user->potCalibration = ((_pg_data[_pg_byteindex] >> 2) & 0x1);
// Servo voltage is below operational threshold
_pg_user->underVoltage = ((_pg_data[_pg_byteindex] >> 1) & 0x1);
// Servo voltage is above operational threshold
_pg_user->overVoltage = ((_pg_data[_pg_byteindex]) & 0x1);
// Reserved for future use
// Range of reservedB is 0 to 255.
_pg_user->reservedB = _pg_data[_pg_byteindex + 1];
_pg_byteindex += 2; // close bit field
*_pg_bytecount = _pg_byteindex;
return 1;
}// decodeServo_WarningBits_t
/*!
* \brief Encode a Servo_ErrorBits_t into a byte array
*
* Servo error bits
* \param _pg_data points to the byte array to add encoded data to
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of encoded bytes.
* \param _pg_user is the data to encode in the byte array
*/
void encodeServo_ErrorBits_t(uint8_t* _pg_data, int* _pg_bytecount, const Servo_ErrorBits_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// Servo position is outside measurement range
_pg_data[_pg_byteindex] = (uint8_t)_pg_user->position << 7;
// Error reading servo position
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->potError << 6;
// Error reading servo accelerometer data
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->accError << 5;
// Servo input/output map is not correctly calibrated
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->mapCalibration << 4;
// Error reading servo settings from memory
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->settingsError << 3;
// Error reading servo health tracking information from memory
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->healthError << 2;
// Servo position cannot match commanded position
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->tracking << 1;
// Reserved for future use
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->reservedB;
_pg_byteindex += 1; // close bit field
*_pg_bytecount = _pg_byteindex;
}// encodeServo_ErrorBits_t
/*!
* \brief Decode a Servo_ErrorBits_t from a byte array
*
* Servo error bits
* \param _pg_data points to the byte array to decoded data from
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of bytes decoded
* \param _pg_user is the data to decode from the byte array
* \return 1 if the data are decoded, else 0.
*/
int decodeServo_ErrorBits_t(const uint8_t* _pg_data, int* _pg_bytecount, Servo_ErrorBits_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// Servo position is outside measurement range
_pg_user->position = (_pg_data[_pg_byteindex] >> 7);
// Error reading servo position
_pg_user->potError = ((_pg_data[_pg_byteindex] >> 6) & 0x1);
// Error reading servo accelerometer data
_pg_user->accError = ((_pg_data[_pg_byteindex] >> 5) & 0x1);
// Servo input/output map is not correctly calibrated
_pg_user->mapCalibration = ((_pg_data[_pg_byteindex] >> 4) & 0x1);
// Error reading servo settings from memory
_pg_user->settingsError = ((_pg_data[_pg_byteindex] >> 3) & 0x1);
// Error reading servo health tracking information from memory
_pg_user->healthError = ((_pg_data[_pg_byteindex] >> 2) & 0x1);
// Servo position cannot match commanded position
_pg_user->tracking = ((_pg_data[_pg_byteindex] >> 1) & 0x1);
// Reserved for future use
_pg_user->reservedB = ((_pg_data[_pg_byteindex]) & 0x1);
_pg_byteindex += 1; // close bit field
*_pg_bytecount = _pg_byteindex;
return 1;
}// decodeServo_ErrorBits_t
/*!
* \brief Encode a Servo_TelemetryPackets_t into a byte array
*
* Servo packet selection
* \param _pg_data points to the byte array to add encoded data to
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of encoded bytes.
* \param _pg_user is the data to encode in the byte array
*/
void encodeServo_TelemetryPackets_t(uint8_t* _pg_data, int* _pg_bytecount, const Servo_TelemetryPackets_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// Select the *STATUS_A* packet
_pg_data[_pg_byteindex] = (uint8_t)_pg_user->statusA << 7;
// Select the *STATUS_B* packet
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->statusB << 6;
// Select the *STATUS_C* packet
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->statusC << 5;
// Select the *STATUS_D* packet
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->statusD << 4;
// Select the *ACCELEROMETER* packet
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->accelerometer << 3;
// Reserved field (set to zero)
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->dbgA << 2;
// Reserved field (set to zero)
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->dbgB << 1;
// Reserved field (set to zero)
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->dbgC;
_pg_byteindex += 1; // close bit field
*_pg_bytecount = _pg_byteindex;
}// encodeServo_TelemetryPackets_t
/*!
* \brief Decode a Servo_TelemetryPackets_t from a byte array
*
* Servo packet selection
* \param _pg_data points to the byte array to decoded data from
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of bytes decoded
* \param _pg_user is the data to decode from the byte array
* \return 1 if the data are decoded, else 0.
*/
int decodeServo_TelemetryPackets_t(const uint8_t* _pg_data, int* _pg_bytecount, Servo_TelemetryPackets_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// Select the *STATUS_A* packet
_pg_user->statusA = (_pg_data[_pg_byteindex] >> 7);
// Select the *STATUS_B* packet
_pg_user->statusB = ((_pg_data[_pg_byteindex] >> 6) & 0x1);
// Select the *STATUS_C* packet
_pg_user->statusC = ((_pg_data[_pg_byteindex] >> 5) & 0x1);
// Select the *STATUS_D* packet
_pg_user->statusD = ((_pg_data[_pg_byteindex] >> 4) & 0x1);
// Select the *ACCELEROMETER* packet
_pg_user->accelerometer = ((_pg_data[_pg_byteindex] >> 3) & 0x1);
// Reserved field (set to zero)
_pg_user->dbgA = ((_pg_data[_pg_byteindex] >> 2) & 0x1);
// Reserved field (set to zero)
_pg_user->dbgB = ((_pg_data[_pg_byteindex] >> 1) & 0x1);
// Reserved field (set to zero)
_pg_user->dbgC = ((_pg_data[_pg_byteindex]) & 0x1);
_pg_byteindex += 1; // close bit field
*_pg_bytecount = _pg_byteindex;
return 1;
}// decodeServo_TelemetryPackets_t
/*!
* \brief Encode a Servo_TelemetrySettings_t into a byte array
*
* Servo telemetry configuration
* \param _pg_data points to the byte array to add encoded data to
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of encoded bytes.
* \param _pg_user is the data to encode in the byte array
*/
void encodeServo_TelemetrySettings_t(uint8_t* _pg_data, int* _pg_bytecount, const Servo_TelemetrySettings_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// Servo telemetry period
// Range of period is 0 to 255.
uint8ToBytes(_pg_user->period, _pg_data, &_pg_byteindex);
// Servo silence period (after boot)
// Range of silence is 0 to 255.
uint8ToBytes(_pg_user->silence, _pg_data, &_pg_byteindex);
// Telemetry packet selection
encodeServo_TelemetryPackets_t(_pg_data, &_pg_byteindex, &_pg_user->packets);
// Command response packet selection
encodeServo_TelemetryPackets_t(_pg_data, &_pg_byteindex, &_pg_user->responsePackets);
*_pg_bytecount = _pg_byteindex;
}// encodeServo_TelemetrySettings_t
/*!
* \brief Decode a Servo_TelemetrySettings_t from a byte array
*
* Servo telemetry configuration
* \param _pg_data points to the byte array to decoded data from
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of bytes decoded
* \param _pg_user is the data to decode from the byte array
* \return 1 if the data are decoded, else 0.
*/
int decodeServo_TelemetrySettings_t(const uint8_t* _pg_data, int* _pg_bytecount, Servo_TelemetrySettings_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// Servo telemetry period
// Range of period is 0 to 255.
_pg_user->period = uint8FromBytes(_pg_data, &_pg_byteindex);
// Servo silence period (after boot)
// Range of silence is 0 to 255.
_pg_user->silence = uint8FromBytes(_pg_data, &_pg_byteindex);
// Telemetry packet selection
if(decodeServo_TelemetryPackets_t(_pg_data, &_pg_byteindex, &_pg_user->packets) == 0)
return 0;
// Command response packet selection
if(decodeServo_TelemetryPackets_t(_pg_data, &_pg_byteindex, &_pg_user->responsePackets) == 0)
return 0;
*_pg_bytecount = _pg_byteindex;
return 1;
}// decodeServo_TelemetrySettings_t
/*!
* \brief Encode a Servo_ConfigBits_t into a byte array
*
* \param _pg_data points to the byte array to add encoded data to
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of encoded bytes.
* \param _pg_user is the data to encode in the byte array
*/
void encodeServo_ConfigBits_t(uint8_t* _pg_data, int* _pg_bytecount, const Servo_ConfigBits_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// 1 = I/O map is enabled
_pg_data[_pg_byteindex] = (uint8_t)_pg_user->mapEnabled << 7;
// 1 = The servo will listen for Piccolo autopilot CAN messages
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->piccoloCmd << 6;
// 1 = Servo neutral position is enabled, 0 = Neutral position disabled
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->neutralPositionEnabled << 5;
// Channel the servo will use to listen for Piccolo messages.
// Range of piccoloChannel is 0 to 31.
_pg_data[_pg_byteindex] |= (uint8_t)_pg_user->piccoloChannel;
// Servo action at powerup
// Range of timeoutAction is 0 to 3.
_pg_data[_pg_byteindex + 1] = (uint8_t)_pg_user->timeoutAction << 6;
// reserved for future use
// Range of reservedB is 0 to 3.
_pg_data[_pg_byteindex + 1] |= (uint8_t)_pg_user->reservedB << 4;
// reserved for future use
// Range of reservedC is 0 to 15.
_pg_data[_pg_byteindex + 1] |= (uint8_t)_pg_user->reservedC;
_pg_byteindex += 2; // close bit field
*_pg_bytecount = _pg_byteindex;
}// encodeServo_ConfigBits_t
/*!
* \brief Decode a Servo_ConfigBits_t from a byte array
*
* \param _pg_data points to the byte array to decoded data from
* \param _pg_bytecount points to the starting location in the byte array, and will be incremented by the number of bytes decoded
* \param _pg_user is the data to decode from the byte array
* \return 1 if the data are decoded, else 0.
*/
int decodeServo_ConfigBits_t(const uint8_t* _pg_data, int* _pg_bytecount, Servo_ConfigBits_t* _pg_user)
{
int _pg_byteindex = *_pg_bytecount;
// 1 = I/O map is enabled
_pg_user->mapEnabled = (_pg_data[_pg_byteindex] >> 7);
// 1 = The servo will listen for Piccolo autopilot CAN messages
_pg_user->piccoloCmd = ((_pg_data[_pg_byteindex] >> 6) & 0x1);
// 1 = Servo neutral position is enabled, 0 = Neutral position disabled
_pg_user->neutralPositionEnabled = ((_pg_data[_pg_byteindex] >> 5) & 0x1);
// Channel the servo will use to listen for Piccolo messages.
// Range of piccoloChannel is 0 to 31.
_pg_user->piccoloChannel = ((_pg_data[_pg_byteindex]) & 0x1F);
// Servo action at powerup
// Range of timeoutAction is 0 to 3.
_pg_user->timeoutAction = (_pg_data[_pg_byteindex + 1] >> 6);
// reserved for future use
// Range of reservedB is 0 to 3.
_pg_user->reservedB = ((_pg_data[_pg_byteindex + 1] >> 4) & 0x3);
// reserved for future use
// Range of reservedC is 0 to 15.
_pg_user->reservedC = ((_pg_data[_pg_byteindex + 1]) & 0xF);
_pg_byteindex += 2; // close bit field
*_pg_bytecount = _pg_byteindex;
return 1;
}// decodeServo_ConfigBits_t
// end of ServoDefines.c

191
libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.h Normal file
View File

@ -0,0 +1,191 @@
// ServoDefines.h was generated by ProtoGen version 3.2.a
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#ifndef _SERVODEFINES_H
#define _SERVODEFINES_H
// Language target is C, C++ compilers: don't mangle us
#ifdef __cplusplus
extern "C" {
#endif
/*!
* \file
*/
#include <stdbool.h>
#include "ServoProtocol.h"
/*!
* Servo status bits
*/
typedef struct
{
unsigned enabled : 1; //!< Servo enabled status
ServoModes mode; //!< Servo mode
unsigned cmdReceived : 1; //!< Set if the servo has received a valid position command within the configured timeout period
unsigned reservedA : 1; //!< Reserved for future use
unsigned reservedB : 1; //!< Reserved for future use
unsigned reservedC : 1; //!< Reserved for future use
}Servo_StatusBits_t;
//! return the minimum encoded length for the Servo_StatusBits_t structure
#define getMinLengthOfServo_StatusBits_t() (1)
//! return the maximum encoded length for the Servo_StatusBits_t structure
#define getMaxLengthOfServo_StatusBits_t() (1)
//! Encode a Servo_StatusBits_t into a byte array
void encodeServo_StatusBits_t(uint8_t* data, int* bytecount, const Servo_StatusBits_t* user);
//! Decode a Servo_StatusBits_t from a byte array
int decodeServo_StatusBits_t(const uint8_t* data, int* bytecount, Servo_StatusBits_t* user);
/*!
* Servo warning bits
*/
typedef struct
{
unsigned overCurrent : 1; //!< Servo current exceeds warning threshold
unsigned overTemperature : 1; //!< Servo temperature exceeds warning threshold
unsigned overAcceleration : 1; //!< Servo accelerometer reading exceeds warning threshold
unsigned invalidInput : 1; //!< Most recent servo position command was outside valid range
unsigned position : 1; //!< Servo position is outside calibrated range
unsigned potCalibration : 1; //!< Servo potentiometer is not correctly calibrated
unsigned underVoltage : 1; //!< Servo voltage is below operational threshold
unsigned overVoltage : 1; //!< Servo voltage is above operational threshold
unsigned reservedB : 8; //!< Reserved for future use
}Servo_WarningBits_t;
//! return the minimum encoded length for the Servo_WarningBits_t structure
#define getMinLengthOfServo_WarningBits_t() (2)
//! return the maximum encoded length for the Servo_WarningBits_t structure
#define getMaxLengthOfServo_WarningBits_t() (2)
//! Encode a Servo_WarningBits_t into a byte array
void encodeServo_WarningBits_t(uint8_t* data, int* bytecount, const Servo_WarningBits_t* user);
//! Decode a Servo_WarningBits_t from a byte array
int decodeServo_WarningBits_t(const uint8_t* data, int* bytecount, Servo_WarningBits_t* user);
/*!
* Servo error bits
*/
typedef struct
{
unsigned position : 1; //!< Servo position is outside measurement range
unsigned potError : 1; //!< Error reading servo position
unsigned accError : 1; //!< Error reading servo accelerometer data
unsigned mapCalibration : 1; //!< Servo input/output map is not correctly calibrated
unsigned settingsError : 1; //!< Error reading servo settings from memory
unsigned healthError : 1; //!< Error reading servo health tracking information from memory
unsigned tracking : 1; //!< Servo position cannot match commanded position
unsigned reservedB : 1; //!< Reserved for future use
}Servo_ErrorBits_t;
//! return the minimum encoded length for the Servo_ErrorBits_t structure
#define getMinLengthOfServo_ErrorBits_t() (1)
//! return the maximum encoded length for the Servo_ErrorBits_t structure
#define getMaxLengthOfServo_ErrorBits_t() (1)
//! Encode a Servo_ErrorBits_t into a byte array
void encodeServo_ErrorBits_t(uint8_t* data, int* bytecount, const Servo_ErrorBits_t* user);
//! Decode a Servo_ErrorBits_t from a byte array
int decodeServo_ErrorBits_t(const uint8_t* data, int* bytecount, Servo_ErrorBits_t* user);
/*!
* Servo packet selection
*/
typedef struct
{
unsigned statusA : 1; //!< Select the *STATUS_A* packet
unsigned statusB : 1; //!< Select the *STATUS_B* packet
unsigned statusC : 1; //!< Select the *STATUS_C* packet
unsigned statusD : 1; //!< Select the *STATUS_D* packet
unsigned accelerometer : 1; //!< Select the *ACCELEROMETER* packet
unsigned dbgA : 1; //!< Reserved field (set to zero)
unsigned dbgB : 1; //!< Reserved field (set to zero)
unsigned dbgC : 1; //!< Reserved field (set to zero)
}Servo_TelemetryPackets_t;
//! return the minimum encoded length for the Servo_TelemetryPackets_t structure
#define getMinLengthOfServo_TelemetryPackets_t() (1)
//! return the maximum encoded length for the Servo_TelemetryPackets_t structure
#define getMaxLengthOfServo_TelemetryPackets_t() (1)
//! Encode a Servo_TelemetryPackets_t into a byte array
void encodeServo_TelemetryPackets_t(uint8_t* data, int* bytecount, const Servo_TelemetryPackets_t* user);
//! Decode a Servo_TelemetryPackets_t from a byte array
int decodeServo_TelemetryPackets_t(const uint8_t* data, int* bytecount, Servo_TelemetryPackets_t* user);
/*!
* Servo telemetry configuration
*/
typedef struct
{
uint8_t period; //!< Servo telemetry period
uint8_t silence; //!< Servo silence period (after boot)
Servo_TelemetryPackets_t packets; //!< Telemetry packet selection
Servo_TelemetryPackets_t responsePackets; //!< Command response packet selection
}Servo_TelemetrySettings_t;
//! return the minimum encoded length for the Servo_TelemetrySettings_t structure
#define getMinLengthOfServo_TelemetrySettings_t() (4)
//! return the maximum encoded length for the Servo_TelemetrySettings_t structure
#define getMaxLengthOfServo_TelemetrySettings_t() (4)
//! Encode a Servo_TelemetrySettings_t into a byte array
void encodeServo_TelemetrySettings_t(uint8_t* data, int* bytecount, const Servo_TelemetrySettings_t* user);
//! Decode a Servo_TelemetrySettings_t from a byte array
int decodeServo_TelemetrySettings_t(const uint8_t* data, int* bytecount, Servo_TelemetrySettings_t* user);
typedef struct
{
unsigned mapEnabled : 1; //!< 1 = I/O map is enabled
unsigned piccoloCmd : 1; //!< 1 = The servo will listen for Piccolo autopilot CAN messages
unsigned neutralPositionEnabled : 1; //!< 1 = Servo neutral position is enabled, 0 = Neutral position disabled
unsigned piccoloChannel : 5; //!< Channel the servo will use to listen for Piccolo messages.
unsigned timeoutAction : 2; //!< Servo action at powerup
unsigned reservedB : 2; //!< reserved for future use
unsigned reservedC : 4; //!< reserved for future use
}Servo_ConfigBits_t;
//! return the minimum encoded length for the Servo_ConfigBits_t structure
#define getMinLengthOfServo_ConfigBits_t() (2)
//! return the maximum encoded length for the Servo_ConfigBits_t structure
#define getMaxLengthOfServo_ConfigBits_t() (2)
//! Encode a Servo_ConfigBits_t into a byte array
void encodeServo_ConfigBits_t(uint8_t* data, int* bytecount, const Servo_ConfigBits_t* user);
//! Decode a Servo_ConfigBits_t from a byte array
int decodeServo_ConfigBits_t(const uint8_t* data, int* bytecount, Servo_ConfigBits_t* user);
#ifdef __cplusplus
}
#endif
#endif // _SERVODEFINES_H

1736
libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c Normal file
View File

File diff suppressed because it is too large Load Diff

509
libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h Normal file
View File

@ -0,0 +1,509 @@
// ServoPackets.h was generated by ProtoGen version 3.2.a
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#ifndef _SERVOPACKETS_H
#define _SERVOPACKETS_H
// Language target is C, C++ compilers: don't mangle us
#ifdef __cplusplus
extern "C" {
#endif
/*!
* \file
*/
#include <stdbool.h>
#include "ServoProtocol.h"
#include "ServoDefines.h"
/*!
* General servo configuration settings
*/
typedef struct
{
Servo_ConfigBits_t options; //!< Servo configuration parameters
uint16_t commandTimeout; //!< Servo command timeout
int16_t homePosition; //!< Servo neutral position. Servo can be configured to return to this position at powerup, or after loss of communication
uint8_t reserved[2]; //!< Reserved for future use
}Servo_Config_t;
//! Create the Servo_Config packet
void encodeServo_ConfigPacketStructure(void* pkt, const Servo_Config_t* user);
//! Decode the Servo_Config packet
int decodeServo_ConfigPacketStructure(const void* pkt, Servo_Config_t* user);
//! Create the Servo_Config packet from parameters
void encodeServo_ConfigPacket(void* pkt, const Servo_ConfigBits_t* options, uint16_t commandTimeout, int16_t homePosition, const uint8_t reserved[2]);
//! Decode the Servo_Config packet to parameters
int decodeServo_ConfigPacket(const void* pkt, Servo_ConfigBits_t* options, uint16_t* commandTimeout, int16_t* homePosition, uint8_t reserved[2]);
//! Encode a Servo_Config_t into a byte array
void encodeServo_Config_t(uint8_t* data, int* bytecount, const Servo_Config_t* user);
//! Decode a Servo_Config_t from a byte array
int decodeServo_Config_t(const uint8_t* data, int* bytecount, Servo_Config_t* user);
//! return the packet ID for the Servo_Config packet
#define getServo_ConfigPacketID() (PKT_SERVO_CONFIG)
//! return the minimum encoded length for the Servo_Config packet
#define getServo_ConfigMinDataLength() (8)
//! return the maximum encoded length for the Servo_Config packet
#define getServo_ConfigMaxDataLength() (8)
/*!
* This command can be used to command multiple servos (1 to 4) with a single
* CAN frame). The addresses of the targetted servos must be sequential, with
* the initial address based on the packet ID. The example packet shown below
* corresponds to servos with IDs {1, 2, 3, 4}. For other addresses refer to
* the *ServoMultiCommandPackets* enumeration. Note: The CAN frame must be sent
* to the broadcast ID (0xFF) for all servos to receive this message.
*/
typedef struct
{
int16_t commandA; //!< Servo command for servo with address offset 0
int16_t commandB; //!< Servo command for servo with address offset 1
int16_t commandC; //!< Servo command for servo with address offset 3
int16_t commandD; //!< Servo command for servo with address offset 3
}Servo_MultiPositionCommand_t;
//! Create the Servo_MultiPositionCommand packet from parameters
void encodeServo_MultiPositionCommandPacket(void* pkt, int16_t commandA, int16_t commandB, int16_t commandC, int16_t commandD);
//! Decode the Servo_MultiPositionCommand packet to parameters
int decodeServo_MultiPositionCommandPacket(const void* pkt, int16_t* commandA, int16_t* commandB, int16_t* commandC, int16_t* commandD);
//! return the packet ID for the Servo_MultiPositionCommand packet
#define getServo_MultiPositionCommandPacketID() (PKT_SERVO_MULTI_COMMAND_1)
//! return the minimum encoded length for the Servo_MultiPositionCommand packet
#define getServo_MultiPositionCommandMinDataLength() (8)
//! return the maximum encoded length for the Servo_MultiPositionCommand packet
#define getServo_MultiPositionCommandMaxDataLength() (8)
/*!
* Send this command to move the servo(s) to the commanded position. Position
* command units depend on the configuration of the servo. Send with the
* broadcast ID (0xFF) to send the position command to *all* servos.
*/
typedef struct
{
int16_t command; //!< Servo command
}Servo_PositionCommand_t;
//! Create the Servo_PositionCommand packet from parameters
void encodeServo_PositionCommandPacket(void* pkt, int16_t command);
//! Decode the Servo_PositionCommand packet to parameters
int decodeServo_PositionCommandPacket(const void* pkt, int16_t* command);
//! return the packet ID for the Servo_PositionCommand packet
#define getServo_PositionCommandPacketID() (PKT_SERVO_POSITION_COMMAND)
//! return the minimum encoded length for the Servo_PositionCommand packet
#define getServo_PositionCommandMinDataLength() (2)
//! return the maximum encoded length for the Servo_PositionCommand packet
#define getServo_PositionCommandMaxDataLength() (2)
//! Create the Servo_NeutralPositionCommand packet from parameters
void encodeServo_NeutralPositionCommandPacket(void* pkt);
//! Decode the Servo_NeutralPositionCommand packet to parameters
int decodeServo_NeutralPositionCommandPacket(const void* pkt);
//! return the packet ID for the Servo_NeutralPositionCommand packet
#define getServo_NeutralPositionCommandPacketID() (PKT_SERVO_NEUTRAL_COMMAND)
//! return the minimum encoded length for the Servo_NeutralPositionCommand packet
#define getServo_NeutralPositionCommandMinDataLength() 0
//! return the maximum encoded length for the Servo_NeutralPositionCommand packet
#define getServo_NeutralPositionCommandMaxDataLength() 0
//! Create the Servo_Disable packet from parameters
void encodeServo_DisablePacket(void* pkt);
//! Decode the Servo_Disable packet to parameters
int decodeServo_DisablePacket(const void* pkt);
//! return the packet ID for the Servo_Disable packet
#define getServo_DisablePacketID() (PKT_SERVO_DISABLE)
//! return the minimum encoded length for the Servo_Disable packet
#define getServo_DisableMinDataLength() 0
//! return the maximum encoded length for the Servo_Disable packet
#define getServo_DisableMaxDataLength() 0
//! Create the Servo_Enable packet from parameters
void encodeServo_EnablePacket(void* pkt);
//! Decode the Servo_Enable packet to parameters
int decodeServo_EnablePacket(const void* pkt);
//! return the packet ID for the Servo_Enable packet
#define getServo_EnablePacketID() (PKT_SERVO_ENABLE)
//! return the minimum encoded length for the Servo_Enable packet
#define getServo_EnableMinDataLength() 0
//! return the maximum encoded length for the Servo_Enable packet
#define getServo_EnableMaxDataLength() 0
/*!
* Set the human-readable description of this servo
*/
typedef struct
{
uint8_t title[8];
}Servo_SetTitle_t;
//! Create the Servo_SetTitle packet from parameters
void encodeServo_SetTitlePacket(void* pkt, const uint8_t title[8]);
//! Decode the Servo_SetTitle packet to parameters
int decodeServo_SetTitlePacket(const void* pkt, uint8_t title[8]);
//! return the packet ID for the Servo_SetTitle packet
#define getServo_SetTitlePacketID() (PKT_SERVO_SET_TITLE)
//! return the minimum encoded length for the Servo_SetTitle packet
#define getServo_SetTitleMinDataLength() (8)
//! return the maximum encoded length for the Servo_SetTitle packet
#define getServo_SetTitleMaxDataLength() (8)
/*!
* The *SERVO_STATUS_A* packet contains status, warning and error information,
* in addition to the servo position
*/
typedef struct
{
Servo_StatusBits_t status; //!< Status bits contain information on servo operation
Servo_WarningBits_t warnings; //!< Warning bits indicate servo is operation outside of desired range
Servo_ErrorBits_t errors; //!< These bits indicate critical system error information
int16_t position; //!< Servo position, mapped to input units
int16_t command; //!< Servo commanded position
}Servo_StatusA_t;
//! Create the Servo_StatusA packet from parameters
void encodeServo_StatusAPacket(void* pkt, const Servo_StatusBits_t* status, const Servo_WarningBits_t* warnings, const Servo_ErrorBits_t* errors, int16_t position, int16_t command);
//! Decode the Servo_StatusA packet to parameters
int decodeServo_StatusAPacket(const void* pkt, Servo_StatusBits_t* status, Servo_WarningBits_t* warnings, Servo_ErrorBits_t* errors, int16_t* position, int16_t* command);
//! return the packet ID for the Servo_StatusA packet
#define getServo_StatusAPacketID() (PKT_SERVO_STATUS_A)
//! return the minimum encoded length for the Servo_StatusA packet
#define getServo_StatusAMinDataLength() (8)
//! return the maximum encoded length for the Servo_StatusA packet
#define getServo_StatusAMaxDataLength() (8)
/*!
* The *SERVO_STATUS_B* packet contains various servo feedback data
*/
typedef struct
{
uint16_t current; //!< Servo current
uint16_t voltage; //!< Servo supply voltage
int8_t temperature; //!< Servo temperature
}Servo_StatusB_t;
//! Create the Servo_StatusB packet from parameters
void encodeServo_StatusBPacket(void* pkt, uint16_t current, uint16_t voltage, int8_t temperature);
//! Decode the Servo_StatusB packet to parameters
int decodeServo_StatusBPacket(const void* pkt, uint16_t* current, uint16_t* voltage, int8_t* temperature);
//! return the packet ID for the Servo_StatusB packet
#define getServo_StatusBPacketID() (PKT_SERVO_STATUS_B)
//! return the minimum encoded length for the Servo_StatusB packet
#define getServo_StatusBMinDataLength() (5)
//! return the maximum encoded length for the Servo_StatusB packet
#define getServo_StatusBMaxDataLength() (5)
/*!
* The *SERVO_STATUS_C* packet contains servo position data. It is a cut-down
* packet to allow high-speed feedback on servo position
*/
typedef struct
{
int16_t position; //!< Servo position, mapped to input units
}Servo_StatusC_t;
//! Create the Servo_StatusC packet from parameters
void encodeServo_StatusCPacket(void* pkt, int16_t position);
//! Decode the Servo_StatusC packet to parameters
int decodeServo_StatusCPacket(const void* pkt, int16_t* position);
//! return the packet ID for the Servo_StatusC packet
#define getServo_StatusCPacketID() (PKT_SERVO_STATUS_C)
//! return the minimum encoded length for the Servo_StatusC packet
#define getServo_StatusCMinDataLength() (2)
//! return the maximum encoded length for the Servo_StatusC packet
#define getServo_StatusCMaxDataLength() (2)
/*!
* Raw accelerometer data. To convert these raw readings to 'real' units, use
* the formula acc = 0.5 * raw * fullscale / (2^resolution)
*/
typedef struct
{
int16_t xAcc; //!< X axis acceleration value
int16_t yAcc; //!< Y axis acceleration value
int16_t zAcc; //!< Z axis acceleration value
uint8_t fullscale; //!< Accelerometer full-scale range
uint8_t resolution; //!< Accelerometer measurement resolution, in 'bits'.
}Servo_Accelerometer_t;
//! Create the Servo_Accelerometer packet from parameters
void encodeServo_AccelerometerPacket(void* pkt, int16_t xAcc, int16_t yAcc, int16_t zAcc, uint8_t fullscale, uint8_t resolution);
//! Decode the Servo_Accelerometer packet to parameters
int decodeServo_AccelerometerPacket(const void* pkt, int16_t* xAcc, int16_t* yAcc, int16_t* zAcc, uint8_t* fullscale, uint8_t* resolution);
//! return the packet ID for the Servo_Accelerometer packet
#define getServo_AccelerometerPacketID() (PKT_SERVO_ACCELEROMETER)
//! return the minimum encoded length for the Servo_Accelerometer packet
#define getServo_AccelerometerMinDataLength() (8)
//! return the maximum encoded length for the Servo_Accelerometer packet
#define getServo_AccelerometerMaxDataLength() (8)
/*!
* Servo address information
*/
typedef struct
{
uint8_t hwRev; //!< Hardware revision
uint32_t serialNumber; //!< Servo serial number
uint16_t userIDA; //!< Programmable User ID value 1/2
uint16_t userIDB; //!< Programmable User ID value 2/2
}Servo_Address_t;
//! Create the Servo_Address packet
void encodeServo_AddressPacketStructure(void* pkt, const Servo_Address_t* user);
//! Decode the Servo_Address packet
int decodeServo_AddressPacketStructure(const void* pkt, Servo_Address_t* user);
//! Create the Servo_Address packet from parameters
void encodeServo_AddressPacket(void* pkt, uint8_t hwRev, uint32_t serialNumber, uint16_t userIDA, uint16_t userIDB);
//! Decode the Servo_Address packet to parameters
int decodeServo_AddressPacket(const void* pkt, uint8_t* hwRev, uint32_t* serialNumber, uint16_t* userIDA, uint16_t* userIDB);
//! return the packet ID for the Servo_Address packet
#define getServo_AddressPacketID() (PKT_SERVO_ADDRESS)
//! return the minimum encoded length for the Servo_Address packet
#define getServo_AddressMinDataLength() (8)
//! return the maximum encoded length for the Servo_Address packet
#define getServo_AddressMaxDataLength() (8)
/*!
* Servo title information
*/
typedef struct
{
uint8_t title[8]; //!< Human readable description string for the servo
}Servo_Title_t;
//! Create the Servo_Title packet from parameters
void encodeServo_TitlePacket(void* pkt, const uint8_t title[8]);
//! Decode the Servo_Title packet to parameters
int decodeServo_TitlePacket(const void* pkt, uint8_t title[8]);
//! return the packet ID for the Servo_Title packet
#define getServo_TitlePacketID() (PKT_SERVO_TITLE)
//! return the minimum encoded length for the Servo_Title packet
#define getServo_TitleMinDataLength() (8)
//! return the maximum encoded length for the Servo_Title packet
#define getServo_TitleMaxDataLength() (8)
/*!
* Servo firmware information
*/
typedef struct
{
uint8_t major; //!< Firmware version, major number
uint8_t minor; //!< Firmware version, minor number
uint8_t day; //!< Firmware release date, day-of-month
uint8_t month; //!< Firmware release date, month-of-year
uint16_t year; //!< Firmware release date, year
uint16_t checksum; //!< Firmware checksum, 16-bit
}Servo_Firmware_t;
//! Create the Servo_Firmware packet
void encodeServo_FirmwarePacketStructure(void* pkt, const Servo_Firmware_t* user);
//! Decode the Servo_Firmware packet
int decodeServo_FirmwarePacketStructure(const void* pkt, Servo_Firmware_t* user);
//! Create the Servo_Firmware packet from parameters
void encodeServo_FirmwarePacket(void* pkt, uint8_t major, uint8_t minor, uint8_t day, uint8_t month, uint16_t year, uint16_t checksum);
//! Decode the Servo_Firmware packet to parameters
int decodeServo_FirmwarePacket(const void* pkt, uint8_t* major, uint8_t* minor, uint8_t* day, uint8_t* month, uint16_t* year, uint16_t* checksum);
//! return the packet ID for the Servo_Firmware packet
#define getServo_FirmwarePacketID() (PKT_SERVO_FIRMWARE)
//! return the minimum encoded length for the Servo_Firmware packet
#define getServo_FirmwareMinDataLength() (8)
//! return the maximum encoded length for the Servo_Firmware packet
#define getServo_FirmwareMaxDataLength() (8)
/*!
* Servo system info (uptime, etc)
*/
typedef struct
{
uint32_t msSinceReset; //!< Time since last power cycle (milliseconds)
uint16_t powerCycles; //!< Number of recorded power cycles
uint8_t resetCode; //!< Processor code indicating cause of most recent reset event
uint8_t cpuOccupancy;
}Servo_SystemInfo_t;
//! Create the Servo_SystemInfo packet
void encodeServo_SystemInfoPacketStructure(void* pkt, const Servo_SystemInfo_t* user);
//! Decode the Servo_SystemInfo packet
int decodeServo_SystemInfoPacketStructure(const void* pkt, Servo_SystemInfo_t* user);
//! Create the Servo_SystemInfo packet from parameters
void encodeServo_SystemInfoPacket(void* pkt, uint32_t msSinceReset, uint16_t powerCycles, uint8_t resetCode, uint8_t cpuOccupancy);
//! Decode the Servo_SystemInfo packet to parameters
int decodeServo_SystemInfoPacket(const void* pkt, uint32_t* msSinceReset, uint16_t* powerCycles, uint8_t* resetCode, uint8_t* cpuOccupancy);
//! return the packet ID for the Servo_SystemInfo packet
#define getServo_SystemInfoPacketID() (PKT_SERVO_SYSTEM_INFO)
//! return the minimum encoded length for the Servo_SystemInfo packet
#define getServo_SystemInfoMinDataLength() (8)
//! return the maximum encoded length for the Servo_SystemInfo packet
#define getServo_SystemInfoMaxDataLength() (8)
/*!
* Telemetry settings configuration packet
*/
typedef struct
{
Servo_TelemetrySettings_t settings; //!< Servo telemetry settings
uint8_t icdVersion; //!< Servo ICD revision. Field is encoded constant.
}Servo_TelemetryConfig_t;
//! Create the Servo_TelemetryConfig packet from parameters
void encodeServo_TelemetryConfigPacket(void* pkt, const Servo_TelemetrySettings_t* settings);
//! Decode the Servo_TelemetryConfig packet to parameters
int decodeServo_TelemetryConfigPacket(const void* pkt, Servo_TelemetrySettings_t* settings, uint8_t* icdVersion);
//! return the packet ID for the Servo_TelemetryConfig packet
#define getServo_TelemetryConfigPacketID() (PKT_SERVO_TELEMETRY_CONFIG)
//! return the minimum encoded length for the Servo_TelemetryConfig packet
#define getServo_TelemetryConfigMinDataLength() (8)
//! return the maximum encoded length for the Servo_TelemetryConfig packet
#define getServo_TelemetryConfigMaxDataLength() (8)
/*!
* Non-volatile settings configuration information
*/
typedef struct
{
unsigned eeUnlocked : 1; //!< Set if the servo is unlocked and ready to receive settings updates
unsigned eeVersion : 7; //!< Version of non-volatile settings configuration
uint16_t eeSize; //!< Size of non-volatile settings data
uint16_t eeChecksum; //!< NV settings checksum
uint8_t mramVersion;
uint16_t mramSize;
}Servo_SettingsInfo_t;
//! Create the Servo_SettingsInfo packet
void encodeServo_SettingsInfoPacketStructure(void* pkt, const Servo_SettingsInfo_t* user);
//! Decode the Servo_SettingsInfo packet
int decodeServo_SettingsInfoPacketStructure(const void* pkt, Servo_SettingsInfo_t* user);
//! Create the Servo_SettingsInfo packet from parameters
void encodeServo_SettingsInfoPacket(void* pkt, unsigned eeUnlocked, unsigned eeVersion, uint16_t eeSize, uint16_t eeChecksum, uint8_t mramVersion, uint16_t mramSize);
//! Decode the Servo_SettingsInfo packet to parameters
int decodeServo_SettingsInfoPacket(const void* pkt, unsigned* eeUnlocked, unsigned* eeVersion, uint16_t* eeSize, uint16_t* eeChecksum, uint8_t* mramVersion, uint16_t* mramSize);
//! return the packet ID for the Servo_SettingsInfo packet
#define getServo_SettingsInfoPacketID() (PKT_SERVO_SETTINGS_INFO)
//! return the minimum encoded length for the Servo_SettingsInfo packet
#define getServo_SettingsInfoMinDataLength() (8)
//! return the maximum encoded length for the Servo_SettingsInfo packet
#define getServo_SettingsInfoMaxDataLength() (8)
/*!
* Servo telltale data
*/
typedef struct
{
int8_t minTemperature; //!< Minimum temperature seen by the servo
int8_t maxTemperature; //!< Maximum temperature seen by the servo
}Servo_TelltaleA_t;
//! Create the Servo_TelltaleA packet from parameters
void encodeServo_TelltaleAPacket(void* pkt, int8_t minTemperature, int8_t maxTemperature);
//! Decode the Servo_TelltaleA packet to parameters
int decodeServo_TelltaleAPacket(const void* pkt, int8_t* minTemperature, int8_t* maxTemperature);
//! return the packet ID for the Servo_TelltaleA packet
#define getServo_TelltaleAPacketID() (PKT_SERVO_TELLTALE_A)
//! return the minimum encoded length for the Servo_TelltaleA packet
#define getServo_TelltaleAMinDataLength() (2)
//! return the maximum encoded length for the Servo_TelltaleA packet
#define getServo_TelltaleAMaxDataLength() (2)
#ifdef __cplusplus
}
#endif
#endif // _SERVOPACKETS_H

283
libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.c Normal file
View File

@ -0,0 +1,283 @@
// ServoProtocol.c was generated by ProtoGen version 3.2.a
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#include "ServoProtocol.h"
/*!
* \brief Lookup label for 'ServoModes' enum entry
*
* \param value is the integer value of the enum entry
* \return string label of the given entry
*/
const char* ServoModes_EnumLabel(int value)
{
switch (value)
{
default:
return "";
case SERVO_MODE_NORMAL:
return translateServo("SERVO_MODE_NORMAL");
case SERVO_MODE_CALIBRATING:
return translateServo("SERVO_MODE_CALIBRATING");
case SERVO_MODE_TEST:
return translateServo("SERVO_MODE_TEST");
case SERVO_MODE_NUM_MODES:
return translateServo("SERVO_MODE_NUM_MODES");
}
}
/*!
* \brief Lookup label for 'ServoMultiCommandPackets' enum entry
*
* \param value is the integer value of the enum entry
* \return string label of the given entry
*/
const char* ServoMultiCommandPackets_EnumLabel(int value)
{
switch (value)
{
default:
return "";
case PKT_SERVO_MULTI_COMMAND_1:
return translateServo("PKT_SERVO_MULTI_COMMAND_1");
case PKT_SERVO_MULTI_COMMAND_2:
return translateServo("PKT_SERVO_MULTI_COMMAND_2");
case PKT_SERVO_MULTI_COMMAND_3:
return translateServo("PKT_SERVO_MULTI_COMMAND_3");
case PKT_SERVO_MULTI_COMMAND_4:
return translateServo("PKT_SERVO_MULTI_COMMAND_4");
case PKT_SERVO_MULTI_COMMAND_5:
return translateServo("PKT_SERVO_MULTI_COMMAND_5");
case PKT_SERVO_MULTI_COMMAND_6:
return translateServo("PKT_SERVO_MULTI_COMMAND_6");
case PKT_SERVO_MULTI_COMMAND_7:
return translateServo("PKT_SERVO_MULTI_COMMAND_7");
case PKT_SERVO_MULTI_COMMAND_8:
return translateServo("PKT_SERVO_MULTI_COMMAND_8");
case PKT_SERVO_MULTI_COMMAND_9:
return translateServo("PKT_SERVO_MULTI_COMMAND_9");
case PKT_SERVO_MULTI_COMMAND_10:
return translateServo("PKT_SERVO_MULTI_COMMAND_10");
case PKT_SERVO_MULTI_COMMAND_11:
return translateServo("PKT_SERVO_MULTI_COMMAND_11");
case PKT_SERVO_MULTI_COMMAND_12:
return translateServo("PKT_SERVO_MULTI_COMMAND_12");
case PKT_SERVO_MULTI_COMMAND_13:
return translateServo("PKT_SERVO_MULTI_COMMAND_13");
case PKT_SERVO_MULTI_COMMAND_14:
return translateServo("PKT_SERVO_MULTI_COMMAND_14");
case PKT_SERVO_MULTI_COMMAND_15:
return translateServo("PKT_SERVO_MULTI_COMMAND_15");
case PKT_SERVO_MULTI_COMMAND_16:
return translateServo("PKT_SERVO_MULTI_COMMAND_16");
}
}
/*!
* \brief Lookup label for 'ServoPackets' enum entry
*
* \param value is the integer value of the enum entry
* \return string label of the given entry
*/
const char* ServoPackets_EnumLabel(int value)
{
switch (value)
{
default:
return "";
case PKT_SERVO_POSITION_COMMAND:
return translateServo("PKT_SERVO_POSITION_COMMAND");
case PKT_SERVO_NEUTRAL_COMMAND:
return translateServo("PKT_SERVO_NEUTRAL_COMMAND");
case PKT_SERVO_DISABLE:
return translateServo("PKT_SERVO_DISABLE");
case PKT_SERVO_ENABLE:
return translateServo("PKT_SERVO_ENABLE");
case PKT_SERVO_SYSTEM_COMMAND:
return translateServo("PKT_SERVO_SYSTEM_COMMAND");
case PKT_SERVO_SET_TITLE:
return translateServo("PKT_SERVO_SET_TITLE");
case PKT_SERVO_STATUS_A:
return translateServo("PKT_SERVO_STATUS_A");
case PKT_SERVO_STATUS_B:
return translateServo("PKT_SERVO_STATUS_B");
case PKT_SERVO_STATUS_C:
return translateServo("PKT_SERVO_STATUS_C");
case PKT_SERVO_STATUS_D:
return translateServo("PKT_SERVO_STATUS_D");
case PKT_SERVO_ACCELEROMETER:
return translateServo("PKT_SERVO_ACCELEROMETER");
case PKT_SERVO_ADDRESS:
return translateServo("PKT_SERVO_ADDRESS");
case PKT_SERVO_TITLE:
return translateServo("PKT_SERVO_TITLE");
case PKT_SERVO_FIRMWARE:
return translateServo("PKT_SERVO_FIRMWARE");
case PKT_SERVO_SYSTEM_INFO:
return translateServo("PKT_SERVO_SYSTEM_INFO");
case PKT_SERVO_TELEMETRY_CONFIG:
return translateServo("PKT_SERVO_TELEMETRY_CONFIG");
case PKT_SERVO_SETTINGS_INFO:
return translateServo("PKT_SERVO_SETTINGS_INFO");
case PKT_SERVO_FACTORY:
return translateServo("PKT_SERVO_FACTORY");
case PKT_SERVO_TELLTALE_A:
return translateServo("PKT_SERVO_TELLTALE_A");
case PKT_SERVO_LIMITS:
return translateServo("PKT_SERVO_LIMITS");
case PKT_SERVO_CURRENT_LIMITS:
return translateServo("PKT_SERVO_CURRENT_LIMITS");
case PKT_SERVO_POTENTIOMETER:
return translateServo("PKT_SERVO_POTENTIOMETER");
case PKT_SERVO_BACKLASH:
return translateServo("PKT_SERVO_BACKLASH");
case PKT_SERVO_BIN_DATA:
return translateServo("PKT_SERVO_BIN_DATA");
case PKT_SERVO_WEAR_LEVEL_A:
return translateServo("PKT_SERVO_WEAR_LEVEL_A");
case PKT_SERVO_WEAR_LEVEL_B:
return translateServo("PKT_SERVO_WEAR_LEVEL_B");
case PKT_SERVO_LOOKUP_TABLE:
return translateServo("PKT_SERVO_LOOKUP_TABLE");
case PKT_SERVO_LOOKUP_ELEMENT:
return translateServo("PKT_SERVO_LOOKUP_ELEMENT");
case PKT_SERVO_CONFIG:
return translateServo("PKT_SERVO_CONFIG");
case PKT_SERVO_DELTA_CONFIG:
return translateServo("PKT_SERVO_DELTA_CONFIG");
case PKT_SERVO_CALIBRATION:
return translateServo("PKT_SERVO_CALIBRATION");
case PKT_SERVO_MOTION_CONTROL:
return translateServo("PKT_SERVO_MOTION_CONTROL");
case PKT_SERVO_LIMIT_VALUES:
return translateServo("PKT_SERVO_LIMIT_VALUES");
case PKT_SERVO_DEBUG_DELTA:
return translateServo("PKT_SERVO_DEBUG_DELTA");
case PKT_SERVO_DEBUG_CTRL_LOOP:
return translateServo("PKT_SERVO_DEBUG_CTRL_LOOP");
case PKT_SERVO_DEBUG_MOTOR:
return translateServo("PKT_SERVO_DEBUG_MOTOR");
case PKT_SERVO_DEBUG_MOTION_CTRL:
return translateServo("PKT_SERVO_DEBUG_MOTION_CTRL");
case PKT_SERVO_CTRL_LOOP_SETTINGS:
return translateServo("PKT_SERVO_CTRL_LOOP_SETTINGS");
case PKT_SERVO_TELLTALE_SETTINGS:
return translateServo("PKT_SERVO_TELLTALE_SETTINGS");
case PKT_SERVO_USER_SETTINGS:
return translateServo("PKT_SERVO_USER_SETTINGS");
case PKT_SERVO_SYSTEM_SETTINGS:
return translateServo("PKT_SERVO_SYSTEM_SETTINGS");
}
}
/*!
* \brief Lookup label for 'ServoCommands' enum entry
*
* \param value is the integer value of the enum entry
* \return string label of the given entry
*/
const char* ServoCommands_EnumLabel(int value)
{
switch (value)
{
default:
return "";
case CMD_SERVO_CONFIGURE_LOOKUP_TABLE:
return translateServo("CMD_SERVO_CONFIGURE_LOOKUP_TABLE");
case CMD_SERVO_SET_LOOKUP_TABLE_ELEMENT:
return translateServo("CMD_SERVO_SET_LOOKUP_TABLE_ELEMENT");
case CMD_SERVO_GET_LOOKUP_TABLE_ELEMENT:
return translateServo("CMD_SERVO_GET_LOOKUP_TABLE_ELEMENT");
case CMD_SERVO_SET_CONFIG:
return translateServo("CMD_SERVO_SET_CONFIG");
case CMD_SERVO_SET_CURRENT_LIMIT:
return translateServo("CMD_SERVO_SET_CURRENT_LIMIT");
case CMD_SERVO_SET_TEMPERATURE_LIMIT:
return translateServo("CMD_SERVO_SET_TEMPERATURE_LIMIT");
case CMD_SERVO_SET_RATE_LIMIT:
return translateServo("CMD_SERVO_SET_RATE_LIMIT");
case CMD_SERVO_SET_STRENGTH:
return translateServo("CMD_SERVO_SET_STRENGTH");
case CMD_SERVO_SET_ILIMIT_KP:
return translateServo("CMD_SERVO_SET_ILIMIT_KP");
case CMD_SERVO_SET_ILIMIT_KI:
return translateServo("CMD_SERVO_SET_ILIMIT_KI");
case CMD_SERVO_SET_MIN_PWM_LIMIT:
return translateServo("CMD_SERVO_SET_MIN_PWM_LIMIT");
case CMD_SERVO_SET_MAX_PWM_LIMIT:
return translateServo("CMD_SERVO_SET_MAX_PWM_LIMIT");
case CMD_SERVO_SET_NEUTRAL_POSITION:
return translateServo("CMD_SERVO_SET_NEUTRAL_POSITION");
case CMD_SERVO_SET_TELEMETRY_PERIOD:
return translateServo("CMD_SERVO_SET_TELEMETRY_PERIOD");
case CMD_SERVO_SET_SILENCE_PERIOD:
return translateServo("CMD_SERVO_SET_SILENCE_PERIOD");
case CMD_SERVO_SET_TELEMETRY_PACKETS:
return translateServo("CMD_SERVO_SET_TELEMETRY_PACKETS");
case CMD_SERVO_REQUEST_HF_DATA:
return translateServo("CMD_SERVO_REQUEST_HF_DATA");
case CMD_SERVO_SET_CMD_TIMEOUT:
return translateServo("CMD_SERVO_SET_CMD_TIMEOUT");
case CMD_SERVO_SET_NODE_ID:
return translateServo("CMD_SERVO_SET_NODE_ID");
case CMD_SERVO_SET_USER_ID_A:
return translateServo("CMD_SERVO_SET_USER_ID_A");
case CMD_SERVO_SET_USER_ID_B:
return translateServo("CMD_SERVO_SET_USER_ID_B");
case CMD_SERVO_CALIBRATE_POT:
return translateServo("CMD_SERVO_CALIBRATE_POT");
case CMD_SERVO_START_TEST_MODE:
return translateServo("CMD_SERVO_START_TEST_MODE");
case CMD_SERVO_STOP_TEST_MODE:
return translateServo("CMD_SERVO_STOP_TEST_MODE");
case CMD_SERVO_START_BACKLASH_TEST:
return translateServo("CMD_SERVO_START_BACKLASH_TEST");
case CMD_SERVO_RESET_HALL_COUNTS:
return translateServo("CMD_SERVO_RESET_HALL_COUNTS");
case CMD_SERVO_SET_MIDDLE_POS:
return translateServo("CMD_SERVO_SET_MIDDLE_POS");
case CMD_SERVO_RESET_TELLTALES:
return translateServo("CMD_SERVO_RESET_TELLTALES");
case CMD_SERVO_CLEAR_BIN_DATA:
return translateServo("CMD_SERVO_CLEAR_BIN_DATA");
case CMD_SERVO_ERASE_EEPROM:
return translateServo("CMD_SERVO_ERASE_EEPROM");
case CMD_SERVO_SET_COMMISSIONING_FLAG:
return translateServo("CMD_SERVO_SET_COMMISSIONING_FLAG");
case CMD_SERVO_RESET_DEFAULT_SETTINGS:
return translateServo("CMD_SERVO_RESET_DEFAULT_SETTINGS");
case CMD_SERVO_SET_PROFILE_TASK:
return translateServo("CMD_SERVO_SET_PROFILE_TASK");
case CMD_SERVO_UNLOCK_SETTINGS:
return translateServo("CMD_SERVO_UNLOCK_SETTINGS");
case CMD_SERVO_LOCK_SETTINGS:
return translateServo("CMD_SERVO_LOCK_SETTINGS");
case CMD_SERVO_ENTER_BOOTLOADER:
return translateServo("CMD_SERVO_ENTER_BOOTLOADER");
case CMD_SERVO_RESET:
return translateServo("CMD_SERVO_RESET");
case CMD_SERVO_SET_SERIAL_NUMBER:
return translateServo("CMD_SERVO_SET_SERIAL_NUMBER");
}
}
// end of ServoProtocol.c

224
libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h Normal file
View File

@ -0,0 +1,224 @@
// ServoProtocol.h was generated by ProtoGen version 3.2.a
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#ifndef _SERVOPROTOCOL_H
#define _SERVOPROTOCOL_H
// Language target is C, C++ compilers: don't mangle us
#ifdef __cplusplus
extern "C" {
#endif
/*!
* \file
* \mainpage Servo protocol stack
*
* This is the ICD for the Currawong Engineering CAN Servo. This document
* details the Servo command and packet structure for communication with and
* configuration of the Servo
*
* The protocol API enumeration is incremented anytime the protocol is changed
* in a way that affects compatibility with earlier versions of the protocol.
* The protocol enumeration for this version is: 20
*
* The protocol version is 2.10
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h> // C string manipulation function header
//! \return the protocol API enumeration
#define getServoApi() 20
//! \return the protocol version string
#define getServoVersion() "2.10"
// Translation provided externally. The macro takes a `const char *` and returns a `const char *`
#ifndef translateServo
#define translateServo(x) x
#endif
typedef enum
{
SERVO_MODE_NORMAL, //!< The servo is in normal operational mode
SERVO_MODE_CALIBRATING,//!< The servo is in calibration mode (factory setting only)
SERVO_MODE_TEST, //!< The servo is in test mode (factory setting only)
SERVO_MODE_NUM_MODES
} ServoModes;
//! \return the label of a 'ServoModes' enum entry, based on its value
const char* ServoModes_EnumLabel(int value);
typedef enum
{
SERVO_TIMEOUT_ACTION_HOLD, //!< The servo will hold the current position
SERVO_TIMEOUT_ACTION_DISABLE,//!< The servo will be disabled
SERVO_TIMEOUT_ACTION_NEUTRAL,//!< The servo will move to its programmed neutral position
SERVO_NUM_TIMEOUT_MODES
} ServoTimeoutModes;
/*!
* Command multiple servo positions with a single command
*/
typedef enum
{
PKT_SERVO_MULTI_COMMAND_1 = 0x00,//!< Send a position command to servos 1,2,3,4
PKT_SERVO_MULTI_COMMAND_2, //!< Send a position command to servos 5,6,7,8
PKT_SERVO_MULTI_COMMAND_3, //!< Send a position command to servos 9,10,11,12
PKT_SERVO_MULTI_COMMAND_4, //!< Send a position command to servos 13,14,15,16
PKT_SERVO_MULTI_COMMAND_5, //!< Send a position command to servos 17,18,19,20
PKT_SERVO_MULTI_COMMAND_6, //!< Send a position command to servos 21,22,23,24
PKT_SERVO_MULTI_COMMAND_7, //!< Send a position command to servos 25,26,27,28
PKT_SERVO_MULTI_COMMAND_8, //!< Send a position command to servos 29,30,31,32
PKT_SERVO_MULTI_COMMAND_9, //!< Send a position command to servos 33,34,35,36
PKT_SERVO_MULTI_COMMAND_10, //!< Send a position command to servos 37,38,39,40
PKT_SERVO_MULTI_COMMAND_11, //!< Send a position command to servos 41,42,43,44
PKT_SERVO_MULTI_COMMAND_12, //!< Send a position command to servos 45,46,47,48
PKT_SERVO_MULTI_COMMAND_13, //!< Send a position command to servos 49,50,51,52
PKT_SERVO_MULTI_COMMAND_14, //!< Send a position command to servos 53,54,55,56
PKT_SERVO_MULTI_COMMAND_15, //!< Send a position command to servos 57,58,59,60
PKT_SERVO_MULTI_COMMAND_16 //!< Send a position command to servos 61,62,63,64
} ServoMultiCommandPackets;
//! \return the label of a 'ServoMultiCommandPackets' enum entry, based on its value
const char* ServoMultiCommandPackets_EnumLabel(int value);
/*!
* Servo CAN packet identifiers
*/
typedef enum
{
PKT_SERVO_POSITION_COMMAND = 0x10, //!< Send a position command to an individual servo
PKT_SERVO_NEUTRAL_COMMAND = 0x15, //!< Move the servo to the neutral position
PKT_SERVO_DISABLE = 0x20, //!< Disable a single servo, or all servos with a broadcast message
PKT_SERVO_ENABLE, //!< Enable a single servo, or all servos with a broadcast message
PKT_SERVO_SYSTEM_COMMAND = 0x50, //!< Servo system command
PKT_SERVO_SET_TITLE, //!< Set the title of this servo
PKT_SERVO_STATUS_A = 0x60, //!< Servo *STATUS_A* packet contains status and position information
PKT_SERVO_STATUS_B, //!< Servo *STATUS_B* packet contains current, temperature and other information
PKT_SERVO_STATUS_C, //!< Reserved for future use
PKT_SERVO_STATUS_D, //!< Reserved for future use
PKT_SERVO_ACCELEROMETER, //!< Raw accelerometer data
PKT_SERVO_ADDRESS = 0x70, //!< Servo address information
PKT_SERVO_TITLE, //!< Servo title information
PKT_SERVO_FIRMWARE, //!< Servo firmware information
PKT_SERVO_SYSTEM_INFO, //!< Servo system info (uptime, etc)
PKT_SERVO_TELEMETRY_CONFIG, //!< Telemetry settings
PKT_SERVO_SETTINGS_INFO, //!< Non-volatile settings configuration information
PKT_SERVO_FACTORY, //!< Factory data
PKT_SERVO_TELLTALE_A = 0x7A, //!< Servo telltale information
PKT_SERVO_LIMITS = 0x80, //!< Servo limits (current, strength, temperature)
PKT_SERVO_CURRENT_LIMITS, //!< Current limit control system settings
PKT_SERVO_POTENTIOMETER, //!< Potentiometer configuration
PKT_SERVO_BACKLASH,
PKT_SERVO_BIN_DATA, //!< Servo telltale binning data
PKT_SERVO_WEAR_LEVEL_A, //!< Wear estimate information packet 1 of 2
PKT_SERVO_WEAR_LEVEL_B, //!< Wear estimate information packet 2 of 2
PKT_SERVO_LOOKUP_TABLE, //!< Input/output mapping table information
PKT_SERVO_LOOKUP_ELEMENT, //!< Input/output mapping table element information
PKT_SERVO_CONFIG, //!< General servo configuration
PKT_SERVO_DELTA_CONFIG, //!< Position error configuration
PKT_SERVO_CALIBRATION, //!< Servo calibration data
PKT_SERVO_MOTION_CONTROL = 0x8C, //!< Servo motion control settings
PKT_SERVO_LIMIT_VALUES, //!< Servo limit value settings
PKT_SERVO_DEBUG_DELTA = 0x90, //!< Position debug information
PKT_SERVO_DEBUG_CTRL_LOOP, //!< Control loop debug information
PKT_SERVO_DEBUG_MOTOR, //!< Motor information
PKT_SERVO_DEBUG_MOTION_CTRL, //!< Motion control debug info
PKT_SERVO_CTRL_LOOP_SETTINGS = 0xA0, //!< Position control loop settings
PKT_SERVO_TELLTALE_SETTINGS = 0x101, //!< Telltale settings packet
PKT_SERVO_USER_SETTINGS = 0x105, //!< User settings (title, ID etc)
PKT_SERVO_SYSTEM_SETTINGS = 0x1FF //!< System settings (serial number, etc)
} ServoPackets;
//! \return the label of a 'ServoPackets' enum entry, based on its value
const char* ServoPackets_EnumLabel(int value);
/*!
* These commands are sent to the servo using the SERVO_SYSTEM_COMMAND packet
*/
typedef enum
{
CMD_SERVO_CONFIGURE_LOOKUP_TABLE = 0x00, //!< Configure I/O map parameters
CMD_SERVO_SET_LOOKUP_TABLE_ELEMENT, //!< Configure an individual I/O element
CMD_SERVO_GET_LOOKUP_TABLE_ELEMENT, //!< Request an individual I/O element
CMD_SERVO_SET_CONFIG = 0x10, //!< Set the servo configuration bits
CMD_SERVO_SET_CURRENT_LIMIT = 0x20, //!< Set the servo current limit
CMD_SERVO_SET_TEMPERATURE_LIMIT, //!< Set the over-temperature warning level
CMD_SERVO_SET_RATE_LIMIT, //!< Set the servo rate limit
CMD_SERVO_SET_STRENGTH, //!< Set the servo 'strength' (maximum motor duty cycle)
CMD_SERVO_SET_ILIMIT_KP, //!< Set the servo current-limit proportional gain value
CMD_SERVO_SET_ILIMIT_KI, //!< Set the servo current-limit integral gain value
CMD_SERVO_SET_MIN_PWM_LIMIT = 0x30, //!< Set the minimum clipping value for the servo PWM signal
CMD_SERVO_SET_MAX_PWM_LIMIT, //!< Set the maximum clipping value for the servo PWM signal
CMD_SERVO_SET_NEUTRAL_POSITION, //!< Set the neutral position for the servo
CMD_SERVO_SET_TELEMETRY_PERIOD = 0x40, //!< Set the servo telemetry period
CMD_SERVO_SET_SILENCE_PERIOD, //!< Set the servo silence period
CMD_SERVO_SET_TELEMETRY_PACKETS, //!< Configure telemetry packets
CMD_SERVO_REQUEST_HF_DATA = 0x43, //!< Request high-frequency telemetry data
CMD_SERVO_SET_CMD_TIMEOUT, //!< Set the command timeout for the servo
CMD_SERVO_SET_NODE_ID = 0x50, //!< Configure the CAN servo node ID
CMD_SERVO_SET_USER_ID_A, //!< Set User ID A
CMD_SERVO_SET_USER_ID_B, //!< Set User ID B
CMD_SERVO_CALIBRATE_POT = 0x60, //!< Initiate servo potentiometer calibration procedure
CMD_SERVO_START_TEST_MODE,
CMD_SERVO_STOP_TEST_MODE,
CMD_SERVO_START_BACKLASH_TEST,
CMD_SERVO_RESET_HALL_COUNTS,
CMD_SERVO_SET_MIDDLE_POS = 0x70,
CMD_SERVO_RESET_TELLTALES = 0x80, //!< Reset servo telltale data
CMD_SERVO_CLEAR_BIN_DATA,
CMD_SERVO_ERASE_EEPROM = 0x90,
CMD_SERVO_SET_COMMISSIONING_FLAG = 0x95, //!< Set servo commissioning flag
CMD_SERVO_RESET_DEFAULT_SETTINGS = 0xA0, //!< Reset servo configuration settings to default values
CMD_SERVO_SET_PROFILE_TASK = 0xF0, //!< Set the task to profile
CMD_SERVO_UNLOCK_SETTINGS = 0xF5, //!< Unlock servo settings
CMD_SERVO_LOCK_SETTINGS, //!< Lock servo settings
CMD_SERVO_ENTER_BOOTLOADER = 0xFB, //!< Enter bootloader mode
CMD_SERVO_RESET, //!< Reset servo
CMD_SERVO_SET_SERIAL_NUMBER = 0xFF //!< Set the serial number
} ServoCommands;
//! \return the label of a 'ServoCommands' enum entry, based on its value
const char* ServoCommands_EnumLabel(int value);
// The prototypes below provide an interface to the packets.
// They are not auto-generated functions, but must be hand-written
//! \return the packet data pointer from the packet
uint8_t* getServoPacketData(void* pkt);
//! \return the packet data pointer from the packet, const
const uint8_t* getServoPacketDataConst(const void* pkt);
//! Complete a packet after the data have been encoded
void finishServoPacket(void* pkt, int size, uint32_t packetID);
//! \return the size of a packet from the packet header
int getServoPacketSize(const void* pkt);
//! \return the ID of a packet from the packet header
uint32_t getServoPacketID(const void* pkt);
#ifdef __cplusplus
}
#endif
#endif // _SERVOPROTOCOL_H

342
libraries/AP_PiccoloCAN/piccolo_protocol/Servo_source.xml Normal file
View File

@ -0,0 +1,342 @@
<Protocol name="Servo" prefix="Servo_" api="20" version="2.10" mapfile="Servo_SettingsMap" verifyfile="Servo_SettingsVerify" endian="big" supportSpecialFloat="false" supportInt64="false" supportFloat64="false" supportBool="true" comment="This is the ICD for the Currawong Engineering CAN Servo. This document details the Servo command and packet structure for communication with and configuration of the Servo"><Include name="string.h" comment="C string manipulation function header" global="true" />
<Enum name="ServoModes" lookup="true" prefix="SERVO_MODE_">
<Value name="NORMAL" comment="The servo is in normal operational mode" />
<Value name="CALIBRATING" comment="The servo is in calibration mode (factory setting only)" />
<Value name="TEST" comment="The servo is in test mode (factory setting only)" />
<Value name="NUM_MODES" />
</Enum>
<Enum name="ServoTimeoutModes" prefix="SERVO_TIMEOUT_ACTION_">
<Value name="HOLD" comment="The servo will hold the current position" />
<Value name="DISABLE" comment="The servo will be disabled" />
<Value name="NEUTRAL" comment="The servo will move to its programmed neutral position" />
<Value name="SERVO_NUM_TIMEOUT_MODES" ignorePrefix="true" />
</Enum>
<Structure name="StatusBits" comment="Servo status bits" file="ServoDefines">
<Data name="enabled" inMemoryType="bitfield1" comment="Servo enabled status" />
<Data name="mode" enum="ServoModes" encodedType="bitfield3" comment="Servo mode" />
<Data name="cmdReceived" inMemoryType="bitfield1" comment="Set if the servo has received a valid position command within the configured timeout period" />
<Data name="reservedA" inMemoryType="bitfield1" comment="Reserved for future use" />
<Data name="reservedB" inMemoryType="bitfield1" comment="Reserved for future use" />
<Data name="reservedC" inMemoryType="bitfield1" comment="Reserved for future use" />
</Structure>
<Structure name="WarningBits" comment="Servo warning bits" file="ServoDefines">
<Data name="overCurrent" inMemoryType="bitfield1" comment="Servo current exceeds warning threshold" />
<Data name="overTemperature" inMemoryType="bitfield1" comment="Servo temperature exceeds warning threshold" />
<Data name="overAcceleration" inMemoryType="bitfield1" comment="Servo accelerometer reading exceeds warning threshold" />
<Data name="invalidInput" inMemoryType="bitfield1" comment="Most recent servo position command was outside valid range" />
<Data name="position" inMemoryType="bitfield1" comment="Servo position is outside calibrated range" />
<Data name="potCalibration" inMemoryType="bitfield1" comment="Servo potentiometer is not correctly calibrated" />
<Data name="underVoltage" inMemoryType="bitfield1" comment="Servo voltage is below operational threshold" />
<Data name="overVoltage" inMemoryType="bitfield1" comment="Servo voltage is above operational threshold" />
<Data name="reservedB" inMemoryType="bitfield8" comment="Reserved for future use" />
</Structure>
<Structure name="ErrorBits" comment="Servo error bits" file="ServoDefines">
<Data name="position" inMemoryType="bitfield1" comment="Servo position is outside measurement range" />
<Data name="potError" inMemoryType="bitfield1" comment="Error reading servo position" />
<Data name="accError" inMemoryType="bitfield1" comment="Error reading servo accelerometer data" />
<Data name="mapCalibration" inMemoryType="bitfield1" comment="Servo input/output map is not correctly calibrated" />
<Data name="settingsError" inMemoryType="bitfield1" comment="Error reading servo settings from memory" />
<Data name="healthError" inMemoryType="bitfield1" comment="Error reading servo health tracking information from memory" />
<Data name="tracking" inMemoryType="bitfield1" comment="Servo position cannot match commanded position" />
<Data name="reservedB" inMemoryType="bitfield1" comment="Reserved for future use" />
</Structure>
<Structure name="TelemetryPackets" comment="Servo packet selection" map="true" file="ServoDefines">
<Data name="statusA" inMemoryType="bitfield1" comment="Select the *STATUS_A* packet" initialValue="1" verifyMaxValue="1" />
<Data name="statusB" inMemoryType="bitfield1" comment="Select the *STATUS_B* packet" initialValue="1" />
<Data name="statusC" inMemoryType="bitfield1" comment="Select the *STATUS_C* packet" initialValue="0" />
<Data name="statusD" inMemoryType="bitfield1" comment="Select the *STATUS_D* packet" initialValue="0" />
<Data name="accelerometer" inMemoryType="bitfield1" comment="Select the *ACCELEROMETER* packet" initialValue="0" />
<Data name="dbgA" inMemoryType="bitfield1" comment="Reserved field (set to zero)" map="false" initialValue="0" />
<Data name="dbgB" inMemoryType="bitfield1" comment="Reserved field (set to zero)" map="false" initialValue="0" />
<Data name="dbgC" inMemoryType="bitfield1" comment="Reserved field (set to zero)" map="false" initialValue="0" />
</Structure>
<Structure name="TelemetrySettings" map="true" comment="Servo telemetry configuration" file="ServoDefines">
<Data name="period" inMemoryType="unsigned8" comment="Servo telemetry period" units="50ms per bit" range="0 (Disabled) to 250 (12.5s)" initialValue="10" verifyMaxValue="250" />
<Data name="silence" inMemoryType="unsigned8" comment="Servo silence period (after boot)" units="50ms per bit" range="0 to 250 (12.5s)" initialValue="10" verifyMaxValue="250" />
<Data name="packets" struct="TelemetryPackets" comment="Telemetry packet selection" />
<Data name="responsePackets" struct="TelemetryPackets" comment="Command response packet selection" default="0" />
</Structure>
<Structure name="ConfigBits" file="ServoDefines" map="true">
<Data name="mapEnabled" inMemoryType="bitfield1" comment="1 = I/O map is enabled" initialValue="0" />
<Data name="piccoloCmd" inMemoryType="bitfield1" comment="1 = The servo will listen for Piccolo autopilot CAN messages" initialValue="1" />
<Data name="neutralPositionEnabled" inMemoryType="bitfield1" comment="1 = Servo neutral position is enabled, 0 = Neutral position disabled" initialValue="0" />
<Data name="piccoloChannel" inMemoryType="bitfield5" comment="Channel the servo will use to listen for Piccolo messages." range="1 to 16" notes="A value of zero indicates that the servo's CAN ID will be used to determine the Piccolo Channel. Values above 16 will be set to zero" verifyMaxValue="16" initialValue="0" />
<Data name="timeoutAction" inMemoryType="bitfield2" comment="Servo action at powerup" initialValue="0" />
<Data name="reservedB" inMemoryType="bitfield2" map="false" comment="reserved for future use" />
<Data name="reservedC" inMemoryType="bitfield4" map="false" comment="reserved for future use" />
</Structure>
<Enum name="ServoMultiCommandPackets" lookup="true" prefix="PKT_SERVO_" comment="Command multiple servo positions with a single command" description="Using a single CAN frame, the position(s) of up to four servos can be commanded. The address IDs of these four servos must be sequential, and the CAN frame must be broadcast such that all servos on the CAN bus receive the frame.">
<Value name="MULTI_COMMAND_1" value="0x00" comment="Send a position command to servos 1,2,3,4" />
<Value name="MULTI_COMMAND_2" comment="Send a position command to servos 5,6,7,8" />
<Value name="MULTI_COMMAND_3" comment="Send a position command to servos 9,10,11,12" />
<Value name="MULTI_COMMAND_4" comment="Send a position command to servos 13,14,15,16" />
<Value name="MULTI_COMMAND_5" comment="Send a position command to servos 17,18,19,20" />
<Value name="MULTI_COMMAND_6" comment="Send a position command to servos 21,22,23,24" />
<Value name="MULTI_COMMAND_7" comment="Send a position command to servos 25,26,27,28" />
<Value name="MULTI_COMMAND_8" comment="Send a position command to servos 29,30,31,32" />
<Value name="MULTI_COMMAND_9" comment="Send a position command to servos 33,34,35,36" />
<Value name="MULTI_COMMAND_10" comment="Send a position command to servos 37,38,39,40" />
<Value name="MULTI_COMMAND_11" comment="Send a position command to servos 41,42,43,44" />
<Value name="MULTI_COMMAND_12" comment="Send a position command to servos 45,46,47,48" />
<Value name="MULTI_COMMAND_13" comment="Send a position command to servos 49,50,51,52" />
<Value name="MULTI_COMMAND_14" comment="Send a position command to servos 53,54,55,56" />
<Value name="MULTI_COMMAND_15" comment="Send a position command to servos 57,58,59,60" />
<Value name="MULTI_COMMAND_16" comment="Send a position command to servos 61,62,63,64" />
</Enum>
<Enum name="ServoPackets" comment="Servo CAN packet identifiers" prefix="PKT_SERVO_" lookup="true" description="Following are the enumerated identifiers for the various servo CAN packets available">
<Value name="POSITION_COMMAND" value="0x10" comment="Send a position command to an individual servo" />
<Value name="NEUTRAL_COMMAND" value="0x15" comment="Move the servo to the neutral position" />
<Value name="DISABLE" value="0x20" comment="Disable a single servo, or all servos with a broadcast message" />
<Value name="ENABLE" comment="Enable a single servo, or all servos with a broadcast message" />
<Value name="SYSTEM_COMMAND" value="0x50" comment="Servo system command" />
<Value name="SET_TITLE" comment="Set the title of this servo" />
<Value name="STATUS_A" value="0x60" comment="Servo *STATUS_A* packet contains status and position information" />
<Value name="STATUS_B" comment="Servo *STATUS_B* packet contains current, temperature and other information" />
<Value name="STATUS_C" comment="Reserved for future use" />
<Value name="STATUS_D" comment="Reserved for future use" />
<Value name="ACCELEROMETER" comment="Raw accelerometer data" />
<Value name="ADDRESS" value="0x70" comment="Servo address information" />
<Value name="TITLE" comment="Servo title information" />
<Value name="FIRMWARE" comment="Servo firmware information" />
<Value name="SYSTEM_INFO" comment="Servo system info (uptime, etc)" />
<Value name="TELEMETRY_CONFIG" comment="Telemetry settings" />
<Value name="EEPROM" comment="Non-volatile settings configuration information" />
<Value name="TELLTALE_A" value="0x7A" comment="Servo telltale information" hidden="true" />
<Value name="CURRENT_LIMITS" comment="Current limit control system settings" hidden="true" />
<Value name="BACKLASH" comment="" hidden="true" />
<Value name="WEAR_LEVEL_A" comment="Wear estimate information packet 1 of 2" hidden="true" />
<Value name="LOOKUP_TABLE" comment="Input/output mapping table information" hidden="true" />
<Value name="CONFIG" comment="General servo configuration" />
<Value name="CALIBRATION" comment="Servo calibration data" hidden="true" />
<Value name="LIMIT_VALUES" hidden="true" comment="Servo limit value settings" />
<Value name="DEBUG_CTRL_LOOP" comment="Control loop debug information" hidden="true" />
<Value name="DEBUG_MOTION_CTRL" comment="Motion control debug info" hidden="true" />
<Value name="TELLTALE_SETTINGS" value="0x101" comment="Telltale settings packet" hidden="true" />
<Value name="SYSTEM_SETTINGS" value="0x1FF" comment="System settings (serial number, etc)" hidden="true" />
</Enum>
<Enum name="ServoCommands" prefix="CMD_SERVO_" lookup="true" comment="These commands are sent to the servo using the SERVO_SYSTEM_COMMAND packet" description="Multiple commands can be sent to the servo using the SERVO_SYSTEM_COMMAND packet, where the first byte of the packet describes the command type. Below are the enumerated identifiers for the available servo commands. Each command is described at length further in this document.">
<Value name="SET_LOOKUP_TABLE_ELEMENT" comment="Configure an individual I/O element" hidden="true" />
<Value name="SET_CONFIG" value="0x10" comment="Set the servo configuration bits" hidden="true" />
<Value name="SET_TEMPERATURE_LIMIT" hidden="true" comment="Set the over-temperature warning level" />
<Value name="SET_STRENGTH" hidden="true" comment="Set the servo 'strength' (maximum motor duty cycle)" />
<Value name="SET_ILIMIT_KI" hidden="true" comment="Set the servo current-limit integral gain value" />
<Value name="SET_MAX_PWM_LIMIT" hidden="true" comment="Set the maximum clipping value for the servo PWM signal" />
<Value name="SET_TELEMETRY_PERIOD" value="0x40" comment="Set the servo telemetry period" hidden="true" />
<Value name="SET_TELEMETRY_PACKETS" comment="Configure telemetry packets" hidden="true" />
<Value name="REQUEST_HF_DATA" value="0x43" comment="Request high-frequency telemetry data" />
<Value name="SET_NODE_ID" value="0x50" comment="Configure the CAN servo node ID" />
<Value name="SET_USER_ID_A" comment="Set User ID A" />
<Value name="SET_USER_ID_B" comment="Set User ID B" />
<Value name="START_TEST_MODE" hidden="true" />
<Value name="START_BACKLASH_TEST" comment="" hidden="true" />
<Value name="SET_MIDDLE_POS" value="0x70" hidden="true" />
<Value name="CLEAR_BIN_DATA" hidden="true" />
<Value name="SET_COMMISSIONING_FLAG" value="0x95" hidden="true" comment="Set servo commissioning flag" />
<Value name="RESET_DEFAULT_SETTINGS" value="0xA0" comment="Reset servo configuration settings to default values" />
<Value name="UNLOCK_SETTINGS" value="0xF5" comment="Unlock servo settings" />
<Value name="LOCK_SETTINGS" comment="Lock servo settings" />
<Value name="ENTER_BOOTLOADER" value="0xFB" comment="Enter bootloader mode" />
<Value name="RESET" comment="Reset servo" />
</Enum>
<Packet name="MultiPositionCommand" ID="PKT_SERVO_MULTI_COMMAND_1" map="false" file="ServoPackets" parameterInterface="true" comment="This command can be used to command multiple servos (1 to 4) with a single CAN frame). The addresses of the targetted servos must be sequential, with the initial address based on the packet ID. The example packet shown below corresponds to servos with IDs {1, 2, 3, 4}. For other addresses refer to the *ServoMultiCommandPackets* enumeration. Note: The CAN frame must be sent to the broadcast ID (0xFF) for all servos to receive this message.">
<Data name="commandA" inMemoryType="signed16" comment="Servo command for servo with address offset 0" units="dimensionless" range="-20,000 to +20,000" notes="Input limits depend on servo I/O mapping" />
<Data name="commandB" inMemoryType="signed16" comment="Servo command for servo with address offset 1" units="dimensionless" range="-20,000 to +20,000" notes="Input limits depend on servo I/O mapping" />
<Data name="commandC" inMemoryType="signed16" comment="Servo command for servo with address offset 3" units="dimensionless" range="-20,000 to +20,000" notes="Input limits depend on servo I/O mapping" />
<Data name="commandD" inMemoryType="signed16" comment="Servo command for servo with address offset 3" units="dimensionless" range="-20,000 to +20,000" notes="Input limits depend on servo I/O mapping" />
</Packet>
<Packet name="PositionCommand" ID="PKT_SERVO_POSITION_COMMAND" comment="Send this command to move the servo(s) to the commanded position. Position command units depend on the configuration of the servo. Send with the broadcast ID (0xFF) to send the position command to *all* servos." file="ServoPackets" parameterInterface="true">
<Data name="command" inMemoryType="signed16" comment="Servo command" units="dimensionless" range="-20,000 to +20,000" notes="Input limits depend on servo I/O mapping" />
</Packet>
<Packet name="NeutralPositionCommand" ID="PKT_SERVO_NEUTRAL_COMMAND" comment="Send this command (with zero data bytes) to move the servo(s) to the neutral position (if enabled). Send with the broadcast ID (0xFF) to move *all* servos to their neutral positions" file="ServoPackets" parameterInterface="true">
</Packet>
<Packet name="Disable" ID="PKT_SERVO_DISABLE" file="ServoPackets" parameterInterface="true" comment="Send this command (with zero data bytes) to disable the servo. Send with the broadcast ID (0xFF) to disable *all* servos.">
</Packet>
<Packet name="Enable" ID="PKT_SERVO_ENABLE" file="ServoPackets" parameterInterface="true" comment="Send this command (with zero data bytes) to enable the servo. Send with the broadcast ID (0xFF) to enable *all* servos.">
</Packet>
<Packet name="SetTitle" ID="PKT_SERVO_SET_TITLE" file="ServoPackets" parameterInterface="true" comment="Set the human-readable description of this servo">
<Data name="title" inMemoryType="unsigned8" array="8" />
</Packet>
<Packet name="StatusA" ID="PKT_SERVO_STATUS_A" file="ServoPackets" parameterInterface="true" comment="The *SERVO_STATUS_A* packet contains status, warning and error information, in addition to the servo position">
<Data name="status" struct="StatusBits" comment="Status bits contain information on servo operation" />
<Data name="warnings" struct="WarningBits" comment="Warning bits indicate servo is operation outside of desired range" />
<Data name="errors" struct="ErrorBits" comment="These bits indicate critical system error information" />
<Data name="position" inMemoryType="signed16" comment="Servo position, mapped to input units" />
<Data name="command" inMemoryType="signed16" comment="Servo commanded position" />
</Packet>
<Packet name="StatusB" ID="PKT_SERVO_STATUS_B" file="ServoPackets" parameterInterface="true" comment="The *SERVO_STATUS_B* packet contains various servo feedback data">
<Data name="current" inMemoryType="unsigned16" units="10mA per bit" comment="Servo current" />
<Data name="voltage" inMemoryType="unsigned16" units="10mV per bit" comment="Servo supply voltage" />
<Data name="temperature" inMemoryType="signed8" comment="Servo temperature" units="1C per bit" />
</Packet>
<Packet name="StatusC" ID="PKT_SERVO_STATUS_C" file="ServoPackets" parameterInterface="true" comment="The *SERVO_STATUS_C* packet contains servo position data. It is a cut-down packet to allow high-speed feedback on servo position">
<Data name="position" inMemoryType="signed16" comment="Servo position, mapped to input units" />
</Packet>
<Packet name="Accelerometer" ID="PKT_SERVO_ACCELEROMETER" file="ServoPackets" parameterInterface="true" comment="Raw accelerometer data. To convert these raw readings to 'real' units, use the formula acc = 0.5 * raw * fullscale / (2^resolution)">
<Data name="xAcc" inMemoryType="signed16" comment="X axis acceleration value" range="-0x7FFF to +0x7FFF" notes="Multiply by (0.5 * fullscale / 2^resolution) to get acceleration value in 'g' units" />
<Data name="yAcc" inMemoryType="signed16" comment="Y axis acceleration value" range="-0x7FFF to +0x7FFF" notes="Multiply by (0.5 * fullscale / 2^resolution) to get acceleration value in 'g' units" />
<Data name="zAcc" inMemoryType="signed16" comment="Z axis acceleration value" range="-0x7FFF to +0x7FFF" notes="Multiply by (0.5 * fullscale / 2^resolution) to get acceleration value in 'g' units" />
<Data name="fullscale" inMemoryType="unsigned8" comment="Accelerometer full-scale range" />
<Data name="resolution" inMemoryType="unsigned8" comment="Accelerometer measurement resolution, in 'bits'." />
</Packet>
<Packet name="Address" ID="PKT_SERVO_ADDRESS" file="ServoPackets" parameterInterface="true">
<Data name="hwRev" inMemoryType="unsigned8" comment="Hardware revision" />
<Data name="serialNumber" inMemoryType="unsigned32" encodedType="unsigned24" comment="Servo serial number" />
<Data name="userIDA" inMemoryType="unsigned16" comment="Programmable User ID value 1/2" />
<Data name="userIDB" inMemoryType="unsigned16" comment="Programmable User ID value 2/2" />
</Packet>
<Packet name="Title" ID="PKT_SERVO_TITLE" file="ServoPackets" parameterInterface="true">
<Data name="title" inMemoryType="unsigned8" array="8" comment="Human readable description string for the servo" />
</Packet>
<Packet name="Firmware" ID="PKT_SERVO_FIRMWARE" file="ServoPackets" parameterInterface="true">
<Data name="major" inMemoryType="unsigned8" comment="Firmware version, major number" />
<Data name="minor" inMemoryType="unsigned8" comment="Firmware version, minor number" />
<Data name="day" inMemoryType="unsigned8" comment="Firmware release date, day-of-month" />
<Data name="month" inMemoryType="unsigned8" comment="Firmware release date, month-of-year" />
<Data name="year" inMemoryType="unsigned16" comment="Firmware release date, year" />
<Data name="checksum" inMemoryType="unsigned16" comment="Firmware checksum, 16-bit" />
</Packet>
<Packet name="SystemInfo" ID="PKT_SERVO_SYSTEM_INFO" file="ServoPackets" parameterInterface="true">
<Data name="msSinceReset" inMemoryType="unsigned32" comment="Time since last power cycle (milliseconds)" />
<Data name="powerCycles" inMemoryType="unsigned16" comment="Number of recorded power cycles" />
<Data name="resetCode" inMemoryType="unsigned8" comment="Processor code indicating cause of most recent reset event" />
<Data name="cpuOccupancy" inMemoryType="unsigned8" units="1% per bit" />
</Packet>
<Packet name="TelemetryConfig" ID="PKT_SERVO_TELEMETRY_CONFIG" file="ServoPackets" parameterInterface="true" comment="Telemetry settings configuration packet">
<Data name="settings" struct="TelemetrySettings" comment="Servo telemetry settings" />
<Data name="reserved" array="3" inMemoryType="null" encodedType="unsigned8" constant="0" comment="Reserved for future use" />
<Data name="icdVersion" inMemoryType="unsigned8" constant="getServoApi()" comment="Servo ICD revision" />
</Packet>
<Packet name="EEPROMInformation" ID="PKT_SERVO_EEPROM" file="ServoPackets" parameterInterface="true" structureInterface="true">
<Data name="eeUnlocked" inMemoryType="bitfield1" comment="Set if the servo is unlocked and ready to receive settings updates" />
<Data name="eeVersion" inMemoryType="bitfield7" comment="Version of non-volatile settings configuration" />
<Data name="eeSize" inMemoryType="unsigned16" comment="Size of non-volatile settings data" />
<Data name="eeChecksum" inMemoryType="unsigned16" comment="NV settings checksum" />
<Data name="mramVersion" inMemoryType="unsigned8" />
<Data name="mramSize" inMemoryType="unsigned16" />
</Packet>
<Packet name="TelltaleA" ID="PKT_SERVO_TELLTALE_A" file="ServoPackets" parameterInterface="true" comment="Servo telltale data">
<Data name="minTemperature" inMemoryType="signed8" comment="Minimum temperature seen by the servo" />
<Data name="maxTemperature" inMemoryType="signed8" comment="Maximum temperature seen by the servo" />
</Packet>
<Packet name="Config" useInOtherPackets="true" map="true" ID="PKT_SERVO_CONFIG" file="ServoPackets" parameterInterface="true" structureInterface="true" comment="General servo configuration settings">
<Data name="options" struct="ConfigBits" comment="Servo configuration parameters" />
<Data name="commandTimeout" inMemoryType="unsigned16" comment="Servo command timeout" units="1ms per bit" range="0 to 65535" notes="0 = Timeout disabled" initialValue="1000" verifyMaxValue="60000" />
<Data name="homePosition" inMemoryType="signed16" comment="Servo neutral position. Servo can be configured to return to this position at powerup, or after loss of communication" notes="Neutral position is specified in *input* units. i.e. if the I/O map is enabled, it will be applied to the neutral position" initialValue="1500" verifyMinValue="-20000" verifyMaxValue="20000" units="dimensionless" />
<Data name="reserved" inMemoryType="unsigned8" array="2" comment="Reserved for future use" map="false" />
</Packet>
<Packet name="RequestHighFrequencyData" ID="PKT_SERVO_SYSTEM_COMMAND" file="ServoCommands" parameterInterface="true" comment="Select packets for high-frequency transmission. Selected packets will be transmitted at 500Hz for one second. Sending this command again resets the timer, allowing continuous data as long as this packet is received">
<Data name="command" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="CMD_SERVO_REQUEST_HF_DATA" />
<Data name="packets" struct="TelemetryPackets" comment="Select which telemetry packets are transmitted at high-speed by the servo" />
</Packet>
<Packet name="SetNodeID" ID="PKT_SERVO_SYSTEM_COMMAND" file="ServoCommands" parameterInterface="true" comment="Set the Node ID (CAN Address) for the servo">
<Data name="command" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="CMD_SERVO_SET_NODE_ID" />
<Data name="serialNumber" inMemoryType="unsigned32" comment="The serial number must match that of the servo for the command to be accepted" />
<Data name="nodeID" inMemoryType="unsigned8" comment="Set the CAN Node ID of the target servo" range="0 to 254" notes="A servo with a Node ID of zero (0) will be disabled" />
</Packet>
<Packet name="SetUserIdA" ID="PKT_SERVO_SYSTEM_COMMAND" file="ServoCommands" parameterInterface="true" comment="Set user programmable ID value">
<Data name="command" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="CMD_SERVO_SET_USER_ID_A" />
<Data name="id" inMemoryType="unsigned16" comment="User configurable value, 16-bit" />
</Packet>
<Packet name="SetUserIdB" ID="PKT_SERVO_SYSTEM_COMMAND" file="ServoCommands" parameterInterface="true" comment="Set user programmable ID value">
<Data name="command" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="CMD_SERVO_SET_USER_ID_B" />
<Data name="id" inMemoryType="unsigned16" comment="User configurable value, 16-bit" />
</Packet>
<Packet name="ResetDefaultSettings" ID="PKT_SERVO_SYSTEM_COMMAND" file="ServoCommands" parameterInterface="true" comment="Reset servo settings to default values">
<Data name="command" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="CMD_SERVO_RESET_DEFAULT_SETTINGS" />
<Data name="resetSeqA" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="0xAA" />
<Data name="resetSeqB" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="0x55" />
</Packet>
<Packet name="UnlockSettings" ID="PKT_SERVO_SYSTEM_COMMAND" file="ServoCommands">
<Data name="command" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="CMD_SERVO_UNLOCK_SETTINGS" />
<Data name="unlockSeqA" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="0xA0" />
<Data name="unlockSeqB" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="0xB0" />
<Data name="unlockSeqC" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="0xC0" />
</Packet>
<Packet name="LockSettings" ID="PKT_SERVO_SYSTEM_COMMAND" file="ServoCommands">
<Data name="command" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="CMD_SERVO_LOCK_SETTINGS" />
<Data name="lockSeqA" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="0x0A" />
<Data name="lockSeqB" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="0x0B" />
<Data name="lockSeqC" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="0x0C" />
</Packet>
<Packet name="EnterBootloader" ID="PKT_SERVO_SYSTEM_COMMAND" file="ServoCommands">
<Data name="command" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="CMD_SERVO_ENTER_BOOTLOADER" />
<Data name="bootSeqA" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="0xAA" comment="This byte is required for the command to be accepted" />
<Data name="bootSeqB" inMemoryType="null" encodedType="unsigned8" checkConstant="true" constant="0x55" comment="This byte is required for the command to be accepted" />
</Packet>
</Protocol>

2
libraries/AP_PiccoloCAN/piccolo_protocol/fielddecode.c
View File

@ -14,7 +14,7 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#include "fielddecode.h"

2
libraries/AP_PiccoloCAN/piccolo_protocol/fielddecode.h
View File

@ -14,7 +14,7 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#ifndef _FIELDDECODE_H

2
libraries/AP_PiccoloCAN/piccolo_protocol/fieldencode.c
View File

@ -14,7 +14,7 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#include "fieldencode.h"

2
libraries/AP_PiccoloCAN/piccolo_protocol/fieldencode.h
View File

@ -14,7 +14,7 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#ifndef _FIELDENCODE_H

2
libraries/AP_PiccoloCAN/piccolo_protocol/scaleddecode.c
View File

@ -14,7 +14,7 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#include "scaleddecode.h"

2
libraries/AP_PiccoloCAN/piccolo_protocol/scaleddecode.h
View File

@ -14,7 +14,7 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#ifndef _SCALEDDECODE_H

2
libraries/AP_PiccoloCAN/piccolo_protocol/scaledencode.c
View File

@ -14,7 +14,7 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#include "scaledencode.h"

2
libraries/AP_PiccoloCAN/piccolo_protocol/scaledencode.h
View File

@ -14,7 +14,7 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Oliver Walters
* Author: Oliver Walters / Currawong Engineering Pty Ltd
*/
#ifndef _SCALEDENCODE_H