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

Revert "Autolaunch - Added a smooth end launch feature and code refactor"

Browse source
This commit is contained in:
Konstantin Sharlaimov 2020-11-16 14:04:13 +01:00 • committed by GitHub
parent 8fe6ec54fc
commit d028b19ec7
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 161 additions and 367 deletions
Download patch file Download diff file Expand all files Collapse all files

1
docs/Cli.md
View file

@ -147,4 +147,5 @@ For targets that have a flash data chip, typically used for blackbox logs, the f
| `flash_write <address> <data>` | Writes `data` to `address` |
## CLI Variable Reference
See [Settings.md](Settings.md).

1
src/main/cms/cms_menu_navigation.c
View file

@ -134,7 +134,6 @@ static const OSD_Entry cmsx_menuFWLaunchEntries[] =
OSD_SETTING_ENTRY("IDLE THROTTLE", SETTING_NAV_FW_LAUNCH_IDLE_THR),
OSD_SETTING_ENTRY("MOTOR SPINUP TIME", SETTING_NAV_FW_LAUNCH_SPINUP_TIME),
OSD_SETTING_ENTRY("TIMEOUT", SETTING_NAV_FW_LAUNCH_TIMEOUT),
OSD_SETTING_ENTRY("END TRANSITION TIME", SETTING_NAV_FW_LAUNCH_END_TIME),
OSD_SETTING_ENTRY("MAX ALTITUDE", SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE),
OSD_SETTING_ENTRY("CLIMB ANGLE", SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE),
OSD_SETTING_ENTRY("MAX BANK ANGLE", SETTING_NAV_FW_LAUNCH_MAX_ANGLE),

6
src/main/fc/settings.yaml
View file

@ -2371,12 +2371,6 @@ groups:
field: fw.launch_motor_spinup_time
min: 0
max: 1000
- name: nav_fw_launch_end_time
description: "Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms]"
default_value: "2000"
field: fw.launch_end_time
min: 0
max: 5000
- name: nav_fw_launch_min_time
description: "Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]."
default_value: "0"

4
src/main/io/osd.c
View file

@ -3327,10 +3327,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
}
} else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH);
const char *launchStateMessage = fixedWingLaunchStateMessage();
if (launchStateMessage) {
messages[messageCount++] = launchStateMessage;
}
} else {
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
// ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO

1
src/main/navigation/navigation.c
View file

@ -171,7 +171,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_idle_throttle = 1000, // Motor idle or MOTOR_STOP
.launch_motor_timer = 500, // ms
.launch_motor_spinup_time = 100, // ms, time to gredually increase throttle from idle to launch
.launch_end_time = 3000, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
.launch_min_time = 0, // ms, min time in launch mode
.launch_timeout = 5000, // ms, timeout for launch procedure
.launch_max_altitude = 0, // cm, altitude where to consider launch ended

2
src/main/navigation/navigation.h
View file

@ -239,7 +239,6 @@ typedef struct navConfig_s {
uint16_t launch_throttle; // Launch throttle
uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms)
uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention)
uint16_t launch_end_time; // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position
uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early
uint16_t launch_timeout; // Launch timeout to disable launch mode and swith to normal flight (ms)
uint16_t launch_max_altitude; // cm, altitude where to consider launch ended
@ -525,7 +524,6 @@ bool navigationRTHAllowsLanding(void);
bool isNavLaunchEnabled(void);
bool isFixedWingLaunchDetected(void);
bool isFixedWingLaunchFinishedOrAborted(void);
const char * fixedWingLaunchStateMessage(void);
float calculateAverageSpeed(void);

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

