Line data    Source code

       1             : #include <AP_Winch/AP_Winch_Backend.h>
       2             : 
       3             : #if AP_WINCH_ENABLED
       4             : 
       5             : #include <RC_Channel/RC_Channel.h>
       6             : #include <SRV_Channel/SRV_Channel.h>
       7             : 
       8             : // setup rc input and output
       9           2 : void AP_Winch_Backend::init()
      10             : {
      11             :     // set servo output range
      12           2 :     SRV_Channels::set_angle(SRV_Channel::k_winch,  1000);
      13             : 
      14           2 :     RC_Channel *rc_input = rc().find_channel_for_option(RC_Channel::AUX_FUNC::WINCH_CONTROL);
      15           2 :     if (rc_input != nullptr) {
      16             :         // rc input deadzone is required or pilot input will always override autonomous control of winch
      17           2 :         rc_input->set_default_dead_zone(30);
      18             :     }
      19           2 : }
      20             : 
      21             : // calculate the pilot desired rate (+ve deploys line, -ve retracts line, 0 stops) from rc input
      22             : // may change the state to RATE and update config.rate_desired
      23        5120 : void AP_Winch_Backend::read_pilot_desired_rate()
      24             : {
      25             :     // fail if no input channel defined
      26        5120 :     const RC_Channel *rc_input = rc().find_channel_for_option(RC_Channel::AUX_FUNC::WINCH_CONTROL);
      27        5120 :     if (rc_input == nullptr) {
      28           0 :         return;
      29             :     }
      30             : 
      31             :     // if pilot is controlling winch, set rate to zero during RC failsafe
      32        5120 :     if (!rc().has_valid_input() && (config.control_mode == AP_Winch::ControlMode::RATE_FROM_RC)) {
      33           0 :         config.rate_desired = 0.0f;
      34           0 :         return;
      35             :     }
      36             : 
      37             :     // initialise previous_radio_in
      38        5120 :     if (previous_radio_in == -1) {
      39           2 :         previous_radio_in = rc_input->get_radio_in();
      40             :     }
      41             : 
      42             :     // if significant change in rc input switch to rate mode
      43        5120 :     const int16_t radio_in = rc_input->get_radio_in();
      44        5120 :     if (config.control_mode != AP_Winch::ControlMode::RATE_FROM_RC) {
      45        5120 :         if (abs(radio_in - previous_radio_in) > rc_input->get_dead_zone()) {
      46           0 :             config.control_mode = AP_Winch::ControlMode::RATE_FROM_RC;
      47             :         }
      48             :     }
      49             : 
      50             :     // update desired rate
      51        5120 :     if (config.control_mode == AP_Winch::ControlMode::RATE_FROM_RC) {
      52           0 :         config.rate_desired = rc_input->norm_input_dz() * config.rate_max;
      53           0 :         previous_radio_in = radio_in;
      54             :     }
      55             : }
      56             : 
      57             : // returns the rate limited by the maximum acceleration
      58             : // this also updates the previous rate so should not be called more than once per loop
      59         858 : float AP_Winch_Backend::get_rate_limited_by_accel(float rate, float dt)
      60             : {
      61             :     // use maximum rate x 2 for maximum acceleration
      62         858 :     const float rate_change_max = MAX(config.rate_max * 2.0f, 0.1f) * dt;
      63             : 
      64             :     // apply acceleration limit
      65         858 :     const float rate_limited = constrain_float(rate, previous_rate - rate_change_max, previous_rate + rate_change_max);
      66             : 
      67             :     // update previous rate for next time
      68         858 :     set_previous_rate(rate_limited);
      69             : 
      70         858 :     return rate_limited;
      71             : }
      72             : 
      73             : #endif  // AP_WINCH_ENABLED