ardupilot/libraries/AP_PiccoloCAN/piccolo_protocol/ServoCommands.c

// ServoCommands.c was generated by ProtoGen version 3.2.a

/*
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Author: Oliver Walters / Currawong Engineering Pty Ltd
 */

#include "ServoCommands.h"
#include "fielddecode.h"
#include "fieldencode.h"
#include "scaleddecode.h"
#include "scaledencode.h"

/*!
 * \brief Create the Servo_RequestHighFrequencyData packet
 *
 * Select packets for high-frequency transmission. Selected packets will be
 * transmitted at 500Hz for one second. Sending this command again resets the
 * timer, allowing continuous data as long as this packet is received
 * \param _pg_pkt points to the packet which will be created by this function
 * \param packets is Select which telemetry packets are transmitted at high-speed by the servo
 */
void encodeServo_RequestHighFrequencyDataPacket(void* _pg_pkt, const Servo_TelemetryPackets_t* packets)
{
    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
    int _pg_byteindex = 0;

    uint8ToBytes((uint8_t)(CMD_SERVO_REQUEST_HF_DATA), _pg_data, &_pg_byteindex);

    // Select which telemetry packets are transmitted at high-speed by the servo
    encodeServo_TelemetryPackets_t(_pg_data, &_pg_byteindex, packets);

    // complete the process of creating the packet
    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_RequestHighFrequencyDataPacketID());

}// encodeServo_RequestHighFrequencyDataPacket

/*!
 * \brief Decode the Servo_RequestHighFrequencyData packet
 *
 * Select packets for high-frequency transmission. Selected packets will be
 * transmitted at 500Hz for one second. Sending this command again resets the
 * timer, allowing continuous data as long as this packet is received
 * \param _pg_pkt points to the packet being decoded by this function
 * \param packets receives Select which telemetry packets are transmitted at high-speed by the servo
 * \return 0 is returned if the packet ID or size is wrong, else 1
 */
int decodeServo_RequestHighFrequencyDataPacket(const void* _pg_pkt, Servo_TelemetryPackets_t* packets)
{
    int _pg_byteindex = 0;
    const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
    int _pg_numbytes = getServoPacketSize(_pg_pkt);

    // Verify the packet identifier
    if(getServoPacketID(_pg_pkt) != getServo_RequestHighFrequencyDataPacketID())
        return 0;

    if(_pg_numbytes < getServo_RequestHighFrequencyDataMinDataLength())
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_REQUEST_HF_DATA)
        return 0;

    // Select which telemetry packets are transmitted at high-speed by the servo
    if(decodeServo_TelemetryPackets_t(_pg_data, &_pg_byteindex, packets) == 0)
        return 0;

    return 1;

}// decodeServo_RequestHighFrequencyDataPacket

/*!
 * \brief Create the Servo_SetNodeID packet
 *
 * Set the Node ID (CAN Address) for the servo
 * \param _pg_pkt points to the packet which will be created by this function
 * \param serialNumber is The serial number must match that of the servo for the command to be accepted
 * \param nodeID is Set the CAN Node ID of the target servo
 */
void encodeServo_SetNodeIDPacket(void* _pg_pkt, uint32_t serialNumber, uint8_t nodeID)
{
    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
    int _pg_byteindex = 0;

    uint8ToBytes((uint8_t)(CMD_SERVO_SET_NODE_ID), _pg_data, &_pg_byteindex);

    // The serial number must match that of the servo for the command to be accepted
    // Range of serialNumber is 0 to 4294967295.
    uint32ToBeBytes(serialNumber, _pg_data, &_pg_byteindex);

    // Set the CAN Node ID of the target servo
    // Range of nodeID is 0 to 255.
    uint8ToBytes(nodeID, _pg_data, &_pg_byteindex);

    // complete the process of creating the packet
    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_SetNodeIDPacketID());

}// encodeServo_SetNodeIDPacket

