Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-10 15:03:43 +08: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
2018-03-16 13:50:57 +09:00
parent a727305a59
commit da17034a3d
10 changed files with 27 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ArduSub/control_poshold.cpp
+4 -4
View File
@@ -36,7 +36,7 @@ bool Sub::poshold_init()
void Sub::poshold_run()
{
uint32_t tnow = AP_HAL::millis();
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@@ -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);
}
}