New functions for Fixed wing

Apa, Cruise Mode and fix for Vector trust.

Moved Motor code

Moved Motor code

Syntax

Syntax

Formatting

Formatting

Formatting

Formatting

Formatting

Update fw_nav.c

Update mw.c

Cleaning up mixerTrustVector

Update mixer.c

Update fw_nav.c

Update mw.c

Removed Hex file

Renaming vector Thrust

Update mw.c

Update mw.c

Author: EmilsPa

src/cli.c

@@ -262,16 +262,19 @@ const clivalue_t valueTable[] = {
     { "p_vel", VAR_UINT8, &cfg.P8[PIDVEL], 0, 200 },
     { "i_vel", VAR_UINT8, &cfg.I8[PIDVEL], 0, 200 },
     { "d_vel", VAR_UINT8, &cfg.D8[PIDVEL], 0, 200 },
+    { "fw_vector_thrust", VAR_UINT8, &cfg.fw_vector_thrust, 0, 1},
     { "fw_gps_maxcorr", VAR_INT16, &cfg.fw_gps_maxcorr, -45, 45 },
     { "fw_gps_rudder", VAR_INT16, &cfg.fw_gps_rudder,  -45, 45 },
     { "fw_gps_maxclimb", VAR_INT16, &cfg.fw_gps_maxclimb,  -45, 45 },
     { "fw_gps_maxdive", VAR_INT16, &cfg.fw_gps_maxdive,  -45, 45 },
+    { "fw_glide_angle", VAR_UINT8, &cfg.fw_glide_angle, 0, 100 },
     { "fw_climb_throttle", VAR_UINT16, &cfg.fw_climb_throttle, 1000, 2000 },
     { "fw_cruise_throttle", VAR_UINT16, &cfg.fw_cruise_throttle, 1000, 2000 },
     { "fw_idle_throttle", VAR_UINT16, &cfg.fw_idle_throttle, 1000, 2000 },
     { "fw_scaler_throttle", VAR_UINT16, &cfg.fw_scaler_throttle, 0, 15 },
-    { "fw_roll_comp", VAR_FLOAT, &cfg.fw_roll_comp, 0, 2 },
-    { "fw_rth_alt", VAR_UINT8, &cfg.fw_rth_alt, 0, 200 },
+    { "fw_roll_comp", VAR_UINT8, &cfg.fw_roll_comp, 0, 250 },
+    { "fw_rth_alt", VAR_UINT8, &cfg.fw_rth_alt, 0, 250 },
+    { "fw_cruise_distance", VAR_UINT16, &cfg.fw_cruise_distance, 0, 2000},
 };
 
 #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))
@@ -1044,12 +1047,11 @@ static void cliMixer(char *cmdline)
             // Presets for planes. Not functional with current reset
             // Really Ugly Hack
             if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) {
-                cfg.dynThrPID = 50;
+                cfg.dynThrPID = 90;
                 cfg.rcExpo8 = 0;
                 cfg.P8[PIDALT] = 30;
                 cfg.I8[PIDALT] = 20;
                 cfg.D8[PIDALT] = 45;
-                cfg.D8[PIDPOSR] = 50; // RTH Alt
                 cfg.P8[PIDNAVR] = 30;
                 cfg.I8[PIDNAVR] = 20;
                 cfg.D8[PIDNAVR] = 45;

src/config.c

@@ -24,7 +24,7 @@ master_t mcfg;  // master config struct with data independent from profiles
 config_t cfg;   // profile config struct
 const char rcChannelLetters[] = "AERT123456789LMNOP";  // hack for the char-based channel mapping stuff, 18 channels hard max
 
-static const uint8_t EEPROM_CONF_VERSION = 75;
+static const uint8_t EEPROM_CONF_VERSION = 76;
 static uint32_t enabledSensors = 0;
 static void resetConf(void);
 static const uint32_t FLASH_WRITE_ADDR = 0x08000000 + (FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - (CONFIG_SIZE / 1024)));
