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

Sub: move handle_attitude to where it belongs

This commit is contained in:
Willian Galvani 2023-07-14 15:37:18 -03:00
parent 6d4b90a915
commit 7cc4ee5614
2 changed files with 44 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
ArduSub/control_althold.cpp
View File

@ -65,49 +65,6 @@ void Sub::handle_mavlink_attitude_target(){
} }
} }
void Sub::handle_attitude()
{
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);
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
handle_mavlink_attitude_target();
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), desired_roll_rate, desired_pitch_rate, attitude_control.get_althold_lean_angle_max());
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);
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;
}
attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
}
}
}
// althold_run - runs the althold controller // althold_run - runs the althold controller
// should be called at 100hz or more // should be called at 100hz or more
void Sub::althold_run() void Sub::althold_run()

44
ArduSub/control_stabilize.cpp
View File

@ -38,3 +38,47 @@ void Sub::stabilize_run()
motors.set_forward(channel_forward->norm_input()); motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->norm_input()); motors.set_lateral(channel_lateral->norm_input());
} }
void Sub::handle_attitude()
{
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);
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
handle_mavlink_attitude_target();
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), desired_roll_rate, desired_pitch_rate, attitude_control.get_althold_lean_angle_max());
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);
}
}
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;
}
attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
}
}
}