Browse Source

Added multiple new tuning parameters to the inflight channel 6 tuning feature:

#define CH6_YAW_KI 24
        #define CH6_ACRO_KP 25
        #define CH6_YAW_RATE_KD 26
        #define CH6_LOITER_KI 27
        #define CH6_LOITER_RATE_KI 28
mission-4.1.18
rmackay9 13 years ago
parent
commit
599cea21bb
  1. 30
      ArduCopter/ArduCopter.pde
  2. 17
      ArduCopter/defines.h

30
ArduCopter/ArduCopter.pde

@ -2044,6 +2044,10 @@ static void tuning(){ @@ -2044,6 +2044,10 @@ static void tuning(){
g.pi_stabilize_pitch.kI(tuning_value);
break;
case CH6_ACRO_KP:
g.acro_p = tuning_value;
break;
case CH6_RATE_KP:
g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value);
@ -2058,10 +2062,18 @@ static void tuning(){ @@ -2058,10 +2062,18 @@ static void tuning(){
g.pi_stabilize_yaw.kP(tuning_value);
break;
case CH6_YAW_KI:
g.pi_stabilize_yaw.kI(tuning_value);
break;
case CH6_YAW_RATE_KP:
g.pid_rate_yaw.kP(tuning_value);
break;
case CH6_YAW_RATE_KD:
g.pid_rate_yaw.kD(tuning_value);
break;
case CH6_THROTTLE_KP:
g.pid_throttle.kP(tuning_value);
break;
@ -2079,22 +2091,32 @@ static void tuning(){ @@ -2079,22 +2091,32 @@ static void tuning(){
g.waypoint_speed_max = g.rc_6.control_in;
break;
case CH6_LOITER_P:
case CH6_LOITER_KP:
g.pi_loiter_lat.kP(tuning_value);
g.pi_loiter_lon.kP(tuning_value);
break;
case CH6_NAV_P:
case CH6_LOITER_KI:
g.pi_loiter_lat.kI(tuning_value);
g.pi_loiter_lon.kI(tuning_value);
break;
case CH6_NAV_KP:
g.pid_nav_lat.kP(tuning_value);
g.pid_nav_lon.kP(tuning_value);
break;
case CH6_LOITER_RATE_P:
case CH6_LOITER_RATE_KP:
g.pid_loiter_rate_lon.kP(tuning_value);
g.pid_loiter_rate_lat.kP(tuning_value);
break;
case CH6_LOITER_RATE_D:
case CH6_LOITER_RATE_KI:
g.pid_loiter_rate_lon.kI(tuning_value);
g.pid_loiter_rate_lat.kI(tuning_value);
break;
case CH6_LOITER_RATE_KD:
g.pid_loiter_rate_lon.kD(tuning_value);
g.pid_loiter_rate_lat.kD(tuning_value);
break;

17
ArduCopter/defines.h

@ -142,20 +142,26 @@ @@ -142,20 +142,26 @@
#define CH6_STABILIZE_KP 1
#define CH6_STABILIZE_KI 2
#define CH6_YAW_KP 3
#define CH6_YAW_KI 24
// Rate
#define CH6_ACRO_KP 25
#define CH6_RATE_KP 4
#define CH6_RATE_KI 5
#define CH6_RATE_KD 21
#define CH6_YAW_RATE_KP 6
#define CH6_YAW_RATE_KD 26
// Altitude rate controller
#define CH6_THROTTLE_KP 7
// Extras
#define CH6_TOP_BOTTOM_RATIO 8
#define CH6_RELAY 9
#define CH6_TRAVERSE_SPEED 10
// Navigation
#define CH6_TRAVERSE_SPEED 10 // maximum speed to next way point
#define CH6_NAV_KP 11
#define CH6_LOITER_KP 12
#define CH6_LOITER_KI 27
#define CH6_NAV_P 11
#define CH6_LOITER_P 12
// Trad Heli specific
#define CH6_HELI_EXTERNAL_GYRO 13
// altitude controller
@ -169,8 +175,9 @@ @@ -169,8 +175,9 @@
#define CH6_OPTFLOW_KD 19
#define CH6_NAV_I 20
#define CH6_LOITER_RATE_P 22
#define CH6_LOITER_RATE_D 23
#define CH6_LOITER_RATE_KP 22
#define CH6_LOITER_RATE_KI 28
#define CH6_LOITER_RATE_KD 23
// nav byte mask

Loading…
Cancel
Save