Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: replace smoothing gain with AC_AttitudeControl::set_input_tc

Browse Source
This commit is contained in:
Randy Mackay 2017-07-10 11:49:42 +09:00
parent a727305a59
commit da17034a3d
10 changed files with 27 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduSub/Attitude.cpp
View File

@ -1,12 +1,5 @@
#include "Sub.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 Sub::get_smoothing_gain()
{
return (2.0f + (float)g.rc_feel_rp/10.0f);
}
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void Sub::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)

9
ArduSub/Parameters.cpp
View File

@ -255,15 +255,6 @@ const AP_Param::Info Sub::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: FS_EKF_ACTION
// @DisplayName: EKF Failsafe Action
// @Description: Controls the action that will be taken when an EKF failsafe is invoked

6
ArduSub/control_althold.cpp
View File

@ -59,7 +59,7 @@ void Sub::althold_run()
// call attitude controller
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
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);
last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
@ -71,11 +71,11 @@ void Sub::althold_run()
target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate on yaw axis
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);
last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
}
}

14
ArduSub/control_auto.cpp
View File

@ -165,10 +165,10 @@ void Sub::auto_wp_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(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 {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true);
}
}
@ -242,10 +242,10 @@ void Sub::auto_spline_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(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 {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true);
}
}
@ -332,7 +332,7 @@ void Sub::auto_circle_run()
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true);
}
#if NAV_GUIDED == ENABLED
@ -425,7 +425,7 @@ void Sub::auto_loiter_run()
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// roll & pitch from waypoint controller, yaw rate from pilot
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);
}
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
@ -761,5 +761,5 @@ void Sub::auto_terrain_recover_run()
float target_yaw_rate = 0;
// 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);
}

4
ArduSub/control_circle.cpp
View File

@ -78,9 +78,9 @@ void Sub::circle_run()
// call attitude controller
if (circle_pilot_yaw_override) {
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
} else {
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true);
}
// adjust climb rate using rangefinder

15
ArduSub/control_guided.cpp
View File

@ -39,7 +39,6 @@ bool Sub::guided_init(bool ignore_checks)
if (!position_ok() && !ignore_checks) {
return false;
}
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// start in position control mode
@ -329,10 +328,10 @@ void Sub::guided_pos_control_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(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
}
}
@ -383,10 +382,10 @@ void Sub::guided_vel_control_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(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
}
}
@ -459,10 +458,10 @@ void Sub::guided_posvel_control_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(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
}
}
@ -509,7 +508,7 @@ void Sub::guided_angle_control_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
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
pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);

6
ArduSub/control_poshold.cpp
View File

@ -85,7 +85,7 @@ void Sub::poshold_run()
// update attitude controller targets
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
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);
last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
@ -97,11 +97,11 @@ void Sub::poshold_run()
target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate on yaw axis
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);
last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
}
}

6
ArduSub/control_stabilize.cpp
View File

@ -39,7 +39,7 @@ void Sub::stabilize_run()
// update attitude controller targets
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
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);
last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
@ -51,11 +51,11 @@ void Sub::stabilize_run()
target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate on yaw axis
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);
last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
}
}

2
ArduSub/control_surface.cpp
View File

@ -48,7 +48,7 @@ void Sub::surface_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->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);
// set target climb rate
float cmb_rate = constrain_float(abs(wp_nav.get_speed_up()), 1, pos_control.get_speed_up());

7
ArduSub/defines.h
View File

@ -65,13 +65,6 @@ enum mode_reason_t {
#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