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

Sub: fix surface bottom detection

Browse Source 
output_min() to make sure throttle limits are false when disarmed
This commit is contained in:
jaxxzer 2016-06-23 10:59:51 -04:00 committed by Andrew Tridgell
parent fa5af82d4c
commit d3f5f59b8c
2 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/control_stabilize.cpp
View File

@ -29,6 +29,7 @@ void Sub::stabilize_run()
// if not armed set throttle to zero and exit immediately
if (!motors.armed() || !motors.get_interlock()) {
motors.output_min();
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;

20
ArduSub/surface_bottom_detector.cpp
View File

@ -10,6 +10,7 @@ static float current_depth = 0;
// checks if we have have hit bottom or surface and updates the ap.at_bottom and ap.at_surface flags
// called at MAIN_LOOP_RATE
// ToDo: doesn't need to be called this fast
void Sub::update_surface_and_bottom_detector()
{
Vector3f velocity;
@ -19,9 +20,15 @@ void Sub::update_surface_and_bottom_detector()
bool vel_stationary = velocity.z > -0.05 && velocity.z < 0.05;
if (ap.depth_sensor_present) { // we can use the external pressure sensor for a very accurate and current measure of our z axis position
current_depth = barometer.get_altitude();
current_depth = barometer.get_altitude(); // cm
if(ap.at_surface) {
set_surfaced(current_depth > g.surface_depth/100.0 - 0.05); // add a 5cm buffer so it doesn't trigger too often
} else {
set_surfaced(current_depth > g.surface_depth/100.0); // If we are above surface depth, we are surfaced
}
set_surfaced(current_depth > g.surface_depth/100.0); // If we are above surface depth, we are surfaced
if(motors.limit.throttle_lower && vel_stationary) {
// bottom criteria met - increment the counter and check if we've triggered
@ -37,8 +44,8 @@ void Sub::update_surface_and_bottom_detector()
// with no external baro, the only thing we have to go by is a vertical velocity estimate
} else if (vel_stationary) {
if(motors.limit.throttle_upper) {
// surface criteria met, increment counter and see if we've triggered
if( surface_detector_count < ((float)SURFACE_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
surface_detector_count++;
@ -66,6 +73,8 @@ void Sub::update_surface_and_bottom_detector()
}
void Sub::set_surfaced(bool at_surface) {
if(ap.at_surface == at_surface) { // do nothing if state unchanged
return;
}
@ -73,16 +82,17 @@ void Sub::set_surfaced(bool at_surface) {
ap.at_surface = at_surface;
if(!ap.at_surface) {
surface_detector_count = 0;
Log_Write_Event(DATA_SURFACED);
gcs_send_text(MAV_SEVERITY_INFO, "Off Surface");
} else {
surface_detector_count = 0;
Log_Write_Event(DATA_NOT_SURFACED);
gcs_send_text(MAV_SEVERITY_INFO, "Surfaced");
}
}
void Sub::set_bottomed(bool at_bottom) {
if(ap.at_bottom == at_bottom) { // do nothing if state unchanged
return;
}
@ -90,10 +100,10 @@ void Sub::set_bottomed(bool at_bottom) {
ap.at_bottom = at_bottom;
if(!ap.at_bottom) {
bottom_detector_count = 0;
Log_Write_Event(DATA_BOTTOMED);
gcs_send_text(MAV_SEVERITY_INFO, "Off Bottom");
} else {
bottom_detector_count = 0;
Log_Write_Event(DATA_NOT_BOTTOMED);
gcs_send_text(MAV_SEVERITY_INFO, "Bottomed");
}