Move rudder arming up into RC_Channel library

ArduCopter/AP_Arming.cpp

@@ -554,6 +554,16 @@ bool AP_Arming_Copter::alt_checks(bool display_failure)
 //  has side-effect that logging is started
 bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
 {
+    if (method == AP_Arming::Method::RUDDER) {
+#if TOY_MODE_ENABLED
+        if (g2.toy_mode.enabled()) {
+            // not armed with sticks in toy mode
+            check_failed(true, "Rudder-arming: disabled in toy mode");
+            return false;
+        }
+#endif
+    }
+
     const auto &ahrs = AP::ahrs();
 
     // always check if inertial nav has started and is ready
@@ -787,6 +797,12 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che
         return false;
     }
 
+    if (method == AP_Arming::Method::RUDDER) {
+        if (!copter.flightmode->has_manual_throttle() && !copter.ap.land_complete) {
+            return false;
+        }
+    }
+
     if (!AP_Arming::disarm(method, do_disarm_checks)) {
         return false;
     }

ArduCopter/Copter.cpp

@@ -159,7 +159,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
 #endif
     SCHED_TASK(update_batt_compass,   10,    120, 15),
     SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all,    10,  50,  18),
-    SCHED_TASK(arm_motors_check,      10,     50, 21),
 #if TOY_MODE_ENABLED
     SCHED_TASK_CLASS(ToyMode,              &copter.g2.toy_mode,         update,          10,  50,  24),
 #endif

ArduCopter/Copter.h

@@ -493,10 +493,6 @@ class Copter : public AP_Vehicle {
     // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed.
     uint32_t arm_time_ms;
 
-    // Used to exit the roll and pitch auto trim function
-    uint8_t auto_trim_counter;
-    bool auto_trim_started = false;
-
     // Camera
 #if AP_CAMERA_ENABLED
     AP_Camera camera{MASK_LOG_CAMERA};
@@ -950,7 +946,6 @@ class Copter : public AP_Vehicle {
     void motor_test_stop();
 
     // motors.cpp
-    void arm_motors_check();
     void auto_disarm_check();
     void motors_output(bool full_push = true);
     void motors_output_main();
@@ -996,11 +991,6 @@ class Copter : public AP_Vehicle {
     // takeoff_check.cpp
     void takeoff_check();
 
-    // RC_Channel.cpp
-    void save_trim();
-    void auto_trim();
-    void auto_trim_cancel();
-
     // system.cpp
     void init_ardupilot() override;
     void startup_INS_ground();

ArduCopter/RC_Channel.cpp

@@ -210,7 +210,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
             if ((ch_flag == AuxSwitchPos::HIGH) &&
                 (copter.flightmode->allows_save_trim()) &&
                 (copter.channel_throttle->get_control_in() == 0)) {
-                copter.save_trim();
+                copter.g2.rc_channels.save_trim();
             }
             break;
 
@@ -703,27 +703,53 @@ void RC_Channel_Copter::do_aux_function_change_force_flying(const AuxSwitchPos c
     }
 }
 
+void RC_Channels_Copter::rudder_10_second_callback()
+{
+    // if the user holds right-rudder for 10 seconds in stabilize then
+    // we enter auto-trim for 25 seconds
+
+    if (!copter.motors->armed()) {
+        return;
+    }
+
+    if (copter.flightmode != &copter.mode_stabilize) {
+        return;
+    }
+
+    if (auto_trim_counter) {
+        // already in auto-trim
+        return;
+    }
+
+    gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start");
+    auto_trim_counter = 250;
+    auto_trim_started = false;
+    // ensure auto-disarm doesn't trigger immediately
+    copter.auto_disarm_begin = millis();
+}
+
+
 // save_trim - adds roll and pitch trims from the radio to ahrs
-void Copter::save_trim()
+void RC_Channels_Copter::save_trim()
 {
     // save roll and pitch trim
     float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f);
     float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f);
-    ahrs.add_trim(roll_trim, pitch_trim);
+    AP::ahrs().add_trim(roll_trim, pitch_trim);
     LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM);
     gcs().send_text(MAV_SEVERITY_INFO, "Trim saved");
 }
 
 // auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
 // meant to be called continuously while the pilot attempts to keep the copter level
-void Copter::auto_trim_cancel()
+void RC_Channels_Copter::auto_trim_cancel()
 {
     auto_trim_counter = 0;
     AP_Notify::flags.save_trim = false;
     gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim cancelled");
 }
 
