Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 06:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Remove throw mode

Browse Source
This commit is contained in:
Jacob Walser 2016-11-25 19:25:35 -05:00 committed by Andrew Tridgell
parent 4571dbc57c
commit f10f1a0bae
8 changed files with 4 additions and 285 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduSub/AP_State.cpp
View File

@ -110,9 +110,8 @@ void Sub::set_pre_arm_rc_check(bool b)
void Sub::update_using_interlock()
{
// check if we are using motor interlock control on an aux switch or are in throw mode
// which uses the interlock to stop motors while the copter is being thrown
ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) || (control_mode == THROW);
// check if we are using motor interlock control on an aux switch
ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK);
}
void Sub::set_motor_emergency_stop(bool b)

7
ArduSub/Parameters.cpp
View File

@ -1025,13 +1025,6 @@ const AP_Param::Info Sub::var_info[] = {
// @Path: ../libraries/AP_Notify/AP_Notify.cpp
GOBJECT(notify, "NTF_", AP_Notify),
// @Param: THROW_MOT_START
// @DisplayName: Start motors before throwing is detected
// @Description: Used by THROW mode. Controls whether motors will run at the speed set by THR_MIN or will be stopped when armed and waiting for the throw.
// @Values: 0:Stopped,1:Running
// @User: Standard
GSCALAR(throw_motor_start, "THROW_MOT_START", 0),
// @Param: TERRAIN_FOLLOW
// @DisplayName: Terrain Following use control
// @Description: This enables terrain following for RTL and LAND flight modes. To use this option TERRAIN_ENABLE must be 1 and the GCS must support sending terrain data to the aircraft. In RTL the RTL_ALT will be considered a height above the terrain. In LAND mode the vehicle will slow to LAND_SPEED 10m above terrain (instead of 10m above home). This parameter does not affect AUTO and Guided which use a per-command flag to determine if the height is above-home, absolute or above-terrain.

2
ArduSub/Parameters.h
View File

@ -120,7 +120,6 @@ public:
k_param_motors = 90,
k_param_disarm_delay,
k_param_fs_crash_check,
k_param_throw_motor_start,
k_param_terrain_follow, // 94
k_param_avoid,
@ -380,7 +379,6 @@ public:
AP_Float fs_ekf_thresh;
AP_Int16 gcs_pid_mask;
AP_Int8 throw_motor_start;
AP_Int8 terrain_follow;
// RC channels

14
ArduSub/Sub.h
View File

@ -371,12 +371,6 @@ private:
int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.)
uint32_t nav_delay_time_start;
// Throw
bool throw_early_exit_interlock = true; // value of the throttle interlock that must be restored when exiting throw mode early
bool throw_flight_commenced = false; // true when the throw has been detected and the motors and control loops are running
uint32_t throw_free_fall_start_ms = 0; // system time free fall was detected
float throw_free_fall_start_velz = 0.0f;// vertical velocity when free fall was detected
// Battery Sensors
AP_BattMonitor battery;
@ -710,14 +704,6 @@ private:
bool poshold_init(bool ignore_checks);
void poshold_run();
// Throw to launch functionality
bool throw_init(bool ignore_checks);
void throw_exit();
void throw_run();
bool throw_detected();
bool throw_attitude_good();
bool throw_height_good();
bool rtl_init(bool ignore_checks);
void rtl_restart_without_terrain();
void rtl_run();

230
ArduSub/control_throw.cpp
View File

