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

Sub: revert attitude control to 3.5 behavior

This commit is contained in:
Willian Galvani 2023-07-16 11:12:45 -03:00
parent 7cc4ee5614
commit 7d4693339e
1 changed files with 44 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
ArduSub/control_stabilize.cpp
View File

@ -42,6 +42,7 @@ void Sub::stabilize_run()
void Sub::handle_attitude()
{
uint32_t tnow = AP_HAL::millis();
float desired_roll_rate, desired_pitch_rate, desired_yaw_rate;
// initialize vertical speeds and acceleration
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
@ -53,32 +54,50 @@ void Sub::handle_attitude()
float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * gain, channel_yaw->get_radio_trim());
desired_yaw_rate = get_pilot_desired_yaw_rate(yaw_input);
switch (g.control_frame) {
case MAV_FRAME_BODY_FRD:
{
if (abs(desired_roll_rate) > 50 || abs(desired_pitch_rate) > 50 || abs(desired_yaw_rate) > 50) {
attitude_control.input_rate_bf_roll_pitch_yaw(desired_roll_rate, desired_pitch_rate, desired_yaw_rate);
Vector3f attitude_target = attitude_control.get_att_target_euler_cd();
last_roll = attitude_target.x;
last_pitch = attitude_target.y;
last_pilot_heading = attitude_target.z;
} else {
attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
switch (g.control_frame)
{
case MAV_FRAME_BODY_FRD:
{
if (abs(desired_roll_rate) > 50 || abs(desired_pitch_rate) > 50 || abs(desired_yaw_rate) > 50)
{
attitude_control.input_rate_bf_roll_pitch_yaw(desired_roll_rate, desired_pitch_rate, desired_yaw_rate);
Quaternion attitude_target = attitude_control.get_attitude_target_quat();
last_roll = degrees(attitude_target.get_euler_roll()) * 100;
last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
}
else
{
attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
}
}
}
break;
default:
{
if (!is_zero(desired_roll_rate) || !is_zero(desired_pitch_rate) || !is_zero(desired_yaw_rate)) {
attitude_control.input_euler_rate_roll_pitch_yaw(desired_roll_rate, desired_pitch_rate, desired_yaw_rate);
Vector3f attitude_target = attitude_control.get_att_target_euler_cd();
last_roll = attitude_target.x;
last_pitch = attitude_target.y;
last_pilot_heading = attitude_target.z;
last_input_ms = AP_HAL::millis();
return;
break;
default:
{
if (!is_zero(desired_yaw_rate))
{ // call attitude controller with rate yaw determined by pilot input
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(last_roll, last_pitch, desired_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
}
else
{ // hold current heading
// this check is required to prevent bounce back after very fast yaw maneuvers
// the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
if (tnow < last_pilot_yaw_input_ms + 250)
{ // give 250ms to slow down, then set target heading
desired_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate onqq yaw axis
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(last_roll, last_pitch, desired_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(last_roll, last_pitch, last_pilot_heading, true);
}
}
}
attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
}
}
}