Pixhawk原生固件PX4之常用函数解读

sky

经常有人将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_ctrl产生期望力矩

• 最后通过mixer和motor_driver控制电机

  
  

启动函数

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

• 1
  

命令名和各参数之间用空格分隔。命令名是可执行文件名(此文件包含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这个文件。

• 
  
  extern "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;
  
• 
  
  }
  
  

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
  

在一系列头文件之后，这里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( ) 吧：

坐标系

惯性导航的基础是精确定义一系列的笛卡儿参考坐标系，每一个坐标系都是正交的右手坐标系或轴系。

对地球上进行的导航，所定义的坐标系要将惯导系统的测量值与地球的主要方向联系起来。也就是说，当在近地面导航时，该坐标系具有实际意义。因此，习惯上将原点位于地球中心、相对于恒星固定的坐标系定义为惯性参考坐标系，下图给出了用于陆地导航的固连于地球的参考坐标系和当地地理导航坐标系以及惯性参考坐标系。

• 地球坐标系（系）。原点位于地球中心，坐标轴与地球固连，轴向定义为,, 。其中， 沿地球极轴方向，轴沿格林尼泊子午面和地球赤道平面的交线。地球坐标系相对于惯性坐标系绕 轴以角速度转动。

• 导航坐标系(系)。是一种当地地理坐标系，原点位于导航系统所处的位置点，坐标轴指向北、东和当地垂线方向(向下)。导航坐标系相对于地球固连坐标系的旋转角速率取决于P 点相对于地球的运动，通常称为转移速率。

• 载体坐标系(系)。一个正交坐标系，轴向分别沿安装有导航系统的运载体的横滚轴、俯仰轴和偏航轴。

  
  

在PX4中，

Local position setpoint in NED frame 导航坐标系(以起飞点home为原点) 北东地 xyz

Global position setpoint in NED frame 由位置估计器发布的位置信息，融合了包括GPS原始信息的多个传感器数据

GPS position in WGS84 coordinates 为GPS全球定位系统使用而建立的坐标系统（原始GPS信息）

NED earth-fixed frame 个人觉得是GPS投影到地面的坐标系,原点？

NED body-fixed frame 机体坐标系，轴正方向为机头，轴正方向下

这里有两个函数不得不提：

• 
  
  //将地理学坐标系(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;
  
• 
  
  }
  
  

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
  

将球面坐标转化为平面坐标的过程便称为投影。这里将经纬度转换成地坐标系xy值,也就是说是基于GPS的位置自动控制 。

采用的是等距方位投影的方法(Azimuthal Equidistant Projection)。

方位投影既不是等面积也不是保形的。让和作为投影中心的纬度和经度，则变换方程由下式给出

这里

并且

在这里代表距中心的角距离(一定点到两物体之间所量度的夹角)。公式的逆表达如下：

以及

到中心的角距离由下式给出：

• 
  
  //将本地方位等距平面中的点投影到地理学坐标系
  
• 
  
  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;
  
• 
  
  }
  
  

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
  

先map_projection_reproject( )再map_projection_project( )。这种方式将位置转换为经纬度和高度， 然后用位置估计参数来更新经纬度和高度，接着转换回位置参考点，属于GPS数据转换的方式。

pollint poll(struct pollfd fds[], nfds_t nfds, int timeout)

• 1
  

功能：监控文件描述符（多个）；
说明：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);

• 1
  

参数：

​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);

• 1
  

参数：

meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;

handlerb_advertise函数返回的句柄；

data:指向待发布数据的指针；

返回值：

OK表示成功；错误返回ERROR；否则则有根据的去设置errno;

uORB不会缓存多个更新，当用户检查一个主题，他们将只能看到最新的更新。

• 订阅主题
  

订阅主题的要求如下：

• 调用ORB_DEFINE()或ORB_DEFINE_OPTIONAL()宏（在订阅者的头文件中包含他们）
• 发布到主题的数据结构定义（通常与发布者使用同一头文件）
  