@@ -355,7 +355,9 @@ static void resetConf(void)
     cfg.fw_cruise_throttle = 1500;
     cfg.fw_idle_throttle = 1300;
     cfg.fw_scaler_throttle = 8;
-    cfg.fw_roll_comp = 1;
+    cfg.fw_roll_comp = 100;
+    cfg.fw_cruise_distance = 500;
+    cfg.fw_rth_alt = 50;
     // control stuff
     mcfg.reboot_character = 'R';
 

src/fw_nav.c

@@ -4,17 +4,21 @@
 // from gps.c
 extern int32_t nav_bearing;
 extern int32_t wp_distance;
+extern int32_t GPS_WP[2];
+extern float GPS_scaleLonDown;
+
 extern PID_PARAM navPID_PARAM;
 extern PID_PARAM altPID_PARAM;
 
 #define GPS_UPD_HZ 5            // Set loop time for NavUpdate 5 Hz is enough
 #define PITCH_COMP 0.5f         // Compensate throttle relative angle of attack
 // Candidates for CLI
-#define SAFE_NAV_ALT 20         // Safe Altitude during climbouts Wings Level below this Alt. (ex. trees & buildings..)
+#define SAFE_NAV_ALT 25         // Safe Altitude during climbouts Wings Level below this Alt. (ex. trees & buildings..)
 #define SAFE_DECSCEND_ZONE 50   // Radius around home where descending is OK
 // For speedBoost
 #define GPS_MINSPEED 500        // 500= ~18km/h
 #define I_TERM 0.1f
+#define GEO_SKALEFACT  89.832f  // Scale to match meters  
 
 float navErrorI;
 float altErrorI;
@@ -40,6 +44,19 @@ void fw_nav_reset(void)
     }
 }
 
+void fw_FlyTo(void) // PatrikE CruiseMode version
+{
+    float wp_lat_diff, wp_lon_diff, scaler;
+    int32_t holdHeading = GPS_ground_course / 10;
+    if (holdHeading > 180)
+        holdHeading -= 360;
+    scaler = (GEO_SKALEFACT / GPS_scaleLonDown) * cfg.fw_cruise_distance;
+    wp_lat_diff = cos(holdHeading * 0.0174532925f);
+    wp_lon_diff = sin(holdHeading * 0.0174532925f) * GPS_scaleLonDown;
+    GPS_WP[LAT] += wp_lat_diff * scaler;
+    GPS_WP[LON] += wp_lon_diff * scaler;
+}
+
 void fw_nav(void)
 {
     int16_t GPS_Heading = GPS_ground_course;    // Store current bearing
@@ -130,7 +147,6 @@ void fw_nav(void)
         if (abs(navDiff) > 170)
             navDiff = 175;      // Forced turn.
 
-
         // PID for Navigating planes.
         navDT = (float) (millis() - nav_loopT) / 1000;
         nav_loopT = millis();
@@ -201,16 +217,15 @@ void fw_nav(void)
         // Elevator compensation depending on behaviour.
         // Prevent stall with Disarmed motor
         if (f.MOTORS_STOPPED)
-            GPS_angle[PITCH] = constrain(GPS_angle[PITCH], 0, cfg.fw_gps_maxdive * 10);
+            GPS_angle[PITCH] = constrain(GPS_angle[PITCH], -cfg.fw_glide_angle, cfg.fw_gps_maxdive * 10);
 
         // Add elevator compared with rollAngle
         if (!f.CLIMBOUT_FW)
-            GPS_angle[PITCH] -= (abs(angle[ROLL]) * cfg.fw_roll_comp);
+            GPS_angle[PITCH] -= (abs(angle[ROLL]) * (cfg.fw_roll_comp / 100));
 
         // Throttle compensation depending on behaviour.
         // Compensate throttle with pitch Angle
         NAV_Thro -= constrain(angle[PITCH] * PITCH_COMP, 0, 450);
-        NAV_Thro = constrain(NAV_Thro, cfg.fw_idle_throttle, cfg.fw_climb_throttle);
 
         // Force the Plane move forward in headwind with speedBoost
         groundSpeed = GPS_speed;
@@ -221,6 +236,9 @@ void fw_nav(void)
 
         speedBoost = constrain(speedBoost, 0, 500);
         NAV_Thro += speedBoost;
+
+        // constrain throttle to Max climb.
+        NAV_Thro = constrain(NAV_Thro, cfg.fw_idle_throttle, cfg.fw_climb_throttle);
     }
     // End of NavTimer
 

