You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
2090 lines
72 KiB
2090 lines
72 KiB
// 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
|
|
|