From 20e29b6b3aa4aeeff432668703346fc28fab4fef Mon Sep 17 00:00:00 2001 From: jaxxzer Date: Sat, 23 Jan 2016 22:20:07 -0500 Subject: [PATCH] Sub: Set EK2_ALT_NOISE on startup according to baro sensors detected. --- ArduSub/land_detector.cpp | 49 ++++++++++++++++++++++++++++++- ArduSub/motors.cpp | 4 +-- ArduSub/system.cpp | 8 +++-- libraries/AP_NavEKF/AP_NavEKF.h | 2 ++ libraries/AP_NavEKF2/AP_NavEKF2.h | 2 ++ 5 files changed, 60 insertions(+), 5 deletions(-) diff --git a/ArduSub/land_detector.cpp b/ArduSub/land_detector.cpp index d42f8925ee..ab93587b3e 100644 --- a/ArduSub/land_detector.cpp +++ b/ArduSub/land_detector.cpp @@ -24,7 +24,7 @@ void Sub::update_land_and_crash_detectors() // called at MAIN_LOOP_RATE void Sub::update_land_detector() { - if(barometer.num_instances() > 1 && barometer.get_altitude() > SURFACE_END_DEPTH && ap.throttle_zero) { + if(barometer.num_instances() > 1 && barometer.get_altitude() > LAND_END_DEPTH && ap.throttle_zero) { set_land_complete(true); } else { set_land_complete(false); @@ -46,3 +46,50 @@ void Sub::set_land_complete(bool b) } ap.land_complete = b; } + +// update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state +// low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle +// has no effect when throttle is above hover throttle +void Sub::update_throttle_thr_mix() +{ +#if FRAME_CONFIG != HELI_FRAME + // if disarmed or landed prioritise throttle + if(!motors.armed() || ap.land_complete) { + motors.set_throttle_mix_min(); + return; + } + + if (mode_has_manual_throttle(control_mode)) { + // manual throttle + if(channel_throttle->control_in <= 0) { + motors.set_throttle_mix_min(); + } else { + motors.set_throttle_mix_mid(); + } + } else { + // autopilot controlled throttle + + // check for aggressive flight requests - requested roll or pitch angle below 15 degrees + const Vector3f angle_target = attitude_control.get_att_target_euler_cd(); + bool large_angle_request = (pythagorous2(angle_target.x, angle_target.y) > 1500.0f); + + // check for large external disturbance - angle error over 30 degrees + const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd(); + bool large_angle_error = (pythagorous2(angle_error.x, angle_error.y) > 3000.0f); + + // check for large acceleration - falling or high turbulence + Vector3f accel_ef = ahrs.get_accel_ef_blended(); + accel_ef.z += GRAVITY_MSS; + bool accel_moving = (accel_ef.length() > 3.0f); + + // check for requested decent + bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f; + + if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) { + motors.set_throttle_mix_max(); + } else { + motors.set_throttle_mix_min(); + } + } +#endif +} diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 5199576be6..a36c0366c3 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -233,8 +233,8 @@ void Sub::init_disarm_motors() #endif // we are not in the air - set_land_complete(true); - set_land_complete_maybe(true); +// set_land_complete(true);// We will let the land detector decide this in sub +// set_land_complete_maybe(true); // log disarm to the dataflash Log_Write_Event(DATA_DISARMED); diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 6e71451b71..9419da785e 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -249,6 +249,11 @@ void Sub::init_ardupilot() } barometer.set_primary_baro(1); //Set the primary baro to external MS58XX + EKF.set_baro_alt_noise(0.1f); + EKF2.set_baro_alt_noise(0.1f); + } else { //We only have onboard baro + EKF.set_baro_alt_noise(10.0f); + EKF2.set_baro_alt_noise(10.0f); } // read Baro pressure at ground //----------------------------- @@ -273,8 +278,7 @@ void Sub::init_ardupilot() startup_INS_ground(); // set landed flags - set_land_complete(true); - set_land_complete_maybe(true); + set_land_complete(false); // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 455911a21f..77710bbd71 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -256,6 +256,8 @@ public: // report any reason for why the backend is refusing to initialise const char *prearm_failure_reason(void) const; + void set_baro_alt_noise(float noise) { _baroAltNoise.set_and_save(noise); }; + static const struct AP_Param::GroupInfo var_info[]; private: diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 8c2f5faa94..31ac440225 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -264,6 +264,8 @@ public: // report any reason for why the backend is refusing to initialise const char *prearm_failure_reason(void) const; + void set_baro_alt_noise(float noise) { _baroAltNoise.set_and_save(noise); }; + // allow the enable flag to be set by Replay void set_enable(bool enable) { _enable.set(enable); }