Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3             : /*
       4             :   allow for runtime change of control channel ordering
       5             :  */
       6       12811 : void Rover::set_control_channels(void)
       7             : {
       8             :     // check change on RCMAP
       9             :     // the library guarantees that these are non-nullptr:
      10       12811 :     channel_steer    = &rc().get_roll_channel();
      11       12811 :     channel_throttle = &rc().get_throttle_channel();
      12       12811 :     channel_lateral  = &rc().get_yaw_channel();
      13             : 
      14             :     // set rc channel ranges
      15       12811 :     channel_steer->set_angle(SERVO_MAX);
      16       12811 :     channel_throttle->set_angle(100);
      17       12811 :     if (channel_lateral != nullptr) {
      18       12811 :         channel_lateral->set_angle(100);
      19             :     }
      20             : 
      21             :     // walking robots rc input init
      22       12811 :     channel_roll = rc().find_channel_for_option(RC_Channel::AUX_FUNC::ROLL);
      23       12811 :     channel_pitch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::PITCH);
      24       12811 :     channel_walking_height = rc().find_channel_for_option(RC_Channel::AUX_FUNC::WALKING_HEIGHT);
      25       12811 :     if (channel_roll != nullptr) {
      26           0 :         channel_roll->set_angle(SERVO_MAX);
      27           0 :         channel_roll->set_default_dead_zone(30);
      28             :     }
      29       12811 :     if (channel_pitch != nullptr) {
      30           0 :         channel_pitch->set_angle(SERVO_MAX);
      31           0 :         channel_pitch->set_default_dead_zone(30);
      32             :     }
      33       12811 :     if (channel_walking_height != nullptr) {
      34           0 :         channel_walking_height->set_angle(SERVO_MAX);
      35           0 :         channel_walking_height->set_default_dead_zone(30);
      36             :     }    
      37             : 
      38             :     // sailboat rc input init
      39       12811 :     g2.sailboat.init_rc_in();
      40             : 
      41             :     // Allow to reconfigure output when not armed
      42       12811 :     if (!arming.is_armed()) {
      43        7799 :         g2.motors.setup_servo_output();
      44             :         // For a rover safety is TRIM throttle
      45        7799 :         g2.motors.setup_safety_output();
      46             :     }
      47             :     // setup correct scaling for ESCs like the UAVCAN ESCs which
      48             :     // take a proportion of speed. Default to 1000 to 2000 for systems without
      49             :     // a k_throttle output
      50       12811 :     hal.rcout->set_esc_scaling(1000, 2000);
      51       12811 :     g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle);
      52       12811 : }
      53             : 
      54         283 : void Rover::init_rc_in()
      55             : {
      56             :     // set rc dead zones
      57         283 :     channel_steer->set_default_dead_zone(30);
      58         283 :     channel_throttle->set_default_dead_zone(30);
      59         283 :     if (channel_lateral != nullptr) {
      60         283 :         channel_lateral->set_default_dead_zone(30);
      61             :     }
      62         283 : }
      63             : 
      64      629048 : void Rover::read_radio()
      65             : {
      66      629048 :     if (!rc().read_input()) {
      67             :         // check if we lost RC link
      68        7147 :         radio_failsafe_check(channel_throttle->get_radio_in());
      69        7147 :         return;
      70             :     }
      71             : 
      72      621901 :     failsafe.last_valid_rc_ms = AP_HAL::millis();
      73             :     // check that RC value are valid
      74      621901 :     radio_failsafe_check(channel_throttle->get_radio_in());
      75             : }
      76             : 
      77      629048 : void Rover::radio_failsafe_check(uint16_t pwm)
      78             : {
      79      629048 :     if (!g.fs_throttle_enabled) {
      80             :         // radio failsafe disabled
      81       13020 :         AP_Notify::flags.failsafe_radio = false;
      82       13020 :         return;
      83             :     }
      84             : 
      85      616028 :     bool failed = pwm < static_cast<uint16_t>(g.fs_throttle_value);
      86      616028 :     if (AP_HAL::millis() - failsafe.last_valid_rc_ms > rc().get_fs_timeout_ms()) {
      87             :         // we haven't had a valid RC frame for RC_FS_TIMEOUT seconds
      88         920 :         failed = true;
      89             :     }
      90      616028 :     AP_Notify::flags.failsafe_radio = failed;
      91      616028 :     failsafe_trigger(FAILSAFE_EVENT_THROTTLE, "Radio", failed);
      92             : }