TradHeli: move pilot desired rotor speed to heli.pde

ArduCopter/ArduCopter.pde

@@ -1086,6 +1086,9 @@ static void throttle_loop()
     update_auto_armed();
 
 #if FRAME_CONFIG == HELI_FRAME
+    // update rotor speed
+    heli_update_rotor_speed_targets();
+
     // update trad heli swash plate movement
     heli_update_landing_swash();
 #endif

ArduCopter/heli.pde

@@ -225,4 +225,29 @@ static void heli_update_landing_swash()
     }
 }
 
+// heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
+static void heli_update_rotor_speed_targets()
+{
+    // get rotor control method
+    uint8_t rsc_control_mode = motors.get_rsc_mode();
+
+    switch (rsc_control_mode) {
+        case AP_MOTORS_HELI_RSC_MODE_NONE:
+            // even though pilot passes rotors speed directly to rotor ESC via receiver, motor lib needs to know if
+            // rotor is spinning in case we are using direct drive tailrotor which must be spun up at same time
+        case AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH:
+            // pass through pilot desired rotor speed
+            motors.set_desired_rotor_speed(g.rc_8.control_in);
+            break;
+        case AP_MOTORS_HELI_RSC_MODE_SETPOINT:
+            // pass setpoint through as desired rotor speed
+            if (g.rc_8.control_in > 0) {
+                motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
+            }else{
+                motors.set_desired_rotor_speed(0);
+            }
+            break;
+    }
+}
+
 #endif  // FRAME_CONFIG == HELI_FRAME

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -361,6 +361,12 @@ int16_t AP_MotorsHeli::get_pilot_desired_collective(int16_t control_in)
     return collective_out;
 }
 
+// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
+void AP_MotorsHeli::set_desired_rotor_speed(int16_t desired_speed)
+{
+    _rotor_desired = desired_speed;
+}
+
 // return true if the main rotor is up to speed
 bool AP_MotorsHeli::motor_runup_complete()
 {
@@ -398,7 +404,6 @@ void AP_MotorsHeli::output_armed()
         _rc_yaw->servo_out = _rc_yaw->control_in;
     }
 
-    //static int counter = 0;
     _rc_roll->calc_pwm();
     _rc_pitch->calc_pwm();
     _rc_throttle->calc_pwm();
@@ -406,8 +411,8 @@ void AP_MotorsHeli::output_armed()
 
     move_swash( _rc_roll->servo_out, _rc_pitch->servo_out, _rc_throttle->servo_out, _rc_yaw->servo_out );
 
-    // To-Do: clean-up the passing in of the desired rotor speed
+    // update rotor and direct drive esc speeds
-    rsc_control(_servo_rsc->control_in);
+    rsc_control();
 }
 
 // output_disarmed - sends commands to the motors
@@ -657,7 +662,7 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll
 
 // rsc_control - update value to send to tail and main rotor's ESC
 // desired_rotor_speed is a desired speed from 0 to 1000
-void AP_MotorsHeli::rsc_control(int16_t desired_rotor_speed)
+void AP_MotorsHeli::rsc_control()
 {
     // if disarmed output minimums
     if (!armed()) {
@@ -674,63 +679,13 @@ void AP_MotorsHeli::rsc_control(int16_t desired_rotor_speed)
         return;
     }
 
-    // handle simpler case of pilot directly controlling main rotor
-    if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) {
-        switch (_tail_type) {
-            // return immediately if no direct drive tail motor
-            case AP_MOTORS_HELI_TAILTYPE_SERVO:
-            case AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO:
-            default:
-                return;
-                break;
-
-            // direct drive variable pitch tail
-            case AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH:
-                if (desired_rotor_speed > 0) {
-                    // ramp up tail if main rotor on
-                    tail_ramp(_direct_drive_setpoint);
-                }else{
-                    // ramp down tail rotor if main rotor off
-                    tail_ramp(0);
-                }
-                return;
-                break;
-
-            // direct drive fixed pitch tail
-            case AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH:
-                // output fixed-pitch speed control if Ch8 is high
-                if (desired_rotor_speed > 0) {
-                    // copy yaw output to tail esc
-                    write_aux(_servo_4->servo_out);
-                }else{
-                    write_aux(0);
-                }
-                return;
-                break;
-        }
-        return;
-    }
-
-    // handle more complex case of channel 8 passthrough and external governor
-    int16_t temp_rotor_speed;
-    if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH) {
-        temp_rotor_speed = desired_rotor_speed;
-    }else{
-        // rotor speed is predefined set-point
-        if (desired_rotor_speed > 0) {
-            temp_rotor_speed = _rsc_setpoint;
-        }else{
-            temp_rotor_speed = 0;
-        }
-    }
-
     // ramp up or down main rotor and tail
