crazyflie2.0_crazyflie-firmware_PARAM和LOG实现原理

最近读crazyflie-firmware的代码，发现它其中的飞行器这边的变量显示在client端很容易，而且是可以动态调整的，感觉很方便，然后就稍微研究了下实现的过程。

(1) 首先看到Stabilizer.c文件中最后对PARAM参数的定义：

// Params for altitude hold
PARAM_GROUP_START(altHold)
PARAM_ADD(PARAM_FLOAT, aslAlpha, &aslAlpha)
PARAM_ADD(PARAM_FLOAT, aslAlphaLong, &aslAlphaLong)
PARAM_ADD(PARAM_FLOAT, errDeadband, &errDeadband)
PARAM_ADD(PARAM_FLOAT, altHoldChangeSens, &altHoldChange_SENS)
PARAM_ADD(PARAM_FLOAT, altHoldErrMax, &altHoldErrMax)
PARAM_ADD(PARAM_FLOAT, kd, &altHoldKd)
PARAM_ADD(PARAM_FLOAT, ki, &altHoldKi)
PARAM_ADD(PARAM_FLOAT, kp, &altHoldKp)
PARAM_ADD(PARAM_FLOAT, pidAlpha, &pidAlpha)
PARAM_ADD(PARAM_FLOAT, pidAslFac, &pidAslFac)
PARAM_ADD(PARAM_FLOAT, vAccDeadband, &vAccDeadband)
PARAM_ADD(PARAM_FLOAT, vBiasAlpha, &vBiasAlpha)
PARAM_ADD(PARAM_FLOAT, vSpeedAccFac, &vSpeedAccFac)
PARAM_ADD(PARAM_FLOAT, vSpeedASLDeadband, &vSpeedASLDeadband)
PARAM_ADD(PARAM_FLOAT, vSpeedASLFac, &vSpeedASLFac)
PARAM_ADD(PARAM_FLOAT, vSpeedLimit, &vSpeedLimit)
PARAM_ADD(PARAM_UINT16, baseThrust, &altHoldBaseThrust)
PARAM_ADD(PARAM_UINT16, maxThrust, &altHoldMaxThrust)
PARAM_ADD(PARAM_UINT16, minThrust, &altHoldMinThrust)
PARAM_GROUP_STOP(altHold)

猛的一看，好复杂，首先看下PARAM_GROUP_START，PARAM_ADD和PARAM_GROUP_STOP的相关定义：

