Skip to content

Commit 9ebbfeb

Browse files
breadovenbreadoven
authored
Merge pull request #9294 from breadoven/abo_acro_attitude_hold
Fixed wing attitude stabilisation mode
2 parents 3406d0d + 279974b commit 9ebbfeb

File tree

18 files changed

+265
-174
lines changed

18 files changed

+265
-174
lines changed

‎src/main/fc/fc_core.c‎

100755100644
Lines changed: 58 additions & 69 deletions
Original file line numberDiff line numberDiff line change
@@ -548,7 +548,7 @@ void tryArm(void)
548548
if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) {
549549
sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
550550
ENABLE_ARMING_FLAG(ARMED);
551-
enableFlightMode(TURTLE_MODE);
551+
ENABLE_FLIGHT_MODE(TURTLE_MODE);
552552
return;
553553
}
554554
#endif
@@ -678,28 +678,21 @@ void processRx(timeUs_t currentTimeUs)
678678
// Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR)
679679
bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs);
680680
bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce;
681-
bool canUseHorizonMode = true;
682681

683-
if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) {
684-
// bumpless transfer to Level mode
685-
canUseHorizonMode = false;
682+
/* Disable stabilised modes initially, will be enabled as required with priority ANGLE > HORIZON > ANGLEHOLD
683+
* MANUAL mode has priority over these modes except when ANGLE auto enabled */
684+
DISABLE_FLIGHT_MODE(ANGLE_MODE);
685+
DISABLE_FLIGHT_MODE(HORIZON_MODE);
686+
DISABLE_FLIGHT_MODE(ANGLEHOLD_MODE);
686687

687-
if (!FLIGHT_MODE(ANGLE_MODE)) {
688+
if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) {
689+
if (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle) {
688690
ENABLE_FLIGHT_MODE(ANGLE_MODE);
689-
}
690-
} else {
691-
DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
692-
}
693-
694-
if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
695-
696-
DISABLE_FLIGHT_MODE(ANGLE_MODE);
697-
698-
if (!FLIGHT_MODE(HORIZON_MODE)) {
691+
} else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) {
699692
ENABLE_FLIGHT_MODE(HORIZON_MODE);
693+
} else if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
694+
ENABLE_FLIGHT_MODE(ANGLEHOLD_MODE);
700695
}
701-
} else {
702-
DISABLE_FLIGHT_MODE(HORIZON_MODE);
703696
}
704697

705698
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
@@ -710,18 +703,14 @@ void processRx(timeUs_t currentTimeUs)
710703

711704
/* Flaperon mode */
712705
if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
713-
if (!FLIGHT_MODE(FLAPERON)) {
714-
ENABLE_FLIGHT_MODE(FLAPERON);
715-
}
706+
ENABLE_FLIGHT_MODE(FLAPERON);
716707
} else {
717708
DISABLE_FLIGHT_MODE(FLAPERON);
718709
}
719710

720711
/* Turn assistant mode */
721712
if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
722-
if (!FLIGHT_MODE(TURN_ASSISTANT)) {
723-
ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
724-
}
713+
ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
725714
} else {
726715
DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
727716
}
@@ -740,9 +729,7 @@ void processRx(timeUs_t currentTimeUs)
740729
#if defined(USE_MAG)
741730
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
742731
if (IS_RC_MODE_ACTIVE(BOXHEADFREE) && STATE(MULTIROTOR)) {
743-
if (!FLIGHT_MODE(HEADFREE_MODE)) {
744-
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
745-
}
732+
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
746733
} else {
747734
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
748735
}
@@ -760,7 +747,7 @@ void processRx(timeUs_t currentTimeUs)
760747
} else {
761748
DISABLE_FLIGHT_MODE(MANUAL_MODE);
762749
}
763-
}else{
750+
} else {
764751
DISABLE_FLIGHT_MODE(MANUAL_MODE);
765752
}
766753

