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

Sub: top and bottom detection working well

Browse Source 
althold makes good use of the information and will refuse to attempt to
fly out of the water or dig underground
This commit is contained in:
jaxxzer 2016-02-28 19:13:06 -05:00 committed by Andrew Tridgell
parent de669c1d41
commit 00e1c847a6
3 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/Sub.h
View File

@ -393,7 +393,6 @@ private:
int32_t baro_alt; // barometer altitude in cm above home int32_t baro_alt; // barometer altitude in cm above home
float baro_climbrate; // barometer climbrate in cm/s float baro_climbrate; // barometer climbrate in cm/s
LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
LowPassFilterVector3f depth_accel_ef_filter; // accelerations for land and crash detector tests
// 3D Location vectors // 3D Location vectors
// Current location of the Sub (altitude is relative to home) // Current location of the Sub (altitude is relative to home)

11
ArduSub/control_althold.cpp
View File

@ -124,13 +124,18 @@ 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) { // We could use the alt max parameter for this } else if(ap.at_surface) {
if(motors.limit.throttle_upper) // ToDo use a better condition if(pos_control.get_vel_target_z() > 0.0) {
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 below surface pos_control.set_alt_target(SURFACE_DEPTH); // set alt target to the same depth that triggers the surface detector.
}
if(target_climb_rate < 0) { // Dive if the pilot wants to
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
}
} 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);
} }
pos_control.update_z_controller(); pos_control.update_z_controller();
break; break;
} }

4
ArduSub/defines.h
View File

@ -13,6 +13,10 @@
#define ENABLE ENABLED #define ENABLE ENABLED
#define DISABLE DISABLED #define DISABLE DISABLED
#define SURFACE_DEPTH -0.05 // 5cm, depends on where the external depth sensor is mounted
#define BOTTOM_DETECTOR_TRIGGER_SEC 1.0
#define SURFACE_DETECTOR_TRIGGER_SEC 1.0
// Autopilot Yaw Mode enumeration // Autopilot Yaw Mode enumeration
enum autopilot_yaw_mode { enum autopilot_yaw_mode {
AUTO_YAW_HOLD = 0, // pilot controls the heading AUTO_YAW_HOLD = 0, // pilot controls the heading