From 15a117dfb4ea848836f2894e096ba26aac17db84 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Wed, 22 Mar 2017 23:48:49 -0400 Subject: [PATCH] Sub: Remove ch6 tuning --- ArduSub/ArduSub.cpp | 5 -- ArduSub/Log.cpp | 28 ------ ArduSub/Parameters.cpp | 23 ----- ArduSub/Parameters.h | 12 +-- ArduSub/Sub.h | 4 - ArduSub/config.h | 8 -- ArduSub/tuning.cpp | 192 ----------------------------------------- 7 files changed, 1 insertion(+), 271 deletions(-) delete mode 100644 ArduSub/tuning.cpp diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index a84a43eab5..f887047652 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -357,11 +357,6 @@ void Sub::three_hz_loop() #endif // AC_FENCE_ENABLED update_events(); - -#if CH6_TUNE_ENABLED == ENABLED - // update ch6 in flight tuning - tuning(); -#endif } // one_hz_loop - runs at 1Hz diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 9eb9045620..c819365bf6 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -509,31 +509,6 @@ void Sub::Log_Write_Baro(void) } } -struct PACKED log_ParameterTuning { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t parameter; // parameter we are tuning, e.g. 39 is CH6_CIRCLE_RATE - float tuning_value; // normalized value used inside tuning() function - int16_t control_in; // raw tune input value - int16_t tuning_low; // tuning low end value - int16_t tuning_high; // tuning high end value -}; - -void Sub::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) -{ - struct log_ParameterTuning pkt_tune = { - LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG), - time_us : AP_HAL::micros64(), - parameter : param, - tuning_value : tuning_val, - control_in : control_in, - tuning_low : tune_low, - tuning_high : tune_high - }; - - DataFlash.WriteBlock(&pkt_tune, sizeof(pkt_tune)); -} - // log EKF origin and ahrs home to dataflash void Sub::Log_Write_Home_And_Origin() { @@ -603,8 +578,6 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target const struct LogStructure Sub::log_structure[] = { LOG_COMMON_STRUCTURES, - { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning), - "PTUN", "QBfHHH", "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi" }, { LOG_OPTFLOW_MSG, sizeof(log_Optflow), "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY" }, { LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning), @@ -717,7 +690,6 @@ void Sub::Log_Write_Data(uint8_t id, uint16_t value) {} void Sub::Log_Write_Data(uint8_t id, float value) {} void Sub::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {} void Sub::Log_Write_Baro(void) {} -void Sub::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {} void Sub::Log_Write_Home_And_Origin() {} void Sub::Log_Sensor_Health() {} void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 624052c4bf..28446e2470 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -286,29 +286,6 @@ const AP_Param::Info Sub::var_info[] = { // @Values: 0:Normal Start-up, 1:Start-up in ESC Calibration mode if throttle high, 2:Start-up in ESC Calibration mode regardless of throttle, 9:Disabled GSCALAR(esc_calibrate, "ESC_CALIBRATION", 0), -#if CH6_TUNE_ENABLED == ENABLED - // @Param: TUNE - // @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,14:Altitude Hold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,42:Loiter Speed,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,13:Heli Ext Gyro,17:OF Loiter kP,18:OF Loiter kI,19:OF Loiter kD,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 - GSCALAR(radio_tuning, "TUNE", 0), - - // @Param: TUNE_LOW - // @DisplayName: Tuning minimum - // @Description: The minimum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob - // @User: Standard - // @Range: 0 32767 - GSCALAR(radio_tuning_low, "TUNE_LOW", 0), - - // @Param: TUNE_HIGH - // @DisplayName: Tuning maximum - // @Description: The maximum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob - // @User: Standard - // @Range: 0 32767 - GSCALAR(radio_tuning_high, "TUNE_HIGH", 1000), -#endif - // @Param: DISARM_DELAY // @DisplayName: Disarm delay // @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm. diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 3c0dd7c7fe..ea85be32be 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -222,12 +222,7 @@ public: // RC_Mapper Library k_param_rcmap, // Disabled - // CH6 Tuning - k_param_radio_tuning, // Disabled - k_param_radio_tuning_high, // Disabled - k_param_radio_tuning_low, // Disabled - - k_param_cam_slew_limit, + k_param_cam_slew_limit = 237, }; @@ -289,11 +284,6 @@ public: // AP_Int32 log_bitmask; AP_Int8 esc_calibrate; -#if CH6_TUNE_ENABLED == ENABLED - AP_Int8 radio_tuning; - AP_Int16 radio_tuning_high; - AP_Int16 radio_tuning_low; -#endif AP_Int8 disarm_delay; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 25656f8273..4cf79b46cd 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -539,7 +539,6 @@ private: void Log_Write_Data(uint8_t id, float value); void Log_Write_Error(uint8_t sub_system, uint8_t error_code); void Log_Write_Baro(void); - void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high); void Log_Write_Home_And_Origin(); void Log_Sensor_Health(); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); @@ -741,9 +740,6 @@ private: void check_usb_mux(void); bool should_log(uint32_t mask); void print_hit_enter(); -#if CH6_TUNE_ENABLED == ENABLED - void tuning(); -#endif void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...); bool start_command(const AP_Mission::Mission_Command& cmd); bool verify_command(const AP_Mission::Mission_Command& cmd); diff --git a/ArduSub/config.h b/ArduSub/config.h index 078fa3af90..3aaea8450e 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -71,14 +71,6 @@ # define CIRCLE_NAV_ENABLED ENABLED #endif -////////////////////////////////////////////////////////////////////////////// -// Channel 6 Tuning -// - -#ifndef CH6_TUNE_ENABLED -# define CH6_TUNE_ENABLED DISABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // RPM // diff --git a/ArduSub/tuning.cpp b/ArduSub/tuning.cpp deleted file mode 100644 index c1755bf084..0000000000 --- a/ArduSub/tuning.cpp +++ /dev/null @@ -1,192 +0,0 @@ -#include "Sub.h" -#if CH6_TUNE_ENABLED == ENABLED - -/* - * tuning.pde - function to update various parameters in flight using the ch6 tuning knob - */ - -// tuning - updates parameters based on the ch6 tuning knob's position -// should be called at 3.3hz -void Sub::tuning() -{ - - // exit immediately if not tuning of when radio failsafe is invoked so tuning values are not set to zero - if ((g.radio_tuning <= 0) || failsafe.manual_control) { - return; - } - - float tuning_value = (float)g.rc_6.get_control_in() / 1000.0f; - g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); - - Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g.rc_6.get_control_in(), g.radio_tuning_low, g.radio_tuning_high); - - switch (g.radio_tuning) { - - // Roll, Pitch tuning - case TUNING_STABILIZE_ROLL_PITCH_KP: - attitude_control.get_angle_roll_p().kP(tuning_value); - attitude_control.get_angle_pitch_p().kP(tuning_value); - break; - - case TUNING_RATE_ROLL_PITCH_KP: - attitude_control.get_rate_roll_pid().kP(tuning_value); - attitude_control.get_rate_pitch_pid().kP(tuning_value); - break; - - case TUNING_RATE_ROLL_PITCH_KI: - attitude_control.get_rate_roll_pid().kI(tuning_value); - attitude_control.get_rate_pitch_pid().kI(tuning_value); - break; - - case TUNING_RATE_ROLL_PITCH_KD: - attitude_control.get_rate_roll_pid().kD(tuning_value); - attitude_control.get_rate_pitch_pid().kD(tuning_value); - break; - - // Yaw tuning - case TUNING_STABILIZE_YAW_KP: - attitude_control.get_angle_yaw_p().kP(tuning_value); - break; - - case TUNING_YAW_RATE_KP: - attitude_control.get_rate_yaw_pid().kP(tuning_value); - break; - - case TUNING_YAW_RATE_KD: - attitude_control.get_rate_yaw_pid().kD(tuning_value); - break; - - // Altitude and throttle tuning - case TUNING_ALTITUDE_HOLD_KP: - g.p_alt_hold.kP(tuning_value); - break; - - case TUNING_THROTTLE_RATE_KP: - g.p_vel_z.kP(tuning_value); - break; - - case TUNING_ACCEL_Z_KP: - g.pid_accel_z.kP(tuning_value); - break; - - case TUNING_ACCEL_Z_KI: - g.pid_accel_z.kI(tuning_value); - break; - - case TUNING_ACCEL_Z_KD: - g.pid_accel_z.kD(tuning_value); - break; - - // Loiter and navigation tuning - case TUNING_LOITER_POSITION_KP: - g.p_pos_xy.kP(tuning_value); - break; - - case TUNING_VEL_XY_KP: - g.pi_vel_xy.kP(tuning_value); - break; - - case TUNING_VEL_XY_KI: - g.pi_vel_xy.kI(tuning_value); - break; - - case TUNING_WP_SPEED: - // set waypoint navigation horizontal speed to 0 ~ 1000 cm/s - wp_nav.set_speed_xy(g.rc_6.get_control_in()); - break; - - // Acro roll pitch gain - case TUNING_ACRO_RP_KP: - g.acro_rp_p = tuning_value; - break; - - // Acro yaw gain - case TUNING_ACRO_YAW_KP: - g.acro_yaw_p = tuning_value; - break; - - case TUNING_DECLINATION: - // set declination to +-20degrees - compass.set_declination(ToRad((2.0f * g.rc_6.get_control_in() - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact - break; - - case TUNING_CIRCLE_RATE: - // set circle rate up to approximately 45 deg/sec in either direction - circle_nav.set_rate((float)g.rc_6.get_control_in()/25.0f-20.0f); - break; - - case TUNING_RANGEFINDER_GAIN: - // set rangefinder gain - g.rangefinder_gain.set(tuning_value); - break; - -#if 0 - // disabled for now - we need accessor functions - case TUNING_EKF_VERTICAL_POS: - // Tune the EKF that is being used - // EKF's baro vs accel (higher rely on accels more, baro impact is reduced) - if (!ahrs.get_NavEKF2().enabled()) { - ahrs.get_NavEKF()._gpsVertPosNoise = tuning_value; - } else { - ahrs.get_NavEKF2()._gpsVertPosNoise = tuning_value; - } - break; - - case TUNING_EKF_HORIZONTAL_POS: - // EKF's gps vs accel (higher rely on accels more, gps impact is reduced) - if (!ahrs.get_NavEKF2().enabled()) { - ahrs.get_NavEKF()._gpsHorizPosNoise = tuning_value; - } else { - ahrs.get_NavEKF2()._gpsHorizPosNoise = tuning_value; - } - break; - - case TUNING_EKF_ACCEL_NOISE: - // EKF's accel noise (lower means trust accels more, gps & baro less) - if (!ahrs.get_NavEKF2().enabled()) { - ahrs.get_NavEKF()._accNoise = tuning_value; - } else { - ahrs.get_NavEKF2()._accNoise = tuning_value; - } - break; -#endif - - case TUNING_RC_FEEL_RP: - // roll-pitch input smoothing - g.rc_feel_rp = g.rc_6.get_control_in() / 10; - break; - - case TUNING_RATE_PITCH_KP: - attitude_control.get_rate_pitch_pid().kP(tuning_value); - break; - - case TUNING_RATE_PITCH_KI: - attitude_control.get_rate_pitch_pid().kI(tuning_value); - break; - - case TUNING_RATE_PITCH_KD: - attitude_control.get_rate_pitch_pid().kD(tuning_value); - break; - - case TUNING_RATE_ROLL_KP: - attitude_control.get_rate_roll_pid().kP(tuning_value); - break; - - case TUNING_RATE_ROLL_KI: - attitude_control.get_rate_roll_pid().kI(tuning_value); - break; - - case TUNING_RATE_ROLL_KD: - attitude_control.get_rate_roll_pid().kD(tuning_value); - break; - - case TUNING_RATE_MOT_YAW_HEADROOM: - motors.set_yaw_headroom(tuning_value*1000); - break; - - case TUNING_RATE_YAW_FILT: - attitude_control.get_rate_yaw_pid().filt_hz(tuning_value); - break; - } -} -#endif