From 787954fa37e0cd539fe3555465380b61797efd04 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 5 Oct 2017 11:21:23 +0900 Subject: [PATCH] Copter: integrate winch library fixes after peer review: rename winch-disable to winch-relaxed add DO_WINCH to do-verify winch release-length accepts rate --- ArduCopter/ArduCopter.cpp | 1 + ArduCopter/Copter.h | 5 +++++ ArduCopter/GCS_Mavlink.cpp | 33 +++++++++++++++++++++++++++++++++ ArduCopter/Parameters.cpp | 22 +++++++++++++++------- ArduCopter/Parameters.h | 4 ++++ ArduCopter/commands_logic.cpp | 28 ++++++++++++++++++++++++++++ ArduCopter/defines.h | 8 +++++++- ArduCopter/sensors.cpp | 14 ++++++++++++++ ArduCopter/switches.cpp | 33 +++++++++++++++++++++++++++++++++ ArduCopter/system.cpp | 3 +++ ArduCopter/tuning.cpp | 14 +++++++++++++- 11 files changed, 156 insertions(+), 9 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 8e409bab12..aed896be98 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -142,6 +142,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if GRIPPER_ENABLED == ENABLED SCHED_TASK(gripper_update, 10, 75), #endif + SCHED_TASK(winch_update, 10, 50), #ifdef USERHOOK_FASTLOOP SCHED_TASK(userhook_FastLoop, 100, 75), #endif diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index da1c170d2f..4b06e7acdf 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -93,6 +93,8 @@ #include #include #include +#include +#include // Configuration #include "defines.h" @@ -1068,6 +1070,8 @@ private: void read_receiver_rssi(void); void epm_update(); void gripper_update(); + void winch_init(); + void winch_update(); void terrain_update(); void terrain_logging(); bool terrain_use(); @@ -1143,6 +1147,7 @@ private: #if GRIPPER_ENABLED == ENABLED void do_gripper(const AP_Mission::Mission_Command& cmd); #endif + void do_winch(const AP_Mission::Mission_Command& cmd); bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); bool verify_circle(const AP_Mission::Mission_Command& cmd); bool verify_spline_wp(const AP_Mission::Mission_Command& cmd); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 51da3c7a05..2d387f5dc9 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1199,6 +1199,39 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) break; #endif + case MAV_CMD_DO_WINCH: + // param1 : winch number (ignored) + // param2 : action (0=relax, 1=relative length control, 2=rate control). See WINCH_ACTIONS enum. + if (!copter.g2.winch.enabled()) { + result = MAV_RESULT_FAILED; + } else { + result = MAV_RESULT_ACCEPTED; + switch ((uint8_t)packet.param2) { + case WINCH_RELAXED: + copter.g2.winch.relax(); + copter.Log_Write_Event(DATA_WINCH_RELAXED); + break; + case WINCH_RELATIVE_LENGTH_CONTROL: { + copter.g2.winch.release_length(packet.param3, fabsf(packet.param4)); + copter.Log_Write_Event(DATA_WINCH_LENGTH_CONTROL); + break; + } + case WINCH_RATE_CONTROL: { + if (fabsf(packet.param4) < copter.g2.winch.get_rate_max()) { + copter.g2.winch.set_desired_rate(packet.param4); + copter.Log_Write_Event(DATA_WINCH_RATE_CONTROL); + } else { + result = MAV_RESULT_FAILED; + } + break; + } + default: + result = MAV_RESULT_FAILED; + break; + } + } + break; + /* Solo user presses Fly button */ case MAV_CMD_SOLO_BTN_FLY_CLICK: { result = MAV_RESULT_ACCEPTED; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index cc4adb4fd5..3cc8a2aaf2 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -361,7 +361,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 + // @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 GSCALAR(radio_tuning, "TUNE", 0), // @Param: TUNE_LOW @@ -389,42 +389,42 @@ const AP_Param::Info Copter::var_info[] = { // @Param: CH7_OPT // @DisplayName: Channel 7 option // @Description: Select which function is performed when CH7 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl // @User: Standard GSCALAR(ch7_option, "CH7_OPT", AUXSW_DO_NOTHING), // @Param: CH8_OPT // @DisplayName: Channel 8 option // @Description: Select which function is performed when CH8 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl // @User: Standard GSCALAR(ch8_option, "CH8_OPT", AUXSW_DO_NOTHING), // @Param: CH9_OPT // @DisplayName: Channel 9 option // @Description: Select which function is performed when CH9 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl // @User: Standard GSCALAR(ch9_option, "CH9_OPT", AUXSW_DO_NOTHING), // @Param: CH10_OPT // @DisplayName: Channel 10 option // @Description: Select which function is performed when CH10 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl // @User: Standard GSCALAR(ch10_option, "CH10_OPT", AUXSW_DO_NOTHING), // @Param: CH11_OPT // @DisplayName: Channel 11 option // @Description: Select which function is performed when CH11 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl // @User: Standard GSCALAR(ch11_option, "CH11_OPT", AUXSW_DO_NOTHING), // @Param: CH12_OPT // @DisplayName: Channel 12 option // @Description: Select which function is performed when CH12 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl // @User: Standard GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING), @@ -1010,6 +1010,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp AP_SUBGROUPINFO(smart_rtl, "SRTL_", 21, ParametersG2, AP_SmartRTL), + // @Group: WENC + // @Path: ../libraries/AP_WheelEncoder/AP_WheelEncoder.cpp + AP_SUBGROUPINFO(wheel_encoder, "WENC", 22, ParametersG2, AP_WheelEncoder), + + // @Group: WINCH_ + // @Path: ../libraries/AP_Winch/AP_Winch.cpp + AP_SUBGROUPINFO(winch, "WINCH", 23, ParametersG2, AP_Winch), + AP_GROUPEND }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 8784391474..45467682d3 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -570,6 +570,10 @@ public: // Safe RTL library AP_SmartRTL smart_rtl; + + // wheel encoder and winch + AP_WheelEncoder wheel_encoder; + AP_Winch winch; }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 7d023afcbb..8e2c82886a 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -160,6 +160,10 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd) break; #endif + case MAV_CMD_DO_WINCH: // Mission command to control winch + do_winch(cmd); + break; + default: // do nothing with unrecognized MAVLink messages break; @@ -269,6 +273,7 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_GRIPPER: case MAV_CMD_DO_GUIDED_LIMITS: case MAV_CMD_DO_FENCE_ENABLE: + case MAV_CMD_DO_WINCH: return true; default: @@ -651,6 +656,29 @@ void Copter::do_guided_limits(const AP_Mission::Mission_Command& cmd) } #endif +// control winch based on mission command +void Copter::do_winch(const AP_Mission::Mission_Command& cmd) +{ + // Note: we ignore the gripper num parameter because we only support one gripper + switch (cmd.content.winch.action) { + case WINCH_RELAXED: + g2.winch.relax(); + Log_Write_Event(DATA_WINCH_RELAXED); + break; + case WINCH_RELATIVE_LENGTH_CONTROL: + g2.winch.release_length(cmd.content.winch.release_length, cmd.content.winch.release_rate); + Log_Write_Event(DATA_WINCH_LENGTH_CONTROL); + break; + case WINCH_RATE_CONTROL: + g2.winch.set_desired_rate(cmd.content.winch.release_rate); + Log_Write_Event(DATA_WINCH_RATE_CONTROL); + break; + default: + // do nothing + break; + } +} + /********************************************************************************/ // Verify Nav (Must) commands /********************************************************************************/ diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index cfd1127cd0..4582916148 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -72,6 +72,8 @@ enum aux_sw_func { AUXSW_ARMDISARM = 41, // arm or disarm vehicle AUXSW_SMART_RTL = 42, // change to SmartRTL flight mode AUXSW_INVERTED = 43, // enable inverted flight + AUXSW_WINCH_ENABLE = 44, // winch enable/disable + AUXSW_WINCH_CONTROL = 45, // winch control AUXSW_SWITCH_MAX, }; @@ -169,7 +171,8 @@ enum tuning_func { TUNING_RATE_ROLL_FF = 53, // body frame roll rate controller FF term 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_RATE_YAW_FILT = 56, // yaw rate input filter + TUNING_WINCH = 57 // winch control (not actually a value to be tuned) }; // Acro Trainer types @@ -404,6 +407,9 @@ enum DevOptions { #define DATA_AVOIDANCE_PROXIMITY_ENABLE 65 #define DATA_AVOIDANCE_PROXIMITY_DISABLE 66 #define DATA_GPS_PRIMARY_CHANGED 67 +#define DATA_WINCH_RELAXED 68 +#define DATA_WINCH_LENGTH_CONTROL 69 +#define DATA_WINCH_RATE_CONTROL 70 // Centi-degrees to radians #define DEGX100 5729.57795f diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 6bb5cf52dc..5cf2a96128 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -526,3 +526,17 @@ void Copter::update_visual_odom() } #endif } + +// winch and wheel encoder initialisation +void Copter::winch_init() +{ + g2.wheel_encoder.init(); + g2.winch.init(&g2.wheel_encoder); +} + +// winch and wheel encoder update +void Copter::winch_update() +{ + g2.wheel_encoder.update(); + g2.winch.update(); +} diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 55320b1c66..cc631aea17 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -197,6 +197,7 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) case AUXSW_PRECISION_LOITER: case AUXSW_AVOID_PROXIMITY: case AUXSW_INVERTED: + case AUXSW_WINCH_ENABLE: do_aux_switch_function(ch_option, ch_flag); break; } @@ -624,6 +625,38 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) } #endif break; + + case AUXSW_WINCH_ENABLE: + switch (ch_flag) { + case AUX_SWITCH_HIGH: + // high switch maintains current position + g2.winch.release_length(0.0f); + Log_Write_Event(DATA_WINCH_LENGTH_CONTROL); + break; + default: + // all other position relax winch + g2.winch.relax(); + Log_Write_Event(DATA_WINCH_RELAXED); + break; + } + break; + + case AUXSW_WINCH_CONTROL: + switch (ch_flag) { + case AUX_SWITCH_LOW: + // raise winch at maximum speed + g2.winch.set_desired_rate(-g2.winch.get_rate_max()); + break; + case AUX_SWITCH_HIGH: + // lower winch at maximum speed + g2.winch.set_desired_rate(g2.winch.get_rate_max()); + break; + case AUX_SWITCH_MIDDLE: + default: + g2.winch.set_desired_rate(0.0f); + break; + } + break; } } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index e4a957cb9d..ce30368695 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -77,6 +77,9 @@ void Copter::init_ardupilot() g2.gripper.init(); #endif + // init winch and wheel encoder + winch_init(); + // initialise notify system notify.init(true); notify_flight_mode(control_mode); diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 64a008a20a..974fd3420d 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -212,5 +212,17 @@ void Copter::tuning() { case TUNING_RATE_YAW_FILT: attitude_control->get_rate_yaw_pid().filt_hz(tuning_value); break; - } + + case TUNING_WINCH: { + float desired_rate = 0.0f; + if (v > 0.8f) { + desired_rate = g2.winch.get_rate_max(); + } + if (v < 0.2f) { + desired_rate = -g2.winch.get_rate_max(); + } + g2.winch.set_desired_rate(desired_rate); + break; + } + } }