Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

first build

Browse source
This commit is contained in:
breadoven 2022-06-18 12:18:06 +01:00
parent eb8cb98db3
commit 23dc73e78b
4 changed files with 41 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

35
src/main/navigation/navigation_fw_launch.c
View file

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