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

Merge pull request #664 from iNavFlight/airplane-launch-detector

Browse source 
Airplane launch mode
This commit is contained in:
Konstantin Sharlaimov 2016-11-03 18:01:20 +10:00 • committed by GitHub
commit 589ad78bcb
16 changed files with 371 additions and 37 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -461,6 +461,7 @@ HIGHEND_SRC = \
flight/navigation_rewrite.c \
flight/navigation_rewrite_multicopter.c \
flight/navigation_rewrite_fixedwing.c \
flight/navigation_rewrite_fw_launch.c \
flight/navigation_rewrite_pos_estimator.c \
flight/navigation_rewrite_geo.c \
flight/gps_conversion.c \

8
src/main/config/config.c
View file

@ -226,6 +226,14 @@ void resetNavConfig(navConfig_t * navConfig)
navConfig->fw_pitch_to_throttle = 10;
navConfig->fw_roll_to_pitch = 75;
navConfig->fw_loiter_radius = 5000; // 50m
// Fixed wing launch
navConfig->fw_launch_accel_thresh = 1.9f * 981; // cm/s/s (1.9*G)
navConfig->fw_launch_time_thresh = 40; // 40ms
navConfig->fw_launch_throttle = 1700;
navConfig->fw_launch_motor_timer = 500; // ms
navConfig->fw_launch_timeout = 5000; // ms, timeout for launch procedure
navConfig->fw_launch_climb_angle = 10; // 10 deg
}
void validateNavConfig(navConfig_t * navConfig)

3
src/main/fc/fc_msp.c
View file

