mirror of https://github.com/ArduPilot/ardupilot
Copter: optionalize the winch
This commit is contained in:
parent
ec98cab262
commit
c31c2a4cf1
|
@ -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
|
||||
|
|
|
@ -95,8 +95,6 @@
|
|||
#include <AP_Arming/AP_Arming.h>
|
||||
#include <AP_VisualOdom/AP_VisualOdom.h>
|
||||
#include <AP_SmartRTL/AP_SmartRTL.h>
|
||||
#include <AP_WheelEncoder/AP_WheelEncoder.h>
|
||||
#include <AP_Winch/AP_Winch.h>
|
||||
#include <AP_TempCalibration/AP_TempCalibration.h>
|
||||
|
||||
// Configuration
|
||||
|
@ -134,6 +132,11 @@
|
|||
#include "toy_mode.h"
|
||||
#endif
|
||||
|
||||
#if WINCH_ENABLED == ENABLED
|
||||
#include <AP_WheelEncoder/AP_WheelEncoder.h>
|
||||
#include <AP_Winch/AP_Winch.h>
|
||||
#endif
|
||||
|
||||
// Local modules
|
||||
#include "Parameters.h"
|
||||
#include "avoidance_adsb.h"
|
||||
|
|
|
@ -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: {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -243,6 +243,12 @@
|
|||
# define GRIPPER_ENABLED ENABLED
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Winch support
|
||||
#ifndef WINCH_ENABLED
|
||||
# define WINCH_ENABLED ENABLED
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Parachute release
|
||||
#ifndef PARACHUTE
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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
|
||||
}
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue