Merge branch '6dof-mixer-upgrade' into 'master'

6dof mixer upgrade

See merge request !4

src/main/CMakeLists.txt

@@ -546,6 +546,8 @@ main_sources(COMMON_SRC
     navigation/navigation_fixedwing.c
     navigation/navigation_fw_launch.c
     navigation/navigation_geo.c
+    navigation/navigation_delta_controller.c
+    navigation/navigation_delta_controller.h
     navigation/navigation_multicopter.c
     navigation/navigation_pos_estimator.c
     navigation/navigation_pos_estimator_private.h

src/main/flight/mixer.c

@@ -52,7 +52,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/servos.h"
 
 #include "navigation/navigation.h"
-
+#include "navigation/navigation_delta_controller.h"
 #include "rx/rx.h"
 
 #include "sensors/battery.h"
@@ -60,6 +60,7 @@ FILE_COMPILE_FOR_SPEED
 FASTRAM int16_t mixerForwardCommand = 0;
 FASTRAM int16_t mixerLateralCommand = 0;
 FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS];
+FASTRAM motorMixer_t delta_motor_mixer[MAX_SUPPORTED_MOTORS];
 FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 static float motorMixRange;
 static float mixerScale = 1.0f;
@@ -75,6 +76,7 @@ static EXTENDED_FASTRAM int throttleRangeMin = 0;
 static EXTENDED_FASTRAM int throttleRangeMax = 0;
 static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1;
 
+
 int motorZeroCommand = 0;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
@@ -294,6 +296,7 @@ void mixerInit(void)
     } else {
         motorYawMultiplier = 1;
     }
+
 }
 
 void mixerResetDisarmedMotors(void)
@@ -351,10 +354,12 @@ static uint16_t handleOutputScaling(
     }
     return value;
 }
