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

Move variables into a struct. Refine some states

Browse source
This commit is contained in:
DESKTOP-N53JVUO\mix 2020-07-21 17:43:36 +03:00
parent 605efa8057
commit 169fe5feea
1 changed files with 37 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -85,15 +85,17 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs);
typedef struct fixedWingLaunchStateDescriptor_s { typedef struct fixedWingLaunchStateDescriptor_s {
fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs);
fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs); fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT];
fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT];
} fixedWingLaunchStateDescriptor_t; } fixedWingLaunchStateDescriptor_t;
static EXTENDED_FASTRAM timeUs_t currentStateTimeUs; typedef struct fixedWingLaunchData_s {
static EXTENDED_FASTRAM fixedWingLaunchState_t currentState; timeUs_t currentStateTimeUs;
static EXTENDED_FASTRAM uint8_t pitchAngle; 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] = { static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = {
@ -134,14 +136,14 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY,
.onEvent = { .onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP, [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP,
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_FINISH [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE
} }
}, },
[FW_LAUNCH_STATE_MOTOR_SPINUP] = { [FW_LAUNCH_STATE_MOTOR_SPINUP] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP,
.onEvent = { .onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS, [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS,
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_FINISH [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE
} }
}, },
[FW_LAUNCH_STATE_IN_PROGRESS] = { [FW_LAUNCH_STATE_IN_PROGRESS] = {
@ -161,12 +163,12 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
/* Current State Handlers */ /* Current State Handlers */
static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) { static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) {
return US2MS(currentTimeUs - currentStateTimeUs); return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs);
} }
static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) {
currentState = nextState; fwLaunch.currentState = nextState;
currentStateTimeUs = currentTimeUs; fwLaunch.currentStateTimeUs = currentTimeUs;
} }
/* Wing control Helpers */ /* Wing control Helpers */
@ -198,15 +200,16 @@ static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs)
static void resetPidsIfNeeded(void) { static void resetPidsIfNeeded(void) {
// Until motors are started don't use PID I-term and reset TPA filter // Until motors are started don't use PID I-term and reset TPA filter
if (currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { if (fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) {
pidResetErrorAccumulators(); pidResetErrorAccumulators();
pidResetTPAFilter(); pidResetTPAFilter();
} }
} }
static void updateRcCommand(void) { static void updateRcCommand(void) {
// lock roll and yaw and apply needed pitch angle
rcCommand[ROLL] = 0; rcCommand[ROLL] = 0;
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = 0; rcCommand[YAW] = 0;
} }
@ -221,7 +224,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t curren
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) { static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs); UNUSED(currentTimeUs);
pitchAngle = 0; fwLaunch.pitchAngle = 0;
if (!isThrottleLow()) { if (!isThrottleLow()) {
if (isThrottleIdleEnabled()) { if (isThrottleIdleEnabled()) {
@ -236,7 +239,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) { static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) {
if (isThrottleLow()) { if (isThrottleLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
} }
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) { if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) {
@ -244,7 +247,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
} else { } else {
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle);
} }
return FW_LAUNCH_EVENT_NONE; return FW_LAUNCH_EVENT_NONE;
@ -252,7 +255,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) { static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) {
if (isThrottleLow()) { if (isThrottleLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
} }
const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0; const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
@ -266,10 +269,10 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU
if (isBungeeLaunched || isSwingLaunched) { if (isBungeeLaunched || isSwingLaunched) {
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) {
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED
} }
} else { } else {
currentStateTimeUs = currentTimeUs; // reset the state counter fwLaunch.currentStateTimeUs = currentTimeUs; // reset the state counter
} }
return FW_LAUNCH_EVENT_NONE; return FW_LAUNCH_EVENT_NONE;
@ -277,7 +280,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) { static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs); UNUSED(currentTimeUs);
// waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY
applyThrottleIdle(); applyThrottleIdle();
return FW_LAUNCH_EVENT_NONE; return FW_LAUNCH_EVENT_NONE;
@ -287,7 +290,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t
applyThrottleIdle(); applyThrottleIdle();
if (areSticksMoved(0, currentTimeUs)) { if (areSticksMoved(0, currentTimeUs)) {
return FW_LAUNCH_EVENT_ABORT; return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE
} }
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) {
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
@ -298,7 +301,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) { static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) {
if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) { if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) {
return FW_LAUNCH_EVENT_ABORT; return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE
} }
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
@ -320,13 +323,13 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
if (isLaunchMaxAltitudeReached()) { if (isLaunchMaxAltitudeReached()) {
return FW_LAUNCH_EVENT_SUCCESS; // We're on air, cancel the launch and do the FW_LAUNCH_STATE_FINISH state 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(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 return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
} }
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) {
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH
} }
return FW_LAUNCH_EVENT_NONE; return FW_LAUNCH_EVENT_NONE;
@ -339,8 +342,9 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
if (elapsedTimeMs > endTimeMs) { if (elapsedTimeMs > endTimeMs) {
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
} else { } 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]); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]);
pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
} }
return FW_LAUNCH_EVENT_NONE; return FW_LAUNCH_EVENT_NONE;
@ -351,12 +355,13 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
void applyFixedWingLaunchController(timeUs_t currentTimeUs) { void applyFixedWingLaunchController(timeUs_t currentTimeUs) {
// Called at PID rate // Called at PID rate
while (launchStateMachine[currentState].onEntry) { // process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE
fixedWingLaunchEvent_t newEvent = launchStateMachine[currentState].onEntry(currentTimeUs); while (launchStateMachine[fwLaunch.currentState].onEntry) {
fixedWingLaunchEvent_t newEvent = launchStateMachine[fwLaunch.currentState].onEntry(currentTimeUs);
if (newEvent == FW_LAUNCH_EVENT_NONE) { if (newEvent == FW_LAUNCH_EVENT_NONE) {
break; break;
} }
setCurrentState(launchStateMachine[currentState].onEvent[newEvent], currentTimeUs); setCurrentState(launchStateMachine[fwLaunch.currentState].onEvent[newEvent], currentTimeUs);
} }
resetPidsIfNeeded(); resetPidsIfNeeded();
@ -371,7 +376,7 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs) {
} }
bool isFixedWingLaunchDetected(void) { bool isFixedWingLaunchDetected(void) {
return currentState == FW_LAUNCH_STATE_DETECTED; return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED;
} }
void enableFixedWingLaunchController(timeUs_t currentTimeUs) { void enableFixedWingLaunchController(timeUs_t currentTimeUs) {
@ -379,7 +384,7 @@ void enableFixedWingLaunchController(timeUs_t currentTimeUs) {
} }
bool isFixedWingLaunchFinishedOrAborted(void) { bool isFixedWingLaunchFinishedOrAborted(void) {
return currentState == FW_LAUNCH_STATE_IDLE; return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE;
} }
void abortFixedWingLaunch(void) { void abortFixedWingLaunch(void) {
@ -387,7 +392,7 @@ void abortFixedWingLaunch(void) {
} }
const char * fixedWingLaunchStateMessage(void) { const char * fixedWingLaunchStateMessage(void) {
switch (currentState) { switch (fwLaunch.currentState) {
case FW_LAUNCH_STATE_WAIT_DETECTION: case FW_LAUNCH_STATE_WAIT_DETECTION:
return "READY"; return "READY";
case FW_LAUNCH_STATE_FINISH: case FW_LAUNCH_STATE_FINISH: