Skip to content

Commit

Permalink
demonstrate new filter idea for radar altitude est
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Jun 29, 2023
1 parent 7e678ae commit b9871c9
Show file tree
Hide file tree
Showing 4 changed files with 70 additions and 11 deletions.
12 changes: 9 additions & 3 deletions ArduCopter/Copter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -161,7 +161,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(auto_trim, 10, 75, 30),
#if RANGEFINDER_ENABLED == ENABLED
#if MODE_AUTOROTATE_ENABLED == ENABLED
SCHED_TASK(read_rangefinder, 100, 100, 33),
SCHED_TASK(read_rangefinder, 20, 100, 33),
#else
SCHED_TASK(read_rangefinder, 20, 100, 33),
#endif
Expand Down Expand Up @@ -534,6 +534,12 @@ void Copter::loop_rate_logging()
if (should_log(MASK_LOG_IMU_FAST)) {
AP::ins().Write_IMU();
}
#if MODE_AUTOROTATE_ENABLED == ENABLED
if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
//update autorotation log
g2.arot.Log_Write_Autorotation();
}
#endif
}

// ten_hz_logging_loop
Expand Down Expand Up @@ -600,12 +606,12 @@ void Copter::twentyfive_hz_logging()
AP::ins().Write_IMU();
}

#if MODE_AUTOROTATE_ENABLED == ENABLED
/* #if MODE_AUTOROTATE_ENABLED == ENABLED
if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
//update autorotation log
g2.arot.Log_Write_Autorotation();
}
#endif
#endif */
#if HAL_GYROFFT_ENABLED
if (should_log(MASK_LOG_FTN_FAST)) {
gyro_fft.write_log_messages();
Expand Down
11 changes: 7 additions & 4 deletions ArduCopter/mode_autorotate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@ bool ModeAutorotate::init(bool ignore_checks)
// This must be done before RPM value is fetched
g2.arot.init_hs_controller();
g2.arot.init_fwd_spd_controller();
g2.arot.init_est_radar_alt();

// Retrieve rpm and start rpm sensor health checks
_initial_rpm = g2.arot.get_rpm(true);
Expand Down Expand Up @@ -125,6 +126,8 @@ void ModeAutorotate::run()
// Current time
uint32_t now = millis(); //milliseconds
float alt = g2.arot.get_ground_distance();
// have autorotation library update estimated radar altitude
g2.arot.update_est_radar_alt();
if(alt < 4800){
g2.arot._using_rfnd = true;
}else{
Expand Down Expand Up @@ -222,7 +225,7 @@ void ModeAutorotate::run()
_pitch_target = 0.0f;
g2.arot.set_collective_minimum_drag(motors->get_coll_mid());
g2.arot.set_entry_sink_rate(inertial_nav.get_velocity_z_up_cms());
g2.arot.alt_error();
// g2.arot.alt_error();
g2.arot.set_entry_alt(g2.arot.get_ground_distance());
}

Expand Down Expand Up @@ -250,7 +253,7 @@ void ModeAutorotate::run()
_flags.ss_glide_initial = 0;
}
//estimate altitude error
g2.arot.alt_error();
// g2.arot.alt_error();
g2.arot.calc_flr_alt();

// Run airspeed/attitude controller
Expand Down Expand Up @@ -285,7 +288,7 @@ void ModeAutorotate::run()
// Prevent running the initial flare functions again
_flags.flare_initial = 0;
}
g2.arot.alt_error();
// g2.arot.alt_error();

// Run flare controller
g2.arot.set_dt(G_Dt);
Expand All @@ -310,7 +313,7 @@ void ModeAutorotate::run()
g2.arot.set_col_cutoff_freq(g2.arot.get_col_cushion_freq());
// g2.arot.set_ground_clearance(copter.rangefinder.ground_clearance_cm_orient(ROTATION_PITCH_270));
}
g2.arot.alt_error();
// g2.arot.alt_error();
g2.arot.set_dt(G_Dt);
g2.arot.touchdown_controller();
_pitch_target = g2.arot.get_pitch();
Expand Down
49 changes: 46 additions & 3 deletions libraries/AC_Autorotation/AC_Autorotation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -339,7 +339,7 @@ void AC_Autorotation::Log_Write_Autorotation(void) const

//Write to data flash log
AP::logger().WriteStreaming("AROT",
"TimeUS,P,hs_e,C_Out,FFCol,SpdF,DH,p,ff,AccO,AccT,PitT,DV",
"TimeUS,P,hs_e,C_Out,FFCol,SpdF,DH,p,ff,AccO,AccT,Ealt,Ralt",
"Qffffffffffff",
AP_HAL::micros64(),
(double)_p_term_hs,
Expand All @@ -352,8 +352,8 @@ void AC_Autorotation::Log_Write_Autorotation(void) const
(double)_vel_ff,
(double)_accel_out,
(double)_accel_target,
(double)_alt_error,
(double)(_desired_sink_rate*0.01f));
(double)_est_alt * 0.01f,
(double)(_radar_alt * 0.01f));
}


Expand Down Expand Up @@ -548,6 +548,49 @@ void AC_Autorotation::alt_error()

}

void AC_Autorotation::init_est_radar_alt()
{
// set descent rate filter cutoff frequency
descent_rate_lpf.set_cutoff_frequency(10.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 * 1.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::calc_flr_alt()
{
if(_inav.get_velocity_z_up_cms()<0.0f){
Expand Down
9 changes: 8 additions & 1 deletion libraries/AC_Autorotation/AC_Autorotation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,8 @@ class AC_Autorotation
void alt_error();
void calc_flr_alt();
float get_flare_alt() const { return _flare_alt; }

void init_est_radar_alt();
void update_est_radar_alt();

// User Settable Parameters
static const struct AP_Param::GroupInfo var_info[];
Expand Down Expand Up @@ -115,6 +116,9 @@ class AC_Autorotation
float _glide_ratio;
float _max_rf_error;
float _est_alt;
float _descent_rate_filtered;
float _radar_alt_prev;
float _radar_alt_calc;


LowPassFilterFloat _accel_target_filter; // acceleration target filter
Expand Down Expand Up @@ -144,6 +148,9 @@ 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;
};

0 comments on commit b9871c9

Please sign in to comment.