From 99adf3e94dd29876c618f4987810f873393d6f83 Mon Sep 17 00:00:00 2001 From: Ferruccio1984 Date: Thu, 24 Mar 2022 10:05:22 -0700 Subject: [PATCH 01/61] AC_Autorotation: Assisted Autorotation Implementation --- libraries/AC_Autorotation/AC_Autorotation.cpp | 384 ++++++++++++++++-- libraries/AC_Autorotation/AC_Autorotation.h | 93 ++++- 2 files changed, 444 insertions(+), 33 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 627dd7894ec31..aaf9432fa8712 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -2,22 +2,30 @@ #include #include #include +#include //Autorotation controller defaults #define AROT_BAIL_OUT_TIME 2.0f // Default time for bail out controller to run (unit: s) +#define ROT_SOLIDITY 0.05f // Main rotor solidity +#define ROT_DIAMETER 1.25f // Main rotor diameter // Head Speed (HS) controller specific default definitions #define HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ 2.0f // low-pass filter on accel error (unit: hz) #define HS_CONTROLLER_HEADSPEED_P 0.7f // Default P gain for head speed controller (unit: -) #define HS_CONTROLLER_ENTRY_COL_FILTER 0.7f // Default low pass filter frequency during the entry phase (unit: Hz) #define HS_CONTROLLER_GLIDE_COL_FILTER 0.1f // Default low pass filter frequency during the glide phase (unit: Hz) +#define HS_CONTROLLER_CUSHION_COL_FILTER 0.5f // Speed Height controller specific default definitions for autorotation use #define FWD_SPD_CONTROLLER_GND_SPEED_TARGET 1100 // Default target ground speed for speed height controller (unit: cm/s) #define FWD_SPD_CONTROLLER_MAX_ACCEL 60 // Default acceleration limit for speed height controller (unit: cm/s/s) #define AP_FW_VEL_P 0.9f +#define TCH_P 0.1f #define AP_FW_VEL_FF 0.15f +// flare controller default definitions +#define AP_ALPHA_TPP 20.0f +#define AP_T_TO_G 0.55f const AP_Param::GroupInfo AC_Autorotation::var_info[] = { @@ -114,14 +122,60 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Increment: 0.01 // @User: Advanced AP_GROUPINFO("FW_V_FF", 11, AC_Autorotation, _param_fwd_k_ff, AP_FW_VEL_FF), + + // @Param: TCH_P + // @DisplayName: P gain for vertical touchdown controller + // @Description: proportional term based on sink rate error + // @Range: 0.3 1 + // @Increment: 0.01 + // @User: Advanced + AP_SUBGROUPINFO(_p_coll_tch, "TCH_", 12, AC_Autorotation, AC_P), + + // @Param: COL_FILT_C + // @DisplayName: Touchdown Phase Collective Filter + // @Description: Cut-off frequency for collective low pass filter. For the touchdown phase. Acts as a following trim. + // @Units: Hz + // @Range: 0.2 0.8 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO("COL_FILT_C", 13, AC_Autorotation, _param_col_cushion_cutoff_freq, HS_CONTROLLER_CUSHION_COL_FILTER), + + // @Param: SOLIDITY + // @DisplayName: rotor solidity + // @Description: helicopter specific main rotor solidity + // @Range: 0.001 0.01 + // @Increment: 0.001 + // @User: Advanced + AP_GROUPINFO("ROT_SOL", 14, AC_Autorotation, _param_solidity, ROT_SOLIDITY), + + // @Param: ROT_DIAM + // @DisplayName: rotor diameter + // @Description: helicopter specific main rotor diameter + // @Units: m + // @Range: 0.001 0.01 + // @Increment: 0.001 + // @User: Advanced + AP_GROUPINFO("ROT_DIAM", 15, AC_Autorotation, _param_diameter, ROT_DIAMETER), + + // @Param: time touchdown + // @DisplayName: time touchdown + // @Description: time touchdown + // @Units: s + // @Range: 0.001 0.01 + // @Increment: 0.001 + // @User: Advanced + AP_GROUPINFO("T_TCH", 16, AC_Autorotation, _t_tch, AP_T_TO_G), AP_GROUPEND }; // Constructor -AC_Autorotation::AC_Autorotation() : +AC_Autorotation::AC_Autorotation(AP_InertialNav& inav, AP_AHRS& ahrs) : + _inav(inav), + _ahrs(ahrs), _p_hs(HS_CONTROLLER_HEADSPEED_P), - _p_fw_vel(AP_FW_VEL_P) + _p_fw_vel(AP_FW_VEL_P), + _p_coll_tch(TCH_P) { AP_Param::setup_object_defaults(this, var_info); } @@ -130,7 +184,7 @@ AC_Autorotation::AC_Autorotation() : void AC_Autorotation::init_hs_controller() { // Set initial collective position to be the collective position on initialisation - _collective_out = 0.4f; + _collective_out = _col_mid; // Reset feed forward filter col_trim_lpf.reset(_collective_out); @@ -144,10 +198,11 @@ void AC_Autorotation::init_hs_controller() // Protect against divide by zero _param_head_speed_set_point.set(MAX(_param_head_speed_set_point,500)); -} +} -bool AC_Autorotation::update_hs_glide_controller(float dt) +// Rotor Speed controller for entry, glide and flare phases of autorotation +bool AC_Autorotation::update_hs_glide_controller(void) { // Reset rpm health flag _flags.bad_rpm = false; @@ -170,24 +225,40 @@ bool AC_Autorotation::update_hs_glide_controller(float dt) _p_term_hs = _p_hs.get_p(_head_speed_error); // Adjusting collective trim using feed forward (not yet been updated, so this value is the previous time steps collective position) - _ff_term_hs = col_trim_lpf.apply(_collective_out, dt); + _ff_term_hs = col_trim_lpf.apply(_collective_out, _dt); // Calculate collective position to be set - _collective_out = _p_term_hs + _ff_term_hs; + _collective_out = constrain_value((_p_term_hs + _ff_term_hs), 0.0f, 1.0f) ; } else { // RPM sensor is bad set collective to minimum - _collective_out = -1.0f; + _collective_out = 0.0f; _flags.bad_rpm_warning = true; } // Send collective to setting to motors output library set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); + return _flags.bad_rpm_warning; } +void AC_Autorotation::update_hover_autorotation_controller() +{ + + // Set collective trim low pass filter cut off frequency + col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); + + // use zero thrust collective to minimize rotor speed loss + _ff_term_hs = col_trim_lpf.apply(_col_mid, _dt); + + // Calculate collective position to be set + _collective_out = constrain_value(_ff_term_hs, 0.0f, 1.0f) ; + + // Send collective to setting to motors output library + set_collective(_col_cutoff_freq); +} // Function to set collective and collective filter in motor library void AC_Autorotation::set_collective(float collective_filter_cutoff) const @@ -199,7 +270,6 @@ void AC_Autorotation::set_collective(float collective_filter_cutoff) const } } - //function that gets rpm and does rpm signal checking to ensure signal is reliable //before using it in the controller float AC_Autorotation::get_rpm(bool update_counter) @@ -251,6 +321,74 @@ float AC_Autorotation::get_rpm(bool update_counter) return current_rpm; } +void AC_Autorotation::initial_flare_estimate() +{ + //estimate hover thrust + float _col_hover_rad = radians(_col_min + (_col_max - _col_min)*_col_hover); + float b = _param_solidity*6.28f; + _disc_area=3.14*sq(_param_diameter*0.5f); + float lambda = (-(b/8.0f) + safe_sqrt((sq(b))/64.0f +((b/3.0f)*_col_hover_rad)))*0.5f; + float freq=_governed_rpm/60.0f; + float tip_speed= freq*6.28f*_param_diameter*0.5f; + _lift_hover = ((1.225f*sq(tip_speed)*(_param_solidity*_disc_area))*((_col_hover_rad/3.0f) - (lambda/2.0f))*5.8f)*0.5f; + + //estimate rate of descent + float omega_auto=(_param_head_speed_set_point/60.0f)*6.28f; + float tip_speed_auto = omega_auto*_param_diameter*0.5f; + float c_t = _lift_hover/(0.6125f*_disc_area*sq(tip_speed)); + _est_rod=((0.25f*(_param_solidity*0.011f/c_t)*tip_speed_auto)+((sq(c_t)/(_param_solidity*0.011f))*tip_speed_auto)); + + //estimate rotor C_d + _c=(_lift_hover/(sq(_est_rod)*0.5f*1.225f*_disc_area))*1.15f; + _c=constrain_float(_c, 0.7f, 1.4f); + + //calc flare altitude + float des_spd_fwd=_param_target_speed*0.01f; + calc_flare_alt(-_est_rod,des_spd_fwd); + + //initialize sink rate monitor and flare bools + _flare_complete = false; + _flare_update_check = false; + _avg_sink_deriv = 0.0f; + _avg_sink_deriv_sum = 0.0f; + _index_sink_rate = 0; + memset(_curr_sink_deriv, 0, sizeof(_curr_sink_deriv)); + + gcs().send_text(MAV_SEVERITY_INFO, "Ct/σ=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/9.8065f, _flare_alt_calc*0.01f, _c); + +} + + + +void AC_Autorotation::calc_flare_alt(float sink_rate, float fwd_speed) +{ + //compute speed module and glide path angle during descent + float speed_module=norm(sink_rate,fwd_speed); + float glide_angle=safe_asin(3.14/2-(fwd_speed/speed_module)); + + //estimate inflow velocity at beginning of flare + float inflow= -speed_module*sinf(glide_angle+radians(AP_ALPHA_TPP)); + + //estimate flare duration + float k_1=fabsf((-sink_rate+0.001f-safe_sqrt(_lift_hover/_c))/(-sink_rate+0.001f+safe_sqrt(_lift_hover/_c))); + float k_2=fabsf((inflow-safe_sqrt(_lift_hover/_c))/(inflow+safe_sqrt(_lift_hover/_c))); + float delta_t_flare=(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1))-(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_2)); + + //estimate flare delta altitude + float a=(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*delta_t_flare + (2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1)); + float x=1-expf(a); + float s=1-expf((2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1))); + float d=safe_sqrt(_lift_hover/_c); + float flare_distance=((2*d/(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065))))*(a-logf(fabsf(x))-(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1)) +logf(fabsf(s))))-d*delta_t_flare; + float delta_h= -flare_distance*cosf(radians(AP_ALPHA_TPP)); + + //estimate altitude to begin collective pull + _cushion_alt = (-(sink_rate*cosf(radians(AP_ALPHA_TPP)))*_t_tch)*100.0f; + gcs().send_text(MAV_SEVERITY_INFO, "vs_final_est=%f ", sink_rate*cosf(radians(20))); + //total delta altitude to ground + _flare_alt_calc = _cushion_alt+delta_h*100.0f; +} + #if HAL_LOGGING_ENABLED void AC_Autorotation::Log_Write_Autorotation(void) const @@ -260,35 +398,35 @@ void AC_Autorotation::Log_Write_Autorotation(void) const // @Description: Helicopter AutoRotation information // @Field: TimeUS: Time since system startup // @Field: P: P-term for headspeed controller response -// @Field: hserr: head speed error; difference between current and desired head speed -// @Field: ColOut: Collective Out +// @Field: hs_e: head speed error; difference between current and desired head speed +// @Field: C_Out: Collective Out // @Field: FFCol: FF-term for headspeed controller response -// @Field: CRPM: current headspeed RPM // @Field: SpdF: current forward speed -// @Field: CmdV: desired forward speed +// @Field: DH: desired forward speed // @Field: p: p-term of velocity response // @Field: ff: ff-term of velocity response -// @Field: AccO: forward acceleration output -// @Field: AccT: forward acceleration target -// @Field: PitT: pitch target +// @Field: AccZ: average z acceleration +// @Field: DesV: Desired Sink Rate +// @Field: Rfnd: rangefinder altitude +// @Field: Hest: estimated altitude //Write to data flash log AP::logger().WriteStreaming("AROT", - "TimeUS,P,hserr,ColOut,FFCol,CRPM,SpdF,CmdV,p,ff,AccO,AccT,PitT", + "TimeUS,P,hs_e,C_Out,FFCol,SpdF,DH,p,ff,AccZ,DesV,Rfnd,Hest", "Qffffffffffff", AP_HAL::micros64(), (double)_p_term_hs, (double)_head_speed_error, (double)_collective_out, (double)_ff_term_hs, - (double)_current_rpm, - (double)_speed_forward, - (double)_cmd_vel, + (double)(_speed_forward*0.01f), + (double)(_cmd_vel*0.01f), (double)_vel_p, (double)_vel_ff, - (double)_accel_out, - (double)_accel_target, - (double)_pitch_target); + (double)_avg_acc_z, + (double)_desired_sink_rate, + (double)(_radar_alt*0.01f), + (double)(_est_alt*0.01f)) ; } #endif // HAL_LOGGING_ENABLED @@ -299,7 +437,7 @@ void AC_Autorotation::init_fwd_spd_controller(void) _accel_target = 0.0f; // Ensure parameter acceleration doesn't exceed hard-coded limit - _accel_max = MIN(_param_accel_max, 60.0f); + _accel_max = MIN(_param_accel_max, 500.0f); // Reset cmd vel and last accel to sensible values _cmd_vel = calc_speed_forward(); //(cm/s) @@ -370,9 +508,46 @@ void AC_Autorotation::update_forward_speed_controller(void) } _accel_out_last = _accel_out; - // update angle targets that will be passed to stabilize controller - _pitch_target = accel_to_angle(-_accel_out*0.01) * 100; + if (_est_alt >= _flare_alt_calc*1.25f){ + _pitch_target = accel_to_angle(-_accel_out*0.01) * 100; + }else{ + _pitch_target = 0.0f; + } +} + +void AC_Autorotation::calc_sink_d_avg() +{ + float vertical_speed = _inav.get_velocity_z_up_cms(); + _sink_deriv= ((vertical_speed - _last_vertical_speed)*0.01f)/_dt; + _last_vertical_speed = vertical_speed; + + if(_index_sink_rate < 20){ + _avg_sink_deriv_sum -= _curr_sink_deriv[_index_sink_rate]; + _curr_sink_deriv[_index_sink_rate]= _sink_deriv; + _avg_sink_deriv_sum += _curr_sink_deriv[_index_sink_rate]; + _index_sink_rate = _index_sink_rate+1; + }else{ + _index_sink_rate = 0; + } + _avg_sink_deriv = _avg_sink_deriv_sum/20.0f; + _avg_sink_deriv = constrain_float( _avg_sink_deriv, -10.0f,10.0f); +} +void AC_Autorotation::update_flare_alt() +{ + if(!_flare_update_check){ + float delta_v_z = fabsf((_inav.get_velocity_z_up_cms())*0.01f+_est_rod); + + if(_speed_forward >= 0.8f*_param_target_speed && delta_v_z<=1 && fabsf(_avg_sink_deriv)<=0.005 ){ + float vel_z=_inav.get_velocity_z_up_cms()*0.01f; + float spd_fwd=_speed_forward*0.01f; + _c=(_lift_hover/(sq(vel_z)*0.5f*1.225f*_disc_area))*1.15f; + _c=constrain_float(_c, 0.7f, 1.4f); + calc_flare_alt(vel_z,spd_fwd); + _flare_update_check = true; + gcs().send_text(MAV_SEVERITY_INFO, "Flare_alt_updated=%f C_updated=%f", _flare_alt_calc*0.01f, _c); + } + } } @@ -385,3 +560,160 @@ float AC_Autorotation::calc_speed_forward(void) return speed_forward; } +void AC_Autorotation::flare_controller() +{ + + // Specify forward velocity component and determine delta velocity with respect to time + _speed_forward = calc_speed_forward(); //(cm/s) + _delta_speed_fwd = _speed_forward - _speed_forward_last; //(cm/s) + _speed_forward_last = _speed_forward; //(cm/s) + _desired_speed = linear_interpolate(0.0f, _flare_entry_speed, _est_alt, _cushion_alt, _flare_alt_calc); + + // get p + _vel_p = _p_fw_vel.get_p(_desired_speed - _speed_forward); + + //calculate acceleration target based on PI controller + _accel_target = _vel_p ; + + // filter correction acceleration + _accel_target_filter.set_cutoff_frequency(10.0f); + _accel_target_filter.apply(_accel_target, _dt); + + //Limits the maximum change in pitch attitude based on acceleration + if (_accel_target > _accel_out_last + _accel_max) { + _accel_target = _accel_out_last + _accel_max; + } else if (_accel_target < _accel_out_last - _accel_max) { + _accel_target = _accel_out_last - _accel_max; + } + + //Limiting acceleration based on velocity gained during the previous time step + if (fabsf(_delta_speed_fwd) > _accel_max * _dt) { + _flag_limit_accel = true; + } else { + _flag_limit_accel = false; + } + + if ((_flag_limit_accel && fabsf(_accel_target) < fabsf(_accel_out_last)) || !_flag_limit_accel) { + _accel_out = _accel_target; + } else { + _accel_out = _accel_out_last; + } + _accel_out_last = _accel_out; + + + + //estimate flare effectiveness + + if(_speed_forward <= (0.6*_flare_entry_speed) && fabsf(_avg_sink_deriv)<=0.005f && _avg_acc_z>= -(1.1*9.80665f) ){ + if(!_flare_complete){ + _flare_complete = true; + gcs().send_text(MAV_SEVERITY_INFO, "Flare_complete"); + } + } + + if(!_flare_complete){ + _pitch_target = atanf(-_accel_out/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI); + _pitch_target = constrain_float(_pitch_target, 0.0f, AP_ALPHA_TPP*100.0f); + }else{ + _pitch_target *= 0.95f; + } + + +} + +void AC_Autorotation::touchdown_controller() +{ + float _current_sink_rate = _inav.get_velocity_z_up_cms(); + if(_radar_alt>=_ground_clearance){ + _desired_sink_rate = linear_interpolate(0.0f, _entry_sink_rate, _radar_alt, _ground_clearance, _entry_alt); + }else{ + _desired_sink_rate = 0.0f; + } + // update forward speed for logging + _speed_forward = calc_speed_forward(); //(cm/s) + + _collective_out = constrain_value((_p_coll_tch.get_p(_desired_sink_rate - _current_sink_rate))*0.01f + _ff_term_hs, 0.0f, 1.0f); + col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); + _ff_term_hs = col_trim_lpf.apply(_collective_out, _dt); + set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); + _pitch_target *= 0.95f; +} + +void AC_Autorotation::get_entry_speed() +{ + _flare_entry_speed = calc_speed_forward(); +} + +void AC_Autorotation::time_to_ground() +{ + if(_inav.get_velocity_z_up_cms() < 0.0f ) { + _time_to_ground = -(_radar_alt/_inav.get_velocity_z_up_cms()); + }else{ + _time_to_ground = _t_tch +1.0f; + } +} + +void AC_Autorotation::init_est_radar_alt() +{ + // set descent rate filter cutoff frequency + descent_rate_lpf.set_cutoff_frequency(40.0f); + + // Reset feed descent rate filter + descent_rate_lpf.reset(_inav.get_velocity_z_up_cms()); + + _radar_alt_calc = _radar_alt; + _radar_alt_prev = _radar_alt; + _est_alt = _radar_alt; + +} + +void AC_Autorotation::update_est_radar_alt() +{ + if(_using_rfnd) { + // continue calculating radar altitude based on the most recent update and descent rate + if (is_equal(_radar_alt, _radar_alt_prev)) { + _radar_alt_calc += (_inav.get_velocity_z_up_cms() * _dt); + } else { + _radar_alt_calc = _radar_alt; + _radar_alt_prev = _radar_alt; + } + // determine the error between a calculated radar altitude based on each update at 20 hz and the estimated update + float alt_error = _radar_alt_calc - _est_alt; + // drive the estimated altitude to the actual altitude with a proportional altitude error feedback + float descent_rate_corr = _inav.get_velocity_z_up_cms() + alt_error * 2.0f; + // update descent rate filter + _descent_rate_filtered = descent_rate_lpf.apply(descent_rate_corr); + _est_alt += (_descent_rate_filtered * _dt); + } else { + _est_alt = _radar_alt; + // Reset feed descent rate filter + descent_rate_lpf.reset(_inav.get_velocity_z_up_cms()); + // reset variables until using rangefinder + _radar_alt_calc = _radar_alt; + _radar_alt_prev = _radar_alt; + _est_alt = _radar_alt; + } +} + +void AC_Autorotation::init_avg_acc_z() +{ + _avg_acc_z = 0.0f; + _acc_z_sum = 0.0f; + _index = 0; + memset(_curr_acc_z, 0, sizeof(_curr_acc_z)); + +} + +void AC_Autorotation::calc_avg_acc_z() +{ + if(_index < 10){ + _acc_z_sum -= _curr_acc_z[_index]; + _curr_acc_z[_index]= _ahrs.get_accel_ef().z; + _acc_z_sum += _curr_acc_z[_index]; + _index = _index+1; + }else{ + _index = 0; + } + _avg_acc_z = _acc_z_sum/10.0f; +} + diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index d5cfa52097b3f..c65dcd5c74e8f 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -8,19 +8,20 @@ #include #include #include - +#include // Inertial Navigation library class AC_Autorotation { public: //Constructor - AC_Autorotation(); + AC_Autorotation(AP_InertialNav& inav, AP_AHRS& ahrs); //--------Functions-------- void init_hs_controller(void); // Initialise head speed controller + void initial_flare_estimate(void); void init_fwd_spd_controller(void); // Initialise forward speed controller - bool update_hs_glide_controller(float dt); // Update head speed controller + bool update_hs_glide_controller(void); // Update head speed controller float get_rpm(void) const { return _current_rpm; } // Function just returns the rpm as last read in this library float get_rpm(bool update_counter); // Function fetches fresh rpm update and continues sensor health monitoring void set_target_head_speed(float ths) { _target_head_speed = ths; } // Sets the normalised target head speed @@ -28,6 +29,7 @@ class AC_Autorotation int16_t get_hs_set_point(void) { return _param_head_speed_set_point; } float get_col_entry_freq(void) { return _param_col_entry_cutoff_freq; } float get_col_glide_freq(void) { return _param_col_glide_cutoff_freq; } + float get_col_cushion_freq(void) { return _param_col_cushion_cutoff_freq; } float get_bail_time(void) { return _param_bail_time; } float get_last_collective() const { return _collective_out; } bool is_enable(void) { return _param_enable; } @@ -37,10 +39,44 @@ class AC_Autorotation void set_desired_fwd_speed(float speed) { _vel_target = speed; } // Overloaded: Set desired speed to argument value int32_t get_pitch(void) const { return _pitch_target; } // Get pitch target float calc_speed_forward(void); // Calculates the forward speed in the horizontal plane - void set_dt(float delta_sec); + void set_dt(float delta_sec); // set the loop time + void flare_controller(); // Update the flare controller + void touchdown_controller(); // Update the touchdown controller + void set_ground_distance(float radalt) { _radar_alt = radalt; } + void get_entry_speed(); + float get_ground_distance() const { return _radar_alt; } + float get_time_to_ground() const { return _time_to_ground; } + void time_to_ground(); + void set_collective_minimum_drag(float col_mid ) { _col_mid = col_mid; } + void set_collective_hover(float col_hover) {_col_hover = col_hover; } + void set_collective_max(float col_max) {_col_max = col_max; } + void set_collective_min(float col_min) {_col_min = col_min; } + void get_gov_rpm(float gov_rpm) {_governed_rpm = gov_rpm; } + void set_entry_sink_rate (float sink_rate) { _entry_sink_rate = sink_rate; } + void set_entry_alt (float entry_alt) { _entry_alt = entry_alt; } + void set_ground_clearance(float ground_clearance) { _ground_clearance = ground_clearance; } + void init_est_radar_alt(); + void update_est_radar_alt(); + float get_est_alt() const {return _est_alt;} + void update_hover_autorotation_controller(); + void init_avg_acc_z(); + void calc_avg_acc_z(); + float get_flare_alt() const {return _flare_alt_calc;} + void update_flare_alt(); + void calc_flare_alt(float sink_rate, float fwd_speed); + float get_t_touchdown() const {return _t_tch;} + float get_cushion_alt() const {return _cushion_alt;} + bool get_flare_status(void) { return _flare_complete; } + void calc_sink_d_avg(); // User Settable Parameters static const struct AP_Param::GroupInfo var_info[]; + AP_Int16 _param_target_speed; + AP_Int16 _param_head_speed_set_point; + AP_Float _param_solidity; + AP_Float _param_diameter; + AP_Float _t_tch; + bool _using_rfnd; private: @@ -54,7 +90,6 @@ class AC_Autorotation float _target_head_speed; // Normalised target head speed. Normalised by head speed set point RPM. float _p_term_hs; // Proportional contribution to collective setting. float _ff_term_hs; // Following trim feed forward contribution to collective setting. - float _vel_target; // Forward velocity target. float _pitch_target; // Pitch angle target. float _accel_max; // Maximum acceleration limit. @@ -69,6 +104,41 @@ class AC_Autorotation float _vel_p; // Forward velocity P term. float _vel_ff; // Forward velocity Feed Forward term. float _accel_out; // Acceleration value used to calculate pitch target. + float _entry_sink_rate; + float _entry_alt; + float _radar_alt; + float _flare_entry_speed; + float _desired_speed; + float _time_to_ground; + float _desired_sink_rate; + float _col_mid; + float _ground_clearance; + float _est_alt; + float _descent_rate_filtered; + float _radar_alt_prev; + float _radar_alt_calc; + float _avg_acc_z; + float _acc_z_sum; + int16_t _index; + float _curr_acc_z[10]{}; + float _col_hover; + float _col_max; + float _col_min; + float _flare_alt_calc; + float _lift_hover; + float _c; + float _governed_rpm; + float _cushion_alt; + float _disc_area; + float _last_vertical_speed; + float _sink_deriv; + float _est_rod; + float _avg_sink_deriv; + float _avg_sink_deriv_sum; + int16_t _index_sink_rate; + float _curr_sink_deriv[20]{}; + bool _flare_complete; + bool _flare_update_check; LowPassFilterFloat _accel_target_filter; // acceleration target filter @@ -76,15 +146,16 @@ class AC_Autorotation AP_Int8 _param_enable; AC_P _p_hs; AC_P _p_fw_vel; - AP_Int16 _param_head_speed_set_point; - AP_Int16 _param_target_speed; + AC_P _p_coll_tch; AP_Float _param_col_entry_cutoff_freq; AP_Float _param_col_glide_cutoff_freq; + AP_Float _param_col_cushion_cutoff_freq; AP_Int16 _param_accel_max; AP_Float _param_bail_time; AP_Int8 _param_rpm_instance; AP_Float _param_fwd_k_ff; + //--------Internal Flags-------- struct controller_flags { bool bad_rpm : 1; @@ -92,8 +163,16 @@ class AC_Autorotation } _flags; //--------Internal Functions-------- + // set the collective in the motors library void set_collective(float _collective_filter_cutoff) const; // low pass filter for collective trim LowPassFilterFloat col_trim_lpf; + + // low pass filter for descent rate + LowPassFilterFloat descent_rate_lpf; + + //--------References to Other Libraries-------- + AP_InertialNav& _inav; + AP_AHRS& _ahrs; }; From 18df15c4a219f80ba4ee5348766935590a6159ee Mon Sep 17 00:00:00 2001 From: Ferruccio1984 Date: Thu, 24 Mar 2022 10:05:22 -0700 Subject: [PATCH 02/61] Copter: Assisted Autorotation Implementation --- ArduCopter/Parameters.cpp | 2 +- ArduCopter/config.h | 18 +-- ArduCopter/heli.cpp | 21 +++ ArduCopter/mode.h | 5 + ArduCopter/mode_autorotate.cpp | 244 ++++++++++++++++++++++++--------- 5 files changed, 212 insertions(+), 78 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index d1de6fe45d78f..901060e9fee01 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1283,7 +1283,7 @@ ParametersG2::ParametersG2(void) ,mode_systemid_ptr(&copter.mode_systemid) #endif #if MODE_AUTOROTATE_ENABLED == ENABLED - ,arot() + ,arot(copter.inertial_nav, copter.ahrs) #endif #if HAL_BUTTON_ENABLED ,button_ptr(&copter.button) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 97793741c9895..d695767a8234c 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -279,17 +279,13 @@ ////////////////////////////////////////////////////////////////////////////// // Autorotate - autonomous auto-rotation - helicopters only #ifndef MODE_AUTOROTATE_ENABLED -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - #if FRAME_CONFIG == HELI_FRAME - #ifndef MODE_AUTOROTATE_ENABLED - # define MODE_AUTOROTATE_ENABLED ENABLED - #endif - #else - # define MODE_AUTOROTATE_ENABLED DISABLED - #endif -#else - # define MODE_AUTOROTATE_ENABLED DISABLED -#endif + #if FRAME_CONFIG == HELI_FRAME + #ifndef MODE_AUTOROTATE_ENABLED + # define MODE_AUTOROTATE_ENABLED ENABLED + #endif + #else + # define MODE_AUTOROTATE_ENABLED DISABLED + #endif #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index a9c9fde1ac7cf..f51cc3d0f486e 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -208,6 +208,27 @@ void Copter::heli_update_autorotation() heli_flags.in_autorotation = true; motors->set_in_autorotation(heli_flags.in_autorotation); motors->set_enable_bailout(true); + int32_t gnd_dist = flightmode->get_alt_above_ground_cm(); + + // use rangefinder if available + switch (rangefinder.status_orient(ROTATION_PITCH_270)) { + case RangeFinder::Status::NotConnected: + case RangeFinder::Status::NoData: + case RangeFinder::Status::OutOfRangeHigh: + // use altitude above home for non-functioning rangefinder + break; + + case RangeFinder::Status::OutOfRangeLow: + // altitude is close to zero (gear should deploy) + gnd_dist = 0; + break; + + case RangeFinder::Status::Good: + // use last good reading + gnd_dist = rangefinder_state.alt_cm; + break; + } + g2.arot.set_ground_distance(gnd_dist); } #endif if (flightmode->has_manual_throttle() && motors->arot_man_enabled()) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index dd06ee0149c29..bd33cdd298105 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1934,6 +1934,11 @@ class ModeAutorotate : public Mode { uint32_t _bail_time_start_ms; // Time at start of bail out float _target_climb_rate_adjust;// Target vertical acceleration used during bail out phase float _target_pitch_adjust; // Target pitch rate used during bail out phase + uint32_t _touchdown_time_ms; + bool hover_autorotation; + bool initial_energy_check; + float last_tti; + float time_to_impact; enum class Autorotation_Phase { ENTRY, diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 85f278ac0d801..ec017e0835277 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -16,6 +16,7 @@ #define AUTOROTATE_ENTRY_TIME 2.0f // (s) number of seconds that the entry phase operates for #define BAILOUT_MOTOR_RAMP_TIME 1.0f // (s) time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single #define HEAD_SPEED_TARGET_RATIO 1.0f // Normalised target main rotor head speed (unit: -) +#define TOUCHDOWN_TIME 5.0f // time in seconds after land complete flag until aircraft is disarmed bool ModeAutorotate::init(bool ignore_checks) { @@ -36,11 +37,17 @@ bool ModeAutorotate::init(bool ignore_checks) return false; } + // zero thrust collective is set in library. Must be set before init_hs_controller is called. + g2.arot.set_collective_minimum_drag(motors->get_coll_mid()); + // Initialise controllers // This must be done before RPM value is fetched g2.arot.init_hs_controller(); g2.arot.init_fwd_spd_controller(); + // initialize radar altitude estimator + g2.arot.init_est_radar_alt(); + // Retrieve rpm and start rpm sensor health checks _initial_rpm = g2.arot.get_rpm(true); @@ -57,6 +64,15 @@ bool ModeAutorotate::init(bool ignore_checks) _flags.straight_ahead_initial = true; _flags.bail_out_initial = true; _msg_flags.bad_rpm = true; + initial_energy_check = true; + g2.arot._using_rfnd = false; + g2.arot.init_avg_acc_z(); + g2.arot.set_collective_minimum_drag(motors->get_coll_mid()); + g2.arot.set_collective_hover(motors->get_coll_hover()); + g2.arot.set_collective_max(motors->get_coll_max_pitch()); + g2.arot.set_collective_min(motors->get_coll_min_pitch()); + g2.arot.get_gov_rpm(motors->get_rpm_setpoint()); + g2.arot.initial_flare_estimate(); // Setting default starting switches phase_switch = Autorotation_Phase::ENTRY; @@ -85,25 +101,62 @@ void ModeAutorotate::run() // Current time uint32_t now = millis(); //milliseconds + // set dt in library + float const last_loop_time_s = AP::scheduler().get_last_loop_time_s(); + g2.arot.set_dt(last_loop_time_s); + + float alt = g2.arot.get_ground_distance(); + // have autorotation library update estimated radar altitude + g2.arot.update_est_radar_alt(); + if(alt < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)){ + g2.arot._using_rfnd = true; + }else{ + g2.arot._using_rfnd = false; + } + // Initialise internal variables float curr_vel_z = inertial_nav.get_velocity_z_up_cms(); // Current vertical descent + //calculate time to impact + if(phase_switch != Autorotation_Phase::TOUCH_DOWN){ + g2.arot.time_to_ground(); + time_to_impact = g2.arot.get_time_to_ground(); + last_tti=time_to_impact; + }else { + time_to_impact = last_tti; + } + //---------------------------------------------------------------- // State machine logic //---------------------------------------------------------------- - - // Setting default phase switch positions - nav_pos_switch = Navigation_Decision::USER_CONTROL_STABILISED; - - // Timer from entry phase to progress to glide phase - if (phase_switch == Autorotation_Phase::ENTRY){ - - if ((now - _entry_time_start_ms)/1000.0f > AUTOROTATE_ENTRY_TIME) { - // Flight phase can be progressed to steady state glide - phase_switch = Autorotation_Phase::SS_GLIDE; - } - - } + + if(initial_energy_check){ + //initial check for total energy monitoring + if ( inertial_nav.get_speed_xy_cms() < 250.0f && g2.arot.get_ground_distance() < g2.arot.get_flare_alt()){ + hover_autorotation = true; + }else{ + hover_autorotation = false; + } + initial_energy_check=false; + } + + //total energy check + if(hover_autorotation){ + if(_flags.entry_initial == false && time_to_impact <= g2.arot.get_t_touchdown()){ + phase_switch = Autorotation_Phase::TOUCH_DOWN; + } + } else { + if ( _flags.ss_glide_initial == true && _flags.flare_initial == true && _flags.touch_down_initial == true && g2.arot.get_est_alt() > g2.arot.get_flare_alt() ){ + if ((now - _entry_time_start_ms)/1000.0f > AUTOROTATE_ENTRY_TIME ) { + // Flight phase can be progressed to steady state glide + phase_switch = Autorotation_Phase::SS_GLIDE; + } + }else if( g2.arot.get_est_alt()<=g2.arot.get_flare_alt() && g2.arot.get_est_alt()>g2.arot.get_cushion_alt() && !g2.arot.get_flare_status() ){ + phase_switch = Autorotation_Phase::FLARE; + }else if(time_to_impact <= g2.arot.get_t_touchdown() && _flags.flare_initial == false ){ + phase_switch = Autorotation_Phase::TOUCH_DOWN; + } + } //---------------------------------------------------------------- @@ -115,10 +168,8 @@ void ModeAutorotate::run() { // Entry phase functions to be run only once if (_flags.entry_initial == true) { - - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase"); - #endif + + gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase"); // Set following trim low pass cut off frequency g2.arot.set_col_cutoff_freq(g2.arot.get_col_entry_freq()); @@ -134,25 +185,29 @@ void ModeAutorotate::run() } - // Slowly change the target head speed until the target head speed matches the parameter defined value - if (g2.arot.get_rpm() > HEAD_SPEED_TARGET_RATIO*1.005f || g2.arot.get_rpm() < HEAD_SPEED_TARGET_RATIO*0.995f) { - _target_head_speed -= _hs_decay*G_Dt; + if(!hover_autorotation){ + // Slowly change the target head speed until the target head speed matches the parameter defined value + float norm_rpm = g2.arot.get_rpm()/g2.arot.get_hs_set_point(); + if (norm_rpm > HEAD_SPEED_TARGET_RATIO*1.005f || norm_rpm < HEAD_SPEED_TARGET_RATIO*0.995f) { + _target_head_speed -= _hs_decay * last_loop_time_s; } else { _target_head_speed = HEAD_SPEED_TARGET_RATIO; } - - // Set target head speed in head speed controller - g2.arot.set_target_head_speed(_target_head_speed); - - // Run airspeed/attitude controller - g2.arot.set_dt(G_Dt); - g2.arot.update_forward_speed_controller(); - - // Retrieve pitch target - _pitch_target = g2.arot.get_pitch(); - - // Update controllers - _flags.bad_rpm = g2.arot.update_hs_glide_controller(G_Dt); //run head speed/ collective controller + // Set target head speed in head speed controller + g2.arot.set_target_head_speed(_target_head_speed); + // Run airspeed/attitude controller + g2.arot.update_forward_speed_controller(); + // Retrieve pitch target + _pitch_target = g2.arot.get_pitch(); + // Update controllers + _flags.bad_rpm = g2.arot.update_hs_glide_controller(); //run head speed/ collective controller + }else{ + _pitch_target = 0.0f; + g2.arot.update_hover_autorotation_controller(); //run head speed/ collective controller + g2.arot.set_collective_minimum_drag(motors->get_coll_mid()); + g2.arot.set_entry_sink_rate(curr_vel_z); + g2.arot.set_entry_alt(g2.arot.get_ground_distance()); + } break; } @@ -162,9 +217,7 @@ void ModeAutorotate::run() // Steady state glide functions to be run only once if (_flags.ss_glide_initial == true) { - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase"); - #endif + gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase"); // Set following trim low pass cut off frequency g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq()); @@ -173,7 +226,7 @@ void ModeAutorotate::run() g2.arot.set_desired_fwd_speed(); // Set target head speed in head speed controller - _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide in case hs has not reached target for glide + _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide in case hs hasn't reached target for glide g2.arot.set_target_head_speed(_target_head_speed); // Prevent running the initial glide functions again @@ -181,22 +234,79 @@ void ModeAutorotate::run() } // Run airspeed/attitude controller - g2.arot.set_dt(G_Dt); g2.arot.update_forward_speed_controller(); + //update sink rate derivative + g2.arot.calc_sink_d_avg(); + + //update flare altitude estimate + g2.arot.update_flare_alt(); + // Retrieve pitch target _pitch_target = g2.arot.get_pitch(); // Update head speed/ collective controller - _flags.bad_rpm = g2.arot.update_hs_glide_controller(G_Dt); + _flags.bad_rpm = g2.arot.update_hs_glide_controller(); // Attitude controller is updated in navigation switch-case statements + g2.arot.calc_avg_acc_z(); break; } case Autorotation_Phase::FLARE: - case Autorotation_Phase::TOUCH_DOWN: { + // Steady state glide functions to be run only once + if (_flags.flare_initial == true) { + gcs().send_text(MAV_SEVERITY_INFO, "Flare_Phase"); + + // Set following trim low pass cut off frequency + g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq()); + + // Set target head speed in head speed controller + _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide incase hs has not reached target for glide + g2.arot.set_target_head_speed(_target_head_speed); + g2.arot.get_entry_speed(); + // Prevent running the initial flare functions again + _flags.flare_initial = false; + } + // Run flare controller + g2.arot.flare_controller(); + + //update sink rate derivative + g2.arot.calc_sink_d_avg(); + + // Update head speed/ collective controller + _flags.bad_rpm = g2.arot.update_hs_glide_controller(); + // Retrieve pitch target + _pitch_target = g2.arot.get_pitch(); + + //calc average acceleration on z axis for estimating flare effectiveness + g2.arot.calc_avg_acc_z(); + + break; + } + case Autorotation_Phase::TOUCH_DOWN: + { + if (_flags.touch_down_initial == true) { + gcs().send_text(MAV_SEVERITY_INFO, "Touchdown_Phase"); + // Prevent running the initial touchdown functions again + _flags.touch_down_initial = false; + _touchdown_time_ms = millis(); + g2.arot.set_col_cutoff_freq(g2.arot.get_col_cushion_freq()); + g2.arot.set_entry_sink_rate(curr_vel_z); + g2.arot.set_entry_alt(g2.arot.get_ground_distance()); + g2.arot.set_ground_clearance(copter.rangefinder.ground_clearance_cm_orient(ROTATION_PITCH_270)); + } + g2.arot.touchdown_controller(); + _pitch_target = g2.arot.get_pitch(); + + if(fabsf(inertial_nav.get_velocity_z_up_cms()) < 10) { + copter.ap.land_complete = true; + } + if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE && ((now - _touchdown_time_ms)/1000.0f > TOUCHDOWN_TIME )) { + copter.arming.disarm(AP_Arming::Method::LANDED); + } + break; } @@ -204,10 +314,7 @@ void ModeAutorotate::run() { if (_flags.bail_out_initial == true) { // Functions and settings to be done once are done here. - - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation"); - #endif + gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation"); // Set bail out timer remaining equal to the parameter value, bailout time // cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME. @@ -248,8 +355,8 @@ void ModeAutorotate::run() if ((now - _bail_time_start_ms)/1000.0f >= BAILOUT_MOTOR_RAMP_TIME) { // Update desired vertical speed and pitch target after the bailout motor ramp timer has completed - _desired_v_z -= _target_climb_rate_adjust*G_Dt; - _pitch_target -= _target_pitch_adjust*G_Dt; + _desired_v_z -= _target_climb_rate_adjust * last_loop_time_s; + _pitch_target -= _target_pitch_adjust * last_loop_time_s; } // Set position controller pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z); @@ -271,31 +378,36 @@ void ModeAutorotate::run() } } + float pilot_roll, pilot_pitch, pilot_yaw_rate; + // Operator is in control of roll and yaw. Controls act as if in stabilise flight mode. Pitch + // is controlled by speed-height controller. + get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max); + + float target_roll; - switch (nav_pos_switch) { + // Grab inertial velocity + const Vector3f& vel = inertial_nav.get_velocity_neu_cms(); - case Navigation_Decision::USER_CONTROL_STABILISED: - { - // Operator is in control of roll and yaw. Controls act as if in stabilise flight mode. Pitch - // is controlled by speed-height controller. - float pilot_roll, pilot_pitch; - get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max); + // rotate roll, pitch input from north facing to vehicle's perspective + float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel + float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel - // Get pilot's desired yaw rate - float pilot_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + // gain scheduling for yaw + float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); + pilot_yaw_rate = ((float)pilot_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g2.command_model_pilot.get_rate() / 45.0; - // Pitch target is calculated in autorotation phase switch above - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pilot_roll, _pitch_target, pilot_yaw_rate); - break; - } + roll_vel = constrain_float(roll_vel, -560.0f, 560.0f); + pitch_vel = constrain_float(pitch_vel, -560.0f, 560.0f); - case Navigation_Decision::STRAIGHT_AHEAD: - case Navigation_Decision::INTO_WIND: - case Navigation_Decision::NEAREST_RALLY: - { - break; - } - } + // convert user input into desired roll velocity + float roll_vel_error = roll_vel - (pilot_roll / 0.8f); + + // roll velocity is feed into roll acceleration to minimize slip + target_roll = roll_vel_error * -0.8f; + target_roll = constrain_float(target_roll, -4500.0f, 4500.0f); + + // Pitch target is calculated in autorotation phase switch above + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, _pitch_target, pilot_yaw_rate); // Output warning messaged if rpm signal is bad if (_flags.bad_rpm) { From 9147f2fa2ba561a5baf497ffcaef7f3e2dbdf853 Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Tue, 29 Aug 2023 17:49:30 +0200 Subject: [PATCH 03/61] AP_Motors: Support for Assisted Autorotation retrieve collective hover setting get governor rpm setpoint --- libraries/AP_Motors/AP_MotorsHeli.h | 11 +++++++++++ libraries/AP_Motors/AP_MotorsHeli_RSC.h | 1 + 2 files changed, 12 insertions(+) diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index f2a8ce6100400..434ee14b9e833 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -84,6 +84,9 @@ class AP_MotorsHeli : public AP_Motors { // get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1) float get_rsc_setpoint() const { return _main_rotor._rsc_setpoint.get() * 0.01f; } + //return governor rpm setpoint + float get_rpm_setpoint() const {return _main_rotor.get_governor_setpoint();} + // arot_man_enabled - gets contents of manual_autorotation_enabled parameter bool arot_man_enabled() const { return (_main_rotor._rsc_arot_man_enable.get() == 1) ? true : false; } @@ -145,6 +148,13 @@ class AP_MotorsHeli : public AP_Motors { //return zero lift collective position float get_coll_mid() const { return _collective_zero_thrust_pct; } + //return collective hover + float get_coll_hover() const { return _collective_hover; } + + float get_coll_max_pitch() const { return _collective_max_deg;} + + float get_coll_min_pitch() const { return _collective_min_deg;} + // enum for heli optional features enum class HeliOption { USE_LEAKY_I = (1<<0), // 1 @@ -278,6 +288,7 @@ class AP_MotorsHeli : public AP_Motors { // internal variables float _collective_zero_thrust_pct; // collective zero thrutst parameter value converted to 0 ~ 1 range float _collective_land_min_pct; // collective land min parameter value converted to 0 ~ 1 range + float _collective_hover_rad; uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup motor_frame_type _frame_type; diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 276728ca4d1dc..8df9a47d6737c 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -87,6 +87,7 @@ class AP_MotorsHeli_RSC { // functions for autothrottle, throttle curve, governor, idle speed, output to servo void set_governor_output(float governor_output) {_governor_output = governor_output; } float get_governor_output() const { return _governor_output; } + float get_governor_setpoint() const { return _governor_rpm; } void governor_reset(); float get_control_output() const { return _control_output; } void set_idle_output(float idle_output) { _idle_output.set(idle_output); } From 9b09716513ff2f7709e7e252d3720000dd321f9b Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Sat, 23 Dec 2023 10:42:01 -0500 Subject: [PATCH 04/61] AC_Autorotation: fix error for autotest --- libraries/AC_Autorotation/AC_Autorotation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index aaf9432fa8712..1f777d67486d2 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -354,7 +354,7 @@ void AC_Autorotation::initial_flare_estimate() _index_sink_rate = 0; memset(_curr_sink_deriv, 0, sizeof(_curr_sink_deriv)); - gcs().send_text(MAV_SEVERITY_INFO, "Ct/σ=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/9.8065f, _flare_alt_calc*0.01f, _c); + gcs().send_text(MAV_SEVERITY_INFO, "Ct/sigma=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/9.8065f, _flare_alt_calc*0.01f, _c); } From 4c9e07619ba936c0a7ecd0c63e7783049153eb91 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Sat, 23 Dec 2023 14:17:50 -0500 Subject: [PATCH 05/61] AC_Autorotation: fix parameter naming --- libraries/AC_Autorotation/AC_Autorotation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 1f777d67486d2..f450e20d283c6 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -140,7 +140,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @User: Advanced AP_GROUPINFO("COL_FILT_C", 13, AC_Autorotation, _param_col_cushion_cutoff_freq, HS_CONTROLLER_CUSHION_COL_FILTER), - // @Param: SOLIDITY + // @Param: ROT_SOL // @DisplayName: rotor solidity // @Description: helicopter specific main rotor solidity // @Range: 0.001 0.01 @@ -157,7 +157,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @User: Advanced AP_GROUPINFO("ROT_DIAM", 15, AC_Autorotation, _param_diameter, ROT_DIAMETER), - // @Param: time touchdown + // @Param: T_TCH // @DisplayName: time touchdown // @Description: time touchdown // @Units: s From e31ea19a9082d7c062d37cf4cdffcb47be683c88 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Sat, 23 Dec 2023 23:33:32 -0500 Subject: [PATCH 06/61] Copter: fix style on mode_autorotate.cpp --- ArduCopter/mode_autorotate.cpp | 402 ++++++++++++++++----------------- 1 file changed, 198 insertions(+), 204 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index ec017e0835277..e446d84fae61b 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -51,10 +51,10 @@ bool ModeAutorotate::init(bool ignore_checks) // Retrieve rpm and start rpm sensor health checks _initial_rpm = g2.arot.get_rpm(true); - // Display message + // Display message gcs().send_text(MAV_SEVERITY_INFO, "Autorotation initiated"); - // Set all intial flags to on + // Set all intial flags to on _flags.entry_initial = true; _flags.ss_glide_initial = true; _flags.flare_initial = true; @@ -64,15 +64,15 @@ bool ModeAutorotate::init(bool ignore_checks) _flags.straight_ahead_initial = true; _flags.bail_out_initial = true; _msg_flags.bad_rpm = true; - initial_energy_check = true; - g2.arot._using_rfnd = false; - g2.arot.init_avg_acc_z(); - g2.arot.set_collective_minimum_drag(motors->get_coll_mid()); - g2.arot.set_collective_hover(motors->get_coll_hover()); - g2.arot.set_collective_max(motors->get_coll_max_pitch()); - g2.arot.set_collective_min(motors->get_coll_min_pitch()); - g2.arot.get_gov_rpm(motors->get_rpm_setpoint()); - g2.arot.initial_flare_estimate(); + initial_energy_check = true; + g2.arot._using_rfnd = false; + g2.arot.init_avg_acc_z(); + g2.arot.set_collective_minimum_drag(motors->get_coll_mid()); + g2.arot.set_collective_hover(motors->get_coll_hover()); + g2.arot.set_collective_max(motors->get_coll_max_pitch()); + g2.arot.set_collective_min(motors->get_coll_min_pitch()); + g2.arot.get_gov_rpm(motors->get_rpm_setpoint()); + g2.arot.initial_flare_estimate(); // Setting default starting switches phase_switch = Autorotation_Phase::ENTRY; @@ -108,55 +108,55 @@ void ModeAutorotate::run() float alt = g2.arot.get_ground_distance(); // have autorotation library update estimated radar altitude g2.arot.update_est_radar_alt(); - if(alt < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)){ + if (alt < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) { g2.arot._using_rfnd = true; - }else{ + } else { g2.arot._using_rfnd = false; } // Initialise internal variables float curr_vel_z = inertial_nav.get_velocity_z_up_cms(); // Current vertical descent //calculate time to impact - if(phase_switch != Autorotation_Phase::TOUCH_DOWN){ - g2.arot.time_to_ground(); - time_to_impact = g2.arot.get_time_to_ground(); - last_tti=time_to_impact; - }else { - time_to_impact = last_tti; - } + if (phase_switch != Autorotation_Phase::TOUCH_DOWN) { + g2.arot.time_to_ground(); + time_to_impact = g2.arot.get_time_to_ground(); + last_tti=time_to_impact; + } else { + time_to_impact = last_tti; + } //---------------------------------------------------------------- // State machine logic //---------------------------------------------------------------- - - if(initial_energy_check){ - //initial check for total energy monitoring - if ( inertial_nav.get_speed_xy_cms() < 250.0f && g2.arot.get_ground_distance() < g2.arot.get_flare_alt()){ - hover_autorotation = true; - }else{ - hover_autorotation = false; - } - initial_energy_check=false; - } - - //total energy check - if(hover_autorotation){ - if(_flags.entry_initial == false && time_to_impact <= g2.arot.get_t_touchdown()){ - phase_switch = Autorotation_Phase::TOUCH_DOWN; - } - } else { - if ( _flags.ss_glide_initial == true && _flags.flare_initial == true && _flags.touch_down_initial == true && g2.arot.get_est_alt() > g2.arot.get_flare_alt() ){ + + if (initial_energy_check) { + //initial check for total energy monitoring + if ( inertial_nav.get_speed_xy_cms() < 250.0f && g2.arot.get_ground_distance() < g2.arot.get_flare_alt()) { + hover_autorotation = true; + } else { + hover_autorotation = false; + } + initial_energy_check=false; + } + + //total energy check + if (hover_autorotation) { + if (_flags.entry_initial == false && time_to_impact <= g2.arot.get_t_touchdown()) { + phase_switch = Autorotation_Phase::TOUCH_DOWN; + } + } else { + if ( _flags.ss_glide_initial == true && _flags.flare_initial == true && _flags.touch_down_initial == true && g2.arot.get_est_alt() > g2.arot.get_flare_alt() ) { if ((now - _entry_time_start_ms)/1000.0f > AUTOROTATE_ENTRY_TIME ) { // Flight phase can be progressed to steady state glide phase_switch = Autorotation_Phase::SS_GLIDE; } - }else if( g2.arot.get_est_alt()<=g2.arot.get_flare_alt() && g2.arot.get_est_alt()>g2.arot.get_cushion_alt() && !g2.arot.get_flare_status() ){ - phase_switch = Autorotation_Phase::FLARE; - }else if(time_to_impact <= g2.arot.get_t_touchdown() && _flags.flare_initial == false ){ - phase_switch = Autorotation_Phase::TOUCH_DOWN; - } - } + } else if ( g2.arot.get_est_alt()<=g2.arot.get_flare_alt() && g2.arot.get_est_alt()>g2.arot.get_cushion_alt() && !g2.arot.get_flare_status() ) { + phase_switch = Autorotation_Phase::FLARE; + } else if (time_to_impact <= g2.arot.get_t_touchdown() && _flags.flare_initial == false ) { + phase_switch = Autorotation_Phase::TOUCH_DOWN; + } + } //---------------------------------------------------------------- @@ -164,194 +164,189 @@ void ModeAutorotate::run() //---------------------------------------------------------------- switch (phase_switch) { - case Autorotation_Phase::ENTRY: - { - // Entry phase functions to be run only once - if (_flags.entry_initial == true) { - - gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase"); + case Autorotation_Phase::ENTRY: { + // Entry phase functions to be run only once + if (_flags.entry_initial == true) { - // Set following trim low pass cut off frequency - g2.arot.set_col_cutoff_freq(g2.arot.get_col_entry_freq()); + gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase"); - // Target head speed is set to rpm at initiation to prevent unwanted changes in attitude - _target_head_speed = _initial_rpm/g2.arot.get_hs_set_point(); + // Set following trim low pass cut off frequency + g2.arot.set_col_cutoff_freq(g2.arot.get_col_entry_freq()); - // Set desired forward speed target - g2.arot.set_desired_fwd_speed(); + // Target head speed is set to rpm at initiation to prevent unwanted changes in attitude + _target_head_speed = _initial_rpm/g2.arot.get_hs_set_point(); - // Prevent running the initial entry functions again - _flags.entry_initial = false; + // Set desired forward speed target + g2.arot.set_desired_fwd_speed(); - } + // Prevent running the initial entry functions again + _flags.entry_initial = false; - if(!hover_autorotation){ - // Slowly change the target head speed until the target head speed matches the parameter defined value - float norm_rpm = g2.arot.get_rpm()/g2.arot.get_hs_set_point(); - if (norm_rpm > HEAD_SPEED_TARGET_RATIO*1.005f || norm_rpm < HEAD_SPEED_TARGET_RATIO*0.995f) { + } + + if (!hover_autorotation) { + // Slowly change the target head speed until the target head speed matches the parameter defined value + float norm_rpm = g2.arot.get_rpm()/g2.arot.get_hs_set_point(); + if (norm_rpm > HEAD_SPEED_TARGET_RATIO*1.005f || norm_rpm < HEAD_SPEED_TARGET_RATIO*0.995f) { _target_head_speed -= _hs_decay * last_loop_time_s; } else { _target_head_speed = HEAD_SPEED_TARGET_RATIO; } - // Set target head speed in head speed controller - g2.arot.set_target_head_speed(_target_head_speed); - // Run airspeed/attitude controller - g2.arot.update_forward_speed_controller(); - // Retrieve pitch target - _pitch_target = g2.arot.get_pitch(); - // Update controllers - _flags.bad_rpm = g2.arot.update_hs_glide_controller(); //run head speed/ collective controller - }else{ - _pitch_target = 0.0f; - g2.arot.update_hover_autorotation_controller(); //run head speed/ collective controller - g2.arot.set_collective_minimum_drag(motors->get_coll_mid()); - g2.arot.set_entry_sink_rate(curr_vel_z); - g2.arot.set_entry_alt(g2.arot.get_ground_distance()); - } - - break; + // Set target head speed in head speed controller + g2.arot.set_target_head_speed(_target_head_speed); + // Run airspeed/attitude controller + g2.arot.update_forward_speed_controller(); + // Retrieve pitch target + _pitch_target = g2.arot.get_pitch(); + // Update controllers + _flags.bad_rpm = g2.arot.update_hs_glide_controller(); //run head speed/ collective controller + } else { + _pitch_target = 0.0f; + g2.arot.update_hover_autorotation_controller(); //run head speed/ collective controller + g2.arot.set_collective_minimum_drag(motors->get_coll_mid()); + g2.arot.set_entry_sink_rate(curr_vel_z); + g2.arot.set_entry_alt(g2.arot.get_ground_distance()); } - case Autorotation_Phase::SS_GLIDE: - { - // Steady state glide functions to be run only once - if (_flags.ss_glide_initial == true) { + break; + } - gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase"); + case Autorotation_Phase::SS_GLIDE: { + // Steady state glide functions to be run only once + if (_flags.ss_glide_initial == true) { - // Set following trim low pass cut off frequency - g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq()); + gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase"); - // Set desired forward speed target - g2.arot.set_desired_fwd_speed(); + // Set following trim low pass cut off frequency + g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq()); - // Set target head speed in head speed controller - _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide in case hs hasn't reached target for glide - g2.arot.set_target_head_speed(_target_head_speed); + // Set desired forward speed target + g2.arot.set_desired_fwd_speed(); - // Prevent running the initial glide functions again - _flags.ss_glide_initial = false; - } + // Set target head speed in head speed controller + _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide in case hs hasn't reached target for glide + g2.arot.set_target_head_speed(_target_head_speed); - // Run airspeed/attitude controller - g2.arot.update_forward_speed_controller(); - - //update sink rate derivative - g2.arot.calc_sink_d_avg(); + // Prevent running the initial glide functions again + _flags.ss_glide_initial = false; + } - //update flare altitude estimate - g2.arot.update_flare_alt(); + // Run airspeed/attitude controller + g2.arot.update_forward_speed_controller(); - // Retrieve pitch target - _pitch_target = g2.arot.get_pitch(); + //update sink rate derivative + g2.arot.calc_sink_d_avg(); - // Update head speed/ collective controller - _flags.bad_rpm = g2.arot.update_hs_glide_controller(); - // Attitude controller is updated in navigation switch-case statements - g2.arot.calc_avg_acc_z(); + //update flare altitude estimate + g2.arot.update_flare_alt(); - break; - } + // Retrieve pitch target + _pitch_target = g2.arot.get_pitch(); - case Autorotation_Phase::FLARE: - { - // Steady state glide functions to be run only once - if (_flags.flare_initial == true) { - gcs().send_text(MAV_SEVERITY_INFO, "Flare_Phase"); - - // Set following trim low pass cut off frequency - g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq()); - - // Set target head speed in head speed controller - _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide incase hs has not reached target for glide - g2.arot.set_target_head_speed(_target_head_speed); - g2.arot.get_entry_speed(); - // Prevent running the initial flare functions again - _flags.flare_initial = false; - } - // Run flare controller - g2.arot.flare_controller(); + // Update head speed/ collective controller + _flags.bad_rpm = g2.arot.update_hs_glide_controller(); + // Attitude controller is updated in navigation switch-case statements + g2.arot.calc_avg_acc_z(); - //update sink rate derivative - g2.arot.calc_sink_d_avg(); + break; + } - // Update head speed/ collective controller - _flags.bad_rpm = g2.arot.update_hs_glide_controller(); - // Retrieve pitch target - _pitch_target = g2.arot.get_pitch(); + case Autorotation_Phase::FLARE: { + // Steady state glide functions to be run only once + if (_flags.flare_initial == true) { + gcs().send_text(MAV_SEVERITY_INFO, "Flare_Phase"); - //calc average acceleration on z axis for estimating flare effectiveness - g2.arot.calc_avg_acc_z(); + // Set following trim low pass cut off frequency + g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq()); - break; + // Set target head speed in head speed controller + _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide incase hs has not reached target for glide + g2.arot.set_target_head_speed(_target_head_speed); + g2.arot.get_entry_speed(); + // Prevent running the initial flare functions again + _flags.flare_initial = false; } - case Autorotation_Phase::TOUCH_DOWN: - { - if (_flags.touch_down_initial == true) { - gcs().send_text(MAV_SEVERITY_INFO, "Touchdown_Phase"); - // Prevent running the initial touchdown functions again - _flags.touch_down_initial = false; - _touchdown_time_ms = millis(); - g2.arot.set_col_cutoff_freq(g2.arot.get_col_cushion_freq()); - g2.arot.set_entry_sink_rate(curr_vel_z); - g2.arot.set_entry_alt(g2.arot.get_ground_distance()); - g2.arot.set_ground_clearance(copter.rangefinder.ground_clearance_cm_orient(ROTATION_PITCH_270)); - } - g2.arot.touchdown_controller(); - _pitch_target = g2.arot.get_pitch(); + // Run flare controller + g2.arot.flare_controller(); - if(fabsf(inertial_nav.get_velocity_z_up_cms()) < 10) { - copter.ap.land_complete = true; - } - if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE && ((now - _touchdown_time_ms)/1000.0f > TOUCHDOWN_TIME )) { - copter.arming.disarm(AP_Arming::Method::LANDED); - } + //update sink rate derivative + g2.arot.calc_sink_d_avg(); + + // Update head speed/ collective controller + _flags.bad_rpm = g2.arot.update_hs_glide_controller(); + // Retrieve pitch target + _pitch_target = g2.arot.get_pitch(); - break; + //calc average acceleration on z axis for estimating flare effectiveness + g2.arot.calc_avg_acc_z(); + + break; + } + case Autorotation_Phase::TOUCH_DOWN: { + if (_flags.touch_down_initial == true) { + gcs().send_text(MAV_SEVERITY_INFO, "Touchdown_Phase"); + // Prevent running the initial touchdown functions again + _flags.touch_down_initial = false; + _touchdown_time_ms = millis(); + g2.arot.set_col_cutoff_freq(g2.arot.get_col_cushion_freq()); + g2.arot.set_entry_sink_rate(curr_vel_z); + g2.arot.set_entry_alt(g2.arot.get_ground_distance()); + g2.arot.set_ground_clearance(copter.rangefinder.ground_clearance_cm_orient(ROTATION_PITCH_270)); + } + g2.arot.touchdown_controller(); + _pitch_target = g2.arot.get_pitch(); + + if (fabsf(inertial_nav.get_velocity_z_up_cms()) < 10) { + copter.ap.land_complete = true; + } + if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE && ((now - _touchdown_time_ms)/1000.0f > TOUCHDOWN_TIME )) { + copter.arming.disarm(AP_Arming::Method::LANDED); } - case Autorotation_Phase::BAIL_OUT: - { + break; + } + + case Autorotation_Phase::BAIL_OUT: { if (_flags.bail_out_initial == true) { - // Functions and settings to be done once are done here. - gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation"); + // Functions and settings to be done once are done here. + gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation"); - // Set bail out timer remaining equal to the parameter value, bailout time - // cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME. - _bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f); + // Set bail out timer remaining equal to the parameter value, bailout time + // cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME. + _bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f); - // Set bail out start time - _bail_time_start_ms = now; + // Set bail out start time + _bail_time_start_ms = now; - // Set initial target vertical speed - _desired_v_z = curr_vel_z; + // Set initial target vertical speed + _desired_v_z = curr_vel_z; - // Initialise position and desired velocity - if (!pos_control->is_active_z()) { - pos_control->relax_z_controller(g2.arot.get_last_collective()); - } + // Initialise position and desired velocity + if (!pos_control->is_active_z()) { + pos_control->relax_z_controller(g2.arot.get_last_collective()); + } - // Get pilot parameter limits - const float pilot_spd_dn = -get_pilot_speed_dn(); - const float pilot_spd_up = g.pilot_speed_up; + // Get pilot parameter limits + const float pilot_spd_dn = -get_pilot_speed_dn(); + const float pilot_spd_up = g.pilot_speed_up; - float pilot_des_v_z = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - pilot_des_v_z = constrain_float(pilot_des_v_z, pilot_spd_dn, pilot_spd_up); + float pilot_des_v_z = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + pilot_des_v_z = constrain_float(pilot_des_v_z, pilot_spd_dn, pilot_spd_up); - // Calculate target climb rate adjustment to transition from bail out descent speed to requested climb rate on stick. - _target_climb_rate_adjust = (curr_vel_z - pilot_des_v_z)/(_bail_time - BAILOUT_MOTOR_RAMP_TIME); //accounting for 0.5s motor spool time + // Calculate target climb rate adjustment to transition from bail out descent speed to requested climb rate on stick. + _target_climb_rate_adjust = (curr_vel_z - pilot_des_v_z)/(_bail_time - BAILOUT_MOTOR_RAMP_TIME); //accounting for 0.5s motor spool time - // Calculate pitch target adjustment rate to return to level - _target_pitch_adjust = _pitch_target/_bail_time; + // Calculate pitch target adjustment rate to return to level + _target_pitch_adjust = _pitch_target/_bail_time; - // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); - pos_control->set_correction_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); + // set vertical speed and acceleration limits + pos_control->set_max_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); + pos_control->set_correction_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); - motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - _flags.bail_out_initial = false; - } + _flags.bail_out_initial = false; + } if ((now - _bail_time_start_ms)/1000.0f >= BAILOUT_MOTOR_RAMP_TIME) { // Update desired vertical speed and pitch target after the bailout motor ramp timer has completed @@ -375,14 +370,14 @@ void ModeAutorotate::run() } break; - } + } } float pilot_roll, pilot_pitch, pilot_yaw_rate; - // Operator is in control of roll and yaw. Controls act as if in stabilise flight mode. Pitch - // is controlled by speed-height controller. - get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max); - + // Operator is in control of roll and yaw. Controls act as if in stabilise flight mode. Pitch + // is controlled by speed-height controller. + get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max); + float target_roll; // Grab inertial velocity @@ -394,7 +389,7 @@ void ModeAutorotate::run() // gain scheduling for yaw float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); - pilot_yaw_rate = ((float)pilot_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g2.command_model_pilot.get_rate() / 45.0; + pilot_yaw_rate = ((float)pilot_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g2.command_model_pilot.get_rate() / 45.0; roll_vel = constrain_float(roll_vel, -560.0f, 560.0f); pitch_vel = constrain_float(pitch_vel, -560.0f, 560.0f); @@ -405,7 +400,7 @@ void ModeAutorotate::run() // roll velocity is feed into roll acceleration to minimize slip target_roll = roll_vel_error * -0.8f; target_roll = constrain_float(target_roll, -4500.0f, 4500.0f); - + // Pitch target is calculated in autorotation phase switch above attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, _pitch_target, pilot_yaw_rate); @@ -419,16 +414,15 @@ void ModeAutorotate::run() void ModeAutorotate::warning_message(uint8_t message_n) { switch (message_n) { - case 1: - { - if (_msg_flags.bad_rpm) { - // Bad rpm sensor health. - gcs().send_text(MAV_SEVERITY_INFO, "Warning: Poor RPM Sensor Health"); - gcs().send_text(MAV_SEVERITY_INFO, "Action: Minimum Collective Applied"); - _msg_flags.bad_rpm = false; - } - break; + case 1: { + if (_msg_flags.bad_rpm) { + // Bad rpm sensor health. + gcs().send_text(MAV_SEVERITY_INFO, "Warning: Poor RPM Sensor Health"); + gcs().send_text(MAV_SEVERITY_INFO, "Action: Minimum Collective Applied"); + _msg_flags.bad_rpm = false; } + break; + } } } From 4053ff8412a190973bd4185af4d5135c32f7524d Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Sat, 23 Dec 2023 23:34:16 -0500 Subject: [PATCH 07/61] AC_Autorotation: fix style on AC_Autorotation.cpp --- libraries/AC_Autorotation/AC_Autorotation.cpp | 402 +++++++++--------- 1 file changed, 201 insertions(+), 201 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index f450e20d283c6..f55e6d146dcf4 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -99,7 +99,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { AP_GROUPINFO("BAIL_TIME", 8, AC_Autorotation, _param_bail_time, AROT_BAIL_OUT_TIME), // @Param: HS_SENSOR - // @DisplayName: Main Rotor RPM Sensor + // @DisplayName: Main Rotor RPM Sensor // @Description: Allocate the RPM sensor instance to use for measuring head speed. RPM1 = 0. RPM2 = 1. // @Units: s // @Range: 0.5 3 @@ -122,7 +122,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Increment: 0.01 // @User: Advanced AP_GROUPINFO("FW_V_FF", 11, AC_Autorotation, _param_fwd_k_ff, AP_FW_VEL_FF), - + // @Param: TCH_P // @DisplayName: P gain for vertical touchdown controller // @Description: proportional term based on sink rate error @@ -130,10 +130,10 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Increment: 0.01 // @User: Advanced AP_SUBGROUPINFO(_p_coll_tch, "TCH_", 12, AC_Autorotation, AC_P), - + // @Param: COL_FILT_C // @DisplayName: Touchdown Phase Collective Filter - // @Description: Cut-off frequency for collective low pass filter. For the touchdown phase. Acts as a following trim. + // @Description: Cut-off frequency for collective low pass filter. For the touchdown phase. Acts as a following trim. // @Units: Hz // @Range: 0.2 0.8 // @Increment: 0.01 @@ -172,13 +172,13 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // Constructor AC_Autorotation::AC_Autorotation(AP_InertialNav& inav, AP_AHRS& ahrs) : _inav(inav), - _ahrs(ahrs), + _ahrs(ahrs), _p_hs(HS_CONTROLLER_HEADSPEED_P), _p_fw_vel(AP_FW_VEL_P), _p_coll_tch(TCH_P) - { - AP_Param::setup_object_defaults(this, var_info); - } +{ + AP_Param::setup_object_defaults(this, var_info); +} // Initialisation of head speed controller void AC_Autorotation::init_hs_controller() @@ -218,7 +218,7 @@ bool AC_Autorotation::update_hs_glide_controller(void) // Set collective trim low pass filter cut off frequency col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); - // Calculate the head speed error. Current rpm is normalised by the set point head speed. + // Calculate the head speed error. Current rpm is normalised by the set point head speed. // Target head speed is defined as a percentage of the set point. _head_speed_error = head_speed_norm - _target_head_speed; @@ -239,7 +239,7 @@ bool AC_Autorotation::update_hs_glide_controller(void) // Send collective to setting to motors output library set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); - + return _flags.bad_rpm_warning; } @@ -323,38 +323,38 @@ float AC_Autorotation::get_rpm(bool update_counter) void AC_Autorotation::initial_flare_estimate() { - //estimate hover thrust - float _col_hover_rad = radians(_col_min + (_col_max - _col_min)*_col_hover); - float b = _param_solidity*6.28f; - _disc_area=3.14*sq(_param_diameter*0.5f); - float lambda = (-(b/8.0f) + safe_sqrt((sq(b))/64.0f +((b/3.0f)*_col_hover_rad)))*0.5f; - float freq=_governed_rpm/60.0f; - float tip_speed= freq*6.28f*_param_diameter*0.5f; - _lift_hover = ((1.225f*sq(tip_speed)*(_param_solidity*_disc_area))*((_col_hover_rad/3.0f) - (lambda/2.0f))*5.8f)*0.5f; - - //estimate rate of descent - float omega_auto=(_param_head_speed_set_point/60.0f)*6.28f; - float tip_speed_auto = omega_auto*_param_diameter*0.5f; - float c_t = _lift_hover/(0.6125f*_disc_area*sq(tip_speed)); - _est_rod=((0.25f*(_param_solidity*0.011f/c_t)*tip_speed_auto)+((sq(c_t)/(_param_solidity*0.011f))*tip_speed_auto)); - - //estimate rotor C_d - _c=(_lift_hover/(sq(_est_rod)*0.5f*1.225f*_disc_area))*1.15f; - _c=constrain_float(_c, 0.7f, 1.4f); + //estimate hover thrust + float _col_hover_rad = radians(_col_min + (_col_max - _col_min)*_col_hover); + float b = _param_solidity*6.28f; + _disc_area=3.14*sq(_param_diameter*0.5f); + float lambda = (-(b/8.0f) + safe_sqrt((sq(b))/64.0f +((b/3.0f)*_col_hover_rad)))*0.5f; + float freq=_governed_rpm/60.0f; + float tip_speed= freq*6.28f*_param_diameter*0.5f; + _lift_hover = ((1.225f*sq(tip_speed)*(_param_solidity*_disc_area))*((_col_hover_rad/3.0f) - (lambda/2.0f))*5.8f)*0.5f; + + //estimate rate of descent + float omega_auto=(_param_head_speed_set_point/60.0f)*6.28f; + float tip_speed_auto = omega_auto*_param_diameter*0.5f; + float c_t = _lift_hover/(0.6125f*_disc_area*sq(tip_speed)); + _est_rod=((0.25f*(_param_solidity*0.011f/c_t)*tip_speed_auto)+((sq(c_t)/(_param_solidity*0.011f))*tip_speed_auto)); + + //estimate rotor C_d + _c=(_lift_hover/(sq(_est_rod)*0.5f*1.225f*_disc_area))*1.15f; + _c=constrain_float(_c, 0.7f, 1.4f); //calc flare altitude - float des_spd_fwd=_param_target_speed*0.01f; - calc_flare_alt(-_est_rod,des_spd_fwd); + float des_spd_fwd=_param_target_speed*0.01f; + calc_flare_alt(-_est_rod,des_spd_fwd); - //initialize sink rate monitor and flare bools - _flare_complete = false; - _flare_update_check = false; - _avg_sink_deriv = 0.0f; - _avg_sink_deriv_sum = 0.0f; - _index_sink_rate = 0; - memset(_curr_sink_deriv, 0, sizeof(_curr_sink_deriv)); + //initialize sink rate monitor and flare bools + _flare_complete = false; + _flare_update_check = false; + _avg_sink_deriv = 0.0f; + _avg_sink_deriv_sum = 0.0f; + _index_sink_rate = 0; + memset(_curr_sink_deriv, 0, sizeof(_curr_sink_deriv)); - gcs().send_text(MAV_SEVERITY_INFO, "Ct/sigma=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/9.8065f, _flare_alt_calc*0.01f, _c); + gcs().send_text(MAV_SEVERITY_INFO, "Ct/sigma=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/9.8065f, _flare_alt_calc*0.01f, _c); } @@ -362,71 +362,71 @@ void AC_Autorotation::initial_flare_estimate() void AC_Autorotation::calc_flare_alt(float sink_rate, float fwd_speed) { - //compute speed module and glide path angle during descent - float speed_module=norm(sink_rate,fwd_speed); - float glide_angle=safe_asin(3.14/2-(fwd_speed/speed_module)); - - //estimate inflow velocity at beginning of flare - float inflow= -speed_module*sinf(glide_angle+radians(AP_ALPHA_TPP)); - - //estimate flare duration - float k_1=fabsf((-sink_rate+0.001f-safe_sqrt(_lift_hover/_c))/(-sink_rate+0.001f+safe_sqrt(_lift_hover/_c))); - float k_2=fabsf((inflow-safe_sqrt(_lift_hover/_c))/(inflow+safe_sqrt(_lift_hover/_c))); - float delta_t_flare=(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1))-(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_2)); - - //estimate flare delta altitude - float a=(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*delta_t_flare + (2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1)); - float x=1-expf(a); - float s=1-expf((2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1))); - float d=safe_sqrt(_lift_hover/_c); - float flare_distance=((2*d/(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065))))*(a-logf(fabsf(x))-(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1)) +logf(fabsf(s))))-d*delta_t_flare; - float delta_h= -flare_distance*cosf(radians(AP_ALPHA_TPP)); - - //estimate altitude to begin collective pull + //compute speed module and glide path angle during descent + float speed_module=norm(sink_rate,fwd_speed); + float glide_angle=safe_asin(3.14/2-(fwd_speed/speed_module)); + + //estimate inflow velocity at beginning of flare + float inflow= -speed_module*sinf(glide_angle+radians(AP_ALPHA_TPP)); + + //estimate flare duration + float k_1=fabsf((-sink_rate+0.001f-safe_sqrt(_lift_hover/_c))/(-sink_rate+0.001f+safe_sqrt(_lift_hover/_c))); + float k_2=fabsf((inflow-safe_sqrt(_lift_hover/_c))/(inflow+safe_sqrt(_lift_hover/_c))); + float delta_t_flare=(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1))-(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_2)); + + //estimate flare delta altitude + float a=(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*delta_t_flare + (2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1)); + float x=1-expf(a); + float s=1-expf((2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1))); + float d=safe_sqrt(_lift_hover/_c); + float flare_distance=((2*d/(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065))))*(a-logf(fabsf(x))-(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1)) +logf(fabsf(s))))-d*delta_t_flare; + float delta_h= -flare_distance*cosf(radians(AP_ALPHA_TPP)); + + //estimate altitude to begin collective pull _cushion_alt = (-(sink_rate*cosf(radians(AP_ALPHA_TPP)))*_t_tch)*100.0f; - gcs().send_text(MAV_SEVERITY_INFO, "vs_final_est=%f ", sink_rate*cosf(radians(20))); - //total delta altitude to ground - _flare_alt_calc = _cushion_alt+delta_h*100.0f; + gcs().send_text(MAV_SEVERITY_INFO, "vs_final_est=%f ", sink_rate*cosf(radians(20))); + //total delta altitude to ground + _flare_alt_calc = _cushion_alt+delta_h*100.0f; } #if HAL_LOGGING_ENABLED void AC_Autorotation::Log_Write_Autorotation(void) const { -// @LoggerMessage: AROT -// @Vehicles: Copter -// @Description: Helicopter AutoRotation information -// @Field: TimeUS: Time since system startup -// @Field: P: P-term for headspeed controller response -// @Field: hs_e: head speed error; difference between current and desired head speed -// @Field: C_Out: Collective Out -// @Field: FFCol: FF-term for headspeed controller response -// @Field: SpdF: current forward speed -// @Field: DH: desired forward speed -// @Field: p: p-term of velocity response -// @Field: ff: ff-term of velocity response -// @Field: AccZ: average z acceleration -// @Field: DesV: Desired Sink Rate -// @Field: Rfnd: rangefinder altitude -// @Field: Hest: estimated altitude + // @LoggerMessage: AROT + // @Vehicles: Copter + // @Description: Helicopter AutoRotation information + // @Field: TimeUS: Time since system startup + // @Field: P: P-term for headspeed controller response + // @Field: hs_e: head speed error; difference between current and desired head speed + // @Field: C_Out: Collective Out + // @Field: FFCol: FF-term for headspeed controller response + // @Field: SpdF: current forward speed + // @Field: DH: desired forward speed + // @Field: p: p-term of velocity response + // @Field: ff: ff-term of velocity response + // @Field: AccZ: average z acceleration + // @Field: DesV: Desired Sink Rate + // @Field: Rfnd: rangefinder altitude + // @Field: Hest: estimated altitude //Write to data flash log AP::logger().WriteStreaming("AROT", - "TimeUS,P,hs_e,C_Out,FFCol,SpdF,DH,p,ff,AccZ,DesV,Rfnd,Hest", - "Qffffffffffff", - AP_HAL::micros64(), - (double)_p_term_hs, - (double)_head_speed_error, - (double)_collective_out, - (double)_ff_term_hs, - (double)(_speed_forward*0.01f), - (double)(_cmd_vel*0.01f), - (double)_vel_p, - (double)_vel_ff, - (double)_avg_acc_z, - (double)_desired_sink_rate, - (double)(_radar_alt*0.01f), - (double)(_est_alt*0.01f)) ; + "TimeUS,P,hs_e,C_Out,FFCol,SpdF,DH,p,ff,AccZ,DesV,Rfnd,Hest", + "Qffffffffffff", + AP_HAL::micros64(), + (double)_p_term_hs, + (double)_head_speed_error, + (double)_collective_out, + (double)_ff_term_hs, + (double)(_speed_forward*0.01f), + (double)(_cmd_vel*0.01f), + (double)_vel_p, + (double)_vel_ff, + (double)_avg_acc_z, + (double)_desired_sink_rate, + (double)(_radar_alt*0.01f), + (double)(_est_alt*0.01f)) ; } #endif // HAL_LOGGING_ENABLED @@ -435,7 +435,7 @@ void AC_Autorotation::init_fwd_spd_controller(void) { // Reset I term and acceleration target _accel_target = 0.0f; - + // Ensure parameter acceleration doesn't exceed hard-coded limit _accel_max = MIN(_param_accel_max, 500.0f); @@ -494,7 +494,7 @@ void AC_Autorotation::update_forward_speed_controller(void) _accel_target = _accel_out_last - _accel_max; } - //Limiting acceleration based on velocity gained during the previous time step + //Limiting acceleration based on velocity gained during the previous time step if (fabsf(_delta_speed_fwd) > _accel_max * _dt) { _flag_limit_accel = true; } else { @@ -508,46 +508,46 @@ void AC_Autorotation::update_forward_speed_controller(void) } _accel_out_last = _accel_out; - if (_est_alt >= _flare_alt_calc*1.25f){ - _pitch_target = accel_to_angle(-_accel_out*0.01) * 100; - }else{ - _pitch_target = 0.0f; + if (_est_alt >= _flare_alt_calc*1.25f) { + _pitch_target = accel_to_angle(-_accel_out*0.01) * 100; + } else { + _pitch_target = 0.0f; } } void AC_Autorotation::calc_sink_d_avg() { - float vertical_speed = _inav.get_velocity_z_up_cms(); - _sink_deriv= ((vertical_speed - _last_vertical_speed)*0.01f)/_dt; - _last_vertical_speed = vertical_speed; - - if(_index_sink_rate < 20){ - _avg_sink_deriv_sum -= _curr_sink_deriv[_index_sink_rate]; - _curr_sink_deriv[_index_sink_rate]= _sink_deriv; - _avg_sink_deriv_sum += _curr_sink_deriv[_index_sink_rate]; - _index_sink_rate = _index_sink_rate+1; - }else{ - _index_sink_rate = 0; - } - _avg_sink_deriv = _avg_sink_deriv_sum/20.0f; - _avg_sink_deriv = constrain_float( _avg_sink_deriv, -10.0f,10.0f); + float vertical_speed = _inav.get_velocity_z_up_cms(); + _sink_deriv= ((vertical_speed - _last_vertical_speed)*0.01f)/_dt; + _last_vertical_speed = vertical_speed; + + if (_index_sink_rate < 20) { + _avg_sink_deriv_sum -= _curr_sink_deriv[_index_sink_rate]; + _curr_sink_deriv[_index_sink_rate]= _sink_deriv; + _avg_sink_deriv_sum += _curr_sink_deriv[_index_sink_rate]; + _index_sink_rate = _index_sink_rate+1; + } else { + _index_sink_rate = 0; + } + _avg_sink_deriv = _avg_sink_deriv_sum/20.0f; + _avg_sink_deriv = constrain_float( _avg_sink_deriv, -10.0f,10.0f); } void AC_Autorotation::update_flare_alt() { - if(!_flare_update_check){ - float delta_v_z = fabsf((_inav.get_velocity_z_up_cms())*0.01f+_est_rod); - - if(_speed_forward >= 0.8f*_param_target_speed && delta_v_z<=1 && fabsf(_avg_sink_deriv)<=0.005 ){ - float vel_z=_inav.get_velocity_z_up_cms()*0.01f; - float spd_fwd=_speed_forward*0.01f; - _c=(_lift_hover/(sq(vel_z)*0.5f*1.225f*_disc_area))*1.15f; - _c=constrain_float(_c, 0.7f, 1.4f); - calc_flare_alt(vel_z,spd_fwd); - _flare_update_check = true; - gcs().send_text(MAV_SEVERITY_INFO, "Flare_alt_updated=%f C_updated=%f", _flare_alt_calc*0.01f, _c); - } - } + if (!_flare_update_check) { + float delta_v_z = fabsf((_inav.get_velocity_z_up_cms())*0.01f+_est_rod); + + if (_speed_forward >= 0.8f*_param_target_speed && delta_v_z<=1 && fabsf(_avg_sink_deriv)<=0.005 ) { + float vel_z=_inav.get_velocity_z_up_cms()*0.01f; + float spd_fwd=_speed_forward*0.01f; + _c=(_lift_hover/(sq(vel_z)*0.5f*1.225f*_disc_area))*1.15f; + _c=constrain_float(_c, 0.7f, 1.4f); + calc_flare_alt(vel_z,spd_fwd); + _flare_update_check = true; + gcs().send_text(MAV_SEVERITY_INFO, "Flare_alt_updated=%f C_updated=%f", _flare_alt_calc*0.01f, _c); + } + } } @@ -562,15 +562,15 @@ float AC_Autorotation::calc_speed_forward(void) void AC_Autorotation::flare_controller() { - - // Specify forward velocity component and determine delta velocity with respect to time + + // Specify forward velocity component and determine delta velocity with respect to time _speed_forward = calc_speed_forward(); //(cm/s) _delta_speed_fwd = _speed_forward - _speed_forward_last; //(cm/s) _speed_forward_last = _speed_forward; //(cm/s) _desired_speed = linear_interpolate(0.0f, _flare_entry_speed, _est_alt, _cushion_alt, _flare_alt_calc); - // get p - _vel_p = _p_fw_vel.get_p(_desired_speed - _speed_forward); + // get p + _vel_p = _p_fw_vel.get_p(_desired_speed - _speed_forward); //calculate acceleration target based on PI controller _accel_target = _vel_p ; @@ -581,77 +581,77 @@ void AC_Autorotation::flare_controller() //Limits the maximum change in pitch attitude based on acceleration if (_accel_target > _accel_out_last + _accel_max) { - _accel_target = _accel_out_last + _accel_max; - } else if (_accel_target < _accel_out_last - _accel_max) { - _accel_target = _accel_out_last - _accel_max; - } - - //Limiting acceleration based on velocity gained during the previous time step - if (fabsf(_delta_speed_fwd) > _accel_max * _dt) { - _flag_limit_accel = true; - } else { - _flag_limit_accel = false; - } - - if ((_flag_limit_accel && fabsf(_accel_target) < fabsf(_accel_out_last)) || !_flag_limit_accel) { - _accel_out = _accel_target; - } else { - _accel_out = _accel_out_last; - } - _accel_out_last = _accel_out; - - - - //estimate flare effectiveness - - if(_speed_forward <= (0.6*_flare_entry_speed) && fabsf(_avg_sink_deriv)<=0.005f && _avg_acc_z>= -(1.1*9.80665f) ){ - if(!_flare_complete){ - _flare_complete = true; - gcs().send_text(MAV_SEVERITY_INFO, "Flare_complete"); - } - } - - if(!_flare_complete){ - _pitch_target = atanf(-_accel_out/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI); - _pitch_target = constrain_float(_pitch_target, 0.0f, AP_ALPHA_TPP*100.0f); - }else{ - _pitch_target *= 0.95f; - } + _accel_target = _accel_out_last + _accel_max; + } else if (_accel_target < _accel_out_last - _accel_max) { + _accel_target = _accel_out_last - _accel_max; + } + + //Limiting acceleration based on velocity gained during the previous time step + if (fabsf(_delta_speed_fwd) > _accel_max * _dt) { + _flag_limit_accel = true; + } else { + _flag_limit_accel = false; + } + + if ((_flag_limit_accel && fabsf(_accel_target) < fabsf(_accel_out_last)) || !_flag_limit_accel) { + _accel_out = _accel_target; + } else { + _accel_out = _accel_out_last; + } + _accel_out_last = _accel_out; + + + + //estimate flare effectiveness + + if (_speed_forward <= (0.6*_flare_entry_speed) && fabsf(_avg_sink_deriv)<=0.005f && _avg_acc_z>= -(1.1*9.80665f) ) { + if (!_flare_complete) { + _flare_complete = true; + gcs().send_text(MAV_SEVERITY_INFO, "Flare_complete"); + } + } + + if (!_flare_complete) { + _pitch_target = atanf(-_accel_out/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI); + _pitch_target = constrain_float(_pitch_target, 0.0f, AP_ALPHA_TPP*100.0f); + } else { + _pitch_target *= 0.95f; + } } void AC_Autorotation::touchdown_controller() { - float _current_sink_rate = _inav.get_velocity_z_up_cms(); - if(_radar_alt>=_ground_clearance){ - _desired_sink_rate = linear_interpolate(0.0f, _entry_sink_rate, _radar_alt, _ground_clearance, _entry_alt); - }else{ - _desired_sink_rate = 0.0f; - } + float _current_sink_rate = _inav.get_velocity_z_up_cms(); + if (_radar_alt>=_ground_clearance) { + _desired_sink_rate = linear_interpolate(0.0f, _entry_sink_rate, _radar_alt, _ground_clearance, _entry_alt); + } else { + _desired_sink_rate = 0.0f; + } // update forward speed for logging _speed_forward = calc_speed_forward(); //(cm/s) - _collective_out = constrain_value((_p_coll_tch.get_p(_desired_sink_rate - _current_sink_rate))*0.01f + _ff_term_hs, 0.0f, 1.0f); - col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); - _ff_term_hs = col_trim_lpf.apply(_collective_out, _dt); - set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); - _pitch_target *= 0.95f; + _collective_out = constrain_value((_p_coll_tch.get_p(_desired_sink_rate - _current_sink_rate))*0.01f + _ff_term_hs, 0.0f, 1.0f); + col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); + _ff_term_hs = col_trim_lpf.apply(_collective_out, _dt); + set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); + _pitch_target *= 0.95f; } void AC_Autorotation::get_entry_speed() { - _flare_entry_speed = calc_speed_forward(); + _flare_entry_speed = calc_speed_forward(); } -void AC_Autorotation::time_to_ground() +void AC_Autorotation::time_to_ground() { - if(_inav.get_velocity_z_up_cms() < 0.0f ) { - _time_to_ground = -(_radar_alt/_inav.get_velocity_z_up_cms()); - }else{ - _time_to_ground = _t_tch +1.0f; - } -} + if (_inav.get_velocity_z_up_cms() < 0.0f ) { + _time_to_ground = -(_radar_alt/_inav.get_velocity_z_up_cms()); + } else { + _time_to_ground = _t_tch +1.0f; + } +} void AC_Autorotation::init_est_radar_alt() { @@ -669,7 +669,7 @@ void AC_Autorotation::init_est_radar_alt() void AC_Autorotation::update_est_radar_alt() { - if(_using_rfnd) { + if (_using_rfnd) { // continue calculating radar altitude based on the most recent update and descent rate if (is_equal(_radar_alt, _radar_alt_prev)) { _radar_alt_calc += (_inav.get_velocity_z_up_cms() * _dt); @@ -684,8 +684,8 @@ void AC_Autorotation::update_est_radar_alt() // update descent rate filter _descent_rate_filtered = descent_rate_lpf.apply(descent_rate_corr); _est_alt += (_descent_rate_filtered * _dt); - } else { - _est_alt = _radar_alt; + } else { + _est_alt = _radar_alt; // Reset feed descent rate filter descent_rate_lpf.reset(_inav.get_velocity_z_up_cms()); // reset variables until using rangefinder @@ -697,23 +697,23 @@ void AC_Autorotation::update_est_radar_alt() void AC_Autorotation::init_avg_acc_z() { - _avg_acc_z = 0.0f; - _acc_z_sum = 0.0f; - _index = 0; - memset(_curr_acc_z, 0, sizeof(_curr_acc_z)); + _avg_acc_z = 0.0f; + _acc_z_sum = 0.0f; + _index = 0; + memset(_curr_acc_z, 0, sizeof(_curr_acc_z)); } void AC_Autorotation::calc_avg_acc_z() { - if(_index < 10){ - _acc_z_sum -= _curr_acc_z[_index]; - _curr_acc_z[_index]= _ahrs.get_accel_ef().z; - _acc_z_sum += _curr_acc_z[_index]; - _index = _index+1; - }else{ - _index = 0; - } - _avg_acc_z = _acc_z_sum/10.0f; + if (_index < 10) { + _acc_z_sum -= _curr_acc_z[_index]; + _curr_acc_z[_index]= _ahrs.get_accel_ef().z; + _acc_z_sum += _curr_acc_z[_index]; + _index = _index+1; + } else { + _index = 0; + } + _avg_acc_z = _acc_z_sum/10.0f; } From 079c189b36c931da83897f0c0bdb6243ab8c8e77 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 4 Jan 2024 13:36:17 +0000 Subject: [PATCH 08/61] Heli: Simplify passing rangefinder measurement to autorotation --- ArduCopter/heli.cpp | 25 ++++++++----------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index f51cc3d0f486e..c82fa2a2b409c 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -204,30 +204,21 @@ void Copter::heli_update_autorotation() // set autonomous autorotation flight mode set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START); } - // set flag to facilitate both auto and manual autorotations + // set flag to facilitate auto autorotation heli_flags.in_autorotation = true; motors->set_in_autorotation(heli_flags.in_autorotation); motors->set_enable_bailout(true); - int32_t gnd_dist = flightmode->get_alt_above_ground_cm(); - // use rangefinder if available - switch (rangefinder.status_orient(ROTATION_PITCH_270)) { - case RangeFinder::Status::NotConnected: - case RangeFinder::Status::NoData: - case RangeFinder::Status::OutOfRangeHigh: - // use altitude above home for non-functioning rangefinder - break; + // get height above ground. If using a healthy LiDaR below will return + // an interpolated distance based on inertial measurement. + int32_t gnd_dist = flightmode->get_alt_above_ground_cm(); - case RangeFinder::Status::OutOfRangeLow: - // altitude is close to zero (gear should deploy) + // handle the out of range case, assume we are on the ground by that point + if (rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::OutOfRangeLow) { gnd_dist = 0; - break; - - case RangeFinder::Status::Good: - // use last good reading - gnd_dist = rangefinder_state.alt_cm; - break; } + + // set the height in the autorotation controller g2.arot.set_ground_distance(gnd_dist); } #endif From 321bb44a4d18afce0cb396842da480c57c07cec1 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 4 Jan 2024 15:28:25 +0000 Subject: [PATCH 09/61] AP_Motors_Heli: Add helper to get hover col angle --- libraries/AP_Motors/AP_MotorsHeli.cpp | 6 ++++++ libraries/AP_Motors/AP_MotorsHeli.h | 3 +++ 2 files changed, 9 insertions(+) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index fc93264ed5d7f..c151539e20e1f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -613,3 +613,9 @@ AP_MotorsHeli_RSC::RotorControlState AP_MotorsHeli::get_rotor_control_state() co // Should be unreachable, but needed to keep the compiler happy return AP_MotorsHeli_RSC::RotorControlState::STOP; } + +// Return collective hover position as an angle in deg +float AP_MotorsHeli::get_hover_coll_ang(void) +{ + return _collective_min_deg.get() + (_collective_max_deg.get() - _collective_min_deg.get()) * _collective_hover.get(); +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 434ee14b9e833..9d626df445c9f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -155,6 +155,9 @@ class AP_MotorsHeli : public AP_Motors { float get_coll_min_pitch() const { return _collective_min_deg;} + // Return collective hover position as an angle in deg + float get_hover_coll_ang(void); + // enum for heli optional features enum class HeliOption { USE_LEAKY_I = (1<<0), // 1 From 9dcec13fc43e86565f8e163914375ba4bf5da23d Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 4 Jan 2024 15:30:12 +0000 Subject: [PATCH 10/61] AC_Autorotation: Create Init function and directly access AP_Motors_Heli for param values to delete object globals --- libraries/AC_Autorotation/AC_Autorotation.cpp | 42 +++++++++++-------- libraries/AC_Autorotation/AC_Autorotation.h | 16 ++----- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index f55e6d146dcf4..36052e1cf84b1 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -180,11 +180,30 @@ AC_Autorotation::AC_Autorotation(AP_InertialNav& inav, AP_AHRS& ahrs) : AP_Param::setup_object_defaults(this, var_info); } + +void AC_Autorotation::init(AP_MotorsHeli* motors) { + + _motors_heli = motors; + if (_motors_heli == nullptr) { + AP_HAL::panic("AROT: _motors_heli is nullptr"); + } + + _using_rfnd = false; + + // reset z acceleration average variables + _avg_acc_z = 0.0f; + _acc_z_sum = 0.0f; + _index = 0; + memset(_curr_acc_z, 0, sizeof(_curr_acc_z)); + + initial_flare_estimate(); +} + // Initialisation of head speed controller void AC_Autorotation::init_hs_controller() { // Set initial collective position to be the collective position on initialisation - _collective_out = _col_mid; + _collective_out = _motors_heli->get_coll_mid(); // Reset feed forward filter col_trim_lpf.reset(_collective_out); @@ -251,7 +270,7 @@ void AC_Autorotation::update_hover_autorotation_controller() col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); // use zero thrust collective to minimize rotor speed loss - _ff_term_hs = col_trim_lpf.apply(_col_mid, _dt); + _ff_term_hs = col_trim_lpf.apply(_motors_heli->get_coll_mid(), _dt); // Calculate collective position to be set _collective_out = constrain_value(_ff_term_hs, 0.0f, 1.0f) ; @@ -263,11 +282,8 @@ void AC_Autorotation::update_hover_autorotation_controller() // Function to set collective and collective filter in motor library void AC_Autorotation::set_collective(float collective_filter_cutoff) const { - AP_Motors *motors = AP::motors(); - if (motors) { - motors->set_throttle_filter_cutoff(collective_filter_cutoff); - motors->set_throttle(_collective_out); - } + _motors_heli->set_throttle_filter_cutoff(collective_filter_cutoff); + _motors_heli->set_throttle(_collective_out); } //function that gets rpm and does rpm signal checking to ensure signal is reliable @@ -324,11 +340,11 @@ float AC_Autorotation::get_rpm(bool update_counter) void AC_Autorotation::initial_flare_estimate() { //estimate hover thrust - float _col_hover_rad = radians(_col_min + (_col_max - _col_min)*_col_hover); + float _col_hover_rad = radians(_motors_heli->get_hover_coll_ang()); float b = _param_solidity*6.28f; _disc_area=3.14*sq(_param_diameter*0.5f); float lambda = (-(b/8.0f) + safe_sqrt((sq(b))/64.0f +((b/3.0f)*_col_hover_rad)))*0.5f; - float freq=_governed_rpm/60.0f; + float freq = _motors_heli->get_rpm_setpoint()/60.0f; float tip_speed= freq*6.28f*_param_diameter*0.5f; _lift_hover = ((1.225f*sq(tip_speed)*(_param_solidity*_disc_area))*((_col_hover_rad/3.0f) - (lambda/2.0f))*5.8f)*0.5f; @@ -695,14 +711,6 @@ void AC_Autorotation::update_est_radar_alt() } } -void AC_Autorotation::init_avg_acc_z() -{ - _avg_acc_z = 0.0f; - _acc_z_sum = 0.0f; - _index = 0; - memset(_curr_acc_z, 0, sizeof(_curr_acc_z)); - -} void AC_Autorotation::calc_avg_acc_z() { diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index c65dcd5c74e8f..4f9d72bfe4f34 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -17,7 +18,7 @@ class AC_Autorotation //Constructor AC_Autorotation(AP_InertialNav& inav, AP_AHRS& ahrs); - //--------Functions-------- + void init(AP_MotorsHeli* motors); // object initialisation void init_hs_controller(void); // Initialise head speed controller void initial_flare_estimate(void); void init_fwd_spd_controller(void); // Initialise forward speed controller @@ -47,11 +48,6 @@ class AC_Autorotation float get_ground_distance() const { return _radar_alt; } float get_time_to_ground() const { return _time_to_ground; } void time_to_ground(); - void set_collective_minimum_drag(float col_mid ) { _col_mid = col_mid; } - void set_collective_hover(float col_hover) {_col_hover = col_hover; } - void set_collective_max(float col_max) {_col_max = col_max; } - void set_collective_min(float col_min) {_col_min = col_min; } - void get_gov_rpm(float gov_rpm) {_governed_rpm = gov_rpm; } void set_entry_sink_rate (float sink_rate) { _entry_sink_rate = sink_rate; } void set_entry_alt (float entry_alt) { _entry_alt = entry_alt; } void set_ground_clearance(float ground_clearance) { _ground_clearance = ground_clearance; } @@ -59,7 +55,6 @@ class AC_Autorotation void update_est_radar_alt(); float get_est_alt() const {return _est_alt;} void update_hover_autorotation_controller(); - void init_avg_acc_z(); void calc_avg_acc_z(); float get_flare_alt() const {return _flare_alt_calc;} void update_flare_alt(); @@ -111,7 +106,6 @@ class AC_Autorotation float _desired_speed; float _time_to_ground; float _desired_sink_rate; - float _col_mid; float _ground_clearance; float _est_alt; float _descent_rate_filtered; @@ -121,13 +115,9 @@ class AC_Autorotation float _acc_z_sum; int16_t _index; float _curr_acc_z[10]{}; - float _col_hover; - float _col_max; - float _col_min; float _flare_alt_calc; float _lift_hover; float _c; - float _governed_rpm; float _cushion_alt; float _disc_area; float _last_vertical_speed; @@ -175,4 +165,6 @@ class AC_Autorotation //--------References to Other Libraries-------- AP_InertialNav& _inav; AP_AHRS& _ahrs; + + AP_MotorsHeli* _motors_heli; }; From 006a3b627e11ebfc3fa53c0c2ecd2475241a767f Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 4 Jan 2024 15:30:47 +0000 Subject: [PATCH 11/61] Copter: Pass motors ptr to autorotation object --- ArduCopter/mode_autorotate.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index e446d84fae61b..ed2cb4d01be63 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -40,6 +40,9 @@ bool ModeAutorotate::init(bool ignore_checks) // zero thrust collective is set in library. Must be set before init_hs_controller is called. g2.arot.set_collective_minimum_drag(motors->get_coll_mid()); + // init autorotation controllers object + g2.arot.init(motors); + // Initialise controllers // This must be done before RPM value is fetched g2.arot.init_hs_controller(); From 3959534f52bf10b0e7cfc7c03c91f39fbb4ba117 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 4 Jan 2024 15:31:26 +0000 Subject: [PATCH 12/61] Copter: Remove now unused function calls to Autorotate object --- ArduCopter/mode_autorotate.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index ed2cb4d01be63..5482413cbf8b1 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -37,9 +37,6 @@ bool ModeAutorotate::init(bool ignore_checks) return false; } - // zero thrust collective is set in library. Must be set before init_hs_controller is called. - g2.arot.set_collective_minimum_drag(motors->get_coll_mid()); - // init autorotation controllers object g2.arot.init(motors); @@ -68,14 +65,6 @@ bool ModeAutorotate::init(bool ignore_checks) _flags.bail_out_initial = true; _msg_flags.bad_rpm = true; initial_energy_check = true; - g2.arot._using_rfnd = false; - g2.arot.init_avg_acc_z(); - g2.arot.set_collective_minimum_drag(motors->get_coll_mid()); - g2.arot.set_collective_hover(motors->get_coll_hover()); - g2.arot.set_collective_max(motors->get_coll_max_pitch()); - g2.arot.set_collective_min(motors->get_coll_min_pitch()); - g2.arot.get_gov_rpm(motors->get_rpm_setpoint()); - g2.arot.initial_flare_estimate(); // Setting default starting switches phase_switch = Autorotation_Phase::ENTRY; @@ -206,7 +195,6 @@ void ModeAutorotate::run() } else { _pitch_target = 0.0f; g2.arot.update_hover_autorotation_controller(); //run head speed/ collective controller - g2.arot.set_collective_minimum_drag(motors->get_coll_mid()); g2.arot.set_entry_sink_rate(curr_vel_z); g2.arot.set_entry_alt(g2.arot.get_ground_distance()); } From e7887eb6d7690b4e4643a2255be601316f1ac856 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 4 Jan 2024 16:15:18 +0000 Subject: [PATCH 13/61] AC_Autorotation: White space changes and minor formatting --- libraries/AC_Autorotation/AC_Autorotation.cpp | 278 +++++++++--------- libraries/AC_Autorotation/AC_Autorotation.h | 154 +++++++--- 2 files changed, 252 insertions(+), 180 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 36052e1cf84b1..29b3170bf8528 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -169,6 +169,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { AP_GROUPEND }; + // Constructor AC_Autorotation::AC_Autorotation(AP_InertialNav& inav, AP_AHRS& ahrs) : _inav(inav), @@ -190,7 +191,7 @@ void AC_Autorotation::init(AP_MotorsHeli* motors) { _using_rfnd = false; - // reset z acceleration average variables + // Reset z acceleration average variables _avg_acc_z = 0.0f; _acc_z_sum = 0.0f; _index = 0; @@ -199,6 +200,7 @@ void AC_Autorotation::init(AP_MotorsHeli* motors) { initial_flare_estimate(); } + // Initialisation of head speed controller void AC_Autorotation::init_hs_controller() { @@ -216,10 +218,11 @@ void AC_Autorotation::init_hs_controller() _healthy_rpm_counter = 0; // Protect against divide by zero - _param_head_speed_set_point.set(MAX(_param_head_speed_set_point,500)); + _param_head_speed_set_point.set(MAX(_param_head_speed_set_point, 500)); } + // Rotor Speed controller for entry, glide and flare phases of autorotation bool AC_Autorotation::update_hs_glide_controller(void) { @@ -230,7 +233,7 @@ bool AC_Autorotation::update_hs_glide_controller(void) // Get current rpm and update healthy signal counters _current_rpm = get_rpm(true); - if (_unhealthy_rpm_counter <=30) { + if (_unhealthy_rpm_counter <= 30) { // Normalised head speed float head_speed_norm = _current_rpm / _param_head_speed_set_point; @@ -259,26 +262,26 @@ bool AC_Autorotation::update_hs_glide_controller(void) // Send collective to setting to motors output library set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); - return _flags.bad_rpm_warning; } + void AC_Autorotation::update_hover_autorotation_controller() { - // Set collective trim low pass filter cut off frequency col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); - // use zero thrust collective to minimize rotor speed loss + // Use zero thrust collective to minimize rotor speed loss _ff_term_hs = col_trim_lpf.apply(_motors_heli->get_coll_mid(), _dt); // Calculate collective position to be set - _collective_out = constrain_value(_ff_term_hs, 0.0f, 1.0f) ; + _collective_out = constrain_value(_ff_term_hs, 0.0f, 1.0f); // Send collective to setting to motors output library set_collective(_col_cutoff_freq); } + // Function to set collective and collective filter in motor library void AC_Autorotation::set_collective(float collective_filter_cutoff) const { @@ -286,8 +289,9 @@ void AC_Autorotation::set_collective(float collective_filter_cutoff) const _motors_heli->set_throttle(_collective_out); } -//function that gets rpm and does rpm signal checking to ensure signal is reliable -//before using it in the controller + +// Function that gets rpm and does rpm signal checking to ensure signal is reliable +// before using it in the controller float AC_Autorotation::get_rpm(bool update_counter) { float current_rpm = 0.0f; @@ -296,17 +300,17 @@ float AC_Autorotation::get_rpm(bool update_counter) // Get singleton for RPM library const AP_RPM *rpm = AP_RPM::get_singleton(); - //Get current rpm, checking to ensure no nullptr + // Get current rpm, checking to ensure no nullptr if (rpm != nullptr) { - //Check requested rpm instance to ensure either 0 or 1. Always defaults to 0. + // Check requested rpm instance to ensure either 0 or 1. Always defaults to 0. if ((_param_rpm_instance > 1) || (_param_rpm_instance < 0)) { _param_rpm_instance.set(0); } - //Get RPM value + // Get RPM value uint8_t instance = _param_rpm_instance; - //Check RPM sensor is returning a healthy status + // Check RPM sensor is returning a healthy status if (!rpm->get_rpm(instance, current_rpm) || current_rpm <= -1) { //unhealthy, rpm unreliable _flags.bad_rpm = true; @@ -320,16 +324,16 @@ float AC_Autorotation::get_rpm(bool update_counter) #endif if (_flags.bad_rpm) { - //count unhealthy rpm updates and reset healthy rpm counter + // Count unhealthy rpm updates and reset healthy rpm counter _unhealthy_rpm_counter++; _healthy_rpm_counter = 0; } else if (!_flags.bad_rpm && _unhealthy_rpm_counter > 0) { - //poor rpm reading may have cleared. Wait 10 update cycles to clear. + // Poor rpm reading may have cleared. Wait 10 update cycles to clear. _healthy_rpm_counter++; if (_healthy_rpm_counter >= 10) { - //poor rpm health has cleared, reset counters + // Poor rpm health has cleared, reset counters _unhealthy_rpm_counter = 0; _healthy_rpm_counter = 0; } @@ -337,32 +341,33 @@ float AC_Autorotation::get_rpm(bool update_counter) return current_rpm; } -void AC_Autorotation::initial_flare_estimate() + +void AC_Autorotation::initial_flare_estimate(void) { - //estimate hover thrust + // Estimate hover thrust float _col_hover_rad = radians(_motors_heli->get_hover_coll_ang()); - float b = _param_solidity*6.28f; - _disc_area=3.14*sq(_param_diameter*0.5f); - float lambda = (-(b/8.0f) + safe_sqrt((sq(b))/64.0f +((b/3.0f)*_col_hover_rad)))*0.5f; - float freq = _motors_heli->get_rpm_setpoint()/60.0f; - float tip_speed= freq*6.28f*_param_diameter*0.5f; - _lift_hover = ((1.225f*sq(tip_speed)*(_param_solidity*_disc_area))*((_col_hover_rad/3.0f) - (lambda/2.0f))*5.8f)*0.5f; - - //estimate rate of descent - float omega_auto=(_param_head_speed_set_point/60.0f)*6.28f; - float tip_speed_auto = omega_auto*_param_diameter*0.5f; - float c_t = _lift_hover/(0.6125f*_disc_area*sq(tip_speed)); - _est_rod=((0.25f*(_param_solidity*0.011f/c_t)*tip_speed_auto)+((sq(c_t)/(_param_solidity*0.011f))*tip_speed_auto)); - - //estimate rotor C_d - _c=(_lift_hover/(sq(_est_rod)*0.5f*1.225f*_disc_area))*1.15f; - _c=constrain_float(_c, 0.7f, 1.4f); - - //calc flare altitude - float des_spd_fwd=_param_target_speed*0.01f; - calc_flare_alt(-_est_rod,des_spd_fwd); - - //initialize sink rate monitor and flare bools + float b = _param_solidity * 6.28f; + _disc_area = 3.14 * sq(_param_diameter * 0.5f); + float lambda = (-(b / 8.0f) + safe_sqrt((sq(b)) / 64.0f +((b / 3.0f) * _col_hover_rad))) * 0.5f; + float freq = _motors_heli->get_rpm_setpoint() / 60.0f; + float tip_speed= freq * 6.28f * _param_diameter * 0.5f; + _lift_hover = ((1.225f * sq(tip_speed) * (_param_solidity * _disc_area)) * ((_col_hover_rad / 3.0f) - (lambda / 2.0f)) * 5.8f) * 0.5f; + + // Estimate rate of descent + float omega_auto = (_param_head_speed_set_point / 60.0f) * 6.28f; + float tip_speed_auto = omega_auto * _param_diameter * 0.5f; + float c_t = _lift_hover / (0.6125f * _disc_area * sq(tip_speed)); + _est_rod = ((0.25f * (_param_solidity * 0.011f / c_t) * tip_speed_auto) + ((sq(c_t) / (_param_solidity * 0.011f)) * tip_speed_auto)); + + // Estimate rotor C_d + _c = (_lift_hover / (sq(_est_rod) * 0.5f * 1.225f * _disc_area)) * 1.15f; + _c = constrain_float(_c, 0.7f, 1.4f); + + // Calc flare altitude + float des_spd_fwd = _param_target_speed * 0.01f; + calc_flare_alt(-_est_rod, des_spd_fwd); + + // Initialize sink rate monitor and flare bools _flare_complete = false; _flare_update_check = false; _avg_sink_deriv = 0.0f; @@ -371,38 +376,37 @@ void AC_Autorotation::initial_flare_estimate() memset(_curr_sink_deriv, 0, sizeof(_curr_sink_deriv)); gcs().send_text(MAV_SEVERITY_INFO, "Ct/sigma=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/9.8065f, _flare_alt_calc*0.01f, _c); - } - void AC_Autorotation::calc_flare_alt(float sink_rate, float fwd_speed) { - //compute speed module and glide path angle during descent - float speed_module=norm(sink_rate,fwd_speed); - float glide_angle=safe_asin(3.14/2-(fwd_speed/speed_module)); - - //estimate inflow velocity at beginning of flare - float inflow= -speed_module*sinf(glide_angle+radians(AP_ALPHA_TPP)); - - //estimate flare duration - float k_1=fabsf((-sink_rate+0.001f-safe_sqrt(_lift_hover/_c))/(-sink_rate+0.001f+safe_sqrt(_lift_hover/_c))); - float k_2=fabsf((inflow-safe_sqrt(_lift_hover/_c))/(inflow+safe_sqrt(_lift_hover/_c))); - float delta_t_flare=(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1))-(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_2)); - - //estimate flare delta altitude - float a=(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*delta_t_flare + (2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1)); - float x=1-expf(a); - float s=1-expf((2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1))); - float d=safe_sqrt(_lift_hover/_c); - float flare_distance=((2*d/(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065))))*(a-logf(fabsf(x))-(2*safe_sqrt(_c*9.8065/(_lift_hover/9.8065)))*(0.5f*(_lift_hover/(9.8065*_c))*safe_sqrt(_c/_lift_hover)*logf(k_1)) +logf(fabsf(s))))-d*delta_t_flare; - float delta_h= -flare_distance*cosf(radians(AP_ALPHA_TPP)); - - //estimate altitude to begin collective pull - _cushion_alt = (-(sink_rate*cosf(radians(AP_ALPHA_TPP)))*_t_tch)*100.0f; + // Compute speed module and glide path angle during descent + float speed_module = norm(sink_rate, fwd_speed); + float glide_angle = safe_asin(3.14 / 2 - (fwd_speed / speed_module)); + + // Estimate inflow velocity at beginning of flare + float inflow = - speed_module * sinf(glide_angle + radians(AP_ALPHA_TPP)); + + // Estimate flare duration + float k_1 = fabsf((-sink_rate + 0.001f - safe_sqrt(_lift_hover / _c))/(-sink_rate + 0.001f + safe_sqrt(_lift_hover / _c))); + float k_2 = fabsf((inflow - safe_sqrt(_lift_hover / _c)) / (inflow + safe_sqrt(_lift_hover / _c))); + float delta_t_flare = (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) - (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_2)); + + // Estimate flare delta altitude + float a = (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * delta_t_flare + (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)); + float x = 1 - expf(a); + float s = 1 - expf((2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * (0.5f * (_lift_hover/(9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1))); + float d = safe_sqrt(_lift_hover / _c); + float flare_distance = ((2 * d / (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065)))) * (a - logf(fabsf(x)) - (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) + logf(fabsf(s)))) - d * delta_t_flare; + float delta_h = -flare_distance * cosf(radians(AP_ALPHA_TPP)); + + // Estimate altitude to begin collective pull + _cushion_alt = (-(sink_rate * cosf(radians(AP_ALPHA_TPP))) * _t_tch) * 100.0f; gcs().send_text(MAV_SEVERITY_INFO, "vs_final_est=%f ", sink_rate*cosf(radians(20))); - //total delta altitude to ground - _flare_alt_calc = _cushion_alt+delta_h*100.0f; + + // Total delta altitude to ground + _flare_alt_calc = _cushion_alt + delta_h * 100.0f; } @@ -446,6 +450,7 @@ void AC_Autorotation::Log_Write_Autorotation(void) const } #endif // HAL_LOGGING_ENABLED + // Initialise forward speed controller void AC_Autorotation::init_fwd_spd_controller(void) { @@ -457,18 +462,18 @@ void AC_Autorotation::init_fwd_spd_controller(void) // Reset cmd vel and last accel to sensible values _cmd_vel = calc_speed_forward(); //(cm/s) - _accel_out_last = _cmd_vel*_param_fwd_k_ff; + _accel_out_last = _cmd_vel * _param_fwd_k_ff; } -// set_dt - sets time delta in seconds for all controllers +// Sets time delta in seconds for all controllers void AC_Autorotation::set_dt(float delta_sec) { _dt = delta_sec; } -// update speed controller +// Update speed controller void AC_Autorotation::update_forward_speed_controller(void) { // Specify forward velocity component and determine delta velocity with respect to time @@ -490,27 +495,27 @@ void AC_Autorotation::update_forward_speed_controller(void) } } - // get p - _vel_p = _p_fw_vel.get_p(_cmd_vel-_speed_forward); + // Get p + _vel_p = _p_fw_vel.get_p(_cmd_vel - _speed_forward); - // get ff - _vel_ff = _cmd_vel*_param_fwd_k_ff; + // Get ff + _vel_ff = _cmd_vel * _param_fwd_k_ff; - //calculate acceleration target based on PI controller + // Calculate acceleration target based on PI controller _accel_target = _vel_ff + _vel_p; - // filter correction acceleration + // Filter correction acceleration _accel_target_filter.set_cutoff_frequency(10.0f); _accel_target_filter.apply(_accel_target, _dt); - //Limits the maximum change in pitch attitude based on acceleration + // Limits the maximum change in pitch attitude based on acceleration if (_accel_target > _accel_out_last + _accel_max) { _accel_target = _accel_out_last + _accel_max; } else if (_accel_target < _accel_out_last - _accel_max) { _accel_target = _accel_out_last - _accel_max; } - //Limiting acceleration based on velocity gained during the previous time step + // Limiting acceleration based on velocity gained during the previous time step if (fabsf(_delta_speed_fwd) > _accel_max * _dt) { _flag_limit_accel = true; } else { @@ -524,41 +529,43 @@ void AC_Autorotation::update_forward_speed_controller(void) } _accel_out_last = _accel_out; - if (_est_alt >= _flare_alt_calc*1.25f) { - _pitch_target = accel_to_angle(-_accel_out*0.01) * 100; + if (_est_alt >= _flare_alt_calc * 1.25f) { + _pitch_target = accel_to_angle(-_accel_out * 0.01) * 100; } else { _pitch_target = 0.0f; } } -void AC_Autorotation::calc_sink_d_avg() + +void AC_Autorotation::calc_sink_d_avg(void) { float vertical_speed = _inav.get_velocity_z_up_cms(); - _sink_deriv= ((vertical_speed - _last_vertical_speed)*0.01f)/_dt; + _sink_deriv = ((vertical_speed - _last_vertical_speed) * 0.01f) / _dt; _last_vertical_speed = vertical_speed; if (_index_sink_rate < 20) { _avg_sink_deriv_sum -= _curr_sink_deriv[_index_sink_rate]; - _curr_sink_deriv[_index_sink_rate]= _sink_deriv; + _curr_sink_deriv[_index_sink_rate] = _sink_deriv; _avg_sink_deriv_sum += _curr_sink_deriv[_index_sink_rate]; - _index_sink_rate = _index_sink_rate+1; + _index_sink_rate = _index_sink_rate + 1; } else { _index_sink_rate = 0; } - _avg_sink_deriv = _avg_sink_deriv_sum/20.0f; - _avg_sink_deriv = constrain_float( _avg_sink_deriv, -10.0f,10.0f); + _avg_sink_deriv = _avg_sink_deriv_sum / 20.0f; + _avg_sink_deriv = constrain_float( _avg_sink_deriv, -10.0f, 10.0f); } -void AC_Autorotation::update_flare_alt() + +void AC_Autorotation::update_flare_alt(void) { if (!_flare_update_check) { - float delta_v_z = fabsf((_inav.get_velocity_z_up_cms())*0.01f+_est_rod); + float delta_v_z = fabsf((_inav.get_velocity_z_up_cms()) * 0.01f + _est_rod); - if (_speed_forward >= 0.8f*_param_target_speed && delta_v_z<=1 && fabsf(_avg_sink_deriv)<=0.005 ) { - float vel_z=_inav.get_velocity_z_up_cms()*0.01f; - float spd_fwd=_speed_forward*0.01f; - _c=(_lift_hover/(sq(vel_z)*0.5f*1.225f*_disc_area))*1.15f; - _c=constrain_float(_c, 0.7f, 1.4f); + if ((_speed_forward >= 0.8f * _param_target_speed) && (delta_v_z <= 1) && (fabsf(_avg_sink_deriv)<=0.005)) { + float vel_z = _inav.get_velocity_z_up_cms() * 0.01f; + float spd_fwd = _speed_forward * 0.01f; + _c = (_lift_hover / (sq(vel_z) * 0.5f * 1.225f * _disc_area)) * 1.15f; + _c = constrain_float(_c, 0.7f, 1.4f); calc_flare_alt(vel_z,spd_fwd); _flare_update_check = true; gcs().send_text(MAV_SEVERITY_INFO, "Flare_alt_updated=%f C_updated=%f", _flare_alt_calc*0.01f, _c); @@ -572,37 +579,38 @@ float AC_Autorotation::calc_speed_forward(void) { auto &ahrs = AP::ahrs(); Vector2f groundspeed_vector = ahrs.groundspeed_vector(); - float speed_forward = (groundspeed_vector.x*ahrs.cos_yaw() + groundspeed_vector.y*ahrs.sin_yaw())* 100; //(c/s) + float speed_forward = (groundspeed_vector.x*ahrs.cos_yaw() + groundspeed_vector.y * ahrs.sin_yaw()) * 100.0; // (cm/s) return speed_forward; } -void AC_Autorotation::flare_controller() + +void AC_Autorotation::flare_controller(void) { // Specify forward velocity component and determine delta velocity with respect to time - _speed_forward = calc_speed_forward(); //(cm/s) - _delta_speed_fwd = _speed_forward - _speed_forward_last; //(cm/s) - _speed_forward_last = _speed_forward; //(cm/s) + _speed_forward = calc_speed_forward(); // (cm/s) + _delta_speed_fwd = _speed_forward - _speed_forward_last; // (cm/s) + _speed_forward_last = _speed_forward; // (cm/s) _desired_speed = linear_interpolate(0.0f, _flare_entry_speed, _est_alt, _cushion_alt, _flare_alt_calc); - // get p + // Get p _vel_p = _p_fw_vel.get_p(_desired_speed - _speed_forward); - //calculate acceleration target based on PI controller - _accel_target = _vel_p ; + // Calculate acceleration target based on PI controller + _accel_target = _vel_p; - // filter correction acceleration + // Filter correction acceleration _accel_target_filter.set_cutoff_frequency(10.0f); _accel_target_filter.apply(_accel_target, _dt); - //Limits the maximum change in pitch attitude based on acceleration + // Limits the maximum change in pitch attitude based on acceleration if (_accel_target > _accel_out_last + _accel_max) { _accel_target = _accel_out_last + _accel_max; } else if (_accel_target < _accel_out_last - _accel_max) { _accel_target = _accel_out_last - _accel_max; } - //Limiting acceleration based on velocity gained during the previous time step + // Limiting acceleration based on velocity gained during the previous time step if (fabsf(_delta_speed_fwd) > _accel_max * _dt) { _flag_limit_accel = true; } else { @@ -616,11 +624,8 @@ void AC_Autorotation::flare_controller() } _accel_out_last = _accel_out; - - - //estimate flare effectiveness - - if (_speed_forward <= (0.6*_flare_entry_speed) && fabsf(_avg_sink_deriv)<=0.005f && _avg_acc_z>= -(1.1*9.80665f) ) { + // Estimate flare effectiveness + if (_speed_forward <= (0.6 * _flare_entry_speed) && (fabsf(_avg_sink_deriv) <= 0.005f) && (_avg_acc_z>= -1.1 * 9.80665f)) { if (!_flare_complete) { _flare_complete = true; gcs().send_text(MAV_SEVERITY_INFO, "Flare_complete"); @@ -628,25 +633,25 @@ void AC_Autorotation::flare_controller() } if (!_flare_complete) { - _pitch_target = atanf(-_accel_out/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI); - _pitch_target = constrain_float(_pitch_target, 0.0f, AP_ALPHA_TPP*100.0f); + _pitch_target = atanf(-_accel_out / (GRAVITY_MSS * 100.0f)) * (18000.0f/M_PI); + _pitch_target = constrain_float(_pitch_target, 0.0f, AP_ALPHA_TPP * 100.0f); } else { _pitch_target *= 0.95f; } - - } -void AC_Autorotation::touchdown_controller() + +void AC_Autorotation::touchdown_controller(void) { float _current_sink_rate = _inav.get_velocity_z_up_cms(); - if (_radar_alt>=_ground_clearance) { + if (_radar_alt >= _ground_clearance) { _desired_sink_rate = linear_interpolate(0.0f, _entry_sink_rate, _radar_alt, _ground_clearance, _entry_alt); } else { _desired_sink_rate = 0.0f; } - // update forward speed for logging - _speed_forward = calc_speed_forward(); //(cm/s) + + // Update forward speed for logging + _speed_forward = calc_speed_forward(); // (cm/s) _collective_out = constrain_value((_p_coll_tch.get_p(_desired_sink_rate - _current_sink_rate))*0.01f + _ff_term_hs, 0.0f, 1.0f); col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); @@ -655,23 +660,26 @@ void AC_Autorotation::touchdown_controller() _pitch_target *= 0.95f; } -void AC_Autorotation::get_entry_speed() + +void AC_Autorotation::get_entry_speed(void) { _flare_entry_speed = calc_speed_forward(); } -void AC_Autorotation::time_to_ground() + +void AC_Autorotation::time_to_ground(void) { if (_inav.get_velocity_z_up_cms() < 0.0f ) { - _time_to_ground = -(_radar_alt/_inav.get_velocity_z_up_cms()); + _time_to_ground = (-_radar_alt / _inav.get_velocity_z_up_cms()); } else { - _time_to_ground = _t_tch +1.0f; + _time_to_ground = _t_tch + 1.0f; } } -void AC_Autorotation::init_est_radar_alt() + +void AC_Autorotation::init_est_radar_alt(void) { - // set descent rate filter cutoff frequency + // Set descent rate filter cutoff frequency descent_rate_lpf.set_cutoff_frequency(40.0f); // Reset feed descent rate filter @@ -683,28 +691,33 @@ void AC_Autorotation::init_est_radar_alt() } -void AC_Autorotation::update_est_radar_alt() + +void AC_Autorotation::update_est_radar_alt(void) { if (_using_rfnd) { - // continue calculating radar altitude based on the most recent update and descent rate + // Continue calculating radar altitude based on the most recent update and descent rate if (is_equal(_radar_alt, _radar_alt_prev)) { _radar_alt_calc += (_inav.get_velocity_z_up_cms() * _dt); } else { _radar_alt_calc = _radar_alt; _radar_alt_prev = _radar_alt; } - // determine the error between a calculated radar altitude based on each update at 20 hz and the estimated update + + // Determine the error between a calculated radar altitude based on each update at 20 hz and the estimated update float alt_error = _radar_alt_calc - _est_alt; - // drive the estimated altitude to the actual altitude with a proportional altitude error feedback + // Drive the estimated altitude to the actual altitude with a proportional altitude error feedback float descent_rate_corr = _inav.get_velocity_z_up_cms() + alt_error * 2.0f; - // update descent rate filter + // Update descent rate filter _descent_rate_filtered = descent_rate_lpf.apply(descent_rate_corr); _est_alt += (_descent_rate_filtered * _dt); + } else { _est_alt = _radar_alt; + // Reset feed descent rate filter descent_rate_lpf.reset(_inav.get_velocity_z_up_cms()); - // reset variables until using rangefinder + + // Reset variables until using rangefinder _radar_alt_calc = _radar_alt; _radar_alt_prev = _radar_alt; _est_alt = _radar_alt; @@ -712,16 +725,15 @@ void AC_Autorotation::update_est_radar_alt() } -void AC_Autorotation::calc_avg_acc_z() +void AC_Autorotation::calc_avg_acc_z(void) { if (_index < 10) { _acc_z_sum -= _curr_acc_z[_index]; - _curr_acc_z[_index]= _ahrs.get_accel_ef().z; + _curr_acc_z[_index] = _ahrs.get_accel_ef().z; _acc_z_sum += _curr_acc_z[_index]; - _index = _index+1; + _index = _index + 1; } else { _index = 0; } - _avg_acc_z = _acc_z_sum/10.0f; + _avg_acc_z = _acc_z_sum / 10.0f; } - diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 4f9d72bfe4f34..f08ecc985424e 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -18,59 +18,115 @@ class AC_Autorotation //Constructor AC_Autorotation(AP_InertialNav& inav, AP_AHRS& ahrs); - void init(AP_MotorsHeli* motors); // object initialisation - void init_hs_controller(void); // Initialise head speed controller + // object initialisation + void init(AP_MotorsHeli* motors); + + // Initialise head speed controller + void init_hs_controller(void); + void initial_flare_estimate(void); - void init_fwd_spd_controller(void); // Initialise forward speed controller - bool update_hs_glide_controller(void); // Update head speed controller - float get_rpm(void) const { return _current_rpm; } // Function just returns the rpm as last read in this library - float get_rpm(bool update_counter); // Function fetches fresh rpm update and continues sensor health monitoring - void set_target_head_speed(float ths) { _target_head_speed = ths; } // Sets the normalised target head speed - void set_col_cutoff_freq(float freq) { _col_cutoff_freq = freq; } // Sets the collective low pass filter cut off frequency + + // Initialise forward speed controller + void init_fwd_spd_controller(void); + + // Update head speed controller + bool update_hs_glide_controller(void); + + // Function just returns the rpm as last read in this library + float get_rpm(void) const { return _current_rpm; } + + // Function fetches fresh rpm update and continues sensor health monitoring + float get_rpm(bool update_counter); + + // Sets the normalised target head speed + void set_target_head_speed(float ths) { _target_head_speed = ths; } + + // Sets the collective low pass filter cut off frequency + void set_col_cutoff_freq(float freq) { _col_cutoff_freq = freq; } + int16_t get_hs_set_point(void) { return _param_head_speed_set_point; } + float get_col_entry_freq(void) { return _param_col_entry_cutoff_freq; } + float get_col_glide_freq(void) { return _param_col_glide_cutoff_freq; } - float get_col_cushion_freq(void) { return _param_col_cushion_cutoff_freq; } + + float get_col_cushion_freq(void) { return _param_col_cushion_cutoff_freq; } + float get_bail_time(void) { return _param_bail_time; } + float get_last_collective() const { return _collective_out; } + bool is_enable(void) { return _param_enable; } + void Log_Write_Autorotation(void) const; - void update_forward_speed_controller(void); // Update forward speed controller - void set_desired_fwd_speed(void) { _vel_target = _param_target_speed; } // Overloaded: Set desired speed for forward controller to parameter value - void set_desired_fwd_speed(float speed) { _vel_target = speed; } // Overloaded: Set desired speed to argument value - int32_t get_pitch(void) const { return _pitch_target; } // Get pitch target - float calc_speed_forward(void); // Calculates the forward speed in the horizontal plane - void set_dt(float delta_sec); // set the loop time - void flare_controller(); // Update the flare controller - void touchdown_controller(); // Update the touchdown controller - void set_ground_distance(float radalt) { _radar_alt = radalt; } - void get_entry_speed(); - float get_ground_distance() const { return _radar_alt; } - float get_time_to_ground() const { return _time_to_ground; } - void time_to_ground(); + + // Update forward speed controller + void update_forward_speed_controller(void); + + // Overloaded: Set desired speed for forward controller to parameter value + void set_desired_fwd_speed(void) { _vel_target = _param_target_speed; } + + // Overloaded: Set desired speed to argument value + void set_desired_fwd_speed(float speed) { _vel_target = speed; } + + // Get pitch target + int32_t get_pitch(void) const { return _pitch_target; } + + // Calculates the forward speed in the horizontal plane + float calc_speed_forward(void); + + // set the loop time + void set_dt(float delta_sec); + + // Update the flare controller + void flare_controller(void); + + // Update the touchdown controller + void touchdown_controller(void); + + void set_ground_distance(float radalt) { _radar_alt = radalt; } + + void get_entry_speed(void); + + float get_ground_distance(void) const { return _radar_alt; } + + float get_time_to_ground(void) const { return _time_to_ground; } + + void time_to_ground(void); + void set_entry_sink_rate (float sink_rate) { _entry_sink_rate = sink_rate; } + void set_entry_alt (float entry_alt) { _entry_alt = entry_alt; } - void set_ground_clearance(float ground_clearance) { _ground_clearance = ground_clearance; } - void init_est_radar_alt(); - void update_est_radar_alt(); - float get_est_alt() const {return _est_alt;} + + void set_ground_clearance(float ground_clearance) { _ground_clearance = ground_clearance; } + + void init_est_radar_alt(void); + + void update_est_radar_alt(void); + + float get_est_alt(void) const { return _est_alt; } + void update_hover_autorotation_controller(); - void calc_avg_acc_z(); - float get_flare_alt() const {return _flare_alt_calc;} - void update_flare_alt(); + + void calc_avg_acc_z(void); + + float get_flare_alt(void) const { return _flare_alt_calc; } + + void update_flare_alt(void); + void calc_flare_alt(float sink_rate, float fwd_speed); - float get_t_touchdown() const {return _t_tch;} - float get_cushion_alt() const {return _cushion_alt;} - bool get_flare_status(void) { return _flare_complete; } - void calc_sink_d_avg(); + + float get_t_touchdown(void) const { return _t_tch; } + + float get_cushion_alt(void) const { return _cushion_alt; } + + bool get_flare_status(void) { return _flare_complete; } + + void calc_sink_d_avg(void); // User Settable Parameters static const struct AP_Param::GroupInfo var_info[]; - AP_Int16 _param_target_speed; - AP_Int16 _param_head_speed_set_point; - AP_Float _param_solidity; - AP_Float _param_diameter; - AP_Float _t_tch; + bool _using_rfnd; private: @@ -88,18 +144,18 @@ class AC_Autorotation float _vel_target; // Forward velocity target. float _pitch_target; // Pitch angle target. float _accel_max; // Maximum acceleration limit. - int16_t _speed_forward_last; // The forward speed calculated in the previous cycle. + int16_t _speed_forward_last; // The forward speed calculated in the previous cycle. bool _flag_limit_accel; // Maximum acceleration limit reached flag. float _accel_out_last; // Acceleration value used to calculate pitch target in previous cycle. float _cmd_vel; // Command velocity, used to get PID values for acceleration calculation. float _accel_target; // Acceleration target, calculated from PID. float _delta_speed_fwd; // Change in forward speed between computation cycles. float _dt; // Time step. - int16_t _speed_forward; // Measured forward speed. + int16_t _speed_forward; // Measured forward speed. float _vel_p; // Forward velocity P term. float _vel_ff; // Forward velocity Feed Forward term. float _accel_out; // Acceleration value used to calculate pitch target. - float _entry_sink_rate; + float _entry_sink_rate; float _entry_alt; float _radar_alt; float _flare_entry_speed; @@ -136,14 +192,19 @@ class AC_Autorotation AP_Int8 _param_enable; AC_P _p_hs; AC_P _p_fw_vel; - AC_P _p_coll_tch; + AC_P _p_coll_tch; AP_Float _param_col_entry_cutoff_freq; AP_Float _param_col_glide_cutoff_freq; - AP_Float _param_col_cushion_cutoff_freq; + AP_Float _param_col_cushion_cutoff_freq; AP_Int16 _param_accel_max; AP_Float _param_bail_time; AP_Int8 _param_rpm_instance; AP_Float _param_fwd_k_ff; + AP_Int16 _param_target_speed; + AP_Int16 _param_head_speed_set_point; + AP_Float _param_solidity; + AP_Float _param_diameter; + AP_Float _t_tch; //--------Internal Flags-------- @@ -158,13 +219,12 @@ class AC_Autorotation // low pass filter for collective trim LowPassFilterFloat col_trim_lpf; - + // low pass filter for descent rate LowPassFilterFloat descent_rate_lpf; - //--------References to Other Libraries-------- + //--------References to Other Libraries-------- AP_InertialNav& _inav; AP_AHRS& _ahrs; - - AP_MotorsHeli* _motors_heli; + AP_MotorsHeli* _motors_heli; }; From a9e2c26d87d19bf21e92c7808f7cbfde3ef7cf89 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 4 Jan 2024 16:25:15 +0000 Subject: [PATCH 14/61] AC_Autorotation: Fix and rename average z accel function --- libraries/AC_Autorotation/AC_Autorotation.cpp | 18 ++++++++++-------- libraries/AC_Autorotation/AC_Autorotation.h | 2 +- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 29b3170bf8528..4d26e3357117d 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -725,15 +725,17 @@ void AC_Autorotation::update_est_radar_alt(void) } -void AC_Autorotation::calc_avg_acc_z(void) +void AC_Autorotation::update_avg_acc_z(void) { - if (_index < 10) { - _acc_z_sum -= _curr_acc_z[_index]; - _curr_acc_z[_index] = _ahrs.get_accel_ef().z; - _acc_z_sum += _curr_acc_z[_index]; - _index = _index + 1; - } else { + // Wrap index + if (_index >= 10) { _index = 0; } - _avg_acc_z = _acc_z_sum / 10.0f; + + _acc_z_sum -= _curr_acc_z[_index]; + _curr_acc_z[_index] = _ahrs.get_accel_ef().z; + _acc_z_sum += _curr_acc_z[_index]; + _index = _index + 1; + + _avg_acc_z = _acc_z_sum / 10.0; } diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index f08ecc985424e..7c8e0c2e9e4df 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -108,7 +108,7 @@ class AC_Autorotation void update_hover_autorotation_controller(); - void calc_avg_acc_z(void); + void update_avg_acc_z(void); float get_flare_alt(void) const { return _flare_alt_calc; } From ed8d1aa522980dfa5d6a6e64930e1a60ccff33aa Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 4 Jan 2024 16:26:01 +0000 Subject: [PATCH 15/61] Copter: Update average z accel function name in autorotation --- ArduCopter/mode_autorotate.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 5482413cbf8b1..3c84a792e431e 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -237,7 +237,7 @@ void ModeAutorotate::run() // Update head speed/ collective controller _flags.bad_rpm = g2.arot.update_hs_glide_controller(); // Attitude controller is updated in navigation switch-case statements - g2.arot.calc_avg_acc_z(); + g2.arot.update_avg_acc_z(); break; } @@ -269,7 +269,7 @@ void ModeAutorotate::run() _pitch_target = g2.arot.get_pitch(); //calc average acceleration on z axis for estimating flare effectiveness - g2.arot.calc_avg_acc_z(); + g2.arot.update_avg_acc_z(); break; } From 6cfd4bb6e5008bb1044d19812946cc2a426b4e6e Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 4 Jan 2024 16:41:36 +0000 Subject: [PATCH 16/61] AC_Autorotation: tidy up logging --- libraries/AC_Autorotation/AC_Autorotation.cpp | 50 +++++++++---------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 4d26e3357117d..e574e8822e731 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -417,36 +417,36 @@ void AC_Autorotation::Log_Write_Autorotation(void) const // @Vehicles: Copter // @Description: Helicopter AutoRotation information // @Field: TimeUS: Time since system startup - // @Field: P: P-term for headspeed controller response - // @Field: hs_e: head speed error; difference between current and desired head speed - // @Field: C_Out: Collective Out - // @Field: FFCol: FF-term for headspeed controller response - // @Field: SpdF: current forward speed - // @Field: DH: desired forward speed - // @Field: p: p-term of velocity response - // @Field: ff: ff-term of velocity response - // @Field: AccZ: average z acceleration - // @Field: DesV: Desired Sink Rate - // @Field: Rfnd: rangefinder altitude - // @Field: Hest: estimated altitude + // @Field: hsp: P-term for headspeed controller response + // @Field: hse: head speed error; difference between current and desired head speed + // @Field: co: Collective Out + // @Field: cff: FF-term for headspeed controller response + // @Field: sf: current forward speed + // @Field: dsf: desired forward speed + // @Field: vp: p-term of velocity response + // @Field: vff: ff-term of velocity response + // @Field: az: average z acceleration + // @Field: dvz: Desired Sink Rate + // @Field: rfnd: rangefinder altitude + // @Field: h: estimated altitude //Write to data flash log AP::logger().WriteStreaming("AROT", - "TimeUS,P,hs_e,C_Out,FFCol,SpdF,DH,p,ff,AccZ,DesV,Rfnd,Hest", + "TimeUS,hsp,hse,co,cff,sf,dsf,vp,vff,az,dvz,rfnd,h", "Qffffffffffff", AP_HAL::micros64(), - (double)_p_term_hs, - (double)_head_speed_error, - (double)_collective_out, - (double)_ff_term_hs, - (double)(_speed_forward*0.01f), - (double)(_cmd_vel*0.01f), - (double)_vel_p, - (double)_vel_ff, - (double)_avg_acc_z, - (double)_desired_sink_rate, - (double)(_radar_alt*0.01f), - (double)(_est_alt*0.01f)) ; + _p_term_hs, + _head_speed_error, + _collective_out, + _ff_term_hs, + (_speed_forward*0.01f), + (_cmd_vel*0.01f), + _vel_p, + _vel_ff, + _avg_acc_z, + _desired_sink_rate, + (_radar_alt*0.01f), + (_est_alt*0.01f)); } #endif // HAL_LOGGING_ENABLED From aa3d54a1219c21b842c8076dc1199c44994152ff Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 4 Jan 2024 17:59:57 +0000 Subject: [PATCH 17/61] AC_Autorotation: Consolidate controller inits --- libraries/AC_Autorotation/AC_Autorotation.cpp | 36 ++++++++----------- libraries/AC_Autorotation/AC_Autorotation.h | 6 ---- 2 files changed, 15 insertions(+), 27 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index e574e8822e731..21bf6588cddd7 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -198,12 +198,8 @@ void AC_Autorotation::init(AP_MotorsHeli* motors) { memset(_curr_acc_z, 0, sizeof(_curr_acc_z)); initial_flare_estimate(); -} - -// Initialisation of head speed controller -void AC_Autorotation::init_hs_controller() -{ + // Initialisation of head speed controller // Set initial collective position to be the collective position on initialisation _collective_out = _motors_heli->get_coll_mid(); @@ -218,8 +214,21 @@ void AC_Autorotation::init_hs_controller() _healthy_rpm_counter = 0; // Protect against divide by zero - _param_head_speed_set_point.set(MAX(_param_head_speed_set_point, 500)); + _param_head_speed_set_point.set(MAX(_param_head_speed_set_point, 500.0)); + + // Initialise forward speed controller + // Reset I term and acceleration target + _accel_target = 0.0; + + // Ensure parameter acceleration doesn't exceed hard-coded limit + _accel_max = MIN(_param_accel_max, 500.0); + + // Reset cmd vel and last accel to sensible values + _cmd_vel = calc_speed_forward(); //(cm/s) + _accel_out_last = _cmd_vel * _param_fwd_k_ff; + // initialize radar altitude estimator + init_est_radar_alt(); } @@ -451,21 +460,6 @@ void AC_Autorotation::Log_Write_Autorotation(void) const #endif // HAL_LOGGING_ENABLED -// Initialise forward speed controller -void AC_Autorotation::init_fwd_spd_controller(void) -{ - // Reset I term and acceleration target - _accel_target = 0.0f; - - // Ensure parameter acceleration doesn't exceed hard-coded limit - _accel_max = MIN(_param_accel_max, 500.0f); - - // Reset cmd vel and last accel to sensible values - _cmd_vel = calc_speed_forward(); //(cm/s) - _accel_out_last = _cmd_vel * _param_fwd_k_ff; -} - - // Sets time delta in seconds for all controllers void AC_Autorotation::set_dt(float delta_sec) { diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 7c8e0c2e9e4df..56a94f739572e 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -21,14 +21,8 @@ class AC_Autorotation // object initialisation void init(AP_MotorsHeli* motors); - // Initialise head speed controller - void init_hs_controller(void); - void initial_flare_estimate(void); - // Initialise forward speed controller - void init_fwd_spd_controller(void); - // Update head speed controller bool update_hs_glide_controller(void); From af175b962509f9f6ebee55957d97861ed261b9bc Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 4 Jan 2024 18:00:23 +0000 Subject: [PATCH 18/61] Copter: Autorotation: Consolidate controller inits --- ArduCopter/mode_autorotate.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 3c84a792e431e..3d76347333df4 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -40,14 +40,6 @@ bool ModeAutorotate::init(bool ignore_checks) // init autorotation controllers object g2.arot.init(motors); - // Initialise controllers - // This must be done before RPM value is fetched - g2.arot.init_hs_controller(); - g2.arot.init_fwd_spd_controller(); - - // initialize radar altitude estimator - g2.arot.init_est_radar_alt(); - // Retrieve rpm and start rpm sensor health checks _initial_rpm = g2.arot.get_rpm(true); From 4ce4e2c474ea8940136aef244978da05181f5d7c Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Sat, 20 Jan 2024 03:50:30 +0000 Subject: [PATCH 19/61] AC_Autorotation -Fix constants definition -Description of variables in header --- libraries/AC_Autorotation/AC_Autorotation.cpp | 26 ++++---- libraries/AC_Autorotation/AC_Autorotation.h | 60 +++++++++---------- 2 files changed, 43 insertions(+), 43 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 21bf6588cddd7..2e56bf231b401 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -355,21 +355,21 @@ void AC_Autorotation::initial_flare_estimate(void) { // Estimate hover thrust float _col_hover_rad = radians(_motors_heli->get_hover_coll_ang()); - float b = _param_solidity * 6.28f; - _disc_area = 3.14 * sq(_param_diameter * 0.5f); + float b = _param_solidity * M_2PI; + _disc_area = M_PI * sq(_param_diameter * 0.5f); float lambda = (-(b / 8.0f) + safe_sqrt((sq(b)) / 64.0f +((b / 3.0f) * _col_hover_rad))) * 0.5f; float freq = _motors_heli->get_rpm_setpoint() / 60.0f; - float tip_speed= freq * 6.28f * _param_diameter * 0.5f; - _lift_hover = ((1.225f * sq(tip_speed) * (_param_solidity * _disc_area)) * ((_col_hover_rad / 3.0f) - (lambda / 2.0f)) * 5.8f) * 0.5f; + float tip_speed= freq * M_2PI * _param_diameter * 0.5f; + _lift_hover = ((SSL_AIR_DENSITY * sq(tip_speed) * (_param_solidity * _disc_area)) * ((_col_hover_rad / 3.0f) - (lambda / 2.0f)) * 5.8f) * 0.5f; // Estimate rate of descent - float omega_auto = (_param_head_speed_set_point / 60.0f) * 6.28f; + float omega_auto = (_param_head_speed_set_point / 60.0f) * M_2PI; float tip_speed_auto = omega_auto * _param_diameter * 0.5f; float c_t = _lift_hover / (0.6125f * _disc_area * sq(tip_speed)); _est_rod = ((0.25f * (_param_solidity * 0.011f / c_t) * tip_speed_auto) + ((sq(c_t) / (_param_solidity * 0.011f)) * tip_speed_auto)); // Estimate rotor C_d - _c = (_lift_hover / (sq(_est_rod) * 0.5f * 1.225f * _disc_area)) * 1.15f; + _c = (_lift_hover / (sq(_est_rod) * 0.5f * SSL_AIR_DENSITY * _disc_area)) * 1.15f; _c = constrain_float(_c, 0.7f, 1.4f); // Calc flare altitude @@ -384,7 +384,7 @@ void AC_Autorotation::initial_flare_estimate(void) _index_sink_rate = 0; memset(_curr_sink_deriv, 0, sizeof(_curr_sink_deriv)); - gcs().send_text(MAV_SEVERITY_INFO, "Ct/sigma=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/9.8065f, _flare_alt_calc*0.01f, _c); + gcs().send_text(MAV_SEVERITY_INFO, "Ct/sigma=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/GRAVITY_MSS, _flare_alt_calc*0.01f, _c); } @@ -392,7 +392,7 @@ void AC_Autorotation::calc_flare_alt(float sink_rate, float fwd_speed) { // Compute speed module and glide path angle during descent float speed_module = norm(sink_rate, fwd_speed); - float glide_angle = safe_asin(3.14 / 2 - (fwd_speed / speed_module)); + float glide_angle = safe_asin(M_PI / 2 - (fwd_speed / speed_module)); // Estimate inflow velocity at beginning of flare float inflow = - speed_module * sinf(glide_angle + radians(AP_ALPHA_TPP)); @@ -400,14 +400,14 @@ void AC_Autorotation::calc_flare_alt(float sink_rate, float fwd_speed) // Estimate flare duration float k_1 = fabsf((-sink_rate + 0.001f - safe_sqrt(_lift_hover / _c))/(-sink_rate + 0.001f + safe_sqrt(_lift_hover / _c))); float k_2 = fabsf((inflow - safe_sqrt(_lift_hover / _c)) / (inflow + safe_sqrt(_lift_hover / _c))); - float delta_t_flare = (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) - (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_2)); + float delta_t_flare = (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) - (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_2)); // Estimate flare delta altitude - float a = (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * delta_t_flare + (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)); + float a = (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * delta_t_flare + (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)); float x = 1 - expf(a); - float s = 1 - expf((2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * (0.5f * (_lift_hover/(9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1))); + float s = 1 - expf((2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * (0.5f * (_lift_hover/(GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1))); float d = safe_sqrt(_lift_hover / _c); - float flare_distance = ((2 * d / (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065)))) * (a - logf(fabsf(x)) - (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) + logf(fabsf(s)))) - d * delta_t_flare; + float flare_distance = ((2 * d / (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS)))) * (a - logf(fabsf(x)) - (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) + logf(fabsf(s)))) - d * delta_t_flare; float delta_h = -flare_distance * cosf(radians(AP_ALPHA_TPP)); // Estimate altitude to begin collective pull @@ -558,7 +558,7 @@ void AC_Autorotation::update_flare_alt(void) if ((_speed_forward >= 0.8f * _param_target_speed) && (delta_v_z <= 1) && (fabsf(_avg_sink_deriv)<=0.005)) { float vel_z = _inav.get_velocity_z_up_cms() * 0.01f; float spd_fwd = _speed_forward * 0.01f; - _c = (_lift_hover / (sq(vel_z) * 0.5f * 1.225f * _disc_area)) * 1.15f; + _c = (_lift_hover / (sq(vel_z) * 0.5f * SSL_AIR_DENSITY * _disc_area)) * 1.15f; _c = constrain_float(_c, 0.7f, 1.4f); calc_flare_alt(vel_z,spd_fwd); _flare_update_check = true; diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 56a94f739572e..f5aec3c3e2cdf 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -149,36 +149,36 @@ class AC_Autorotation float _vel_p; // Forward velocity P term. float _vel_ff; // Forward velocity Feed Forward term. float _accel_out; // Acceleration value used to calculate pitch target. - float _entry_sink_rate; - float _entry_alt; - float _radar_alt; - float _flare_entry_speed; - float _desired_speed; - float _time_to_ground; - float _desired_sink_rate; - float _ground_clearance; - float _est_alt; - float _descent_rate_filtered; - float _radar_alt_prev; - float _radar_alt_calc; - float _avg_acc_z; - float _acc_z_sum; - int16_t _index; - float _curr_acc_z[10]{}; - float _flare_alt_calc; - float _lift_hover; - float _c; - float _cushion_alt; - float _disc_area; - float _last_vertical_speed; - float _sink_deriv; - float _est_rod; - float _avg_sink_deriv; - float _avg_sink_deriv_sum; - int16_t _index_sink_rate; - float _curr_sink_deriv[20]{}; - bool _flare_complete; - bool _flare_update_check; + float _entry_sink_rate; // Descent rate at beginning of touvhdown collective pull + float _entry_alt; // Altitude at beginning of touchdown coll pull + float _radar_alt; // Altitude above ground (RF) + float _flare_entry_speed; // Traslational velocity at beginning of flare maneuver + float _desired_speed; // Desired traslational velocity during flare + float _time_to_ground; // Time to ground + float _desired_sink_rate; // Desired vertical velocity during touchdown + float _ground_clearance; // Sensor offset distance + float _est_alt; // Estimated altitude above ground + float _descent_rate_filtered; // Filtered vertical speed + float _radar_alt_prev; // Last cycle calculated altitude + float _radar_alt_calc; // Inertial calculated altitude + float _avg_acc_z; // Averaged vertical acceleration + float _acc_z_sum; // Sum of vertical acceleration samples + int16_t _index; // Index for vertical acceleration rolling average + float _curr_acc_z[10]{}; // Array for storing vertical acceleration samples + float _flare_alt_calc; // Calculated flare altitude + float _lift_hover; // Main rotor thrust in hover condition + float _c; // Main rotor drag coefficient + float _cushion_alt; // Altitude for touchdown collective pull + float _disc_area; // Main rotor disc area + float _last_vertical_speed; // Last cycle measured vertical speed + float _sink_deriv; // Derivative of sink rate + float _est_rod; // Estimated rate of descent (vertical autorotation) + float _avg_sink_deriv; // Averaged derivative of rate of descent + float _avg_sink_deriv_sum; // Sum of averaged sink rate derivative + int16_t _index_sink_rate; // Index for sink rate derivative rolling average + float _curr_sink_deriv[20]{}; // Array for storing sink rate derivatives + bool _flare_complete; // Flare completed + bool _flare_update_check; // Check for flare altitude updating LowPassFilterFloat _accel_target_filter; // acceleration target filter From 55492446445a4463423bc1d4420b28932d0b4084 Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Tue, 23 Jan 2024 02:42:42 +0000 Subject: [PATCH 20/61] AC_Autorotation: update name of altitude estimation function --- libraries/AC_Autorotation/AC_Autorotation.cpp | 6 +++--- libraries/AC_Autorotation/AC_Autorotation.h | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 2e56bf231b401..011c3fd188506 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -228,7 +228,7 @@ void AC_Autorotation::init(AP_MotorsHeli* motors) { _accel_out_last = _cmd_vel * _param_fwd_k_ff; // initialize radar altitude estimator - init_est_radar_alt(); + init_est_rangefinder_alt(); } @@ -671,7 +671,7 @@ void AC_Autorotation::time_to_ground(void) } -void AC_Autorotation::init_est_radar_alt(void) +void AC_Autorotation::init_est_rangefinder_alt(void) { // Set descent rate filter cutoff frequency descent_rate_lpf.set_cutoff_frequency(40.0f); @@ -686,7 +686,7 @@ void AC_Autorotation::init_est_radar_alt(void) } -void AC_Autorotation::update_est_radar_alt(void) +void AC_Autorotation::update_est_rangefinder_alt(void) { if (_using_rfnd) { // Continue calculating radar altitude based on the most recent update and descent rate diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index f5aec3c3e2cdf..99a571ea2a1d6 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -94,9 +94,9 @@ class AC_Autorotation void set_ground_clearance(float ground_clearance) { _ground_clearance = ground_clearance; } - void init_est_radar_alt(void); + void init_est_rangefinder_alt(void); - void update_est_radar_alt(void); + void update_est_rangefinder_alt(void); float get_est_alt(void) const { return _est_alt; } From 1aba9ad65df5d2faef6f4e06f81bf9b02e808cb3 Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Tue, 23 Jan 2024 02:59:15 +0000 Subject: [PATCH 21/61] AC_Autorotation: squared gravitational acc const --- libraries/AC_Autorotation/AC_Autorotation.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 011c3fd188506..94f469c83fc1d 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -403,11 +403,12 @@ void AC_Autorotation::calc_flare_alt(float sink_rate, float fwd_speed) float delta_t_flare = (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) - (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_2)); // Estimate flare delta altitude - float a = (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * delta_t_flare + (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)); + float sq_gravity = sq(GRAVITY_MSS); + float a = (2 * safe_sqrt(_c * sq_gravity / _lift_hover )) * delta_t_flare + (2 * safe_sqrt(_c * sq_gravity / _lift_hover )) * (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)); float x = 1 - expf(a); - float s = 1 - expf((2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * (0.5f * (_lift_hover/(GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1))); + float s = 1 - expf((2 * safe_sqrt(_c * sq_gravity / _lift_hover )) * (0.5f * (_lift_hover/(GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1))); float d = safe_sqrt(_lift_hover / _c); - float flare_distance = ((2 * d / (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS)))) * (a - logf(fabsf(x)) - (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) + logf(fabsf(s)))) - d * delta_t_flare; + float flare_distance = ((2 * d / (2 * safe_sqrt(_c * sq_gravity / _lift_hover ))) * (a - logf(fabsf(x)) - (2 * safe_sqrt(_c * sq_gravity / _lift_hover )) * (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) + logf(fabsf(s)))) - d * delta_t_flare; float delta_h = -flare_distance * cosf(radians(AP_ALPHA_TPP)); // Estimate altitude to begin collective pull From b61eee31ff461cab2bb18173b629c6539bec68ff Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Tue, 23 Jan 2024 03:03:56 +0000 Subject: [PATCH 22/61] AC_Autorotation: remove array zeroing of sink rate derivative --- libraries/AC_Autorotation/AC_Autorotation.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 99a571ea2a1d6..30ba4fb0dbec2 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -176,7 +176,7 @@ class AC_Autorotation float _avg_sink_deriv; // Averaged derivative of rate of descent float _avg_sink_deriv_sum; // Sum of averaged sink rate derivative int16_t _index_sink_rate; // Index for sink rate derivative rolling average - float _curr_sink_deriv[20]{}; // Array for storing sink rate derivatives + float _curr_sink_deriv[20]; // Array for storing sink rate derivatives bool _flare_complete; // Flare completed bool _flare_update_check; // Check for flare altitude updating From b2fe6a31ed2abe08f3275688551442848aa2a4b6 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Fri, 26 Jan 2024 21:14:00 -0500 Subject: [PATCH 23/61] Copter: move time_to_impact to local float --- ArduCopter/mode.h | 1 - ArduCopter/mode_autorotate.cpp | 3 ++- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index bd33cdd298105..f3a00d076bae1 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1938,7 +1938,6 @@ class ModeAutorotate : public Mode { bool hover_autorotation; bool initial_energy_check; float last_tti; - float time_to_impact; enum class Autorotation_Phase { ENTRY, diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 3d76347333df4..cd1e08d1be21f 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -91,7 +91,7 @@ void ModeAutorotate::run() float alt = g2.arot.get_ground_distance(); // have autorotation library update estimated radar altitude - g2.arot.update_est_radar_alt(); + g2.arot.update_est_rangefinder_alt(); if (alt < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) { g2.arot._using_rfnd = true; } else { @@ -101,6 +101,7 @@ void ModeAutorotate::run() // Initialise internal variables float curr_vel_z = inertial_nav.get_velocity_z_up_cms(); // Current vertical descent //calculate time to impact + float time_to_impact; if (phase_switch != Autorotation_Phase::TOUCH_DOWN) { g2.arot.time_to_ground(); time_to_impact = g2.arot.get_time_to_ground(); From ee2dff79434e520e2d63085f6ee6bcb78460b69a Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 25 Jan 2024 11:13:44 +0000 Subject: [PATCH 24/61] Copter: Autorotation mode: simplify time_to_impact --- ArduCopter/mode.h | 2 +- ArduCopter/mode_autorotate.cpp | 8 ++------ 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index f3a00d076bae1..c35e841c7d87f 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1937,7 +1937,7 @@ class ModeAutorotate : public Mode { uint32_t _touchdown_time_ms; bool hover_autorotation; bool initial_energy_check; - float last_tti; + float time_to_impact; enum class Autorotation_Phase { ENTRY, diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index cd1e08d1be21f..9498d9c0666fc 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -100,14 +100,10 @@ void ModeAutorotate::run() // Initialise internal variables float curr_vel_z = inertial_nav.get_velocity_z_up_cms(); // Current vertical descent - //calculate time to impact - float time_to_impact; + + // Update time to impact only if we are not in the touch down phase. This must be updated before the state machine. if (phase_switch != Autorotation_Phase::TOUCH_DOWN) { - g2.arot.time_to_ground(); time_to_impact = g2.arot.get_time_to_ground(); - last_tti=time_to_impact; - } else { - time_to_impact = last_tti; } From 71c92afc813a6194da6575b6029602105c448e3d Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 25 Jan 2024 11:16:43 +0000 Subject: [PATCH 25/61] AC_Autorotation: Tidy up time to impact calculation --- libraries/AC_Autorotation/AC_Autorotation.cpp | 10 ++++++---- libraries/AC_Autorotation/AC_Autorotation.h | 5 +---- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 94f469c83fc1d..b964612695fdb 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -662,13 +662,15 @@ void AC_Autorotation::get_entry_speed(void) } -void AC_Autorotation::time_to_ground(void) +float AC_Autorotation::get_time_to_ground(void) { + // Take the default position that time to ground is the touch down time + 1 s. If we are descending + // then we can calc an appropriate value. + float time_to_ground = _t_tch + 1.0f; if (_inav.get_velocity_z_up_cms() < 0.0f ) { - _time_to_ground = (-_radar_alt / _inav.get_velocity_z_up_cms()); - } else { - _time_to_ground = _t_tch + 1.0f; + time_to_ground = (-_radar_alt / _inav.get_velocity_z_up_cms()); } + return time_to_ground; } diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 30ba4fb0dbec2..30a5d77f54f60 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -84,9 +84,7 @@ class AC_Autorotation float get_ground_distance(void) const { return _radar_alt; } - float get_time_to_ground(void) const { return _time_to_ground; } - - void time_to_ground(void); + float get_time_to_ground(void); void set_entry_sink_rate (float sink_rate) { _entry_sink_rate = sink_rate; } @@ -154,7 +152,6 @@ class AC_Autorotation float _radar_alt; // Altitude above ground (RF) float _flare_entry_speed; // Traslational velocity at beginning of flare maneuver float _desired_speed; // Desired traslational velocity during flare - float _time_to_ground; // Time to ground float _desired_sink_rate; // Desired vertical velocity during touchdown float _ground_clearance; // Sensor offset distance float _est_alt; // Estimated altitude above ground From 651e3f723239153af7d7a81faaec5803ac59a426 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 25 Jan 2024 17:21:24 +0000 Subject: [PATCH 26/61] AC_Autorotation: Add helper functions to move phase init functionaility from mode down into library --- libraries/AC_Autorotation/AC_Autorotation.cpp | 56 ++++++++++++++++--- libraries/AC_Autorotation/AC_Autorotation.h | 30 +++++----- 2 files changed, 62 insertions(+), 24 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index b964612695fdb..cea679fecc490 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -138,7 +138,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.2 0.8 // @Increment: 0.01 // @User: Advanced - AP_GROUPINFO("COL_FILT_C", 13, AC_Autorotation, _param_col_cushion_cutoff_freq, HS_CONTROLLER_CUSHION_COL_FILTER), + AP_GROUPINFO("COL_FILT_C", 13, AC_Autorotation, _param_col_touchdown_cutoff_freq, HS_CONTROLLER_CUSHION_COL_FILTER), // @Param: ROT_SOL // @DisplayName: rotor solidity @@ -182,7 +182,7 @@ AC_Autorotation::AC_Autorotation(AP_InertialNav& inav, AP_AHRS& ahrs) : } -void AC_Autorotation::init(AP_MotorsHeli* motors) { +void AC_Autorotation::init(AP_MotorsHeli* motors, float gnd_clear) { _motors_heli = motors; if (_motors_heli == nullptr) { @@ -227,11 +227,57 @@ void AC_Autorotation::init(AP_MotorsHeli* motors) { _cmd_vel = calc_speed_forward(); //(cm/s) _accel_out_last = _cmd_vel * _param_fwd_k_ff; + // Store the ground clearance on the lidar sensor for use in touch down calculations + _ground_clearance = gnd_clear; + // initialize radar altitude estimator init_est_rangefinder_alt(); } +void AC_Autorotation::init_entry(void) +{ + // Set following trim low pass cut off frequency + _col_cutoff_freq = _param_col_entry_cutoff_freq.get(); + + // Set desired forward speed target + _vel_target = _param_target_speed.get(); +} + +void AC_Autorotation::init_glide(float hs_targ) +{ + // Set following trim low pass cut off frequency + _col_cutoff_freq = _param_col_glide_cutoff_freq.get(); + + // Ensure desired forward speed target is set to param value + _vel_target = _param_target_speed.get(); + + // Update head speed target + _target_head_speed = hs_targ; +} + +void AC_Autorotation::init_flare(float hs_targ) +{ + // Ensure following trim low pass cut off frequency + _col_cutoff_freq = _param_col_glide_cutoff_freq.get(); + + _flare_entry_speed = calc_speed_forward(); + + // Update head speed target + _target_head_speed = hs_targ; +} + +void AC_Autorotation::init_touchdown(void) +{ + // Set following trim low pass cut off frequency + _col_cutoff_freq = _param_col_touchdown_cutoff_freq.get(); + + // store the descent speed and height at the start of the touch down + _entry_sink_rate = _inav.get_velocity_z_up_cms(); + _entry_alt = _radar_alt; +} + + // Rotor Speed controller for entry, glide and flare phases of autorotation bool AC_Autorotation::update_hs_glide_controller(void) { @@ -656,12 +702,6 @@ void AC_Autorotation::touchdown_controller(void) } -void AC_Autorotation::get_entry_speed(void) -{ - _flare_entry_speed = calc_speed_forward(); -} - - float AC_Autorotation::get_time_to_ground(void) { // Take the default position that time to ground is the touch down time + 1 s. If we are descending diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 30a5d77f54f60..9ee6ee7755cb9 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -19,7 +19,19 @@ class AC_Autorotation AC_Autorotation(AP_InertialNav& inav, AP_AHRS& ahrs); // object initialisation - void init(AP_MotorsHeli* motors); + void init(AP_MotorsHeli* motors, float gnd_clear); + + // Helper to set all necessary variables needed for the entry phase + void init_entry(void); + + // Helper to set all necessary variables needed for the glide phase + void init_glide(float hs_targ); + + // Helper to set all necessary variables needed for the flare phase + void init_flare(float hs_targ); + + // Helper to set all necessary variables needed for the touchdown phase + void init_touchdown(void); void initial_flare_estimate(void); @@ -40,12 +52,6 @@ class AC_Autorotation int16_t get_hs_set_point(void) { return _param_head_speed_set_point; } - float get_col_entry_freq(void) { return _param_col_entry_cutoff_freq; } - - float get_col_glide_freq(void) { return _param_col_glide_cutoff_freq; } - - float get_col_cushion_freq(void) { return _param_col_cushion_cutoff_freq; } - float get_bail_time(void) { return _param_bail_time; } float get_last_collective() const { return _collective_out; } @@ -57,12 +63,6 @@ class AC_Autorotation // Update forward speed controller void update_forward_speed_controller(void); - // Overloaded: Set desired speed for forward controller to parameter value - void set_desired_fwd_speed(void) { _vel_target = _param_target_speed; } - - // Overloaded: Set desired speed to argument value - void set_desired_fwd_speed(float speed) { _vel_target = speed; } - // Get pitch target int32_t get_pitch(void) const { return _pitch_target; } @@ -90,8 +90,6 @@ class AC_Autorotation void set_entry_alt (float entry_alt) { _entry_alt = entry_alt; } - void set_ground_clearance(float ground_clearance) { _ground_clearance = ground_clearance; } - void init_est_rangefinder_alt(void); void update_est_rangefinder_alt(void); @@ -186,7 +184,7 @@ class AC_Autorotation AC_P _p_coll_tch; AP_Float _param_col_entry_cutoff_freq; AP_Float _param_col_glide_cutoff_freq; - AP_Float _param_col_cushion_cutoff_freq; + AP_Float _param_col_touchdown_cutoff_freq; AP_Int16 _param_accel_max; AP_Float _param_bail_time; AP_Int8 _param_rpm_instance; From 9aece0554eb77db8656b34e3074409f343b2559b Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 25 Jan 2024 17:23:36 +0000 Subject: [PATCH 27/61] Copter: autorotation mode: Move phase init functions down into autorotation obj to simplify code --- ArduCopter/mode_autorotate.cpp | 40 +++++++++++----------------------- 1 file changed, 13 insertions(+), 27 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 9498d9c0666fc..8bcba82766e58 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -38,7 +38,7 @@ bool ModeAutorotate::init(bool ignore_checks) } // init autorotation controllers object - g2.arot.init(motors); + g2.arot.init(motors, copter.rangefinder.ground_clearance_cm_orient(ROTATION_PITCH_270)); // Retrieve rpm and start rpm sensor health checks _initial_rpm = g2.arot.get_rpm(true); @@ -151,15 +151,12 @@ void ModeAutorotate::run() gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase"); - // Set following trim low pass cut off frequency - g2.arot.set_col_cutoff_freq(g2.arot.get_col_entry_freq()); + // Set necessary variables for the entry phase in the autorotation obj + g2.arot.init_entry(); // Target head speed is set to rpm at initiation to prevent unwanted changes in attitude _target_head_speed = _initial_rpm/g2.arot.get_hs_set_point(); - // Set desired forward speed target - g2.arot.set_desired_fwd_speed(); - // Prevent running the initial entry functions again _flags.entry_initial = false; @@ -183,9 +180,7 @@ void ModeAutorotate::run() _flags.bad_rpm = g2.arot.update_hs_glide_controller(); //run head speed/ collective controller } else { _pitch_target = 0.0f; - g2.arot.update_hover_autorotation_controller(); //run head speed/ collective controller - g2.arot.set_entry_sink_rate(curr_vel_z); - g2.arot.set_entry_alt(g2.arot.get_ground_distance()); + g2.arot.update_hover_autorotation_controller(); } break; @@ -197,15 +192,11 @@ void ModeAutorotate::run() gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase"); - // Set following trim low pass cut off frequency - g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq()); + // Ensure target hs is set to glide in case hs hasn't reached target for glide + _target_head_speed = HEAD_SPEED_TARGET_RATIO; - // Set desired forward speed target - g2.arot.set_desired_fwd_speed(); - - // Set target head speed in head speed controller - _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide in case hs hasn't reached target for glide - g2.arot.set_target_head_speed(_target_head_speed); + // Set necessary variables for the glide phase in the autorotation obj + g2.arot.init_glide(_target_head_speed); // Prevent running the initial glide functions again _flags.ss_glide_initial = false; @@ -236,13 +227,11 @@ void ModeAutorotate::run() if (_flags.flare_initial == true) { gcs().send_text(MAV_SEVERITY_INFO, "Flare_Phase"); - // Set following trim low pass cut off frequency - g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq()); - - // Set target head speed in head speed controller + // Ensure target head speed is set in head speed controller _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide incase hs has not reached target for glide - g2.arot.set_target_head_speed(_target_head_speed); - g2.arot.get_entry_speed(); + + g2.arot.init_flare(_target_head_speed); + // Prevent running the initial flare functions again _flags.flare_initial = false; } @@ -268,10 +257,7 @@ void ModeAutorotate::run() // Prevent running the initial touchdown functions again _flags.touch_down_initial = false; _touchdown_time_ms = millis(); - g2.arot.set_col_cutoff_freq(g2.arot.get_col_cushion_freq()); - g2.arot.set_entry_sink_rate(curr_vel_z); - g2.arot.set_entry_alt(g2.arot.get_ground_distance()); - g2.arot.set_ground_clearance(copter.rangefinder.ground_clearance_cm_orient(ROTATION_PITCH_270)); + g2.arot.init_touchdown(); } g2.arot.touchdown_controller(); _pitch_target = g2.arot.get_pitch(); From e314f5c6885003006d92c395c7a9ce16383284de Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 25 Jan 2024 17:24:10 +0000 Subject: [PATCH 28/61] Copter: autorotation mode: remove unused variables --- ArduCopter/mode.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index c35e841c7d87f..1a4cd881e65ba 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1945,12 +1945,6 @@ class ModeAutorotate : public Mode { FLARE, TOUCH_DOWN, BAIL_OUT } phase_switch; - - enum class Navigation_Decision { - USER_CONTROL_STABILISED, - STRAIGHT_AHEAD, - INTO_WIND, - NEAREST_RALLY} nav_pos_switch; // --- Internal flags --- struct controller_flags { From e3fa52d3404adc4f5cc1eff9a778afcab648217d Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 25 Jan 2024 17:24:46 +0000 Subject: [PATCH 29/61] Copter: autorotation mode: simplify hover autorotation check --- ArduCopter/mode_autorotate.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 8bcba82766e58..9a9ee2a05c5b5 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -111,14 +111,10 @@ void ModeAutorotate::run() // State machine logic //---------------------------------------------------------------- + // initial check for total energy monitoring if (initial_energy_check) { - //initial check for total energy monitoring - if ( inertial_nav.get_speed_xy_cms() < 250.0f && g2.arot.get_ground_distance() < g2.arot.get_flare_alt()) { - hover_autorotation = true; - } else { - hover_autorotation = false; - } - initial_energy_check=false; + hover_autorotation = (inertial_nav.get_speed_xy_cms() < 250.0f) && (g2.arot.get_ground_distance() < g2.arot.get_flare_alt()); + initial_energy_check = false; } //total energy check From 26659addd6dab582941c4c3e045b1c247ce980c5 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Sun, 28 Jan 2024 08:23:08 +0000 Subject: [PATCH 30/61] Copter: Autorotation Mode: rename get_flare_status to is_flare_complete to improve readability --- ArduCopter/mode_autorotate.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 9a9ee2a05c5b5..7b1e68a052a87 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -128,7 +128,7 @@ void ModeAutorotate::run() // Flight phase can be progressed to steady state glide phase_switch = Autorotation_Phase::SS_GLIDE; } - } else if ( g2.arot.get_est_alt()<=g2.arot.get_flare_alt() && g2.arot.get_est_alt()>g2.arot.get_cushion_alt() && !g2.arot.get_flare_status() ) { + } else if ( g2.arot.get_est_alt()<=g2.arot.get_flare_alt() && g2.arot.get_est_alt()>g2.arot.get_cushion_alt() && !g2.arot.is_flare_complete() ) { phase_switch = Autorotation_Phase::FLARE; } else if (time_to_impact <= g2.arot.get_t_touchdown() && _flags.flare_initial == false ) { phase_switch = Autorotation_Phase::TOUCH_DOWN; From 93ddcc4b907caf264e23c78e5aaffb616cd5b556 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Sun, 28 Jan 2024 08:24:36 +0000 Subject: [PATCH 31/61] AC_Autorotation: Rename get_flare_status to is_flare_complete to improve readability --- libraries/AC_Autorotation/AC_Autorotation.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 9ee6ee7755cb9..b17dd9ef9c0e3 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -110,7 +110,7 @@ class AC_Autorotation float get_cushion_alt(void) const { return _cushion_alt; } - bool get_flare_status(void) { return _flare_complete; } + bool is_flare_complete(void) { return _flare_complete; } void calc_sink_d_avg(void); From e153490eba43655be7342ae4ca91de91d5090c8a Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Feb 2024 09:53:13 +0000 Subject: [PATCH 32/61] AC_Autorotation: Add flare height helper --- libraries/AC_Autorotation/AC_Autorotation.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index b17dd9ef9c0e3..ea41d9c332c04 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -96,6 +96,8 @@ class AC_Autorotation float get_est_alt(void) const { return _est_alt; } + bool above_flare_height(void) const { return _est_alt > _flare_alt_calc; } + void update_hover_autorotation_controller(); void update_avg_acc_z(void); From ea61936739144f937e16609a7df432688200e190 Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Mon, 29 Jan 2024 03:36:38 +0000 Subject: [PATCH 33/61] Copter: remove land detector in mode autorotate and fix for disarm delay --- ArduCopter/mode_autorotate.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 7b1e68a052a87..b0c87f084ade5 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -257,11 +257,8 @@ void ModeAutorotate::run() } g2.arot.touchdown_controller(); _pitch_target = g2.arot.get_pitch(); - - if (fabsf(inertial_nav.get_velocity_z_up_cms()) < 10) { - copter.ap.land_complete = true; - } - if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE && ((now - _touchdown_time_ms)/1000.0f > TOUCHDOWN_TIME )) { + uint32_t disarm_delay_ms = 1000*g.disarm_delay; + if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE && ((now - _touchdown_time_ms) > disarm_delay_ms )) { copter.arming.disarm(AP_Arming::Method::LANDED); } From afff5a1a0ab84e1d6e92add741bd80d55eb27c0d Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Mon, 29 Jan 2024 03:37:49 +0000 Subject: [PATCH 34/61] AC_Autorotation: add touchdown complete check and subsequent lowering of the collective --- ArduCopter/mode_autorotate.cpp | 6 ++---- libraries/AC_Autorotation/AC_Autorotation.cpp | 13 ++++++++++++- libraries/AC_Autorotation/AC_Autorotation.h | 2 ++ 3 files changed, 16 insertions(+), 5 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index b0c87f084ade5..66da2b1496d29 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -255,12 +255,10 @@ void ModeAutorotate::run() _touchdown_time_ms = millis(); g2.arot.init_touchdown(); } + // Run touchdown controller g2.arot.touchdown_controller(); + // Retrieve pitch target _pitch_target = g2.arot.get_pitch(); - uint32_t disarm_delay_ms = 1000*g.disarm_delay; - if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE && ((now - _touchdown_time_ms) > disarm_delay_ms )) { - copter.arming.disarm(AP_Arming::Method::LANDED); - } break; } diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index cea679fecc490..bff68aacf61fc 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -26,6 +26,7 @@ // flare controller default definitions #define AP_ALPHA_TPP 20.0f #define AP_T_TO_G 0.55f +#define MIN_TIME_ON_GROUND 3000.0f const AP_Param::GroupInfo AC_Autorotation::var_info[] = { @@ -422,9 +423,10 @@ void AC_Autorotation::initial_flare_estimate(void) float des_spd_fwd = _param_target_speed * 0.01f; calc_flare_alt(-_est_rod, des_spd_fwd); - // Initialize sink rate monitor and flare bools + // Initialize sink rate monitor and flare/touchdown bools _flare_complete = false; _flare_update_check = false; + _touchdown_complete = false; _avg_sink_deriv = 0.0f; _avg_sink_deriv_sum = 0.0f; _index_sink_rate = 0; @@ -697,6 +699,15 @@ void AC_Autorotation::touchdown_controller(void) _collective_out = constrain_value((_p_coll_tch.get_p(_desired_sink_rate - _current_sink_rate))*0.01f + _ff_term_hs, 0.0f, 1.0f); col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); _ff_term_hs = col_trim_lpf.apply(_collective_out, _dt); + if(_current_sink_rate < 10.0 && _radar_alt<=_ground_clearance && !_touchdown_complete){ + _time_on_ground = AP_HAL::millis(); + _touchdown_complete = true; + } + float now = AP_HAL::millis(); + if((now - _time_on_ground) > MIN_TIME_ON_GROUND && _touchdown_complete){ + //on ground, collective can be bottomed now + _collective_out = 0; + } set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); _pitch_target *= 0.95f; } diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index ea41d9c332c04..4e14f227f67b4 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -176,6 +176,8 @@ class AC_Autorotation float _curr_sink_deriv[20]; // Array for storing sink rate derivatives bool _flare_complete; // Flare completed bool _flare_update_check; // Check for flare altitude updating + float _time_on_ground; // Time elapsed after touch down + bool _touchdown_complete; // Touchdown completed check LowPassFilterFloat _accel_target_filter; // acceleration target filter From 0887aff250a3bdb803cdf3ff907bcffe7588b9d6 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Feb 2024 10:51:42 +0000 Subject: [PATCH 35/61] AC_Autorotation: Remove touch down complete bool and just use the timer. Also make the lowering of the collective to zero thrust when on ground and do it smoothly. --- libraries/AC_Autorotation/AC_Autorotation.cpp | 33 ++++++++++++------- libraries/AC_Autorotation/AC_Autorotation.h | 1 - 2 files changed, 22 insertions(+), 12 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index bff68aacf61fc..e67709bb546e7 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -231,6 +231,9 @@ void AC_Autorotation::init(AP_MotorsHeli* motors, float gnd_clear) { // Store the ground clearance on the lidar sensor for use in touch down calculations _ground_clearance = gnd_clear; + // reset on ground timer + _time_on_ground = 0; + // initialize radar altitude estimator init_est_rangefinder_alt(); } @@ -423,10 +426,9 @@ void AC_Autorotation::initial_flare_estimate(void) float des_spd_fwd = _param_target_speed * 0.01f; calc_flare_alt(-_est_rod, des_spd_fwd); - // Initialize sink rate monitor and flare/touchdown bools + // Initialize sink rate monitor and flare bools _flare_complete = false; _flare_update_check = false; - _touchdown_complete = false; _avg_sink_deriv = 0.0f; _avg_sink_deriv_sum = 0.0f; _index_sink_rate = 0; @@ -696,19 +698,28 @@ void AC_Autorotation::touchdown_controller(void) // Update forward speed for logging _speed_forward = calc_speed_forward(); // (cm/s) - _collective_out = constrain_value((_p_coll_tch.get_p(_desired_sink_rate - _current_sink_rate))*0.01f + _ff_term_hs, 0.0f, 1.0f); - col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); - _ff_term_hs = col_trim_lpf.apply(_collective_out, _dt); - if(_current_sink_rate < 10.0 && _radar_alt<=_ground_clearance && !_touchdown_complete){ + // Check to see if we think the aircraft is on the ground + if(_current_sink_rate < 10.0 && _radar_alt<=_ground_clearance && _time_on_ground == 0){ _time_on_ground = AP_HAL::millis(); - _touchdown_complete = true; } - float now = AP_HAL::millis(); - if((now - _time_on_ground) > MIN_TIME_ON_GROUND && _touchdown_complete){ - //on ground, collective can be bottomed now - _collective_out = 0; + + // Use a timer to get confidence that the aircraft is on the ground. + // Note: The landing detector uses the zero thust collective as an indicator for being on the ground. The touch down controller will have + // driven the collective high to cushion the landing so the landing detector will not trip until we drive the collective back to zero thrust and below. + if ((_time_on_ground > 0) && ((AP_HAL::millis() - _time_on_ground) > MIN_TIME_ON_GROUND)) { + // On ground, smoothly lower collective to just bellow zero thrust, to make sure we trip the landing detector + float desired_col = _motors_heli->get_coll_mid() * 0.95; + _collective_out = _collective_out*0.9 + desired_col*0.1; + + } else { + _collective_out = constrain_value((_p_coll_tch.get_p(_desired_sink_rate - _current_sink_rate))*0.01f + _ff_term_hs, 0.0f, 1.0f); + col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); + _ff_term_hs = col_trim_lpf.apply(_collective_out, _dt); } + set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); + + // Smoothly scale the pitch target back to zero (level) _pitch_target *= 0.95f; } diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 4e14f227f67b4..472d0c658908e 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -177,7 +177,6 @@ class AC_Autorotation bool _flare_complete; // Flare completed bool _flare_update_check; // Check for flare altitude updating float _time_on_ground; // Time elapsed after touch down - bool _touchdown_complete; // Touchdown completed check LowPassFilterFloat _accel_target_filter; // acceleration target filter From ee06b79bd91e28d881a9c2fa81a119db275c031c Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Feb 2024 15:43:56 +0000 Subject: [PATCH 36/61] AC_Autorotation: Let copter do all of the height above ground estimation, change all measurements to height above ground to be the same variable. --- libraries/AC_Autorotation/AC_Autorotation.cpp | 77 +++---------------- libraries/AC_Autorotation/AC_Autorotation.h | 23 +----- 2 files changed, 14 insertions(+), 86 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index e67709bb546e7..22d0bcf652095 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -190,8 +190,6 @@ void AC_Autorotation::init(AP_MotorsHeli* motors, float gnd_clear) { AP_HAL::panic("AROT: _motors_heli is nullptr"); } - _using_rfnd = false; - // Reset z acceleration average variables _avg_acc_z = 0.0f; _acc_z_sum = 0.0f; @@ -233,9 +231,6 @@ void AC_Autorotation::init(AP_MotorsHeli* motors, float gnd_clear) { // reset on ground timer _time_on_ground = 0; - - // initialize radar altitude estimator - init_est_rangefinder_alt(); } @@ -278,7 +273,7 @@ void AC_Autorotation::init_touchdown(void) // store the descent speed and height at the start of the touch down _entry_sink_rate = _inav.get_velocity_z_up_cms(); - _entry_alt = _radar_alt; + _entry_alt = _gnd_hgt; } @@ -487,13 +482,12 @@ void AC_Autorotation::Log_Write_Autorotation(void) const // @Field: vff: ff-term of velocity response // @Field: az: average z acceleration // @Field: dvz: Desired Sink Rate - // @Field: rfnd: rangefinder altitude - // @Field: h: estimated altitude + // @Field: h: height above ground //Write to data flash log AP::logger().WriteStreaming("AROT", - "TimeUS,hsp,hse,co,cff,sf,dsf,vp,vff,az,dvz,rfnd,h", - "Qffffffffffff", + "TimeUS,hsp,hse,co,cff,sf,dsf,vp,vff,az,dvz,h", + "Qfffffffffff", AP_HAL::micros64(), _p_term_hs, _head_speed_error, @@ -505,8 +499,7 @@ void AC_Autorotation::Log_Write_Autorotation(void) const _vel_ff, _avg_acc_z, _desired_sink_rate, - (_radar_alt*0.01f), - (_est_alt*0.01f)); + (_gnd_hgt*0.01f)); } #endif // HAL_LOGGING_ENABLED @@ -574,7 +567,7 @@ void AC_Autorotation::update_forward_speed_controller(void) } _accel_out_last = _accel_out; - if (_est_alt >= _flare_alt_calc * 1.25f) { + if (_gnd_hgt >= _flare_alt_calc * 1.25f) { _pitch_target = accel_to_angle(-_accel_out * 0.01) * 100; } else { _pitch_target = 0.0f; @@ -636,7 +629,7 @@ void AC_Autorotation::flare_controller(void) _speed_forward = calc_speed_forward(); // (cm/s) _delta_speed_fwd = _speed_forward - _speed_forward_last; // (cm/s) _speed_forward_last = _speed_forward; // (cm/s) - _desired_speed = linear_interpolate(0.0f, _flare_entry_speed, _est_alt, _cushion_alt, _flare_alt_calc); + _desired_speed = linear_interpolate(0.0f, _flare_entry_speed, _gnd_hgt, _cushion_alt, _flare_alt_calc); // Get p _vel_p = _p_fw_vel.get_p(_desired_speed - _speed_forward); @@ -689,8 +682,8 @@ void AC_Autorotation::flare_controller(void) void AC_Autorotation::touchdown_controller(void) { float _current_sink_rate = _inav.get_velocity_z_up_cms(); - if (_radar_alt >= _ground_clearance) { - _desired_sink_rate = linear_interpolate(0.0f, _entry_sink_rate, _radar_alt, _ground_clearance, _entry_alt); + if (_gnd_hgt >= _ground_clearance) { + _desired_sink_rate = linear_interpolate(0.0f, _entry_sink_rate, _gnd_hgt, _ground_clearance, _entry_alt); } else { _desired_sink_rate = 0.0f; } @@ -699,7 +692,7 @@ void AC_Autorotation::touchdown_controller(void) _speed_forward = calc_speed_forward(); // (cm/s) // Check to see if we think the aircraft is on the ground - if(_current_sink_rate < 10.0 && _radar_alt<=_ground_clearance && _time_on_ground == 0){ + if(_current_sink_rate < 10.0 && _gnd_hgt <= _ground_clearance && _time_on_ground == 0){ _time_on_ground = AP_HAL::millis(); } @@ -730,60 +723,12 @@ float AC_Autorotation::get_time_to_ground(void) // then we can calc an appropriate value. float time_to_ground = _t_tch + 1.0f; if (_inav.get_velocity_z_up_cms() < 0.0f ) { - time_to_ground = (-_radar_alt / _inav.get_velocity_z_up_cms()); + time_to_ground = (-_gnd_hgt / _inav.get_velocity_z_up_cms()); } return time_to_ground; } -void AC_Autorotation::init_est_rangefinder_alt(void) -{ - // Set descent rate filter cutoff frequency - descent_rate_lpf.set_cutoff_frequency(40.0f); - - // Reset feed descent rate filter - descent_rate_lpf.reset(_inav.get_velocity_z_up_cms()); - - _radar_alt_calc = _radar_alt; - _radar_alt_prev = _radar_alt; - _est_alt = _radar_alt; - -} - - -void AC_Autorotation::update_est_rangefinder_alt(void) -{ - if (_using_rfnd) { - // Continue calculating radar altitude based on the most recent update and descent rate - if (is_equal(_radar_alt, _radar_alt_prev)) { - _radar_alt_calc += (_inav.get_velocity_z_up_cms() * _dt); - } else { - _radar_alt_calc = _radar_alt; - _radar_alt_prev = _radar_alt; - } - - // Determine the error between a calculated radar altitude based on each update at 20 hz and the estimated update - float alt_error = _radar_alt_calc - _est_alt; - // Drive the estimated altitude to the actual altitude with a proportional altitude error feedback - float descent_rate_corr = _inav.get_velocity_z_up_cms() + alt_error * 2.0f; - // Update descent rate filter - _descent_rate_filtered = descent_rate_lpf.apply(descent_rate_corr); - _est_alt += (_descent_rate_filtered * _dt); - - } else { - _est_alt = _radar_alt; - - // Reset feed descent rate filter - descent_rate_lpf.reset(_inav.get_velocity_z_up_cms()); - - // Reset variables until using rangefinder - _radar_alt_calc = _radar_alt; - _radar_alt_prev = _radar_alt; - _est_alt = _radar_alt; - } -} - - void AC_Autorotation::update_avg_acc_z(void) { // Wrap index diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 472d0c658908e..0e5a32d3660ef 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -78,25 +78,17 @@ class AC_Autorotation // Update the touchdown controller void touchdown_controller(void); - void set_ground_distance(float radalt) { _radar_alt = radalt; } + void set_ground_distance(float gnd_dist) { _gnd_hgt = (float)gnd_dist; } void get_entry_speed(void); - float get_ground_distance(void) const { return _radar_alt; } - float get_time_to_ground(void); void set_entry_sink_rate (float sink_rate) { _entry_sink_rate = sink_rate; } void set_entry_alt (float entry_alt) { _entry_alt = entry_alt; } - void init_est_rangefinder_alt(void); - - void update_est_rangefinder_alt(void); - - float get_est_alt(void) const { return _est_alt; } - - bool above_flare_height(void) const { return _est_alt > _flare_alt_calc; } + bool above_flare_height(void) const { return _gnd_hgt > _flare_alt_calc; } void update_hover_autorotation_controller(); @@ -119,8 +111,6 @@ class AC_Autorotation // User Settable Parameters static const struct AP_Param::GroupInfo var_info[]; - bool _using_rfnd; - private: //--------Internal Variables-------- @@ -149,15 +139,11 @@ class AC_Autorotation float _accel_out; // Acceleration value used to calculate pitch target. float _entry_sink_rate; // Descent rate at beginning of touvhdown collective pull float _entry_alt; // Altitude at beginning of touchdown coll pull - float _radar_alt; // Altitude above ground (RF) float _flare_entry_speed; // Traslational velocity at beginning of flare maneuver float _desired_speed; // Desired traslational velocity during flare float _desired_sink_rate; // Desired vertical velocity during touchdown float _ground_clearance; // Sensor offset distance - float _est_alt; // Estimated altitude above ground - float _descent_rate_filtered; // Filtered vertical speed - float _radar_alt_prev; // Last cycle calculated altitude - float _radar_alt_calc; // Inertial calculated altitude + float _gnd_hgt; // Height above ground, passed down from copter, can be from lidar or terrain float _avg_acc_z; // Averaged vertical acceleration float _acc_z_sum; // Sum of vertical acceleration samples int16_t _index; // Index for vertical acceleration rolling average @@ -212,9 +198,6 @@ class AC_Autorotation // low pass filter for collective trim LowPassFilterFloat col_trim_lpf; - // low pass filter for descent rate - LowPassFilterFloat descent_rate_lpf; - //--------References to Other Libraries-------- AP_InertialNav& _inav; AP_AHRS& _ahrs; From 9214267d173cc2f0ec41a544d850b081d8ba960c Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Feb 2024 15:47:37 +0000 Subject: [PATCH 37/61] Copter: Use copters inertial navigation interpolation of the rangefinder measurement for heigh above ground in the autorotation lib. We don't need to check for range finder out of range because inertial nave will interpolate for this case and we check against ground clearance in the autorotation lib. --- ArduCopter/heli.cpp | 12 ++++-------- ArduCopter/mode.cpp | 4 ++-- ArduCopter/mode.h | 2 +- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index c82fa2a2b409c..e87ed0d882e9d 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -209,14 +209,10 @@ void Copter::heli_update_autorotation() motors->set_in_autorotation(heli_flags.in_autorotation); motors->set_enable_bailout(true); - // get height above ground. If using a healthy LiDaR below will return - // an interpolated distance based on inertial measurement. - int32_t gnd_dist = flightmode->get_alt_above_ground_cm(); - - // handle the out of range case, assume we are on the ground by that point - if (rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::OutOfRangeLow) { - gnd_dist = 0; - } + // Get height above ground. If using a healthy LiDaR below func will return an interpolated + // distance based on inertial measurement. If LiDaR is unhealthy and terrain is available + // we will get a terrain database estimate. Otherwise we will get height above home. + int32_t gnd_dist = flightmode->get_alt_above_ground_cm(false); // set the height in the autorotation controller g2.arot.set_ground_distance(gnd_dist); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index f235cc22ef390..9fcdfe6c2988a 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -609,13 +609,13 @@ void Mode::make_safe_ground_handling(bool force_throttle_unlimited) /* get a height above ground estimate for landing */ -int32_t Mode::get_alt_above_ground_cm(void) +int32_t Mode::get_alt_above_ground_cm(bool use_psc_case) { int32_t alt_above_ground_cm; if (copter.get_rangefinder_height_interpolated_cm(alt_above_ground_cm)) { return alt_above_ground_cm; } - if (!pos_control->is_active_xy()) { + if (!pos_control->is_active_xy() && use_psc_case) { return copter.current_loc.alt; } if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_cm)) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 1a4cd881e65ba..00e297f13de30 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -152,7 +152,7 @@ class Mode { virtual bool set_speed_up(float speed_xy_cms) {return false;} virtual bool set_speed_down(float speed_xy_cms) {return false;} - int32_t get_alt_above_ground_cm(void); + int32_t get_alt_above_ground_cm(bool use_psc_case=true); // pilot input processing void get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const; From c2b8589b70365874166b4d76445bc5369f5605e0 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Feb 2024 18:18:24 +0000 Subject: [PATCH 38/61] Copter: Autorotation Mode: Make the hover autorotation it's own flight phase --- ArduCopter/mode.h | 14 ++------- ArduCopter/mode_autorotate.cpp | 57 ++++++++++++++++++---------------- 2 files changed, 34 insertions(+), 37 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 00e297f13de30..2f6b769862e5d 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1935,12 +1935,12 @@ class ModeAutorotate : public Mode { float _target_climb_rate_adjust;// Target vertical acceleration used during bail out phase float _target_pitch_adjust; // Target pitch rate used during bail out phase uint32_t _touchdown_time_ms; - bool hover_autorotation; - bool initial_energy_check; + bool initial_energy_check; float time_to_impact; enum class Autorotation_Phase { ENTRY, + HOVER_ENTRY, SS_GLIDE, FLARE, TOUCH_DOWN, @@ -1948,15 +1948,7 @@ class ModeAutorotate : public Mode { // --- Internal flags --- struct controller_flags { - bool entry_initial : 1; - bool ss_glide_initial : 1; - bool flare_initial : 1; - bool touch_down_initial : 1; - bool straight_ahead_initial : 1; - bool level_initial : 1; - bool break_initial : 1; - bool bail_out_initial : 1; - bool bad_rpm : 1; + bool hover_entry_init : 1; } _flags; struct message_flags { diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 66da2b1496d29..37d1e056daaaa 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -47,14 +47,7 @@ bool ModeAutorotate::init(bool ignore_checks) gcs().send_text(MAV_SEVERITY_INFO, "Autorotation initiated"); // Set all intial flags to on - _flags.entry_initial = true; - _flags.ss_glide_initial = true; - _flags.flare_initial = true; - _flags.touch_down_initial = true; - _flags.level_initial = true; - _flags.break_initial = true; - _flags.straight_ahead_initial = true; - _flags.bail_out_initial = true; + _flags.hover_entry_init = false; _msg_flags.bad_rpm = true; initial_energy_check = true; @@ -158,27 +151,39 @@ void ModeAutorotate::run() } - if (!hover_autorotation) { - // Slowly change the target head speed until the target head speed matches the parameter defined value - float norm_rpm = g2.arot.get_rpm()/g2.arot.get_hs_set_point(); - if (norm_rpm > HEAD_SPEED_TARGET_RATIO*1.005f || norm_rpm < HEAD_SPEED_TARGET_RATIO*0.995f) { - _target_head_speed -= _hs_decay * last_loop_time_s; - } else { - _target_head_speed = HEAD_SPEED_TARGET_RATIO; - } - // Set target head speed in head speed controller - g2.arot.set_target_head_speed(_target_head_speed); - // Run airspeed/attitude controller - g2.arot.update_forward_speed_controller(); - // Retrieve pitch target - _pitch_target = g2.arot.get_pitch(); - // Update controllers - _flags.bad_rpm = g2.arot.update_hs_glide_controller(); //run head speed/ collective controller + // Slowly change the target head speed until the target head speed matches the parameter defined value + float norm_rpm = g2.arot.get_rpm()/g2.arot.get_hs_set_point(); + + if (norm_rpm > HEAD_SPEED_TARGET_RATIO*1.005f || norm_rpm < HEAD_SPEED_TARGET_RATIO*0.995f) { + _target_head_speed -= _hs_decay * last_loop_time_s; } else { - _pitch_target = 0.0f; - g2.arot.update_hover_autorotation_controller(); + _target_head_speed = HEAD_SPEED_TARGET_RATIO; } + // Set target head speed in head speed controller + g2.arot.set_target_head_speed(_target_head_speed); + + // Run airspeed/attitude controller + g2.arot.update_forward_speed_controller(); + + // Retrieve pitch target + _pitch_target = g2.arot.get_pitch(); + + // Update controllers + _flags.bad_rpm = g2.arot.update_hs_glide_controller(); //run head speed/ collective controller + + break; + } + + case Autorotation_Phase::HOVER_ENTRY: { + // Controller phase where the aircraft is too low and too slow to perform a full autorotation + // instead, we will try to minimize rotor drag until we can jump to the touch down phase + if (!_flags.hover_entry_init) { + gcs().send_text(MAV_SEVERITY_INFO, "Hover Entry Phase"); + _flags.hover_entry_init = true; + } + _pitch_target = 0.0f; + g2.arot.update_hover_autorotation_controller(); break; } From ee7b8956d968b896e9c95c470788faace8e092bb Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Feb 2024 18:26:01 +0000 Subject: [PATCH 39/61] AC_Autorotation: Add helpers for state machine logic tidy up and remove now needed variables and functions. --- libraries/AC_Autorotation/AC_Autorotation.cpp | 19 ++++++++++++------- libraries/AC_Autorotation/AC_Autorotation.h | 9 ++------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 22d0bcf652095..8df701c1deaa8 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -717,15 +717,20 @@ void AC_Autorotation::touchdown_controller(void) } -float AC_Autorotation::get_time_to_ground(void) +// Determine if we are above the touchdown height using the descent rate and param values +bool AC_Autorotation::should_begin_touchdown(void) { - // Take the default position that time to ground is the touch down time + 1 s. If we are descending - // then we can calc an appropriate value. - float time_to_ground = _t_tch + 1.0f; - if (_inav.get_velocity_z_up_cms() < 0.0f ) { - time_to_ground = (-_gnd_hgt / _inav.get_velocity_z_up_cms()); + float vz = _inav.get_velocity_z_up_cms(); + + // We need to be descending for the touch down controller to interpolate the target + // sensibly between the entry of the touch down phase zero. + if (vz >= 0.0) { + return false; } - return time_to_ground; + + float time_to_ground = (-_gnd_hgt / _inav.get_velocity_z_up_cms()); + + return time_to_ground <= _t_tch.get(); } diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 0e5a32d3660ef..484831e5324ea 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -82,8 +82,6 @@ class AC_Autorotation void get_entry_speed(void); - float get_time_to_ground(void); - void set_entry_sink_rate (float sink_rate) { _entry_sink_rate = sink_rate; } void set_entry_alt (float entry_alt) { _entry_alt = entry_alt; } @@ -94,17 +92,14 @@ class AC_Autorotation void update_avg_acc_z(void); - float get_flare_alt(void) const { return _flare_alt_calc; } - void update_flare_alt(void); void calc_flare_alt(float sink_rate, float fwd_speed); - float get_t_touchdown(void) const { return _t_tch; } + // Estimate the time to impact and compare it with the touchdown time param + bool should_begin_touchdown(void); - float get_cushion_alt(void) const { return _cushion_alt; } - bool is_flare_complete(void) { return _flare_complete; } void calc_sink_d_avg(void); From a42288dea4e456ef6e437e6909b08ff618ea6d65 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Feb 2024 18:30:50 +0000 Subject: [PATCH 40/61] Copter: Autorotation Mode: Rework logic machine to make it easier to read and to ensure flight phase progression based on heights. Also flip flag bools to be init checks. --- ArduCopter/mode.h | 9 ++- ArduCopter/mode_autorotate.cpp | 109 +++++++++++++++------------------ 2 files changed, 56 insertions(+), 62 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 2f6b769862e5d..937b64d072f80 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1935,8 +1935,7 @@ class ModeAutorotate : public Mode { float _target_climb_rate_adjust;// Target vertical acceleration used during bail out phase float _target_pitch_adjust; // Target pitch rate used during bail out phase uint32_t _touchdown_time_ms; - bool initial_energy_check; - float time_to_impact; + bool _hover_autorotation; // bool to determine if we should enter the hover autorotation or not enum class Autorotation_Phase { ENTRY, @@ -1948,7 +1947,13 @@ class ModeAutorotate : public Mode { // --- Internal flags --- struct controller_flags { + bool entry_init : 1; bool hover_entry_init : 1; + bool ss_glide_init : 1; + bool flare_init : 1; + bool touch_down_init : 1; + bool bail_out_init : 1; + bool bad_rpm : 1; } _flags; struct message_flags { diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 37d1e056daaaa..33fe4492e50fb 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -13,7 +13,7 @@ #if MODE_AUTOROTATE_ENABLED == ENABLED -#define AUTOROTATE_ENTRY_TIME 2.0f // (s) number of seconds that the entry phase operates for +#define AUTOROTATE_ENTRY_TIME 2000 // (ms) number of seconds that the entry phase operates for #define BAILOUT_MOTOR_RAMP_TIME 1.0f // (s) time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single #define HEAD_SPEED_TARGET_RATIO 1.0f // Normalised target main rotor head speed (unit: -) #define TOUCHDOWN_TIME 5.0f // time in seconds after land complete flag until aircraft is disarmed @@ -47,9 +47,17 @@ bool ModeAutorotate::init(bool ignore_checks) gcs().send_text(MAV_SEVERITY_INFO, "Autorotation initiated"); // Set all intial flags to on + _flags.entry_init = false; _flags.hover_entry_init = false; + _flags.ss_glide_init = false; + _flags.flare_init = false; + _flags.touch_down_init = false; + _flags.bail_out_init = false; _msg_flags.bad_rpm = true; - initial_energy_check = true; + + // Check if we have sufficient speed or height to do a full autorotation otherwise we have to do one from the hover + // Note: This must be called after arot.init() + _hover_autorotation = (inertial_nav.get_speed_xy_cms() < 250.0f) && !g2.arot.above_flare_height(); // Setting default starting switches phase_switch = Autorotation_Phase::ENTRY; @@ -58,7 +66,7 @@ bool ModeAutorotate::init(bool ignore_checks) _entry_time_start_ms = millis(); // The decay rate to reduce the head speed from the current to the target - _hs_decay = ((_initial_rpm/g2.arot.get_hs_set_point()) - HEAD_SPEED_TARGET_RATIO) / AUTOROTATE_ENTRY_TIME; + _hs_decay = ((_initial_rpm/g2.arot.get_hs_set_point()) - HEAD_SPEED_TARGET_RATIO) / (AUTOROTATE_ENTRY_TIME*1e-3); return true; } @@ -67,65 +75,48 @@ bool ModeAutorotate::init(bool ignore_checks) void ModeAutorotate::run() { - // Check if interlock becomes engaged - if (motors->get_interlock() && !copter.ap.land_complete) { - phase_switch = Autorotation_Phase::BAIL_OUT; - } else if (motors->get_interlock() && copter.ap.land_complete) { - // Aircraft is landed and no need to bail out - set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT); - } - // Current time uint32_t now = millis(); //milliseconds - // set dt in library + // Set dt in library float const last_loop_time_s = AP::scheduler().get_last_loop_time_s(); g2.arot.set_dt(last_loop_time_s); - float alt = g2.arot.get_ground_distance(); - // have autorotation library update estimated radar altitude - g2.arot.update_est_rangefinder_alt(); - if (alt < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) { - g2.arot._using_rfnd = true; - } else { - g2.arot._using_rfnd = false; - } + float curr_vel_z = inertial_nav.get_velocity_z_up_cms(); - // Initialise internal variables - float curr_vel_z = inertial_nav.get_velocity_z_up_cms(); // Current vertical descent + //---------------------------------------------------------------- + // State machine logic + //---------------------------------------------------------------- + // State machine progresses through the autorotation phases as you read down through the if statements. + // More urgent phases (the ones closer to the ground) take precedence later in the if statements. Init + // flags are used to prevent flight phase regression - // Update time to impact only if we are not in the touch down phase. This must be updated before the state machine. - if (phase_switch != Autorotation_Phase::TOUCH_DOWN) { - time_to_impact = g2.arot.get_time_to_ground(); + if (!_hover_autorotation && !_flags.ss_glide_init && g2.arot.above_flare_height() && ((now - _entry_time_start_ms) > AUTOROTATE_ENTRY_TIME)) { + // Flight phase can be progressed to steady state glide + phase_switch = Autorotation_Phase::SS_GLIDE; } + // Check if we are between the flare start height and the touchdown height + if (!_hover_autorotation && !_flags.flare_init && !g2.arot.above_flare_height() && !g2.arot.should_begin_touchdown()) { + phase_switch = Autorotation_Phase::FLARE; + } - //---------------------------------------------------------------- - // State machine logic - //---------------------------------------------------------------- + // Initial check to see if we need to perform a hover autorotation + if (_hover_autorotation && !_flags.hover_entry_init) { + phase_switch = Autorotation_Phase::HOVER_ENTRY; + } - // initial check for total energy monitoring - if (initial_energy_check) { - hover_autorotation = (inertial_nav.get_speed_xy_cms() < 250.0f) && (g2.arot.get_ground_distance() < g2.arot.get_flare_alt()); - initial_energy_check = false; + // Begin touch down if within touch down time + if (!_flags.touch_down_init && g2.arot.should_begin_touchdown()) { + phase_switch = Autorotation_Phase::TOUCH_DOWN; } - //total energy check - if (hover_autorotation) { - if (_flags.entry_initial == false && time_to_impact <= g2.arot.get_t_touchdown()) { - phase_switch = Autorotation_Phase::TOUCH_DOWN; - } - } else { - if ( _flags.ss_glide_initial == true && _flags.flare_initial == true && _flags.touch_down_initial == true && g2.arot.get_est_alt() > g2.arot.get_flare_alt() ) { - if ((now - _entry_time_start_ms)/1000.0f > AUTOROTATE_ENTRY_TIME ) { - // Flight phase can be progressed to steady state glide - phase_switch = Autorotation_Phase::SS_GLIDE; - } - } else if ( g2.arot.get_est_alt()<=g2.arot.get_flare_alt() && g2.arot.get_est_alt()>g2.arot.get_cushion_alt() && !g2.arot.is_flare_complete() ) { - phase_switch = Autorotation_Phase::FLARE; - } else if (time_to_impact <= g2.arot.get_t_touchdown() && _flags.flare_initial == false ) { - phase_switch = Autorotation_Phase::TOUCH_DOWN; - } + // Check if interlock becomes engaged + if (motors->get_interlock() && !copter.ap.land_complete) { + phase_switch = Autorotation_Phase::BAIL_OUT; + } else if (motors->get_interlock() && copter.ap.land_complete) { + // Aircraft is landed and no need to bail out + set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT); } @@ -136,8 +127,7 @@ void ModeAutorotate::run() case Autorotation_Phase::ENTRY: { // Entry phase functions to be run only once - if (_flags.entry_initial == true) { - + if (!_flags.entry_init) { gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase"); // Set necessary variables for the entry phase in the autorotation obj @@ -147,8 +137,7 @@ void ModeAutorotate::run() _target_head_speed = _initial_rpm/g2.arot.get_hs_set_point(); // Prevent running the initial entry functions again - _flags.entry_initial = false; - + _flags.entry_init = true; } // Slowly change the target head speed until the target head speed matches the parameter defined value @@ -189,7 +178,7 @@ void ModeAutorotate::run() case Autorotation_Phase::SS_GLIDE: { // Steady state glide functions to be run only once - if (_flags.ss_glide_initial == true) { + if (!_flags.ss_glide_init) { gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase"); @@ -200,7 +189,7 @@ void ModeAutorotate::run() g2.arot.init_glide(_target_head_speed); // Prevent running the initial glide functions again - _flags.ss_glide_initial = false; + _flags.ss_glide_init = true; } // Run airspeed/attitude controller @@ -225,7 +214,7 @@ void ModeAutorotate::run() case Autorotation_Phase::FLARE: { // Steady state glide functions to be run only once - if (_flags.flare_initial == true) { + if (!_flags.flare_init) { gcs().send_text(MAV_SEVERITY_INFO, "Flare_Phase"); // Ensure target head speed is set in head speed controller @@ -234,7 +223,7 @@ void ModeAutorotate::run() g2.arot.init_flare(_target_head_speed); // Prevent running the initial flare functions again - _flags.flare_initial = false; + _flags.flare_init = true; } // Run flare controller g2.arot.flare_controller(); @@ -253,10 +242,10 @@ void ModeAutorotate::run() break; } case Autorotation_Phase::TOUCH_DOWN: { - if (_flags.touch_down_initial == true) { + if (!_flags.touch_down_init) { gcs().send_text(MAV_SEVERITY_INFO, "Touchdown_Phase"); // Prevent running the initial touchdown functions again - _flags.touch_down_initial = false; + _flags.touch_down_init = true; _touchdown_time_ms = millis(); g2.arot.init_touchdown(); } @@ -269,7 +258,7 @@ void ModeAutorotate::run() } case Autorotation_Phase::BAIL_OUT: { - if (_flags.bail_out_initial == true) { + if (!_flags.bail_out_init) { // Functions and settings to be done once are done here. gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation"); @@ -307,7 +296,7 @@ void ModeAutorotate::run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - _flags.bail_out_initial = false; + _flags.bail_out_init = true; } if ((now - _bail_time_start_ms)/1000.0f >= BAILOUT_MOTOR_RAMP_TIME) { From 29c2d35ef341a429fadae02ca2175508d9ac1ad6 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Feb 2024 18:41:16 +0000 Subject: [PATCH 41/61] AC_Autorotation: Make param defaults numeric values if the defines are not used elsewhere in the code. --- libraries/AC_Autorotation/AC_Autorotation.cpp | 35 +++++++------------ 1 file changed, 12 insertions(+), 23 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 8df701c1deaa8..4e9214f0c8a28 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -4,24 +4,13 @@ #include #include -//Autorotation controller defaults -#define AROT_BAIL_OUT_TIME 2.0f // Default time for bail out controller to run (unit: s) -#define ROT_SOLIDITY 0.05f // Main rotor solidity -#define ROT_DIAMETER 1.25f // Main rotor diameter - // Head Speed (HS) controller specific default definitions #define HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ 2.0f // low-pass filter on accel error (unit: hz) #define HS_CONTROLLER_HEADSPEED_P 0.7f // Default P gain for head speed controller (unit: -) -#define HS_CONTROLLER_ENTRY_COL_FILTER 0.7f // Default low pass filter frequency during the entry phase (unit: Hz) -#define HS_CONTROLLER_GLIDE_COL_FILTER 0.1f // Default low pass filter frequency during the glide phase (unit: Hz) -#define HS_CONTROLLER_CUSHION_COL_FILTER 0.5f // Speed Height controller specific default definitions for autorotation use -#define FWD_SPD_CONTROLLER_GND_SPEED_TARGET 1100 // Default target ground speed for speed height controller (unit: cm/s) -#define FWD_SPD_CONTROLLER_MAX_ACCEL 60 // Default acceleration limit for speed height controller (unit: cm/s/s) -#define AP_FW_VEL_P 0.9f -#define TCH_P 0.1f -#define AP_FW_VEL_FF 0.15f +#define AP_FW_VEL_P 0.9f // Default forward speed controller P gain +#define TCH_P 0.1f // Default touchdown phase collective controller P gain // flare controller default definitions #define AP_ALPHA_TPP 20.0f @@ -61,7 +50,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 800 2000 // @Increment: 50 // @User: Advanced - AP_GROUPINFO("TARG_SP", 4, AC_Autorotation, _param_target_speed, FWD_SPD_CONTROLLER_GND_SPEED_TARGET), + AP_GROUPINFO("TARG_SP", 4, AC_Autorotation, _param_target_speed, 1100), // @Param: COL_FILT_E // @DisplayName: Entry Phase Collective Filter @@ -70,7 +59,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.2 0.5 // @Increment: 0.01 // @User: Advanced - AP_GROUPINFO("COL_FILT_E", 5, AC_Autorotation, _param_col_entry_cutoff_freq, HS_CONTROLLER_ENTRY_COL_FILTER), + AP_GROUPINFO("COL_FILT_E", 5, AC_Autorotation, _param_col_entry_cutoff_freq, 0.7), // @Param: COL_FILT_G // @DisplayName: Glide Phase Collective Filter @@ -79,7 +68,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.03 0.15 // @Increment: 0.01 // @User: Advanced - AP_GROUPINFO("COL_FILT_G", 6, AC_Autorotation, _param_col_glide_cutoff_freq, HS_CONTROLLER_GLIDE_COL_FILTER), + AP_GROUPINFO("COL_FILT_G", 6, AC_Autorotation, _param_col_glide_cutoff_freq, 0.1), // @Param: AS_ACC_MAX // @DisplayName: Forward Acceleration Limit @@ -88,7 +77,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 30 60 // @Increment: 10 // @User: Advanced - AP_GROUPINFO("AS_ACC_MAX", 7, AC_Autorotation, _param_accel_max, FWD_SPD_CONTROLLER_MAX_ACCEL), + AP_GROUPINFO("AS_ACC_MAX", 7, AC_Autorotation, _param_accel_max, 60), // @Param: BAIL_TIME // @DisplayName: Bail Out Timer @@ -97,7 +86,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.5 4 // @Increment: 0.1 // @User: Advanced - AP_GROUPINFO("BAIL_TIME", 8, AC_Autorotation, _param_bail_time, AROT_BAIL_OUT_TIME), + AP_GROUPINFO("BAIL_TIME", 8, AC_Autorotation, _param_bail_time, 2.0), // @Param: HS_SENSOR // @DisplayName: Main Rotor RPM Sensor @@ -122,7 +111,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0 1 // @Increment: 0.01 // @User: Advanced - AP_GROUPINFO("FW_V_FF", 11, AC_Autorotation, _param_fwd_k_ff, AP_FW_VEL_FF), + AP_GROUPINFO("FW_V_FF", 11, AC_Autorotation, _param_fwd_k_ff, 0.15), // @Param: TCH_P // @DisplayName: P gain for vertical touchdown controller @@ -139,7 +128,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.2 0.8 // @Increment: 0.01 // @User: Advanced - AP_GROUPINFO("COL_FILT_C", 13, AC_Autorotation, _param_col_touchdown_cutoff_freq, HS_CONTROLLER_CUSHION_COL_FILTER), + AP_GROUPINFO("COL_FILT_C", 13, AC_Autorotation, _param_col_touchdown_cutoff_freq, 0.5), // @Param: ROT_SOL // @DisplayName: rotor solidity @@ -147,7 +136,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.001 0.01 // @Increment: 0.001 // @User: Advanced - AP_GROUPINFO("ROT_SOL", 14, AC_Autorotation, _param_solidity, ROT_SOLIDITY), + AP_GROUPINFO("ROT_SOL", 14, AC_Autorotation, _param_solidity, 0.05), // @Param: ROT_DIAM // @DisplayName: rotor diameter @@ -156,7 +145,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.001 0.01 // @Increment: 0.001 // @User: Advanced - AP_GROUPINFO("ROT_DIAM", 15, AC_Autorotation, _param_diameter, ROT_DIAMETER), + AP_GROUPINFO("ROT_DIAM", 15, AC_Autorotation, _param_diameter, 1.25), // @Param: T_TCH // @DisplayName: time touchdown @@ -165,7 +154,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Range: 0.001 0.01 // @Increment: 0.001 // @User: Advanced - AP_GROUPINFO("T_TCH", 16, AC_Autorotation, _t_tch, AP_T_TO_G), + AP_GROUPINFO("T_TCH", 16, AC_Autorotation, _t_tch, 0.55), AP_GROUPEND }; From ad64639243842d5a4f60c29edc857cc997c7a972 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Feb 2024 18:46:45 +0000 Subject: [PATCH 42/61] AC_Autorotation: Added Comments and whitespace fixups --- libraries/AC_Autorotation/AC_Autorotation.cpp | 8 +++----- libraries/AC_Autorotation/AC_Autorotation.h | 5 +---- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 4e9214f0c8a28..0185a6f474507 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -13,9 +13,8 @@ #define TCH_P 0.1f // Default touchdown phase collective controller P gain // flare controller default definitions -#define AP_ALPHA_TPP 20.0f -#define AP_T_TO_G 0.55f -#define MIN_TIME_ON_GROUND 3000.0f +#define AP_ALPHA_TPP 20.0f // (deg) Maximum angle of the Tip Path Plane +#define MIN_TIME_ON_GROUND 3000 // (ms) Time on ground required before collective is slewed to zero thrust const AP_Param::GroupInfo AC_Autorotation::var_info[] = { @@ -188,7 +187,7 @@ void AC_Autorotation::init(AP_MotorsHeli* motors, float gnd_clear) { initial_flare_estimate(); // Initialisation of head speed controller - // Set initial collective position to be the collective position on initialisation + // Set initial collective position to be the zero thrust collective and minimize loss in head speed _collective_out = _motors_heli->get_coll_mid(); // Reset feed forward filter @@ -205,7 +204,6 @@ void AC_Autorotation::init(AP_MotorsHeli* motors, float gnd_clear) { _param_head_speed_set_point.set(MAX(_param_head_speed_set_point, 500.0)); // Initialise forward speed controller - // Reset I term and acceleration target _accel_target = 0.0; // Ensure parameter acceleration doesn't exceed hard-coded limit diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 484831e5324ea..84137b4e58d41 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -99,8 +99,6 @@ class AC_Autorotation // Estimate the time to impact and compare it with the touchdown time param bool should_begin_touchdown(void); - - void calc_sink_d_avg(void); // User Settable Parameters @@ -154,7 +152,7 @@ class AC_Autorotation float _avg_sink_deriv; // Averaged derivative of rate of descent float _avg_sink_deriv_sum; // Sum of averaged sink rate derivative int16_t _index_sink_rate; // Index for sink rate derivative rolling average - float _curr_sink_deriv[20]; // Array for storing sink rate derivatives + float _curr_sink_deriv[20]; // Array for storing sink rate derivatives bool _flare_complete; // Flare completed bool _flare_update_check; // Check for flare altitude updating float _time_on_ground; // Time elapsed after touch down @@ -179,7 +177,6 @@ class AC_Autorotation AP_Float _param_diameter; AP_Float _t_tch; - //--------Internal Flags-------- struct controller_flags { bool bad_rpm : 1; From f101a212bf5eba50aa2604c4b5f85f63b18f3893 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Feb 2024 21:18:05 +0000 Subject: [PATCH 43/61] AC_Autorotation: Fix data type for time_on_ground --- libraries/AC_Autorotation/AC_Autorotation.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 84137b4e58d41..ecb1a9eca8e21 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -155,7 +155,7 @@ class AC_Autorotation float _curr_sink_deriv[20]; // Array for storing sink rate derivatives bool _flare_complete; // Flare completed bool _flare_update_check; // Check for flare altitude updating - float _time_on_ground; // Time elapsed after touch down + uint32_t _time_on_ground; // Time elapsed after touch down LowPassFilterFloat _accel_target_filter; // acceleration target filter From 61c6e60fe1e0f6712be6344b2a6809ce4bb0c662 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Tue, 6 Feb 2024 21:55:53 +0000 Subject: [PATCH 44/61] Copter: Autorotation Mode: Whitespace changes, comments updated, and remove unused define. --- ArduCopter/mode_autorotate.cpp | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 33fe4492e50fb..0ba55aed122a1 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -13,10 +13,9 @@ #if MODE_AUTOROTATE_ENABLED == ENABLED -#define AUTOROTATE_ENTRY_TIME 2000 // (ms) number of seconds that the entry phase operates for -#define BAILOUT_MOTOR_RAMP_TIME 1.0f // (s) time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single -#define HEAD_SPEED_TARGET_RATIO 1.0f // Normalised target main rotor head speed (unit: -) -#define TOUCHDOWN_TIME 5.0f // time in seconds after land complete flag until aircraft is disarmed +#define AUTOROTATE_ENTRY_TIME 2000 // (ms) Number of milliseconds that the entry phase operates for +#define BAILOUT_MOTOR_RAMP_TIME 1.0f // (s) Time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single +#define HEAD_SPEED_TARGET_RATIO 1.0f // Normalised target main rotor head speed bool ModeAutorotate::init(bool ignore_checks) { @@ -37,7 +36,7 @@ bool ModeAutorotate::init(bool ignore_checks) return false; } - // init autorotation controllers object + // Init autorotation controllers object g2.arot.init(motors, copter.rangefinder.ground_clearance_cm_orient(ROTATION_PITCH_270)); // Retrieve rpm and start rpm sensor health checks @@ -72,7 +71,6 @@ bool ModeAutorotate::init(bool ignore_checks) } - void ModeAutorotate::run() { // Current time @@ -158,8 +156,8 @@ void ModeAutorotate::run() // Retrieve pitch target _pitch_target = g2.arot.get_pitch(); - // Update controllers - _flags.bad_rpm = g2.arot.update_hs_glide_controller(); //run head speed/ collective controller + // Update head speed controllers + _flags.bad_rpm = g2.arot.update_hs_glide_controller(); break; } @@ -233,6 +231,7 @@ void ModeAutorotate::run() // Update head speed/ collective controller _flags.bad_rpm = g2.arot.update_hs_glide_controller(); + // Retrieve pitch target _pitch_target = g2.arot.get_pitch(); @@ -241,16 +240,23 @@ void ModeAutorotate::run() break; } + case Autorotation_Phase::TOUCH_DOWN: { if (!_flags.touch_down_init) { gcs().send_text(MAV_SEVERITY_INFO, "Touchdown_Phase"); - // Prevent running the initial touchdown functions again - _flags.touch_down_init = true; + + // Save the time on init of touchdown _touchdown_time_ms = millis(); + g2.arot.init_touchdown(); + + // Prevent running the initial touchdown functions again + _flags.touch_down_init = true; } + // Run touchdown controller g2.arot.touchdown_controller(); + // Retrieve pitch target _pitch_target = g2.arot.get_pitch(); @@ -304,6 +310,7 @@ void ModeAutorotate::run() _desired_v_z -= _target_climb_rate_adjust * last_loop_time_s; _pitch_target -= _target_pitch_adjust * last_loop_time_s; } + // Set position controller pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z); From 0798ca61270559ab38bd3d519f41306b88a3a494 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 7 Feb 2024 17:32:57 +0000 Subject: [PATCH 45/61] Copter: Autorotation Mode: Use standard earth_to_body2D velocity rotation --- ArduCopter/mode_autorotate.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 0ba55aed122a1..b3ab86d8be37e 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -338,12 +338,10 @@ void ModeAutorotate::run() float target_roll; - // Grab inertial velocity - const Vector3f& vel = inertial_nav.get_velocity_neu_cms(); - // rotate roll, pitch input from north facing to vehicle's perspective - float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel - float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel + Vector2f vel = ahrs.earth_to_body2D(inertial_nav.get_velocity_neu_cms().xy()); + float roll_vel = vel.y; + float pitch_vel = vel.x; // gain scheduling for yaw float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); From ba3fbafe3e484081239aca1977423535ab05c66f Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 7 Feb 2024 17:54:36 +0000 Subject: [PATCH 46/61] AC_Autorotation: Add options param and give roll-yaw control scheme options --- libraries/AC_Autorotation/AC_Autorotation.cpp | 6 ++++++ libraries/AC_Autorotation/AC_Autorotation.h | 7 +++++++ 2 files changed, 13 insertions(+) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 0185a6f474507..be700b61acd68 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -155,6 +155,12 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @User: Advanced AP_GROUPINFO("T_TCH", 16, AC_Autorotation, _t_tch, 0.55), + // @Param: OPTIONS + // @DisplayName: Autorotation options + // @Description: Bitmask for autorotation options. + // @Bitmask: 0: Use stabilize-like controls (roll angle, yaw rate) + AP_GROUPINFO("OPTIONS", 16, AC_Autorotation, _options, 0), + AP_GROUPEND }; diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index ecb1a9eca8e21..311ad32b3ba96 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -101,6 +101,8 @@ class AC_Autorotation void calc_sink_d_avg(void); + bool use_stabilise_controls(void) const { return _options.get() & int32_t(OPTION::STABILISE_CONTROLS); } + // User Settable Parameters static const struct AP_Param::GroupInfo var_info[]; @@ -176,6 +178,11 @@ class AC_Autorotation AP_Float _param_solidity; AP_Float _param_diameter; AP_Float _t_tch; + AP_Int32 _options; + + enum class OPTION { + STABILISE_CONTROLS=(1<<0), + }; //--------Internal Flags-------- struct controller_flags { From a1b0725adcf36371925631b2b66ba437e6939843 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 7 Feb 2024 17:55:27 +0000 Subject: [PATCH 47/61] Copter: Autorotation mode: Make control schemes for roll and yaw optional --- ArduCopter/mode_autorotate.cpp | 53 +++++++++++++++++++--------------- 1 file changed, 30 insertions(+), 23 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index b3ab86d8be37e..ba2bde39af087 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -331,31 +331,38 @@ void ModeAutorotate::run() } } - float pilot_roll, pilot_pitch, pilot_yaw_rate; - // Operator is in control of roll and yaw. Controls act as if in stabilise flight mode. Pitch - // is controlled by speed-height controller. + float pilot_roll, pilot_pitch, pilot_yaw_rate, target_roll; + // Operator is in control of roll and yaw. Pitch is controlled by speed-height controller. get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max); - float target_roll; - - // rotate roll, pitch input from north facing to vehicle's perspective - Vector2f vel = ahrs.earth_to_body2D(inertial_nav.get_velocity_neu_cms().xy()); - float roll_vel = vel.y; - float pitch_vel = vel.x; - - // gain scheduling for yaw - float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); - pilot_yaw_rate = ((float)pilot_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g2.command_model_pilot.get_rate() / 45.0; - - roll_vel = constrain_float(roll_vel, -560.0f, 560.0f); - pitch_vel = constrain_float(pitch_vel, -560.0f, 560.0f); - - // convert user input into desired roll velocity - float roll_vel_error = roll_vel - (pilot_roll / 0.8f); - - // roll velocity is feed into roll acceleration to minimize slip - target_roll = roll_vel_error * -0.8f; - target_roll = constrain_float(target_roll, -4500.0f, 4500.0f); + // Allow pilot control of the roll and yaw. Two control schemes available, a stabilise-like control scheme + // and a loiter like control scheme. + if (g2.arot.use_stabilise_controls()) { + // Get pilot's desired yaw rate + pilot_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + target_roll = pilot_roll; + + } else { + // loiter-like control scheme + // rotate roll, pitch input from north facing to vehicle's perspective + Vector2f vel = ahrs.earth_to_body2D(inertial_nav.get_velocity_neu_cms().xy()); + float roll_vel = vel.y; + float pitch_vel = vel.x; + + // gain scheduling for yaw + float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); + pilot_yaw_rate = ((float)pilot_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g2.command_model_pilot.get_rate() / 45.0; + + roll_vel = constrain_float(roll_vel, -560.0f, 560.0f); + pitch_vel = constrain_float(pitch_vel, -560.0f, 560.0f); + + // convert user input into desired roll velocity + float roll_vel_error = roll_vel - (pilot_roll / 0.8f); + + // roll velocity is feed into roll acceleration to minimize slip + target_roll = roll_vel_error * -0.8f; + target_roll = constrain_float(target_roll, -4500.0f, 4500.0f); + } // Pitch target is calculated in autorotation phase switch above attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, _pitch_target, pilot_yaw_rate); From 3384684c6e875a7e4f9311cce79eed572473e062 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 7 Feb 2024 18:42:13 +0000 Subject: [PATCH 48/61] AC_Autorotation: Remove sink derivative and only use filtered accel. --- libraries/AC_Autorotation/AC_Autorotation.cpp | 30 +++---------------- libraries/AC_Autorotation/AC_Autorotation.h | 7 ----- 2 files changed, 4 insertions(+), 33 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index be700b61acd68..2f9c5b597c7b6 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -414,13 +414,10 @@ void AC_Autorotation::initial_flare_estimate(void) float des_spd_fwd = _param_target_speed * 0.01f; calc_flare_alt(-_est_rod, des_spd_fwd); - // Initialize sink rate monitor and flare bools + // Initialize flare bools _flare_complete = false; _flare_update_check = false; - _avg_sink_deriv = 0.0f; - _avg_sink_deriv_sum = 0.0f; - _index_sink_rate = 0; - memset(_curr_sink_deriv, 0, sizeof(_curr_sink_deriv)); + gcs().send_text(MAV_SEVERITY_INFO, "Ct/sigma=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/GRAVITY_MSS, _flare_alt_calc*0.01f, _c); } @@ -568,31 +565,12 @@ void AC_Autorotation::update_forward_speed_controller(void) } -void AC_Autorotation::calc_sink_d_avg(void) -{ - float vertical_speed = _inav.get_velocity_z_up_cms(); - _sink_deriv = ((vertical_speed - _last_vertical_speed) * 0.01f) / _dt; - _last_vertical_speed = vertical_speed; - - if (_index_sink_rate < 20) { - _avg_sink_deriv_sum -= _curr_sink_deriv[_index_sink_rate]; - _curr_sink_deriv[_index_sink_rate] = _sink_deriv; - _avg_sink_deriv_sum += _curr_sink_deriv[_index_sink_rate]; - _index_sink_rate = _index_sink_rate + 1; - } else { - _index_sink_rate = 0; - } - _avg_sink_deriv = _avg_sink_deriv_sum / 20.0f; - _avg_sink_deriv = constrain_float( _avg_sink_deriv, -10.0f, 10.0f); -} - - void AC_Autorotation::update_flare_alt(void) { if (!_flare_update_check) { float delta_v_z = fabsf((_inav.get_velocity_z_up_cms()) * 0.01f + _est_rod); - if ((_speed_forward >= 0.8f * _param_target_speed) && (delta_v_z <= 1) && (fabsf(_avg_sink_deriv)<=0.005)) { + if ((_speed_forward >= 0.8f * _param_target_speed) && (delta_v_z <= 1) && (fabsf(_avg_acc_z+GRAVITY_MSS) <= 0.005f)) { float vel_z = _inav.get_velocity_z_up_cms() * 0.01f; float spd_fwd = _speed_forward * 0.01f; _c = (_lift_hover / (sq(vel_z) * 0.5f * SSL_AIR_DENSITY * _disc_area)) * 1.15f; @@ -656,7 +634,7 @@ void AC_Autorotation::flare_controller(void) _accel_out_last = _accel_out; // Estimate flare effectiveness - if (_speed_forward <= (0.6 * _flare_entry_speed) && (fabsf(_avg_sink_deriv) <= 0.005f) && (_avg_acc_z>= -1.1 * 9.80665f)) { + if (_speed_forward <= (0.6 * _flare_entry_speed) && (fabsf(_avg_acc_z+GRAVITY_MSS) <= 0.005f) && (_avg_acc_z>= -1.1 * GRAVITY_MSS)) { if (!_flare_complete) { _flare_complete = true; gcs().send_text(MAV_SEVERITY_INFO, "Flare_complete"); diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 311ad32b3ba96..a79429763d2d6 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -99,8 +99,6 @@ class AC_Autorotation // Estimate the time to impact and compare it with the touchdown time param bool should_begin_touchdown(void); - void calc_sink_d_avg(void); - bool use_stabilise_controls(void) const { return _options.get() & int32_t(OPTION::STABILISE_CONTROLS); } // User Settable Parameters @@ -149,12 +147,7 @@ class AC_Autorotation float _cushion_alt; // Altitude for touchdown collective pull float _disc_area; // Main rotor disc area float _last_vertical_speed; // Last cycle measured vertical speed - float _sink_deriv; // Derivative of sink rate float _est_rod; // Estimated rate of descent (vertical autorotation) - float _avg_sink_deriv; // Averaged derivative of rate of descent - float _avg_sink_deriv_sum; // Sum of averaged sink rate derivative - int16_t _index_sink_rate; // Index for sink rate derivative rolling average - float _curr_sink_deriv[20]; // Array for storing sink rate derivatives bool _flare_complete; // Flare completed bool _flare_update_check; // Check for flare altitude updating uint32_t _time_on_ground; // Time elapsed after touch down From 64829ac04150db0646a873ba070e96d124b2e22d Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 7 Feb 2024 18:43:10 +0000 Subject: [PATCH 49/61] Copter: Autorotation Mode: Remove calls to update derive sink rate --- ArduCopter/mode_autorotate.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index ba2bde39af087..bae5fb72cdabe 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -193,9 +193,6 @@ void ModeAutorotate::run() // Run airspeed/attitude controller g2.arot.update_forward_speed_controller(); - //update sink rate derivative - g2.arot.calc_sink_d_avg(); - //update flare altitude estimate g2.arot.update_flare_alt(); @@ -226,9 +223,6 @@ void ModeAutorotate::run() // Run flare controller g2.arot.flare_controller(); - //update sink rate derivative - g2.arot.calc_sink_d_avg(); - // Update head speed/ collective controller _flags.bad_rpm = g2.arot.update_hs_glide_controller(); From 72553f7839d061c44730a0280056f575053dc29c Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 7 Feb 2024 18:44:36 +0000 Subject: [PATCH 50/61] Copter: Autorotation mode: Simplify rpm warning message code structure and utilise a timer --- ArduCopter/mode.h | 7 +------ ArduCopter/mode_autorotate.cpp | 22 +++++++--------------- 2 files changed, 8 insertions(+), 21 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 937b64d072f80..97a54aae5ef9a 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1956,12 +1956,7 @@ class ModeAutorotate : public Mode { bool bad_rpm : 1; } _flags; - struct message_flags { - bool bad_rpm : 1; - } _msg_flags; - - //--- Internal functions --- - void warning_message(uint8_t message_n); //Handles output messages to the terminal + uint32_t _last_bad_rpm_ms; }; #endif diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index bae5fb72cdabe..c8d9fdec464a2 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -16,6 +16,7 @@ #define AUTOROTATE_ENTRY_TIME 2000 // (ms) Number of milliseconds that the entry phase operates for #define BAILOUT_MOTOR_RAMP_TIME 1.0f // (s) Time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single #define HEAD_SPEED_TARGET_RATIO 1.0f // Normalised target main rotor head speed +#define MSG_TIMER 7000 // (ms) time interval between sending repeat warning messages bool ModeAutorotate::init(bool ignore_checks) { @@ -52,7 +53,8 @@ bool ModeAutorotate::init(bool ignore_checks) _flags.flare_init = false; _flags.touch_down_init = false; _flags.bail_out_init = false; - _msg_flags.bad_rpm = true; + + _last_bad_rpm_ms = 0; // Check if we have sufficient speed or height to do a full autorotation otherwise we have to do one from the hover // Note: This must be called after arot.init() @@ -363,24 +365,14 @@ void ModeAutorotate::run() // Output warning messaged if rpm signal is bad if (_flags.bad_rpm) { - warning_message(1); - } - -} // End function run() - -void ModeAutorotate::warning_message(uint8_t message_n) -{ - switch (message_n) { - case 1: { - if (_msg_flags.bad_rpm) { + if ((millis() - _last_bad_rpm_ms > MSG_TIMER) || (_last_bad_rpm_ms == 0)) { // Bad rpm sensor health. gcs().send_text(MAV_SEVERITY_INFO, "Warning: Poor RPM Sensor Health"); gcs().send_text(MAV_SEVERITY_INFO, "Action: Minimum Collective Applied"); - _msg_flags.bad_rpm = false; + _last_bad_rpm_ms = millis(); } - break; - } } -} + +} // End function run() #endif From 7fddf29a516191503a8cbadb9ca2fd274781851a Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 7 Feb 2024 18:45:24 +0000 Subject: [PATCH 51/61] Copter: Autorotation Mode: Update comments and white space change --- ArduCopter/mode_autorotate.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index c8d9fdec464a2..70481f07d8373 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -203,6 +203,7 @@ void ModeAutorotate::run() // Update head speed/ collective controller _flags.bad_rpm = g2.arot.update_hs_glide_controller(); + // Attitude controller is updated in navigation switch-case statements g2.arot.update_avg_acc_z(); @@ -346,6 +347,7 @@ void ModeAutorotate::run() float pitch_vel = vel.x; // gain scheduling for yaw + // TODO: Need to change consts to sensibly named defines float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); pilot_yaw_rate = ((float)pilot_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g2.command_model_pilot.get_rate() / 45.0; From bbebd62c7d2ff6bfc1a52f18f32157cf2df78161 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 7 Feb 2024 18:45:44 +0000 Subject: [PATCH 52/61] AC_Autorotation: update comments --- libraries/AC_Autorotation/AC_Autorotation.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index a79429763d2d6..a3936d3ef90e1 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -90,6 +90,7 @@ class AC_Autorotation void update_hover_autorotation_controller(); + // update rolling average z-accel filter void update_avg_acc_z(void); void update_flare_alt(void); From a895d15d1e2633ac9ff2684393aa8863d324c028 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 7 Feb 2024 18:49:16 +0000 Subject: [PATCH 53/61] AC_Autorotation: Remove unused variables --- libraries/AC_Autorotation/AC_Autorotation.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index a3936d3ef90e1..503a4eee20de3 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -141,13 +141,12 @@ class AC_Autorotation float _avg_acc_z; // Averaged vertical acceleration float _acc_z_sum; // Sum of vertical acceleration samples int16_t _index; // Index for vertical acceleration rolling average - float _curr_acc_z[10]{}; // Array for storing vertical acceleration samples + float _curr_acc_z[10]; // Array for storing vertical acceleration samples float _flare_alt_calc; // Calculated flare altitude float _lift_hover; // Main rotor thrust in hover condition float _c; // Main rotor drag coefficient float _cushion_alt; // Altitude for touchdown collective pull float _disc_area; // Main rotor disc area - float _last_vertical_speed; // Last cycle measured vertical speed float _est_rod; // Estimated rate of descent (vertical autorotation) bool _flare_complete; // Flare completed bool _flare_update_check; // Check for flare altitude updating From aaee06348a4bd5896a58b9b6c5bff775da0fa328 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 14 Feb 2024 14:54:58 +0000 Subject: [PATCH 54/61] AC_Autorotation: fix options param index --- libraries/AC_Autorotation/AC_Autorotation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 2f9c5b597c7b6..2661064fe12ac 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -159,7 +159,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @DisplayName: Autorotation options // @Description: Bitmask for autorotation options. // @Bitmask: 0: Use stabilize-like controls (roll angle, yaw rate) - AP_GROUPINFO("OPTIONS", 16, AC_Autorotation, _options, 0), + AP_GROUPINFO("OPTIONS", 17, AC_Autorotation, _options, 0), AP_GROUPEND }; From 9e110c97b2584326145368a2c5d798ff7ca64981 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 14 Feb 2024 15:08:34 +0000 Subject: [PATCH 55/61] Copter: Always update the ground distance in the autorotation library to prevent a race on mode init. --- ArduCopter/heli.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index e87ed0d882e9d..d8cdc4402c9c9 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -196,6 +196,19 @@ void Copter::heli_update_rotor_speed_targets() // to autorotation flight mode if manual collective is not being used. void Copter::heli_update_autorotation() { +#if MODE_AUTOROTATE_ENABLED == ENABLED + // Always update the ground distance to prevent a race on init + if (motors->armed() && g2.arot.is_enable()) { + // Get height above ground. If using a healthy LiDaR below func will return an interpolated + // distance based on inertial measurement. If LiDaR is unhealthy and terrain is available + // we will get a terrain database estimate. Otherwise we will get height above home. + int32_t gnd_dist = flightmode->get_alt_above_ground_cm(false); + + // set the height in the autorotation controller + g2.arot.set_ground_distance(gnd_dist); + } +#endif + // check if flying and interlock disengaged if (!ap.land_complete && !motors->get_interlock()) { #if MODE_AUTOROTATE_ENABLED == ENABLED @@ -208,14 +221,6 @@ void Copter::heli_update_autorotation() heli_flags.in_autorotation = true; motors->set_in_autorotation(heli_flags.in_autorotation); motors->set_enable_bailout(true); - - // Get height above ground. If using a healthy LiDaR below func will return an interpolated - // distance based on inertial measurement. If LiDaR is unhealthy and terrain is available - // we will get a terrain database estimate. Otherwise we will get height above home. - int32_t gnd_dist = flightmode->get_alt_above_ground_cm(false); - - // set the height in the autorotation controller - g2.arot.set_ground_distance(gnd_dist); } #endif if (flightmode->has_manual_throttle() && motors->arot_man_enabled()) { From 7fd07b4a006d6ed31f23f08136d3aaab976b0cb6 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 14 Feb 2024 16:44:47 +0000 Subject: [PATCH 56/61] AP_Motors: Heli: Add helper to calculated norm collective from a given blade pitch angle --- libraries/AP_Motors/AP_MotorsHeli.cpp | 7 +++++++ libraries/AP_Motors/AP_MotorsHeli.h | 3 +++ 2 files changed, 10 insertions(+) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index c151539e20e1f..d84b463a20064 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -619,3 +619,10 @@ float AP_MotorsHeli::get_hover_coll_ang(void) { return _collective_min_deg.get() + (_collective_max_deg.get() - _collective_min_deg.get()) * _collective_hover.get(); } + +// Helper function to calculate the normalised collective position given a desired blade pitch angle (deg) +float AP_MotorsHeli::calc_coll_from_ang(float col_ang_deg) +{ + float col_norm = col_ang_deg / MAX((_collective_max_deg.get() - _collective_min_deg.get()), 1.0); + return constrain_float(col_norm, 0.0, 1.0); +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 9d626df445c9f..eca79b93523d3 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -158,6 +158,9 @@ class AP_MotorsHeli : public AP_Motors { // Return collective hover position as an angle in deg float get_hover_coll_ang(void); + // Helper function to calculate the normalised collective position given a desired blade pitch angle (deg) + float calc_coll_from_ang(float col_ang_deg); + // enum for heli optional features enum class HeliOption { USE_LEAKY_I = (1<<0), // 1 From 533e202aaed53aafee5f56ed9f363087e8fad096 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 14 Feb 2024 16:47:33 +0000 Subject: [PATCH 57/61] AC_Autorotation: Simplify bad RPM handling, leaving to RPM lib to determine unhealthy. Col defaults to ang of -2 if no RPM --- libraries/AC_Autorotation/AC_Autorotation.cpp | 55 +++---------------- libraries/AC_Autorotation/AC_Autorotation.h | 16 +----- 2 files changed, 12 insertions(+), 59 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 2661064fe12ac..80e0679a6adce 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -199,13 +199,6 @@ void AC_Autorotation::init(AP_MotorsHeli* motors, float gnd_clear) { // Reset feed forward filter col_trim_lpf.reset(_collective_out); - // Reset flags - _flags.bad_rpm = false; - - // Reset RPM health monitoring - _unhealthy_rpm_counter = 0; - _healthy_rpm_counter = 0; - // Protect against divide by zero _param_head_speed_set_point.set(MAX(_param_head_speed_set_point, 500.0)); @@ -271,16 +264,12 @@ void AC_Autorotation::init_touchdown(void) // Rotor Speed controller for entry, glide and flare phases of autorotation -bool AC_Autorotation::update_hs_glide_controller(void) +void AC_Autorotation::update_hs_glide_controller(void) { - // Reset rpm health flag - _flags.bad_rpm = false; - _flags.bad_rpm_warning = false; - // Get current rpm and update healthy signal counters - _current_rpm = get_rpm(true); + _current_rpm = get_rpm(); - if (_unhealthy_rpm_counter <= 30) { + if (_current_rpm > 0.0) { // Normalised head speed float head_speed_norm = _current_rpm / _param_head_speed_set_point; @@ -300,16 +289,12 @@ bool AC_Autorotation::update_hs_glide_controller(void) _collective_out = constrain_value((_p_term_hs + _ff_term_hs), 0.0f, 1.0f) ; } else { - // RPM sensor is bad set collective to minimum - _collective_out = 0.0f; - - _flags.bad_rpm_warning = true; + // RPM sensor is bad, set collective to angle of -2 deg and hope for the best + _collective_out = _motors_heli->calc_coll_from_ang(-2.0); } // Send collective to setting to motors output library set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); - - return _flags.bad_rpm_warning; } @@ -339,9 +324,9 @@ void AC_Autorotation::set_collective(float collective_filter_cutoff) const // Function that gets rpm and does rpm signal checking to ensure signal is reliable // before using it in the controller -float AC_Autorotation::get_rpm(bool update_counter) +float AC_Autorotation::get_rpm(void) { - float current_rpm = 0.0f; + float current_rpm = 0.0; #if AP_RPM_ENABLED // Get singleton for RPM library @@ -358,33 +343,11 @@ float AC_Autorotation::get_rpm(bool update_counter) uint8_t instance = _param_rpm_instance; // Check RPM sensor is returning a healthy status - if (!rpm->get_rpm(instance, current_rpm) || current_rpm <= -1) { - //unhealthy, rpm unreliable - _flags.bad_rpm = true; + if (!rpm->get_rpm(instance, current_rpm)) { + current_rpm = 0.0; } - - } else { - _flags.bad_rpm = true; } -#else - _flags.bad_rpm = true; #endif - - if (_flags.bad_rpm) { - // Count unhealthy rpm updates and reset healthy rpm counter - _unhealthy_rpm_counter++; - _healthy_rpm_counter = 0; - - } else if (!_flags.bad_rpm && _unhealthy_rpm_counter > 0) { - // Poor rpm reading may have cleared. Wait 10 update cycles to clear. - _healthy_rpm_counter++; - - if (_healthy_rpm_counter >= 10) { - // Poor rpm health has cleared, reset counters - _unhealthy_rpm_counter = 0; - _healthy_rpm_counter = 0; - } - } return current_rpm; } diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 503a4eee20de3..e44f14a059004 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -36,13 +36,10 @@ class AC_Autorotation void initial_flare_estimate(void); // Update head speed controller - bool update_hs_glide_controller(void); - - // Function just returns the rpm as last read in this library - float get_rpm(void) const { return _current_rpm; } + void update_hs_glide_controller(void); // Function fetches fresh rpm update and continues sensor health monitoring - float get_rpm(bool update_counter); + float get_rpm(void); // Sets the normalised target head speed void set_target_head_speed(float ths) { _target_head_speed = ths; } @@ -112,8 +109,7 @@ class AC_Autorotation float _collective_out; float _head_speed_error; // Error between target head speed and current head speed. Normalised by head speed set point RPM. float _col_cutoff_freq; // Lowpass filter cutoff frequency (Hz) for collective. - uint8_t _unhealthy_rpm_counter; // Counter used to track RPM sensor unhealthy signal. - uint8_t _healthy_rpm_counter; // Counter used to track RPM sensor healthy signal. + uint16_t _unhealthy_rpm_counter; // Counter used to track RPM sensor unhealthy signal. float _target_head_speed; // Normalised target head speed. Normalised by head speed set point RPM. float _p_term_hs; // Proportional contribution to collective setting. float _ff_term_hs; // Following trim feed forward contribution to collective setting. @@ -177,12 +173,6 @@ class AC_Autorotation STABILISE_CONTROLS=(1<<0), }; - //--------Internal Flags-------- - struct controller_flags { - bool bad_rpm : 1; - bool bad_rpm_warning : 1; - } _flags; - //--------Internal Functions-------- // set the collective in the motors library void set_collective(float _collective_filter_cutoff) const; From 6547e24c85a405d9034aa44f56b15f4a95c85c80 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Wed, 14 Feb 2024 16:48:39 +0000 Subject: [PATCH 58/61] Copter: Autorotation mode: remove bad rpm handling --- ArduCopter/mode_autorotate.cpp | 20 ++++---------------- 1 file changed, 4 insertions(+), 16 deletions(-) diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 70481f07d8373..34979b28c9f6b 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -41,7 +41,7 @@ bool ModeAutorotate::init(bool ignore_checks) g2.arot.init(motors, copter.rangefinder.ground_clearance_cm_orient(ROTATION_PITCH_270)); // Retrieve rpm and start rpm sensor health checks - _initial_rpm = g2.arot.get_rpm(true); + _initial_rpm = g2.arot.get_rpm(); // Display message gcs().send_text(MAV_SEVERITY_INFO, "Autorotation initiated"); @@ -54,8 +54,6 @@ bool ModeAutorotate::init(bool ignore_checks) _flags.touch_down_init = false; _flags.bail_out_init = false; - _last_bad_rpm_ms = 0; - // Check if we have sufficient speed or height to do a full autorotation otherwise we have to do one from the hover // Note: This must be called after arot.init() _hover_autorotation = (inertial_nav.get_speed_xy_cms() < 250.0f) && !g2.arot.above_flare_height(); @@ -159,7 +157,7 @@ void ModeAutorotate::run() _pitch_target = g2.arot.get_pitch(); // Update head speed controllers - _flags.bad_rpm = g2.arot.update_hs_glide_controller(); + g2.arot.update_hs_glide_controller(); break; } @@ -202,7 +200,7 @@ void ModeAutorotate::run() _pitch_target = g2.arot.get_pitch(); // Update head speed/ collective controller - _flags.bad_rpm = g2.arot.update_hs_glide_controller(); + g2.arot.update_hs_glide_controller(); // Attitude controller is updated in navigation switch-case statements g2.arot.update_avg_acc_z(); @@ -227,7 +225,7 @@ void ModeAutorotate::run() g2.arot.flare_controller(); // Update head speed/ collective controller - _flags.bad_rpm = g2.arot.update_hs_glide_controller(); + g2.arot.update_hs_glide_controller(); // Retrieve pitch target _pitch_target = g2.arot.get_pitch(); @@ -365,16 +363,6 @@ void ModeAutorotate::run() // Pitch target is calculated in autorotation phase switch above attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, _pitch_target, pilot_yaw_rate); - // Output warning messaged if rpm signal is bad - if (_flags.bad_rpm) { - if ((millis() - _last_bad_rpm_ms > MSG_TIMER) || (_last_bad_rpm_ms == 0)) { - // Bad rpm sensor health. - gcs().send_text(MAV_SEVERITY_INFO, "Warning: Poor RPM Sensor Health"); - gcs().send_text(MAV_SEVERITY_INFO, "Action: Minimum Collective Applied"); - _last_bad_rpm_ms = millis(); - } - } - } // End function run() #endif From 6f3eff12080265bb328554e87a3d35b33c5f1de3 Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Fri, 16 Feb 2024 02:37:31 +0000 Subject: [PATCH 59/61] AP_Motors: temporary fix to prevent I-term decay during autorotations --- libraries/AP_Motors/AP_MotorsHeli_RSC.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 8df9a47d6737c..2dfe073569ab1 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -130,7 +130,7 @@ class AP_MotorsHeli_RSC { uint32_t get_output_mask() const; // rotor_speed_above_critical - return true if rotor speed is above that critical for flight - bool rotor_speed_above_critical(void) const { return get_rotor_speed() > get_critical_speed(); } + bool rotor_speed_above_critical(void) const { return get_rotor_speed() >= get_critical_speed(); } // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; From 2cde5712588822f93796813ec4b45e826b5fb925 Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Fri, 16 Feb 2024 04:45:12 +0000 Subject: [PATCH 60/61] AP_Motors: return collective land position post touchdown condition requires collective to be lowered below land_col_min_pct in order to get the disarm logic to disarm the aircraft --- libraries/AP_Motors/AP_MotorsHeli.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index eca79b93523d3..7b2cb11db4dee 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -148,6 +148,9 @@ class AP_MotorsHeli : public AP_Motors { //return zero lift collective position float get_coll_mid() const { return _collective_zero_thrust_pct; } + //return landing collective position + float get_coll_land_min()const { return _collective_land_min_pct;} + //return collective hover float get_coll_hover() const { return _collective_hover; } From 241959ee1c6247d4bd9719b5bf5a4e050354972e Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Fri, 16 Feb 2024 04:47:55 +0000 Subject: [PATCH 61/61] AC_Autorotation: fix for post touchdown disarming -collective gets lowered below land_col_min_pct in order to get the aircraft disarmed -adjusted thresholds of lfare update and flare complete check --- libraries/AC_Autorotation/AC_Autorotation.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 80e0679a6adce..00b7b44708d7a 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -383,6 +383,7 @@ void AC_Autorotation::initial_flare_estimate(void) gcs().send_text(MAV_SEVERITY_INFO, "Ct/sigma=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/GRAVITY_MSS, _flare_alt_calc*0.01f, _c); + gcs().send_text(MAV_SEVERITY_INFO, "est_rod=%f", _est_rod); } @@ -411,7 +412,6 @@ void AC_Autorotation::calc_flare_alt(float sink_rate, float fwd_speed) // Estimate altitude to begin collective pull _cushion_alt = (-(sink_rate * cosf(radians(AP_ALPHA_TPP))) * _t_tch) * 100.0f; - gcs().send_text(MAV_SEVERITY_INFO, "vs_final_est=%f ", sink_rate*cosf(radians(20))); // Total delta altitude to ground _flare_alt_calc = _cushion_alt + delta_h * 100.0f; @@ -533,7 +533,7 @@ void AC_Autorotation::update_flare_alt(void) if (!_flare_update_check) { float delta_v_z = fabsf((_inav.get_velocity_z_up_cms()) * 0.01f + _est_rod); - if ((_speed_forward >= 0.8f * _param_target_speed) && (delta_v_z <= 1) && (fabsf(_avg_acc_z+GRAVITY_MSS) <= 0.005f)) { + if ((_speed_forward >= 0.8f * _param_target_speed) && (delta_v_z <= 2) && (fabsf(_avg_acc_z+GRAVITY_MSS) <= 0.5f)) { float vel_z = _inav.get_velocity_z_up_cms() * 0.01f; float spd_fwd = _speed_forward * 0.01f; _c = (_lift_hover / (sq(vel_z) * 0.5f * SSL_AIR_DENSITY * _disc_area)) * 1.15f; @@ -597,7 +597,7 @@ void AC_Autorotation::flare_controller(void) _accel_out_last = _accel_out; // Estimate flare effectiveness - if (_speed_forward <= (0.6 * _flare_entry_speed) && (fabsf(_avg_acc_z+GRAVITY_MSS) <= 0.005f) && (_avg_acc_z>= -1.1 * GRAVITY_MSS)) { + if (_speed_forward <= (0.6 * _flare_entry_speed) && (fabsf(_avg_acc_z+GRAVITY_MSS) <= 0.5f)) { if (!_flare_complete) { _flare_complete = true; gcs().send_text(MAV_SEVERITY_INFO, "Flare_complete"); @@ -608,7 +608,7 @@ void AC_Autorotation::flare_controller(void) _pitch_target = atanf(-_accel_out / (GRAVITY_MSS * 100.0f)) * (18000.0f/M_PI); _pitch_target = constrain_float(_pitch_target, 0.0f, AP_ALPHA_TPP * 100.0f); } else { - _pitch_target *= 0.95f; + _pitch_target *= 0.9995f; } } @@ -635,7 +635,7 @@ void AC_Autorotation::touchdown_controller(void) // driven the collective high to cushion the landing so the landing detector will not trip until we drive the collective back to zero thrust and below. if ((_time_on_ground > 0) && ((AP_HAL::millis() - _time_on_ground) > MIN_TIME_ON_GROUND)) { // On ground, smoothly lower collective to just bellow zero thrust, to make sure we trip the landing detector - float desired_col = _motors_heli->get_coll_mid() * 0.95; + float desired_col = _motors_heli->get_coll_land_min() * 0.95; _collective_out = _collective_out*0.9 + desired_col*0.1; } else {