print("("); print(x); print(","); print(y); println(")");
把数组拆分输出
PX4Firmware
经常有人将Pixhawk、PX4、APM还有ArduPilot弄混。这里首先还是简要说明一下：
Pixhawk是飞控硬件平台，PX4和ArduPilot都是开源的可以烧写到Pixhawk飞控中的自驾仪软件，PX4称为原生固件，专为Pixhawk打造。APM(Ardupilot Mega)早期也是一款自驾仪硬件，到APM3.0版本，这款基于Arduino Mega的自驾仪已经走到了它的终点。ArduPilot早期是APM自驾仪的固件，Pixhawk作为APM的升级版，也兼容ArduPilot固件，APM自驾仪卒了之后，ArduPilot现在全面支持Pixhawk，现在大家亲切的称ArduPilot固件为APM。
Pixhawk
APM 2.5
PX4
ArduPilot
笔者一直使用的是Pixhawk飞控，研究PX4Firmware。用Source Insight 看代码是极好的。
PX4固件主要是用C++语言编写，真是学好C++，走遍天下都不怕。使用了NuttX实时操作系统，整体软件架构不可谓不庞大。
不知道如何开头，复述一个人工智能和机器人领域著名的莫拉维克悖论：和传统假设不同，对计算机而言，实现逻辑推理等人类高级智慧只需要相对很少的计算能力，而实现感知、运动等低等级智慧却需要巨大的计算资源。
且从系统说起吧。
地面站配置的文件应该在芯片flash中，格式化SD卡同时擦除芯片后配置信息依然存在。
RTFSC
Pixhawk整体逻辑大致为：
commander和navigator产生期望位置
position_estimator估计当前位置
通过pos_ctrl产生期望姿态
attitude_estimator估计当前姿态
通过att_estimator产生PWM数值
最后通过mixer和motor_driver控制电机
一直都还只是停留在底层，什么时候能感受一下ETHz这帮人的成果呢，这才是pixhawk啊。
启动函数
Pixhawk是没有main函数的，飞控上电后，会自动执行Firmware/ROMFS/px4fmu_common/init.d文件夹下的rcS 启动脚本(startup script)。这个脚本位于被编译到固件中的 ROM文件系统中。这个脚本检测可用的硬件， 加载硬件驱动，并且根据你的设置启动系统正常运行所需的有 app(任务软件 ，包括位置和姿态估计，控制遥测等)。所有属于自启动程序的脚本文件可以在init.d文件夹中找到。
uORB是Pixhawk系统中非常重要且关键的一个模块，它肩负了整数据传输任务，所有的感器、 数据传输任务、 GPS、PPM信号等都要从芯片获取后通过uORB进行传输到各个模块进行计算处理。
uORB 的入口点是 uorb_main函数，在这里它检查 uORB的启动参数来完成对应的功能， uORB支持 start/test/status这 3条启动参数，在 PX4的rcS启动脚本中，使用start参数来进行初始化，其他 2个参数分别用来进行uORB功能的自检和列出 uORB的当前状态。
在rcS中使用 start参数启动uORB后，uORB会创建并初始化它的设备实例，其中的实现大部分都在CDev基类完成。
rcS启动顺序
extern “C” __EXPORT int main(int agrc, char *agrv[ ]);
argc和argv是main函数的形参，它们是程序的“命令行参数”。agrc(argument count的缩写，意思是参数个数)，argv(argument vector的缩写，意思是参数向量)，它是一个*char指针数组，数组中每一个元素指向命令行中的一个字符串。
main函数是操作系统调用的，实参只能由操作系统给出。在操作命令状态下，实参是和执行文件的命令一起给出的。例如在DOS、UNIX或Linux等系统的操作命令状态下，在命令行中包括了命令名和需要传给main函数的参数。
命令行的一般形式为：
命令名 参数1 参数2 …… 参数n
命令名和各参数之间用空格分隔。命令名是可执行文件名(此文件包含main函数)。
在rcS执行的时候，比如attitude_estimator_q_main start
那么agrc就等于2，agrv[0]就是attitude_estimator_q_main这个字符串，argv[1]就是start。
所以要判断agrv[1]是start还是stop。
就像你在dos命令行里输入attitude_estimator_q start，自然就给agrc和agrv[]赋值。NuttX系统下的模块的主函数名字都是以”_main”开始的，但是调用的时候不加“_main”。
不管了，就地举个栗子，还是attitude_estimator_q_main.cpp这个文件。
?
1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495extern "C" _EXPORT int attitude_estimator_q_main(int argc, char *argv[]); …… int attitude_estimator_q_main(int argc, char *argv[]) { if (argc < 1) { warnx("usage: attitude_estimator_q {start|stop|status}"); return 1; } if (!strcmp(argv[1], "start")) { if (attitude_estimator_q::instance != nullptr) { warnx("already running"); return 1; } attitude_estimator_q::instance = new AttitudeEstimatorQ; if (attitude_estimator_q::instance == nullptr) { warnx("alloc failed"); return 1; } if (OK != attitude_estimator_q::instance->start()) { delete attitude_estimator_q::instance; attitude_estimator_q::instance = nullptr; warnx("start failed"); return 1; } return 0; } if (!strcmp(argv[1], "stop")) { if (attitude_estimator_q::instance == nullptr) { warnx("not running"); return 1; } delete attitude_estimator_q::instance; attitude_estimator_q::instance = nullptr; return 0; } if (!strcmp(argv[1], "status")) { if (attitude_estimator_q::instance) { attitude_estimator_q::instance->print(); warnx("running"); return 0; } else { warnx("not running"); return 1; } } warnx("unrecognized command"); return 1; }在一系列头文件之后，这里extern “C”告诉编译器在编译attitude_estimator_q_main这个函数时按照C的规则去翻译相关的函数名而不是C++的; __EXPORT 表示将函数名输出到链接器(Linker)。
然后跳转到函数的定义部分int attitude_estimator_q_main(int argc, char *argv[])，判断系统给出的命令行的参数，一系列的判断，C++在大型项目上的优势这里有没有发挥出来!总之你要的是start就对了。
和attitude_estimator_q相似，一个正常的应用程序启动如下图所示，直接task_main( ) 吧：
坐标系
惯性导航的基础是精确定义一系列的笛卡儿参考坐标系，每一个坐标系都是正交的右手坐标系或轴系。
对地球上进行的导航，所定义的坐标系要将惯导系统的测量值与地球的主要方向联系起来。也就是说，当在近地面导航时，该坐标系具有实际意义。因此，习惯上将原点位于地球中心、相对于恒星固定的坐标系定义为惯性参考坐标系，下图给出了用于陆地导航的固连于地球的参考坐标系和当地地理导航坐标系以及惯性参考坐标系。
地球坐标系(e系)。原点位于地球中心，坐标轴与地球固连，轴向定义为Oxe,Oye,Oze 。其中，Oze 沿地球极轴方向，Oxe轴沿格林尼泊子午面和地球赤道平面的交线。地球坐标系相对于惯性坐标系绕Ozi 轴以角速度Ω转动。
导航坐标系(n系)。是一种当地地理坐标系，原点位于导航系统所处的位置P点，坐标轴指向北、东和当地垂线方向(向下)。导航坐标系相对于地球固连坐标系的旋转角速率wen取决于P 点相对于地球的运动，通常称为转移速率。
载体坐标系(b系)。一个正交坐标系，轴向分别沿安装有导航系统的运载体的横滚轴、俯仰轴和偏航轴。
在PX4中，
Local position setpoint in NED frame → 导航坐标系(以起飞点home为原点) 北东地 xyz
Global position in WGS84 coordinates → 世界大地坐标系(原点位于地球质心)
NED earth-fixed frame →个人觉得是GPS投影到地面的坐标系,原点?
NED body-fixed frame →机体坐标系，x轴正方向为机头，z轴正方向下
这里有两个函数不得不提：
?
123456789101112131415161718192021222324252627282930313233343536373839404142434445//将地理学坐标系(geographic coordinate system)中的点(球)投影到本地方位等距平面(XOY)中 int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,float *y) { if (!map_projection_initialized(ref)) { return -1; } double lat_rad = lat * M_DEG_TO_RAD; // 度 -> 弧度 A/57.295 double lon_rad = lon * M_DEG_TO_RAD; // GPS数据角度单位为弧度 double sin_lat = sin(lat_rad); //程序中三角运算使用的是弧度 double cos_lat = cos(lat_rad); double cos_d_lon = cos(lon_rad - ref->lon_rad); double arg = ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon; if (arg > 1.0) { arg = 1.0; } else if (arg < -1.0) { arg = -1.0; //限幅 } double c = acos(arg); double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));// c为正数 *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; return 0; }将球面坐标转化为平面坐标的过程便称为投影。这里将经纬度转换成地坐标系xy值,也就是说是基于GPS的位置自动控制 。
采用的是等距方位投影的方法(Azimuthal Equidistant Projection)。
方位投影既不是等面积也不是保形的。让φ1和λ0作为投影中心的纬度和经度，则变换方程由下式给出
x=k′cos?sin(λ?λ0)
y=k′[cos?1sin??sin?1cos?sin(λ?λ0)]
这里
k′=csinc
并且
cosc=sin?1sin?+cos?1cos?cos(λ?λ0)]
c在这里代表距中心的角距离(一定点到两物体之间所量度的夹角)。公式的逆表达如下：
?=sin?1(coscsin?1+ysinccos?1c)
以及
λ=???????λ0+tan?1(xsincccos?1cosc?ysin?1sinc),λ0+tan?1(?xy),λ0+tan?1(xy),for?1≠±90°for?1=90°for?1=-90°
到中心的角距离由下式给出：
c=x2+y2??????√
?
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657//将本地方位等距平面中的点投影到地理学坐标系 int map_projection_global_reproject(float x, float y, double *lat, double *lon) { return map_projection_reproject(&mp_ref, x, y, lat, lon); } __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { if (!map_projection_initialized(ref)) { return -1; } double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; // 地球半径 double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); double lat_rad; double lon_rad; if (fabs(c) > DBL_EPSILON) { lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); } else { lat_rad = ref->lat_rad; lon_rad = ref->lon_rad; } *lat = lat_rad * 180.0 / M_PI; // 弧度 -> 度 *lon = lon_rad * 180.0 / M_PI; return 0; }先map_projection_reproject( )再map_projection_project( )。这种方式将位置转换为经纬度和高度， 然后用位置估计参数来更新经纬度和高度，接着转换回位置参考点，属于GPS数据转换的方式。
poll
int poll(struct pollfd fds[], nfds_t nfds, int timeout)
功能：监控文件描述符(多个);
说明：timemout=0,poll()函数立即返回而不阻塞;timeout=INFTIM(-1),poll()会一直阻塞下去，直到检测到return > 0;
参数：
? fds:struct pollfd结构类型的数组;
? nfds:用于标记数组fds中的结构体元素的总数量;
? timeout:是poll函数调用阻塞的时间，单位：毫秒;
返回值：
? >0：数组fds中准备好读、写或出错状态的那些socket描述符的总数量;
? ==0:poll()函数会阻塞timeout所指定的毫秒时间长度之后返回;
? -1:poll函数调用失败;同时会自动设置全局变量errno;
poll( )函数用于监测多个等待事件，若事件未发生，进程睡眠，放弃CPU控制权。若监测的任何一个事件发生，poll函数将唤醒睡眠的进程，并判断是什么等待事件发生，并执行相应的操作。poll( )函数退出后，struct polldf变量的所有值被清零，需要重新设置。
uORB
uORB(Micro Object Request Broker,微对象请求代理器)是PX4/Pixhawk系统中非常重要且关键的一个模块，它肩负了整个系统的数据传输任务，所有的传感器数据、GPS、PPM信号等都要从芯片获取后通过uORB进行传输到各个模块进行计算处理。实际上uORB是一套跨「进程」 的IPC通讯模块。在Pixhawk中， 所有的功能被独立以进程模块为单位进行实现并工作。而进程间的数据交互就由为重要，必须要能够符合实时、有序的特点。
Pixhawk使用的是NuttX实时ARM系统，uORB实际上是多个进程打开同一个设备文件，进程间通过此文件节点进行数据交互和共享。进程通过命名的「总线」交换的消息称之为「主题」(topic)，在Pixhawk 中，一个主题仅包含一种消息类型，通俗点就是数据类型。每个进程可以「订阅」或者「发布」主题，可以存在多个发布者，或者一个进程可以订阅多个主题，但是一条总线上始终只有一条消息。
应用层中操作基础飞行的应用之间都是隔离的，这样提供了一种安保模式，以确保基础操作独立的高级别系统状态的稳定性。而沟通它们的就是uORB。
源码中关于uORB有几个常见的函数：
公告主题
在数据被发布到一个主题前，它必须被公告，发布者可以使用下面的API来公告一个新的主题
extern int orb_advertise(const struct orb_metadata *meta, const void *data);
参数：
?meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;
data:指向一个已被初始化，发布者要发布的数据存储变量的指针;
返回值：
错误则返回ERROR;成功则返回一个可以发布主题的句柄;如果待发布的主题没有定义或声明则会返回-1，然后会将errno赋值为ENOENT;
公告也可以发布初始化数据到主题，meta参数是传递给API的一个指针，指向由ORB_DEFINE()宏定义好的数据，通常使用ORB_ID()宏来根据主题名称获取该指针。请注意，虽然主题更新可以从中断处理函数发布，公告主题必须在常规的线程上下文中执行。
发布更新
一旦公告了一个主题，公告主题后返回的句柄可使用下面的API来发布主题更新。
extern int orb_publish(const struct orb_metadata *meta, int handle, const void *data);
参数：
meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;
handle:orb_advertise函数返回的句柄;
data:指向待发布数据的指针;
返回值：
OK表示成功;错误返回ERROR;否则则有根据的去设置errno;
uORB不会缓存多个更新，当用户检查一个主题，他们将只能看到最新的更新。
订阅主题
订阅主题的要求如下：
调用ORB_DEFINE()或ORB_DEFINE_OPTIONAL()宏(在订阅者的头文件中包含他们) 发布到主题的数据结构定义(通常与发布者使用同一头文件)
如果满足上面的条件后，订阅者可以使用下面的api来订阅一个主题：
extern int orb_subscribe(const struct orb_metadata *meta);
参数：
meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;
返回值：
错误则返回ERROR;成功则返回一个可以读取数据、更新话题的句柄;如果待订阅的主题没有定义或声明则会返回-1，然后会将errno赋值为ENOENT;
即使订阅的主题没有被公告，但是也能订阅成功;但是在这种情况下，却得不到数据，直到主题被公告;如果可选主题不存在于固件之中，订阅到可选的主题将会失败，但其他主题即便发布者没有进行公告也会订阅成功，这样可大大降低系统对启动顺序的安排。
取消订阅
要取消订阅一个主题，可以用下面的API
extern int orb_unsubscribe(int handle);
拷贝数据
订阅者不能引用ORB中存储的数据或其他订阅共享的数据，而是在订阅者请求时从ORB拷贝数据到订阅者的临时缓冲区。副本拷贝的方式可以避免锁定ORB的问题，并保持两者之间(发布者，订阅者)的API接口简单。它也允许订阅者在必要的时候直接修改拷贝副本的数据供自己使用。从订阅的主题中获取数据并将数据保存到buffer中。
当订阅者想要把主题中的最新数据拷贝一份全新的副本，可以使用：
extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer);
参数：
meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;
handle:订阅主题返回的句柄;
buffer:从主题中获取的数据;
返回值：
返回OK表示获取数据成功，错误返回ERROR;否则则有根据的去设置errno;
拷贝是以原子操作进行的，所以可以保证获取到发布者最新的数据。
检查更新
订阅者可以使用下面的API来检查一个主题在发布者最后更新后，有没有人调用过orb_copy来接收，处理：
extern int orb_check(int handle, bool *updated);
语句，很多时候Source Insight无法跳转到
