00001 00005 #ifndef TEST_TESTSUITE_H 00006 #define TEST_TESTSUITE_H 00007 00008 #ifdef __cplusplus 00009 extern "C" { 00010 #endif 00011 00012 #ifndef MAVLINK_TEST_ALL 00013 #define MAVLINK_TEST_ALL 00014 00015 static void mavlink_test_test(uint8_t, uint8_t, mavlink_message_t *last_msg); 00016 00017 static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 00018 { 00019 00020 mavlink_test_test(system_id, component_id, last_msg); 00021 } 00022 #endif 00023 00024 00025 00026 00027 static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 00028 { 00029 mavlink_message_t msg; 00030 uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; 00031 uint16_t i; 00032 mavlink_test_types_t packet_in = { 00033 'A', 00034 "BCDEFGHIJ", 00035 230, 00036 17859, 00037 963498192, 00038 93372036854776941ULL, 00039 211, 00040 18639, 00041 963498972, 00042 93372036854777886LL, 00043 304.0, 00044 438.0, 00045 { 228, 229, 230 }, 00046 { 20147, 20148, 20149 }, 00047 { 963500688, 963500689, 963500690 }, 00048 { 93372036854780469, 93372036854780470, 93372036854780471 }, 00049 { 171, 172, 173 }, 00050 { 22487, 22488, 22489 }, 00051 { 963503028, 963503029, 963503030 }, 00052 { 93372036854783304, 93372036854783305, 93372036854783306 }, 00053 { 1018.0, 1019.0, 1020.0 }, 00054 { 1208.0, 1209.0, 1210.0 }, 00055 }; 00056 mavlink_test_types_t packet1, packet2; 00057 memset(&packet1, 0, sizeof(packet1)); 00058 packet1.c = packet_in.c; 00059 packet1.u8 = packet_in.u8; 00060 packet1.u16 = packet_in.u16; 00061 packet1.u32 = packet_in.u32; 00062 packet1.u64 = packet_in.u64; 00063 packet1.s8 = packet_in.s8; 00064 packet1.s16 = packet_in.s16; 00065 packet1.s32 = packet_in.s32; 00066 packet1.s64 = packet_in.s64; 00067 packet1.f = packet_in.f; 00068 packet1.d = packet_in.d; 00069 00070 mav_array_memcpy(packet1.s, packet_in.s, sizeof(char)*10); 00071 mav_array_memcpy(packet1.u8_array, packet_in.u8_array, sizeof(uint8_t)*3); 00072 mav_array_memcpy(packet1.u16_array, packet_in.u16_array, sizeof(uint16_t)*3); 00073 mav_array_memcpy(packet1.u32_array, packet_in.u32_array, sizeof(uint32_t)*3); 00074 mav_array_memcpy(packet1.u64_array, packet_in.u64_array, sizeof(uint64_t)*3); 00075 mav_array_memcpy(packet1.s8_array, packet_in.s8_array, sizeof(int8_t)*3); 00076 mav_array_memcpy(packet1.s16_array, packet_in.s16_array, sizeof(int16_t)*3); 00077 mav_array_memcpy(packet1.s32_array, packet_in.s32_array, sizeof(int32_t)*3); 00078 mav_array_memcpy(packet1.s64_array, packet_in.s64_array, sizeof(int64_t)*3); 00079 mav_array_memcpy(packet1.f_array, packet_in.f_array, sizeof(float)*3); 00080 mav_array_memcpy(packet1.d_array, packet_in.d_array, sizeof(double)*3); 00081 00082 00083 memset(&packet2, 0, sizeof(packet2)); 00084 mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1); 00085 mavlink_msg_test_types_decode(&msg, &packet2); 00086 MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 00087 00088 memset(&packet2, 0, sizeof(packet2)); 00089 mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); 00090 mavlink_msg_test_types_decode(&msg, &packet2); 00091 MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 00092 00093 memset(&packet2, 0, sizeof(packet2)); 00094 mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); 00095 mavlink_msg_test_types_decode(&msg, &packet2); 00096 MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 00097 00098 memset(&packet2, 0, sizeof(packet2)); 00099 mavlink_msg_to_send_buffer(buffer, &msg); 00100 for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { 00101 comm_send_ch(MAVLINK_COMM_0, buffer[i]); 00102 } 00103 mavlink_msg_test_types_decode(last_msg, &packet2); 00104 MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 00105 00106 memset(&packet2, 0, sizeof(packet2)); 00107 mavlink_msg_test_types_send(MAVLINK_COMM_1 , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); 00108 mavlink_msg_test_types_decode(last_msg, &packet2); 00109 MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); 00110 } 00111 00112 static void mavlink_test_test(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 00113 { 00114 mavlink_test_test_types(system_id, component_id, last_msg); 00115 } 00116 00117 #ifdef __cplusplus 00118 } 00119 #endif // __cplusplus 00120 #endif // TEST_TESTSUITE_H