compare 4.2 L1 to 3.6 L1

ArduCopter/Copter.h

@@ -52,6 +52,7 @@
 #include <AP_Vehicle/AP_Vehicle.h>          // needed for AHRS build
 #include <AP_InertialNav/AP_InertialNav.h>  // inertial navigation library
 #include <AC_WPNav/AC_WPNav.h>              // ArduCopter waypoint navigation library
+#include <AC_WPNav/AC_WPNav_Heli.h>
 #include <AC_WPNav/AC_Loiter.h>             // ArduCopter Loiter Mode Library
 #include <AC_WPNav/AC_Circle.h>             // circle navigation library
 #include <AP_Declination/AP_Declination.h>  // ArduPilot Mega Declination Helper Library
@@ -69,6 +70,8 @@
 #include <AC_Sprayer/AC_Sprayer.h>          // Crop sprayer library
 #include <AP_ADSB/AP_ADSB.h>                // ADS-B RF based collision avoidance module library
 #include <AP_Proximity/AP_Proximity.h>      // ArduPilot proximity sensor library
+#include <AP_L1_Control/AP_L1_Control_Heli.h>
+#include <AP_SpdHgtControl/AP_SpdHgtControl_Heli.h>
 
 // Configuration
 #include "defines.h"
@@ -80,6 +83,12 @@
     #define AC_AttitudeControl_t AC_AttitudeControl_Multi
 #endif
 
+#if FRAME_CONFIG == HELI_FRAME
+    #define AC_WPNav_t AC_WPNav_Heli
+#else
+    #define AC_WPNav_t AC_WPNav
+#endif
+
 #if FRAME_CONFIG == HELI_FRAME
  #define MOTOR_CLASS AP_MotorsHeli
 #else
@@ -461,13 +470,18 @@ class Copter : public AP_Vehicle {
     // To-Do: move inertial nav up or other navigation variables down here
     AC_AttitudeControl_t *attitude_control;
     AC_PosControl *pos_control;
-    AC_WPNav *wp_nav;
+    AC_WPNav_t *wp_nav;
     AC_Loiter *loiter_nav;
 
 #if MODE_CIRCLE_ENABLED == ENABLED
     AC_Circle *circle_nav;
 #endif
 
+    AP_SpdHgtControl_Heli helispdhgtctrl{ahrs};
+
+    // L1 Controller and SpdHgt controller declarations
+    AP_L1_Control_Heli L1_controller{ahrs, &helispdhgtctrl};
+
     // System Timers
     // --------------
     // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed.

ArduCopter/Parameters.cpp

@@ -493,8 +493,12 @@ const AP_Param::Info Copter::var_info[] = {
     GOBJECT(ins,            "INS_", AP_InertialSensor),
 
     // @Group: WPNAV_
-    // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp
+    // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp,../libraries/AC_WPNav/AC_WPNav_Heli.cpp
+#if FRAME_CONFIG == HELI_FRAME
+    GOBJECTPTR(wp_nav, "WPNAV_",       AC_WPNav_Heli),
+#else
     GOBJECTPTR(wp_nav, "WPNAV_",       AC_WPNav),
+#endif
 
     // @Group: LOIT_
     // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp
@@ -1137,6 +1141,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_SUBGROUPINFO(command_model_pilot, "PILOT_Y_", 56, ParametersG2, AC_CommandModel),
 
+    // @Group: NAVL1_
+    // @Path: ../libraries/AP_L1_Control/AP_L1_Control_Heli.cpp
+    AP_SUBGROUPPTR(L1_controller_ptr, "NAVL1_", 57, ParametersG2, AP_L1_Control_Heli),
+
+    // @Group: SPDHGT_
+    // @Path: ../libraries/AP_SpdHgtControl/AP_SpdHgtControl_Heli.cpp
+    AP_SUBGROUPPTR(helispdhgtctrl_ptr, "SPDHGT_", 58, ParametersG2, AP_SpdHgtControl_Heli),
+
     AP_GROUPEND
 };
 
@@ -1191,6 +1203,9 @@ ParametersG2::ParametersG2(void)
 #endif
 
     ,command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f)
