AC_InputManager: Change STAB_COL params to percent

libraries/AC_InputManager/AC_InputManager_Heli.cpp

@@ -1,6 +1,7 @@
 #include "AC_InputManager_Heli.h"
 #include <AP_Math/AP_Math.h>
 #include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -9,41 +10,7 @@ const AP_Param::GroupInfo AC_InputManager_Heli::var_info[] = {
     // parameters from parent vehicle
     AP_NESTEDGROUPINFO(AC_InputManager, 0),
 
-    // @Param: STAB_COL_1
+    // Indicies 1-4 (STAB_COL_1 thru STAB_COL_4) have been replaced. 
-    // @DisplayName: Stabilize Mode Collective Point 1
-    // @Description: Helicopter's minimum collective pitch setting at zero throttle input in Stabilize mode
-    // @Range: 0 500
-    // @Units: d%
-    // @Increment: 1
-    // @User: Standard
-    AP_GROUPINFO("STAB_COL_1",    1, AC_InputManager_Heli, _heli_stab_col_min, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT),
-
-    // @Param: STAB_COL_2
-    // @DisplayName: Stabilize Mode Collective Point 2
-    // @Description: Helicopter's collective pitch setting at mid-low throttle input in Stabilize mode
-    // @Range: 0 500
-    // @Units: d%
-    // @Increment: 1
-    // @User: Standard
-    AP_GROUPINFO("STAB_COL_2",    2, AC_InputManager_Heli, _heli_stab_col_low, AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT),
-
-    // @Param: STAB_COL_3
-    // @DisplayName: Stabilize Mode Collective Point 3
-    // @Description: Helicopter's collective pitch setting at mid-high throttle input in Stabilize mode
-    // @Range: 500 1000
-    // @Units: d%
-    // @Increment: 1
-    // @User: Standard
-    AP_GROUPINFO("STAB_COL_3",    3, AC_InputManager_Heli, _heli_stab_col_high, AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT),
-
-    // @Param: STAB_COL_4
-    // @DisplayName: Stabilize Mode Collective Point 4
-    // @Description: Helicopter's maximum collective pitch setting at full throttle input in Stabilize mode
-    // @Range: 500 1000
-    // @Units: d%
-    // @Increment: 1
-    // @User: Standard
-    AP_GROUPINFO("STAB_COL_4",    4, AC_InputManager_Heli, _heli_stab_col_max, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT),
 
     // @Param: ACRO_COL_EXP
     // @DisplayName: Acro Mode Collective Expo
@@ -52,6 +19,42 @@ const AP_Param::GroupInfo AC_InputManager_Heli::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ACRO_COL_EXP",    5, AC_InputManager_Heli, _acro_col_expo, 0),
 
+    // @Param: STAB_COL_MIN
+    // @DisplayName: Stabilize Collective Low
+    // @Description: Helicopter's minimum collective pitch setting at zero collective stick input in Stabilize mode.  Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN.
+    // @Range: 0 50
+    // @Units: %
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("STB_COL_1",    6, AC_InputManager_Heli, _heli_stab_col_min, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT),
+
+    // @Param: STAB_COL_LOW
+    // @DisplayName: Stabilize Collective Mid-Low
+    // @Description: Helicopter's collective pitch setting at mid-low (40%) collective stick input in Stabilize mode. Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN.
+    // @Range: 0 50
+    // @Units: %
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("STB_COL_2",    7, AC_InputManager_Heli, _heli_stab_col_low, AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT),
+
+    // @Param: STAB_COL_HGH
+    // @DisplayName: Stabilize Collective Mid-High
+    // @Description: Helicopter's collective pitch setting at mid-high (60%) collective stick input in Stabilize mode. Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN.
+    // @Range: 50 100
+    // @Units: %
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("STB_COL_3",    8, AC_InputManager_Heli, _heli_stab_col_high, AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT),
+
+    // @Param: STAB_COL_MAX
+    // @DisplayName: Stabilize Collective High
+    // @Description: Helicopter's maximum collective pitch setting at full collective stick input in Stabilize mode. Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN.
+    // @Range: 50 100
+    // @Units: %
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("STB_COL_4",    9, AC_InputManager_Heli, _heli_stab_col_max, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT),
+
     AP_GROUPEND
 };
 
@@ -63,21 +66,21 @@ float AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in)
 
     // calculate stabilize collective value which scales pilot input to reduced collective range
     // code implements a 3-segment curve with knee points at 40% and 60% throttle input
