Copter: add throw_nextmode

vehicle switches to mode specified in THROW_NEXTMODE parameter after the
throw is completed.
This commit is contained in:
Randy Mackay 2016-08-01 21:42:09 +09:00
parent 64ac18da6c
commit f0f87a2f0c
5 changed files with 39 additions and 3 deletions

View File

@ -277,7 +277,8 @@ private:
// throw mode state
struct {
ThrowModeStage stage;
} throw_state = {Throw_Disarmed};
bool nextmode_attempted;
} throw_state = {Throw_Disarmed, false};
uint32_t precland_last_update_ms;
@ -852,6 +853,7 @@ private:
bool throw_detected();
bool throw_attitude_good();
bool throw_height_good();
bool throw_position_good();
bool rtl_init(bool ignore_checks);
void rtl_restart_without_terrain();

View File

@ -973,7 +973,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Group: BTN_
// @Path: ../libraries/AP_Button/AP_Button.cpp
AP_SUBGROUPINFO(button, "BTN_", 2, ParametersG2, AP_Button),
// @Param: THROW_NEXTMODE
// @DisplayName: Throw mode's follow up mode
// @Description: Vehicle will switch to this mode after the throw is successfully completed. Default is to stay in throw mode (18)
// @Values: 3:Auto,4:Guided,6:RTL,9:Land,17:Brake,18:Throw
// @User: Standard
AP_GROUPINFO("THROW_NEXTMODE", 3, ParametersG2, throw_nextmode, 18),
AP_GROUPEND
};

View File

@ -550,6 +550,9 @@ public:
// button checking
AP_Button button;
// Throw mode state
AP_Int8 throw_nextmode;
};
extern const AP_Param::Info var_info[];

View File

@ -16,7 +16,9 @@ bool Copter::throw_init(bool ignore_checks)
return false;
}
// this mode needs a position reference
// init state
throw_state.nextmode_attempted = false;
return true;
}
@ -113,6 +115,22 @@ void Copter::throw_run()
// 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.stage == Throw_PosHold && throw_position_good()) {
if (!throw_state.nextmode_attempted) {
switch (g2.throw_nextmode) {
case AUTO:
case GUIDED:
case RTL:
case LAND:
case BRAKE:
set_mode((control_mode_t)g2.throw_nextmode.get(), MODE_REASON_THROW_COMPLETE);
break;
default:
// do nothing
break;
}
throw_state.nextmode_attempted = true;
}
}
// Throw State Processing
@ -225,3 +243,8 @@ bool Copter::throw_height_good()
return (pos_control.get_alt_error() < 50.0f);
}
bool Copter::throw_position_good()
{
// check that our horizontal position error is within 50cm
return (pos_control.get_horizontal_error() < 50.0f);
}

View File

@ -126,6 +126,7 @@ enum mode_reason_t {
MODE_REASON_FLIP_COMPLETE,
MODE_REASON_AVOIDANCE,
MODE_REASON_AVOIDANCE_RECOVERY,
MODE_REASON_THROW_COMPLETE,
};
// Tuning enumeration