ardupilot/libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.c

515 lines
18 KiB
C

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