Ardupilot 新增一個新的引數在地面站上顯示(Adding a New Parameter to Copter)
目錄
摘要
本節主要記錄自己在ardupilot多旋翼無人機程式碼中新增一個新的引數,並顯示到地面站上;同時實現美國手與日本手切換的過程。歡迎批評指正!!!
一.ardupilot官網增加一個新的引數
1.官方連結
2.文件翻譯
引數既可以是主程式碼的一部分,也可以是庫的一部分。
1.增加一個引數到主程式碼
第1步:在這個檔案(Parameters.h)中找到引數類的列舉的備用間隙,並新增新引數(也就是找個沒有被佔用的列舉資料,新增自己的引數),如下面所示的紅色。
enum {
// Misc
//
k_param_log_bitmask = 20,
k_param_log_last_filenumber, // *** Deprecated - remove
// with next eeprom number
// change
k_param_toy_yaw_rate, // THOR The memory
// location for the
// Yaw Rate 1 = fast,
// 2 = med, 3 = slow
k_param_crosstrack_min_distance, // deprecated - remove with next eeprom number change
k_param_rssi_pin,
k_param_throttle_accel_enabled, // deprecated - remove
k_param_wp_yaw_behavior,
k_param_acro_trainer,
k_param_pilot_velocity_z_max,
k_param_circle_rate,
k_param_sonar_gain,
k_param_ch8_option,
k_param_arming_check_enabled,
k_param_sprayer,
k_param_angle_max,
k_param_gps_hdop_good, // 35
//新增加的引數:這裡為什麼要這樣寫後面會講解,一定要這樣寫,不然會報錯。一定要k_param_開頭,當然這個程式碼已經比較古老了,可以看下怎麼新增最新的,我也貼出自己新增的引數
k_param_my_new_parameter, // 36
官網上的文件太古老了,這裡重新寫個程式碼(直接使用254,最後一個列舉數)
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
k_param_ins_old, // *** Deprecated, remove with next eeprom number change
k_param_ins, // libraries/AP_InertialSensor variables
k_param_NavEKF2_old, // deprecated - remove
k_param_NavEKF2,
k_param_g2, // 2nd block of parameters
k_param_NavEKF3,
// simulation
k_param_sitl = 10,
// barometer object (needed for SITL)
k_param_barometer,
// scheduler object (for debugging)
k_param_scheduler,
// relay object
k_param_relay,
// (old) EPM object
k_param_epm_unused,
// BoardConfig object
k_param_BoardConfig,
// GPS object
k_param_gps,
// Parachute object
k_param_parachute,
// Landing gear object
k_param_landinggear, // 18
// Input Management object
k_param_input_manager, // 19
// Misc
//
k_param_log_bitmask_old = 20, // Deprecated
k_param_log_last_filenumber, // *** Deprecated - remove
// with next eeprom number
// change
k_param_toy_yaw_rate, // deprecated - remove
k_param_crosstrack_min_distance, // deprecated - remove with next eeprom number change
k_param_rssi_pin, // unused, replaced by rssi_ library parameters
k_param_throttle_accel_enabled, // deprecated - remove
k_param_wp_yaw_behavior,
k_param_acro_trainer,
k_param_pilot_speed_up,
k_param_circle_rate, // deprecated - remove
k_param_rangefinder_gain,
k_param_ch8_option,
k_param_arming_check_old, // deprecated - remove
k_param_sprayer,
k_param_angle_max,
k_param_gps_hdop_good,
k_param_battery,
k_param_fs_batt_mah,
k_param_angle_rate_max, // remove
k_param_rssi_range, // unused, replaced by rssi_ library parameters
k_param_rc_feel_rp,
k_param_NavEKF, // deprecated - remove
k_param_mission, // mission library
k_param_rc_13_old,
k_param_rc_14_old,
k_param_rally,
k_param_poshold_brake_rate,
k_param_poshold_brake_angle_max,
k_param_pilot_accel_z,
k_param_serial0_baud, // deprecated - remove
k_param_serial1_baud, // deprecated - remove
k_param_serial2_baud, // deprecated - remove
k_param_land_repositioning,
k_param_rangefinder, // rangefinder object
k_param_fs_ekf_thresh,
k_param_terrain,
k_param_acro_rp_expo,
k_param_throttle_deadzone,
k_param_optflow,
k_param_dcmcheck_thresh, // deprecated - remove
k_param_log_bitmask,
k_param_cli_enabled,
k_param_throttle_filt,
k_param_throttle_behavior,
k_param_pilot_takeoff_alt, // 64
// 65: AP_Limits Library
k_param_limits = 65, // deprecated - remove
k_param_gpslock_limit, // deprecated - remove
k_param_geofence_limit, // deprecated - remove
k_param_altitude_limit, // deprecated - remove
k_param_fence,
k_param_gps_glitch, // deprecated
k_param_baro_glitch, // 71 - deprecated
// AP_ADSB Library
k_param_adsb, // 72
k_param_notify, // 73
// 74: precision landing object
k_param_precland = 74,
//
// 75: Singlecopter, CoaxCopter
//
k_param_single_servo_1 = 75, // remove
k_param_single_servo_2, // remove
k_param_single_servo_3, // remove
k_param_single_servo_4, // 78 - remove
//
// 80: Heli
//
k_param_heli_servo_1 = 80, // remove
k_param_heli_servo_2, // remove
k_param_heli_servo_3, // remove
k_param_heli_servo_4, // remove
k_param_heli_pitch_ff, // remove
k_param_heli_roll_ff, // remove
k_param_heli_yaw_ff, // remove
k_param_heli_stab_col_min, // remove
k_param_heli_stab_col_max, // remove
k_param_heli_servo_rsc, // 89 = full! - remove
//
// 90: misc2
//
k_param_motors = 90,
k_param_disarm_delay,
k_param_fs_crash_check,
k_param_throw_motor_start,
k_param_terrain_follow, // 94
k_param_avoid,
k_param_avoidance_adsb,
// 97: RSSI
k_param_rssi = 97,
//
// 100: Inertial Nav
//
k_param_inertial_nav = 100, // deprecated
k_param_wp_nav,
k_param_attitude_control,
k_param_pos_control,
k_param_circle_nav, // 104
k_param_loiter_nav, // 105
// 110: Telemetry control
//
k_param_gcs0 = 110,
k_param_gcs1,
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial1_baud_old, // deprecated
k_param_telem_delay,
k_param_gcs2,
k_param_serial2_baud_old, // deprecated
k_param_serial2_protocol, // deprecated
k_param_serial_manager,
k_param_ch9_option,
k_param_ch10_option,
k_param_ch11_option,
k_param_ch12_option,
k_param_takeoff_trigger_dz,
k_param_gcs3,
k_param_gcs_pid_mask, // 126
//
// 135 : reserved for Solo until features merged with master
//
k_param_rtl_speed_cms = 135,
k_param_fs_batt_curr_rtl,
k_param_rtl_cone_slope, // 137
//
// 140: Sensor parameters
//
k_param_imu = 140, // deprecated - can be deleted
k_param_battery_monitoring = 141, // deprecated - can be deleted
k_param_volt_div_ratio, // deprecated - can be deleted
k_param_curr_amp_per_volt, // deprecated - can be deleted
k_param_input_voltage, // deprecated - can be deleted
k_param_pack_capacity, // deprecated - can be deleted
k_param_compass_enabled,
k_param_compass,
k_param_rangefinder_enabled_old, // deprecated
k_param_frame_type,
k_param_optflow_enabled, // deprecated
k_param_fs_batt_voltage,
k_param_ch7_option,
k_param_auto_slew_rate, // deprecated - can be deleted
k_param_rangefinder_type_old, // deprecated
k_param_super_simple = 155,
k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change
k_param_copter_leds_mode, // deprecated - remove with next eeprom number change
k_param_ahrs, // AHRS group // 159
//
// 160: Navigation parameters
//
k_param_rtl_altitude = 160,
k_param_crosstrack_gain, // deprecated - remove with next eeprom number change
k_param_rtl_loiter_time,
k_param_rtl_alt_final,
k_param_tilt_comp, //164 deprecated - remove with next eeprom number change
//
// Camera and mount parameters
//
k_param_camera = 165,
k_param_camera_mount,
k_param_camera_mount2, // deprecated
//
// Batery monitoring parameters
//
k_param_battery_volt_pin = 168, // deprecated - can be deleted
k_param_battery_curr_pin, // 169 deprecated - can be deleted
//
// 170: Radio settings
//
k_param_rc_1_old = 170,
k_param_rc_2_old,
k_param_rc_3_old,
k_param_rc_4_old,
k_param_rc_5_old,
k_param_rc_6_old,
k_param_rc_7_old,
k_param_rc_8_old,
k_param_rc_10_old,
k_param_rc_11_old,
k_param_throttle_min, // remove
k_param_throttle_max, // remove
k_param_failsafe_throttle,
k_param_throttle_fs_action, // remove
k_param_failsafe_throttle_value,
k_param_throttle_trim, // remove
k_param_esc_calibrate,
k_param_radio_tuning,
k_param_radio_tuning_high,
k_param_radio_tuning_low,
k_param_rc_speed = 192,
k_param_failsafe_battery_enabled,
k_param_throttle_mid, // remove
k_param_failsafe_gps_enabled, // remove
k_param_rc_9_old,
k_param_rc_12_old,
k_param_failsafe_gcs,
k_param_rcmap, // 199
//
// 200: flight modes
//
k_param_flight_mode1 = 200,
k_param_flight_mode2,
k_param_flight_mode3,
k_param_flight_mode4,
k_param_flight_mode5,
k_param_flight_mode6,
k_param_simple_modes,
//
// 210: Waypoint data
//
k_param_waypoint_mode = 210, // remove
k_param_command_total, // remove
k_param_command_index, // remove
k_param_command_nav_index, // remove
k_param_waypoint_radius, // remove
k_param_circle_radius, // remove
k_param_waypoint_speed_max, // remove
k_param_land_speed,
k_param_auto_velocity_z_min, // remove
k_param_auto_velocity_z_max, // remove - 219
k_param_land_speed_high,
//
// 220: PI/D Controllers
//
k_param_acro_rp_p = 221,
k_param_axis_lock_p, // remove
k_param_pid_rate_roll, // remove
k_param_pid_rate_pitch, // remove
k_param_pid_rate_yaw, // remove
k_param_p_stabilize_roll, // remove
k_param_p_stabilize_pitch, // remove
k_param_p_stabilize_yaw, // remove
k_param_p_pos_xy,
k_param_p_loiter_lon, // remove
k_param_pid_loiter_rate_lat, // remove
k_param_pid_loiter_rate_lon, // remove
k_param_pid_nav_lat, // remove
k_param_pid_nav_lon, // remove
k_param_p_alt_hold,
k_param_p_vel_z,
k_param_pid_optflow_roll, // remove
k_param_pid_optflow_pitch, // remove
k_param_acro_balance_roll_old, // remove
k_param_acro_balance_pitch_old, // remove
k_param_pid_accel_z,
k_param_acro_balance_roll,
k_param_acro_balance_pitch,
k_param_acro_yaw_p,
k_param_autotune_axis_bitmask,
k_param_autotune_aggressiveness,
k_param_pi_vel_xy,
k_param_fs_ekf_action,
k_param_rtl_climb_min,
k_param_rpm_sensor,
k_param_autotune_min_d, // 251
k_param_arming, // 252 - AP_Arming
k_param_DataFlash = 253, // 253 - Logging Group
/********************這裡是我自己新增的引數****************/
k_param_radio_mode = 254, // 254 - radio mode American or japan arm
/********************這裡是我自己新增的引數****************/
// the k_param_* space is 9-bits in size
// 511: reserved
};
需要注意的:
(1)儘量在執行類似功能的引數區域新增新的引數,或者最壞的情形下就是在“Misc(混合)”區域的末尾新增。(個人建議可以在最後新增好點)
2)確保你新增的引數區域中還可以有編號新增新的引數。檢查是否能繼續新增引數的方法是:檢查引數的計數,確保你所要新增的引數的上一個元素編號要小於你的下一部分程式碼的編號。比如,Misc部分的第一個引數起始於#20,。my_new_parameter是#36。如果下一部分引數開始於#36,那麼我們就不能在這裡新增這個新引數。
3)不要在一個程式碼塊的中間新增新的引數,那樣容易造成現存引數對應的資訊的改變。
4)不要在引數旁邊用“棄用(deprecated)”或“移除(remove)”做註解,這是因為一些使用者將此註釋用作在eeprom上的舊的引數的預設註解,如果你新增的新引數也是這樣註解,那麼就讓人就會看起來很奇怪和疑惑。
第2步:
在列舉變數後面的引數類中宣告上面列舉變種提到的引數。可使用的型別包括
AP_Int8,AP_Int16,AP_Float,AP_Int32,AP_Vector3
(目前還不支援unsigned integer無符號整型)。新的列舉變數的名稱應該保持一致,只是去掉了字首k_param_。
// 254,255: reserved
};
AP_Int16 format_version;
AP_Int8 software_type;
// Telemetry control
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 serial3_baud;
AP_Int8 telem_delay;
AP_Int16 rtl_altitude;
AP_Int8 sonar_enabled;
AP_Int8 sonar_type; // 0 = XL, 1 = LV,
// 2 = XLL (XL with 10m range)
// 3 = HRLV
AP_Float sonar_gain;
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only,
// 4=voltage and current
AP_Float volt_div_ratio;
AP_Float curr_amp_per_volt;
AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled
AP_Int8 failsafe_gps_enabled; // gps failsafe enabled
AP_Int8 failsafe_gcs; // ground station failsafe behavior
AP_Int16 gps_hdop_good; // GPS Hdop value below which represent a good position
//注意這裡是官網上新增的引數,我現在還是按自己的新增
AP_Int16 my_new_parameter; // my new parameter's description goes here
我自己新增的引數
AP_Int16 format_version;
AP_Int8 software_type;
// Telemetry control
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 telem_delay;
#if CLI_ENABLED == ENABLED
AP_Int8 cli_enabled;
#endif
AP_Float throttle_filt;
AP_Int16 throttle_behavior;
AP_Int16 takeoff_trigger_dz;
AP_Float pilot_takeoff_alt;
AP_Int16 rtl_altitude;
AP_Int16 rtl_speed_cms;
AP_Float rtl_cone_slope;
AP_Float rangefinder_gain;
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled
AP_Float fs_batt_voltage; // battery voltage below which failsafe will be triggered
AP_Float fs_batt_mah; // battery capacity (in mah) below which failsafe will be triggered
AP_Int8 failsafe_gcs; // ground station failsafe behavior
AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position
AP_Int8 compass_enabled;
AP_Int8 super_simple;
AP_Int16 rtl_alt_final;
AP_Int16 rtl_climb_min; // rtl minimum climb in cm
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
//AP_Int8 rc_feel_rp; // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp
AP_Int16 poshold_brake_rate; // PosHold flight mode's rotation rate during braking in deg/sec
AP_Int16 poshold_brake_angle_max; // PosHold flight mode's max lean angle during braking in centi-degrees
// Waypoints
//
AP_Int32 rtl_loiter_time;
AP_Int16 land_speed;
AP_Int16 land_speed_high;
AP_Int16 pilot_speed_up; // rename pilot_speed_up maximum vertical velocity the pilot may request
AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request
// Throttle
//
AP_Int8 failsafe_throttle;
AP_Int16 failsafe_throttle_value;
AP_Int16 throttle_deadzone;
// Flight modes
//
AP_Int8 flight_mode1;
AP_Int8 flight_mode2;
AP_Int8 flight_mode3;
AP_Int8 flight_mode4;
AP_Int8 flight_mode5;
AP_Int8 flight_mode6;
AP_Int8 simple_modes;
// Misc
//
AP_Int32 log_bitmask;
AP_Int8 esc_calibrate;
AP_Int8 radio_tuning;
AP_Int16 radio_tuning_high;
AP_Int16 radio_tuning_low;
AP_Int8 frame_type;
AP_Int8 ch7_option;
AP_Int8 ch8_option;
AP_Int8 ch9_option;
AP_Int8 ch10_option;
AP_Int8 ch11_option;
AP_Int8 ch12_option;
AP_Int8 disarm_delay;
AP_Int8 land_repositioning;
AP_Int8 fs_ekf_action;
AP_Int8 fs_crash_check;
AP_Float fs_ekf_thresh;
AP_Int16 gcs_pid_mask;
AP_Int8 throw_motor_start;
AP_Int8 terrain_follow;
AP_Int16 rc_speed; // speed of fast RC Channels in Hz
// Acro parameters
AP_Float acro_rp_p;
AP_Float acro_yaw_p;
AP_Float acro_balance_roll;
AP_Float acro_balance_pitch;
AP_Int8 acro_trainer;
AP_Float acro_rp_expo;
// Autotune
AP_Int8 autotune_axis_bitmask;
AP_Float autotune_aggressiveness;
AP_Float autotune_min_d;
AP_Int32 Zigzag_time;
AP_Int8 Zigzag_width;
AP_Int8 radio_type;
//增加新的遙控器型別引數
AP_Int8 radio_mode ;
第3步:
將變數宣告新增到Parameters.cpp的var_info表中
// @Param: MY_NEW_PARAMETER
// @DisplayName: My New Parameter
// @Description: A description of my new parameter goes here
// @Range: -32768 32767
// @User: Advanced
GSCALAR(my_new_parameter, "MY_NEW_PARAMETER", MY_NEW_PARAMETER_DEFAULT),
自己新增程式碼
// @Group:
// @Path: Parameters.cpp
GOBJECT(g2, "", ParametersG2),
//增加遙控器模式引數
GSCALAR(radio_mode, "radio_mode", RADIO_MODE),
AP_VAREND
地面站(如Mission Planner)中將使用@Param ~ @User的註釋資訊向使用者說明使用者所設定的變數的範圍等。
第4步:
在config.h中新增你的新引數。
#ifndef MY_NEW_PARAMETER_DEFAULT
# define MY_NEW_PARAMETER_DEFAULT 100 // default value for my new parameter
#endif
自己新增程式碼
#ifndef RADIO_MODE
# define RADIO_MODE 2
#endif
最後在地面站可以看到引數
向主執行程式碼新增引數的工作就完成了!新增到主程式碼中(並非庫中)的引數就可以通過諸如g.my_new_parameter這樣來使用。
2.增加一個引數到庫
同樣可以使用下列步驟向庫中新增新的引數。以AP_Compass庫為例:
第一步:
首先在庫程式碼的.h((如Compass.h))標頭檔案新增新的類變數。可使用的型別包括AP_Int8,AP_Int16,AP_Float,AP_Int32,AP_Vector3f。然後可以新增你想要的引數的預設值(我們將在Step #2中使用)。
#define MY_NEW_PARAM_DEFAULT 100
class Compass
{
public:
int16_t product_id; /// product id
int16_t mag_x; ///< magnetic field strength along the X axis
int16_t mag_y; ///< magnetic field strength along the Y axis
int16_t mag_z; ///< magnetic field strength along the Z axis
uint32_t last_update; ///< micros() time of last update
bool healthy; ///< true if last read OK
/// Constructor
///
Compass();
protected:
AP_Int8 _orientation;
AP_Vector3f _offset;
AP_Float _declination;
AP_Int8 _use_for_yaw; ///<enable use for yaw calculation
AP_Int8 _auto_declination; ///<enable automatic declination code
//這裡是增加的變數
AP_Int16 _my_new_lib_parameter; /// description of my new parameter
};
自己新增程式碼
(第一步)
class AP_Proximity
{
public:
..............................
//增加引數
AP_Int8 MY_DATA;
..............................
private:
Proximity_State state[PROXIMITY_MAX_INSTANCES];
AP_Proximity_Backend *drivers[PROXIMITY_MAX_INSTANCES];
const RangeFinder *_rangefinder;
uint8_t primary_instance:3;
uint8_t num_instances:3;
AP_SerialManager &serial_manager;
// parameters for all instances
AP_Int8 _type[PROXIMITY_MAX_INSTANCES];
AP_Int8 _orientation[PROXIMITY_MAX_INSTANCES];
AP_Int16 _yaw_correction[PROXIMITY_MAX_INSTANCES];
AP_Int16 _ignore_angle_deg[PROXIMITY_MAX_IGNORE]; // angle (in degrees) of area that should be ignored by sensor (i.e. leg shows up)
AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE]; // width of beam (in degrees) that should be ignored
void detect_instance(uint8_t instance);
void update_instance(uint8_t instance);
};
第二步:
然後在.cpp檔案(如Compass.cpp)中新增變數包含有@Param ~ @Increment的var_info表資訊,以便允許GCS向用戶顯示來自地面站的關於該引數值的範圍設定。當新增新引數時應注意:
(1)自己新增的程式碼編號(下面的編號9)一定要比之前變數的大。
(2)引數的名稱(如MY_NEW_P)包括物件自動新增的字首要少於16個字元。比如羅盤物件的字首為“COMPASS_”。//這裡要注意,起的名字不能太長!!!
const AP_Param::GroupInfo Compass::var_info[] = {
// index 0 was used for the old orientation matrix
// @Param: OFS_X
// @DisplayName: Compass offsets on the X axis
// @Description: Offset to be added to the compass x-axis values to compensate for metal in the frame
// @Range: -400 400
// @Increment: 1
<snip>
// @Param: ORIENT
// @DisplayName: Compass orientation
// @Description: The orientation of the compass relative to the autopilot board.
// @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180
AP_GROUPINFO("ORIENT", 8, Compass, _orientation, ROTATION_NONE),
// @Param: MY_NEW_P
// @DisplayName: My New Library Parameter
// @Description: The new library parameter description goes here
// @Range: -32768 32767
// @User: Advanced
//注意這裡是增加的名字
AP_GROUPINFO("MY_NEW_P", 9, Compass, _my_new_lib_parameter, MY_NEW_PARAM_DEFAULT),
AP_GROUPEND
};
這樣,新新增的引數將以_my_new_lib_parameter包含在庫中。需要指明的是:protected保護型別的引數是不能夠在類外被訪問的。如果我們將其變為public型別,那麼我們就可以在主程式碼中使用compass._my_new_lib_parameter引數了。
第三步:
除了compass.cpp檔案中的定義之外,還將var_info的宣告新增到新庫類的compass.h檔案的公共部分:
static const struct AP_Param::GroupInfo var_info[];
第四步:
前面提到的是在已經存在的類(比如AP_Compass)中定義一個新的變數。如果你重新定義了一個新類,在這個新類中新增引數。新增引數的方法如第二步。不過你還有一個工作要做,就是將這個新類,新增到Parameters.cpp檔案的var_info 陣列列表中去。下面加粗的紅色羅盤類程式碼就是一個示例。
const AP_Param::Info var_info[] = {
// @Param: SYSID_SW_MREV
// @DisplayName: Eeprom format version number
// @Description: This value is incremented when changes are made to the eeprom format
// @User: Advanced
GSCALAR(format_version, "SYSID_SW_MREV", 0),
<snip>
// @Group: COMPASS_
// @Path: ../libraries/AP_Compass/Compass.cpp
//注意這裡
GOBJECT(compass, "COMPASS_", Compass),
<snip>
// @Group: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
GOBJECT(ins, "INS_", AP_InertialSensor),
AP_VAREND
};
第五步:
如果所新增的類是一個完全新新增的程式碼,那麼還要將k_param_my_new_lib新增到Parameters.h中的列舉中,其中my_new_lib是Parameters.cpp.中的GOBJECT宣告的第一個引數。讀取列舉上的註釋,以瞭解在何處放置新的值,這是因為放置的順序很重要,(只能使用不佔用的列舉資料,一般放在相關的類後面,或者放在最後面。
到這裡官網上的註釋就結束了!!!
下面進行動手講解怎麼新增一個新引數到新建的庫中,這裡以adrc庫實現為例;
(1)首先增加AC_ADRC庫,包含(AC_ADRC.cpp和AC_ADRC.h)在AC_ADRC.h定義變數類*
#ifndef AC_ADRC_H_
#define AC_ADRC_H_
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
class AC_ADRC {
public:
...............此處省略..............
static const struct AP_Param::GroupInfo var_info[];
protected:
//中間省略
AP_Float _kp, _kd;
//中間省略
};
#endif
(2)首先增加資料表:var_info[]
const AP_Param::GroupInfo AC_ADRC::var_info[] =
{
// @Param: P
// @DisplayName: PID Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value
AP_GROUPINFO("P", 0, AC_ADRC, _kp, 0),
// @Param: D
// @DisplayName: PID Derivative Gain
// @Description: D Gain which produces an output that is proportional to the rate of change of the error
AP_GROUPINFO("D", 1, AC_ADRC, _kd, 0),
AP_GROUPEND
};
(3)在AC_ADRC.h中宣告var_info[]
class AC_ADRC {
public:
...............此處省略..............
//注意是說的這個定義
static const struct AP_Param::GroupInfo var_info[];
//注意是說的這個定義
protected:
//中間省略
AP_Float _kp, _kd;
//中間省略
};
第四步:在Parameters.cpp中增加引數宣告
const AP_Param::Info Copter::var_info[] = {
#if FRAME_CONFIG == HELI_FRAME
GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Heli),
#else
GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Multi),
#endif
};
//繼續在AC_AttitudeControl_Multi.cpp和AC_AttitudeControl_Multi.h中安裝新增
const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
AP_SUBGROUPINFO(_adrc_rate_roll, "ADRC_R_", 15, AC_AttitudeControl_Multi, AC_ADRC),
AP_SUBGROUPINFO(_adrc_rate_pitch, "ADRC_P_", 16, AC_AttitudeControl_Multi, AC_ADRC),
AP_SUBGROUPINFO(_adrc_rate_yaw, "ADRC_Y_", 17, AC_AttitudeControl_Multi, AC_ADRC),
}
到這基本完成,如果還不會,可以隨便找一個庫,自己抄襲一遍就知道怎麼添加了。
二.重點函式的理解與分析
1.為什麼Parameters.h中的enum檔案必須按照下面那樣寫?
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
k_param_ins_old, // *** Deprecated, remove with next eeprom number change
k_param_ins, // libraries/AP_InertialSensor variables
k_param_NavEKF2_old, // deprecated - remove
k_param_NavEKF2,
k_param_g2, // 2nd block of parameters
k_param_NavEKF3,
// simulation
k_param_sitl = 10,
// barometer object (needed for SITL)
k_param_barometer,
// scheduler object (for debugging)
k_param_scheduler,
// relay object
k_param_relay,
// (old) EPM object
k_param_epm_unused,
// BoardConfig object
k_param_BoardConfig,
/*****************後面省略****************/
}
重點在這:
#define GSCALAR(v, name, def) { copter.g.v.vtype, name, Parameters::k_param_ ## v, &copter.g.v, {def_value : def} }
#define ASCALAR(v, name, def) { copter.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&copter.aparm.v, {def_value : def} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &copter.g.v, {group_info : class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info : class::var_info} }
#define GOBJECTPTR(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info : class::var_info}, AP_PARAM_FLAG_POINTER }
#define GOBJECTVARPTR(v, name, var_info_ptr) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info_ptr : var_info_ptr}, AP_PARAM_FLAG_POINTER | AP_PARAM_FLAG_INFO_POINTER }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&copter.v, {group_info : class::var_info} }
其實一個類中的enum變數名字可以任意定義,只要按照列舉順序就可以,關鍵在於其他地方呼叫這個列舉變數,是其他地方對這個有要求,所以我們需要按照這種方式去定義。
(1)分析重要函式
#define GSCALAR(v, name, def) { copter.g.v.vtype, name, Parameters::k_param_ ## v, &copter.g.v, {def_value : def} }
v是變數類物件
name是名稱,可以隨便起,但是還是要靠譜點,方便識別
def是定義