mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 15:38:29 -04:00
ca8141cfb6
this new implementation reduces code size, and also reduces stack usage, while avoiding the gcc union stack bug Note that we will gain even more when we move to the new protocol version, especially in terms of code size git-svn-id: https://arducopter.googlecode.com/svn/trunk@3200 f9c3cf11-9bcb-44bc-f272-b75c42450872
153 lines
6.4 KiB
C
153 lines
6.4 KiB
C
/** @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 = {
|
|
17235,
|
|
17339,
|
|
17443,
|
|
59.0,
|
|
963497984,
|
|
963498192,
|
|
143.0,
|
|
171.0,
|
|
199.0,
|
|
227.0,
|
|
255.0,
|
|
283.0,
|
|
};
|
|
mavlink_sensor_offsets_t packet1, packet2;
|
|
memset(&packet1, 0, sizeof(packet1));
|
|
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;
|
|
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;
|
|
|
|
|
|
|
|
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<mavlink_msg_get_send_buffer_length(&msg); i++) {
|
|
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
|
|
}
|
|
mavlink_msg_sensor_offsets_decode(last_msg, &packet2);
|
|
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
|
|
|
memset(&packet2, 0, sizeof(packet2));
|
|
mavlink_msg_sensor_offsets_send(MAVLINK_COMM_1 , 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(last_msg, &packet2);
|
|
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
|
}
|
|
|
|
static void mavlink_test_set_mag_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_set_mag_offsets_t packet_in = {
|
|
5,
|
|
72,
|
|
17339,
|
|
17443,
|
|
17547,
|
|
};
|
|
mavlink_set_mag_offsets_t packet1, packet2;
|
|
memset(&packet1, 0, sizeof(packet1));
|
|
packet1.target_system = packet_in.target_system;
|
|
packet1.target_component = packet_in.target_component;
|
|
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_set_mag_offsets_encode(system_id, component_id, &msg, &packet1);
|
|
mavlink_msg_set_mag_offsets_decode(&msg, &packet2);
|
|
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
|
|
|
memset(&packet2, 0, sizeof(packet2));
|
|
mavlink_msg_set_mag_offsets_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z );
|
|
mavlink_msg_set_mag_offsets_decode(&msg, &packet2);
|
|
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
|
|
|
memset(&packet2, 0, sizeof(packet2));
|
|
mavlink_msg_set_mag_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z );
|
|
mavlink_msg_set_mag_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<mavlink_msg_get_send_buffer_length(&msg); i++) {
|
|
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
|
|
}
|
|
mavlink_msg_set_mag_offsets_decode(last_msg, &packet2);
|
|
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
|
|
|
memset(&packet2, 0, sizeof(packet2));
|
|
mavlink_msg_set_mag_offsets_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z );
|
|
mavlink_msg_set_mag_offsets_decode(last_msg, &packet2);
|
|
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
|
}
|
|
|
|
static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
|
|
{
|
|
mavlink_test_sensor_offsets(system_id, component_id, last_msg);
|
|
mavlink_test_set_mag_offsets(system_id, component_id, last_msg);
|
|
}
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif // __cplusplus
|
|
#endif // ARDUPILOTMEGA_TESTSUITE_H
|