+
+    ,L1_controller_ptr(&copter.L1_controller)
+    ,helispdhgtctrl_ptr(&copter.helispdhgtctrl)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }

ArduCopter/Parameters.h

@@ -663,6 +663,10 @@ class ParametersG2 {
     AP_Float guided_timeout;
 #endif
 
+    void *L1_controller_ptr;
+    void *helispdhgtctrl_ptr;
+
+
     AP_Int8                 surftrak_mode;
     AP_Int8                 failsafe_dr_enable;
     AP_Int16                failsafe_dr_timeout;

ArduCopter/mode.h

@@ -166,7 +166,7 @@ class Mode {
     // convenience references to avoid code churn in conversion:
     Parameters &g;
     ParametersG2 &g2;
-    AC_WPNav *&wp_nav;
+    AC_WPNav_t *&wp_nav;
     AC_Loiter *&loiter_nav;
     AC_PosControl *&pos_control;
     AP_InertialNav &inertial_nav;

ArduCopter/mode_auto.cpp

@@ -975,13 +975,37 @@ void ModeAuto::wp_run()
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
+#if FRAME_CONFIG == HELI_FRAME
+    if (wp_nav->use_l1_navigation() && !wp_nav->is_spline()) {
+        copter.failsafe_terrain_set_status(wp_nav->update_l1_wpnav());
+    } else {
+        // run waypoint controller
+        copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
+    }
+#else
     // run waypoint controller
     copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
+#endif
 
     // WP_Nav has set the vertical position control targets
     // run the vertical position controller and set output throttle
     pos_control->update_z_controller();
 
+#if FRAME_CONFIG == HELI_FRAME
+    if (wp_nav->use_l1_navigation() && !wp_nav->is_spline()) {
+        target_yaw_rate += wp_nav->get_yaw_rate();
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_l1_roll(), wp_nav->get_l1_pitch(), target_yaw_rate);
+    } else {
+        // call attitude controller
+        if (auto_yaw.mode() == AUTO_YAW_HOLD) {
+            // roll & pitch from waypoint controller, yaw rate from pilot
+            attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
+        } else {
+            // roll, pitch from waypoint controller, yaw heading from auto_heading()
+            attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
+        }
+    }
+#else
     // call attitude controller
     if (auto_yaw.mode() == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
@@ -990,6 +1014,7 @@ void ModeAuto::wp_run()
         // roll, pitch from waypoint controller, yaw heading from auto_heading()
         attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
     }
+#endif
 }
 
 // auto_land_run - lands in auto mode
@@ -1368,14 +1393,17 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 // returns true on success, false on failure which should only happen due to a failure to retrieve terrain data
 bool ModeAuto::set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc)
 {
+    wp_nav->set_stopping_at_wp(false);
     // do not add next wp if current command has a delay meaning the vehicle will stop at the destination
     if (current_cmd.p1 > 0) {
+    wp_nav->set_stopping_at_wp(true);
         return true;
     }
 
     // do not add next wp if there are no more navigation commands
     AP_Mission::Mission_Command next_cmd;
     if (!mission.get_next_nav_cmd(current_cmd.index+1, next_cmd)) {
+    wp_nav->set_stopping_at_wp(true);
         return true;
     }
 
@@ -1489,8 +1517,18 @@ void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
         circle_radius_m *= 10;
     }
 
+#if FRAME_CONFIG == HELI_FRAME
+    if (wp_nav->use_l1_navigation()) {
+        // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
+        circle_movetoedge_start(circle_center, 0);
+    } else {
+        // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
+        circle_movetoedge_start(circle_center, circle_radius_m);
+    }
+#else
     // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
     circle_movetoedge_start(circle_center, circle_radius_m);
