From fb8dfd1c3276ff63b13f7cdff1d285a9813f6c35 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 19 Sep 2025 16:18:43 +0930 Subject: [PATCH] Copter: Remove unit conversion comments --- ArduCopter/mode_drift.cpp | 18 +++--------------- ArduCopter/mode_flip.cpp | 6 +++--- ArduCopter/mode_poshold.cpp | 11 ++++------- 3 files changed, 10 insertions(+), 25 deletions(-) diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index de49f918d0c..e843b3f2485 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -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); diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index 5bca152b808..bfc7a1ed4cb 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -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 diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index ceaafe5b2f5..39d5fe7d40b 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -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