Browse Source

Plane: use yaw rate controller in NAV_SCRIPT_TIME

gps-1.3.1
Andrew Tridgell 3 years ago committed by Peter Barker
parent
commit
56870ad7d6
  1. 4
      ArduPlane/Attitude.cpp
  2. 1
      ArduPlane/Plane.h
  3. 1
      ArduPlane/commands_logic.cpp

4
ArduPlane/Attitude.cpp

@ -519,6 +519,10 @@ void Plane::stabilize() @@ -519,6 +519,10 @@ void Plane::stabilize()
const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
if (yawController.rate_control_enabled()) {
const float rudder = yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false);
steering_control.rudder = rudder;
}
#endif
} else {
if (allow_stick_mixing && g.stick_mixing == StickMixing::FBW && control_mode != &mode_stabilize) {

1
ArduPlane/Plane.h

@ -523,6 +523,7 @@ private: @@ -523,6 +523,7 @@ private:
uint16_t id;
float roll_rate_dps;
float pitch_rate_dps;
float yaw_rate_dps;
float throttle_pct;
uint32_t start_ms;
bool done;

1
ArduPlane/commands_logic.cpp

@ -1188,6 +1188,7 @@ bool Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps @@ -1188,6 +1188,7 @@ bool Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps
}
nav_scripting.roll_rate_dps = roll_rate_dps;
nav_scripting.pitch_rate_dps = pitch_rate_dps;
nav_scripting.yaw_rate_dps = yaw_rate_dps;
nav_scripting.throttle_pct = throttle_pct;
return true;
}

Loading…
Cancel
Save