From 16f68453c63e83f7676b7b99d1ab56e473ae32ad Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Fri, 11 Nov 2016 09:07:43 -0800 Subject: [PATCH 1/7] pid: Tweak when integration is capped. Remove errorMax and use outputLimit instead. --- src/modules/interface/pid.h | 6 ++-- src/modules/src/pid.c | 63 ++++++++++++++++--------------------- 2 files changed, 29 insertions(+), 40 deletions(-) diff --git a/src/modules/interface/pid.h b/src/modules/interface/pid.h index bd8ba837f2..54a94e27de 100644 --- a/src/modules/interface/pid.h +++ b/src/modules/interface/pid.h @@ -104,10 +104,9 @@ typedef struct { - float desired; //< set point + float desired; //< set point float error; //< error float prevError; //< previous error - float errorMax; //< maximum error float integ; //< integral float deriv; //< derivative float kp; //< proportional gain @@ -117,8 +116,7 @@ typedef struct float outI; //< integral output (debugging) float outD; //< derivative output (debugging) float iLimit; //< integral limit - float iLimitLow; //< integral limit - bool iCapped; //< true to stop integration + float outputLimit; //< output limit float dt; //< delta-time dt lpf2pData dFilter; //< filter for D term bool enableDFilter; //< filter for D term enable flag diff --git a/src/modules/src/pid.c b/src/modules/src/pid.c index 0535b502fd..c82a0e0a25 100644 --- a/src/modules/src/pid.c +++ b/src/modules/src/pid.c @@ -34,19 +34,17 @@ void pidInit(PidObject* pid, const float desired, const float kp, const float samplingRate, const float cutoffFreq, bool enableDFilter) { - pid->error = 0; - pid->errorMax = 0.0f; - pid->prevError = 0; - pid->integ = 0; - pid->deriv = 0; - pid->desired = desired; - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->iLimit = DEFAULT_PID_INTEGRATION_LIMIT; - pid->iLimitLow = -DEFAULT_PID_INTEGRATION_LIMIT; - pid->iCapped = false; - pid->dt = dt; + pid->error = 0; + pid->prevError = 0; + pid->integ = 0; + pid->deriv = 0; + pid->desired = desired; + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->iLimit = DEFAULT_PID_INTEGRATION_LIMIT; + pid->outputLimit = DEFAULT_PID_INTEGRATION_LIMIT; + pid->dt = dt; pid->enableDFilter = enableDFilter; if (pid->enableDFilter) { @@ -56,28 +54,15 @@ void pidInit(PidObject* pid, const float desired, const float kp, float pidUpdate(PidObject* pid, const float measured, const bool updateError) { - float output; + float output = 0.0f; if (updateError) { pid->error = pid->desired - measured; - if (pid->errorMax > FLT_MIN) { - float errorMaxScaled = pid->errorMax / pid->kp; - pid->error = constrain(pid->error, -errorMaxScaled, errorMaxScaled); - } } - if (pid->iCapped == false) { - pid->integ += pid->error * pid->dt; - if (pid->integ > pid->iLimit) - { - pid->integ = pid->iLimit; - } - else if (pid->integ < pid->iLimitLow) - { - pid->integ = pid->iLimitLow; - } - } + pid->outP = pid->kp * pid->error; + output += pid->outP; float deriv = (pid->error - pid->prevError) / pid->dt; if (pid->enableDFilter) @@ -86,12 +71,22 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError) } else { pid->deriv = deriv; } + if (isnan(pid->deriv)) { + pid->deriv = 0; + } + pid->outD = pid->kd * pid->deriv; + output += pid->outD; + + float i = pid->integ + pid->error * pid->dt; + // Check if integral is saturated + if (abs(i) <= pid->iLimit || abs(pid->ki * i + output) <= pid->outputLimit) { + pid->integ = i; + } - pid->outP = pid->kp * pid->error; pid->outI = pid->ki * pid->integ; - pid->outD = pid->kd * pid->deriv; + output += pid->outI; - output = pid->outP + pid->outI + pid->outD; + output = constrain(output, -pid->outputLimit, pid->outputLimit); pid->prevError = pid->error; @@ -103,10 +98,6 @@ void pidSetIntegralLimit(PidObject* pid, const float limit) { } -void pidSetIntegralLimitLow(PidObject* pid, const float limitLow) { - pid->iLimitLow = limitLow; -} - void pidReset(PidObject* pid) { pid->error = 0; From 9c578e158b2c69530207434e60a3b7284087df27 Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Fri, 11 Nov 2016 10:42:22 -0800 Subject: [PATCH 2/7] num: Add an isnan macro. --- src/utils/interface/num.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/utils/interface/num.h b/src/utils/interface/num.h index 033198ed22..cb7b59f34e 100644 --- a/src/utils/interface/num.h +++ b/src/utils/interface/num.h @@ -37,6 +37,9 @@ #undef abs #define abs(a) ((a) > 0 ? (a) : (-1*(a))) +#undef isnan +#define isnan(n) ((n != n) ? 1 : 0) + uint16_t single2half(float number); float half2single(uint16_t number); From d2b0c53fa78e543e9db525a8291754aec99e9a69 Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Fri, 11 Nov 2016 10:45:38 -0800 Subject: [PATCH 3/7] position_controller: Set proper caps on the velocity pid and add xy & z velocity max as a parameter. --- src/modules/src/position_controller_pid.c | 44 +++++++++++------------ 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/src/modules/src/position_controller_pid.c b/src/modules/src/position_controller_pid.c index c03883dd22..df58ab9281 100644 --- a/src/modules/src/position_controller_pid.c +++ b/src/modules/src/position_controller_pid.c @@ -65,10 +65,12 @@ struct this_s { // Maximum roll/pitch angle permited static float rpLimit = 20; +static float rpLimitOverhead = 1.10f; // Velocity maximums static float xyVelMax = 1.0f; static float zVelMax = 1.0f; static float velMaxOverhead = 1.10f; +static const float thrustScale = 1000.0f; #define DT (float)(1.0f/POSITION_RATE) #define POSITION_LPF_CUTOFF_FREQ 20.0f @@ -96,8 +98,8 @@ static struct this_s this = { .pidVZ = { .init = { - .kp = 25, - .ki = 15, + .kp = 15, + .ki = 20, .kd = 0, }, .pid.dt = DT, @@ -125,7 +127,7 @@ static struct this_s this = { .init = { .kp = 2.0f, .ki = 0, - .kd = 0.01f, + .kd = 0, }, .pid.dt = DT, }, @@ -150,10 +152,6 @@ void positionControllerInit() this.pidVY.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE); pidInit(&this.pidVZ.pid, this.pidVZ.setpoint, this.pidVZ.init.kp, this.pidVZ.init.ki, this.pidVZ.init.kd, this.pidVZ.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE); - - this.pidX.pid.errorMax = xyVelMax * velMaxOverhead; - this.pidY.pid.errorMax = xyVelMax * velMaxOverhead; - this.pidZ.pid.errorMax = zVelMax * velMaxOverhead; } static float runPid(float input, struct pidAxis_s *axis, float setpoint, float dt) { @@ -166,6 +164,12 @@ static float runPid(float input, struct pidAxis_s *axis, float setpoint, float d void positionController(float* thrust, attitude_t *attitude, const state_t *state, const setpoint_t *setpoint) { + this.pidX.pid.outputLimit = xyVelMax * velMaxOverhead; + this.pidY.pid.outputLimit = xyVelMax * velMaxOverhead; + // The ROS landing detector will prematurely trip if + // this value is below 0.5 + this.pidZ.pid.outputLimit = max(zVelMax, 0.5) * velMaxOverhead; + // X, Y pidSetDesired(&this.pidVX.pid, runPid(state->position.x, &this.pidX, setpoint->position.x, DT)); pidSetDesired(&this.pidVY.pid, runPid(state->position.y, &this.pidY, setpoint->position.y, DT)); @@ -177,6 +181,12 @@ void positionController(float* thrust, attitude_t *attitude, const state_t *stat void velocityController(float* thrust, attitude_t *attitude, const state_t *state, const setpoint_t *setpoint) { + this.pidVX.pid.outputLimit = rpLimit * rpLimitOverhead; + this.pidVY.pid.outputLimit = rpLimit * rpLimitOverhead; + // Set the output limit to the maximum thrust range + this.pidVZ.pid.outputLimit = (UINT16_MAX / 2 / thrustScale); + //this.pidVZ.pid.outputLimit = (this.thrustBase - this.thrustMin) / thrustScale; + // Roll and Pitch float rollRaw = pidUpdate(&this.pidVX.pid, state->velocity.x, true); float pitchRaw = pidUpdate(&this.pidVY.pid, state->velocity.y, true); @@ -185,28 +195,16 @@ void velocityController(float* thrust, attitude_t *attitude, const state_t *stat attitude->pitch = -(rollRaw * cosf(yawRad)) - (pitchRaw * sinf(yawRad)); attitude->roll = -(pitchRaw * cosf(yawRad)) + (rollRaw * sinf(yawRad)); - // Check for roll/pitch limits and prevent I from accumulating when capped - if (abs(attitude->roll) > rpLimit || abs(attitude->pitch) > rpLimit) { - this.pidVX.pid.iCapped = true; - this.pidVY.pid.iCapped = true; - } else { - this.pidVX.pid.iCapped = false; - this.pidVY.pid.iCapped = false; - } - attitude->roll = constrain(attitude->roll, -rpLimit, rpLimit); attitude->pitch = constrain(attitude->pitch, -rpLimit, rpLimit); // Thrust float thrustRaw = pidUpdate(&this.pidVZ.pid, state->velocity.z, true); // Scale the thrust and add feed forward term - *thrust = thrustRaw*1000 + this.thrustBase; - // Check for minimum thrust, prevent I from accumulating when capped + *thrust = thrustRaw*thrustScale + this.thrustBase; + // Check for minimum thrust if (*thrust < this.thrustMin) { *thrust = this.thrustMin; - this.pidVZ.pid.iCapped = true; - } else { - this.pidVZ.pid.iCapped = false; } } @@ -281,6 +279,8 @@ PARAM_ADD(PARAM_FLOAT, zKd, &this.pidZ.pid.kd) PARAM_ADD(PARAM_UINT16, thrustBase, &this.thrustBase) PARAM_ADD(PARAM_UINT16, thrustMin, &this.thrustMin) -PARAM_ADD(PARAM_FLOAT, rpLimit, &rpLimit) +PARAM_ADD(PARAM_FLOAT, rpLimit, &rpLimit) +PARAM_ADD(PARAM_FLOAT, xyVelMax, &xyVelMax) +PARAM_ADD(PARAM_FLOAT, zVelMax, &zVelMax) PARAM_GROUP_STOP(posCtlPid) From 8c97090409454cf0f0844fcc56368961f668c643 Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Fri, 11 Nov 2016 10:52:38 -0800 Subject: [PATCH 4/7] attitude_pid_controller: Add roll, pitch, and yaw rate limits to the pid controller. --- src/modules/src/attitude_pid_controller.c | 45 +++++++++++++++++++++-- 1 file changed, 42 insertions(+), 3 deletions(-) diff --git a/src/modules/src/attitude_pid_controller.c b/src/modules/src/attitude_pid_controller.c index b95d73d42d..860923b391 100644 --- a/src/modules/src/attitude_pid_controller.c +++ b/src/modules/src/attitude_pid_controller.c @@ -30,6 +30,7 @@ #include "attitude_controller.h" #include "pid.h" #include "param.h" +#include "log.h" #define ATTITUDE_LPF_CUTOFF_FREQ 15.0f #define ATTITUDE_LPF_ENABLE false @@ -55,9 +56,13 @@ PidObject pidRoll; PidObject pidPitch; PidObject pidYaw; -int16_t rollOutput; -int16_t pitchOutput; -int16_t yawOutput; +static int16_t rollOutput; +static int16_t pitchOutput; +static int16_t yawOutput; + +static float rpRateLimit = 150.0f; +static float yawRateLimit = 150.0f; +static float rpyRateLimitOverhead = 1.1f; static bool isInit; @@ -89,6 +94,10 @@ void attitudeControllerInit(const float updateDt) pidSetIntegralLimit(&pidPitch, PID_PITCH_INTEGRATION_LIMIT); pidSetIntegralLimit(&pidYaw, PID_YAW_INTEGRATION_LIMIT); + pidRollRate.outputLimit = INT16_MAX; + pidPitchRate.outputLimit = INT16_MAX; + pidYawRate.outputLimit = INT16_MAX; + isInit = true; } @@ -116,6 +125,10 @@ void attitudeControllerCorrectAttitudePID( float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired, float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired) { + pidRoll.outputLimit = rpRateLimit * rpyRateLimitOverhead; + pidPitch.outputLimit = rpRateLimit * rpyRateLimitOverhead; + pidYaw.outputLimit = yawRateLimit * rpyRateLimitOverhead; + pidSetDesired(&pidRoll, eulerRollDesired); *rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, true); @@ -151,6 +164,30 @@ void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* *yaw = yawOutput; } +LOG_GROUP_START(pid_attitude) +LOG_ADD(LOG_FLOAT, roll_outP, &pidRoll.outP) +LOG_ADD(LOG_FLOAT, roll_outI, &pidRoll.outI) +LOG_ADD(LOG_FLOAT, roll_outD, &pidRoll.outD) +LOG_ADD(LOG_FLOAT, pitch_outP, &pidPitch.outP) +LOG_ADD(LOG_FLOAT, pitch_outI, &pidPitch.outI) +LOG_ADD(LOG_FLOAT, pitch_outD, &pidPitch.outD) +LOG_ADD(LOG_FLOAT, yaw_outP, &pidYaw.outP) +LOG_ADD(LOG_FLOAT, yaw_outI, &pidYaw.outI) +LOG_ADD(LOG_FLOAT, yaw_outD, &pidYaw.outD) +LOG_GROUP_STOP(pid_attitude) + +LOG_GROUP_START(pid_rate) +LOG_ADD(LOG_FLOAT, roll_outP, &pidRollRate.outP) +LOG_ADD(LOG_FLOAT, roll_outI, &pidRollRate.outI) +LOG_ADD(LOG_FLOAT, roll_outD, &pidRollRate.outD) +LOG_ADD(LOG_FLOAT, pitch_outP, &pidPitchRate.outP) +LOG_ADD(LOG_FLOAT, pitch_outI, &pidPitchRate.outI) +LOG_ADD(LOG_FLOAT, pitch_outD, &pidPitchRate.outD) +LOG_ADD(LOG_FLOAT, yaw_outP, &pidYawRate.outP) +LOG_ADD(LOG_FLOAT, yaw_outI, &pidYawRate.outI) +LOG_ADD(LOG_FLOAT, yaw_outD, &pidYawRate.outD) +LOG_GROUP_STOP(pid_rate) + PARAM_GROUP_START(pid_attitude) PARAM_ADD(PARAM_FLOAT, roll_kp, &pidRoll.kp) PARAM_ADD(PARAM_FLOAT, roll_ki, &pidRoll.ki) @@ -164,6 +201,8 @@ PARAM_ADD(PARAM_FLOAT, yaw_kd, &pidYaw.kd) PARAM_GROUP_STOP(pid_attitude) PARAM_GROUP_START(pid_rate) +PARAM_ADD(PARAM_FLOAT, rpRateLimit, &rpRateLimit) +PARAM_ADD(PARAM_FLOAT, yawRateLimit, &yawRateLimit) PARAM_ADD(PARAM_FLOAT, roll_kp, &pidRollRate.kp) PARAM_ADD(PARAM_FLOAT, roll_ki, &pidRollRate.ki) PARAM_ADD(PARAM_FLOAT, roll_kd, &pidRollRate.kd) From 1319a8c75a96fe4d80cca68c8458cbfdee2bb7f0 Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Fri, 11 Nov 2016 11:01:40 -0800 Subject: [PATCH 5/7] commander: Change commander packet to be a float instead. Rework ordering of when thrust command is sent. --- src/modules/interface/commander.h | 2 +- src/modules/src/commander.c | 54 +++++++++++++------------------ 2 files changed, 24 insertions(+), 32 deletions(-) diff --git a/src/modules/interface/commander.h b/src/modules/interface/commander.h index b7f4ce7aed..ec169bf8b9 100644 --- a/src/modules/interface/commander.h +++ b/src/modules/interface/commander.h @@ -47,7 +47,7 @@ struct CommanderCrtpValues float roll; // deg float pitch; // deg float yaw; // deg - uint16_t thrust; + float thrust; } __attribute__((packed)); void commanderInit(void); diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c index 2cce73280b..84505127c9 100644 --- a/src/modules/src/commander.c +++ b/src/modules/src/commander.c @@ -37,7 +37,8 @@ #include "debug.h" #include "ext_position.h" -#define MIN_THRUST 1000 +#define MIN_THRUST 0 +#define IDLE_THRUST 1000 #define MAX_THRUST 60000 /** @@ -77,7 +78,6 @@ static CommanderCache* activeCache; static uint32_t lastUpdate; static bool isInactive; -static bool thrustLocked; static bool altHoldMode = false; static bool posHoldMode = false; static bool posSetMode = false; @@ -93,7 +93,7 @@ static void commanderCrtpCB(CRTPPacket* pk); static void commanderCacheSelectorUpdate(void); /* Private functions */ -static void commanderSetActiveThrust(uint16_t thrust) +static void commanderSetActiveThrust(float thrust) { activeCache->targetVal[activeCache->activeSide].thrust = thrust; } @@ -113,7 +113,7 @@ static void commanderSetActiveYaw(float yaw) activeCache->targetVal[activeCache->activeSide].yaw = yaw; } -static uint16_t commanderGetActiveThrust(void) +static float commanderGetActiveThrust(void) { commanderCacheSelectorUpdate(); return activeCache->targetVal[activeCache->activeSide].thrust; @@ -144,6 +144,8 @@ static void commanderLevelRPY(void) static void commanderDropToGround(void) { altHoldMode = false; + posHoldMode = false; + posSetMode = false; commanderSetActiveThrust(0); commanderLevelRPY(); } @@ -174,10 +176,6 @@ static void commanderCrtpCB(CRTPPacket* pk) crtpCache.targetVal[!crtpCache.activeSide] = *((struct CommanderCrtpValues*)pk->data); crtpCache.activeSide = !crtpCache.activeSide; crtpCache.timestamp = xTaskGetTickCount(); - - if (crtpCache.targetVal[crtpCache.activeSide].thrust == 0) { - thrustLocked = false; - } } /** @@ -266,7 +264,6 @@ void commanderInit(void) activeCache = &crtpCache; lastUpdate = xTaskGetTickCount(); isInactive = true; - thrustLocked = true; isInit = true; } @@ -281,10 +278,6 @@ void commanderExtrxSet(const struct CommanderCrtpValues *val) extrxCache.targetVal[!extrxCache.activeSide] = *((struct CommanderCrtpValues*)val); extrxCache.activeSide = !extrxCache.activeSide; extrxCache.timestamp = xTaskGetTickCount(); - - if (extrxCache.targetVal[extrxCache.activeSide].thrust == 0) { - thrustLocked = false; - } } uint32_t commanderGetInactivityTime(void) @@ -295,35 +288,24 @@ uint32_t commanderGetInactivityTime(void) void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state) { // Thrust - uint16_t rawThrust = commanderGetActiveThrust(); - - if (thrustLocked || (rawThrust < MIN_THRUST)) { - setpoint->thrust = 0; - } else { - setpoint->thrust = min(rawThrust, MAX_THRUST); - } - - if (altHoldMode) { - setpoint->thrust = 0; - setpoint->mode.z = modeVelocity; - - setpoint->velocity.z = ((float) rawThrust - 32767.f) / 32767.f; - } else { - setpoint->mode.z = modeDisable; - } + float rawThrust = commanderGetActiveThrust(); + setpoint->thrust = constrain(rawThrust, MIN_THRUST, MAX_THRUST); // roll/pitch if (posHoldMode) { setpoint->mode.x = modeVelocity; setpoint->mode.y = modeVelocity; + setpoint->mode.z = modeVelocity; setpoint->mode.roll = modeDisable; setpoint->mode.pitch = modeDisable; setpoint->velocity.x = commanderGetActivePitch()/30.0f; setpoint->velocity.y = commanderGetActiveRoll()/30.0f; + setpoint->velocity.z = rawThrust; setpoint->attitude.roll = 0; setpoint->attitude.pitch = 0; - } else if (posSetMode && commanderGetActiveThrust() != 0) { + setpoint->thrust = 0; + } else if (posSetMode) { setpoint->mode.x = modeAbs; setpoint->mode.y = modeAbs; setpoint->mode.z = modeAbs; @@ -333,7 +315,7 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state) setpoint->position.x = -commanderGetActivePitch(); setpoint->position.y = commanderGetActiveRoll(); - setpoint->position.z = commanderGetActiveThrust()/1000.0f; + setpoint->position.z = commanderGetActiveThrust(); setpoint->attitude.roll = 0; setpoint->attitude.pitch = 0; @@ -343,6 +325,16 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state) setpoint->mode.x = modeDisable; setpoint->mode.y = modeDisable; + if (altHoldMode) { + setpoint->thrust = 0; + setpoint->mode.z = modeVelocity; + setpoint->velocity.z = rawThrust; + } else { + setpoint->thrust = constrain(rawThrust, IDLE_THRUST, MAX_THRUST); + setpoint->mode.z = modeDisable; + setpoint->velocity.z = 0; + } + if (stabilizationModeRoll == RATE) { setpoint->mode.roll = modeVelocity; setpoint->attitudeRate.roll = commanderGetActiveRoll(); From f61c3c2da2acc97f543a54b4bef7d31084477c8f Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Fri, 11 Nov 2016 14:58:33 -0800 Subject: [PATCH 6/7] commander: Don't set thrust when no modes are active. --- src/modules/src/commander.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c index 84505127c9..bdf37b7242 100644 --- a/src/modules/src/commander.c +++ b/src/modules/src/commander.c @@ -38,7 +38,6 @@ #include "ext_position.h" #define MIN_THRUST 0 -#define IDLE_THRUST 1000 #define MAX_THRUST 60000 /** @@ -330,7 +329,6 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state) setpoint->mode.z = modeVelocity; setpoint->velocity.z = rawThrust; } else { - setpoint->thrust = constrain(rawThrust, IDLE_THRUST, MAX_THRUST); setpoint->mode.z = modeDisable; setpoint->velocity.z = 0; } From da5152d138ecd91cdb91a2989d3d934a3ccfe3ac Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Fri, 18 Nov 2016 17:07:14 -0800 Subject: [PATCH 7/7] commander: Revert changes thrust changes. --- src/modules/interface/commander.h | 2 +- src/modules/src/commander.c | 52 ++++++++++++++++++------------- 2 files changed, 32 insertions(+), 22 deletions(-) diff --git a/src/modules/interface/commander.h b/src/modules/interface/commander.h index ec169bf8b9..b7f4ce7aed 100644 --- a/src/modules/interface/commander.h +++ b/src/modules/interface/commander.h @@ -47,7 +47,7 @@ struct CommanderCrtpValues float roll; // deg float pitch; // deg float yaw; // deg - float thrust; + uint16_t thrust; } __attribute__((packed)); void commanderInit(void); diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c index bdf37b7242..2cce73280b 100644 --- a/src/modules/src/commander.c +++ b/src/modules/src/commander.c @@ -37,7 +37,7 @@ #include "debug.h" #include "ext_position.h" -#define MIN_THRUST 0 +#define MIN_THRUST 1000 #define MAX_THRUST 60000 /** @@ -77,6 +77,7 @@ static CommanderCache* activeCache; static uint32_t lastUpdate; static bool isInactive; +static bool thrustLocked; static bool altHoldMode = false; static bool posHoldMode = false; static bool posSetMode = false; @@ -92,7 +93,7 @@ static void commanderCrtpCB(CRTPPacket* pk); static void commanderCacheSelectorUpdate(void); /* Private functions */ -static void commanderSetActiveThrust(float thrust) +static void commanderSetActiveThrust(uint16_t thrust) { activeCache->targetVal[activeCache->activeSide].thrust = thrust; } @@ -112,7 +113,7 @@ static void commanderSetActiveYaw(float yaw) activeCache->targetVal[activeCache->activeSide].yaw = yaw; } -static float commanderGetActiveThrust(void) +static uint16_t commanderGetActiveThrust(void) { commanderCacheSelectorUpdate(); return activeCache->targetVal[activeCache->activeSide].thrust; @@ -143,8 +144,6 @@ static void commanderLevelRPY(void) static void commanderDropToGround(void) { altHoldMode = false; - posHoldMode = false; - posSetMode = false; commanderSetActiveThrust(0); commanderLevelRPY(); } @@ -175,6 +174,10 @@ static void commanderCrtpCB(CRTPPacket* pk) crtpCache.targetVal[!crtpCache.activeSide] = *((struct CommanderCrtpValues*)pk->data); crtpCache.activeSide = !crtpCache.activeSide; crtpCache.timestamp = xTaskGetTickCount(); + + if (crtpCache.targetVal[crtpCache.activeSide].thrust == 0) { + thrustLocked = false; + } } /** @@ -263,6 +266,7 @@ void commanderInit(void) activeCache = &crtpCache; lastUpdate = xTaskGetTickCount(); isInactive = true; + thrustLocked = true; isInit = true; } @@ -277,6 +281,10 @@ void commanderExtrxSet(const struct CommanderCrtpValues *val) extrxCache.targetVal[!extrxCache.activeSide] = *((struct CommanderCrtpValues*)val); extrxCache.activeSide = !extrxCache.activeSide; extrxCache.timestamp = xTaskGetTickCount(); + + if (extrxCache.targetVal[extrxCache.activeSide].thrust == 0) { + thrustLocked = false; + } } uint32_t commanderGetInactivityTime(void) @@ -287,24 +295,35 @@ uint32_t commanderGetInactivityTime(void) void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state) { // Thrust - float rawThrust = commanderGetActiveThrust(); - setpoint->thrust = constrain(rawThrust, MIN_THRUST, MAX_THRUST); + uint16_t rawThrust = commanderGetActiveThrust(); + + if (thrustLocked || (rawThrust < MIN_THRUST)) { + setpoint->thrust = 0; + } else { + setpoint->thrust = min(rawThrust, MAX_THRUST); + } + + if (altHoldMode) { + setpoint->thrust = 0; + setpoint->mode.z = modeVelocity; + + setpoint->velocity.z = ((float) rawThrust - 32767.f) / 32767.f; + } else { + setpoint->mode.z = modeDisable; + } // roll/pitch if (posHoldMode) { setpoint->mode.x = modeVelocity; setpoint->mode.y = modeVelocity; - setpoint->mode.z = modeVelocity; setpoint->mode.roll = modeDisable; setpoint->mode.pitch = modeDisable; setpoint->velocity.x = commanderGetActivePitch()/30.0f; setpoint->velocity.y = commanderGetActiveRoll()/30.0f; - setpoint->velocity.z = rawThrust; setpoint->attitude.roll = 0; setpoint->attitude.pitch = 0; - setpoint->thrust = 0; - } else if (posSetMode) { + } else if (posSetMode && commanderGetActiveThrust() != 0) { setpoint->mode.x = modeAbs; setpoint->mode.y = modeAbs; setpoint->mode.z = modeAbs; @@ -314,7 +333,7 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state) setpoint->position.x = -commanderGetActivePitch(); setpoint->position.y = commanderGetActiveRoll(); - setpoint->position.z = commanderGetActiveThrust(); + setpoint->position.z = commanderGetActiveThrust()/1000.0f; setpoint->attitude.roll = 0; setpoint->attitude.pitch = 0; @@ -324,15 +343,6 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state) setpoint->mode.x = modeDisable; setpoint->mode.y = modeDisable; - if (altHoldMode) { - setpoint->thrust = 0; - setpoint->mode.z = modeVelocity; - setpoint->velocity.z = rawThrust; - } else { - setpoint->mode.z = modeDisable; - setpoint->velocity.z = 0; - } - if (stabilizationModeRoll == RATE) { setpoint->mode.roll = modeVelocity; setpoint->attitudeRate.roll = commanderGetActiveRoll();