Copter: tradheli-high rate rangefinder estimation

ArduPilot · Jun 30, 2023 · 3870277 · 3870277
1 parent b3e435c
commit 3870277
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
@@ -160,12 +160,8 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(auto_disarm_check,     10,     50,  27),
     SCHED_TASK(auto_trim,             10,     75,  30),
 #if RANGEFINDER_ENABLED == ENABLED
-#if MODE_AUTOROTATE_ENABLED == ENABLED
-    SCHED_TASK(read_rangefinder,      100,    100,  33),
-#else
     SCHED_TASK(read_rangefinder,      20,    100,  33),
 #endif
-#endif
 #if HAL_PROXIMITY_ENABLED
     SCHED_TASK_CLASS(AP_Proximity,         &copter.g2.proximity,        update,         200,  50,  36),
 #endif

diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp
@@ -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);
@@ -122,6 +123,14 @@ 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{
+        g2.arot._using_rfnd = false;
+    }
 
     // Initialise internal variables
     float curr_vel_z = inertial_nav.get_velocity_z_up_cms();   // Current vertical descent