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

Copter: use AP_Math control functions

This commit is contained in:
Leonard Hall 2020-12-15 23:18:30 +10:30 committed by Andrew Tridgell
parent 4e2c7880c5
commit 5efe94a771
4 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/mode.cpp
View File

@ -554,7 +554,7 @@ void Mode::land_run_vertical_control(bool pause_descent)
max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed)); max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
// Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low. // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low.
cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt); cmb_rate = sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt);
// Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed)); cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));

8
ArduCopter/mode_acro.cpp
View File

@ -159,15 +159,15 @@ void ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in,
if (g.acro_trainer == (uint8_t)Trainer::LIMITED) { if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {
const float angle_max = copter.aparm.angle_max; const float angle_max = copter.aparm.angle_max;
if (roll_angle > angle_max){ if (roll_angle > angle_max){
rate_ef_level.x += AC_AttitudeControl::sqrt_controller(angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); rate_ef_level.x += sqrt_controller(angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt);
}else if (roll_angle < -angle_max) { }else if (roll_angle < -angle_max) {
rate_ef_level.x += AC_AttitudeControl::sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); rate_ef_level.x += sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt);
} }
if (pitch_angle > angle_max){ if (pitch_angle > angle_max){
rate_ef_level.y += AC_AttitudeControl::sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); rate_ef_level.y += sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt);
}else if (pitch_angle < -angle_max) { }else if (pitch_angle < -angle_max) {
rate_ef_level.y += AC_AttitudeControl::sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); rate_ef_level.y += sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt);
} }
} }

2
ArduCopter/mode_auto.cpp
View File

@ -963,7 +963,7 @@ void ModeAuto::loiter_to_alt_run()
// Compute a vertical velocity demand such that the vehicle // Compute a vertical velocity demand such that the vehicle
// approaches the desired altitude. // approaches the desired altitude.
float target_climb_rate = AC_AttitudeControl::sqrt_controller( float target_climb_rate = sqrt_controller(
-alt_error_cm, -alt_error_cm,
pos_control->get_pos_z_p().kP(), pos_control->get_pos_z_p().kP(),
pos_control->get_max_accel_z(), pos_control->get_max_accel_z(),

8
ArduCopter/mode_sport.cpp
View File

@ -54,15 +54,15 @@ void ModeSport::run()
const float angle_max = copter.aparm.angle_max; const float angle_max = copter.aparm.angle_max;
if (roll_angle > angle_max){ if (roll_angle > angle_max){
target_roll_rate += AC_AttitudeControl::sqrt_controller(angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); target_roll_rate += sqrt_controller(angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt);
}else if (roll_angle < -angle_max) { }else if (roll_angle < -angle_max) {
target_roll_rate += AC_AttitudeControl::sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); target_roll_rate += sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt);
} }
if (pitch_angle > angle_max){ if (pitch_angle > angle_max){
target_pitch_rate += AC_AttitudeControl::sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); target_pitch_rate += sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt);
}else if (pitch_angle < -angle_max) { }else if (pitch_angle < -angle_max) {
target_pitch_rate += AC_AttitudeControl::sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); target_pitch_rate += sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt);
} }
// get pilot's desired yaw rate // get pilot's desired yaw rate