AC_Autorotation: Add arming checks

ArduPilot · Sep 9, 2024 · 8448b60 · 8448b60
1 parent 8ee67fd
commit 8448b60
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 1 deletion.
diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp
@@ -115,6 +115,7 @@ AC_Autorotation::AC_Autorotation() :
         AP_Param::setup_object_defaults(this, var_info);
     }
 
+
 // Initialisation of head speed controller
 void AC_Autorotation::init_hs_controller()
 {
@@ -374,3 +375,39 @@ float AC_Autorotation::calc_speed_forward(void)
     return speed_forward;
 }
 
+// Arming checks for autorotation, mostly checking for miss-configurations
+bool AC_Autorotation::arming_checks(size_t buflen, char *buffer) const
+{
+    if (!enabled()) {
+        // Don't run arming checks if not enabled
+        return true;
+    }
+
+    const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+    // Check for correct RPM sensor config
+#if AP_RPM_ENABLED
+    // Get singleton for RPM library
+    const AP_RPM *rpm = AP_RPM::get_singleton();
+
+    // Get current rpm, checking to ensure no nullptr
+    if (rpm == nullptr) {
+        hal.util->snprintf(buffer, buflen, "Can't access RPM");
+        return false;
+    }
+
+    // Sanity check that the designated rpm sensor instance is there
+    if ((_param_rpm_instance.get() < 0)) {
+        hal.util->snprintf(buffer, buflen, "RPM instance <0");
+        return false;
+    }
+
+    if (!rpm->enabled(_param_rpm_instance.get())) {
+        hal.util->snprintf(buffer, buflen, "RPM%i not enabled", _param_rpm_instance.get()+1);
+        return false;
+    }
+#endif
+
+    return true;
+}
+
diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h
@@ -18,6 +18,16 @@ class AC_Autorotation
     AC_Autorotation();
 
     //--------Functions--------
+    bool enabled(void) const { return _param_enable.get() > 0; }
+
+
+
+    // Arming checks for autorotation, mostly checking for miss-configurations
+    bool arming_checks(size_t buflen, char *buffer) const;
+
+
+
+    // not yet checked
     void init_hs_controller(void);  // Initialise head speed controller
     void init_fwd_spd_controller(void);  // Initialise forward speed controller
     bool update_hs_glide_controller(float dt);  // Update head speed controller
@@ -29,7 +39,6 @@ class AC_Autorotation
     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_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