diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index d5dfeebe48..e115f060ea 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -7,7 +7,7 @@ * * Crazyflie control firmware * - * Copyright (C) 2011-2012 Bitcraze AB + * Copyright (C) 2011-2022 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 @@ -167,15 +167,43 @@ typedef struct state_s { acc_t acc; // Gs (but acc.z without considering gravity) } state_t; +#define STABILIZER_NR_OF_MOTORS 4 + +typedef enum control_mode_e { + controlModeLegacy = 0, // legacy mode with int16_t roll, pitch, yaw and float thrust + controlModeForceTorque = 1, + controlModeForce = 2, +} control_mode_t; + typedef struct control_s { - int16_t roll; - int16_t pitch; - int16_t yaw; - float thrust; -} control_t; + union { + // controlModeLegacy + struct { + int16_t roll; + int16_t pitch; + int16_t yaw; + float thrust; + }; + // controlModeForceTorque + struct { + float thrustSi; // N + union { // Nm + float torque[3]; + struct { + float torqueX; + float torqueY; + float torqueZ; + }; + }; + }; -#define STABILIZER_NR_OF_MOTORS 4 + // controlModeForce + float normalizedForces[STABILIZER_NR_OF_MOTORS]; // 0.0 ... 1.0 + }; + + control_mode_t controlMode; +} control_t; typedef union { int32_t list[STABILIZER_NR_OF_MOTORS]; diff --git a/src/modules/src/controller_indi.c b/src/modules/src/controller_indi.c index 7483edaf84..63ef120be9 100644 --- a/src/modules/src/controller_indi.c +++ b/src/modules/src/controller_indi.c @@ -143,10 +143,11 @@ bool controllerINDITest(void) } void controllerINDI(control_t *control, setpoint_t *setpoint, - const sensorData_t *sensors, - const state_t *state, - const uint32_t tick) + const sensorData_t *sensors, + const state_t *state, + const uint32_t tick) { + control->controlMode = controlModeLegacy; //The z_distance decoder adds a negative sign to the yaw command, the position decoder doesn't if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { @@ -194,7 +195,7 @@ void controllerINDI(control_t *control, setpoint_t *setpoint, // INDI position controller not active, INDI attitude controller is main loop attitudeDesired.roll = radians(setpoint->attitude.roll); //no sign conversion as CF coords is equal to NED for roll - + }else{ if (outerLoopActive) { // INDI position controller active, INDI attitude controller becomes inner loop @@ -213,7 +214,7 @@ void controllerINDI(control_t *control, setpoint_t *setpoint, attitudeDesired.pitch = refOuterINDI.y; //outer loop provides radians } } - + //Proportional controller on attitude angles [rad] rateDesired.roll = indi.reference_acceleration.err_p*(attitudeDesired.roll - radians(state->attitude.roll)); rateDesired.pitch = indi.reference_acceleration.err_q*(attitudeDesired.pitch - radians(state->attitude.pitch)); @@ -235,7 +236,7 @@ void controllerINDI(control_t *control, setpoint_t *setpoint, * 1 - Update the gyro filter with the new measurements. */ - body_rates.p = radians(sensors->gyro.x); + body_rates.p = radians(sensors->gyro.x); body_rates.q = -radians(sensors->gyro.y); //Account for gyro measuring pitch rate in opposite direction relative to both the CF coords and INDI coords body_rates.r = -radians(sensors->gyro.z); //Account for conversion of ENU -> NED @@ -262,7 +263,7 @@ void controllerINDI(control_t *control, setpoint_t *setpoint, */ //Calculate the attitude rate error, using the unfiltered gyroscope measurements (only the preapplied filters in bmi088) - float attitude_error_p = rateDesired.roll - body_rates.p; + float attitude_error_p = rateDesired.roll - body_rates.p; float attitude_error_q = rateDesired.pitch - body_rates.q; float attitude_error_r = rateDesired.yaw - body_rates.r; @@ -348,28 +349,28 @@ PARAM_ADD(PARAM_FLOAT, thrust_threshold, &thrust_threshold) PARAM_ADD(PARAM_FLOAT, bound_ctrl_input, &bound_control_input) /** - * @brief INDI Controller effeciveness G1 p + * @brief INDI Controller effeciveness G1 p */ PARAM_ADD(PARAM_FLOAT, g1_p, &indi.g1.p) /** - * @brief INDI Controller effectiveness G1 q + * @brief INDI Controller effectiveness G1 q */ PARAM_ADD(PARAM_FLOAT, g1_q, &indi.g1.q) /** - * @brief INDI Controller effectiveness G1 r + * @brief INDI Controller effectiveness G1 r */ PARAM_ADD(PARAM_FLOAT, g1_r, &indi.g1.r) /** - * @brief INDI Controller effectiveness G2 + * @brief INDI Controller effectiveness G2 */ PARAM_ADD(PARAM_FLOAT, g2, &indi.g2) /** - * @brief INDI proportional gain, attitude error p + * @brief INDI proportional gain, attitude error p */ PARAM_ADD(PARAM_FLOAT, ref_err_p, &indi.reference_acceleration.err_p) /** - * @brief INDI proportional gain, attitude error q + * @brief INDI proportional gain, attitude error q */ PARAM_ADD(PARAM_FLOAT, ref_err_q, &indi.reference_acceleration.err_q) /** @@ -413,7 +414,7 @@ PARAM_ADD(PARAM_FLOAT, filt_cutoff, &indi.filt_cutoff) PARAM_ADD(PARAM_FLOAT, filt_cutoff_r, &indi.filt_cutoff_r) /** - * @brief Activate INDI for position control + * @brief Activate INDI for position control */ PARAM_ADD(PARAM_UINT8, outerLoopActive, &outerLoopActive) diff --git a/src/modules/src/controller_mellinger.c b/src/modules/src/controller_mellinger.c index 521c4edf57..67226fec57 100644 --- a/src/modules/src/controller_mellinger.c +++ b/src/modules/src/controller_mellinger.c @@ -138,6 +138,8 @@ void controllerMellinger(control_t *control, setpoint_t *setpoint, float dt; float desiredYaw = 0; //deg + control->controlMode = controlModeLegacy; + if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { return; } diff --git a/src/modules/src/controller_pid.c b/src/modules/src/controller_pid.c index 1cdf12a6f9..c88cd613b8 100644 --- a/src/modules/src/controller_pid.c +++ b/src/modules/src/controller_pid.c @@ -58,11 +58,13 @@ void controllerPid(control_t *control, setpoint_t *setpoint, const state_t *state, const uint32_t tick) { + control->controlMode = controlModeLegacy; + if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { // Rate-controled YAW is moving YAW angle setpoint if (setpoint->mode.yaw == modeVelocity) { attitudeDesired.yaw = capAngle(attitudeDesired.yaw + setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT); - + float yawMaxDelta = attitudeControllerGetYawMaxDelta(); if (yawMaxDelta != 0.0f) { diff --git a/src/modules/src/power_distribution_flapper.c b/src/modules/src/power_distribution_flapper.c index 848daaa259..495d72b4ab 100644 --- a/src/modules/src/power_distribution_flapper.c +++ b/src/modules/src/power_distribution_flapper.c @@ -36,6 +36,7 @@ #include "debug.h" #include "math.h" +#include "cfassert.h" #ifndef CONFIG_MOTORS_DEFAULT_IDLE_THRUST # define DEFAULT_IDLE_THRUST 0 @@ -151,6 +152,9 @@ uint16_t limitThrust(int32_t value, int32_t min, int32_t max) void powerDistribution(const control_t *control, motors_thrust_uncapped_t* motorThrustUncapped) { + // Only legacy mode is currently supported + ASSERT(control->controlMode == controlModeLegacy); + thrust = fmin(control->thrust, flapperConfig.maxThrust); flapperConfig.pitchServoNeutral=limitServoNeutral(flapperConfig.pitchServoNeutral); diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c index 55fc1b798a..cda7d8ccfa 100644 --- a/src/modules/src/power_distribution_quadrotor.c +++ b/src/modules/src/power_distribution_quadrotor.c @@ -32,6 +32,7 @@ #include "num.h" #include "autoconf.h" #include "config.h" +#include "math.h" #ifndef CONFIG_MOTORS_DEFAULT_IDLE_THRUST # define DEFAULT_IDLE_THRUST 0 @@ -40,6 +41,12 @@ #endif static uint32_t idleThrust = DEFAULT_IDLE_THRUST; +static float armLength = 0.046f; // m; +static float thrustToTorque = 0.005964552f; + +// thrust = a * pwm^2 + b * pwm +static float pwmToThrustA = 0.091492681f; +static float pwmToThrustB = 0.067673604f; int powerDistributionMotorType(uint32_t id) { @@ -69,7 +76,7 @@ static uint16_t capMinThrust(float thrust, uint32_t minThrust) { return thrust; } -void powerDistribution(const control_t *control, motors_thrust_uncapped_t* motorThrustUncapped) +static void powerDistributionLegacy(const control_t *control, motors_thrust_uncapped_t* motorThrustUncapped) { int16_t r = control->roll / 2.0f; int16_t p = control->pitch / 2.0f; @@ -80,6 +87,53 @@ void powerDistribution(const control_t *control, motors_thrust_uncapped_t* motor motorThrustUncapped->motors.m4 = control->thrust + r + p - control->yaw; } +static void powerDistributionForceTorque(const control_t *control, motors_thrust_uncapped_t* motorThrustUncapped) { + static float motorForces[STABILIZER_NR_OF_MOTORS]; + + const float arm = 0.707106781f * armLength; + const float rollPart = 0.25f / arm * control->torqueX; + const float pitchPart = 0.25f / arm * control->torqueY; + const float thrustPart = 0.25f * control->thrustSi; // N (per rotor) + const float yawPart = 0.25f * control->torqueZ / thrustToTorque; + + motorForces[0] = thrustPart - rollPart - pitchPart - yawPart; + motorForces[1] = thrustPart - rollPart + pitchPart + yawPart; + motorForces[2] = thrustPart + rollPart + pitchPart - yawPart; + motorForces[3] = thrustPart + rollPart - pitchPart + yawPart; + + for (int motorIndex = 0; motorIndex < STABILIZER_NR_OF_MOTORS; motorIndex++) { + float motorForce = motorForces[motorIndex]; + if (motorForce < 0.0f) { + motorForce = 0.0f; + } + + float motor_pwm = (-pwmToThrustB + sqrtf(pwmToThrustB * pwmToThrustB + 4.0f * pwmToThrustA * motorForce)) / (2.0f * pwmToThrustA); + motorThrustUncapped->list[motorIndex] = motor_pwm * UINT16_MAX; + } +} + +static void powerDistributionForce(const control_t *control, motors_thrust_uncapped_t* motorThrustUncapped) { + // Not implemented yet +} + +void powerDistribution(const control_t *control, motors_thrust_uncapped_t* motorThrustUncapped) +{ + switch (control->controlMode) { + case controlModeLegacy: + powerDistributionLegacy(control, motorThrustUncapped); + break; + case controlModeForceTorque: + powerDistributionForceTorque(control, motorThrustUncapped); + break; + case controlModeForce: + powerDistributionForce(control, motorThrustUncapped); + break; + default: + // Nothing here + break; + } +} + void powerDistributionCap(const motors_thrust_uncapped_t* motorThrustBatCompUncapped, motors_thrust_pwm_t* motorPwm) { const int32_t maxAllowedThrust = UINT16_MAX; @@ -121,3 +175,20 @@ PARAM_GROUP_START(powerDist) */ PARAM_ADD_CORE(PARAM_UINT32 | PARAM_PERSISTENT, idleThrust, &idleThrust) PARAM_GROUP_STOP(powerDist) + +/** + * System identification parameters for quad rotor + */ +PARAM_GROUP_START(quadSysId) + +PARAM_ADD(PARAM_FLOAT, thrustToTorque, &thrustToTorque) +PARAM_ADD(PARAM_FLOAT, pwmToThrustA, &pwmToThrustA) +PARAM_ADD(PARAM_FLOAT, pwmToThrustB, &pwmToThrustB) + +/** + * @brief Length of arms (m) + * + * The distance from the center to a motor + */ +PARAM_ADD(PARAM_FLOAT, armLength, &armLength) +PARAM_GROUP_STOP(quadSysId) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 455ca2e8c3..9a3f417579 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -7,7 +7,7 @@ * * Crazyflie Firmware * - * Copyright (C) 2011-2016 Bitcraze AB + * Copyright (C) 2011-2022 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 diff --git a/test_python/test_power_distribution.py b/test_python/test_power_distribution.py index d115833d2e..9ecfe42410 100644 --- a/test_python/test_power_distribution.py +++ b/test_python/test_power_distribution.py @@ -2,13 +2,15 @@ import cffirmware -def test_power_distribution_for_hover(): + +def test_power_distribution_legacy_for_hover(): # Fixture control = cffirmware.control_t() control.thrust = 10 control.roll = 0 control.pitch = 0 control.yaw = 0 + control.controlMode = cffirmware.controlModeLegacy actual = cffirmware.motors_thrust_uncapped_t() @@ -16,19 +18,20 @@ def test_power_distribution_for_hover(): cffirmware.powerDistribution(control, actual) # Assert - # control.thrust will be at a (tuned) hover-state assert actual.motors.m1 == control.thrust assert actual.motors.m2 == control.thrust assert actual.motors.m3 == control.thrust assert actual.motors.m4 == control.thrust -def test_power_distribution_for_roll(): + +def test_power_distribution_legacy_for_roll(): # Fixture control = cffirmware.control_t() control.thrust = 0 control.roll = 10 control.pitch = 0 control.yaw = 0 + control.controlMode = cffirmware.controlModeLegacy actual = cffirmware.motors_thrust_uncapped_t() @@ -36,19 +39,20 @@ def test_power_distribution_for_roll(): cffirmware.powerDistribution(control, actual) # Assert - # control.thrust will be at a (tuned) hover-state assert actual.motors.m1 == -control.roll / 2 assert actual.motors.m2 == -control.roll / 2 assert actual.motors.m3 == control.roll / 2 assert actual.motors.m4 == control.roll / 2 -def test_power_distribution_for_pitch(): + +def test_power_distribution_legacy_for_pitch(): # Fixture control = cffirmware.control_t() control.thrust = 0 control.roll = 0 control.pitch = 12 control.yaw = 0 + control.controlMode = cffirmware.controlModeLegacy actual = cffirmware.motors_thrust_uncapped_t() @@ -56,19 +60,20 @@ def test_power_distribution_for_pitch(): cffirmware.powerDistribution(control, actual) # Assert - # control.thrust will be at a (tuned) hover-state assert actual.motors.m1 == control.pitch / 2 assert actual.motors.m2 == -control.pitch / 2 assert actual.motors.m3 == -control.pitch / 2 assert actual.motors.m4 == control.pitch / 2 -def test_power_distribution_for_yaw(): + +def test_power_distribution_legacy_for_yaw(): # Fixture control = cffirmware.control_t() control.thrust = 0 control.roll = 0 control.pitch = 0 control.yaw = 20 + control.controlMode = cffirmware.controlModeLegacy actual = cffirmware.motors_thrust_uncapped_t() @@ -76,12 +81,34 @@ def test_power_distribution_for_yaw(): cffirmware.powerDistribution(control, actual) # Assert - # control.thrust will be at a (tuned) hover-state assert actual.motors.m1 == control.yaw assert actual.motors.m2 == -control.yaw assert actual.motors.m3 == control.yaw assert actual.motors.m4 == -control.yaw + +def test_power_distribution_force_torque(): + # Fixture + control = cffirmware.control_t() + control.thrustSi = 0.1 + control.torqueX = 0 + control.torqueY = 0 + control.torqueZ = 0 + control.controlMode = cffirmware.controlModeForceTorque + + actual = cffirmware.motors_thrust_uncapped_t() + + # Test + cffirmware.powerDistribution(control, actual) + + # Assert + # For now only check that we get a non zero output + assert actual.motors.m1 > 0 + assert actual.motors.m2 > 0 + assert actual.motors.m3 > 0 + assert actual.motors.m4 > 0 + + def test_power_distribution_cap_when_in_range(): # Fixture input = cffirmware.motors_thrust_uncapped_t() @@ -102,6 +129,7 @@ def test_power_distribution_cap_when_in_range(): assert actual.motors.m3 == input.motors.m3 assert actual.motors.m4 == input.motors.m4 + def test_power_distribution_cap_when_all_negative(): # Fixture input = cffirmware.motors_thrust_uncapped_t() @@ -121,6 +149,7 @@ def test_power_distribution_cap_when_all_negative(): assert actual.motors.m3 == 0 assert actual.motors.m4 == 0 + def test_power_distribution_cap_when_all_above_range(): # Fixture input = cffirmware.motors_thrust_uncapped_t() @@ -140,6 +169,7 @@ def test_power_distribution_cap_when_all_above_range(): assert actual.motors.m3 == 0xffff assert actual.motors.m4 == 0xffff + def test_power_distribution_cap_reduces_thrust_equally_much(): # Fixture input = cffirmware.motors_thrust_uncapped_t() @@ -159,6 +189,7 @@ def test_power_distribution_cap_reduces_thrust_equally_much(): assert actual.motors.m3 == 0xffff - 5 assert actual.motors.m4 == 0xffff - 0 + def test_power_distribution_cap_reduces_thrust_equally_much_with_lower_cap(): # Fixture input = cffirmware.motors_thrust_uncapped_t()