-    if (temp_rotor_speed > 0) {
+    if (_rotor_desired > 0) {
         // ramp up tail rotor (this does nothing if not using direct drive variable pitch tail)
         tail_ramp(_direct_drive_tailspeed);
         // note: this always returns true if not using direct drive variable pitch tail
         if (tail_rotor_runup_complete()) {
-            rotor_ramp(temp_rotor_speed);
+            rotor_ramp(_rotor_desired);
         }
     }else{
         // shutting down main rotor
@@ -744,7 +699,7 @@ void AP_MotorsHeli::rsc_control(int16_t desired_rotor_speed)
     // direct drive fixed pitch tail servo gets copy of yaw servo out (ch4) while main rotor is running
     if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) {
         // output fixed-pitch speed control if Ch8 is high
-        if (temp_rotor_speed > 0 || _rotor_out > 0) {
+        if (_rotor_desired > 0 || _rotor_out > 0) {
             // copy yaw output to tail esc
             write_aux(_servo_4->servo_out);
         }else{

libraries/AP_Motors/AP_MotorsHeli.h

@@ -114,6 +114,7 @@ public:
         _collective_scalar_manual(1),
         _collective_out(0),
         _collective_mid_pwm(0),
+        _rotor_desired(0),
         _rotor_out(0),
         _rsc_ramp_increment(0.0f),
         _tail_direct_drive_out(0)
@@ -175,6 +176,15 @@ public:
     // set_collective_for_landing - limits collective from going too low if we know we are landed
     void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; }
 
+    // get_rsc_mode - gets the rotor speed control method (AP_MOTORS_HELI_RSC_MODE_NONE, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH or AP_MOTORS_HELI_RSC_MODE_SETPOINT)
+    uint8_t get_rsc_mode() { return _rsc_mode; }
+
+    // get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1000)
+    int16_t get_rsc_setpoint() { return _rsc_setpoint; }
+
+    // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
+    void set_desired_rotor_speed(int16_t desired_speed);
+
     // return true if the main rotor is up to speed
     bool motor_runup_complete();
 
@@ -205,7 +215,7 @@ private:
     void calculate_roll_pitch_collective_factors();
 
     // rsc_control - main function to update values to send to main rotor and tail rotor ESCs
-    void rsc_control(int16_t desired_rotor_speed);
+    void rsc_control();
 
     // rotor_ramp - ramps rotor towards target. result put rotor_out and sent to ESC
     void rotor_ramp(int16_t rotor_target);
@@ -272,7 +282,8 @@ private:
     float           _collective_scalar_manual;  // collective scalar to reduce the range of the collective movement while collective is being controlled manually (i.e. directly by the pilot)
     int16_t         _collective_out;            // actual collective pitch value.  Required by the main code for calculating cruise throttle
     int16_t         _collective_mid_pwm;        // collective mid parameter value converted to pwm form (i.e. 0 ~ 1000)
-    float           _rotor_out;                 // output to the rotor (0 ~ 1000)
+    int16_t         _rotor_desired;             // latest desired rotor speed from pilot
+    float           _rotor_out;                 // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000)
     float           _rsc_ramp_increment;        // the amount we can increase the rotor output during each 100hz iteration
     int16_t         _tail_direct_drive_out;     // current ramped speed of output on ch7 when using direct drive variable pitch tail type
 };