@@ -781,52 +768,52 @@ void processRx(timeUs_t currentTimeUs)
781768
}
782769
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
783770
if (throttleIsLow) {
784-
if (STATE(AIRMODE_ACTIVE)) {
785-
if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) {
786-
ENABLE_STATE(ANTI_WINDUP);
787-
}
788-
else {
789-
DISABLE_STATE(ANTI_WINDUP);
790-
}
791-
}
792-
else {
793-
DISABLE_STATE(ANTI_WINDUP);
794-
pidResetErrorAccumulators();
795-
}
796-
}
797-
else {
798-
DISABLE_STATE(ANTI_WINDUP);
799-
}
771+
if (STATE(AIRMODE_ACTIVE)) {
772+
if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) {
773+
ENABLE_STATE(ANTI_WINDUP);
774+
}
775+
else {
776+
DISABLE_STATE(ANTI_WINDUP);
777+
}
778+
}
779+
else {
780+
DISABLE_STATE(ANTI_WINDUP);
781+
pidResetErrorAccumulators();
782+
}
783+
}
784+
else {
785+
DISABLE_STATE(ANTI_WINDUP);
786+
}
800787
}
801788
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) {
802789
if (throttleIsLow) {
803-
if (STATE(AIRMODE_ACTIVE)) {
804-
if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
805-
ENABLE_STATE(ANTI_WINDUP);
806-
}
807-
else {
808-
DISABLE_STATE(ANTI_WINDUP);
809-
}
810-
}
811-
else {
812-
DISABLE_STATE(ANTI_WINDUP);
813-
pidResetErrorAccumulators();
814-
}
815-
}
816-
else {
817-
DISABLE_STATE(ANTI_WINDUP);
818-
if (rollPitchStatus != CENTERED) {
819-
ENABLE_STATE(ANTI_WINDUP_DEACTIVATED);
820-
}
821-
}
790+
if (STATE(AIRMODE_ACTIVE)) {
791+
if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
792+
ENABLE_STATE(ANTI_WINDUP);
793+
}
794+
else {
795+
DISABLE_STATE(ANTI_WINDUP);
796+
}
797+
}
798+
else {
799+
DISABLE_STATE(ANTI_WINDUP);
800+
pidResetErrorAccumulators();
801+
}
802+
}
803+
else {
804+
DISABLE_STATE(ANTI_WINDUP);
805+
if (rollPitchStatus != CENTERED) {
806+
ENABLE_STATE(ANTI_WINDUP_DEACTIVATED);
807+
}
808+
}
822809
}
823810
else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
824-
DISABLE_STATE(ANTI_WINDUP);
825-
//This case applies only to MR when Airmode management is throttle threshold activated
826-
if (throttleIsLow && !STATE(AIRMODE_ACTIVE)) {
827-
pidResetErrorAccumulators();
828-
}
829-
}
811+
DISABLE_STATE(ANTI_WINDUP);
812+
//This case applies only to MR when Airmode management is throttle threshold activated
813+
if (throttleIsLow && !STATE(AIRMODE_ACTIVE)) {
814+
pidResetErrorAccumulators();
815+
}
816+
}
830817
//---------------------------------------------------------
831818
if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) {
832819
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
@@ -849,6 +836,8 @@ void processRx(timeUs_t currentTimeUs)
849836
}
850837
}
851838
#endif
839+
// Sound a beeper if the flight mode state has changed
840+
updateFlightModeChangeBeeper();
852841
}
853842

854843
// Function for loop trigger

‎src/main/fc/fc_msp_box.c‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -102,6 +102,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
102102
{ .boxId = BOXMULTIFUNCTION, .boxName = "MULTI FUNCTION", .permanentId = 61 },
103103
{ .boxId = BOXMIXERPROFILE, .boxName = "MIXER PROFILE 2", .permanentId = 62 },
104104
{ .boxId = BOXMIXERTRANSITION, .boxName = "MIXER TRANSITION", .permanentId = 63 },
105+
{ .boxId = BOXANGLEHOLD, .boxName = "ANGLE HOLD", .permanentId = 64 },
105106
{ .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
106107
};
107108

@@ -259,7 +260,7 @@ void initActiveBoxIds(void)
259260
ADD_ACTIVE_BOX(BOXNAVALTHOLD);
260261
}
261262

262-
if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT) ||
263+
if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT) ||
263264
platformTypeConfigured(PLATFORM_AIRPLANE) || platformTypeConfigured(PLATFORM_ROVER) || platformTypeConfigured(PLATFORM_BOAT)) {
264265
ADD_ACTIVE_BOX(BOXMANUAL);
265266
}
@@ -279,6 +280,9 @@ void initActiveBoxIds(void)
279280
if (sensors(SENSOR_BARO)) {
280281
ADD_ACTIVE_BOX(BOXAUTOLEVEL);
281282
}
283+
if (sensors(SENSOR_ACC)) {
284+
ADD_ACTIVE_BOX(BOXANGLEHOLD);
285+
}
282286
}
283287