+#endif
 }
 
 // do_loiter_time - initiate loitering at a point for a given time period
@@ -1544,7 +1582,8 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
 {
     // calculate default location used when lat, lon or alt is zero
     Location default_loc = copter.current_loc;
-    if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
+    if ((wp_nav->is_active() && wp_nav->reached_wp_destination())
+         || (wp_nav->is_L1_active() && wp_nav->reached_l1_destination())) {
         if (!wp_nav->get_wp_destination_loc(default_loc)) {
             // this should never happen
             INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
@@ -1831,6 +1870,26 @@ bool ModeAuto::verify_land()
 
     switch (state) {
         case State::FlyToLocation:
+#if FRAME_CONFIG == HELI_FRAME
+            if (wp_nav->use_l1_navigation()) {
+                if (copter.wp_nav->reached_l1_destination()) {
+                    // initialise landing controller
+                    land_start();
+
+                    // advance to next state
+                    state = State::Descending;
+                }
+            } else {
+                // check if we've reached the location
+                if (copter.wp_nav->reached_wp_destination()) {
+                    // initialise landing controller
+                    land_start();
+
+                    // advance to next state
+                    state = State::Descending;
+                }
+            }
+#else
             // check if we've reached the location
             if (copter.wp_nav->reached_wp_destination()) {
                 // initialise landing controller
@@ -1839,6 +1898,7 @@ bool ModeAuto::verify_land()
                 // advance to next state
                 state = State::Descending;
             }
+#endif
             break;
 
         case State::Descending:
@@ -2039,10 +2099,24 @@ bool ModeAuto::verify_loiter_unlimited()
 // verify_loiter_time - check if we have loitered long enough
 bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    if (wp_nav->use_l1_navigation()) {
+        // check if we have reached the waypoint
+        if ( !copter.wp_nav->reached_l1_destination() ) {
+            return false;
+        }
+    } else {
+        // return immediately if we haven't reached our destination
+        if (!copter.wp_nav->reached_wp_destination()) {
+            return false;
+        }
+    }
+#else
     // return immediately if we haven't reached our destination
     if (!copter.wp_nav->reached_wp_destination()) {
         return false;
     }
+#endif
 
     // start our loiter timer
     if ( loiter_time == 0 ) {
@@ -2114,7 +2188,17 @@ bool ModeAuto::verify_yaw()
 // verify_nav_wp - check if we have reached the next way point
 bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
-    // check if we have reached the waypoint
+#if FRAME_CONFIG == HELI_FRAME
+    if (wp_nav->use_l1_navigation() && !wp_nav->is_spline()) {
+        // check if we have reached the waypoint
+        if ( !copter.wp_nav->reached_l1_destination() ) {
+            return false;
+        } 
+        gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
+        return true;
+    } else {
+#endif
+     // check if we have reached the waypoint
     if ( !copter.wp_nav->reached_wp_destination() ) {
         return false;
     }
@@ -2138,11 +2222,37 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
         return true;
     }
     return false;
+#if FRAME_CONFIG == HELI_FRAME
+    }
+#endif
+
 }
 
 // verify_circle - check if we have circled the point enough
 bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    if (wp_nav->use_l1_navigation()) {
+        // check if we have reached the waypoint
+        if ( !copter.wp_nav->reached_l1_destination()) {
+            return false;
+        }
+        // check if we have completed circling
+        return fabsf((float)copter.wp_nav->get_angle_total()/36000.0f) >= LOWBYTE(cmd.p1);
+    } else {
+        // check if we've reached the edge
+        if (_mode == SubMode::CIRCLE_MOVE_TO_EDGE) {
+            if (copter.wp_nav->reached_wp_destination()) {
+                // start circling
+                circle_start();
+            }
+            return false;
+        }
+
+        // check if we have completed circling
+        return fabsf(copter.circle_nav->get_angle_total()/float(M_2PI)) >= LOWBYTE(cmd.p1);
+    }
+#else
     // check if we've reached the edge
     if (_mode == SubMode::CIRCLE_MOVE_TO_EDGE) {
         if (copter.wp_nav->reached_wp_destination()) {
@@ -2154,6 +2264,7 @@ bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
 
     // check if we have completed circling
     return fabsf(copter.circle_nav->get_angle_total()/float(M_2PI)) >= LOWBYTE(cmd.p1);
+#endif
 }
 
 // verify_spline_wp - check if we have reached the next way point using spline

ArduCopter/system.cpp

@@ -68,6 +68,7 @@ void Copter::init_ardupilot()
 #endif
 #if FRAME_CONFIG == HELI_FRAME
     input_manager.set_loop_rate(scheduler.get_loop_rate_hz());
+    helispdhgtctrl.set_dt(scheduler.get_loop_period_s());
 #endif
 
     init_rc_in();               // sets up rc channels from radio
@@ -466,10 +467,14 @@ void Copter::allocate_motors(void)
     }
     AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info);
 
+#if FRAME_CONFIG != HELI_FRAME
 #if AC_OAPATHPLANNER_ENABLED == ENABLED
     wp_nav = new AC_WPNav_OA(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
 #else
     wp_nav = new AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
+#endif
+#else
+    wp_nav = new AC_WPNav_Heli(inertial_nav, *ahrs_view, *pos_control, *attitude_control, &helispdhgtctrl, L1_controller);
 #endif
     if (wp_nav == nullptr) {
         AP_BoardConfig::allocation_error("WPNav");

ArduCopter/wscript

@@ -12,6 +12,8 @@ def build(bld):
             'AC_PrecLand',
             'AC_Sprayer',
             'AC_Autorotation',
+            'AP_SpdHgtControl',
+            'AP_L1_Control',
             'AC_WPNav',
             'AP_Camera',
             'AP_IRLock',

libraries/AC_WPNav/AC_WPNav.h

@@ -52,7 +52,7 @@ class AC_WPNav
     ///     speed_cms is the desired max speed to travel between waypoints.  should be a positive value or omitted to use the default speed
     ///     updates target roll, pitch targets and I terms based on vehicle lean angles
     ///     should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
-    void wp_and_spline_init(float speed_cms = 0.0f, Vector3f stopping_point = Vector3f{});
+    virtual void wp_and_spline_init(float speed_cms = 0.0f, Vector3f stopping_point = Vector3f{});
 
     /// set current target horizontal speed during wp navigation
     void set_speed_xy(float speed_cms);
@@ -97,8 +97,8 @@ class AC_WPNav
     /// set_wp_destination waypoint using location class
     ///     provide the next_destination if known
     ///     returns false if conversion from location to vector from ekf origin cannot be calculated
-    bool set_wp_destination_loc(const Location& destination);
-    bool set_wp_destination_next_loc(const Location& destination);
+    virtual bool set_wp_destination_loc(const Location& destination);
+    virtual bool set_wp_destination_next_loc(const Location& destination);
 
     // get destination as a location.  Altitude frame will be absolute (AMSL) or above terrain
     // returns false if unable to return a destination (for example if origin has not yet been set)
@@ -111,7 +111,7 @@ class AC_WPNav
     /// set_wp_destination waypoint using position vector (distance from ekf origin in cm)
     ///     terrain_alt should be true if destination.z is a desired altitude above terrain
     virtual bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false);
-    bool set_wp_destination_next(const Vector3f& destination, bool terrain_alt = false);
+    virtual bool set_wp_destination_next(const Vector3f& destination, bool terrain_alt = false);
 
     /// set waypoint destination using NED position vector from ekf origin in meters
     ///     provide next_destination_NED if known
@@ -156,6 +156,9 @@ class AC_WPNav
     // returns true if update_wpnav has been run very recently
     bool is_active() const;
 
+    // returns true if this leg is spline
+    bool is_spline() { return _this_leg_is_spline; }
+
     ///
     /// spline methods
     ///