Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #define ARM_DELAY               20  // called at 10hz so 2 seconds
       4             : #define DISARM_DELAY            20  // called at 10hz so 2 seconds
       5             : #define LOST_VEHICLE_DELAY      10  // called at 10hz so 1 second
       6             : 
       7             : static uint32_t auto_disarm_begin;
       8             : 
       9             : // auto_disarm_check - disarm after a configurable delay if landed and throttle is low,
      10             : // unless takeoff/spool-up is being requested or auto-disarm is disabled.
      11      759512 : void Copter::auto_disarm_check()
      12             : {
      13      759512 :     uint32_t tnow_ms = millis();
      14      759512 :     uint32_t disarm_delay_ms = 1000*constrain_int16(g.disarm_delay, 0, INT8_MAX);
      15             : 
      16             :     // Reset timer and exit if disarmed, auto-disarm disabled, or in THROW mode.
      17      759512 :     if (!motors->armed() || disarm_delay_ms == 0 || flightmode->mode_number() == Mode::Number::THROW) {
      18      272385 :         auto_disarm_begin = tnow_ms;
      19      272385 :         return;
      20             :     }
      21             : 
      22             :     // If takeoff/spool-up is being requested (desired spool state beyond ground idle),
      23             :     // inhibit auto-disarm (current spool state may lag during delays/checks).
      24      487127 :     if (motors->get_desired_spool_state() > AP_Motors::DesiredSpoolState::GROUND_IDLE
      25      487127 :         || motors->get_spool_state() > AP_Motors::SpoolState::GROUND_IDLE) {
      26      474243 :         auto_disarm_begin = tnow_ms;
      27      474243 :         return;
      28             :     }
      29             : 
      30             :     // If throttle interlock is used and disengaged, or Emergency Stop is active,
      31             :     // shorten the auto-disarm delay (motors may be stopped so arming is less obvious)
      32       12884 :     if ((ap.using_interlock && !motors->get_interlock()) || SRV_Channels::get_emergency_stop()) {
      33             : #if FRAME_CONFIG != HELI_FRAME
      34             :         // Shorten delay when motors may be stopped (interlock off / e-stop), since arming is less obvious.
      35           0 :         disarm_delay_ms /= 2;
      36             : #endif
      37             :     } else {
      38       11176 :         bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
      39             :         bool thr_low;
      40       11176 :         if (flightmode->has_manual_throttle() || !sprung_throttle_stick) {
      41       11136 :             thr_low = ap.throttle_zero;
      42             :         } else {
      43          40 :             float deadband_top = get_throttle_mid() + g.throttle_deadzone;
      44          40 :             thr_low = channel_throttle->get_control_in() <= deadband_top;
      45             :         }
      46             : 
      47       11176 :         if (!thr_low || !ap.land_complete) {
      48             :             // Reset timer whenever throttle is not low or vehicle is not landed.
      49        4485 :             auto_disarm_begin = tnow_ms;
      50             :         }
      51             :     }
      52             : 
      53             :     // disarm once timer expires
      54       12884 :     if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) {
      55          22 :         arming.disarm(AP_Arming::Method::DISARMDELAY);
      56          22 :         auto_disarm_begin = tnow_ms;
      57             :     }
      58             : }
      59             : 
      60             : // motors_output - send output to motors library which will adjust and send to ESCs and servos
      61             : // full_push is true when slower rate updates (e.g. servo output) need to be performed at the main loop rate.
      62    30585542 : void Copter::motors_output(bool full_push)
      63             : {
      64             : #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
      65             :     // this is to allow the failsafe module to deliberately crash
      66             :     // the vehicle. Only used in extreme circumstances to meet the
      67             :     // OBC rules
      68             :     if (g2.afs.should_crash_vehicle()) {
      69             :         g2.afs.terminate_vehicle();
      70             :         if (!g2.afs.terminating_vehicle_via_landing()) {
      71             :             return;
      72             :         }
      73             :         // landing must continue to run the motors output
      74             :     }
      75             : #endif
      76             : 
      77             :     // Update arming delay state
      78    30585542 :     if (ap.in_arming_delay && (!motors->armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || flightmode->mode_number() == Mode::Number::THROW)) {
      79         497 :         ap.in_arming_delay = false;
      80             :     }
      81             : 
      82             :     // output any servo channels
      83    30585542 :     SRV_Channels::calc_pwm();
      84             : 
      85    30585542 :     auto &srv = AP::srv();
      86             : 
      87             :     // cork now, so that all channel outputs happen at once
      88    30585542 :     srv.cork();
      89             : 
      90             :     // update output on any aux channels, for manual passthru
      91    30585542 :     SRV_Channels::output_ch_all();
      92             : 
      93             :     // update motors interlock state
      94    30585542 :     bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop();
      95    30585542 :     if (!motors->get_interlock() && interlock) {
      96         495 :         motors->set_interlock(true);
      97         495 :         LOGGER_WRITE_EVENT(LogEvent::MOTORS_INTERLOCK_ENABLED);
      98    30585047 :     } else if (motors->get_interlock() && !interlock) {
      99         493 :         motors->set_interlock(false);
     100         493 :         LOGGER_WRITE_EVENT(LogEvent::MOTORS_INTERLOCK_DISABLED);
     101             :     }
     102             : 
     103    30585542 :     if (ap.motor_test) {
     104             :         // check if we are performing the motor test
     105        8001 :         motor_test_output();
     106             :     } else {
     107             :         // send output signals to motors
     108    30577541 :         flightmode->output_to_motors();
     109             :     }
     110             : 
     111             :     // push all channels
     112    30585542 :     if (full_push) {
     113             :         // motor output including servos and other updates that need to run at the main loop rate
     114    30441431 :         srv.push();
     115             :     } else {
     116             :         // motor output only at main loop rate or faster
     117      144111 :         hal.rcout->push();
     118             :     }
     119    30585542 : }
     120             : 
     121             : // motors_output from main thread at main loop rate
     122    30412448 : void Copter::motors_output_main()
     123             : {
     124    30412448 :     if (!using_rate_thread) {
     125    30297322 :         motors_output();
     126             :     }
     127    30412448 : }
     128             : 
     129             : // check for pilot stick input to trigger lost vehicle alarm
     130      759512 : void Copter::lost_vehicle_check()
     131             : {
     132             :     static uint8_t soundalarm_counter;
     133             : 
     134             :     // disable if aux switch is setup to vehicle alarm as the two could interfere
     135      759512 :     if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::LOST_VEHICLE_SOUND)) {
     136           0 :         return;
     137             :     }
     138             : 
     139             :     // ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch
     140      759512 :     if (ap.throttle_zero && !motors->armed() && (channel_roll->get_control_in() > 4000) && (channel_pitch->get_control_in() > 4000)) {
     141           0 :         if (soundalarm_counter >= LOST_VEHICLE_DELAY) {
     142           0 :             if (AP_Notify::flags.vehicle_lost == false) {
     143           0 :                 AP_Notify::flags.vehicle_lost = true;
     144           0 :                 gcs().send_text(MAV_SEVERITY_NOTICE,"Locate Copter alarm");
     145             :             }
     146             :         } else {
     147           0 :             soundalarm_counter++;
     148             :         }
     149             :     } else {
     150      759512 :         soundalarm_counter = 0;
     151      759512 :         if (AP_Notify::flags.vehicle_lost == true) {
     152           0 :             AP_Notify::flags.vehicle_lost = false;
     153             :         }
     154             :     }
     155             : }