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

Sub: Remove Brake mode

Browse Source
This commit is contained in:
Jacob Walser 2016-11-30 22:36:02 -05:00 committed by Andrew Tridgell
parent 2959ecc70f
commit bc4084931f
5 changed files with 1 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/GCS_Mavlink.cpp
View File

@ -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;

7
ArduSub/Sub.h
View File

@ -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);

87
ArduSub/control_brake.cpp
View File

@ -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;
}

4
ArduSub/defines.h
View File

@ -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
};

12
ArduSub/flight_mode.cpp
View File

@ -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;