From 368c174db998bff58a4c707fd9c1a06ced45728a Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 4 Jan 2023 10:20:36 +0100 Subject: [PATCH 1/2] Mellinger: support to change gains in bindings To use the controller in a simulator (and for testing), we need two changes: 1.) have a data structure that holds the state, so we can instantiate multiple controllers. 2.) the ability to change gains without recompiling. This change introduces a state/param struct to achieve both objectives, similar to the logic in collision_avoidance.c. --- src/modules/interface/controller_mellinger.h | 76 ++++- src/modules/src/controller.c | 2 +- src/modules/src/controller_mellinger.c | 305 ++++++++++--------- test_python/test_controller_mellinger.py | 6 +- 4 files changed, 237 insertions(+), 152 deletions(-) diff --git a/src/modules/interface/controller_mellinger.h b/src/modules/interface/controller_mellinger.h index fa704d74fe..4cc0e32c9d 100644 --- a/src/modules/interface/controller_mellinger.h +++ b/src/modules/interface/controller_mellinger.h @@ -27,12 +27,82 @@ #define __CONTROLLER_MELLINGER_H__ #include "stabilizer_types.h" +#include "math3d.h" -void controllerMellingerInit(void); -bool controllerMellingerTest(void); -void controllerMellinger(control_t *control, const setpoint_t *setpoint, +typedef struct { + float mass; + float massThrust; + + // XY Position PID + float kp_xy; // P + float kd_xy; // D + float ki_xy; // I + float i_range_xy; + + // Z Position + float kp_z; // P + float kd_z; // D + float ki_z; // I + float i_range_z; + + // Attitude + float kR_xy; // P + float kw_xy; // D + float ki_m_xy; // I + float i_range_m_xy; + + // Yaw + float kR_z; // P + float kw_z; // D + float ki_m_z; // I + float i_range_m_z; + + // roll and pitch angular velocity + float kd_omega_rp; // D + + // Helper variables + float i_error_x; + float i_error_y; + float i_error_z; + + float prev_omega_roll; + float prev_omega_pitch; + float prev_setpoint_omega_roll; + float prev_setpoint_omega_pitch; + + float i_error_m_x; + float i_error_m_y; + float i_error_m_z; + + // Logging variables + struct vec z_axis_desired; + + float cmd_thrust; + float cmd_roll; + float cmd_pitch; + float cmd_yaw; + float r_roll; + float r_pitch; + float r_yaw; + float accelz; +} controllerMellinger_t; + +void controllerMellingerInit(controllerMellinger_t* self); +bool controllerMellingerTest(controllerMellinger_t* self); +void controllerMellinger(controllerMellinger_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 controllerMellingerFirmwareInit(void); +bool controllerMellingerFirmwareTest(void); +void controllerMellingerFirmware(control_t *control, const setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick); +#endif // CRAZYFLIE_FW + #endif //__CONTROLLER_MELLINGER_H__ diff --git a/src/modules/src/controller.c b/src/modules/src/controller.c index b3be1dc378..c58ea7e212 100644 --- a/src/modules/src/controller.c +++ b/src/modules/src/controller.c @@ -25,7 +25,7 @@ typedef struct { static ControllerFcns controllerFunctions[] = { {.init = 0, .test = 0, .update = 0, .name = "None"}, // Any {.init = controllerPidInit, .test = controllerPidTest, .update = controllerPid, .name = "PID"}, - {.init = controllerMellingerInit, .test = controllerMellingerTest, .update = controllerMellinger, .name = "Mellinger"}, + {.init = controllerMellingerFirmwareInit, .test = controllerMellingerFirmwareTest, .update = controllerMellingerFirmware, .name = "Mellinger"}, {.init = controllerINDIInit, .test = controllerINDITest, .update = controllerINDI, .name = "INDI"}, {.init = controllerBrescianiniInit, .test = controllerBrescianiniTest, .update = controllerBrescianini, .name = "Brescianini"}, }; diff --git a/src/modules/src/controller_mellinger.c b/src/modules/src/controller_mellinger.c index 231ab42e3e..e9b696e833 100644 --- a/src/modules/src/controller_mellinger.c +++ b/src/modules/src/controller_mellinger.c @@ -39,89 +39,80 @@ We added the following: #include "param.h" #include "log.h" -#include "math3d.h" #include "position_controller.h" #include "controller_mellinger.h" #include "physicalConstants.h" -static float g_vehicleMass = CF_MASS; -static float massThrust = 132000; - -// XY Position PID -static float kp_xy = 0.4; // P -static float kd_xy = 0.2; // D -static float ki_xy = 0.05; // I -static float i_range_xy = 2.0; - -// Z Position -static float kp_z = 1.25; // P -static float kd_z = 0.4; // D -static float ki_z = 0.05; // I -static float i_range_z = 0.4; - -// Attitude -static float kR_xy = 70000; // P -static float kw_xy = 20000; // D -static float ki_m_xy = 0.0; // I -static float i_range_m_xy = 1.0; - -// Yaw -static float kR_z = 60000; // P -static float kw_z = 12000; // D -static float ki_m_z = 500; // I -static float i_range_m_z = 1500; - -// roll and pitch angular velocity -static float kd_omega_rp = 200; // D - - -// Helper variables -static float i_error_x = 0; -static float i_error_y = 0; -static float i_error_z = 0; - -static float prev_omega_roll; -static float prev_omega_pitch; -static float prev_setpoint_omega_roll; -static float prev_setpoint_omega_pitch; - -static float i_error_m_x = 0; -static float i_error_m_y = 0; -static float i_error_m_z = 0; - -// Logging variables -static struct vec z_axis_desired; - -static float cmd_thrust; -static float cmd_roll; -static float cmd_pitch; -static float cmd_yaw; -static float r_roll; -static float r_pitch; -static float r_yaw; -static float accelz; - -void controllerMellingerReset(void) +// Global state variable used in the +// firmware as the only instance and in bindings +// to hold the default values +static controllerMellinger_t g_self = { + .mass = CF_MASS, + .massThrust = 132000, + + // XY Position PID + .kp_xy = 0.4, // P + .kd_xy = 0.2, // D + .ki_xy = 0.05, // I + .i_range_xy = 2.0, + + // Z Position + .kp_z = 1.25, // P + .kd_z = 0.4, // D + .ki_z = 0.05, // I + .i_range_z = 0.4, + + // Attitude + .kR_xy = 70000, // P + .kw_xy = 20000, // D + .ki_m_xy = 0.0, // I + .i_range_m_xy = 1.0, + + // Yaw + .kR_z = 60000, // P + .kw_z = 12000, // D + .ki_m_z = 500, // I + .i_range_m_z = 1500, + + // roll and pitch angular velocity + .kd_omega_rp = 200, // D + + + // Helper variables + .i_error_x = 0, + .i_error_y = 0, + .i_error_z = 0, + + .i_error_m_x = 0, + .i_error_m_y = 0, + .i_error_m_z = 0, +}; + + +void controllerMellingerReset(controllerMellinger_t* self) { - i_error_x = 0; - i_error_y = 0; - i_error_z = 0; - i_error_m_x = 0; - i_error_m_y = 0; - i_error_m_z = 0; + self->i_error_x = 0; + self->i_error_y = 0; + self->i_error_z = 0; + self->i_error_m_x = 0; + self->i_error_m_y = 0; + self->i_error_m_z = 0; } -void controllerMellingerInit(void) +void controllerMellingerInit(controllerMellinger_t* self) { - controllerMellingerReset(); + // copy default values (bindings), or does nothing (firmware) + *self = g_self; + + controllerMellingerReset(self); } -bool controllerMellingerTest(void) +bool controllerMellingerTest(controllerMellinger_t* self) { return true; } -void controllerMellinger(control_t *control, const setpoint_t *setpoint, +void controllerMellinger(controllerMellinger_t* self, control_t *control, const setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick) @@ -157,27 +148,27 @@ void controllerMellinger(control_t *control, const setpoint_t *setpoint, v_error = vsub(setpointVel, stateVel); // Integral Error - i_error_z += r_error.z * dt; - i_error_z = clamp(i_error_z, -i_range_z, i_range_z); + self->i_error_z += r_error.z * dt; + self->i_error_z = clamp(self->i_error_z, -self->i_range_z, self->i_range_z); - i_error_x += r_error.x * dt; - i_error_x = clamp(i_error_x, -i_range_xy, i_range_xy); + self->i_error_x += r_error.x * dt; + self->i_error_x = clamp(self->i_error_x, -self->i_range_xy, self->i_range_xy); - i_error_y += r_error.y * dt; - i_error_y = clamp(i_error_y, -i_range_xy, i_range_xy); + self->i_error_y += r_error.y * dt; + self->i_error_y = clamp(self->i_error_y, -self->i_range_xy, self->i_range_xy); // Desired thrust [F_des] if (setpoint->mode.x == modeAbs) { - target_thrust.x = g_vehicleMass * setpoint->acceleration.x + kp_xy * r_error.x + kd_xy * v_error.x + ki_xy * i_error_x; - target_thrust.y = g_vehicleMass * setpoint->acceleration.y + kp_xy * r_error.y + kd_xy * v_error.y + ki_xy * i_error_y; - target_thrust.z = g_vehicleMass * (setpoint->acceleration.z + GRAVITY_MAGNITUDE) + kp_z * r_error.z + kd_z * v_error.z + ki_z * i_error_z; + target_thrust.x = self->mass * setpoint->acceleration.x + self->kp_xy * r_error.x + self->kd_xy * v_error.x + self->ki_xy * self->i_error_x; + target_thrust.y = self->mass * setpoint->acceleration.y + self->kp_xy * r_error.y + self->kd_xy * v_error.y + self->ki_xy * self->i_error_y; + target_thrust.z = self->mass * (setpoint->acceleration.z + GRAVITY_MAGNITUDE) + self->kp_z * r_error.z + self->kd_z * v_error.z + self->ki_z * self->i_error_z; } else { target_thrust.x = -sinf(radians(setpoint->attitude.pitch)); target_thrust.y = -sinf(radians(setpoint->attitude.roll)); // In case of a timeout, the commander tries to level, ie. x/y are disabled, but z will use the previous setting // In that case we ignore the last feedforward term for acceleration if (setpoint->mode.z == modeAbs) { - target_thrust.z = g_vehicleMass * GRAVITY_MAGNITUDE + kp_z * r_error.z + kd_z * v_error.z + ki_z * i_error_z; + target_thrust.z = self->mass * GRAVITY_MAGNITUDE + self->kp_z * r_error.z + self->kd_z * v_error.z + self->ki_z * self->i_error_z; } else { target_thrust.z = 1; } @@ -213,7 +204,7 @@ void controllerMellinger(control_t *control, const setpoint_t *setpoint, current_thrust = vdot(target_thrust, z_axis); // Calculate axis [zB_des] - z_axis_desired = vnormalize(target_thrust); + self->z_axis_desired = vnormalize(target_thrust); // [xC_des] // x_axis_desired = z_axis_desired x [sin(yaw), cos(yaw), 0]^T @@ -221,9 +212,9 @@ void controllerMellinger(control_t *control, const setpoint_t *setpoint, x_c_des.y = sinf(radians(desiredYaw)); x_c_des.z = 0; // [yB_des] - y_axis_desired = vnormalize(vcross(z_axis_desired, x_c_des)); + y_axis_desired = vnormalize(vcross(self->z_axis_desired, x_c_des)); // [xB_des] - x_axis_desired = vcross(y_axis_desired, z_axis_desired); + x_axis_desired = vcross(y_axis_desired, self->z_axis_desired); // [eR] // Slow version @@ -246,8 +237,8 @@ void controllerMellinger(control_t *control, const setpoint_t *setpoint, float y = q.y; float z = q.z; float w = q.w; - eR.x = (-1 + 2*fsqr(x) + 2*fsqr(y))*y_axis_desired.z + z_axis_desired.y - 2*(x*y_axis_desired.x*z + y*y_axis_desired.y*z - x*y*z_axis_desired.x + fsqr(x)*z_axis_desired.y + fsqr(z)*z_axis_desired.y - y*z*z_axis_desired.z) + 2*w*(-(y*y_axis_desired.x) - z*z_axis_desired.x + x*(y_axis_desired.y + z_axis_desired.z)); - eR.y = x_axis_desired.z - z_axis_desired.x - 2*(fsqr(x)*x_axis_desired.z + y*(x_axis_desired.z*y - x_axis_desired.y*z) - (fsqr(y) + fsqr(z))*z_axis_desired.x + x*(-(x_axis_desired.x*z) + y*z_axis_desired.y + z*z_axis_desired.z) + w*(x*x_axis_desired.y + z*z_axis_desired.y - y*(x_axis_desired.x + z_axis_desired.z))); + eR.x = (-1 + 2*fsqr(x) + 2*fsqr(y))*y_axis_desired.z + self->z_axis_desired.y - 2*(x*y_axis_desired.x*z + y*y_axis_desired.y*z - x*y*self->z_axis_desired.x + fsqr(x)*self->z_axis_desired.y + fsqr(z)*self->z_axis_desired.y - y*z*self->z_axis_desired.z) + 2*w*(-(y*y_axis_desired.x) - z*self->z_axis_desired.x + x*(y_axis_desired.y + self->z_axis_desired.z)); + eR.y = x_axis_desired.z - self->z_axis_desired.x - 2*(fsqr(x)*x_axis_desired.z + y*(x_axis_desired.z*y - x_axis_desired.y*z) - (fsqr(y) + fsqr(z))*self->z_axis_desired.x + x*(-(x_axis_desired.x*z) + y*self->z_axis_desired.y + z*self->z_axis_desired.z) + w*(x*x_axis_desired.y + z*self->z_axis_desired.y - y*(x_axis_desired.x + self->z_axis_desired.z))); eR.z = y_axis_desired.x - 2*(y*(x*x_axis_desired.x + y*y_axis_desired.x - x*y_axis_desired.y) + w*(x*x_axis_desired.z + y*y_axis_desired.z)) + 2*(-(x_axis_desired.z*y) + w*(x_axis_desired.x + y_axis_desired.y) + x*y_axis_desired.z)*z - 2*y_axis_desired.x*fsqr(z) + x_axis_desired.y*(-1 + 2*fsqr(x) + 2*fsqr(z)); // Account for Crazyflie coordinate system @@ -264,65 +255,85 @@ void controllerMellinger(control_t *control, const setpoint_t *setpoint, ew.x = radians(setpoint->attitudeRate.roll) - stateAttitudeRateRoll; ew.y = -radians(setpoint->attitudeRate.pitch) - stateAttitudeRatePitch; ew.z = radians(setpoint->attitudeRate.yaw) - stateAttitudeRateYaw; - if (prev_omega_roll == prev_omega_roll) { /*d part initialized*/ - err_d_roll = ((radians(setpoint->attitudeRate.roll) - prev_setpoint_omega_roll) - (stateAttitudeRateRoll - prev_omega_roll)) / dt; - err_d_pitch = (-(radians(setpoint->attitudeRate.pitch) - prev_setpoint_omega_pitch) - (stateAttitudeRatePitch - prev_omega_pitch)) / dt; + if (self->prev_omega_roll == self->prev_omega_roll) { /*d part initialized*/ + err_d_roll = ((radians(setpoint->attitudeRate.roll) - self->prev_setpoint_omega_roll) - (stateAttitudeRateRoll - self->prev_omega_roll)) / dt; + err_d_pitch = (-(radians(setpoint->attitudeRate.pitch) - self->prev_setpoint_omega_pitch) - (stateAttitudeRatePitch - self->prev_omega_pitch)) / dt; } - prev_omega_roll = stateAttitudeRateRoll; - prev_omega_pitch = stateAttitudeRatePitch; - prev_setpoint_omega_roll = radians(setpoint->attitudeRate.roll); - prev_setpoint_omega_pitch = radians(setpoint->attitudeRate.pitch); + self->prev_omega_roll = stateAttitudeRateRoll; + self->prev_omega_pitch = stateAttitudeRatePitch; + self->prev_setpoint_omega_roll = radians(setpoint->attitudeRate.roll); + self->prev_setpoint_omega_pitch = radians(setpoint->attitudeRate.pitch); // Integral Error - i_error_m_x += (-eR.x) * dt; - i_error_m_x = clamp(i_error_m_x, -i_range_m_xy, i_range_m_xy); + self->i_error_m_x += (-eR.x) * dt; + self->i_error_m_x = clamp(self->i_error_m_x, -self->i_range_m_xy, self->i_range_m_xy); - i_error_m_y += (-eR.y) * dt; - i_error_m_y = clamp(i_error_m_y, -i_range_m_xy, i_range_m_xy); + self->i_error_m_y += (-eR.y) * dt; + self->i_error_m_y = clamp(self->i_error_m_y, -self->i_range_m_xy, self->i_range_m_xy); - i_error_m_z += (-eR.z) * dt; - i_error_m_z = clamp(i_error_m_z, -i_range_m_z, i_range_m_z); + self->i_error_m_z += (-eR.z) * dt; + self->i_error_m_z = clamp(self->i_error_m_z, -self->i_range_m_z, self->i_range_m_z); // Moment: - M.x = -kR_xy * eR.x + kw_xy * ew.x + ki_m_xy * i_error_m_x + kd_omega_rp * err_d_roll; - M.y = -kR_xy * eR.y + kw_xy * ew.y + ki_m_xy * i_error_m_y + kd_omega_rp * err_d_pitch; - M.z = -kR_z * eR.z + kw_z * ew.z + ki_m_z * i_error_m_z; + M.x = -self->kR_xy * eR.x + self->kw_xy * ew.x + self->ki_m_xy * self->i_error_m_x + self->kd_omega_rp * err_d_roll; + M.y = -self->kR_xy * eR.y + self->kw_xy * ew.y + self->ki_m_xy * self->i_error_m_y + self->kd_omega_rp * err_d_pitch; + M.z = -self->kR_z * eR.z + self->kw_z * ew.z + self->ki_m_z * self->i_error_m_z; // Output if (setpoint->mode.z == modeDisable) { control->thrust = setpoint->thrust; } else { - control->thrust = massThrust * current_thrust; + control->thrust = self->massThrust * current_thrust; } - cmd_thrust = control->thrust; - r_roll = radians(sensors->gyro.x); - r_pitch = -radians(sensors->gyro.y); - r_yaw = radians(sensors->gyro.z); - accelz = sensors->acc.z; + self->cmd_thrust = control->thrust; + self->r_roll = radians(sensors->gyro.x); + self->r_pitch = -radians(sensors->gyro.y); + self->r_yaw = radians(sensors->gyro.z); + self->accelz = sensors->acc.z; if (control->thrust > 0) { control->roll = clamp(M.x, -32000, 32000); control->pitch = clamp(M.y, -32000, 32000); control->yaw = clamp(-M.z, -32000, 32000); - cmd_roll = control->roll; - cmd_pitch = control->pitch; - cmd_yaw = control->yaw; + self->cmd_roll = control->roll; + self->cmd_pitch = control->pitch; + self->cmd_yaw = control->yaw; } else { control->roll = 0; control->pitch = 0; control->yaw = 0; - cmd_roll = control->roll; - cmd_pitch = control->pitch; - cmd_yaw = control->yaw; + self->cmd_roll = control->roll; + self->cmd_pitch = control->pitch; + self->cmd_yaw = control->yaw; - controllerMellingerReset(); + controllerMellingerReset(self); } } +#ifdef CRAZYFLIE_FW + +void controllerMellingerFirmwareInit(void) +{ + controllerMellingerInit(&g_self); +} + +bool controllerMellingerFirmwareTest(void) +{ + return controllerMellingerTest(&g_self); +} + +void controllerMellingerFirmware(control_t *control, const setpoint_t *setpoint, + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick) +{ + controllerMellinger(&g_self, control, setpoint, sensors, state, tick); +} + /** * Tunning variables for the full state Mellinger Controller @@ -331,79 +342,79 @@ PARAM_GROUP_START(ctrlMel) /** * @brief Position P-gain (horizontal xy plane) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kp_xy, &kp_xy) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kp_xy, &g_self.kp_xy) /** * @brief Position D-gain (horizontal xy plane) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kd_xy, &kd_xy) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kd_xy, &g_self.kd_xy) /** * @brief Position I-gain (horizontal xy plane) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, ki_xy, &ki_xy) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, ki_xy, &g_self.ki_xy) /** * @brief Attitude maximum accumulated error (roll and pitch) */ -PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_xy, &i_range_xy) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_xy, &g_self.i_range_xy) /** * @brief Position P-gain (vertical z plane) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kp_z, &kp_z) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kp_z, &g_self.kp_z) /** * @brief Position D-gain (vertical z plane) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kd_z, &kd_z) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kd_z, &g_self.kd_z) /** * @brief Position I-gain (vertical z plane) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, ki_z, &ki_z) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, ki_z, &g_self.ki_z) /** * @brief Position maximum accumulated error (vertical z plane) */ -PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_z, &i_range_z) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_z, &g_self.i_range_z) /** * @brief total mass [kg] */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, mass, &g_vehicleMass) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, mass, &g_self.mass) /** * @brief Force to PWM stretch factor */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, massThrust, &massThrust) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, massThrust, &g_self.massThrust) /** * @brief Attitude P-gain (roll and pitch) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kR_xy, &kR_xy) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kR_xy, &g_self.kR_xy) /** * @brief Attitude P-gain (yaw) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kR_z, &kR_z) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kR_z, &g_self.kR_z) /** * @brief Attitude D-gain (roll and pitch) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kw_xy, &kw_xy) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kw_xy, &g_self.kw_xy) /** * @brief Attitude D-gain (yaw) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kw_z, &kw_z) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kw_z, &g_self.kw_z) /** * @brief Attitude I-gain (roll and pitch) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, ki_m_xy, &ki_m_xy) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, ki_m_xy, &g_self.ki_m_xy) /** * @brief Attitude I-gain (yaw) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, ki_m_z, &ki_m_z) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, ki_m_z, &g_self.ki_m_z) /** * @brief Angular velocity D-Gain (roll and pitch) */ -PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kd_omega_rp, &kd_omega_rp) +PARAM_ADD_CORE(PARAM_FLOAT | PARAM_PERSISTENT, kd_omega_rp, &g_self.kd_omega_rp) /** * @brief Attitude maximum accumulated error (roll and pitch) */ -PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_m_xy, &i_range_m_xy) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_m_xy, &g_self.i_range_m_xy) /** * @brief Attitude maximum accumulated error (yaw) */ -PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_m_z, &i_range_m_z) +PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, i_range_m_z, &g_self.i_range_m_z) PARAM_GROUP_STOP(ctrlMel) /** @@ -411,18 +422,20 @@ PARAM_GROUP_STOP(ctrlMel) * Mellinger controller */ LOG_GROUP_START(ctrlMel) -LOG_ADD(LOG_FLOAT, cmd_thrust, &cmd_thrust) -LOG_ADD(LOG_FLOAT, cmd_roll, &cmd_roll) -LOG_ADD(LOG_FLOAT, cmd_pitch, &cmd_pitch) -LOG_ADD(LOG_FLOAT, cmd_yaw, &cmd_yaw) -LOG_ADD(LOG_FLOAT, r_roll, &r_roll) -LOG_ADD(LOG_FLOAT, r_pitch, &r_pitch) -LOG_ADD(LOG_FLOAT, r_yaw, &r_yaw) -LOG_ADD(LOG_FLOAT, accelz, &accelz) -LOG_ADD(LOG_FLOAT, zdx, &z_axis_desired.x) -LOG_ADD(LOG_FLOAT, zdy, &z_axis_desired.y) -LOG_ADD(LOG_FLOAT, zdz, &z_axis_desired.z) -LOG_ADD(LOG_FLOAT, i_err_x, &i_error_x) -LOG_ADD(LOG_FLOAT, i_err_y, &i_error_y) -LOG_ADD(LOG_FLOAT, i_err_z, &i_error_z) +LOG_ADD(LOG_FLOAT, cmd_thrust, &g_self.cmd_thrust) +LOG_ADD(LOG_FLOAT, cmd_roll, &g_self.cmd_roll) +LOG_ADD(LOG_FLOAT, cmd_pitch, &g_self.cmd_pitch) +LOG_ADD(LOG_FLOAT, cmd_yaw, &g_self.cmd_yaw) +LOG_ADD(LOG_FLOAT, r_roll, &g_self.r_roll) +LOG_ADD(LOG_FLOAT, r_pitch, &g_self.r_pitch) +LOG_ADD(LOG_FLOAT, r_yaw, &g_self.r_yaw) +LOG_ADD(LOG_FLOAT, accelz, &g_self.accelz) +LOG_ADD(LOG_FLOAT, zdx, &g_self.z_axis_desired.x) +LOG_ADD(LOG_FLOAT, zdy, &g_self.z_axis_desired.y) +LOG_ADD(LOG_FLOAT, zdz, &g_self.z_axis_desired.z) +LOG_ADD(LOG_FLOAT, i_err_x, &g_self.i_error_x) +LOG_ADD(LOG_FLOAT, i_err_y, &g_self.i_error_y) +LOG_ADD(LOG_FLOAT, i_err_z, &g_self.i_error_z) LOG_GROUP_STOP(ctrlMel) + +#endif // CRAZYFLIE_FW \ No newline at end of file diff --git a/test_python/test_controller_mellinger.py b/test_python/test_controller_mellinger.py index ad239a7988..9819927d98 100644 --- a/test_python/test_controller_mellinger.py +++ b/test_python/test_controller_mellinger.py @@ -4,7 +4,9 @@ def test_controller_mellinger(): - cffirmware.controllerMellingerInit() + ctrl = cffirmware.controllerMellinger_t() + + cffirmware.controllerMellingerInit(ctrl) control = cffirmware.control_t() setpoint = cffirmware.setpoint_t() @@ -35,7 +37,7 @@ def test_controller_mellinger(): tick = 100 - cffirmware.controllerMellinger(control, setpoint,sensors,state,tick) + cffirmware.controllerMellinger(ctrl, control, setpoint,sensors,state,tick) assert control.controlMode == cffirmware.controlModeLegacy # control.thrust will be at a (tuned) hover-state assert control.roll == 0 From 5b0dd9a8f7e2c27d54c00488990ea5972f6ccd4e Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 6 Jan 2023 10:40:35 +0100 Subject: [PATCH 2/2] remove unnecessary ifdef --- src/modules/src/controller_mellinger.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/src/controller_mellinger.c b/src/modules/src/controller_mellinger.c index e9b696e833..20cbc054fb 100644 --- a/src/modules/src/controller_mellinger.c +++ b/src/modules/src/controller_mellinger.c @@ -314,7 +314,6 @@ void controllerMellinger(controllerMellinger_t* self, control_t *control, const } } -#ifdef CRAZYFLIE_FW void controllerMellingerFirmwareInit(void) { @@ -437,5 +436,3 @@ LOG_ADD(LOG_FLOAT, i_err_x, &g_self.i_error_x) LOG_ADD(LOG_FLOAT, i_err_y, &g_self.i_error_y) LOG_ADD(LOG_FLOAT, i_err_z, &g_self.i_error_z) LOG_GROUP_STOP(ctrlMel) - -#endif // CRAZYFLIE_FW \ No newline at end of file