Copter: turtle mode should output to motors from within motors_output()

move motor output to flightmode

ArduCopter/mode.cpp

@@ -859,6 +859,12 @@ float Mode::get_avoidance_adjusted_climbrate(float target_rate)
 #endif
 }
 
+// send output to the motors, can be overridden by subclasses
+void Mode::output_to_motors()
+{
+    motors->output();
+}
+
 Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms)
 {
     // Alt Hold State Machine Determination

ArduCopter/mode.h

@@ -103,6 +103,9 @@ public:
         return pos_control->get_vel_desired_cms();
     }
 
+    // send output to the motors, can be overridden by subclasses
+    virtual void output_to_motors();
+
 protected:
 
     // helper functions
@@ -1529,10 +1532,15 @@ public:
     bool allows_arming(AP_Arming::Method method) const override;
     bool is_autopilot() const override { return false; }
     void change_motor_direction(bool reverse);
+    void output_to_motors() override;
 
 protected:
     const char *name() const override { return "TURTLE"; }
     const char *name4() const override { return "TRTL"; }
+
+private:
+    float motors_output;
+    Vector2f motors_input;
 };
 #endif
 

ArduCopter/mode_turtle.cpp

@@ -112,9 +112,6 @@ void ModeTurtle::run()
         stickDeflectionExpoLength = stickDeflectionYawExpo;
         signRoll = 0;
         signPitch = 0;
-    } else {
-        // If pitch/roll dominant, disable yaw
-        //signYaw = 0;
     }
 
     const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength) : 0;
@@ -138,11 +135,14 @@ void ModeTurtle::run()
 
     // notmalise the roll and pitch input to match the motors
     Vector2f input {signRoll, signPitch};
-    input = input.normalized() * 0.5;
+    motors_input = input.normalized() * 0.5;
-
     // we bypass spin min and friends in the deadzone because we only want spin up when the sticks are moved
-    float motorOutput = !is_zero(flipPower) ? motors->thrust_to_actuator(flipPower) : 0.0f;
+    motors_output = !is_zero(flipPower) ? motors->thrust_to_actuator(flipPower) : 0.0f;
+}
 
+// actually write values to the motors
+void ModeTurtle::output_to_motors()
+{
     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
         if (!motors->is_motor_enabled(i)) {
             continue;
@@ -150,13 +150,13 @@ void ModeTurtle::run()
 
         const Vector2f output {motors->get_roll_factor(i), motors->get_pitch_factor(i)};
         // if output aligns with input then use this motor
-        if ((input - output).length() > 0.5) {
+        if ((motors_input - output).length() > 0.5) {
             motors->rc_write(i, motors->get_pwm_output_min());
             continue;
         }
 
         int16_t pwm = motors->get_pwm_output_min()
-            + (motors->get_pwm_output_max() - motors->get_pwm_output_min()) * motorOutput;
+            + (motors->get_pwm_output_max() - motors->get_pwm_output_min()) * motors_output;
 
         motors->rc_write(i, pwm);
     }

ArduCopter/motors.cpp

@@ -175,7 +175,7 @@ void Copter::motors_output()
         }
 
         // send output signals to motors
-        motors->output();
+        flightmode->output_to_motors();
     }
 
     // push all channels