如果满足上面的条件后，订阅者可以使用下面的api来订阅一个主题：

extern int orb_subscribe(const struct orb_metadata *meta);

• 1
  

参数：

meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值；

返回值：

错误则返回ERROR;成功则返回一个可以读取数据、更新话题的句柄；如果待订阅的主题没有定义或声明则会返回-1，然后会将errno赋值为ENOENT;

即使订阅的主题没有被公告，但是也能订阅成功；但是在这种情况下，却得不到数据，直到主题被公告；如果可选主题不存在于固件之中，订阅到可选的主题将会失败，但其他主题即便发布者没有进行公告也会订阅成功，这样可大大降低系统对启动顺序的安排。

• 取消订阅
  

要取消订阅一个主题，可以用下面的API

extern int orb_unsubscribe(int handle);  

• 1
  

• 拷贝数据
  

订阅者不能引用ORB中存储的数据或其他订阅共享的数据，而是在订阅者请求时从ORB拷贝数据到订阅者的临时缓冲区。副本拷贝的方式可以避免锁定ORB的问题，并保持两者之间（发布者，订阅者）的API接口简单。它也允许订阅者在必要的时候直接修改拷贝副本的数据供自己使用。从订阅的主题中获取数据并将数据保存到buffer中。

当订阅者想要把主题中的最新数据拷贝一份全新的副本，可以使用：

extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer);   

• 1
  

参数：

meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;

handle:订阅主题返回的句柄；

buffer:从主题中获取的数据；

返回值：

返回OK表示获取数据成功，错误返回ERROR;否则则有根据的去设置errno;

拷贝是以原子操作进行的，所以可以保证获取到发布者最新的数据。

• 检查更新
  

订阅者可以使用下面的API来检查一个主题在发布者最后更新后，有没有人调用过orb_copy来接收，处理：

extern int orb_check(int handle, bool *updated);

• 1
  

参数：

​ handle:主题句柄；

​ updated:如果当最后一次更新的数据被获取了，检测到并设置updated为ture;

返回值：

​ OK表示检测成功；错误返回ERROR;否则则有根据的去设置errno;

如果主题在被公告前就有人订阅，那么这个API将返回“not-updated”直到主题被公告。

• 发布时间戳

  订阅者可以使用下面的API来检查一个主题最后发布的时间:

  
  

extern int orb_stat(int handle, uint64_t *time);

• 1
  

参数：

​ handle:主题句柄；

​ *time:时间指针；

返回值：

​ 主题最后的发布时间。

需要注意的是，要小心的使用这个调用，因为不能保证再调用返回后不久主题就不会被发布（调用返回后不久，主题可能马上又被发布，导致最后更新时间错误）

注意：

• 必须有了orb_advertise( )函数和orb_subscribe( )函数(通过它们可以获取orb_copy( )函数中的参数handle句柄)之后才可以使用orb_copy( )函数来获取订阅的主题数据。
• 源码中常用的调用顺序为: orb_subscribe( ) - > orb_check( ) -> orb_copy( )
  

Topics

所有的主题都可以在Firmware/Build_px4fmu-v2_default/Src/Modules/uORB/Topics文件夹下找到。
对于xxx_sub = orb_subscribe(ORB_ID(xxx))这类语句，很多时候Source Insight无法跳转到这些主题（ORB_ID）定义的地方，可以去Topics中查看。

或者也可以从Msg中查看这些结构体成员的具体使用情况，消息在Firmware/Msg下都可以找到。

Daemon

守护进程daemon是运行在后台的进程。
在NuttX中守护进程是一个任务，在POSIX(Linux/Mac OS)中是一个线程

• 
  
  daemon_task = px4_task_spawn_cmd("commander",
  
• 
  
                   SCHED_DEFAULT,
  
• 
  
                   SCHED_PRIORITY_DEFAULT + 40,
  
• 
  
                   3600,
  
• 
  
                   commander_thread_main,
  
• 
  
                   (char * const *)&argv[0]);
  
  

