aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: replace smoothing gain with AC_AttitudeControl::set_input_tc

Browse Source
This commit is contained in:
Leonard Hall 2017-06-26 17:48:04 +09:00 committed by Randy Mackay
parent a356cfa529
commit 9544b1763b
27 changed files with 64 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/Attitude.cpp
View File

@ -1,12 +1,5 @@
#include "Copter.h"
// 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
float Copter::get_smoothing_gain()
{
return (2.0f + (float)g.rc_feel_rp/10.0f);
}
// get_pilot_desired_heading - transform pilot's yaw input into a
// desired yaw rate
// returns desired yaw rate in centi-degrees per second

1
ArduCopter/Copter.h
View File

@ -658,7 +658,6 @@ private:
void update_altitude();
// Attitude.cpp
float get_smoothing_gain();
float get_pilot_desired_yaw_rate(int16_t stick_angle);
float get_roi_yaw();
float get_look_ahead_yaw();

9
ArduCopter/Parameters.cpp
View File

@ -459,15 +459,6 @@ const AP_Param::Info Copter::var_info[] = {
// @User: Advanced
ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX),
// @Param: RC_FEEL_RP
// @DisplayName: RC Feel Roll/Pitch
// @Description: RC feel for roll/pitch which controls vehicle response to user input with 0 being extremely soft and 100 being crisp
// @Range: 0 100
// @Increment: 10
// @User: Standard
// @Values: 0:Very Soft, 25:Soft, 50:Medium, 75:Crisp, 100:Very Crisp
GSCALAR(rc_feel_rp, "RC_FEEL_RP", RC_FEEL_RP_MEDIUM),
// @Param: PHLD_BRAKE_RATE
// @DisplayName: PosHold braking rate
// @Description: PosHold flight mode's rotation rate during braking in deg/sec

3
ArduCopter/Parameters.h
View File

@ -111,7 +111,7 @@ public:
k_param_fs_batt_mah,
k_param_angle_rate_max, // remove
k_param_rssi_range, // unused, replaced by rssi_ library parameters
k_param_rc_feel_rp,
k_param_rc_feel_rp, // deprecated
k_param_NavEKF, // deprecated - remove
k_param_mission, // mission library
k_param_rc_13_old,
@ -407,7 +407,6 @@ public:
AP_Int16 rtl_climb_min; // rtl minimum climb in cm
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
AP_Int8 rc_feel_rp; // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp
AP_Int16 poshold_brake_rate; // PosHold flight mode's rotation rate during braking in deg/sec
AP_Int16 poshold_brake_angle_max; // PosHold flight mode's max lean angle during braking in centi-degrees

7
ArduCopter/defines.h
View File

@ -184,13 +184,6 @@ enum tuning_func {
#define ACRO_TRAINER_LEVELING 1
#define ACRO_TRAINER_LIMITED 2
// RC Feel roll/pitch definitions
#define RC_FEEL_RP_VERY_SOFT 0
#define RC_FEEL_RP_SOFT 25
#define RC_FEEL_RP_MEDIUM 50
#define RC_FEEL_RP_CRISP 75
#define RC_FEEL_RP_VERY_CRISP 100
// Yaw behaviours during missions - possible values for WP_YAW_BEHAVIOR parameter
#define WP_YAW_BEHAVIOR_NONE 0 // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received)
#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl

6
ArduCopter/mode.cpp
View File

@ -372,7 +372,7 @@ void Copter::Mode::zero_throttle_and_relax_ac()
{
#if FRAME_CONFIG == HELI_FRAME
// Helicopters always stabilize roll/pitch/yaw
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -414,10 +414,6 @@ void Copter::Mode::update_simple_mode(void) {
copter.update_simple_mode();
}
float Copter::Mode::get_smoothing_gain() {
return copter.get_smoothing_gain();
}
bool Copter::Mode::set_mode(control_mode_t mode, mode_reason_t reason)
{
return copter.set_mode(mode, reason);

1
ArduCopter/mode.h
View File

@ -84,7 +84,6 @@ protected:
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f);
float get_non_takeoff_throttle(void);
void update_simple_mode(void);
float get_smoothing_gain(void);
bool set_mode(control_mode_t mode, mode_reason_t reason);
void set_land_complete(bool b);
GCS_Copter &gcs();

2
ArduCopter/mode_acro_heli.cpp
View File

@ -28,7 +28,7 @@ void Copter::ModeAcro_Heli::run()
{
float target_roll, target_pitch, target_yaw;
float pilot_throttle_scaled;
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
// we may be in autorotation flight. These should be reset only when transitioning from disarmed
// to armed, because the pilot will have placed the helicopter down on the landing pad. This is so

8
ArduCopter/mode_althold.cpp
View File

@ -66,7 +66,7 @@ void Copter::ModeAltHold::run()
case AltHold_MotorStopped:
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
#if FRAME_CONFIG == HELI_FRAME
@ -104,7 +104,7 @@ void Copter::ModeAltHold::run()
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
// call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
@ -132,7 +132,7 @@ void Copter::ModeAltHold::run()
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
#endif
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
break;
@ -146,7 +146,7 @@ void Copter::ModeAltHold::run()
#endif
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
// adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) {

14
ArduCopter/mode_auto.cpp
View File

@ -280,10 +280,10 @@ void Copter::ModeAuto::wp_run()
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
} else {
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
}
}
@ -345,10 +345,10 @@ void Copter::ModeAuto::spline_run()
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
} else {
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true);
}
}
@ -505,7 +505,7 @@ void Copter::ModeAuto::circle_run()
pos_control->update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(),true, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true);
}
#if NAV_GUIDED == ENABLED
@ -576,7 +576,7 @@ void Copter::ModeAuto::loiter_run()
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
}
#endif
@ -863,7 +863,7 @@ void Copter::ModeAuto::payload_place_run_loiter()
// call attitude controller
const float target_yaw_rate = 0;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
// call position controller
pos_control->update_z_controller();

