zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
master
Randy Mackay 7 years ago
parent
2c511b4269
commit
787954fa37
11 changed files with 156 additions and 9 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      ArduCopter/ArduCopter.cpp
  2. 5
      ArduCopter/Copter.h
  3. 33
      ArduCopter/GCS_Mavlink.cpp
  4. 22
      ArduCopter/Parameters.cpp
  5. 4
      ArduCopter/Parameters.h
  6. 28
      ArduCopter/commands_logic.cpp
  7. 8
      ArduCopter/defines.h
  8. 14
      ArduCopter/sensors.cpp
  9. 33
      ArduCopter/switches.cpp
  10. 3
      ArduCopter/system.cpp
  11. 14
      ArduCopter/tuning.cpp

1
ArduCopter/ArduCopter.cpp
Unescape View File

@ -142,6 +142,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { @@ -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

5
ArduCopter/Copter.h
Unescape View File

@ -93,6 +93,8 @@ @@ -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: @@ -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: @@ -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);

33
ArduCopter/GCS_Mavlink.cpp
Unescape View File

@ -1199,6 +1199,39 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) @@ -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;

22
ArduCopter/Parameters.cpp
Unescape View File

@ -361,7 +361,7 @@ const AP_Param::Info Copter::var_info[] = { @@ -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[] = { @@ -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[] = { @@ -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
};

4
ArduCopter/Parameters.h
Unescape View File

@ -570,6 +570,10 @@ public: @@ -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[];

28
ArduCopter/commands_logic.cpp
Unescape View File

@ -160,6 +160,10 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd) @@ -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) @@ -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) @@ -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
/********************************************************************************/

8
ArduCopter/defines.h
Unescape View File

@ -72,6 +72,8 @@ enum aux_sw_func { @@ -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 { @@ -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 { @@ -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

14
ArduCopter/sensors.cpp
Unescape View File

@ -526,3 +526,17 @@ void Copter::update_visual_odom() @@ -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();
}

33
ArduCopter/switches.cpp
Unescape View File

@ -197,6 +197,7 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) @@ -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) @@ -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;
}
}

3
ArduCopter/system.cpp
Unescape View File

@ -77,6 +77,9 @@ void Copter::init_ardupilot() @@ -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);

14
ArduCopter/tuning.cpp
Unescape View File

@ -212,5 +212,17 @@ void Copter::tuning() { @@ -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;
}
}
}

Write Preview
Loading…
Cancel
Save