Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3          83 : void Sub::init_rc_in()
       4             : {
       5          83 :     channel_roll     = &rc().get_roll_channel();
       6          83 :     channel_pitch    = &rc().get_pitch_channel();
       7          83 :     channel_throttle = &rc().get_throttle_channel();
       8          83 :     channel_yaw      = &rc().get_yaw_channel();
       9          83 :     channel_forward  = &rc().get_forward_channel();
      10          83 :     channel_lateral  = &rc().get_lateral_channel();
      11             : 
      12             :     // set rc channel ranges
      13          83 :     channel_roll->set_angle(ROLL_PITCH_INPUT_MAX);
      14          83 :     channel_pitch->set_angle(ROLL_PITCH_INPUT_MAX);
      15          83 :     channel_yaw->set_angle(ROLL_PITCH_INPUT_MAX);
      16          83 :     channel_throttle->set_range(1000);
      17          83 :     channel_forward->set_angle(ROLL_PITCH_INPUT_MAX);
      18          83 :     channel_lateral->set_angle(ROLL_PITCH_INPUT_MAX);
      19             : 
      20             :     // set default dead zones
      21          83 :     channel_roll->set_default_dead_zone(30);
      22          83 :     channel_pitch->set_default_dead_zone(30);
      23          83 :     channel_throttle->set_default_dead_zone(30);
      24          83 :     channel_yaw->set_default_dead_zone(40);
      25          83 :     channel_forward->set_default_dead_zone(30);
      26          83 :     channel_lateral->set_default_dead_zone(30);
      27             : 
      28             :     // initialize rc input to 1500 on control channels (rather than 0)
      29          83 :     uint32_t tnow = AP_HAL::millis();
      30          83 :     channel_roll->set_override(1500, tnow);
      31          83 :     channel_pitch->set_override(1500, tnow);
      32          83 :     channel_yaw->set_override(1500, tnow);
      33          83 :     channel_throttle->set_override(1500, tnow);
      34          83 :     channel_forward->set_override(1500, tnow);
      35          83 :     channel_lateral->set_override(1500, tnow);
      36             : 
      37             : #if HAL_MOUNT_ENABLED
      38             :     // initialize camera mount RC inputs to centered
      39          83 :     RC_Channel *cam_pan_chan = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOUNT1_YAW);
      40          83 :     if (cam_pan_chan != nullptr) {
      41          83 :         cam_pan_chan->set_override(1500, tnow);
      42             :     }
      43          83 :     RC_Channel *cam_tilt_chan = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOUNT1_PITCH);
      44          83 :     if (cam_tilt_chan != nullptr) {
      45          83 :         cam_tilt_chan->set_override(1500, tnow);
      46             :     }
      47             : #endif  // HAL_MOUNT_ENABLED
      48          83 : }
      49             : 
      50             : // init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration
      51          83 : void Sub::init_rc_out()
      52             : {
      53          83 :     motors.set_update_rate(g.rc_speed);
      54          83 :     motors.init((AP_Motors::motor_frame_class)g.frame_configuration.get(), AP_Motors::motor_frame_type::MOTOR_FRAME_TYPE_PLUS);
      55          83 :     motors.convert_pwm_min_max_param(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
      56          83 :     motors.update_throttle_range();
      57             : 
      58             :     // enable output to motors
      59          83 :     if (arming.rc_calibration_checks(true)) {
      60          83 :         enable_motor_output();
      61             :     }
      62             : 
      63             :     // refresh auxiliary channel to function map
      64          83 :     SRV_Channels::update_aux_servo_function();
      65          83 : }
      66             : #if AP_SUB_RC_ENABLED
      67     2180683 : void Sub::read_radio()
      68             : { 
      69     2180683 :     const uint32_t tnow_ms = AP_HAL::millis();
      70             : 
      71     2180683 :     if (rc().read_input()) {
      72             :         //got valid input
      73      271795 :         last_radio_update_ms = tnow_ms;
      74      271795 :         failsafe.last_pilot_input_ms = tnow_ms;
      75      271795 :         set_throttle_and_failsafe(channel_throttle->get_radio_in());
      76      271795 :         return;
      77             :     }
      78             :     
      79             :    // No radio input this time
      80     1908888 :     if (failsafe.radio) {
      81             :         // already in failsafe! no further action
      82         841 :         return;
      83             :     }
      84             : 
      85             :     // trigger failsafe if no update from the RC Radio for RC_FS_TIMEOUT seconds
      86     1908047 :     const uint32_t elapsed_ms = tnow_ms - last_radio_update_ms;
      87     1908047 :     if (elapsed_ms < rc().get_fs_timeout_ms()) {
      88             :         // not timed out yet
      89     1907924 :         return;
      90             :     }
      91         123 :     if (!g.failsafe_throttle) {
      92             :         // throttle failsafe not enabled
      93         122 :         return;
      94             :     }
      95           1 :     if (!rc().has_ever_seen_rc_input() && !sub.motors.armed()) {
      96             :         // we only failsafe if we are armed OR we have ever seen an RC receiver
      97           0 :         return;
      98             :     }
      99             :     
     100             :     // Log an error and enter failsafe.
     101           1 :     LOGGER_WRITE_ERROR(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME);
     102           1 :     set_failsafe_radio(true);
     103             : }
     104             : 
     105             : #define FS_COUNTER 3        // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value
     106      271795 : void Sub::set_throttle_and_failsafe(uint16_t throttle_pwm)
     107             : {
     108             :     // if failsafe not enabled pass through throttle, clear RC failsafe if it exists, and exit
     109      271795 :     if(g.failsafe_throttle == FS_THR_DISABLED) {
     110      271795 :         set_failsafe_radio(false);
     111      271795 :         return;
     112             :     }
     113             : 
     114             :     //check for low throttle value
     115           0 :     if (throttle_pwm < (uint16_t)g.failsafe_throttle_value) {
     116             : 
     117             :         // if we are already in failsafe or motors not armed pass through throttle and exit
     118           0 :         if (failsafe.radio || !(rc().has_ever_seen_rc_input() || sub.motors.armed())) {
     119           0 :             return;
     120             :         }
     121             : 
     122             :         // check for 3 low throttle values
     123             :         // Note: we do not pass through the low throttle until 3 low throttle values are received
     124           0 :         failsafe.radio_counter++;
     125           0 :         if( failsafe.radio_counter >= FS_COUNTER ) {
     126           0 :             failsafe.radio_counter = FS_COUNTER;  // check to ensure we don't overflow the counter
     127           0 :             set_failsafe_radio(true);
     128             :         }
     129             :     }else{
     130             :         // we have a good throttle so reduce failsafe counter
     131           0 :         failsafe.radio_counter--;
     132           0 :         if( failsafe.radio_counter <= 0 ) {
     133           0 :             failsafe.radio_counter = 0;   // check to ensure we don't underflow the counter
     134             : 
     135             :             // disengage failsafe after three (nearly) consecutive valid throttle values
     136           0 :             if (failsafe.radio) {
     137           0 :                 set_failsafe_radio(false);
     138             :             }
     139             :         }
     140             :         // pass through throttle
     141             :     }
     142             : }
     143             : #endif