/* Macros */
//#NAME 是将NAME转换为字符串赋值给name
#define PARAM_ADD(TYPE, NAME, ADDRESS) \
   { .type = TYPE, .name = #NAME, .address = (void*)(ADDRESS), },

#define PARAM_ADD_GROUP(TYPE, NAME, ADDRESS) \
   { \
  .type = TYPE, .name = #NAME, .address = (void*)(ADDRESS), },

#define PARAM_GROUP_START(NAME)  \
  static const struct param_s __params_##NAME[] __attribute__((section(".param." #NAME), used)) = { \
  PARAM_ADD_GROUP(PARAM_GROUP | PARAM_START, NAME, 0x0)

//#define PARAM_GROUP_START_SYNC(NAME, LOCK) PARAM_ADD_GROUP(PARAM_GROUP | PARAM_START, NAME, LOCK);

#define PARAM_GROUP_STOP(NAME) \
  PARAM_ADD_GROUP(PARAM_GROUP | PARAM_STOP, stop_##NAME, 0x0) \
  };

从 PARAM_GROUP_START看到一个结构体param_s，定义如下：

/* Basic parameter structure */
struct param_s {
  uint8_t type;
  char * name;
  void * address;
};

所以这个结构体就是根源，大概猜到是说在各个不同任务中定义的变量会添加到这个结构体中，记录数据类型type(无符号整数，有符号整数，浮点等)，变量名称name和变量的地址，所以这个变量的地址肯定是固定的，不是动态分配的，这个我们可以在 Stabilizer.c文件中看到，需要增加到PARAM中的参数都是静态分配的，这样的话就可以保证变量的地址是固定的。

// Altitude hold & Baro Params
static float altHoldKp              = 0.5;  // PID gain constants, used everytime we reinitialise the PID controller
static float altHoldKi              = 0.18;
static float altHoldKd              = 0.0;
static float altHoldChange          = 0;     // Change in target altitude
static float altHoldTarget          = -1;    // Target altitude
static float altHoldErrMax          = 1.0;   // max cap on current estimated altitude vs target altitude in meters
static float altHoldChange_SENS     = 200;   // sensitivity of target altitude change (thrust input control) while hovering. Lower = more sensitive & faster changes
static float pidAslFac              = 13000; // relates meters asl to thrust
static float pidAlpha               = 0.8;   // PID Smoothing //TODO: shouldnt need to do this
static float vSpeedASLFac           = 0;    // multiplier
static float vSpeedAccFac           = -48;  // multiplier
static float vAccDeadband           = 0.05;  // Vertical acceleration deadband
static float vSpeedASLDeadband      = 0.005; // Vertical speed based on barometer readings deadband
static float vSpeedLimit            = 0.05;  // used to constrain vertical velocity
static float errDeadband            = 0.00;  // error (target - altitude) deadband
static float vBiasAlpha             = 0.98; // Blending factor we use to fuse vSpeedASL and vSpeedAcc
static float aslAlpha               = 0.92; // Short term smoothing
static float aslAlphaLong           = 0.93; // Long term smoothing
static uint16_t altHoldMinThrust    = 00000; // minimum hover thrust - not used yet
static uint16_t altHoldBaseThrust   = 43000; // approximate throttle needed when in perfect hover. More weight/older battery can use a higher value
static uint16_t altHoldMaxThrust    = 60000; // max altitude hold thrust

大概猜到意思之后，深入分析下 PARAM_GROUP_START， PARAM_ADD和 PARAM_GROUP_STOP的实现过程：

先来PARAM_GROUP_START：

#define PARAM_GROUP_START(NAME)  \
  static const struct param_s __params_##NAME[] __attribute__((section(".param." #NAME), used)) = { \
  PARAM_ADD_GROUP(PARAM_GROUP | PARAM_START, NAME, 0x0)

其中##是粘合作用，例如PARAM_GROUP_START(altHold)，展开之后就变成：

  static const struct param_s __params_altHold[] __attribute__((section(".param.altHold), used)) = { \
  PARAM_ADD_GROUP(PARAM_GROUP | PARAM_START, altHold, 0x0)

__attribute__是给结构体param_s设置属性，保存在段.param.altHold中，used同时告诉编译器该部分有用，即使没有用编译也不会有警告。需要注意的是 = 号之后的大括号{，这个是定义结构体的前奏，再来看PARAM_GROUP_STOP：

#define PARAM_GROUP_STOP(NAME) \
  PARAM_ADD_GROUP(PARAM_GROUP | PARAM_STOP, stop_##NAME, 0x0) \
  };

注意最后的大括号和分号 }; ,所以这明显是把结构体的定义拆开了，然后再使用 PARAM_ADD或者PARAM_ADD_GROUP将具体结构体的内容插进去，所以我们可以看到如下的定义结构：

PARAM_GROUP_START（结构体前奏）

PARAM_ADD（结构体内容）

PARAM_ADD（结构体内容）

PARAM_ADD（结构体内容）

...................................................

PARAM_GROUP_STOP（结构体后尾）

所以我们想看什么变量就增加结构体内容就可以了，注意地址一定是用静态变量static定义的或者说地址固定的。

同时我们也可以看到它将变量的类型做了细分：

//变量占用的字节数，定义在低2bit，bit0和bit1
#define PARAM_BYTES_MASK 0x03
#define PARAM_1BYTE  0x00
#define PARAM_2BYTES 0x01
#define PARAM_4BYTES 0x02
#define PARAM_8BYTES 0x03

//整形还是浮点类型,定义在bit2
#define PARAM_TYPE_INT   (0x00<<2)
#define PARAM_TYPE_FLOAT (0x01<<2)

//有符号还是无符号bit3
#define PARAM_SIGNED (0x00<<3)
#define PARAM_UNSIGNED (0x01<<3)

//变量还是组bit7
#define PARAM_VARIABLE (0x00<<7)
#define PARAM_GROUP    (0x01<<7)

//是否是只读信息bit6
#define PARAM_RONLY (1<<6)

//
#define PARAM_START 1
#define PARAM_STOP  0

#define PARAM_SYNC 0x02

所以上述细分定义之后，就可以很容的得到我们想用的type 

// User-friendly macros
#define PARAM_UINT8 (PARAM_1BYTE | PARAM_TYPE_INT | PARAM_UNSIGNED)
#define PARAM_INT8  (PARAM_1BYTE | PARAM_TYPE_INT | PARAM_SIGNED)
#define PARAM_UINT16 (PARAM_2BYTES | PARAM_TYPE_INT | PARAM_UNSIGNED)
#define PARAM_INT16  (PARAM_2BYTES | PARAM_TYPE_INT | PARAM_SIGNED)
#define PARAM_UINT32 (PARAM_4BYTES | PARAM_TYPE_INT | PARAM_UNSIGNED)
#define PARAM_INT32  (PARAM_4BYTES | PARAM_TYPE_INT | PARAM_SIGNED)

#define PARAM_FLOAT (PARAM_4BYTES | PARAM_TYPE_FLOAT | PARAM_SIGNED)

另外LOG和PARAM的实现过程是一样的，从bitcraze官网看到他们对LOG subsystem的解释：

The purpose of the logging is to be able to log variables that are available in the copter during runtime. The Crazyflie log system is design in such a way that the copter firmware is mostly independent from the client and all information required to get the log system to work are communicated at connection time. The copter contains a table of content of all the variable possible to log. The client retrieve the list at connection time. The client then program in the copter log packets (ie. list of variable to log at the same time) to be sent back at regular interval.

This mechanism is used both for regular log operation (ie. user requested log to inspect and debug flight data) but also to update indication in the GUI application.

Each variable to log is contained in a group group and has a name. The system is designed to make it easy to add a log variable in the Firmware and have it available in the ground station GUI:

Firmware usage

A couple of macros are declared in log.h to define new log variables. All log variables should be in a log group. Example:

LOG_GROUP_START(stabilizer)
LOG_ADD(LOG_FLOAT, roll, &eulerRollActual)
LOG_ADD(LOG_FLOAT, pitch, &eulerPitchActual)
LOG_ADD(LOG_FLOAT, yaw, &eulerYawActual)
LOG_GROUP_STOP(stabilizer)

Macro	Usage
LOG_GROUP_START(grp_name)	Start a log group named grp_name
LOG_GROUP_STOP(grp_name)	Stop log group declaration for group grp_name
LOG_ADD(type, name, address)	Add a log variable. type is the variable type (see bellow), name is the name as it will be sent to the ground and address is the memory address of the variable

Type defines	Corresponding C99 type	Note
LOG_UINT8	uint8_t
LOG_UINT16	uint16_t
LOG_UINT32	uint32_t
LOG_INT8	int8_t
LOG_INT16	int16_t
LOG_INT32	int32_t
LOG_FLOAT	float	IEEE 754 binary32 (single precision float)
LOG_FP16	N/A?	IEEE 754 binary16, intended for log report only (not in memory)