From 6cd632152f58b8d7607de3a1389c8b5a43f5aa08 Mon Sep 17 00:00:00 2001 From: jaxxzer Date: Sun, 28 Feb 2016 20:14:54 -0500 Subject: [PATCH] Fence: enable altitude fences for sub --- ArduSub/APM_Config.h | 2 +- ArduSub/Attitude.cpp | 18 ++++------ ArduSub/fence.cpp | 4 +-- .../AC_AttitudeControl/AC_PosControl.cpp | 35 +++++++++++++++++++ libraries/AC_AttitudeControl/AC_PosControl.h | 10 ++++++ libraries/AC_Fence/AC_Fence.cpp | 12 ++++++- libraries/AC_Fence/AC_Fence.h | 12 +++++++ 7 files changed, 77 insertions(+), 16 deletions(-) diff --git a/ArduSub/APM_Config.h b/ArduSub/APM_Config.h index a362bde98a..8a656fc50b 100644 --- a/ArduSub/APM_Config.h +++ b/ArduSub/APM_Config.h @@ -16,7 +16,7 @@ //#define LOGGING_ENABLED DISABLED // disable dataflash logging to save 11K of flash space //#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space #define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash -#define AC_FENCE DISABLED // disable fence to save 2k of flash +//#define AC_FENCE DISABLED // disable fence to save 2k of flash //#define CAMERA DISABLED // disable camera trigger to save 1k of flash //#define CONFIG_SONAR DISABLED // disable sonar to save 1k of flash #define POSHOLD_ENABLED DISABLED // disable PosHold flight mode to save 4.5k of flash diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index 231519876e..cab8b39403 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -289,25 +289,19 @@ void Sub::set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle) // updates position controller's maximum altitude using fence and EKF limits void Sub::update_poscon_alt_max() { - float alt_limit_cm = 0.0f; // interpreted as no limit if left as zero + float min_alt_cm = 0.0f; // interpreted as no limit if left as zero + float max_alt_cm = 0.0f; #if AC_FENCE == ENABLED // set fence altitude limit in position controller if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { - alt_limit_cm = pv_alt_above_origin(fence.get_safe_alt()*100.0f); + min_alt_cm = fence.get_safe_depth()*100.0f; + max_alt_cm = fence.get_safe_alt()*100.0f; } #endif - - // get alt limit from EKF (limited during optical flow flight) - float ekf_limit_cm = 0.0f; - if (inertial_nav.get_hgt_ctrl_limit(ekf_limit_cm)) { - if ((alt_limit_cm <= 0.0f) || (ekf_limit_cm < alt_limit_cm)) { - alt_limit_cm = ekf_limit_cm; - } - } - // pass limit to pos controller - pos_control.set_alt_max(alt_limit_cm); + pos_control.set_alt_min(min_alt_cm); + pos_control.set_alt_max(max_alt_cm); } // rotate vector from vehicle's perspective to North-East frame diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp index bb6aac46f1..8578c15083 100644 --- a/ArduSub/fence.cpp +++ b/ArduSub/fence.cpp @@ -8,7 +8,7 @@ // fence_check - ask fence library to check for breaches and initiate the response // called at 1hz -void Copter::fence_check() +void Sub::fence_check() { uint8_t new_breaches; // the type of fence that has been breached uint8_t orig_breaches = fence.get_breaches(); @@ -58,7 +58,7 @@ void Copter::fence_check() } // fence_send_mavlink_status - send fence status to ground station -void Copter::fence_send_mavlink_status(mavlink_channel_t chan) +void Sub::fence_send_mavlink_status(mavlink_channel_t chan) { if (fence.enabled()) { // traslate fence library breach types to mavlink breach types diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 5392893fea..febdda55d1 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -55,6 +55,9 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, _roll_target(0.0f), _pitch_target(0.0f), _alt_max(0.0f), +#if APM_BUILD_TYPE(APM_BUILD_ArduSub) + _alt_min(0.0f), +#endif _distance_to_target(0.0f), _accel_target_jerk_limited(0.0f,0.0f), _accel_target_filter(POSCONTROL_ACCEL_FILTER_HZ) @@ -165,11 +168,25 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d _pos_target.z += climb_rate_cms * dt; } +#if APM_BUILD_TYPE(APM_BUILD_ArduSub) + // do not let target alt get above limit + if (_alt_max < 0 && _alt_max > _alt_min && _pos_target.z > _alt_max) { + _pos_target.z = _alt_max; + _limit.pos_up = true; + } + + // do not let target alt get below limit + if (_alt_min < 0 && _pos_target.z < _alt_min) { + _pos_target.z = _alt_min; + _limit.pos_down = true; + } +#else // do not let target alt get above limit if (_alt_max > 0 && _pos_target.z > _alt_max) { _pos_target.z = _alt_max; _limit.pos_up = true; } +#endif // do not use z-axis desired velocity feed forward // vel_desired set to desired climb rate for reporting and land-detector @@ -212,6 +229,23 @@ void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, floa _pos_target.z += _vel_desired.z * dt; } +#if APM_BUILD_TYPE(APM_BUILD_ArduSub) + // do not let target alt get above limit + if (_alt_max < 0 && _alt_max > _alt_min && _pos_target.z > _alt_max) { + _pos_target.z = _alt_max; + _limit.pos_up = true; + // decelerate feed forward to zero + _vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit); + } + + // do not let target alt get below limit + if (_alt_min < 0 && _pos_target.z < _alt_min) { + _pos_target.z = _alt_min; + _limit.pos_down = true; + // decelerate feed forward to zero + _vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit); + } +#else // do not let target alt get above limit if (_alt_max > 0 && _pos_target.z > _alt_max) { _pos_target.z = _alt_max; @@ -219,6 +253,7 @@ void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, floa // decelerate feed forward to zero _vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit); } +#endif } /// add_takeoff_climb_rate - adjusts alt target up or down using a climb rate in cm/s diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index b1ec34c855..1c90fdb22b 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -84,6 +84,13 @@ public: /// set to zero to disable limit void set_alt_max(float alt) { _alt_max = alt; } + /// set_alt_min - sets the minimum altitude (maximum depth) in cm + /// only enforced when set_alt_target_from_climb_rate is used + /// set to zero to disable limit +#if APM_BUILD_TYPE(APM_BUILD_ArduSub) + void set_alt_min(float alt) { _alt_min = alt; } +#endif + /// set_speed_z - sets maximum climb and descent rates /// speed_down can be positive or negative but will always be interpreted as a descent speed /// leash length will be recalculated the next time update_z_controller() is called @@ -401,6 +408,9 @@ private: Vector3f _accel_error; // desired acceleration in cm/s/s // To-Do: are xy actually required? Vector3f _accel_feedforward; // feedforward acceleration in cm/s/s float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence +#if APM_BUILD_TYPE(APM_BUILD_ArduSub) + float _alt_min; +#endif float _distance_to_target; // distance to position target - for reporting only LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index d39b3e3583..508f569665 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -50,7 +50,17 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @Range: 1 10 // @User: Standard AP_GROUPINFO("MARGIN", 5, AC_Fence, _margin, AC_FENCE_MARGIN_DEFAULT), - + +#if APM_BUILD_TYPE(APM_BUILD_ArduSub) + // @Param: DEPTH_MAX + // @DisplayName: Fence Maximum Depth + // @Description: Maximum depth allowed before geofence triggers + // @Units: Meters + // @Range: 10 1000 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("DEPTH_MAX", 6, AC_Fence, _alt_min, AC_FENCE_DEPTH_MAX_DEFAULT), +#endif AP_GROUPEND }; diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 5a3330e80d..8d917c0fad 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -24,6 +24,10 @@ #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further out #define AC_FENCE_MARGIN_DEFAULT 2.0f // default distance in meters that autopilot's should maintain from the fence to avoid a breach +#if APM_BUILD_TYPE(APM_BUILD_ArduSub) +#define AC_FENCE_DEPTH_MAX_DEFAULT -10.0f // default maximum depth in meters +#endif + // give up distance #define AC_FENCE_GIVE_UP_DISTANCE 100.0f // distance outside the fence at which we should give up and just land. Note: this is not used by library directly but is intended to be used by the main code #define AC_FENCE_MANUAL_RECOVERY_TIME_MIN 10000 // pilot has 10seconds to recover during which time the autopilot will not attempt to re-take control @@ -73,6 +77,10 @@ public: /// get_safe_alt - returns maximum safe altitude (i.e. alt_max - margin) float get_safe_alt() const { return _alt_max - _margin; } +#if APM_BUILD_TYPE(APM_BUILD_ArduSub) + float get_safe_depth() const { return _alt_min + _margin; } +#endif + /// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds /// should be called whenever the pilot changes the flight mode /// has no effect if no breaches have occurred @@ -106,6 +114,10 @@ private: AP_Float _circle_radius; // circle fence radius in meters AP_Float _margin; // distance in meters that autopilot's should maintain from the fence to avoid a breach +#if APM_BUILD_TYPE(APM_BUILD_ArduSub) + AP_Float _alt_min; +#endif + // backup fences float _alt_max_backup; // backup altitude upper limit in meters used to refire the breach if the vehicle continues to move further away float _circle_radius_backup; // backup circle fence radius in meters used to refire the breach if the vehicle continues to move further away