-    if (control_in < 400){
+    if (control_in < 400){  // control_in ranges from 0 to 1000
-        slope_low = _heli_stab_col_min / 1000.0f;
+        slope_low = _heli_stab_col_min / 100.0f;
-        slope_high = _heli_stab_col_low / 1000.0f;
+        slope_high = _heli_stab_col_low / 100.0f;
         slope_range = 0.4f;
         slope_run = control_in / 1000.0f;
-    } else if(control_in <600){
+    } else if(control_in <600){  // control_in ranges from 0 to 1000
-        slope_low = _heli_stab_col_low / 1000.0f;
+        slope_low = _heli_stab_col_low / 100.0f;
-        slope_high = _heli_stab_col_high / 1000.0f;
+        slope_high = _heli_stab_col_high / 100.0f;
         slope_range = 0.2f;
-        slope_run = (control_in - 400) / 1000.0f;
+        slope_run = (control_in - 400) / 1000.0f;  // control_in ranges from 0 to 1000
     } else {
-        slope_low = _heli_stab_col_high / 1000.0f;
+        slope_low = _heli_stab_col_high / 100.0f;
-        slope_high = _heli_stab_col_max / 1000.0f;
+        slope_high = _heli_stab_col_max / 100.0f;
         slope_range = 0.4f;
-        slope_run = (control_in - 600) / 1000.0f;
+        slope_run = (control_in - 600) / 1000.0f;  // control_in ranges from 0 to 1000
     }    
 
     scalar = (slope_high - slope_low)/slope_range;
@@ -92,11 +95,11 @@ float AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in)
     }
 
     if (_acro_col_expo <= 0.0f) {
-        acro_col_out = control_in / 1000.0f;
+        acro_col_out = control_in / 1000.0f;  // control_in ranges from 0 to 1000
     } else {
         // expo variables
         float col_in, col_in3, col_out;
-        col_in = (float)(control_in-500)/500.0f;
+        col_in = (float)(control_in-500)/500.0f;  // control_in ranges from 0 to 1000
         col_in3 = col_in*col_in*col_in;
         col_out = (_acro_col_expo * col_in3) + ((1.0f-_acro_col_expo)*col_in);
         acro_col_out = 0.5f + col_out*0.5f;
@@ -119,4 +122,36 @@ float AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in)
     return collective_out;
 }
 
+// parameter_check - check if input manager specific parameters are sensible
+bool AC_InputManager_Heli::parameter_check(char* fail_msg, uint8_t fail_msg_len) const
+{
+
+    const struct StabCheck {
+        const char *name;
+        int16_t value;
+    } stab_checks[] = {
+        {"IM_STB_COL_1", _heli_stab_col_min },
+        {"IM_STB_COL_2", _heli_stab_col_low },
+        {"IM_STB_COL_3", _heli_stab_col_high },
+        {"IM_STB_COL_4", _heli_stab_col_max },
+    };
+
+    // check values are within valid range
+    for (uint8_t i=0; i<ARRAY_SIZE(stab_checks); i++) {
+        const StabCheck check = stab_checks[i];
+        if ((check.value < 0) || (check.value > 100)){
+            hal.util->snprintf(fail_msg, fail_msg_len, "%s out of range", check.name);
+            return false;
+        }
+    }
+    // check values are in correct order
+    for (uint8_t i=1; i<ARRAY_SIZE(stab_checks); i++) {
+        if ((stab_checks[i-1].value >= stab_checks[i].value)){
+            hal.util->snprintf(fail_msg, fail_msg_len, "%s must be < %s", stab_checks[i-1].name, stab_checks[i].name);
+            return false;
+        }
+    }
+    // all other cases parameters are OK
+    return true;
+}
 

libraries/AC_InputManager/AC_InputManager_Heli.h

@@ -8,9 +8,9 @@
 #include "AC_InputManager.h"
 
 # define AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT     0
-# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT     400
+# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT     40
-# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT    600
+# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT    60
-# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT     1000
+# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT     100
 
 /// @class  AP_InputManager_Heli
 /// @brief  Class managing the pilot's control inputs   for Conventional Helicopter
@@ -37,6 +37,9 @@ public:
     // set_heli_stab_col_ramp - setter function
     void set_stab_col_ramp(float ramp) { _stab_col_ramp = constrain_float(ramp, 0.0, 1.0); }
 
+    // parameter_check - returns true if input manager specific parameters are sensible, used for pre-arm check
+    bool parameter_check(char* fail_msg, uint8_t fail_msg_len) const;
+
     static const struct AP_Param::GroupInfo        var_info[];
 
 private: