Add SI-units to control structure

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];

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)
 

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;
   }

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)
       {

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);

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)

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