Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: integrate winch changes

Browse Source 
includes the following changes
winch_update called at 50hz
removed ability to set winch rate from ch6 tuning
remove wheel encoder
call winch library to log at 10hz
fix winch param prefix
This commit is contained in:
Randy Mackay 2020-07-24 17:38:12 +09:00
parent f7ec08ff18
commit 925f76c048
11 changed files with 20 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/Copter.cpp
View File

@ -179,7 +179,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75),
#endif
#if WINCH_ENABLED == ENABLED
SCHED_TASK(winch_update, 10, 50),
SCHED_TASK_CLASS(AP_Winch, &copter.g2.winch, update, 50, 50),
#endif
#if GENERATOR_ENABLED
SCHED_TASK_CLASS(AP_Generator_RichenPower, &copter.generator, update, 10, 50),
@ -426,6 +426,11 @@ void Copter::ten_hz_logging_loop()
#if FRAME_CONFIG == HELI_FRAME
Log_Write_Heli();
#endif
#if WINCH_ENABLED == ENABLED
if (should_log(MASK_LOG_ANY)) {
g2.winch.write_log();
}
#endif
}
// twentyfive_hz_logging - should be run at 25hz

3
ArduCopter/Copter.h
View File

@ -154,7 +154,6 @@
# include "toy_mode.h"
#endif
#if WINCH_ENABLED == ENABLED
# include <AP_WheelEncoder/AP_WheelEncoder.h>
# include <AP_Winch/AP_Winch.h>
#endif
#if RPM_ENABLED == ENABLED
@ -851,8 +850,6 @@ private:
void accel_cal_update(void);
void init_proximity();
void update_proximity();
void winch_init();
void winch_update();
// RC_Channel.cpp
void save_trim();

12
ArduCopter/GCS_Mavlink.cpp
View File

@ -809,20 +809,14 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
switch ((uint8_t)packet.param2) {
case WINCH_RELAXED:
copter.g2.winch.relax();
AP::logger().Write_Event(LogEvent::WINCH_RELAXED);
return MAV_RESULT_ACCEPTED;
case WINCH_RELATIVE_LENGTH_CONTROL: {
copter.g2.winch.release_length(packet.param3, fabsf(packet.param4));
AP::logger().Write_Event(LogEvent::WINCH_LENGTH_CONTROL);
copter.g2.winch.release_length(packet.param3);
return MAV_RESULT_ACCEPTED;
}
case WINCH_RATE_CONTROL:
if (fabsf(packet.param4) <= copter.g2.winch.get_rate_max()) {
copter.g2.winch.set_desired_rate(packet.param4);
AP::logger().Write_Event(LogEvent::WINCH_RATE_CONTROL);
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
copter.g2.winch.set_desired_rate(packet.param4);
return MAV_RESULT_ACCEPTED;
default:
break;
}

8
ArduCopter/Parameters.cpp
View File

@ -333,7 +333,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,57:Winch,58:SysID Magnitude
// @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,58:SysID Magnitude
GSCALAR(radio_tuning, "TUNE", 0),
// @Param: FRAME_TYPE
@ -863,11 +863,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
#endif
#if WINCH_ENABLED == ENABLED
// @Group: WENC
// @Path: ../libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
AP_SUBGROUPINFO(wheel_encoder, "WENC", 22, ParametersG2, AP_WheelEncoder),
// 22 was AP_WheelEncoder
// @Group: WINCH_
// @Group: WINCH
// @Path: ../libraries/AP_Winch/AP_Winch.cpp
AP_SUBGROUPINFO(winch, "WINCH", 23, ParametersG2, AP_Winch),
#endif

1
ArduCopter/Parameters.h
View File

@ -549,7 +549,6 @@ public:
// wheel encoder and winch
#if WINCH_ENABLED == ENABLED
AP_WheelEncoder wheel_encoder;
AP_Winch winch;
#endif

22
ArduCopter/RC_Channel.cpp
View File

@ -448,36 +448,20 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi
#if WINCH_ENABLED == ENABLED
switch (ch_flag) {
case AuxSwitchPos::HIGH:
// high switch maintains current position
copter.g2.winch.release_length(0.0f);
AP::logger().Write_Event(LogEvent::WINCH_LENGTH_CONTROL);
// high switch maintains changes the winch to rate control mode
copter.g2.winch.set_desired_rate(0.0f);
break;
case AuxSwitchPos::MIDDLE:
case AuxSwitchPos::LOW:
// all other position relax winch
copter.g2.winch.relax();
AP::logger().Write_Event(LogEvent::WINCH_RELAXED);
break;
}
#endif
break;
case AUX_FUNC::WINCH_CONTROL:
#if WINCH_ENABLED == ENABLED
switch (ch_flag) {
case AuxSwitchPos::LOW:
// raise winch at maximum speed
copter.g2.winch.set_desired_rate(-copter.g2.winch.get_rate_max());
break;
case AuxSwitchPos::HIGH:
// lower winch at maximum speed
copter.g2.winch.set_desired_rate(copter.g2.winch.get_rate_max());
break;
case AuxSwitchPos::MIDDLE:
copter.g2.winch.set_desired_rate(0.0f);
break;
}
#endif
// do nothing, used to control the rate of the winch and is processed within AP_Winch
break;
#ifdef USERHOOK_AUXSWITCH

2
ArduCopter/defines.h
View File

@ -73,7 +73,7 @@ enum tuning_func {
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_WINCH = 57, // winch control (not actually a value to be tuned)
UNUSED = 57, // was winch control
TUNING_SYSTEM_ID_MAGNITUDE = 58 // magnitude of the system ID signal
};

5
ArduCopter/mode_auto.cpp
View File

@ -1436,15 +1436,12 @@ void ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
switch (cmd.content.winch.action) {
case WINCH_RELAXED:
g2.winch.relax();
AP::logger().Write_Event(LogEvent::WINCH_RELAXED);
break;
case WINCH_RELATIVE_LENGTH_CONTROL:
g2.winch.release_length(cmd.content.winch.release_length, cmd.content.winch.release_rate);
AP::logger().Write_Event(LogEvent::WINCH_LENGTH_CONTROL);
g2.winch.release_length(cmd.content.winch.release_length);
break;
case WINCH_RATE_CONTROL:
g2.winch.set_desired_rate(cmd.content.winch.release_rate);
AP::logger().Write_Event(LogEvent::WINCH_RATE_CONTROL);
break;
default:
// do nothing

18
ArduCopter/sensors.cpp
View File

@ -223,21 +223,3 @@ void Copter::init_proximity(void)
g2.proximity.init();
#endif
}
// 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
}

4
ArduCopter/system.cpp
View File

@ -36,7 +36,9 @@ void Copter::init_ardupilot()
#endif
// init winch and wheel encoder
winch_init();
#if WINCH_ENABLED == ENABLED
g2.winch.init();
#endif
// initialise notify system
notify.init();

10
ArduCopter/tuning.cpp
View File

@ -187,16 +187,6 @@ void Copter::tuning()
attitude_control->get_rate_yaw_pid().filt_E_hz(tuning_value);
break;
#if WINCH_ENABLED == ENABLED
case TUNING_WINCH:
// add small deadzone
if (fabsf(tuning_value) < 0.05f) {
tuning_value = 0;
}
g2.winch.set_desired_rate(tuning_value);
break;
#endif
case TUNING_SYSTEM_ID_MAGNITUDE:
#if MODE_SYSTEMID_ENABLED == ENABLED
copter.mode_systemid.set_magnitude(tuning_value);