@ -139,6 +139,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXSURFACE, "SURFACE;", 33 },
{ BOXFLAPERON, "FLAPERON;", 34 },
{ BOXTURNASSIST, "TURN ASSIST;", 35 },
{ BOXNAVLAUNCH, "NAV LAUNCH;", 36 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -291,6 +292,7 @@ static void initActiveBoxIds(void)
if (isFixedWing) {
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
}
/*
@ -359,6 +361,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)) << BOXSURFACE |
IS_ENABLED(FLIGHT_MODE(FLAPERON)) << BOXFLAPERON |
IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)) << BOXTURNASSIST |
IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)) << BOXNAVLAUNCH |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET;
uint32_t ret = 0;

2
src/main/fc/rc_controls.h
View file

@ -34,9 +34,9 @@ typedef enum {
BOXBEEPERON,
BOXLEDLOW,
BOXLLIGHTS,
BOXNAVLAUNCH,
BOXOSD,
BOXTELEMETRY,
//BOXGTUNE,
BOXBLACKBOX,
BOXFAILSAFE,
BOXNAVWP,

10
src/main/fc/runtime_config.c
View file

@ -23,7 +23,7 @@
uint8_t armingFlags = 0;
uint8_t stateFlags = 0;
uint16_t flightModeFlags = 0;
uint32_t flightModeFlags = 0;
static uint32_t enabledSensors = 0;
@ -31,9 +31,9 @@ static uint32_t enabledSensors = 0;
* Enables the given flight mode. A beep is sounded if the flight mode
* has changed. Returns the new 'flightModeFlags' value.
*/
uint16_t enableFlightMode(flightModeFlags_e mask)
uint32_t enableFlightMode(flightModeFlags_e mask)
{
uint16_t oldVal = flightModeFlags;
uint32_t oldVal = flightModeFlags;
flightModeFlags |= (mask);
if (flightModeFlags != oldVal)
@ -45,9 +45,9 @@ uint16_t enableFlightMode(flightModeFlags_e mask)
* Disables the given flight mode. A beep is sounded if the flight mode
* has changed. Returns the new 'flightModeFlags' value.
*/
uint16_t disableFlightMode(flightModeFlags_e mask)
uint32_t disableFlightMode(flightModeFlags_e mask)
{
uint16_t oldVal = flightModeFlags;
uint32_t oldVal = flightModeFlags;
flightModeFlags &= ~(mask);
if (flightModeFlags != oldVal)

26
src/main/fc/runtime_config.h
View file

@ -39,31 +39,31 @@ typedef enum {
NAV_RTH_MODE = (1 << 4), // old GPS_HOME
NAV_POSHOLD_MODE= (1 << 5), // old GPS_HOLD
HEADFREE_MODE = (1 << 6),
UNUSED_MODE = (1 << 7), // old autotune
NAV_LAUNCH_MODE = (1 << 7),
PASSTHRU_MODE = (1 << 8),
FAILSAFE_MODE = (1 << 10),
UNUSED_MODE2 = (1 << 11), // old G-Tune
UNUSED_MODE = (1 << 11), // old G-Tune
NAV_WP_MODE = (1 << 12),
HEADING_LOCK = (1 << 13),
FLAPERON = (1 << 14),
TURN_ASSISTANT = (1 << 15),
} flightModeFlags_e;
extern uint16_t flightModeFlags;
extern uint32_t flightModeFlags;
#define DISABLE_FLIGHT_MODE(mask) disableFlightMode(mask)
#define ENABLE_FLIGHT_MODE(mask) enableFlightMode(mask)
#define FLIGHT_MODE(mask) (flightModeFlags & (mask))
typedef enum {
GPS_FIX_HOME = (1 << 0),
GPS_FIX = (1 << 1),
CALIBRATE_MAG = (1 << 2),
SMALL_ANGLE = (1 << 3),
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
ANTI_WINDUP = (1 << 5),
//PID_ATTENUATE = (1 << 6),
FLAPERON_AVAILABLE = (1 << 7)
GPS_FIX_HOME = (1 << 0),
GPS_FIX = (1 << 1),
CALIBRATE_MAG = (1 << 2),
SMALL_ANGLE = (1 << 3),
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
ANTI_WINDUP = (1 << 5),
FLAPERON_AVAILABLE = (1 << 6),
NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available
} stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))
@ -72,8 +72,8 @@ typedef enum {
extern uint8_t stateFlags;
uint16_t enableFlightMode(flightModeFlags_e mask);
uint16_t disableFlightMode(flightModeFlags_e mask);
uint32_t enableFlightMode(flightModeFlags_e mask);
uint32_t disableFlightMode(flightModeFlags_e mask);
bool sensors(uint32_t mask);
void sensorsSet(uint32_t mask);

4
src/main/flight/failsafe.c
View file

@ -217,8 +217,8 @@ void failsafeUpdateState(void)
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData
reprocessState = true;
} else if (!receivingRxData) {
if (millis() > failsafeState.throttleLowPeriod) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
if (millis() > failsafeState.throttleLowPeriod || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData

2
src/main/flight/mixer.c
View file

@ -504,7 +504,7 @@ void mixTable(void)
// Motor stop handling
if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D)) {
if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
if (((rcData[THROTTLE]) < rxConfig->mincheck) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
motor[i] = motorConfig->mincommand;
}
}

115
src/main/flight/navigation_rewrite.c
View file

@ -118,6 +118,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
/** Idle state ******************************************************/
@ -135,6 +138,7 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE,
}
},
@ -629,6 +633,50 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_LAUNCH_INITIALIZE] = {
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_REQUIRE_ANGLE,
.mapToFlightModes = NAV_LAUNCH_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_WAIT,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_LAUNCH_WAIT] = {
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_WAIT,
.timeoutMs = 10,
.stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
.mapToFlightModes = NAV_LAUNCH_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_WAIT, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_LAUNCH_IN_PROGRESS] = {
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
.mapToFlightModes = NAV_LAUNCH_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
};
static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
@ -1227,6 +1275,40 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS
return NAV_FSM_EVENT_SUCCESS;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState)
{
const uint32_t currentTime = micros();
UNUSED(previousState);
resetFixedWingLaunchController(currentTime);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_WAIT
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState)
{
const uint32_t currentTime = micros();
UNUSED(previousState);
if (isFixedWingLaunchDetected()) {
enableFixedWingLaunchController(currentTime);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_MOTOR_DELAY
}
return NAV_FSM_EVENT_NONE;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (isFixedWingLaunchFinishedOrAborted()) {
return NAV_FSM_EVENT_SUCCESS;
}
return NAV_FSM_EVENT_NONE;
}
static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
{
navigationFSMState_t previousState;
@ -2111,6 +2193,9 @@ void applyWaypointNavigationAndAltitudeHold(void)
#endif
#endif
// Reset all navigation requests - NAV controllers will set them if necessary
DISABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
// No navigation when disarmed
if (!ARMING_FLAG(ARMED)) {
// If we are disarmed, abort forced RTH
@ -2152,7 +2237,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
void swithNavigationFlightModes(void)
{
flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE) & (~enabledNavFlightModes);
flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE) & (~enabledNavFlightModes);
DISABLE_FLIGHT_MODE(disabledFlightModes);
ENABLE_FLIGHT_MODE(enabledNavFlightModes);
}
@ -2173,6 +2258,7 @@ static bool canActivatePosHoldMode(void)
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
{
static bool canActivateWaypoint = false;
static bool canActivateLaunchMode = false;
//We can switch modes only when ARMED
if (ARMING_FLAG(ARMED)) {
@ -2180,6 +2266,26 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
bool canActivateAltHold = canActivateAltHoldMode();
bool canActivatePosHold = canActivatePosHoldMode();
// LAUNCH mode has priority over any other NAV mode
if (STATE(FIXED_WING)) {
if (IS_RC_MODE_ACTIVE(BOXNAVLAUNCH)) { // FIXME: Only available for fixed wing aircrafts now
if (canActivateLaunchMode) {
canActivateLaunchMode = false;
return NAV_FSM_EVENT_SWITCH_TO_LAUNCH;
}
else if FLIGHT_MODE(NAV_LAUNCH_MODE) {
// Make sure we don't bail out to IDLE
return NAV_FSM_EVENT_NONE;
}
}
else {
// If we were in LAUNCH mode - force switch to IDLE
if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
}
}
// RTH/Failsafe_RTH can override PASSTHRU
if (posControl.flags.forcedRTHActivated || IS_RC_MODE_ACTIVE(BOXNAVRTH)) {
// If we request forced RTH - attempt to activate it no matter what
@ -2220,6 +2326,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
else {
canActivateWaypoint = false;
// Launch mode can only be activated if BOX is turned on prior to arming (avoid switching to LAUNCH in flight)
canActivateLaunchMode = IS_RC_MODE_ACTIVE(BOXNAVLAUNCH);
}
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
@ -2263,6 +2372,8 @@ int8_t naivationGetHeadingControlState(void)
bool naivationBlockArming(void)
{
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD);
const bool navLaunchComboModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP));
bool shouldBlockArming = false;
if (!posControl.navConfig->flags.extra_arming_safety)
@ -2274,7 +2385,7 @@ bool naivationBlockArming(void)
}
// Don't allow arming if any of NAV modes is active
if ((!ARMING_FLAG(ARMED)) && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD))) {
if (!ARMING_FLAG(ARMED) && navBoxModesEnabled && !navLaunchComboModesEnabled) {
shouldBlockArming = true;
}

7
src/main/flight/navigation_rewrite.h
View file

@ -129,6 +129,13 @@ typedef struct navConfig_s {
uint8_t fw_pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
uint8_t fw_roll_to_pitch; // Roll to pitch compensation (in %)
uint16_t fw_loiter_radius; // Loiter radius when executing PH on a fixed wing
uint16_t fw_launch_accel_thresh; // Acceleration threshold for launch detection (cm/s/s)
uint16_t fw_launch_time_thresh; // Time threshold for launch detection (ms)
uint16_t fw_launch_throttle; // Launch throttle
uint16_t fw_launch_motor_timer; // Time to wait before setting launch_throttle (ms)
uint16_t fw_launch_timeout; // Launch timeout to disable launch mode and swith to normal flight (ms)
uint8_t fw_launch_climb_angle; // Target climb angle for launch (deg)
} navConfig_t;
typedef struct gpsOrigin_s {

5
src/main/flight/navigation_rewrite_fixedwing.c
View file

@ -485,7 +485,10 @@ void resetFixedWingHeadingController(void)
void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, uint32_t currentTime)
{
if (navStateFlags & NAV_CTL_EMERG) {
if (navStateFlags & NAV_CTL_LAUNCH) {
applyFixedWingLaunchController(currentTime);
}
else if (navStateFlags & NAV_CTL_EMERG) {
applyFixedWingEmergencyLandingController();
}
else {

173
src/main/flight/navigation_rewrite_fw_launch.c Executable file
View file

@ -0,0 +1,173 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include <string.h>
#include "platform.h"
#if defined(NAV)
#include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/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"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/navigation_rewrite.h"
#include "flight/navigation_rewrite_private.h"
#include "fc/runtime_config.h"
#include "config/config.h"
#include "config/feature.h"
typedef struct FixedWingLaunchState_s {
/* Launch detection */
uint32_t launchDetectorPreviosUpdate;
uint32_t launchDetectionTimeAccum;
bool launchDetected;
/* Launch progress */
uint32_t launchStartedTime;
bool launchFinished;
bool motorControlAllowed;
} FixedWingLaunchState_t;
static FixedWingLaunchState_t launchState;
#define COS_MAX_LAUNCH_ANGLE 0.70710678f // cos(45), just to be safe
static void updateFixedWingLaunchDetector(const uint32_t currentTime)
{
const bool isForwardAccelerationHigh = (imuAccelInBodyFrame.A[X] > posControl.navConfig->fw_launch_accel_thresh);
const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= COS_MAX_LAUNCH_ANGLE);
if (isForwardAccelerationHigh && isAircraftAlmostLevel) {
launchState.launchDetectionTimeAccum += (currentTime - launchState.launchDetectorPreviosUpdate);
launchState.launchDetectorPreviosUpdate = currentTime;
if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)posControl.navConfig->fw_launch_time_thresh)) {
launchState.launchDetected = true;
}
}
else {
launchState.launchDetectorPreviosUpdate = currentTime;
launchState.launchDetectionTimeAccum = 0;
}
}
void resetFixedWingLaunchController(const uint32_t currentTime)
{
launchState.launchDetectorPreviosUpdate = currentTime;
launchState.launchDetectionTimeAccum = 0;
launchState.launchStartedTime = 0;
launchState.launchDetected = false;
launchState.launchFinished = false;
launchState.motorControlAllowed = false;
}
bool isFixedWingLaunchDetected(void)
{
return launchState.launchDetected;
}
void enableFixedWingLaunchController(const uint32_t currentTime)
{
launchState.launchStartedTime = currentTime;
launchState.motorControlAllowed = true;
}
bool isFixedWingLaunchFinishedOrAborted(void)
{
return launchState.launchFinished;
}
void applyFixedWingLaunchController(const uint32_t currentTime)
{
// Called at PID rate
if (launchState.launchDetected) {
// If launch detected we are in launch procedure - control airplane
const float timeElapsedSinceLaunchMs = US2MS(currentTime - launchState.launchStartedTime);
// If user moves the stick - finish the launch
if ((ABS(rcCommand[ROLL]) > posControl.rcControlsConfig->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > posControl.rcControlsConfig->pos_hold_deadband)) {
launchState.launchFinished = true;
}
// Motor control enabled
if (launchState.motorControlAllowed) {
// Abort launch after a pre-set time
if (timeElapsedSinceLaunchMs >= posControl.navConfig->fw_launch_timeout) {
launchState.launchFinished = true;
}
// Control throttle
if (timeElapsedSinceLaunchMs >= posControl.navConfig->fw_launch_motor_timer) {
rcCommand[THROTTLE] = posControl.navConfig->fw_launch_throttle;
}
else {
// Until motors are started don't use PID I-term
pidResetErrorAccumulators();
// Throttle control logic
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
rcCommand[THROTTLE] = posControl.motorConfig->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle
}
}
}
else {
// We are waiting for launch - update launch detector
updateFixedWingLaunchDetector(currentTime);
// Until motors are started don't use PID I-term
pidResetErrorAccumulators();
// Throttle control logic
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
rcCommand[THROTTLE] = posControl.motorConfig->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle
}
// Control beeper
if (!launchState.launchFinished) {
beeper(BEEPER_LAUNCH_MODE_ENABLED);
}
// Lock out controls
rcCommand[ROLL] = 0;
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(posControl.navConfig->fw_launch_climb_angle), posControl.pidProfile->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = 0;
}
#endif

35
src/main/flight/navigation_rewrite_private.h
View file

@ -36,6 +36,7 @@
#define HZ2US(hz) (1000000 / (hz))
#define US2S(us) ((us) * 1e-6f)
#define US2MS(us) ((us) * 1e-3f)
#define MS2US(ms) ((ms) * 1000)
#define HZ2S(hz) US2S(HZ2US(hz))
@ -148,6 +149,7 @@ typedef enum {
NAV_FSM_EVENT_SWITCH_TO_RTH_3D,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT,
NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING,
NAV_FSM_EVENT_SWITCH_TO_LAUNCH,
NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event
NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event
@ -197,9 +199,14 @@ typedef enum {
NAV_STATE_WAYPOINT_FINISHED, // 27
NAV_STATE_WAYPOINT_RTH_LAND, // 28
NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
NAV_STATE_EMERGENCY_LANDING_FINISHED,
NAV_STATE_EMERGENCY_LANDING_INITIALIZE, // 29
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // 30
NAV_STATE_EMERGENCY_LANDING_FINISHED, // 31
NAV_STATE_LAUNCH_INITIALIZE, // 32
NAV_STATE_LAUNCH_WAIT, // 33
NAV_STATE_LAUNCH_MOTOR_DELAY, // 34
NAV_STATE_LAUNCH_IN_PROGRESS, // 35
NAV_STATE_COUNT,
} navigationFSMState_t;
@ -210,19 +217,20 @@ typedef enum {
NAV_CTL_POS = (1 << 1), // Position controller
NAV_CTL_YAW = (1 << 2),
NAV_CTL_EMERG = (1 << 3),
NAV_CTL_LAUNCH = (1 << 4),
/* Navigation requirements for flight modes and controllers */
NAV_REQUIRE_ANGLE = (1 << 4),
NAV_REQUIRE_ANGLE_FW = (1 << 5),
NAV_REQUIRE_MAGHOLD = (1 << 6),
NAV_REQUIRE_THRTILT = (1 << 7),
NAV_REQUIRE_ANGLE = (1 << 5),
NAV_REQUIRE_ANGLE_FW = (1 << 6),
NAV_REQUIRE_MAGHOLD = (1 << 7),
NAV_REQUIRE_THRTILT = (1 << 8),
/* Navigation autonomous modes */
NAV_AUTO_RTH = (1 << 8),
NAV_AUTO_WP = (1 << 9),
NAV_AUTO_RTH = (1 << 9),
NAV_AUTO_WP = (1 << 10),
/* Adjustments for navigation modes from RC input */
NAV_RC_ALT = (1 << 10),
NAV_RC_ALT = (1 << 11),
NAV_RC_POS = (1 << 12),
NAV_RC_YAW = (1 << 13),
} navigationFSMStateFlags_t;
@ -356,4 +364,11 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
void calculateFixedWingInitialHoldPosition(t_fp_vector * pos);
/* Fixed-wing launch controller */
void resetFixedWingLaunchController(const uint32_t currentTime);
bool isFixedWingLaunchDetected(void);
void enableFixedWingLaunchController(const uint32_t currentTime);
bool isFixedWingLaunchFinishedOrAborted(void);
void applyFixedWingLaunchController(const uint32_t currentTime);
#endif

9
src/main/io/beeper.c
View file

@ -119,6 +119,10 @@ static const uint8_t beep_2longerBeeps[] = {
static const uint8_t beep_gyroCalibrated[] = {
20, 10, 20, 10, 20, 10, BEEPER_COMMAND_STOP
};
// two short beeps and a pause (first pause, then short beep)
static const uint8_t beep_launchModeBeep[] = {
5, 5, 5, 100, BEEPER_COMMAND_STOP
};
// array used for variable # of beeps (reporting GPS sat count, etc)
static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 2];
@ -173,9 +177,10 @@ typedef struct beeperTableEntry_s {
{ BEEPER_ENTRY(BEEPER_ARMED, 15, beep_armedBeep, "ARMED") },
{ BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 16, NULL, "SYSTEM_INIT") },
{ BEEPER_ENTRY(BEEPER_USB, 17, NULL, "ON_USB") },
{ BEEPER_ENTRY(BEEPER_LAUNCH_MODE_ENABLED, 18, beep_launchModeBeep, "LAUNCH_MODE") },
{ BEEPER_ENTRY(BEEPER_ALL, 18, NULL, "ALL") },
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 19, NULL, "PREFERED") },
{ BEEPER_ENTRY(BEEPER_ALL, 19, NULL, "ALL") },
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 20, NULL, "PREFERED") },
};
static const beeperTableEntry_t *currentBeeperEntry = NULL;

