Line data    Source code

       1             : #include "AP_Winch_PWM.h"
       2             : 
       3             : #if AP_WINCH_PWM_ENABLED
       4             : 
       5             : #include <AP_Logger/AP_Logger.h>
       6             : #include <GCS_MAVLink/GCS.h>
       7             : #include <SRV_Channel/SRV_Channel.h>
       8             : 
       9             : extern const AP_HAL::HAL& hal;
      10             : 
      11             : // true if winch is healthy
      12        2354 : bool AP_Winch_PWM::healthy() const
      13             : {
      14             :     // return immediately if no servo is assigned to control the winch
      15        2354 :     if (!SRV_Channels::function_assigned(SRV_Channel::k_winch)) {
      16           0 :         return false;
      17             :     }
      18             : 
      19        2354 :     return true;
      20             : }
      21             : 
      22        5120 : void AP_Winch_PWM::update()
      23             : {
      24             :     // return immediately if no servo is assigned to control the winch
      25        5120 :     if (!SRV_Channels::function_assigned(SRV_Channel::k_winch)) {
      26           0 :         return;
      27             :     }
      28             : 
      29             :     // read pilot input
      30        5120 :     read_pilot_desired_rate();
      31             : 
      32             :     // send outputs to winch
      33        5120 :     control_winch();
      34             : }
      35             : 
      36             : // update pwm outputs to control winch
      37        5120 : void AP_Winch_PWM::control_winch()
      38             : {
      39        5120 :     const uint32_t now_ms = AP_HAL::millis();
      40        5120 :     float dt = (now_ms - control_update_ms) * 0.001f;
      41        5120 :     if (dt > 1.0f) {
      42           2 :         dt = 0.0f;
      43             :     }
      44        5120 :     control_update_ms = now_ms;
      45             : 
      46             :     // if relaxed stop outputting pwm signals
      47        5120 :     if (config.control_mode == AP_Winch::ControlMode::RELAXED) {
      48             :         // if not doing any control stop sending pwm to winch
      49        4262 :         SRV_Channels::set_output_pwm(SRV_Channel::k_winch, 0);
      50             : 
      51             :         // rate used for acceleration limiting reset to zero
      52        4262 :         set_previous_rate(0.0f);
      53        4262 :         return;
      54             :     }
      55             : 
      56             :     // if doing position control, calculate position error to desired rate
      57         858 :     if ((config.control_mode == AP_Winch::ControlMode::POSITION) && healthy()) {
      58         718 :         float position_error = config.length_desired - line_length;
      59         718 :         config.rate_desired = constrain_float(position_error * config.pos_p, -config.rate_max, config.rate_max);
      60             :     }
      61             : 
      62             :     // apply acceleration limited to rate
      63         858 :     const float rate_limited = get_rate_limited_by_accel(config.rate_desired, dt);
      64             : 
      65             :     // use linear interpolation to calculate output to move winch at desired rate
      66         858 :     const float scaled_output = linear_interpolate(-1000, 1000, rate_limited, -config.rate_max, config.rate_max);
      67         858 :     SRV_Channels::set_output_scaled(SRV_Channel::k_winch, scaled_output);
      68             : 
      69             :     // update distance estimate assuming winch will move exactly as requested
      70         858 :     line_length += config.rate_desired * dt;
      71             : }
      72             : 
      73             : //send generator status
      74         510 : void AP_Winch_PWM::send_status(const GCS_MAVLINK &channel)
      75             : {
      76             :     // prepare status bitmask
      77         510 :     uint32_t status_bitmask = 0;
      78         510 :     if (healthy()) {
      79         510 :         status_bitmask |= MAV_WINCH_STATUS_HEALTHY;
      80             :     }
      81             : 
      82             :     // send status
      83         510 :     mavlink_msg_winch_status_send(
      84             :         channel.get_chan(),
      85             :         AP_HAL::micros64(),     // time_usec
      86             :         line_length,            // line_length
      87         510 :         config.rate_desired,    // speed
      88         510 :         std::numeric_limits<double>::quiet_NaN(),   // tension
      89         510 :         std::numeric_limits<double>::quiet_NaN(),   // voltage
      90         510 :         std::numeric_limits<double>::quiet_NaN(),   // current
      91             :         INT16_MAX,              // temperature
      92             :         status_bitmask);        // status flags
      93         510 : }
      94             : 
      95             : // write log
      96             : #if HAL_LOGGING_ENABLED
      97        1024 : void AP_Winch_PWM::write_log()
      98             : {
      99        2048 :     AP::logger().Write_Winch(
     100        1024 :             healthy(),
     101             :             0,              // thread end (unsupported)
     102             :             0,              // moving (unsupported)
     103             :             0,              // clutch (unsupported)
     104        1024 :             (uint8_t)config.control_mode,
     105        1024 :             config.length_desired,
     106        1024 :             get_current_length(),
     107        1024 :             config.rate_desired,
     108             :             0,              // tension (unsupported)
     109             :             0,              // voltage (unsupported)
     110             :             0);             // temp (unsupported)
     111        1024 : }
     112             : #endif
     113             : 
     114             : #endif  // AP_WINCH_PWM_ENABLED