diff --git a/Makefile b/Makefile
index 1d57ff0fb0..bd7c7f865c 100644
--- a/Makefile
+++ b/Makefile
@@ -144,7 +144,7 @@ PROJ_OBJ_CF2 += libdw1000.o libdw1000Spi.o
# Modules
PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o mem.o
-PROJ_OBJ += commander.o attitude_pid_controller.o sensfusion6.o stabilizer.o position_estimator_altitude.o position_controller_altitude.o
+PROJ_OBJ += commander.o attitude_pid_controller.o sensfusion6.o stabilizer.o position_estimator_altitude.o position_controller_altitude.o altitude_hold.o
PROJ_OBJ += log.o worker.o trigger.o sitaw.o queuemonitor.o
PROJ_OBJ_CF1 += sound_cf1.o
PROJ_OBJ_CF2 += platformservice.o sound_cf2.o extrx.o
diff --git a/src/modules/interface/altitude_hold.h b/src/modules/interface/altitude_hold.h
new file mode 100644
index 0000000000..c025929dd8
--- /dev/null
+++ b/src/modules/interface/altitude_hold.h
@@ -0,0 +1,34 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2016 Bitcraze AB
+ *
+ * This program 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, in version 3.
+ *
+ * This program 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 this program. If not, see .
+ *
+ *
+ */
+#ifndef ALTITUDE_HOLD_H_
+#define ALTITUDE_HOLD_H_
+
+#include "stabilizer_types.h"
+
+bool altHoldIsActive();
+void altHoldGetNewSetPoint(setpointZ_t* setpoint, const estimate_t* estimate);
+
+#endif /* ALTITUDE_HOLD_H_ */
diff --git a/src/modules/interface/commander.h b/src/modules/interface/commander.h
index 1d1d27a4ff..4b7ec22b49 100644
--- a/src/modules/interface/commander.h
+++ b/src/modules/interface/commander.h
@@ -75,7 +75,6 @@ void commanderGetRPY(float* eulerRollDesired, float* eulerPitchDesired, float* e
void commanderGetRPYType(RPYType* rollType, RPYType* pitchType, RPYType* yawType);
void commanderGetThrust(uint16_t* thrust);
void commanderGetAltHold(bool* altHold, bool* setAltHold, float* altHoldChange);
-bool commanderGetAltHoldMode(void);
void commanderSetAltHoldMode(bool altHoldModeNew);
YawModeType commanderGetYawMode(void);
bool commanderGetYawModeCarefreeResetFront(void);
diff --git a/src/modules/interface/position_controller.h b/src/modules/interface/position_controller.h
index 818574bf42..0fb1910569 100644
--- a/src/modules/interface/position_controller.h
+++ b/src/modules/interface/position_controller.h
@@ -29,5 +29,6 @@
#include "stabilizer_types.h"
void positionControllerUpdate(uint16_t* actuatorThrust, const estimate_t* estimate, float dt);
+void positionControllerSetZTarget(const setpointZ_t* setpoint, float dt);
#endif /* POSITION_CONTROLLER_H_ */
diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
index 99ac4cfcbd..0a0330e5e3 100644
--- a/src/modules/interface/stabilizer_types.h
+++ b/src/modules/interface/stabilizer_types.h
@@ -77,4 +77,10 @@ typedef struct estimate_s {
point_t position;
} estimate_t;
+/** Setpoint for althold */
+typedef struct setpointZ_s {
+ float z;
+ bool isUpdate; // True = small update of setpoint, false = completely new
+} setpointZ_t;
+
#endif
diff --git a/src/modules/src/altitude_hold.c b/src/modules/src/altitude_hold.c
new file mode 100644
index 0000000000..70f2a404e7
--- /dev/null
+++ b/src/modules/src/altitude_hold.c
@@ -0,0 +1,186 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie Firmware
+ *
+ * Copyright (C) 2016 Bitcraze AB
+ *
+ * This program 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, in version 3.
+ *
+ * This program 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 this program. If not, see .
+ *
+ * position_estimator_altitude.c: Altitude-only position estimator
+ */
+
+#define DEBUG_MODULE "ALTHOLD"
+#include "debug.h"
+
+#include "commander.h"
+#include "log.h"
+#include "param.h"
+#include "sitaw.h"
+#include "altitude_hold.h"
+
+struct state_s {
+ float targetZ; // Target altitude
+ float targetChangeSens; // sensitivity of target altitude change (thrust input control) while hovering. Lower = more sensitive & faster changes
+
+ bool isActive;
+ bool justActivated;
+ float pilotChange;
+
+ #if defined(SITAW_ENABLED)
+ // Automatic take-off variables
+ bool autoTOActive; // Flag indicating if automatic take-off is active / deactive.
+ float autoTOAltBase; // Base altitude for the automatic take-off. Set to targetZ when automatic take-off is activated.
+ float autoTOAltCurrent; // Current target altitude adjustment. Equals 0 when function is activated, increases to autoTOThresh when function is deactivated.
+ // Automatic take-off parameters
+ float autoTOAlpha; // Smoothing factor when adjusting the targetZ altitude.
+ float autoTOTargetAdjust; // Meters to add to targetZ to reach auto take-off altitude.
+ float autoTOThresh; // Threshold for when to deactivate auto Take-Off. A value of 0.97 means 97% of the target altitude adjustment.
+ #endif
+};
+
+static struct state_s state = {
+ .targetZ = -1,
+ .targetChangeSens = 200,
+
+ .isActive = false,
+ .justActivated = false,
+ .pilotChange = 0.0,
+
+ #if defined(SITAW_ENABLED)
+ // Automatic take-off variables
+ .autoTOActive = false,
+ .autoTOAltBase = 0.0f,
+ .autoTOAltCurrent = 0.0f,
+
+ .autoTOAlpha = 0.98f,
+ .autoTOTargetAdjust = 1.5f,
+ .autoTOThresh = 0.97f,
+ #endif
+};
+
+static float preAltHoldComputeZCallOut(struct state_s* state);
+static bool altHoldIsActiveInternal(struct state_s* state);
+static void altHoldGetNewSetPointInternal(setpointZ_t* setpoint, const estimate_t* estimate, struct state_s* state);
+
+bool altHoldIsActive() {
+ return altHoldIsActiveInternal(&state);
+}
+
+void altHoldGetNewSetPoint(setpointZ_t* setpoint, const estimate_t* estimate) {
+ altHoldGetNewSetPointInternal(setpoint, estimate, &state);
+}
+
+static bool altHoldIsActiveInternal(struct state_s* state) {
+ // Get altitude hold commands from pilot
+ commanderGetAltHold(&state->isActive, &state->justActivated, &state->pilotChange);
+ return state->isActive;
+}
+
+static void altHoldGetNewSetPointInternal(setpointZ_t* setpoint, const estimate_t* estimate, struct state_s* state) {
+ if (state->justActivated) {
+ setpoint->isUpdate = false;
+
+ state->targetZ = estimate->position.z;
+ } else {
+ setpoint->isUpdate = true;
+
+ float delta = state->pilotChange / state->targetChangeSens;
+ state->targetZ += delta;
+ }
+
+ state->targetZ = preAltHoldComputeZCallOut(state);
+
+ setpoint->z = state->targetZ;
+}
+
+static float preAltHoldComputeZCallOut(struct state_s* state) {
+ float result = state->targetZ;
+ /* Code that shall run BEFORE each altHold z target computation, should be placed here. */
+
+#if defined(SITAW_ENABLED)
+ /*
+ * The number of variables used for automatic Take-Off could be reduced, however that would
+ * cause debugging and tuning to become more difficult. The variables currently used ensure
+ * that tuning can easily be done through the LOG and PARAM frameworks.
+ *
+ * Note that while the automatic take-off function is active, it will overrule any other
+ * changes to targetZ by the user.
+ *
+ * The automatic take-off function will automatically deactivate once the take-off has been
+ * conducted.
+ */
+ if(!state->autoTOActive) {
+ /*
+ * Enabling automatic take-off: When At Rest, Not Tumbled, and the user pressing the AltHold button
+ */
+ if(sitAwARDetected() && !sitAwTuDetected() && state->justActivated) {
+ /* Enable automatic take-off. */
+ state->autoTOActive = true;
+ state->autoTOAltBase = state->targetZ;
+ state->autoTOAltCurrent = 0.0f;
+ }
+ }
+
+ if(state->autoTOActive) {
+ /*
+ * Automatic take-off is quite simple: Slowly increase targetZ until reaching the target altitude.
+ */
+
+ /* Calculate the new current setpoint for targetZ. autoTOAltCurrent is normalized to values from 0 to 1. */
+ state->autoTOAltCurrent = state->autoTOAltCurrent * state->autoTOAlpha + (1 - state->autoTOAlpha);
+
+ /* Update the targetZ variable. */
+ result = state->autoTOAltBase + state->autoTOAltCurrent * state->autoTOTargetAdjust;
+
+ if((state->autoTOAltCurrent >= state->autoTOThresh)) {
+ /* Disable the automatic take-off mode if target altitude has been reached. */
+ state->autoTOActive = false;
+ state->autoTOAltBase = 0.0f;
+ state->autoTOAltCurrent = 0.0f;
+ }
+ }
+#endif
+
+ return result;
+}
+
+
+LOG_GROUP_START(altHold)
+LOG_ADD(LOG_FLOAT, targetZ, &state.targetZ)
+LOG_GROUP_STOP(altHold)
+
+PARAM_GROUP_START(altHold)
+PARAM_ADD(PARAM_FLOAT, targetChangeSens, &state.targetChangeSens)
+PARAM_GROUP_STOP(altHold)
+
+
+#if defined(SITAW_ENABLED)
+// Automatic take-off logs
+LOG_GROUP_START(autoTO)
+LOG_ADD(LOG_UINT8, Active, &state.autoTOActive)
+LOG_ADD(LOG_FLOAT, AltBase, &state.autoTOAltBase)
+LOG_ADD(LOG_FLOAT, AltCurrent, &state.autoTOAltCurrent)
+LOG_GROUP_STOP(autoTO)
+
+// Automatic take-off parameters
+PARAM_GROUP_START(autoTO)
+PARAM_ADD(PARAM_FLOAT, TargetAdjust, &state.autoTOTargetAdjust)
+PARAM_ADD(PARAM_FLOAT, Thresh, &state.autoTOThresh)
+PARAM_ADD(PARAM_FLOAT, Alpha, &state.autoTOAlpha)
+PARAM_GROUP_STOP(autoTO)
+#endif
diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c
index 5c90a476ea..b9f7d2c6e4 100644
--- a/src/modules/src/commander.c
+++ b/src/modules/src/commander.c
@@ -219,11 +219,6 @@ void commanderGetAltHold(bool* altHold, bool* setAltHold, float* altHoldChange)
altHoldModeOld = altHoldMode;
}
-bool commanderGetAltHoldMode(void)
-{
- return (altHoldMode);
-}
-
void commanderSetAltHoldMode(bool altHoldModeNew)
{
altHoldMode = altHoldModeNew;
diff --git a/src/modules/src/position_controller_altitude.c b/src/modules/src/position_controller_altitude.c
index 3049c8c108..686bb250e3 100644
--- a/src/modules/src/position_controller_altitude.c
+++ b/src/modules/src/position_controller_altitude.c
@@ -30,7 +30,6 @@
#include "log.h"
#include "param.h"
#include "pid.h"
-#include "sitaw.h"
#include "num.h"
#include "position_controller.h"
@@ -38,7 +37,6 @@ struct state_s {
PidObject pid;
float targetZ; // Target altitude
- float targetChangeSens; // sensitivity of target altitude change (thrust input control) while hovering. Lower = more sensitive & faster changes
float pidInitKp;
float pidInitKi;
@@ -46,165 +44,56 @@ struct state_s {
uint16_t thrustBase; // approximate throttle needed when in perfect hover. More weight/older battery can use a higher value
float thrust; // The thrust from regulator, expressed as an offset to thrustBase
-
-
- #if defined(SITAW_ENABLED)
- // Automatic take-off variables
- bool autoTOActive; // Flag indicating if automatic take-off is active / deactive.
- float autoTOAltBase; // Base altitude for the automatic take-off. Set to targetZ when automatic take-off is activated.
- float autoTOAltCurrent; // Current target altitude adjustment. Equals 0 when function is activated, increases to autoTOThresh when function is deactivated.
- // Automatic take-off parameters
- float autoTOAlpha; // Smoothing factor when adjusting the targetZ altitude.
- float autoTOTargetAdjust; // Meters to add to targetZ to reach auto take-off altitude.
- float autoTOThresh; // Threshold for when to deactivate auto Take-Off. A value of 0.97 means 97% of the target altitude adjustment.
- #endif
};
static struct state_s state = {
.targetZ = -1,
- .targetChangeSens = 200,
.pidInitKp = 30000.0,
.pidInitKi = 0.0,
.pidInitKd = 10000.0,
.thrustBase = 32000,
-
- #if defined(SITAW_ENABLED)
- // Automatic take-off variables
- .autoTOActive = false,
- .autoTOAltBase = 0.0f,
- .autoTOAltCurrent = 0.0f,
-
- .autoTOAlpha = 0.98f,
- .autoTOTargetAdjust = 1.5f,
- .autoTOThresh = 0.97f,
- #endif
};
-static void stabilizerPreAltHoldComputeThrustCallOut(struct state_s* state, bool setAltHold);
-static void resetTarget(struct state_s* state, float newTargetZ, float dt);
-static void updateTargetForPilotControl(struct state_s* state, float altHoldChange);
static void positionPidControllerUpdateInternal(uint16_t* actuatorThrust, const estimate_t* estimate, float dt, struct state_s* state);
+static void positionControllerSetZTargetInternal(const setpointZ_t* setpoint, float dt, struct state_s* state);
void positionControllerUpdate(uint16_t* actuatorThrust, const estimate_t* estimate, float dt) {
positionPidControllerUpdateInternal(actuatorThrust, estimate, dt, &state);
}
-static void positionPidControllerUpdateInternal(uint16_t* actuatorThrust, const estimate_t* estimate, float dt, struct state_s* state) {
- bool isActive = false;
- bool justActivated = false;
- float pilotChange = 0;
-
- // Get altitude hold commands from pilot
- commanderGetAltHold(&isActive, &justActivated, &pilotChange);
-
- if (isActive) {
- // Altitude hold mode just activated
- if (justActivated) {
- resetTarget(state, estimate->position.z, dt);
- }
-
- updateTargetForPilotControl(state, pilotChange);
-
- // Call out before performing altHold thrust regulation.
- stabilizerPreAltHoldComputeThrustCallOut(state, justActivated);
-
- state->thrust = pidUpdate(&state->pid, estimate->position.z, true);
- *actuatorThrust = limitUint16(state->thrustBase + state->thrust);
- }
+void positionControllerSetZTarget(const setpointZ_t* setpoint, float dt) {
+ positionControllerSetZTargetInternal(setpoint, dt, &state);
}
-static void resetTarget(struct state_s* state, float newTargetZ, float dt) {
- state->targetZ = newTargetZ;
- pidInit(&state->pid, state->targetZ, state->pidInitKp, state->pidInitKi, state->pidInitKd, dt);
-}
-
-static void updateTargetForPilotControl(struct state_s* state, float pilotChange) {
- float delta = pilotChange / state->targetChangeSens;
- state->targetZ += delta;
- pidSetDesired(&state->pid, state->targetZ);
-}
-
-static void stabilizerPreAltHoldComputeThrustCallOut(struct state_s* state, bool justActivated) {
- /* Code that shall run BEFORE each altHold thrust computation, should be placed here. */
-
-#if defined(SITAW_ENABLED)
- /*
- * The number of variables used for automatic Take-Off could be reduced, however that would
- * cause debugging and tuning to become more difficult. The variables currently used ensure
- * that tuning can easily be done through the LOG and PARAM frameworks.
- *
- * Note that while the automatic take-off function is active, it will overrule any other
- * changes to targetZ by the user.
- *
- * The automatic take-off function will automatically deactivate once the take-off has been
- * conducted.
- */
- if(!state->autoTOActive) {
- /*
- * Enabling automatic take-off: When At Rest, Not Tumbled, and the user pressing the AltHold button
- */
- if(sitAwARDetected() && !sitAwTuDetected() && justActivated) {
- /* Enable automatic take-off. */
- state->autoTOActive = true;
- state->autoTOAltBase = state->targetZ;
- state->autoTOAltCurrent = 0.0f;
- }
+static void positionControllerSetZTargetInternal(const setpointZ_t* setpoint, float dt, struct state_s* state) {
+ state->targetZ = setpoint->z;
+ if (setpoint->isUpdate) {
+ pidSetDesired(&state->pid, state->targetZ);
+ } else {
+ pidInit(&state->pid, state->targetZ, state->pidInitKp, state->pidInitKi, state->pidInitKd, dt);
}
+}
- if(state->autoTOActive) {
- /*
- * Automatic take-off is quite simple: Slowly increase targetZ until reaching the target altitude.
- */
-
- /* Calculate the new current setpoint for targetZ. autoTOAltCurrent is normalized to values from 0 to 1. */
- state->autoTOAltCurrent = state->autoTOAltCurrent * state->autoTOAlpha + (1 - state->autoTOAlpha);
-
- /* Update the targetZ variable. */
- state->targetZ = state->autoTOAltBase + state->autoTOAltCurrent * state->autoTOTargetAdjust;
-
- if((state->autoTOAltCurrent >= state->autoTOThresh)) {
- /* Disable the automatic take-off mode if target altitude has been reached. */
- state->autoTOActive = false;
- state->autoTOAltBase = 0.0f;
- state->autoTOAltCurrent = 0.0f;
- }
- }
-#endif
+static void positionPidControllerUpdateInternal(uint16_t* actuatorThrust, const estimate_t* estimate, float dt, struct state_s* state) {
+ state->thrust = pidUpdate(&state->pid, estimate->position.z, true);
+ *actuatorThrust = limitUint16(state->thrustBase + state->thrust);
}
-LOG_GROUP_START(altHold)
+LOG_GROUP_START(posCtlAlt)
LOG_ADD(LOG_FLOAT, p, &state.pid.outP)
LOG_ADD(LOG_FLOAT, i, &state.pid.outI)
LOG_ADD(LOG_FLOAT, d, &state.pid.outD)
LOG_ADD(LOG_FLOAT, thrust, &state.thrust)
LOG_ADD(LOG_FLOAT, targetZ, &state.targetZ)
-LOG_GROUP_STOP(altHold)
+LOG_GROUP_STOP(posCtlAlt)
-PARAM_GROUP_START(altHold)
-PARAM_ADD(PARAM_FLOAT, targetChangeSens, &state.targetChangeSens)
+PARAM_GROUP_START(posCtlAlt)
PARAM_ADD(PARAM_FLOAT, pidInitKp, &state.pidInitKp)
PARAM_ADD(PARAM_FLOAT, pidInitKi, &state.pidInitKi)
PARAM_ADD(PARAM_FLOAT, pidInitKd, &state.pidInitKd)
PARAM_ADD(PARAM_UINT16, thrustBase, &state.thrustBase)
-PARAM_GROUP_STOP(altHold)
-
-
-#if defined(SITAW_ENABLED)
-// Automatic take-off logs
-LOG_GROUP_START(autoTO)
-LOG_ADD(LOG_UINT8, Active, &state.autoTOActive)
-LOG_ADD(LOG_FLOAT, AltBase, &state.autoTOAltBase)
-LOG_ADD(LOG_FLOAT, AltCurrent, &state.autoTOAltCurrent)
-LOG_GROUP_STOP(autoTO)
-
-// Automatic take-off parameters
-PARAM_GROUP_START(autoTO)
-PARAM_ADD(PARAM_FLOAT, TargetAdjust, &state.autoTOTargetAdjust)
-PARAM_ADD(PARAM_FLOAT, Thresh, &state.autoTOThresh)
-PARAM_ADD(PARAM_FLOAT, Alpha, &state.autoTOAlpha)
-PARAM_GROUP_STOP(autoTO)
-#endif
+PARAM_GROUP_STOP(posCtlAlt)
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 067240be04..2a2359be44 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -49,6 +49,7 @@
#include "num.h"
#include "position_estimator.h"
#include "position_controller.h"
+#include "altitude_hold.h"
/**
@@ -108,6 +109,8 @@ static void readBarometerData(float* pressure, float* temperature, float* asl);
static float temperature; // temp from barometer in celcius
static float pressure; // pressure from barometer in bar
static float asl; // raw Altitude over Sea Level from pressure sensor, in meters. Has an offset.
+static estimate_t estimatedPosition;
+
void stabilizerInit(void)
{
@@ -264,18 +267,22 @@ static void stabilizerTask(void* param)
if (++altHoldCounter >= ALTHOLD_UPDATE_RATE_DIVIDER)
{
if (imuHasBarometer()) {
+ // TODO krri add struct indicating whether we have baro or not
readBarometerData(&pressure, &temperature, &asl);
+ }
+ positionEstimate(&estimatedPosition, asl, ALTHOLD_UPDATE_DT);
+
+ if (altHoldIsActive()) {
+ setpointZ_t setpoint;
+ altHoldGetNewSetPoint(&setpoint, &estimatedPosition);
+ positionControllerSetZTarget(&setpoint, ALTHOLD_UPDATE_DT);
- estimate_t estimatedPosition;
- positionEstimate(&estimatedPosition, asl, ALTHOLD_UPDATE_DT);
positionControllerUpdate(&actuatorThrust, &estimatedPosition, ALTHOLD_UPDATE_DT);
+ } else {
+ commanderGetThrust(&actuatorThrust);
}
- altHoldCounter = 0;
- }
- if (!commanderGetAltHoldMode() || !imuHasBarometer())
- {
- commanderGetThrust(&actuatorThrust);
+ altHoldCounter = 0;
}
/* Call out before performing thrust updates, if any functions would like to influence the thrust. */