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

Bugfix for not-working right stick abort; bugfix for LAUNCH-mode re-enable in flight; remove motor stop (not working)

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-10-06 10:17:10 +10:00
parent 377ae4bf02
commit a4486e832e
5 changed files with 36 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -361,7 +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(LAUNCH_MODE)) << BOXNAVLAUNCH |
IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)) << BOXNAVLAUNCH |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET;
uint32_t ret = 0;

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

@ -39,7 +39,7 @@ typedef enum {
NAV_RTH_MODE = (1 << 4), // old GPS_HOME
NAV_POSHOLD_MODE= (1 << 5), // old GPS_HOLD
HEADFREE_MODE = (1 << 6),
LAUNCH_MODE = (1 << 7),
NAV_LAUNCH_MODE = (1 << 7),
PASSTHRU_MODE = (1 << 8),
FAILSAFE_MODE = (1 << 10),
UNUSED_MODE = (1 << 11), // old G-Tune

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

@ -638,7 +638,7 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_REQUIRE_ANGLE,
.mapToFlightModes = LAUNCH_MODE,
.mapToFlightModes = NAV_LAUNCH_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
@ -652,10 +652,11 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_WAIT,
.timeoutMs = 10,
.stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
.mapToFlightModes = LAUNCH_MODE,
.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,
@ -666,10 +667,11 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
.mapToFlightModes = LAUNCH_MODE,
.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,
@ -1278,7 +1280,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navig
const uint32_t currentTime = micros();
UNUSED(previousState);
resetFixedWingLaunchMode(currentTime);
resetFixedWingLaunchController(currentTime);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_WAIT
}
@ -1289,7 +1291,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
UNUSED(previousState);
if (isFixedWingLaunchDetected()) {
resetFixedWingLaunchController(currentTime);
enableFixedWingLaunchController(currentTime);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_MOTOR_DELAY
}
@ -2232,7 +2234,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);
}
@ -2264,15 +2266,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
// 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 (FLIGHT_MODE(LAUNCH_MODE) || canActivateLaunchMode) {
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(LAUNCH_MODE))
if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
}
}

37
src/main/flight/navigation_rewrite_fw_launch.c
View file

@ -62,13 +62,18 @@ typedef struct FixedWingLaunchState_s {
/* 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)
{
if (imuAccelInBodyFrame.A[X] > posControl.navConfig->fw_launch_accel_thresh) {
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)) {
@ -81,13 +86,14 @@ static void updateFixedWingLaunchDetector(const uint32_t currentTime)
}
}
void resetFixedWingLaunchMode(const uint32_t currentTime)
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)
@ -95,9 +101,10 @@ bool isFixedWingLaunchDetected(void)
return launchState.launchDetected;
}
void resetFixedWingLaunchController(const uint32_t currentTime)
void enableFixedWingLaunchController(const uint32_t currentTime)
{
launchState.launchStartedTime = currentTime;
launchState.motorControlAllowed = true;
}
bool isFixedWingLaunchFinishedOrAborted(void)
@ -109,7 +116,7 @@ void applyFixedWingLaunchController(const uint32_t currentTime)
{
// Called at PID rate
if (isFixedWingLaunchDetected()) {
if (launchState.launchDetected) {
// If launch detected we are in launch procedure - control airplane
const float timeElapsedSinceLaunchMs = US2MS(currentTime - launchState.launchStartedTime);
@ -119,20 +126,15 @@ void applyFixedWingLaunchController(const uint32_t currentTime)
}
// Control throttle
if (timeElapsedSinceLaunchMs < posControl.navConfig->fw_launch_motor_timer) {
if (timeElapsedSinceLaunchMs >= posControl.navConfig->fw_launch_motor_timer && launchState.motorControlAllowed) {
rcCommand[THROTTLE] = posControl.navConfig->fw_launch_throttle;
}
else {
// Until motors are started don't use PID I-term
pidResetErrorAccumulators();
// Throttle control logic - if motor stop is enabled - keep motors stopped
if (feature(FEATURE_MOTOR_STOP)) {
rcCommand[THROTTLE] = posControl.escAndServoConfig->mincommand;
}
else {
rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle;
}
}
else {
rcCommand[THROTTLE] = posControl.navConfig->fw_launch_throttle;
rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle;
}
}
else {
@ -143,12 +145,7 @@ void applyFixedWingLaunchController(const uint32_t currentTime)
pidResetErrorAccumulators();
// Throttle control logic
if (feature(FEATURE_MOTOR_STOP)) {
rcCommand[THROTTLE] = posControl.escAndServoConfig->mincommand;
}
else {
rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle;
}
rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle;
}
// Lock out controls

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

@ -365,9 +365,9 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
void calculateFixedWingInitialHoldPosition(t_fp_vector * pos);
/* Fixed-wing launch controller */
void resetFixedWingLaunchMode(const uint32_t currentTime);
bool isFixedWingLaunchDetected(void);
void resetFixedWingLaunchController(const uint32_t currentTime);
bool isFixedWingLaunchDetected(void);
void enableFixedWingLaunchController(const uint32_t currentTime);
bool isFixedWingLaunchFinishedOrAborted(void);
void applyFixedWingLaunchController(const uint32_t currentTime);