diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 41d9e7e8c7..37fd58f3b3 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -153,7 +153,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if GRIPPER_ENABLED == ENABLED SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75), #endif +#if WINCH_ENABLED == ENABLED SCHED_TASK(winch_update, 10, 50), +#endif #ifdef USERHOOK_FASTLOOP SCHED_TASK(userhook_FastLoop, 100, 75), #endif diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 7e0be89936..c9510570f6 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -95,8 +95,6 @@ #include #include #include -#include -#include #include // Configuration @@ -134,6 +132,11 @@ #include "toy_mode.h" #endif +#if WINCH_ENABLED == ENABLED +#include +#include +#endif + // Local modules #include "Parameters.h" #include "avoidance_adsb.h" diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index a810c79ef5..dfdc0a61e4 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1212,6 +1212,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) break; #endif +#if WINCH_ENABLED == ENABLED case MAV_CMD_DO_WINCH: // param1 : winch number (ignored) // param2 : action (0=relax, 1=relative length control, 2=rate control). See WINCH_ACTIONS enum. @@ -1244,6 +1245,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) } } break; +#endif /* Solo user presses Fly button */ case MAV_CMD_SOLO_BTN_FLY_CLICK: { diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index cd1cf2597b..9caabd2dda 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -944,6 +944,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp AP_SUBGROUPINFO(smart_rtl, "SRTL_", 21, ParametersG2, AP_SmartRTL), +#if WINCH_ENABLED == ENABLED // @Group: WENC // @Path: ../libraries/AP_WheelEncoder/AP_WheelEncoder.cpp AP_SUBGROUPINFO(wheel_encoder, "WENC", 22, ParametersG2, AP_WheelEncoder), @@ -951,6 +952,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Group: WINCH_ // @Path: ../libraries/AP_Winch/AP_Winch.cpp AP_SUBGROUPINFO(winch, "WINCH", 23, ParametersG2, AP_Winch), +#endif // @Param: PILOT_SPEED_DN // @DisplayName: Pilot maximum vertical speed descending diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0f2bcab87a..54f5cb344b 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -553,8 +553,10 @@ public: AP_SmartRTL smart_rtl; // wheel encoder and winch +#if WINCH_ENABLED == ENABLED AP_WheelEncoder wheel_encoder; AP_Winch winch; +#endif // Additional pilot velocity items AP_Int16 pilot_speed_dn; diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 56d0a824f7..14efd1a3e5 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -160,9 +160,11 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) break; #endif +#if WINCH_ENABLED == ENABLED case MAV_CMD_DO_WINCH: // Mission command to control winch do_winch(cmd); break; +#endif default: // do nothing with unrecognized MAVLink messages @@ -664,6 +666,7 @@ void Copter::ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd) } #endif +#if WINCH_ENABLED == ENABLED // control winch based on mission command void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd) { @@ -686,6 +689,7 @@ void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd) break; } } +#endif /********************************************************************************/ // Verify Nav (Must) commands diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 1c2176163e..324e36e516 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -243,6 +243,12 @@ # define GRIPPER_ENABLED ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// Winch support +#ifndef WINCH_ENABLED +# define WINCH_ENABLED ENABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // Parachute release #ifndef PARACHUTE diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 28db1327a3..de89d8c17a 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -269,7 +269,9 @@ private: #if GRIPPER_ENABLED == ENABLED void do_gripper(const AP_Mission::Mission_Command& cmd); #endif +#if WINCH_ENABLED == ENABLED void do_winch(const AP_Mission::Mission_Command& cmd); +#endif void do_payload_place(const AP_Mission::Mission_Command& cmd); void do_RTL(void); diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 4287fa04f3..5e5daf1364 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -507,13 +507,17 @@ void Copter::update_visual_odom() // 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 } diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 443e9cec17..5c38e7808c 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -633,6 +633,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; case AUXSW_WINCH_ENABLE: +#if WINCH_ENABLED == ENABLED switch (ch_flag) { case AUX_SWITCH_HIGH: // high switch maintains current position @@ -645,9 +646,11 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) Log_Write_Event(DATA_WINCH_RELAXED); break; } +#endif break; case AUXSW_WINCH_CONTROL: +#if WINCH_ENABLED == ENABLED switch (ch_flag) { case AUX_SWITCH_LOW: // raise winch at maximum speed @@ -662,6 +665,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) g2.winch.set_desired_rate(0.0f); break; } +#endif break; case AUXSW_RC_OVERRIDE_ENABLE: diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index bdb9a18b82..41f14375a1 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -213,6 +213,7 @@ void Copter::tuning() { attitude_control->get_rate_yaw_pid().filt_hz(tuning_value); break; +#if WINCH_ENABLED == ENABLED case TUNING_WINCH: { float desired_rate = 0.0f; if (v > 0.6f) { @@ -224,5 +225,6 @@ void Copter::tuning() { g2.winch.set_desired_rate(desired_rate); break; } +#endif } }