Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : //
       4             : // pre-takeoff checks
       5             : //
       6             : 
       7             : // detects if the vehicle should be allowed to takeoff or not and sets the motors.blocked flag
       8     3800955 : void Copter::takeoff_check()
       9             : {
      10             : #if HAL_WITH_ESC_TELEM && FRAME_CONFIG != HELI_FRAME
      11             :     // if motors have become unblocked return immediately
      12             :     // this ensures the motors can only be blocked immediately after arming
      13     3396732 :     uint32_t now_ms = AP_HAL::millis();
      14     3396732 :     if (!motors->get_spoolup_block()) {
      15     3392057 :         takeoff_check_warning_ms = now_ms;
      16     3392057 :         takeoff_check_state.warning_ms = now_ms;
      17     3392477 :         return;
      18             :     }
      19             : 
      20             :     // Motors Library has enabled the spool up block.
      21             : 
      22             :     // Immediately clear the spool up block if not landed
      23        4675 :     if (!ap.land_complete) {
      24           0 :         motors->set_spoolup_block(false);
      25           0 :         return;
      26             :     }
      27             : 
      28             :     // Run the common motor checks (called early so it can clear its warning timer when disarmed)
      29        4675 :     const bool motor_check_passed = motors_takeoff_check(g2.takeoff_rpm_min, g2.takeoff_rpm_max);
      30             : 
      31             :     // Check system load
      32             :     float avg_load, peak_load;
      33        4675 :     bool load_adequate = true;
      34        4675 :     if (hal.util->get_system_load(avg_load, peak_load)) {
      35           0 :         if (avg_load > 95.0f || peak_load > 99.5f) {
      36           0 :             load_adequate = false;
      37             :         }
      38             :     }
      39             : 
      40             :     // Clear block if all checks passed
      41        4675 :     if (motor_check_passed && load_adequate) {
      42         420 :         motors->set_spoolup_block(false);
      43         420 :         return;
      44             :     }
      45             : 
      46             :     // warn about CPU load every 2 seconds
      47        4255 :     if (now_ms - takeoff_check_warning_ms > 2000) {
      48          42 :         takeoff_check_warning_ms = now_ms;
      49          42 :         const char* prefix_str = "Takeoff blocked:";
      50          42 :         if (!load_adequate) {
      51           0 :             gcs().send_text(MAV_SEVERITY_CRITICAL, "%s CPU overload (%4.1f%%)", prefix_str, avg_load);
      52             :         }
      53             :     }
      54             : #endif
      55      404223 : }