From ca85c332d6645170c1f89230ac7b4abb14cc34f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Dec 2015 18:45:42 +1100 Subject: [PATCH] Plane: separate QSTABILIZE and QHOVER modes --- ArduPlane/ArduPlane.cpp | 17 ++++--- ArduPlane/Attitude.cpp | 14 ++++-- ArduPlane/GCS_Mavlink.cpp | 6 ++- ArduPlane/defines.h | 3 +- ArduPlane/events.cpp | 14 +++++- ArduPlane/quadplane.cpp | 99 ++++++++++++++++++++++++++++++++------- ArduPlane/quadplane.h | 13 ++++- ArduPlane/system.cpp | 11 ++++- 8 files changed, 141 insertions(+), 36 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 46111f0fbd..93b20a3ae2 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -35,11 +35,11 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(read_radio, 50, 700), SCHED_TASK(check_short_failsafe, 50, 1000), - SCHED_TASK(ahrs_update, 50, 6400), + SCHED_TASK(ahrs_update, 400, 6400), SCHED_TASK(update_speed_height, 50, 1600), - SCHED_TASK(update_flight_mode, 50, 1400), - SCHED_TASK(stabilize, 50, 3500), - SCHED_TASK(set_servos, 50, 1600), + SCHED_TASK(update_flight_mode, 400, 1400), + SCHED_TASK(stabilize, 400, 3500), + SCHED_TASK(set_servos, 400, 1600), SCHED_TASK(read_control_switch, 7, 1000), SCHED_TASK(gcs_retry_deferred, 50, 1000), SCHED_TASK(update_GPS_50Hz, 50, 2500), @@ -172,6 +172,9 @@ void Plane::ahrs_update() // frame yaw rate steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt; steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err); + + // update inertial_nav for quadplane + quadplane.inertial_nav.update(G_Dt); } /* @@ -686,7 +689,8 @@ void Plane::update_flight_mode(void) //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 - case HOVER: { + case QSTABILIZE: + case QHOVER: { // 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); @@ -764,7 +768,8 @@ void Plane::update_navigation() case AUTOTUNE: case FLY_BY_WIRE_B: case CIRCLE: - case HOVER: + case QSTABILIZE: + case QHOVER: // nothing to do break; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 6396ab55c0..dc9ab7bc8d 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -140,7 +140,8 @@ void Plane::stabilize_stick_mixing_direct() control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || - control_mode == HOVER || + control_mode == QSTABILIZE || + control_mode == QHOVER || control_mode == TRAINING) { return; } @@ -160,7 +161,8 @@ void Plane::stabilize_stick_mixing_fbw() control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || - control_mode == HOVER || + control_mode == QSTABILIZE || + control_mode == QHOVER || control_mode == TRAINING || (control_mode == AUTO && g.auto_fbw_steer)) { return; @@ -356,8 +358,10 @@ void Plane::stabilize() stabilize_training(speed_scaler); } else if (control_mode == ACRO) { stabilize_acro(speed_scaler); - } else if (control_mode == HOVER) { - quadplane.stabilize_hover(); + } else if (control_mode == QSTABILIZE) { + quadplane.control_stabilize(); + } else if (control_mode == QHOVER) { + quadplane.control_hover(); } else { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { stabilize_stick_mixing_fbw(); @@ -922,7 +926,7 @@ 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 == HOVER) { + } else if (control_mode == QSTABILIZE || control_mode == QHOVER) { // 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 c0daf2dec7..2cb3792120 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -39,7 +39,8 @@ void Plane::send_heartbeat(mavlink_channel_t chan) case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: - case HOVER: + case QSTABILIZE: + case QHOVER: case CRUISE: base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; break; @@ -170,7 +171,8 @@ void Plane::send_extended_status1(mavlink_channel_t chan) case STABILIZE: case FLY_BY_WIRE_A: case AUTOTUNE: - case HOVER: + case QSTABILIZE: + case QHOVER: 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 8be362c5ae..a91f9b6cd9 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -64,7 +64,8 @@ enum FlightMode { LOITER = 12, GUIDED = 15, INITIALISING = 16, - HOVER = 17 + QSTABILIZE = 17, + QHOVER = 18 }; // type of stick mixing enabled diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 39217e9f7e..1e8b7d46e5 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -13,7 +13,6 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) case MANUAL: case STABILIZE: case ACRO: - case HOVER: case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: @@ -28,6 +27,12 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) } break; + case QSTABILIZE: + failsafe.saved_mode = control_mode; + failsafe.saved_mode_set = 1; + set_mode(QHOVER); + break; + case AUTO: case GUIDED: case LOITER: @@ -44,6 +49,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) case CIRCLE: case RTL: + case QHOVER: default: break; } @@ -62,7 +68,6 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) case MANUAL: case STABILIZE: case ACRO: - case HOVER: case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: @@ -76,6 +81,10 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) } break; + case QSTABILIZE: + set_mode(QHOVER); + break; + case AUTO: case GUIDED: case LOITER: @@ -87,6 +96,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) break; case RTL: + case QHOVER: default: break; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 2d3891bda3..121c2f7f7d 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -141,35 +141,35 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Advanced AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 10000), - // @Param: POS_Z_P + // @Param: PZ_P // @DisplayName: Position (vertical) controller P gain // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller // @Range: 1.000 3.000 // @User: Standard AP_SUBGROUPINFO(p_alt_hold, "PZ_", 11, QuadPlane, AC_P), - // @Param: POS_XY_P + // @Param: PXY_P // @DisplayName: Position (horizonal) controller P gain // @Description: Loiter position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller // @Range: 0.500 2.000 // @User: Standard AP_SUBGROUPINFO(p_pos_xy, "PXY_", 12, QuadPlane, AC_P), - // @Param: VEL_XY_P + // @Param: VXY_P // @DisplayName: Velocity (horizontal) P gain // @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration // @Range: 0.1 6.0 // @Increment: 0.1 // @User: Advanced - // @Param: VEL_XY_I + // @Param: VXY_I // @DisplayName: Velocity (horizontal) I gain // @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration // @Range: 0.02 1.00 // @Increment: 0.01 // @User: Advanced - // @Param: VEL_XY_IMAX + // @Param: VXY_IMAX // @DisplayName: Velocity (horizontal) integrator maximum // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output // @Range: 0 4500 @@ -178,39 +178,39 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Advanced AP_SUBGROUPINFO(pi_vel_xy, "VXY_", 13, QuadPlane, AC_PI_2D), - // @Param: VEL_Z_P + // @Param: VZ_P // @DisplayName: Velocity (vertical) P gain // @Description: Velocity (vertical) P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller // @Range: 1.000 8.000 // @User: Standard AP_SUBGROUPINFO(p_vel_z, "VZ_", 14, QuadPlane, AC_P), - // @Param: ACCEL_Z_P + // @Param: AZ_P // @DisplayName: Throttle acceleration controller P gain // @Description: Throttle acceleration controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output // @Range: 0.500 1.500 // @User: Standard - // @Param: ACCEL_Z_I + // @Param: AZ_I // @DisplayName: Throttle acceleration controller I gain // @Description: Throttle acceleration controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration // @Range: 0.000 3.000 // @User: Standard - // @Param: ACCEL_Z_IMAX + // @Param: AZ_IMAX // @DisplayName: Throttle acceleration controller I gain maximum // @Description: Throttle acceleration controller I gain maximum. Constrains the maximum pwm that the I term will generate // @Range: 0 1000 // @Units: Percent*10 // @User: Standard - // @Param: ACCEL_Z_D + // @Param: AZ_D // @DisplayName: Throttle acceleration controller D gain // @Description: Throttle acceleration controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration // @Range: 0.000 0.400 // @User: Standard - // @Param: ACCEL_Z_FILT_HZ + // @Param: AZ_FILT_HZ // @DisplayName: Throttle acceleration filter // @Description: Filter applied to acceleration to reduce noise. Lower values reduce noise but add delay. // @Range: 1.000 100.000 @@ -218,10 +218,28 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Standard AP_SUBGROUPINFO(pid_accel_z, "AZ_", 15, QuadPlane, AC_PID), - // @Group: POSCON_ + // @Group: P_ // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp AP_SUBGROUPINFO(pos_control, "P", 16, QuadPlane, AC_PosControl), + + // @Param: VELZ_MAX + // @DisplayName: Pilot maximum vertical speed + // @Description: The maximum vertical velocity the pilot may request in cm/s + // @Units: Centimeters/Second + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("VELZ_MAX", 17, QuadPlane, pilot_velocity_z_max, 250), + // @Param: ACCEL_Z + // @DisplayName: Pilot vertical acceleration + // @Description: The vertical acceleration used when pilot is controlling the altitude + // @Units: cm/s/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("ACCEL_Z", 18, QuadPlane, pilot_accel_z, 250), + AP_GROUPEND }; @@ -244,17 +262,27 @@ void QuadPlane::setup(void) pid_rate_pitch.set_dt(plane.ins.get_loop_delta_t()); pid_rate_yaw.set_dt(plane.ins.get_loop_delta_t()); pid_accel_z.set_dt(plane.ins.get_loop_delta_t()); + pos_control.set_dt(plane.ins.get_loop_delta_t()); } -// stabilize in hover mode -void QuadPlane::stabilize_hover(void) +// init quadplane stabilize mode +void QuadPlane::init_stabilize(void) +{ + // nothing to do +} + +// quadplane stabilize mode +void QuadPlane::control_stabilize(void) { // 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, smoothing_gain); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, + plane.nav_pitch_cd, + yaw_rate_ef_cds, + smoothing_gain); // output pilot's throttle int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10; @@ -267,6 +295,45 @@ void QuadPlane::stabilize_hover(void) last_throttle = pilot_throttle_scaled; } +// init quadplane hover mode +void QuadPlane::init_hover(void) +{ + // initialize vertical speeds and leash lengths + 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()); +} + +void QuadPlane::control_hover(void) +{ + // initialize vertical speeds and acceleration + 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; + + // get pilot desired climb rate + float target_climb_rate = pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0; + + // 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, + smoothing_gain); + + // call position controller + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, plane.G_Dt, false); + pos_control.update_z_controller(); + last_run_ms = millis(); + + // run low level rate controllers that only require IMU data + attitude_control.rate_controller_run(); +} + // set motor arming void QuadPlane::set_armed(bool armed) { @@ -299,7 +366,7 @@ void QuadPlane::update_transition(void) */ void QuadPlane::update(void) { - if (plane.control_mode != HOVER) { + if (plane.control_mode != QSTABILIZE && plane.control_mode != QHOVER) { update_transition(); } else { motors.output(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 1abe69f064..b1d05e3afe 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -21,8 +21,11 @@ public: // setup quadplane void setup(void); - // stabilize in hover mode - void stabilize_hover(void); + // main entry points for VTOL flight modes + void init_stabilize(void); + void control_stabilize(void); + void init_hover(void); + void control_hover(void); // update transition handling void update(void); @@ -57,6 +60,12 @@ private: p_alt_hold, p_vel_z, pid_accel_z, p_pos_xy, pi_vel_xy}; + // maximum vertical velocity the pilot may request + AP_Int16 pilot_velocity_z_max; + + // vertical acceleration the pilot may request + AP_Int16 pilot_accel_z; + // update transition handling void update_transition(void); diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 57ec05fa80..254613540c 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -446,8 +446,14 @@ void Plane::set_mode(enum FlightMode mode) set_guided_WP(); break; - case HOVER: + case QSTABILIZE: auto_throttle_mode = false; + quadplane.init_stabilize(); + break; + + case QHOVER: + auto_throttle_mode = false; + quadplane.init_hover(); break; } @@ -483,7 +489,8 @@ bool Plane::mavlink_set_mode(uint8_t mode) case AUTO: case RTL: case LOITER: - case HOVER: + case QSTABILIZE: + case QHOVER: set_mode((enum FlightMode)mode); return true; }