Copter: integrate winch changes

includes the following changes
winch_update called at 50hz
removed ability to set winch rate from ch6 tuning
remove wheel encoder
call winch library to log at 10hz
fix winch param prefix

ArduCopter/Copter.cpp

@@ -179,7 +179,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_Gripper,           &copter.g2.gripper,          update,          10,  75),
 #endif
 #if WINCH_ENABLED == ENABLED
-    SCHED_TASK(winch_update,          10,     50),
+    SCHED_TASK_CLASS(AP_Winch,             &copter.g2.winch,            update,          50,  50),
 #endif
 #if GENERATOR_ENABLED
     SCHED_TASK_CLASS(AP_Generator_RichenPower,     &copter.generator,      update,    10,     50),
@@ -426,6 +426,11 @@ void Copter::ten_hz_logging_loop()
 #if FRAME_CONFIG == HELI_FRAME
     Log_Write_Heli();
 #endif
+#if WINCH_ENABLED == ENABLED
+    if (should_log(MASK_LOG_ANY)) {
+        g2.winch.write_log();
+    }
+#endif
 }
 
 // twentyfive_hz_logging - should be run at 25hz

ArduCopter/Copter.h

@@ -154,7 +154,6 @@
  # include "toy_mode.h"
 #endif
 #if WINCH_ENABLED == ENABLED
- # include <AP_WheelEncoder/AP_WheelEncoder.h>
  # include <AP_Winch/AP_Winch.h>
 #endif
 #if RPM_ENABLED == ENABLED
@@ -851,8 +850,6 @@ private:
     void accel_cal_update(void);
     void init_proximity();
     void update_proximity();
-    void winch_init();
-    void winch_update();
 
     // RC_Channel.cpp
     void save_trim();

ArduCopter/GCS_Mavlink.cpp

@@ -809,20 +809,14 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
         switch ((uint8_t)packet.param2) {
         case WINCH_RELAXED:
             copter.g2.winch.relax();
-            AP::logger().Write_Event(LogEvent::WINCH_RELAXED);
             return MAV_RESULT_ACCEPTED;
         case WINCH_RELATIVE_LENGTH_CONTROL: {
-            copter.g2.winch.release_length(packet.param3, fabsf(packet.param4));
+            copter.g2.winch.release_length(packet.param3);
-            AP::logger().Write_Event(LogEvent::WINCH_LENGTH_CONTROL);
             return MAV_RESULT_ACCEPTED;
         }
         case WINCH_RATE_CONTROL:
-            if (fabsf(packet.param4) <= copter.g2.winch.get_rate_max()) {
+            copter.g2.winch.set_desired_rate(packet.param4);
-                copter.g2.winch.set_desired_rate(packet.param4);
+            return MAV_RESULT_ACCEPTED;
-                AP::logger().Write_Event(LogEvent::WINCH_RATE_CONTROL);
-                return MAV_RESULT_ACCEPTED;
-            }
-            return MAV_RESULT_FAILED;
         default:
             break;
         }

ArduCopter/Parameters.cpp

@@ -333,7 +333,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @DisplayName: Channel 6 Tuning
     // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob
     // @User: Standard
-    // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,57:Winch,58:SysID Magnitude
+    // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,58:SysID Magnitude
     GSCALAR(radio_tuning, "TUNE",                   0),
 
     // @Param: FRAME_TYPE
@@ -863,11 +863,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
 #endif
 
 #if WINCH_ENABLED == ENABLED
-    // @Group: WENC
+    // 22 was AP_WheelEncoder
-    // @Path: ../libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
-    AP_SUBGROUPINFO(wheel_encoder, "WENC", 22, ParametersG2, AP_WheelEncoder),
 
-    // @Group: WINCH_
+    // @Group: WINCH
     // @Path: ../libraries/AP_Winch/AP_Winch.cpp
     AP_SUBGROUPINFO(winch, "WINCH", 23, ParametersG2, AP_Winch),
 #endif

ArduCopter/Parameters.h

@@ -549,7 +549,6 @@ public:
 
     // wheel encoder and winch
 #if WINCH_ENABLED == ENABLED
-    AP_WheelEncoder wheel_encoder;
     AP_Winch winch;
 #endif
 

ArduCopter/RC_Channel.cpp