8
ArduCopter/mode_autotune.cpp
View File

@ -366,7 +366,7 @@ void Copter::ModeAutoTune::run()
}
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
pos_control->relax_alt_hold_controllers(0.0f);
pos_control->update_z_controller();
}else{
@ -406,7 +406,7 @@ void Copter::ModeAutoTune::run()
// if pilot override call attitude controller
if (pilot_override || mode != TUNING) {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
}else{
// somehow get attitude requests from autotuning
autotune_attitude_control();
@ -483,7 +483,7 @@ void Copter::ModeAutoTune::autotune_attitude_control()
get_poshold_attitude(roll_cd, pitch_cd, desired_yaw);
// hold level attitude
attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw, true, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw, true);
// hold the copter level for 0.5 seconds before we begin a twitch
// reset counter if we are no longer level
@ -859,7 +859,7 @@ void Copter::ModeAutoTune::autotune_attitude_control()
positive_direction = !positive_direction;
if (axis == YAW) {
attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs.yaw_sensor, false, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs.yaw_sensor, false);
}
// set gains to their intra-test values (which are very close to the original gains)

2
ArduCopter/mode_brake.cpp
View File

@ -62,7 +62,7 @@ void Copter::ModeBrake::run()
wp_nav->update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f);
// body-frame rate controller is run directly from 100hz loop

4
ArduCopter/mode_circle.cpp
View File

@ -79,11 +79,11 @@ void Copter::ModeCircle::run()
if (pilot_yaw_override) {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(),
copter.circle_nav->get_pitch(),
target_yaw_rate, get_smoothing_gain());
target_yaw_rate);
}else{
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(),
copter.circle_nav->get_pitch(),
copter.circle_nav->get_yaw(),true, get_smoothing_gain());
copter.circle_nav->get_yaw(), true);
}
// adjust climb rate using rangefinder

2
ArduCopter/mode_drift.cpp
View File

@ -102,7 +102,7 @@ void Copter::ModeDrift::run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
// output pilot's throttle with angle boost
attitude_control->set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);

2
ArduCopter/mode_flip.cpp
View File

@ -180,7 +180,7 @@ void Copter::ModeFlip::run()
case Flip_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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false);
// increase throttle to gain any lost altitude
throttle_out += FLIP_THR_INC;

8
ArduCopter/mode_flowhold.cpp
View File

@ -290,7 +290,7 @@ void Copter::ModeFlowHold::run()
case FlowHold_MotorStopped:
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate, copter.get_smoothing_gain());
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
#if FRAME_CONFIG == HELI_FRAME
// force descent rate and call position controller
copter.pos_control->set_alt_target_from_climb_rate(-abs(copter.g.land_speed), copter.G_Dt, false);
@ -323,7 +323,7 @@ void Copter::ModeFlowHold::run()
target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate);
// call attitude controller
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate, copter.get_smoothing_gain());
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
// call position controller
copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, copter.G_Dt, false);
@ -341,7 +341,7 @@ void Copter::ModeFlowHold::run()
copter.attitude_control->reset_rate_controller_I_terms();
copter.attitude_control->set_yaw_target_to_current_heading();
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate, copter.get_smoothing_gain());
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
copter.pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
copter.pos_control->update_z_controller();
break;
@ -355,7 +355,7 @@ void Copter::ModeFlowHold::run()
#endif
// call attitude controller
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate, copter.get_smoothing_gain());
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
// adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) {

22
ArduCopter/mode_guided.cpp
View File

