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

Copter: consolidate input_euler_angle calls to use smoothing gain 

Previously we had _smooth and non-smooth methods in the attitude
controller but as part of the move to quaternions these have been replaced
by a single call which always takes a smoothing gain
This commit is contained in:
Leonard Hall 2016-06-17 12:17:59 +09:30 committed by Randy Mackay
parent a5bb3c206e
commit 7d54c268f6
16 changed files with 71 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Attitude.cpp
View File

@ -2,7 +2,7 @@
#include "Copter.h" #include "Copter.h"
// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth // get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw
// result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp // result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp
float Copter::get_smoothing_gain() float Copter::get_smoothing_gain()
{ {

10
ArduCopter/control_althold.cpp
View File

@ -84,7 +84,7 @@ void Copter::althold_run()
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying // helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// force descent rate and call position controller // force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
@ -102,7 +102,7 @@ void Copter::althold_run()
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// Helicopters always stabilize roll/pitch/yaw // Helicopters always stabilize roll/pitch/yaw
attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_yaw_target_to_current_heading();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
// Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) // Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
@ -129,7 +129,7 @@ void Copter::althold_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// call position controller // call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
@ -143,7 +143,7 @@ void Copter::althold_run()
attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_yaw_target_to_current_heading();
#endif #endif
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt); attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
// set motors to spin-when-armed if throttle at zero, otherwise full range // set motors to spin-when-armed if throttle at zero, otherwise full range
if (ap.throttle_zero) { if (ap.throttle_zero) {
@ -157,7 +157,7 @@ void Copter::althold_run()
case AltHold_Flying: case AltHold_Flying:
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
if (rangefinder_alt_ok()) { if (rangefinder_alt_ok()) {

26
ArduCopter/control_auto.cpp
View File

@ -150,7 +150,7 @@ void Copter::auto_takeoff_run()
wp_nav.shift_wp_origin_to_current_pos(); wp_nav.shift_wp_origin_to_current_pos();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed #else // multicopters do not stabilize roll/pitch/yaw when disarmed
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -179,7 +179,7 @@ void Copter::auto_takeoff_run()
pos_control.update_z_controller(); pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
} }
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination // auto_wp_start - initialises waypoint controller to implement flying to a particular destination
@ -226,7 +226,7 @@ void Copter::auto_wp_run()
// (of course it would be better if people just used take-off) // (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed #else // multicopters do not stabilize roll/pitch/yaw when disarmed
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -259,10 +259,10 @@ void Copter::auto_wp_run()
// call attitude controller // call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) { if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{ }else{
// roll, pitch from waypoint controller, yaw heading from auto_heading() // roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true); attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
} }
} }
@ -298,7 +298,7 @@ void Copter::auto_spline_run()
// (of course it would be better if people just used take-off) // (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed #else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -331,10 +331,10 @@ void Copter::auto_spline_run()
// call attitude controller // call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) { if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{ }else{
// roll, pitch from waypoint controller, yaw heading from auto_heading() // roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true); attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
} }
} }
@ -375,7 +375,7 @@ void Copter::auto_land_run()
if (!motors.armed() || !ap.auto_armed || ap.land_complete) { if (!motors.armed() || !ap.auto_armed || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -433,7 +433,7 @@ void Copter::auto_land_run()
desired_climb_rate = cmb_rate; desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
} }
// auto_rtl_start - initialises RTL in AUTO flight mode // auto_rtl_start - initialises RTL in AUTO flight mode
@ -528,7 +528,7 @@ void Copter::auto_circle_run()
pos_control.update_z_controller(); pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true); attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain());
} }
#if NAV_GUIDED == ENABLED #if NAV_GUIDED == ENABLED
@ -587,7 +587,7 @@ void Copter::auto_loiter_run()
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -610,7 +610,7 @@ void Copter::auto_loiter_run()
failsafe_terrain_set_status(wp_nav.update_wpnav()); failsafe_terrain_set_status(wp_nav.update_wpnav());
pos_control.update_z_controller(); pos_control.update_z_controller();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
} }
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter

14
ArduCopter/control_autotune.cpp
View File

