ArduCopter/esc_calibration.cpp

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