src/gps.c

@@ -505,15 +505,15 @@ static void reset_PID(PID *pid)
 
 static float dTnav;             // Delta Time in milliseconds for navigation computations, updated with every good GPS read
 static int16_t actual_speed[2] = { 0, 0 };
-static float GPS_scaleLonDown = 1.0f;  // this is used to offset the shrinking longitude as we go towards the poles
+float GPS_scaleLonDown = 1.0f;  // this is used to offset the shrinking longitude as we go towards the poles
 
 // The difference between the desired rate of travel and the actual rate of travel
 // updated after GPS read - 5-10hz
 static int16_t rate_error[2];
 static int32_t error[2];
 
 // Currently used WP
-static int32_t GPS_WP[2];
+int32_t GPS_WP[2];
 
 ////////////////////////////////////////////////////////////////////////////////
 // Location & Navigation
@@ -768,6 +768,9 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
     GPS_WP[LON] = *lon;
 
     GPS_calc_longitude_scaling(*lat);
+    if (f.CRUISE_MODE)
+        fw_FlyTo();  // PatrikE CruiseMode version
+
     GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
 
     nav_bearing = target_bearing;

src/mixer.c

@@ -134,7 +134,7 @@ static const motorMixer_t mixerDualcopter[] = {
     { 1.0f,  0.0f,  0.0f,  1.0f },          // RIGHT
 };
 
-static const motorMixer_t mixerTrustVector[] = {
+static const motorMixer_t mixerVectorThrust[] = {
     { 1.0f,  0.0f,  0.0f, -0.5f },          // LEFT
     { 1.0f,  0.0f,  0.0f,  0.5f },          // RIGHT
 };
@@ -150,13 +150,13 @@ const mixer_t mixers[] = {
     { 0, 1, NULL },                // * MULTITYPE_GIMBAL
     { 6, 0, mixerY6 },             // MULTITYPE_Y6
     { 6, 0, mixerHex6P },          // MULTITYPE_HEX6
-    { 2, 1, mixerTrustVector },     // * MULTITYPE_FLYING_WING
+    { 2, 1, mixerVectorThrust },     // * MULTITYPE_FLYING_WING
     { 4, 0, mixerY4 },             // MULTITYPE_Y4
     { 6, 0, mixerHex6X },          // MULTITYPE_HEX6X
     { 8, 0, mixerOctoX8 },         // MULTITYPE_OCTOX8
     { 8, 0, mixerOctoFlatP },      // MULTITYPE_OCTOFLATP
     { 8, 0, mixerOctoFlatX },      // MULTITYPE_OCTOFLATX
-    { 1, 1, NULL },                // * MULTITYPE_AIRPLANE
+    { 2, 1, mixerVectorThrust },    // * MULTITYPE_AIRPLANE
     { 0, 1, NULL },                // * MULTITYPE_HELI_120_CCPM
     { 0, 1, NULL },                // * MULTITYPE_HELI_90_DEG
     { 4, 0, mixerVtail4 },         // MULTITYPE_VTAIL4
@@ -166,7 +166,7 @@ const mixer_t mixers[] = {
     { 1, 1, NULL },                // MULTITYPE_SINGLECOPTER
     { 4, 0, mixerAtail4 },         // MULTITYPE_ATAIL4
     { 0, 0, NULL },                // MULTITYPE_CUSTOM
-    { 1, 1, NULL },                // MULTITYPE_CUSTOM_PLANE
+    { 2, 1, mixerVectorThrust },    // MULTITYPE_CUSTOM_PLANE
 };
 
 // mixer rule format servo, input, rate, speed, min, max, box
@@ -545,15 +545,18 @@ void mixTable(void)
     }
 
     // motors for non-servo mixes
-    if (numberMotor > 1)
-        for (i = 0; i < numberMotor; i++)
+    if (numberMotor > 1) {
+        for (i = 0; i < numberMotor; i++) {
             motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
-
-    if (f.FIXED_WING) {
-        if (!f.ARMED)
-            motor[0] = mcfg.mincommand; // Kill throttle when disarmed
-        else
-            motor[0] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
+            if (f.FIXED_WING) { // vector_thrust handeling
+                if (cfg.fw_vector_thrust) {
+                    if (f.PASSTHRU_MODE)
+                        motor[i] = rcCommand[THROTTLE] - rcCommand[YAW] * (i - 0.5f);
+                } else { // Override mixerVectorThrust
+                    motor[i] = rcCommand[THROTTLE];
+                }
+            }
+        }
     }
 
     // airplane / servo mixes
@@ -639,6 +642,7 @@ void mixTable(void)
         }
         if (!f.ARMED) {
             motor[i] = motor_disarmed[i];
+            f.MOTORS_STOPPED = 1;
         }
     }
 }

src/mw.c

@@ -101,6 +101,7 @@ void annexCode(void)
     static uint32_t calibratedAccTime;
     int32_t tmp, tmp2;
     int32_t axis, prop1, prop2;
+    static uint16_t MaxBrkpoint = 300; // Max angle of APA
 
     // vbat shit
     static uint8_t vbatTimer = 0;
@@ -109,17 +110,39 @@ void annexCode(void)
     static int64_t mAhdrawnRaw = 0;
     static int32_t vbatCycleTime = 0;
 
-    // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
-    if (rcData[THROTTLE] < cfg.tpa_breakpoint) {
-        prop2 = 100;
+    if (!f.FIXED_WING) { // Baseflight original dynamic PID adjustemnt
+        // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
+        if (rcData[THROTTLE] < cfg.tpa_breakpoint) {
+            prop2 = 100;
+        } else {
+            if (rcData[THROTTLE] < 2000) {
+                prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpa_breakpoint) / (2000 - cfg.tpa_breakpoint);
+            } else {
+                prop2 = 100 - cfg.dynThrPID;
+            }
+        }
     } else {
-        if (rcData[THROTTLE] < 2000) {
-            prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpa_breakpoint) / (2000 - cfg.tpa_breakpoint);
+        // Throttle & Angle combined PID Attenuation
+        // Will dampen the PID's in High speeds dive on Fixed Wing Only
+        prop2 = 128; // prop2 was 100, is 128 now
+        if (rcData[THROTTLE] < cfg.tpa_breakpoint) {
+            prop2 = 128; // Higher prop2 for Fixed wing Same as used in MWii
         } else {
-            prop2 = 100 - cfg.dynThrPID;
+            if (rcCommand[THROTTLE] > cfg.dynThrPID) { // Using rcCommand() to include Tpa even in Gps modes.
+                if (rcCommand[THROTTLE] < 2000) {
+                    prop2 -=  ((uint16_t)cfg.dynThrPID * (rcCommand[THROTTLE] - cfg.dynThrPID) >> 9);
+                } else {
+                    prop2 -=  cfg.dynThrPID;
+                }
+            }
         }
+        // APA dynamic PID adjustemnt, depending on Angle of attack
+        if (angle[1] > 20)
+            prop2 -= ((uint16_t)cfg.dynThrPID * (min(angle[1], MaxBrkpoint)) >> 8);
+        prop2 = max((128 - cfg.dynThrPID), prop2);
     }
 
