diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 93b20a3ae2..03594dfc40 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index dc9ab7bc8d..abf5bf577e 100644 --- a/ArduPlane/Attitude.cpp +++ b/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(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 2cb3792120..0fead43dcf 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/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; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index a91f9b6cd9..94ea0c8aed 100644 --- a/ArduPlane/defines.h +++ b/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 diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 1e8b7d46e5..70873627fd 100644 --- a/ArduPlane/events.cpp +++ b/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; diff --git a/ArduPlane/make.inc b/ArduPlane/make.inc index 6e73b694aa..70b8988554 100644 --- a/ArduPlane/make.inc +++ b/ArduPlane/make.inc @@ -53,3 +53,4 @@ LIBRARIES += AP_Motors LIBRARIES += AC_AttitudeControl LIBRARIES += AC_PID LIBRARIES += AP_InertialNav +LIBRARIES += AC_WPNav diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 725bf3f3b0..03c319a0d0 100644 --- a/ArduPlane/quadplane.cpp +++ b/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 }; @@ -273,23 +277,17 @@ 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(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 1355a4211b..f6ac2884a1 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -5,6 +5,7 @@ #include // Attitude control library #include #include +#include /* 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; @@ -74,11 +81,17 @@ 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; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 254613540c..8c63237b4c 100644 --- a/ArduPlane/system.cpp +++ b/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; }