ArduCopter/radio.cpp

   1 #include "Copter.h"
   2
   3
   4 // Function that will read the radio data, limit servos and trigger a failsafe
   5 // ----------------------------------------------------------------------------
   6
   7 void Copter::default_dead_zones()
   8 {
   9     channel_roll->set_default_dead_zone(20);
  10     channel_pitch->set_default_dead_zone(20);
  11 #if FRAME_CONFIG == HELI_FRAME
  12     channel_throttle->set_default_dead_zone(10);
  13     channel_yaw->set_default_dead_zone(15);
  14 #else
  15     channel_throttle->set_default_dead_zone(30);
  16     channel_yaw->set_default_dead_zone(20);
  17 #endif
  18     rc().channel(CH_6)->set_default_dead_zone(0);
  19 }
  20
  21 void Copter::init_rc_in()
  22 {
  23     // the library gaurantees that these are non-nullptr:
  24     channel_roll     = &rc().get_roll_channel();
  25     channel_pitch    = &rc().get_pitch_channel();
  26     channel_throttle = &rc().get_throttle_channel();
  27     channel_yaw      = &rc().get_yaw_channel();
  28
  29     // set rc channel ranges
  30     channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
  31     channel_pitch->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
  32     channel_yaw->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
  33     channel_throttle->set_range(1000);
  34
  35     // set default dead zones
  36     default_dead_zones();
  37
  38     // initialise throttle_zero flag
  39     ap.throttle_zero = true;
  40 }
  41
  42  // init_rc_out -- initialise motors
  43 void Copter::init_rc_out()
  44 {
  45     motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
  46
  47     // enable aux servos to cope with multiple output channels per motor
  48     AP::srv().enable_aux_servos();
  49
  50     // update rate must be set after motors->init() to allow for motor mapping
  51     motors->set_update_rate(g.rc_speed);
  52
  53 #if FRAME_CONFIG != HELI_FRAME
  54     if (channel_throttle->configured()) {
  55         // throttle inputs setup, use those to set motor PWM min and max if not already configured
  56         motors->convert_pwm_min_max_param(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
  57     } else {
  58         // throttle inputs default, force set motor PWM min and max to defaults so they will not be over-written by a future change in RC min / max
  59         motors->convert_pwm_min_max_param(1000, 2000);
  60     }
  61     motors->update_throttle_range();
  62 #else
  63     // setup correct scaling for ESCs like the UAVCAN ESCs which
  64     // take a proportion of speed.
  65     hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
  66 #endif
  67
  68     // refresh auxiliary channel to function map
  69     SRV_Channels::update_aux_servo_function();
  70
  71 #if FRAME_CONFIG != HELI_FRAME
  72     /*
  73       setup a default safety ignore mask, so that servo gimbals can be active while safety is on
  74      */
  75     uint16_t safety_ignore_mask = (~copter.motors->get_motor_mask()) & 0x3FFF;
  76     BoardConfig.set_default_safety_ignore_mask(safety_ignore_mask);
  77 #endif
  78 }
  79
  80
  81 void Copter::read_radio()
  82 {
  83     const uint32_t tnow_ms = millis();
  84
  85     if (rc().read_input()) {
  86         ap.new_radio_frame = true;
  87
  88         set_throttle_and_failsafe(channel_throttle->get_radio_in());
  89         set_throttle_zero_flag(channel_throttle->get_control_in());
  90
  91         // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
  92         radio_passthrough_to_motors();
  93
  94         const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
  95         rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt);
  96         last_radio_update_ms = tnow_ms;
  97         return;
  98     }
  99
 100     // No radio input this time
 101     if (failsafe.radio) {
 102         // already in failsafe!
 103         return;
 104     }
 105
 106     // trigger failsafe if no update from the RC Radio for RC_FS_TIMEOUT seconds
 107     const uint32_t elapsed_ms = tnow_ms - last_radio_update_ms;
 108     if (elapsed_ms < rc().get_fs_timeout_ms()) {
 109         // not timed out yet
 110         return;
 111     }
 112     if (!g.failsafe_throttle) {
 113         // throttle failsafe not enabled
 114         return;
 115     }
 116     if (!rc().has_ever_seen_rc_input() && !motors->armed()) {
 117         // we only failsafe if we are armed OR we have ever seen an RC receiver
 118         return;
 119     }
 120
 121     // Log an error and enter failsafe.
 122     LOGGER_WRITE_ERROR(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME);
 123     set_failsafe_radio(true);
 124 }
 125
 126 #define FS_COUNTER 3        // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value
 127 void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm)
 128 {
 129     // if failsafe not enabled pass through throttle and exit
 130     if(g.failsafe_throttle == FS_THR_DISABLED) {
 131         return;
 132     }
 133
 134     //check for low throttle value
 135     if (throttle_pwm < (uint16_t)g.failsafe_throttle_value) {
 136
 137         // if we are already in failsafe or motors not armed pass through throttle and exit
 138         if (failsafe.radio || !(rc().has_ever_seen_rc_input() || motors->armed())) {
 139             return;
 140         }
 141
 142         // check for 3 low throttle values
 143         // Note: we do not pass through the low throttle until 3 low throttle values are received
 144         failsafe.radio_counter++;
 145         if( failsafe.radio_counter >= FS_COUNTER ) {
 146             failsafe.radio_counter = FS_COUNTER;  // check to ensure we don't overflow the counter
 147             set_failsafe_radio(true);
 148         }
 149     }else{
 150         // we have a good throttle so reduce failsafe counter
 151         failsafe.radio_counter--;
 152         if( failsafe.radio_counter <= 0 ) {
 153             failsafe.radio_counter = 0;   // check to ensure we don't underflow the counter
 154
 155             // disengage failsafe after three (nearly) consecutive valid throttle values
 156             if (failsafe.radio) {
 157                 set_failsafe_radio(false);
 158             }
 159         }
 160         // pass through throttle
 161     }
 162 }
 163
 164 #define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400
 165 // set_throttle_zero_flag - set throttle_zero flag from debounced throttle control
 166 // throttle_zero is used to determine if the pilot intends to shut down the motors
 167 // Basically, this signals when we are not flying.  We are either on the ground
 168 // or the pilot has shut down the copter in the air and it is free-falling
 169 void Copter::set_throttle_zero_flag(int16_t throttle_control)
 170 {
 171     static uint32_t last_nonzero_throttle_ms = 0;
 172     uint32_t tnow_ms = millis();
 173
 174     // if not using throttle interlock and non-zero throttle and not E-stopped,
 175     // or using motor interlock and it's enabled, then motors are running,
 176     // and we are flying. Immediately set as non-zero
 177     if ((!ap.using_interlock && (throttle_control > 0) && !SRV_Channels::get_emergency_stop()) ||
 178         (ap.using_interlock && motors->get_interlock()) ||
 179         ap.armed_with_airmode_switch || air_mode == AirMode::AIRMODE_ENABLED) {
 180         last_nonzero_throttle_ms = tnow_ms;
 181         ap.throttle_zero = false;
 182     } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) {
 183         ap.throttle_zero = true;
 184     }
 185 }
 186
 187 // pass pilot's inputs to motors library (used to allow wiggling servos while disarmed on heli, single, coax copters)
 188 void Copter::radio_passthrough_to_motors()
 189 {
 190     motors->set_radio_passthrough(channel_roll->norm_input(),
 191                                   channel_pitch->norm_input(),
 192                                   channel_throttle->get_control_in_zero_dz()*0.001f,
 193                                   channel_yaw->norm_input());
 194 }
 195
 196 /*
 197   return the throttle input for mid-stick as a control-in value
 198  */
 199 int16_t Copter::get_throttle_mid(void)
 200 {
 201 #if TOY_MODE_ENABLED
 202     if (g2.toy_mode.enabled()) {
 203         return g2.toy_mode.get_throttle_mid();
 204     }
 205 #endif
 206     return channel_throttle->get_control_mid();
 207 }