@ -332,7 +332,7 @@ void Copter::autotune_run()
// if pilot override call attitude controller // if pilot override call attitude controller
if (autotune_state.pilot_override || autotune_state.mode != AUTOTUNE_MODE_TUNING) { if (autotune_state.pilot_override || autotune_state.mode != AUTOTUNE_MODE_TUNING) {
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
}else{ }else{
// somehow get attitude requests from autotuning // somehow get attitude requests from autotuning
autotune_attitude_control(); autotune_attitude_control();
@ -360,7 +360,7 @@ void Copter::autotune_attitude_control()
attitude_control.limit_angle_to_rate_request(true); attitude_control.limit_angle_to_rate_request(true);
// hold level attitude // hold level attitude
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, autotune_desired_yaw, true); attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, autotune_desired_yaw, true, get_smoothing_gain());
// hold the copter level for 0.5 seconds before we begin a twitch // hold the copter level for 0.5 seconds before we begin a twitch
// reset counter if we are no longer level // reset counter if we are no longer level
@ -439,22 +439,22 @@ void Copter::autotune_attitude_control()
switch (autotune_state.axis) { switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL: case AUTOTUNE_AXIS_ROLL:
// request roll to 20deg // request roll to 20deg
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, 0.0f); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, 0.0f, get_smoothing_gain());
break; break;
case AUTOTUNE_AXIS_PITCH: case AUTOTUNE_AXIS_PITCH:
// request pitch to 20deg // request pitch to 20deg
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, direction_sign * autotune_target_angle + autotune_start_angle, 0.0f); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, get_smoothing_gain());
break; break;
case AUTOTUNE_AXIS_YAW: case AUTOTUNE_AXIS_YAW:
// request pitch to 20deg // request pitch to 20deg
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd(direction_sign * autotune_target_angle + autotune_start_angle), false); attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd(direction_sign * autotune_target_angle + autotune_start_angle), false, get_smoothing_gain());
break; break;
} }
} else { } else {
// Testing rate P and D gains so will set body-frame rate targets. // Testing rate P and D gains so will set body-frame rate targets.
// Rate controller will use existing body-frame rates and convert to motor outputs // Rate controller will use existing body-frame rates and convert to motor outputs
// for all axes except the one we override here. // for all axes except the one we override here.
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, 0.0f, 0.0f); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, 0.0f, 0.0f, get_smoothing_gain());
switch (autotune_state.axis) { switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL: case AUTOTUNE_AXIS_ROLL:
// override body-frame roll rate // override body-frame roll rate
@ -730,7 +730,7 @@ void Copter::autotune_attitude_control()
autotune_state.positive_direction = !autotune_state.positive_direction; autotune_state.positive_direction = !autotune_state.positive_direction;
if (autotune_state.axis == AUTOTUNE_AXIS_YAW) { if (autotune_state.axis == AUTOTUNE_AXIS_YAW) {
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, ahrs.yaw_sensor, false); attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, ahrs.yaw_sensor, false, get_smoothing_gain());
} }
// set gains to their intra-test values (which are very close to the original gains) // set gains to their intra-test values (which are very close to the original gains)

4
ArduCopter/control_brake.cpp
View File

@ -42,7 +42,7 @@ void Copter::brake_run()
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE); wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -70,7 +70,7 @@ void Copter::brake_run()
wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler); wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain());
// body-frame rate controller is run directly from 100hz loop // body-frame rate controller is run directly from 100hz loop

6
ArduCopter/control_circle.cpp
View File

@ -46,7 +46,7 @@ void Copter::circle_run()
// To-Do: add some initialisation of position controllers // To-Do: add some initialisation of position controllers
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -85,9 +85,9 @@ void Copter::circle_run()
// call attitude controller // call attitude controller
if (circle_pilot_yaw_override) { if (circle_pilot_yaw_override) {
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{ }else{
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true); attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain());
} }
// adjust climb rate using rangefinder // adjust climb rate using rangefinder

2
ArduCopter/control_drift.cpp
View File

@ -98,7 +98,7 @@ void Copter::drift_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// output pilot's throttle with angle boost // output pilot's throttle with angle boost
attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt); attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);

2
ArduCopter/control_flip.cpp
View File

@ -177,7 +177,7 @@ void Copter::flip_run()
case Flip_Recover: case Flip_Recover:
// use originally captured earth-frame angle targets to recover // use originally captured earth-frame angle targets to recover
attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false); attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false, get_smoothing_gain());
// increase throttle to gain any lost altitude // increase throttle to gain any lost altitude
throttle_out += FLIP_THR_INC; throttle_out += FLIP_THR_INC;

26
ArduCopter/control_guided.cpp
View File

@ -326,7 +326,7 @@ void Copter::guided_takeoff_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -353,7 +353,7 @@ void Copter::guided_takeoff_run()
pos_control.update_z_controller(); pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
} }
// guided_pos_control_run - runs the guided position controller // guided_pos_control_run - runs the guided position controller
@ -364,7 +364,7 @@ void Copter::guided_pos_control_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) { if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -396,10 +396,10 @@ void Copter::guided_pos_control_run()
// call attitude controller // call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) { if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{ }else{
// roll, pitch from waypoint controller, yaw heading from auto_heading() // roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true); attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
} }
} }
@ -413,7 +413,7 @@ void Copter::guided_vel_control_run()
pos_control.init_vel_controller_xyz(); pos_control.init_vel_controller_xyz();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -448,10 +448,10 @@ void Copter::guided_vel_control_run()
// call attitude controller // call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) { if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{ }else{
// roll, pitch from waypoint controller, yaw heading from auto_heading() // roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true); attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
} }
} }
@ -466,7 +466,7 @@ void Copter::guided_posvel_control_run()
pos_control.set_desired_velocity(Vector3f(0,0,0)); pos_control.set_desired_velocity(Vector3f(0,0,0));
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -522,10 +522,10 @@ void Copter::guided_posvel_control_run()
// call attitude controller // call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) { if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{ }else{
// roll, pitch from waypoint controller, yaw heading from auto_heading() // roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true); attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
} }
} }
@ -538,7 +538,7 @@ void Copter::guided_angle_control_run()
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_yaw_target_to_current_heading();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0.0f, 0.0f, 0.0f, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
attitude_control.set_throttle_out(0.0f,false,g.throttle_filt); attitude_control.set_throttle_out(0.0f,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -578,7 +578,7 @@ void Copter::guided_angle_control_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
// call position controller // call position controller
pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false); pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);

