Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 07:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Recover 256 bytes of RAM by making the packet transmission code smarter. Now we emit the packet directly from the procedure parameters, rather than wasting time and space packing it into a temporary buffer.

Browse Source 
Revert the buffer overflow test until I can work out what James was doing with it.

Don't try to send a text message in response to a message we don't like; we should probably implement a NAK message instead.

Improve the text string sender a bit.  We need to fix the protocol generator for this to be less sucky on the send side.


git-svn-id: https://arducopter.googlecode.com/svn/trunk@827 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com 2010-11-11 07:16:32 +00:00
parent 2e048125ef
commit c0c28f8eae
6 changed files with 562 additions and 510 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

294
libraries/APM_BinComm/APM_BinComm.cpp
View File

@ -1,4 +1,4 @@
// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// //
// Copyright (c) 2010 Michael Smith. All rights reserved. // Copyright (c) 2010 Michael Smith. All rights reserved.
// //
@ -45,194 +45,192 @@
#define DEC_MESSAGE_TIMEOUT 1000 #define DEC_MESSAGE_TIMEOUT 1000
BinComm::BinComm(const BinComm::MessageHandler *handlerTable, BinComm::BinComm(const BinComm::MessageHandler *handlerTable,
uint16_t rxBufferSize, Stream *interface) : Stream *interface) :
_handlerTable(handlerTable), _handlerTable(handlerTable),
_decodePhase(DEC_WAIT_P1), _decodePhase(DEC_WAIT_P1),
_lastReceived(millis()), _lastReceived(millis())
_rxBufferSize(rxBufferSize)
{ {
init(interface); init(interface);
}; };
void void
BinComm::init(Stream *interface) BinComm::init(Stream *interface)
{ {
_interface = interface; _interface = interface;
} }
void void
BinComm::_sendMessage(void) BinComm::_startMessage(uint8_t messageId, uint8_t messageLength, uint8_t messageVersion)
{ {
uint8_t bytesToSend; // send the preamble first
uint8_t sumA, sumB; _interface->write((uint8_t)MSG_PREAMBLE_1);
const uint8_t *p; _interface->write((uint8_t)MSG_PREAMBLE_2);
// send the preamble first // initialise the checksum accumulators
_interface->write((uint8_t)MSG_PREAMBLE_1); _encoderSumA = _encoderSumB = 0;
_interface->write((uint8_t)MSG_PREAMBLE_2);
// set up to send the payload // send the header
bytesToSend = _encodeBuf.header.length + sizeof(_encodeBuf.header); _emit(messageLength);
sumA = sumB = 0; _emit(messageId);
p = (const uint8_t *)&_encodeBuf; _emit(messageVersion);
}
// send message bytes and compute checksum on the fly void
while (bytesToSend--) { BinComm::_send(const void *bytes, uint8_t count)
sumA += *p; {
sumB += sumA; const uint8_t *p = (const uint8_t *)bytes;
_interface->write(*p++); uint8_t c;
}
// send the checksum while (count--) {
_interface->write(sumA); c = *p++;
_interface->write(sumB); _encoderSumA += c;
_encoderSumB += _encoderSumA;
_interface->write(c);
}
}
void
BinComm::_endMessage(void)
{
_interface->write(_encoderSumA);
_interface->write(_encoderSumB);
} }
void void
BinComm::update(void) BinComm::update(void)
{ {
uint8_t count; uint8_t count;
// Ensure that we don't spend too long here by only processing // Ensure that we don't spend too long here by only processing
// the bytes that were available when we started. // the bytes that were available when we started.
// //
// XXX we might want to further constrain this count // XXX we might want to further constrain this count
count = _interface->available(); count = _interface->available();
if (count >= _rxBufferSize) while (count--)
{ _decode(_interface->read());
char text[50];
strncpy(text,"<bincomm> buffer overflow",50);
send_msg_status_text(SEVERITY_LOW,text);
}
while (count--)
_decode(_interface->read());
} }
void void
BinComm::_decode(uint8_t inByte) BinComm::_decode(uint8_t inByte)
{ {
uint8_t tableIndex; uint8_t tableIndex;
// handle inter-byte timeouts (resync after link loss, etc.) // handle inter-byte timeouts (resync after link loss, etc.)
// //
if ((millis() - _lastReceived) > DEC_MESSAGE_TIMEOUT) if ((millis() - _lastReceived) > DEC_MESSAGE_TIMEOUT)
_decodePhase = DEC_WAIT_P1; _decodePhase = DEC_WAIT_P1;
_lastReceived = millis(); _lastReceived = millis();
// run the decode state machine // run the decode state machine
// //
switch (_decodePhase) { switch (_decodePhase) {
// Preamble detection // Preamble detection
// //
// Note the fallthrough from P2 to P1 deals with the case where // Note the fallthrough from P2 to P1 deals with the case where
// we see 0x34, 0x34, 0x44 where the first 0x34 is garbage or // we see 0x34, 0x34, 0x44 where the first 0x34 is garbage or
// a SUM_B byte we never looked at. // a SUM_B byte we never looked at.
// //
case DEC_WAIT_P2: case DEC_WAIT_P2:
if (MSG_PREAMBLE_2 == inByte) { if (MSG_PREAMBLE_2 == inByte) {
_decodePhase++; _decodePhase++;
// prepare for the header // prepare for the header
_bytesIn = 0; _bytesIn = 0;
_bytesExpected = sizeof(MessageHeader); _bytesExpected = sizeof(MessageHeader);
// intialise the checksum accumulators // intialise the checksum accumulators
_sumA = _sumB = 0; _decoderSumA = _decoderSumB = 0;
break; break;
} }
_decodePhase = DEC_WAIT_P1; _decodePhase = DEC_WAIT_P1;
// FALLTHROUGH // FALLTHROUGH
case DEC_WAIT_P1: case DEC_WAIT_P1:
if (MSG_PREAMBLE_1 == inByte) { if (MSG_PREAMBLE_1 == inByte) {
_decodePhase++; _decodePhase++;
} }
break; break;
// receiving the header // receiving the header
// //
case DEC_WAIT_HEADER: case DEC_WAIT_HEADER:
// do checksum accumulation // do checksum accumulation
_sumA += inByte; _decoderSumA += inByte;
_sumB += _sumA; _decoderSumB += _decoderSumA;
// store the byte // store the byte
_decodeBuf.bytes[_bytesIn++] = inByte; _decodeBuf.bytes[_bytesIn++] = inByte;
// check for complete header received // check for complete header received
if (_bytesIn == _bytesExpected) { if (_bytesIn == _bytesExpected) {
_decodePhase++; _decodePhase++;
// prepare for the payload // prepare for the payload
// variable-length data? // variable-length data?
_bytesIn = 0; _bytesIn = 0;
_bytesExpected = _decodeBuf.header.length; _bytesExpected = _decodeBuf.header.length;
_messageID = _decodeBuf.header.messageID; _messageID = _decodeBuf.header.messageID;
_messageVersion = _decodeBuf.header.messageVersion; _messageVersion = _decodeBuf.header.messageVersion;
// sanity check to avoid buffer overflow - revert back to waiting // sanity check to avoid buffer overflow - revert back to waiting
if (_bytesExpected > sizeof(_decodeBuf)) if (_bytesExpected > sizeof(_decodeBuf))
_decodePhase = DEC_WAIT_P1; _decodePhase = DEC_WAIT_P1;
} }
break; break;
// receiving payload data // receiving payload data
// //
case DEC_WAIT_MESSAGE: case DEC_WAIT_MESSAGE:
// do checksum accumulation // do checksum accumulation
_sumA += inByte; _decoderSumA += inByte;
_sumB += _sumA; _decoderSumB += _decoderSumA;
// store the byte // store the byte
_decodeBuf.bytes[_bytesIn++] = inByte; _decodeBuf.bytes[_bytesIn++] = inByte;
// check for complete payload received // check for complete payload received
if (_bytesIn == _bytesExpected) { if (_bytesIn == _bytesExpected) {
_decodePhase++; _decodePhase++;
} }
break; break;
// waiting for the checksum bytes // waiting for the checksum bytes
// //
case DEC_WAIT_SUM_A: case DEC_WAIT_SUM_A:
if (inByte != _sumA) { if (inByte != _decoderSumA) {
badMessagesReceived++; badMessagesReceived++;
_decodePhase = DEC_WAIT_P1; _decodePhase = DEC_WAIT_P1;
} else { } else {
_decodePhase++; _decodePhase++;
} }
break; break;
case DEC_WAIT_SUM_B: case DEC_WAIT_SUM_B:
if (inByte == _sumB) { if (inByte == _decoderSumB) {
// if we got this far, we have a message // if we got this far, we have a message
messagesReceived++; messagesReceived++;
// call any handler interested in this message // call any handler interested in this message
for (tableIndex = 0; MSG_NULL != _handlerTable[tableIndex].messageID; tableIndex++) for (tableIndex = 0; MSG_NULL != _handlerTable[tableIndex].messageID; tableIndex++) {
if(_handlerTable[tableIndex].messageID == MSG_ANY || if(_handlerTable[tableIndex].messageID == MSG_ANY ||
_handlerTable[tableIndex].messageID == _messageID ) _handlerTable[tableIndex].messageID == _messageID ) {
{ _handlerTable[tableIndex].handler(_handlerTable[tableIndex].arg,
_handlerTable[tableIndex].handler(_handlerTable[tableIndex].arg, _messageID, _messageVersion, &_decodeBuf);
_messageID, _messageVersion, &_decodeBuf); // dont' acknowledge an acknowledgement, will echo infinetly
// dont' acknowledge an acknowledgement, will echo infinetly if (_messageID != MSG_ACKNOWLEDGE)
if (_messageID != MSG_ACKNOWLEDGE) send_msg_acknowledge(_messageID,_sumA,_sumB); send_msg_acknowledge(_messageID, _decoderSumA, _decoderSumB);
} } else {
else // XXX should send a NAK of some sort here
{ }
char str[50]; }
sprintf(str,"<bincomm> unhandled messageID:%x",_messageID); } else {
send_msg_status_text(SEVERITY_LOW,str); badMessagesReceived++;
} }
} else { // FALLTHROUGH
badMessagesReceived++; default:
} _decodePhase = DEC_WAIT_P1;
// FALLTHROUGH break;
default:
_decodePhase = DEC_WAIT_P1;
break;
} }
} }

