FBWA Rudder Mixing Logic + Motor Trim

stm32h753iitx/boardfiles/model/src/drivers.cpp

@@ -143,14 +143,14 @@ void initDrivers()
     rfdHandle->startReceive();
 
     // Motor instance bindings
-    leftAileronMotorInstance = {leftAileronMotorHandle, true};
-    rightAileronMotorInstance = {rightAileronMotorHandle, true};
-    elevatorMotorInstance = {elevatorMotorHandle, false};
-    rudderMotorInstance = {rudderMotorHandle, false};
-    throttleMotorInstance = {throttleMotorHandle, false};
-    leftFlapMotorInstance = {leftFlapMotorHandle, false};
-    rightFlapMotorInstance = {rightFlapMotorHandle, true};
-    steeringMotorInstance = {steeringMotorHandle, true};
+    leftAileronMotorInstance = {leftAileronMotorHandle, true, 0};
+    rightAileronMotorInstance = {rightAileronMotorHandle, true, 0};
+    elevatorMotorInstance = {elevatorMotorHandle, false, 0};
+    rudderMotorInstance = {rudderMotorHandle, false, 0};
+    throttleMotorInstance = {throttleMotorHandle, false, 0};
+    leftFlapMotorInstance = {leftFlapMotorHandle, false, 0};
+    rightFlapMotorInstance = {rightFlapMotorHandle, true, 0};
+    steeringMotorInstance = {steeringMotorHandle, true, 0};
 
     aileronMotorInstances[0] = leftAileronMotorInstance;
     aileronMotorInstances[1] = rightAileronMotorInstance;

stm32l552xx/boardfiles/model/src/drivers.cpp

@@ -139,14 +139,14 @@ void initDrivers()
     rfdHandle->startReceive();
 
     // Motor instance bindings
-    leftAileronMotorInstance = {leftAileronMotorHandle, true};
-    rightAileronMotorInstance = {rightAileronMotorHandle, true};
-    elevatorMotorInstance = {elevatorMotorHandle, false};
-    rudderMotorInstance = {rudderMotorHandle, false};
-    throttleMotorInstance = {throttleMotorHandle, false};
-    leftFlapMotorInstance = {leftFlapMotorHandle, false};
-    rightFlapMotorInstance = {rightFlapMotorHandle, true};
-    steeringMotorInstance = {steeringMotorHandle, true};
+    leftAileronMotorInstance = {leftAileronMotorHandle, true, 0};
+    rightAileronMotorInstance = {rightAileronMotorHandle, true, 0};
+    elevatorMotorInstance = {elevatorMotorHandle, false, 0};
+    rudderMotorInstance = {rudderMotorHandle, false, 0};
+    throttleMotorInstance = {throttleMotorHandle, false, 0};
+    leftFlapMotorInstance = {leftFlapMotorHandle, false, 0};
+    rightFlapMotorInstance = {rightFlapMotorHandle, true, 0};
+    steeringMotorInstance = {steeringMotorHandle, true, 0};
 
     aileronMotorInstances[0] = leftAileronMotorInstance;
     aileronMotorInstances[1] = rightAileronMotorInstance;

zeropilot4.0/include/attitude_manager/fbwa_mapping.hpp