8
ArduCopter/control_land.cpp
View File

@ -59,7 +59,7 @@ void Copter::land_gps_run()
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -132,7 +132,7 @@ void Copter::land_gps_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// pause 4 seconds before beginning land descent // pause 4 seconds before beginning land descent
float cmb_rate; float cmb_rate;
@ -183,7 +183,7 @@ void Copter::land_nogps_run()
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -209,7 +209,7 @@ void Copter::land_nogps_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// pause 4 seconds before beginning land descent // pause 4 seconds before beginning land descent
float cmb_rate; float cmb_rate;

10
ArduCopter/control_loiter.cpp
View File

@ -97,7 +97,7 @@ void Copter::loiter_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// force descent rate and call position controller // force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
@ -116,7 +116,7 @@ void Copter::loiter_run()
wp_nav.init_loiter_target(); wp_nav.init_loiter_target();
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// Helicopters always stabilize roll/pitch/yaw // Helicopters always stabilize roll/pitch/yaw
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
// Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) // Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
@ -145,7 +145,7 @@ void Copter::loiter_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// update altitude target and call position controller // update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
@ -157,7 +157,7 @@ void Copter::loiter_run()
wp_nav.init_loiter_target(); wp_nav.init_loiter_target();
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground // move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt); attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
// if throttle zero reset attitude and exit immediately // if throttle zero reset attitude and exit immediately
@ -178,7 +178,7 @@ void Copter::loiter_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
if (rangefinder_alt_ok()) { if (rangefinder_alt_ok()) {

2
ArduCopter/control_poshold.cpp
View File

@ -517,7 +517,7 @@ void Copter::poshold_run()
poshold.pitch = constrain_int16(poshold.pitch, -aparm.angle_max, aparm.angle_max); poshold.pitch = constrain_int16(poshold.pitch, -aparm.angle_max, aparm.angle_max);
// update attitude controller targets // update attitude controller targets
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
if (rangefinder_alt_ok()) { if (rangefinder_alt_ok()) {

20
ArduCopter/control_rtl.cpp
View File

@ -137,7 +137,7 @@ void Copter::rtl_climb_return_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -171,10 +171,10 @@ void Copter::rtl_climb_return_run()
// call attitude controller // call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) { if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{ }else{
// roll, pitch from waypoint controller, yaw heading from auto_heading() // roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true); attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
} }
// check if we've completed this stage of RTL // check if we've completed this stage of RTL
@ -204,7 +204,7 @@ void Copter::rtl_loiterathome_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -238,10 +238,10 @@ void Copter::rtl_loiterathome_run()
// call attitude controller // call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) { if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{ }else{
// roll, pitch from waypoint controller, yaw heading from auto_heading() // roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true); attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
} }
// check if we've completed this stage of RTL // check if we've completed this stage of RTL
@ -285,7 +285,7 @@ void Copter::rtl_descent_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -334,7 +334,7 @@ void Copter::rtl_descent_run()
pos_control.update_z_controller(); pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// check if we've reached within 20cm of final altitude // check if we've reached within 20cm of final altitude
rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f; rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f;
@ -366,7 +366,7 @@ void Copter::rtl_land_run()
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -439,7 +439,7 @@ void Copter::rtl_land_run()
desired_climb_rate = cmb_rate; desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot // roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// check if we've completed this stage of RTL // check if we've completed this stage of RTL
rtl_state_complete = ap.land_complete; rtl_state_complete = ap.land_complete;

2
ArduCopter/control_stabilize.cpp
View File

@ -50,7 +50,7 @@ void Copter::stabilize_run()
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in()); pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// body-frame rate controller is run directly from 100hz loop // body-frame rate controller is run directly from 100hz loop

6
ArduCopter/control_throw.cpp
View File

@ -139,7 +139,7 @@ void Copter::throw_run()
case Throw_Uprighting: case Throw_Uprighting:
// demand a level roll/pitch attitude with zero yaw rate // demand a level roll/pitch attitude with zero yaw rate
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
// output 50% throttle and turn off angle boost to maximise righting moment // output 50% throttle and turn off angle boost to maximise righting moment
attitude_control.set_throttle_out(500, false, g.throttle_filt); attitude_control.set_throttle_out(500, false, g.throttle_filt);
@ -149,7 +149,7 @@ void Copter::throw_run()
case Throw_HgtStabilise: case Throw_HgtStabilise:
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
// call height controller // call height controller
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
@ -163,7 +163,7 @@ void Copter::throw_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain());
// call height controller // call height controller
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);

2
ArduCopter/heli_control_stabilize.cpp
View File

@ -60,7 +60,7 @@ void Copter::heli_stabilize_run()
pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->get_control_in()); pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// output pilot's throttle - note that TradHeli does not used angle-boost // output pilot's throttle - note that TradHeli does not used angle-boost
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);