@@ -448,36 +448,20 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi
 #if WINCH_ENABLED == ENABLED
             switch (ch_flag) {
                 case AuxSwitchPos::HIGH:
-                    // high switch maintains current position
+                    // high switch maintains changes the winch to rate control mode
-                    copter.g2.winch.release_length(0.0f);
+                    copter.g2.winch.set_desired_rate(0.0f);
-                    AP::logger().Write_Event(LogEvent::WINCH_LENGTH_CONTROL);
                     break;
                 case AuxSwitchPos::MIDDLE:
                 case AuxSwitchPos::LOW:
                     // all other position relax winch
                     copter.g2.winch.relax();
-                    AP::logger().Write_Event(LogEvent::WINCH_RELAXED);
                     break;
                 }
 #endif
             break;
 
         case AUX_FUNC::WINCH_CONTROL:
-#if WINCH_ENABLED == ENABLED
+            // do nothing, used to control the rate of the winch and is processed within AP_Winch
-            switch (ch_flag) {
-                case AuxSwitchPos::LOW:
-                    // raise winch at maximum speed
-                    copter.g2.winch.set_desired_rate(-copter.g2.winch.get_rate_max());
-                    break;
-                case AuxSwitchPos::HIGH:
-                    // lower winch at maximum speed
-                    copter.g2.winch.set_desired_rate(copter.g2.winch.get_rate_max());
-                    break;
-                case AuxSwitchPos::MIDDLE:
-                    copter.g2.winch.set_desired_rate(0.0f);
-                    break;
-                }
-#endif
             break;
 
 #ifdef USERHOOK_AUXSWITCH

ArduCopter/defines.h

@@ -73,7 +73,7 @@ enum tuning_func {
     TUNING_RATE_YAW_FF =                54, // body frame yaw rate controller FF term
     TUNING_RATE_MOT_YAW_HEADROOM =      55, // motors yaw headroom minimum
     TUNING_RATE_YAW_FILT =              56, // yaw rate input filter
-    TUNING_WINCH =                      57, // winch control (not actually a value to be tuned)
+    UNUSED =                            57, // was winch control
     TUNING_SYSTEM_ID_MAGNITUDE =        58  // magnitude of the system ID signal
 };
 

ArduCopter/mode_auto.cpp

@@ -1436,15 +1436,12 @@ void ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
     switch (cmd.content.winch.action) {
         case WINCH_RELAXED:
             g2.winch.relax();
-            AP::logger().Write_Event(LogEvent::WINCH_RELAXED);
             break;
         case WINCH_RELATIVE_LENGTH_CONTROL:
-            g2.winch.release_length(cmd.content.winch.release_length, cmd.content.winch.release_rate);
+            g2.winch.release_length(cmd.content.winch.release_length);
-            AP::logger().Write_Event(LogEvent::WINCH_LENGTH_CONTROL);
             break;
         case WINCH_RATE_CONTROL:
             g2.winch.set_desired_rate(cmd.content.winch.release_rate);
-            AP::logger().Write_Event(LogEvent::WINCH_RATE_CONTROL);
             break;
         default:
             // do nothing

ArduCopter/sensors.cpp

@@ -223,21 +223,3 @@ void Copter::init_proximity(void)
     g2.proximity.init();
 #endif
 }
-
-// winch and wheel encoder initialisation
-void Copter::winch_init()
-{
-#if WINCH_ENABLED == ENABLED
-    g2.wheel_encoder.init();
-    g2.winch.init(&g2.wheel_encoder);
-#endif
-}
-
-// winch and wheel encoder update
-void Copter::winch_update()
-{
-#if WINCH_ENABLED == ENABLED
-    g2.wheel_encoder.update();
-    g2.winch.update();
-#endif
-}

ArduCopter/system.cpp

@@ -36,7 +36,9 @@ void Copter::init_ardupilot()
 #endif
 
     // init winch and wheel encoder
-    winch_init();
+#if WINCH_ENABLED == ENABLED
+    g2.winch.init();
+#endif
 
     // initialise notify system
     notify.init();

ArduCopter/tuning.cpp

@@ -187,16 +187,6 @@ void Copter::tuning()
          attitude_control->get_rate_yaw_pid().filt_E_hz(tuning_value);
          break;
 
-#if WINCH_ENABLED == ENABLED
-     case TUNING_WINCH:
-         // add small deadzone
-         if (fabsf(tuning_value) < 0.05f) {
-             tuning_value = 0;
-         }
-         g2.winch.set_desired_rate(tuning_value);
-         break;
-#endif
-
      case TUNING_SYSTEM_ID_MAGNITUDE:
 #if MODE_SYSTEMID_ENABLED == ENABLED
          copter.mode_systemid.set_magnitude(tuning_value);