From 545e31b1109174a9877f3af520594c3691234c82 Mon Sep 17 00:00:00 2001 From: Dennis Schmidt Date: Tue, 24 Oct 2023 11:03:18 +0200 Subject: [PATCH] dennis test lee success --- src/deck/drivers/src/usddeck.c | 4 +- src/modules/interface/controller/controller.h | 1 + .../interface/controller/controller_lee.h | 79 ++++ src/modules/interface/pptraj.h | 1 + src/modules/interface/stabilizer_types.h | 2 + src/modules/src/controller/Kbuild | 1 + src/modules/src/controller/controller.c | 2 + src/modules/src/controller/controller_lee.c | 386 ++++++++++++++++++ src/modules/src/crtp_commander_generic.c | 3 +- src/modules/src/crtp_commander_high_level.c | 4 + src/modules/src/pptraj.c | 6 +- vendor/libdw1000 | 2 +- 12 files changed, 485 insertions(+), 6 deletions(-) create mode 100644 src/modules/interface/controller/controller_lee.h create mode 100644 src/modules/src/controller/controller_lee.c diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c index 8427843ad8..77a9c87b1d 100644 --- a/src/deck/drivers/src/usddeck.c +++ b/src/deck/drivers/src/usddeck.c @@ -90,8 +90,8 @@ #define SPI_END_TRANSACTION spiEndTransaction #endif -#define MAX_USD_LOG_VARIABLES_PER_EVENT (20) -#define MAX_USD_LOG_EVENTS (20) +#define MAX_USD_LOG_VARIABLES_PER_EVENT (40) +#define MAX_USD_LOG_EVENTS (5) #define FIXED_FREQUENCY_EVENT_ID (0xFFFF) #define FIXED_FREQUENCY_EVENT_NAME "fixedFrequency" diff --git a/src/modules/interface/controller/controller.h b/src/modules/interface/controller/controller.h index a720b4008a..4b1792a0fc 100644 --- a/src/modules/interface/controller/controller.h +++ b/src/modules/interface/controller/controller.h @@ -37,6 +37,7 @@ typedef enum { #ifdef CONFIG_CONTROLLER_OOT ControllerTypeOot, #endif + ControllerTypeLee, ControllerType_COUNT, } ControllerType; diff --git a/src/modules/interface/controller/controller_lee.h b/src/modules/interface/controller/controller_lee.h new file mode 100644 index 0000000000..9be2ba6cba --- /dev/null +++ b/src/modules/interface/controller/controller_lee.h @@ -0,0 +1,79 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-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 __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; // TODO: should be CF global for other modules + 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 vec qr; + 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/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..19a3d28911 --- /dev/null +++ b/src/modules/src/controller/controller_lee.c @@ -0,0 +1,386 @@ +/* +The MIT License (MIT) + +Copyright (c) 2019 Wolfgang Hoenig + +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 + +TODO: + * Switch position controller + * consider Omega_d dot (currently assumes this is zero) +*/ + +#include +#include + +#include "math3d.h" +#include "controller_lee.h" + +#define GRAVITY_MAGNITUDE (9.81f) + +static controllerLee_t g_self = { + .mass = 0.034, // TODO: should be CF global for other modules + + // 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 = {9, 9, 9}, // Kp in paper + .Kpos_P_limit = 100, + .Kpos_D = {7, 7, 7}, // Kv in paper + .Kpos_D_limit = 100, + .Kpos_I = {5, 5, 5}, // not in paper + .Kpos_I_limit = 2, + + // Attitude PID + .KR = {0.0055, 0.0055, 0.01}, + .Komega = {0.0013, 0.0013, 0.002}, + .KI = {0.012, 0.018, 0.015}, +}; + +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; + } + } + // On CF2, thrust is mapped 65536 <==> 4 * 12 grams + const float max_thrust = 70.0f / 1000.0f * 9.81f; // N + control->thrustSi = setpoint->thrust / UINT16_MAX * max_thrust; + + self->qr = mkvec( + radians(setpoint->attitude.roll), + -radians(setpoint->attitude.pitch), // This is in the legacy coordinate system where pitch is inverted + desiredYaw); + } + + // 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); + // struct vec desJerk = vzero(); + + 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))); + + // if (enableNN > 1) { + // u = vsub(u, tau_a); + // } + + 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 83161463f6..dee2ba9e81 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/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/vendor/libdw1000 b/vendor/libdw1000 index a77610327f..448a0efde4 160000 --- a/vendor/libdw1000 +++ b/vendor/libdw1000 @@ -1 +1 @@ -Subproject commit a77610327fb875a69543f3a8d19507683b68aff1 +Subproject commit 448a0efde42a258139cc8ca61fe53ff3bb4a9b94