Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : /*****************************************************************************
       4             : * Functions to check and perform ESC calibration
       5             : *****************************************************************************/
       6             : 
       7             : #define ESC_CALIBRATION_HIGH_THROTTLE   950
       8             : 
       9             : // check if we should enter esc calibration mode
      10        1001 : void Copter::esc_calibration_startup_check()
      11             : {
      12        1001 :     if (motors->is_brushed_pwm_type()) {
      13             :         // ESC cal not valid for brushed motors
      14           0 :         return;
      15             :     }
      16             : 
      17             : #if FRAME_CONFIG != HELI_FRAME
      18             :     // delay up to 2 second for first radio input
      19         918 :     uint8_t i = 0;
      20        9511 :     while ((i++ < 100) && (last_radio_update_ms == 0)) {
      21        8593 :         hal.scheduler->delay(20);
      22        8593 :         read_radio();
      23             :     }
      24             : 
      25             :     // exit immediately if pre-arm rc checks fail
      26         918 :     if (!arming.rc_calibration_checks(true)) {
      27             :         // clear esc flag for next time
      28           1 :         if ((g.esc_calibrate != ESCCalibrationModes::ESCCAL_NONE) && (g.esc_calibrate != ESCCalibrationModes::ESCCAL_DISABLED)) {
      29           0 :             g.esc_calibrate.set_and_save(ESCCalibrationModes::ESCCAL_NONE);
      30             :         }
      31           1 :         return;
      32             :     }
      33             : 
      34             :     // check ESC parameter
      35         917 :     switch (g.esc_calibrate) {
      36         917 :         case ESCCalibrationModes::ESCCAL_NONE:
      37             :             // check if throttle is high
      38         917 :             if (channel_throttle->get_control_in() >= ESC_CALIBRATION_HIGH_THROTTLE) {
      39             :                 // we will enter esc_calibrate mode on next reboot
      40           0 :                 g.esc_calibrate.set_and_save(ESCCalibrationModes::ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH);
      41             :                 // send message to gcs
      42           0 :                 gcs().send_text(MAV_SEVERITY_CRITICAL,"ESC calibration: Restart board");
      43             :                 // turn on esc calibration notification
      44           0 :                 AP_Notify::flags.esc_calibration = true;
      45             :                 // block until we restart
      46           0 :                 while(1) { hal.scheduler->delay(5); }
      47             :             }
      48         917 :             break;
      49           0 :         case ESCCalibrationModes::ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH:
      50             :             // check if throttle is high
      51           0 :             if (channel_throttle->get_control_in() >= ESC_CALIBRATION_HIGH_THROTTLE) {
      52             :                 // pass through pilot throttle to escs
      53           0 :                 esc_calibration_passthrough();
      54             :             }
      55           0 :             break;
      56           0 :         case ESCCalibrationModes::ESCCAL_PASSTHROUGH_ALWAYS:
      57             :             // pass through pilot throttle to escs
      58           0 :             esc_calibration_passthrough();
      59           0 :             break;
      60           0 :         case ESCCalibrationModes::ESCCAL_AUTO:
      61             :             // perform automatic ESC calibration
      62           0 :             esc_calibration_auto();
      63           0 :             break;
      64           0 :         case ESCCalibrationModes::ESCCAL_DISABLED:
      65             :         default:
      66             :             // do nothing
      67           0 :             break;
      68             :     }
      69             : 
      70             :     // clear esc flag for next time
      71         917 :     if (g.esc_calibrate != ESCCalibrationModes::ESCCAL_DISABLED) {
      72         917 :         g.esc_calibrate.set_and_save(ESCCalibrationModes::ESCCAL_NONE);
      73             :     }
      74             : #endif  // FRAME_CONFIG != HELI_FRAME
      75             : }
      76             : 
      77             : // esc_calibration_passthrough - pass through pilot throttle to escs
      78           0 : void Copter::esc_calibration_passthrough()
      79             : {
      80             : #if FRAME_CONFIG != HELI_FRAME
      81             :     // send message to GCS
      82           0 :     gcs().send_text(MAV_SEVERITY_INFO,"ESC calibration: Passing pilot throttle to ESCs");
      83             : 
      84           0 :     esc_calibration_setup();
      85             : 
      86             :     while(1) {
      87             :         // flash LEDs
      88           0 :         esc_calibration_notify();
      89             : 
      90             :         // read pilot input
      91           0 :         read_radio();
      92             : 
      93             :         // we run at high rate to make oneshot ESCs happy. Normal ESCs
      94             :         // will only see pulses at the RC_SPEED
      95           0 :         hal.scheduler->delay(3);
      96             : 
      97             :         // pass through to motors
      98           0 :         auto &srv = AP::srv();
      99           0 :         srv.cork();
     100           0 :         motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f);
     101           0 :         srv.push();
     102           0 :     }
     103             : #endif  // FRAME_CONFIG != HELI_FRAME
     104           0 : }
     105             : 
     106             : // esc_calibration_auto - calibrate the ESCs automatically using a timer and no pilot input
     107           0 : void Copter::esc_calibration_auto()
     108             : {
     109             : #if FRAME_CONFIG != HELI_FRAME
     110             :     // send message to GCS
     111           0 :     gcs().send_text(MAV_SEVERITY_INFO,"ESC calibration: Auto calibration");
     112             : 
     113           0 :     esc_calibration_setup();
     114             : 
     115             :     // raise throttle to maximum
     116           0 :     auto &srv = AP::srv();
     117           0 :     srv.cork();
     118           0 :     motors->set_throttle_passthrough_for_esc_calibration(1.0f);
     119           0 :     srv.push();
     120             : 
     121             :     // delay for 5 seconds while outputting pulses
     122           0 :     uint32_t tstart = millis();
     123           0 :     while (millis() - tstart < 5000) {
     124           0 :         srv.cork();
     125           0 :         motors->set_throttle_passthrough_for_esc_calibration(1.0f);
     126           0 :         srv.push();
     127           0 :         esc_calibration_notify();
     128           0 :         hal.scheduler->delay(3);
     129             :     }
     130             : 
     131             :     // block until we restart
     132             :     while(1) {
     133           0 :         srv.cork();
     134           0 :         motors->set_throttle_passthrough_for_esc_calibration(0.0f);
     135           0 :         srv.push();
     136           0 :         esc_calibration_notify();
     137           0 :         hal.scheduler->delay(3);
     138             :     }
     139             : #endif // FRAME_CONFIG != HELI_FRAME
     140           0 : }
     141             : 
     142             : // flash LEDs to notify the user that ESC calibration is happening
     143           0 : void Copter::esc_calibration_notify()
     144             : {
     145           0 :     AP_Notify::flags.esc_calibration = true;
     146           0 :     uint32_t now = AP_HAL::millis();
     147           0 :     if (now - esc_calibration_notify_update_ms > 20) {
     148           0 :         esc_calibration_notify_update_ms = now;
     149           0 :         notify.update();
     150             :     }
     151           0 : }
     152             : 
     153           0 : void Copter::esc_calibration_setup()
     154             : {
     155             :     // clear esc flag for next time
     156           0 :     g.esc_calibrate.set_and_save(ESCCAL_NONE);
     157             : 
     158           0 :     if (motors->is_normal_pwm_type()) {
     159             :         // run at full speed for oneshot ESCs (actually done on push)
     160           0 :         motors->set_update_rate(g.rc_speed);
     161             :     } else {
     162             :         // reduce update rate to motors to 50Hz
     163           0 :         motors->set_update_rate(50);
     164             :     }
     165             : 
     166             :     // disable safety if requested
     167           0 :     BoardConfig.init_safety();
     168             : 
     169             :     // wait for safety switch to be pressed
     170           0 :     uint32_t tstart = 0;
     171           0 :     while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
     172           0 :         const uint32_t tnow = AP_HAL::millis();
     173           0 :         if (tnow - tstart >= 5000) {
     174           0 :             gcs().send_text(MAV_SEVERITY_INFO,"ESC calibration: Push safety switch");
     175           0 :             tstart = tnow;
     176             :         }
     177           0 :         esc_calibration_notify();
     178           0 :         hal.scheduler->delay(3);
     179             :     }
     180             : 
     181             :     // arm and enable motors
     182           0 :     motors->armed(true);
     183           0 :     SRV_Channels::enable_by_mask(motors->get_motor_mask());
     184           0 :     hal.util->set_soft_armed(true);
     185           0 : }