+
 static void applyFlipOverAfterCrashModeToMotors(void) {
 
     if (ARMING_FLAG(ARMED)) {
         const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f;
+        const float stickDeflectionAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
         const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
         const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
         const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
@@ -577,8 +582,8 @@ void FAST_CODE mixTable(const float dT)
         input[ROLL] = axisPID[ROLL];
         input[PITCH] = axisPID[PITCH];
         input[YAW] = axisPID[YAW];
-        input[3] = mixerForwardCommand;
-        input[4] = mixerLateralCommand;
+        input[3] = forward_thrust_command;
+        input[4] = lateral_thrust_command;
     }
 
     // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
@@ -675,7 +680,13 @@ void FAST_CODE mixTable(const float dT)
     if (ARMING_FLAG(ARMED)) {
         const motorStatus_e currentMotorStatus = getMotorStatus();
         for (int i = 0; i < motorCount; i++) {
-            motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
+            if(!is_delta_thruster(i)) {
+                motor[i] = rpyMix[i] +
+                           constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
+            }
+            else{
+                constrain(rpyMix[i], throttleMin, throttleMax);
+            }
 
             if (failsafeIsActive()) {
                 motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
@@ -750,7 +761,9 @@ motorStatus_e getMotorStatus(void)
 void loadPrimaryMotorMixer(void) {
     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
         currentMixer[i] = *primaryMotorMixer(i);
+        delta_motor_mixer[i] = *primaryMotorMixer(i);
     }
+    init_delta_thrusters();
 }
 
 bool areMotorsRunning(void)
@@ -767,3 +780,10 @@ bool areMotorsRunning(void)
 
     return false;
 }
+
+struct motorMixer_s getMotorMixerRow(int row){
+    if(row < MAX_SUPPORTED_MOTORS) {
+        return currentMixer[row];
+    }
+    return  currentMixer[0];
+}

src/main/flight/mixer.h

@@ -116,7 +116,7 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 extern int mixerThrottleCommand;
 extern int16_t mixerForwardCommand;
 extern int16_t mixerLateralCommand;
-
+extern struct motorMixer_s delta_motor_mixer[MAX_SUPPORTED_MOTORS];
 int getThrottleIdleValue(void);
 int16_t getThrottlePercent(void);
 uint8_t getMotorCount(void);
@@ -137,4 +137,6 @@ void stopMotors(void);
 void stopPwmAllMotors(void);
 
 void loadPrimaryMotorMixer(void);
-bool areMotorsRunning(void);
\ No newline at end of file
+bool areMotorsRunning(void);
+
+struct motorMixer_s getMotorMixerRow(int row_number);
\ No newline at end of file

src/main/navigation/navigation_delta_controller.c

+//
+// Created by Sam Winkelstein 4/26/21
+//
+
+#include "navigation/navigation_delta_controller.h"
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+#include "platform.h"
+
+FILE_COMPILE_FOR_SPEED
+
+#include "build/debug.h"
+#include "common/axis.h"
+#include "common/filter.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "config/feature.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "drivers/pwm_output.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/time.h"
+
+#include "fc/config.h"
+#include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
+#include "fc/runtime_config.h"
+#include "fc/controlrate_profile.h"
+#include "fc/settings.h"
+#include "fc/cli.h"
+
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "flight/servos.h"
+
+#include "navigation/navigation.h"
+
+#include "rx/rx.h"
+
+#include "sensors/battery.h"
+
+FASTRAM int16_t forward_thrust_command = 1000;
+FASTRAM int16_t lateral_thrust_command = 1000;
+
+FASTRAM deltaThruster_t thrusters[MAX_SUPPORTED_MOTORS];
+FASTRAM int thruster_count;
+static const float acceptable_angle_error = .05; //about 3 degrees
+
+float lateral_thrust_roll(float desired_pitch_angle);
+float forward_thrust_pitch(float desired_pitch_angle);
+float get_thruster_pitch(float throttle, float forward);
+float get_thruster_roll(float throttle, float lateral);
+const float dead_band = .15f;
+
+void init_delta_thrusters() {
+    thruster_count = 0;                                              //no thrusters by default
+    int i = 0;
+    for(i = 0; i < MAX_SUPPORTED_MOTORS; i++){
+        motorMixer_t m = delta_motor_mixer[i];
+        float z = m.throttle;                                       // local z component
+        float x = m.forward;                                        // local x component
+        float y = m.lateral;                                        // local y component
+        bool is_thruster = false;                                     // nothing is a thruster by default
+        if (z != 0) {                                                 // if z thrust is zero then it has to be a thruster
+            is_thruster = ((abs(atan2(z, x)) > .78) || (abs(atan2(z, x)) > .78));
+        }
+        else if (x != 0 || y != 0){
+            is_thruster = true;
+        }
+            if (is_thruster) {                                                 // this is my current cutoff I might change it later
+                thrusters[thruster_count].vertical = z;                       //components based on local cooridinates
+                thrusters[thruster_count].lateral = y;
+                thrusters[thruster_count].forward = x;
+                thrusters[thruster_count].pitch = get_thruster_pitch(z, x);
+                thrusters[thruster_count].roll = get_thruster_pitch(z, y);
+                m.throttle = 0.0;
+                thruster_count++;
+            }//increase thruster count for calculation
+    }
+}
+
+float get_thruster_pitch(float throttle, float forward){
+    float pitch = 0;
+    if(throttle != 0) {
+        pitch = atan2(forward, throttle);
+    }
+    return pitch;
+}
+// calculate desired pitch if thrusters are active
+float get_thruster_roll(float throttle, float lateral) {    // calculate desired pitch if thrusters are active
+    float roll = 0;
+    if (throttle != 0){
+        roll = atan2(lateral, throttle);   //using atan roll
+    }
+    return roll;
+}
+
+float get_delta_pitch_angle( float desired_pitch_angle){
+
+    float pitch_angle = desired_pitch_angle;
+    //note this means output thruster
+        float power_level = desired_pitch_angle*1.25;
+        if (power_level > dead_band || power_level < -dead_band) {
+            pitch_angle = get_delta_pitch_angle(desired_pitch_angle);
+            forward_thrust_command = constrain(2000* -desired_pitch_angle * 1.25, -2000, 2000); //range 1000 to 2000
+        }
+        else {
+            forward_thrust_command = 0;
+        }
+    return pitch_angle;
+    }
+
+float get_delta_roll_angle( float desired_roll_angle){
+    float roll_angle = desired_roll_angle;
+    //note this means output thruster
+    float power_level = desired_roll_angle*1.25;
+    if (power_level > dead_band || power_level < -dead_band){
+        roll_angle = forward_thrust_pitch(desired_roll_angle);
+        lateral_thrust_command = constrain(2000* -desired_roll_angle * 1.25, -2000, 2000); //range 1000 to 2000
+    }
+    else {
+        lateral_thrust_command = 0;
+    }
+    return roll_angle;
+}
+
+//hopefully this is still fast enough to still be used
+// I might make these private
+float forward_thrust_pitch(float desired_pitch_angle){
+    float pitch_angle = 0;
+    float component_count = 0;
+    for(int i = 0; i < thruster_count; i++) {
+        deltaThruster_t thruster = thrusters[i];
+        if (desired_pitch_angle < 0) {
+            if (thruster.forward > 0) {
+                pitch_angle += thruster.pitch * thruster.forward;
+                component_count += thruster.forward;
+            }
+        } else if (desired_pitch_angle > 0) {
+            if (thruster.forward < 0) {
+                pitch_angle += thruster.pitch * thruster.forward;
+                component_count += thruster.forward;
+            }
+        }
+    }
+    if(component_count == 0){
+        pitch_angle = desired_pitch_angle;
+    }
+    else{
+        pitch_angle = pitch_angle/component_count;
+    }
+    return pitch_angle;
+}
+
+float lateral_thrust_roll(float desired_roll_angle){
+    float roll_angle = 0;
+    float component_count = 0;
+    for(int i = 0; i < thruster_count; i++) {
+        deltaThruster_t thruster = thrusters[i];
+        if (desired_roll_angle > 0) {
+            if (thruster.lateral < 0) {
+                roll_angle += thruster.roll * thruster.lateral;
+                component_count += thruster.lateral;
+            }
+        } else if (desired_roll_angle < 0) {
+            if (thruster.lateral > 0) {
+                roll_angle += thruster.roll * thruster.lateral;
+                component_count += thruster.lateral;
+            }
+        }
+    }
+    if(component_count == 0){
+        roll_angle = desired_roll_angle;
+    }
+    else{
+        roll_angle = roll_angle/component_count;
+    }
+    return roll_angle;
+}
+
+bool is_delta_thruster(int index){
+    for(int i = 0; i < thruster_count; i++){
+        if(thrusters[i].motor_port == index)
+            return true;
+    }
+    return false;
+}
+float convert_rc_to_angle(int rc_input){
+    float scaled_rc = rc_input/500.0f;
+    scaled_rc *= .78;       //angle in radians for calculations.
+    return scaled_rc;
+}

src/main/navigation/navigation_delta_controller.h

+//
+// Created by Sam Winkelstein on 4/26/21.
+//
+
+#pragma once
+#include "config/parameter_group.h"
+/*** Delta Thruster struct for motor mixer ***/
+extern int16_t forward_thrust_command;
+extern int16_t lateral_thrust_command;
+extern bool  thruster_control;
+
+typedef struct deltaThruster_S {
+     bool bidirectional;              //this might be_important_later_on
+     float forward;         // forward component given by the mixer, this value is local cooridinates to the drone
+     float lateral;         // lateral component given by the mixer, this value is local cooridinates to the drone
+     float vertical;        // throttle component given by the mixer, this value is local cooridinates to the drone
+     uint8_t motor_port;                 //no need to use an entire integer here
+     float pitch;
+     float roll;
+} deltaThruster_t;
+
+float get_delta_pitch_angle( float desired_pitch_angle);
+float get_delta_roll_angle( float desired_roll_angle);
+void init_delta_thrusters();
+float calc_yaw_angle_offset();
+bool is_delta_thruster(int index);
+float convert_rc_to_angle(int rc_input);
+
+
+
+void output_thrusters( float desired_angle);
+

src/main/navigation/navigation_multicopter.c

@@ -50,6 +50,7 @@
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
+#include "navigation/navigation_delta_controller.h"
 
 /*-----------------------------------------------------------
  * Altitude controller for multicopter aircraft
@@ -622,16 +623,9 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
     const float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS);
 
     // Decide whether to get the requested acceleration from roll/pitch or directly from actuators
-    if (mixerHasForwardAuthority()) {
-        mixerForwardCommand = accelForward;
-    } else {
-        posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle);
-    }
-    if (mixerHasLateralAuthority()) {
-        mixerLateralCommand = accelRight;
-    } else {
-        posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle);
-    }
+
+        posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(get_delta_pitch_angle(desiredPitch)), -maxBankAngle, maxBankAngle);
+        posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(get_delta_roll_angle(desiredRoll)), -maxBankAngle, maxBankAngle);
 
 }
 
@@ -671,14 +665,17 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
     }
     else {
         /* No position data, disable automatic adjustment, rcCommand passthrough */
+        /* this appears to be an all stop */
         posControl.rcAdjustment[PITCH] = 0;
         posControl.rcAdjustment[ROLL] = 0;
+        forward_thrust_command = 1000;
+        lateral_thrust_command = 1000;
         bypassPositionController = true;
     }
 
     if (!bypassPositionController) {
-        rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]);
-        rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
+        rcCommand[PITCH] = pidAngleToRcCommand(RADIANS_TO_CENTIDEGREES(get_delta_pitch_angle(CENTIDEGREES_TO_RADIANS(posControl.rcAdjustment[PITCH]))), pidProfile()->max_angle_inclination[FD_PITCH]);
+        rcCommand[ROLL] = pidAngleToRcCommand(RADIANS_TO_CENTIDEGREES(get_delta_pitch_angle(CENTIDEGREES_TO_RADIANS(posControl.rcAdjustment[ROLL]))), pidProfile()->max_angle_inclination[FD_ROLL]);
     }
 }