Copter: add THROW_TYPE and allow dropping vehicle to trigger motors

This commit is contained in:
chambana 2016-08-02 11:26:48 +09:00 committed by Randy Mackay
parent f0f87a2f0c
commit 25940c8e0f
4 changed files with 29 additions and 6 deletions

View File

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

View File

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

View File

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

View File

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