Plane: added QLOITER mode

quadplane loiter

ArduPlane/ArduPlane.cpp

@@ -690,7 +690,8 @@ void Plane::update_flight_mode(void)
 
 
     case QSTABILIZE:
-    case QHOVER: {
+    case QHOVER:
+    case QLOITER: {
         // set nav_roll and nav_pitch using sticks
         nav_roll_cd  = channel_roll->norm_input() * roll_limit_cd;
         nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
@@ -770,6 +771,7 @@ void Plane::update_navigation()
     case CIRCLE:
     case QSTABILIZE:
     case QHOVER:
+    case QLOITER:
         // nothing to do
         break;
     }

ArduPlane/Attitude.cpp

@@ -142,6 +142,7 @@ void Plane::stabilize_stick_mixing_direct()
         control_mode == CRUISE ||
         control_mode == QSTABILIZE ||
         control_mode == QHOVER ||
+        control_mode == QLOITER ||
         control_mode == TRAINING) {
         return;
     }
@@ -163,6 +164,7 @@ void Plane::stabilize_stick_mixing_fbw()
         control_mode == CRUISE ||
         control_mode == QSTABILIZE ||
         control_mode == QHOVER ||
+        control_mode == QLOITER ||
         control_mode == TRAINING ||
         (control_mode == AUTO && g.auto_fbw_steer)) {
         return;
@@ -362,6 +364,8 @@ void Plane::stabilize()
         quadplane.control_stabilize();
     } else if (control_mode == QHOVER) {
         quadplane.control_hover();
+    } else if (control_mode == QLOITER) {
+        quadplane.control_loiter();
     } else {
         if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {
             stabilize_stick_mixing_fbw();
@@ -926,7 +930,9 @@ void Plane::set_servos(void)
                    guided_throttle_passthru) {
             // manual pass through of throttle while in GUIDED
             channel_throttle->radio_out = channel_throttle->radio_in;
-        } else if (control_mode == QSTABILIZE || control_mode == QHOVER) {
+        } else if (control_mode == QSTABILIZE ||
+                   control_mode == QHOVER ||
+                   control_mode == QLOITER) {
             // no forward throttle for now
             channel_throttle->servo_out = 0;
             channel_throttle->calc_pwm();

ArduPlane/GCS_Mavlink.cpp

@@ -41,6 +41,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
     case FLY_BY_WIRE_B:
     case QSTABILIZE:
     case QHOVER:
+    case QLOITER:
     case CRUISE:
         base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
         break;
@@ -173,6 +174,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
     case AUTOTUNE:
     case QSTABILIZE:
     case QHOVER:
+    case QLOITER:
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
         break;

ArduPlane/defines.h

@@ -65,7 +65,8 @@ enum FlightMode {
     GUIDED        = 15,
     INITIALISING  = 16,
     QSTABILIZE    = 17,
-    QHOVER        = 18
+    QHOVER        = 18,
+    QLOITER       = 19
 };
 
 // type of stick mixing enabled

ArduPlane/events.cpp

@@ -28,6 +28,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
         break;
 
     case QSTABILIZE:
+    case QLOITER:
         failsafe.saved_mode = control_mode;
         failsafe.saved_mode_set = 1;
         set_mode(QHOVER);
@@ -82,6 +83,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
         break;
 
     case QSTABILIZE:
+    case QLOITER:
         set_mode(QHOVER);
         break;
         

ArduPlane/make.inc

@@ -53,3 +53,4 @@ LIBRARIES += AP_Motors
 LIBRARIES += AC_AttitudeControl
 LIBRARIES += AC_PID
 LIBRARIES += AP_InertialNav
+LIBRARIES += AC_WPNav

ArduPlane/quadplane.cpp

@@ -135,11 +135,11 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
 
     // @Param: TRANSITION_MS
     // @DisplayName: Transition time
-    // @Description: Transition time in milliseconds
+    // @Description: Transition time in milliseconds after minimum airspeed is reached
     // @Units: milli-seconds
     // @Range: 0 30000
     // @User: Advanced
-    AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 3000),
+    AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 5000),
 
     // @Param: PZ_P
     // @DisplayName: Position (vertical) controller P gain
