Commit 43eaf10d authored by Konstantin Sharlaimov's avatar Konstantin Sharlaimov Committed by GitHub
Browse files

Turn assistant - turn on parallel to ground plane (#384)

* Turn assistant - turn on parallel to ground plane
parent 9ef4de82
......@@ -46,6 +46,7 @@ typedef enum {
NAV_WP_MODE = (1 << 12),
HEADING_LOCK = (1 << 13),
FLAPERON = (1 << 14),
TURN_ASSISTANT = (1 << 15),
} flightModeFlags_e;
extern uint16_t flightModeFlags;
......
......@@ -438,6 +438,26 @@ float pidMagHold(const pidProfile_t *pidProfile)
return magHoldRate;
}
/*
* TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more
* and keeping ROLL and PITCH attitude though the turn.
*/
static void pidTurnAssistant(pidState_t *pidState)
{
t_fp_vector targetRates;
targetRates.V.X = 0.0f;
targetRates.V.Y = 0.0f;
targetRates.V.Z = pidState[YAW].rateTarget;
imuTransformVectorEarthToBody(&targetRates);
// Add in roll and pitch, replace yaw completery
pidState[ROLL].rateTarget += targetRates.V.X;
pidState[PITCH].rateTarget += targetRates.V.Y;
pidState[YAW].rateTarget = targetRates.V.Z;
}
void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig)
{
......@@ -475,6 +495,10 @@ void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *co
pidApplyHeadingLock(pidProfile, &pidState[FD_YAW]);
}
if (FLIGHT_MODE(TURN_ASSISTANT)) {
pidTurnAssistant(pidState);
}
// Step 4: Run gyro-driven control
for (int axis = 0; axis < 3; axis++) {
// Apply PID setpoint controller
......
......@@ -52,6 +52,7 @@ typedef enum {
BOXHEADINGLOCK,
BOXSURFACE,
BOXFLAPERON,
BOXTURNASSIST,
CHECKBOX_ITEM_COUNT
} boxId_e;
......
......@@ -149,6 +149,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXHEADINGLOCK, "HEADING LOCK;", 32 },
{ BOXSURFACE, "SURFACE;", 33 },
{ BOXFLAPERON, "FLAPERON;", 34 },
{ BOXTURNASSIST, "TURN ASSIST;", 35 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
......@@ -479,6 +480,7 @@ void mspInit(void)
if (sensors(SENSOR_ACC)) {
activeBoxIds[activeBoxIdCount++] = BOXANGLE;
activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
activeBoxIds[activeBoxIdCount++] = BOXTURNASSIST;
}
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
......@@ -595,6 +597,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(FLIGHT_MODE(HEADING_LOCK)) << BOXHEADINGLOCK |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)) << BOXSURFACE |
IS_ENABLED(FLIGHT_MODE(FLAPERON)) << BOXFLAPERON |
IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)) << BOXTURNASSIST |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET;
for (i = 0; i < activeBoxIdCount; i++) {
......
......@@ -383,9 +383,7 @@ void processRx(void)
DISABLE_FLIGHT_MODE(HEADING_LOCK);
}
/*
Flaperon mode
*/
/* Flaperon mode */
if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
if (!FLIGHT_MODE(FLAPERON)) {
ENABLE_FLIGHT_MODE(FLAPERON);
......@@ -394,6 +392,15 @@ void processRx(void)
DISABLE_FLIGHT_MODE(FLAPERON);
}
/* Turn assistant mode */
if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
if (!FLIGHT_MODE(TURN_ASSISTANT)) {
ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
}
} else {
DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
}
#if defined(MAG)
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
if (IS_RC_MODE_ACTIVE(BOXMAG)) {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment