mirror of https://github.com/ArduPilot/ardupilot
Add init method so that the comms stream can be set after construction.
Template the pack/unpack functions, make the code a little cleaner. Fix a few message definitions. git-svn-id: https://arducopter.googlecode.com/svn/trunk@770 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com
parent
eb5f4d7d32
commit
1049bf2153
|
|
@ -47,10 +47,16 @@
|
|||
BinComm::BinComm(const BinComm::MessageHandler *handlerTable,
|
||||
Stream *interface) :
|
||||
_handlerTable(handlerTable),
|
||||
_interface(interface)
|
||||
{
|
||||
init(interface);
|
||||
};
|
||||
|
||||
void
|
||||
BinComm::init(Stream *interface)
|
||||
{
|
||||
_interface = interface;
|
||||
}
|
||||
|
||||
void
|
||||
BinComm::_sendMessage(void)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -71,7 +71,18 @@ public:
|
|||
/// for telemetry communications.
|
||||
///
|
||||
BinComm(const MessageHandler *handlerTable,
|
||||
Stream *interface);
|
||||
Stream *interface = NULL);
|
||||
|
||||
///
|
||||
/// Optional initialiser.
|
||||
///
|
||||
/// If the interface stream isn't known at construction time, it
|
||||
/// can be set here instead.
|
||||
///
|
||||
/// @param interface The stream that will be used for telemetry
|
||||
/// communications.
|
||||
///
|
||||
void init(Stream *interface);
|
||||
|
||||
private:
|
||||
/// OTA message header
|
||||
|
|
@ -101,25 +112,58 @@ private:
|
|||
/// @name Message pack/unpack utility functions
|
||||
///
|
||||
//@{
|
||||
inline void _pack(uint8_t *&ptr, const uint8_t x) { *(uint8_t *)ptr = x; ptr += sizeof(x); }
|
||||
inline void _pack(uint8_t *&ptr, const uint16_t x) { *(uint16_t *)ptr = x; ptr += sizeof(x); }
|
||||
inline void _pack(uint8_t *&ptr, const int16_t x) { *(int16_t *)ptr = x; ptr += sizeof(x); }
|
||||
inline void _pack(uint8_t *&ptr, const uint32_t x) { *(uint32_t *)ptr = x; ptr += sizeof(x); }
|
||||
inline void _pack(uint8_t *&ptr, const int32_t x) { *(int32_t *)ptr = x; ptr += sizeof(x); }
|
||||
|
||||
inline void _pack(uint8_t *&ptr, const char *msg, uint8_t size) { strlcpy((char *)ptr, msg, size); ptr += size; }
|
||||
inline void _pack(uint8_t *&ptr, const uint8_t *values, uint8_t count) { memcpy(ptr, values, count); ptr += count; }
|
||||
inline void _pack(uint8_t *&ptr, const uint16_t *values, uint8_t count) { memcpy(ptr, values, count * 2); ptr += count * 2; }
|
||||
|
||||
inline void _unpack(uint8_t *&ptr, uint8_t &x) { x = *(uint8_t *)ptr; ptr += sizeof(x); }
|
||||
inline void _unpack(uint8_t *&ptr, uint16_t &x) { x = *(uint16_t *)ptr; ptr += sizeof(x); }
|
||||
inline void _unpack(uint8_t *&ptr, int16_t &x) { x = *(int16_t *)ptr; ptr += sizeof(x); }
|
||||
inline void _unpack(uint8_t *&ptr, uint32_t &x) { x = *(uint32_t *)ptr; ptr += sizeof(x); }
|
||||
inline void _unpack(uint8_t *&ptr, int32_t &x) { x = *(int32_t *)ptr; ptr += sizeof(x); }
|
||||
/// Pack any scalar type
|
||||
///
|
||||
/// @param ptr Buffer pointer.
|
||||
/// @param x Value to pack.
|
||||
///
|
||||
template <typename T> inline void _pack(uint8_t *&ptr, const T x) { *(T *)ptr = x; ptr += sizeof(T); }
|
||||
|
||||
inline void _unpack(uint8_t *&ptr, char *msg, uint8_t size) { strlcpy(msg, (char *)ptr, size); ptr += size; }
|
||||
inline void _unpack(uint8_t *&ptr, uint8_t *values, uint8_t count) { memcpy(values, ptr, count); ptr += count; }
|
||||
inline void _unpack(uint8_t *&ptr, uint16_t *values, uint8_t count) { memcpy(values, ptr, count * 2); ptr += count * 2; }
|
||||
/// Pack an array of any scalar type
|
||||
///
|
||||
/// @param ptr Buffer pointer.
|
||||
/// @param x Array to pack.
|
||||
/// @param count Array size.
|
||||
///
|
||||
template <typename T> inline void _pack(uint8_t *&ptr, const T *values, uint8_t count) {
|
||||
memcpy(ptr, values, count * sizeof(T));
|
||||
ptr += count * sizeof(T);
|
||||
}
|
||||
|
||||
/// Pack a string into a fixed-size buffer.
|
||||
///
|
||||
/// @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) { strlcpy((char *)ptr, msg, size); ptr += size; }
|
||||
|
||||
/// Unpack any scalar type
|
||||
///
|
||||
/// @param buf Buffer pointer.
|
||||
/// @param x Unpacked result.
|
||||
///
|
||||
template <typename T> inline void _unpack(uint8_t *&ptr, T &x) { x = *(T *)ptr; ptr += sizeof(T); }
|
||||
|
||||
/// Unpack an array of any scalar type.
|
||||
///
|
||||
/// @param buf Buffer pointer.
|
||||
/// @param values Array to receive the unpacked values.
|
||||
///
|
||||
template <typename T> inline void _unpack(uint8_t *&ptr, T *values, uint8_t count) {
|
||||
memcpy(values, ptr, count * sizeof(T));
|
||||
ptr += count * sizeof(T);
|
||||
}
|
||||
|
||||
/// Unpack a string from a fixed-size buffer.
|
||||
///
|
||||
/// @param ptr Buffer pointer.
|
||||
/// @param msg Pointer to the result buffer.
|
||||
/// @param size The size of the buffer.
|
||||
///
|
||||
inline void _unpack(uint8_t *&ptr, char *msg, uint8_t size) { strlcpy(msg, (char *)ptr, size); ptr += size; }
|
||||
//@}
|
||||
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -62,7 +62,8 @@
|
|||
#
|
||||
message 0x00 MSG_ACKNOWLEDGE
|
||||
uint8_t msgID
|
||||
uint16_t msgSum
|
||||
uint8_t sum1
|
||||
uint8_t sum2
|
||||
|
||||
#
|
||||
# System heartbeat
|
||||
|
|
@ -112,9 +113,12 @@ message 0x05 MSG_STATUS_TEXT
|
|||
message 0x06 MSG_PERF_REPORT
|
||||
uint32_t interval
|
||||
uint16_t mainLoopCycles
|
||||
uint8_t mainLoopTime
|
||||
uint8_t mainLoopCycleTime
|
||||
uint8_t gyroSaturationCount
|
||||
uint8_t adcConstraintCount
|
||||
uint8_t renormSqrtCount
|
||||
uint8_t renormBlowupCount
|
||||
uint8_t gpsFixCount
|
||||
uint16_t imuHealth
|
||||
uint16_t gcsMessageCount
|
||||
|
||||
|
|
@ -198,14 +202,20 @@ message 0x42 MSG_PID
|
|||
#
|
||||
# Radio settings and values
|
||||
#
|
||||
# The trim/radio values should be unsigned, but
|
||||
# currently aren't.
|
||||
#
|
||||
message 0x50 MSG_TRIM_STARTUP
|
||||
uint16_t value 8
|
||||
int value 8
|
||||
# uint16_t value 8
|
||||
|
||||
message 0x51 MSG_TRIM_MIN
|
||||
uint16_t value 8
|
||||
int value 8
|
||||
# uint16_t value 8
|
||||
|
||||
message 0x52 MSG_TRIM_MAX
|
||||
uint16_t value 8
|
||||
int value 8
|
||||
# uint16_t value 8
|
||||
|
||||
message 0x53 MSG_SERVOS
|
||||
int16_t ch1
|
||||
|
|
|
|||
|
|
@ -11,18 +11,21 @@
|
|||
/// Structure describing the payload section of the MSG_ACKNOWLEDGE message
|
||||
struct msg_acknowledge {
|
||||
uint8_t msgID;
|
||||
uint16_t msgSum;
|
||||
uint8_t sum1;
|
||||
uint8_t sum2;
|
||||
};
|
||||
|
||||
/// Send a MSG_ACKNOWLEDGE message
|
||||
inline void
|
||||
send_msg_acknowledge(
|
||||
const uint8_t msgID,
|
||||
const uint16_t msgSum)
|
||||
const uint8_t sum1,
|
||||
const uint8_t sum2)
|
||||
{
|
||||
uint8_t *__p = &_encodeBuf.payload[0];
|
||||
_pack(__p, msgID);
|
||||
_pack(__p, msgSum);
|
||||
_pack(__p, sum1);
|
||||
_pack(__p, sum2);
|
||||
_encodeBuf.header.length = 3;
|
||||
_encodeBuf.header.messageID = MSG_ACKNOWLEDGE;
|
||||
_encodeBuf.header.messageVersion = MSG_VERSION_1;
|
||||
|
|
@ -33,11 +36,13 @@ send_msg_acknowledge(
|
|||
inline void
|
||||
unpack_msg_acknowledge(
|
||||
uint8_t &msgID,
|
||||
uint16_t &msgSum)
|
||||
uint8_t &sum1,
|
||||
uint8_t &sum2)
|
||||
{
|
||||
uint8_t *__p = &_decodeBuf.payload[0];
|
||||
_unpack(__p, msgID);
|
||||
_unpack(__p, msgSum);
|
||||
_unpack(__p, sum1);
|
||||
_unpack(__p, sum2);
|
||||
};
|
||||
//@}
|
||||
|
||||
|
|
@ -269,9 +274,12 @@ unpack_msg_status_text(
|
|||
struct msg_perf_report {
|
||||
uint32_t interval;
|
||||
uint16_t mainLoopCycles;
|
||||
uint8_t mainLoopTime;
|
||||
uint8_t mainLoopCycleTime;
|
||||
uint8_t gyroSaturationCount;
|
||||
uint8_t adcConstraintCount;
|
||||
uint8_t renormSqrtCount;
|
||||
uint8_t renormBlowupCount;
|
||||
uint8_t gpsFixCount;
|
||||
uint16_t imuHealth;
|
||||
uint16_t gcsMessageCount;
|
||||
};
|
||||
|
|
@ -281,21 +289,27 @@ inline void
|
|||
send_msg_perf_report(
|
||||
const uint32_t interval,
|
||||
const uint16_t mainLoopCycles,
|
||||
const uint8_t mainLoopTime,
|
||||
const uint8_t mainLoopCycleTime,
|
||||
const uint8_t gyroSaturationCount,
|
||||
const uint8_t adcConstraintCount,
|
||||
const uint8_t renormSqrtCount,
|
||||
const uint8_t renormBlowupCount,
|
||||
const uint8_t gpsFixCount,
|
||||
const uint16_t imuHealth,
|
||||
const uint16_t gcsMessageCount)
|
||||
{
|
||||
uint8_t *__p = &_encodeBuf.payload[0];
|
||||
_pack(__p, interval);
|
||||
_pack(__p, mainLoopCycles);
|
||||
_pack(__p, mainLoopTime);
|
||||
_pack(__p, mainLoopCycleTime);
|
||||
_pack(__p, gyroSaturationCount);
|
||||
_pack(__p, adcConstraintCount);
|
||||
_pack(__p, renormSqrtCount);
|
||||
_pack(__p, renormBlowupCount);
|
||||
_pack(__p, gpsFixCount);
|
||||
_pack(__p, imuHealth);
|
||||
_pack(__p, gcsMessageCount);
|
||||
_encodeBuf.header.length = 13;
|
||||
_encodeBuf.header.length = 16;
|
||||
_encodeBuf.header.messageID = MSG_PERF_REPORT;
|
||||
_encodeBuf.header.messageVersion = MSG_VERSION_1;
|
||||
_sendMessage();
|
||||
|
|
@ -306,18 +320,24 @@ inline void
|
|||
unpack_msg_perf_report(
|
||||
uint32_t &interval,
|
||||
uint16_t &mainLoopCycles,
|
||||
uint8_t &mainLoopTime,
|
||||
uint8_t &mainLoopCycleTime,
|
||||
uint8_t &gyroSaturationCount,
|
||||
uint8_t &adcConstraintCount,
|
||||
uint8_t &renormSqrtCount,
|
||||
uint8_t &renormBlowupCount,
|
||||
uint8_t &gpsFixCount,
|
||||
uint16_t &imuHealth,
|
||||
uint16_t &gcsMessageCount)
|
||||
{
|
||||
uint8_t *__p = &_decodeBuf.payload[0];
|
||||
_unpack(__p, interval);
|
||||
_unpack(__p, mainLoopCycles);
|
||||
_unpack(__p, mainLoopTime);
|
||||
_unpack(__p, mainLoopCycleTime);
|
||||
_unpack(__p, gyroSaturationCount);
|
||||
_unpack(__p, adcConstraintCount);
|
||||
_unpack(__p, renormSqrtCount);
|
||||
_unpack(__p, renormBlowupCount);
|
||||
_unpack(__p, gpsFixCount);
|
||||
_unpack(__p, imuHealth);
|
||||
_unpack(__p, gcsMessageCount);
|
||||
};
|
||||
|
|
@ -848,17 +868,17 @@ unpack_msg_pid(
|
|||
|
||||
/// Structure describing the payload section of the MSG_TRIM_STARTUP message
|
||||
struct msg_trim_startup {
|
||||
uint16_t value[8];
|
||||
int value[8];
|
||||
};
|
||||
|
||||
/// Send a MSG_TRIM_STARTUP message
|
||||
inline void
|
||||
send_msg_trim_startup(
|
||||
const uint16_t (&value)[8])
|
||||
const int (&value)[8])
|
||||
{
|
||||
uint8_t *__p = &_encodeBuf.payload[0];
|
||||
_pack(__p, value, 8);
|
||||
_encodeBuf.header.length = 16;
|
||||
_encodeBuf.header.length = 8;
|
||||
_encodeBuf.header.messageID = MSG_TRIM_STARTUP;
|
||||
_encodeBuf.header.messageVersion = MSG_VERSION_1;
|
||||
_sendMessage();
|
||||
|
|
@ -867,7 +887,7 @@ send_msg_trim_startup(
|
|||
/// Unpack a MSG_TRIM_STARTUP message
|
||||
inline void
|
||||
unpack_msg_trim_startup(
|
||||
uint16_t (&value)[8])
|
||||
int (&value)[8])
|
||||
{
|
||||
uint8_t *__p = &_decodeBuf.payload[0];
|
||||
_unpack(__p, value, 8);
|
||||
|
|
@ -880,17 +900,17 @@ unpack_msg_trim_startup(
|
|||
|
||||
/// Structure describing the payload section of the MSG_TRIM_MIN message
|
||||
struct msg_trim_min {
|
||||
uint16_t value[8];
|
||||
int value[8];
|
||||
};
|
||||
|
||||
/// Send a MSG_TRIM_MIN message
|
||||
inline void
|
||||
send_msg_trim_min(
|
||||
const uint16_t (&value)[8])
|
||||
const int (&value)[8])
|
||||
{
|
||||
uint8_t *__p = &_encodeBuf.payload[0];
|
||||
_pack(__p, value, 8);
|
||||
_encodeBuf.header.length = 16;
|
||||
_encodeBuf.header.length = 8;
|
||||
_encodeBuf.header.messageID = MSG_TRIM_MIN;
|
||||
_encodeBuf.header.messageVersion = MSG_VERSION_1;
|
||||
_sendMessage();
|
||||
|
|
@ -899,7 +919,7 @@ send_msg_trim_min(
|
|||
/// Unpack a MSG_TRIM_MIN message
|
||||
inline void
|
||||
unpack_msg_trim_min(
|
||||
uint16_t (&value)[8])
|
||||
int (&value)[8])
|
||||
{
|
||||
uint8_t *__p = &_decodeBuf.payload[0];
|
||||
_unpack(__p, value, 8);
|
||||
|
|
@ -912,17 +932,17 @@ unpack_msg_trim_min(
|
|||
|
||||
/// Structure describing the payload section of the MSG_TRIM_MAX message
|
||||
struct msg_trim_max {
|
||||
uint16_t value[8];
|
||||
int value[8];
|
||||
};
|
||||
|
||||
/// Send a MSG_TRIM_MAX message
|
||||
inline void
|
||||
send_msg_trim_max(
|
||||
const uint16_t (&value)[8])
|
||||
const int (&value)[8])
|
||||
{
|
||||
uint8_t *__p = &_encodeBuf.payload[0];
|
||||
_pack(__p, value, 8);
|
||||
_encodeBuf.header.length = 16;
|
||||
_encodeBuf.header.length = 8;
|
||||
_encodeBuf.header.messageID = MSG_TRIM_MAX;
|
||||
_encodeBuf.header.messageVersion = MSG_VERSION_1;
|
||||
_sendMessage();
|
||||
|
|
@ -931,7 +951,7 @@ send_msg_trim_max(
|
|||
/// Unpack a MSG_TRIM_MAX message
|
||||
inline void
|
||||
unpack_msg_trim_max(
|
||||
uint16_t (&value)[8])
|
||||
int (&value)[8])
|
||||
{
|
||||
uint8_t *__p = &_decodeBuf.payload[0];
|
||||
_unpack(__p, value, 8);
|
||||
|
|
|
|||
Loading…
Reference in New Issue