mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 17:25:18 +03:00
first build
This commit is contained in:
parent
eb8cb98db3
commit
23dc73e78b
4 changed files with 41 additions and 6 deletions
|
@ -2824,6 +2824,11 @@ groups:
|
|||
field: fw.launch_climb_angle
|
||||
min: 5
|
||||
max: 45
|
||||
- name: nav_fw_launch_manual_throttle
|
||||
description: "Allow launch with manually controlled throttle. INAV only levels wings and controls climb angle during launch. Throttle is controlled directly by throttle stick movement. Launch timers start when throttle stick is raised above idle."
|
||||
default_value: OFF
|
||||
field: fw.launch_manual_throttle
|
||||
type: bool
|
||||
- name: nav_fw_cruise_yaw_rate
|
||||
description: "Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]"
|
||||
default_value: 20
|
||||
|
|
|
@ -201,6 +201,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended
|
||||
.launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees
|
||||
.launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg
|
||||
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
|
||||
.cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
.allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
|
||||
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
|
||||
|
@ -1750,6 +1751,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
|
|||
const timeUs_t currentTimeUs = micros();
|
||||
UNUSED(previousState);
|
||||
|
||||
// Continue immediately to launch in progress if manual launch throttle used
|
||||
if (navConfig()->fw.launch_manual_throttle) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
if (fixedWingLaunchStatus() == FW_LAUNCH_DETECTED) {
|
||||
enableFixedWingLaunchController(currentTimeUs);
|
||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS
|
||||
|
|
|
@ -294,6 +294,7 @@ typedef struct navConfig_s {
|
|||
uint16_t launch_max_altitude; // cm, altitude where to consider launch ended
|
||||
uint8_t launch_climb_angle; // Target climb angle for launch (deg)
|
||||
uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
|
||||
bool launch_manual_throttle; // Allows launch with manual throttle control
|
||||
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
|
||||
bool allow_manual_thr_increase;
|
||||
bool useFwNavYawControl;
|
||||
|
|
|
@ -416,13 +416,27 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
|
|||
|
||||
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs)
|
||||
{
|
||||
rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
uint16_t initialTime = navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time;
|
||||
|
||||
if (navConfig()->fw.launch_manual_throttle) {
|
||||
// reset timers when throttle is low and abort launch regardless of launch settings
|
||||
if (isThrottleLow()) {
|
||||
fwLaunch.currentStateTimeUs = currentTimeUs;
|
||||
initialTime = 0;
|
||||
if (isRollPitchStickDeflected()) {
|
||||
return FW_LAUNCH_EVENT_ABORT;
|
||||
}
|
||||
}
|
||||
fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle;
|
||||
} else {
|
||||
rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
}
|
||||
|
||||
if (isLaunchMaxAltitudeReached()) {
|
||||
return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
|
||||
}
|
||||
|
||||
if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) {
|
||||
if (areSticksMoved(initialTime, currentTimeUs)) {
|
||||
return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_ABORTED state
|
||||
}
|
||||
|
||||
|
@ -442,9 +456,12 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
|
|||
return FW_LAUNCH_EVENT_SUCCESS; // End launch go to FW_LAUNCH_STATE_FLYING state
|
||||
}
|
||||
else {
|
||||
// make a smooth transition from the launch state to the current state for throttle and the pitch angle
|
||||
const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, launchThrottle, rcCommand[THROTTLE]);
|
||||
// Make a smooth transition from the launch state to the current state for pitch angle
|
||||
// Do the same for throttle when manual launch throttle isn't used
|
||||
if (!navConfig()->fw.launch_manual_throttle) {
|
||||
const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, launchThrottle, rcCommand[THROTTLE]);
|
||||
}
|
||||
fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
|
||||
}
|
||||
|
||||
|
@ -498,7 +515,13 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs)
|
|||
|
||||
void resetFixedWingLaunchController(timeUs_t currentTimeUs)
|
||||
{
|
||||
setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs);
|
||||
if (navConfig()->fw.launch_manual_throttle) {
|
||||
// no detection or motor control required with manual launch throttle
|
||||
// so start at launch in progress
|
||||
setCurrentState(FW_LAUNCH_STATE_IN_PROGRESS, currentTimeUs);
|
||||
} else {
|
||||
setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs);
|
||||
}
|
||||
}
|
||||
|
||||
void enableFixedWingLaunchController(timeUs_t currentTimeUs)
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue