移植mavlink协议到STM32详细教程

#define MAVPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) )

#define MAVPACKED( __Declaration__ )  __Declaration__

#define  inline  __INLINE 

MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])
{
    float tr = dcm[0][0] + dcm[1][1] + dcm[2][2];
    if (tr > 0.0f) {
        float s = sqrtf(tr + 1.0f);
        quaternion[0] = s * 0.5f;
        s = 0.5f / s;
        quaternion[1] = (dcm[2][1] - dcm[1][2]) * s;
        quaternion[2] = (dcm[0][2] - dcm[2][0]) * s;
        quaternion[3] = (dcm[1][0] - dcm[0][1]) * s;
    } else {
        /* Find maximum diagonal element in dcm
         * store index in dcm_i */
        int dcm_i = 0;
        int i;
        for (i = 1; i < 3; i++) {
            if (dcm[i][i] > dcm[dcm_i][dcm_i]) {
                dcm_i = i;
            }
        }
        int dcm_j = (dcm_i + 1) % 3;
        int dcm_k = (dcm_i + 2) % 3;
        float s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -
                    dcm[dcm_k][dcm_k]) + 1.0f);
        quaternion[dcm_i + 1] = s * 0.5f;
        s = 0.5f / s;
        quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s;
        quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s;
        quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s;
    }
}

MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])
{
		int dcm_j,dcm_k;
		float s;
    float tr = dcm[0][0] + dcm[1][1] + dcm[2][2];
    if (tr > 0.0f) {
        float s = sqrtf(tr + 1.0f);
        quaternion[0] = s * 0.5f;
        s = 0.5f / s;
        quaternion[1] = (dcm[2][1] - dcm[1][2]) * s;
        quaternion[2] = (dcm[0][2] - dcm[2][0]) * s;
        quaternion[3] = (dcm[1][0] - dcm[0][1]) * s;
    } else {
        /* Find maximum diagonal element in dcm
         * store index in dcm_i */
        int dcm_i = 0;
        int i;
        for (i = 1; i < 3; i++) {
            if (dcm[i][i] > dcm[dcm_i][dcm_i]) {
                dcm_i = i;
            }
        }
        dcm_j = (dcm_i + 1) % 3;
        dcm_k = (dcm_i + 2) % 3;
        s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -
                    dcm[dcm_k][dcm_k]) + 1.0f);
        quaternion[dcm_i + 1] = s * 0.5f;
        s = 0.5f / s;
        quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s;
        quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s;
        quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s;
    }
}

MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
{
	memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
	uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
	ck[0] = (uint8_t)(msg->checksum & 0xFF);
	ck[1] = (uint8_t)(msg->checksum >> 8);
	return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
}

MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
{
	uint8_t *ck; 
	memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
	ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
	ck[0] = (uint8_t)(msg->checksum & 0xFF);
	ck[1] = (uint8_t)(msg->checksum >> 8);
	return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
}

#define _MAV_RETURN_char(msg, wire_offset)             (const char)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_int8_t(msg, wire_offset)   (const int8_t)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset]

#define _MAV_RETURN_char(msg, wire_offset)             (const char)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_int8_t(msg, wire_offset)   (int8_t)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_uint8_t(msg, wire_offset) (uint8_t)_MAV_PAYLOAD(msg)[wire_offset]