Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: obey pitch trim and throttle pitch feed forward in new controllers 

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
Andrew Tridgell 2013-05-05 20:29:04 +10:00
parent e87a6d5564
commit 6251d0510a
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduPlane/Attitude.pde
View File

@ -98,18 +98,19 @@ static void stabilize_roll(float speed_scaler)
*/ */
static void stabilize_pitch(float speed_scaler) static void stabilize_pitch(float speed_scaler)
{ {
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + g.channel_throttle.servo_out * g.kff_throttle_to_pitch;
switch (g.att_controller) { switch (g.att_controller) {
case ATT_CONTROL_APMCONTROL: case ATT_CONTROL_APMCONTROL: {
g.channel_pitch.servo_out = g.pitchController.get_servo_out(nav_pitch_cd, speed_scaler, g.channel_pitch.servo_out = g.pitchController.get_servo_out(demanded_pitch,
speed_scaler,
control_mode == STABILIZE, control_mode == STABILIZE,
g.flybywire_airspeed_min, g.flybywire_airspeed_max); g.flybywire_airspeed_min, g.flybywire_airspeed_max);
break; break;
}
default: { default: {
int32_t tempcalc = nav_pitch_cd + int32_t tempcalc = demanded_pitch - ahrs.pitch_sensor;
fabsf(ahrs.roll_sensor * g.kff_pitch_compensation) + tempcalc += fabsf(ahrs.roll_sensor * g.kff_pitch_compensation);
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
(ahrs.pitch_sensor - g.pitch_trim_cd);
if (abs(ahrs.roll_sensor) > 9000) { if (abs(ahrs.roll_sensor) > 9000) {
// when flying upside down the elevator control is inverted // when flying upside down the elevator control is inverted
tempcalc = -tempcalc; tempcalc = -tempcalc;