diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index d826528e4e..70c7f57eef 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -274,9 +274,6 @@ void Sub::fast_loop() // update home from EKF if necessary update_home_from_EKF(); - // check if we've landed or crashed - update_land_and_crash_detectors(); - // check if we've reached the surface or bottom update_surface_and_bottom_detector(); @@ -302,9 +299,6 @@ void Sub::rc_loop() // --------------------------- void Sub::throttle_loop() { - // update throttle_low_comp value (controls priority of throttle vs attitude control) - update_throttle_thr_mix(); - // check auto_armed status update_auto_armed(); diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index fd7d0bbf9d..d8cafa31a3 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -29,7 +29,7 @@ void Sub::gcs_send_deferred(void) NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan) { uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - uint8_t system_status = ap.land_complete ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE; + uint8_t system_status = motors.armed() ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE; uint32_t custom_mode = control_mode; // set system as critical if any failsafe have triggered diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 47f599257a..369ee1e525 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -236,14 +236,12 @@ private: uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised uint8_t logging_started : 1; // 6 // true if dataflash logging has started - uint8_t land_complete : 1; // 7 // true if we have detected a landing uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio uint8_t usb_connected : 1; // 9 // true if APM is powered from USB connection uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes - uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete) uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock uint8_t system_time_set : 1; // 16 // true if the system time has been set from the GPS uint8_t gps_base_pos_set : 1; // 17 // true when the gps base position has been set (used for RTK gps only) @@ -568,8 +566,6 @@ private: void set_simple_mode(uint8_t b); void set_failsafe_radio(bool b); void set_failsafe_battery(bool b); - void set_land_complete(bool b); - void set_land_complete_maybe(bool b); void set_pre_arm_check(bool b); void set_pre_arm_rc_check(bool b); void update_using_interlock(); @@ -804,10 +800,6 @@ private: void check_dynamic_flight(void); void read_inertia(); void read_inertial_altitude(); - bool land_complete_maybe(); - void update_land_and_crash_detectors(); - void update_land_detector(); - void update_throttle_thr_mix(); void update_surface_and_bottom_detector(); void set_surfaced(bool at_surface); void set_bottomed(bool at_bottom); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 698e845e57..73ba0d33e3 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -186,9 +186,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_WAYPOINT: return verify_nav_wp(cmd); - case MAV_CMD_NAV_LAND: - return verify_land(); - case MAV_CMD_NAV_LOITER_UNLIM: return verify_loiter_unlimited(); @@ -592,42 +589,6 @@ void Sub::do_guided_limits(const AP_Mission::Mission_Command& cmd) // Verify Nav (Must) commands /********************************************************************************/ -// verify_land - returns true if landing has been completed -bool Sub::verify_land() -{ - bool retval = false; - - switch( land_state ) { - case LAND_STATE_FLY_TO_LOCATION: - // check if we've reached the location - if (wp_nav.reached_wp_destination()) { - // get destination so we can use it for loiter target - Vector3f dest = wp_nav.get_wp_destination(); - - // initialise landing controller - auto_land_start(dest); - - // advance to next state - land_state = LAND_STATE_DESCENDING; - } - break; - - case LAND_STATE_DESCENDING: - // rely on THROTTLE_LAND mode to correctly update landing status - retval = ap.land_complete; - break; - - default: - // this should never happen - // TO-DO: log an error - retval = true; - break; - } - - // true is returned if we've successfully landed - return retval; -} - // verify_nav_wp - check if we have reached the next way point bool Sub::verify_nav_wp(const AP_Mission::Mission_Command& cmd) { diff --git a/ArduSub/compassmot.cpp b/ArduSub/compassmot.cpp index 6eb4a2bbf8..165624db4d 100644 --- a/ArduSub/compassmot.cpp +++ b/ArduSub/compassmot.cpp @@ -69,13 +69,6 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) return 1; } - // check we are landed - if (!ap.land_complete) { - gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Not landed"); - ap.compass_mot = false; - return 1; - } - // disable cpu failsafe failsafe_disable(); diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index abe9c06e23..3f40d85f42 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -42,7 +42,7 @@ void Sub::circle_run() pos_control.set_accel_z(g.pilot_accel_z); // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { + if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { // To-Do: add some initialisation of position controllers motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed @@ -63,11 +63,13 @@ void Sub::circle_run() // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); - // check for pilot requested take-off - if (ap.land_complete && target_climb_rate > 0) { - // indicate we are taking off - set_land_complete(false); - } +// // check for pilot requested take-off +// if (ap.land_complete && target_climb_rate > 0) { +// // indicate we are taking off +// set_land_complete(false); +// // clear i term when we're taking off +// set_throttle_takeoff(); +// } } // set motors to full range diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 0c9e9068c9..4bc53efda7 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -346,7 +346,7 @@ void Sub::guided_takeoff_run() void Sub::guided_pos_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) { + if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); @@ -388,7 +388,7 @@ void Sub::guided_pos_control_run() void Sub::guided_vel_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) { + if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { // initialise velocity controller pos_control.init_vel_controller_xyz(); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); @@ -435,7 +435,7 @@ void Sub::guided_vel_control_run() void Sub::guided_posvel_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) { + if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { // set target position and velocity to current position and velocity pos_control.set_pos_target(inertial_nav.get_position()); pos_control.set_desired_velocity(Vector3f(0,0,0)); @@ -504,7 +504,7 @@ void Sub::guided_posvel_control_run() void Sub::guided_angle_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) { + if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt); diff --git a/ArduSub/control_manual.cpp b/ArduSub/control_manual.cpp index 0414c6e8c5..838b973b70 100644 --- a/ArduSub/control_manual.cpp +++ b/ArduSub/control_manual.cpp @@ -4,10 +4,6 @@ // manual_init - initialise manual controller bool Sub::manual_init(bool ignore_checks) { - // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high - if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode)) { - return false; - } // set target altitude to zero for reporting pos_control.set_alt_target(0); diff --git a/ArduSub/control_rtl.cpp b/ArduSub/control_rtl.cpp index 758bd26d79..9e5c943f70 100644 --- a/ArduSub/control_rtl.cpp +++ b/ArduSub/control_rtl.cpp @@ -348,7 +348,7 @@ void Sub::rtl_land_run() int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { + if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); @@ -358,18 +358,18 @@ void Sub::rtl_land_run() #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum - if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { - init_disarm_motors(); - } +// if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { +// init_disarm_motors(); +// } #else // disarm when the landing detector says we've landed - if (ap.land_complete) { - init_disarm_motors(); - } +// if (ap.land_complete) { +// init_disarm_motors(); +// } #endif // check if we've completed this stage of RTL - rtl_state_complete = ap.land_complete; +// rtl_state_complete = ap.land_complete; return; } @@ -414,7 +414,7 @@ void Sub::rtl_land_run() attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); // check if we've completed this stage of RTL - rtl_state_complete = ap.land_complete; +// rtl_state_complete = ap.land_complete; } void Sub::rtl_build_path(bool terrain_following_allowed) diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 3cf83a77f6..d1f64732fd 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -313,8 +313,8 @@ enum ThrowModeState { #define DATA_ARMED 10 #define DATA_DISARMED 11 #define DATA_AUTO_ARMED 15 -#define DATA_LAND_COMPLETE_MAYBE 17 -#define DATA_LAND_COMPLETE 18 +//#define DATA_LAND_COMPLETE_MAYBE 17 +//#define DATA_LAND_COMPLETE 18 #define DATA_NOT_LANDED 28 #define DATA_LOST_GPS 19 //#define DATA_FLIP_START 21 diff --git a/ArduSub/events.cpp b/ArduSub/events.cpp index 9591f1110e..8e7e61d303 100644 --- a/ArduSub/events.cpp +++ b/ArduSub/events.cpp @@ -298,16 +298,17 @@ bool Sub::should_disarm_on_failsafe() { case STABILIZE: case ACRO: // if throttle is zero OR vehicle is landed disarm motors - return ap.throttle_zero || ap.land_complete; + return ap.throttle_zero; break; case AUTO: // if mission has not started AND vehicle is landed, disarm motors - return !ap.auto_armed && ap.land_complete; + return !ap.auto_armed; break; default: // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold // if landed disarm - return ap.land_complete; +// return ap.land_complete; + return false; break; } } diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index 73c7e773d0..92b9ea2e1e 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -225,7 +225,7 @@ void Sub::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_ } // smooth throttle transition when switching from manual to automatic flight modes - if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) { + if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed()) { // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in)); } diff --git a/ArduSub/land_detector.cpp b/ArduSub/land_detector.cpp deleted file mode 100644 index 3391d32dee..0000000000 --- a/ArduSub/land_detector.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Sub.h" -#define LAND_END_DEPTH -20 //Landed at 20cm depth - -// Code to detect a crash main ArduCopter code -#define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing -#define LAND_CHECK_LARGE_ANGLE_CD 1500.0f // maximum angle target to be considered landing -#define LAND_CHECK_ACCEL_MOVING 3.0f // maximum acceleration after subtracting gravity - -// counter to verify landings -static uint32_t land_detector_count = 0; - -// run land and crash detectors -// called at MAIN_LOOP_RATE -void Sub::update_land_and_crash_detectors() -{ - // update 1hz filtered acceleration - Vector3f accel_ef = ahrs.get_accel_ef_blended(); - accel_ef.z += GRAVITY_MSS; - land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS); - - update_land_detector(); - - crash_check(); -} - -// update_land_detector - checks if we have landed and updates the ap.land_complete flag -// called at MAIN_LOOP_RATE -void Sub::update_land_detector() -{ -// if(barometer.num_instances() > 1 && barometer.get_altitude() > LAND_END_DEPTH && ap.throttle_zero) { -// set_land_complete(true); -// } else { - set_land_complete(false); -// } -} - -void Sub::set_land_complete(bool b) -{ - // if no change, exit immediately - if( ap.land_complete == b ) - return; - - land_detector_count = 0; - - if(b){ - Log_Write_Event(DATA_LAND_COMPLETE); - } else { - Log_Write_Event(DATA_NOT_LANDED); - } - ap.land_complete = b; - - // trigger disarm-on-land if configured - bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0; - bool mode_disarms_on_land = mode_allows_arming(control_mode,false) && !mode_has_manual_throttle(control_mode); - - if (ap.land_complete && motors.armed() && disarm_on_land_configured && mode_disarms_on_land) { - init_disarm_motors(); - } -} - -// 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() -{ - return; // placeholder, was used by heli only -} diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 5f886b9450..fde87054aa 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -114,7 +114,7 @@ void Sub::auto_disarm_check() thr_low = g.rc_3.control_in <= deadband_top; } - if (!thr_low || !ap.land_complete) { + if (!thr_low) { // reset timer auto_disarm_begin = tnow_ms; } @@ -246,10 +246,6 @@ void Sub::init_disarm_motors() autotune_save_tuning_gains(); #endif - // we are not in the air -// 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);