00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00029
00030 #ifndef APM_BinComm_h
00031 #define APM_BinComm_h
00032
00033 #include <string.h>
00034 #include <inttypes.h>
00035 #include "WProgram.h"
00036
00054 class BinComm {
00055 public:
00056 struct MessageHandler;
00057
00076
00078 BinComm(const MessageHandler *handlerTable,
00079 Stream *interface = NULL);
00080
00090 void init(Stream *interface);
00091
00092 private:
00094 struct MessageHeader {
00095 uint8_t length;
00096 uint8_t messageID;
00097 uint8_t messageVersion;
00098 };
00099
00102 union {
00103 uint8_t bytes[0];
00104 MessageHeader header;
00105 uint8_t payload[256];
00106 } _decodeBuf;
00107
00111
00112
00117 template <typename T> inline void _emit(const T x) { _send(&x, sizeof(T)); }
00118
00124 template <typename T> inline void _emit(const T *values, uint8_t count) { _send(values, count * sizeof(T)); }
00125
00126
00134 inline void _emit(const char *msg, uint8_t size) {
00135 while (size--) {
00136 char c = *msg;
00137 _emit(c);
00138 if (0 != c)
00139 msg++;
00140 }
00141 }
00142
00148 template <typename T> inline void _unpack(uint8_t *&ptr, T &x) { x = *(T *)ptr; ptr += sizeof(T); }
00149
00155 template <typename T> inline void _unpack(uint8_t *&ptr, T *values, uint8_t count) {
00156 memcpy(values, ptr, count * sizeof(T));
00157 ptr += count * sizeof(T);
00158 }
00159
00166 inline void _unpack(uint8_t *&ptr, char *msg, uint8_t size) {
00167 strncpy(msg, (char *)ptr, size); msg[size-1] = '\0'; ptr += size;
00168 }
00170
00171 public:
00175
00176 #include "protocol/protocol.h"
00178
00184
00185
00187 enum severities
00188 {
00189 SEVERITY_LOW = 1,
00190 SEVERITY_MEDIUM = 2,
00191 SEVERITY_HIGH = 3,
00192 SEVERITY_CRITICAL = 4,
00193 };
00194
00197 enum variableID {
00198 MSG_VAR_ROLL_MODE = 0x00,
00199 MSG_VAR_PITCH_MODE = 0x01,
00200 MSG_VAR_THROTTLE_MODE = 0x02,
00201 MSG_VAR_YAW_MODE = 0x03,
00202 MSG_VAR_ELEVON_TRIM_1 = 0x04,
00203 MSG_VAR_ELEVON_TRIM_2 = 0x05,
00204
00205 MSG_VAR_INTEGRATOR_0 = 0x10,
00206 MSG_VAR_INTEGRATOR_1 = 0x11,
00207 MSG_VAR_INTEGRATOR_2 = 0x12,
00208 MSG_VAR_INTEGRATOR_3 = 0x13,
00209 MSG_VAR_INTEGRATOR_4 = 0x14,
00210 MSG_VAR_INTEGRATOR_5 = 0x15,
00211 MSG_VAR_INTEGRATOR_6 = 0x16,
00212 MSG_VAR_INTEGRATOR_7 = 0x17,
00213
00214 MSG_VAR_KFF_0 = 0x1a,
00215 MSG_VAR_KFF_1 = 0x1b,
00216 MSG_VAR_KFF_2 = 0x1c,
00217
00218 MSG_VAR_TARGET_BEARING = 0x20,
00219 MSG_VAR_NAV_BEARING = 0x21,
00220 MSG_VAR_BEARING_ERROR = 0x22,
00221 MSG_VAR_CROSSTRACK_BEARING = 0x23,
00222 MSG_VAR_CROSSTRACK_ERROR = 0x24,
00223 MSG_VAR_ALTITUDE_ERROR = 0x25,
00224 MSG_VAR_WP_RADIUS = 0x26,
00225 MSG_VAR_LOITER_RADIUS = 0x27,
00226 MSG_VAR_WP_MODE = 0x28,
00227 MSG_VAR_LOOP_COMMANDS = 0x29,
00228 MSG_VAR_NAV_GAIN_SCALER = 0x2a,
00229 };
00230
00232 enum PIDSet {
00233 MSG_SERVO_ROLL = 0,
00234 MSG_SERVO_PITCH = 1,
00235 MSG_SERVO_RUDDER = 2,
00236 MSG_SERVO_NAV_ROLL = 3,
00237 MSG_SERVO_NAV_PITCH_ASP = 4,
00238 MSG_SERVO_NAV_PITCH_ALT = 5,
00239 MSG_SERVO_TE_THROTTLE = 6,
00240 MSG_SERVO_ALT_THROTTLE = 7,
00241 MSG_SERVO_ELEVATOR = 8
00242 };
00243
00245
00252 struct MessageHandler {
00253 MessageID messageID;
00254 void (* handler)(void *arg,
00255 uint8_t messageId,
00256 uint8_t messageVersion,
00257 void *messageData);
00258 void *arg;
00259 };
00260
00263
00264
00275
00276 void update(void);
00277
00278 uint32_t messagesReceived;
00279 uint32_t badMessagesReceived;
00280
00282
00291
00292 uint32_t messagesSent;
00293
00294
00295
00296 private:
00297 const MessageHandler *_handlerTable;
00298 Stream *_interface;
00299
00301 enum MagicNumbers {
00302 MSG_PREAMBLE_1 = 0x34,
00303 MSG_PREAMBLE_2 = 0x44,
00304 MSG_VERSION_1 = 1,
00305 MSG_VARIABLE_LENGTH = 0xff
00306 };
00307
00310
00311 uint8_t _decodePhase;
00312 uint8_t _bytesIn;
00313 uint8_t _bytesExpected;
00314 uint8_t _decoderSumA;
00315 uint8_t _decoderSumB;
00316
00317 uint8_t _messageID;
00318 uint8_t _messageVersion;
00319
00320 unsigned long _lastReceived;
00321
00322
00327 void _decode(uint8_t inByte);
00328
00331
00332 uint8_t _encoderSumA;
00333 uint8_t _encoderSumB;
00334
00335
00342 void _startMessage(uint8_t messageId, uint8_t messageLength, uint8_t messageVersion = 1);
00343
00348 void _send(const void *bytes, uint8_t count);
00349
00352 void _endMessage(void);
00353 };
00354
00355 #endif // BinComm_h