Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Top and bottom detection working for alt hold

Browse Source
This commit is contained in:
jaxxzer 2016-02-26 21:01:10 -05:00 committed by Andrew Tridgell
parent 8b337a9c7e
commit 48983c38f8
3 changed files with 31 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduSub/control_althold.cpp
View File

@ -124,8 +124,9 @@ void Sub::althold_run()
if(ap.at_bottom) { if(ap.at_bottom) {
pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator
pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
} else if(ap.at_surface) { } else if(ap.at_surface) { // We could use the alt max parameter for this
pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator if(motors.limit.throttle_upper) // ToDo use a better condition
pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator
pos_control.set_alt_target(inertial_nav.get_altitude() - 10.0f); // set target to 10 cm below surface pos_control.set_alt_target(inertial_nav.get_altitude() - 10.0f); // set target to 10 cm below surface
} else { } else {
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);

6
ArduSub/motors.cpp
View File

@ -175,8 +175,10 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
if (ap.home_state == HOME_UNSET) { if (ap.home_state == HOME_UNSET) {
// Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
ahrs.resetHeightDatum();
Log_Write_Event(DATA_EKF_ALT_RESET); // Always use absolute altitude for ROV
// ahrs.resetHeightDatum();
// Log_Write_Event(DATA_EKF_ALT_RESET);
} else if (ap.home_state == HOME_SET_NOT_LOCKED) { } else if (ap.home_state == HOME_SET_NOT_LOCKED) {
// Reset home position if it has already been set before (but not locked) // Reset home position if it has already been set before (but not locked)
set_home_to_current_location(); set_home_to_current_location();

47
ArduSub/surface_bottom_detector.cpp
View File

@ -3,7 +3,7 @@
#include "Sub.h" #include "Sub.h"
#define SURFACE_DEPTH -0.02 // 2cm depends on where the external depth sensor is mounted #define SURFACE_DEPTH -0.05 // 5cm depends on where the external depth sensor is mounted
#define DIVE_DEPTH_CONSTRAINT 0.10 // 10cm if we cannot achieve this target, we are trying to dig into the bottom #define DIVE_DEPTH_CONSTRAINT 0.10 // 10cm if we cannot achieve this target, we are trying to dig into the bottom
#define BOTTOM_DETECTOR_TRIGGER_SEC 1.0 #define BOTTOM_DETECTOR_TRIGGER_SEC 1.0
#define SURFACE_DETECTOR_TRIGGER_SEC 1.0 #define SURFACE_DETECTOR_TRIGGER_SEC 1.0
@ -52,29 +52,30 @@ void Sub::update_surface_and_bottom_detector()
set_bottomed(false); // If we are not trying to descend at lower throttle limit, we are not at the bottom set_bottomed(false); // If we are not trying to descend at lower throttle limit, we are not at the bottom
} }
} else if (accel_stationary) {
if(motors.limit.throttle_upper) {
if( surface_detector_count < ((float)SURFACE_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
surface_detector_count++;
} else {
set_surfaced(true);
}
} else if(motors.limit.throttle_lower) {
// landed criteria met - increment the counter and check if we've triggered
if( bottom_detector_count < ((float)BOTTOM_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
bottom_detector_count++;
} else {
set_bottomed(true);
}
} else {
set_surfaced(false);
set_bottomed(false);
}
} else {
// we've sensed movement up or down so reset land_detector
set_surfaced(false);
set_bottomed(false);
} }
// else if (accel_stationary) {
// if(motors.limit.throttle_upper) {
// if( surface_detector_count < ((float)SURFACE_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
// surface_detector_count++;
// } else {
// set_surfaced(true);
// }
// } else if(motors.limit.throttle_lower) {
// // landed criteria met - increment the counter and check if we've triggered
// if( bottom_detector_count < ((float)BOTTOM_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
// bottom_detector_count++;
// } else {
// set_bottomed(true);
// }
// } else {
// set_surfaced(false);
// set_bottomed(false);
// }
// } else {
// // we've sensed movement up or down so reset land_detector
// set_surfaced(false);
// set_bottomed(false);
// }
} }