diff --git a/Makefile b/Makefile index 260445095a..1d57ff0fb0 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 controller.o sensfusion6.o stabilizer.o altitudehold.o +PROJ_OBJ += commander.o attitude_pid_controller.o sensfusion6.o stabilizer.o position_estimator_altitude.o position_controller_altitude.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/controller.h b/src/modules/interface/attitude_controller.h similarity index 78% rename from src/modules/interface/controller.h rename to src/modules/interface/attitude_controller.h index 59580c2b27..9335e621b9 100644 --- a/src/modules/interface/controller.h +++ b/src/modules/interface/attitude_controller.h @@ -21,17 +21,18 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . * + * attitude_controller.h: PID-based attitude controller */ -#ifndef CONTROLLER_H_ -#define CONTROLLER_H_ +#ifndef ATTITUDE_CONTROLLER_H_ +#define ATTITUDE_CONTROLLER_H_ #include #include "commander.h" -void controllerInit(void); -bool controllerTest(void); +void attitudeControllerInit(void); +bool attitudeControllerTest(void); /** * Make the controller run an update of the attitude PID. The output is @@ -39,7 +40,7 @@ bool controllerTest(void); * attitude controller can be run in a slower update rate then the rate * controller. */ -void controllerCorrectAttitudePID( +void attitudeControllerCorrectAttitudePID( float eulerRollActual, float eulerPitchActual, float eulerYawActual, float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired, float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired); @@ -48,19 +49,19 @@ void controllerCorrectAttitudePID( * Make the controller run an update of the rate PID. The output is * the actuator force. */ -void controllerCorrectRatePID( +void attitudeControllerCorrectRatePID( float rollRateActual, float pitchRateActual, float yawRateActual, float rollRateDesired, float pitchRateDesired, float yawRateDesired); /** * Reset controller roll, pitch and yaw PID's. */ -void controllerResetAllPID(void); +void attitudeControllerResetAllPID(void); /** * Get the actuator output. */ -void controllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw); +void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw); -#endif /* CONTROLLER_H_ */ +#endif /* ATTITUDE_CONTROLLER_H_ */ diff --git a/src/modules/interface/position_controller.h b/src/modules/interface/position_controller.h new file mode 100644 index 0000000000..818574bf42 --- /dev/null +++ b/src/modules/interface/position_controller.h @@ -0,0 +1,33 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 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 POSITION_CONTROLLER_H_ +#define POSITION_CONTROLLER_H_ + +#include "stabilizer_types.h" + +void positionControllerUpdate(uint16_t* actuatorThrust, const estimate_t* estimate, float dt); + +#endif /* POSITION_CONTROLLER_H_ */ diff --git a/src/modules/interface/altitudehold.h b/src/modules/interface/position_estimator.h similarity index 81% rename from src/modules/interface/altitudehold.h rename to src/modules/interface/position_estimator.h index 07cff3b155..c76818fa5b 100644 --- a/src/modules/interface/altitudehold.h +++ b/src/modules/interface/position_estimator.h @@ -23,9 +23,11 @@ * * */ -#ifndef ALTITUDE_HOLD_H_ -#define ALTITUDE_HOLD_H_ +#ifndef POSITION_ESTIMATOR_H_ +#define POSITION_ESTIMATOR_H_ -void altHoldUpdate(uint16_t* actuatorThrust, float asl, float velocityZ, float dt); +#include "stabilizer_types.h" -#endif /* ALTITUDE_HOLD_H_ */ +void positionEstimate(estimate_t* estimate, float asl, float velocityZ, float dt); + +#endif /* POSITION_ESTIMATOR_H_ */ diff --git a/src/modules/interface/stabilizer.h b/src/modules/interface/stabilizer.h index 23249d5ad1..96537c18cd 100644 --- a/src/modules/interface/stabilizer.h +++ b/src/modules/interface/stabilizer.h @@ -21,12 +21,13 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . * - * + * stabilizer.h: Stabilizer orchestrator */ #ifndef STABALIZER_H_ #define STABALIZER_H_ #include +#include void stabilizerInit(void); diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index 3c61aa0c06..99ac4cfcbd 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -70,4 +70,11 @@ typedef struct quaternion_s { }; } quaternion_t; +/** Estimate of position */ +typedef struct estimate_s { + uint32_t timestamp; // Timestamp when the data was computed + + point_t position; +} estimate_t; + #endif diff --git a/src/modules/src/altitudehold.c b/src/modules/src/altitudehold.c deleted file mode 100644 index 5236b99e85..0000000000 --- a/src/modules/src/altitudehold.c +++ /dev/null @@ -1,198 +0,0 @@ -/** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ - * | 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 . - * - * - */ -#include - -#include "FreeRTOS.h" -#include "pm.h" -#include "commander.h" -#include "imu.h" -#include "log.h" -#include "pid.h" -#include "param.h" -#include "sitaw.h" -#ifdef PLATFORM_CF1 - #include "ms5611.h" -#else - #include "lps25h.h" -#endif -#include "num.h" - -static PidObject pid; - -static float estimatedZ = 0.0; // The current Z estimate, has same offset as asl -static float estAlpha = 0.99; -static float targetZ = -1; // Target altitude -static float targetChangeSens = 200; // sensitivity of target altitude change (thrust input control) while hovering. Lower = more sensitive & faster changes -static float velocityFactor = 1.0; - -static float pidInitKp = 30000.0; -static float pidInitKi = 0.0; -static float pidInitKd = 10000.0; - -static uint16_t thrustBase = 32000; // approximate throttle needed when in perfect hover. More weight/older battery can use a higher value -static float thrust; // The thrust from regulator, expressed as an offset to thrustBase - - -#if defined(SITAW_ENABLED) -// Automatic take-off variables -static bool autoTOActive = false; // Flag indicating if automatic take-off is active / deactive. -static float autoTOAltBase = 0.0f; // Base altitude for the automatic take-off. Set to targetZ when automatic take-off is activated. -static float autoTOAltCurrent = 0.0f; // Current target altitude adjustment. Equals 0 when function is activated, increases to autoTOThresh when function is deactivated. -// Automatic take-off parameters -static float autoTOAlpha = 0.98f; // Smoothing factor when adjusting the targetZ altitude. -static float autoTOTargetAdjust = 1.5f; // Meters to add to targetZ to reach auto take-off altitude. -static float autoTOThresh = 0.97f; // Threshold for when to deactivate auto Take-Off. A value of 0.97 means 97% of the target altitude adjustment. -#endif - - -static void stabilizerPreAltHoldComputeThrustCallOut(bool setAltHold); -static void resetTarget(float newTargetZ, float dt); -static void updateTargetForPilotControl(float altHoldChange); - -void altHoldUpdate(uint16_t* actuatorThrust, float asl, float velocityZ, float dt) { - bool isActive = false; - bool justActivated = false; - float pilotChange = 0; - - // Get altitude hold commands from pilot - commanderGetAltHold(&isActive, &justActivated, &pilotChange); - - // Calculate new estimate. LP measurement from baro and add delta based on speed from accelerometers - estimatedZ = estAlpha * estimatedZ + (1.0 - estAlpha) * asl + - velocityFactor * velocityZ * dt; - - // Altitude hold mode just activated - if (justActivated) { - resetTarget(estimatedZ, dt); - } - - // Call out before performing altHold thrust regulation. - stabilizerPreAltHoldComputeThrustCallOut(justActivated); - - if (isActive) { - updateTargetForPilotControl(pilotChange); - - thrust = pidUpdate(&pid, estimatedZ, true); - *actuatorThrust = limitUint16(thrustBase + thrust); - } -} - -static void resetTarget(float newTargetZ, float dt) { - targetZ = newTargetZ; - pidInit(&pid, targetZ, pidInitKp, pidInitKi, pidInitKd, dt); -} - -static void updateTargetForPilotControl(float pilotChange) { - float delta = pilotChange / targetChangeSens; - targetZ += delta; - pidSetDesired(&pid, targetZ); -} - -static void stabilizerPreAltHoldComputeThrustCallOut(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(!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. */ - autoTOActive = true; - autoTOAltBase = targetZ; - autoTOAltCurrent = 0.0f; - } - } - - if(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. */ - autoTOAltCurrent = autoTOAltCurrent * autoTOAlpha + (1 - autoTOAlpha); - - /* Update the targetZ variable. */ - targetZ = autoTOAltBase + autoTOAltCurrent * autoTOTargetAdjust; - - if((autoTOAltCurrent >= autoTOThresh)) { - /* Disable the automatic take-off mode if target altitude has been reached. */ - autoTOActive = false; - autoTOAltBase = 0.0f; - autoTOAltCurrent = 0.0f; - } - } -#endif -} - - -LOG_GROUP_START(altHold) -LOG_ADD(LOG_FLOAT, p, &pid.outP) -LOG_ADD(LOG_FLOAT, i, &pid.outI) -LOG_ADD(LOG_FLOAT, d, &pid.outD) -LOG_ADD(LOG_FLOAT, thrust, &thrust) - -LOG_ADD(LOG_FLOAT, targetZ, &targetZ) -LOG_ADD(LOG_FLOAT, estimatedZ, &estimatedZ) -LOG_GROUP_STOP(altHold) - -PARAM_GROUP_START(altHold) -PARAM_ADD(PARAM_FLOAT, targetChangeSens, &targetChangeSens) -PARAM_ADD(PARAM_FLOAT, pidInitKp, &pidInitKp) -PARAM_ADD(PARAM_FLOAT, pidInitKi, &pidInitKi) -PARAM_ADD(PARAM_FLOAT, pidInitKd, &pidInitKd) -PARAM_ADD(PARAM_UINT16, thrustBase, &thrustBase) -PARAM_ADD(PARAM_FLOAT, estAlpha, &estAlpha) -PARAM_ADD(PARAM_FLOAT, velocityFactor, &velocityFactor) -PARAM_GROUP_STOP(altHold) - - -#if defined(SITAW_ENABLED) -// Automatic take-off logs -LOG_GROUP_START(autoTO) -LOG_ADD(LOG_UINT8, Active, &autoTOActive) -LOG_ADD(LOG_FLOAT, AltBase, &autoTOAltBase) -LOG_ADD(LOG_FLOAT, AltCurrent, &autoTOAltCurrent) -LOG_GROUP_STOP(autoTO) - -// Automatic take-off parameters -PARAM_GROUP_START(autoTO) -PARAM_ADD(PARAM_FLOAT, TargetAdjust, &autoTOTargetAdjust) -PARAM_ADD(PARAM_FLOAT, Thresh, &autoTOThresh) -PARAM_ADD(PARAM_FLOAT, Alpha, &autoTOAlpha) -PARAM_GROUP_STOP(autoTO) -#endif diff --git a/src/modules/src/controller.c b/src/modules/src/attitude_pid_controller.c similarity index 90% rename from src/modules/src/controller.c rename to src/modules/src/attitude_pid_controller.c index 91d236e4d4..a041da2a7b 100644 --- a/src/modules/src/controller.c +++ b/src/modules/src/attitude_pid_controller.c @@ -21,13 +21,13 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . * - * + * attitude_pid_controller.c: Attitude controler using PID correctors */ #include - + #include "FreeRTOS.h" -#include "controller.h" +#include "attitude_controller.h" #include "pid.h" #include "param.h" #include "imu.h" @@ -56,11 +56,11 @@ int16_t yawOutput; static bool isInit; -void controllerInit() +void attitudeControllerInit() { if(isInit) return; - + //TODO: get parameters from configuration manager instead pidInit(&pidRollRate, 0, PID_ROLL_RATE_KP, PID_ROLL_RATE_KI, PID_ROLL_RATE_KD, IMU_UPDATE_DT); pidInit(&pidPitchRate, 0, PID_PITCH_RATE_KP, PID_PITCH_RATE_KI, PID_PITCH_RATE_KD, IMU_UPDATE_DT); @@ -75,16 +75,16 @@ void controllerInit() pidSetIntegralLimit(&pidRoll, PID_ROLL_INTEGRATION_LIMIT); pidSetIntegralLimit(&pidPitch, PID_PITCH_INTEGRATION_LIMIT); pidSetIntegralLimit(&pidYaw, PID_YAW_INTEGRATION_LIMIT); - + isInit = true; } -bool controllerTest() +bool attitudeControllerTest() { return isInit; } -void controllerCorrectRatePID( +void attitudeControllerCorrectRatePID( float rollRateActual, float pitchRateActual, float yawRateActual, float rollRateDesired, float pitchRateDesired, float yawRateDesired) { @@ -98,7 +98,7 @@ void controllerCorrectRatePID( yawOutput = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, true)); } -void controllerCorrectAttitudePID( +void attitudeControllerCorrectAttitudePID( float eulerRollActual, float eulerPitchActual, float eulerYawActual, float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired, float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired) @@ -121,7 +121,7 @@ void controllerCorrectAttitudePID( *yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, false); } -void controllerResetAllPID(void) +void attitudeControllerResetAllPID(void) { pidReset(&pidRoll); pidReset(&pidPitch); @@ -131,7 +131,7 @@ void controllerResetAllPID(void) pidReset(&pidYawRate); } -void controllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw) +void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw) { *roll = rollOutput; *pitch = pitchOutput; diff --git a/src/modules/src/param.c b/src/modules/src/param.c index d5a47630ea..24fff9b20f 100644 --- a/src/modules/src/param.c +++ b/src/modules/src/param.c @@ -195,9 +195,10 @@ void paramTOCProcess(int command) p.data[0]=CMD_GET_ITEM; p.data[1]=n; p.data[2]=params[ptr].type; + p.size=3+2+strlen(group)+strlen(params[ptr].name); + ASSERT(p.size <= CRTP_MAX_DATA_SIZE); // Too long! The name of the group or the parameter may be too long. memcpy(p.data+3, group, strlen(group)+1); memcpy(p.data+3+strlen(group)+1, params[ptr].name, strlen(params[ptr].name)+1); - p.size=3+2+strlen(group)+strlen(params[ptr].name); crtpSendPacket(&p); } else { p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH); diff --git a/src/modules/src/position_controller_altitude.c b/src/modules/src/position_controller_altitude.c new file mode 100644 index 0000000000..3049c8c108 --- /dev/null +++ b/src/modules/src/position_controller_altitude.c @@ -0,0 +1,210 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 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 + */ + +#include + +#include "commander.h" +#include "log.h" +#include "param.h" +#include "pid.h" +#include "sitaw.h" +#include "num.h" +#include "position_controller.h" + +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; + float pidInitKd; + + 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); + +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); + } +} + +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; + } + } + + 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 +} + + +LOG_GROUP_START(altHold) +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) + +PARAM_GROUP_START(altHold) +PARAM_ADD(PARAM_FLOAT, targetChangeSens, &state.targetChangeSens) +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 diff --git a/src/modules/src/position_estimator_altitude.c b/src/modules/src/position_estimator_altitude.c new file mode 100644 index 0000000000..fcf3cd0f06 --- /dev/null +++ b/src/modules/src/position_estimator_altitude.c @@ -0,0 +1,67 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 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 + */ + +#include "log.h" +#include "param.h" +#include "position_estimator.h" + +struct state_s { + float estimatedZ; // The current Z estimate, has same offset as asl + float estAlpha; + float velocityFactor; +}; + +static struct state_s state = { + .estimatedZ = 0.0, + .estAlpha = 0.99, + .velocityFactor = 1.0, +}; + +static void positionEstimateInternal(estimate_t* estimate, float asl, float velocityZ, float dt, struct state_s* state); + +void positionEstimate(estimate_t* estimate, float asl, float velocityZ, float dt) { + positionEstimateInternal(estimate, asl, velocityZ, dt, &state); +} + +static void positionEstimateInternal(estimate_t* estimate, float asl, float velocityZ, float dt, struct state_s* state) { + state->estimatedZ = state->estAlpha * state->estimatedZ + + (1.0 - state->estAlpha) * asl + + state->velocityFactor * velocityZ * dt; + + estimate->position.x = 0.0; + estimate->position.y = 0.0; + estimate->position.z = state->estimatedZ; +} + + +LOG_GROUP_START(posEstimatorAlt) +LOG_ADD(LOG_FLOAT, estimatedZ, &state.estimatedZ) +LOG_GROUP_STOP(posEstimatorAlt) + +PARAM_GROUP_START(posEst) +PARAM_ADD(PARAM_FLOAT, estAlpha, &state.estAlpha) +PARAM_ADD(PARAM_FLOAT, velFactor, &state.velocityFactor) +PARAM_GROUP_STOP(posEstimatorAlt) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index f5604d8ba0..ba402448ec 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -33,7 +33,7 @@ #include "pm.h" #include "stabilizer.h" #include "commander.h" -#include "controller.h" +#include "attitude_controller.h" #include "sensfusion6.h" #include "imu.h" #include "motors.h" @@ -47,7 +47,8 @@ #include "lps25h.h" #endif #include "num.h" -#include "altitudehold.h" +#include "position_estimator.h" +#include "position_controller.h" /** @@ -123,7 +124,7 @@ void stabilizerInit(void) motorsInit(motorMapDefaultBrushed); imu6Init(); sensfusion6Init(); - controllerInit(); + attitudeControllerInit(); #if defined(SITAW_ENABLED) sitAwInit(); #endif @@ -145,7 +146,7 @@ bool stabilizerTest(void) pass &= motorsTest(); pass &= imu6Test(); pass &= sensfusion6Test(); - pass &= controllerTest(); + pass &= attitudeControllerTest(); return pass; } @@ -244,7 +245,7 @@ static void stabilizerTask(void* param) // Adjust yaw if configured to do so stabilizerYawModeUpdate(); - controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual, + attitudeControllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual, eulerRollDesired, eulerPitchDesired, -eulerYawDesired, &rollRateDesired, &pitchRateDesired, &yawRateDesired); attitudeCounter = 0; @@ -263,17 +264,20 @@ static void stabilizerTask(void* param) } // TODO: Investigate possibility to subtract gyro drift. - controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z, + attitudeControllerCorrectRatePID(gyro.x, -gyro.y, gyro.z, rollRateDesired, pitchRateDesired, yawRateDesired); - controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw); + attitudeControllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw); // 100HZ if (++altHoldCounter >= ALTHOLD_UPDATE_RATE_DIVIDER) { if (imuHasBarometer()) { readBarometerData(&pressure, &temperature, &asl); - altHoldUpdate(&actuatorThrust, asl, velocityZ, ALTHOLD_UPDATE_DT); + + estimate_t estimatedPosition; + positionEstimate(&estimatedPosition, asl, velocityZ, ALTHOLD_UPDATE_DT); + positionControllerUpdate(&actuatorThrust, &estimatedPosition, ALTHOLD_UPDATE_DT); } altHoldCounter = 0; } @@ -303,7 +307,7 @@ static void stabilizerTask(void* param) else { distributePower(0, 0, 0, 0); - controllerResetAllPID(); + attitudeControllerResetAllPID(); // Reset the calculated YAW angle for rate control yawRateAngle = eulerYawActual;