Plane: add acceleration based height control option in TECS

ArduPlane/Attitude.cpp

@@ -265,8 +265,13 @@ float Plane::stabilize_pitch_get_pitch_out()
         demanded_pitch = landing.get_pitch_cd();
     }
 
-    return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator,
+    float up_accel_demand;
+    if (should_run_tecs() && TECS_controller.get_height_accel_demand(up_accel_demand)) {
+        return pitchController.get_servo_out_accln(up_accel_demand, speed_scaler, TECS_controller.get_pitch_min(), TECS_controller.get_pitch_max(), g.pitch_trim);
+    } else {
+        return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator,
                                          ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)));
+    }
 }
 
 /*

ArduPlane/Plane.cpp

@@ -217,28 +217,35 @@ void Plane::ahrs_update()
 }
 
 /*
-  update 50Hz speed/height controller
+  return true if we should be running the TECS speed height controller
  */
-void Plane::update_speed_height(void)
+bool Plane::should_run_tecs(void) const
 {
-    bool should_run_tecs = control_mode->does_auto_throttle();
 #if HAL_QUADPLANE_ENABLED
     if (quadplane.should_disable_TECS()) {
-        should_run_tecs = false;
+        return false;
     }
 #endif
 
     if (auto_state.idle_mode) {
-        should_run_tecs = false;
+        return false;
     }
 
 #if AP_PLANE_GLIDER_PULLUP_ENABLED
     if (mode_auto.in_pullup()) {
-        should_run_tecs = false;
+        return false;
     }
 #endif
 
-    if (should_run_tecs) {
+    return control_mode->does_auto_throttle();
+}
+
+/*
+  update 50Hz speed/height controller
+ */
+void Plane::update_speed_height(void)
+{
+    if (should_run_tecs()) {
 	    // Call TECS 50Hz update. Note that we call this regardless of
 	    // throttle suppressed, as this needs to be running for
 	    // takeoff detection

ArduPlane/Plane.h

@@ -1081,6 +1081,7 @@ class Plane : public AP_Vehicle {
                              uint8_t &task_count,
                              uint32_t &log_bit) override;
     void ahrs_update();
+    bool should_run_tecs(void) const;
     void update_speed_height(void);
     void update_GPS_50Hz(void);
     void update_GPS_10Hz(void);

Tools/autotest/Generic_Missions/CMAC-height-changes.txt

@@ -0,0 +1,11 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.362938	149.165085	584.000000	1
+1	0	3	22	15.000000	0.000000	0.000000	0.000000	-35.361164	149.163986	50.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.359467	149.161697	80.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366333	149.162659	150.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366131	149.164581	60.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.359272	149.163757	120.000000	1
+6	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.359467	149.161697	80.000000	1
+7	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366333	149.162659	140.000000	1
+8	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366131	149.164581	70.000000	1
+9	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.362938	149.165085	100.000000	1

Tools/autotest/arduplane.py

@@ -7731,6 +7731,60 @@ def PlaneFollowAppletSanity(self):
         self.remove_installed_script_module("pid.lua")
         self.remove_installed_script_module("mavlink_attitude.lua")
 
+    def TECSAccelControl(self):
+        '''Test TECS height acceleration control via scripting'''
+        self.context_push()
+        self.context_collect('STATUSTEXT')
+
+        self.set_parameters({
+            "SCR_ENABLE": 1,
+            "RC7_OPTION": 305,  # scripting aux function for TECS control
+        })
+        self.install_example_script_context('TECS_accel_control.lua')
+        self.reboot_sitl()
+
+        self.wait_text("Loaded TECS height accel control switch", check_context=True)
+        self.wait_ready_to_arm()
+
+        # load mission with significant height changes
+        num_wp = self.load_generic_mission("CMAC-height-changes.txt") - 1
+
+        # fly mission with standard TECS first (switch low)
+        self.set_rc(7, 1000)
+        self.arm_vehicle()
+        self.change_mode('AUTO')
+        self.wait_waypoint(1, num_wp, max_dist=100, timeout=300)
+        self.change_mode('LOITER')
+        self.progress("Completed mission with standard TECS")
+
+        # enable height accel control (switch high)
+        self.set_rc(7, 2000)
+        self.wait_text("TECS height accel control enabled", check_context=True)
+
+        # verify TECS_OPTIONS bit is set
+        tecs_options = self.get_parameter('TECS_OPTIONS')
+        if (int(tecs_options) & 4) == 0:
+            raise NotAchievedException("TECS_OPTIONS bit 2 not set after enabling")
+
+        # fly the mission again with height accel control
+        self.set_current_waypoint(1)
+        self.change_mode('AUTO')
+        self.wait_waypoint(1, num_wp, max_dist=100, timeout=300)
+        self.progress("Completed mission with height accel control")
+
+        # disable height accel control (switch low)
+        self.set_rc(7, 1000)
+        self.wait_text("TECS height accel control disabled", check_context=True)
+
+        # verify TECS_OPTIONS bit is cleared
+        tecs_options = self.get_parameter('TECS_OPTIONS')
+        if (int(tecs_options) & 4) != 0:
+            raise NotAchievedException("TECS_OPTIONS bit 2 not cleared after disabling")
+
+        self.fly_home_land_and_disarm()
+        self.context_pop()
+        self.reboot_sitl()
+
     def PreflightRebootComponent(self):
         '''Ensure that PREFLIGHT_REBOOT commands sent to components don't reboot Autopilot'''
         self.run_cmd_int(
@@ -7917,6 +7971,7 @@ def tests1b(self):
             self.ScriptedArmingChecksAppletRally,
             self.PlaneFollowAppletSanity,
             self.PreflightRebootComponent,
+            self.TECSAccelControl,
         ]
 
     def tests1c(self):

Tools/autotest/param_metadata/param.py

@@ -139,6 +139,10 @@ def has_param(self, pname):
              'litres'  : 'litres',
              'Ohm'     : 'Ohm',
              'N'       : 'Newtons',
+             'g'       : 'Gravities',
+             '1/s^2'   : 'per second squared',
+             '1/(m/s)' : 'inverse meters/second',
+             '1/m'     : 'inverse meters',
              }
 
 required_param_fields = [

Tools/autotest/vehicle_test_suite.py

@@ -2911,6 +2911,9 @@ def LoggerDocumentation_whitelist(self):
                 "TEC2",  # only Plane has TECS
                 "TEC3",  # only Plane has TECS
                 "TEC4",  # only Plane has TECS
+                "TEC5",  # only Plane has TECS
+                "TEC6",  # only Plane has TECS
+                "ACC1",  # only Plane has acceleration pitch control
                 "SOAR",  # only Planes can truly soar
                 "SORC",  # soaring is pure magic
                 "QBRK",  # quadplane

libraries/APM_Control/AP_PitchController.cpp

@@ -20,6 +20,8 @@
 #include "AP_PitchController.h"
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Scheduler/AP_Scheduler.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -157,6 +159,22 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
 
     AP_SUBGROUPINFO(rate_pid, "_RATE_", 11, AP_PitchController, AC_PID),
 
+    // @Param: _NOR_GLIM
+    // @DisplayName: Normal accln limit
+    // @Description: Maximum allowed normal g when airspeed = SCALING_SPEED. When flying faster or slower than SCALING_SPEED the limit is adjusted for dynamic pressure. Use this to prevent wing stall. Use this to stay within angle of attack limits.
+    // @Units: g
+    // @Range: 1.5 5.0
+    // @User: Standard
+    AP_GROUPINFO("_NOR_GLIM", 12, AP_PitchController, acro_normal_g_lim, 2.0f),
+
+    // @Param: _NOR_TAU
+    // @DisplayName: Normal accln time constant
+    // @Description: Time constant of the normal accleration control loop.
+    // @Units: s
+    // @Range: 0.1 1.0
+    // @User: Standard
+    AP_GROUPINFO("_NOR_TAU", 13, AP_PitchController, accln_tconst, 0.5f),
+
     AP_GROUPEND
 };
 
@@ -333,3 +351,148 @@ void AP_PitchController::convert_pid()
     rate_pid.kD().set_and_save_ifchanged(0);
     rate_pid.kIMAX().set_and_save_ifchanged(old_imax/4500.0);
 }
+
+/*
+  Return an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
+  A positive demand is up
+  Inputs are:
+  1) up accln in m/s/s - 0
+  2) control gain scaler = scaling_speed / aspeed
+  3) minimum allowed pitch angle in deg
+  4) maximum allowed pitch angle in deg
+*/
+float AP_PitchController::get_servo_out_accln(float up_accln, float scaler, float pitch_angle_min, float pitch_angle_max, float pitch_trim_deg)
+{
+    Vector3f vdot_dem_clipped = Vector3f(0.0f, 0.0f, -constrain_float(up_accln, -7.0f, 7.0f));
+
+    const AP_AHRS &ahrs = AP::ahrs();
+
+    // calculate predicted horizontal accel assuming coordinated turn
+    const float roll_lim_rad = radians(aparm.roll_limit);
+    const float right_accel = (GRAVITY_MSS - vdot_dem_clipped.z) * tanf(constrain_float(ahrs.get_roll_rad(), -roll_lim_rad, roll_lim_rad));
+    vdot_dem_clipped.x = - right_accel * ahrs.sin_yaw();
+    vdot_dem_clipped.y = right_accel * ahrs.cos_yaw();
+
+    // get Euler angles for a 321 rotation sequence
+    const Matrix3f Tnb = ahrs.get_rotation_body_to_ned();
+    float rollAngle, pitchAngle, yawAngle;
+    Tnb.to_euler(&rollAngle, &pitchAngle, &yawAngle);
+
+    Vector3f velRateDemBF; // demanded velocity rate of change - body frame
+    Vector3f acclnDemBF; // demanded specific force - body frame
+
+    // add gravity to make it a specific force demand
+    Vector3f acc_dem_NED = vdot_dem_clipped;
+    acc_dem_NED.z -= GRAVITY_MSS;
+
+    // rotate into body frame
+    velRateDemBF = ahrs.earth_to_body(vdot_dem_clipped);
+    acclnDemBF = ahrs.earth_to_body(acc_dem_NED); // specific force vector
+
+    bool is_clipped = false;
+
+    // limit specific forces to avoid exceeding aerodynamic limits
+    //  aerodynamic forces scale with speed^2
+    const float specificForceScaler = 1.0f / sq(scaler);
+    const float normalLoadFactorLim = MAX(acro_normal_g_lim, 1.1f);
+    // allow a minimum normal g to allow height to be maintained
+    const float acclnLimZ = GRAVITY_MSS * MAX(normalLoadFactorLim * specificForceScaler, 1.1f);
+    if (acclnDemBF.z > acclnLimZ) {
+        acclnDemBF.z = acclnLimZ;
+    } else if (acclnDemBF.z < -acclnLimZ) {
+        acclnDemBF.z = -acclnLimZ;
+    }
+
+    // specific force error vector
+    Vector3f acclnErrBF = acclnDemBF - ahrs.get_accel();
+
+    // true airspeed is required to predict the turn rate
+    float TAS;
+    if (!ahrs.airspeed_TAS(TAS)) {
+        // shouldn't get here in normal flight but if we do pick cruise speed
+        TAS = aparm.airspeed_cruise * ahrs.get_EAS2TAS();
+    } else {
+        TAS = constrain_float(TAS, aparm.airspeed_min * ahrs.get_EAS2TAS(), aparm.airspeed_max * ahrs.get_EAS2TAS());
+    }
+
+    // calculate predicted body frame pitch rate
+    const float pitchRateDemFF = - velRateDemBF.z / TAS;
+
+    uint32_t tnow = AP_HAL::millis();
+    uint32_t dt_msec = tnow - _last_t;
+    if (_last_t == 0 || dt_msec > 1000) {
+        dt_msec = 0;
+        accln_err_integral = 0.0f;
+    }
+    _last_t = tnow;
+
+    // correct body frame pitch rate using integral of acceleration error
+    const float normalAcclnErrGain = 1.0f / (accln_tconst * TAS);
+    float normalAcclnErrIntegDelta = - acclnErrBF.z * normalAcclnErrGain * 0.001f * (float)dt_msec;
+    if (clip_direction > 0 ||  pitchRateDemFF + accln_err_integral > radians(gains.rmax_pos)) {
+        normalAcclnErrIntegDelta = MIN(normalAcclnErrIntegDelta, 0.0f);
+    } else if (clip_direction < 0 ||  pitchRateDemFF + accln_err_integral < - radians(gains.rmax_neg)) {
+        normalAcclnErrIntegDelta = MAX(normalAcclnErrIntegDelta, 0.0f);
+    }
+    accln_err_integral += normalAcclnErrIntegDelta;
+    float desired_rate_dps = degrees(pitchRateDemFF + accln_err_integral);
+
+    // apply upper and lower pitch angle rate required to respect pitch angle limits
+    if (gains.tau < 0.05f) {
+        gains.tau.set(0.05f);
+    }
+    const float pitch_deg = ahrs.get_pitch_deg();
+    float pitch_rate_max_dps = (pitch_angle_max + pitch_trim_deg - pitch_deg) / gains.tau;
+    float pitch_rate_min_dps = (pitch_angle_min + pitch_trim_deg - pitch_deg) / gains.tau;
+    if (desired_rate_dps > pitch_rate_max_dps) {
+        desired_rate_dps = pitch_rate_max_dps;
+        clip_direction = 1;
+        is_clipped = true;
+    } else if (desired_rate_dps < pitch_rate_min_dps) {
+        desired_rate_dps = pitch_rate_min_dps;
+        clip_direction = -1;
+        is_clipped = true;
+    }
+
+    // clear clip direction if none recorded
+    if (!is_clipped) {
+        clip_direction = 0;
+    }
+
+    static uint32_t last_log_ms = 0;
+    uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - last_log_ms > 40) {
+        last_log_ms = now_ms;
+        // @LoggerMessage: ACC1
+        // @Vehicles: Plane
+        // @Description: Pitch controller acceleration control log
+        // @Field: TimeUS: Time since system startup
+        // @Field: VRDBF: velocity rate demand body frame Z component
+        // @Field: ADBF: acceleration demand body frame Z component
+        // @Field: AEBF: acceleration error body frame Z component
+        // @Field: gain: acceleration error to pitch rate gain
+        // @Field: PRDFF: pitch rate demand feedforward
+        // @Field: AEI: acceleration error integral
+        // @Field: PRmin: minimum pitch rate from pitch angle limit
+        // @Field: PRmax: maximum pitch rate from pitch angle limit
+        // @Field: DR: final desired pitch rate
+        // @Field: clip: rate limit clip indicator
+#if HAL_LOGGING_ENABLED
+        AP::logger().WriteStreaming("ACC1","TimeUS,VRDBF,ADBF,AEBF,gain,PRDFF,AEI,PRmin,PRmax,DR,clip",
+                                    "Qfffffffffb",
+                                    AP_HAL::micros64(),
+                                    (double)velRateDemBF.z,
+                                    (double)acclnDemBF.z,
+                                    (double)acclnErrBF.z,
+                                    (double)normalAcclnErrGain,
+                                    (double)pitchRateDemFF,
+                                    (double)accln_err_integral,
+                                    (double)pitch_rate_min_dps,
+                                    (double)pitch_rate_max_dps,
+                                    (double)desired_rate_dps,
+                                    (int8_t)clip_direction);
+#endif // HAL_LOGGING_ENABLED
+    }
+
+    return get_rate_out(desired_rate_dps, scaler);
+}

libraries/APM_Control/AP_PitchController.h

@@ -11,6 +11,7 @@ class AP_PitchController : public AP_FW_Controller
     CLASS_NO_COPY(AP_PitchController);
 
     float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode) override;
+    float get_servo_out_accln(float up_accln, float scaler, float pitch_angle_min, float pitch_angle_max, float pitch_trim_deg);
 
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -24,5 +25,10 @@ class AP_PitchController : public AP_FW_Controller
     float get_airspeed() const override;
     bool is_underspeed(const float aspeed) const override;
     float get_measured_rate() const override;
-
+    // used for acceleration control
+    uint32_t _last_t;
+    float accln_err_integral; // pitch rate demand from acceleration error integral (rad/s)
+    int8_t clip_direction; // 0 when not clipped, >0 when clipping in the up direction, <0 when clipping in teh down direction
+    AP_Float acro_normal_g_lim;
+    AP_Float accln_tconst;
 };