Blimp: move rudder arming up to RC_Channel

Blimp/AP_Arming.cpp

@@ -373,6 +373,12 @@ bool AP_Arming_Blimp::disarm(const AP_Arming::Method method, bool do_disarm_chec
         return true;
     }
 
+    if (method == AP_Arming::Method::RUDDER) {
+        if (!blimp.flightmode->has_manual_throttle() && !blimp.ap.land_complete) {
+            return false;
+        }
+    }
+
     if (!AP_Arming::disarm(method, do_disarm_checks)) {
         return false;
     }

Blimp/Blimp.cpp

@@ -69,7 +69,6 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_GPS, &blimp.gps, update, 50, 200,   9),
     SCHED_TASK(update_batt_compass,   10,    120,  12),
     SCHED_TASK_CLASS(RC_Channels,          (RC_Channels*)&blimp.g2.rc_channels,      read_aux_all,    10,     50,  15),
-    SCHED_TASK(arm_motors_check,      10,     50,  18),
     SCHED_TASK(update_altitude,       10,    100,  21),
     SCHED_TASK(three_hz_loop,          3,     75,  24),
 #if AP_SERVORELAYEVENTS_ENABLED

Blimp/Blimp.h

@@ -239,10 +239,6 @@ private:
     // 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;
-
     // last valid RC input time
     uint32_t last_radio_update_ms;
 
@@ -412,11 +408,6 @@ private:
     bool rangefinder_alt_ok();
     bool rangefinder_up_ok();
 
-    // RC_Channel.cpp
-    void save_trim();
-    void auto_trim();
-    void auto_trim_cancel();
-
     // system.cpp
     void init_ardupilot() override;
     void startup_INS_ground();

Blimp/RC_Channel.cpp

@@ -107,7 +107,7 @@ bool RC_Channel_Blimp::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch
         if ((ch_flag == AuxSwitchPos::HIGH) &&
             (blimp.control_mode <= Mode::Number::MANUAL) &&
             (blimp.channel_up->get_control_in() == 0)) {
-            blimp.save_trim();
+            blimp.g2.rc_channels.save_trim();
         }
         break;
 
@@ -126,26 +126,26 @@ bool RC_Channel_Blimp::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch
 }
 
 // save_trim - adds roll and pitch trims from the radio to ahrs
-void Blimp::save_trim()
+void RC_Channels_Blimp::save_trim()
 {
     // save roll and pitch trim
     float roll_trim = ToRad((float)channel_right->get_control_in()/100.0f);
     float pitch_trim = ToRad((float)channel_front->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 blimp level
-void Blimp::auto_trim_cancel()
+void RC_Channels_Blimp::auto_trim_cancel()
 {
     auto_trim_counter = 0;
     AP_Notify::flags.save_trim = false;
     gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim cancelled");
 }
 
-void Blimp::auto_trim()
+void RC_Channels_Blimp::auto_trim()
 {
     if (auto_trim_counter > 0) {
         if (blimp.flightmode != &blimp.mode_manual ||
@@ -158,14 +158,14 @@ void Blimp::auto_trim()
         AP_Notify::flags.save_trim = true;
 
         if (!auto_trim_started) {
-            if (ap.land_complete) {
+            if (blimp.ap.land_complete) {
                 // haven't taken off yet
                 return;
             }
             auto_trim_started = true;
         }
 
-        if (ap.land_complete) {
+        if (blimp.ap.land_complete) {
             // landed again.
             auto_trim_cancel();
             return;
@@ -174,14 +174,14 @@ void Blimp::auto_trim()
         auto_trim_counter--;
 
         // calculate roll trim adjustment
-        float roll_trim_adjustment = ToRad((float)channel_right->get_control_in() / 4000.0f);
+        const float roll_trim_adjustment = ToRad((float)blimp.channel_right->get_control_in() / 4000.0f);
 
         // calculate pitch trim adjustment
-        float pitch_trim_adjustment = ToRad((float)channel_front->get_control_in() / 4000.0f);
+        const float pitch_trim_adjustment = ToRad((float)blimp.channel_front->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) {

Blimp/RC_Channel.h

@@ -43,8 +43,20 @@ public:
         return &obj_channels[chan];
     }
 
+    // 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;
 
+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;
+
 };

Blimp/motors.cpp

@@ -5,73 +5,6 @@
 #define AUTO_TRIM_DELAY         100 // called at 10hz so 10 seconds
 #define LOST_VEHICLE_DELAY      10  // called at 10hz so 1 second
 
-// arm_motors_check - checks for pilot input to arm or disarm the blimp
-// called at 10hz
-void Blimp::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) {
-        return;
-    }
-
-    // ensure throttle is down
-    if (channel_up->get_control_in() > 0) { //MIR what dow we do with this?
-        arming_counter = 0;
-        return;
-    }
-
-    int16_t yaw_in = channel_yaw->get_control_in();
-
-    // full right
-    if (yaw_in > 900) {
-
-        // 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() && control_mode == Mode::Number::MANUAL) {
-            gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start");
-            auto_trim_counter = 250;
-            auto_trim_started = false;
-        }
-
-        // full left and rudder disarming is enabled
-    } else if ((yaw_in < -900) && (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;
-    }
-}
-
-
 // motors_output - send output to motors library which will adjust and send to ESCs and servos
 void Blimp::motors_output()
 {