86
libraries/APM_BinComm/APM_BinComm.h
View File

@ -76,7 +76,7 @@ public:
/// ///
BinComm(const MessageHandler *handlerTable, BinComm(const MessageHandler *handlerTable,
uint16_t rxBufferSize, Stream *interface = NULL); Stream *interface = NULL);
/// ///
/// Optional initialiser. /// Optional initialiser.
@ -105,51 +105,42 @@ private:
uint8_t payload[256]; uint8_t payload[256];
} _decodeBuf; } _decodeBuf;
uint16_t _rxBufferSize;
/// Outgoing header/packet buffer
/// XXX we could make this smaller
struct {
MessageHeader header;
uint8_t payload[256 - sizeof(MessageHeader)];
} _encodeBuf;
////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////
/// @name Message pack/unpack utility functions /// @name Message pack/unpack utility functions
/// ///
//@{ //@{
/// Pack any scalar type /// Emit any scalar type.
/// ///
/// @param ptr Buffer pointer. /// @param x Value to emit.
/// @param x Value to pack.
/// ///
template <typename T> inline void _pack(uint8_t *&ptr, const T x) { *(T *)ptr = x; ptr += sizeof(T); } template <typename T> inline void _emit(const T x) { _send(&x, sizeof(T)); }
/// Pack an array of any scalar type /// Emit an array of any scalar type.
/// ///
/// @param ptr Buffer pointer. /// @param x Array to emit.
/// @param x Array to pack.
/// @param count Array size. /// @param count Array size.
/// ///
template <typename T> inline void _pack(uint8_t *&ptr, const T *values, uint8_t count) { template <typename T> inline void _emit(const T *values, uint8_t count) { _send(values, count * sizeof(T)); }
memcpy(ptr, values, count * sizeof(T));
ptr += count * sizeof(T);
/// Emit a fixed-size string, from a NUL-terminated buffer.
///
/// The string is NUL-padded if the buffer is larger than the string.
///
/// @param msg The NUL-terminated string to emit.
/// @param size The maximum length of the string to emit.
///
inline void _emit(const char *msg, uint8_t size) {
while (size--) {
char c = *msg;
_emit(c);
if (0 != c)
msg++;
}
} }
/// Pack a string into a fixed-size buffer. /// Unpack any scalar type.
///
/// @param ptr Buffer pointer
/// @param msg The NUL-terminated string to pack.
/// @param size The size of the buffer (which limits the quantity of the string
/// that is actually copied.
///
inline void _pack(uint8_t *&ptr, const char *msg, uint8_t size) {
strncpy((char *)ptr, msg, size); ptr[size-1] = '\0'; ptr += size;
}
/// Unpack any scalar type
/// ///
/// @param buf Buffer pointer. /// @param buf Buffer pointer.
/// @param x Unpacked result. /// @param x Unpacked result.
@ -320,8 +311,8 @@ private:
uint8_t _decodePhase; ///< decoder state machine phase uint8_t _decodePhase; ///< decoder state machine phase
uint8_t _bytesIn; ///< bytes received in the current phase uint8_t _bytesIn; ///< bytes received in the current phase
uint8_t _bytesExpected; ///< bytes expected in the current phase uint8_t _bytesExpected; ///< bytes expected in the current phase
uint8_t _sumA; ///< sum of incoming bytes uint8_t _decoderSumA; ///< sum of incoming bytes
uint8_t _sumB; ///< sum of _sumA values uint8_t _decoderSumB; ///< sum of _sumA values
uint8_t _messageID; ///< messageID from the packet being received uint8_t _messageID; ///< messageID from the packet being received
uint8_t _messageVersion;///< messageVersion from the packet being received uint8_t _messageVersion;///< messageVersion from the packet being received
@ -335,9 +326,30 @@ private:
/// ///
void _decode(uint8_t inByte); void _decode(uint8_t inByte);
/// Send the packet in the encode buffer. //////////////////////////////////////////////////////////////////////
/// @name Encoder state
//@{
uint8_t _encoderSumA; ///< sum of outgoing bytes
uint8_t _encoderSumB; ///< sum of _sumA values
//@}
/// Start transmitting a message.
/// ///
void _sendMessage(void); /// @param messageId The ID of the message to be sent
/// @param messageLength The protocol-defined length of the message in bytes
/// @param messageVersion The message version (optional)
///
void _startMessage(uint8_t messageId, uint8_t messageLength, uint8_t messageVersion = 1);
/// Send bytes as part of a message.
///
/// @param bytes Pointer to the byte(s) to send.
/// @param count Count of bytes to send.
void _send(const void *bytes, uint8_t count);
/// Finalise message transmission.
///
void _endMessage(void);
}; };
#endif // BinComm_h #endif // BinComm_h

