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: Add throw mode to match Copter.

Browse Source 
This is probably never going to be used for Sub unless there is some crazy situation I can't think of. However, we are continuing to maintain Copter changes for now to minimize divergence of the code and make maintenance easy.
This commit is contained in:
Rustom Jehangir 2016-03-02 22:36:13 -08:00 committed by Andrew Tridgell
parent bde64bc919
commit d159320ac0
10 changed files with 307 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduSub/AP_State.cpp
View File

@ -116,8 +116,9 @@ 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
ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_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);
}
void Sub::set_motor_emergency_stop(bool b)

23
ArduSub/Parameters.cpp
View File

@ -311,42 +311,42 @@ const AP_Param::Info Sub::var_info[] = {
// @Param: FLTMODE1
// @DisplayName: Flight Mode 1
// @Description: Flight mode when Channel 5 pwm is <= 1230
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @User: Standard
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
// @Param: FLTMODE2
// @DisplayName: Flight Mode 2
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
// @Param: FLTMODE3
// @DisplayName: Flight Mode 3
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
// @Param: FLTMODE4
// @DisplayName: Flight Mode 4
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
// @Param: FLTMODE5
// @DisplayName: Flight Mode 5
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
// @Param: FLTMODE6
// @DisplayName: Flight Mode 6
// @Description: Flight mode when Channel 5 pwm is >=1750
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),
@ -1090,6 +1090,17 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Standard
GSCALAR(autotune_min_d, "AUTOTUNE_MIN_D", 0.001f),
// @Group: NTF_
// @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),
AP_VAREND
};

4
ArduSub/Parameters.h
View File

@ -147,6 +147,7 @@ public:
// AP_ADSB Library
k_param_adsb, // 72
k_param_notify, // 73
// 74: precision landing object
k_param_precland = 74,
@ -179,6 +180,7 @@ public:
k_param_motors = 90,
k_param_disarm_delay,
k_param_fs_crash_check,
k_param_throw_motor_start,
// 97: RSSI
k_param_rssi = 97,
@ -451,6 +453,8 @@ public:
AP_Float fs_ekf_thresh;
AP_Int16 gcs_pid_mask;
AP_Int8 throw_motor_start;
#if FRAME_CONFIG == SINGLE_FRAME
// Single
RC_Channel single_servo_1, single_servo_2, single_servo_3, single_servo_4; // servos for four flaps

14
ArduSub/Sub.h
View File

@ -375,6 +375,12 @@ private:
// Flip
Vector3f flip_orig_attitude; // original Sub attitude before flip
// 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;
@ -766,6 +772,14 @@ private:
void poshold_roll_controller_to_pilot_override();
void poshold_pitch_controller_to_pilot_override();
// 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_run();
void rtl_climb_start();

235
ArduSub/control_throw.cpp Normal file
View File

@ -0,0 +1,235 @@
/// -*- 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
if (position_ok()) {
return true;
} else {
return false;
}
}
// 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);
motors.slow_start(true);
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);
// 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);
// 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);
// 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);
}

12
ArduSub/defines.h
View File

@ -107,7 +107,8 @@ enum autopilot_modes {
FLIP = 14, // automatically flip the vehicle on the roll axis
AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
BRAKE = 17 // full-brake using inertial/GPS system, no pilot input
BRAKE = 17, // full-brake using inertial/GPS system, no pilot input
THROW = 18 // throw to launch mode using inertial/GPS system, no pilot input
};
// Tuning enumeration
@ -231,6 +232,15 @@ enum FlipState {
Flip_Abandon
};
// 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

@ -87,6 +87,10 @@ bool Sub::set_mode(uint8_t mode)
success = brake_init(ignore_checks);
break;
case THROW:
success = throw_init(ignore_checks);
break;
default:
success = false;
break;
@ -190,6 +194,10 @@ void Sub::update_flight_mode()
case BRAKE:
brake_run();
break;
case THROW:
throw_run();
break;
}
}
@ -212,6 +220,10 @@ void Sub::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
#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() && !ap.land_complete) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
@ -233,6 +245,7 @@ bool Sub::mode_requires_GPS(uint8_t mode) {
case DRIFT:
case POSHOLD:
case BRAKE:
case THROW:
return true;
default:
return false;
@ -257,7 +270,7 @@ bool Sub::mode_has_manual_throttle(uint8_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(uint8_t mode, bool arming_from_gcs) {
if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) {
if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && mode == GUIDED)) {
return true;
}
return false;
@ -335,6 +348,9 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case BRAKE:
port->print("BRAKE");
break;
case THROW:
port->print("THROW");
break;
default:
port->printf("Mode(%u)", (unsigned)mode);
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) {
if (!motors.armed() || disarm_delay_ms == 0 || control_mode == THROW) {
auto_disarm_begin = tnow_ms;
return;
}

3
ArduSub/switches.cpp
View File

@ -562,6 +562,9 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
// control signal in tradheli
motors.set_interlock(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
// remember the current value of the motor interlock so that this condition can be restored if we exit the throw mode early
throw_early_exit_interlock = motors.get_interlock();
// Log new status
if (motors.get_interlock()){
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);

4
ArduSub/system.cpp
View File

@ -396,8 +396,8 @@ void Sub::update_auto_armed()
}
}else{
// arm checks
// if motors are armed and throttle is above zero auto_armed should be true
if(motors.armed() && !ap.throttle_zero) {
// if motors are armed and we are in throw mode, then auto_ermed should be true
if(motors.armed() && (!ap.throttle_zero || control_mode == THROW)) {
set_auto_armed(true);
}
}