1
src/main/io/beeper.h
View file

@ -39,6 +39,7 @@ typedef enum {
BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased)
BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on
BEEPER_USB, // Some boards have beeper powered USB connected
BEEPER_LAUNCH_MODE_ENABLED, // Fixed-wing launch mode enabled
BEEPER_ALL, // Turn ON or OFF all beeper conditions
BEEPER_PREFERENCE, // Save preferred beeper configuration

7
src/main/io/serial_cli.c
View file

@ -718,6 +718,13 @@ const clivalue_t valueTable[] = {
{ "nav_fw_pitch2thr", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.fw_pitch_to_throttle, .config.minmax = { 0, 100 }, 0 },
{ "nav_fw_roll2pitch", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.fw_roll_to_pitch, .config.minmax = { 0, 200 }, 0 },
{ "nav_fw_loiter_radius", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_loiter_radius, .config.minmax = { 0, 10000 }, 0 },
{ "nav_fw_launch_accel", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_launch_accel_thresh, .config.minmax = { 1000, 20000 }, 0 },
{ "nav_fw_launch_detect_time", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_launch_time_thresh, .config.minmax = { 10, 1000 }, 0 },
{ "nav_fw_launch_thr", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_launch_throttle, .config.minmax = { 1000, 2000 }, 0 },
{ "nav_fw_launch_motor_delay", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_launch_motor_timer, .config.minmax = { 0, 5000 }, 0 },
{ "naw_fw_launch_timeout", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_launch_timeout, .config.minmax = { 0, 60000 }, 0 },
{ "naw_fw_launch_climb_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.fw_launch_climb_angle, .config.minmax = { 5, 45 }, 0 },
#endif
#ifdef SERIAL_RX