@ -1,230 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Sub.h"
// throw_init - initialise throw controller
bool Sub::throw_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to use throw to start
return false;
#endif
// do not enter the mode when already armed
if (motors.armed()) {
return false;
}
// this mode needs a position reference
return true;
}
// clean up when exiting throw mode
void Sub::throw_exit()
{
// If exiting throw mode before commencing flight, restore the throttle interlock to the value last set by the switch
if (!throw_flight_commenced) {
motors.set_interlock(throw_early_exit_interlock);
}
}
// runs the throw to start controller
// should be called at 100hz or more
void Sub::throw_run()
{
static ThrowModeState throw_state = Throw_Disarmed;
/* Throw State Machine
Throw_Disarmed - motors are off
Throw_Detecting - motors are on and we are waiting for the throw
Throw_Uprighting - the throw has been detected and the copter is being uprighted
Throw_HgtStabilise - the copter is kept level and height is stabilised about the target height
Throw_PosHold - the copter is kept at a constant position and height
*/
// Don't enter THROW mode if interlock will prevent motors running
if (!motors.armed() && motors.get_interlock()) {
// state machine entry is always from a disarmed state
throw_state = Throw_Disarmed;
// remember the current value of the motor interlock so that this condition can be restored if we exit the throw mode before starting motors
throw_early_exit_interlock = true;
// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == 1) {
motors.set_interlock(true);
} else {
motors.set_interlock(false);
}
// status to let system know flight control has not started which means the interlock setting needs to restored if we exit to another flight mode
// this is necessary because throw mode uses the interlock to achieve a post arm motor start.
throw_flight_commenced = false;
} else if (throw_state == Throw_Disarmed && motors.armed()) {
gcs_send_text(MAV_SEVERITY_INFO,"waiting for throw");
throw_state = Throw_Detecting;
// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == 1) {
motors.set_interlock(true);
} else {
motors.set_interlock(false);
}
} else if (throw_state == Throw_Detecting && throw_detected()){
gcs_send_text(MAV_SEVERITY_INFO,"throw detected - uprighting");
throw_state = Throw_Uprighting;
// Cancel the waiting for throw tone sequence
AP_Notify::flags.waiting_for_throw = false;
// reset the interlock
motors.set_interlock(true);
// status to let system know flight control has started which means the entry interlock setting will not restored if we exit to another flight mode
throw_flight_commenced = true;
} else if (throw_state == Throw_Uprighting && throw_attitude_good()) {
gcs_send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
throw_state = Throw_HgtStabilise;
// initialize vertical speed and acceleration limits
// use brake mode values for rapid response
pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);
// initialise the demanded height to 3m above the throw height
// we want to rapidly clear surrounding obstacles
pos_control.set_alt_target(inertial_nav.get_altitude() + 300);
// set the initial velocity of the height controller demand to the measured velocity if it is going up
// if it is going down, set it to zero to enforce a very hard stop
pos_control.set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f));
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
set_auto_armed(true);
} else if (throw_state == Throw_HgtStabilise && throw_height_good()) {
gcs_send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
throw_state = Throw_PosHold;
// initialise the loiter target to the curent position and velocity
wp_nav.init_loiter_target();
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
set_auto_armed(true);
}
// Throw State Processing
switch (throw_state) {
case Throw_Disarmed:
// demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
break;
case Throw_Detecting:
// Hold throttle at zero during the throw and continually reset the attitude controller
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// Play the waiting for throw tone sequence to alert the user
AP_Notify::flags.waiting_for_throw = true;
break;
case Throw_Uprighting:
// demand a level roll/pitch attitude with zero yaw rate
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
// output 50% throttle and turn off angle boost to maximise righting moment
attitude_control.set_throttle_out(500, false, g.throttle_filt);
break;
case Throw_HgtStabilise:
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
// call height controller
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
pos_control.update_z_controller();
break;
case Throw_PosHold:
// run loiter controller
wp_nav.update_loiter(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());
// call height controller
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
pos_control.update_z_controller();
break;
}
}
bool Sub::throw_detected()
{
// Check that we have a valid navigation solution
nav_filter_status filt_status = inertial_nav.get_filter_status();
if (!filt_status.flags.attitude || !filt_status.flags.horiz_pos_abs || !filt_status.flags.vert_pos) {
return false;
}
// Check for high speed (note get_inertial_nav methods use a cm length scale)
bool high_speed = inertial_nav.get_velocity().length() > 500.0f;
// check for upwards trajectory
bool gaining_height = inertial_nav.get_velocity().z > 50.0f;
// Check the vertical acceleraton is greater than 0.25g
bool free_falling = ahrs.get_accel_ef().z > -0.25 * GRAVITY_MSS;
// Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
bool no_throw_action = ins.get_accel().length() < 1.0f * GRAVITY_MSS;
// High velocity or free-fall combined with incresing height indicate a possible throw release
bool possible_throw_detected = (free_falling || high_speed) && gaining_height && no_throw_action;
// Record time and vertical velocity when we detect the possible throw
if (possible_throw_detected && ((AP_HAL::millis() - throw_free_fall_start_ms) > 500)) {
throw_free_fall_start_ms = AP_HAL::millis();
throw_free_fall_start_velz = inertial_nav.get_velocity().z;
}
// Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm
bool throw_condition_confirmed = ((AP_HAL::millis() - throw_free_fall_start_ms < 500) && ((inertial_nav.get_velocity().z - throw_free_fall_start_velz) < -250.0f));
// start motors and enter the control mode if we are in continuous freefall
if (throw_condition_confirmed) {
return true;
} else {
return false;
}
}
bool Sub::throw_attitude_good()
{
// Check that we have uprighted the Sub
const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
bool is_upright = (rotMat.c.z > 0.866f);
return is_upright;
}
bool Sub::throw_height_good()
{
// Check that we are no more than 0.5m below the demanded height
return (pos_control.get_alt_error() < 50.0f);
}