/*!
 * \brief Decode the Servo_SetNodeID packet
 *
 * Set the Node ID (CAN Address) for the servo
 * \param _pg_pkt points to the packet being decoded by this function
 * \param serialNumber receives The serial number must match that of the servo for the command to be accepted
 * \param nodeID receives Set the CAN Node ID of the target servo
 * \return 0 is returned if the packet ID or size is wrong, else 1
 */
int decodeServo_SetNodeIDPacket(const void* _pg_pkt, uint32_t* serialNumber, uint8_t* nodeID)
{
    int _pg_byteindex = 0;
    const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
    int _pg_numbytes = getServoPacketSize(_pg_pkt);

    // Verify the packet identifier
    if(getServoPacketID(_pg_pkt) != getServo_SetNodeIDPacketID())
        return 0;

    if(_pg_numbytes < getServo_SetNodeIDMinDataLength())
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_SET_NODE_ID)
        return 0;

    // The serial number must match that of the servo for the command to be accepted
    // Range of serialNumber is 0 to 4294967295.
    (*serialNumber) = uint32FromBeBytes(_pg_data, &_pg_byteindex);

    // Set the CAN Node ID of the target servo
    // Range of nodeID is 0 to 255.
    (*nodeID) = uint8FromBytes(_pg_data, &_pg_byteindex);

    return 1;

}// decodeServo_SetNodeIDPacket

/*!
 * \brief Create the Servo_SetUserIdA packet
 *
 * Set user programmable ID value
 * \param _pg_pkt points to the packet which will be created by this function
 * \param id is User configurable value, 16-bit
 */
void encodeServo_SetUserIdAPacket(void* _pg_pkt, uint16_t id)
{
    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
    int _pg_byteindex = 0;

    uint8ToBytes((uint8_t)(CMD_SERVO_SET_USER_ID_A), _pg_data, &_pg_byteindex);

    // User configurable value, 16-bit
    // Range of id is 0 to 65535.
    uint16ToBeBytes(id, _pg_data, &_pg_byteindex);

    // complete the process of creating the packet
    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_SetUserIdAPacketID());

}// encodeServo_SetUserIdAPacket

/*!
 * \brief Decode the Servo_SetUserIdA packet
 *
 * Set user programmable ID value
 * \param _pg_pkt points to the packet being decoded by this function
 * \param id receives User configurable value, 16-bit
 * \return 0 is returned if the packet ID or size is wrong, else 1
 */
int decodeServo_SetUserIdAPacket(const void* _pg_pkt, uint16_t* id)
{
    int _pg_byteindex = 0;
    const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
    int _pg_numbytes = getServoPacketSize(_pg_pkt);

    // Verify the packet identifier
    if(getServoPacketID(_pg_pkt) != getServo_SetUserIdAPacketID())
        return 0;

    if(_pg_numbytes < getServo_SetUserIdAMinDataLength())
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_SET_USER_ID_A)
        return 0;

    // User configurable value, 16-bit
    // Range of id is 0 to 65535.
    (*id) = uint16FromBeBytes(_pg_data, &_pg_byteindex);

    return 1;

}// decodeServo_SetUserIdAPacket

/*!
 * \brief Create the Servo_SetUserIdB packet
 *
 * Set user programmable ID value
 * \param _pg_pkt points to the packet which will be created by this function
 * \param id is User configurable value, 16-bit
 */
void encodeServo_SetUserIdBPacket(void* _pg_pkt, uint16_t id)
{
    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
    int _pg_byteindex = 0;

    uint8ToBytes((uint8_t)(CMD_SERVO_SET_USER_ID_B), _pg_data, &_pg_byteindex);

    // User configurable value, 16-bit
    // Range of id is 0 to 65535.
    uint16ToBeBytes(id, _pg_data, &_pg_byteindex);

    // complete the process of creating the packet
    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_SetUserIdBPacketID());

}// encodeServo_SetUserIdBPacket

