Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Fixed wing launch refactor. Add a smooth transition to the current flight mode at the end of the launch.

Browse source 
Use nav_fw_launch_end_time to change the transition time, default = 2000ms
This commit is contained in:
DESKTOP-N53JVUO\mix 2020-07-16 09:18:49 +03:00
parent 098a574b31
commit bab965da77
4 changed files with 209 additions and 126 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1645,6 +1645,10 @@ groups:
field: fw.launch_motor_spinup_time
min: 0
max: 1000
- name: nav_fw_launch_end_time
field: fw.launch_end_time
min: 0
max: 5000
- name: nav_fw_launch_min_time
field: fw.launch_min_time
min: 0

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

@ -160,6 +160,7 @@ 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 = 2000, // 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

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

@ -211,6 +211,7 @@ 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

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

@ -33,15 +33,6 @@
#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"
@ -57,23 +48,112 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms
enum {
FW_LAUNCH_STATE_WAIT_THROTTLE = 0,
FW_LAUNCH_STATE_MOTOR_IDLE = 1,
FW_LAUNCH_STATE_WAIT_DETECTION = 2,
FW_LAUNCH_STATE_DETECTED = 3,
FW_LAUNCH_STATE_MOTOR_DELAY = 4,
FW_LAUNCH_STATE_MOTOR_SPIN_UP = 5,
FW_LAUNCH_STATE_IN_PROGRESS = 6,
FW_LAUNCH_STATE_FINISHING = 7,
FW_LAUNCH_STATE_FINISHED = 8
};
typedef struct FixedWingLaunchState_s {
/* Launch detection */
timeUs_t launchDetectorPreviousUpdate;
timeUs_t launchDetectionTimeAccum;
bool launchDetected;
/* Launch progress */
timeUs_t launchStartedTime;
bool launchFinished;
bool motorControlAllowed;
int state;
timeUs_t stateStartedTimeUs;
timeUs_t launchStartedTimeUs;
} 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
static void setLaunchState(int state, timeUs_t currentTimeUs)
{
launchState.state = state;
launchState.stateStartedTimeUs = currentTimeUs;
}
static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs)
{
return US2MS(currentTimeUs - launchState.stateStartedTimeUs);
}
static timeMs_t getElapsedMsAndSetNextState(uint16_t input, timeUs_t currentTimeUs, int nextState)
{
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
if (input == 0 || input < elapsedTimeMs) {
setLaunchState(nextState, currentTimeUs);
}
return elapsedTimeMs;
}
static inline bool isFixedWingLaunchMaxAltitudeReached(void)
{
return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
}
static inline bool isLaunchModeMinTimeElapsed(float currentTimeUs)
{
return US2MS(currentTimeUs - launchState.launchStartedTimeUs) > navConfig()->fw.launch_min_time;
}
static void updateFixedWingLaunchPitchAngle(uint8_t pitchAngle)
{
rcCommand[ROLL] = 0;
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = 0;
}
static void lockLaunchPitchAngle(void)
{
updateFixedWingLaunchPitchAngle(navConfig()->fw.launch_climb_angle);
}
static void lockLaunchThrottleIdle(void)
{
rcCommand[THROTTLE] = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
}
static void fixedWingLaunchResetPids(void)
{
// Until motors are started don't use PID I-term and reset TPA filter
if (launchState.state < FW_LAUNCH_STATE_MOTOR_SPIN_UP) {
pidResetErrorAccumulators();
pidResetTPAFilter();
}
}
static void updateFixedWingLaunchAbortTriggers(timeUs_t currentTimeUs)
{
if (launchState.state < FW_LAUNCH_STATE_MOTOR_DELAY) {
return;
}
if (areSticksDeflectedMoreThanPosHoldDeadband() && isLaunchModeMinTimeElapsed(currentTimeUs)) {
setLaunchState(FW_LAUNCH_STATE_FINISHED, currentTimeUs);
}
else if (launchState.state == FW_LAUNCH_STATE_IN_PROGRESS && isFixedWingLaunchMaxAltitudeReached()) {
setLaunchState(FW_LAUNCH_STATE_FINISHING, currentTimeUs);
}
}
static void updateFixedWingLaunchThrottleWait(timeUs_t currentTimeUs)
{
if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) {
setLaunchState(FW_LAUNCH_STATE_MOTOR_IDLE, currentTimeUs);
}
rcCommand[THROTTLE] = getThrottleIdleValue();
}
static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
{
const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
@ -87,7 +167,7 @@ static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate);
launchState.launchDetectorPreviousUpdate = currentTimeUs;
if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) {
launchState.launchDetected = true;
setLaunchState(FW_LAUNCH_STATE_DETECTED, currentTimeUs);
}
}
else {
@ -96,143 +176,140 @@ static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
}
}
static void updateFixedWingLaunchThrottleIdle(timeMs_t currentTimeUs)
{
const int throttleIdleVale = getThrottleIdleValue();
updateFixedWingLaunchDetector(currentTimeUs); // if the launch is detected, the throttle idle will be skipped and jump to FW_LAUNCH_STATE_DETECTED
// Throttle control logic
if (navConfig()->fw.launch_idle_throttle <= throttleIdleVale) {
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
rcCommand[THROTTLE] = throttleIdleVale; // If MOTOR_STOP is disabled, motors will spin at minthrottle
setLaunchState(FW_LAUNCH_STATE_WAIT_DETECTION, currentTimeUs);
lockLaunchPitchAngle();
}
else {
timeMs_t elapsedTimeMs = elapsedTimeMs = getElapsedMsAndSetNextState(LAUNCH_MOTOR_IDLE_SPINUP_TIME, currentTimeUs, FW_LAUNCH_STATE_WAIT_DETECTION);
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, throttleIdleVale, navConfig()->fw.launch_idle_throttle);
updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle));
}
}
static void updateFixedWingLaunchMotorDelay(timeUs_t currentTimeUs) {
getElapsedMsAndSetNextState(navConfig()->fw.launch_motor_timer, currentTimeUs, FW_LAUNCH_STATE_MOTOR_SPIN_UP);
lockLaunchThrottleIdle();
lockLaunchPitchAngle();
}
static void updateFixedWingLaunchSpinUp(timeUs_t currentTimeUs)
{
const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time;
const timeMs_t elapsedTimeMs = getElapsedMsAndSetNextState(motorSpinUpMs, currentTimeUs, FW_LAUNCH_STATE_IN_PROGRESS);
const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, navConfig()->fw.launch_throttle);
lockLaunchPitchAngle();
}
static void updateFixedWingLaunchProgress(timeUs_t currentTimeUs)
{
getElapsedMsAndSetNextState(navConfig()->fw.launch_timeout, currentTimeUs, FW_LAUNCH_STATE_FINISHING);
rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
lockLaunchPitchAngle();
}
static void updateFixedWingLaunchFinishing(timeUs_t currentTimeUs)
{
const uint16_t endTimeMs = navConfig()->fw.launch_end_time;
const timeMs_t elapsedTimeMs = getElapsedMsAndSetNextState(endTimeMs, currentTimeUs, FW_LAUNCH_STATE_FINISHED);
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]);
updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, 0));
}
// Public methods ---------------------------------------------------------------
void applyFixedWingLaunchController(timeUs_t currentTimeUs)
{
// Called at PID rate
fixedWingLaunchResetPids();
updateFixedWingLaunchAbortTriggers(currentTimeUs);
switch (launchState.state) {
case FW_LAUNCH_STATE_WAIT_THROTTLE:
updateFixedWingLaunchThrottleWait(currentTimeUs); // raise throttle stick to jump to FW_LAUNCH_STATE_MOTOR_IDLE and activate detection
break;
case FW_LAUNCH_STATE_MOTOR_IDLE:
updateFixedWingLaunchThrottleIdle(currentTimeUs);
break;
case FW_LAUNCH_STATE_WAIT_DETECTION:
updateFixedWingLaunchDetector(currentTimeUs); // when the launch is detected, we jump to FW_LAUNCH_STATE_DETECTED
lockLaunchThrottleIdle();
lockLaunchPitchAngle();
break;
case FW_LAUNCH_STATE_DETECTED:
lockLaunchThrottleIdle();
lockLaunchPitchAngle();
// nothing else to do, wait for the navigation to switch to NAV_LAUNCH_STATE_MOTOR_DELAY state by calling enableFixedWingLaunchController()
break;
case FW_LAUNCH_STATE_MOTOR_DELAY:
updateFixedWingLaunchMotorDelay(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_MOTOR_SPIN_UP
break;
case FW_LAUNCH_STATE_MOTOR_SPIN_UP:
updateFixedWingLaunchSpinUp(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_IN_PROGRESS
break;
case FW_LAUNCH_STATE_IN_PROGRESS:
updateFixedWingLaunchProgress(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_FINISHING
break;
case FW_LAUNCH_STATE_FINISHING:
updateFixedWingLaunchFinishing(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_FINISHED
break;
case FW_LAUNCH_STATE_FINISHED:
default:
return;
}
// Control beeper
beeper(BEEPER_LAUNCH_MODE_ENABLED);
}
void resetFixedWingLaunchController(timeUs_t currentTimeUs)
{
launchState.launchDetectorPreviousUpdate = currentTimeUs;
launchState.launchDetectionTimeAccum = 0;
launchState.launchStartedTime = 0;
launchState.launchDetected = false;
launchState.launchFinished = false;
launchState.motorControlAllowed = false;
launchState.launchStartedTimeUs = 0;
setLaunchState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs);
}
bool isFixedWingLaunchDetected(void)
{
return launchState.launchDetected;
return launchState.state == FW_LAUNCH_STATE_DETECTED;
}
void enableFixedWingLaunchController(timeUs_t currentTimeUs)
{
launchState.launchStartedTime = currentTimeUs;
launchState.motorControlAllowed = true;
setLaunchState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs);
launchState.launchStartedTimeUs = currentTimeUs;
}
bool isFixedWingLaunchFinishedOrAborted(void)
{
return launchState.launchFinished;
return launchState.state == FW_LAUNCH_STATE_FINISHED;
}
void abortFixedWingLaunch(void)
{
launchState.launchFinished = true;
}
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms
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);
}
}
}
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 inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs)
{
return timeSinceLaunchMs >= navConfig()->fw.launch_timeout;
}
static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs)
{
return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached();
}
void applyFixedWingLaunchController(timeUs_t currentTimeUs)
{
// Called at PID rate
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;
}
}
}
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 (!launchState.launchFinished) {
beeper(BEEPER_LAUNCH_MODE_ENABLED);
}
// 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;
launchState.state = FW_LAUNCH_STATE_FINISHED;
}
#endif