636
libraries/APM_BinComm/protocol/protocol.h
View File

@ -23,14 +23,14 @@ send_msg_acknowledge(
const uint8_t sum1, const uint8_t sum1,
const uint8_t sum2) const uint8_t sum2)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_ACKNOWLEDGE,
_pack(__p, msgID); sizeof(msgID) +
_pack(__p, sum1); sizeof(sum1) +
_pack(__p, sum2); sizeof(sum2) + 0);
_encodeBuf.header.length = 3; _emit(msgID);
_encodeBuf.header.messageID = MSG_ACKNOWLEDGE; _emit(sum1);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(sum2);
_sendMessage(); _endMessage();
}; };
/// Unpack a MSG_ACKNOWLEDGE message /// Unpack a MSG_ACKNOWLEDGE message
@ -63,13 +63,12 @@ send_msg_status_text(
const uint8_t severity, const uint8_t severity,
const char (&text)[50]) const char (&text)[50])
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_STATUS_TEXT,
_pack(__p, severity); sizeof(severity) +
_pack(__p, text, 50); (sizeof(text[0]) * 50) + 0);
_encodeBuf.header.length = 51; _emit(severity);
_encodeBuf.header.messageID = MSG_STATUS_TEXT; _emit(text, 50);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _endMessage();
_sendMessage();
}; };
/// Unpack a MSG_STATUS_TEXT message /// Unpack a MSG_STATUS_TEXT message
@ -104,15 +103,16 @@ send_msg_heartbeat(
const uint16_t batteryVoltage, const uint16_t batteryVoltage,
const uint16_t commandIndex) const uint16_t commandIndex)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_HEARTBEAT,
_pack(__p, flightMode); sizeof(flightMode) +
_pack(__p, timeStamp); sizeof(timeStamp) +
_pack(__p, batteryVoltage); sizeof(batteryVoltage) +
_pack(__p, commandIndex); sizeof(commandIndex) + 0);
_encodeBuf.header.length = 7; _emit(flightMode);
_encodeBuf.header.messageID = MSG_HEARTBEAT; _emit(timeStamp);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(batteryVoltage);
_sendMessage(); _emit(commandIndex);
_endMessage();
}; };
/// Unpack a MSG_HEARTBEAT message /// Unpack a MSG_HEARTBEAT message
@ -149,14 +149,14 @@ send_msg_attitude(
const int16_t pitch, const int16_t pitch,
const uint16_t yaw) const uint16_t yaw)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_ATTITUDE,
_pack(__p, roll); sizeof(roll) +
_pack(__p, pitch); sizeof(pitch) +
_pack(__p, yaw); sizeof(yaw) + 0);
_encodeBuf.header.length = 6; _emit(roll);
_encodeBuf.header.messageID = MSG_ATTITUDE; _emit(pitch);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(yaw);
_sendMessage(); _endMessage();
}; };
/// Unpack a MSG_ATTITUDE message /// Unpack a MSG_ATTITUDE message
@ -197,17 +197,20 @@ send_msg_location(
const uint16_t groundCourse, const uint16_t groundCourse,
const uint32_t timeOfWeek) const uint32_t timeOfWeek)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_LOCATION,
_pack(__p, latitude); sizeof(latitude) +
_pack(__p, longitude); sizeof(longitude) +
_pack(__p, altitude); sizeof(altitude) +
_pack(__p, groundSpeed); sizeof(groundSpeed) +
_pack(__p, groundCourse); sizeof(groundCourse) +
_pack(__p, timeOfWeek); sizeof(timeOfWeek) + 0);
_encodeBuf.header.length = 20; _emit(latitude);
_encodeBuf.header.messageID = MSG_LOCATION; _emit(longitude);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(altitude);
_sendMessage(); _emit(groundSpeed);
_emit(groundCourse);
_emit(timeOfWeek);
_endMessage();
}; };
/// Unpack a MSG_LOCATION message /// Unpack a MSG_LOCATION message
@ -246,13 +249,12 @@ send_msg_pressure(
const int32_t pressureAltitude, const int32_t pressureAltitude,
const int16_t airSpeed) const int16_t airSpeed)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_PRESSURE,
_pack(__p, pressureAltitude); sizeof(pressureAltitude) +
_pack(__p, airSpeed); sizeof(airSpeed) + 0);
_encodeBuf.header.length = 6; _emit(pressureAltitude);
_encodeBuf.header.messageID = MSG_PRESSURE; _emit(airSpeed);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _endMessage();
_sendMessage();
}; };
/// Unpack a MSG_PRESSURE message /// Unpack a MSG_PRESSURE message
@ -299,21 +301,28 @@ send_msg_perf_report(
const uint16_t imuHealth, const uint16_t imuHealth,
const uint16_t gcsMessageCount) const uint16_t gcsMessageCount)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_PERF_REPORT,
_pack(__p, interval); sizeof(interval) +
_pack(__p, mainLoopCycles); sizeof(mainLoopCycles) +
_pack(__p, mainLoopCycleTime); sizeof(mainLoopCycleTime) +
_pack(__p, gyroSaturationCount); sizeof(gyroSaturationCount) +
_pack(__p, adcConstraintCount); sizeof(adcConstraintCount) +
_pack(__p, renormSqrtCount); sizeof(renormSqrtCount) +
_pack(__p, renormBlowupCount); sizeof(renormBlowupCount) +
_pack(__p, gpsFixCount); sizeof(gpsFixCount) +
_pack(__p, imuHealth); sizeof(imuHealth) +
_pack(__p, gcsMessageCount); sizeof(gcsMessageCount) + 0);
_encodeBuf.header.length = 16; _emit(interval);
_encodeBuf.header.messageID = MSG_PERF_REPORT; _emit(mainLoopCycles);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(mainLoopCycleTime);
_sendMessage(); _emit(gyroSaturationCount);
_emit(adcConstraintCount);
_emit(renormSqrtCount);
_emit(renormBlowupCount);
_emit(gpsFixCount);
_emit(imuHealth);
_emit(gcsMessageCount);
_endMessage();
}; };
/// Unpack a MSG_PERF_REPORT message /// Unpack a MSG_PERF_REPORT message
@ -360,13 +369,12 @@ send_msg_version_request(
const uint8_t systemType, const uint8_t systemType,
const uint8_t systemID) const uint8_t systemID)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_VERSION_REQUEST,
_pack(__p, systemType); sizeof(systemType) +
_pack(__p, systemID); sizeof(systemID) + 0);
_encodeBuf.header.length = 2; _emit(systemType);
_encodeBuf.header.messageID = MSG_VERSION_REQUEST; _emit(systemID);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _endMessage();
_sendMessage();
}; };
/// Unpack a MSG_VERSION_REQUEST message /// Unpack a MSG_VERSION_REQUEST message
@ -399,14 +407,14 @@ send_msg_version(
const uint8_t systemID, const uint8_t systemID,
const uint8_t (&firmwareVersion)[3]) const uint8_t (&firmwareVersion)[3])
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_VERSION,
_pack(__p, systemType); sizeof(systemType) +
_pack(__p, systemID); sizeof(systemID) +
_pack(__p, firmwareVersion, 3); (sizeof(firmwareVersion[0]) * 3) + 0);
_encodeBuf.header.length = 5; _emit(systemType);
_encodeBuf.header.messageID = MSG_VERSION; _emit(systemID);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(firmwareVersion, 3);
_sendMessage(); _endMessage();
}; };
/// Unpack a MSG_VERSION message /// Unpack a MSG_VERSION message
@ -437,12 +445,10 @@ inline void
send_msg_command_request( send_msg_command_request(
const uint16_t UNSPECIFIED) const uint16_t UNSPECIFIED)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_COMMAND_REQUEST,
_pack(__p, UNSPECIFIED); sizeof(UNSPECIFIED) + 0);
_encodeBuf.header.length = 2; _emit(UNSPECIFIED);
_encodeBuf.header.messageID = MSG_COMMAND_REQUEST; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_COMMAND_REQUEST message /// Unpack a MSG_COMMAND_REQUEST message
@ -483,19 +489,24 @@ send_msg_command_upload(
const int32_t p3, const int32_t p3,
const int32_t p4) const int32_t p4)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_COMMAND_UPLOAD,
_pack(__p, action); sizeof(action) +
_pack(__p, itemNumber); sizeof(itemNumber) +
_pack(__p, listLength); sizeof(listLength) +
_pack(__p, commandID); sizeof(commandID) +
_pack(__p, p1); sizeof(p1) +
_pack(__p, p2); sizeof(p2) +
_pack(__p, p3); sizeof(p3) +
_pack(__p, p4); sizeof(p4) + 0);
_encodeBuf.header.length = 19; _emit(action);
_encodeBuf.header.messageID = MSG_COMMAND_UPLOAD; _emit(itemNumber);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(listLength);
_sendMessage(); _emit(commandID);
_emit(p1);
_emit(p2);
_emit(p3);
_emit(p4);
_endMessage();
}; };
/// Unpack a MSG_COMMAND_UPLOAD message /// Unpack a MSG_COMMAND_UPLOAD message
@ -548,18 +559,22 @@ send_msg_command_list(
const int32_t p3, const int32_t p3,
const int32_t p4) const int32_t p4)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_COMMAND_LIST,
_pack(__p, itemNumber); sizeof(itemNumber) +
_pack(__p, listLength); sizeof(listLength) +
_pack(__p, commandID); sizeof(commandID) +
_pack(__p, p1); sizeof(p1) +
_pack(__p, p2); sizeof(p2) +
_pack(__p, p3); sizeof(p3) +
_pack(__p, p4); sizeof(p4) + 0);
_encodeBuf.header.length = 18; _emit(itemNumber);
_encodeBuf.header.messageID = MSG_COMMAND_LIST; _emit(listLength);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(commandID);
_sendMessage(); _emit(p1);
_emit(p2);
_emit(p3);
_emit(p4);
_endMessage();
}; };
/// Unpack a MSG_COMMAND_LIST message /// Unpack a MSG_COMMAND_LIST message
@ -598,12 +613,10 @@ inline void
send_msg_command_mode_change( send_msg_command_mode_change(
const uint16_t UNSPECIFIED) const uint16_t UNSPECIFIED)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_COMMAND_MODE_CHANGE,
_pack(__p, UNSPECIFIED); sizeof(UNSPECIFIED) + 0);
_encodeBuf.header.length = 2; _emit(UNSPECIFIED);
_encodeBuf.header.messageID = MSG_COMMAND_MODE_CHANGE; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_COMMAND_MODE_CHANGE message /// Unpack a MSG_COMMAND_MODE_CHANGE message
@ -632,13 +645,12 @@ send_msg_value_request(
const uint8_t valueID, const uint8_t valueID,
const uint8_t broadcast) const uint8_t broadcast)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_VALUE_REQUEST,
_pack(__p, valueID); sizeof(valueID) +
_pack(__p, broadcast); sizeof(broadcast) + 0);
_encodeBuf.header.length = 2; _emit(valueID);
_encodeBuf.header.messageID = MSG_VALUE_REQUEST; _emit(broadcast);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _endMessage();
_sendMessage();
}; };
/// Unpack a MSG_VALUE_REQUEST message /// Unpack a MSG_VALUE_REQUEST message
@ -669,13 +681,12 @@ send_msg_value_set(
const uint8_t valueID, const uint8_t valueID,
const uint32_t value) const uint32_t value)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_VALUE_SET,
_pack(__p, valueID); sizeof(valueID) +
_pack(__p, value); sizeof(value) + 0);
_encodeBuf.header.length = 5; _emit(valueID);
_encodeBuf.header.messageID = MSG_VALUE_SET; _emit(value);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _endMessage();
_sendMessage();
}; };
/// Unpack a MSG_VALUE_SET message /// Unpack a MSG_VALUE_SET message
@ -706,13 +717,12 @@ send_msg_value(
const uint8_t valueID, const uint8_t valueID,
const uint32_t value) const uint32_t value)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_VALUE,
_pack(__p, valueID); sizeof(valueID) +
_pack(__p, value); sizeof(value) + 0);
_encodeBuf.header.length = 5; _emit(valueID);
_encodeBuf.header.messageID = MSG_VALUE; _emit(value);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _endMessage();
_sendMessage();
}; };
/// Unpack a MSG_VALUE message /// Unpack a MSG_VALUE message
@ -741,12 +751,10 @@ inline void
send_msg_pid_request( send_msg_pid_request(
const uint8_t pidSet) const uint8_t pidSet)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_PID_REQUEST,
_pack(__p, pidSet); sizeof(pidSet) + 0);
_encodeBuf.header.length = 1; _emit(pidSet);
_encodeBuf.header.messageID = MSG_PID_REQUEST; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_PID_REQUEST message /// Unpack a MSG_PID_REQUEST message
@ -781,16 +789,18 @@ send_msg_pid_set(
const int32_t d, const int32_t d,
const int16_t integratorMax) const int16_t integratorMax)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_PID_SET,
_pack(__p, pidSet); sizeof(pidSet) +
_pack(__p, p); sizeof(p) +
_pack(__p, i); sizeof(i) +
_pack(__p, d); sizeof(d) +
_pack(__p, integratorMax); sizeof(integratorMax) + 0);
_encodeBuf.header.length = 15; _emit(pidSet);
_encodeBuf.header.messageID = MSG_PID_SET; _emit(p);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(i);
_sendMessage(); _emit(d);
_emit(integratorMax);
_endMessage();
}; };
/// Unpack a MSG_PID_SET message /// Unpack a MSG_PID_SET message
@ -833,16 +843,18 @@ send_msg_pid(
const int32_t d, const int32_t d,
const int16_t integratorMax) const int16_t integratorMax)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_PID,
_pack(__p, pidSet); sizeof(pidSet) +
_pack(__p, p); sizeof(p) +
_pack(__p, i); sizeof(i) +
_pack(__p, d); sizeof(d) +
_pack(__p, integratorMax); sizeof(integratorMax) + 0);
_encodeBuf.header.length = 15; _emit(pidSet);
_encodeBuf.header.messageID = MSG_PID; _emit(p);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(i);
_sendMessage(); _emit(d);
_emit(integratorMax);
_endMessage();
}; };
/// Unpack a MSG_PID message /// Unpack a MSG_PID message
@ -877,12 +889,10 @@ inline void
send_msg_trim_startup( send_msg_trim_startup(
const uint16_t (&value)[8]) const uint16_t (&value)[8])
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_TRIM_STARTUP,
_pack(__p, value, 8); (sizeof(value[0]) * 8) + 0);
_encodeBuf.header.length = 16; _emit(value, 8);
_encodeBuf.header.messageID = MSG_TRIM_STARTUP; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_TRIM_STARTUP message /// Unpack a MSG_TRIM_STARTUP message
@ -909,12 +919,10 @@ inline void
send_msg_trim_min( send_msg_trim_min(
const uint16_t (&value)[8]) const uint16_t (&value)[8])
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_TRIM_MIN,
_pack(__p, value, 8); (sizeof(value[0]) * 8) + 0);
_encodeBuf.header.length = 16; _emit(value, 8);
_encodeBuf.header.messageID = MSG_TRIM_MIN; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_TRIM_MIN message /// Unpack a MSG_TRIM_MIN message
@ -941,12 +949,10 @@ inline void
send_msg_trim_max( send_msg_trim_max(
const uint16_t (&value)[8]) const uint16_t (&value)[8])
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_TRIM_MAX,
_pack(__p, value, 8); (sizeof(value[0]) * 8) + 0);
_encodeBuf.header.length = 16; _emit(value, 8);
_encodeBuf.header.messageID = MSG_TRIM_MAX; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_TRIM_MAX message /// Unpack a MSG_TRIM_MAX message
@ -973,12 +979,10 @@ inline void
send_msg_radio_out( send_msg_radio_out(
const uint16_t (&value)[8]) const uint16_t (&value)[8])
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_RADIO_OUT,
_pack(__p, value, 8); (sizeof(value[0]) * 8) + 0);
_encodeBuf.header.length = 16; _emit(value, 8);
_encodeBuf.header.messageID = MSG_RADIO_OUT; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_RADIO_OUT message /// Unpack a MSG_RADIO_OUT message
@ -1005,12 +1009,10 @@ inline void
send_msg_sensor( send_msg_sensor(
const uint16_t UNSPECIFIED) const uint16_t UNSPECIFIED)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_SENSOR,
_pack(__p, UNSPECIFIED); sizeof(UNSPECIFIED) + 0);
_encodeBuf.header.length = 2; _emit(UNSPECIFIED);
_encodeBuf.header.messageID = MSG_SENSOR; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_SENSOR message /// Unpack a MSG_SENSOR message
@ -1037,12 +1039,10 @@ inline void
send_msg_servo_out( send_msg_servo_out(
const int16_t (&value)[8]) const int16_t (&value)[8])
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_SERVO_OUT,
_pack(__p, value, 8); (sizeof(value[0]) * 8) + 0);
_encodeBuf.header.length = 16; _emit(value, 8);
_encodeBuf.header.messageID = MSG_SERVO_OUT; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_SERVO_OUT message /// Unpack a MSG_SERVO_OUT message
@ -1069,12 +1069,10 @@ inline void
send_msg_pin_request( send_msg_pin_request(
const uint16_t UNSPECIFIED) const uint16_t UNSPECIFIED)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_PIN_REQUEST,
_pack(__p, UNSPECIFIED); sizeof(UNSPECIFIED) + 0);
_encodeBuf.header.length = 2; _emit(UNSPECIFIED);
_encodeBuf.header.messageID = MSG_PIN_REQUEST; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_PIN_REQUEST message /// Unpack a MSG_PIN_REQUEST message
@ -1101,12 +1099,10 @@ inline void
send_msg_pin_set( send_msg_pin_set(
const uint16_t UNSPECIFIED) const uint16_t UNSPECIFIED)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_PIN_SET,
_pack(__p, UNSPECIFIED); sizeof(UNSPECIFIED) + 0);
_encodeBuf.header.length = 2; _emit(UNSPECIFIED);
_encodeBuf.header.messageID = MSG_PIN_SET; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_PIN_SET message /// Unpack a MSG_PIN_SET message
@ -1133,12 +1129,10 @@ inline void
send_msg_dataflash_request( send_msg_dataflash_request(
const uint16_t UNSPECIFIED) const uint16_t UNSPECIFIED)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_DATAFLASH_REQUEST,
_pack(__p, UNSPECIFIED); sizeof(UNSPECIFIED) + 0);
_encodeBuf.header.length = 2; _emit(UNSPECIFIED);
_encodeBuf.header.messageID = MSG_DATAFLASH_REQUEST; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_DATAFLASH_REQUEST message /// Unpack a MSG_DATAFLASH_REQUEST message
@ -1165,12 +1159,10 @@ inline void
send_msg_dataflash_set( send_msg_dataflash_set(
const uint16_t UNSPECIFIED) const uint16_t UNSPECIFIED)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_DATAFLASH_SET,
_pack(__p, UNSPECIFIED); sizeof(UNSPECIFIED) + 0);
_encodeBuf.header.length = 2; _emit(UNSPECIFIED);
_encodeBuf.header.messageID = MSG_DATAFLASH_SET; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_DATAFLASH_SET message /// Unpack a MSG_DATAFLASH_SET message
@ -1197,12 +1189,10 @@ inline void
send_msg_eeprom_request( send_msg_eeprom_request(
const uint16_t UNSPECIFIED) const uint16_t UNSPECIFIED)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_EEPROM_REQUEST,
_pack(__p, UNSPECIFIED); sizeof(UNSPECIFIED) + 0);
_encodeBuf.header.length = 2; _emit(UNSPECIFIED);
_encodeBuf.header.messageID = MSG_EEPROM_REQUEST; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_EEPROM_REQUEST message /// Unpack a MSG_EEPROM_REQUEST message
@ -1229,12 +1219,10 @@ inline void
send_msg_eeprom_set( send_msg_eeprom_set(
const uint16_t UNSPECIFIED) const uint16_t UNSPECIFIED)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_EEPROM_SET,
_pack(__p, UNSPECIFIED); sizeof(UNSPECIFIED) + 0);
_encodeBuf.header.length = 2; _emit(UNSPECIFIED);
_encodeBuf.header.messageID = MSG_EEPROM_SET; _endMessage();
_encodeBuf.header.messageVersion = MSG_VERSION_1;
_sendMessage();
}; };
/// Unpack a MSG_EEPROM_SET message /// Unpack a MSG_EEPROM_SET message
@ -1267,15 +1255,16 @@ send_msg_position_correct(
const int16_t altError, const int16_t altError,
const int16_t groundSpeedError) const int16_t groundSpeedError)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_POSITION_CORRECT,
_pack(__p, latError); sizeof(latError) +
_pack(__p, lonError); sizeof(lonError) +
_pack(__p, altError); sizeof(altError) +
_pack(__p, groundSpeedError); sizeof(groundSpeedError) + 0);
_encodeBuf.header.length = 8; _emit(latError);
_encodeBuf.header.messageID = MSG_POSITION_CORRECT; _emit(lonError);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(altError);
_sendMessage(); _emit(groundSpeedError);
_endMessage();
}; };
/// Unpack a MSG_POSITION_CORRECT message /// Unpack a MSG_POSITION_CORRECT message
@ -1312,14 +1301,14 @@ send_msg_attitude_correct(
const int16_t pitchError, const int16_t pitchError,
const int16_t yawError) const int16_t yawError)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_ATTITUDE_CORRECT,
_pack(__p, rollError); sizeof(rollError) +
_pack(__p, pitchError); sizeof(pitchError) +
_pack(__p, yawError); sizeof(yawError) + 0);
_encodeBuf.header.length = 6; _emit(rollError);
_encodeBuf.header.messageID = MSG_ATTITUDE_CORRECT; _emit(pitchError);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(yawError);
_sendMessage(); _endMessage();
}; };
/// Unpack a MSG_ATTITUDE_CORRECT message /// Unpack a MSG_ATTITUDE_CORRECT message
@ -1356,15 +1345,16 @@ send_msg_position_set(
const int32_t altitude, const int32_t altitude,
const uint16_t heading) const uint16_t heading)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_POSITION_SET,
_pack(__p, latitude); sizeof(latitude) +
_pack(__p, longitude); sizeof(longitude) +
_pack(__p, altitude); sizeof(altitude) +
_pack(__p, heading); sizeof(heading) + 0);
_encodeBuf.header.length = 14; _emit(latitude);
_encodeBuf.header.messageID = MSG_POSITION_SET; _emit(longitude);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(altitude);
_sendMessage(); _emit(heading);
_endMessage();
}; };
/// Unpack a MSG_POSITION_SET message /// Unpack a MSG_POSITION_SET message
@ -1401,14 +1391,14 @@ send_msg_attitude_set(
const int16_t pitch, const int16_t pitch,
const uint16_t yaw) const uint16_t yaw)
{ {
uint8_t *__p = &_encodeBuf.payload[0]; _startMessage(MSG_ATTITUDE_SET,
_pack(__p, roll); sizeof(roll) +
_pack(__p, pitch); sizeof(pitch) +
_pack(__p, yaw); sizeof(yaw) + 0);
_encodeBuf.header.length = 6; _emit(roll);
_encodeBuf.header.messageID = MSG_ATTITUDE_SET; _emit(pitch);
_encodeBuf.header.messageVersion = MSG_VERSION_1; _emit(yaw);
_sendMessage(); _endMessage();
}; };
/// Unpack a MSG_ATTITUDE_SET message /// Unpack a MSG_ATTITUDE_SET message
@ -1428,42 +1418,84 @@ unpack_msg_attitude_set(
////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////
/// Message ID values /// Message ID values
enum MessageID { enum MessageID {
MSG_ACKNOWLEDGE = 0x0,
MSG_HEARTBEAT = 0x1,
MSG_ATTITUDE = 0x2,
MSG_LOCATION = 0x3,
MSG_PRESSURE = 0x4,
MSG_STATUS_TEXT = 0x5,
MSG_COMMAND_REQUEST = 0x20,
MSG_PERF_REPORT = 0x6,
MSG_COMMAND_UPLOAD = 0x21,
MSG_VERSION_REQUEST = 0x7,
MSG_COMMAND_LIST = 0x22,
MSG_VERSION = 0x8,
MSG_VALUE_REQUEST = 0x30,
MSG_COMMAND_MODE_CHANGE = 0x23,
MSG_VALUE_SET = 0x31,
MSG_VALUE = 0x32,
MSG_PID_REQUEST = 0x40,
MSG_PID_SET = 0x41,
MSG_PID = 0x42, MSG_PID = 0x42,
MSG_TRIM_STARTUP = 0x50,
MSG_TRIM_MIN = 0x51,
MSG_TRIM_MAX = 0x52,
MSG_SENSOR = 0x60,
MSG_RADIO_OUT = 0x53,
MSG_EEPROM_REQUEST = 0xa0,
MSG_EEPROM_SET = 0xa1,
MSG_SERVO_OUT = 0x70,
MSG_POSITION_CORRECT = 0xb0,
MSG_ATTITUDE_CORRECT = 0xb1,
MSG_POSITION_SET = 0xb2,
MSG_PIN_REQUEST = 0x80,
MSG_ATTITUDE_SET = 0xb3,
MSG_PIN_SET = 0x81,
MSG_DATAFLASH_REQUEST = 0x90, MSG_DATAFLASH_REQUEST = 0x90,
MSG_DATAFLASH_SET = 0x91, MSG_DATAFLASH_SET = 0x91,
MSG_SENSOR = 0x60,
MSG_VALUE_REQUEST = 0x30,
MSG_VALUE_SET = 0x31,
MSG_VALUE = 0x32,
MSG_PIN_REQUEST = 0x80,
MSG_PIN_SET = 0x81,
MSG_POSITION_CORRECT = 0xb0,
MSG_ACKNOWLEDGE = 0x0,
MSG_ATTITUDE_CORRECT = 0xb1,
MSG_HEARTBEAT = 0x1,
MSG_POSITION_SET = 0xb2,
MSG_ATTITUDE = 0x2,
MSG_ATTITUDE_SET = 0xb3,
MSG_LOCATION = 0x3,
MSG_PRESSURE = 0x4,
MSG_TRIM_STARTUP = 0x50,
MSG_STATUS_TEXT = 0x5,
MSG_TRIM_MIN = 0x51,
MSG_PERF_REPORT = 0x6,
MSG_TRIM_MAX = 0x52,
MSG_VERSION_REQUEST = 0x7,
MSG_RADIO_OUT = 0x53,
MSG_VERSION = 0x8,
MSG_COMMAND_REQUEST = 0x20,
MSG_COMMAND_UPLOAD = 0x21,
MSG_COMMAND_LIST = 0x22,
MSG_COMMAND_MODE_CHANGE = 0x23,
MSG_SERVO_OUT = 0x70,
MSG_EEPROM_REQUEST = 0xa0,
MSG_EEPROM_SET = 0xa1,
MSG_PID_REQUEST = 0x40,
MSG_PID_SET = 0x41,
MSG_ANY = 0xfe, MSG_ANY = 0xfe,
MSG_NULL = 0xff MSG_NULL = 0xff
}; };
//////////////////////////////////////////////////////////////////////
/// Message buffer sizing
union _binCommBufferSizer {
struct msg_acknowledge msg_acknowledge;
struct msg_status_text msg_status_text;
struct msg_heartbeat msg_heartbeat;
struct msg_attitude msg_attitude;
struct msg_location msg_location;
struct msg_pressure msg_pressure;
struct msg_perf_report msg_perf_report;
struct msg_version_request msg_version_request;
struct msg_version msg_version;
struct msg_command_request msg_command_request;
struct msg_command_upload msg_command_upload;
struct msg_command_list msg_command_list;
struct msg_command_mode_change msg_command_mode_change;
struct msg_value_request msg_value_request;
struct msg_value_set msg_value_set;
struct msg_value msg_value;
struct msg_pid_request msg_pid_request;
struct msg_pid_set msg_pid_set;
struct msg_pid msg_pid;
struct msg_trim_startup msg_trim_startup;
struct msg_trim_min msg_trim_min;
struct msg_trim_max msg_trim_max;
struct msg_radio_out msg_radio_out;
struct msg_sensor msg_sensor;
struct msg_servo_out msg_servo_out;
struct msg_pin_request msg_pin_request;
struct msg_pin_set msg_pin_set;
struct msg_dataflash_request msg_dataflash_request;
struct msg_dataflash_set msg_dataflash_set;
struct msg_eeprom_request msg_eeprom_request;
struct msg_eeprom_set msg_eeprom_set;
struct msg_position_correct msg_position_correct;
struct msg_attitude_correct msg_attitude_correct;
struct msg_position_set msg_position_set;
struct msg_attitude_set msg_attitude_set;
};
#define BINCOMM_MAX_MESSAGE_SIZE sizeof(union _binCommBufferSizer)
#pragma pack(pop) #pragma pack(pop)

50
libraries/APM_BinComm/protocol/protogen.awk
View File

@ -11,6 +11,7 @@ BEGIN {
printf("#pragma pack(1)\n"); printf("#pragma pack(1)\n");
currentMessage = "" currentMessage = ""
structureCount = 0
} }
END { END {
@ -33,6 +34,15 @@ END {
printf("\tMSG_NULL = 0xff\n") printf("\tMSG_NULL = 0xff\n")
printf("};\n") printf("};\n")
printf("\n//////////////////////////////////////////////////////////////////////\n")
printf("/// Message buffer sizing\n")
printf("union _binCommBufferSizer {\n");
for (i = 0; i < structureCount; i++) {
printf("\tstruct %s %s;\n", structs[i], structs[i]);
}
printf("};\n");
printf("#define BINCOMM_MAX_MESSAGE_SIZE sizeof(union _binCommBufferSizer)\n\n");
printf("#pragma pack(pop)\n") printf("#pragma pack(pop)\n")
} }
@ -57,9 +67,15 @@ function EMIT_MESSAGE(payloadSize)
printf(";\n") printf(";\n")
} }
printf("};\n\n") printf("};\n\n")
#
# Record the structure name for later use sizing the receive buffer.
#
structs[structureCount] = tolower(currentMessage);
structureCount++;
# #
# emit a routine to pack the message payload from a set of variables and send it # emit a routine to emit the message payload from a set of variables
# #
printf("/// Send a %s message\n", currentMessage) printf("/// Send a %s message\n", currentMessage)
printf("inline void\nsend_%s(\n", tolower(currentMessage)) printf("inline void\nsend_%s(\n", tolower(currentMessage))
@ -73,21 +89,23 @@ function EMIT_MESSAGE(payloadSize)
printf(",\n"); printf(",\n");
} }
printf(")\n{\n") printf(")\n{\n")
printf("\tuint8_t *__p = &_encodeBuf.payload[0];\n") printf("\t_startMessage(%s,", currentMessage);
payloadSize = 0;
for (i = 0; i < fieldCount; i++) { for (i = 0; i < fieldCount; i++) {
if (counts[i]) { if (counts[i]) {
printf("\t_pack(__p, %s, %s);\n", names[i], counts[i]) printf("\n\t\t(sizeof(%s[0]) * %d) +", names[i], counts[i]);
payloadSize += sizes[i] * counts[i]
} else { } else {
printf("\t_pack(__p, %s);\n", names[i]) printf("\n\t\tsizeof(%s) +", names[i]);
payloadSize += sizes[i]
} }
} }
printf("\t_encodeBuf.header.length = %s;\n", payloadSize) printf(" 0);\n");
printf("\t_encodeBuf.header.messageID = %s;\n", currentMessage) for (i = 0; i < fieldCount; i++) {
printf("\t_encodeBuf.header.messageVersion = MSG_VERSION_1;\n") if (counts[i]) {
printf("\t_sendMessage();\n") printf("\t_emit(%s, %s);\n", names[i], counts[i])
} else {
printf("\t_emit(%s);\n", names[i])
}
}
printf("\t_endMessage();\n")
printf("};\n\n") printf("};\n\n")
# #
@ -109,10 +127,8 @@ function EMIT_MESSAGE(payloadSize)
for (i = 0; i < fieldCount; i++) { for (i = 0; i < fieldCount; i++) {
if (counts[i]) { if (counts[i]) {
printf("\t_unpack(__p, %s, %s);\n", names[i], counts[i]) printf("\t_unpack(__p, %s, %s);\n", names[i], counts[i])
payloadSize += sizes[i] * counts[i]
} else { } else {
printf("\t_unpack(__p, %s);\n", names[i]) printf("\t_unpack(__p, %s);\n", names[i])
payloadSize += sizes[i]
} }
} }
printf("};\n") printf("};\n")
@ -144,7 +160,6 @@ $1=="message" {
fieldCount = 0 fieldCount = 0
delete types delete types
delete names delete names
delete sizes
delete counts delete counts
next next
@ -159,13 +174,6 @@ NF >= 2 {
types[fieldCount] = $1 types[fieldCount] = $1
names[fieldCount] = $2 names[fieldCount] = $2
# guess the field size, note that we only support <inttypes.h> and "char"
sizes[fieldCount] = 1
if ($1 ~ ".*16.*")
sizes[fieldCount] = 2
if ($1 ~ ".*32.*")
sizes[fieldCount] = 4
# if an array size was supplied, save it # if an array size was supplied, save it
if (NF >= 3) { if (NF >= 3) {
counts[fieldCount] = $3 counts[fieldCount] = $3

4
libraries/APM_BinComm/test/Makefile
View File

@ -4,11 +4,11 @@ SRCS := test.cpp ../APM_BinComm.cpp
OBJS := $(SRCS:.cpp=.o) OBJS := $(SRCS:.cpp=.o)
BinCommTest: $(OBJS) BinCommTest: $(OBJS)
c++ -g -o $@ $(OBJS) /Volumes/Data/Users/msmith/llvm/bin/clang++ -g -o $@ $(OBJS)
.cpp.o: .cpp.o:
@echo C++ $< -> $@ @echo C++ $< -> $@
c++ -g -c -I. -o $@ $< /Volumes/Data/Users/msmith/llvm/bin/clang++ -g -c -I. -o $@ $<
clean: clean:
rm $(PROG) $(OBJS) rm $(PROG) $(OBJS)

2
libraries/APM_BinComm/test/test.cpp
View File

@ -8,6 +8,7 @@
#include <fcntl.h> #include <fcntl.h>
#include <termios.h> #include <termios.h>
#include <stdio.h> #include <stdio.h>
#include <string.h>
#include "WProgram.h" #include "WProgram.h"
@ -51,6 +52,7 @@ Stream::read(void)
switch(::read(port_fd, &c, 1)) { switch(::read(port_fd, &c, 1)) {
case 1: case 1:
printf("0x%02x\n", c);
return c; return c;
case 0: case 0:
errx(1, "device disappeared"); errx(1, "device disappeared");