/*!
 * \brief Decode the Servo_SetUserIdB packet
 *
 * Set user programmable ID value
 * \param _pg_pkt points to the packet being decoded by this function
 * \param id receives User configurable value, 16-bit
 * \return 0 is returned if the packet ID or size is wrong, else 1
 */
int decodeServo_SetUserIdBPacket(const void* _pg_pkt, uint16_t* id)
{
    int _pg_byteindex = 0;
    const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
    int _pg_numbytes = getServoPacketSize(_pg_pkt);

    // Verify the packet identifier
    if(getServoPacketID(_pg_pkt) != getServo_SetUserIdBPacketID())
        return 0;

    if(_pg_numbytes < getServo_SetUserIdBMinDataLength())
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_SET_USER_ID_B)
        return 0;

    // User configurable value, 16-bit
    // Range of id is 0 to 65535.
    (*id) = uint16FromBeBytes(_pg_data, &_pg_byteindex);

    return 1;

}// decodeServo_SetUserIdBPacket

/*!
 * \brief Create the Servo_ResetDefaultSettings packet
 *
 * Reset servo settings to default values
 * \param _pg_pkt points to the packet which will be created by this function
 */
void encodeServo_ResetDefaultSettingsPacket(void* _pg_pkt)
{
    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
    int _pg_byteindex = 0;

    uint8ToBytes((uint8_t)(CMD_SERVO_RESET_DEFAULT_SETTINGS), _pg_data, &_pg_byteindex);

    uint8ToBytes((uint8_t)(0xAA), _pg_data, &_pg_byteindex);

    uint8ToBytes((uint8_t)(0x55), _pg_data, &_pg_byteindex);

    // complete the process of creating the packet
    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_ResetDefaultSettingsPacketID());

}// encodeServo_ResetDefaultSettingsPacket

/*!
 * \brief Decode the Servo_ResetDefaultSettings packet
 *
 * Reset servo settings to default values
 * \param _pg_pkt points to the packet being decoded by this function
 * \return 0 is returned if the packet ID or size is wrong, else 1
 */
int decodeServo_ResetDefaultSettingsPacket(const void* _pg_pkt)
{
    int _pg_byteindex = 0;
    const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
    int _pg_numbytes = getServoPacketSize(_pg_pkt);

    // Verify the packet identifier
    if(getServoPacketID(_pg_pkt) != getServo_ResetDefaultSettingsPacketID())
        return 0;

    if(_pg_numbytes < getServo_ResetDefaultSettingsMinDataLength())
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_RESET_DEFAULT_SETTINGS)
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0xAA)
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0x55)
        return 0;

    return 1;

}// decodeServo_ResetDefaultSettingsPacket

/*!
 * \brief Create the Servo_UnlockSettings packet
 *
 * Servo system command
 * \param _pg_pkt points to the packet which will be created by this function
 */
void encodeServo_UnlockSettingsPacket(void* _pg_pkt)
{
    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
    int _pg_byteindex = 0;

    uint8ToBytes((uint8_t)(CMD_SERVO_UNLOCK_SETTINGS), _pg_data, &_pg_byteindex);

    uint8ToBytes((uint8_t)(0xA0), _pg_data, &_pg_byteindex);

    uint8ToBytes((uint8_t)(0xB0), _pg_data, &_pg_byteindex);

    uint8ToBytes((uint8_t)(0xC0), _pg_data, &_pg_byteindex);

    // complete the process of creating the packet
    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_UnlockSettingsPacketID());

}// encodeServo_UnlockSettingsPacket

/*!
 * \brief Decode the Servo_UnlockSettings packet
 *
 * Servo system command
 * \param _pg_pkt points to the packet being decoded by this function
 * \return 0 is returned if the packet ID or size is wrong, else 1
 */
