mirror of https://github.com/ArduPilot/ardupilot
2091 lines
72 KiB
C
2091 lines
72 KiB
C
// 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 <http://www.gnu.org/licenses/>.
|
|
*
|
|
* 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 packet can be used to simultaneously command multiple servos which have
|
|
* sequential CAN ID values. This packet must be sent as a broadcast packet
|
|
* (address = 0xFF) such that all servos can receive it. These commands can be
|
|
* sent to groups of servos with ID values up to 64, using different
|
|
* PKT_SERVO_MULTI_COMMAND_x packet ID values.
|
|
* \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
|
|
* \param id is the packet identifier for _pg_pkt
|
|
*/
|
|
void encodeServo_MultiPositionCommandPacket(void* _pg_pkt, int16_t commandA, int16_t commandB, int16_t commandC, int16_t commandD, uint32_t _pg_id)
|
|
{
|
|
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, _pg_id);
|
|
|
|
}// encodeServo_MultiPositionCommandPacket
|
|
|
|
/*!
|
|
* \brief Decode the Servo_MultiPositionCommand packet
|
|
*
|
|
* This packet can be used to simultaneously command multiple servos which have
|
|
* sequential CAN ID values. This packet must be sent as a broadcast packet
|
|
* (address = 0xFF) such that all servos can receive it. These commands can be
|
|
* sent to groups of servos with ID values up to 64, using different
|
|
* PKT_SERVO_MULTI_COMMAND_x packet ID values.
|
|
* \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, multiple options exist
|
|
uint32_t packetid = getServoPacketID(_pg_pkt);
|
|
if( packetid != PKT_SERVO_MULTI_COMMAND_1 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_2 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_3 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_4 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_5 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_6 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_7 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_8 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_9 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_10 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_11 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_12 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_13 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_14 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_15 &&
|
|
packetid != PKT_SERVO_MULTI_COMMAND_16 )
|
|
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 _pg_user points to the user data that will be encoded in _pg_pkt
|
|
*/
|
|
void encodeServo_StatusAPacketStructure(void* _pg_pkt, const Servo_StatusA_t* _pg_user)
|
|
{
|
|
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, &_pg_user->status);
|
|
|
|
// Warning bits indicate servo is operation outside of desired range
|
|
encodeServo_WarningBits_t(_pg_data, &_pg_byteindex, &_pg_user->warnings);
|
|
|
|
// These bits indicate critical system error information
|
|
encodeServo_ErrorBits_t(_pg_data, &_pg_byteindex, &_pg_user->errors);
|
|
|
|
// Servo position, mapped to input units
|
|
// Range of position is -32768 to 32767.
|
|
int16ToBeBytes(_pg_user->position, _pg_data, &_pg_byteindex);
|
|
|
|
// Servo commanded position
|
|
// Range of command is -32768 to 32767.
|
|
int16ToBeBytes(_pg_user->command, _pg_data, &_pg_byteindex);
|
|
|
|
// complete the process of creating the packet
|
|
finishServoPacket(_pg_pkt, _pg_byteindex, getServo_StatusAPacketID());
|
|
|
|
}// encodeServo_StatusAPacketStructure
|
|
|
|
/*!
|
|
* \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 _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_StatusAPacketStructure(const void* _pg_pkt, Servo_StatusA_t* _pg_user)
|
|
{
|
|
int _pg_numbytes;
|
|
int _pg_byteindex = 0;
|
|
const uint8_t* _pg_data;
|
|
|
|
// Verify the packet identifier
|
|
if(getServoPacketID(_pg_pkt) != getServo_StatusAPacketID())
|
|
return 0;
|
|
|
|
// Verify the packet size
|
|
_pg_numbytes = getServoPacketSize(_pg_pkt);
|
|
if(_pg_numbytes < getServo_StatusAMinDataLength())
|
|
return 0;
|
|
|
|
// The raw data from the packet
|
|
_pg_data = getServoPacketDataConst(_pg_pkt);
|
|
|
|
// Status bits contain information on servo operation
|
|
if(decodeServo_StatusBits_t(_pg_data, &_pg_byteindex, &_pg_user->status) == 0)
|
|
return 0;
|
|
|
|
// Warning bits indicate servo is operation outside of desired range
|
|
if(decodeServo_WarningBits_t(_pg_data, &_pg_byteindex, &_pg_user->warnings) == 0)
|
|
return 0;
|
|
|
|
// These bits indicate critical system error information
|
|
if(decodeServo_ErrorBits_t(_pg_data, &_pg_byteindex, &_pg_user->errors) == 0)
|
|
return 0;
|
|
|
|
// Servo position, mapped to input units
|
|
// Range of position is -32768 to 32767.
|
|
_pg_user->position = int16FromBeBytes(_pg_data, &_pg_byteindex);
|
|
|
|
// Servo commanded position
|
|
// Range of command is -32768 to 32767.
|
|
_pg_user->command = int16FromBeBytes(_pg_data, &_pg_byteindex);
|
|
|
|
return 1;
|
|
|
|
}// decodeServo_StatusAPacketStructure
|
|
|
|
/*!
|
|
* \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 _pg_user points to the user data that will be encoded in _pg_pkt
|
|
*/
|
|
void encodeServo_StatusBPacketStructure(void* _pg_pkt, const Servo_StatusB_t* _pg_user)
|
|
{
|
|
uint8_t* _pg_data = getServoPacketData(_pg_pkt);
|
|
int _pg_byteindex = 0;
|
|
|
|
// Servo current
|
|
// Range of current is 0 to 65535.
|
|
uint16ToBeBytes(_pg_user->current, _pg_data, &_pg_byteindex);
|
|
|
|
// Servo supply voltage
|
|
// Range of voltage is 0 to 65535.
|
|
uint16ToBeBytes(_pg_user->voltage, _pg_data, &_pg_byteindex);
|
|
|
|
// Servo temperature
|
|
// Range of temperature is -128 to 127.
|
|
int8ToBytes(_pg_user->temperature, _pg_data, &_pg_byteindex);
|
|
|
|
// Motor duty cycle
|
|
// Range of dutyCycle is -128 to 127.
|
|
int8ToBytes(_pg_user->dutyCycle, _pg_data, &_pg_byteindex);
|
|
|
|
// Servo output shaft speed
|
|
// Range of speed is -32768 to 32767.
|
|
int16ToBeBytes(_pg_user->speed, _pg_data, &_pg_byteindex);
|
|
|
|
// complete the process of creating the packet
|
|
finishServoPacket(_pg_pkt, _pg_byteindex, getServo_StatusBPacketID());
|
|
|
|
}// encodeServo_StatusBPacketStructure
|
|
|
|
/*!
|
|
* \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 _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_StatusBPacketStructure(const void* _pg_pkt, Servo_StatusB_t* _pg_user)
|
|
{
|
|
int _pg_numbytes;
|
|
int _pg_byteindex = 0;
|
|
const uint8_t* _pg_data;
|
|
|
|
// Verify the packet identifier
|
|
if(getServoPacketID(_pg_pkt) != getServo_StatusBPacketID())
|
|
return 0;
|
|
|
|
// Verify the packet size
|
|
_pg_numbytes = getServoPacketSize(_pg_pkt);
|
|
if(_pg_numbytes < getServo_StatusBMinDataLength())
|
|
return 0;
|
|
|
|
// The raw data from the packet
|
|
_pg_data = getServoPacketDataConst(_pg_pkt);
|
|
|
|
// this packet has default fields, make sure they are set
|
|
_pg_user->dutyCycle = 0;
|
|
_pg_user->speed = 0;
|
|
|
|
// Servo current
|
|
// Range of current is 0 to 65535.
|
|
_pg_user->current = uint16FromBeBytes(_pg_data, &_pg_byteindex);
|
|
|
|
// Servo supply voltage
|
|
// Range of voltage is 0 to 65535.
|
|
_pg_user->voltage = uint16FromBeBytes(_pg_data, &_pg_byteindex);
|
|
|
|
// Servo temperature
|
|
// Range of temperature is -128 to 127.
|
|
_pg_user->temperature = int8FromBytes(_pg_data, &_pg_byteindex);
|
|
|
|
if(_pg_byteindex + 1 > _pg_numbytes)
|
|
return 1;
|
|
|
|
// Motor duty cycle
|
|
// Range of dutyCycle is -128 to 127.
|
|
_pg_user->dutyCycle = int8FromBytes(_pg_data, &_pg_byteindex);
|
|
|
|
if(_pg_byteindex + 2 > _pg_numbytes)
|
|
return 1;
|
|
|
|
// Servo output shaft speed
|
|
// Range of speed is -32768 to 32767.
|
|
_pg_user->speed = int16FromBeBytes(_pg_data, &_pg_byteindex);
|
|
|
|
return 1;
|
|
|
|
}// decodeServo_StatusBPacketStructure
|
|
|
|
/*!
|
|
* \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
|
|
* \param dutyCycle is Motor duty cycle
|
|
* \param speed is Servo output shaft speed
|
|
*/
|
|
void encodeServo_StatusBPacket(void* _pg_pkt, uint16_t current, uint16_t voltage, int8_t temperature, int8_t dutyCycle, int16_t speed)
|
|
{
|
|
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);
|
|
|
|
// Motor duty cycle
|
|
// Range of dutyCycle is -128 to 127.
|
|
int8ToBytes(dutyCycle, _pg_data, &_pg_byteindex);
|
|
|
|
// Servo output shaft speed
|
|
// Range of speed is -32768 to 32767.
|
|
int16ToBeBytes(speed, _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
|
|
* \param dutyCycle receives Motor duty cycle
|
|
* \param speed receives Servo output shaft speed
|
|
* \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, int8_t* dutyCycle, int16_t* speed)
|
|
{
|
|
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;
|
|
|
|
// this packet has default fields, make sure they are set
|
|
(*dutyCycle) = 0;
|
|
(*speed) = 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);
|
|
|
|
if(_pg_byteindex + 1 > _pg_numbytes)
|
|
return 1;
|
|
|
|
// Motor duty cycle
|
|
// Range of dutyCycle is -128 to 127.
|
|
(*dutyCycle) = int8FromBytes(_pg_data, &_pg_byteindex);
|
|
|
|
if(_pg_byteindex + 2 > _pg_numbytes)
|
|
return 1;
|
|
|
|
// Servo output shaft speed
|
|
// Range of speed is -32768 to 32767.
|
|
(*speed) = int16FromBeBytes(_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 _pg_user points to the user data that will be encoded in _pg_pkt
|
|
*/
|
|
void encodeServo_StatusCPacketStructure(void* _pg_pkt, const Servo_StatusC_t* _pg_user)
|
|
{
|
|
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(_pg_user->position, _pg_data, &_pg_byteindex);
|
|
|
|
// complete the process of creating the packet
|
|
finishServoPacket(_pg_pkt, _pg_byteindex, getServo_StatusCPacketID());
|
|
|
|
}// encodeServo_StatusCPacketStructure
|
|
|
|
/*!
|
|
* \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 _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_StatusCPacketStructure(const void* _pg_pkt, Servo_StatusC_t* _pg_user)
|
|
{
|
|
int _pg_numbytes;
|
|
int _pg_byteindex = 0;
|
|
const uint8_t* _pg_data;
|
|
|
|
// Verify the packet identifier
|
|
if(getServoPacketID(_pg_pkt) != getServo_StatusCPacketID())
|
|
return 0;
|
|
|
|
// Verify the packet size
|
|
_pg_numbytes = getServoPacketSize(_pg_pkt);
|
|
if(_pg_numbytes < getServo_StatusCMinDataLength())
|
|
return 0;
|
|
|
|
// The raw data from the packet
|
|
_pg_data = getServoPacketDataConst(_pg_pkt);
|
|
|
|
// Servo position, mapped to input units
|
|
// Range of position is -32768 to 32767.
|
|
_pg_user->position = int16FromBeBytes(_pg_data, &_pg_byteindex);
|
|
|
|
return 1;
|
|
|
|
}// decodeServo_StatusCPacketStructure
|
|
|
|
/*!
|
|
* \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 _pg_user points to the user data that will be encoded in _pg_pkt
|
|
*/
|
|
void encodeServo_TelemetryConfigPacketStructure(void* _pg_pkt, const Servo_TelemetryConfig_t* _pg_user)
|
|
{
|
|
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, &_pg_user->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_TelemetryConfigPacketStructure
|
|
|
|
/*!
|
|
* \brief Decode the Servo_TelemetryConfig packet
|
|
*
|
|
* Telemetry settings configuration packet
|
|
* \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_TelemetryConfigPacketStructure(const void* _pg_pkt, Servo_TelemetryConfig_t* _pg_user)
|
|
{
|
|
int _pg_numbytes;
|
|
int _pg_byteindex = 0;
|
|
const uint8_t* _pg_data;
|
|
|
|
// Verify the packet identifier
|
|
if(getServoPacketID(_pg_pkt) != getServo_TelemetryConfigPacketID())
|
|
return 0;
|
|
|
|
// Verify the packet size
|
|
_pg_numbytes = getServoPacketSize(_pg_pkt);
|
|
if(_pg_numbytes < getServo_TelemetryConfigMinDataLength())
|
|
return 0;
|
|
|
|
// The raw data from the packet
|
|
_pg_data = getServoPacketDataConst(_pg_pkt);
|
|
|
|
// Servo telemetry settings
|
|
if(decodeServo_TelemetrySettings_t(_pg_data, &_pg_byteindex, &_pg_user->settings) == 0)
|
|
return 0;
|
|
|
|
// Reserved for future use
|
|
_pg_byteindex += 1*3;
|
|
|
|
// Servo ICD revision
|
|
// Range of icdVersion is 0 to 255.
|
|
_pg_user->icdVersion = uint8FromBytes(_pg_data, &_pg_byteindex);
|
|
|
|
return 1;
|
|
|
|
}// decodeServo_TelemetryConfigPacketStructure
|
|
|
|
/*!
|
|
* \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
|