11
ArduSub/defines.h
View File

@ -81,7 +81,6 @@ enum aux_sw_func {
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)
AUXSW_THROW = 37 // change to THROW flight mode
};
// Frame types
@ -120,7 +119,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
THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input
MANUAL = 19 // Pass-through input with no stabilization
};
@ -254,15 +252,6 @@ enum LoiterModeState {
Loiter_Landed
};
// Throw states
enum ThrowModeState {
Throw_Disarmed,
Throw_Detecting,
Throw_Uprighting,
Throw_HgtStabilise,
Throw_PosHold
};
// LAND state
#define LAND_STATE_FLY_TO_LOCATION 0
#define LAND_STATE_DESCENDING 1

18
ArduSub/flight_mode.cpp
View File

@ -81,10 +81,6 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
break;
#endif
case THROW:
success = throw_init(ignore_checks);
break;
case MANUAL:
success = manual_init(ignore_checks);
break;
@ -188,10 +184,6 @@ void Sub::update_flight_mode()
break;
#endif
case THROW:
throw_run();
break;
case MANUAL:
manual_run();
break;
@ -220,10 +212,6 @@ void Sub::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_
#endif // MOUNT == ENABLED
}
if (old_control_mode == THROW) {
throw_exit();
}
// 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()) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
@ -241,7 +229,6 @@ bool Sub::mode_requires_GPS(control_mode_t mode) {
case RTL:
case CIRCLE:
case POSHOLD:
case THROW:
case TRANSECT:
return true;
default:
@ -268,7 +255,7 @@ bool Sub::mode_has_manual_throttle(control_mode_t mode) {
// mode_allows_arming - returns true if vehicle can be armed in the specified mode
// arming_from_gcs should be set to true if the arming request comes from the ground station
bool Sub::mode_allows_arming(control_mode_t mode, bool arming_from_gcs) {
if (mode_has_manual_throttle(mode) || mode == VELHOLD || mode == ALT_HOLD || mode == POSHOLD || mode == TRANSECT || mode == THROW || (arming_from_gcs && mode == GUIDED)) {
if (mode_has_manual_throttle(mode) || mode == VELHOLD || mode == ALT_HOLD || mode == POSHOLD || mode == TRANSECT || (arming_from_gcs && mode == GUIDED)) {
return true;
}
return false;
@ -337,9 +324,6 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case POSHOLD:
port->print("POSHOLD");
break;
case THROW:
port->print("THROW");
break;
case MANUAL:
port->print("MANUAL");
break;

2
ArduSub/motors.cpp
View File

@ -94,7 +94,7 @@ void Sub::auto_disarm_check()
// exit immediately if we are already disarmed, or if auto
// disarming is disabled
if (!motors.armed() || disarm_delay_ms == 0 || control_mode == THROW) {
if (!motors.armed() || disarm_delay_ms == 0) {
auto_disarm_begin = tnow_ms;
return;
}