ardupilot/libraries/AP_PiccoloCAN/piccolo_protocol/ServoDefines.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