@@ -240,6 +240,10 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("ACCEL_Z",  18, QuadPlane, pilot_accel_z,  250),
 
+    // @Group: WP_
+    // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp
+    AP_SUBGROUPINFO(wp_nav, "WP_",  19, QuadPlane, AC_WPNav),
+    
     AP_GROUPEND
 };
 
@@ -274,22 +278,16 @@ void QuadPlane::init_stabilize(void)
 // hold in stabilize with given throttle
 void QuadPlane::hold_stabilize(float throttle_in)
 {    
-    // max 100 degrees/sec for now
-    const float yaw_rate_max_dps = 100;
-    float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps;
-    
     // call attitude controller
     attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
                                                                         plane.nav_pitch_cd,
-                                                                        yaw_rate_ef_cds,
+                                                                        get_pilot_desired_yaw_rate_cds(),
                                                                         smoothing_gain);
 
     attitude_control.set_throttle_out(throttle_in, true, 0);
 
     // run low level rate controllers that only require IMU data
     attitude_control.rate_controller_run();
-
-    last_throttle = motors.get_throttle();
 }
 
 // quadplane stabilize mode
@@ -298,8 +296,8 @@ void QuadPlane::control_stabilize(void)
     int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10;
     hold_stabilize(pilot_throttle_scaled);
 
-    last_run_ms = millis();
     transition_state = TRANSITION_AIRSPEED_WAIT;
+    last_throttle = motors.get_throttle();
 }
 
 // init quadplane hover mode 
@@ -323,13 +321,10 @@ void QuadPlane::hold_hover(float target_climb_rate)
     pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
     pos_control.set_accel_z(pilot_accel_z);
 
-    const float yaw_rate_max_dps = 100;
-    float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps;
-    
     // call attitude controller
     attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
                                                                         plane.nav_pitch_cd,
-                                                                        yaw_rate_ef_cds,
+                                                                        get_pilot_desired_yaw_rate_cds(),
                                                                         smoothing_gain);
 
     // call position controller
@@ -347,15 +342,79 @@ void QuadPlane::hold_hover(float target_climb_rate)
  */
 void QuadPlane::control_hover(void)
 {
-    // get pilot desired climb rate
-    float target_climb_rate = pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0;
-
-    hold_hover(target_climb_rate);
+    hold_hover(get_pilot_desired_climb_rate_cms());
 
-    last_run_ms = millis();
     transition_state = TRANSITION_AIRSPEED_WAIT;
 }
 
