mirror of https://github.com/ArduPilot/ardupilot
135 lines
6.4 KiB
C
135 lines
6.4 KiB
C
// LegacyESCPackets.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
|
|
*/
|
|
|
|
#include "LegacyESCPackets.h"
|
|
#include "fielddecode.h"
|
|
#include "fieldencode.h"
|
|
#include "scaleddecode.h"
|
|
#include "scaledencode.h"
|
|
|
|
/*!
|
|
* \brief Create the ESC_LegacyStatusA packet
|
|
*
|
|
* Legacy (gen-1) definition for the STATUS_A packet
|
|
* \param _pg_pkt points to the packet which will be created by this function
|
|
* \param mode is ESC operating mode. The lower four bits indicate the operational mode of the ESC, in accordance with the ESCOperatingModes enumeration. The upper 3 bits are used for debugging and should be ignored for general use.
|
|
* \param status is ESC status bits
|
|
* \param warnings is ESC warning bits
|
|
* \param errors is ESC *error* bits
|
|
* \param command is ESC operational command - value depends on 'mode' available in this packet. If the ESC is disabled, data reads 0x0000. If the ESC is in open-loop PWM mode, this value is the PWM command in units of 1us, in the range 1000us to 2000us. If the ESC is in closed-loop RPM mode, this value is the RPM command in units of 1RPM
|
|
* \param rpm is Motor speed
|
|
*/
|
|
void encodeESC_LegacyStatusAPacket(void* _pg_pkt, uint8_t mode, const ESC_LegacyStatusBits_t* status, const ESC_LegacyWarningBits_t* warnings, const ESC_LegacyErrorBits_t* errors, uint16_t command, uint16_t rpm)
|
|
{
|
|
uint8_t* _pg_data = getESCVelocityPacketData(_pg_pkt);
|
|
int _pg_byteindex = 0;
|
|
|
|
// Set to 0 to indicate a Gen-2 ESC
|
|
_pg_data[_pg_byteindex] = 0;
|
|
|
|
// ESC operating mode. The lower four bits indicate the operational mode of the ESC, in accordance with the ESCOperatingModes enumeration. The upper 3 bits are used for debugging and should be ignored for general use.
|
|
// Range of mode is 0 to 127.
|
|
_pg_data[_pg_byteindex] |= (uint8_t)limitMax(mode, 127);
|
|
_pg_byteindex += 1; // close bit field
|
|
|
|
// ESC status bits
|
|
encodeESC_LegacyStatusBits_t(_pg_data, &_pg_byteindex, status);
|
|
|
|
// ESC warning bits
|
|
encodeESC_LegacyWarningBits_t(_pg_data, &_pg_byteindex, warnings);
|
|
|
|
// ESC *error* bits
|
|
encodeESC_LegacyErrorBits_t(_pg_data, &_pg_byteindex, errors);
|
|
|
|
// ESC operational command - value depends on 'mode' available in this packet. If the ESC is disabled, data reads 0x0000. If the ESC is in open-loop PWM mode, this value is the PWM command in units of 1us, in the range 1000us to 2000us. If the ESC is in closed-loop RPM mode, this value is the RPM command in units of 1RPM
|
|
// Range of command is 0 to 65535.
|
|
uint16ToBeBytes(command, _pg_data, &_pg_byteindex);
|
|
|
|
// Motor speed
|
|
// Range of rpm is 0 to 65535.
|
|
uint16ToBeBytes(rpm, _pg_data, &_pg_byteindex);
|
|
|
|
// complete the process of creating the packet
|
|
finishESCVelocityPacket(_pg_pkt, _pg_byteindex, getESC_LegacyStatusAPacketID());
|
|
|
|
}// encodeESC_LegacyStatusAPacket
|
|
|
|
/*!
|
|
* \brief Decode the ESC_LegacyStatusA packet
|
|
*
|
|
* Legacy (gen-1) definition for the STATUS_A packet
|
|
* \param _pg_pkt points to the packet being decoded by this function
|
|
* \param mode receives ESC operating mode. The lower four bits indicate the operational mode of the ESC, in accordance with the ESCOperatingModes enumeration. The upper 3 bits are used for debugging and should be ignored for general use.
|
|
* \param status receives ESC status bits
|
|
* \param warnings receives ESC warning bits
|
|
* \param errors receives ESC *error* bits
|
|
* \param command receives ESC operational command - value depends on 'mode' available in this packet. If the ESC is disabled, data reads 0x0000. If the ESC is in open-loop PWM mode, this value is the PWM command in units of 1us, in the range 1000us to 2000us. If the ESC is in closed-loop RPM mode, this value is the RPM command in units of 1RPM
|
|
* \param rpm receives Motor speed
|
|
* \return 0 is returned if the packet ID or size is wrong, else 1
|
|
*/
|
|
int decodeESC_LegacyStatusAPacket(const void* _pg_pkt, uint8_t* mode, ESC_LegacyStatusBits_t* status, ESC_LegacyWarningBits_t* warnings, ESC_LegacyErrorBits_t* errors, uint16_t* command, uint16_t* rpm)
|
|
{
|
|
unsigned int _pg_tempbitfield = 0;
|
|
int _pg_byteindex = 0;
|
|
const uint8_t* _pg_data = getESCVelocityPacketDataConst(_pg_pkt);
|
|
int _pg_numbytes = getESCVelocityPacketSize(_pg_pkt);
|
|
|
|
// Verify the packet identifier
|
|
if(getESCVelocityPacketID(_pg_pkt) != getESC_LegacyStatusAPacketID())
|
|
return 0;
|
|
|
|
if(_pg_numbytes < getESC_LegacyStatusAMinDataLength())
|
|
return 0;
|
|
|
|
// Set to 0 to indicate a Gen-2 ESC
|
|
_pg_tempbitfield = (_pg_data[_pg_byteindex] >> 7);
|
|
// Decoded value must be 0
|
|
if(_pg_tempbitfield != 0)
|
|
return 0;
|
|
|
|
// ESC operating mode. The lower four bits indicate the operational mode of the ESC, in accordance with the ESCOperatingModes enumeration. The upper 3 bits are used for debugging and should be ignored for general use.
|
|
// Range of mode is 0 to 127.
|
|
(*mode) = ((_pg_data[_pg_byteindex]) & 0x7F);
|
|
_pg_byteindex += 1; // close bit field
|
|
|
|
// ESC status bits
|
|
if(decodeESC_LegacyStatusBits_t(_pg_data, &_pg_byteindex, status) == 0)
|
|
return 0;
|
|
|
|
// ESC warning bits
|
|
if(decodeESC_LegacyWarningBits_t(_pg_data, &_pg_byteindex, warnings) == 0)
|
|
return 0;
|
|
|
|
// ESC *error* bits
|
|
if(decodeESC_LegacyErrorBits_t(_pg_data, &_pg_byteindex, errors) == 0)
|
|
return 0;
|
|
|
|
// ESC operational command - value depends on 'mode' available in this packet. If the ESC is disabled, data reads 0x0000. If the ESC is in open-loop PWM mode, this value is the PWM command in units of 1us, in the range 1000us to 2000us. If the ESC is in closed-loop RPM mode, this value is the RPM command in units of 1RPM
|
|
// Range of command is 0 to 65535.
|
|
(*command) = uint16FromBeBytes(_pg_data, &_pg_byteindex);
|
|
|
|
// Motor speed
|
|
// Range of rpm is 0 to 65535.
|
|
(*rpm) = uint16FromBeBytes(_pg_data, &_pg_byteindex);
|
|
|
|
return 1;
|
|
|
|
}// decodeESC_LegacyStatusAPacket
|
|
// end of LegacyESCPackets.c
|