284288
/*
@@ -432,6 +436,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
432436
CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex), BOXMIXERPROFILE);
433437
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION);
434438
#endif
439+
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD);
435440
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
436441
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
437442
if (activeBoxes[activeBoxIds[i]]) {

‎src/main/fc/rc_modes.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,7 @@ typedef enum {
8181
BOXMULTIFUNCTION = 52,
8282
BOXMIXERPROFILE = 53,
8383
BOXMIXERTRANSITION = 54,
84+
BOXANGLEHOLD = 55,
8485
CHECKBOX_ITEM_COUNT
8586
} boxId_e;
8687

‎src/main/fc/runtime_config.c‎

Lines changed: 8 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -91,31 +91,16 @@ armingFlag_e isArmingDisabledReason(void)
9191
}
9292

9393
/**
94-
* Enables the given flight mode. A beep is sounded if the flight mode
95-
* has changed. Returns the new 'flightModeFlags' value.
94+
* Called at Rx update rate. Beeper sounded if flight mode state has changed.
9695
*/
97-
uint32_t enableFlightMode(flightModeFlags_e mask)
96+
void updateFlightModeChangeBeeper(void)
9897
{
99-
uint32_t oldVal = flightModeFlags;
98+
static uint32_t previousFlightModeFlags = 0;
10099

101-
flightModeFlags |= (mask);
102-
if (flightModeFlags != oldVal)
100+
if (flightModeFlags != previousFlightModeFlags) {
103101
beeperConfirmationBeeps(1);
104-
return flightModeFlags;
105-
}
106-
107-
/**
108-
* Disables the given flight mode. A beep is sounded if the flight mode
109-
* has changed. Returns the new 'flightModeFlags' value.
110-
*/
111-
uint32_t disableFlightMode(flightModeFlags_e mask)
112-
{
113-
uint32_t oldVal = flightModeFlags;
114-
115-
flightModeFlags &= ~(mask);
116-
if (flightModeFlags != oldVal)
117-
beeperConfirmationBeeps(1);
118-
return flightModeFlags;
102+
}
103+
previousFlightModeFlags = flightModeFlags;
119104
}
120105

121106
bool sensors(uint32_t mask)
@@ -173,6 +158,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
173158
if (FLIGHT_MODE(HORIZON_MODE))
174159
return FLM_HORIZON;
175160

161+
if (FLIGHT_MODE(ANGLEHOLD_MODE))
162+
return FLM_ANGLEHOLD;
176163

177164
return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
178165
}

‎src/main/fc/runtime_config.h‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -104,12 +104,13 @@ typedef enum {
104104
TURN_ASSISTANT = (1 << 14),
105105
TURTLE_MODE = (1 << 15),
106106
SOARING_MODE = (1 << 16),
107+
ANGLEHOLD_MODE = (1 << 17),
107108
} flightModeFlags_e;
108109

109110
extern uint32_t flightModeFlags;
110111

111-
#define DISABLE_FLIGHT_MODE(mask) disableFlightMode(mask)
112-
#define ENABLE_FLIGHT_MODE(mask) enableFlightMode(mask)
112+
#define DISABLE_FLIGHT_MODE(mask) (flightModeFlags &= ~(mask))
113+
#define ENABLE_FLIGHT_MODE(mask) (flightModeFlags |= (mask))
113114
#define FLIGHT_MODE(mask) (flightModeFlags & (mask))
114115

115116
typedef enum {
@@ -162,6 +163,7 @@ typedef enum {
162163
FLM_CRUISE,
163164
FLM_LAUNCH,
164165
FLM_FAILSAFE,
166+
FLM_ANGLEHOLD,
165167
FLM_COUNT
166168
} flightModeForTelemetry_e;
167169

@@ -200,8 +202,7 @@ extern simulatorData_t simulatorData;
200202

201203
#endif
202204

203-
uint32_t enableFlightMode(flightModeFlags_e mask);
204-
uint32_t disableFlightMode(flightModeFlags_e mask);
205+
void updateFlightModeChangeBeeper(void);
205206

206207
bool sensors(uint32_t mask);
207208
void sensorsSet(uint32_t mask);

‎src/main/fc/settings.yaml‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -192,7 +192,7 @@ tables:
192192
enum: gpsBaudRate_e
193193
- name: nav_mc_althold_throttle
194194
values: ["STICK", "MID_STICK", "HOVER"]
195-
enum: navMcAltHoldThrottle_e
195+
enum: navMcAltHoldThrottle_e
196196
- name: led_pin_pwm_mode
197197
values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"]
198198
enum: led_pin_pwm_mode_e
@@ -1186,7 +1186,7 @@ groups:
11861186
min: 0
11871187
max: 200
11881188

1189-
1189+
11901190

11911191
- name: PG_REVERSIBLE_MOTORS_CONFIG
11921192
type: reversibleMotorsConfig_t
@@ -3586,7 +3586,7 @@ groups:
35863586
min: 1000
35873587
max: 5000
35883588
default_value: 1500
3589-
3589+
35903590
- name: osd_switch_indicator_zero_name
35913591
description: "Character to use for OSD switch incicator 0."
35923592
field: osd_switch_indicator0_name

0 commit comments

Comments
 (0)