@@ -15,6 +15,9 @@ class FBWAMapping : public Flightmode {
         // Setter for *pitch* PID consts
         void setPitchPIDConstants(float newKp, float newKi, float newKd, float newTau) noexcept;
 
+        // Setter for *yaw* rudder mixing const
+        void setYawRudderMixingConstant(float newMixingConst) noexcept;
+
         // Destructor
         ~FBWAMapping() noexcept override;
 
@@ -23,6 +26,9 @@ class FBWAMapping : public Flightmode {
         PID rollPID;
         PID pitchPID;
 
+        // Yaw rudder mixing constant
+        float yawRudderMixingConst;
+
         // Roll integral limits
         static constexpr float ROLL_INTEGRAL_MIN_LIM = -50.0f;
         static constexpr float ROLL_INTEGRAL_MAX_LIM = +50.0f;

zeropilot4.0/include/driver_ifaces/motor_datatype.hpp

@@ -5,6 +5,7 @@
 typedef struct {
     IMotorControl *motorInstance; 
     bool isInverted;
+    int trim; // trim value to adjust motor output, can be positive or negative in range [-50,50]
 } MotorInstance_t;
 
 typedef struct {   

zeropilot4.0/src/attitude_manager/attitude_manager.cpp

@@ -162,12 +162,21 @@ void AttitudeManager::outputToMotor(ControlAxis_t axis, uint8_t percent) {
     for (uint8_t i = 0; i < motorGroup->motorCount; i++) {
         MotorInstance_t *motor = (motorGroup->motors + i);
 
-        if (motor->isInverted) {
-            motor->motorInstance->set(100 - percent);
+        int32_t cmd = (int32_t)percent + motor->trim;
+
+        // Clamp cmd to [0, 100]
+        if (cmd > 100) {
+            cmd = 100;
+        } else if (cmd < 0) {
+            cmd = 0;
         }
-        else {
-            motor->motorInstance->set(percent);
+
+        // Invert command if motor is inverted
+        if (motor->isInverted) {
+            cmd = 100 - cmd;
         }
+
+        motor->motorInstance->set(cmd);
     }
 }
 

zeropilot4.0/src/attitude_manager/fbwa_mapping.cpp

@@ -8,7 +8,8 @@ FBWAMapping::FBWAMapping(float control_iter_period_s) noexcept :
         ROLL_INTEGRAL_MIN_LIM, ROLL_INTEGRAL_MAX_LIM, control_iter_period_s),
     pitchPID(0.0f, 0.0f, 0.0f,
         0.0f, OUTPUT_MIN, OUTPUT_MAX,
-        PITCH_INTEGRAL_MIN_LIM, PITCH_INTEGRAL_MAX_LIM, control_iter_period_s)
+        PITCH_INTEGRAL_MIN_LIM, PITCH_INTEGRAL_MAX_LIM, control_iter_period_s),
+    yawRudderMixingConst(0.0f)
 {
     rollPID.pidInitState();
     pitchPID.pidInitState();
@@ -26,6 +27,12 @@ void FBWAMapping::setPitchPIDConstants(float newKp, float newKi, float newKd, fl
     pitchPID.setConstants(newKp, newKi, newKd, newTau);
 }
 
+// Setter for *yaw* rudder mixing const
+void FBWAMapping::setYawRudderMixingConstant(float newMixingConst) noexcept {
+    yawRudderMixingConst = newMixingConst;
+}
+
+// Main control mapping function for FBWA mode
 RCMotorControlMessage_t FBWAMapping::runControl(RCMotorControlMessage_t controlInputs, const DroneState_t &droneState){
     // Convert RC inputs into radians
     float rollSetpoint = (controlInputs.roll / MAX_RC_INPUT_VAL) * (ROLL_MAX_ANGLE_RAD - ROLL_MIN_ANGLE_RAD) + ROLL_MIN_ANGLE_RAD;
@@ -42,5 +49,14 @@ RCMotorControlMessage_t FBWAMapping::runControl(RCMotorControlMessage_t controlI
     controlInputs.roll = rollOutput + FBWA_PID_OUTPUT_SHIFT; // setting desired roll angle, adding 50 to shift to [0,100] range
     controlInputs.pitch = pitchOutput + FBWA_PID_OUTPUT_SHIFT; // setting desired pitch angle, adding 50 to shift to [0,100] range
 
+    // Yaw control via rudder mixing
+    float aileronSignalCentered = controlInputs.roll - (MAX_RC_INPUT_VAL / 2.0f); // Centering aileron signal around 0 for mixing calculation
+    controlInputs.yaw += (yawRudderMixingConst * aileronSignalCentered); // Yaw adjustment based on roll PID output and mixing constant
+    if (controlInputs.yaw < 0.0f) {
+        controlInputs.yaw = 0.0f; // Ensuring yaw does not go below 0
+    } else if (controlInputs.yaw > MAX_RC_INPUT_VAL) {
+        controlInputs.yaw = MAX_RC_INPUT_VAL; // Ensuring yaw does not exceed max RC input value
+    }
+
     return controlInputs;
 }