diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 0895316139..4b69234910 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -53,7 +53,6 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan) case GUIDED: case CIRCLE: case POSHOLD: - case BRAKE: case TRANSECT: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what @@ -174,7 +173,6 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan) case SURFACE: case OF_LOITER: case POSHOLD: - case BRAKE: case TRANSECT: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 53bb4a3270..bd9686291b 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -387,10 +387,6 @@ private: uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) uint32_t loiter_time; // How long have we been loitering - The start time in millis - // Brake - uint32_t brake_timeout_start; - uint32_t brake_timeout_ms; - // Delay the next navigation command int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.) uint32_t nav_delay_time_start; @@ -736,9 +732,6 @@ private: void autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max); void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max); - bool brake_init(bool ignore_checks); - void brake_run(); - void brake_timeout_to_loiter_ms(uint32_t timeout_ms); bool circle_init(bool ignore_checks); void circle_run(); bool drift_init(bool ignore_checks); diff --git a/ArduSub/control_brake.cpp b/ArduSub/control_brake.cpp deleted file mode 100644 index 3deebdcfd9..0000000000 --- a/ArduSub/control_brake.cpp +++ /dev/null @@ -1,87 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Sub.h" - -/* - * control_brake.pde - init and run calls for brake flight mode - */ - -// brake_init - initialise brake controller -bool Sub::brake_init(bool ignore_checks) -{ - if (position_ok() || ignore_checks) { - - // set desired acceleration to zero - wp_nav.clear_pilot_desired_acceleration(); - - // set target to current position - wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE); - - // initialize vertical speed and acceleration - pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z); - pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE); - - // initialise position and desired velocity - pos_control.set_alt_target(inertial_nav.get_altitude()); - pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); - - brake_timeout_ms = 0; - - return true; - }else{ - return false; - } -} - -// brake_run - runs the brake controller -// should be called at 100hz or more -void Sub::brake_run() -{ - // if not auto armed set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { - wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE); - 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); - - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-motors.get_throttle_hover()); - return; - } - - // relax stop target if we might be landed - if (ap.land_complete_maybe) { - wp_nav.loiter_soften_for_landing(); - } - - // if landed immediately disarm - if (ap.land_complete) { - init_disarm_motors(); - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // run brake controller - wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler); - - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain()); - - // body-frame rate controller is run directly from 100hz loop - - // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); - pos_control.update_z_controller(); - - if (brake_timeout_ms != 0 && millis()-brake_timeout_start >= brake_timeout_ms) { - if (!set_mode(POSHOLD, MODE_REASON_BRAKE_TIMEOUT)) { - set_mode(ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT); - } - } -} - -void Sub::brake_timeout_to_loiter_ms(uint32_t timeout_ms) -{ - brake_timeout_start = millis(); - brake_timeout_ms = timeout_ms; -} diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 392087e874..5387e935c8 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -68,7 +68,7 @@ enum aux_sw_func { AUXSW_LOST_COPTER_SOUND = 30, // Play lost copter sound AUXSW_MOTOR_ESTOP = 31, // Emergency Stop Switch AUXSW_MOTOR_INTERLOCK = 32, // Motor On/Off switch - AUXSW_BRAKE = 33, // Brake flight mode +// AUXSW_BRAKE = 33, // Brake flight mode AUXSW_RELAY2 = 34, // Relay2 pin on/off (in Mission planner set CH8_OPT = 34) AUXSW_RELAY3 = 35, // Relay3 pin on/off (in Mission planner set CH9_OPT = 35) AUXSW_RELAY4 = 36, // Relay4 pin on/off (in Mission planner set CH10_OPT = 36) @@ -112,7 +112,6 @@ enum control_mode_t { TRANSECT = 13, // automatic x/y velocity, automatic heading/crosstrack error compensation, automatic depth/throttle AUTOTUNE = 15, // not implemented in sub // automatically tune the vehicle's roll and pitch gains POSHOLD = 16, // automatic position hold with manual override, with automatic throttle - BRAKE = 17, // not implemented in sub // full-brake using inertial/GPS system, no pilot input THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input MANUAL = 19 // Pass-through input with no stabilization }; @@ -130,7 +129,6 @@ enum mode_reason_t { MODE_REASON_THROTTLE_LAND_ESCAPE, MODE_REASON_FENCE_BREACH, MODE_REASON_TERRAIN_FAILSAFE, - MODE_REASON_BRAKE_TIMEOUT, MODE_REASON_SURFACE_COMPLETE, MODE_REASON_LEAK_FAILSAFE }; diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index 304b32a2cc..b6fa11d0c1 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -85,10 +85,6 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason) break; #endif - case BRAKE: - success = brake_init(ignore_checks); - break; - case THROW: success = throw_init(ignore_checks); break; @@ -200,10 +196,6 @@ void Sub::update_flight_mode() break; #endif - case BRAKE: - brake_run(); - break; - case THROW: throw_run(); break; @@ -260,7 +252,6 @@ bool Sub::mode_requires_GPS(control_mode_t mode) { case CIRCLE: case DRIFT: case POSHOLD: - case BRAKE: case THROW: case TRANSECT: return true; @@ -360,9 +351,6 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case POSHOLD: port->print("POSHOLD"); break; - case BRAKE: - port->print("BRAKE"); - break; case THROW: port->print("THROW"); break;