@ -33,6 +33,15 @@
#include "config/feature.h"
#include "drivers/time.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/rangefinder.h"
#include "sensors/barometer.h"
#include "sensors/acceleration.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "io/gps.h"
#include "io/beeper.h"
@ -48,238 +57,25 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
typedef struct FixedWingLaunchState_s {
/* Launch detection */
timeUs_t launchDetectorPreviousUpdate;
timeUs_t launchDetectionTimeAccum;
bool launchDetected;
/* Launch progress */
timeUs_t launchStartedTime;
bool launchFinished;
bool motorControlAllowed;
} FixedWingLaunchState_t;
static EXTENDED_FASTRAM FixedWingLaunchState_t launchState;
#define COS_MAX_LAUNCH_ANGLE 0.70710678f // cos(45), just to be safe
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms
#define UNUSED(x) ((void)(x))
#define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE"
#define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY"
#define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT"
#define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING"
typedef enum {
FW_LAUNCH_MESSAGE_TYPE_NONE = 0,
FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE,
FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION,
FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS,
FW_LAUNCH_MESSAGE_TYPE_FINISHING
} fixedWingLaunchMessage_t;
typedef enum {
FW_LAUNCH_EVENT_NONE = 0,
FW_LAUNCH_EVENT_SUCCESS,
FW_LAUNCH_EVENT_GOTO_DETECTION,
FW_LAUNCH_EVENT_ABORT,
FW_LAUNCH_EVENT_THROTTLE_LOW,
FW_LAUNCH_EVENT_COUNT
} fixedWingLaunchEvent_t;
typedef enum {
FW_LAUNCH_STATE_IDLE = 0,
FW_LAUNCH_STATE_WAIT_THROTTLE,
FW_LAUNCH_STATE_MOTOR_IDLE,
FW_LAUNCH_STATE_WAIT_DETECTION,
FW_LAUNCH_STATE_DETECTED,
FW_LAUNCH_STATE_MOTOR_DELAY,
FW_LAUNCH_STATE_MOTOR_SPINUP,
FW_LAUNCH_STATE_IN_PROGRESS,
FW_LAUNCH_STATE_FINISH,
FW_LAUNCH_STATE_COUNT
} fixedWingLaunchState_t;
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs);
typedef struct fixedWingLaunchStateDescriptor_s {
fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs);
fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT];
fixedWingLaunchMessage_t messageType;
} fixedWingLaunchStateDescriptor_t;
typedef struct fixedWingLaunchData_s {
timeUs_t currentStateTimeUs;
fixedWingLaunchState_t currentState;
uint8_t pitchAngle; // used to smooth the transition of the pitch angle
} fixedWingLaunchData_t;
static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch;
static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = {
[FW_LAUNCH_STATE_IDLE] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE,
.onEvent = {
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_NONE
},
[FW_LAUNCH_STATE_WAIT_THROTTLE] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE,
[FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
},
[FW_LAUNCH_STATE_MOTOR_IDLE] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_DETECTION,
[FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
},
[FW_LAUNCH_STATE_WAIT_DETECTION] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_DETECTED,
[FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
},
[FW_LAUNCH_STATE_DETECTED] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED,
.onEvent = {
// waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
},
[FW_LAUNCH_STATE_MOTOR_DELAY] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP,
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
},
[FW_LAUNCH_STATE_MOTOR_SPINUP] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS,
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
},
[FW_LAUNCH_STATE_IN_PROGRESS] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISH,
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
},
[FW_LAUNCH_STATE_FINISH] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING
}
};
/* Current State Handlers */
static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) {
return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs);
}
static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) {
fwLaunch.currentState = nextState;
fwLaunch.currentStateTimeUs = currentTimeUs;
}
/* Wing control Helpers */
static bool isThrottleIdleEnabled(void) {
return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue();
}
static void applyThrottleIdle(void) {
if (isThrottleIdleEnabled()) {
rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle;
} else {
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle
}
}
static inline bool isThrottleLow(void) {
return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
}
static inline bool isLaunchMaxAltitudeReached(void) {
return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
}
static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) {
return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband();
}
static void resetPidsIfNeeded(void) {
// Until motors are started don't use PID I-term and reset TPA filter
if (fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) {
pidResetErrorAccumulators();
pidResetTPAFilter();
}
}
static void updateRcCommand(void) {
// lock roll and yaw and apply needed pitch angle
rcCommand[ROLL] = 0;
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = 0;
}
/* onEntry state handlers */
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
if (!isThrottleLow()) {
if (isThrottleIdleEnabled()) {
return FW_LAUNCH_EVENT_SUCCESS;
} else {
fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle;
return FW_LAUNCH_EVENT_GOTO_DETECTION;
}
}
fwLaunch.pitchAngle = 0;
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) {
if (isThrottleLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) {
applyThrottleIdle();
return FW_LAUNCH_EVENT_SUCCESS;
} else {
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle);
}
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) {
if (isThrottleLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
{
const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh);
const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
@ -287,149 +83,160 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU
const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
applyThrottleIdle();
if (isBungeeLaunched || isSwingLaunched) {
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) {
return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED
launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate);
launchState.launchDetectorPreviousUpdate = currentTimeUs;
if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) {
launchState.launchDetected = true;
}
}
else {
launchState.launchDetectorPreviousUpdate = currentTimeUs;
launchState.launchDetectionTimeAccum = 0;
}
} else {
fwLaunch.currentStateTimeUs = currentTimeUs; // reset the state counter
}
return FW_LAUNCH_EVENT_NONE;
void resetFixedWingLaunchController(timeUs_t currentTimeUs)
{
launchState.launchDetectorPreviousUpdate = currentTimeUs;
launchState.launchDetectionTimeAccum = 0;
launchState.launchStartedTime = 0;
launchState.launchDetected = false;
launchState.launchFinished = false;
launchState.motorControlAllowed = false;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
// waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY
applyThrottleIdle();
return FW_LAUNCH_EVENT_NONE;
bool isFixedWingLaunchDetected(void)
{
return launchState.launchDetected;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) {
applyThrottleIdle();
if (areSticksMoved(0, currentTimeUs)) {
return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE
}
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) {
return FW_LAUNCH_EVENT_SUCCESS;
void enableFixedWingLaunchController(timeUs_t currentTimeUs)
{
launchState.launchStartedTime = currentTimeUs;
launchState.motorControlAllowed = true;
}
return FW_LAUNCH_EVENT_NONE;
bool isFixedWingLaunchFinishedOrAborted(void)
{
return launchState.launchFinished;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) {
if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) {
return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE
void abortFixedWingLaunch(void)
{
launchState.launchFinished = true;
}
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time;
const uint16_t launchThrottle = navConfig()->fw.launch_throttle;
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms
if (elapsedTimeMs > motorSpinUpMs) {
rcCommand[THROTTLE] = launchThrottle;
return FW_LAUNCH_EVENT_SUCCESS;
} else {
const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle);
static void applyFixedWingLaunchIdleLogic(void)
{
// Until motors are started don't use PID I-term
pidResetErrorAccumulators();
// We're not flying yet, reset TPA filter
pidResetTPAFilter();
// Throttle control logic
if (navConfig()->fw.launch_idle_throttle <= getThrottleIdleValue())
{
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle
}
else
{
static float timeThrottleRaisedMs;
if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW)
{
timeThrottleRaisedMs = millis();
}
else
{
const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME);
rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME,
getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
}
}
}
return FW_LAUNCH_EVENT_NONE;
static inline bool isFixedWingLaunchMaxAltitudeReached(void)
{
return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
}
static inline bool isLaunchModeMinTimeElapsed(float timeSinceLaunchMs)
{
return timeSinceLaunchMs > navConfig()->fw.launch_min_time;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) {
rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
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)) {
return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
}
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) {
return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH
static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs)
{
return timeSinceLaunchMs >= navConfig()->fw.launch_timeout;
}
return FW_LAUNCH_EVENT_NONE;
static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs)
{
return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached();
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs) {
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
if (elapsedTimeMs > endTimeMs) {
return FW_LAUNCH_EVENT_SUCCESS;
} else {
// make a smooth transition from the launch state to the current state for throttle and the pitch angle
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]);
fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
}
return FW_LAUNCH_EVENT_NONE;
}
// Public methods ---------------------------------------------------------------
void applyFixedWingLaunchController(timeUs_t currentTimeUs) {
void applyFixedWingLaunchController(timeUs_t currentTimeUs)
{
// Called at PID rate
// process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE
while (launchStateMachine[fwLaunch.currentState].onEntry) {
fixedWingLaunchEvent_t newEvent = launchStateMachine[fwLaunch.currentState].onEntry(currentTimeUs);
if (newEvent == FW_LAUNCH_EVENT_NONE) {
break;
if (launchState.launchDetected) {
bool applyLaunchIdleLogic = true;
if (launchState.motorControlAllowed) {
// If launch detected we are in launch procedure - control airplane
const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime);
launchState.launchFinished = isFixedWingLaunchCompleted(timeElapsedSinceLaunchMs);
// Motor control enabled
if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) {
// Don't apply idle logic anymore
applyLaunchIdleLogic = false;
// Increase throttle gradually over `launch_motor_spinup_time` milliseconds
if (navConfig()->fw.launch_motor_spinup_time > 0) {
const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time);
const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
0.0f, navConfig()->fw.launch_motor_spinup_time,
minIdleThrottle, navConfig()->fw.launch_throttle);
}
else {
rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
}
}
setCurrentState(launchStateMachine[fwLaunch.currentState].onEvent[newEvent], currentTimeUs);
}
resetPidsIfNeeded();
updateRcCommand();
if (applyLaunchIdleLogic) {
// Launch idle logic - low throttle and zero out PIDs
applyFixedWingLaunchIdleLogic();
}
}
else {
// We are waiting for launch - update launch detector
updateFixedWingLaunchDetector(currentTimeUs);
// Launch idle logic - low throttle and zero out PIDs
applyFixedWingLaunchIdleLogic();
}
// Control beeper
if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) {
beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE);
} else {
if (!launchState.launchFinished) {
if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) {
beeper(BEEPER_LAUNCH_MODE_ENABLED);
} else {
beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE);
}
}
void resetFixedWingLaunchController(timeUs_t currentTimeUs) {
setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs);
}
bool isFixedWingLaunchDetected(void) {
return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED;
}
void enableFixedWingLaunchController(timeUs_t currentTimeUs) {
setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs);
}
bool isFixedWingLaunchFinishedOrAborted(void) {
return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE;
}
void abortFixedWingLaunch(void) {
setCurrentState(FW_LAUNCH_STATE_IDLE, 0);
}
const char * fixedWingLaunchStateMessage(void) {
switch (launchStateMachine[fwLaunch.currentState].messageType) {
case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE:
return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE;
case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION:
return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION;
case FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS:
return FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS;
case FW_LAUNCH_MESSAGE_TYPE_FINISHING:
return FW_LAUNCH_MESSAGE_TEXT_FINISHING;
default:
return NULL;
}
// Lock out controls
rcCommand[ROLL] = 0;
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = 0;
}
#endif