From 925f76c048442474484d1b9920e3ede37577938f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 24 Jul 2020 17:38:12 +0900 Subject: [PATCH] Copter: integrate winch changes 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 --- ArduCopter/Copter.cpp | 7 ++++++- ArduCopter/Copter.h | 3 --- ArduCopter/GCS_Mavlink.cpp | 12 +++--------- ArduCopter/Parameters.cpp | 8 +++----- ArduCopter/Parameters.h | 1 - ArduCopter/RC_Channel.cpp | 22 +++------------------- ArduCopter/defines.h | 2 +- ArduCopter/mode_auto.cpp | 5 +---- ArduCopter/sensors.cpp | 18 ------------------ ArduCopter/system.cpp | 4 +++- ArduCopter/tuning.cpp | 10 ---------- 11 files changed, 20 insertions(+), 72 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 78d59ebf7a..981322c72f 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -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 diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0ede2500d6..e274208012 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -154,7 +154,6 @@ # include "toy_mode.h" #endif #if WINCH_ENABLED == ENABLED - # include # include #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(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 22dc5ac999..cd9db07202 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -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; } diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 6fa4d53ef8..ffcb84a6a6 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -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 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 00ba8a89de..7a2224301c 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -549,7 +549,6 @@ public: // wheel encoder and winch #if WINCH_ENABLED == ENABLED - AP_WheelEncoder wheel_encoder; AP_Winch winch; #endif diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index ccc4e07c29..be9c53eb9a 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 4a901fb318..fb9ff1bcb1 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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 }; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index a96c956fcd..e25884ee29 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -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 diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index ac301a66e9..9f61e53f19 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -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 -} diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index b5fef155cf..b808b1bbc0 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -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(); diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 43fc191bde..c48533b622 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -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);