本筆記內容基於v1.8.2固件,基於px4官方文檔中章節參數設置。
0. 命令行使用
PX4控制台(NSH)提供了param
命令,可以用該命令來實現參數設置、讀取、保存、導出和導入。
1. 獲得和設置參數
使用param show
命令可以獲得系統的全部參數
並且可以使用通配符搜索
例如:
param shwo RC_MAP_A*
可以使用-c命令來查看已經改變的數值(不同於默認值的)
2. 導出和加載參數
使用 param save
命令可以保存參數到默認文件。
也可以增加一個參數來指定參數文件保存位置。
param save /fs/microsd/vtol_param_backup
可以看出來,存儲后在對應目錄下生成了vtol_param_backup的參數文件。
有兩個命令可以加載參數:param load
和 param import
。
param load
命令首先重置所有參數到默認值,然后再從文件中加載參數並重寫。param import
命令從文件中加載參數,重寫參數值,並保存結果(調用 param save)。
load
命令和import
不同的是load
會恢復文件中保存的參數,而把其他參數恢復為默認值;import
將文件中保存的參數融合進當前的飛機。例如可以用來導入校准參數,而不重置系統設置。
3. 參數命名
參數名字不超過16個ASCII字符。
通常,同一組中的每個參數使用相同的前綴后跟一個下划線,例如MC_
和FW_
分別表示多旋翼和固定翼相關參數,不過該慣例不是強制的。
代碼中的參數命名必須和參數元數據(parameter metadata)中的一致,從而將參數和他的元數據(包括固件中的默認值)關聯起來。
4. C/C++ API
有單獨的C和C++ API用來在PX4的模塊和驅動中訪問參數值。
C++版本的API有一個更有效的標准化機制來同步參數值的更改(從地面站)。
參數同步特別重要,因為參數值在什么時候都有可能更改,如果沒法立即獲得參數的最新值,那就需要在參數改變之后重啟然后更新參數值(可以使用@reboot_required
元數據來設置該特性)。
C++ API還具有更好的類型安全性,並且占用的內存更小。但是C++ API必須在編譯時知道參數名,而C API可以動態使用字符串來創建參數名。
4.1 C++ API
C++ API使用宏來聲明參數作為類屬性。可以添加模板代碼來監聽和參數更新相關的uORB主題。框架代碼跟蹤影響參數特性的uORB消息來維持參數同步。在其他代碼中,你可以使用定義好的參數特性,他們會一直保持同步。
首先,包含px4_platform_common/module_params.h在類的頭文件中(以便可以使用DEFINE_PARAMETERS
宏)。
從類ModuleParams
來派生你的自定義類,並使用DEFINE_PARAMETERS
宏來指定參數列表和對應的參數屬性。參數名必須和他們對應的參數元數據定義一致。
示例代碼如下:
class MyModule : ..., public ModuleParams
{
public:
...
private:
/**
* Check for parameter changes and update them if needed.
* @param parameter_update_sub uorb subscription to parameter_update
*/
void parameters_update(int parameter_update_sub, bool force = false);
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_AUTOSTART>) _sys_autostart, /**< example parameter */
(ParamFloat<px4::params::ATT_BIAS_MAX>) _att_bias_max /**< another parameter */
)
};
然后使用模板來修改cpp源文件檢查和uORB消息相關的參數更新。首先增加如下頭文件來訪問uORB中的parameter_update消息:
#include <uORB/topics/parameter_update.h>
然后在模塊或者驅動啟動時訂閱參數更新消息,並在模塊或驅動結束運行時解除訂閱。orb_subscribe()
返回的parameter_update_sub
是個句柄,我們可以使用該句柄來引用該訂閱。
# Subscribe to parameter_update message
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
...
# Unsubscribe to parameter_update messages
orb_unsubscribe(parameter_update_sub);
在代碼中周期調用parameters_update(parameter_update_sub);
來檢查是否有參數更新:
void Module::parameters_update(int parameter_update_sub, bool force)
{
bool updated;
struct parameter_update_s param_upd;
// Check if any parameter updated
orb_check(parameter_update_sub, &updated);
// If any parameter updated copy it to: param_upd
if (updated) {
orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_upd);
}
if (force || updated) {
// If any parameter updated, call updateParams() to check if
// this class attributes need updating (and do so).
updateParams();
}
}
4.2 C API
C API可以在模塊或者驅動中使用,首先包含參數API頭文件:
#include <parameters/param.h>
然后就可以使用param_get
函數來獲得參數值,並賦給一個變量。例如如下代碼將PARAM_NAME
的值賦給my_param
變量。
int32_t my_param = 0;
param_get(param_find("PARAM_NAME"), &my_param);
如果需要多次讀取參數值,可以緩存參數句柄並且使用param_get來獲得參數值:
# Get the handle to the parameter
param_t my_param_handle = PARAM_INVALID;
my_param_handle = param_find("PARAM_NAME");
# Query the value of the parameter when needed
int32_t my_param = 0;
param_get(my_param_handle, &my_param);
5.參數元數據
PX4使用參數元數據來驅動用戶界面上的參數表達,並設置固件中每個參數的默認值。正確的參數元數據對於地面站的用戶體驗特別重要.
參數元數據可以存在代碼的任何地方,可以使用.c或者.yaml文件來定義參數(YAML定義比較新,且更靈活).參數元數據文件通常存儲在相關模塊中。
編譯系統通過使用make parameters_metadata
來編譯參數參考和地面站使用的參數信息。在增加新的參數之后,在重新編譯之前必須使用make clean
來生成新的參數。
5.1 C參數元數據
C語言的參數元數據定義在.c文件中(當前代碼中使用的最多的一種方式)。
參數元數據部分如下列代碼所示:
/**
* Pitch P gain
*
* Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
*
* @unit 1/s
* @min 0.0
* @max 10
* @decimal 2
* @increment 0.0005
* @reboot_required true
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f);
/**
* Acceleration compensation based on GPS
* velocity.
*
* @group Attitude Q estimator
* @boolean
*/
PARAM_DEFINE_INT32(ATT_ACC_COMP, 1);
可以看出,C語言版本的參數元數據采用PARAM_DEFINE_*
宏來定義新參數,用注釋來表示元數據(分組,說明,類型等信息),每行注釋的說明如下所示(更多信息見module_schema.yaml):
/**
* <title>
*
* <longer description, can be multi-line>
*
* @unit <the unit, e.g. m for meters>
* @min <the minimum sane value. Can be overridden by the user>
* @max <the maximum sane value. Can be overridden by the user>
* @decimal <the minimum sane value. Can be overridden by the user>
* @increment <the "ticks" in which this value will increment in the UI>
* @reboot_required true <add this if changing the param requires a system restart.>
* @boolean <add this for integer parameters that represent a boolean value>
* @group <a title for parameters that form a group>
*/
5.2 YAML 元數據
在當前階段YAML的參數元數據定義不能在Libraries里面使用
YAML用來代替.c定義方式,支持所有同樣的元數據,並且支持一些新特性,例如多實例定義。
- YAML參數元數據框架見:module_schema.yaml
- 在MAVLink參數定義中,使用了YAML定義,見:/src/modules/mavlink/module.yaml
多實例(模板)參數元數據定義
YAML中支持多實例參數的定義,可以在參數名和參數描述中使用${i}
。例如,要生成 MY_PARAM_1_RATE
和MY_PARAM_2_RATE
等,可以使用如下代碼:
MY_PARAM_${i}_RATE:
description:
short: Maximum rate for instance ${i}
6. 試驗
由於代碼中使用C方式增加參數的比較多,所以在此使用C API方式添加自定義參數,例如某飛機要使用兩套參數進行切換,其中一套標准的,一套機翼比較短,我們以俯仰角速率控制為例,原本俯仰角速率積分系數元數據定義為:
/**
* Pitch rate integrator gain.
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @unit %/rad
* @min 0.005
* @max 0.5
* @decimal 3
* @increment 0.005
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_I, 0.02f);
我們增加一個后綴SS的,表示Short Span,元數據定義如下:
/**
* Short Span Pitch rate integrator gain.
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @unit %/rad
* @min 0.005
* @max 0.5
* @decimal 3
* @increment 0.005
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_I_SP, 0.01f);
運行make clean
和make parameters_metadata
,然后編譯固件並下載到飛控,然后進Mavlink的控制台,用param show
指令可以看到該參數:
但是可以看到該參數沒有使用,所以在QGC的Parameters標簽頁的FW Attitude Control中是看不到的,經過查找,發現在Other->Misc組里,好像要在QGC里對應到FW Attitude Control組里的話,需要在QGC源碼里添加對應分組信息,並重新編譯。
要使用該參數,在FixedwingAttitudeControl.hpp中的_parameter_handels
結構體中加入param_t p_i_ss;
並在_parameters
中定義該參數float p_i_ss;
。然后在FixedwingAttitudeControl::parameters_update()
函數中加入
param_get(_parameter_handles.p_i_ss, &(_parameters.p_i_ss));
即可將_parameters.p_i_ss的值更新為參數值。