mirror of https://github.com/ArduPilot/ardupilot
Copter: add THROW_TYPE and allow dropping vehicle to trigger motors
This commit is contained in:
parent
f0f87a2f0c
commit
25940c8e0f
|
@ -981,6 +981,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
// @User: Standard
|
||||
AP_GROUPINFO("THROW_NEXTMODE", 3, ParametersG2, throw_nextmode, 18),
|
||||
|
||||
// @Param: THROW_TYPE
|
||||
// @DisplayName: Type of Type
|
||||
// @Description: Used by THROW mode. Specifies whether Copter is thrown upward or dropped.
|
||||
// @Values: 0:Upward Throw,1:Drop
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, ThrowType_Upward),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
|
@ -551,8 +551,9 @@ public:
|
|||
// button checking
|
||||
AP_Button button;
|
||||
|
||||
// Throw mode state
|
||||
// Throw mode parameters
|
||||
AP_Int8 throw_nextmode;
|
||||
AP_Int8 throw_type;
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
|
|
@ -97,7 +97,11 @@ void Copter::throw_run()
|
|||
|
||||
// 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);
|
||||
if (g2.throw_type == ThrowType_Drop) {
|
||||
pos_control.set_alt_target(inertial_nav.get_altitude() - 100);
|
||||
} else {
|
||||
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
|
||||
|
@ -200,8 +204,13 @@ bool Copter::throw_detected()
|
|||
// 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 for upwards or downwards trajectory (airdrop)
|
||||
bool changing_height;
|
||||
if (g2.throw_type == ThrowType_Drop) {
|
||||
changing_height = inertial_nav.get_velocity().z < -50.0f;
|
||||
} else {
|
||||
changing_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;
|
||||
|
@ -209,8 +218,8 @@ bool Copter::throw_detected()
|
|||
// 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;
|
||||
// High velocity or free-fall combined with incresing height indicate a possible air-drop or throw release
|
||||
bool possible_throw_detected = (free_falling || high_speed) && changing_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)) {
|
||||
|
|
|
@ -259,6 +259,12 @@ enum ThrowModeStage {
|
|||
Throw_PosHold
|
||||
};
|
||||
|
||||
// Throw types
|
||||
enum ThrowModeType {
|
||||
ThrowType_Upward = 0,
|
||||
ThrowType_Drop = 1
|
||||
};
|
||||
|
||||
// LAND state
|
||||
#define LAND_STATE_FLY_TO_LOCATION 0
|
||||
#define LAND_STATE_DESCENDING 1
|
||||
|
|
Loading…
Reference in New Issue