mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 23:48:31 -04:00
Sub: Top and bottom detection working for alt hold
This commit is contained in:
parent
8b337a9c7e
commit
48983c38f8
@ -124,8 +124,9 @@ void Sub::althold_run()
|
||||
if(ap.at_bottom) {
|
||||
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
|
||||
} else if(ap.at_surface) {
|
||||
pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator
|
||||
} else if(ap.at_surface) { // We could use the alt max parameter for this
|
||||
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
|
||||
} else {
|
||||
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
|
@ -175,8 +175,10 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
|
||||
|
||||
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)
|
||||
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) {
|
||||
// Reset home position if it has already been set before (but not locked)
|
||||
set_home_to_current_location();
|
||||
|
@ -3,7 +3,7 @@
|
||||
|
||||
#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 BOTTOM_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
|
||||
}
|
||||
|
||||
} 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);
|
||||
// }
|
||||
|
||||
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user