Line data    Source code

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