/** @file * @brief MAVLink comm protocol testsuite generated from ardupilotmega.xml * @see http://qgroundcontrol.org/mavlink/ */ #ifndef ARDUPILOTMEGA_TESTSUITE_H #define ARDUPILOTMEGA_TESTSUITE_H #ifdef __cplusplus extern "C" { #endif #ifndef MAVLINK_TEST_ALL #define MAVLINK_TEST_ALL static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); static void mavlink_test_ardupilotmega(uint8_t, uint8_t, mavlink_message_t *last_msg); static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_test_common(system_id, component_id, last_msg); mavlink_test_ardupilotmega(system_id, component_id, last_msg); } #endif #include "../common/testsuite.h" static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_sensor_offsets_t packet_in = { 17.0, 963497672, 963497880, 101.0, 129.0, 157.0, 185.0, 213.0, 241.0, 19107, 19211, 19315, }; mavlink_sensor_offsets_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.mag_declination = packet_in.mag_declination; packet1.raw_press = packet_in.raw_press; packet1.raw_temp = packet_in.raw_temp; packet1.gyro_cal_x = packet_in.gyro_cal_x; packet1.gyro_cal_y = packet_in.gyro_cal_y; packet1.gyro_cal_z = packet_in.gyro_cal_z; packet1.accel_cal_x = packet_in.accel_cal_x; packet1.accel_cal_y = packet_in.accel_cal_y; packet1.accel_cal_z = packet_in.accel_cal_z; packet1.mag_ofs_x = packet_in.mag_ofs_x; packet1.mag_ofs_y = packet_in.mag_ofs_y; packet1.mag_ofs_z = packet_in.mag_ofs_z; memset(&packet2, 0, sizeof(packet2)); mavlink_msg_sensor_offsets_encode(system_id, component_id, &msg, &packet1); mavlink_msg_sensor_offsets_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); mavlink_msg_sensor_offsets_pack(system_id, component_id, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); mavlink_msg_sensor_offsets_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); mavlink_msg_sensor_offsets_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); mavlink_msg_to_send_buffer(buffer, &msg); for (i=0; i