mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
Randy Mackay
parent
2c511b4269
commit
787954fa37
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -93,6 +93,8 @@
|
|||
#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>
|
||||
|
||||
// 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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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[];
|
||||
|
|
|
|||
|
|
@ -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
|
||||
/********************************************************************************/
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue