diff --git a/bindings/cffirmware.i b/bindings/cffirmware.i index 289706ba69..3b8ea9a8d4 100755 --- a/bindings/cffirmware.i +++ b/bindings/cffirmware.i @@ -19,6 +19,7 @@ #include "num.h" #include "controller_mellinger.h" #include "controller_brescianini.h" +#include "controller_lee.h" #include "power_distribution.h" #include "axis3fSubSampler.h" #include "outlierFilterTdoa.h" @@ -35,6 +36,7 @@ %include "imu_types.h" %include "controller_mellinger.h" %include "controller_brescianini.h" +%include "controller_lee.h" %include "power_distribution.h" %include "axis3fSubSampler.h" %include "outlierFilterTdoa.h" diff --git a/bindings/setup.py b/bindings/setup.py index a543e31c62..4f67015624 100644 --- a/bindings/setup.py +++ b/bindings/setup.py @@ -41,6 +41,7 @@ "src/modules/src/controller/attitude_pid_controller.c", "src/modules/src/controller/controller_mellinger.c", "src/modules/src/controller/controller_brescianini.c", + "src/modules/src/controller/controller_lee.c", "src/utils/src/pid.c", "src/utils/src/filter.c", "src/utils/src/num.c", diff --git a/docs/functional-areas/sensor-to-control/controllers.md b/docs/functional-areas/sensor-to-control/controllers.md index 1608f4a203..e7684186fe 100644 --- a/docs/functional-areas/sensor-to-control/controllers.md +++ b/docs/functional-areas/sensor-to-control/controllers.md @@ -6,10 +6,12 @@ page_id: controllers Once the [state estimator](/docs/functional-areas/sensor-to-control/state_estimators.md) have outputed the current (estimated) situation of the crazyflie in position velocity and attitude, it is time for the controllers to keep it that way or to move the crazyflie into a new position based on a setpoint. This is an important part of the stabilization system in the crazyflie. - * [Overview of Control](#overview-of-control) - * [Cascaded PID controller](#cascaded-pid-controller) - * Mellinger Controller (TO DO) - * INDI Controller (TO DO) +- [Overview of control](#overview-of-control) +- [Cascaded PID controller](#cascaded-pid-controller) +- [Mellinger Controller](#mellinger-controller) +- [INDI Controller](#indi-controller) +- [Brescianini Controller](#brescianini-controller) +- [Lee Controller](#lee-controller) ## Overview of control There are four levels to control in the Crazyflie: @@ -55,3 +57,51 @@ Check the implementation details in `attitude_pid_controller.c` in `attitudeCont The most outer-loop of the cascaded PID controller is the position and velocity controller. It receives position or velocity input from a commander which are handled, since it is possible to set in the variable `setpoint_t` which stabilization mode to use `stab_mode_t` (either position: `modeAbs` or `modeVelocity`). These can be found in `stabilizer_types.h`. The control loop runs at 100 Hz. Check the implementation details in `position_controller_pid.c` in `positionController()` and `velocityController()`. + +## Mellinger Controller + +Conceptually the Mellinger controller is similar to the cascaded PID controller, i.e. there is an attitude controller (running at 250 Hz) and a position controller (running at 100 Hz). The main difference to the cascaded PID controller is how errors are defined and how the position error is translated into desired attitude setpoints. Like the cascaded PID, this is a reactive geometric controller that uses the mathematical property of differential flatness. Details are given in the following scientific publication: + +``` +Daniel Mellinger, and Vijay Kumar +Minimum snap trajectory generation and control for quadrotors +IEEE International Conference on Robotics and Automation (ICRA), 2011 +https://doi.org/10.1109/ICRA.2011.5980409 +``` + +The implementation follows the paper, also for the names of the variables. The main difference is the addition of I-gains and a D-term for the angular velocity. + +## INDI Controller + +This control algorithm is the Incremental Nonlinear Dynamic Inversion (INDI) controller. Details are given in the following scientific publication: + +``` +Ewoud J. J. Smeur, Qiping Chu, and Guido C. H. E. de Croon +Adaptive Incremental Nonlinear Dynamic Inversion for Attitude Control of Micro Air Vehicles +JCGD 2015 +https://doi.org/10.2514/1.G001490 +``` + +## Brescianini Controller + +Details of this controller are in the following scientific publication: + +``` +Dario Brescianini, Markus Hehn, and Raffaello D'Andrea +Nonlinear quadrocopter attitude control +Technical Report ETHZ, 2013 +https://doi.org/10.3929/ethz-a-009970340 +``` + +## Lee Controller + +Conceptually the Lee controller is similar to the cascaded PID controller, i.e. there is an attitude controller (running at 250 Hz) and a position controller (running at 100 Hz). The main difference to the cascaded PID controller is how errors are defined and how the position error is translated into desired attitude setpoints. Like the cascaded PID, this is a reactive geometric controller that uses the mathematical property of differential flatness. Compared to the Mellinger controller, a different angular velocity error and higher-order terms in the attitude controller are used. Details including a stability proof are given in the following scientific publication: + +``` +Taeyoung Lee, Melvin Leok, and N. Harris McClamroch +Geometric Tracking Control of a Quadrotor UAV on SE(3) +CDC 2010 +https://doi.org/10.1109/CDC.2010.5717652 +``` + +The implementation follows the paper, also for the names of the variables. The main difference is the addition of I-gains, which are not needed for the theoretical proof, but helpful on the practical system. diff --git a/src/modules/interface/controller/controller.h b/src/modules/interface/controller/controller.h index a720b4008a..4854009b61 100644 --- a/src/modules/interface/controller/controller.h +++ b/src/modules/interface/controller/controller.h @@ -34,6 +34,7 @@ typedef enum { ControllerTypeMellinger, ControllerTypeINDI, ControllerTypeBrescianini, + ControllerTypeLee, #ifdef CONFIG_CONTROLLER_OOT ControllerTypeOot, #endif diff --git a/src/modules/interface/controller/controller_lee.h b/src/modules/interface/controller/controller_lee.h new file mode 100644 index 0000000000..0d64dfac2c --- /dev/null +++ b/src/modules/interface/controller/controller_lee.h @@ -0,0 +1,76 @@ +/* +The MIT License (MIT) + +Copyright (c) 2024 Khaled Wahba + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ +#ifndef __CONTROLLER_LEE_H__ +#define __CONTROLLER_LEE_H__ + +#include "stabilizer_types.h" + +// This structure contains the mutable state and inmutable parameters +typedef struct controllerLee_s { + float mass; + float thrustSi; + struct vec J; // Inertia matrix (diagonal matrix); kg m^2 + + // Position PID + struct vec Kpos_P; // Kp in paper + float Kpos_P_limit; + struct vec Kpos_D; // Kv in paper + float Kpos_D_limit; + struct vec Kpos_I; // not in paper + float Kpos_I_limit; + struct vec i_error_pos; + struct vec p_error; + struct vec v_error; + // Attitude PID + struct vec KR; + struct vec Komega; + struct vec KI; + struct vec i_error_att; + // Logging variables + struct vec rpy; + struct vec rpy_des; + struct mat33 R_des; + struct vec omega; + struct vec omega_r; + struct vec u; +} controllerLee_t; + + +void controllerLeeInit(controllerLee_t* self); +void controllerLeeReset(controllerLee_t* self); +void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick); + +#ifdef CRAZYFLIE_FW +void controllerLeeFirmwareInit(void); +bool controllerLeeFirmwareTest(void); +void controllerLeeFirmware(control_t *control, const setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick); +#endif // CRAZYFLIE_FW defined + +#endif //__CONTROLLER_LEE_H__ diff --git a/src/modules/interface/power_distribution.h b/src/modules/interface/power_distribution.h index 945d734a7f..07dd866f66 100644 --- a/src/modules/interface/power_distribution.h +++ b/src/modules/interface/power_distribution.h @@ -69,4 +69,11 @@ uint16_t powerDistributionStopRatio(uint32_t id); */ uint32_t powerDistributionGetIdleThrust(); +/** + * @brief Compute the maximum thrust + * + * @return float The maximum thrust of the robot [N] + */ +float powerDistributionGetMaxThrust(); + #endif //__POWER_DISTRIBUTION_H__ diff --git a/src/modules/interface/pptraj.h b/src/modules/interface/pptraj.h index 568b299be4..22616ff93a 100644 --- a/src/modules/interface/pptraj.h +++ b/src/modules/interface/pptraj.h @@ -120,6 +120,7 @@ struct traj_eval struct vec pos; struct vec vel; struct vec acc; + struct vec jerk; struct vec omega; float yaw; }; diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index d49f852b6a..dff1eb361b 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -65,6 +65,7 @@ typedef struct vec3_s vector_t; typedef struct vec3_s point_t; typedef struct vec3_s velocity_t; typedef struct vec3_s acc_t; +typedef struct vec3_s jerk_t; /* Orientation as a quaternion */ typedef struct quaternion_s { @@ -257,6 +258,7 @@ typedef struct setpoint_s { point_t position; // m velocity_t velocity; // m/s acc_t acceleration; // m/s^2 + jerk_t jerk; // m/s^3 bool velocity_body; // true if velocity is given in body frame; false if velocity is given in world frame struct { diff --git a/src/modules/src/controller/Kbuild b/src/modules/src/controller/Kbuild index 8aebac4e89..33401b23d3 100644 --- a/src/modules/src/controller/Kbuild +++ b/src/modules/src/controller/Kbuild @@ -6,3 +6,4 @@ obj-y += controller_pid.o obj-y += controller_brescianini.o obj-y += position_controller_indi.o obj-y += position_controller_pid.o +obj-y += controller_lee.o diff --git a/src/modules/src/controller/controller.c b/src/modules/src/controller/controller.c index 827511c81f..0ef658a8ab 100644 --- a/src/modules/src/controller/controller.c +++ b/src/modules/src/controller/controller.c @@ -7,6 +7,7 @@ #include "controller_mellinger.h" #include "controller_indi.h" #include "controller_brescianini.h" +#include "controller_lee.h" #include "autoconf.h" @@ -31,6 +32,7 @@ static ControllerFcns controllerFunctions[] = { #ifdef CONFIG_CONTROLLER_OOT {.init = controllerOutOfTreeInit, .test = controllerOutOfTreeTest, .update = controllerOutOfTree, .name = "OutOfTree"}, #endif + {.init = controllerLeeFirmwareInit, .test = controllerLeeFirmwareTest, .update = controllerLeeFirmware, .name = "Lee"}, }; diff --git a/src/modules/src/controller/controller_lee.c b/src/modules/src/controller/controller_lee.c new file mode 100644 index 0000000000..bfde9de410 --- /dev/null +++ b/src/modules/src/controller/controller_lee.c @@ -0,0 +1,375 @@ +/* +The MIT License (MIT) + +Copyright (c) 2024 Khaled Wahba + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* +This controller is based on the following publication: + +Taeyoung Lee, Melvin Leok, and N. Harris McClamroch +Geometric Tracking Control of a Quadrotor UAV on SE(3) +CDC 2010 + +* Difference to Mellinger: + * Different angular velocity error + * Higher-order terms in attitude controller +*/ + +#include +#include + +#include "math3d.h" +#include "controller_lee.h" +#include "physicalConstants.h" +#include "power_distribution.h" +#include "platform_defaults.h" + +static controllerLee_t g_self = { + .mass = CF_MASS, + + // Inertia matrix (diagonal matrix), see + // System Identification of the Crazyflie 2.0 Nano Quadrocopter + // BA theses, Julian Foerster, ETHZ + // https://polybox.ethz.ch/index.php/s/20dde63ee00ffe7085964393a55a91c7 + .J = {16.571710e-6, 16.655602e-6, 29.261652e-6}, // kg m^2 + + // Position PID + .Kpos_P = {7.0, 7.0, 7.0}, // Kp in paper + .Kpos_P_limit = 100, + .Kpos_D = {4.0, 4.0, 4.0}, // Kv in paper + .Kpos_D_limit = 100, + .Kpos_I = {0.0, 0.0, 0.0}, // not in paper + .Kpos_I_limit = 2, + + // Attitude PID + .KR = {0.007, 0.007, 0.008}, + .Komega = {0.00115, 0.00115, 0.002}, + .KI = {0.03, 0.03, 0.03}, +}; + +static inline struct vec vclampscl(struct vec value, float min, float max) { + return mkvec( + clamp(value.x, min, max), + clamp(value.y, min, max), + clamp(value.z, min, max)); +} + +void controllerLeeReset(controllerLee_t* self) +{ + self->i_error_pos = vzero(); + self->i_error_att = vzero(); +} + +void controllerLeeInit(controllerLee_t* self) +{ + // copy default values (bindings), or NOP (firmware) + *self = g_self; + + controllerLeeReset(self); +} + +bool controllerLeeTest(controllerLee_t* self) +{ + return true; +} + +void controllerLee(controllerLee_t* self, control_t *control, const setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick) +{ + + if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { + return; + } + + // uint64_t startTime = usecTimestamp(); + + float dt = (float)(1.0f/ATTITUDE_RATE); + // struct vec dessnap = vzero(); + // Address inconsistency in firmware where we need to compute our own desired yaw angle + // Rate-controlled YAW is moving YAW angle setpoint + float desiredYaw = 0; //rad + if (setpoint->mode.yaw == modeVelocity) { + desiredYaw = radians(state->attitude.yaw + setpoint->attitudeRate.yaw * dt); + } else if (setpoint->mode.yaw == modeAbs) { + desiredYaw = radians(setpoint->attitude.yaw); + } else if (setpoint->mode.quat == modeAbs) { + struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w); + self->rpy_des = quat2rpy(setpoint_quat); + desiredYaw = self->rpy_des.z; + } + + // Position controller + if ( setpoint->mode.x == modeAbs + || setpoint->mode.y == modeAbs + || setpoint->mode.z == modeAbs) { + struct vec pos_d = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z); + struct vec vel_d = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z); + struct vec acc_d = mkvec(setpoint->acceleration.x, setpoint->acceleration.y, setpoint->acceleration.z + GRAVITY_MAGNITUDE); + struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z); + struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z); + + // errors + struct vec pos_e = vclampscl(vsub(pos_d, statePos), -self->Kpos_P_limit, self->Kpos_P_limit); + struct vec vel_e = vclampscl(vsub(vel_d, stateVel), -self->Kpos_D_limit, self->Kpos_D_limit); + self->i_error_pos = vadd(self->i_error_pos, vscl(dt, pos_e)); + self->p_error = pos_e; + self->v_error = vel_e; + + struct vec F_d = vadd4( + acc_d, + veltmul(self->Kpos_D, vel_e), + veltmul(self->Kpos_P, pos_e), + veltmul(self->Kpos_I, self->i_error_pos)); + + struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + struct mat33 R = quat2rotmat(q); + struct vec z = vbasis(2); + control->thrustSi = self->mass*vdot(F_d , mvmul(R, z)); + self->thrustSi = control->thrustSi; + // Reset the accumulated error while on the ground + if (control->thrustSi < 0.01f) { + controllerLeeReset(self); + } + + // Compute Desired Rotation matrix + float normFd = control->thrustSi; + + struct vec xdes = vbasis(0); + struct vec ydes = vbasis(1); + struct vec zdes = vbasis(2); + + if (normFd > 0) { + zdes = vnormalize(F_d); + } + struct vec xcdes = mkvec(cosf(desiredYaw), sinf(desiredYaw), 0); + struct vec zcrossx = vcross(zdes, xcdes); + float normZX = vmag(zcrossx); + + if (normZX > 0) { + ydes = vnormalize(zcrossx); + } + xdes = vcross(ydes, zdes); + + self->R_des = mcolumns(xdes, ydes, zdes); + + } else { + if (setpoint->mode.z == modeDisable) { + if (setpoint->thrust < 1000) { + control->controlMode = controlModeForceTorque; + control->thrustSi = 0; + control->torque[0] = 0; + control->torque[1] = 0; + control->torque[2] = 0; + controllerLeeReset(self); + return; + } + } + const float max_thrust = powerDistributionGetMaxThrust(); // N + control->thrustSi = setpoint->thrust / UINT16_MAX * max_thrust; + + struct quat q = rpy2quat(mkvec( + radians(setpoint->attitude.roll), + -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted + desiredYaw)); + self->R_des = quat2rotmat(q); + } + + // Attitude controller + + // current rotation [R] + struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w); + self->rpy = quat2rpy(q); + struct mat33 R = quat2rotmat(q); + + // desired rotation [Rdes] + struct quat q_des = mat2quat(self->R_des); + self->rpy_des = quat2rpy(q_des); + + // rotation error + struct mat33 eRM = msub(mmul(mtranspose(self->R_des), R), mmul(mtranspose(R), self->R_des)); + + struct vec eR = vscl(0.5f, mkvec(eRM.m[2][1], eRM.m[0][2], eRM.m[1][0])); + + // angular velocity + self->omega = mkvec( + radians(sensors->gyro.x), + radians(sensors->gyro.y), + radians(sensors->gyro.z)); + + // Compute desired omega + struct vec xdes = mcolumn(self->R_des, 0); + struct vec ydes = mcolumn(self->R_des, 1); + struct vec zdes = mcolumn(self->R_des, 2); + struct vec hw = vzero(); + // Desired Jerk and snap for now are zeros vector + struct vec desJerk = mkvec(setpoint->jerk.x, setpoint->jerk.y, setpoint->jerk.z); + + if (control->thrustSi != 0) { + struct vec tmp = vsub(desJerk, vscl(vdot(zdes, desJerk), zdes)); + hw = vscl(self->mass/control->thrustSi, tmp); + } + struct vec z_w = mkvec(0,0,1); + float desiredYawRate = radians(setpoint->attitudeRate.yaw) * vdot(zdes,z_w); + struct vec omega_des = mkvec(-vdot(hw,ydes), vdot(hw,xdes), desiredYawRate); + + self->omega_r = mvmul(mmul(mtranspose(R), self->R_des), omega_des); + + struct vec omega_error = vsub(self->omega, self->omega_r); + + // Integral part on angle + self->i_error_att = vadd(self->i_error_att, vscl(dt, eR)); + + // compute moments + // M = -kR eR - kw ew + w x Jw - J(w x wr) + self->u = vadd4( + vneg(veltmul(self->KR, eR)), + vneg(veltmul(self->Komega, omega_error)), + vneg(veltmul(self->KI, self->i_error_att)), + vcross(self->omega, veltmul(self->J, self->omega))); + + control->controlMode = controlModeForceTorque; + control->torque[0] = self->u.x; + control->torque[1] = self->u.y; + control->torque[2] = self->u.z; + + // ticks = usecTimestamp() - startTime; +} + +#ifdef CRAZYFLIE_FW + +#include "param.h" +#include "log.h" + +void controllerLeeFirmwareInit(void) +{ + controllerLeeInit(&g_self); +} + +bool controllerLeeFirmwareTest(void) +{ + return true; +} + +void controllerLeeFirmware(control_t *control, const setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick) +{ + controllerLee(&g_self, control, setpoint, sensors, state, tick); +} + +PARAM_GROUP_START(ctrlLee) +PARAM_ADD(PARAM_FLOAT, KR_x, &g_self.KR.x) +PARAM_ADD(PARAM_FLOAT, KR_y, &g_self.KR.y) +PARAM_ADD(PARAM_FLOAT, KR_z, &g_self.KR.z) +// Attitude I +PARAM_ADD(PARAM_FLOAT, KI_x, &g_self.KI.x) +PARAM_ADD(PARAM_FLOAT, KI_y, &g_self.KI.y) +PARAM_ADD(PARAM_FLOAT, KI_z, &g_self.KI.z) +// Attitude D +PARAM_ADD(PARAM_FLOAT, Kw_x, &g_self.Komega.x) +PARAM_ADD(PARAM_FLOAT, Kw_y, &g_self.Komega.y) +PARAM_ADD(PARAM_FLOAT, Kw_z, &g_self.Komega.z) + +// J +PARAM_ADD(PARAM_FLOAT, J_x, &g_self.J.x) +PARAM_ADD(PARAM_FLOAT, J_y, &g_self.J.y) +PARAM_ADD(PARAM_FLOAT, J_z, &g_self.J.z) + +// Position P +PARAM_ADD(PARAM_FLOAT, Kpos_Px, &g_self.Kpos_P.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Py, &g_self.Kpos_P.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Pz, &g_self.Kpos_P.z) +PARAM_ADD(PARAM_FLOAT, Kpos_P_limit, &g_self.Kpos_P_limit) +// Position D +PARAM_ADD(PARAM_FLOAT, Kpos_Dx, &g_self.Kpos_D.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Dy, &g_self.Kpos_D.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Dz, &g_self.Kpos_D.z) +PARAM_ADD(PARAM_FLOAT, Kpos_D_limit, &g_self.Kpos_D_limit) +// Position I +PARAM_ADD(PARAM_FLOAT, Kpos_Ix, &g_self.Kpos_I.x) +PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &g_self.Kpos_I.y) +PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &g_self.Kpos_I.z) +PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &g_self.Kpos_I_limit) + +PARAM_ADD(PARAM_FLOAT, mass, &g_self.mass) +PARAM_GROUP_STOP(ctrlLee) + + +LOG_GROUP_START(ctrlLee) + +LOG_ADD(LOG_FLOAT, KR_x, &g_self.KR.x) +LOG_ADD(LOG_FLOAT, KR_y, &g_self.KR.y) +LOG_ADD(LOG_FLOAT, KR_z, &g_self.KR.z) +LOG_ADD(LOG_FLOAT, Kw_x, &g_self.Komega.x) +LOG_ADD(LOG_FLOAT, Kw_y, &g_self.Komega.y) +LOG_ADD(LOG_FLOAT, Kw_z, &g_self.Komega.z) + +LOG_ADD(LOG_FLOAT,Kpos_Px, &g_self.Kpos_P.x) +LOG_ADD(LOG_FLOAT,Kpos_Py, &g_self.Kpos_P.y) +LOG_ADD(LOG_FLOAT,Kpos_Pz, &g_self.Kpos_P.z) +LOG_ADD(LOG_FLOAT,Kpos_Dx, &g_self.Kpos_D.x) +LOG_ADD(LOG_FLOAT,Kpos_Dy, &g_self.Kpos_D.y) +LOG_ADD(LOG_FLOAT,Kpos_Dz, &g_self.Kpos_D.z) + + +LOG_ADD(LOG_FLOAT, thrustSi, &g_self.thrustSi) +LOG_ADD(LOG_FLOAT, torquex, &g_self.u.x) +LOG_ADD(LOG_FLOAT, torquey, &g_self.u.y) +LOG_ADD(LOG_FLOAT, torquez, &g_self.u.z) + +// current angles +LOG_ADD(LOG_FLOAT, rpyx, &g_self.rpy.x) +LOG_ADD(LOG_FLOAT, rpyy, &g_self.rpy.y) +LOG_ADD(LOG_FLOAT, rpyz, &g_self.rpy.z) + +// desired angles +LOG_ADD(LOG_FLOAT, rpydx, &g_self.rpy_des.x) +LOG_ADD(LOG_FLOAT, rpydy, &g_self.rpy_des.y) +LOG_ADD(LOG_FLOAT, rpydz, &g_self.rpy_des.z) + +// errors +LOG_ADD(LOG_FLOAT, error_posx, &g_self.p_error.x) +LOG_ADD(LOG_FLOAT, error_posy, &g_self.p_error.y) +LOG_ADD(LOG_FLOAT, error_posz, &g_self.p_error.z) + +LOG_ADD(LOG_FLOAT, error_velx, &g_self.v_error.x) +LOG_ADD(LOG_FLOAT, error_vely, &g_self.v_error.y) +LOG_ADD(LOG_FLOAT, error_velz, &g_self.v_error.z) + +// omega +LOG_ADD(LOG_FLOAT, omegax, &g_self.omega.x) +LOG_ADD(LOG_FLOAT, omegay, &g_self.omega.y) +LOG_ADD(LOG_FLOAT, omegaz, &g_self.omega.z) + +// omega_r +LOG_ADD(LOG_FLOAT, omegarx, &g_self.omega_r.x) +LOG_ADD(LOG_FLOAT, omegary, &g_self.omega_r.y) +LOG_ADD(LOG_FLOAT, omegarz, &g_self.omega_r.z) + +// LOG_ADD(LOG_UINT32, ticks, &ticks) + +LOG_GROUP_STOP(ctrlLee) + +#endif // CRAZYFLIE_FW defined diff --git a/src/modules/src/crtp_commander_generic.c b/src/modules/src/crtp_commander_generic.c index 6148d608ae..74e98d4730 100644 --- a/src/modules/src/crtp_commander_generic.c +++ b/src/modules/src/crtp_commander_generic.c @@ -353,7 +353,8 @@ static void fullStateDecoder(setpoint_t *setpoint, uint8_t type, const void *dat setpoint->position.x = values->x / 1000.0f; \ setpoint->velocity.x = (values->v ## x) / 1000.0f; \ setpoint->acceleration.x = (values->a ## x) / 1000.0f; \ - + setpoint->jerk.x = 0.0f; \ + UNPACK(x) UNPACK(y) UNPACK(z) diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index da61d6ba6f..7c0e3f4b9e 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -351,6 +351,10 @@ bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat setpoint->acceleration.x = ev.acc.x; setpoint->acceleration.y = ev.acc.y; setpoint->acceleration.z = ev.acc.z; + setpoint->jerk.x = ev.jerk.x; + setpoint->jerk.y = ev.jerk.y; + setpoint->jerk.z = ev.jerk.z; + // store the last setpoint pos = ev.pos; diff --git a/src/modules/src/power_distribution_flapper.c b/src/modules/src/power_distribution_flapper.c index 98529703c0..b50868bb55 100644 --- a/src/modules/src/power_distribution_flapper.c +++ b/src/modules/src/power_distribution_flapper.c @@ -204,6 +204,12 @@ uint32_t powerDistributionGetIdleThrust() { return idleThrust; } +float powerDistributionGetMaxThrust() { + // Unknown + ASSERT(false); + return 0.0f; +} + /** * Power distribution parameters */ diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c index 0f3a9d046e..a58e1e4155 100644 --- a/src/modules/src/power_distribution_quadrotor.c +++ b/src/modules/src/power_distribution_quadrotor.c @@ -46,6 +46,8 @@ static float armLength = ARM_LENGTH; // m static float thrustToTorque = 0.005964552f; // thrust = a * pwm^2 + b * pwm +// where PWM is normalized (range 0...1) +// thrust is in Newtons (per rotor) static float pwmToThrustA = 0.091492681f; static float pwmToThrustB = 0.067673604f; @@ -171,6 +173,12 @@ uint32_t powerDistributionGetIdleThrust() { return idleThrust; } +float powerDistributionGetMaxThrust() { + // max thrust per rotor occurs if normalized PWM is 1 + // pwmToThrustA * pwm * pwm + pwmToThrustB * pwm = pwmToThrustA + pwmToThrustB + return STABILIZER_NR_OF_MOTORS * (pwmToThrustA + pwmToThrustB); +} + /** * Power distribution parameters */ diff --git a/src/modules/src/pptraj.c b/src/modules/src/pptraj.c index d24dc2f93c..ea7c4be493 100644 --- a/src/modules/src/pptraj.c +++ b/src/modules/src/pptraj.c @@ -309,7 +309,7 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t) // 3rd derivative polyder4d(deriv); - struct vec jerk = polyval_xyz(deriv, t); + out.jerk = polyval_xyz(deriv, t); struct vec thrust = vadd(out.acc, mkvec(0, 0, GRAV)); // float thrust_mag = mass * vmag(thrust); @@ -319,7 +319,7 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t) struct vec y_body = vnormalize(vcross(z_body, x_world)); struct vec x_body = vcross(y_body, z_body); - struct vec jerk_orth_zbody = vorthunit(jerk, z_body); + struct vec jerk_orth_zbody = vorthunit(out.jerk, z_body); struct vec h_w = vscl(1.0f / vmag(thrust), jerk_orth_zbody); out.omega.x = -vdot(h_w, y_body); @@ -356,6 +356,7 @@ struct traj_eval piecewise_eval( ev.pos = vadd(ev.pos, traj->shift); ev.vel = vzero(); ev.acc = vzero(); + ev.jerk = vzero(); ev.omega = vzero(); return ev; } @@ -386,6 +387,7 @@ struct traj_eval piecewise_eval_reversed( ev.pos = vadd(ev.pos, traj->shift); ev.vel = vzero(); ev.acc = vzero(); + ev.jerk = vzero(); ev.omega = vzero(); return ev; } diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 49d4fd1a08..0a3c1d8cfd 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -367,7 +367,7 @@ PARAM_GROUP_START(stabilizer) */ PARAM_ADD_CORE(PARAM_UINT8, estimator, &estimatorType) /** - * @brief Controller type Auto select(0), PID(1), Mellinger(2), INDI(3), Brescianini(4) (Default: 0) + * @brief Controller type Auto select(0), PID(1), Mellinger(2), INDI(3), Brescianini(4), Lee(5) (Default: 0) */ PARAM_ADD_CORE(PARAM_UINT8, controller, &controllerType) PARAM_GROUP_STOP(stabilizer) diff --git a/test_python/test_controller_lee.py b/test_python/test_controller_lee.py new file mode 100644 index 0000000000..eed35560b7 --- /dev/null +++ b/test_python/test_controller_lee.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python + +import cffirmware + +def test_controller_lee(): + + ctrl = cffirmware.controllerLee_t() + + cffirmware.controllerLeeInit(ctrl) + + control = cffirmware.control_t() + setpoint = cffirmware.setpoint_t() + setpoint.mode.z = cffirmware.modeAbs + setpoint.position.z = 0 + setpoint.mode.x = cffirmware.modeVelocity + setpoint.velocity.x = 0 + setpoint.mode.y = cffirmware.modeVelocity + setpoint.velocity.y = 0 + setpoint.mode.yaw = cffirmware.modeVelocity + setpoint.attitudeRate.yaw = 0 + + state = cffirmware.state_t() + state.attitude.roll = 0 + state.attitude.pitch = -0 # WARNING: This needs to be negated + state.attitude.yaw = 0 + state.position.x = 0 + state.position.y = 0 + state.position.z = 0 + state.velocity.x = 0 + state.velocity.y = 0 + state.velocity.z = 0 + + sensors = cffirmware.sensorData_t() + sensors.gyro.x = 0 + sensors.gyro.y = 0 + sensors.gyro.z = 0 + + step = 100 + + cffirmware.controllerLee(ctrl, control, setpoint,sensors,state,step) + assert control.controlMode == cffirmware.controlModeForceTorque + # control.thrust will be at a (tuned) hover-state + assert control.torqueX == 0 + assert control.torqueY == 0 + assert control.torqueZ == 0