+
     for (axis = 0; axis < 3; axis++) {
         tmp = min(abs(rcData[axis] - mcfg.midrc), 500);
         if (axis != 2) {        // ROLL & PITCH
@@ -692,6 +715,11 @@ void loop(void)
             auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2);
         for (i = 0; i < CHECKBOXITEMS; i++)
             rcOptions[i] = (auxState & cfg.activate[i]) > 0;
+        f.CRUISE_MODE = rcOptions[BOXGCRUISE];
+        if (f.CRUISE_MODE) {
+            rcOptions[BOXGPSHOLD] = true;
+            rcOptions[BOXHORIZON] = true;
+        }
 
         // note: if FAILSAFE is disable, failsafeCnt > 5 * FAILSAVE_DELAY is always false
         if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
@@ -783,6 +811,9 @@ void loop(void)
 #ifdef GPS
         if (sensors(SENSOR_GPS)) {
             if (f.GPS_FIX && GPS_numSat >= 5) {
+                if (nav_mode != NAV_MODE_NONE && (!f.HORIZON_MODE && !f.ANGLE_MODE))
+                    f.ANGLE_MODE = true; // Force a stable mode in GPS Mode
+
                 // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority
                 if (rcOptions[BOXGPSHOME] || f.FW_FAILSAFE_RTH_ENABLE ) {
                     if (!f.GPS_HOME_MODE) {
@@ -833,7 +864,7 @@ void loop(void)
             f.PASSTHRU_MODE = 0;
         }
 
-        if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_CUSTOM_PLANE) {
+        if (f.FIXED_WING) {
             f.HEADFREE_MODE = 0;
             if (feature(FEATURE_FAILSAFE) && failsafeCnt > (6 * cfg.failsafe_delay)) {
                 f.PASSTHRU_MODE = 0;

src/mw.h

@@ -121,6 +121,7 @@ enum {
     BOXSERVO1,
     BOXSERVO2,
     BOXSERVO3,
+    BOXGCRUISE,
     CHECKBOXITEMS
 };
 
@@ -282,15 +283,18 @@ typedef struct config_t {
     uint16_t ap_mode;                       // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS
 
     // fw-related stuff
+    uint8_t fw_vector_thrust;                   // Enable Vector trust on Twin Engine models
     int16_t fw_gps_maxcorr;                    // Degrees banking Allowed by GPS.
-    int16_t fw_gps_rudder;                     // Maximum Rudder
+    int16_t fw_gps_rudder;                     // Maximum input of Rudder Allowed by GPS.
     int16_t fw_gps_maxclimb;                   // Degrees climbing . To much can stall the plane.
     int16_t fw_gps_maxdive;                    // Degrees Diving . To much can overspeed the plane.
+    uint8_t fw_glide_angle;                    // Glide angle in power off
     uint16_t fw_climb_throttle;                // Max allowed throttle in GPS modes.
     uint16_t fw_cruise_throttle;               // Throttle to set for cruisespeed.
     uint16_t fw_idle_throttle;                 // Lowest throttleValue during Descend
     uint16_t fw_scaler_throttle;               // Adjust to Match Power/Weight ratio of your model
-    float fw_roll_comp;                        // How much Elevator compensates Roll in GPS modes
+    uint8_t fw_roll_comp;                      // Adds Elevator Based on Roll Angle
+    int16_t fw_cruise_distance;                // Distance to viritual WP.
     uint8_t fw_rth_alt;                        // Min Altitude to keep during RTH. (Max 200m)
 
 } config_t;
@@ -427,6 +431,7 @@ typedef struct flags_t {
     uint8_t MOTORS_STOPPED;
     uint8_t FW_FAILSAFE_RTH_ENABLE;
     uint8_t CLIMBOUT_FW;
+    uint8_t CRUISE_MODE;
 } flags_t;
 
 extern int16_t gyroZero[3];
@@ -606,4 +611,5 @@ void GPS_reset_nav(void);
 void GPS_set_next_wp(int32_t *lat, int32_t *lon);
 int32_t wrap_18000(int32_t error);
 void fw_nav(void);
+void fw_FlyTo(void);