• 1
• 2
• 3
• 4
• 5
• 6
  

以下是参数：
- arg0: 进程名 commander
- arg1: 调度类型（RR or FIFO）the scheduling type (RR or FIFO)
- arg2: 调度优先级
- arg3: 新进程或线程堆栈大小
- arg4: 任务/线程主函数
- arg5: 一个void指针传递给新任务,在这种情况下是命令行参数

在Unix和其他多任务操作系统中daemon程序是指作为一个后台进程运行的计算机程序，而不是由用户直接控制的程序，daemon概念的好处是它不需要被用户或者shell发送到后台就能被启动，并且当它在运行时可以通过shell查询它的状态，它也可以被终止。

后台应用程序只是暂时存在用与开始后台作业，在MakeFile中指定的堆栈大小仅适用于这个管理任务。实际的堆栈大小应在task_create( )调用中设置。

主函数由一个daemon控制函数代替，主函数中原来的部分现在由一个后台任务(task)/线程(thread)来代替。

数据操作

• memset

  void *memset(void *s, int c, size_t n)
  
  • 1
    

  参数：

  ​ dest 目标指针；

  ​ src 数据源；

  ​ n 数据长短

  功能：

  ​ 作用是在一段内存块中填充某个给定的值，将s中当前位置后面的n个字节 （typedef unsigned int size_t ）用 c 替换并返回 s

  例子：

  memset(&actuator_controls, 0, sizeof(actuator_controls));
  • 1
    

  表示将actuator_controls的各位置0

  ​

• memcpy

  
  
  • 
    
    FAR void *memcpy(FAR void *dest, FAR const void *src, size_t n)
    
  • 
    
    {
    
  • 
    
    FAR unsigned char pout = (FAR unsigned char)dest;
    
  • 
    
    FAR unsigned char pin  = (FAR unsigned char)src;
    
  • 
    
    while (n-- > 0) *pout++ = *pin++;
    
  • 
    
    return dest;
    
  • 
    
    }
    
    
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
    

  参数：

  ​ dest 目标指针；

  ​ src 数据源；

  ​ n 数据长短

  功能：

  ​ 从源src所指的内存地址的起始位置开始拷贝n个字节到目标dest所指的内存地址的起始位置中。

  例子：

  memcpy(val, v, param_size(param));
  • 1
    

  表示将v中的前n个值复制到val中

  ​

• param_find

  
  
  • 
    
    uintptr_t param_find(const char *name)   //unsigned int pointer
    
  • 
    
    {
    
  • 
    
    return param_find_internal(name, false);
    
  • 
    
    }
    
  • 
    
    
    
  • 
    
    uintptr_t param_find_internal(const char *name, bool notification)
    
  • 
    
    {
    
  • 
    
    uintptr_t param;
    
  • 
    
    /* perform a linear search of the known parameters */
    
  • 
    
      // 对已知参数执行线性搜索
    
  • 
    
    for (param = 0; handle_in_range(param); param++) {
    
  • 
    
        if (!strcmp(param_info_base[param].name, name)) {
    
  • 
    
            if (notification) {
    
  • 
    
                param_set_used_internal(param);
    
  • 
    
            }
    
  • 
    
            return param;
    
  • 
    
        }
    
  • 
    
    }
    
  • 
    
    /* not found 未找到对应的参数*/
    
  • 
    
    return PARAM_INVALID;  // 参数无效
    
  • 
    
    }
    
    
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
    

  参数：

  ​ *name 指针名

  功能：

  ​ 返回指针指向的值

  例子：

  • 
    
    param_t _param_system_id = param_find("MAV_SYS_ID");
    
  • 
    
    // 其中
    
  • 
    
    PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
    
    
  • 1
  • 2
  • 3
    

  表示将MAV_SYS_ID所代表的值1赋给_param_system_id

  ​

• param_get

  
  
  • 
    
    int param_get(param_t param, void *val)
    
  • 
    
    {
    
  • 
    
    int result = -1;
    
  • 
    
    param_lock(); // lock和unlock主要就是通过sem信号量控制对某一数据的互斥访问
    
  • 
    
    const void *v = param_get_value_ptr(param);
    
  • 
    
    if (val != NULL) {
    
  • 
    
        memcpy(val, v, param_size(param));
    
  • 
    
        result = 0;
    
  • 
    
    }
    
  • 
    
    param_unlock();
    
  • 
    
    return result;
    
  • 
    
    }
    
    
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
    

  参数：

  ​ param 源参数；

  ​ val 目标参数指针

  功能：获取param的地址并赋给val

  例子：

  param_get(_param_system_id, &(status.system_id));
  • 1
    

  表示将_param_system_id的地址写入status.system_id中

  
  

param_find与param_get是一起使用的，先find定义的值再get到目标指针变量中

nsh

关于terateam中的nsh乱码问题 先拔内存卡
依然乱码
修改掉rcS
具体做法改为

• 新固件 (line766)
  

• 
  
      # Start MAVLink
  
• 
  
      mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
  
  

• 1
• 2
  

按老固件方法加一个判断

• 
  
      # Start USB shell if no microSD present, MAVLink else
  
• 
  
      if [ $LOG_FILE == /dev/null ]
  
• 
  
      then
  
• 
  
          # Try toget an USB console
  
• 
  
          nshterm /dev/ttyACM0 &
  
• 
  
      else
  
• 
  
          mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
  
• 
  
      fi
  
  

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
  

进而make并upload

掉高

在一个执行器饱和的情况下，所有执行器的值将被重新缩放来使得饱和的那个执行器的值限制在1.0以内（由于是同时缩放所有执行器的值，根据比例钳位（Ratio Clamping）的原理，各个执行器间大小的比例不变，从而防止出现侧翻的情况。当然，这样会导致掉高情况的出现）
以上是对pixhawk掉高的解释，在mixer中进行了说明,位置是Firmware/Romfs/Px4fmu_common/Mixers

任何钳位都是根据输入的比例进行的，如果一个电机出现正饱和或者负饱和，那么总的油门将会减小，从而淡出各个电机点的比例将会得到满足而不会饱和，保证了飞行器不会出现侧翻的情况。

现举例说明四旋翼的钳位：

输入（四个电机），限制为100：

150 75 75 75

钳位结果：

100 50 50 50

本例中，进行钳位后，电机1的转速依然是另外三个的两倍，所以目标姿态会是正确的。相比于没有进行钳位的情况，飞行器将不会增加高度。如果值是对电机1进行了限制（输出100 75 75 75），飞行器将出现翻滚或者不稳定的情况，这是因为电机间的比例发生了变化。

查看Firmware版本

• Console / shell

  
  
  • 
    
    
    
  • 
    
    #../px4/Firmware
    
  • 
    
    
    
  • 
    
    git describe --always --tags
    
    
  • 1
  • 2
  • 3
  • 4
    

  将输出git clone下来的固件版本

  v1.5.2-661-gc2db188
  • 1
    

• NSH

  nsh>ver all
  
  • 1
    

  将输出

  • 
    
    HW arch: PX4FMU_V2
    
  • 
    
    FW git-hash: c2db1886ac12475677ef691a84a62975c162e8ae
    
  • 
    
    FW version: 1.5.2 0 (17105408)
    
  • 
    
    OS: NuttX
    
  • 
    
    OS version: Release 1.8.0 (17301759)
    
  • 
    
    Build datetime: Jan 16 2017 19:30:23
    
  • 
    
    Build uri: localhost
    
  • 
    
    Toolchain: GNU GCC, 5.4.1 20160609 (release) [ARM/embedded-5-branch revision 237715]
    
  • 
    
    MCU: STM32F42x, rev. 1
    
  • 
    
    UID: 40002D:31345116:36383835
    
    
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
    

  可知当前固件版本为v1.5.2