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

Sub: Enable altitude fences for sub

Browse Source
This commit is contained in:
jaxxzer 2016-02-28 20:14:54 -05:00 committed by Andrew Tridgell
parent 00e1c847a6
commit 8ff479ca1e
3 changed files with 9 additions and 15 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