aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fence: enable altitude fences for sub

Browse Source
This commit is contained in:
jaxxzer 2016-02-28 20:14:54 -05:00 committed by Rustom Jehangir
parent 6ea44b4537
commit 6cd632152f
7 changed files with 77 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/APM_Config.h
View File

@ -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

18
ArduSub/Attitude.cpp
View File

@ -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

4
ArduSub/fence.cpp
View File

@ -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

35
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -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

10
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -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

12
libraries/AC_Fence/AC_Fence.cpp
View File

@ -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
};

12
libraries/AC_Fence/AC_Fence.h
View File

@ -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