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

Copter: Add units to the AC_AttitudeControl Library

Browse Source
This commit is contained in:
Leonard Hall 2021-09-06 19:14:18 +09:30 committed by Randy Mackay
parent ad278779e3
commit f3dc47ce3d
16 changed files with 26 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/mode.cpp
View File

@ -623,7 +623,7 @@ void Mode::land_run_horizontal_control()
update_simple_mode();
// convert pilot input to lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
// record if pilot has overridden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
@ -737,7 +737,7 @@ void Mode::precland_retry_position(const Vector3f &retry_pos)
float target_roll = 0.0f;
float target_pitch = 0.0f;
// convert pilot input to lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
// record if pilot has overridden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_pitch)) {

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) {
const float angle_max = copter.aparm.angle_max;
if (roll_angle > angle_max){
rate_ef_level.x += 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_cdss(), G_Dt);
}else if (roll_angle < -angle_max) {
rate_ef_level.x += 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_cdss(), G_Dt);
}
if (pitch_angle > angle_max){
rate_ef_level.y += 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_cdss(), G_Dt);
}else if (pitch_angle < -angle_max) {
rate_ef_level.y += 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_cdss(), G_Dt);
}
}

2
ArduCopter/mode_althold.cpp
View File

@ -33,7 +33,7 @@ void ModeAltHold::run()
// get pilot desired lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

2
ArduCopter/mode_autotune.cpp
View File

@ -84,7 +84,7 @@ float AutoTune::get_pilot_desired_climb_rate_cms(void) const
void AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitch_cd, float &yaw_rate_cds)
{
copter.mode_autotune.get_pilot_desired_lean_angles(des_roll_cd, des_pitch_cd, copter.aparm.angle_max,
copter.attitude_control->get_althold_lean_angle_max());
copter.attitude_control->get_althold_lean_angle_max_cd());
yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in());
}

2
ArduCopter/mode_flowhold.cpp
View File

@ -315,7 +315,7 @@ void ModeFlowHold::run()
int16_t roll_in = copter.channel_roll->get_control_in();
int16_t pitch_in = copter.channel_pitch->get_control_in();
float angle_max = copter.aparm.angle_max;
get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y, angle_max, attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y, angle_max, attitude_control->get_althold_lean_angle_max_cd());
if (quality_filtered >= flow_min_quality &&
AP_HAL::millis() - copter.arm_time_ms > 3000) {

2
ArduCopter/mode_guided.cpp
View File

@ -806,7 +806,7 @@ void ModeGuided::angle_control_run()
float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd;
float total_in = norm(roll_in, pitch_in);
float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), copter.aparm.angle_max);
float angle_max = MIN(attitude_control->get_althold_lean_angle_max_cd(), copter.aparm.angle_max);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;

2
ArduCopter/mode_land.cpp
View File

@ -110,7 +110,7 @@ void ModeLand::nogps_run()
update_simple_mode();
// get pilot desired lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
}
// get pilot's desired yaw rate

4
ArduCopter/mode_loiter.cpp
View File

@ -15,7 +15,7 @@ bool ModeLoiter::init(bool ignore_checks)
update_simple_mode();
// convert pilot input to lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
// process pilot's roll and pitch input
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
@ -90,7 +90,7 @@ void ModeLoiter::run()
update_simple_mode();
// convert pilot input to lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
// process pilot's roll and pitch input
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);

2
ArduCopter/mode_poshold.cpp
View File

@ -80,7 +80,7 @@ void ModePosHold::run()
// convert pilot input to lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

2
ArduCopter/mode_rtl.cpp
View File

@ -319,7 +319,7 @@ void ModeRTL::descent_run()
update_simple_mode();
// convert pilot input to lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
// record if pilot has overridden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_pitch)) {

8
ArduCopter/mode_sport.cpp
View File

@ -50,15 +50,15 @@ void ModeSport::run()
const float angle_max = copter.aparm.angle_max;
if (roll_angle > angle_max){
target_roll_rate += 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_cdss(), G_Dt);
}else if (roll_angle < -angle_max) {
target_roll_rate += 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_cdss(), G_Dt);
}
if (pitch_angle > angle_max){
target_pitch_rate += 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_cdss(), G_Dt);
}else if (pitch_angle < -angle_max) {
target_pitch_rate += 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_cdss(), G_Dt);
}
// get pilot's desired yaw rate

4
ArduCopter/mode_systemid.cpp
View File

@ -185,9 +185,9 @@ void ModeSystemId::run()
gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: Landed");
break;
}
if (attitude_control->lean_angle()*100 > attitude_control->lean_angle_max()) {
if (attitude_control->lean_angle_deg()*100 > attitude_control->lean_angle_max_cd()) {
systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED;
gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: lean=%f max=%f", (double)attitude_control->lean_angle(), (double)attitude_control->lean_angle_max());
gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: lean=%f max=%f", (double)attitude_control->lean_angle_deg(), (double)attitude_control->lean_angle_max_cd());
break;
}
if (waveform_time > SYSTEM_ID_DELAY + time_fade_in + time_const_freq + time_record + time_fade_out) {

4
ArduCopter/mode_zigzag.cpp
View File

@ -73,7 +73,7 @@ bool ModeZigZag::init(bool ignore_checks)
// convert pilot input to lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
// process pilot's roll and pitch input
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
@ -300,7 +300,7 @@ void ModeZigZag::manual_control()
update_simple_mode();
// convert pilot input to lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
// process pilot's roll and pitch input
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);

2
ArduPlane/mode_qloiter.cpp
View File

@ -63,7 +63,7 @@ void ModeQLoiter::run()
// process pilot's roll and pitch input
float target_roll_cd, target_pitch_cd;
quadplane.get_pilot_desired_lean_angles(target_roll_cd, target_pitch_cd, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
quadplane.get_pilot_desired_lean_angles(target_roll_cd, target_pitch_cd, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
loiter_nav->set_pilot_desired_acceleration(target_roll_cd, target_pitch_cd);
// run loiter controller

2
ArduSub/control_althold.cpp
View File

@ -67,7 +67,7 @@ void Sub::althold_run()
return;
}
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max_cd());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

2
ArduSub/control_guided.cpp
View File

@ -470,7 +470,7 @@ void Sub::guided_angle_control_run()
float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd;
float total_in = norm(roll_in, pitch_in);
float angle_max = MIN(attitude_control.get_althold_lean_angle_max(), aparm.angle_max);
float angle_max = MIN(attitude_control.get_althold_lean_angle_max_cd(), aparm.angle_max);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;