testmav.cpp
Go to the documentation of this file.
00001 // testmav.cpp : Defines the entry point for the console application.
00002 //
00003 
00004 #include "stdafx.h"
00005 #include "stdio.h"
00006 #include "stdint.h"
00007 #include "stddef.h"
00008 #include "assert.h"
00009 
00010 
00011 #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
00012 #define MAVLINK_COMM_NUM_BUFFERS 2
00013 
00014 #include <mavlink_types.h>
00015 static mavlink_system_t mavlink_system = {42,11,};
00016 
00017 #define MAVLINK_ASSERT(x) assert(x)
00018 static void comm_send_ch(mavlink_channel_t chan, uint8_t c);
00019 
00020 static mavlink_message_t last_msg;
00021 
00022 #include <common/mavlink.h>
00023 #include <common/testsuite.h>
00024 
00025 static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS];
00026 
00027 static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
00028 static unsigned error_count;
00029 
00030 static const mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO;
00031 
00032 static void print_one_field(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx)
00033 {
00034 #define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def)
00035         switch (f->type) {
00036         case MAVLINK_TYPE_CHAR:
00037                 printf(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1));
00038                 break;
00039         case MAVLINK_TYPE_UINT8_T:
00040                 printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1));
00041                 break;
00042         case MAVLINK_TYPE_INT8_T:
00043                 printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1));
00044                 break;
00045         case MAVLINK_TYPE_UINT16_T:
00046                 printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2));
00047                 break;
00048         case MAVLINK_TYPE_INT16_T:
00049                 printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2));
00050                 break;
00051         case MAVLINK_TYPE_UINT32_T:
00052                 printf(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4));
00053                 break;
00054         case MAVLINK_TYPE_INT32_T:
00055                 printf(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4));
00056                 break;
00057         case MAVLINK_TYPE_UINT64_T:
00058                 printf(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8));
00059                 break;
00060         case MAVLINK_TYPE_INT64_T:
00061                 printf(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8));
00062                 break;
00063         case MAVLINK_TYPE_FLOAT:
00064                 printf(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4));
00065                 break;
00066         case MAVLINK_TYPE_DOUBLE:
00067                 printf(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8));
00068                 break;
00069         }
00070 }
00071 
00072 static void print_field(mavlink_message_t *msg, const mavlink_field_info_t *f)
00073 {
00074         printf("%s: ", f->name);
00075         if (f->array_length == 0) {
00076                 print_one_field(msg, f, 0);
00077                 printf(" ");
00078         } else {
00079                 unsigned i;
00080                 /* print an array */
00081                 if (f->type == MAVLINK_TYPE_CHAR) {
00082                         printf("'%.*s'", f->array_length,
00083                                f->wire_offset+(const char *)_MAV_PAYLOAD(msg));
00084                         
00085                 } else {
00086                         printf("[ ");
00087                         for (i=0; i<f->array_length; i++) {
00088                                 print_one_field(msg, f, i);
00089                                 if (i < f->array_length) {
00090                                         printf(", ");
00091                                 }
00092                         }
00093                         printf("]");
00094                 }
00095         }
00096         printf(" ");
00097 }
00098 
00099 static void print_message(mavlink_message_t *msg)
00100 {
00101         const mavlink_message_info_t *m = &message_info[msg->msgid];
00102         const mavlink_field_info_t *f = m->fields;
00103         unsigned i;
00104         printf("%s { ", m->name);
00105         for (i=0; i<m->num_fields; i++) {
00106                 print_field(msg, &f[i]);
00107         }
00108         printf("}\n");
00109 }
00110 
00111 static void comm_send_ch(mavlink_channel_t chan, uint8_t c)
00112 {
00113         mavlink_status_t status;
00114         if (mavlink_parse_char(chan, c, &last_msg, &status)) {
00115                 print_message(&last_msg);
00116                 chan_counts[chan]++;
00117                 /* channel 0 gets 3 messages per message, because of
00118                    the channel defaults for _pack() and _encode() */
00119                 if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) {
00120                         printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n", 
00121                                chan_counts[chan], status.current_rx_seq);
00122                         error_count++;
00123                 } else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) {
00124                         printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n", 
00125                                (unsigned)chan, chan_counts[chan], status.current_rx_seq);
00126                         error_count++;
00127                 }
00128                 if (message_lengths[last_msg.msgid] != last_msg.len) {
00129                         printf("Incorrect message length %u for message %u - expected %u\n", 
00130                                (unsigned)last_msg.len, (unsigned)last_msg.msgid, message_lengths[last_msg.msgid]);
00131                         error_count++;
00132                 }
00133         }
00134         if (status.packet_rx_drop_count != 0) {
00135                 printf("Parse error at packet %u\n", chan_counts[chan]);
00136                 error_count++;
00137         }
00138 }
00139         
00140 int _tmain(int argc, _TCHAR* argv[])
00141 {
00142         int chan;
00143         mavlink_test_all(11, 10, &last_msg);
00144         for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) {
00145                 printf("Received %u messages on channel %u OK\n", 
00146                        chan_counts[chan], (unsigned)chan);
00147         }
00148         if (error_count != 0) {
00149                 printf("Error count %u\n", error_count);
00150                 return(1);
00151         }
00152         printf("No errors detected\n");
00153         return 0;
00154 }


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43