Rover: AP_MotorsUGV: add wingsail output

APMrover2/AP_MotorsUGV.cpp

@@ -171,6 +171,9 @@ void AP_MotorsUGV::setup_servo_output()
 
     // mainsail range from 0 to 100
     SRV_Channels::set_range(SRV_Channel::k_mainsail_sheet, 100);
+    // wing sail -100 to 100
+    SRV_Channels::set_angle(SRV_Channel::k_wingsail_elevator, 100);
+
 }
 
 // set steering as a value from -4500 to +4500
@@ -206,6 +209,12 @@ void AP_MotorsUGV::set_mainsail(float mainsail)
     _mainsail = constrain_float(mainsail, 0.0f, 100.0f);
 }
 
+// set wingsail input as a value from -100 to 100
+void AP_MotorsUGV::set_wingsail(float wingsail)
+{
+    _wingsail = constrain_float(wingsail, -100.0f, 100.0f);
+}
+
 // get slew limited throttle
 // used by manual mode to avoid bad steering behaviour during transitions from forward to reverse
 // same as private slew_limit_throttle method (see below) but does not update throttle state
@@ -234,7 +243,7 @@ bool AP_MotorsUGV::have_skid_steering() const
 // true if the vehicle has a mainsail
 bool AP_MotorsUGV::has_sail() const
 {
-    return SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet);
+    return SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet) || SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator);
 }
 
 void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
@@ -260,8 +269,8 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
     // output for omni frames
     output_omni(armed, _steering, _throttle, _lateral);
 
-    // output to mainsail
-    output_mainsail();
+    // output to sails
+    output_sail();
 
     // send values to the PWM timers for output
     SRV_Channels::calc_pwm();
@@ -321,6 +330,9 @@ bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct)
             if (SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet)) {
                 SRV_Channels::set_output_scaled(SRV_Channel::k_mainsail_sheet, pct);
             }
+            if (SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator)) {
+                SRV_Channels::set_output_scaled(SRV_Channel::k_wingsail_elevator, pct);
+            }
             break;
         }
         case MOTOR_TEST_LAST:
@@ -381,6 +393,9 @@ bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm)
             if (SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet)) {
                 SRV_Channels::set_output_pwm(SRV_Channel::k_mainsail_sheet, pwm);
             }
+            if (SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator)) {
+                SRV_Channels::set_output_pwm(SRV_Channel::k_wingsail_elevator, pwm);
+            }
             break;
         }
         default:
@@ -786,14 +801,15 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f
     }
 }
 
-// output for sailboat's mainsail
-void AP_MotorsUGV::output_mainsail()
+// output for sailboat's sails
+void AP_MotorsUGV::output_sail()
 {
     if (!has_sail()) {
         return;
     }
 
     SRV_Channels::set_output_scaled(SRV_Channel::k_mainsail_sheet, _mainsail);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_wingsail_elevator, _wingsail);
 }
 
 // slew limit throttle for one iteration

APMrover2/AP_MotorsUGV.h

@@ -64,10 +64,14 @@ public:
     // set lateral input as a value from -100 to +100
     void set_lateral(float lateral);
 
-    // set mainsail input as a value from 0 to 100
+    // set or get mainsail input as a value from 0 to 100
     void set_mainsail(float mainsail);
     float get_mainsail() const { return _mainsail; }
 
+    // set or get wingsail input as a value from -100 to 100
+    void set_wingsail(float wingsail);
+    float get_wingsail() const { return _wingsail; }
+
     // get slew limited throttle
     // used by manual mode to avoid bad steering behaviour during transitions from forward to reverse
     // same as private slew_limit_throttle method (see below) but does not update throttle state
@@ -138,10 +142,10 @@ protected:
     // dt is the main loop time interval and is required when rate control is required
     void output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle, float dt = 0.0f);
 
-    // output for sailboat's mainsail in the range of 0 to 100
-    void output_mainsail();
+    // output for sailboat's mainsail in the range of 0 to 100 and wing sail in the range +- 100
+    void output_sail();
 
-    // true if the vehicle has a mainsail
+    // true if the vehicle has a mainsail or wing sail
     bool has_sail() const;
 
     // slew limit throttle for one iteration
@@ -179,6 +183,7 @@ protected:
     bool    _scale_steering = true; // true if we should scale steering by speed or angle
     float   _lateral;  // requested lateral input as a value from -100 to +100
     float   _mainsail;  // requested mainsail input as a value from 0 to 100
+    float   _wingsail;  // requested wing sail input as a value in the range +- 100
 
     // omni variables
     float   _throttle_factor[AP_MOTORS_NUM_MOTORS_MAX];