Add AUTO_TRIM flight mode

ArduCustom · Dec 9, 2023 · b53ab9f · b53ab9f
1 parent 5aaed60
commit b53ab9f
Show file tree

Hide file tree

Showing 14 changed files with 53 additions and 5 deletions.
diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
@@ -513,7 +513,7 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler)
     bool disable_integrator = false;
     int16_t rudder_in = rudder_input();
 
-    if ((control_mode == &mode_course_hold || control_mode == &mode_cruise) && g2.flight_options & FlightOptions::COURSE_HOLD_HEADING_CONTROL_WITH_YAW_STICK) {
+    if ((control_mode == &mode_course_hold || control_mode == &mode_auto_trim || control_mode == &mode_cruise) && g2.flight_options & FlightOptions::COURSE_HOLD_HEADING_CONTROL_WITH_YAW_STICK) {
         rudder_in = 0;
     }
 

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
@@ -47,6 +47,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
 #endif
 #endif  // HAL_QUADPLANE_ENABLED
     case Mode::Number::COURSE_HOLD:
+    case Mode::Number::MODE_AUTO_TRIM:
     case Mode::Number::CRUISE:
         _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
         break;

diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp
@@ -52,6 +52,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
 #endif  // HAL_QUADPLANE_ENABLED
     case Mode::Number::FLY_BY_WIRE_B:
     case Mode::Number::COURSE_HOLD:
+    case Mode::Number::MODE_AUTO_TRIM:
     case Mode::Number::CRUISE:
         rate_controlled = true;
         attitude_stabilized = true;

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -274,6 +274,7 @@ class Plane : public AP_Vehicle {
     ModeFBWA mode_fbwa;
     ModeFBWB mode_fbwb;
     ModeCourseHold mode_course_hold;
+    ModeAutoTrim mode_auto_trim;
     ModeCruise mode_cruise;
     ModeAutoTune mode_autotune;
     ModeAuto mode_auto;
@@ -1326,7 +1327,7 @@ class Plane : public AP_Vehicle {
     bool is_auto_throttle_gliding(void) const override { return auto_thr_gliding_state == ATGS_GLIDING; }
 
     bool get_course_hold_heading(uint16_t &locked_heading) const override {
-        if (control_mode != &mode_cruise && control_mode != &mode_course_hold) {
+        if (control_mode != &mode_cruise && control_mode != &mode_course_hold && control_mode != &mode_auto_trim) {
             return false;
         }
 

diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
@@ -1240,6 +1240,7 @@ bool Plane::nav_scripting_enable(uint8_t mode)
        case Mode::Number::FLY_BY_WIRE_A:
        case Mode::Number::FLY_BY_WIRE_B:
        case Mode::Number::COURSE_HOLD:
+       case Mode::Number::MODE_AUTO_TRIM:
        case Mode::Number::CRUISE:
        case Mode::Number::LOITER:
            nav_scripting.enabled = true;

diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp
@@ -31,6 +31,9 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
     case Mode::Number::COURSE_HOLD:
         ret = &mode_course_hold;
         break;
+    case Mode::Number::MODE_AUTO_TRIM:
+        ret = &mode_auto_trim;
+        break;
     case Mode::Number::CRUISE:
         ret = &mode_cruise;
         break;

diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp
@@ -34,6 +34,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
     case Mode::Number::AUTOTUNE:
     case Mode::Number::FLY_BY_WIRE_B:
     case Mode::Number::COURSE_HOLD:
+    case Mode::Number::MODE_AUTO_TRIM:
     case Mode::Number::CRUISE:
     case Mode::Number::TRAINING:  
         if(plane.emergency_landing) {
@@ -126,6 +127,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
     case Mode::Number::AUTOTUNE:
     case Mode::Number::FLY_BY_WIRE_B:
     case Mode::Number::COURSE_HOLD:
+    case Mode::Number::MODE_AUTO_TRIM:
     case Mode::Number::CRUISE:
     case Mode::Number::TRAINING:
     case Mode::Number::CIRCLE:

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -53,6 +53,7 @@ class Mode
         LOITER_ALT_QLAND = 25,
 #endif
         COURSE_HOLD   = 26,
+        MODE_AUTO_TRIM     = 27,
     };
 
     // Constructor
@@ -435,6 +436,20 @@ class ModeCourseHold : public Mode
     bool _enter() override;
 };
 
+class ModeAutoTrim : public ModeCourseHold
+{
+public:
+
+    Number mode_number() const override { return Number::MODE_AUTO_TRIM; }
+    const char *name() const override { return "AUTO TRIM"; }
+    const char *name4() const override { return "ATRM"; }
+
+protected:
+
+    bool _enter() override;
+    void _exit() override;
+};
+
 class ModeCruise : public ModeCourseHold
 {
 public:

diff --git a/ArduPlane/mode_auto_trim.cpp b/ArduPlane/mode_auto_trim.cpp
@@ -0,0 +1,18 @@
+#include "mode.h"
+#include "Plane.h"
+
+bool ModeAutoTrim::_enter()
+{
+    if (!ModeCourseHold::_enter()) {
+        return false;
+    }
+
+    plane.servos_auto_trim_start();
+
+    return true;
+}
+
+void ModeAutoTrim::_exit() {
+    plane.servos_auto_trim_stop();
+    ModeCourseHold::_enter();
+}
diff --git a/ArduPlane/mode_thermal.cpp b/ArduPlane/mode_thermal.cpp
@@ -136,6 +136,10 @@ bool ModeThermal::exit_heading_aligned() const
         int32_t target_heading_cd;
         return (!plane.mode_course_hold.get_target_heading_cd(target_heading_cd) || plane.mode_loiter.isHeadingLinedUp_cd(target_heading_cd));
     }
+    case Mode::Number::MODE_AUTO_TRIM: {
+        int32_t target_heading_cd;
+        return (!plane.mode_auto_trim.get_target_heading_cd(target_heading_cd) || plane.mode_loiter.isHeadingLinedUp_cd(target_heading_cd));
+    }
     case Mode::Number::CRUISE: {
         int32_t target_heading_cd;
         return (!plane.mode_cruise.get_target_heading_cd(target_heading_cd) || plane.mode_loiter.isHeadingLinedUp_cd(target_heading_cd));

diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp
@@ -223,7 +223,7 @@ int16_t Plane::rudder_input(void)
         return 0;
     }
 
-    if (control_mode->does_auto_navigation() || ((control_mode == &mode_cruise || control_mode == &mode_course_hold) && (g2.flight_options & FlightOptions::COURSE_HOLD_HEADING_CONTROL_WITH_YAW_STICK))) {
+    if (control_mode->does_auto_navigation() || ((control_mode == &mode_cruise || control_mode == &mode_course_hold || control_mode == &mode_auto_trim) && (g2.flight_options & FlightOptions::COURSE_HOLD_HEADING_CONTROL_WITH_YAW_STICK))) {
         return 0;
     }
 

diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp
@@ -78,6 +78,7 @@ bool Plane::allow_reverse_thrust(void) const
         allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CIRCLE);
         break;
     case Mode::Number::COURSE_HOLD:
+    case Mode::Number::MODE_AUTO_TRIM:
         allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_COURSE_HOLD);
         break;
     case Mode::Number::CRUISE:

diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
@@ -731,6 +731,7 @@ void Plane::set_servos_controlled(void)
                control_mode == &mode_acro ||
                control_mode == &mode_fbwa ||
                control_mode == &mode_course_hold ||
+               control_mode == &mode_auto_trim ||
                control_mode == &mode_autotune) {
         // a manual throttle mode
         if (!rc().has_valid_input()) {
@@ -1336,7 +1337,7 @@ bool Plane::servos_auto_trim_set(const Plane::ServoTrimSetEntry *const trim_set,
 void Plane::servos_auto_trim(void)
 {
     // only in auto modes and FBWA
-    if ((!control_mode->does_auto_throttle() || control_mode == &mode_takeoff || control_mode == &mode_auto) && control_mode != &mode_course_hold && control_mode != &mode_fbwa) {
+    if ((!control_mode->does_auto_throttle() || control_mode == &mode_takeoff || control_mode == &mode_auto) && control_mode != &mode_course_hold && control_mode != &mode_auto_trim && control_mode != &mode_fbwa) {
         return;
     }
     if (!hal.util->get_soft_armed()) {

diff --git a/modules/mavlink b/modules/mavlink