From 9af4fa46d3ab9247d66b680cc83e984c01d41ed3 Mon Sep 17 00:00:00 2001 From: Oliver Walters Date: Tue, 10 Nov 2020 13:51:03 +1100 Subject: [PATCH] AP_PiccoloCAN: Add support for CBS servo protocol - Adds protocol files for the servo protocol - Generated using Protogen tool --- libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp | 182 +- libraries/AP_PiccoloCAN/AP_PiccoloCAN.h | 24 +- .../piccolo_protocol/ServoCommands.c | 492 +++++ .../piccolo_protocol/ServoCommands.h | 194 ++ .../piccolo_protocol/ServoDefines.c | 514 +++++ .../piccolo_protocol/ServoDefines.h | 191 ++ .../piccolo_protocol/ServoPackets.c | 1736 +++++++++++++++++ .../piccolo_protocol/ServoPackets.h | 509 +++++ .../piccolo_protocol/ServoProtocol.c | 283 +++ .../piccolo_protocol/ServoProtocol.h | 224 +++ .../piccolo_protocol/Servo_source.xml | 342 ++++ .../piccolo_protocol/fielddecode.c | 2 +- .../piccolo_protocol/fielddecode.h | 2 +- .../piccolo_protocol/fieldencode.c | 2 +- .../piccolo_protocol/fieldencode.h | 2 +- .../piccolo_protocol/scaleddecode.c | 2 +- .../piccolo_protocol/scaleddecode.h | 2 +- .../piccolo_protocol/scaledencode.c | 2 +- .../piccolo_protocol/scaledencode.h | 2 +- 19 files changed, 4692 insertions(+), 15 deletions(-) create mode 100644 libraries/AP_PiccoloCAN/piccolo_protocol/ServoCommands.c create mode 100644 libraries/AP_PiccoloCAN/piccolo_protocol/ServoCommands.h create mode 100644 libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.c create mode 100644 libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.h create mode 100644 libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c create mode 100644 libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h create mode 100644 libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.c create mode 100644 libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h create mode 100644 libraries/AP_PiccoloCAN/piccolo_protocol/Servo_source.xml diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index 3dcc41ee2c..710c47e703 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -12,7 +12,7 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Author: Oliver Walters + * Author: Oliver Walters / Currawong Engineering Pty Ltd */ @@ -35,9 +35,14 @@ #include +// Protocol files for the Velocity ESC #include #include +// Protocol files for the CBS servo +#include +#include + 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 diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h index 96cc64dc42..80b5af3330 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h @@ -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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoCommands.c b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoCommands.c new file mode 100644 index 0000000000..2849eb0f8b --- /dev/null +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoCommands.c @@ -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 . + * + * 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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoCommands.h b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoCommands.h new file mode 100644 index 0000000000..23efb3dc42 --- /dev/null +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoCommands.h @@ -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 . + * + * 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 +#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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.c b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.c new file mode 100644 index 0000000000..93473f903c --- /dev/null +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.c @@ -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 . + * + * 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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.h b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.h new file mode 100644 index 0000000000..905b1f6c68 --- /dev/null +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.h @@ -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 . + * + * 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 +#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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c new file mode 100644 index 0000000000..c310e6c29c --- /dev/null +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c @@ -0,0 +1,1736 @@ +// ServoPackets.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 . + * + * Author: Oliver Walters / Currawong Engineering Pty Ltd + */ + +#include "ServoPackets.h" +#include "fielddecode.h" +#include "fieldencode.h" +#include "scaleddecode.h" +#include "scaledencode.h" + +/*! + * \brief Create the Servo_Config packet + * + * General servo configuration settings + * \param _pg_pkt points to the packet which will be created by this function + * \param _pg_user points to the user data that will be encoded in _pg_pkt + */ +void encodeServo_ConfigPacketStructure(void* _pg_pkt, const Servo_Config_t* _pg_user) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + unsigned _pg_i = 0; + + // Servo configuration parameters + encodeServo_ConfigBits_t(_pg_data, &_pg_byteindex, &_pg_user->options); + + // Servo command timeout + // Range of commandTimeout is 0 to 65535. + uint16ToBeBytes(_pg_user->commandTimeout, _pg_data, &_pg_byteindex); + + // Servo neutral position. Servo can be configured to return to this position at powerup, or after loss of communication + // Range of homePosition is -32768 to 32767. + int16ToBeBytes(_pg_user->homePosition, _pg_data, &_pg_byteindex); + + // Reserved for future use + // Range of reserved is 0 to 255. + for(_pg_i = 0; _pg_i < 2; _pg_i++) + uint8ToBytes(_pg_user->reserved[_pg_i], _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_ConfigPacketID()); + +}// encodeServo_ConfigPacketStructure + +/*! + * \brief Decode the Servo_Config packet + * + * General servo configuration settings + * \param _pg_pkt points to the packet being decoded by this function + * \param _pg_user receives the data decoded from the packet + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_ConfigPacketStructure(const void* _pg_pkt, Servo_Config_t* _pg_user) +{ + int _pg_numbytes; + int _pg_byteindex = 0; + const uint8_t* _pg_data; + unsigned _pg_i = 0; + + // Verify the packet identifier + if(getServoPacketID(_pg_pkt) != getServo_ConfigPacketID()) + return 0; + + // Verify the packet size + _pg_numbytes = getServoPacketSize(_pg_pkt); + if(_pg_numbytes < getServo_ConfigMinDataLength()) + return 0; + + // The raw data from the packet + _pg_data = getServoPacketDataConst(_pg_pkt); + + // Servo configuration parameters + if(decodeServo_ConfigBits_t(_pg_data, &_pg_byteindex, &_pg_user->options) == 0) + return 0; + + // Servo command timeout + // Range of commandTimeout is 0 to 65535. + _pg_user->commandTimeout = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Servo neutral position. Servo can be configured to return to this position at powerup, or after loss of communication + // Range of homePosition is -32768 to 32767. + _pg_user->homePosition = int16FromBeBytes(_pg_data, &_pg_byteindex); + + // Reserved for future use + // Range of reserved is 0 to 255. + for(_pg_i = 0; _pg_i < 2; _pg_i++) + _pg_user->reserved[_pg_i] = uint8FromBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_ConfigPacketStructure + +/*! + * \brief Create the Servo_Config packet + * + * General servo configuration settings + * \param _pg_pkt points to the packet which will be created by this function + * \param options is Servo configuration parameters + * \param commandTimeout is Servo command timeout + * \param homePosition is Servo neutral position. Servo can be configured to return to this position at powerup, or after loss of communication + * \param reserved is Reserved for future use + */ +void encodeServo_ConfigPacket(void* _pg_pkt, const Servo_ConfigBits_t* options, uint16_t commandTimeout, int16_t homePosition, const uint8_t reserved[2]) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + unsigned _pg_i = 0; + + // Servo configuration parameters + encodeServo_ConfigBits_t(_pg_data, &_pg_byteindex, options); + + // Servo command timeout + // Range of commandTimeout is 0 to 65535. + uint16ToBeBytes(commandTimeout, _pg_data, &_pg_byteindex); + + // Servo neutral position. Servo can be configured to return to this position at powerup, or after loss of communication + // Range of homePosition is -32768 to 32767. + int16ToBeBytes(homePosition, _pg_data, &_pg_byteindex); + + // Reserved for future use + // Range of reserved is 0 to 255. + for(_pg_i = 0; _pg_i < 2; _pg_i++) + uint8ToBytes(reserved[_pg_i], _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_ConfigPacketID()); + +}// encodeServo_ConfigPacket + +/*! + * \brief Decode the Servo_Config packet + * + * General servo configuration settings + * \param _pg_pkt points to the packet being decoded by this function + * \param options receives Servo configuration parameters + * \param commandTimeout receives Servo command timeout + * \param homePosition receives Servo neutral position. Servo can be configured to return to this position at powerup, or after loss of communication + * \param reserved receives Reserved for future use + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_ConfigPacket(const void* _pg_pkt, Servo_ConfigBits_t* options, uint16_t* commandTimeout, int16_t* homePosition, uint8_t reserved[2]) +{ + unsigned _pg_i = 0; + 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_ConfigPacketID()) + return 0; + + if(_pg_numbytes < getServo_ConfigMinDataLength()) + return 0; + + // Servo configuration parameters + if(decodeServo_ConfigBits_t(_pg_data, &_pg_byteindex, options) == 0) + return 0; + + // Servo command timeout + // Range of commandTimeout is 0 to 65535. + (*commandTimeout) = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Servo neutral position. Servo can be configured to return to this position at powerup, or after loss of communication + // Range of homePosition is -32768 to 32767. + (*homePosition) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + // Reserved for future use + // Range of reserved is 0 to 255. + for(_pg_i = 0; _pg_i < 2; _pg_i++) + reserved[_pg_i] = uint8FromBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_ConfigPacket + +/*! + * \brief Encode a Servo_Config_t into a byte array + * + * General servo configuration settings + * \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_Config_t(uint8_t* _pg_data, int* _pg_bytecount, const Servo_Config_t* _pg_user) +{ + int _pg_byteindex = *_pg_bytecount; + unsigned _pg_i = 0; + + // Servo configuration parameters + encodeServo_ConfigBits_t(_pg_data, &_pg_byteindex, &_pg_user->options); + + // Servo command timeout + // Range of commandTimeout is 0 to 65535. + uint16ToBeBytes(_pg_user->commandTimeout, _pg_data, &_pg_byteindex); + + // Servo neutral position. Servo can be configured to return to this position at powerup, or after loss of communication + // Range of homePosition is -32768 to 32767. + int16ToBeBytes(_pg_user->homePosition, _pg_data, &_pg_byteindex); + + // Reserved for future use + // Range of reserved is 0 to 255. + for(_pg_i = 0; _pg_i < 2; _pg_i++) + uint8ToBytes(_pg_user->reserved[_pg_i], _pg_data, &_pg_byteindex); + + *_pg_bytecount = _pg_byteindex; + +}// encodeServo_Config_t + +/*! + * \brief Decode a Servo_Config_t from a byte array + * + * General servo configuration settings + * \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_Config_t(const uint8_t* _pg_data, int* _pg_bytecount, Servo_Config_t* _pg_user) +{ + int _pg_byteindex = *_pg_bytecount; + unsigned _pg_i = 0; + + // Servo configuration parameters + if(decodeServo_ConfigBits_t(_pg_data, &_pg_byteindex, &_pg_user->options) == 0) + return 0; + + // Servo command timeout + // Range of commandTimeout is 0 to 65535. + _pg_user->commandTimeout = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Servo neutral position. Servo can be configured to return to this position at powerup, or after loss of communication + // Range of homePosition is -32768 to 32767. + _pg_user->homePosition = int16FromBeBytes(_pg_data, &_pg_byteindex); + + // Reserved for future use + // Range of reserved is 0 to 255. + for(_pg_i = 0; _pg_i < 2; _pg_i++) + _pg_user->reserved[_pg_i] = uint8FromBytes(_pg_data, &_pg_byteindex); + + *_pg_bytecount = _pg_byteindex; + + return 1; + +}// decodeServo_Config_t + +/*! + * \brief Create the Servo_MultiPositionCommand packet + * + * 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. + * \param _pg_pkt points to the packet which will be created by this function + * \param commandA is Servo command for servo with address offset 0 + * \param commandB is Servo command for servo with address offset 1 + * \param commandC is Servo command for servo with address offset 3 + * \param commandD is Servo command for servo with address offset 3 + */ +void encodeServo_MultiPositionCommandPacket(void* _pg_pkt, int16_t commandA, int16_t commandB, int16_t commandC, int16_t commandD) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Servo command for servo with address offset 0 + // Range of commandA is -32768 to 32767. + int16ToBeBytes(commandA, _pg_data, &_pg_byteindex); + + // Servo command for servo with address offset 1 + // Range of commandB is -32768 to 32767. + int16ToBeBytes(commandB, _pg_data, &_pg_byteindex); + + // Servo command for servo with address offset 3 + // Range of commandC is -32768 to 32767. + int16ToBeBytes(commandC, _pg_data, &_pg_byteindex); + + // Servo command for servo with address offset 3 + // Range of commandD is -32768 to 32767. + int16ToBeBytes(commandD, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_MultiPositionCommandPacketID()); + +}// encodeServo_MultiPositionCommandPacket + +/*! + * \brief Decode the Servo_MultiPositionCommand packet + * + * 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. + * \param _pg_pkt points to the packet being decoded by this function + * \param commandA receives Servo command for servo with address offset 0 + * \param commandB receives Servo command for servo with address offset 1 + * \param commandC receives Servo command for servo with address offset 3 + * \param commandD receives Servo command for servo with address offset 3 + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_MultiPositionCommandPacket(const void* _pg_pkt, int16_t* commandA, int16_t* commandB, int16_t* commandC, int16_t* commandD) +{ + 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_MultiPositionCommandPacketID()) + return 0; + + if(_pg_numbytes < getServo_MultiPositionCommandMinDataLength()) + return 0; + + // Servo command for servo with address offset 0 + // Range of commandA is -32768 to 32767. + (*commandA) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + // Servo command for servo with address offset 1 + // Range of commandB is -32768 to 32767. + (*commandB) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + // Servo command for servo with address offset 3 + // Range of commandC is -32768 to 32767. + (*commandC) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + // Servo command for servo with address offset 3 + // Range of commandD is -32768 to 32767. + (*commandD) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_MultiPositionCommandPacket + +/*! + * \brief Create the Servo_PositionCommand packet + * + * 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. + * \param _pg_pkt points to the packet which will be created by this function + * \param command is Servo command + */ +void encodeServo_PositionCommandPacket(void* _pg_pkt, int16_t command) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Servo command + // Range of command is -32768 to 32767. + int16ToBeBytes(command, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_PositionCommandPacketID()); + +}// encodeServo_PositionCommandPacket + +/*! + * \brief Decode the Servo_PositionCommand packet + * + * 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. + * \param _pg_pkt points to the packet being decoded by this function + * \param command receives Servo command + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_PositionCommandPacket(const void* _pg_pkt, int16_t* command) +{ + 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_PositionCommandPacketID()) + return 0; + + if(_pg_numbytes < getServo_PositionCommandMinDataLength()) + return 0; + + // Servo command + // Range of command is -32768 to 32767. + (*command) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_PositionCommandPacket + +/*! + * \brief Create the Servo_NeutralPositionCommand packet + * + * 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 + * \param _pg_pkt points to the packet which will be created by this function + */ +void encodeServo_NeutralPositionCommandPacket(void* _pg_pkt) +{ + + // Zero length packet, no data encoded + finishServoPacket(_pg_pkt, 0, getServo_NeutralPositionCommandPacketID()); + +}// encodeServo_NeutralPositionCommandPacket + +/*! + * \brief Decode the Servo_NeutralPositionCommand packet + * + * 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 + * \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_NeutralPositionCommandPacket(const void* _pg_pkt) +{ + // Verify the packet identifier + if(getServoPacketID(_pg_pkt) != getServo_NeutralPositionCommandPacketID()) + return 0; + else + return 1; + +}// decodeServo_NeutralPositionCommandPacket + +/*! + * \brief Create the Servo_Disable packet + * + * Send this command (with zero data bytes) to disable the servo. Send with the + * broadcast ID (0xFF) to disable *all* servos. + * \param _pg_pkt points to the packet which will be created by this function + */ +void encodeServo_DisablePacket(void* _pg_pkt) +{ + + // Zero length packet, no data encoded + finishServoPacket(_pg_pkt, 0, getServo_DisablePacketID()); + +}// encodeServo_DisablePacket + +/*! + * \brief Decode the Servo_Disable packet + * + * Send this command (with zero data bytes) to disable the servo. Send with the + * broadcast ID (0xFF) to disable *all* servos. + * \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_DisablePacket(const void* _pg_pkt) +{ + // Verify the packet identifier + if(getServoPacketID(_pg_pkt) != getServo_DisablePacketID()) + return 0; + else + return 1; + +}// decodeServo_DisablePacket + +/*! + * \brief Create the Servo_Enable packet + * + * Send this command (with zero data bytes) to enable the servo. Send with the + * broadcast ID (0xFF) to enable *all* servos. + * \param _pg_pkt points to the packet which will be created by this function + */ +void encodeServo_EnablePacket(void* _pg_pkt) +{ + + // Zero length packet, no data encoded + finishServoPacket(_pg_pkt, 0, getServo_EnablePacketID()); + +}// encodeServo_EnablePacket + +/*! + * \brief Decode the Servo_Enable packet + * + * Send this command (with zero data bytes) to enable the servo. Send with the + * broadcast ID (0xFF) to enable *all* servos. + * \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_EnablePacket(const void* _pg_pkt) +{ + // Verify the packet identifier + if(getServoPacketID(_pg_pkt) != getServo_EnablePacketID()) + return 0; + else + return 1; + +}// decodeServo_EnablePacket + +/*! + * \brief Create the Servo_SetTitle packet + * + * Set the human-readable description of this servo + * \param _pg_pkt points to the packet which will be created by this function + * \param title is + */ +void encodeServo_SetTitlePacket(void* _pg_pkt, const uint8_t title[8]) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + unsigned _pg_i = 0; + + // Range of title is 0 to 255. + for(_pg_i = 0; _pg_i < 8; _pg_i++) + uint8ToBytes(title[_pg_i], _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_SetTitlePacketID()); + +}// encodeServo_SetTitlePacket + +/*! + * \brief Decode the Servo_SetTitle packet + * + * Set the human-readable description of this servo + * \param _pg_pkt points to the packet being decoded by this function + * \param title receives + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_SetTitlePacket(const void* _pg_pkt, uint8_t title[8]) +{ + unsigned _pg_i = 0; + 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_SetTitlePacketID()) + return 0; + + if(_pg_numbytes < getServo_SetTitleMinDataLength()) + return 0; + + // Range of title is 0 to 255. + for(_pg_i = 0; _pg_i < 8; _pg_i++) + title[_pg_i] = uint8FromBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_SetTitlePacket + +/*! + * \brief Create the Servo_StatusA packet + * + * The *SERVO_STATUS_A* packet contains status, warning and error information, + * in addition to the servo position + * \param _pg_pkt points to the packet which will be created by this function + * \param status is Status bits contain information on servo operation + * \param warnings is Warning bits indicate servo is operation outside of desired range + * \param errors is These bits indicate critical system error information + * \param position is Servo position, mapped to input units + * \param command is Servo commanded position + */ +void encodeServo_StatusAPacket(void* _pg_pkt, const Servo_StatusBits_t* status, const Servo_WarningBits_t* warnings, const Servo_ErrorBits_t* errors, int16_t position, int16_t command) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Status bits contain information on servo operation + encodeServo_StatusBits_t(_pg_data, &_pg_byteindex, status); + + // Warning bits indicate servo is operation outside of desired range + encodeServo_WarningBits_t(_pg_data, &_pg_byteindex, warnings); + + // These bits indicate critical system error information + encodeServo_ErrorBits_t(_pg_data, &_pg_byteindex, errors); + + // Servo position, mapped to input units + // Range of position is -32768 to 32767. + int16ToBeBytes(position, _pg_data, &_pg_byteindex); + + // Servo commanded position + // Range of command is -32768 to 32767. + int16ToBeBytes(command, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_StatusAPacketID()); + +}// encodeServo_StatusAPacket + +/*! + * \brief Decode the Servo_StatusA packet + * + * The *SERVO_STATUS_A* packet contains status, warning and error information, + * in addition to the servo position + * \param _pg_pkt points to the packet being decoded by this function + * \param status receives Status bits contain information on servo operation + * \param warnings receives Warning bits indicate servo is operation outside of desired range + * \param errors receives These bits indicate critical system error information + * \param position receives Servo position, mapped to input units + * \param command receives Servo commanded position + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_StatusAPacket(const void* _pg_pkt, Servo_StatusBits_t* status, Servo_WarningBits_t* warnings, Servo_ErrorBits_t* errors, int16_t* position, int16_t* command) +{ + 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_StatusAPacketID()) + return 0; + + if(_pg_numbytes < getServo_StatusAMinDataLength()) + return 0; + + // Status bits contain information on servo operation + if(decodeServo_StatusBits_t(_pg_data, &_pg_byteindex, status) == 0) + return 0; + + // Warning bits indicate servo is operation outside of desired range + if(decodeServo_WarningBits_t(_pg_data, &_pg_byteindex, warnings) == 0) + return 0; + + // These bits indicate critical system error information + if(decodeServo_ErrorBits_t(_pg_data, &_pg_byteindex, errors) == 0) + return 0; + + // Servo position, mapped to input units + // Range of position is -32768 to 32767. + (*position) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + // Servo commanded position + // Range of command is -32768 to 32767. + (*command) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_StatusAPacket + +/*! + * \brief Create the Servo_StatusB packet + * + * The *SERVO_STATUS_B* packet contains various servo feedback data + * \param _pg_pkt points to the packet which will be created by this function + * \param current is Servo current + * \param voltage is Servo supply voltage + * \param temperature is Servo temperature + */ +void encodeServo_StatusBPacket(void* _pg_pkt, uint16_t current, uint16_t voltage, int8_t temperature) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Servo current + // Range of current is 0 to 65535. + uint16ToBeBytes(current, _pg_data, &_pg_byteindex); + + // Servo supply voltage + // Range of voltage is 0 to 65535. + uint16ToBeBytes(voltage, _pg_data, &_pg_byteindex); + + // Servo temperature + // Range of temperature is -128 to 127. + int8ToBytes(temperature, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_StatusBPacketID()); + +}// encodeServo_StatusBPacket + +/*! + * \brief Decode the Servo_StatusB packet + * + * The *SERVO_STATUS_B* packet contains various servo feedback data + * \param _pg_pkt points to the packet being decoded by this function + * \param current receives Servo current + * \param voltage receives Servo supply voltage + * \param temperature receives Servo temperature + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_StatusBPacket(const void* _pg_pkt, uint16_t* current, uint16_t* voltage, int8_t* temperature) +{ + 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_StatusBPacketID()) + return 0; + + if(_pg_numbytes < getServo_StatusBMinDataLength()) + return 0; + + // Servo current + // Range of current is 0 to 65535. + (*current) = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Servo supply voltage + // Range of voltage is 0 to 65535. + (*voltage) = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Servo temperature + // Range of temperature is -128 to 127. + (*temperature) = int8FromBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_StatusBPacket + +/*! + * \brief Create the Servo_StatusC packet + * + * The *SERVO_STATUS_C* packet contains servo position data. It is a cut-down + * packet to allow high-speed feedback on servo position + * \param _pg_pkt points to the packet which will be created by this function + * \param position is Servo position, mapped to input units + */ +void encodeServo_StatusCPacket(void* _pg_pkt, int16_t position) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Servo position, mapped to input units + // Range of position is -32768 to 32767. + int16ToBeBytes(position, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_StatusCPacketID()); + +}// encodeServo_StatusCPacket + +/*! + * \brief Decode the Servo_StatusC packet + * + * The *SERVO_STATUS_C* packet contains servo position data. It is a cut-down + * packet to allow high-speed feedback on servo position + * \param _pg_pkt points to the packet being decoded by this function + * \param position receives Servo position, mapped to input units + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_StatusCPacket(const void* _pg_pkt, int16_t* position) +{ + 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_StatusCPacketID()) + return 0; + + if(_pg_numbytes < getServo_StatusCMinDataLength()) + return 0; + + // Servo position, mapped to input units + // Range of position is -32768 to 32767. + (*position) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_StatusCPacket + +/*! + * \brief Create the Servo_Accelerometer packet + * + * Raw accelerometer data. To convert these raw readings to 'real' units, use + * the formula acc = 0.5 * raw * fullscale / (2^resolution) + * \param _pg_pkt points to the packet which will be created by this function + * \param xAcc is X axis acceleration value + * \param yAcc is Y axis acceleration value + * \param zAcc is Z axis acceleration value + * \param fullscale is Accelerometer full-scale range + * \param resolution is Accelerometer measurement resolution, in 'bits'. + */ +void encodeServo_AccelerometerPacket(void* _pg_pkt, int16_t xAcc, int16_t yAcc, int16_t zAcc, uint8_t fullscale, uint8_t resolution) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // X axis acceleration value + // Range of xAcc is -32768 to 32767. + int16ToBeBytes(xAcc, _pg_data, &_pg_byteindex); + + // Y axis acceleration value + // Range of yAcc is -32768 to 32767. + int16ToBeBytes(yAcc, _pg_data, &_pg_byteindex); + + // Z axis acceleration value + // Range of zAcc is -32768 to 32767. + int16ToBeBytes(zAcc, _pg_data, &_pg_byteindex); + + // Accelerometer full-scale range + // Range of fullscale is 0 to 255. + uint8ToBytes(fullscale, _pg_data, &_pg_byteindex); + + // Accelerometer measurement resolution, in 'bits'. + // Range of resolution is 0 to 255. + uint8ToBytes(resolution, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_AccelerometerPacketID()); + +}// encodeServo_AccelerometerPacket + +/*! + * \brief Decode the Servo_Accelerometer packet + * + * Raw accelerometer data. To convert these raw readings to 'real' units, use + * the formula acc = 0.5 * raw * fullscale / (2^resolution) + * \param _pg_pkt points to the packet being decoded by this function + * \param xAcc receives X axis acceleration value + * \param yAcc receives Y axis acceleration value + * \param zAcc receives Z axis acceleration value + * \param fullscale receives Accelerometer full-scale range + * \param resolution receives Accelerometer measurement resolution, in 'bits'. + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_AccelerometerPacket(const void* _pg_pkt, int16_t* xAcc, int16_t* yAcc, int16_t* zAcc, uint8_t* fullscale, uint8_t* resolution) +{ + 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_AccelerometerPacketID()) + return 0; + + if(_pg_numbytes < getServo_AccelerometerMinDataLength()) + return 0; + + // X axis acceleration value + // Range of xAcc is -32768 to 32767. + (*xAcc) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + // Y axis acceleration value + // Range of yAcc is -32768 to 32767. + (*yAcc) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + // Z axis acceleration value + // Range of zAcc is -32768 to 32767. + (*zAcc) = int16FromBeBytes(_pg_data, &_pg_byteindex); + + // Accelerometer full-scale range + // Range of fullscale is 0 to 255. + (*fullscale) = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Accelerometer measurement resolution, in 'bits'. + // Range of resolution is 0 to 255. + (*resolution) = uint8FromBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_AccelerometerPacket + +/*! + * \brief Create the Servo_Address packet + * + * Servo address information + * \param _pg_pkt points to the packet which will be created by this function + * \param _pg_user points to the user data that will be encoded in _pg_pkt + */ +void encodeServo_AddressPacketStructure(void* _pg_pkt, const Servo_Address_t* _pg_user) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Hardware revision + // Range of hwRev is 0 to 255. + uint8ToBytes(_pg_user->hwRev, _pg_data, &_pg_byteindex); + + // Servo serial number + // Range of serialNumber is 0 to 16777215. + uint24ToBeBytes((uint32_t)(limitMax(_pg_user->serialNumber, 16777215)), _pg_data, &_pg_byteindex); + + // Programmable User ID value 1/2 + // Range of userIDA is 0 to 65535. + uint16ToBeBytes(_pg_user->userIDA, _pg_data, &_pg_byteindex); + + // Programmable User ID value 2/2 + // Range of userIDB is 0 to 65535. + uint16ToBeBytes(_pg_user->userIDB, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_AddressPacketID()); + +}// encodeServo_AddressPacketStructure + +/*! + * \brief Decode the Servo_Address packet + * + * Servo address information + * \param _pg_pkt points to the packet being decoded by this function + * \param _pg_user receives the data decoded from the packet + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_AddressPacketStructure(const void* _pg_pkt, Servo_Address_t* _pg_user) +{ + int _pg_numbytes; + int _pg_byteindex = 0; + const uint8_t* _pg_data; + + // Verify the packet identifier + if(getServoPacketID(_pg_pkt) != getServo_AddressPacketID()) + return 0; + + // Verify the packet size + _pg_numbytes = getServoPacketSize(_pg_pkt); + if(_pg_numbytes < getServo_AddressMinDataLength()) + return 0; + + // The raw data from the packet + _pg_data = getServoPacketDataConst(_pg_pkt); + + // Hardware revision + // Range of hwRev is 0 to 255. + _pg_user->hwRev = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Servo serial number + // Range of serialNumber is 0 to 16777215. + _pg_user->serialNumber = (uint32_t)uint24FromBeBytes(_pg_data, &_pg_byteindex); + + // Programmable User ID value 1/2 + // Range of userIDA is 0 to 65535. + _pg_user->userIDA = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Programmable User ID value 2/2 + // Range of userIDB is 0 to 65535. + _pg_user->userIDB = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_AddressPacketStructure + +/*! + * \brief Create the Servo_Address packet + * + * Servo address information + * \param _pg_pkt points to the packet which will be created by this function + * \param hwRev is Hardware revision + * \param serialNumber is Servo serial number + * \param userIDA is Programmable User ID value 1/2 + * \param userIDB is Programmable User ID value 2/2 + */ +void encodeServo_AddressPacket(void* _pg_pkt, uint8_t hwRev, uint32_t serialNumber, uint16_t userIDA, uint16_t userIDB) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Hardware revision + // Range of hwRev is 0 to 255. + uint8ToBytes(hwRev, _pg_data, &_pg_byteindex); + + // Servo serial number + // Range of serialNumber is 0 to 16777215. + uint24ToBeBytes((uint32_t)(limitMax(serialNumber, 16777215)), _pg_data, &_pg_byteindex); + + // Programmable User ID value 1/2 + // Range of userIDA is 0 to 65535. + uint16ToBeBytes(userIDA, _pg_data, &_pg_byteindex); + + // Programmable User ID value 2/2 + // Range of userIDB is 0 to 65535. + uint16ToBeBytes(userIDB, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_AddressPacketID()); + +}// encodeServo_AddressPacket + +/*! + * \brief Decode the Servo_Address packet + * + * Servo address information + * \param _pg_pkt points to the packet being decoded by this function + * \param hwRev receives Hardware revision + * \param serialNumber receives Servo serial number + * \param userIDA receives Programmable User ID value 1/2 + * \param userIDB receives Programmable User ID value 2/2 + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_AddressPacket(const void* _pg_pkt, uint8_t* hwRev, uint32_t* serialNumber, uint16_t* userIDA, uint16_t* userIDB) +{ + 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_AddressPacketID()) + return 0; + + if(_pg_numbytes < getServo_AddressMinDataLength()) + return 0; + + // Hardware revision + // Range of hwRev is 0 to 255. + (*hwRev) = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Servo serial number + // Range of serialNumber is 0 to 16777215. + (*serialNumber) = (uint32_t)uint24FromBeBytes(_pg_data, &_pg_byteindex); + + // Programmable User ID value 1/2 + // Range of userIDA is 0 to 65535. + (*userIDA) = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Programmable User ID value 2/2 + // Range of userIDB is 0 to 65535. + (*userIDB) = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_AddressPacket + +/*! + * \brief Create the Servo_Title packet + * + * Servo title information + * \param _pg_pkt points to the packet which will be created by this function + * \param title is Human readable description string for the servo + */ +void encodeServo_TitlePacket(void* _pg_pkt, const uint8_t title[8]) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + unsigned _pg_i = 0; + + // Human readable description string for the servo + // Range of title is 0 to 255. + for(_pg_i = 0; _pg_i < 8; _pg_i++) + uint8ToBytes(title[_pg_i], _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_TitlePacketID()); + +}// encodeServo_TitlePacket + +/*! + * \brief Decode the Servo_Title packet + * + * Servo title information + * \param _pg_pkt points to the packet being decoded by this function + * \param title receives Human readable description string for the servo + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_TitlePacket(const void* _pg_pkt, uint8_t title[8]) +{ + unsigned _pg_i = 0; + 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_TitlePacketID()) + return 0; + + if(_pg_numbytes < getServo_TitleMinDataLength()) + return 0; + + // Human readable description string for the servo + // Range of title is 0 to 255. + for(_pg_i = 0; _pg_i < 8; _pg_i++) + title[_pg_i] = uint8FromBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_TitlePacket + +/*! + * \brief Create the Servo_Firmware packet + * + * Servo firmware information + * \param _pg_pkt points to the packet which will be created by this function + * \param _pg_user points to the user data that will be encoded in _pg_pkt + */ +void encodeServo_FirmwarePacketStructure(void* _pg_pkt, const Servo_Firmware_t* _pg_user) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Firmware version, major number + // Range of major is 0 to 255. + uint8ToBytes(_pg_user->major, _pg_data, &_pg_byteindex); + + // Firmware version, minor number + // Range of minor is 0 to 255. + uint8ToBytes(_pg_user->minor, _pg_data, &_pg_byteindex); + + // Firmware release date, day-of-month + // Range of day is 0 to 255. + uint8ToBytes(_pg_user->day, _pg_data, &_pg_byteindex); + + // Firmware release date, month-of-year + // Range of month is 0 to 255. + uint8ToBytes(_pg_user->month, _pg_data, &_pg_byteindex); + + // Firmware release date, year + // Range of year is 0 to 65535. + uint16ToBeBytes(_pg_user->year, _pg_data, &_pg_byteindex); + + // Firmware checksum, 16-bit + // Range of checksum is 0 to 65535. + uint16ToBeBytes(_pg_user->checksum, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_FirmwarePacketID()); + +}// encodeServo_FirmwarePacketStructure + +/*! + * \brief Decode the Servo_Firmware packet + * + * Servo firmware information + * \param _pg_pkt points to the packet being decoded by this function + * \param _pg_user receives the data decoded from the packet + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_FirmwarePacketStructure(const void* _pg_pkt, Servo_Firmware_t* _pg_user) +{ + int _pg_numbytes; + int _pg_byteindex = 0; + const uint8_t* _pg_data; + + // Verify the packet identifier + if(getServoPacketID(_pg_pkt) != getServo_FirmwarePacketID()) + return 0; + + // Verify the packet size + _pg_numbytes = getServoPacketSize(_pg_pkt); + if(_pg_numbytes < getServo_FirmwareMinDataLength()) + return 0; + + // The raw data from the packet + _pg_data = getServoPacketDataConst(_pg_pkt); + + // Firmware version, major number + // Range of major is 0 to 255. + _pg_user->major = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Firmware version, minor number + // Range of minor is 0 to 255. + _pg_user->minor = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Firmware release date, day-of-month + // Range of day is 0 to 255. + _pg_user->day = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Firmware release date, month-of-year + // Range of month is 0 to 255. + _pg_user->month = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Firmware release date, year + // Range of year is 0 to 65535. + _pg_user->year = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Firmware checksum, 16-bit + // Range of checksum is 0 to 65535. + _pg_user->checksum = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_FirmwarePacketStructure + +/*! + * \brief Create the Servo_Firmware packet + * + * Servo firmware information + * \param _pg_pkt points to the packet which will be created by this function + * \param major is Firmware version, major number + * \param minor is Firmware version, minor number + * \param day is Firmware release date, day-of-month + * \param month is Firmware release date, month-of-year + * \param year is Firmware release date, year + * \param checksum is Firmware checksum, 16-bit + */ +void encodeServo_FirmwarePacket(void* _pg_pkt, uint8_t major, uint8_t minor, uint8_t day, uint8_t month, uint16_t year, uint16_t checksum) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Firmware version, major number + // Range of major is 0 to 255. + uint8ToBytes(major, _pg_data, &_pg_byteindex); + + // Firmware version, minor number + // Range of minor is 0 to 255. + uint8ToBytes(minor, _pg_data, &_pg_byteindex); + + // Firmware release date, day-of-month + // Range of day is 0 to 255. + uint8ToBytes(day, _pg_data, &_pg_byteindex); + + // Firmware release date, month-of-year + // Range of month is 0 to 255. + uint8ToBytes(month, _pg_data, &_pg_byteindex); + + // Firmware release date, year + // Range of year is 0 to 65535. + uint16ToBeBytes(year, _pg_data, &_pg_byteindex); + + // Firmware checksum, 16-bit + // Range of checksum is 0 to 65535. + uint16ToBeBytes(checksum, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_FirmwarePacketID()); + +}// encodeServo_FirmwarePacket + +/*! + * \brief Decode the Servo_Firmware packet + * + * Servo firmware information + * \param _pg_pkt points to the packet being decoded by this function + * \param major receives Firmware version, major number + * \param minor receives Firmware version, minor number + * \param day receives Firmware release date, day-of-month + * \param month receives Firmware release date, month-of-year + * \param year receives Firmware release date, year + * \param checksum receives Firmware checksum, 16-bit + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_FirmwarePacket(const void* _pg_pkt, uint8_t* major, uint8_t* minor, uint8_t* day, uint8_t* month, uint16_t* year, uint16_t* checksum) +{ + 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_FirmwarePacketID()) + return 0; + + if(_pg_numbytes < getServo_FirmwareMinDataLength()) + return 0; + + // Firmware version, major number + // Range of major is 0 to 255. + (*major) = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Firmware version, minor number + // Range of minor is 0 to 255. + (*minor) = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Firmware release date, day-of-month + // Range of day is 0 to 255. + (*day) = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Firmware release date, month-of-year + // Range of month is 0 to 255. + (*month) = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Firmware release date, year + // Range of year is 0 to 65535. + (*year) = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Firmware checksum, 16-bit + // Range of checksum is 0 to 65535. + (*checksum) = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_FirmwarePacket + +/*! + * \brief Create the Servo_SystemInfo packet + * + * Servo system info (uptime, etc) + * \param _pg_pkt points to the packet which will be created by this function + * \param _pg_user points to the user data that will be encoded in _pg_pkt + */ +void encodeServo_SystemInfoPacketStructure(void* _pg_pkt, const Servo_SystemInfo_t* _pg_user) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Time since last power cycle (milliseconds) + // Range of msSinceReset is 0 to 4294967295. + uint32ToBeBytes(_pg_user->msSinceReset, _pg_data, &_pg_byteindex); + + // Number of recorded power cycles + // Range of powerCycles is 0 to 65535. + uint16ToBeBytes(_pg_user->powerCycles, _pg_data, &_pg_byteindex); + + // Processor code indicating cause of most recent reset event + // Range of resetCode is 0 to 255. + uint8ToBytes(_pg_user->resetCode, _pg_data, &_pg_byteindex); + + // Range of cpuOccupancy is 0 to 255. + uint8ToBytes(_pg_user->cpuOccupancy, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_SystemInfoPacketID()); + +}// encodeServo_SystemInfoPacketStructure + +/*! + * \brief Decode the Servo_SystemInfo packet + * + * Servo system info (uptime, etc) + * \param _pg_pkt points to the packet being decoded by this function + * \param _pg_user receives the data decoded from the packet + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_SystemInfoPacketStructure(const void* _pg_pkt, Servo_SystemInfo_t* _pg_user) +{ + int _pg_numbytes; + int _pg_byteindex = 0; + const uint8_t* _pg_data; + + // Verify the packet identifier + if(getServoPacketID(_pg_pkt) != getServo_SystemInfoPacketID()) + return 0; + + // Verify the packet size + _pg_numbytes = getServoPacketSize(_pg_pkt); + if(_pg_numbytes < getServo_SystemInfoMinDataLength()) + return 0; + + // The raw data from the packet + _pg_data = getServoPacketDataConst(_pg_pkt); + + // Time since last power cycle (milliseconds) + // Range of msSinceReset is 0 to 4294967295. + _pg_user->msSinceReset = uint32FromBeBytes(_pg_data, &_pg_byteindex); + + // Number of recorded power cycles + // Range of powerCycles is 0 to 65535. + _pg_user->powerCycles = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Processor code indicating cause of most recent reset event + // Range of resetCode is 0 to 255. + _pg_user->resetCode = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Range of cpuOccupancy is 0 to 255. + _pg_user->cpuOccupancy = uint8FromBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_SystemInfoPacketStructure + +/*! + * \brief Create the Servo_SystemInfo packet + * + * Servo system info (uptime, etc) + * \param _pg_pkt points to the packet which will be created by this function + * \param msSinceReset is Time since last power cycle (milliseconds) + * \param powerCycles is Number of recorded power cycles + * \param resetCode is Processor code indicating cause of most recent reset event + * \param cpuOccupancy is + */ +void encodeServo_SystemInfoPacket(void* _pg_pkt, uint32_t msSinceReset, uint16_t powerCycles, uint8_t resetCode, uint8_t cpuOccupancy) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Time since last power cycle (milliseconds) + // Range of msSinceReset is 0 to 4294967295. + uint32ToBeBytes(msSinceReset, _pg_data, &_pg_byteindex); + + // Number of recorded power cycles + // Range of powerCycles is 0 to 65535. + uint16ToBeBytes(powerCycles, _pg_data, &_pg_byteindex); + + // Processor code indicating cause of most recent reset event + // Range of resetCode is 0 to 255. + uint8ToBytes(resetCode, _pg_data, &_pg_byteindex); + + // Range of cpuOccupancy is 0 to 255. + uint8ToBytes(cpuOccupancy, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_SystemInfoPacketID()); + +}// encodeServo_SystemInfoPacket + +/*! + * \brief Decode the Servo_SystemInfo packet + * + * Servo system info (uptime, etc) + * \param _pg_pkt points to the packet being decoded by this function + * \param msSinceReset receives Time since last power cycle (milliseconds) + * \param powerCycles receives Number of recorded power cycles + * \param resetCode receives Processor code indicating cause of most recent reset event + * \param cpuOccupancy receives + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_SystemInfoPacket(const void* _pg_pkt, uint32_t* msSinceReset, uint16_t* powerCycles, uint8_t* resetCode, uint8_t* cpuOccupancy) +{ + 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_SystemInfoPacketID()) + return 0; + + if(_pg_numbytes < getServo_SystemInfoMinDataLength()) + return 0; + + // Time since last power cycle (milliseconds) + // Range of msSinceReset is 0 to 4294967295. + (*msSinceReset) = uint32FromBeBytes(_pg_data, &_pg_byteindex); + + // Number of recorded power cycles + // Range of powerCycles is 0 to 65535. + (*powerCycles) = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Processor code indicating cause of most recent reset event + // Range of resetCode is 0 to 255. + (*resetCode) = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Range of cpuOccupancy is 0 to 255. + (*cpuOccupancy) = uint8FromBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_SystemInfoPacket + +/*! + * \brief Create the Servo_TelemetryConfig packet + * + * Telemetry settings configuration packet + * \param _pg_pkt points to the packet which will be created by this function + * \param settings is Servo telemetry settings + */ +void encodeServo_TelemetryConfigPacket(void* _pg_pkt, const Servo_TelemetrySettings_t* settings) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + unsigned _pg_i = 0; + + // Servo telemetry settings + encodeServo_TelemetrySettings_t(_pg_data, &_pg_byteindex, settings); + + // Reserved for future use + for(_pg_i = 0; _pg_i < 3; _pg_i++) + uint8ToBytes((uint8_t)(0), _pg_data, &_pg_byteindex); + + // Servo ICD revision + uint8ToBytes((uint8_t)(getServoApi()), _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_TelemetryConfigPacketID()); + +}// encodeServo_TelemetryConfigPacket + +/*! + * \brief Decode the Servo_TelemetryConfig packet + * + * Telemetry settings configuration packet + * \param _pg_pkt points to the packet being decoded by this function + * \param settings receives Servo telemetry settings + * \param icdVersion receives Servo ICD revision + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_TelemetryConfigPacket(const void* _pg_pkt, Servo_TelemetrySettings_t* settings, uint8_t* icdVersion) +{ + 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_TelemetryConfigPacketID()) + return 0; + + if(_pg_numbytes < getServo_TelemetryConfigMinDataLength()) + return 0; + + // Servo telemetry settings + if(decodeServo_TelemetrySettings_t(_pg_data, &_pg_byteindex, settings) == 0) + return 0; + + // Reserved for future use + _pg_byteindex += 1*3; + + // Servo ICD revision + // Range of icdVersion is 0 to 255. + (*icdVersion) = uint8FromBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_TelemetryConfigPacket + +/*! + * \brief Create the Servo_SettingsInfo packet + * + * Non-volatile settings configuration information + * \param _pg_pkt points to the packet which will be created by this function + * \param _pg_user points to the user data that will be encoded in _pg_pkt + */ +void encodeServo_SettingsInfoPacketStructure(void* _pg_pkt, const Servo_SettingsInfo_t* _pg_user) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Set if the servo is unlocked and ready to receive settings updates + _pg_data[_pg_byteindex] = (uint8_t)_pg_user->eeUnlocked << 7; + + // Version of non-volatile settings configuration + // Range of eeVersion is 0 to 127. + _pg_data[_pg_byteindex] |= (uint8_t)_pg_user->eeVersion; + _pg_byteindex += 1; // close bit field + + + // Size of non-volatile settings data + // Range of eeSize is 0 to 65535. + uint16ToBeBytes(_pg_user->eeSize, _pg_data, &_pg_byteindex); + + // NV settings checksum + // Range of eeChecksum is 0 to 65535. + uint16ToBeBytes(_pg_user->eeChecksum, _pg_data, &_pg_byteindex); + + // Range of mramVersion is 0 to 255. + uint8ToBytes(_pg_user->mramVersion, _pg_data, &_pg_byteindex); + + // Range of mramSize is 0 to 65535. + uint16ToBeBytes(_pg_user->mramSize, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_SettingsInfoPacketID()); + +}// encodeServo_SettingsInfoPacketStructure + +/*! + * \brief Decode the Servo_SettingsInfo packet + * + * Non-volatile settings configuration information + * \param _pg_pkt points to the packet being decoded by this function + * \param _pg_user receives the data decoded from the packet + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_SettingsInfoPacketStructure(const void* _pg_pkt, Servo_SettingsInfo_t* _pg_user) +{ + int _pg_numbytes; + int _pg_byteindex = 0; + const uint8_t* _pg_data; + + // Verify the packet identifier + if(getServoPacketID(_pg_pkt) != getServo_SettingsInfoPacketID()) + return 0; + + // Verify the packet size + _pg_numbytes = getServoPacketSize(_pg_pkt); + if(_pg_numbytes < getServo_SettingsInfoMinDataLength()) + return 0; + + // The raw data from the packet + _pg_data = getServoPacketDataConst(_pg_pkt); + + // Set if the servo is unlocked and ready to receive settings updates + _pg_user->eeUnlocked = (_pg_data[_pg_byteindex] >> 7); + + // Version of non-volatile settings configuration + // Range of eeVersion is 0 to 127. + _pg_user->eeVersion = ((_pg_data[_pg_byteindex]) & 0x7F); + _pg_byteindex += 1; // close bit field + + // Size of non-volatile settings data + // Range of eeSize is 0 to 65535. + _pg_user->eeSize = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // NV settings checksum + // Range of eeChecksum is 0 to 65535. + _pg_user->eeChecksum = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Range of mramVersion is 0 to 255. + _pg_user->mramVersion = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Range of mramSize is 0 to 65535. + _pg_user->mramSize = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_SettingsInfoPacketStructure + +/*! + * \brief Create the Servo_SettingsInfo packet + * + * Non-volatile settings configuration information + * \param _pg_pkt points to the packet which will be created by this function + * \param eeUnlocked is Set if the servo is unlocked and ready to receive settings updates + * \param eeVersion is Version of non-volatile settings configuration + * \param eeSize is Size of non-volatile settings data + * \param eeChecksum is NV settings checksum + * \param mramVersion is + * \param mramSize is + */ +void encodeServo_SettingsInfoPacket(void* _pg_pkt, unsigned eeUnlocked, unsigned eeVersion, uint16_t eeSize, uint16_t eeChecksum, uint8_t mramVersion, uint16_t mramSize) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Set if the servo is unlocked and ready to receive settings updates + _pg_data[_pg_byteindex] = (uint8_t)eeUnlocked << 7; + + // Version of non-volatile settings configuration + // Range of eeVersion is 0 to 127. + _pg_data[_pg_byteindex] |= (uint8_t)eeVersion; + _pg_byteindex += 1; // close bit field + + // Size of non-volatile settings data + // Range of eeSize is 0 to 65535. + uint16ToBeBytes(eeSize, _pg_data, &_pg_byteindex); + + // NV settings checksum + // Range of eeChecksum is 0 to 65535. + uint16ToBeBytes(eeChecksum, _pg_data, &_pg_byteindex); + + // Range of mramVersion is 0 to 255. + uint8ToBytes(mramVersion, _pg_data, &_pg_byteindex); + + // Range of mramSize is 0 to 65535. + uint16ToBeBytes(mramSize, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_SettingsInfoPacketID()); + +}// encodeServo_SettingsInfoPacket + +/*! + * \brief Decode the Servo_SettingsInfo packet + * + * Non-volatile settings configuration information + * \param _pg_pkt points to the packet being decoded by this function + * \param eeUnlocked receives Set if the servo is unlocked and ready to receive settings updates + * \param eeVersion receives Version of non-volatile settings configuration + * \param eeSize receives Size of non-volatile settings data + * \param eeChecksum receives NV settings checksum + * \param mramVersion receives + * \param mramSize receives + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_SettingsInfoPacket(const void* _pg_pkt, unsigned* eeUnlocked, unsigned* eeVersion, uint16_t* eeSize, uint16_t* eeChecksum, uint8_t* mramVersion, uint16_t* mramSize) +{ + 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_SettingsInfoPacketID()) + return 0; + + if(_pg_numbytes < getServo_SettingsInfoMinDataLength()) + return 0; + + // Set if the servo is unlocked and ready to receive settings updates + (*eeUnlocked) = (_pg_data[_pg_byteindex] >> 7); + + // Version of non-volatile settings configuration + // Range of eeVersion is 0 to 127. + (*eeVersion) = ((_pg_data[_pg_byteindex]) & 0x7F); + _pg_byteindex += 1; // close bit field + + // Size of non-volatile settings data + // Range of eeSize is 0 to 65535. + (*eeSize) = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // NV settings checksum + // Range of eeChecksum is 0 to 65535. + (*eeChecksum) = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + // Range of mramVersion is 0 to 255. + (*mramVersion) = uint8FromBytes(_pg_data, &_pg_byteindex); + + // Range of mramSize is 0 to 65535. + (*mramSize) = uint16FromBeBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_SettingsInfoPacket + +/*! + * \brief Create the Servo_TelltaleA packet + * + * Servo telltale data + * \param _pg_pkt points to the packet which will be created by this function + * \param minTemperature is Minimum temperature seen by the servo + * \param maxTemperature is Maximum temperature seen by the servo + */ +void encodeServo_TelltaleAPacket(void* _pg_pkt, int8_t minTemperature, int8_t maxTemperature) +{ + uint8_t* _pg_data = getServoPacketData(_pg_pkt); + int _pg_byteindex = 0; + + // Minimum temperature seen by the servo + // Range of minTemperature is -128 to 127. + int8ToBytes(minTemperature, _pg_data, &_pg_byteindex); + + // Maximum temperature seen by the servo + // Range of maxTemperature is -128 to 127. + int8ToBytes(maxTemperature, _pg_data, &_pg_byteindex); + + // complete the process of creating the packet + finishServoPacket(_pg_pkt, _pg_byteindex, getServo_TelltaleAPacketID()); + +}// encodeServo_TelltaleAPacket + +/*! + * \brief Decode the Servo_TelltaleA packet + * + * Servo telltale data + * \param _pg_pkt points to the packet being decoded by this function + * \param minTemperature receives Minimum temperature seen by the servo + * \param maxTemperature receives Maximum temperature seen by the servo + * \return 0 is returned if the packet ID or size is wrong, else 1 + */ +int decodeServo_TelltaleAPacket(const void* _pg_pkt, int8_t* minTemperature, int8_t* maxTemperature) +{ + 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_TelltaleAPacketID()) + return 0; + + if(_pg_numbytes < getServo_TelltaleAMinDataLength()) + return 0; + + // Minimum temperature seen by the servo + // Range of minTemperature is -128 to 127. + (*minTemperature) = int8FromBytes(_pg_data, &_pg_byteindex); + + // Maximum temperature seen by the servo + // Range of maxTemperature is -128 to 127. + (*maxTemperature) = int8FromBytes(_pg_data, &_pg_byteindex); + + return 1; + +}// decodeServo_TelltaleAPacket +// end of ServoPackets.c diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h new file mode 100644 index 0000000000..ef88bb0492 --- /dev/null +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h @@ -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 . + * + * 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 +#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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.c b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.c new file mode 100644 index 0000000000..57c32b8fe7 --- /dev/null +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.c @@ -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 . + * + * 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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h new file mode 100644 index 0000000000..fc6718cb2d --- /dev/null +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h @@ -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 . + * + * 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 +#include +#include // 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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/Servo_source.xml b/libraries/AP_PiccoloCAN/piccolo_protocol/Servo_source.xml new file mode 100644 index 0000000000..e5b2754655 --- /dev/null +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/Servo_source.xml @@ -0,0 +1,342 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/fielddecode.c b/libraries/AP_PiccoloCAN/piccolo_protocol/fielddecode.c index da9fc74098..5f39ecc78c 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/fielddecode.c +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/fielddecode.c @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Author: Oliver Walters + * Author: Oliver Walters / Currawong Engineering Pty Ltd */ #include "fielddecode.h" diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/fielddecode.h b/libraries/AP_PiccoloCAN/piccolo_protocol/fielddecode.h index 98a020aed5..75b27c5555 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/fielddecode.h +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/fielddecode.h @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Author: Oliver Walters + * Author: Oliver Walters / Currawong Engineering Pty Ltd */ #ifndef _FIELDDECODE_H diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/fieldencode.c b/libraries/AP_PiccoloCAN/piccolo_protocol/fieldencode.c index 2c34f4b89f..670ca49b05 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/fieldencode.c +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/fieldencode.c @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Author: Oliver Walters + * Author: Oliver Walters / Currawong Engineering Pty Ltd */ #include "fieldencode.h" diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/fieldencode.h b/libraries/AP_PiccoloCAN/piccolo_protocol/fieldencode.h index 6fd5f6f4ff..6c6e2500d4 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/fieldencode.h +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/fieldencode.h @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Author: Oliver Walters + * Author: Oliver Walters / Currawong Engineering Pty Ltd */ #ifndef _FIELDENCODE_H diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/scaleddecode.c b/libraries/AP_PiccoloCAN/piccolo_protocol/scaleddecode.c index 7f58b7b716..e71a5cf9e5 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/scaleddecode.c +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/scaleddecode.c @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Author: Oliver Walters + * Author: Oliver Walters / Currawong Engineering Pty Ltd */ #include "scaleddecode.h" diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/scaleddecode.h b/libraries/AP_PiccoloCAN/piccolo_protocol/scaleddecode.h index 1029c941b5..a0d665d3ff 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/scaleddecode.h +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/scaleddecode.h @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Author: Oliver Walters + * Author: Oliver Walters / Currawong Engineering Pty Ltd */ #ifndef _SCALEDDECODE_H diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/scaledencode.c b/libraries/AP_PiccoloCAN/piccolo_protocol/scaledencode.c index 5ec046b9cf..04d905523f 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/scaledencode.c +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/scaledencode.c @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Author: Oliver Walters + * Author: Oliver Walters / Currawong Engineering Pty Ltd */ #include "scaledencode.h" diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/scaledencode.h b/libraries/AP_PiccoloCAN/piccolo_protocol/scaledencode.h index 1010e01f24..587a173fd5 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/scaledencode.h +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/scaledencode.h @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Author: Oliver Walters + * Author: Oliver Walters / Currawong Engineering Pty Ltd */ #ifndef _SCALEDENCODE_H