Browse Source

PID参数名调整

zr-v5.1
Brown.Z 3 years ago
parent
commit
f20f261085
  1. 29
      ArduCopter/Parameters.cpp
  2. 9
      libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
  3. 10
      libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
  4. 9
      libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
  5. 1
      nohup_sitl.sh
  6. 1
      show_tcp_sitl.sh
  7. 2
      sitl.sh
  8. 1
      tcp_sitl.sh

29
ArduCopter/Parameters.cpp

@ -1201,15 +1201,26 @@ void Copter::convert_pid_parameters(void) @@ -1201,15 +1201,26 @@ void Copter::convert_pid_parameters(void)
{
// conversion info
const AP_Param::ConversionInfo pid_conversion_info[] = {
{ Parameters::k_param_pid_rate_roll, 0, AP_PARAM_FLOAT, "ATC_RAT_RLL_P" },
{ Parameters::k_param_pid_rate_roll, 1, AP_PARAM_FLOAT, "ATC_RAT_RLL_I" },
{ Parameters::k_param_pid_rate_roll, 2, AP_PARAM_FLOAT, "ATC_RAT_RLL_D" },
{ Parameters::k_param_pid_rate_pitch, 0, AP_PARAM_FLOAT, "ATC_RAT_PIT_P" },
{ Parameters::k_param_pid_rate_pitch, 1, AP_PARAM_FLOAT, "ATC_RAT_PIT_I" },
{ Parameters::k_param_pid_rate_pitch, 2, AP_PARAM_FLOAT, "ATC_RAT_PIT_D" },
{ Parameters::k_param_pid_rate_yaw, 0, AP_PARAM_FLOAT, "ATC_RAT_YAW_P" },
{ Parameters::k_param_pid_rate_yaw, 1, AP_PARAM_FLOAT, "ATC_RAT_YAW_I" },
{ Parameters::k_param_pid_rate_yaw, 2, AP_PARAM_FLOAT, "ATC_RAT_YAW_D" },
// { Parameters::k_param_pid_rate_roll, 0, AP_PARAM_FLOAT, "ATC_RAT_RLL_P" },
// { Parameters::k_param_pid_rate_roll, 1, AP_PARAM_FLOAT, "ATC_RAT_RLL_I" },
// { Parameters::k_param_pid_rate_roll, 2, AP_PARAM_FLOAT, "ATC_RAT_RLL_D" },
// { Parameters::k_param_pid_rate_pitch, 0, AP_PARAM_FLOAT, "ATC_RAT_PIT_P" },
// { Parameters::k_param_pid_rate_pitch, 1, AP_PARAM_FLOAT, "ATC_RAT_PIT_I" },
// { Parameters::k_param_pid_rate_pitch, 2, AP_PARAM_FLOAT, "ATC_RAT_PIT_D" },
// { Parameters::k_param_pid_rate_yaw, 0, AP_PARAM_FLOAT, "ATC_RAT_YAW_P" },
// { Parameters::k_param_pid_rate_yaw, 1, AP_PARAM_FLOAT, "ATC_RAT_YAW_I" },
// { Parameters::k_param_pid_rate_yaw, 2, AP_PARAM_FLOAT, "ATC_RAT_YAW_D" },
{ Parameters::k_param_pid_rate_roll, 0, AP_PARAM_FLOAT, "ATC_AA_RLL_P" },
{ Parameters::k_param_pid_rate_roll, 1, AP_PARAM_FLOAT, "ATC_AA_RLL_I" },
{ Parameters::k_param_pid_rate_roll, 2, AP_PARAM_FLOAT, "ATC_AA_RLL_D" },
{ Parameters::k_param_pid_rate_pitch, 0, AP_PARAM_FLOAT, "ATC_AA_PIT_P" },
{ Parameters::k_param_pid_rate_pitch, 1, AP_PARAM_FLOAT, "ATC_AA_PIT_I" },
{ Parameters::k_param_pid_rate_pitch, 2, AP_PARAM_FLOAT, "ATC_AA_PIT_D" },
{ Parameters::k_param_pid_rate_yaw, 0, AP_PARAM_FLOAT, "ATC_AA_YAW_P" },
{ Parameters::k_param_pid_rate_yaw, 1, AP_PARAM_FLOAT, "ATC_AA_YAW_I" },
{ Parameters::k_param_pid_rate_yaw, 2, AP_PARAM_FLOAT, "ATC_AA_YAW_D" },
#if FRAME_CONFIG == HELI_FRAME
{ Parameters::k_param_pid_rate_roll, 4, AP_PARAM_FLOAT, "ATC_RAT_RLL_VFF" },
{ Parameters::k_param_pid_rate_pitch, 4, AP_PARAM_FLOAT, "ATC_RAT_PIT_VFF" },

9
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@ -1069,9 +1069,12 @@ bool AC_AttitudeControl::pre_arm_checks(const char *param_prefix, @@ -1069,9 +1069,12 @@ bool AC_AttitudeControl::pre_arm_checks(const char *param_prefix,
const char *pid_name;
AC_PID &pid;
} pids[] = {
{ "RAT_RLL", get_rate_roll_pid() },
{ "RAT_PIT", get_rate_pitch_pid() },
{ "RAT_YAW", get_rate_yaw_pid() },
// { "RAT_RLL", get_rate_roll_pid() },
// { "RAT_PIT", get_rate_pitch_pid() },
// { "RAT_YAW", get_rate_yaw_pid() },
{ "AA_RLL", get_rate_roll_pid() },
{ "AA_PIT", get_rate_pitch_pid() },
{ "AA_YAW", get_rate_yaw_pid() },
};
for (uint8_t i=0; i<ARRAY_SIZE(pids); i++) {
// if the PID has a positive FF then we just ensure kP and

10
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@ -87,7 +87,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { @@ -87,7 +87,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
// @Increment: 0.5
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 2, AC_AttitudeControl_Heli, AC_HELI_PID),
// AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 2, AC_AttitudeControl_Heli, AC_HELI_PID),
AP_SUBGROUPINFO(_pid_rate_roll, "AA_RLL_", 2, AC_AttitudeControl_Heli, AC_HELI_PID),
// @Param: RAT_PIT_P
// @DisplayName: Pitch axis rate controller P gain
@ -161,8 +162,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { @@ -161,8 +162,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
// @Increment: 0.5
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 3, AC_AttitudeControl_Heli, AC_HELI_PID),
// AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 3, AC_AttitudeControl_Heli, AC_HELI_PID),
AP_SUBGROUPINFO(_pid_rate_pitch, "AA_PIT_", 3, AC_AttitudeControl_Heli, AC_HELI_PID),
// @Param: RAT_YAW_P
// @DisplayName: Yaw axis rate controller P gain
// @Description: Yaw axis rate controller P gain. Converts the difference between desired yaw rate and actual yaw rate into a motor speed output
@ -235,7 +236,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { @@ -235,7 +236,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
// @Increment: 0.5
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 4, AC_AttitudeControl_Heli, AC_HELI_PID),
// AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 4, AC_AttitudeControl_Heli, AC_HELI_PID),
AP_SUBGROUPINFO(_pid_rate_yaw, "AA_YAW_", 4, AC_AttitudeControl_Heli, AC_HELI_PID),
// @Param: PIRO_COMP
// @DisplayName: Piro Comp Enable

9
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp

@ -73,7 +73,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { @@ -73,7 +73,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Increment: 0.5
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 1, AC_AttitudeControl_Multi, AC_PID),
// AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 1, AC_AttitudeControl_Multi, AC_PID),
AP_SUBGROUPINFO(_pid_rate_roll, "AA_RLL_", 1, AC_AttitudeControl_Multi, AC_PID),
// @Param: RAT_PIT_P
// @DisplayName: Pitch axis rate controller P gain
@ -141,7 +142,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { @@ -141,7 +142,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Increment: 0.5
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 2, AC_AttitudeControl_Multi, AC_PID),
// AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 2, AC_AttitudeControl_Multi, AC_PID),
AP_SUBGROUPINFO(_pid_rate_pitch, "AA_PIT_", 2, AC_AttitudeControl_Multi, AC_PID),
// @Param: RAT_YAW_P
// @DisplayName: Yaw axis rate controller P gain
@ -209,7 +211,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { @@ -209,7 +211,8 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Increment: 0.5
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Multi, AC_PID),
// AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Multi, AC_PID),
AP_SUBGROUPINFO(_pid_rate_yaw, "AA_YAW_", 3, AC_AttitudeControl_Multi, AC_PID),
// @Param: THR_MIX_MIN
// @DisplayName: Throttle Mix Minimum

1
nohup_sitl.sh

@ -0,0 +1 @@ @@ -0,0 +1 @@
nohup ./tcp_sitl.sh> ./log/sitl.log 2>&1 &

1
show_tcp_sitl.sh

@ -0,0 +1 @@ @@ -0,0 +1 @@
ps -aux|grep sitl

2
sitl.sh

@ -1 +1 @@ @@ -1 +1 @@
./Tools/autotest/sim_vehicle.py -j4 -L CYDS --console --out 192.168.10.137:14552 --map -A --serial5=uart:/dev/ttyS1:115200 -v ArduCopter
./Tools/autotest/sim_vehicle.py -j4 -L CYDS --console --out 192.168.0.51:14552 --map -A "--serial5=uart:/dev/pts/1:230400" -v ArduCopter

1
tcp_sitl.sh

@ -0,0 +1 @@ @@ -0,0 +1 @@
./Tools/autotest/sim_vehicle.py -j4 -L CYDS --console --out 192.168.0.51:14552 --no-mavproxy ArduCopter -A "--serial5=uart:/dev/pts/1:230400" -v ArduCopter
Loading…
Cancel
Save