1準備材料,html
#define MAVPACKED( __Declaration__ ) __pragma( pack(push,1)) __Declaration__ __pragma( pack(pop))
#define MAVPACKED( __Declaration__ ) __Declaration__
#defineinline __INLINE
MAVLINK_HELPER void mavlink_dcm_to_quaternion(constfloat 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(constfloat 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,constmavlink_message_t*msg)
{
memcpy(buffer,(constuint8_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,constmavlink_message_t*msg)
{
uint8_t*ck;
memcpy(buffer,(constuint8_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)(constchar)_MAV_PAYLOAD(msg)[wire_offset]
#define_MAV_RETURN_int8_t(msg, wire_offset)(constint8_t)_MAV_PAYLOAD(msg)[wire_offset]
#define_MAV_RETURN_uint8_t(msg, wire_offset)(constuint8_t)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_char(msg, wire_offset)(constchar)_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]