int decodeServo_UnlockSettingsPacket(const void* _pg_pkt)
{
    int _pg_byteindex = 0;
    const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
    int _pg_numbytes = getServoPacketSize(_pg_pkt);

    // Verify the packet identifier
    if(getServoPacketID(_pg_pkt) != getServo_UnlockSettingsPacketID())
        return 0;

    if(_pg_numbytes < getServo_UnlockSettingsMinDataLength())
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_UNLOCK_SETTINGS)
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0xA0)
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0xB0)
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0xC0)
        return 0;

    return 1;

}// decodeServo_UnlockSettingsPacket

/*!
 * \brief Create the Servo_LockSettings packet
 *
 * Servo system command
 * \param _pg_pkt points to the packet which will be created by this function
 */
void encodeServo_LockSettingsPacket(void* _pg_pkt)
{
    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
    int _pg_byteindex = 0;

    uint8ToBytes((uint8_t)(CMD_SERVO_LOCK_SETTINGS), _pg_data, &_pg_byteindex);

    uint8ToBytes((uint8_t)(0x0A), _pg_data, &_pg_byteindex);

    uint8ToBytes((uint8_t)(0x0B), _pg_data, &_pg_byteindex);

    uint8ToBytes((uint8_t)(0x0C), _pg_data, &_pg_byteindex);

    // complete the process of creating the packet
    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_LockSettingsPacketID());

}// encodeServo_LockSettingsPacket

/*!
 * \brief Decode the Servo_LockSettings packet
 *
 * Servo system command
 * \param _pg_pkt points to the packet being decoded by this function
 * \return 0 is returned if the packet ID or size is wrong, else 1
 */
int decodeServo_LockSettingsPacket(const void* _pg_pkt)
{
    int _pg_byteindex = 0;
    const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
    int _pg_numbytes = getServoPacketSize(_pg_pkt);

    // Verify the packet identifier
    if(getServoPacketID(_pg_pkt) != getServo_LockSettingsPacketID())
        return 0;

    if(_pg_numbytes < getServo_LockSettingsMinDataLength())
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_LOCK_SETTINGS)
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0x0A)
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0x0B)
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0x0C)
        return 0;

    return 1;

}// decodeServo_LockSettingsPacket

/*!
 * \brief Create the Servo_EnterBootloader packet
 *
 * Servo system command
 * \param _pg_pkt points to the packet which will be created by this function
 */
void encodeServo_EnterBootloaderPacket(void* _pg_pkt)
{
    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
    int _pg_byteindex = 0;

    uint8ToBytes((uint8_t)(CMD_SERVO_ENTER_BOOTLOADER), _pg_data, &_pg_byteindex);

    // This byte is required for the command to be accepted
    uint8ToBytes((uint8_t)(0xAA), _pg_data, &_pg_byteindex);

    // This byte is required for the command to be accepted
    uint8ToBytes((uint8_t)(0x55), _pg_data, &_pg_byteindex);

    // complete the process of creating the packet
    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_EnterBootloaderPacketID());

}// encodeServo_EnterBootloaderPacket

/*!
 * \brief Decode the Servo_EnterBootloader packet
 *
 * Servo system command
 * \param _pg_pkt points to the packet being decoded by this function
 * \return 0 is returned if the packet ID or size is wrong, else 1
 */
int decodeServo_EnterBootloaderPacket(const void* _pg_pkt)
{
    int _pg_byteindex = 0;
    const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
    int _pg_numbytes = getServoPacketSize(_pg_pkt);

    // Verify the packet identifier
    if(getServoPacketID(_pg_pkt) != getServo_EnterBootloaderPacketID())
        return 0;

    if(_pg_numbytes < getServo_EnterBootloaderMinDataLength())
        return 0;

    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) CMD_SERVO_ENTER_BOOTLOADER)
        return 0;

    // This byte is required for the command to be accepted
    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0xAA)
        return 0;

    // This byte is required for the command to be accepted
    if (uint8FromBytes(_pg_data, &_pg_byteindex) != (uint8_t) 0x55)
        return 0;

    return 1;

}// decodeServo_EnterBootloaderPacket
// end of ServoCommands.c