CR101

docs/Settings.md

@@ -2658,7 +2658,7 @@ Max YAW rate when NAV CRUISE mode is enabled (set to 0 to disable) [dps]
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 20 | 0 | 60 |
+| 20 | 0 | 120 |
 
 ---
 
@@ -3282,6 +3282,16 @@ Max allowed above the ground altitude for terrain following mode
 
 ---
 
+### nav_mc_althold_throttle
+
+If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| STICK |  |  |
+
+---
+
 ### nav_mc_bank_angle
 
 Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude
@@ -3762,16 +3772,6 @@ Enables or Disables the use of the heading PID controller on fixed wing. Heading
 
 ---
 
-### nav_use_midthr_for_althold
-
-If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude
-
-| Default | Min | Max |
-| --- | --- | --- |
-| OFF | OFF | ON |
-
----
-
 ### nav_user_control_mode
 
 Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction.

src/main/common/fp_pid.c

@@ -81,9 +81,9 @@ float navPidApply3(
     newDerivative = newDerivative * gainScaler * dTermScaler;
     // CR99
     // zero integral if proportional saturated
-    if (newProportional > outMax || newProportional < outMin) {
-        pidFlags |= PID_ZERO_INTEGRATOR;
-    }
+    // if (newProportional > outMax || newProportional < outMin) {
+        // pidFlags |= PID_ZERO_INTEGRATOR;
+    // }
     // CR99
     if (pidFlags & PID_ZERO_INTEGRATOR) {
         pid->integrator = 0.0f;

src/main/fc/settings.yaml

@@ -2592,7 +2592,7 @@ groups:
         default_value: 20
         field: general.cruise_yaw_rate
         min: 0
-        max: 60
+        max: 120
         # CR101
       - name: nav_mc_bank_angle
         description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"

src/main/io/osd.c

@@ -5028,6 +5028,18 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                         // by OSD_FLYMODE.
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
                     }
+                    // CR101
+                    if (STATE(MULTIROTOR) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
+                        if (posControl.cruise.multicopterSpeed >= 50.0f) {
+                            char buf[6];
+                            osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, false, false);
+                            tfp_sprintf(messageBuf, "(SPD %s)", buf);
+                        } else {
+                            strcpy(messageBuf, "(HOLD)");
+                        }
+                        messages[messageCount++] = messageBuf;
+                    }
+                    // CR101
                     if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM);
                     }

src/main/navigation/navigation.c

@@ -1073,7 +1073,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
-
+    // CR101
+    if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) {  // course hole not possible on MR without yaw control
+        return NAV_FSM_EVENT_ERROR;
+    }
     // if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now  // CR101
 
     DEBUG_SET(DEBUG_CRUISE, 0, 1);
@@ -1112,11 +1115,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
     if (STATE(AIRPLANE) && posControl.flags.isAdjustingPosition) {     // CR101 inhibit for MR, pitch adjusts only speed
         return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ;
     }
-    // User is yawing. We record the desired course and change the desired target in the meanwhile
-    if (posControl.flags.isAdjustingHeading) {
+    // CR101
+    int16_t cruiseYawRate = DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate);
+    int16_t headingAdjustCommand = 0;
+
+    // User demanding yaw (yaw on FW, yaw and roll on MR). We record the desired course and change the desired target in the meanwhile
+    if (posControl.flags.isAdjustingHeading || posControl.flags.isAdjustingPosition) {
+        headingAdjustCommand = rcCommand[YAW];
+        if (posControl.flags.isAdjustingPosition && rcCommand[ROLL] > headingAdjustCommand) {
+            headingAdjustCommand = rcCommand[ROLL];
+        }
+    // if (posControl.flags.isAdjustingHeading) {
         timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime;
         if (timeDifference > 100) timeDifference = 0;   // if adjustment was called long time ago, reset the time difference.
-        float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate));  // CR101
+        float rateTarget = scaleRangef((float)headingAdjustCommand, -500.0f, 500.0f, -cruiseYawRate, cruiseYawRate);  // CR101
+        // float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate));
         float centidegsPerIteration = rateTarget * MS2S(timeDifference);
         posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
         posControl.cruise.lastCourseAdjustmentTime = currentTimeMs;