-void Copter::auto_trim()
+void RC_Channels_Copter::auto_trim()
 {
     if (auto_trim_counter > 0) {
         if (copter.flightmode != &copter.mode_stabilize ||
@@ -736,14 +762,14 @@ void Copter::auto_trim()
         AP_Notify::flags.save_trim = true;
 
         if (!auto_trim_started) {
-            if (ap.land_complete) {
+            if (copter.ap.land_complete) {
                 // haven't taken off yet
                 return;
             }
             auto_trim_started = true;
         }
 
-        if (ap.land_complete) {
+        if (copter.ap.land_complete) {
             // landed again.
             auto_trim_cancel();
             return;
@@ -752,14 +778,14 @@ void Copter::auto_trim()
         auto_trim_counter--;
 
         // calculate roll trim adjustment
-        float roll_trim_adjustment = ToRad((float)channel_roll->get_control_in() / 4000.0f);
+        float roll_trim_adjustment = ToRad((float)copter.channel_roll->get_control_in() / 4000.0f);
 
         // calculate pitch trim adjustment
-        float pitch_trim_adjustment = ToRad((float)channel_pitch->get_control_in() / 4000.0f);
+        float pitch_trim_adjustment = ToRad((float)copter.channel_pitch->get_control_in() / 4000.0f);
 
         // add trim to ahrs object
         // save to eeprom on last iteration
-        ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
+        AP::ahrs().add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
 
         // on last iteration restore leds and accel gains to normal
         if (auto_trim_counter == 0) {

ArduCopter/RC_Channel.h

@@ -46,8 +46,22 @@ class RC_Channels_Copter : public RC_Channels
     // returns true if throttle arming checks should be run
     bool arming_check_throttle() const override;
 
+    // auto-trim functionality; adjust ahrstrim based on momentary RC Input
+    void save_trim();
+    void auto_trim();
+
 protected:
 
     int8_t flight_mode_channel_number() const override;
+    void rudder_10_second_callback() override;
+
+private:
+
+    // Used to exit the roll and pitch auto trim function
+    void auto_trim_cancel();
+
+    uint8_t auto_trim_counter;
+    bool auto_trim_started = false;
+
 
 };

ArduCopter/motors.cpp

@@ -5,84 +5,6 @@
 #define AUTO_TRIM_DELAY         100 // called at 10hz so 10 seconds
 #define LOST_VEHICLE_DELAY      10  // called at 10hz so 1 second
 
-static uint32_t auto_disarm_begin;
-
-// arm_motors_check - checks for pilot input to arm or disarm the copter
-// called at 10hz
-void Copter::arm_motors_check()
-{
-    static int16_t arming_counter;
-
-    // check if arming/disarm using rudder is allowed
-    AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type();
-    if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {
-        arming_counter = 0;
-        return;
-    }
-
-#if TOY_MODE_ENABLED
-    if (g2.toy_mode.enabled()) {
-        // not armed with sticks in toy mode
-        return;
-    }
-#endif
-
-    // ensure throttle is down
-    if (channel_throttle->get_control_in() > 0) {
-        arming_counter = 0;
-        return;
-    }
-
-    int16_t yaw_in = channel_yaw->get_control_in();
-
-    // full right
-    if (yaw_in > 4000) {
-
-        // increase the arming counter to a maximum of 1 beyond the auto trim counter
-        if (arming_counter <= AUTO_TRIM_DELAY) {
-            arming_counter++;
-        }
-
-        // arm the motors and configure for flight
-        if (arming_counter == ARM_DELAY && !motors->armed()) {
-            // reset arming counter if arming fail
-            if (!arming.arm(AP_Arming::Method::RUDDER)) {
-                arming_counter = 0;
-            }
-        }
-
-        // arm the motors and configure for flight
-        if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && flightmode->mode_number() == Mode::Number::STABILIZE) {
-            gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start");
-            auto_trim_counter = 250;
-            auto_trim_started = false;
-            // ensure auto-disarm doesn't trigger immediately
-            auto_disarm_begin = millis();
-        }
-
-    // full left and rudder disarming is enabled
-    } else if ((yaw_in < -4000) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) {
-        if (!flightmode->has_manual_throttle() && !ap.land_complete) {
-            arming_counter = 0;
-            return;
-        }
-
-        // increase the counter to a maximum of 1 beyond the disarm delay
-        if (arming_counter <= DISARM_DELAY) {
-            arming_counter++;
-        }
-
-        // disarm the motors
-        if (arming_counter == DISARM_DELAY && motors->armed()) {
-            arming.disarm(AP_Arming::Method::RUDDER);
-        }
-
-    // Yaw is centered so reset arming counter
-    } else {
-        arming_counter = 0;
-    }
-}
-
 // auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
 void Copter::auto_disarm_check()
 {

ArduPlane/AP_Arming.cpp

@@ -250,24 +250,6 @@ bool AP_Arming_Plane::ins_checks(bool display_failure)
 
 bool AP_Arming_Plane::arm_checks(AP_Arming::Method method)
 {
-    if (method == AP_Arming::Method::RUDDER) {
-        const AP_Arming::RudderArming arming_rudder = get_rudder_arming_type();
-
-        if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {
-            //parameter disallows rudder arming/disabling
-
-            // if we emit a message here then someone doing surface
-            // checks may be bothered by the message being emitted.
-            // check_failed(true, "Rudder arming disabled");
-            return false;
-        }
-
-        // if throttle is not down, then pilot cannot rudder arm/disarm
-        if (!is_zero(plane.get_throttle_input())){
-            check_failed(true, "Non-zero throttle");
-            return false;
-        }
-    }
 
     //are arming checks disabled?
     if (checks_to_perform == 0) {
@@ -339,14 +321,6 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
             return false;
         }
     }
-
-    if (do_disarm_checks && method == AP_Arming::Method::RUDDER) {
-        // option must be enabled:
-        if (get_rudder_arming_type() != AP_Arming::RudderArming::ARMDISARM) {
-            gcs().send_text(MAV_SEVERITY_INFO, "Rudder disarm: disabled");
-            return false;
-        }
-    }
 
     if (!AP_Arming::disarm(method, do_disarm_checks)) {
         return false;

ArduPlane/Plane.h

@@ -828,12 +828,6 @@ class Plane : public AP_Vehicle {
     static const AP_Scheduler::Task scheduler_tasks[];
     static const AP_Param::Info var_info[];
 
-    // time that rudder arming has been running
-    uint32_t rudder_arm_timer;
-
-    // have we seen neutral rudder since arming with rudder?
-    bool seen_neutral_rudder;
-
 #if HAL_QUADPLANE_ENABLED
     // support for quadcopter-plane
     QuadPlane quadplane{ahrs};
@@ -1112,7 +1106,6 @@ class Plane : public AP_Vehicle {
     void init_rc_in();
     void init_rc_out_main();
     void init_rc_out_aux();
-    void rudder_arm_disarm_check();
     void read_radio();
     int16_t rudder_input(void);
     void control_failsafe();