Copter: dev option so set-attitude-target thrust field used as thrust

ArduCopter/Copter.cpp

@@ -322,7 +322,7 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo
     Quaternion q;
     q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
 
-    mode_guided.set_angle(q,climb_rate_ms*100,use_yaw_rate,radians(yaw_rate_degs));
+    mode_guided.set_angle(q, climb_rate_ms*100, use_yaw_rate, radians(yaw_rate_degs), false);
     return true;
 }
 

ArduCopter/GCS_Mavlink.cpp

@@ -994,17 +994,25 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
             break;
         }
 
-        // convert thrust to climb rate
+        // check if the message's thrust field should be interpreted as a climb rate or as thrust
-        packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f);
+        const bool use_thrust = copter.g2.dev_options.get() & DevOptionSetAttitudeTarget_ThrustAsThrust;
-        float climb_rate_cms = 0.0f;
+
-        if (is_equal(packet.thrust, 0.5f)) {
+        float climb_rate_or_thrust;
-            climb_rate_cms = 0.0f;
+        if (use_thrust) {
-        } else if (packet.thrust > 0.5f) {
+            // interpret thrust as thrust
-            // climb at up to WPNAV_SPEED_UP
+            climb_rate_or_thrust = constrain_float(packet.thrust, -1.0f, 1.0f);
-            climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_default_speed_up();
         } else {
-            // descend at up to WPNAV_SPEED_DN
+            // convert thrust to climb rate
-            climb_rate_cms = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_default_speed_down());
+            packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f);
+            if (is_equal(packet.thrust, 0.5f)) {
+                climb_rate_or_thrust = 0.0f;
+            } else if (packet.thrust > 0.5f) {
+                // climb at up to WPNAV_SPEED_UP
+                climb_rate_or_thrust = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_default_speed_up();
+            } else {
+                // descend at up to WPNAV_SPEED_DN
+                climb_rate_or_thrust = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_default_speed_down());
+            }
         }
 
         // if the body_yaw_rate field is ignored, use the commanded yaw position
@@ -1015,7 +1023,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
         }
 
         copter.mode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
-            climb_rate_cms, use_yaw_rate, packet.body_yaw_rate);
+                climb_rate_or_thrust, use_yaw_rate, packet.body_yaw_rate, use_thrust);
 
         break;
     }

ArduCopter/Parameters.cpp

@@ -776,7 +776,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Param: DEV_OPTIONS
     // @DisplayName: Development options
     // @Description: Bitmask of developer options. The meanings of the bit fields in this parameter may vary at any time. Developers should check the source code for current meaning
-    // @Bitmask: 0:ADSBMavlinkProcessing,1:DevOptionVFR_HUDRelativeAlt
+    // @Bitmask: 0:ADSBMavlinkProcessing,1:DevOptionVFR_HUDRelativeAlt,2:SetAttitudeTarget_ThrustAsThrust
     // @User: Advanced
     AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0),
 

ArduCopter/defines.h

@@ -141,6 +141,7 @@ enum PayloadPlaceStateType {
 enum DevOptions {
     DevOptionADSBMAVLink = 1,
     DevOptionVFR_HUDRelativeAlt = 2,
+    DevOptionSetAttitudeTarget_ThrustAsThrust = 4,
 };
 
 //  Logging parameters

ArduCopter/mode.h

@@ -791,7 +791,7 @@ public:
 
     bool requires_terrain_failsafe() const override { return true; }
 
-    void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
+    void set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust);
     bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool terrain_alt = false);
     bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
     bool get_wp(Location &loc) override;

ArduCopter/mode_guided.cpp

@@ -24,8 +24,10 @@ struct {
     float pitch_cd;
     float yaw_cd;
     float yaw_rate_cds;
-    float climb_rate_cms;
+    float climb_rate_cms;   // climb rate in cms.  Used if use_thrust is false
+    float thrust;           // thrust from -1 to 1.  Used if use_thrust is true
     bool use_yaw_rate;
+    bool use_thrust;
 } static guided_angle_state;
 
 struct Guided_Limit {
@@ -310,8 +312,8 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto
     return true;
 }
 
-// set guided mode angle target
+// set guided mode angle target and climbrate
-void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
+void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust)
 {
     // check we are in velocity control mode
     if (guided_mode != Guided_Angle) {
@@ -326,18 +328,26 @@ void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_y
     guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f;
     guided_angle_state.use_yaw_rate = use_yaw_rate;
 
-    guided_angle_state.climb_rate_cms = climb_rate_cms;
+    guided_angle_state.use_thrust = use_thrust;
+    if (use_thrust) {
+        guided_angle_state.thrust = climb_rate_cms_or_thrust;
+        guided_angle_state.climb_rate_cms = 0.0f;
+    } else {
+        guided_angle_state.thrust = 0.0f;
+        guided_angle_state.climb_rate_cms = climb_rate_cms_or_thrust;
+    }
+
     guided_angle_state.update_time_ms = millis();
 
-    // interpret positive climb rate as triggering take-off
+    // interpret positive climb rate or thrust as triggering take-off
-    if (motors->armed() && !copter.ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
+    if (motors->armed() && !copter.ap.auto_armed && is_positive(climb_rate_cms_or_thrust)) {
         copter.set_auto_armed(true);
     }
 
     // log target
     copter.Log_Write_GuidedTarget(guided_mode,
                            Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
-                           Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms));
+                           Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust));
 }
 
 // guided_run - runs the guided controller
@@ -567,11 +577,14 @@ void ModeGuided::angle_control_run()
     float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
     float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);
 
-    // constrain climb rate
+    float climb_rate_cms = 0.0f;
-    float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up());
+    if (!guided_angle_state.use_thrust) {
+        // constrain climb rate
+        climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up());
 
-    // get avoidance adjusted climb rate
+        // get avoidance adjusted climb rate
-    climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);
+        climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);
+    }
 
     // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
     uint32_t tnow = millis();
@@ -580,6 +593,7 @@ void ModeGuided::angle_control_run()
         pitch_in = 0.0f;
         climb_rate_cms = 0.0f;
         yaw_rate_in = 0.0f;
+        guided_angle_state.use_thrust = false;
     }
 
     // if not armed set throttle to zero and exit immediately
@@ -611,8 +625,12 @@ void ModeGuided::angle_control_run()
     }
 
     // call position controller
-    pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
+    if (guided_angle_state.use_thrust) {
-    pos_control->update_z_controller();
+        attitude_control->set_throttle_out(guided_angle_state.thrust, true, copter.g.throttle_filt);
+    } else {
+        pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
+        pos_control->update_z_controller();
+    }
 }
 
 // helper function to update position controller's desired velocity while respecting acceleration limits