testmav.c
Go to the documentation of this file.
00001 /*
00002   simple MAVLink testsuite for C
00003  */
00004 #include <stdio.h>
00005 #include <stdlib.h>
00006 #include <stdint.h>
00007 #include <assert.h>
00008 #include <stddef.h>
00009 #include <stdbool.h>
00010 
00011 #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
00012 #define MAVLINK_USE_MESSAGE_INFO
00013 #define MAVLINK_COMM_NUM_BUFFERS 2
00014 
00015 // this trick allows us to make mavlink_message_t as small as possible
00016 // for this dialect, which saves some memory
00017 #include <version.h>
00018 #define MAVLINK_MAX_PAYLOAD_LEN MAVLINK_MAX_DIALECT_PAYLOAD_SIZE
00019 
00020 #include <mavlink_types.h>
00021 static mavlink_system_t mavlink_system = {42,11,};
00022 
00023 #define MAVLINK_ASSERT(x) assert(x)
00024 static void comm_send_ch(mavlink_channel_t chan, uint8_t c);
00025 
00026 static mavlink_message_t last_msg;
00027 
00028 #include <mavlink.h>
00029 #include <testsuite.h>
00030 
00031 static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS];
00032 
00033 #ifndef MAVLINK_HAVE_EXPECTED_MESSAGE_LENGTH
00034 static const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
00035 #define mavlink_expected_message_length(msg) message_lengths[(msg)->msgid]
00036 #endif
00037 
00038 #ifndef MAVLINK_HAVE_GET_MESSAGE_INFO
00039 static const mavlink_message_info_t message_info[] = MAVLINK_MESSAGE_INFO;
00040 #define mavlink_get_message_info(msg) &message_info[(msg)->msgid]
00041 #endif
00042 
00043 static unsigned error_count;
00044 
00045 
00046 static void print_one_field(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx)
00047 {
00048 #define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def)
00049         switch (f->type) {
00050         case MAVLINK_TYPE_CHAR:
00051                 printf(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1));
00052                 break;
00053         case MAVLINK_TYPE_UINT8_T:
00054                 printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1));
00055                 break;
00056         case MAVLINK_TYPE_INT8_T:
00057                 printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1));
00058                 break;
00059         case MAVLINK_TYPE_UINT16_T:
00060                 printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2));
00061                 break;
00062         case MAVLINK_TYPE_INT16_T:
00063                 printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2));
00064                 break;
00065         case MAVLINK_TYPE_UINT32_T:
00066                 printf(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4));
00067                 break;
00068         case MAVLINK_TYPE_INT32_T:
00069                 printf(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4));
00070                 break;
00071         case MAVLINK_TYPE_UINT64_T:
00072                 printf(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8));
00073                 break;
00074         case MAVLINK_TYPE_INT64_T:
00075                 printf(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8));
00076                 break;
00077         case MAVLINK_TYPE_FLOAT:
00078                 printf(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4));
00079                 break;
00080         case MAVLINK_TYPE_DOUBLE:
00081                 printf(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8));
00082                 break;
00083         }
00084 }
00085 
00086 static void print_field(mavlink_message_t *msg, const mavlink_field_info_t *f)
00087 {
00088         printf("%s: ", f->name);
00089         if (f->array_length == 0) {
00090                 print_one_field(msg, f, 0);
00091                 printf(" ");
00092         } else {
00093                 unsigned i;
00094                 /* print an array */
00095                 if (f->type == MAVLINK_TYPE_CHAR) {
00096                         printf("'%.*s'", f->array_length,
00097                                f->wire_offset+(const char *)_MAV_PAYLOAD(msg));
00098                         
00099                 } else {
00100                         printf("[ ");
00101                         for (i=0; i<f->array_length; i++) {
00102                                 print_one_field(msg, f, i);
00103                                 if (i < f->array_length) {
00104                                         printf(", ");
00105                                 }
00106                         }
00107                         printf("]");
00108                 }
00109         }
00110         printf(" ");
00111 }
00112 
00113 static void print_message(mavlink_message_t *msg)
00114 {
00115         const mavlink_message_info_t *m = mavlink_get_message_info(msg);
00116         if (m == NULL) {
00117                 printf("ERROR: no message info for %u\n", msg->msgid);
00118                 error_count++;
00119                 return;
00120         }
00121         const mavlink_field_info_t *f = m->fields;
00122         unsigned i;
00123         printf("%s { ", m->name);
00124         for (i=0; i<m->num_fields; i++) {
00125                 print_field(msg, &f[i]);
00126         }
00127         printf("}\n");
00128 }
00129 
00130 #ifdef MAVLINK_SIGNING_FLAG_SIGN_OUTGOING
00131 static mavlink_signing_t signing_in[MAVLINK_COMM_NUM_BUFFERS];
00132 static mavlink_signing_streams_t signing_streams_in;
00133 #endif
00134 
00135 static void comm_send_ch(mavlink_channel_t chan, uint8_t c)
00136 {
00137         mavlink_status_t status;
00138         memset(&status, 0, sizeof(status));
00139 #ifdef MAVLINK_SIGNING_FLAG_SIGN_OUTGOING
00140         status.signing = &signing_in[chan];
00141         status.signing_streams = &signing_streams_in;
00142 #endif
00143         if (mavlink_parse_char(chan, c, &last_msg, &status)) {
00144                 print_message(&last_msg);
00145                 chan_counts[chan]++;
00146                 /* channel 0 gets 3 messages per message, because of
00147                    the channel defaults for _pack() and _encode() */
00148                 if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) {
00149                         printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n", 
00150                                chan_counts[chan], status.current_rx_seq);
00151                         error_count++;
00152                 } else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) {
00153                         printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n", 
00154                                (unsigned)chan, chan_counts[chan], status.current_rx_seq);
00155                         error_count++;
00156                 }
00157                 if (mavlink_expected_message_length(&last_msg) > last_msg.len) {
00158                         printf("Incorrect message length %u for message %u - expected %u\n", 
00159                                (unsigned)last_msg.len, (unsigned)last_msg.msgid,
00160                                mavlink_expected_message_length(&last_msg));
00161                         error_count++;
00162                 }
00163         }
00164         if (status.packet_rx_drop_count != 0) {
00165                 printf("Parse error at packet %u\n", chan_counts[chan]);
00166                 error_count++;
00167         }
00168 }
00169 
00170 int main(void)
00171 {
00172         mavlink_channel_t chan;
00173 
00174         mavlink_test_all(11, 10, &last_msg);
00175         for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) {
00176                 printf("Received %u messages on channel %u OK\n", 
00177                        chan_counts[chan], (unsigned)chan);
00178         }
00179         if (error_count != 0) {
00180                 printf("Error count %u\n", error_count);
00181                 exit(1);
00182         }
00183         printf("No errors detected\n");
00184 
00185 #ifdef MAVLINK_SIGNING_FLAG_SIGN_OUTGOING
00186         mavlink_status_t *status;
00187 
00188         printf("Testing signing\n");
00189         mavlink_signing_t signing;
00190         mavlink_signing_streams_t signing_streams;
00191         memset(&signing, 0, sizeof(signing));
00192         memset(&signing_streams, 0, sizeof(signing_streams));
00193         signing.flags = MAVLINK_SIGNING_FLAG_SIGN_OUTGOING;
00194         signing.link_id = 0;
00195         signing.timestamp = 1;
00196         memset(signing.secret_key, 42, sizeof(signing.secret_key));
00197 
00198         status = mavlink_get_channel_status(MAVLINK_COMM_1);
00199         status->signing = &signing;
00200         status->signing_streams = &signing_streams;
00201 
00202         mavlink_test_all(11, 10, &last_msg);
00203         for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) {
00204                 printf("Received %u messages on channel %u OK\n", 
00205                        chan_counts[chan], (unsigned)chan);
00206         }
00207         if (error_count != 0) {
00208                 printf("Error count %u\n", error_count);
00209                 exit(1);
00210         }
00211         printf("No errors detected\n"); 
00212 #endif
00213 
00214 #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
00215         status = mavlink_get_channel_status(MAVLINK_COMM_0);
00216         status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
00217         status->signing = NULL;
00218         status = mavlink_get_channel_status(MAVLINK_COMM_1);
00219         status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
00220         status->signing = NULL;
00221         printf("Testing sending as MAVLink1\n");
00222         
00223         mavlink_test_all(11, 10, &last_msg);
00224         for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) {
00225                 printf("Received %u messages on channel %u OK\n", 
00226                        chan_counts[chan], (unsigned)chan);
00227         }
00228         if (error_count != 0) {
00229                 printf("Error count %u\n", error_count);
00230                 exit(1);
00231         }
00232         printf("No errors detected\n");
00233 #endif
00234 
00235         
00236         return 0;
00237 }
00238 


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