移植mavlink协议到STM32详细教程
1准备材料,







#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]
移植mavlink协议到STM32详细教程的更多相关文章
- 移植mavlink到stm32详细教程,后面附快速移植方法
一:准备材料: mavlink源码 stm32串口程序 1.mavlink源码: a.进入mavlink官网(http://qgroundcontrol.org/mavlink/s ...
- 移植MAVLINK到STM32详细教程之三
在前面教程的基础上继续移植优化,之前的没有加缓冲区,没有接收函数功能,这里进行统一的讲解 作者:恒久力行 qq:624668529 缓冲区对于接 ...
- Xilinx zynq-7000系列FPGA移植Linux操作系统详细教程
Xilinx zynq-7000系列FPGA移植Linux操作系统详细教程 一:前言 最近手上压了一块米联客的Miz7035,一块xilinx zynq-7000系列的开发板,想着正好学习一下linu ...
- mavlink协议移植问题
mavlink协议移植问题 mavlink源代码是一个代码库,使用的时候只需要将mavlink.h头文件包含到工程项目中即可. mavlink通信协议是无状态的连接,一般采用心跳消息跟踪系统是否存在. ...
- git详细教程
Table of Contents 1 Git详细教程 1.1 Git简介 1.1.1 Git是何方神圣? 1.1.2 重要的术语 1.1.3 索引 1.2 Git安装 1.3 Git配置 1.3.1 ...
- GitHub详细教程(转载)
1 Git详细教程 1.1 Git简介 1.1.1 Git是何方神圣? 1.1.2 重要的术语 1.1.3 索引 1.2 Git安装 1.3 Git配置 1.3.1 用户信息 1.3.2 高亮显示 1 ...
- GitHub详细教程
GitHub详细教程 Table of Contents 1 Git详细教程 1.1 Git简介 1.1.1 Git是何方神圣? 1.1.2 重要的术语 1.1.3 索引 1.2 Git安装 1.3 ...
- 【原创】车载实时路况信息接收终端移植于Smart210开发板 --- 综合教程
[原创]车载实时路况信息接收终端移植于Smart210开发板 --- 综合教程 所用工具: windows电脑 Ubuntu12.04 Smart210开发板 4g以上SD卡 U盘 步骤: 1. ...
- Ymodem协议(参考STM32)
相信很多人都希望,不开盖就可以对固件进行升级吧,就像手机那些.下文中的bootload就来实现这样的功能. 前段时间有项目关于Bootload设计.所以就仔细的去了研究了一翻.以前都是用的stm32官 ...
随机推荐
- Spring框架IOC容器和AOP解析 非常 有用
Spring框架IOC容器和AOP解析 主要分析点: 一.Spring开源框架的简介 二.Spring下IOC容器和DI(依赖注入Dependency injection) 三.Spring下面 ...
- unreal3对象管理模块分析二
上一篇主要记了UClass的创建,现在总结一下UObject的创建,可以从几个不同角度来理解. 从途径上看,可以根据UObject的构造函数来分个类: // Constructors. UObject ...
- 【redis对象,集合序列化Demo】
package org.seckill.dao.cache; import java.io.ByteArrayInputStream;import java.io.ByteArrayOutputStr ...
- 服务器控件button点击时执行脚本弹出提示对话框Button2.Attributes.Add("onclick","事件")
<HTML> <HEAD> <title>**********资料更新</title> <meta content="Microso ...
- Ajax.BeginForm 使用过程中遇到的问题
一.Ajax.BeginForm设置的路由在浏览器中解析不到. 问题截图:在视图中设置"Counter/Index",在浏览器中显示的是"action='/'" ...
- PostgreSQL 务实应用(四/5)JSON
JSON 可谓风靡互联网,在数据交换使用上,其优势特别明显,其结构简洁.可读易读.形式灵活.很多 API 接口的数据都采用 JSON 来表示. PostgreSQL 对 JSON 提供了良好的支持.具 ...
- ProtoBuf练习(五)
表类型 protobuf语言的maps字段类型相当于C++语言的std::map类型 工程目录结构 $ ls proto/ sample_maps.proto proto文件 $ cat proto/ ...
- 帝都Day6——图论
//P2O5呢? 一.图的存储: 邻接矩阵:邻接表. 邻接矩阵:n*n的[][],[i][j]节点有边记1没边0 缺点 空间复杂度O(n^2) 占用内存较大(我为什么要把这些东西写到这里呢???) 邻 ...
- day20模块作业
1.模块化作业 1.回顾文件递归遍历. 默写一遍. 入口在: 当文件是个文件夹的时候 出口在: 文件是一个文件 2.计算时间差(用户输入起始时间和结束时间. 计算时间差(小时), 例如, 用户输入20 ...
- 使用jmeter往指定文件中插入一定数量的数据(转)
有一个需求,新建一批账号,把获取的账号相关信息存入文本文件,当文本文件保存的数据达到一定的数量,就自动停止新建账号. 分析下需求: 1.把账号信息保存到文件,需要使用bean shell脚本(bean ...