+void QuadPlane::init_loiter(void)
+{
+    // set target to current position
+    wp_nav.init_loiter_target();
+
+    // initialize vertical speed and acceleration
+    pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
+    pos_control.set_accel_z(pilot_accel_z);
+
+    // initialise position and desired velocity
+    pos_control.set_alt_target(inertial_nav.get_altitude());
+    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+}
+
+
+// run quadplane loiter controller
+void QuadPlane::control_loiter()
+{
+    // initialize vertical speed and acceleration
+    pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
+    pos_control.set_accel_z(pilot_accel_z);
+
+    // process pilot's roll and pitch input
+    wp_nav.set_pilot_desired_acceleration(plane.channel_roll->control_in,
+                                          plane.channel_pitch->control_in);
+
+    // Update EKF speed limit - used to limit speed when we are using optical flow
+    float ekfGndSpdLimit, ekfNavVelGainScaler;    
+    ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
+    
+    // run loiter controller
+    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+
+    // call attitude controller
+    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(),
+                                                                 wp_nav.get_pitch(),
+                                                                 get_pilot_desired_yaw_rate_cds());
+
+    // nav roll and pitch are controller by loiter controller
+    plane.nav_roll_cd = wp_nav.get_roll();
+    plane.nav_pitch_cd = wp_nav.get_pitch();
+
+    // update altitude target and call position controller
+    pos_control.set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false);
+    pos_control.update_z_controller();
+
+    // run low level rate controllers that only require IMU data
+    attitude_control.rate_controller_run();
+
+    last_throttle = motors.get_throttle();
+}
+
+/*
+  get desired yaw rate in cd/s
+ */
+float QuadPlane::get_pilot_desired_yaw_rate_cds(void)
+{
+    const float yaw_rate_max_dps = 100;
+    return plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps;
+}
+
+// get pilot desired climb rate in cm/s
+float QuadPlane::get_pilot_desired_climb_rate_cms(void)
+{
+    return pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0;
+}
+
+
 // set motor arming
 void QuadPlane::set_armed(bool armed)
 {
@@ -383,7 +442,7 @@ void QuadPlane::update_transition(void)
         // we hold in hover until the required airspeed is reached
         float aspeed;
         if (ahrs.airspeed_estimate(&aspeed) && aspeed > plane.aparm.airspeed_min) {
-            last_run_ms = millis();
+            transition_start_ms = millis();
             transition_state = TRANSITION_TIMER;
         }
         hold_hover(0);
@@ -393,10 +452,10 @@ void QuadPlane::update_transition(void)
     case TRANSITION_TIMER: {
         // after airspeed is reached we degrade throttle over the
         // transition time, but continue to stabilize
-        if (millis() - last_run_ms > (unsigned)transition_time_ms) {
+        if (millis() - transition_start_ms > (unsigned)transition_time_ms) {
             transition_state = TRANSITION_DONE;
         }
-        float throttle_scaled = last_throttle * (transition_time_ms - (millis() - last_run_ms)) / (float)transition_time_ms;
+        float throttle_scaled = last_throttle * (transition_time_ms - (millis() - transition_start_ms)) / (float)transition_time_ms;
         if (throttle_scaled < 0) {
             throttle_scaled = 0;
         }
@@ -417,7 +476,8 @@ void QuadPlane::update_transition(void)
 void QuadPlane::update(void)
 {
     if (plane.control_mode != QSTABILIZE &&
-        plane.control_mode != QHOVER) {
+        plane.control_mode != QHOVER &&
+        plane.control_mode != QLOITER) {
         update_transition();
     } else {
         motors.output();

ArduPlane/quadplane.h

@@ -5,6 +5,7 @@
 #include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
 #include <AP_InertialNav/AP_InertialNav.h>
 #include <AC_AttitudeControl/AC_PosControl.h>
+#include <AC_WPNav/AC_WPNav.h>
 
 /*
   QuadPlane specific functionality
@@ -24,9 +25,13 @@ public:
     // main entry points for VTOL flight modes
     void init_stabilize(void);
     void control_stabilize(void);
+
     void init_hover(void);
     void control_hover(void);
 
+    void init_loiter(void);
+    void control_loiter(void);
+    
     // update transition handling
     void update(void);
 
@@ -60,6 +65,8 @@ private:
             p_alt_hold, p_vel_z, pid_accel_z,
             p_pos_xy, pi_vel_xy};
 
+    AC_WPNav wp_nav{inertial_nav, ahrs, pos_control, attitude_control};
+    
     // maximum vertical velocity the pilot may request
     AP_Int16 pilot_velocity_z_max;
 
@@ -75,10 +82,16 @@ private:
     // hold stabilize (for transition)
     void hold_stabilize(float throttle_in);    
 
+    // get desired yaw rate in cd/s
+    float get_pilot_desired_yaw_rate_cds(void);
+
+    // get desired climb rate in cm/s
+    float get_pilot_desired_climb_rate_cms(void);
+    
     AP_Int16 transition_time_ms;
     
-    // last time quadplane was active, used for transition
-    uint32_t last_run_ms;
+    // timer start for transition
+    uint32_t transition_start_ms;
 
     // last throttle value when active
     float last_throttle;

ArduPlane/system.cpp

@@ -455,6 +455,11 @@ void Plane::set_mode(enum FlightMode mode)
         auto_throttle_mode = false;
         quadplane.init_hover();
         break;
+
+    case QLOITER:
+        auto_throttle_mode = false;
+        quadplane.init_loiter();
+        break;
     }
 
     // start with throttle suppressed in auto_throttle modes
@@ -491,6 +496,7 @@ bool Plane::mavlink_set_mode(uint8_t mode)
     case LOITER:
     case QSTABILIZE:
     case QHOVER:
+    case QLOITER:
         set_mode((enum FlightMode)mode);
         return true;
     }