@ -435,13 +435,13 @@ void Copter::ModeGuided::pos_control_run()
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
// roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_yaw_rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true);
}
}
@ -489,13 +489,13 @@ void Copter::ModeGuided::vel_control_run()
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate);
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
// roll & pitch from velocity controller, yaw rate from mavlink command or mission item
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true);
}
}
@ -561,13 +561,13 @@ void Copter::ModeGuided::posvel_control_run()
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate);
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
// roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true);
}
}
@ -620,9 +620,9 @@ void Copter::ModeGuided::angle_control_run()
// call attitude controller
if (guided_angle_state.use_yaw_rate) {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in);
} else {
attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
}
// call position controller

6
ArduCopter/mode_land.cpp
View File

@ -110,7 +110,7 @@ void Copter::ModeLand::nogps_run()
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -129,7 +129,7 @@ void Copter::ModeLand::nogps_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
// pause before beginning land descent
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
@ -288,7 +288,7 @@ void Copter::land_run_horizontal_control()
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate);
}
// do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch

8
ArduCopter/mode_loiter.cpp
View File

@ -128,7 +128,7 @@ void Copter::ModeLoiter::run()
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
pos_control->update_z_controller();
break;
@ -155,7 +155,7 @@ void Copter::ModeLoiter::run()
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
@ -173,7 +173,7 @@ void Copter::ModeLoiter::run()
wp_nav->init_loiter_target();
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0);
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
break;
@ -193,7 +193,7 @@ void Copter::ModeLoiter::run()
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
// adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) {

4
ArduCopter/mode_poshold.cpp
View File

@ -195,7 +195,7 @@ void Copter::ModePosHold::run()
wp_nav->init_loiter_target();
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0);
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
return;
@ -522,7 +522,7 @@ void Copter::ModePosHold::run()
poshold.pitch = constrain_int16(poshold.pitch, -angle_max, angle_max);
// update attitude controller targets
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
// adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) {

10
ArduCopter/mode_rtl.cpp
View File

@ -159,10 +159,10 @@ void Copter::ModeRTL::climb_return_run()
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
}else{
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
}
// check if we've completed this stage of RTL
@ -217,10 +217,10 @@ void Copter::ModeRTL::loiterathome_run()
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
}else{
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
}
// check if we've completed this stage of RTL
@ -305,7 +305,7 @@ void Copter::ModeRTL::descent_run()
pos_control->update_z_controller();
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
// check if we've reached within 20cm of final altitude
_state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;

8
ArduCopter/mode_smart_rtl.cpp
View File

@ -65,7 +65,7 @@ void Copter::ModeSmartRTL::wait_cleanup_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
wp_nav->update_wpnav();
pos_control->update_z_controller();
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
// check if return path is computed and if yes, begin journey home
if (g2.smart_rtl.request_thorough_cleanup()) {
@ -103,10 +103,10 @@ void Copter::ModeSmartRTL::path_follow_run()
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// 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(), 0, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0);
} else {
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
}
}
@ -129,7 +129,7 @@ void Copter::ModeSmartRTL::pre_land_position_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
wp_nav->update_wpnav();
pos_control->update_z_controller();
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true);
}
// save current position for use by the smart_rtl flight mode

2
ArduCopter/mode_stabilize.cpp
View File

@ -53,7 +53,7 @@ void Copter::ModeStabilize::run()
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
// body-frame rate controller is run directly from 100hz loop

2
ArduCopter/mode_stabilize_heli.cpp
View File

@ -65,7 +65,7 @@ void Copter::ModeStabilize_Heli::run()
pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
// output pilot's throttle - note that TradHeli does not used angle-boost
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);

6
ArduCopter/mode_throw.cpp
View File

@ -141,7 +141,7 @@ void Copter::ModeThrow::run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// 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, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
// output 50% throttle and turn off angle boost to maximise righting moment
attitude_control->set_throttle_out(0.5f, false, g.throttle_filt);
@ -154,7 +154,7 @@ void Copter::ModeThrow::run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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);
// call height controller
pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
@ -171,7 +171,7 @@ void Copter::ModeThrow::run()
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f);
// call height controller
pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);

2
ArduCopter/takeoff.cpp
View File

@ -182,5 +182,5 @@ void Copter::auto_takeoff_attitude_run(float target_yaw_rate)
}
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate);
}

4
ArduCopter/tuning.cpp
View File

@ -179,8 +179,8 @@ void Copter::tuning() {
#endif
case TUNING_RC_FEEL_RP:
// roll-pitch input smoothing
g.rc_feel_rp = control_in / 10;
// convert from control_in to input time constant
attitude_control->set_input_tc(1.0f / (2.f + MAX((control_in/100.0f),0.0f)));
break;
case TUNING_RATE_PITCH_KP: