diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index 424f853939a22e..ce9ca3b84d66ad 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -123,6 +123,7 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control) return desired_rate; } +#if 0 // get_surface_tracking_climb_rate - hold vehicle at the desired distance above the ground // returns climb rate (in cm/s) which should be passed to the position controller float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) @@ -162,6 +163,7 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al return (float)target_rate; #endif } +#endif // rotate vector from vehicle's perspective to North-East frame void Sub::rotate_body_frame_to_NE(float &x, float &y) diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index f2649bc8417a92..6b87f29359e172 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -41,7 +41,11 @@ void Sub::Log_Write_Control_Tuning() desired_alt : pos_control.get_pos_target_z_cm() / 100.0f, inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f, baro_alt : barometer.get_altitude(), - desired_rangefinder_alt : (int16_t)target_rangefinder_alt, +#if RANGEFINDER_ENABLED == ENABLED + desired_rangefinder_alt : (int16_t)surface_tracking.get_target_rangefinder_cm(), +#else + desired_rangefinder_alt : 0, +#endif rangefinder_alt : rangefinder_state.alt_cm, terr_alt : terr_alt, target_climb_rate : (int16_t)pos_control.get_vel_target_z_cms(), diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 7bd1ff3eb10f1e..9edcc1c60fe8e9 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -583,6 +583,10 @@ const AP_Param::Info Sub::var_info[] = { // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp GOBJECT(rangefinder, "RNGFND", RangeFinder), + + // @Group: RNGFND_PID_ + // @Path: ../libraries/AC_PID/AC_PID.cpp + GOBJECTN(surface_tracking.pid_rangefinder, pid_rangefinder, "RNGFND_PID_", AC_PID), #endif #if AP_TERRAIN_AVAILABLE diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 731e1321e17fe5..424b023fa0f2d4 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -155,6 +155,7 @@ class Parameters { k_param_pi_vel_xy, // deprecated k_param_p_vel_z, // deprecated k_param_pid_accel_z, // deprecated + k_param_pid_rangefinder, // Failsafes diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index a9864acbc53b2b..9302e6d66162d3 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -62,6 +62,7 @@ #include // Joystick/gamepad button function assignment #include // Leak detector #include +#include // Local modules #include "defines.h" @@ -114,6 +115,7 @@ class Sub : public AP_Vehicle { friend class ModeStabilize; friend class ModeAcro; friend class ModeAlthold; + friend class ModeRnghold; friend class ModeGuided; friend class ModePoshold; friend class ModeAuto; @@ -156,6 +158,35 @@ class Sub : public AP_Vehicle { LowPassFilterFloat alt_cm_filt; // altitude filter } rangefinder_state = { false, false, 0, 0 }; +#if RANGEFINDER_ENABLED == ENABLED + class SurfaceTracking { + public: + // pilot can enable or disable tracking + void enable(bool _enabled); + + // reset controller, target_rangefinder_cm = next healthy rangefinder reading + void reset(); + + // get target rangefinder + float get_target_rangefinder_cm() const { return target_rangefinder_cm; } + + // set target rangefinder + void set_target_rangefinder_cm(float new_target_cm); + + // track seafloor, call from main control loop + void update_surface_offset(); + + // rangefinder PID, must be public so that AP_Param can see it + AC_PID pid_rangefinder{RNGFND_P_DEFAULT, RNGFND_I_DEFAULT, RNGFND_D_DEFAULT, + 0.0, 0.0, 5.0, 5.0, 5.0}; + + private: + bool enabled = false; // true if pilot enabled surface tracking + bool reset_target = false; // true if target should be reset + float target_rangefinder_cm = 0; // target distance to seafloor + } surface_tracking; +#endif + #if AP_RPM_ENABLED AP_RPM rpm_sensor; #endif @@ -273,7 +304,6 @@ class Sub : public AP_Vehicle { // Altitude // The cm/s we are moving up or down based on filtered data - Positive = UP int16_t climb_rate; - float target_rangefinder_alt; // desired altitude in cm above the ground // Turn counter int32_t quarter_turn_count; @@ -566,6 +596,7 @@ class Sub : public AP_Vehicle { ModeStabilize mode_stabilize; ModeAcro mode_acro; ModeAlthold mode_althold; + ModeRnghold mode_rnghold; ModeAuto mode_auto; ModeGuided mode_guided; ModePoshold mode_poshold; diff --git a/ArduSub/config.h b/ArduSub/config.h index 4a517afe13adbb..7d6477bcbecf3b 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -58,6 +58,18 @@ # define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction) #endif +#ifndef RNGFND_P_DEFAULT +# define RNGFND_P_DEFAULT 0.4f // default P term for rangefinder_pid +#endif + +#ifndef RNGFND_I_DEFAULT +# define RNGFND_I_DEFAULT 0.0f // default I term for rangefinder_pid +#endif + +#ifndef RNGFND_D_DEFAULT +# define RNGFND_D_DEFAULT 0.0f // default D term for rangefinder_pid +#endif + #ifndef THR_SURFACE_TRACKING_VELZ_MAX # define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder #endif diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 5f0f83fe9de98f..c841a7ae9f28dc 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -181,6 +181,11 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) case JSButton::button_function_t::k_mode_poshold: set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND); break; +#if RANGEFINDER_ENABLED == ENABLED + case JSButton::button_function_t::k_mode_range_hold: + set_mode(Mode::Number::RNG_HOLD, ModeReason::RC_COMMAND); + break; +#endif case JSButton::button_function_t::k_mount_center: #if HAL_MOUNT_ENABLED diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp index 323836c2f94abb..d64322c283c25c 100644 --- a/ArduSub/mode.cpp +++ b/ArduSub/mode.cpp @@ -38,6 +38,9 @@ Mode *Sub::mode_from_mode_num(const Mode::Number mode) case Mode::Number::ALT_HOLD: ret = &mode_althold; break; + case Mode::Number::RNG_HOLD: + ret = &mode_rnghold; + break; case Mode::Number::POSHOLD: ret = &mode_poshold; break; diff --git a/ArduSub/mode.h b/ArduSub/mode.h index ffe01a525e4dba..a2361f6a9d3968 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -49,7 +49,8 @@ class Mode SURFACE = 9, // automatically return to surface, pilot maintains horizontal control POSHOLD = 16, // automatic position hold with manual override, with automatic throttle MANUAL = 19, // Pass-through input with no stabilization - MOTOR_DETECT = 20 // Automatically detect motors orientation + MOTOR_DETECT = 20, // Automatically detect motors orientation + RNG_HOLD = 21 // Hold rangefinder value (distance above terrain) }; // constructor @@ -266,11 +267,35 @@ class ModeAlthold : public Mode protected: + void run_pre(); + void run_post(); + const char *name() const override { return "ALT_HOLD"; } const char *name4() const override { return "ALTH"; } }; +class ModeRnghold : public ModeAlthold +{ + +public: + // inherit constructor + using ModeAlthold::ModeAlthold; + + virtual void run() override; + + bool init(bool ignore_checks) override; + +protected: + + const char *name() const override { return "RNG_HOLD"; } + const char *name4() const override { return "RNGH"; } + +private: + + void control_range(); +}; + class ModeGuided : public Mode { diff --git a/ArduSub/mode_althold.cpp b/ArduSub/mode_althold.cpp index 3282054d7329ad..7868f55e1d6427 100644 --- a/ArduSub/mode_althold.cpp +++ b/ArduSub/mode_althold.cpp @@ -19,9 +19,14 @@ bool ModeAlthold::init(bool ignore_checks) { return true; } -// althold_run - runs the althold controller -// should be called at 100hz or more void ModeAlthold::run() +{ + run_pre(); + control_depth(); + run_post(); +} + +void ModeAlthold::run_pre() { uint32_t tnow = AP_HAL::millis(); @@ -87,9 +92,10 @@ void ModeAlthold::run() attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true); } } +} - control_depth(); - +void ModeAlthold::run_post() +{ motors.set_forward(channel_forward->norm_input()); motors.set_lateral(channel_lateral->norm_input()); } @@ -110,5 +116,4 @@ void ModeAlthold::control_depth() { position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s); position_control->update_z_controller(); - } diff --git a/ArduSub/mode_rnghold.cpp b/ArduSub/mode_rnghold.cpp new file mode 100644 index 00000000000000..bdc5b4a9b8d65a --- /dev/null +++ b/ArduSub/mode_rnghold.cpp @@ -0,0 +1,73 @@ +#include "Sub.h" + +/* + * RNG_HOLD (rangefinder hold) -- a variation on ALT_HOLD (depth hold) + * + * The real work for ALT_HOLD and RNG_HOLD is handled by AC_PosControl, which provides 2 inputs for depth: + * -- target depth (sub.pos_control._pos_target.z). This is the desired depth, plus an offset. + * -- target offset (sub.pos_control._pos_offset_target_z). This is the desired offset. + * + * ALT_HOLD and RNG_HOLD set the target depth in these situations: + * -- During initialization, we call pos_control.init_z_controller(). This sets target depth to the current depth. + * -- If the sub hits the surface or bottom we call pos_control.set_pos_target_z_cm(). + * -- If the pilot takes control we call pos_control.set_pos_target_z_from_climb_rate_cm(). + * + * At the end of the control loop ALT_HOLD and RNG_HOLD call pos_control.update_z_controller() to pass the buck. + * + * ALT_HOLD does not use the target offset. + * + * RNG_HOLD sets the target offset to implement surface tracking. This is handled by Sub::SurfaceTracking. We call + * SurfaceTracking in these situations: + * -- During initialization, we call surface_tracking.enable(). + * -- During normal operation, we call surface_tracking.update_surface_offset(). + * -- If the sub hits the surface or bottom, or the pilot takes control, we call surface_tracking.reset(). + */ + +bool ModeRnghold::init(bool ignore_checks) +{ + if (!ModeAlthold::init(ignore_checks)) { + return false; + } + + // enable surface tracking + sub.surface_tracking.enable(true); + + return true; +} + +void ModeRnghold::run() +{ + run_pre(); + control_range(); + run_post(); +} + +void ModeRnghold::control_range() { + float target_climb_rate_cm_s = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -sub.get_pilot_speed_dn(), g.pilot_speed_up); + + // desired_climb_rate returns 0 when within the deadzone + if (fabsf(target_climb_rate_cm_s) < 0.05f) { + if (sub.ap.at_surface) { + // set target depth to 5 cm below SURFACE_DEPTH + position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f)); + sub.surface_tracking.reset(); + } else if (sub.ap.at_bottom) { + // set target depth to 10 cm above bottom + position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm())); + sub.surface_tracking.reset(); + } else { + // normal operation + sub.surface_tracking.update_surface_offset(); + } + } else { + // pilot is in control + sub.surface_tracking.reset(); + } + + // set the target z from the climb rate and the z offset, and adjust the z vel and accel targets + position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s); + + // run the z vel and accel PID controllers + position_control->update_z_controller(); +} diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index 481930cd75190a..dca5946605757a 100644 --- a/ArduSub/sensors.cpp +++ b/ArduSub/sensors.cpp @@ -18,6 +18,7 @@ void Sub::read_barometer() void Sub::init_rangefinder() { #if RANGEFINDER_ENABLED == ENABLED + rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN); rangefinder.init(ROTATION_PITCH_270); rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270); @@ -69,5 +70,6 @@ void Sub::read_rangefinder() // return true if rangefinder_alt can be used bool Sub::rangefinder_alt_ok() const { - return (rangefinder_state.enabled && rangefinder_state.alt_healthy); + uint32_t now = AP_HAL::millis(); + return (rangefinder_state.enabled && rangefinder_state.alt_healthy && now - rangefinder_state.last_healthy_ms < RANGEFINDER_TIMEOUT_MS); } diff --git a/ArduSub/surface_tracking.cpp b/ArduSub/surface_tracking.cpp new file mode 100644 index 00000000000000..17b2537780a724 --- /dev/null +++ b/ArduSub/surface_tracking.cpp @@ -0,0 +1,78 @@ +#include "Sub.h" + +#if RANGEFINDER_ENABLED == ENABLED + +/* + * Sub::SurfaceTracking -- use a rangefinder sensor to track the seafloor + * + * The real work is handled by AC_PosControl. + * + * Surface tracking starts in the "reset" state (sub.surface_tracking.reset_target = true). In this state we are waiting + * for a good rangefinder reading. + * + * When we get a good rangefinder reading, we exit the reset state: + * -- We set the target rangefinder to the current rangefinder. + * -- We clear the target offset: sub.pos_control.set_pos_offset_target_z_cm(0). + * -- We clear the offset: sub.pos_control.set_pos_offset_z_cm(0) to clear the offset. + * + * During normal operation, SurfaceTracking is using rangefinder readings to set the target offset, and AC_PosControl + * does the rest. + * + * Callers can put SurfaceTracking back into the reset state when something goes wrong or the pilot takes control. + * + * We generally do not want to reset SurfaceTracking if the rangefinder glitches, since that will result in a new + * target rangefinder. E.g., if a pilot is tracking at a target of 1m above the seafloor, there is a glitch, then the + * next rangefinder reading shows 1.1m, the desired behavior is to move 10cm closer to the seafloor, vs setting a new + * target of 1.1m above the seafloor. + */ + +void Sub::SurfaceTracking::enable(bool _enabled) +{ + enabled = _enabled; + reset(); + + // let pilot know that we're waiting for a rangefinder reading + if (enabled && !sub.rangefinder_alt_ok()) { + sub.gcs().send_text(MAV_SEVERITY_INFO, "holding depth, waiting for a rangefinder reading"); + } +} + +void Sub::SurfaceTracking::reset() +{ + reset_target = true; +} + +void Sub::SurfaceTracking::set_target_rangefinder_cm(float new_target_cm) +{ + target_rangefinder_cm = new_target_cm; + sub.pos_control.set_pos_offset_z_cm(0); + sub.pos_control.set_pos_offset_target_z_cm(0); + sub.gcs().send_text(MAV_SEVERITY_INFO, "rangefinder target is %g m", target_rangefinder_cm * 0.01f); + reset_target = false; +} + +void Sub::SurfaceTracking::update_surface_offset() +{ + if (enabled) { + if (sub.rangefinder_alt_ok()) { + // handle first reading or controller reset + if (reset_target) { + set_target_rangefinder_cm(sub.rangefinder_state.alt_cm_filt.get()); + } + + // calculate the offset target that will keep a constant distance above the seafloor + // use a PID controller to handle long-ish data delays + const float offset_target_z_cm = sub.pos_control.get_pos_offset_z_cm() + + sub.surface_tracking.pid_rangefinder.update_error( + target_rangefinder_cm - sub.rangefinder_state.alt_cm_filt.get(), 0.01); + + // set the offset target, AC_PosControl will do the rest + sub.pos_control.set_pos_offset_target_z_cm(offset_target_z_cm); + } + } else { + sub.pos_control.set_pos_offset_z_cm(0); + sub.pos_control.set_pos_offset_target_z_cm(0); + } +} + +#endif diff --git a/ArduSub/wscript b/ArduSub/wscript index 89da451ee21e7f..6be9ffe8e36bc7 100644 --- a/ArduSub/wscript +++ b/ArduSub/wscript @@ -19,6 +19,7 @@ def build(bld): 'AP_TemperatureSensor', 'AP_Arming', 'AP_OSD', + 'AC_PID', ], ) diff --git a/libraries/AP_JSButton/AP_JSButton.cpp b/libraries/AP_JSButton/AP_JSButton.cpp index a25ae0af3f60c6..18b3ec0f82b691 100644 --- a/libraries/AP_JSButton/AP_JSButton.cpp +++ b/libraries/AP_JSButton/AP_JSButton.cpp @@ -5,14 +5,14 @@ const AP_Param::GroupInfo JSButton::var_info[] = { // @Param: FUNCTION // @DisplayName: Function for button // @Description: Set to 0 to disable or choose a function - // @Values: 0:Disabled,1:shift,2:arm_toggle,3:arm,4:disarm,5:mode_manual,6:mode_stabilize,7:mode_depth_hold,8:mode_poshold,9:mode_auto,10:mode_circle,11:mode_guided,12:mode_acro,21:mount_center,22:mount_tilt_up,23:mount_tilt_down,24:camera_trigger,25:camera_source_toggle,26:mount_pan_right,27:mount_pan_left,31:lights1_cycle,32:lights1_brighter,33:lights1_dimmer,34:lights2_cycle,35:lights2_brighter,36:lights2_dimmer,41:gain_toggle,42:gain_inc,43:gain_dec,44:trim_roll_inc,45:trim_roll_dec,46:trim_pitch_inc,47:trim_pitch_dec,48:input_hold_set,49:roll_pitch_toggle,51:relay_1_on,52:relay_1_off,53:relay_1_toggle,54:relay_2_on,55:relay_2_off,56:relay_2_toggle,57:relay_3_on,58:relay_3_off,59:relay_3_toggle,61:servo_1_inc,62:servo_1_dec,63:servo_1_min,64:servo_1_max,65:servo_1_center,66:servo_2_inc,67:servo_2_dec,68:servo_2_min,69:servo_2_max,70:servo_2_center,71:servo_3_inc,72:servo_3_dec,73:servo_3_min,74:servo_3_max,75:servo_3_center,76:servo_1_min_momentary,77:servo_1_max_momentary,78:servo_1_min_toggle,79:servo_1_max_toggle,80:servo_2_min_momentary,81:servo_2_max_momentary,82:servo_2_min_toggle,83:servo_2_max_toggle,84:servo_3_min_momentary,85:servo_3_max_momentary,86:servo_3_min_toggle,87:servo_3_max_toggle,91:custom_1,92:custom_2,93:custom_3,94:custom_4,95:custom_5,96:custom_6,101:relay_4_on,102:relay_4_off,103:relay_4_toggle,104:relay_1_momentary,105:relay_2_momentary,106:relay_3_momentary,107:relay_4_momentary + // @Values: 0:Disabled,1:shift,2:arm_toggle,3:arm,4:disarm,5:mode_manual,6:mode_stabilize,7:mode_depth_hold,8:mode_poshold,9:mode_auto,10:mode_circle,11:mode_guided,12:mode_acro,13:mode_range_hold,21:mount_center,22:mount_tilt_up,23:mount_tilt_down,24:camera_trigger,25:camera_source_toggle,26:mount_pan_right,27:mount_pan_left,31:lights1_cycle,32:lights1_brighter,33:lights1_dimmer,34:lights2_cycle,35:lights2_brighter,36:lights2_dimmer,41:gain_toggle,42:gain_inc,43:gain_dec,44:trim_roll_inc,45:trim_roll_dec,46:trim_pitch_inc,47:trim_pitch_dec,48:input_hold_set,49:roll_pitch_toggle,51:relay_1_on,52:relay_1_off,53:relay_1_toggle,54:relay_2_on,55:relay_2_off,56:relay_2_toggle,57:relay_3_on,58:relay_3_off,59:relay_3_toggle,61:servo_1_inc,62:servo_1_dec,63:servo_1_min,64:servo_1_max,65:servo_1_center,66:servo_2_inc,67:servo_2_dec,68:servo_2_min,69:servo_2_max,70:servo_2_center,71:servo_3_inc,72:servo_3_dec,73:servo_3_min,74:servo_3_max,75:servo_3_center,76:servo_1_min_momentary,77:servo_1_max_momentary,78:servo_1_min_toggle,79:servo_1_max_toggle,80:servo_2_min_momentary,81:servo_2_max_momentary,82:servo_2_min_toggle,83:servo_2_max_toggle,84:servo_3_min_momentary,85:servo_3_max_momentary,86:servo_3_min_toggle,87:servo_3_max_toggle,91:custom_1,92:custom_2,93:custom_3,94:custom_4,95:custom_5,96:custom_6,101:relay_4_on,102:relay_4_off,103:relay_4_toggle,104:relay_1_momentary,105:relay_2_momentary,106:relay_3_momentary,107:relay_4_momentary // @User: Standard AP_GROUPINFO("FUNCTION", 1, JSButton, _function, 0), // @Param: SFUNCTION // @DisplayName: Function for button when the shift mode is toggled on // @Description: Set to 0 to disable or choose a function - // @Values: 0:Disabled,1:shift,2:arm_toggle,3:arm,4:disarm,5:mode_manual,6:mode_stabilize,7:mode_depth_hold,8:mode_poshold,9:mode_auto,10:mode_circle,11:mode_guided,12:mode_acro,21:mount_center,22:mount_tilt_up,23:mount_tilt_down,24:camera_trigger,25:camera_source_toggle,26:mount_pan_right,27:mount_pan_left,31:lights1_cycle,32:lights1_brighter,33:lights1_dimmer,34:lights2_cycle,35:lights2_brighter,36:lights2_dimmer,41:gain_toggle,42:gain_inc,43:gain_dec,44:trim_roll_inc,45:trim_roll_dec,46:trim_pitch_inc,47:trim_pitch_dec,48:input_hold_set,49:roll_pitch_toggle,51:relay_1_on,52:relay_1_off,53:relay_1_toggle,54:relay_2_on,55:relay_2_off,56:relay_2_toggle,57:relay_3_on,58:relay_3_off,59:relay_3_toggle,61:servo_1_inc,62:servo_1_dec,63:servo_1_min,64:servo_1_max,65:servo_1_center,66:servo_2_inc,67:servo_2_dec,68:servo_2_min,69:servo_2_max,70:servo_2_center,71:servo_3_inc,72:servo_3_dec,73:servo_3_min,74:servo_3_max,75:servo_3_center,76:servo_1_min_momentary,77:servo_1_max_momentary,78:servo_1_min_toggle,79:servo_1_max_toggle,80:servo_2_min_momentary,81:servo_2_max_momentary,82:servo_2_min_toggle,83:servo_2_max_toggle,84:servo_3_min_momentary,85:servo_3_max_momentary,86:servo_3_min_toggle,87:servo_3_max_toggle,91:custom_1,92:custom_2,93:custom_3,94:custom_4,95:custom_5,96:custom_6,101:relay_4_on,102:relay_4_off,103:relay_4_toggle,104:relay_1_momentary,105:relay_2_momentary,106:relay_3_momentary,107:relay_4_momentary + // @Values: 0:Disabled,1:shift,2:arm_toggle,3:arm,4:disarm,5:mode_manual,6:mode_stabilize,7:mode_depth_hold,8:mode_poshold,9:mode_auto,10:mode_circle,11:mode_guided,12:mode_acro,13:mode_range_hold,21:mount_center,22:mount_tilt_up,23:mount_tilt_down,24:camera_trigger,25:camera_source_toggle,26:mount_pan_right,27:mount_pan_left,31:lights1_cycle,32:lights1_brighter,33:lights1_dimmer,34:lights2_cycle,35:lights2_brighter,36:lights2_dimmer,41:gain_toggle,42:gain_inc,43:gain_dec,44:trim_roll_inc,45:trim_roll_dec,46:trim_pitch_inc,47:trim_pitch_dec,48:input_hold_set,49:roll_pitch_toggle,51:relay_1_on,52:relay_1_off,53:relay_1_toggle,54:relay_2_on,55:relay_2_off,56:relay_2_toggle,57:relay_3_on,58:relay_3_off,59:relay_3_toggle,61:servo_1_inc,62:servo_1_dec,63:servo_1_min,64:servo_1_max,65:servo_1_center,66:servo_2_inc,67:servo_2_dec,68:servo_2_min,69:servo_2_max,70:servo_2_center,71:servo_3_inc,72:servo_3_dec,73:servo_3_min,74:servo_3_max,75:servo_3_center,76:servo_1_min_momentary,77:servo_1_max_momentary,78:servo_1_min_toggle,79:servo_1_max_toggle,80:servo_2_min_momentary,81:servo_2_max_momentary,82:servo_2_min_toggle,83:servo_2_max_toggle,84:servo_3_min_momentary,85:servo_3_max_momentary,86:servo_3_min_toggle,87:servo_3_max_toggle,91:custom_1,92:custom_2,93:custom_3,94:custom_4,95:custom_5,96:custom_6,101:relay_4_on,102:relay_4_off,103:relay_4_toggle,104:relay_1_momentary,105:relay_2_momentary,106:relay_3_momentary,107:relay_4_momentary // @User: Standard AP_GROUPINFO("SFUNCTION", 2, JSButton, _sfunction, 0), diff --git a/libraries/AP_JSButton/AP_JSButton.h b/libraries/AP_JSButton/AP_JSButton.h index 3f13f1f07af34f..dcdec82f2e9479 100644 --- a/libraries/AP_JSButton/AP_JSButton.h +++ b/libraries/AP_JSButton/AP_JSButton.h @@ -22,8 +22,9 @@ class JSButton { k_mode_circle = 10, ///< enter circle mode k_mode_guided = 11, ///< enter guided mode k_mode_acro = 12, ///< enter acro mode + k_mode_range_hold = 13, ///< enter range hold mode - // 12-20 reserved for future mode functions + // 14-20 reserved for future mode functions k_mount_center = 21, ///< move mount to center k_mount_tilt_up = 22, ///< tilt mount up k_mount_tilt_down = 23, ///< tilt mount down