Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Remove unit conversion comments

Browse Source
This commit is contained in:
Leonard Hall
2025-09-20 11:30:47 +09:00
committed by Randy Mackay
parent 95f40f8f6d
commit fb8dfd1c32
3 changed files with 10 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ArduCopter/mode_drift.cpp
+3 -15
View File
@@ -5,9 +5,6 @@
/*
* Drift flight mode — meters/second and radians version
*/
// Coupling from lateral speed error (m/s) to roll command (rad):
// Original was 8 cd/(cm/s).
// Converted: 8 [cd/(cm/s)] * (π/18000 rad/cd) * (100 cm/m) = 0.13962634 rad/(m/s)
#ifndef DRIFT_SPEEDGAIN_RAD
# define DRIFT_SPEEDGAIN_RAD 0.13962634f
@@ -16,24 +13,22 @@
#error please convert to radians and use DRIFT_SPEEDGAIN_RAD
#endif
// Speed limits and scheduling thresholds converted from cm/s
#ifndef DRIFT_SPEEDLIMIT_MS
# define DRIFT_SPEEDLIMIT_MS 5.60f // 560 cm/s -> 5.60 m/s
# define DRIFT_SPEEDLIMIT_MS 5.60f
#endif
#ifdef DRIFT_SPEEDLIMIT
#error please convert to meters per second and use DRIFT_SPEEDLIMIT_MS
#endif
#ifndef DRIFT_VEL_FORWARD_MIN_MS
# define DRIFT_VEL_FORWARD_MIN_MS 20.0f // 2000 cm/s -> 20.0 m/s
# define DRIFT_VEL_FORWARD_MIN_MS 20.0f
#endif
#ifdef DRIFT_VEL_FORWARD_MIN
#error please convert to meters per second and use DRIFT_VEL_FORWARD_MIN_MS
#endif
// Throttle assist (velz changed from cm/s to m/s, so gain ×100)
#ifndef DRIFT_THR_ASSIST_GAIN_MS
# define DRIFT_THR_ASSIST_GAIN_MS 0.18f // was 0.0018 with cm/s
# define DRIFT_THR_ASSIST_GAIN_MS 0.18f
#endif
#ifdef DRIFT_THR_ASSIST_GAIN
#error please convert to meters per second and use DRIFT_THR_ASSIST_GAIN_MS
@@ -79,8 +74,6 @@ void ModeDrift::run()
const float vel_forward_2_ms = MIN(fabsf(vel_forward_ms), DRIFT_VEL_FORWARD_MIN_MS);
// yaw-rate schedule:
// original: target_yaw_rate_cds = target_roll_cd * (1 - v/5000) * R / 45
// new: target_yaw_rate_rads = (target_roll_rad / radians(45)) * radians(R) * (1 - v/50)
const float yaw_rate_max_rads = radians(g2.command_model_acro_y.get_rate());
const float target_yaw_rate_rads = (target_roll_rad / radians(45.0f)) * yaw_rate_max_rads * (1.0f - (vel_forward_2_ms / 50.0f));
@@ -94,13 +87,9 @@ void ModeDrift::run()
roll_input_rad = roll_input_rad * 0.96f + yaw_stick_rad * 0.04f;
// convert user input into desired roll velocity term (m/s equivalent)
// original: vel_right_cms - (roll_input_cd / SPEEDGAIN)
// new: vel_right_ms - (roll_input_rad / SPEEDGAIN) [since SPEEDGAIN now rad/(m/s)]
float roll_vel_error_ms = vel_right_ms - (roll_input_rad / DRIFT_SPEEDGAIN_RAD);
// roll velocity is fed into roll angle to minimize slip
// original: target_roll_cd = roll_vel_error_cms * -SPEEDGAIN
// new: target_roll_rad = roll_vel_error_ms * -SPEEDGAIN
target_roll_rad = roll_vel_error_ms * -DRIFT_SPEEDGAIN_RAD;
// constrain to ±45 deg
@@ -108,7 +97,6 @@ void ModeDrift::run()
// If we let go of sticks, bring us to a stop
if (is_zero(target_pitch_rad)) {
// 0.14 / (0.03 * 100) timing comment (call frequency) still applies to "braker" rise;
// Clamp to the same coupling constant, now in rad/(m/s)
braker += 0.03f;
braker = MIN(braker, DRIFT_SPEEDGAIN_RAD);
ArduCopter/mode_flip.cpp
+3 -3
View File
@@ -15,14 +15,14 @@
* Pilot may manually exit flip by switching off ch7/ch8 or by moving roll stick to >40deg left or right
*
* State machine approach:
* FlipState::Start (while copter is leaning <45deg) : roll right at 400deg/sec, increase throttle
* FlipState::Roll (while copter is between +45deg ~ -90) : roll right at 400deg/sec, reduce throttle
* FlipState::Start (while copter is leaning <45deg) : roll right at 400 deg/sec, increase throttle
* FlipState::Roll (while copter is between +45deg ~ -90) : roll right at 400 deg/sec, reduce throttle
* FlipState::Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude
*/
#define FLIP_THR_INC 0.20f // throttle increase during FlipState::Start stage (under 45deg lean angle)
#define FLIP_THR_DEC 0.24f // throttle decrease during FlipState::Roll stage (between 45deg ~ -90deg roll)
#define FLIP_ROTATION_RATE_RADS radians(400.0) // rotation rate request in centi-degrees / sec (i.e. 400 deg/sec)
#define FLIP_ROTATION_RATE_RADS radians(400.0) // rotation rate request in radians / sec (i.e. 400 deg/sec)
#define FLIP_TIMEOUT_MS 2500 // timeout after 2.5sec. Vehicle will switch back to original flight mode
#define FLIP_RECOVERY_ANGLE_RAD radians(5.0) // consider successful recovery when roll is back within 5 degrees of original
ArduCopter/mode_poshold.cpp
+4 -7
View File
@@ -17,7 +17,7 @@
#define TC_WIND_COMP 0.0025f // Time constant for filtering wind compensation lean angle estimates (used in low-pass filter)
// definitions that are independent of main loop rate
#define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE_RAD radians(18.0f) // max angle required (in centi-degrees) after which the smooth stick release effect is applied
#define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE_RAD radians(18.0f) // max angle required (in radians) after which the smooth stick release effect is applied
#define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX_MS 0.10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s
#define POSHOLD_WIND_COMP_LEAN_PCT_MAX 0.6666f // wind compensation no more than 2/3rds of angle max to ensure pilot can always override
@@ -37,7 +37,7 @@ bool ModePosHold::init(bool ignore_checks)
pilot_roll_rad = 0.0f;
pilot_pitch_rad = 0.0f;
// compute brake_gain in rad/(m/s); original (cd/(cm/s)) × (π/180) -> rad/(m/s)
// compute brake_gain in rad/(m/s);
brake.gain = radians((15.0f * (float)g.poshold_brake_rate_degs + 95.0f) * 0.01f);
if (copter.ap.land_complete) {
@@ -289,7 +289,6 @@ void ModePosHold::run()
update_pilot_lean_angle_rad(pilot_pitch_rad, target_pitch_rad);
// switch to BRAKE next iteration if no pilot input
// NOTE: preserve original behavior (cd vs deg quirk) => 0.02 * deg threshold, then to radians
if (is_zero(target_pitch_rad) && (fabsf(pilot_pitch_rad) < radians(2 * g.poshold_brake_rate_degs))) {
// initialise BRAKE mode
pitch_mode = RPMode::BRAKE; // set brake pitch mode
@@ -468,7 +467,7 @@ void ModePosHold::run()
roll_mode = RPMode::BRAKE_READY_TO_LOITER;
brake.roll_rad = 0.0f;
}
// if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time)
// if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time)
}
}
break;
@@ -498,7 +497,7 @@ void ModePosHold::update_pilot_lean_angle_rad(float &lean_angle_filtered_rad, fl
if ((lean_angle_filtered_rad > 0.0 && lean_angle_raw_rad < 0.0) || (lean_angle_filtered_rad < 0.0 && lean_angle_raw_rad > 0.0) || (fabsf(lean_angle_raw_rad) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE_RAD)) {
lean_angle_filtered_rad = lean_angle_raw_rad;
} else {
// lean_angle_raw_cd must be pulling lean_angle_filtered_cd towards zero, smooth the decrease
// lean_angle_raw must be pulling lean_angle_filtered towards zero, smooth the decrease
const float brake_rate_step_rad = radians((float)g.poshold_brake_rate_degs) * G_Dt;
if (lean_angle_filtered_rad > 0.0) {
// reduce the filtered lean angle at 1.25% per step or the brake rate (whichever is faster).
@@ -530,8 +529,6 @@ void ModePosHold::update_brake_angle_from_velocity(float &brake_angle_rad, float
const float brake_delta_rad = radians((float)g.poshold_brake_rate_degs) * G_Dt;
// velocity-shaped lean angle:
// original: -gain * v_cms * (1 + 500/(|v|+60))
// SI: -gain * v_ms * (1 + 5.0/(|v|+0.60))
float lean_angle_rad = -brake.gain * velocity_ms * (1.0f + 5.0f / (fabsf(velocity_ms) + 0.60f));
// do not let lean_angle be too far from brake_angle